diff --git a/conf/modules/bebop_front_camera.xml b/conf/modules/bebop_front_camera.xml new file mode 100644 index 0000000000..2990f8343d --- /dev/null +++ b/conf/modules/bebop_front_camera.xml @@ -0,0 +1,61 @@ + + + + + + Video streaming for the Bebop front camera + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + BEBOP_FRONT_CAMERA_HOST ?= $(MODEM_HOST) + BEBOP_FRONT_CAMERA_PORT_OUT ?= 6000 + BEBOP_FRONT_CAMERA_BROADCAST ?= $(MODEM_BROADCAST) + + 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_NC),) + ap.CFLAGS += $(BEBOPVIEWVID_CFLAGS) -DBEBOP_FRONT_CAMERA_BROADCAST=$(BEBOP_FRONT_CAMERA_BROADCAST) + else + ap.CFLAGS += $(BEBOPVIEWVID_CFLAGS) -DBEBOP_FRONT_CAMERA_USE_NC + endif + + + + + + + + diff --git a/sw/airborne/boards/bebop/board.c b/sw/airborne/boards/bebop/board.c index 75320d3d3e..432a53ed2c 100644 --- a/sw/airborne/boards/bebop/board.c +++ b/sw/airborne/boards/bebop/board.c @@ -36,5 +36,5 @@ void board_init(void) // We also try to initialize the video CMOS chips here (Bottom and front) mt9v117_init(); - //mt9f002_init(); + mt9f002_init(); } diff --git a/sw/airborne/boards/bebop/mt9f002.h b/sw/airborne/boards/bebop/mt9f002.h new file mode 100644 index 0000000000..67fee7c926 --- /dev/null +++ b/sw/airborne/boards/bebop/mt9f002.h @@ -0,0 +1,374 @@ +#ifndef MT9F002_H +#define MT9F002_H + +#define MT9F002_NUM_OF_REGISTERS 367 + +#define MT9F002_MODEL_ID 0x0000 +#define MT9F002_REVISION_NUMBER 0x0002 +#define MT9F002_MANUFACTURER_ID 0x0003 +#define MT9F002_SMIA_VERSION 0x0004 +#define MT9F002_FRAME_COUNT 0x0005 +#define MT9F002_PIXEL_ORDER 0x0006 +#define MT9F002_DATA_PEDESTAL 0x0008 +#define MT9F002_FRAME_FORMAT_MODEL_TYPE 0x0040 +#define MT9F002_FRAME_FORMAT_MODEL_SUBTYPE 0x0041 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_0 0x0042 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_1 0x0044 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_2 0x0046 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_3 0x0048 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_4 0x004A +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_5 0x004C +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_6 0x004E +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_7 0x0050 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_8 0x0052 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_9 0x0054 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_10 0x0056 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_11 0x0058 +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_12 0x005A +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_13 0x005C +#define MT9F002_FRAME_FORMAT_DESCRIPTOR_14 0x005E +#define MT9F002_ANALOG_GAIN_CAPABILITY 0x0080 +#define MT9F002_ANALOG_GAIN_CODE_MIN 0x0084 +#define MT9F002_ANALOG_GAIN_CODE_MAX 0x0086 +#define MT9F002_ANALOG_GAIN_CODE_STEP 0x0088 +#define MT9F002_ANALOG_GAIN_TYPE 0x008A +#define MT9F002_ANALOG_GAIN_M0 0x008C +#define MT9F002_ANALOG_GAIN_C0 0x008E +#define MT9F002_ANALOG_GAIN_M1 0x0090 +#define MT9F002_ANALOG_GAIN_C1 0x0092 +#define MT9F002_DATA_FORMAT_MODEL_TYPE 0x00C0 +#define MT9F002_DATA_FORMAT_MODEL_SUBTYPE 0x00C1 +#define MT9F002_DATA_FORMAT_DESCRIPTOR_0 0x00C2 +#define MT9F002_DATA_FORMAT_DESCRIPTOR_1 0x00C4 +#define MT9F002_DATA_FORMAT_DESCRIPTOR_2 0x00C6 +#define MT9F002_DATA_FORMAT_DESCRIPTOR_3 0x00C8 +#define MT9F002_DATA_FORMAT_DESCRIPTOR_4 0x00CA +#define MT9F002_DATA_FORMAT_DESCRIPTOR_5 0x00CC +#define MT9F002_DATA_FORMAT_DESCRIPTOR_6 0x00CE +#define MT9F002_MODE_SELECT 0x0100 +#define MT9F002_IMAGE_ORIENTATION 0x0101 +#define MT9F002_SOFTWARE_RESET 0x0103 +#define MT9F002_GROUPED_PARAMETER_HOLD 0x0104 +#define MT9F002_MASK_CORRUPTED_FRAMES 0x0105 +#define MT9F002_CPP_CHANNEL_IDENTIFIER 0x0110 +#define MT9F002_CPP2_SIGNALLING_MODE 0x0111 +#define MT9F002_CPP_DATA_FORMAT 0x0112 +#define MT9F002_GAIN_MODE 0x0120 +#define MT9F002_FINE_INTEGRATION_TIME 0x0200 +#define MT9F002_COARSE_INTEGRATION_TIME 0x0202 +#define MT9F002_ANALOG_GAIN_CODE_GLOBAL 0x0204 +#define MT9F002_ANALOG_GAIN_CODE_GREENR 0x0206 +#define MT9F002_ANALOG_GAIN_CODE_RED 0x0208 +#define MT9F002_ANALOG_GAIN_CODE_BLUE 0x020A +#define MT9F002_ANALOG_GAIN_CODE_GREENB 0x020C +#define MT9F002_DIGITAL_GAIN_GREENR 0x020E +#define MT9F002_DIGITAL_GAIN_RED 0x0210 +#define MT9F002_DIGITAL_GAIN_BLUE 0x0212 +#define MT9F002_DIGITAL_GAIN_GREENB 0x0214 +#define MT9F002_VT_PIX_CLK_DIV 0x0300 +#define MT9F002_VT_SYS_CLK_DIV 0x0302 +#define MT9F002_PRE_PLL_CLK_DIV 0x0304 +#define MT9F002_PLL_MULTIPLIER 0x0306 +#define MT9F002_OP_PIX_CLK_DIV 0x0308 +#define MT9F002_OP_SYS_CLK_DIV 0x030A +#define MT9F002_FRAME_LENGTH_LINES 0x0340 +#define MT9F002_LINE_LENGTH_PCK 0x0342 +#define MT9F002_X_ADDR_START 0x0344 +#define MT9F002_Y_ADDR_START 0x0346 +#define MT9F002_X_ADDR_END 0x0348 +#define MT9F002_Y_ADDR_END 0x034A +#define MT9F002_X_OUTPUT_SIZE 0x034C +#define MT9F002_Y_OUTPUT_SIZE 0x034E +#define MT9F002_X_EVEN_INC 0x0380 +#define MT9F002_X_ODD_INC 0x0382 +#define MT9F002_Y_EVEN_INC 0x0384 +#define MT9F002_Y_ODD_INC 0x0386 +#define MT9F002_SCALING_MODE 0x0400 +#define MT9F002_SPATIAL_SAMPLING 0x0402 +#define MT9F002_SCALE_M 0x0404 +#define MT9F002_SCALE_N 0x0406 +#define MT9F002_COMPRESSION_MODE 0x0500 +#define MT9F002_TEST_PATTERN_MODE 0x0600 +#define MT9F002_TEST_DATA_RED 0x0602 +#define MT9F002_TEST_DATA_GREENR 0x0604 +#define MT9F002_TEST_DATA_BLUE 0x0606 +#define MT9F002_TEST_DATA_GREENB 0x0608 +#define MT9F002_HORIZONTAL_CURSOR_WIDTH 0x060A +#define MT9F002_HORIZONTAL_CURSOR_POSITION 0x060C +#define MT9F002_VERTICAL_CURSOR_WIDTH 0x060E +#define MT9F002_VERTICAL_CURSOR_POSITION 0x0610 +#define MT9F002_INTEGRATION_TIME_CAPABILITY 0x1000 +#define MT9F002_COARSE_INTEGRATION_TIME_MIN 0x1004 +#define MT9F002_COARSE_INTEGRATION_TIME_MAX_MARGIN 0x1006 +#define MT9F002_FINE_INTEGRATION_TIME_MIN 0x1008 +#define MT9F002_FINE_INTEGRATION_TIME_MAX_MARGIN 0x100A +#define MT9F002_DIGITAL_GAIN_CAPABILITY 0x1080 +#define MT9F002_DIGITAL_GAIN_MIN 0x1084 +#define MT9F002_DIGITAL_GAIN_MAX 0x1086 +#define MT9F002_DIGITAL_GAIN_STEP_SIZE 0x1088 +#define MT9F002_MIN_EXT_CLK_FREQ_MHZ 0x1100 +#define MT9F002_MAX_EXT_CLK_FREQ_MHZ 0x1104 +#define MT9F002_MIN_PRE_PLL_CLK_DIV 0x1108 +#define MT9F002_MAX_PRE_PLL_CLK_DIV 0x110A +#define MT9F002_MIN_PLL_IP_FREQ_MHZ 0x110C +#define MT9F002_MAX_PLL_IP_FREQ_MHZ 0x1110 +#define MT9F002_MIN_PLL_MULTIPLIER 0x1114 +#define MT9F002_MAX_PLL_MULTIPLIER 0x1116 +#define MT9F002_MIN_PLL_OP_FREQ_MHZ 0x1118 +#define MT9F002_MAX_PLL_OP_FREQ_MHZ 0x111C +#define MT9F002_MIN_VT_SYS_CLK_DIV 0x1120 +#define MT9F002_MAX_VT_SYS_CLK_DIV 0x1122 +#define MT9F002_MIN_VT_SYS_CLK_FREQ_MHZ 0x1124 +#define MT9F002_MAX_VT_SYS_CLK_FREQ_MHZ 0x1128 +#define MT9F002_MIN_VT_PIX_CLK_FREQ_MHZ 0x112C +#define MT9F002_MAX_VT_PIX_CLK_FREQ_MHZ 0x1130 +#define MT9F002_MIN_VT_PIX_CLK_DIV 0x1134 +#define MT9F002_MAX_VT_PIX_CLK_DIV 0x1136 +#define MT9F002_MIN_FRAME_LENGTH_LINES 0x1140 +#define MT9F002_MAX_FRAME_LENGTH_LINES 0x1142 +#define MT9F002_MIN_LINE_LENGTH_PCK 0x1144 +#define MT9F002_MAX_LINE_LENGTH_PCK 0x1146 +#define MT9F002_MIN_LINE_BLANKING_PCK 0x1148 +#define MT9F002_MIN_FRAME_BLANKING_LINES 0x114A +#define MT9F002_MIN_OP_SYS_CLK_DIV 0x1160 +#define MT9F002_MAX_OP_SYS_CLK_DIV 0x1162 +#define MT9F002_MIN_OP_SYS_CLK_FREQ_MHZ 0x1164 +#define MT9F002_MAX_OP_SYS_CLK_FREQ_MHZ 0x1168 +#define MT9F002_MIN_OP_PIX_CLK_DIV 0x116C +#define MT9F002_MAX_OP_PIX_CLK_DIV 0x116E +#define MT9F002_MIN_OP_PIX_CLK_FREQ_MHZ 0x1170 +#define MT9F002_MAX_OP_PIX_CLK_FREQ_MHZ 0x1174 +#define MT9F002_X_ADDR_MIN 0x1180 +#define MT9F002_Y_ADDR_MIN 0x1182 +#define MT9F002_X_ADDR_MAX 0x1184 +#define MT9F002_Y_ADDR_MAX 0x1186 +#define MT9F002_MIN_EVEN_INC 0x11C0 +#define MT9F002_MAX_EVEN_INC 0x11C2 +#define MT9F002_MIN_ODD_INC 0x11C4 +#define MT9F002_MAX_ODD_INC 0x11C6 +#define MT9F002_SCALING_CAPABILITY 0x1200 +#define MT9F002_SCALER_M_MIN 0x1204 +#define MT9F002_SCALER_M_MAX 0x1206 +#define MT9F002_SCALER_N_MIN 0x1208 +#define MT9F002_SCALER_N_MAX 0x120A +#define MT9F002_COMPRESSION_CAPABILITY 0x1300 +#define MT9F002_MATRIX_ELEMENT_REDINRED 0x1400 +#define MT9F002_MATRIX_ELEMENT_GREENINRED 0x1402 +#define MT9F002_MATRIX_ELEMENT_BLUEINRED 0x1404 +#define MT9F002_MATRIX_ELEMENT_REDINGREEN 0x1406 +#define MT9F002_MATRIX_ELEMENT_GREENINGREEN 0x1408 +#define MT9F002_MATRIX_ELEMENT_BLUEINGREEN 0x140A +#define MT9F002_MATRIX_ELEMENT_REDINBLUE 0x140C +#define MT9F002_MATRIX_ELEMENT_GREENINBLUE 0x0140E +#define MT9F002_MATRIX_ELEMENT_BLUEINBLUE 0x1410 +#define MT9F002_MODEL_ID_ 0x3000 +#define MT9F002_Y_ADDR_START_ 0x3002 +#define MT9F002_X_ADDR_START_ 0x3004 +#define MT9F002_Y_ADDR_END_ 0x3006 +#define MT9F002_X_ADDR_END_ 0x3008 +#define MT9F002_FRAME_LENGTH_LINES_ 0x300A +#define MT9F002_LINE_LENGTH_PCK_ 0x300C +#define MT9F002_FINE_CORRECTION 0x3010 +#define MT9F002_COARSE_INTEGRATION_TIME_ 0x3012 +#define MT9F002_FINE_INTEGRATION_TIME_ 0x3014 +#define MT9F002_ROW_SPEED 0x3016 +#define MT9F002_EXTRA_DELAY 0x3018 +#define MT9F002_RESET_REGISTER 0x301A +#define MT9F002_MODE_SELECT_ 0x301C +#define MT9F002_IMAGE_ORIENTATION_ 0x301D +#define MT9F002_DATA_PEDESTAL_ 0x301E +#define MT9F002_SOFTWARE_RESET_ 0x3021 +#define MT9F002_GROUPED_PARAMETER_HOLD_ 0x3022 +#define MT9F002_MASK_CORRUPTED_FRAMES_ 0x3023 +#define MT9F002_PIXEL_ORDER_ 0x3024 +#define MT9F002_GPI_STATUS 0x3026 +#define MT9F002_ANALOG_GAIN_CODE_GLOBAL_ 0x3028 +#define MT9F002_ANALOG_GAIN_CODE_GREENR_ 0x302A +#define MT9F002_ANALOG_GAIN_CODE_RED_ 0x302C +#define MT9F002_ANALOG_GAIN_CODE_BLUE_ 0x302E +#define MT9F002_ANALOG_GAIN_CODE_GREENB_ 0x3030 +#define MT9F002_DIGITAL_GAIN_GREENR_ 0x3032 +#define MT9F002_DIGITAL_GAIN_RED_ 0x3036 +#define MT9F002_DIGITAL_GAIN_BLUE_ 0x3038 +#define MT9F002_DIGITAL_GAIN_GREENB_ 0x3038 +#define MT9F002_SMIA_VERSION_ 0x303A +#define MT9F002_FRAME_COUNT_ 0x303B +#define MT9F002_FRAME_STATUS 0x303C +#define MT9F002_READ_MODE 0x3040 +#define MT9F002_FLASH 0x3046 +#define MT9F002_FLASH_COUNT 0x3048 +#define MT9F002_GREEN1_GAIN 0x3056 +#define MT9F002_BLUE_GAIN 0x3058 +#define MT9F002_RED_GAIN 0x305A +#define MT9F002_GREEN2_GAIN 0x305C +#define MT9F002_GLOBAL_GAIN 0x305E +#define MT9F002_DATAPATH_STATUS 0x306A +#define MT9F002_DATAPATH_SELECT 0x306E +#define MT9F002_TEST_PATTERN_MODE_ 0x3070 +#define MT9F002_TEST_DATA_RED_ 0x3072 +#define MT9F002_TEST_DATA_GREENR_ 0x3074 +#define MT9F002_TEST_DATA_BLUE_ 0x3076 +#define MT9F002_TEST_DATA_GREENB_ 0x3078 +#define MT9F002_TEST_RAW_MODE 0x307A +#define MT9F002_X_EVEN_INC_ 0x30A0 +#define MT9F002_X_ODD_INC_ 0x30A2 +#define MT9F002_Y_EVEN_INC_ 0x30A4 +#define MT9F002_Y_ODD_INC_ 0x30A6 +#define MT9F002_CALIB_GREEN1_ASC1 0x30A8 +#define MT9F002_CALIB_BLUE_ASC1 0x30AA +#define MT9F002_CALIB_RED_ASC1 0x30AC +#define MT9F002_CALIB_GREEN2_ASC1 0x30AE +#define MT9F002_CALIB_GLOBAL 0x30BC +#define MT9F002_CALIB_CONTROL 0x30C0 +#define MT9F002_CALIB_GREEN1 0x30C2 +#define MT9F002_CALIB_BLUE 0x30C4 +#define MT9F002_CALIB_RED 0x30C6 +#define MT9F002_CALIB_GREEN2 0x30C8 +#define MT9F002_CTX_CONTROL_REG 0x30E8 +#define MT9F002_CTX_WR_DATA_REG 0x30EA +#define MT9F002_CTX_RD_DATA_REG 0x30EC +#define MT9F002_DARK_CONTROL3 0x30EE +#define MT9F002_OTPM_TCFG_READ_4B 0x3138 +#define MT9F002_OTPM_CFG 0x3140 +#define MT9F002_GLOBAL_FLASH_START 0x315A +#define MT9F002_GLOBAL_SEQ_TRIGGER 0x315E +#define MT9F002_GLOBAL_RST_END 0x3160 +#define MT9F002_GLOBAL_SHUTTER_START1 0x3162 +#define MT9F002_GLOBAL_SHUTTER_START2 0x3164 +#define MT9F002_GLOBAL_READ_START 0x3166 +#define MT9F002_GLOBAL_READ_START2 0x3168 +#define MT9F002_DAC_RSTLO 0x316A +#define MT9F002_ANALOG_CONTROL5 0x3178 +#define MT9F002_SERIAL_FORMAT_DESCRIPTOR_0 0x31A0 +#define MT9F002_SERIAL_FORMAT_DESCRIPTOR_1 0x31A2 +#define MT9F002_SERIAL_FORMAT_DESCRIPTOR_2 0x31A4 +#define MT9F002_SERIAL_FORMAT_DESCRIPTOR_3 0x31A6 +#define MT9F002_SERIAL_FORMAT_DESCRIPTOR_4 0x31A8 +#define MT9F002_SERIAL_FORMAT_DESCRIPTOR_5 0x31AA +#define MT9F002_SERIAL_FORMAT_DESCRIPTOR_6 0x31AC +#define MT9F002_SERIAL_FORMAT 0x31AE +#define MT9F002_FRAME_PREAMBLE 0x31B0 +#define MT9F002_LINE_PREAMBLE 0x31B2 +#define MT9F002_MIPI_TIMING_0 0x31B4 +#define MT9F002_MIPI_TIMING_1 0x31B6 +#define MT9F002_MIPI_TIMING_2 0x31B8 +#define MT9F002_MIPI_TIMING_3 0x31BA +#define MT9F002_MIPI_TIMING_4 0x31BC +#define MT9F002_HISPI_TIMING 0x31C0 +#define MT9F002_HISPI_CONTROL_STATUS 0x31C6 +#define MT9F002_HORIZONTAL_CURSOR_POSITION_ 0x31E8 +#define MT9F002_VERTICAL_CURSOR_POSITION_ 0x31EA +#define MT9F002_HORIZONTAL_CURSOR_WIDTH_ 0x31EC +#define MT9F002_VERTICAL_CURSOR_WIDTH_ 0x31EE +#define MT9F002_I2C_IDS_MIPI_DEFAULT 0x31F2 +#define MT9F002_I2C_IDS 0x31FC +#define MT9F002_P_GR_P0Q0 0x3600 +#define MT9F002_P_GR_P0Q1 0x3602 +#define MT9F002_P_GR_P0Q2 0x3604 +#define MT9F002_P_GR_P0Q3 0x3606 +#define MT9F002_P_GR_P0Q4 0x3608 +#define MT9F002_P_RD_P0Q0 0x360A +#define MT9F002_P_RD_P0Q1 0x360C +#define MT9F002_P_RD_P0Q2 0x360E +#define MT9F002_P_RD_P0Q3 0x3610 +#define MT9F002_P_RD_P0Q4 0x3612 +#define MT9F002_P_BL_P0Q0 0x3614 +#define MT9F002_P_BL_P0Q1 0x3616 +#define MT9F002_P_BL_P0Q2 0x3618 +#define MT9F002_P_BL_P0Q3 0x361A +#define MT9F002_P_BL_P0Q4 0x361C +#define MT9F002_P_GB_P0Q0 0x361E +#define MT9F002_P_GB_P0Q1 0x3620 +#define MT9F002_P_GB_P0Q2 0x3622 +#define MT9F002_P_GB_P0Q3 0x3624 +#define MT9F002_P_GB_P0Q4 0x3626 +#define MT9F002_P_GR_P1Q0 0x3640 +#define MT9F002_P_GR_P1Q1 0x3642 +#define MT9F002_P_GR_P1Q2 0x3644 +#define MT9F002_P_GR_P1Q3 0x3646 +#define MT9F002_P_GR_P1Q4 0x3648 +#define MT9F002_P_RD_P1Q0 0x364A +#define MT9F002_P_RD_P1Q1 0x364C +#define MT9F002_P_RD_P1Q2 0x364E +#define MT9F002_P_RD_P1Q3 0x3650 +#define MT9F002_P_RD_P1Q4 0x3652 +#define MT9F002_P_BL_P1Q0 0x3654 +#define MT9F002_P_BL_P1Q1 0x3656 +#define MT9F002_P_BL_P1Q2 0x3658 +#define MT9F002_P_BL_P1Q3 0x365A +#define MT9F002_P_BL_P1Q4 0x365C +#define MT9F002_P_GB_P1Q0 0x365E +#define MT9F002_P_GB_P1Q1 0x3660 +#define MT9F002_P_GB_P1Q2 0x3662 +#define MT9F002_P_GB_P1Q3 0x3664 +#define MT9F002_P_GB_P1Q4 0x3666 +#define MT9F002_P_GR_P2Q0 0x3680 +#define MT9F002_P_GR_P2Q1 0x3682 +#define MT9F002_P_GR_P2Q2 0x3684 +#define MT9F002_P_GR_P2Q3 0x3686 +#define MT9F002_P_GR_P2Q4 0x3688 +#define MT9F002_P_RD_P2Q0 0x368A +#define MT9F002_P_RD_P2Q1 0x368C +#define MT9F002_P_RD_P2Q2 0x368E +#define MT9F002_P_RD_P2Q3 0x3690 +#define MT9F002_P_RD_P2Q4 0x3692 +#define MT9F002_P_BL_P2Q0 0x3694 +#define MT9F002_P_BL_P2Q1 0x3696 +#define MT9F002_P_BL_P2Q2 0x3698 +#define MT9F002_P_BL_P2Q3 0x369A +#define MT9F002_P_BL_P2Q4 0x369C +#define MT9F002_P_GB_P2Q0 0x369E +#define MT9F002_P_GB_P2Q1 0x36A0 +#define MT9F002_P_GB_P2Q2 0x36A2 +#define MT9F002_P_GB_P2Q3 0x36A4 +#define MT9F002_P_GB_P2Q4 0x36A6 +#define MT9F002_P_GR_P3Q0 0x36C0 +#define MT9F002_P_GR_P3Q1 0x36C2 +#define MT9F002_P_GR_P3Q2 0x36C4 +#define MT9F002_P_GR_P3Q3 0x36C6 +#define MT9F002_P_GR_P3Q4 0x36C8 +#define MT9F002_P_RD_P3Q0 0x36CA +#define MT9F002_P_RD_P3Q1 0x36CC +#define MT9F002_P_RD_P3Q2 0x36CE +#define MT9F002_P_RD_P3Q3 0x36D0 +#define MT9F002_P_RD_P3Q4 0x36D2 +#define MT9F002_P_BL_P3Q0 0x36D4 +#define MT9F002_P_BL_P3Q1 0x36D6 +#define MT9F002_P_BL_P3Q2 0x36D8 +#define MT9F002_P_BL_P3Q3 0x36DA +#define MT9F002_P_BL_P3Q4 0x36DC +#define MT9F002_P_GB_P3Q0 0x36DE +#define MT9F002_P_GB_P3Q1 0x36E0 +#define MT9F002_P_GB_P3Q2 0x36E2 +#define MT9F002_P_GB_P3Q3 0x36E4 +#define MT9F002_P_GB_P3Q4 0x36E6 +#define MT9F002_P_GR_P4Q0 0x3700 +#define MT9F002_P_GR_P4Q1 0x3702 +#define MT9F002_P_GR_P4Q2 0x3704 +#define MT9F002_P_GR_P4Q3 0x3706 +#define MT9F002_P_GR_P4Q4 0x3708 +#define MT9F002_P_RD_P4Q0 0x370A +#define MT9F002_P_RD_P4Q1 0x370C +#define MT9F002_P_RD_P4Q2 0x370E +#define MT9F002_P_RD_P4Q3 0x3710 +#define MT9F002_P_RD_P4Q4 0x3712 +#define MT9F002_P_BL_P4Q0 0x3714 +#define MT9F002_P_BL_P4Q1 0x3716 +#define MT9F002_P_BL_P4Q2 0x3718 +#define MT9F002_P_BL_P4Q3 0x371A +#define MT9F002_P_BL_P4Q4 0x371C +#define MT9F002_P_GB_P4Q0 0x371E +#define MT9F002_P_GB_P4Q1 0x3720 +#define MT9F002_P_GB_P4Q2 0x3722 +#define MT9F002_P_GB_P4Q3 0x3724 +#define MT9F002_P_GB_P4Q4 0x3726 +#define MT9F002_POLY_SC_ENABLE 0x3780 +#define MT9F002_POLY_ORIGIN_C 0x3782 +#define MT9F002_POLY_ORIGIN_R 0x3784 +#define MT9F002_P_GR_Q5 0x37C0 +#define MT9F002_P_RD_Q5 0x37C2 +#define MT9F002_P_BL_Q5 0x37C4 +#define MT9F002_P_GB_Q5 0x37C6 +#define MT9F002_DAC_ID_FBIAS 0x3EF8 + +#endif /* MT9F002_H */ diff --git a/sw/airborne/boards/bebop/video.c b/sw/airborne/boards/bebop/video.c index 3e52c7778f..4863542011 100644 --- a/sw/airborne/boards/bebop/video.c +++ b/sw/airborne/boards/bebop/video.c @@ -26,12 +26,16 @@ #include "video.h" #include "std.h" +#include "mt9f002.h" #include #include #include #include #include +#include +#include +#include static bool_t write_reg(int fd, char *addr_val, uint8_t cnt) { @@ -122,18 +126,42 @@ void mt9v117_init(void) _write(fd_i2c, "\x09\x82\x00\x01", 4); _write(fd_i2c, "\x09\x8a\x70\x00", 4); - _write(fd_i2c, "\xf0\x00\x72\xcf\xff\x00\x3e\xd0\x92\x00\x71\xcf\xff\xff\xf2\x18\xb1\x10\x92\x05\xb1\x11\x92\x04\xb1\x12\x70\xcf\xff\x00\x30\xc0\x90\x00\x7f\xe0\xb1\x13\x70\xcf\xff\xff\xe7\x1c\x88\x36\x09\x0f\x00\xb3", 50); - _write(fd_i2c, "\xf0\x30\x69\x13\xe1\x80\xd8\x08\x20\xca\x03\x22\x71\xcf\xff\xff\xe5\x68\x91\x35\x22\x0a\x1f\x80\xff\xff\xf2\x18\x29\x05\x00\x3e\x12\x22\x11\x01\x21\x04\x0f\x81\x00\x00\xff\xf0\x21\x8c\xf0\x10\x1a\x22", 50); - _write(fd_i2c, "\xf0\x60\x10\x44\x12\x20\x11\x02\xf7\x87\x22\x4f\x03\x83\x1a\x20\x10\xc4\xf0\x09\xba\xae\x7b\x50\x1a\x20\x10\x84\x21\x45\x01\xc1\x1a\x22\x10\x44\x70\xcf\xff\x00\x3e\xd0\xb0\x60\xb0\x25\x7e\xe0\x78\xe0", 50); - _write(fd_i2c, "\xf0\x90\x71\xcf\xff\xff\xf2\x18\x91\x12\x72\xcf\xff\xff\xe7\x1c\x8a\x57\x20\x04\x0f\x80\x00\x00\xff\xf0\xe2\x80\x20\xc5\x01\x61\x20\xc5\x03\x22\xb1\x12\x71\xcf\xff\x00\x3e\xd0\xb1\x04\x7e\xe0\x78\xe0", 50); - _write(fd_i2c, "\xf0\xc0\x70\xcf\xff\xff\xe7\x1c\x88\x57\x71\xcf\xff\xff\xf2\x18\x91\x13\xea\x84\xb8\xa9\x78\x10\xf0\x03\xb8\x89\xb8\x8c\xb1\x13\x71\xcf\xff\x00\x30\xc0\xb1\x00\x7e\xe0\xc0\xf1\x09\x1e\x03\xc0\xc1\xa1", 50); - _write(fd_i2c, "\xf0\xf0\x75\x08\x76\x28\x77\x48\xc2\x40\xd8\x20\x71\xcf\x00\x03\x20\x67\xda\x02\x08\xae\x03\xa0\x73\xc9\x0e\x25\x13\xc0\x0b\x5e\x01\x60\xd8\x06\xff\xbc\x0c\xce\x01\x00\xd8\x00\xb8\x9e\x0e\x5a\x03\x20", 50); - _write(fd_i2c, "\xf1\x20\xd9\x01\xd8\x00\xb8\x9e\x0e\xb6\x03\x20\xd9\x01\x8d\x14\x08\x17\x01\x91\x8d\x16\xe8\x07\x0b\x36\x01\x60\xd8\x07\x0b\x52\x01\x60\xd8\x11\x8d\x14\xe0\x87\xd8\x00\x20\xca\x02\x62\x00\xc9\x03\xe0", 50); - _write(fd_i2c, "\xf1\x50\xc0\xa1\x78\xe0\xc0\xf1\x08\xb2\x03\xc0\x76\xcf\xff\xff\xe5\x40\x75\xcf\xff\xff\xe5\x68\x95\x17\x96\x40\x77\xcf\xff\xff\xe5\x42\x95\x38\x0a\x0d\x00\x01\x97\x40\x0a\x11\x00\x40\x0b\x0a\x01\x00", 50); - _write(fd_i2c, "\xf1\x80\x95\x17\xb6\x00\x95\x18\xb7\x00\x76\xcf\xff\xff\xe5\x44\x96\x20\x95\x15\x08\x13\x00\x40\x0e\x1e\x01\x20\xd9\x00\x95\x15\xb6\x00\xff\xa1\x75\xcf\xff\xff\xe7\x1c\x77\xcf\xff\xff\xe5\x46\x97\x40", 50); - _write(fd_i2c, "\xf1\xb0\x8d\x16\x76\xcf\xff\xff\xe5\x48\x8d\x37\x08\x0d\x00\x81\x96\x40\x09\x15\x00\x80\x0f\xd6\x01\x00\x8d\x16\xb7\x00\x8d\x17\xb6\x00\xff\xb0\xff\xbc\x00\x41\x03\xc0\xc0\xf1\x0d\x9e\x01\x00\xe8\x04", 50); - _write(fd_i2c, "\xf1\xe0\xff\x88\xf0\x0a\x0d\x6a\x01\x00\x0d\x8e\x01\x00\xe8\x7e\xff\x85\x0d\x72\x01\x00\xff\x8c\xff\xa7\xff\xb2\xd8\x00\x73\xcf\xff\xff\xf2\x40\x23\x15\x00\x01\x81\x41\xe0\x02\x81\x20\x08\xf7\x81\x34", 50); - _write(fd_i2c, "\xf2\x10\xa1\x40\xd8\x00\xc0\xd1\x7e\xe0\x53\x51\x30\x34\x20\x6f\x6e\x5f\x73\x74\x61\x72\x74\x5f\x73\x74\x72\x65\x61\x6d\x69\x6e\x67\x20\x25\x64\x20\x25\x64\x0a\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 50); + _write(fd_i2c, + "\xf0\x00\x72\xcf\xff\x00\x3e\xd0\x92\x00\x71\xcf\xff\xff\xf2\x18\xb1\x10\x92\x05\xb1\x11\x92\x04\xb1\x12\x70\xcf\xff\x00\x30\xc0\x90\x00\x7f\xe0\xb1\x13\x70\xcf\xff\xff\xe7\x1c\x88\x36\x09\x0f\x00\xb3", + 50); + _write(fd_i2c, + "\xf0\x30\x69\x13\xe1\x80\xd8\x08\x20\xca\x03\x22\x71\xcf\xff\xff\xe5\x68\x91\x35\x22\x0a\x1f\x80\xff\xff\xf2\x18\x29\x05\x00\x3e\x12\x22\x11\x01\x21\x04\x0f\x81\x00\x00\xff\xf0\x21\x8c\xf0\x10\x1a\x22", + 50); + _write(fd_i2c, + "\xf0\x60\x10\x44\x12\x20\x11\x02\xf7\x87\x22\x4f\x03\x83\x1a\x20\x10\xc4\xf0\x09\xba\xae\x7b\x50\x1a\x20\x10\x84\x21\x45\x01\xc1\x1a\x22\x10\x44\x70\xcf\xff\x00\x3e\xd0\xb0\x60\xb0\x25\x7e\xe0\x78\xe0", + 50); + _write(fd_i2c, + "\xf0\x90\x71\xcf\xff\xff\xf2\x18\x91\x12\x72\xcf\xff\xff\xe7\x1c\x8a\x57\x20\x04\x0f\x80\x00\x00\xff\xf0\xe2\x80\x20\xc5\x01\x61\x20\xc5\x03\x22\xb1\x12\x71\xcf\xff\x00\x3e\xd0\xb1\x04\x7e\xe0\x78\xe0", + 50); + _write(fd_i2c, + "\xf0\xc0\x70\xcf\xff\xff\xe7\x1c\x88\x57\x71\xcf\xff\xff\xf2\x18\x91\x13\xea\x84\xb8\xa9\x78\x10\xf0\x03\xb8\x89\xb8\x8c\xb1\x13\x71\xcf\xff\x00\x30\xc0\xb1\x00\x7e\xe0\xc0\xf1\x09\x1e\x03\xc0\xc1\xa1", + 50); + _write(fd_i2c, + "\xf0\xf0\x75\x08\x76\x28\x77\x48\xc2\x40\xd8\x20\x71\xcf\x00\x03\x20\x67\xda\x02\x08\xae\x03\xa0\x73\xc9\x0e\x25\x13\xc0\x0b\x5e\x01\x60\xd8\x06\xff\xbc\x0c\xce\x01\x00\xd8\x00\xb8\x9e\x0e\x5a\x03\x20", + 50); + _write(fd_i2c, + "\xf1\x20\xd9\x01\xd8\x00\xb8\x9e\x0e\xb6\x03\x20\xd9\x01\x8d\x14\x08\x17\x01\x91\x8d\x16\xe8\x07\x0b\x36\x01\x60\xd8\x07\x0b\x52\x01\x60\xd8\x11\x8d\x14\xe0\x87\xd8\x00\x20\xca\x02\x62\x00\xc9\x03\xe0", + 50); + _write(fd_i2c, + "\xf1\x50\xc0\xa1\x78\xe0\xc0\xf1\x08\xb2\x03\xc0\x76\xcf\xff\xff\xe5\x40\x75\xcf\xff\xff\xe5\x68\x95\x17\x96\x40\x77\xcf\xff\xff\xe5\x42\x95\x38\x0a\x0d\x00\x01\x97\x40\x0a\x11\x00\x40\x0b\x0a\x01\x00", + 50); + _write(fd_i2c, + "\xf1\x80\x95\x17\xb6\x00\x95\x18\xb7\x00\x76\xcf\xff\xff\xe5\x44\x96\x20\x95\x15\x08\x13\x00\x40\x0e\x1e\x01\x20\xd9\x00\x95\x15\xb6\x00\xff\xa1\x75\xcf\xff\xff\xe7\x1c\x77\xcf\xff\xff\xe5\x46\x97\x40", + 50); + _write(fd_i2c, + "\xf1\xb0\x8d\x16\x76\xcf\xff\xff\xe5\x48\x8d\x37\x08\x0d\x00\x81\x96\x40\x09\x15\x00\x80\x0f\xd6\x01\x00\x8d\x16\xb7\x00\x8d\x17\xb6\x00\xff\xb0\xff\xbc\x00\x41\x03\xc0\xc0\xf1\x0d\x9e\x01\x00\xe8\x04", + 50); + _write(fd_i2c, + "\xf1\xe0\xff\x88\xf0\x0a\x0d\x6a\x01\x00\x0d\x8e\x01\x00\xe8\x7e\xff\x85\x0d\x72\x01\x00\xff\x8c\xff\xa7\xff\xb2\xd8\x00\x73\xcf\xff\xff\xf2\x40\x23\x15\x00\x01\x81\x41\xe0\x02\x81\x20\x08\xf7\x81\x34", + 50); + _write(fd_i2c, + "\xf2\x10\xa1\x40\xd8\x00\xc0\xd1\x7e\xe0\x53\x51\x30\x34\x20\x6f\x6e\x5f\x73\x74\x61\x72\x74\x5f\x73\x74\x72\x65\x61\x6d\x69\x6e\x67\x20\x25\x64\x20\x25\x64\x0a\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + 50); _write(fd_i2c, "\xf2\x40\xff\xff\xe8\x28\xff\xff\xf0\xe8\xff\xff\xe8\x08\xff\xff\xf1\x54", 18); _write(fd_i2c, "\x09\x8e\x00\x00", 4); _write(fd_i2c, "\xe0\x00\x05\xd8", 4); @@ -219,222 +247,274 @@ void mt9v117_init(void) close(fd_i2c); } +int mt9f002_i2c_port; + +void mt9f002_open(void); +void mt9f002_close(void); +void mt9f002_set_address(uint8_t address); +void mt9f002_write_reg8(uint16_t reg, uint8_t value); +void mt9f002_write_reg16(uint16_t reg, uint16_t value); +uint8_t mt9f002_read_reg8(uint16_t reg); +uint16_t mt9f002_read_reg16(uint16_t reg); + +void mt9f002_open(void) +{ + mt9f002_i2c_port = open("/dev/i2c-0", O_RDWR); + if (mt9f002_i2c_port < 0) { + printf("mt9f002_open"); + // exit(1); + } +} + +void mt9f002_close(void) +{ + close(mt9f002_i2c_port); +} + +void mt9f002_set_address(uint8_t address) +{ + if (ioctl(mt9f002_i2c_port, I2C_SLAVE_FORCE, address) < 0) { + printf("mt9f002_set_address"); + // exit(1); + } +} + +void mt9f002_write_reg8(uint16_t reg, uint8_t value) +{ + mt9f002_open(); + mt9f002_set_address(0x10); + + uint8_t data[3]; + data[0] = (uint8_t)(reg >> 8) ; + data[1] = (uint8_t)reg & 0xFF; + data[2] = value; + // if(i2c_smbus_write_byte_data(mt9f002_i2c_port,reg,value) < 0) { + if (write(mt9f002_i2c_port, data, 3) < 0) { + printf("mt9f002_write_reg8"); + // exit(1); + } + + mt9f002_close(); + + usleep(100); +} + +void mt9f002_write_reg16(uint16_t reg, uint16_t value) +{ + mt9f002_open(); + mt9f002_set_address(0x10); + + uint8_t data[4]; + data[0] = (uint8_t)(reg >> 8); + data[1] = (uint8_t)reg & 0xFF; + data[2] = (uint8_t)(value >> 8); + data[3] = value & 0xFF; + if (write(mt9f002_i2c_port, data, 4) < 0) { + printf("mt9f002_write_reg16"); + // exit(1); + } + + mt9f002_close(); + + usleep(100); +} + +uint8_t mt9f002_read_reg8(uint16_t reg) +{ + mt9f002_open(); + mt9f002_set_address(0x10); + + uint8_t data[2]; + data[0] = (uint8_t)(reg >> 8); + data[1] = (uint8_t)reg & 0xFF; + write(mt9f002_i2c_port, data, 2); + usleep(10); + read(mt9f002_i2c_port, data, 1); + + mt9f002_close(); + + return data[0]; +} + + +uint16_t mt9f002_read_reg16(uint16_t reg) +{ + mt9f002_open(); + mt9f002_set_address(0x10); + + uint8_t data[2]; + data[0] = (uint8_t)(reg >> 8); + data[1] = (uint8_t)reg & 0xFF; + write(mt9f002_i2c_port, data, 2); + usleep(10); + read(mt9f002_i2c_port, data, 2); + + mt9f002_close(); + + return data[1] | (data[0] << 8); +} + /** * Initialisation of the Aptina MT9F002 CMOS sensor */ void mt9f002_init(void) { - /* We open the i2c-0 (because else I needed to convert the strace) */ - int fd_i2c = open("/dev/i2c-0", O_RDWR); - if (fd_i2c < 0) { - printf("[MT9F002] Could not open i2c-0\n"); - return; - } - if (ioctl(fd_i2c, 0x706, 0x10) < 0) { - printf("[MT9V117] Could not change the i2c address to 0x10\n"); - return; - } + /* Change to standby mode */ + mt9f002_write_reg8(MT9F002_MODE_SELECT, 0x00); - /* First reset the device */ - write(fd_i2c, "\1\3\1", 3); - //usleep(1); + /* Change registers */ + mt9f002_write_reg16(MT9F002_P_GR_P0Q0, 0); + mt9f002_write_reg16(MT9F002_P_GR_P0Q2, 0); + mt9f002_write_reg16(MT9F002_P_GR_P0Q3, 0); + mt9f002_write_reg16(MT9F002_P_GR_P0Q4, 0); + mt9f002_write_reg16(MT9F002_P_RD_P0Q0, 0); + mt9f002_write_reg16(MT9F002_P_RD_P0Q1, 0); + mt9f002_write_reg16(MT9F002_P_RD_P0Q2, 0); + mt9f002_write_reg16(MT9F002_P_RD_P0Q3, 0); + mt9f002_write_reg16(MT9F002_P_RD_P0Q4, 0); + mt9f002_write_reg16(MT9F002_P_BL_P0Q0, 0); + mt9f002_write_reg16(MT9F002_P_BL_P0Q1, 0); + mt9f002_write_reg16(MT9F002_P_BL_P0Q2, 0); + mt9f002_write_reg16(MT9F002_P_BL_P0Q3, 0); + mt9f002_write_reg16(MT9F002_P_BL_P0Q4, 0); + mt9f002_write_reg16(MT9F002_P_GB_P0Q0, 0); + mt9f002_write_reg16(MT9F002_P_GB_P0Q1, 0); + mt9f002_write_reg16(MT9F002_P_GB_P0Q2, 0); + mt9f002_write_reg16(MT9F002_P_GB_P0Q3, 0); + mt9f002_write_reg16(MT9F002_P_GB_P0Q4, 0); + mt9f002_write_reg16(MT9F002_P_GR_P1Q0, 0); + mt9f002_write_reg16(MT9F002_P_GR_P1Q1, 0); + mt9f002_write_reg16(MT9F002_P_GR_P1Q2, 0); + mt9f002_write_reg16(MT9F002_P_GR_P1Q3, 0); + mt9f002_write_reg16(MT9F002_P_GR_P1Q4, 0); + mt9f002_write_reg16(MT9F002_P_RD_P1Q0, 0); + mt9f002_write_reg16(MT9F002_P_RD_P1Q1, 0); + mt9f002_write_reg16(MT9F002_P_RD_P1Q2, 0); + mt9f002_write_reg16(MT9F002_P_RD_P1Q3, 0); + mt9f002_write_reg16(MT9F002_P_RD_P1Q4, 0); + mt9f002_write_reg16(MT9F002_P_BL_P1Q0, 0); + mt9f002_write_reg16(MT9F002_P_BL_P1Q1, 0); + mt9f002_write_reg16(MT9F002_P_BL_P1Q2, 0); + mt9f002_write_reg16(MT9F002_P_BL_P1Q3, 0); + mt9f002_write_reg16(MT9F002_P_BL_P1Q4, 0); + mt9f002_write_reg16(MT9F002_P_GB_P1Q0, 0); + mt9f002_write_reg16(MT9F002_P_GB_P1Q1, 0); + mt9f002_write_reg16(MT9F002_P_GB_P1Q2, 0); + mt9f002_write_reg16(MT9F002_P_GB_P1Q3, 0); + mt9f002_write_reg16(MT9F002_P_GB_P1Q4, 0); + mt9f002_write_reg16(MT9F002_P_GR_P2Q0, 0); + mt9f002_write_reg16(MT9F002_P_GR_P2Q1, 0); + mt9f002_write_reg16(MT9F002_P_GR_P2Q2, 0); + mt9f002_write_reg16(MT9F002_P_GR_P2Q3, 0); + mt9f002_write_reg16(MT9F002_P_GR_P2Q4, 0); + mt9f002_write_reg16(MT9F002_P_RD_P2Q0, 0); + mt9f002_write_reg16(MT9F002_P_RD_P2Q1, 0); + mt9f002_write_reg16(MT9F002_P_RD_P2Q2, 0); + mt9f002_write_reg16(MT9F002_P_RD_P2Q3, 0); + mt9f002_write_reg16(MT9F002_P_RD_P2Q4, 0); + mt9f002_write_reg16(MT9F002_P_BL_P2Q0, 0); + mt9f002_write_reg16(MT9F002_P_BL_P2Q1, 0); + mt9f002_write_reg16(MT9F002_P_BL_P2Q2, 0); + mt9f002_write_reg16(MT9F002_P_BL_P2Q3, 0); + mt9f002_write_reg16(MT9F002_P_BL_P2Q4, 0); + mt9f002_write_reg16(MT9F002_P_GB_P2Q0, 0); + mt9f002_write_reg16(MT9F002_P_GB_P2Q1, 0); + mt9f002_write_reg16(MT9F002_P_GB_P2Q2, 0); + mt9f002_write_reg16(MT9F002_P_GB_P2Q3, 0); + mt9f002_write_reg16(MT9F002_P_GB_P2Q4, 0); + mt9f002_write_reg16(MT9F002_P_GR_P3Q0, 0); + mt9f002_write_reg16(MT9F002_P_GR_P3Q1, 0); + mt9f002_write_reg16(MT9F002_P_GR_P3Q2, 0); + mt9f002_write_reg16(MT9F002_P_GR_P3Q3, 0); + mt9f002_write_reg16(MT9F002_P_GR_P3Q4, 0); + mt9f002_write_reg16(MT9F002_P_RD_P3Q0, 0); + mt9f002_write_reg16(MT9F002_P_RD_P3Q1, 0); + mt9f002_write_reg16(MT9F002_P_RD_P3Q2, 0); + mt9f002_write_reg16(MT9F002_P_RD_P3Q3, 0); + mt9f002_write_reg16(MT9F002_P_RD_P3Q4, 0); + mt9f002_write_reg16(MT9F002_P_BL_P3Q0, 0); + mt9f002_write_reg16(MT9F002_P_BL_P3Q1, 0); + mt9f002_write_reg16(MT9F002_P_BL_P3Q2, 0); + mt9f002_write_reg16(MT9F002_P_BL_P3Q3, 0); + mt9f002_write_reg16(MT9F002_P_BL_P3Q4, 0); + mt9f002_write_reg16(MT9F002_P_GB_P3Q0, 0); + mt9f002_write_reg16(MT9F002_P_GB_P3Q1, 0); + mt9f002_write_reg16(MT9F002_P_GB_P3Q2, 0); + mt9f002_write_reg16(MT9F002_P_GB_P3Q3, 0); + mt9f002_write_reg16(MT9F002_P_GB_P3Q4, 0); + mt9f002_write_reg16(MT9F002_P_GR_P4Q0, 0); + mt9f002_write_reg16(MT9F002_P_GR_P4Q1, 0); + mt9f002_write_reg16(MT9F002_P_GR_P4Q2, 0); + mt9f002_write_reg16(MT9F002_P_GR_P4Q3, 0); + mt9f002_write_reg16(MT9F002_P_GR_P4Q4, 0); + mt9f002_write_reg16(MT9F002_P_RD_P4Q0, 0); + mt9f002_write_reg16(MT9F002_P_RD_P4Q1, 0); + mt9f002_write_reg16(MT9F002_P_RD_P4Q2, 0); + mt9f002_write_reg16(MT9F002_P_RD_P4Q3, 0); + mt9f002_write_reg16(MT9F002_P_RD_P4Q4, 0); + mt9f002_write_reg16(MT9F002_P_BL_P4Q0, 0); + mt9f002_write_reg16(MT9F002_P_BL_P4Q1, 0); + mt9f002_write_reg16(MT9F002_P_BL_P4Q2, 0); + mt9f002_write_reg16(MT9F002_P_BL_P4Q3, 0); + mt9f002_write_reg16(MT9F002_P_BL_P4Q4, 0); + mt9f002_write_reg16(MT9F002_P_GB_P4Q0, 0); + mt9f002_write_reg16(MT9F002_P_GB_P4Q1, 0); + mt9f002_write_reg16(MT9F002_P_GB_P4Q2, 0); + mt9f002_write_reg16(MT9F002_P_GB_P4Q3, 0); + mt9f002_write_reg16(MT9F002_P_GB_P4Q4, 0); - write(fd_i2c, "0\32\0\20", 4); - write(fd_i2c, "0^\0240", 4); - write(fd_i2c, "0\32\0\20", 4); - write(fd_i2c, "0\32\0\20", 4); - write(fd_i2c, "0\32\0\20", 4); - write(fd_i2c, ">\332\345%", 4); - write(fd_i2c, "0\350\0\0", 4); - write(fd_i2c, "0\352\370s", 4); - write(fd_i2c, "0\352\10\252", 4); - write(fd_i2c, "0\3522\31", 4); - write(fd_i2c, "0\3522\31", 4); - write(fd_i2c, "0\3522\31", 4); - write(fd_i2c, "0\3522\0", 4); - write(fd_i2c, "0\3522\0", 4); - write(fd_i2c, "0\3522\0", 4); - write(fd_i2c, "0\3522\0", 4); - write(fd_i2c, "0\3522\0", 4); - write(fd_i2c, "0\352\27i", 4); - write(fd_i2c, "0\352\246\363", 4); - write(fd_i2c, "0\352\246\363", 4); - write(fd_i2c, "0\352\246\363", 4); - write(fd_i2c, "0\352\246\363", 4); - write(fd_i2c, "0\352\246\363", 4); - write(fd_i2c, "0\352\246\363", 4); - write(fd_i2c, "0\352\246\363", 4); - write(fd_i2c, "0\352\257\363", 4); - write(fd_i2c, "0\352\372d", 4); - write(fd_i2c, "0\352\372d", 4); - write(fd_i2c, "0\352\372d", 4); - write(fd_i2c, "0\352\361d", 4); - write(fd_i2c, "0\352\372d", 4); - write(fd_i2c, "0\352\372d", 4); - write(fd_i2c, "0\352\372d", 4); - write(fd_i2c, "0\352\361d", 4); - write(fd_i2c, "0\352'n", 4); - write(fd_i2c, "0\352\30\317", 4); - write(fd_i2c, "0\352\30\317", 4); - write(fd_i2c, "0\352\30\317", 4); - write(fd_i2c, "0\352(\317", 4); - write(fd_i2c, "0\352\30\317", 4); - write(fd_i2c, "0\352\30\317", 4); - write(fd_i2c, "0\352\30\317", 4); - write(fd_i2c, "0\352\30\317", 4); - write(fd_i2c, "0\352#c", 4); - write(fd_i2c, "0\352#c", 4); - write(fd_i2c, "0\352#R", 4); - write(fd_i2c, "0\352#c", 4); - write(fd_i2c, "0\352#c", 4); - write(fd_i2c, "0\352#c", 4); - write(fd_i2c, "0\352#R", 4); - write(fd_i2c, "0\352#R", 4); - write(fd_i2c, "0\352\243\224", 4); - write(fd_i2c, "0\352\243\224", 4); - write(fd_i2c, "0\352\217\217", 4); - write(fd_i2c, "0\352\243\324", 4); - write(fd_i2c, "0\352\243\224", 4); - write(fd_i2c, "0\352\243\224", 4); - write(fd_i2c, "0\352\217\217", 4); - write(fd_i2c, "0\352\217\317", 4); - write(fd_i2c, "0\352\334#", 4); - write(fd_i2c, "0\352\334c", 4); - write(fd_i2c, "0\352\334c", 4); - write(fd_i2c, "0\352\334#", 4); - write(fd_i2c, "0\352\334#", 4); - write(fd_i2c, "0\352\334c", 4); - write(fd_i2c, "0\352\334c", 4); - write(fd_i2c, "0\352\334#", 4); - write(fd_i2c, "0\352\17s", 4); - write(fd_i2c, "0\352\205\300", 4); - write(fd_i2c, "0\352\205\300", 4); - write(fd_i2c, "0\352\205\300", 4); - write(fd_i2c, "0\352\205\300", 4); - write(fd_i2c, "0\352\205\300", 4); - write(fd_i2c, "0\352\205\300", 4); - write(fd_i2c, "0\352\205\300", 4); - write(fd_i2c, "0\352\205\304", 4); - write(fd_i2c, "0\352\0\0", 4); - write(fd_i2c, "1v\200\0", 4); - write(fd_i2c, ">\332\345%", 4); - write(fd_i2c, "0\36\0\250", 4); - write(fd_i2c, "0\32\0\220", 4); - write(fd_i2c, "1\256\3\1", 4); - write(fd_i2c, "0\32\20\220", 4); - write(fd_i2c, "0d\10E", 4); - write(fd_i2c, "0\32\20\200", 4); - write(fd_i2c, "0n\330\200", 4); - write(fd_i2c, "0\32\220\200", 4); - write(fd_i2c, "0n\330\200", 4); - write(fd_i2c, "0\32\20\310", 4); - write(fd_i2c, "0n\330\200", 4); - write(fd_i2c, "\3\0\0\7", 4); - write(fd_i2c, "\3\2\0\1", 4); - write(fd_i2c, "\3\4\0\1", 4); - write(fd_i2c, "\3\6\0;", 4); - write(fd_i2c, "\3\10\0\10", 4); - write(fd_i2c, "\3\n\0\1", 4); + mt9f002_write_reg16(MT9F002_GREEN1_GAIN, 4176); + mt9f002_write_reg16(MT9F002_BLUE_GAIN, 4176); + mt9f002_write_reg16(MT9F002_RED_GAIN, 4176); + mt9f002_write_reg16(MT9F002_GREEN2_GAIN, 4176); + mt9f002_write_reg16(MT9F002_GLOBAL_GAIN, 4176); + mt9f002_write_reg16(MT9F002_ANALOG_GAIN_CODE_GLOBAL, 10); + mt9f002_write_reg16(MT9F002_ANALOG_GAIN_CODE_GREENR, 10); + mt9f002_write_reg16(MT9F002_ANALOG_GAIN_CODE_RED, 10); + mt9f002_write_reg16(MT9F002_ANALOG_GAIN_CODE_BLUE, 10); + mt9f002_write_reg16(MT9F002_ANALOG_GAIN_CODE_GREENB, 10); + mt9f002_write_reg16(MT9F002_CALIB_GREEN1_ASC1, 4224); + mt9f002_write_reg16(MT9F002_CALIB_BLUE_ASC1, 4224); + mt9f002_write_reg16(MT9F002_CALIB_RED_ASC1, 4224); + mt9f002_write_reg16(MT9F002_CALIB_GREEN2_ASC1, 4224); + mt9f002_write_reg16(MT9F002_CALIB_GREEN1, 4224); + mt9f002_write_reg16(MT9F002_CALIB_BLUE, 4224); + mt9f002_write_reg16(MT9F002_CALIB_RED, 4224); + mt9f002_write_reg16(MT9F002_CALIB_GREEN2, 4224); + mt9f002_write_reg16(MT9F002_POLY_ORIGIN_C, 0); + mt9f002_write_reg16(MT9F002_POLY_ORIGIN_R, 0); + mt9f002_write_reg16(MT9F002_P_GR_Q5, 0); + mt9f002_write_reg16(MT9F002_P_RD_Q5, 0); + mt9f002_write_reg16(MT9F002_P_BL_Q5, 0); + mt9f002_write_reg16(MT9F002_P_GB_Q5, 0); + mt9f002_write_reg16(MT9F002_DAC_ID_FBIAS, 57568); + + mt9f002_write_reg16(MT9F002_FRAME_LENGTH_LINES, 3434); + mt9f002_write_reg16(MT9F002_LINE_LENGTH_PCK, 2504); + mt9f002_write_reg16(MT9F002_ROW_SPEED, 273); + mt9f002_write_reg16(MT9F002_MIPI_TIMING_2, 61452); + mt9f002_write_reg16(MT9F002_HISPI_TIMING, 0); + + mt9f002_write_reg16(MT9F002_VT_PIX_CLK_DIV, 6); + mt9f002_write_reg16(MT9F002_PRE_PLL_CLK_DIV, 6); + mt9f002_write_reg16(MT9F002_PLL_MULTIPLIER, 165); + mt9f002_write_reg16(MT9F002_OP_PIX_CLK_DIV, 12); + + mt9f002_write_reg16(MT9F002_X_ADDR_START_, 500); + mt9f002_write_reg16(MT9F002_Y_ADDR_START_, 32); + mt9f002_write_reg16(MT9F002_X_ADDR_END_, 4527); // 4383 cols + mt9f002_write_reg16(MT9F002_Y_ADDR_END_, 3319); // 3287 rows + mt9f002_write_reg16(MT9F002_READ_MODE, 0x06E7); + mt9f002_write_reg16(MT9F002_X_ODD_INC, 3); + mt9f002_write_reg16(MT9F002_Y_ODD_INC, 3); // Sample 1, skip 7 -> /8 + + mt9f002_write_reg16(MT9F002_SCALING_MODE, 2); + mt9f002_write_reg16(MT9F002_SCALE_M, 48); - ioctl(fd_i2c, 0x707, 0xd0); - write(fd_i2c, "0d\10E", 4); + /* Stream mode */ + mt9f002_write_reg8(MT9F002_MODE_SELECT, 0x01); // Stream mode on - ioctl(fd_i2c, 0x707, 0xd0); - write(fd_i2c, "0\26\1!", 4); - write(fd_i2c, "1v\200\0", 4); - write(fd_i2c, "0@\0A", 4); - write(fd_i2c, "0@\4\303", 4); - write(fd_i2c, "0@\4\303", 4); - write(fd_i2c, "1x\0\0", 4); - write(fd_i2c, "1x\0\0", 4); - write(fd_i2c, "1x\0\0", 4); - write(fd_i2c, "1x\0\0", 4); - write(fd_i2c, ">\350\0G", 4); - write(fd_i2c, "0\324\260\200", 4); - write(fd_i2c, "0\324\261\0", 4); - write(fd_i2c, "0\356\0 ", 4); - write(fd_i2c, ">\344cI", 4); - write(fd_i2c, "1|\200\n", 4); - write(fd_i2c, "0\32\220\310", 4); - write(fd_i2c, "0\350\200\5", 4); - write(fd_i2c, "1|\200\n", 4); - write(fd_i2c, ">\350\0G", 4); - write(fd_i2c, ">\352\25\360", 4); - write(fd_i2c, ">\352\25\360", 4); - write(fd_i2c, ">\352\25\360", 4); - write(fd_i2c, ">\350\0G", 4); - write(fd_i2c, ">\350\0G", 4); - write(fd_i2c, "0\32\20\310", 4); - write(fd_i2c, "\2\2\10\303", 4); - write(fd_i2c, "0\260\0\0", 4); - write(fd_i2c, "0n\330\200", 4); - write(fd_i2c, "0@\0A", 4); - write(fd_i2c, "\1\5\1", 3); - write(fd_i2c, "\3L\2\200", 4); - write(fd_i2c, "\3N\1\340", 4); - write(fd_i2c, "\4\0\0\0", 4); - write(fd_i2c, "0@\0A", 4); - write(fd_i2c, "\3\202\0\1", 4); - write(fd_i2c, "\3\206\0\1", 4); - write(fd_i2c, "\3D\0\0", 4); - write(fd_i2c, "\3H\2\177", 4); - write(fd_i2c, "\3F\0\0", 4); - write(fd_i2c, "\3J\1\337", 4); - - write(fd_i2c, "\3B&\340", 4); - write(fd_i2c, "\3@\2\336", 4); - write(fd_i2c, "\2\2\2\224", 4); - write(fd_i2c, "0\24\27M", 4); - write(fd_i2c, "0V\24@", 4); - write(fd_i2c, "0X\24@", 4); - write(fd_i2c, "0Z\24@", 4); - write(fd_i2c, "0\\\24@", 4); - write(fd_i2c, "\1\4\1", 3); - write(fd_i2c, "\3L\2\200", 4); - write(fd_i2c, "\3N\1h", 4); - write(fd_i2c, "\4\0\0\0", 4); - write(fd_i2c, "0@\0A", 4); - write(fd_i2c, "\3\202\0\1", 4); - write(fd_i2c, "\3\206\0\1", 4); - write(fd_i2c, "\3D\0\0", 4); - write(fd_i2c, "\3H\2\177", 4); - write(fd_i2c, "\3F\0\0", 4); - write(fd_i2c, "\3J\1g", 4); - - write(fd_i2c, "\3B&\340", 4); - write(fd_i2c, "\3@\2\336", 4); - write(fd_i2c, "\2\2\2\224", 4); - write(fd_i2c, "0\24\27M", 4); - write(fd_i2c, "\1\4\0", 3); - - write(fd_i2c, "\1\4\1", 3); - write(fd_i2c, "\3B&\340", 4); - write(fd_i2c, "\3@\2\336", 4); - write(fd_i2c, "\2\2\2\224", 4); - write(fd_i2c, "0\24\27M", 4); - write(fd_i2c, "\1\4\0", 3); - write(fd_i2c, "\1\4\1", 3); - write(fd_i2c, "\2\2\0\334", 4); - write(fd_i2c, "0\24\7\304", 4); - write(fd_i2c, "\1\4\0", 3); - write(fd_i2c, "0VL@", 4); - write(fd_i2c, "0XL@", 4); - write(fd_i2c, "0ZL@", 4); - write(fd_i2c, "0\\L@", 4); - write(fd_i2c, "\3N\0\0", 4); - write(fd_i2c, "\1\0\1", 3); - write(fd_i2c, "\1\4\1", 3); - write(fd_i2c, "\3D\7\320", 4); - write(fd_i2c, "\3H\nO", 4); - write(fd_i2c, "\3F\5\310", 4); - write(fd_i2c, "\3J\7/", 4); - write(fd_i2c, "\1\4\0", 3); - - //usleep(1); - write(fd_i2c, "\3N\1h", 4); - write(fd_i2c, "\1\0\1", 3); - close(fd_i2c); } #pragma GCC diagnostic pop /* end disable -Wunused-result */ diff --git a/sw/airborne/modules/computer_vision/bebop_front_camera.c b/sw/airborne/modules/computer_vision/bebop_front_camera.c new file mode 100644 index 0000000000..e7e7cb6fe2 --- /dev/null +++ b/sw/airborne/modules/computer_vision/bebop_front_camera.c @@ -0,0 +1,239 @@ +/* + * Copyright (C) 2012-2014 The Paparazzi Community + * 2015 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 + * . + * + */ + +/** + * @file modules/computer_vision/bebop_front_camera.c + */ + +// Own header +#include "modules/computer_vision/bebop_front_camera.h" + +#include +#include +#include +#include +#include +#include + +// Video +#include "lib/v4l/v4l2.h" +#include "lib/vision/image.h" +#include "lib/vision/bayern.h" +#include "lib/encoding/jpeg.h" +#include "lib/encoding/rtp.h" +#include "udp_socket.h" + +// Threaded computer vision +#include + +#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 viewvideo_t bebop_front_camera = { + .is_streaming = FALSE, + .take_shot = FALSE, + .shot_number = 0 +}; + +void bebop_front_camera_take_shot(bool_t 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); + + BayernToYUV(&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; + } + } +} diff --git a/sw/airborne/modules/computer_vision/bebop_front_camera.h b/sw/airborne/modules/computer_vision/bebop_front_camera.h new file mode 100644 index 0000000000..68d43d8b9e --- /dev/null +++ b/sw/airborne/modules/computer_vision/bebop_front_camera.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2012-2014 The Paparazzi Community + * 2015 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 + * . + * + */ + +/** + * @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" +#include "viewvideo.h" + +extern struct viewvideo_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_t take); + +#endif /* BEBOP_FRONT_CAMERA_H */ + diff --git a/sw/airborne/modules/computer_vision/lib/encoding/jpeg.c b/sw/airborne/modules/computer_vision/lib/encoding/jpeg.c index 54ca23ef70..a957574fb9 100644 --- a/sw/airborne/modules/computer_vision/lib/encoding/jpeg.c +++ b/sw/airborne/modules/computer_vision/lib/encoding/jpeg.c @@ -432,6 +432,9 @@ void jpeg_encode_image(struct image_t *in, struct image_t *out, uint32_t quality if (in->type == IMAGE_YUV422) { image_format = FOUR_TWO_TWO; } + else if (in->type == IMAGE_GRAYSCALE) { + image_format = FOUR_ZERO_ZERO; + } JPEG_ENCODER_STRUCTURE JpegStruct; JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure = &JpegStruct; diff --git a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.c b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.c index ecd6d498dd..1773d4f866 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.c +++ b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.c @@ -174,7 +174,9 @@ bool_t v4l2_init_subdev(char *subdev_name, uint8_t pad, uint8_t which, uint16_t * @param[in] buffer_cnt The amount of buffers used for mapping * @return The newly create V4L2 device */ -struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height, uint8_t buffers_cnt) { +struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height, uint8_t buffers_cnt, + uint32_t _pixelformat) +{ uint8_t i; struct v4l2_capability cap; struct v4l2_format fmt; @@ -198,12 +200,12 @@ struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height } // Check if the device is capable of capturing and streaming - if (!(cap.capabilities &V4L2_CAP_VIDEO_CAPTURE)) { + if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { printf("[v4l2] %s is no V4L2 video capturing device\n", device_name); close(fd); return NULL; } - if (!(cap.capabilities &V4L2_CAP_STREAMING)) { + if (!(cap.capabilities & V4L2_CAP_STREAMING)) { printf("[v4l2] %s isn't capable of streaming (TODO: support reading)\n", device_name); close(fd); return NULL; @@ -215,7 +217,7 @@ struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; fmt.fmt.pix.width = width; fmt.fmt.pix.height = height; - fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY; + fmt.fmt.pix.pixelformat = _pixelformat; fmt.fmt.pix.field = V4L2_FIELD_NONE; if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) { diff --git a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h index a428ed9070..a42fb363e6 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/v4l2.h @@ -59,7 +59,8 @@ struct v4l2_device { /* External functions */ bool_t v4l2_init_subdev(char *subdev_name, uint8_t pad, uint8_t which, uint16_t code, uint16_t width, uint16_t height); -struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height, uint8_t buffers_cnt); +struct v4l2_device *v4l2_init(char *device_name, uint16_t width, uint16_t height, uint8_t buffers_cnt, + uint32_t _pixelformat); void v4l2_image_get(struct v4l2_device *dev, struct image_t *img); bool_t v4l2_image_get_nonblock(struct v4l2_device *dev, struct image_t *img); void v4l2_image_free(struct v4l2_device *dev, struct image_t *img); diff --git a/sw/airborne/modules/computer_vision/lib/vision/bayern.h b/sw/airborne/modules/computer_vision/lib/vision/bayern.h new file mode 100644 index 0000000000..0ee9cf03e3 --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/vision/bayern.h @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2012-2014 The Paparazzi Community + * 2015 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 + * . + * + */ + +/** + * @file modules/computer_vision/lib/vision/bayern.c + */ + +#ifndef BAYERN_H +#define BAYERN_H + +#include "lib/vision/image.h" + +static inline uint8_t clip(uint16_t x) +{ + if (x < 0) { + return 0; + } + if (x > 255) { + return 255; + } + return (uint8_t) x; +} + + +void BayernToYUV(struct image_t *Input, struct image_t *out, + int RedX, int RedY); + + +/** + * @brief Decode Bayern Pattern + * @param RedX, RedY the coordinates of the upper-rightmost green pixel + * which has a red pixel next to it and a blue underneath + * + */ + +void BayernToYUV(struct image_t *in, struct image_t *out, + int RedX, int RedY) +{ + uint16_t *ii = (uint8_t *) in->buf; + uint8_t *oi = (uint8_t *) out->buf; + int x, y; + + + for (y = 0; y < out->h; y++) { + for (x = 0; x < out->w; x += 2) { + /* RGB Bayern: + * RBRBRBRBRBRBRBRB + * GRGRGRGRGRGRGRGR + */ + int i = 2 * (out->h - y) + RedX; + int j = 2 * x + RedY; + if ((i < in->w) && (j < in->h)) { + uint16_t G1 = ii[i + j * in->w] / 2; + uint16_t R1 = ii[i + j * in->w + 1] / 2; + uint16_t B1 = ii[i + (j + 1) * in->w] / 2; + j += 2; + uint16_t G2 = ii[i + j * in->w] / 2; + uint16_t R2 = ii[i + j * in->w + 1] / 2; + uint16_t B2 = ii[i + (j + 1) * in->w] / 2; + + uint32_t u, y1, v, y2; + + y1 = (0.256788 * R1 + 0.504129 * G1 + 0.097906 * B1) / 128 + 16; + y2 = (0.256788 * R2 + 0.504129 * G2 + 0.097906 * B2) / 128 + 16; + u = (-0.148223 * (R1 + R2) - 0.290993 * (G1 + G2) + 0.439216 * (B1 + B2)) / 256 + 128; + v = (0.439216 * (R1 + R2) - 0.367788 * (G1 + G2) - 0.071427 * (B1 + B2)) / 256 + 128; + + + oi[(x + y * out->w) * 2] = clip(u); + oi[(x + y * out->w) * 2 + 1] = clip(y1); + oi[(x + y * out->w) * 2 + 2] = clip(v); + oi[(x + y * out->w) * 2 + 3] = clip(y2); + } else { + oi[(x + y * out->w) * 2] = 0; + oi[(x + y * out->w) * 2 + 1] = 0; + oi[(x + y * out->w) * 2 + 2] = 0; + oi[(x + y * out->w) * 2 + 3] = 0; + } + } + } +} + +#endif /* BAYERN_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index d2ebbf2b80..b82209304f 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -127,7 +127,7 @@ void opticflow_module_init(void) #endif /* Try to initialize the video device */ - opticflow_dev = v4l2_init(STRINGIFY(OPTICFLOW_DEVICE), OPTICFLOW_DEVICE_SIZE, OPTICFLOW_DEVICE_BUFFERS); + opticflow_dev = v4l2_init(STRINGIFY(OPTICFLOW_DEVICE), OPTICFLOW_DEVICE_SIZE, OPTICFLOW_DEVICE_BUFFERS,V4L2_PIX_FMT_UYVY); if (opticflow_dev == NULL) { printf("[opticflow_module] Could not initialize the video device\n"); } diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index 35d7ee0b53..d95fa1e93d 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -311,7 +311,7 @@ void viewvideo_init(void) #endif // Initialize the V4L2 device - viewvideo.dev = v4l2_init(STRINGIFY(VIEWVIDEO_DEVICE), VIEWVIDEO_DEVICE_SIZE, VIEWVIDEO_DEVICE_BUFFERS); + viewvideo.dev = v4l2_init(STRINGIFY(VIEWVIDEO_DEVICE), VIEWVIDEO_DEVICE_SIZE, VIEWVIDEO_DEVICE_BUFFERS, V4L2_PIX_FMT_UYVY); if (viewvideo.dev == NULL) { printf("[viewvideo] Could not initialize the %s V4L2 device.\n", STRINGIFY(VIEWVIDEO_DEVICE)); return;