diff --git a/conf/modules/video_rtp_stream.xml b/conf/modules/video_rtp_stream.xml index 5318b31dbd..989a33ca2f 100644 --- a/conf/modules/video_rtp_stream.xml +++ b/conf/modules/video_rtp_stream.xml @@ -3,20 +3,23 @@ - Video stream of ARDone 2 front camera. + Video streaming for Linux devices - - sends a RTP/UDP stream of the from camera - - possibility to save an image on the internal memory (JPEG, full size, best quality, 100 pictures max if nothing else) + - Sends a RTP/UDP stream of the camera + - Possibility to save an image(shot) on the internal memory (JPEG, full size, best quality) - - - - + + + + + + + - + @@ -25,15 +28,34 @@ - + + + + + + - - - + + + + VIEWVIDEO_DEV ?= UDP1 + VIEWVIDEO_HOST ?= $(MODEM_HOST) + VIEWVIDEO_PORT_OUT ?= 5000 + VIEWVIDEO_PORT_IN ?= 4999 + VIEWVIDEO_BROADCAST ?= $(MODEM_BROADCAST) + VIEWVIDEO_DEV_LOWER = $(shell echo $(VIEWVIDEO_DEV) | tr A-Z a-z) + + STREAM_CFLAGS = -DUSE_$(VIEWVIDEO_DEV) -DVIEWVIDEO_DEV=$(VIEWVIDEO_DEV_LOWER) -DVIEWVIDEO_PORT_OUT=$(VIEWVIDEO_PORT_OUT) + STREAM_CFLAGS += -D$(VIEWVIDEO_DEV)_PORT_OUT=$(VIEWVIDEO_PORT_OUT) -D$(VIEWVIDEO_DEV)_PORT_IN=$(VIEWVIDEO_PORT_IN) + STREAM_CFLAGS += -D$(VIEWVIDEO_DEV)_BROADCAST=$(VIEWVIDEO_BROADCAST) -D$(VIEWVIDEO_DEV)_HOST=\"$(VIEWVIDEO_HOST)\" + ap.CFLAGS += $(STREAM_CFLAGS) + + + diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c index 119f4bc2cf..e26c69e900 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c @@ -15,6 +15,10 @@ #include "jpeg.h" +/** + * @file modules/computer_vision/cv/encoding/jpeg.c + * Encode images with the use of the JPEG encoding + */ static inline unsigned char svs_size_code(int w) { @@ -37,7 +41,7 @@ static inline unsigned char svs_size_code(int w) return '9'; } -int create_svs_jpeg_header(unsigned char *jpegbuf, int32_t size, int w) +int jpeg_create_svs_header(unsigned char *jpegbuf, int32_t size, int w) { // SVS Surveyor Jpeg UDP format uint32_t s = size; @@ -79,68 +83,49 @@ typedef struct JPEG_ENCODER_STRUCTURE { } JPEG_ENCODER_STRUCTURE; -#define BLOCK_SIZE 64 +static void jpeg_initialization(JPEG_ENCODER_STRUCTURE *, uint32_t, uint32_t, uint32_t); +//static void jpeg_initialize_quantization_tables(uint32_t); -extern uint8_t Lqt [BLOCK_SIZE]; -extern uint8_t Cqt [BLOCK_SIZE]; -extern uint16_t ILqt [BLOCK_SIZE]; -extern uint16_t ICqt [BLOCK_SIZE]; +static uint8_t *jpeg_write_markers(uint8_t *, uint32_t, uint32_t, uint32_t); -extern int16_t Y1 [BLOCK_SIZE]; -extern int16_t Y2 [BLOCK_SIZE]; -//extern int16_t Y3 [BLOCK_SIZE]; -//extern int16_t Y4 [BLOCK_SIZE]; -extern int16_t CB [BLOCK_SIZE]; -extern int16_t CR [BLOCK_SIZE]; -extern int16_t Temp [BLOCK_SIZE]; +static void jpeg_read_400_format(JPEG_ENCODER_STRUCTURE *, uint8_t *); +static void jpeg_read_422_format(JPEG_ENCODER_STRUCTURE *, uint8_t *); -extern uint32_t lcode; -extern uint16_t bitindex; +static uint8_t *jpeg_encodeMCU(JPEG_ENCODER_STRUCTURE *, uint32_t, uint8_t *); -void initialization(JPEG_ENCODER_STRUCTURE *, uint32_t, uint32_t, uint32_t); +static void jpeg_levelshift(int16_t *); +static void jpeg_DCT(int16_t *); -void initialize_quantization_tables(uint32_t); +static void jpeg_quantization(int16_t *, uint16_t *); +static uint8_t *jpeg_huffman(JPEG_ENCODER_STRUCTURE *, uint16_t, uint8_t *); -uint8_t *write_markers(uint8_t *, uint32_t, uint32_t, uint32_t); +static uint8_t *jpeg_close_bitstream(uint8_t *); -void read_400_format(JPEG_ENCODER_STRUCTURE *, uint8_t *); -void read_422_format(JPEG_ENCODER_STRUCTURE *, uint8_t *); +//static int16_t fdct_coeff[8] = {0x5a82, 0x5a82, 0x30fb, 0x7641, 0x18f8, 0x7d8a, 0x471c, 0x6a6d}; +//static int16_t fdct_temp[64]; -uint8_t *encodeMCU(JPEG_ENCODER_STRUCTURE *, uint32_t, uint8_t *); - -void levelshift(int16_t *); -void DCT(int16_t *); - -void quantization(int16_t *, uint16_t *); -uint8_t *huffman(JPEG_ENCODER_STRUCTURE *, uint16_t, uint8_t *); - -uint8_t *close_bitstream(uint8_t *); - -int16_t fdct_coeff[8] = {0x5a82, 0x5a82, 0x30fb, 0x7641, 0x18f8, 0x7d8a, 0x471c, 0x6a6d}; -int16_t fdct_temp[64]; - -uint16_t luminance_dc_code_table [] = { +static uint16_t luminance_dc_code_table [] = { 0x0000, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x000E, 0x001E, 0x003E, 0x007E, 0x00FE, 0x01FE }; -uint16_t luminance_dc_size_table [] = { +static uint16_t luminance_dc_size_table [] = { 0x0002, 0x0003, 0x0003, 0x0003, 0x0003, 0x0003, 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009 }; -uint16_t chrominance_dc_code_table [] = { +static uint16_t chrominance_dc_code_table [] = { 0x0000, 0x0001, 0x0002, 0x0006, 0x000E, 0x001E, 0x003E, 0x007E, 0x00FE, 0x01FE, 0x03FE, 0x07FE }; -uint16_t chrominance_dc_size_table [] = { +static uint16_t chrominance_dc_size_table [] = { 0x0002, 0x0002, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009, 0x000A, 0x000B }; -uint16_t luminance_ac_code_table [] = { +static uint16_t luminance_ac_code_table [] = { 0x000A, 0x0000, 0x0001, 0x0004, 0x000B, 0x001A, 0x0078, 0x00F8, 0x03F6, 0xFF82, 0xFF83, 0x000C, 0x001B, 0x0079, 0x01F6, 0x07F6, 0xFF84, 0xFF85, 0xFF86, 0xFF87, 0xFF88, @@ -161,7 +146,7 @@ uint16_t luminance_ac_code_table [] = { 0x07F9 }; -uint16_t luminance_ac_size_table [] = { +static uint16_t luminance_ac_size_table [] = { 0x0004, 0x0002, 0x0002, 0x0003, 0x0004, 0x0005, 0x0007, 0x0008, 0x000A, 0x0010, 0x0010, 0x0004, 0x0005, 0x0007, 0x0009, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, @@ -182,7 +167,7 @@ uint16_t luminance_ac_size_table [] = { 0x000B }; -uint16_t chrominance_ac_code_table [] = { +static uint16_t chrominance_ac_code_table [] = { 0x0000, 0x0001, 0x0004, 0x000A, 0x0018, 0x0019, 0x0038, 0x0078, 0x01F4, 0x03F6, 0x0FF4, 0x000B, 0x0039, 0x00F6, 0x01F5, 0x07F6, 0x0FF5, 0xFF88, 0xFF89, 0xFF8A, 0xFF8B, @@ -203,7 +188,7 @@ uint16_t chrominance_ac_code_table [] = { 0x03FA }; -uint16_t chrominance_ac_size_table [] = { +static uint16_t chrominance_ac_size_table [] = { 0x0002, 0x0002, 0x0003, 0x0004, 0x0005, 0x0005, 0x0006, 0x0007, 0x0009, 0x000A, 0x000C, 0x0004, 0x0006, 0x0008, 0x0009, 0x000B, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, @@ -224,7 +209,7 @@ uint16_t chrominance_ac_size_table [] = { 0x000A }; -uint8_t bitsize [] = { +static uint8_t bitsize [] = { 0, 1, 2, 2, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5, 5, 5, @@ -259,7 +244,7 @@ uint8_t bitsize [] = { 8, 8, 8, 8, 8, 8, 8, 8 }; -uint8_t markerdata [] = { +static uint8_t markerdata [] = { 0xFF, 0xC4, 0x00, 0x1F, 0x00, 0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0xFF, 0xC4, 0x00, 0xB5, 0x10, 0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03, 0x05, 0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7D, 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, 0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xA1, 0x08, 0x23, 0x42, 0xB1, 0xC1, 0x15, 0x52, 0xD1, 0xF0, 0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0A, 0x16, 0x17, 0x18, 0x19, 0x1A, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9, 0xDA, 0xE1, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, @@ -270,7 +255,7 @@ uint8_t markerdata [] = { }; -uint8_t zigzag_table [] = { +static uint8_t zigzag_table [] = { 0, 1, 5, 6, 14, 15, 27, 28, 2, 4, 7, 13, 16, 26, 29, 42, 3, 8, 12, 17, 25, 30, 41, 43, @@ -280,7 +265,7 @@ uint8_t zigzag_table [] = { 21, 34, 37, 47, 50, 56, 59, 61, 35, 36, 48, 49, 57, 58, 62, 63 }; -uint8_t luminance_quant_table [] = { +/*static uint8_t luminance_quant_table [] = { 16, 11, 10, 16, 24, 40, 51, 61, 12, 12, 14, 19, 26, 58, 60, 55, 14, 13, 16, 24, 40, 57, 69, 56, @@ -291,7 +276,7 @@ uint8_t luminance_quant_table [] = { 72, 92, 95, 98, 112, 100, 103, 99 }; -uint8_t chrominance_quant_table [] = { +static uint8_t chrominance_quant_table [] = { 17, 18, 24, 47, 99, 99, 99, 99, 18, 21, 26, 66, 99, 99, 99, 99, 24, 26, 56, 99, 99, 99, 99, 99, @@ -300,27 +285,28 @@ uint8_t chrominance_quant_table [] = { 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99 -}; +};*/ -uint8_t Lqt [BLOCK_SIZE]; -uint8_t Cqt [BLOCK_SIZE]; -uint16_t ILqt [BLOCK_SIZE]; -uint16_t ICqt [BLOCK_SIZE]; +#define JPEG_BLOCK_SIZE 64 +static uint8_t Lqt [JPEG_BLOCK_SIZE]; +static uint8_t Cqt [JPEG_BLOCK_SIZE]; +static uint16_t ILqt [JPEG_BLOCK_SIZE]; +static uint16_t ICqt [JPEG_BLOCK_SIZE]; -int16_t Y1 [BLOCK_SIZE]; -int16_t Y2 [BLOCK_SIZE]; -int16_t Y3 [BLOCK_SIZE]; -int16_t Y4 [BLOCK_SIZE]; -int16_t CB [BLOCK_SIZE]; -int16_t CR [BLOCK_SIZE]; -int16_t Temp [BLOCK_SIZE]; -uint32_t lcode = 0; -uint16_t bitindex = 0; +static int16_t Y1 [JPEG_BLOCK_SIZE]; +static int16_t Y2 [JPEG_BLOCK_SIZE]; +//static int16_t Y3 [JPEG_BLOCK_SIZE]; +//static int16_t Y4 [JPEG_BLOCK_SIZE]; +static int16_t CB [JPEG_BLOCK_SIZE]; +static int16_t CR [JPEG_BLOCK_SIZE]; +static int16_t Temp [JPEG_BLOCK_SIZE]; +static uint32_t lcode = 0; +static uint16_t bitindex = 0; void (*read_format)(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr); -void initialization(JPEG_ENCODER_STRUCTURE *jpeg, uint32_t image_format, uint32_t image_width, uint32_t image_height) +static void jpeg_initialization(JPEG_ENCODER_STRUCTURE *jpeg, uint32_t image_format, uint32_t image_width, uint32_t image_height) { uint16_t mcu_width, mcu_height, bytes_per_pixel; @@ -332,7 +318,7 @@ void initialization(JPEG_ENCODER_STRUCTURE *jpeg, uint32_t image_format, uint32_ jpeg->vertical_mcus = (uint16_t)((image_height + mcu_height - 1) >> 3); bytes_per_pixel = 1; - read_format = read_400_format; + read_format = jpeg_read_400_format; } else { jpeg->mcu_width = mcu_width = 16; jpeg->horizontal_mcus = (uint16_t)((image_width + mcu_width - 1) >> 4); @@ -340,7 +326,7 @@ void initialization(JPEG_ENCODER_STRUCTURE *jpeg, uint32_t image_format, uint32_ jpeg->mcu_height = mcu_height = 8; jpeg->vertical_mcus = (uint16_t)((image_height + mcu_height - 1) >> 3); bytes_per_pixel = 2; - read_format = read_422_format; + read_format = jpeg_read_422_format; } jpeg->rows_in_bottom_mcus = (uint16_t)(image_height - (jpeg->vertical_mcus - 1) * mcu_height); @@ -433,8 +419,8 @@ void MakeTables(int q) -uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, - uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) +uint8_t *jpeg_encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, + uint32_t image_width, uint32_t image_height, bool_t add_dri_header) { uint16_t i, j; @@ -442,16 +428,16 @@ uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_ JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure = &JpegStruct; /* Initialization of JPEG control structure */ - initialization(jpeg_encoder_structure, image_format, image_width, image_height); + jpeg_initialization(jpeg_encoder_structure, image_format, image_width, image_height); /* Quantization Table Initialization */ - //initialize_quantization_tables (quality_factor); + //jpeg_initialize_quantization_tables (quality_factor); MakeTables(quality_factor); /* Writing Marker Data */ if (add_dri_header) { - output_ptr = write_markers(output_ptr, image_format, image_width, image_height); + output_ptr = jpeg_write_markers(output_ptr, image_format, image_width, image_height); } for (i = 1; i <= jpeg_encoder_structure->vertical_mcus; i++) { @@ -473,7 +459,7 @@ uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_ read_format(jpeg_encoder_structure, input_ptr); /* Encode the data in MCU */ - output_ptr = encodeMCU(jpeg_encoder_structure, image_format, output_ptr); + output_ptr = jpeg_encodeMCU(jpeg_encoder_structure, image_format, output_ptr); input_ptr += jpeg_encoder_structure->mcu_width_size; } @@ -482,38 +468,38 @@ uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_ } /* Close Routine */ - output_ptr = close_bitstream(output_ptr); + output_ptr = jpeg_close_bitstream(output_ptr); return output_ptr; } -uint8_t *encodeMCU(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint32_t image_format, uint8_t *output_ptr) +static uint8_t *jpeg_encodeMCU(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint32_t image_format, uint8_t *output_ptr) { - levelshift(Y1); - DCT(Y1); - quantization(Y1, ILqt); - output_ptr = huffman(jpeg_encoder_structure, 1, output_ptr); + jpeg_levelshift(Y1); + jpeg_DCT(Y1); + jpeg_quantization(Y1, ILqt); + output_ptr = jpeg_huffman(jpeg_encoder_structure, 1, output_ptr); if (image_format == FOUR_TWO_TWO) { - levelshift(Y2); - DCT(Y2); - quantization(Y2, ILqt); - output_ptr = huffman(jpeg_encoder_structure, 1, output_ptr); + jpeg_levelshift(Y2); + jpeg_DCT(Y2); + jpeg_quantization(Y2, ILqt); + output_ptr = jpeg_huffman(jpeg_encoder_structure, 1, output_ptr); - levelshift(CB); - DCT(CB); - quantization(CB, ICqt); - output_ptr = huffman(jpeg_encoder_structure, 2, output_ptr); + jpeg_levelshift(CB); + jpeg_DCT(CB); + jpeg_quantization(CB, ICqt); + output_ptr = jpeg_huffman(jpeg_encoder_structure, 2, output_ptr); - levelshift(CR); - DCT(CR); - quantization(CR, ICqt); - output_ptr = huffman(jpeg_encoder_structure, 3, output_ptr); + jpeg_levelshift(CR); + jpeg_DCT(CR); + jpeg_quantization(CR, ICqt); + output_ptr = jpeg_huffman(jpeg_encoder_structure, 3, output_ptr); } return output_ptr; } /* Level shifting to get 8 bit SIGNED values for the data */ -void levelshift(int16_t *const data) +static void jpeg_levelshift(int16_t *const data) { int16_t i; @@ -523,7 +509,7 @@ void levelshift(int16_t *const data) } /* DCT for One block(8x8) */ -void DCT(int16_t *data) +static void jpeg_DCT(int16_t *data) { //_r8x8dct(data, fdct_coeff, fdct_temp); @@ -635,7 +621,7 @@ void DCT(int16_t *data) } \ } -uint8_t *huffman(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint16_t component, uint8_t *output_ptr) +static uint8_t *jpeg_huffman(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint16_t component, uint8_t *output_ptr) { uint16_t i; uint16_t *DcCodeTable, *DcSizeTable, *AcCodeTable, *AcSizeTable; @@ -732,7 +718,7 @@ uint8_t *huffman(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint16_t compon } /* For bit Stuffing and EOI marker */ -uint8_t *close_bitstream(uint8_t *output_ptr) +static uint8_t *jpeg_close_bitstream(uint8_t *output_ptr) { uint16_t i, count; uint8_t *ptr; @@ -758,7 +744,7 @@ uint8_t *close_bitstream(uint8_t *output_ptr) return output_ptr; } -uint8_t *write_markers(uint8_t *output_ptr, uint32_t image_format, uint32_t image_width, uint32_t image_height) +static uint8_t *jpeg_write_markers(uint8_t *output_ptr, uint32_t image_format, uint32_t image_width, uint32_t image_height) { uint16_t i, header_length; uint8_t number_of_components; @@ -899,7 +885,7 @@ uint8_t *write_markers(uint8_t *output_ptr, uint32_t image_format, uint32_t imag /* Multiply Quantization table with quality factor to get LQT and CQT factor ranges from 1 to 8; 1 = highest quality, 8 = lowest quality */ -void initialize_quantization_tables(uint32_t quality_factor) +/*static void jpeg_initialize_quantization_tables(uint32_t quality_factor) { uint16_t i, index; uint32_t value; @@ -915,7 +901,7 @@ void initialize_quantization_tables(uint32_t quality_factor) for (i = 0; i < 64; i++) { index = zigzag_table [i]; - /* luminance quantization table * quality factor */ + // luminance quantization table * quality factor value = luminance_quant_table [i] * quality_factor; value = (value + 0x200) >> 10; @@ -929,7 +915,7 @@ void initialize_quantization_tables(uint32_t quality_factor) //ILqt [i] = DSP_Division (0x8000, value); ILqt [i] = 0x8000 / value; - /* chrominance quantization table * quality factor */ + // chrominance quantization table * quality factor value = chrominance_quant_table [i] * quality_factor; value = (value + 0x200) >> 10; @@ -943,10 +929,10 @@ void initialize_quantization_tables(uint32_t quality_factor) //ICqt [i] = DSP_Division (0x8000, value); ICqt [i] = 0x8000 / value; } -} +}*/ /* multiply DCT Coefficients with Quantization table and store in ZigZag location */ -void quantization(int16_t *const data, uint16_t *const quant_table_ptr) +static void jpeg_quantization(int16_t *const data, uint16_t *const quant_table_ptr) { int16_t i; int32_t value; @@ -959,7 +945,7 @@ void quantization(int16_t *const data, uint16_t *const quant_table_ptr) } } -void read_400_format(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr) +static void jpeg_read_400_format(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr) { int32_t i, j; int16_t *Y1_Ptr = Y1; @@ -989,7 +975,7 @@ void read_400_format(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *in } } -void read_422_format(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr) +static void jpeg_read_422_format(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr) { int32_t i, j; uint16_t Y1_cols, Y2_cols; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h index d7c2f2591b..f36021f15e 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h +++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h @@ -1,45 +1,47 @@ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * jpeg.c - JPEG compression for SRV-1 robot + * Copyright (C) 2005-2009 Surveyor Corporation + * + * This program 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 of the License, or + * (at your option) any later version. + * + * This program 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 (www.gnu.org/licenses) + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ -#ifndef __MY_JPEG_HEADER__ -#define __MY_JPEG_HEADER__ +/** + * @file modules/computer_vision/cv/encoding/jpeg.h + * Encode images with the use of the JPEG encoding + */ -#include +#ifndef _CV_ENCODING_JPEG_H +#define _CV_ENCODING_JPEG_H -#define FOUR_ZERO_ZERO 0 -#define FOUR_TWO_ZERO 1 -#define FOUR_TWO_TWO 2 -#define FOUR_FOUR_FOUR 3 -#define RGB 4 +#include "std.h" -unsigned char *encode_image( +/* The different type of image encodings */ +#define FOUR_ZERO_ZERO 0 +#define FOUR_TWO_ZERO 1 +#define FOUR_TWO_TWO 2 +#define FOUR_FOUR_FOUR 3 +#define RGB 4 + +/* JPEG encode an image */ +unsigned char *jpeg_encode_image( uint8_t *in, uint8_t *out, uint32_t q, // image quality 1-8 uint32_t fmt, // image format code uint32_t width, // image width uint32_t height, // image height - uint8_t add_dri_header // data only or full jpeg file + bool_t add_dri_header // data only or full jpeg file ); -unsigned char *encode_image_rtp( - uint8_t *in, - uint8_t *out, - uint32_t q, // image quality 1-8 - uint32_t fmt, // image format code - uint32_t width, // image width - uint32_t height, // image height - uint8_t add_dri_header // data only or full jpeg file -); +/* Create an SVS header */ +int jpeg_create_svs_header(unsigned char *buf, int32_t size, int w); -unsigned char *encode_image_std_qt( - uint8_t *in, - uint8_t *out, - uint32_t q, // image quality 0 to 99 - uint32_t fmt, // image format code - uint32_t width, // image width - uint32_t height, // image height - uint8_t add_dri_header // data only or full jpeg file -); - -int create_svs_jpeg_header(unsigned char *buf, int32_t size, int w); - -#endif +#endif /* _CV_ENCODING_JPEG_H */ diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c index 2221126818..92961f6ff1 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c @@ -1,15 +1,39 @@ +/* + * Copyright (C) 2012-2014 The Paparazzi Community + * + * 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/cv/encoding/rtp.c + * + * Encodes a vide stream with RTP (JPEG) + */ #include #include #include -#include "udp/socket.h" - #include "rtp.h" -void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, - uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, - uint8_t has_dri_header); +static void rtp_packet_send(struct udp_periph *udp, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, + uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, + uint8_t has_dri_header); // http://www.ietf.org/rfc/rfc3550.txt @@ -36,11 +60,10 @@ uint8_t JpegScanDataCh2B[KJpegCh2ScanDataLen] = { 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9 }; - - - - -void test_rtp_frame(struct UdpSocket *sock) +/** + * Send a test RTP frame + */ +void rtp_frame_test(struct udp_periph *udp) { static uint32_t framecounter = 0; static uint32_t timecounter = 0; @@ -51,18 +74,20 @@ void test_rtp_frame(struct UdpSocket *sock) uint8_t quality_code = 0x54; if (toggle) { - send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, + rtp_packet_send(udp, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); } else { - send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, + rtp_packet_send(udp, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); } framecounter++; timecounter += 3600; } - -void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, +/** + * Send an RTP frame + */ +void rtp_frame_send(struct udp_periph *udp, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) { static uint32_t packetcounter = 0; @@ -88,7 +113,7 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int len = JpegLen; } - send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, + rtp_packet_send(udp, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header); JpegLen -= len; @@ -110,9 +135,8 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int set in the last packet of a frame. * */ - -void send_rtp_packet( - struct UdpSocket *sock, +static void rtp_packet_send( + struct udp_periph *udp, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, @@ -190,5 +214,5 @@ void send_rtp_packet( // append the JPEG scan data to the RTP buffer memcpy(&RtpBuf[20], Jpeg, JpegLen); - udp_write(sock, RtpBuf, RtpPacketSize); + udp_send_raw(udp, RtpBuf, RtpPacketSize); }; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.h b/sw/airborne/modules/computer_vision/cv/encoding/rtp.h index 0266aa65e4..7452153627 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.h +++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.h @@ -1,11 +1,38 @@ +/* + * Copyright (C) 2012-2014 The Paparazzi Community + * + * 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/cv/encoding/rtp.h + * + * Encodes a vide stream with RTP (JPEG) + */ +#ifndef _CV_ENCODING_RTP_H +#define _CV_ENCODING_RTP_H -#include -#include "udp/socket.h" +#include "std.h" +#include "mcu_periph/udp.h" -void send_rtp_frame( - struct UdpSocket *sock, // socket +void rtp_frame_send( + struct udp_periph *udp, // socket uint8_t *Jpeg, uint32_t JpegLen, // jpeg data int w, int h, // width and height uint8_t format_code, // 0=422, 1=421 @@ -14,4 +41,6 @@ void send_rtp_frame( uint32_t delta_t // time step 90kHz ); -void test_rtp_frame(struct UdpSocket *sock); +void rtp_frame_test(struct udp_periph *udp); + +#endif /* _CV_ENCODING_RTP_H */ diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index 39ca322117..8c662217a2 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2012-2014 The Paparazzi Community + * 2015 Freek van Tienen * * This file is part of Paparazzi. * @@ -36,122 +37,114 @@ #include #include #include +#include +#include "mcu_periph/udp.h" -// UDP RTP Images -#include "modules/computer_vision/lib/udp/socket.h" // Video -#include "modules/computer_vision/lib/v4l/v4l2.h" -#include "modules/computer_vision/cv/resize.h" -#include "modules/computer_vision/cv/encoding/jpeg.h" -#include "modules/computer_vision/cv/encoding/rtp.h" +#include "lib/v4l/v4l2.h" +#include "cv/resize.h" +#include "cv/encoding/jpeg.h" +#include "cv/encoding/rtp.h" // Threaded computer vision #include -// Default broadcast IP -#ifndef VIDEO_SOCK_IP -#define VIDEO_SOCK_IP "192.168.1.255" +// The video device +#ifndef VIEWVIDEO_DEVICE +#define VIEWVIDEO_DEVICE "/dev/video1" #endif +PRINT_CONFIG_VAR(VIEWVIDEO_DEVICE); -// Output socket can be defined from an offset -#ifdef VIDEO_SOCK_OUT_OFFSET -#define VIDEO_SOCK_OUT (5000+VIDEO_SOCK_OUT_OFFSET) +// The video device size (width, height) +#ifndef VIEWVIDEO_DEVICE_SIZE +#define VIEWVIDEO_DEVICE_SIZE 1280,720 #endif +#define __SIZE_HELPER(x, y) #x", "#y +#define _SIZE_HELPER(x) __SIZE_HELPER(x) +PRINT_CONFIG_MSG("VIEWVIDEO_DEVICE_SIZE = " _SIZE_HELPER(VIEWVIDEO_DEVICE_SIZE)); -#ifndef VIDEO_SOCK_OUT -#define VIDEO_SOCK_OUT 5000 -#endif - -#ifndef VIDEO_SOCK_IN -#define VIDEO_SOCK_IN 4999 +// The video device buffers (the amount of V4L2 buffers) +#ifndef VIEWVIDEO_DEVICE_BUFFERS +#define VIEWVIDEO_DEVICE_BUFFERS 10 #endif +PRINT_CONFIG_VAR(VIEWVIDEO_DEVICE_BUFFERS); // Downsize factor for video stream -#ifndef VIDEO_DOWNSIZE_FACTOR -#define VIDEO_DOWNSIZE_FACTOR 4 +#ifndef VIEWVIDEO_DOWNSIZE_FACTOR +#define VIEWVIDEO_DOWNSIZE_FACTOR 4 #endif +PRINT_CONFIG_VAR(VIEWVIDEO_DOWNSIZE_FACTOR); // From 0 to 99 (99=high) -#ifndef VIDEO_QUALITY_FACTOR -#define VIDEO_QUALITY_FACTOR 50 +#ifndef VIEWVIDEO_QUALITY_FACTOR +#define VIEWVIDEO_QUALITY_FACTOR 50 #endif +PRINT_CONFIG_VAR(VIEWVIDEO_QUALITY_FACTOR); -// Frame Per Seconds -#ifndef VIDEO_FPS -#define VIDEO_FPS 4. +// Frames Per Seconds +#ifndef VIEWVIDEO_FPS +#define VIEWVIDEO_FPS 4 #endif +PRINT_CONFIG_VAR(VIEWVIDEO_FPS); -void viewvideo_run(void) {} +// The place where the shots are saved (without slash on the end) +#ifndef VIEWVIDEO_SHOT_PATH +#define VIEWVIDEO_SHOT_PATH "/data/video/images" +#endif +PRINT_CONFIG_VAR(VIEWVIDEO_SHOT_PATH); -// take shot flag -int viewvideo_shot = 0; -volatile int viewvideo_save_shot_number = 0; +/* These are defined with configure */ +PRINT_CONFIG_VAR(VIEWVIDEO_DEV); +PRINT_CONFIG_VAR(VIEWVIDEO_PORT_OUT); + +// Main thread +static void *viewvideo_thread(void *data); +void viewvideo_periodic(void) { } + +// Initialize the viewvideo structure with the defaults +struct viewvideo_t viewvideo = { + .is_streaming = FALSE, + .downsize_factor = VIEWVIDEO_DOWNSIZE_FACTOR, + .quality_factor = VIEWVIDEO_QUALITY_FACTOR, + .fps = VIEWVIDEO_FPS, + .take_shot = FALSE, + .shot_number = 0 +}; -///////////////////////////////////////////////////////////////////////// -// COMPUTER VISION THREAD - -pthread_t computervision_thread; -volatile uint8_t computervision_thread_status = 0; -volatile uint8_t computer_vision_thread_command = 0; -void *computervision_thread_main(void *data); -void *computervision_thread_main(void *data) +/** + * 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 *viewvideo_thread(void *data __attribute__((unused))) { - // Create a V4L2 device -#if USE_BOTTOM_CAMERA - struct v4l2_device *dev = v4l2_init("/dev/video2", 320, 240, 10); -#else - struct v4l2_device *dev = v4l2_init("/dev/video1", 1280, 720, 4); -#endif - if (dev == NULL) { - printf("Error initialising video\n"); + // Start the streaming of the V4L2 device + if (!v4l2_start_capture(viewvideo.dev)) { + printf("[viewvideo-thread] Could not start capture of %s.\n", viewvideo.dev->name); return 0; } - - // Start the streaming on the V4L2 device - if(!v4l2_start_capture(dev)) { - printf("Could not start capture\n"); - return 0; - } - - // Video Resizing - uint8_t quality_factor = VIDEO_QUALITY_FACTOR; - uint8_t dri_jpeg_header = 0; - int microsleep = (int)(1000000. / VIDEO_FPS); - + // Resize image if needed struct img_struct small; - small.w = dev->w / VIDEO_DOWNSIZE_FACTOR; - small.h = dev->h / VIDEO_DOWNSIZE_FACTOR; - small.buf = (uint8_t *)malloc(small.w * small.h * 2); - - // Video Compression - uint8_t *jpegbuf = (uint8_t *)malloc(dev->h * dev->w * 2); - - // Network Transmit - struct UdpSocket *vsock; - vsock = udp_socket(VIDEO_SOCK_IP, VIDEO_SOCK_OUT, VIDEO_SOCK_IN, FMS_BROADCAST); - - // Create SPD file and make folder if necessary - FILE *sdp; - if (system("mkdir -p /data/video/sdp") == 0) { - sdp = fopen("/data/video/sdp/x86_config-mjpeg.sdp", "w"); - if (sdp != NULL) { - fprintf(sdp, "v=0\n"); - fprintf(sdp, "m=video %d RTP/AVP 26\n", (int)(VIDEO_SOCK_OUT)); - fprintf(sdp, "c=IN IP4 0.0.0.0"); - fclose(sdp); - } + small.w = viewvideo.dev->w / viewvideo.downsize_factor; + small.h = viewvideo.dev->h / viewvideo.downsize_factor; + if (viewvideo.downsize_factor != 1) { + small.buf = (uint8_t *)malloc(small.w * small.h * 2); + } else { + small.buf = NULL; } - // file index (search from 0) - int file_index = 0; + // JPEG compression (8.25 bits are required for a 100% quality image) + uint8_t *jpegbuf = (uint8_t *)malloc(ceil(small.w * small.h * (8.25 / 8.))); // time + uint32_t microsleep = (uint32_t)(1000000. / (float)viewvideo.fps); struct timeval last_time; gettimeofday(&last_time, NULL); - while (computer_vision_thread_command > 0) { + // Start streaming + viewvideo.is_streaming = TRUE; + while (viewvideo.is_streaming) { // compute usleep to have a more stable frame rate struct timeval vision_thread_sleep_time; gettimeofday(&vision_thread_sleep_time, NULL); @@ -159,74 +152,65 @@ void *computervision_thread_main(void *data) if (dt < microsleep) { usleep(microsleep - dt); } last_time = vision_thread_sleep_time; - // Wait for a new frame - struct v4l2_img_buf *img = v4l2_image_get(dev); + // Wait for a new frame (blocking) + struct v4l2_img_buf *img = v4l2_image_get(viewvideo.dev); - // Save picture on disk - if (computer_vision_thread_command == 2) { - uint8_t *end = encode_image(img->buf, jpegbuf, 99, FOUR_TWO_TWO, dev->w, dev->h, 1); - uint32_t size = end - (jpegbuf); - FILE *save; + // Check if we need to take a shot + if (viewvideo.take_shot) { + // Create a high quality image (99% JPEG encoded) + uint8_t *jpegbuf_hr = (uint8_t *)malloc(ceil(viewvideo.dev->w * viewvideo.dev->h * (8.25 / 8.))); + uint8_t *end = jpeg_encode_image(img->buf, jpegbuf_hr, 99, FOUR_TWO_TWO, viewvideo.dev->w, viewvideo.dev->h, TRUE); + uint32_t size = end - (jpegbuf_hr); + + // Search for a file where we can write to char save_name[128]; -#if LOG_ON_USB - if (system("mkdir -p /data/video/usb/images") == 0) { -#else - if (system("mkdir -p /data/video/images") == 0) { -#endif - // search available index (max is 99) - for (; file_index < 99999; file_index++) { - printf("search %d\n", file_index); -#if LOG_ON_USB - sprintf(save_name, "/data/video/usb/images/img_%05d.jpg", file_index); -#else - sprintf(save_name, "/data/video/images/img_%05d.jpg", file_index); -#endif - // test if file exists or not - if (access(save_name, F_OK) == -1) { - printf("access\n"); - save = fopen(save_name, "w"); - if (save != NULL) { - // Atomic copy - viewvideo_save_shot_number = file_index; - fwrite(jpegbuf, sizeof(uint8_t), size, save); - fclose(save); - } else { - printf("Error when opening file %s\n", save_name); - } - // leave for loop - break; + for (; viewvideo.shot_number < 99999; viewvideo.shot_number++) { + sprintf(save_name, "%s/img_%05d.jpg", VIEWVIDEO_SHOT_PATH, viewvideo.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("[viewvideo-thread] Could not write shot %s.\n", save_name); } else { - //printf("file exists\n"); + // Save it to the file and close it + fwrite(jpegbuf_hr, sizeof(uint8_t), size, fp); + fclose(fp); } + + // We don't need to seek for a next index anymore + break; } } - if (computer_vision_thread_command == 2) - computer_vision_thread_command = 1; - viewvideo_shot = 0; + + // We finished the shot + free(jpegbuf_hr); + viewvideo.take_shot = FALSE; } - // Resize - struct img_struct input; - input.buf = img->buf; - input.w = dev->w; - input.h = dev->h; - resize_uyuv(&input, &small, VIDEO_DOWNSIZE_FACTOR); + // Only resize when needed + if (viewvideo.downsize_factor != 1) { + struct img_struct input; + input.buf = img->buf; + input.w = viewvideo.dev->w; + input.h = viewvideo.dev->h; + resize_uyuv(&input, &small, viewvideo.downsize_factor); + } else { + small.buf = img->buf; + } // JPEG encode the image: - uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h) - uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header); + uint8_t *end = jpeg_encode_image(small.buf, jpegbuf, VIEWVIDEO_QUALITY_FACTOR, FOUR_TWO_TWO, small.w, small.h, FALSE); uint32_t size = end - (jpegbuf); // Send image with RTP - printf("Sending an image ...%u\n", size); - send_rtp_frame( - vsock, // UDP - jpegbuf, size, // JPEG - small.w, small.h, // Img Size - 0, // Format 422 - quality_factor, // Jpeg-Quality - dri_jpeg_header, // DRI Header - 0 // 90kHz time increment + rtp_frame_send( + &VIEWVIDEO_DEV, // UDP device + jpegbuf, size, // JPEG + small.w, small.h, // Img Size + 0, // Format 422 + VIEWVIDEO_QUALITY_FACTOR, // Jpeg-Quality + 0, // DRI Header + 0 // 90kHz time increment ); // Extra note: when the time increment is set to 0, // it is automaticaly calculated by the send_rtp_frame function @@ -238,34 +222,99 @@ void *computervision_thread_main(void *data) // (1 = 1/90000 s) which is probably stupid but is actually working. // Free the image - v4l2_image_free(dev, img); + v4l2_image_free(viewvideo.dev, img); } - printf("Thread Closed\n"); - v4l2_close(dev); - computervision_thread_status = -100; return 0; } +/** + * Initialize the view video + */ +void viewvideo_init(void) +{ +#ifdef VIEWVIDEO_SUBDEV + PRINT_CONFIG_MSG("[viewvideo] Configuring a subdevice!"); + PRINT_CONFIG_VAR(VIEWVIDEO_SUBDEV); + + // Initialize the V4L2 subdevice (TODO: fix hardcoded path, which and code) + if (!v4l2_init_subdev("/dev/v4l-subdev0", 0, 1, V4L2_MBUS_FMT_UYVY8_2X8, VIEWVIDEO_DEVICE_SIZE)) { + printf("[viewvideo] Could not initialize the %s subdevice.\n", VIEWVIDEO_SUBDEV); + return; + } +#endif + + // Initialize the V4L2 device + viewvideo.dev = v4l2_init(VIEWVIDEO_DEVICE, VIEWVIDEO_DEVICE_SIZE, VIEWVIDEO_DEVICE_BUFFERS); + if (viewvideo.dev == NULL) { + printf("[viewvideo] Could not initialize the %s V4L2 device.\n", VIEWVIDEO_DEVICE); + return; + } + + // Create the shot directory + char save_name[128]; + sprintf(save_name, "mkdir -p %s", VIEWVIDEO_SHOT_PATH); + if (system(save_name) != 0) { + printf("[viewvideo] Could not create shot directory %s.\n", VIEWVIDEO_SHOT_PATH); + return; + } + + // Create an SDP file for the streaming + sprintf(save_name, "%s/stream.sdp", 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"); + fclose(fp); + } +} + +/** + * Start with streaming + */ void viewvideo_start(void) { - computer_vision_thread_command = 1; - int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL); - if (rc) { - printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); + // Check if we are already running + if (viewvideo.is_streaming) { + return; + } + + // Start the streaming thread + pthread_t tid; + if (pthread_create(&tid, NULL, viewvideo_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 viewvideo_stop(void) { - computer_vision_thread_command = 0; -} - -int viewvideo_save_shot(void) -{ - if (computer_vision_thread_command > 0) { - computer_vision_thread_command = 2; + // Check if not already stopped streaming + if (!viewvideo.is_streaming) { + return; } - return 0; + + // Stop the streaming thread + viewvideo.is_streaming = FALSE; + + // Stop the capturing + if (!v4l2_stop_capture(viewvideo.dev)) { + printf("[viewvideo] Could not stop capture of %s.\n", viewvideo.dev->name); + return; + } + + // TODO: wait for the thread to finish to be able to start the thread again! } - +/** + * Take a shot and save it + * This will only work when the streaming is enabled + */ +void viewvideo_take_shot(bool_t take) +{ + viewvideo.take_shot = take; +} diff --git a/sw/airborne/modules/computer_vision/viewvideo.h b/sw/airborne/modules/computer_vision/viewvideo.h index a4177815e9..8d24ebdf77 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.h +++ b/sw/airborne/modules/computer_vision/viewvideo.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2012-2014 The Paparazzi Community + * 2015 Freek van Tienen * * This file is part of Paparazzi. * @@ -31,21 +32,27 @@ #ifndef VIEW_VIDEO_H #define VIEW_VIDEO_H +#include "std.h" + +// Main viewvideo structure +struct viewvideo_t { + volatile bool_t is_streaming; //< When the device is streaming + struct v4l2_device *dev; //< The V4L2 device that is used for the video stream + uint8_t downsize_factor; //< Downsize factor during the stream + uint8_t quality_factor; //< Quality factor during the stream + uint8_t fps; //< The amount of frames per second + + volatile bool_t take_shot; //< Wether to take an image + uint16_t shot_number; //< The last shot number +}; +extern struct viewvideo_t viewvideo; + // Module functions -extern void viewvideo_run(void); +extern void viewvideo_init(void); +extern void viewvideo_periodic(void); //< A dummy for now extern void viewvideo_start(void); extern void viewvideo_stop(void); - -// Save picture on disk at full resolution -// can be called from flight plan -extern int viewvideo_save_shot(void); -extern volatile int viewvideo_save_shot_number; - -extern int viewvideo_shot; -#define viewvideo_SaveShot(_v) { \ - viewvideo_shot = 1; \ - viewvideo_save_shot(); \ - } +extern void viewvideo_take_shot(bool_t take); #endif /* VIEW_VIDEO_H */ diff --git a/sw/airborne/modules/computer_vision/viewvideo_nps.c b/sw/airborne/modules/computer_vision/viewvideo_nps.c index cc893e165a..f2f91a3ea3 100644 --- a/sw/airborne/modules/computer_vision/viewvideo_nps.c +++ b/sw/airborne/modules/computer_vision/viewvideo_nps.c @@ -19,21 +19,27 @@ * Boston, MA 02111-1307, USA. */ -/** Dummy C implementation for simulation +/** + * Dummy C implementation for simulation + * The V4L2 could also work in simulation, but must be adapted a bit. */ // Own header #include "viewvideo.h" -int viewvideo_shot = 0; +// Initialize the viewvideo structure with the defaults +struct viewvideo_t viewvideo = { + .is_streaming = FALSE, + .downsize_factor = 1, + .quality_factor = 99, + .fps = 30, + .take_shot = FALSE, + .shot_number = 0 +}; -void viewvideo_run(void) {} +// All dummy functions +void viewvideo_init(void) {} +void viewvideo_periodic(void) {} void viewvideo_start(void) {} void viewvideo_stop(void) {} - -int viewvideo_save_shot(void) -{ - return 0; -} - - +void viewvideo_take_shot(bool_t take __attribute__((unused))) {}