Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge pull request #1908 from kirkscheper/dual_video_stream

Browse Source 
Dual video stream

Adds Auto White Balance and Auto Exposure for front camera on the Bebop (needs a small change to the exposure, found some inconsistencies when performed together with subsampling).
Adds dual video streaming capabilities for the bebop and update the rtp viewer (naming convention may be still changed).

Also updates to the ISP configuration which allows for subsampling and image scaling of the bebop front camera, replaces #1856
This commit is contained in:
Felix Ruess
2017-01-10 20:47:17 +01:00
committed by GitHub
parent 4db76e17cb 2baf51ea4f
commit 2f80d2ab7a
23 changed files with 1350 additions and 887 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/bebop.xml
+18 -5
View File
@@ -10,6 +10,17 @@
<module name="udp"/>
</target>
<define name="MT9F002_OUTPUT_HEIGHT" value="640" />
<define name="MT9F002_OUTPUT_WIDTH" value="640" />
<define name="MT9F002_TARGET_EXPOSURE" value="30" />
<define name="MT9F002_GAIN_GREEN1" value="4"/>
<define name="MT9F002_GAIN_GREEN2" value="4"/>
<define name="MT9F002_GAIN_RED" value="5"/>
<define name="MT9F002_GAIN_BLUE" value="5"/>
<define name="MT9F002_OUTPUT_SCALER" value="0.25"/>
<define name="MT9F002_X_ODD_INC_VAL" value="1"/>
<define name="MT9F002_Y_ODD_INC_VAL" value="1"/>
<!--define name="USE_SONAR" value="TRUE"/-->
<!-- Subsystem section -->
@@ -32,15 +43,17 @@
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</module-->
<module name="video_thread">
</module>
<module name="video_thread"/>
<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="20"/>
<define name="VIEWVIDEO_CAMERA" value="bottom_camera"/>
<define name="VIEWVIDEO_CAMERA2" value="front_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="2"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
</module>
<module name="bebop_ae_awb"/>
</modules>
<commands>
conf/control_panel_example.xml
+5
View File
@@ -68,6 +68,11 @@
<program name="IridiumDialer" command="sw/tools/iridium/iridium_link.py"/>
<program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/>
<program name="AeronauticalInfo" command="sw/ground_segment/python/atc/atc.py"/>
<program name="RtpViewer" command="sw/tools/rtp_viewer/rtp_viewer.py">
<arg flag="-p" constant="5000"/>
<arg flag="-s" constant="0.75"/>
<arg flag="-r" constant="3"/>
</program>
</section>
<section name="sessions">
conf/modules/bebop_ae_awb.xml
+22
View File
@@ -0,0 +1,22 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="bebop_ae_awb" dir="computer_vision">
<doc>
<description>Auto exposure and Auto white balancing for the Bebop 1 and 2</description>
<section name="bebop_ae_awb">
<define name="BEBOP_AUTO_EXPOSURE" value="true" description="perform auto exposure (Default: true)"/>
<define name="BEBOP_AUTO_WHITE_BALANCE" value="true" description="Perform auto white balance (Default: true)"/>
</section>
</doc>
<header>
<file name="bebop_ae_awb.h"/>
</header>
<init fun="bebop_ae_awb_init()"/>
<periodic fun="bebop_ae_awb_periodic()" freq="5" autorun="TRUE"/>
<makefile target="ap">
<file name="bebop_ae_awb.c"/>
</makefile>
</module>
conf/modules/bebop_front_camera.xml
-66
View File
@@ -1,66 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="bebop_front_camera" dir="computer_vision">
<doc>
<description>
Video streaming for the Bebop front camera
</description>
<define name="BEBOP_FRONT_CAMERA_SHOT_PATH" value="/data/ftp/internal_000/images" description="Path where the images should be saved"/>
<configure name="BEBOP_FRONT_CAMERA_HOST" value="192.168.1.255" description="GCS IP (default: MODEM_HOST)"/>
<configure name="BEBOP_FRONT_CAMERA_PORT_OUT" value="6000" description="Port (default: 5000)"/>
<configure name="BEBOP_FRONT_CAMERA_BROADCAST" value="FALSE|TRUE" description="Enable broadcast of image stream (default: TRUE)"/>
</doc>
<settings>
<dl_settings>
<dl_settings name="video">
<dl_setting var="bebop_front_camera.take_shot" min="0" step="1" max="1" shortname="take_shot" module="computer_vision/bebop_front_camera" handler="take_shot">
<strip_button name="Shoot" icon="digital-camera.png" value="1" group="video"/>
</dl_setting>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="bebop_front_camera.h"/>
</header>
<init fun="bebop_front_camera_init()"/>
<periodic fun="bebop_front_camera_periodic()" freq="1" start="bebop_front_camera_start()" stop="bebop_front_camera_stop()" autorun="TRUE"/>
<makefile target="ap">
<file name="bebop_front_camera.c"/>
<!-- Include the needed Computer Vision files -->
<include name="modules/computer_vision"/>
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
<file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
<!-- Random flags -->
<define name="__USE_GNU"/>
<flag name="LDFLAGS" value="lrt"/>
<flag name="LDFLAGS" value="static-libgcc"/>
<raw>
include $(CFG_SHARED)/udp.makefile
BEBOP_FRONT_CAMERA_HOST ?= $(MODEM_HOST)
BEBOP_FRONT_CAMERA_PORT_OUT ?= 6000
BEBOP_FRONT_CAMERA_BROADCAST ?= TRUE
BEBOPVIEWVID_CFLAGS = -DBEBOP_FRONT_CAMERA_HOST=$(BEBOP_FRONT_CAMERA_HOST) -DBEBOP_FRONT_CAMERA_PORT_OUT=$(BEBOP_FRONT_CAMERA_PORT_OUT)
ifeq ($(BEBOP_FRONT_CAMERA_USE_NETCAT),)
ap.CFLAGS += $(BEBOPVIEWVID_CFLAGS) -DBEBOP_FRONT_CAMERA_BROADCAST=$(BEBOP_FRONT_CAMERA_BROADCAST)
else
ap.CFLAGS += $(BEBOPVIEWVID_CFLAGS) -DBEBOP_FRONT_CAMERA_USE_NETCAT
endif
</raw>
</makefile>
<makefile target="nps">
<file name="BEBOP_FRONT_CAMERA_nps.c"/>
</makefile>
</module>
conf/modules/video_rtp_stream.xml
+9 -8
View File
@@ -5,20 +5,20 @@
<description>
Video streaming for Linux based devices.
Works e.g. in conjunction with Parrot Drones where the autopilot is the Paparazzi autopilot.
Features:
- Sends a RTP/UDP stream of the camera image, a.k.a. live video
- Possibility to save an image(shot) on internal or external storage space even in full size, best quality
Example to add to ARdrone2 airframe with extra USB stick plugger in:
Sends a RTP/UDP stream of the camera image, a.k.a. live video
</description>
<configure name="VIEWVIDEO_USE_NETCAT" value="FALSE|TRUE" description="Use netcat for transfering images instead of RTP stream (default: FALSE)"/>
<configure name="VIEWVIDEO_USE_NETCAT" value="FALSE|TRUE" description="Use netcat for transferring images instead of RTP stream (default: FALSE)"/>
<configure name="VIEWVIDEO_HOST" value="192.168.1.255" description="GCS IP (default: MODEM_HOST)"/>
<configure name="VIEWVIDEO_PORT_OUT" value="5000" description="Port (default: 5000)"/>
<configure name="VIEWVIDEO_PORT2_OUT" value="6000" description="Port (default: 6000)"/>
<configure name="VIEWVIDEO_BROADCAST" value="FALSE|TRUE" description="Enable broadcast of image stream (default: TRUE)"/>
<define name="VIEWVIDEO_CAMERA" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="4" description="Reduction factor of the video stream"/>
<define name="VIEWVIDEO_CAMERA2" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="4" description="Reduction factor of the video stream, the image width and height should be divisible by this factor"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="50" description="JPEG encoding compression factor [0-99]"/>
<define name="VIEWVIDEO_USE_RTP" value="TRUE|FALSE" description="Enable RTP at startup for transfering images (default: TRUE)"/>
<define name="VIEWVIDEO_FPS" value="5" description="Image frequency for the RTP viewer (recommended >=5Hz)"/>
<define name="VIEWVIDEO_USE_RTP" value="TRUE|FALSE" description="Enable RTP at startup for transferring images (default: TRUE)"/>
</doc>
<settings>
<dl_settings>
@@ -47,10 +47,11 @@
VIEWVIDEO_HOST ?= $(MODEM_HOST)
VIEWVIDEO_PORT_OUT ?= 5000
VIEWVIDEO_PORT2_OUT ?= 6000
VIEWVIDEO_BROADCAST ?= TRUE
VIEWVIDEO_USE_NETCAT ?= FALSE
VIEWVID_CFLAGS = -DVIEWVIDEO_HOST=$(VIEWVIDEO_HOST) -DVIEWVIDEO_PORT_OUT=$(VIEWVIDEO_PORT_OUT)
VIEWVID_CFLAGS = -DVIEWVIDEO_HOST=$(VIEWVIDEO_HOST) -DVIEWVIDEO_PORT_OUT=$(VIEWVIDEO_PORT_OUT) -DVIEWVIDEO_PORT2_OUT=$(VIEWVIDEO_PORT2_OUT)
ifneq (,$(findstring $(VIEWVIDEO_USE_NETCAT),0 FALSE))
ap.CFLAGS += $(VIEWVID_CFLAGS) -DVIEWVIDEO_BROADCAST=$(VIEWVIDEO_BROADCAST)
nps.CFLAGS += $(VIEWVID_CFLAGS) -DVIEWVIDEO_BROADCAST=FALSE
sw/airborne/boards/bebop.h
+4 -52
View File
@@ -27,58 +27,7 @@
#include "std.h"
#include "peripherals/video_device.h"
#ifndef MT9F002_OUTPUT_HEIGHT
#define MT9F002_OUTPUT_HEIGHT 3320
#endif
#ifndef MT9F002_OUTPUT_WIDTH
#define MT9F002_OUTPUT_WIDTH 2048
#endif
#ifndef MT9F002_INITIAL_OFFSET_X
#define MT9F002_INITIAL_OFFSET_X 1000 // pixels in the raw sensor!!
#endif
#ifndef MT9F002_INITIAL_OFFSET_Y
#define MT9F002_INITIAL_OFFSET_Y 0 // pixels in the raw sensor!!
#endif
/** Our output is only OUTPUT_SCALER of the pixels we take of the sensor
* It is programmable in 1/16 steps determined by ScaleFactor = 16/scale_m.
* Legal values for scale_m are 16 through 128, giving you the ability to scale from
* 1:1 to 1:8 (with m=128).
* Example:
* output_width = 512
* output_height = 830
* output_scaler = 0.25
* We now get an image of 512 by 830 which contains a "compressed version"
* of what would normally be an image of 2048 by 3320.
* Be warned: set your offset x appropriately.
* Example of what could go wrong:
* output_width = 512
* output_height = 830
* output_scaler = 0.25
* offset_x = 1500
* We now ask for pixels outside the 4608H x 2592V sensor or the 3320H x 2048W of the ISP.
*/
#ifndef MT9F002_OUTPUT_SCALER
#define MT9F002_OUTPUT_SCALER 1.0
#endif
/** Exposure of the front camera of the bebop. Experimental values:
* Outside: 15
* Inside well lit: 30
* Inside poorly lit: 60
*/
#ifndef MT9F002_TARGET_EXPOSURE
#define MT9F002_TARGET_EXPOSURE 30
#endif
#ifndef MT9F002_TARGET_FPS
#define MT9F002_TARGET_FPS 5
#endif
#include "boards/bebop/mt9f002.h"
/** uart connected to GPS internally */
#define UART1_DEV /dev/ttyPA1
@@ -96,6 +45,9 @@
extern struct video_config_t bottom_camera;
extern struct video_config_t front_camera;
/* ISP */
struct mt9f002_t mt9f002;
/* by default activate onboard baro */
#ifndef USE_BARO_BOARD
#define USE_BARO_BOARD 1
sw/airborne/boards/bebop/board.c
+35 -32
View File
@@ -35,6 +35,41 @@
#include "mcu.h"
#include "boards/bebop.h"
/* Initialize MT9F002 chipset (Front camera) */
struct mt9f002_t mt9f002 = {
// Precomputed values to go from InputCLK of (26/2)MHz to 96MH
.interface = MT9F002_PARALLEL,
.input_clk_freq = (26 / 2),
.vt_pix_clk_div = 7,
.vt_sys_clk_div = 1,
.pre_pll_clk_div = 1,
.pll_multiplier = 59,
.op_pix_clk_div = 8,
.op_sys_clk_div = 1,
.shift_vt_pix_clk_div = 1,
.rowSpeed_2_0 = 1,
.row_speed_10_8 = 1,
// Initial values
.target_fps = MT9F002_TARGET_FPS,
.target_exposure = MT9F002_TARGET_EXPOSURE,
.gain_green1 = MT9F002_GAIN_GREEN1,
.gain_blue = MT9F002_GAIN_BLUE,
.gain_red = MT9F002_GAIN_RED,
.gain_green2 = MT9F002_GAIN_GREEN2,
.output_width = MT9F002_OUTPUT_WIDTH,
.output_height = MT9F002_OUTPUT_HEIGHT,
.output_scaler = MT9F002_OUTPUT_SCALER,
.offset_x = MT9F002_INITIAL_OFFSET_X,
.offset_y = MT9F002_INITIAL_OFFSET_Y,
.x_odd_inc = MT9F002_X_ODD_INC_VAL,
.y_odd_inc = MT9F002_Y_ODD_INC_VAL,
// I2C connection port
.i2c_periph = &i2c0
};
static int kill_gracefull(char *process_name)
{
/* "pidof" always in /bin on Bebop firmware tested 1.98, 2.0.57, no need for "which" */
@@ -99,37 +134,5 @@ void board_init2(void)
};
mt9v117_init(&mt9v117);
/* Initialize MT9F002 chipset (Front camera) */
struct mt9f002_t mt9f002 = {
// Precomputed values to go from InputCLK of (26/2)MHz to 96MH
.interface = MT9F002_PARALLEL,
.input_clk_freq = (26 / 2),
.vt_pix_clk_div = 7,
.vt_sys_clk_div = 1,
.pre_pll_clk_div = 1,
.pll_multiplier = 59,
.op_pix_clk_div = 8,
.op_sys_clk_div = 1,
.shift_vt_pix_clk_div = 1,
.rowSpeed_2_0 = 1,
.row_speed_10_8 = 1,
// Initial values
.target_fps = MT9F002_TARGET_FPS,
.target_exposure = MT9F002_TARGET_EXPOSURE,
.gain_green1 = 2.0,
.gain_blue = 2.0,
.gain_red = 2.0,
.gain_green2 = 2.0,
.output_width = MT9F002_OUTPUT_WIDTH,
.output_height = MT9F002_OUTPUT_HEIGHT,
.output_scaler = MT9F002_OUTPUT_SCALER,
.offset_x = MT9F002_INITIAL_OFFSET_X,
.offset_y = MT9F002_INITIAL_OFFSET_Y,
// I2C connection port
.i2c_periph = &i2c0
};
mt9f002_init(&mt9f002);
}
sw/airborne/boards/bebop/mt9f002.c
+142 -93
View File
@@ -29,6 +29,7 @@
#include "mt9f002_regs.h"
#include "math/pprz_algebra_int.h"
#include "boards/bebop.h"
#include "modules/computer_vision/lib/isp/libisp.h"
#include <stdio.h>
#include <unistd.h>
@@ -38,7 +39,10 @@
#include <linux/videodev2.h>
#include <linux/v4l2-mediabus.h>
#define MAX(x,y) (((x) > (y)) ? (x) : (y))
#define MT9F002_MAX_WIDTH 4608
#define MT9F002_MAX_HEIGHT 3288
extern struct libisp_config isp_config;
/* Camera structure */
struct video_config_t front_camera = {
@@ -60,7 +64,7 @@ struct video_config_t front_camera = {
.subdev_name = "/dev/v4l-subdev1",
.format = V4L2_PIX_FMT_UYVY,
.subdev_format = V4L2_MBUS_FMT_SGRBG10_1X10,
.buf_cnt = 3,
.buf_cnt = 5,
.filters = VIDEO_FILTER_ISP,
.cv_listener = NULL,
.fps = MT9F002_TARGET_FPS
@@ -75,20 +79,17 @@ static void write_reg(struct mt9f002_t *mt, uint16_t addr, uint32_t val, uint8_t
mt->i2c_trans.buf[1] = addr & 0xFF;
// Fix sigdness based on length
if(len == 1) {
if (len == 1) {
mt->i2c_trans.buf[2] = val & 0xFF;
}
else if(len == 2) {
} else if (len == 2) {
mt->i2c_trans.buf[2] = (val >> 8) & 0xFF;
mt->i2c_trans.buf[3] = val & 0xFF;
}
else if(len == 4) {
} else if (len == 4) {
mt->i2c_trans.buf[2] = (val >> 24) & 0xFF;
mt->i2c_trans.buf[3] = (val >> 16) & 0xFF;
mt->i2c_trans.buf[4] = (val >> 8) & 0xFF;
mt->i2c_trans.buf[5] = val & 0xFF;
}
else {
} else {
printf("[MT9F002] write_reg with incorrect length %d\r\n", len);
}
@@ -109,8 +110,8 @@ static uint32_t read_reg(struct mt9f002_t *mt, uint16_t addr, uint8_t len)
i2c_transceive(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, 2, len);
/* Fix sigdness */
for(uint8_t i =0; i < len; i++) {
ret |= mt->i2c_trans.buf[len-i-1] << (8*i);
for (uint8_t i = 0; i < len; i++) {
ret |= mt->i2c_trans.buf[len - i - 1] << (8 * i);
}
return ret;
}
@@ -125,10 +126,9 @@ static inline void mt9f002_mipi_stage1(struct mt9f002_t *mt)
uint32_t serialFormat;
if (mt->interface == MT9F002_HiSPi) {
serialFormat = (3<<8) | 2; // 2 Serial lanes
}
else {
serialFormat = (2<<8) | 2; // 2 Serial lanes
serialFormat = (3 << 8) | 2; // 2 Serial lanes
} else {
serialFormat = (2 << 8) | 2; // 2 Serial lanes
}
write_reg(mt, MT9F002_SERIAL_FORMAT, serialFormat, 2);
uint32_t dataFormat = (8 << 8) | 8; // 8 Bits pixel depth
@@ -524,8 +524,8 @@ static inline void mt9f002_parallel_stage2(struct mt9f002_t *mt)
//write_reg(mt, MT9F002_DATAPATH_SELECT , 0xd881, 2); // permanent line valid
write_reg(mt, MT9F002_DATAPATH_SELECT , 0xd880, 2);
write_reg(mt, MT9F002_READ_MODE , 0x0041, 2);
write_reg(mt, MT9F002_X_ODD_INC , 0x0001, 2);
write_reg(mt, MT9F002_Y_ODD_INC , 0x0001, 2);
write_reg(mt, MT9F002_X_ODD_INC , mt->x_odd_inc, 2);
write_reg(mt, MT9F002_Y_ODD_INC , mt->y_odd_inc, 2);
write_reg(mt, MT9F002_MASK_CORRUPTED_FRAMES , 0x0001, 1); // 0 output corrupted frame, 1 mask them
}
@@ -543,19 +543,19 @@ static inline void mt9f002_set_pll(struct mt9f002_t *mt)
write_reg(mt, MT9F002_OP_SYS_CLK_DIV , mt->op_sys_clk_div, 2);
uint16_t smia = read_reg(mt, MT9F002_SMIA_TEST, 2);
write_reg(mt, MT9F002_SMIA_TEST, (smia & 0xFFBF) | (mt->shift_vt_pix_clk_div<<6), 2); // shift_vt_pix_clk_div
write_reg(mt, MT9F002_SMIA_TEST, (smia & 0xFFBF) | (mt->shift_vt_pix_clk_div << 6), 2); // shift_vt_pix_clk_div
uint16_t row_speed = read_reg(mt, MT9F002_ROW_SPEED, 2);
row_speed = (row_speed & 0xFFF8) | (mt->rowSpeed_2_0 & 0x07); // rowSpeed_2_0
row_speed = (row_speed & 0xF8FF) | ((mt->row_speed_10_8 & 0x07)<<8); // row_speed_10_8
row_speed = (row_speed&(~0x70)) | (0x2<<4); // Change opclk_delay
row_speed = (row_speed & 0xF8FF) | ((mt->row_speed_10_8 & 0x07) << 8); // row_speed_10_8
row_speed = (row_speed & (~0x70)) | (0x2 << 4); // Change opclk_delay
write_reg(mt, MT9F002_ROW_SPEED, row_speed, 2);
// Compute clocks
mt->vt_pix_clk = mt->input_clk_freq * (float)mt->pll_multiplier * (float)(1+mt->shift_vt_pix_clk_div)
/((float)mt->pre_pll_clk_div * (float)mt->vt_sys_clk_div * (float)mt->vt_pix_clk_div);
mt->vt_pix_clk = mt->input_clk_freq * (float)mt->pll_multiplier * (float)(1 + mt->shift_vt_pix_clk_div)
/ ((float)mt->pre_pll_clk_div * (float)mt->vt_sys_clk_div * (float)mt->vt_pix_clk_div);
mt->op_pix_clk = mt->input_clk_freq * (float)mt->pll_multiplier
/((float)mt->pre_pll_clk_div * (float)mt->op_sys_clk_div * (float)mt->op_pix_clk_div);
/ ((float)mt->pre_pll_clk_div * (float)mt->op_sys_clk_div * (float)mt->op_pix_clk_div);
}
/**
@@ -572,14 +572,17 @@ static void mt9f002_set_blanking(struct mt9f002_t *mt)
/* Calculate minimum line length */
float subsampling_factor = (float)(1 + x_odd_inc) / 2.0f; // See page 52
uint16_t min_line_length = MAX(min_line_length_pck, mt->scaled_width/subsampling_factor + min_line_blanking_pck); // EQ 9
min_line_length = MAX(min_line_length, (mt->scaled_width-1 + x_odd_inc) / subsampling_factor/2 + min_line_blanking_pck);
uint16_t min_line_length = Max(min_line_length_pck,
mt->scaled_width / subsampling_factor + min_line_blanking_pck); // EQ 9
min_line_length = Max(min_line_length,
(mt->scaled_width - 1 + x_odd_inc) / subsampling_factor / 2 + min_line_blanking_pck);
if (mt->interface == MT9F002_MIPI ||
mt->interface == MT9F002_HiSPi) {
min_line_length = MAX(min_line_length, ((uint16_t)((float)mt->scaled_width * mt->vt_pix_clk / mt->op_pix_clk)/2) + 0x005E); // 2 lanes, pll clocks
}
else {
min_line_length = MAX(min_line_length, ((uint16_t)((float)mt->scaled_width * mt->vt_pix_clk / mt->op_pix_clk)) + 0x005E); // pll clocks
min_line_length = Max(min_line_length,
((uint16_t)((float)mt->scaled_width * mt->vt_pix_clk / mt->op_pix_clk) / 2) + 0x005E); // 2 lanes, pll clocks
} else {
min_line_length = Max(min_line_length,
((uint16_t)((float)mt->scaled_width * mt->vt_pix_clk / mt->op_pix_clk)) + 0x005E); // pll clocks
}
/* Do some magic to get it to work with P7 ISP (with horizontal blanking) */
@@ -591,9 +594,9 @@ static void mt9f002_set_blanking(struct mt9f002_t *mt)
clkRatio_num = clkRatio_num / clkRatio_gcd;
clkRatio_den = clkRatio_den / clkRatio_gcd;
/* Calculate minimum horizontal blanking, since fpga line_length must be divideable by 2 */
/* Calculate minimum horizontal blanking, since fpga line_length must be divisible by 2 */
uint32_t min_horizontal_blanking = clkRatio_num;
if((clkRatio_den % 2) != 0) {
if ((clkRatio_den % 2) != 0) {
min_horizontal_blanking = 2 * clkRatio_num;
}
@@ -611,19 +614,19 @@ static void mt9f002_set_blanking(struct mt9f002_t *mt)
mt->real_fps = mt->vt_pix_clk * 1000000 / (float)(mt->line_length * mt->frame_length);
/* Check if we need to downscale the FPS and bruteforce better solution */
if(mt->target_fps < mt->real_fps) {
if (mt->target_fps < mt->real_fps) {
float min_fps_err = fabs(mt->target_fps - mt->real_fps);
float new_fps = mt->real_fps;
// Go through all possible line lengths
for(uint16_t ll = min_line_length; ll <= MT9F002_LINE_LENGTH_MAX; ll += min_horizontal_blanking) {
for (uint16_t ll = min_line_length; ll <= MT9F002_LINE_LENGTH_MAX; ll += min_horizontal_blanking) {
// Go through all possible frame lengths
for(uint16_t fl = min_frame_length; fl < MT9F002_FRAME_LENGTH_MAX; ++fl) {
for (uint16_t fl = min_frame_length; fl < MT9F002_FRAME_LENGTH_MAX; ++fl) {
new_fps = mt->vt_pix_clk * 1000000 / (float)(ll * fl);
// Calculate FPS error and save if it is better
float fps_err = fabs(mt->target_fps - new_fps);
if(fps_err < min_fps_err) {
if (fps_err < min_fps_err) {
min_fps_err = fps_err;
mt->line_length = ll;
mt->frame_length = fl;
@@ -631,7 +634,7 @@ static void mt9f002_set_blanking(struct mt9f002_t *mt)
}
// Stop searching if FPS is lower or equal
if(mt->target_fps > new_fps) {
if (mt->target_fps > new_fps) {
break;
}
}
@@ -640,7 +643,7 @@ static void mt9f002_set_blanking(struct mt9f002_t *mt)
new_fps = mt->vt_pix_clk * 1000000 / (float)(ll * min_frame_length);
// Stop searching if FPS is lower or equal
if(mt->target_fps > new_fps) {
if (mt->target_fps > new_fps) {
break;
}
}
@@ -655,7 +658,7 @@ static void mt9f002_set_blanking(struct mt9f002_t *mt)
* Set the exposure configuration
* Depends on the blanking (and therefore the FPS)
*/
static void mt9f002_set_exposure(struct mt9f002_t *mt)
void mt9f002_set_exposure(struct mt9f002_t *mt)
{
/* Fetch minimum and maximum integration times */
uint16_t coarse_integration_min = read_reg(mt, MT9F002_COARSE_INTEGRATION_TIME_MIN, 2);
@@ -669,8 +672,7 @@ static void mt9f002_set_exposure(struct mt9f002_t *mt)
uint16_t fine_integration = integration % mt->line_length;
/* Make sure fine integration is inside bounds */
if(fine_integration_min > fine_integration || fine_integration > fine_integration_max)
{
if (fine_integration_min > fine_integration || fine_integration > fine_integration_max) {
int32_t upper_coarse_integration = coarse_integration + 1;
int32_t upper_fine_integration = fine_integration_min;
@@ -678,12 +680,12 @@ static void mt9f002_set_exposure(struct mt9f002_t *mt)
int32_t lower_fine_integration = fine_integration_max;
// Check if lower case is invalid (take upper coarse)
if(lower_coarse_integration < coarse_integration_min) {
if (lower_coarse_integration < coarse_integration_min) {
coarse_integration = upper_coarse_integration;
fine_integration = upper_fine_integration;
}
// Check if upper case is invalid (take lower coarse)
else if(upper_coarse_integration > coarse_integration_max) {
else if (upper_coarse_integration > coarse_integration_max) {
coarse_integration = lower_coarse_integration;
fine_integration = lower_fine_integration;
}
@@ -693,11 +695,10 @@ static void mt9f002_set_exposure(struct mt9f002_t *mt)
int32_t upper_error = abs((mt->line_length * upper_coarse_integration + upper_fine_integration) - integration);
int32_t lower_error = abs((mt->line_length * lower_coarse_integration + lower_fine_integration) - integration);
if(upper_error < lower_error) {
if (upper_error < lower_error) {
coarse_integration = upper_coarse_integration;
fine_integration = upper_fine_integration;
}
else {
} else {
coarse_integration = lower_coarse_integration;
fine_integration = lower_fine_integration;
}
@@ -717,54 +718,44 @@ static void mt9f002_set_exposure(struct mt9f002_t *mt)
/**
* Calculate the gain based on value of 1.0 -> 63.50
*/
static uint16_t mt9f002_calc_gain(float gain) {
static uint16_t mt9f002_calc_gain(float gain)
{
// Check if gain is valid
if(gain < 1.0) {
if (gain < 1.0) {
gain = 1.0;
}
// Calculation of colamp, analg3 and digital gain based on table 19 p56
uint8_t colamp_gain, analog_gain3, digital_gain;
if (gain < 1.50)
{
if (gain < 1.50) {
// This is not recommended
colamp_gain = 0;
analog_gain3 = 0;
digital_gain = 1;
}
else if (gain < 3.0)
{
} else if (gain < 3.0) {
colamp_gain = 1;
analog_gain3 = 0;
digital_gain = 1;
}
else if (gain < 6.0)
{
} else if (gain < 6.0) {
colamp_gain = 2;
analog_gain3 = 0;
digital_gain = 1;
}
else if (gain < 16.0)
{
} else if (gain < 16.0) {
colamp_gain = 3;
analog_gain3 = 0;
digital_gain = 1;
}
else if (gain < 32.0)
{
} else if (gain < 32.0) {
colamp_gain = 3;
analog_gain3 = 0;
digital_gain = 2;
}
else
{
} else {
colamp_gain = 3;
analog_gain3 = 0;
digital_gain = 4;
}
// Calculate gain 2 (fine gain)
uint16_t analog_gain2 = gain / (float)digital_gain / (float)(1<<colamp_gain) / (float)(1<<analog_gain3) * 64.0;
uint16_t analog_gain2 = gain / (float)digital_gain / (float)(1 << colamp_gain) / (float)(1 << analog_gain3) * 64.0;
Bound(analog_gain2, 1, 127);
return (analog_gain2 & 0x7F) | ((analog_gain3 & 0x7) << 7) | ((colamp_gain & 0x3) << 10) | ((digital_gain & 0xF) << 12);
@@ -773,7 +764,7 @@ static uint16_t mt9f002_calc_gain(float gain) {
/**
* Sets the GreenR, Blue, Red and GreenB gains
*/
static void mt9f002_set_gains(struct mt9f002_t *mt)
void mt9f002_set_gains(struct mt9f002_t *mt)
{
write_reg(mt, MT9F002_GREEN1_GAIN, mt9f002_calc_gain(mt->gain_green1), 2);
write_reg(mt, MT9F002_BLUE_GAIN, mt9f002_calc_gain(mt->gain_blue), 2);
@@ -781,6 +772,85 @@ static void mt9f002_set_gains(struct mt9f002_t *mt)
write_reg(mt, MT9F002_GREEN2_GAIN, mt9f002_calc_gain(mt->gain_green2), 2);
}
void mt9f002_set_resolution(struct mt9f002_t *mt)
{
/* Set output resolution */
write_reg(mt, MT9F002_X_OUTPUT_SIZE, mt->output_width, 2);
write_reg(mt, MT9F002_Y_OUTPUT_SIZE, mt->output_height, 2);
/* Set scaling */
if (mt->output_scaler < 0.125) {
mt->output_scaler = 0.125;
printf("[MT9F002] Warning, ouput_scaler too small, changing to %f\n", mt->output_scaler);
} else if (mt->output_scaler > 1.) {
mt->output_scaler = 1.;
printf("[MT9F002] Warning, ouput_scaler too small, changing to %f\n", mt->output_scaler);
}
uint16_t scaleFactor = ceil((float)MT9F002_SCALER_N / mt->output_scaler);
mt->output_scaler = (float)MT9F002_SCALER_N / scaleFactor;
int x_skip_factor = (mt->x_odd_inc + 1) / 2;
int y_skip_factor = (mt->y_odd_inc + 1) / 2;
mt->scaled_width = mt->output_width * x_skip_factor / mt->output_scaler + mt->x_odd_inc - 1;
mt->scaled_height = mt->output_height * y_skip_factor / mt->output_scaler + mt->y_odd_inc - 1;
if (mt->scaled_width % (x_skip_factor * 8) != 0) {
mt->scaled_width = floor(mt->scaled_width / (x_skip_factor * 8)) * (x_skip_factor * 8);
printf("[MT9F002] Warning, scaled_width not a multiple of %i, changing to %i\n", 8 * x_skip_factor, mt->scaled_width);
}
if (mt->scaled_height % (y_skip_factor * 8) != 0) {
mt->scaled_height = floor(mt->scaled_height / (y_skip_factor * 8)) * (y_skip_factor * 8);
printf("[MT9F002] Warning, scaled_height not a multiple of %i, changing to %i\n", 8 * y_skip_factor, mt->scaled_height);
}
if (mt->output_scaler < 1.0) {
write_reg(mt, MT9F002_SCALING_MODE, 2, 2); // Vertical and horizontal scaling
write_reg(mt, MT9F002_SCALE_M, scaleFactor, 2);
}
printf("[MT9F002] OUTPUT_SIZE: (%i, %i)\tSCALED_SIZE: (%i, %i)\n", mt->output_width, mt->output_height,
mt->scaled_width,
mt->scaled_height);
/* bound offsets */
if (mt->offset_x * MT9F002_MAX_WIDTH + mt->scaled_width / 2 > MT9F002_MAX_WIDTH) {mt->offset_x = 1 - (float)mt->scaled_width / 2 / MT9F002_MAX_WIDTH;}
if (-mt->offset_x * MT9F002_MAX_WIDTH > mt->scaled_width) {mt->offset_x = -(float)mt->scaled_width / 2 / MT9F002_MAX_WIDTH;}
if (mt->offset_y * MT9F002_MAX_HEIGHT + mt->scaled_height / 2 > MT9F002_MAX_HEIGHT) {mt->offset_y = 1 - (float)mt->scaled_height / 2 / MT9F002_MAX_HEIGHT;}
if (-mt->offset_y * MT9F002_MAX_HEIGHT > mt->scaled_height / 2) {mt->offset_y = -(float)mt->scaled_height / 2 / MT9F002_MAX_HEIGHT;}
/* Set position (based on offset and subsample increment) */
uint16_t start_addr_x = (uint16_t)((MT9F002_MAX_WIDTH - mt->scaled_width) / 2 + mt->offset_x * MT9F002_MAX_WIDTH);
uint16_t start_addr_y = (uint16_t)((MT9F002_MAX_HEIGHT - mt->scaled_height) / 2 + mt->offset_y * MT9F002_MAX_HEIGHT);
if (start_addr_x < 24) {
start_addr_x = 24;
printf("[MT9F002] Warning, offset_y too small with given output_scaler, changing to %i\n", start_addr_x);
}
if (start_addr_x % (x_skip_factor * 8) != 0) {
start_addr_x = round(start_addr_x / (x_skip_factor * 8)) * (x_skip_factor * 8);
printf("[MT9F002] Warning, offset_x not a multiple of %i, changing to %i\n", 8 * x_skip_factor, start_addr_x);
}
if (start_addr_y % (y_skip_factor * 8) != 0) {
start_addr_y = round(start_addr_y / (y_skip_factor * 8)) * (y_skip_factor * 8);
printf("[MT9F002] Warning, offset_y not a multiple of %i, changing to %i\n", 8 * y_skip_factor, start_addr_y);
}
write_reg(mt, MT9F002_X_ADDR_START, start_addr_x, 2);
write_reg(mt, MT9F002_Y_ADDR_START, start_addr_y , 2);
uint16_t end_addr_x = start_addr_x + mt->scaled_width - 1;
uint16_t end_addr_y = start_addr_y + mt->scaled_height - 1;
write_reg(mt, MT9F002_X_ADDR_END, end_addr_x, 2);
write_reg(mt, MT9F002_Y_ADDR_END, end_addr_y, 2);
// set statistics to correct values
isp_config.statistics_bayer.window_x.x_offset = mt->scaled_width - (mt->scaled_width / BAYERSTATS_STATX) / 2;
isp_config.statistics_bayer.window_y.y_offset = mt->scaled_height - (mt->scaled_height / BAYERSTATS_STATY) / 2;
isp_config.statistics_bayer.window_x.x_width = mt->scaled_width / BAYERSTATS_STATX;
isp_config.statistics_bayer.window_y.y_width = mt->scaled_height / BAYERSTATS_STATY;
isp_config.statistics_yuv.window_pos_x.window_x_end = mt->scaled_width - 1;
isp_config.statistics_yuv.window_pos_y.window_y_end = mt->scaled_height - 1;
isp_config.statistics_yuv.increments_log2.x_log2_inc = log2(x_skip_factor);
isp_config.statistics_yuv.increments_log2.x_log2_inc = log2(y_skip_factor);
}
/**
* Initialisation of the Aptina MT9F002 CMOS sensor
* (front camera)
@@ -798,10 +868,9 @@ void mt9f002_init(struct mt9f002_t *mt)
usleep(1000000); // Wait for one second
/* Based on the interface configure stage 1 */
if(mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
if (mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
mt9f002_mipi_stage1(mt);
}
else {
} else {
mt9f002_parallel_stage1(mt);
}
@@ -809,33 +878,13 @@ void mt9f002_init(struct mt9f002_t *mt)
mt9f002_set_pll(mt);
/* Based on the interface configure stage 2 */
if(mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
if (mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
mt9f002_mipi_stage2(mt);
}
else {
} else {
mt9f002_parallel_stage2(mt);
}
/* Set output resolution */
write_reg(mt, MT9F002_X_OUTPUT_SIZE, mt->output_width, 2);
write_reg(mt, MT9F002_Y_OUTPUT_SIZE, mt->output_height, 2);
/* Set scaling */
uint16_t scaleFactor = ceil((float)MT9F002_SCALER_N / mt->output_scaler);
mt->output_scaler = (float)MT9F002_SCALER_N / scaleFactor;
mt->scaled_width = ceil((float)mt->output_width / mt->output_scaler);
mt->scaled_height = ceil((float)mt->output_height / mt->output_scaler);
if (mt->output_scaler != 1.0)
{
write_reg(mt, MT9F002_SCALING_MODE, 2, 2); // Vertical and horizontal scaling
write_reg(mt, MT9F002_SCALE_M, scaleFactor, 2);
}
/* Set position (based on offset) */
write_reg(mt, MT9F002_X_ADDR_START, mt->offset_x , 2);
write_reg(mt, MT9F002_X_ADDR_END , mt->offset_x + mt->scaled_width - 1, 2);
write_reg(mt, MT9F002_Y_ADDR_START, mt->offset_y, 2);
write_reg(mt, MT9F002_Y_ADDR_END , mt->offset_y + mt->scaled_height - 1, 2);
mt9f002_set_resolution(mt);
/* Update blanking (based on FPS) */
mt9f002_set_blanking(mt);
@@ -847,7 +896,7 @@ void mt9f002_init(struct mt9f002_t *mt)
mt9f002_set_gains(mt);
/* Based on the interface configure stage 3 */
if(mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
if (mt->interface == MT9F002_MIPI || mt->interface == MT9F002_HiSPi) {
mt9f002_mipi_stage3(mt);
}
sw/airborne/boards/bebop/mt9f002.h
+86 -7
View File
@@ -18,11 +18,11 @@
* <http://www.gnu.org/licenses/>.
*/
/**
* @file boards/bebop/mt9f002.h
*
* Initialization and configuration of the MT9F002 CMOS Chip
*/
/**
* @file boards/bebop/mt9f002.h
*
* Initialization and configuration of the MT9F002 CMOS Chip
*/
#ifndef MT9F002H
#define MT9F002H
@@ -30,6 +30,79 @@
#include "std.h"
#include "mcu_periph/i2c.h"
/* define required ouput image size */
#ifndef MT9F002_OUTPUT_HEIGHT
#define MT9F002_OUTPUT_HEIGHT 822 // full resolution 3288
#endif
#ifndef MT9F002_OUTPUT_WIDTH
#define MT9F002_OUTPUT_WIDTH 1152 // full resolution 4608
#endif
#ifndef MT9F002_INITIAL_OFFSET_X
#define MT9F002_INITIAL_OFFSET_X 0. // signed fractional offset from centre of image of original sensor [-0.5,0.5]
#endif
#ifndef MT9F002_INITIAL_OFFSET_Y
#define MT9F002_INITIAL_OFFSET_Y 0. // signed fractional offset from centre of image of original sensor [-0.5,0.5]
#endif
/** Our output is only OUTPUT_SCALER of the pixels we take of the sensor
* It is programmable in 1/16 steps determined by ScaleFactor = 16/scale_m.
* Legal values for scale_m are 16 through 128, giving you the ability to scale from
* 1:1 to 1:8 (with m=128).
* Example:
* output_width = 512
* output_height = 830
* output_scaler = 0.25
* We now get an image of 512 by 830 which contains a "compressed version"
* of what would normally be an image of 2048 by 3320 ISP (4608H x 2592V sensor)
*/
#ifndef MT9F002_OUTPUT_SCALER
#define MT9F002_OUTPUT_SCALER 1.
#endif
/** Exposure of the front camera of the bebop. Experimental values:
* Outside: 15
* Inside well lit: 30
* Inside poorly lit: 60
*/
#ifndef MT9F002_TARGET_EXPOSURE
#define MT9F002_TARGET_EXPOSURE 30
#endif
#ifndef MT9F002_TARGET_FPS
#define MT9F002_TARGET_FPS 15
#endif
/* Set the colour balance gains */
#ifndef MT9F002_GAIN_GREEN1
#define MT9F002_GAIN_GREEN1 3.0
#endif
#ifndef MT9F002_GAIN_GREEN2
#define MT9F002_GAIN_GREEN2 3.0
#endif
#ifndef MT9F002_GAIN_RED
#define MT9F002_GAIN_RED 3.0
#endif
#ifndef MT9F002_GAIN_BLUE
#define MT9F002_GAIN_BLUE 4.0
#endif
/* Set pixel increment value to implement subsampling */
/* Supported values for MT9F002_X_ODD_INC_VAL are 3, 7, 15 and 31 */
#ifndef MT9F002_X_ODD_INC_VAL
#define MT9F002_X_ODD_INC_VAL 1
#endif
/* Supported values for MT9F002_Y_ODD_INC_VAL are 1, 3 and 7 */
#ifndef MT9F002_Y_ODD_INC_VAL
#define MT9F002_Y_ODD_INC_VAL 1
#endif
/* Interface types for the MT9F002 connection */
enum mt9f002_interface {
MT9F002_MIPI, ///< MIPI type connection
@@ -70,13 +143,19 @@ struct mt9f002_t {
float output_scaler; ///< Output scale
uint16_t scaled_width; ///< Width after corrected scaling
uint16_t scaled_height; ///< Height after corrected scaling
uint16_t offset_x; ///< Offset from left in pixels
uint16_t offset_y; ///< Offset from top in pixels
float offset_x; ///< Offset from left in pixels
float offset_y; ///< Offset from top in pixels
uint8_t x_odd_inc; ///< X increment for subsampling (1,3,7,15,31 accepted)
uint8_t y_odd_inc; ///< Y increment for subsampling (1,3,7 accepted)
struct i2c_periph *i2c_periph; ///< I2C peripheral used to communicate over
struct i2c_transaction i2c_trans; ///< I2C transaction for comminication with CMOS chip
};
void mt9f002_init(struct mt9f002_t *mt);
void mt9f002_set_resolution(struct mt9f002_t *mt);
void mt9f002_set_exposure(struct mt9f002_t *mt);
void mt9f002_set_gains(struct mt9f002_t *mt);
#endif /* MT9F002_H */
sw/airborne/modules/computer_vision/bebop_ae_awb.c
+126
View File
@@ -0,0 +1,126 @@
/*
* Copyright (C) Freek van Tienen
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/computer_vision/bebop_ae_awb.c"
* @author Freek van Tienen, Kirk Scheper
* Auto exposure and Auto white balancing for the Bebop 1 and 2
*/
#include "bebop_ae_awb.h"
#include "boards/bebop.h"
#include "boards/bebop/mt9f002.h"
#include "lib/isp/libisp.h"
#define MAX_HIST_Y 255
#define sgn(x) (float)((x < 0) ? -1 : (x > 0))
#ifndef BEBOP_AUTO_EXPOSURE
#define BEBOP_AUTO_EXPOSURE true
#endif
#ifndef BEBOP_AUTO_WHITE_BALANCE
#define BEBOP_AUTO_WHITE_BALANCE true
#endif
#define BEBOP_AWB_MIN_GAIN 2
#define BEBOP_AWB_MAX_GAIN 75
void bebop_ae_awb_init(void) {}
void bebop_ae_awb_periodic(void)
{
struct isp_yuv_stats_t yuv_stats;
if (isp_get_statistics_yuv(&yuv_stats) == 0) {
#if BEBOP_AUTO_EXPOSURE
// Calculate the CDF based on the histogram
uint32_t cdf[MAX_HIST_Y];
cdf[0] = yuv_stats.ae_histogram_Y[0];
for (int i = 1; i < MAX_HIST_Y; i++) {
cdf[i] = cdf[i - 1] + yuv_stats.ae_histogram_Y[i];
}
// Calculate bright and saturated pixels
uint32_t bright_pixels = cdf[MAX_HIST_Y - 1] - cdf[MAX_HIST_Y - 26]; // Top 25 bins
uint32_t saturated_pixels = cdf[MAX_HIST_Y - 1] - cdf[MAX_HIST_Y - 6]; // top 5 bins
uint32_t target_bright_pixels = yuv_stats.nb_valid_Y / 20; // 5%
uint32_t max_saturated_pixels = yuv_stats.nb_valid_Y / 100; // 1%
float adjustment = 1.0f;
if (saturated_pixels + max_saturated_pixels / 10 > max_saturated_pixels) {
// Fix saturated pixels
adjustment = 1.0f - (float)saturated_pixels / yuv_stats.nb_valid_Y;
adjustment *= adjustment * adjustment; // speed up
} else if (bright_pixels + target_bright_pixels / 10 < target_bright_pixels) {
// increase brightness to try and hit the desired number of well exposed pixels
int l = MAX_HIST_Y - 1;
while (bright_pixels < target_bright_pixels && l > 0) {
bright_pixels += cdf[l];
bright_pixels -= cdf[l - 1];
l--;
}
adjustment = (float)MAX_HIST_Y / (l + 1);
} else if (bright_pixels - target_bright_pixels / 10 > target_bright_pixels) {
// decrease brightness to try and hit the desired number of well exposed pixels
int l = MAX_HIST_Y - 20;
while (bright_pixels > target_bright_pixels && l < MAX_HIST_Y) {
bright_pixels -= cdf[l];
bright_pixels += cdf[l - 1];
l++;
}
adjustment = (float)(MAX_HIST_Y - 20) / l;
adjustment *= adjustment; // speedup
}
// Calculate exposure
Bound(adjustment, 1 / 16.0f, 4.0);
mt9f002.target_exposure = mt9f002.real_exposure * adjustment;
mt9f002_set_exposure(&mt9f002);
#endif
#if BEBOP_AUTO_WHITE_BALANCE
// It is very important that the auto exposure converges faster than the color correction
// Calculate AWB and project from original scale [0,255] onto more typical scale[-0.5,0.5]
float avgU = ((float) yuv_stats.awb_sum_U / (float) yuv_stats.awb_nb_grey_pixels) / 256. - 0.5;
float avgV = ((float) yuv_stats.awb_sum_V / (float) yuv_stats.awb_nb_grey_pixels) / 256. - 0.5;
float threshold = 0.002f;
float gain = 0.5;
bool changed = false;
if (fabs(avgU) > threshold) {
mt9f002.gain_blue -= gain * avgU;
changed = true;
}
if (fabs(avgV) > threshold) {
mt9f002.gain_red -= gain * avgV;
changed = true;
}
if (changed) {
Bound(mt9f002.gain_blue, 2, 75);
Bound(mt9f002.gain_red, 2, 75);
mt9f002_set_gains(&mt9f002);
}
#endif
}
}
sw/airborne/modules/computer_vision/bebop_ae_awb.h
+33
View File
@@ -0,0 +1,33 @@
/*
* Copyright (C) Freek van Tienen
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/computer_vision/bebop_ae_awb.h"
* @author Freek van Tienen, Kirk Scheper
* Auto exposure and Auto white balancing for the Bebop 1 and 2
*/
#ifndef BEBOP_AE_AWB_H
#define BEBOP_AE_AWB_H
extern void bebop_ae_awb_init(void);
extern void bebop_ae_awb_periodic(void);
#endif
sw/airborne/modules/computer_vision/bebop_front_camera.c
-241
View File
@@ -1,241 +0,0 @@
/*
* Copyright (C) 2012-2014 The Paparazzi Community
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file modules/computer_vision/bebop_front_camera.c
*/
// Own header
#include "modules/computer_vision/bebop_front_camera.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/time.h>
#include <math.h>
// Video
#include "lib/v4l/v4l2.h"
#include "lib/vision/image.h"
#include "lib/vision/bayer.h"
#include "lib/encoding/jpeg.h"
#include "lib/encoding/rtp.h"
#include "udp_socket.h"
// Threaded computer vision
#include <pthread.h>
#define MT9F002_WIDTH 1408
#define MT9F002_HEIGHT 2112
#define BEBOP_FRONT_CAMERA_WIDTH 272
#define BEBOP_FRONT_CAMERA_HEIGHT 272
// The place where the shots are saved (without slash on the end)
#ifndef BEBOP_FRONT_CAMERA_SHOT_PATH
#define BEBOP_FRONT_CAMERA_SHOT_PATH /data/ftp/internal_000/images
#endif
PRINT_CONFIG_VAR(BEBOP_FRONT_CAMERA_SHOT_PATH)
// Main thread
static void *bebop_front_camera_thread(void *data);
static void bebop_front_camera_save_shot(struct image_t *img_color, struct image_t *img_jpeg, struct image_t *raw_img);
void bebop_front_camera_periodic(void) { }
// Initialize the bebop_front_camera structure with the defaults
struct bebopfrontcamera_t bebop_front_camera = {
.is_streaming = FALSE,
.take_shot = FALSE,
.shot_number = 0,
.take_shot = FALSE,
.shot_number = 0
};
void bebop_front_camera_take_shot(bool take)
{
bebop_front_camera.take_shot = true;
}
/**
* Handles all the video streaming and saving of the image shots
* This is a sepereate thread, so it needs to be thread safe!
*/
static void *bebop_front_camera_thread(void *data __attribute__((unused)))
{
struct UdpSocket video_sock;
udp_socket_create(&video_sock, STRINGIFY(BEBOP_FRONT_CAMERA_HOST), BEBOP_FRONT_CAMERA_PORT_OUT, -1,
BEBOP_FRONT_CAMERA_BROADCAST);
// Create the JPEG encoded image
struct image_t img_jpeg;
image_create(&img_jpeg, BEBOP_FRONT_CAMERA_WIDTH, BEBOP_FRONT_CAMERA_HEIGHT, IMAGE_JPEG);
// Create the Color image
struct image_t img_color;
image_create(&img_color, BEBOP_FRONT_CAMERA_WIDTH, BEBOP_FRONT_CAMERA_HEIGHT, IMAGE_YUV422);
// Start the streaming of the V4L2 device
if (!v4l2_start_capture(bebop_front_camera.dev)) {
printf("[bebop_front_camera-thread] Could not start capture of %s.\n", bebop_front_camera.dev->name);
return 0;
}
// Start streaming
bebop_front_camera.is_streaming = true;
while (bebop_front_camera.is_streaming) {
// Wait for a new frame (blocking)
struct image_t img;
v4l2_image_get(bebop_front_camera.dev, &img);
BayerToYUV(&img, &img_color, 0, 0);
if (bebop_front_camera.take_shot) {
// Save the image
bebop_front_camera_save_shot(&img_color, &img_jpeg, &img);
bebop_front_camera.take_shot = false;
}
jpeg_encode_image(&img_color, &img_jpeg, 80, 0);
// Send image with RTP
rtp_frame_send(
&video_sock, // UDP socket
&img_jpeg,
0, // Format 422
80, // Jpeg-Quality
0, // DRI Header
0 // 90kHz time increment
);
// Free the image
v4l2_image_free(bebop_front_camera.dev, &img);
}
return 0;
}
/**
* Initialize the view video
*/
void bebop_front_camera_init(void)
{
// Initialize the V4L2 subdevice (TODO: fix hardcoded path, which and code)
if (!v4l2_init_subdev("/dev/v4l-subdev1", 0, 0, V4L2_MBUS_FMT_SGBRG10_1X10, MT9F002_WIDTH, MT9F002_HEIGHT)) {
printf("[bebop_front_camera] Could not initialize the v4l-subdev1 subdevice.\n");
return;
}
// Initialize the V4L2 device
bebop_front_camera.dev = v4l2_init("/dev/video1", MT9F002_WIDTH, MT9F002_HEIGHT, 10, V4L2_PIX_FMT_SGBRG10);
if (bebop_front_camera.dev == NULL) {
printf("[bebop_front_camera] Could not initialize the /dev/video1 V4L2 device.\n");
return;
}
// Create the shot directory
char save_name[128];
sprintf(save_name, "mkdir -p %s", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH));
if (system(save_name) != 0) {
printf("[bebop_front_camera] Could not create shot directory %s.\n", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH));
return;
}
}
/**
* Start with streaming
*/
void bebop_front_camera_start(void)
{
// Check if we are already running
if (bebop_front_camera.is_streaming) {
return;
}
// Start the streaming thread
pthread_t tid;
if (pthread_create(&tid, NULL, bebop_front_camera_thread, NULL) != 0) {
printf("[vievideo] Could not create streaming thread.\n");
return;
}
}
/**
* Stops the streaming
* This could take some time, because the thread is stopped asynchronous.
*/
void bebop_front_camera_stop(void)
{
// Check if not already stopped streaming
if (!bebop_front_camera.is_streaming) {
return;
}
// Stop the streaming thread
bebop_front_camera.is_streaming = false;
// Stop the capturing
if (!v4l2_stop_capture(bebop_front_camera.dev)) {
printf("[bebop_front_camera] Could not stop capture of %s.\n", bebop_front_camera.dev->name);
return;
}
// TODO: wait for the thread to finish to be able to start the thread again!
}
static void bebop_front_camera_save_shot(struct image_t *img, struct image_t *img_jpeg, struct image_t *raw_img)
{
// Search for a file where we can write to
char save_name[128];
for (; bebop_front_camera.shot_number < 99999; bebop_front_camera.shot_number++) {
sprintf(save_name, "%s/img_%05d.pgm", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH), bebop_front_camera.shot_number);
// Check if file exists or not
if (access(save_name, F_OK) == -1) {
FILE *fp = fopen(save_name, "w");
if (fp == NULL) {
printf("[bebop_front_camera-thread] Could not write shot %s.\n", save_name);
} else {
// Save it to the file and close it
char pgm_header[] = "P5\n272 272\n255\n";
fwrite(pgm_header, sizeof(char), 15, fp);
fwrite(img->buf, sizeof(uint8_t), img->buf_size, fp);
fclose(fp);
// JPEG
jpeg_encode_image(img, img_jpeg, 99, 1);
sprintf(save_name, "%s/img_%05d.jpg", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH), bebop_front_camera.shot_number);
fp = fopen(save_name, "w");
fwrite(img_jpeg->buf, sizeof(uint8_t), img_jpeg->buf_size, fp);
fclose(fp);
sprintf(save_name, "%s/img_%05d.raw", STRINGIFY(BEBOP_FRONT_CAMERA_SHOT_PATH), bebop_front_camera.shot_number);
fp = fopen(save_name, "w");
fwrite(raw_img->buf, sizeof(uint8_t), raw_img->buf_size, fp);
fclose(fp);
}
// We don't need to seek for a next index anymore
break;
}
}
}
sw/airborne/modules/computer_vision/bebop_front_camera.h
-58
View File
@@ -1,58 +0,0 @@
/*
* Copyright (C) 2012-2014 The Paparazzi Community
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @file modules/computer_vision/bebop_front_camera.h
*
* Get live images from a RTP/UDP stream
* and save pictures on internal memory
*
* Works on Bebop platforms
*/
#ifndef BEBOP_FRONT_CAMERA_H
#define BEBOP_FRONT_CAMERA_H
#include "std.h"
// Main viewvideo structure
struct bebopfrontcamera_t {
struct v4l2_device *dev; ///< The V4L2 device that is used for the video stream
uint8_t fps; ///< The amount of frames per second
volatile bool take_shot; ///< Wether to take an image
uint16_t shot_number; ///< The last shot number
volatile bool is_streaming; ///< When the device is streaming
uint8_t downsize_factor; ///< Downsize factor during the stream
uint8_t quality_factor; ///< Quality factor during the stream
bool use_rtp; ///< Stream over RTP
};
extern struct bebopfrontcamera_t bebop_front_camera;
// Module functions
extern void bebop_front_camera_init(void);
extern void bebop_front_camera_periodic(void); ///< A dummy for now
extern void bebop_front_camera_start(void);
extern void bebop_front_camera_stop(void);
extern void bebop_front_camera_take_shot(bool take);
#endif /* BEBOP_FRONT_CAMERA_H */
sw/airborne/modules/computer_vision/cv.c
+3 -2
View File
@@ -105,7 +105,7 @@ struct video_listener *cv_add_to_device_async(struct video_config_t *device, cv_
int8_t cv_async_function(struct cv_async *async, struct image_t *img)
{
// If the previous image is not yet processed, return
if (pthread_mutex_trylock(&async->img_mutex) != 0 || !async->img_processed) {
if (!async->img_processed || pthread_mutex_trylock(&async->img_mutex) != 0) {
return -1;
}
@@ -115,6 +115,7 @@ int8_t cv_async_function(struct cv_async *async, struct image_t *img)
}
// Copy image
// TODO:this takes time causing some thread lag, should be replaced with gpu operation
image_copy(img, &async->img_copy);
// Inform thread of new image
@@ -175,7 +176,7 @@ void cv_run_device(struct video_config_t *device, struct image_t *img)
}
if (listener->async != NULL) {
// Send image to asynchronous thread, only update listener if succesful
// Send image to asynchronous thread, only update listener if successful
if (!cv_async_function(listener->async, img)) {
// Store timestamp
listener->ts = img->ts;
sw/airborne/modules/computer_vision/lib/encoding/rtp.c
+16 -19
View File
@@ -92,24 +92,22 @@ void rtp_frame_test(struct UdpSocket *udp)
* @param[in] format_code 0 for YUV422 and 1 for YUV421
* @param[in] quality_code The JPEG encoding quality
* @param[in] has_dri_header Whether we have an DRI header or not
* @param[in] delta_t Time between images (if set to 0 or less it is calculated)
* @param[in] frame_time Time image was taken in usec (if set to 0 or less it is calculated)
* @param[in] packet_number The frame number of the rtp stream
*/
void rtp_frame_send(struct UdpSocket *udp, struct image_t *img, uint8_t format_code,
uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
uint8_t quality_code, uint8_t has_dri_header, uint32_t frame_time, uint32_t *packet_number)
{
static uint32_t packetcounter = 0;
static uint32_t timecounter = 0;
uint32_t offset = 0;
uint32_t jpeg_size = img->buf_size;
uint8_t *jpeg_ptr = img->buf;
#define MAX_PACKET_SIZE 1400
if (delta_t <= 0) {
if (frame_time <= 0) {
struct timeval tv;
gettimeofday(&tv, 0);
uint32_t t = (tv.tv_sec % (256 * 256)) * 90000 + tv.tv_usec * 9 / 100;
timecounter = t;
frame_time = (tv.tv_sec % (256 * 256)) + tv.tv_usec;
}
// Split frame into packets
@@ -122,31 +120,29 @@ void rtp_frame_send(struct UdpSocket *udp, struct image_t *img, uint8_t format_c
len = jpeg_size;
}
rtp_packet_send(udp, jpeg_ptr, len, packetcounter, timecounter, offset, lastpacket, img->w, img->h, format_code,
rtp_packet_send(udp, jpeg_ptr, len, (*packet_number)++, frame_time, offset, lastpacket, img->w, img->h, format_code,
quality_code, has_dri_header);
jpeg_size -= len;
jpeg_ptr += len;
offset += len;
packetcounter++;
}
if (delta_t > 0) {
// timestamp = 1 / 90 000 seconds
timecounter += delta_t;
}
}
/*
* The RTP timestamp is in units of 90000Hz. The same timestamp MUST
appear in each fragment of a given frame. The RTP marker bit MUST be
set in the last packet of a frame.
* The same timestamp MUST appear in each fragment of a given frame.
* The RTP marker bit MUST be set in the last packet of a frame.
* Extra note: When the time difference between frames is non-constant,
there seems to introduce some lag or jitter in the video streaming.
One way to solve this is to send the timestamp in units of 90000Hz
rather than 100000 (when the frame is received the timestamp is always
"late" so the frame is displayed immediately). (1 = 1/90000 s) which
is probably stupid but is actually working.
* @param[in] *udp The UDP socket to send the RTP packet over
* @param[in] *Jpeg JPEG encoded image byte buffer
* @param[in] JpegLen The length of the byte buffer
* @param[in] m_SequenceNumber RTP sequence number
* @param[in] m_Timestamp Timestamp of the image
* @param[in] m_Timestamp Timestamp of the image in usec
* @param[in] m_offset 3 byte fragmentation offset for fragmented images
* @param[in] marker_bit RTP marker bit
* @param[in] w The width of the JPEG image
@@ -190,6 +186,7 @@ static void rtp_packet_send(
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* */
m_Timestamp *= 9 / 100; // convert timestamp to units of 1 / 90000 Hz
// Prepare the 12 byte RTP header
RtpBuf[0] = 0x80; // RTP version
RtpBuf[1] = 0x1a + (marker_bit << 7); // JPEG payload (26) and marker bit
sw/airborne/modules/computer_vision/lib/encoding/rtp.h
+1 -1
View File
@@ -33,7 +33,7 @@
#include "udp_socket.h"
void rtp_frame_send(struct UdpSocket *udp, struct image_t *img, uint8_t format_code, uint8_t quality_code,
uint8_t has_dri_header, uint32_t delta_t);
uint8_t has_dri_header, uint32_t frame_time, uint32_t *packet_number);
void rtp_frame_test(struct UdpSocket *udp);
#endif /* _CV_ENCODING_RTP_H */
sw/airborne/modules/computer_vision/lib/isp/libisp.c
+183 -210
View File
@@ -8,19 +8,19 @@
#include <sys/mman.h>
#include <sys/ioctl.h>
#include "libisp.h"
//#include "libisp.h"
#include "libisp_config.h"
#define AVI_BASE 0x400000
#define AVI_SIZE 0x100000
#define AVI_MASK (AVI_SIZE - 1)
struct avi_isp_offsets
{
uint32_t chain_bayer;
uint32_t gamma_corrector;
uint32_t chroma;
uint32_t statistics_yuv;
uint32_t chain_yuv;
struct avi_isp_offsets {
uint32_t chain_bayer;
uint32_t gamma_corrector;
uint32_t chroma;
uint32_t statistics_yuv;
uint32_t chain_yuv;
};
/* IOCTL implemented in AVI drivers */
@@ -30,39 +30,44 @@ struct avi_isp_offsets
#define readl(_addr) (*((volatile uint32_t *)(_addr)))
#define writel(_val, _addr) (*((volatile uint32_t *)(_addr)) = _val)
/* ISP context */
static struct libisp_context isp_ctx = {
.devmem = -1
};
static const unsigned isp_bases[] = {
AVI_ISP_CHAIN_BAYER_INTER,
AVI_ISP_VLFORMAT_32TO40,
AVI_ISP_PEDESTAL,
AVI_ISP_GREEN_IMBALANCE,
AVI_ISP_GREEN_IMBALANCE + AVI_ISP_GREEN_IMBALANCE_GREEN_RED_COEFF_MEM,
AVI_ISP_GREEN_IMBALANCE + AVI_ISP_GREEN_IMBALANCE_GREEN_BLUE_COEFF_MEM,
AVI_ISP_DEAD_PIXEL_CORRECTION + AVI_ISP_DEAD_PIXEL_CORRECTION_CFA,
AVI_ISP_DEAD_PIXEL_CORRECTION + AVI_ISP_DEAD_PIXEL_CORRECTION_LIST_MEM,
AVI_ISP_DENOISING,
AVI_ISP_STATISTICS_BAYER,
AVI_ISP_LENS_SHADING_CORRECTION,
AVI_ISP_LENS_SHADING_CORRECTION + AVI_ISP_LENS_SHADING_CORRECTION_RED_COEFF_MEM,
AVI_ISP_LENS_SHADING_CORRECTION + AVI_ISP_LENS_SHADING_CORRECTION_GREEN_COEFF_MEM,
AVI_ISP_LENS_SHADING_CORRECTION + AVI_ISP_LENS_SHADING_CORRECTION_BLUE_COEFF_MEM,
AVI_ISP_CHROMATIC_ABERRATION,
AVI_ISP_BAYER,
AVI_ISP_COLOR_CORRECTION,
AVI_ISP_VLFORMAT_40TO32,
0, /* GAMMA conf */
AVI_ISP_GAMMA_CORRECTOR_RY_LUT,
AVI_ISP_GAMMA_CORRECTOR_GU_LUT,
AVI_ISP_GAMMA_CORRECTOR_BV_LUT,
0, /* CHROMA */
0, /* STATS YUV*/
AVI_ISP_STATISTICS_YUV_AE_HISTOGRAM_Y,
AVI_ISP_CHAIN_YUV_INTER,
AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER + AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER_EE_KERNEL_COEFF,
AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER + AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER_EE_LUT,
AVI_ISP_I3D_LUT + AVI_ISP_I3D_LUT_CLIP_MODE,
AVI_ISP_I3D_LUT + AVI_ISP_I3D_LUT_LUT_OUTSIDE,
AVI_ISP_I3D_LUT + AVI_ISP_I3D_LUT_LUT_INSIDE,
AVI_ISP_DROP,
AVI_ISP_CHAIN_BAYER_INTER,
AVI_ISP_VLFORMAT_32TO40,
AVI_ISP_PEDESTAL,
AVI_ISP_GREEN_IMBALANCE,
AVI_ISP_GREEN_IMBALANCE + AVI_ISP_GREEN_IMBALANCE_GREEN_RED_COEFF_MEM,
AVI_ISP_GREEN_IMBALANCE + AVI_ISP_GREEN_IMBALANCE_GREEN_BLUE_COEFF_MEM,
AVI_ISP_DEAD_PIXEL_CORRECTION + AVI_ISP_DEAD_PIXEL_CORRECTION_CFA,
AVI_ISP_DEAD_PIXEL_CORRECTION + AVI_ISP_DEAD_PIXEL_CORRECTION_LIST_MEM,
AVI_ISP_DENOISING,
AVI_ISP_STATISTICS_BAYER,
AVI_ISP_LENS_SHADING_CORRECTION,
AVI_ISP_LENS_SHADING_CORRECTION + AVI_ISP_LENS_SHADING_CORRECTION_RED_COEFF_MEM,
AVI_ISP_LENS_SHADING_CORRECTION + AVI_ISP_LENS_SHADING_CORRECTION_GREEN_COEFF_MEM,
AVI_ISP_LENS_SHADING_CORRECTION + AVI_ISP_LENS_SHADING_CORRECTION_BLUE_COEFF_MEM,
AVI_ISP_CHROMATIC_ABERRATION,
AVI_ISP_BAYER,
AVI_ISP_COLOR_CORRECTION,
AVI_ISP_VLFORMAT_40TO32,
0, /* GAMMA conf */
AVI_ISP_GAMMA_CORRECTOR_RY_LUT,
AVI_ISP_GAMMA_CORRECTOR_GU_LUT,
AVI_ISP_GAMMA_CORRECTOR_BV_LUT,
0, /* CHROMA */
0, /* STATS YUV*/
AVI_ISP_STATISTICS_YUV_AE_HISTOGRAM_Y,
AVI_ISP_CHAIN_YUV_INTER,
AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER + AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER_EE_KERNEL_COEFF,
AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER + AVI_ISP_EDGE_ENHANCEMENT_COLOR_REDUCTION_FILTER_EE_LUT,
AVI_ISP_I3D_LUT + AVI_ISP_I3D_LUT_CLIP_MODE,
AVI_ISP_I3D_LUT + AVI_ISP_I3D_LUT_LUT_OUTSIDE,
AVI_ISP_I3D_LUT + AVI_ISP_I3D_LUT_LUT_INSIDE,
AVI_ISP_DROP,
};
/**
@@ -71,13 +76,13 @@ static const unsigned isp_bases[] = {
*/
static int avi_isp_get_offsets_fd(int fd, struct avi_isp_offsets *off)
{
if (ioctl(fd, AVI_ISP_IOGET_OFFSETS, off) < 0) {
printf("sizeof: %d, %X\n", sizeof(struct avi_isp_offsets), AVI_ISP_IOGET_OFFSETS);
perror("ioctl(AVI_ISP_IOGET_OFFSETS) failed");
return -1;
}
if (ioctl(fd, AVI_ISP_IOGET_OFFSETS, off) < 0) {
printf("sizeof: %d, %X\n", sizeof(struct avi_isp_offsets), AVI_ISP_IOGET_OFFSETS);
perror("ioctl(AVI_ISP_IOGET_OFFSETS) failed");
return -1;
}
return 0;
return 0;
}
/**
@@ -86,214 +91,182 @@ static int avi_isp_get_offsets_fd(int fd, struct avi_isp_offsets *off)
*/
static int open_isp_fd(struct libisp_context *ctx, int fd)
{
struct avi_isp_offsets off;
int i;
struct avi_isp_offsets off;
int i;
ctx->devmem = open("/dev/mem", O_RDWR);
ctx->devmem = open("/dev/mem", O_RDWR);
if (ctx->devmem < 0) {
perror("Can't open /dev/mem");
goto open_failed;
}
if (ctx->devmem < 0) {
perror("Can't open /dev/mem");
goto open_failed;
}
ctx->avi_base = (unsigned long) mmap(NULL, AVI_SIZE,
PROT_READ | PROT_WRITE,
MAP_SHARED, ctx->devmem, AVI_BASE & ~AVI_MASK);
ctx->avi_base = (unsigned long) mmap(NULL, AVI_SIZE,
PROT_READ | PROT_WRITE,
MAP_SHARED, ctx->devmem, AVI_BASE & ~AVI_MASK);
if (ctx->avi_base == (unsigned long) MAP_FAILED) {
perror("mmap failed");
goto mmap_failed;
}
if (ctx->avi_base == (unsigned long) MAP_FAILED) {
perror("mmap failed");
goto mmap_failed;
}
if (avi_isp_get_offsets_fd(fd, &off) < 0)
goto get_offsets_failed;
if (avi_isp_get_offsets_fd(fd, &off) < 0) {
goto get_offsets_failed;
}
/* Compute all the sub-modules offsets */
/* Chain Bayer */
for (i = chain_bayer_inter ; i < gamma_corrector ; i++)
ctx->offsets[i] = ctx->avi_base + isp_bases[i] + off.chain_bayer;
/* Compute all the sub-modules offsets */
/* Chain Bayer */
for (i = chain_bayer_inter ; i < gamma_corrector ; i++) {
ctx->offsets[i] = ctx->avi_base + isp_bases[i] + off.chain_bayer;
}
ctx->offsets[gamma_corrector] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
ctx->offsets[gamma_corrector_ry_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
ctx->offsets[gamma_corrector_gu_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
ctx->offsets[gamma_corrector_bv_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
ctx->offsets[chroma] = ctx->avi_base + isp_bases[i++] + off.chroma;
ctx->offsets[statistics_yuv] = ctx->avi_base + isp_bases[i++] + off.statistics_yuv;
ctx->offsets[statistics_yuv_ae_histogram_y] = ctx->avi_base + isp_bases[i++] + off.statistics_yuv;
ctx->offsets[gamma_corrector] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
ctx->offsets[gamma_corrector_ry_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
ctx->offsets[gamma_corrector_gu_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
ctx->offsets[gamma_corrector_bv_lut] = ctx->avi_base + isp_bases[i++] + off.gamma_corrector;
ctx->offsets[chroma] = ctx->avi_base + isp_bases[i++] + off.chroma;
ctx->offsets[statistics_yuv] = ctx->avi_base + isp_bases[i++] + off.statistics_yuv;
ctx->offsets[statistics_yuv_ae_histogram_y] = ctx->avi_base + isp_bases[i++] + off.statistics_yuv;
/* Chain YUV */
for (i = chain_yuv_inter ; i < ISP_NODE_NR ; i++)
ctx->offsets[i] = ctx->avi_base + isp_bases[i] + off.chain_yuv;
/* Chain YUV */
for (i = chain_yuv_inter ; i < ISP_NODE_NR ; i++) {
ctx->offsets[i] = ctx->avi_base + isp_bases[i] + off.chain_yuv;
}
return 0;
return 0;
get_offsets_failed:
munmap((void *) ctx->avi_base, AVI_SIZE);
munmap((void *) ctx->avi_base, AVI_SIZE);
mmap_failed:
close(ctx->devmem);
close(ctx->devmem);
ctx->devmem = -1;
open_failed:
return -1;
return -1;
}
static int close_isp(struct libisp_context *ctx)
/*static int close_isp(struct libisp_context *ctx)
{
int ret = 0;
int ret = 0;
if (munmap((void *) ctx->avi_base, AVI_SIZE) == -1) {
perror("munmap failed");
ret = -1;
}
if (munmap((void *) ctx->avi_base, AVI_SIZE) == -1) {
perror("munmap failed");
ret = -1;
}
close(ctx->devmem);
close(ctx->devmem);
ctx->devmem = -1;
return ret;
}
return ret;
}*/
int configure_isp(struct v4l2_device *dev)
{
struct libisp_context isp_ctx;
if (open_isp_fd(&isp_ctx, dev->fd) < 0) {
return -1;
}
struct avi_isp_chain_bayer_inter_regs bayer_inter = {{
.pedestal_bypass = 1,
.grim_bypass = 1,
.rip_bypass = 1,
.denoise_bypass = 0,
.lsc_bypass = 1,
.chroma_aber_bypass = 1,
.bayer_bypass = 0,
.color_matrix_bypass = 0,
}};
avi_isp_vlformat_32to40_set_registers(&isp_ctx, &isp_config.vlformat_32to40);
avi_isp_chain_bayer_inter_set_registers(&isp_ctx, &isp_config.bayer_inter);
avi_isp_pedestal_set_registers(&isp_ctx, &isp_config.pedestal);
avi_isp_green_imbalance_set_registers(&isp_ctx, &isp_config.green_imbalance);
avi_isp_green_imbalance_green_red_coeff_mem_set_registers(&isp_ctx, &isp_config.grim_gr);
avi_isp_green_imbalance_green_blue_coeff_mem_set_registers(&isp_ctx, &isp_config.grim_gb);
avi_isp_dead_pixel_correction_set_registers(&isp_ctx, &isp_config.dead_pixel_correction);
avi_isp_denoising_set_registers(&isp_ctx, &isp_config.denoising);
avi_isp_statistics_bayer_set_registers(&isp_ctx, &isp_config.statistics_bayer);
avi_isp_lens_shading_correction_set_registers(&isp_ctx, &isp_config.lens_shading_correction);
avi_isp_lens_shading_correction_red_coeff_mem_set_registers(&isp_ctx, &isp_config.lsc_red_coeffs);
avi_isp_lens_shading_correction_green_coeff_mem_set_registers(&isp_ctx, &isp_config.lsc_green_coeffs);
avi_isp_lens_shading_correction_blue_coeff_mem_set_registers(&isp_ctx, &isp_config.lsc_blue_coeffs);
avi_isp_bayer_set_registers(&isp_ctx, &isp_config.bayer);
avi_isp_color_correction_set_registers(&isp_ctx, &isp_config.color_correction);
avi_isp_vlformat_40to32_set_registers(&isp_ctx, &isp_config.vlformat_40to32);
avi_isp_gamma_corrector_set_registers(&isp_ctx, &isp_config.gamma_corrector);
avi_isp_gamma_corrector_ry_lut_set_registers(&isp_ctx, &isp_config.gc_ry_lut);
avi_isp_gamma_corrector_gu_lut_set_registers(&isp_ctx, &isp_config.gc_gu_lut);
avi_isp_gamma_corrector_bv_lut_set_registers(&isp_ctx, &isp_config.gc_bv_lut);
avi_isp_chroma_set_registers(&isp_ctx, &isp_config.chroma);
avi_isp_statistics_yuv_set_registers(&isp_ctx, &isp_config.statistics_yuv);
avi_isp_edge_enhancement_color_reduction_filter_set_registers(&isp_ctx, &isp_config.eecrf);
avi_isp_edge_enhancement_color_reduction_filter_ee_lut_set_registers(&isp_ctx, &isp_config.eecrf_lut);
avi_isp_chain_yuv_inter_get_registers(&isp_ctx, &isp_config.chain_yuv_inter);
struct avi_isp_vlformat_32to40_regs vlf_32to40 = {{
.format = 0x0,
}};
//close_isp(&isp_ctx);
struct avi_isp_bayer_regs bay = {
.cfa = { ._register = 0x03 },
.threshold_1 = { ._register = 0x19 },
.threshold_2 = { ._register = 0xc8 },
};
struct avi_isp_color_correction_regs cc = {
.coeff_01_00 = { ._register = 0xF3811477 },
.coeff_10_02 = { ._register = 0xFDF0021d },
.coeff_12_11 = { ._register = 0xFF9E0A33 },
.coeff_21_20 = { ._register = 0xF4DFFE25 },
.coeff_22 = { ._register = 0x00001B83 },
.offset_ry = { ._register = 0x00000000 },
.clip_ry = { ._register = 0x03FF0000 },
.offset_gu = { ._register = 0x00000000 },
.clip_gu = { ._register = 0x03FF0000 },
.offset_bv = { ._register = 0x00000000 },
.clip_bv = { ._register = 0x03FF0000 },
};
struct avi_isp_vlformat_40to32_regs vlf_40to32 = {{
.format = 0x4,
}};
#define COMPLEMENT_2(i, r) (((i) >= 0) ? (r) : (~(r) + 1) & 0x3fff)
#define Q311(i) (COMPLEMENT_2(i, (unsigned)(((ABS(i)) * (1 << 11)) + 0.5)))
/*
* Chroma converter parameters to convert input stream from a given
* color space to another.
* See http://www.fourcc.org/fccyvrgb.php
*/
#define AVI_CONV_MATRIX(_c00, _c01, _c02, \
_c10, _c11, _c12, \
_c20, _c21, _c22) \
.coeff_01_00 = {{ .coeff_00 = Q311(_c00), .coeff_01 = Q311(_c01) }}, \
.coeff_10_02 = {{ .coeff_02 = Q311(_c02), .coeff_10 = Q311(_c10) }}, \
.coeff_12_11 = {{ .coeff_11 = Q311(_c11), .coeff_12 = Q311(_c12) }}, \
.coeff_21_20 = {{ .coeff_20 = Q311(_c20), .coeff_21 = Q311(_c21) }}, \
.coeff_22 = {{ .coeff_22 = Q311(_c22) }}
#define AVI_CONV_OFFSETS(_ryin, _ryout, \
_guin, _guout, \
_bvin, _bvout) \
.offset_ry = {{ .offset_in = _ryin, .offset_out = _ryout }}, \
.offset_gu = {{ .offset_in = _guin, .offset_out = _guout }}, \
.offset_bv = {{ .offset_in = _bvin, .offset_out = _bvout }}
#define AVI_CONV_CLIPS(_rymin, _rymax, \
_gumin, _gumax, \
_bvmin, _bvmax) \
.clip_ry = {{ .clip_min = _rymin, .clip_max = _rymax }}, \
.clip_gu = {{ .clip_min = _gumin, .clip_max = _gumax }}, \
.clip_bv = {{ .clip_min = _bvmin, .clip_max = _bvmax }}
struct avi_isp_chroma_regs chr = {
AVI_CONV_MATRIX( 0.213, 0.715, 0.072,
-0.100, -0.336, 0.436,
0.615, -0.515, -0.100),
AVI_CONV_OFFSETS(0, 16,
0, 128,
0, 128),
AVI_CONV_CLIPS(16, 235,
16, 240,
16, 240),
};
if(open_isp_fd(&isp_ctx, dev->fd) < 0)
return -1;
avi_isp_chain_bayer_inter_set_registers(&isp_ctx, &bayer_inter);
avi_isp_vlformat_32to40_set_registers(&isp_ctx, &vlf_32to40);
avi_isp_bayer_set_registers(&isp_ctx, &bay);
avi_isp_color_correction_set_registers(&isp_ctx, &cc);
avi_isp_vlformat_40to32_set_registers(&isp_ctx, &vlf_40to32);
avi_isp_chroma_set_registers(&isp_ctx, &chr);
close_isp(&isp_ctx);
return 0;
return 0;
}
/* Get YUV statistics */
int isp_get_statistics_yuv(struct isp_yuv_stats_t *yuv_stats)
{
uint16_t i;
if (isp_ctx.devmem < 0) {
return -1;
}
struct avi_isp_statistics_yuv_regs stats_yuv;
avi_isp_statistics_yuv_get_registers(&isp_ctx, &stats_yuv);
yuv_stats->awb_sum_Y = stats_yuv.awb_sum_y.awb_sum_y;
yuv_stats->awb_sum_U = stats_yuv.awb_sum_u.awb_sum_u;
yuv_stats->awb_sum_V = stats_yuv.awb_sum_v.awb_sum_v;
yuv_stats->awb_nb_grey_pixels = stats_yuv.awb_nb_grey_pixels.nb_grey_pixels;
yuv_stats->nb_valid_Y = stats_yuv.ae_nb_valid_y.nb_valid_y;
// Histogram
struct avi_isp_statistics_yuv_ae_histogram_y_regs histogram;
avi_isp_statistics_yuv_ae_histogram_y_get_registers(&isp_ctx, &histogram);
for (i = 0; i < 256; ++i) {
yuv_stats->ae_histogram_Y[i] = histogram.ae_histogram_y[i].histogram_y;
}
avi_isp_statistics_yuv_set_registers(&isp_ctx, &isp_config.statistics_yuv);
return 0;
}
static inline void memcpy_to_registers(unsigned long addr,
const void *reg_base,
size_t s)
{
const uint32_t *reg = reg_base;
unsigned i;
const uint32_t *reg = reg_base;
unsigned i;
s /= sizeof(uint32_t); /* we write one register at a time */
s /= sizeof(uint32_t); /* we write one register at a time */
for (i = 0; i < s; i++)
writel(reg[i], addr + i * sizeof(uint32_t));
for (i = 0; i < s; i++) {
writel(reg[i], addr + i * sizeof(uint32_t));
}
}
static inline void memcpy_from_registers(void *reg_base,
unsigned long addr,
size_t s)
unsigned long addr,
size_t s)
{
uint32_t *reg = reg_base;
unsigned i;
uint32_t *reg = reg_base;
unsigned i;
s /= sizeof(uint32_t); /* we read one register at a time */
s /= sizeof(uint32_t); /* we read one register at a time */
for (i = 0; i < s; i++)
reg[i] = readl(addr + i * sizeof(uint32_t));
for (i = 0; i < s; i++) {
reg[i] = readl(addr + i * sizeof(uint32_t));
}
}
#define EXPAND_AS_FUNCTION(_node) \
void avi_isp_ ## _node ## _set_registers(struct libisp_context *c, \
struct avi_isp_ ## _node ## _regs const *regs) \
{ \
memcpy_to_registers(c->offsets[_node], regs, sizeof(*regs)); \
} \
\
void avi_isp_ ## _node ## _get_registers(struct libisp_context *c, \
struct avi_isp_ ## _node ## _regs *regs) \
{ \
memcpy_from_registers(regs, c->offsets[_node], sizeof(*regs)); \
}
void avi_isp_ ## _node ## _set_registers(struct libisp_context *c, \
struct avi_isp_ ## _node ## _regs const *regs) \
{ \
memcpy_to_registers(c->offsets[_node], regs, sizeof(*regs)); \
} \
\
void avi_isp_ ## _node ## _get_registers(struct libisp_context *c, \
struct avi_isp_ ## _node ## _regs *regs) \
{ \
memcpy_from_registers(regs, c->offsets[_node], sizeof(*regs)); \
}
AVI_DEFINE_NODE(EXPAND_AS_FUNCTION)
sw/airborne/modules/computer_vision/lib/isp/libisp.h
+91 -44
View File
@@ -4,64 +4,111 @@
#include "reg_avi.h"
#include "modules/computer_vision/lib/v4l/v4l2.h"
#define BAYERSTATS_STATX 64
#define BAYERSTATS_STATY 48
#define AVI_DEFINE_NODE(EXPANDER) \
EXPANDER(chain_bayer_inter) \
EXPANDER(vlformat_32to40) \
EXPANDER(pedestal) \
EXPANDER(green_imbalance) \
EXPANDER(green_imbalance_green_red_coeff_mem) \
EXPANDER(green_imbalance_green_blue_coeff_mem) \
EXPANDER(dead_pixel_correction) \
EXPANDER(dead_pixel_correction_list_mem) \
EXPANDER(denoising) \
EXPANDER(statistics_bayer) \
EXPANDER(lens_shading_correction) \
EXPANDER(lens_shading_correction_red_coeff_mem) \
EXPANDER(lens_shading_correction_green_coeff_mem) \
EXPANDER(lens_shading_correction_blue_coeff_mem) \
EXPANDER(chromatic_aberration) \
EXPANDER(bayer) \
EXPANDER(color_correction) \
EXPANDER(vlformat_40to32) \
EXPANDER(gamma_corrector) \
EXPANDER(gamma_corrector_ry_lut) \
EXPANDER(gamma_corrector_gu_lut) \
EXPANDER(gamma_corrector_bv_lut) \
EXPANDER(chroma) \
EXPANDER(statistics_yuv) \
EXPANDER(statistics_yuv_ae_histogram_y) \
EXPANDER(chain_yuv_inter) \
EXPANDER(edge_enhancement_color_reduction_filter) \
EXPANDER(edge_enhancement_color_reduction_filter_ee_lut) \
EXPANDER(i3d_lut) \
EXPANDER(i3d_lut_lut_outside) \
EXPANDER(i3d_lut_lut_inside) \
EXPANDER(drop)
EXPANDER(chain_bayer_inter) \
EXPANDER(vlformat_32to40) \
EXPANDER(pedestal) \
EXPANDER(green_imbalance) \
EXPANDER(green_imbalance_green_red_coeff_mem) \
EXPANDER(green_imbalance_green_blue_coeff_mem) \
EXPANDER(dead_pixel_correction) \
EXPANDER(dead_pixel_correction_list_mem) \
EXPANDER(denoising) \
EXPANDER(statistics_bayer) \
EXPANDER(lens_shading_correction) \
EXPANDER(lens_shading_correction_red_coeff_mem) \
EXPANDER(lens_shading_correction_green_coeff_mem) \
EXPANDER(lens_shading_correction_blue_coeff_mem) \
EXPANDER(chromatic_aberration) \
EXPANDER(bayer) \
EXPANDER(color_correction) \
EXPANDER(vlformat_40to32) \
EXPANDER(gamma_corrector) \
EXPANDER(gamma_corrector_ry_lut) \
EXPANDER(gamma_corrector_gu_lut) \
EXPANDER(gamma_corrector_bv_lut) \
EXPANDER(chroma) \
EXPANDER(statistics_yuv) \
EXPANDER(statistics_yuv_ae_histogram_y) \
EXPANDER(chain_yuv_inter) \
EXPANDER(edge_enhancement_color_reduction_filter) \
EXPANDER(edge_enhancement_color_reduction_filter_ee_lut) \
EXPANDER(i3d_lut) \
EXPANDER(i3d_lut_lut_outside) \
EXPANDER(i3d_lut_lut_inside) \
EXPANDER(drop)
#define EXPAND_AS_ENUM(_node) \
_node,
_node,
enum {
AVI_DEFINE_NODE(EXPAND_AS_ENUM)
ISP_NODE_NR,
AVI_DEFINE_NODE(EXPAND_AS_ENUM)
ISP_NODE_NR,
};
/* ISP Context */
struct libisp_context
{
int devmem;
unsigned long avi_base;
unsigned offsets[ISP_NODE_NR];
struct libisp_context {
int devmem;
unsigned long avi_base;
unsigned offsets[ISP_NODE_NR];
};
/* Configuration of ISP */
struct libisp_config {
struct avi_isp_vlformat_32to40_regs vlformat_32to40; ///< Conversion factor (10bit to 10bit default)
struct avi_isp_chain_bayer_inter_regs bayer_inter; ///< Enable or disable bayer ISP functions by bypassing them
struct avi_isp_pedestal_regs pedestal; ///< Pedestral parameters (substract from pixels)
struct avi_isp_green_imbalance_regs green_imbalance; ///< Green imbalance correction
struct avi_isp_green_imbalance_green_red_coeff_mem_regs grim_gr; ///< Green imbalance GR coefficients
struct avi_isp_green_imbalance_green_blue_coeff_mem_regs grim_gb; ///< Green imbalance GB coefficients
struct avi_isp_dead_pixel_correction_regs dead_pixel_correction; ///< Dead pixel correction (disabled)
struct avi_isp_denoising_regs denoising; ///< Denoising parameters
struct avi_isp_statistics_bayer_regs statistics_bayer; ///< Statistics bayer parameters
struct avi_isp_lens_shading_correction_regs lens_shading_correction; ///< Lens shade correction
struct avi_isp_lens_shading_correction_red_coeff_mem_regs
lsc_red_coeffs; ///< Lens shade correction red coefficients
struct avi_isp_lens_shading_correction_green_coeff_mem_regs
lsc_green_coeffs; ///< Lens shade correction green coefficients
struct avi_isp_lens_shading_correction_blue_coeff_mem_regs
lsc_blue_coeffs; ///< Lens shade correction blue coefficients
/*struct avi_isp_chromatic_aberration_regs chromatic_aberration;*/ ///< Chromatic abberation (Disabled for now)
struct avi_isp_bayer_regs bayer; ///< Demosaicking parameters
struct avi_isp_color_correction_regs color_correction; ///< Color correction parameters
struct avi_isp_vlformat_40to32_regs vlformat_40to32; ///< Conversion factor (10bit to 10bit default)
struct avi_isp_gamma_corrector_regs gamma_corrector; ///< Gamma corrector (Curves)
struct avi_isp_gamma_corrector_ry_lut_regs gc_ry_lut; ///< Gamma corrector RY lut
struct avi_isp_gamma_corrector_gu_lut_regs gc_gu_lut; ///< Gamma corrector GU lut
struct avi_isp_gamma_corrector_bv_lut_regs gc_bv_lut; ///< Gamma corrector BV lut
struct avi_isp_chroma_regs chroma; ///< Color space conversion
struct avi_isp_statistics_yuv_regs statistics_yuv; ///< YUV statistics parameters
struct avi_isp_edge_enhancement_color_reduction_filter_regs eecrf; ///< Edge enhancement + Color reduction
struct avi_isp_edge_enhancement_color_reduction_filter_ee_lut_regs
eecrf_lut; ///< Edge enhancement + Color correction lut
struct avi_isp_chain_yuv_inter_regs chain_yuv_inter; ///< YUV chain bypass configuration (enable/disable features)
};
/* Output YUV statistics */
struct isp_yuv_stats_t {
uint32_t awb_sum_Y;
uint32_t awb_sum_U;
uint32_t awb_sum_V;
uint32_t awb_nb_grey_pixels;
uint32_t nb_valid_Y;
uint32_t ae_histogram_Y[256];
};
int configure_isp(struct v4l2_device *dev);
int isp_get_statistics_yuv(struct isp_yuv_stats_t *yuv_stats);
/* Registers access */
#define EXPAND_AS_PROTOTYPE(_node) \
void avi_isp_ ## _node ## _set_registers(struct libisp_context *, \
struct avi_isp_ ## _node ## _regs const *regs); \
void avi_isp_ ## _node ## _get_registers(struct libisp_context *, \
struct avi_isp_ ## _node ## _regs *regs);
void avi_isp_ ## _node ## _set_registers(struct libisp_context *, \
struct avi_isp_ ## _node ## _regs const *regs); \
void avi_isp_ ## _node ## _get_registers(struct libisp_context *, \
struct avi_isp_ ## _node ## _regs *regs);
AVI_DEFINE_NODE(EXPAND_AS_PROTOTYPE)
sw/airborne/modules/computer_vision/lib/isp/libisp_config.h
+500
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/computer_vision/viewvideo.c
+51 -46
View File
@@ -48,7 +48,6 @@
#include BOARD_CONFIG
// Downsize factor for video stream
#ifndef VIEWVIDEO_DOWNSIZE_FACTOR
#define VIEWVIDEO_DOWNSIZE_FACTOR 4
@@ -67,19 +66,9 @@ PRINT_CONFIG_VAR(VIEWVIDEO_QUALITY_FACTOR)
#endif
PRINT_CONFIG_VAR(VIEWVIDEO_RTP_TIME_INC)
// Default image folder
#ifndef VIEWVIDEO_SHOT_PATH
#ifdef VIDEO_THREAD_SHOT_PATH
#define VIEWVIDEO_SHOT_PATH VIDEO_THREAD_SHOT_PATH
#else
#define VIEWVIDEO_SHOT_PATH /data/video/images
#endif
#endif
PRINT_CONFIG_VAR(VIEWVIDEO_SHOT_PATH)
// Define stream framerate
#ifndef VIEWVIDEO_FPS
#define VIEWVIDEO_FPS 10
#define VIEWVIDEO_FPS 5
#endif
PRINT_CONFIG_VAR(VIEWVIDEO_FPS)
@@ -102,7 +91,8 @@ PRINT_CONFIG_VAR(VIEWVIDEO_NICE_LEVEL)
#include <sys/wait.h>
PRINT_CONFIG_MSG("[viewvideo] Using netcat.")
#else
struct UdpSocket video_sock;
struct UdpSocket video_sock1;
struct UdpSocket video_sock2;
PRINT_CONFIG_MSG("[viewvideo] Using RTP/UDP stream.")
PRINT_CONFIG_VAR(VIEWVIDEO_USE_RTP)
#endif
@@ -110,6 +100,7 @@ PRINT_CONFIG_VAR(VIEWVIDEO_USE_RTP)
/* These are defined with configure */
PRINT_CONFIG_VAR(VIEWVIDEO_HOST)
PRINT_CONFIG_VAR(VIEWVIDEO_PORT_OUT)
PRINT_CONFIG_VAR(VIEWVIDEO_PORT2_OUT)
// Initialize the viewvideo structure with the defaults
struct viewvideo_t viewvideo = {
@@ -123,10 +114,9 @@ struct viewvideo_t viewvideo = {
/**
* Handles all the video streaming and saving of the image shots
* This is a sepereate thread, so it needs to be thread safe!
* This is a separate thread, so it needs to be thread safe!
*/
struct image_t *viewvideo_function(struct image_t *img);
struct image_t *viewvideo_function(struct image_t *img)
static struct image_t *viewvideo_function(struct UdpSocket *socket, struct image_t *img, uint32_t *rtp_frame_nr)
{
// Resize image if needed
struct image_t img_small;
@@ -178,27 +168,18 @@ struct image_t *viewvideo_function(struct image_t *img)
}
#else
if (viewvideo.use_rtp) {
// Send image with RTP
rtp_frame_send(
&video_sock, // UDP socket
socket, // UDP socket
&img_jpeg,
0, // Format 422
VIEWVIDEO_QUALITY_FACTOR, // Jpeg-Quality
0, // DRI Header
VIEWVIDEO_RTP_TIME_INC // 90kHz time increment
(img->ts.tv_sec * 1000000 + img->ts.tv_usec),
rtp_frame_nr
);
// Extra note: when the time increment is set to 0,
// it is automaticaly calculated by the send_rtp_frame function
// based on gettimeofday value. This seems to introduce some lag or jitter.
// An other way is to compute the time increment and set the correct value.
// It seems that a lower value is also working (when the frame is received
// the timestamp is always "late" so the frame is displayed immediately).
// Here, we set the time increment to the lowest possible value
// (1 = 1/90000 s) which is probably stupid but is actually working.
}
#endif
}
// Free all buffers
@@ -207,16 +188,27 @@ struct image_t *viewvideo_function(struct image_t *img)
return NULL; // No new images were created
}
#ifdef VIEWVIDEO_CAMERA
static struct image_t *viewvideo_function1(struct image_t *img)
{
static uint32_t rtp_frame_nr = 0;
return viewvideo_function(&video_sock1, img, &rtp_frame_nr);
}
#endif
#ifdef VIEWVIDEO_CAMERA2
static struct image_t *viewvideo_function2(struct image_t *img)
{
static uint32_t rtp_frame_nr = 0;
return viewvideo_function(&video_sock2, img, &rtp_frame_nr);
}
#endif
/**
* Initialize the view video
*/
void viewvideo_init(void)
{
char save_name[512];
struct video_listener *listener = cv_add_to_device_async(&VIEWVIDEO_CAMERA, viewvideo_function, VIEWVIDEO_NICE_LEVEL);
listener->maximum_fps = VIEWVIDEO_FPS;
viewvideo.is_streaming = true;
#if VIEWVIDEO_USE_NETCAT
@@ -237,19 +229,32 @@ void viewvideo_init(void)
}
#else
// Open udp socket
udp_socket_create(&video_sock, STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT_OUT, -1, VIEWVIDEO_BROADCAST);
// Create an SDP file for the streaming
sprintf(save_name, "%s/stream.sdp", STRINGIFY(VIEWVIDEO_SHOT_PATH));
FILE *fp = fopen(save_name, "w");
if (fp != NULL) {
fprintf(fp, "v=0\n");
fprintf(fp, "m=video %d RTP/AVP 26\n", (int)(VIEWVIDEO_PORT_OUT));
fprintf(fp, "c=IN IP4 0.0.0.0\n");
fclose(fp);
} else {
printf("[viewvideo] Failed to create SDP file.\n");
#ifdef VIEWVIDEO_CAMERA
if (udp_socket_create(&video_sock1, STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT_OUT, -1, VIEWVIDEO_BROADCAST)) {
printf("[viewvideo]: failed to open view video socket, HOST=%s, port=%d\n", STRINGIFY(VIEWVIDEO_HOST),
VIEWVIDEO_PORT_OUT);
}
#endif
}
#ifdef VIEWVIDEO_CAMERA2
if (udp_socket_create(&video_sock2, STRINGIFY(VIEWVIDEO_HOST), VIEWVIDEO_PORT2_OUT, -1, VIEWVIDEO_BROADCAST)) {
printf("[viewvideo]: failed to open view video socket, HOST=%s, port=%d\n", STRINGIFY(VIEWVIDEO_HOST),
VIEWVIDEO_PORT2_OUT);
}
#endif
#endif
#ifdef VIEWVIDEO_CAMERA
struct video_listener *listener1 = cv_add_to_device_async(&VIEWVIDEO_CAMERA, viewvideo_function1,
VIEWVIDEO_NICE_LEVEL);
listener1->maximum_fps = VIEWVIDEO_FPS;
fprintf(stderr, "[viewvideo] Added asynchronous video streamer lister for CAMERA1\n");
#endif
#ifdef VIEWVIDEO_CAMERA2
struct video_listener *listener2 = cv_add_to_device_async(&VIEWVIDEO_CAMERA2, viewvideo_function2,
VIEWVIDEO_NICE_LEVEL);
listener2->maximum_fps = VIEWVIDEO_FPS;
fprintf(stderr, "[viewvideo] Added asynchronous video streamer lister for CAMERA2\n");
#endif
}
sw/tools/rtp_viewer/rtp_stream.sdp → sw/tools/rtp_viewer/rtp_5000.sdp
View File
sw/tools/rtp_viewer/rtp_6000.sdp
+3
View File
@@ -0,0 +1,3 @@
v=0
m=video 6000 RTP/AVP 26
c=IN IP4 0.0.0.0
sw/tools/rtp_viewer/rtp_viewer.py
+22 -3
View File
@@ -2,6 +2,7 @@
import cv2
import sys
import argparse
from os import path, getenv
PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../')))
@@ -10,9 +11,9 @@ sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python")
from pprzlink.ivy import IvyMessagesInterface
from pprzlink.message import PprzMessage
class RtpViewer:
running = False
scale = 1
rotate = 0
frame = None
mouse = dict()
@@ -60,6 +61,10 @@ class RtpViewer:
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'], (0, 255, 0), 2)
if self.scale != 1:
h, w = self.frame.shape[:2]
self.frame = cv2.resize(self.frame, (int(self.scale * w), int(self.scale * h)))
# Show the image in a window
cv2.imshow('rtp', self.frame)
@@ -110,11 +115,25 @@ class RtpViewer:
if __name__ == '__main__':
viewer = RtpViewer("rtp_stream.sdp")
import sys
import os
parser = argparse.ArgumentParser()
parser.add_argument("-p", "--port", type=int, default=5000, help="The port number to open for the RTP stream (5000 or 6000)")
parser.add_argument("-s", "--scale", type=float, default=1., help="The scaling factor to apply to the incoming video stream (default: 1)")
parser.add_argument("-r", "--rotate", type=int, default=0, help="The number of clockwise 90deg rotations to apply to the stream [0-3] (default: 0)")
args = parser.parse_args()
filename = os.path.dirname(os.path.abspath(__file__)) + "/rtp_" + str(args.port) + ".sdp"
viewer = RtpViewer(filename)
viewer.scale = args.scale
viewer.rotate = args.rotate
if not viewer.cap.isOpened():
viewer.cleanup()
sys.exit("Can't open video stream")
raise IOError("Can't open video stream")
viewer.run()
viewer.cleanup()