diff --git a/conf/airframes/ENAC/quadrotor/ard2_101.xml b/conf/airframes/ENAC/quadrotor/ard2_101.xml
new file mode 100644
index 0000000000..e898f3d937
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/ard2_101.xml
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/ard2_102.xml b/conf/airframes/ENAC/quadrotor/ard2_102.xml
new file mode 100644
index 0000000000..047c3c364f
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/ard2_102.xml
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/ard2_103.xml b/conf/airframes/ENAC/quadrotor/ard2_103.xml
new file mode 100644
index 0000000000..9ab4c43387
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/ard2_103.xml
@@ -0,0 +1,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/ard2_104.xml b/conf/airframes/ENAC/quadrotor/ard2_104.xml
new file mode 100644
index 0000000000..35798f50a1
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/ard2_104.xml
@@ -0,0 +1,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_control.xml b/conf/airframes/ENAC/quadrotor/ard2_base_control.xml
new file mode 100644
index 0000000000..8936b75b85
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/ard2_base_control.xml
@@ -0,0 +1,166 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml b/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml
new file mode 100644
index 0000000000..e948a401df
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/ard2_base_digit.xml
@@ -0,0 +1,81 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml b/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml
new file mode 100644
index 0000000000..e52c35ae25
--- /dev/null
+++ b/conf/airframes/ENAC/quadrotor/ard2_base_vision.xml
@@ -0,0 +1,86 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/image_nc_send.xml b/conf/modules/image_nc_send.xml
new file mode 100644
index 0000000000..ed5072a1e1
--- /dev/null
+++ b/conf/modules/image_nc_send.xml
@@ -0,0 +1,34 @@
+
+
+
+
+ Download Images from ARDone 2 with netcat
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/video_rtp_stream.xml b/conf/modules/video_rtp_stream.xml
new file mode 100644
index 0000000000..a7c02fc1dc
--- /dev/null
+++ b/conf/modules/video_rtp_stream.xml
@@ -0,0 +1,47 @@
+
+
+
+
+
+ Video stream of ARDone 2 front camera.
+
+ - 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)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/computer_vision/cv/color.h b/sw/airborne/modules/computer_vision/cv/color.h
new file mode 100644
index 0000000000..61a78bbca4
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/color.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2012-2013
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+
+#include
+#include "image.h"
+
+inline void grayscale_uyvy(struct img_struct* input, struct img_struct* output);
+inline void grayscale_uyvy(struct img_struct* input, struct img_struct* output)
+{
+ uint8_t *source = input->buf;
+ uint8_t *dest = output->buf;
+ source++;
+
+ for (int y=0;yh;y++)
+ {
+ for (int x=0;xw;x++)
+ {
+ // UYVY
+ *dest++ = 127; // U
+ *dest++ = *source; // Y
+ source+=2;
+ }
+ }
+}
+
+inline int colorfilt_uyvy(struct img_struct* input, struct img_struct* output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M);
+inline int colorfilt_uyvy(struct img_struct* input, struct img_struct* output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
+{
+ int cnt = 0;
+ uint8_t *source = input->buf;
+ uint8_t *dest = output->buf;
+
+ for (int y=0;yh;y++)
+ {
+ for (int x=0;xw;x+=2)
+ {
+ // Color Check:
+ if (
+ // Light
+ (dest[1] >= y_m)
+ && (dest[1] <= y_M)
+ && (dest[0] >= u_m)
+ && (dest[0] <= u_M)
+ && (dest[2] >= v_m)
+ && (dest[2] <= v_M)
+ )// && (dest[2] > 128))
+ {
+ cnt ++;
+ // UYVY
+ dest[0] = 64; // U
+ dest[1] = source[1]; // Y
+ dest[2] = 255; // V
+ dest[3] = source[3]; // Y
+ }
+ else
+ {
+ // UYVY
+ char u = source[0]-127;
+ u/=4;
+ dest[0] = 127; // U
+ dest[1] = source[1]; // Y
+ u = source[2]-127;
+ u/=4;
+ dest[2] = 127; // V
+ dest[3] = source[3]; // Y
+ }
+
+ dest+=4;
+ source+=4;
+ }
+ }
+ return cnt;
+}
+
diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c
new file mode 100644
index 0000000000..b9c32cb1d6
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c
@@ -0,0 +1,1102 @@
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ * 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)
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+#include "jpeg.h"
+
+
+static inline unsigned char svs_size_code(int w)
+{
+ // 1=(40,30) 2=(128,96) 3=(160,120) 5=(320,240) 7=(640,480) 9=(1280,1024);
+ if (w<=40)
+ return '1';
+ if (w<=128)
+ return '2';
+ if (w<=160)
+ return '3';
+ if (w<=320)
+ return '4';
+ if (w<=640)
+ return '7';
+ return '9';
+}
+
+int create_svs_jpeg_header(unsigned char* jpegbuf, int32_t size, int w)
+{
+ // SVS Surveyor Jpeg UDP format
+ uint32_t s = size;
+ uint8_t* p = (uint8_t*) & s;
+ jpegbuf[0]='#';
+ jpegbuf[1]='#';
+ jpegbuf[2]='I';
+ jpegbuf[3]='M';
+ jpegbuf[4]='J';
+ jpegbuf[5]=svs_size_code(w);
+ jpegbuf[6]=p[0];
+ jpegbuf[7]=p[1];
+ jpegbuf[8]=p[2];
+ jpegbuf[9]=0x00;
+ return size + 10;
+}
+
+
+typedef struct JPEG_ENCODER_STRUCTURE
+{
+ uint16_t mcu_width;
+ uint16_t mcu_height;
+ uint16_t horizontal_mcus;
+ uint16_t vertical_mcus;
+ uint16_t cols_in_right_mcus;
+ uint16_t rows_in_bottom_mcus;
+
+ uint16_t rows;
+ uint16_t cols;
+
+ uint16_t length_minus_mcu_width;
+ uint16_t length_minus_width;
+ uint16_t incr;
+ uint16_t mcu_width_size;
+ uint16_t offset;
+
+ int16_t ldc1;
+ int16_t ldc2;
+ int16_t ldc3;
+
+} JPEG_ENCODER_STRUCTURE;
+
+#define BLOCK_SIZE 64
+
+
+extern uint8_t Lqt [BLOCK_SIZE];
+extern uint8_t Cqt [BLOCK_SIZE];
+extern uint16_t ILqt [BLOCK_SIZE];
+extern uint16_t ICqt [BLOCK_SIZE];
+
+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];
+
+extern uint32_t lcode;
+extern uint16_t bitindex;
+
+void initialization (JPEG_ENCODER_STRUCTURE *, uint32_t, uint32_t, uint32_t);
+
+void initialize_quantization_tables (uint32_t);
+
+uint8_t* write_markers (uint8_t *, uint32_t, uint32_t, uint32_t);
+
+void read_400_format (JPEG_ENCODER_STRUCTURE *, uint8_t *);
+void read_422_format (JPEG_ENCODER_STRUCTURE *, uint8_t *);
+
+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 [] =
+{
+ 0x0000, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006,
+ 0x000E, 0x001E, 0x003E, 0x007E, 0x00FE, 0x01FE
+};
+
+uint16_t luminance_dc_size_table [] =
+{
+ 0x0002, 0x0003, 0x0003, 0x0003, 0x0003, 0x0003,
+ 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009
+};
+
+uint16_t chrominance_dc_code_table [] =
+{
+ 0x0000, 0x0001, 0x0002, 0x0006, 0x000E, 0x001E,
+ 0x003E, 0x007E, 0x00FE, 0x01FE, 0x03FE, 0x07FE
+};
+
+uint16_t chrominance_dc_size_table [] =
+{
+ 0x0002, 0x0002, 0x0002, 0x0003, 0x0004, 0x0005,
+ 0x0006, 0x0007, 0x0008, 0x0009, 0x000A, 0x000B
+};
+
+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,
+ 0x001C, 0x00F9, 0x03F7, 0x0FF4, 0xFF89, 0xFF8A, 0xFF8b, 0xFF8C, 0xFF8D, 0xFF8E,
+ 0x003A, 0x01F7, 0x0FF5, 0xFF8F, 0xFF90, 0xFF91, 0xFF92, 0xFF93, 0xFF94, 0xFF95,
+ 0x003B, 0x03F8, 0xFF96, 0xFF97, 0xFF98, 0xFF99, 0xFF9A, 0xFF9B, 0xFF9C, 0xFF9D,
+ 0x007A, 0x07F7, 0xFF9E, 0xFF9F, 0xFFA0, 0xFFA1, 0xFFA2, 0xFFA3, 0xFFA4, 0xFFA5,
+ 0x007B, 0x0FF6, 0xFFA6, 0xFFA7, 0xFFA8, 0xFFA9, 0xFFAA, 0xFFAB, 0xFFAC, 0xFFAD,
+ 0x00FA, 0x0FF7, 0xFFAE, 0xFFAF, 0xFFB0, 0xFFB1, 0xFFB2, 0xFFB3, 0xFFB4, 0xFFB5,
+ 0x01F8, 0x7FC0, 0xFFB6, 0xFFB7, 0xFFB8, 0xFFB9, 0xFFBA, 0xFFBB, 0xFFBC, 0xFFBD,
+ 0x01F9, 0xFFBE, 0xFFBF, 0xFFC0, 0xFFC1, 0xFFC2, 0xFFC3, 0xFFC4, 0xFFC5, 0xFFC6,
+ 0x01FA, 0xFFC7, 0xFFC8, 0xFFC9, 0xFFCA, 0xFFCB, 0xFFCC, 0xFFCD, 0xFFCE, 0xFFCF,
+ 0x03F9, 0xFFD0, 0xFFD1, 0xFFD2, 0xFFD3, 0xFFD4, 0xFFD5, 0xFFD6, 0xFFD7, 0xFFD8,
+ 0x03FA, 0xFFD9, 0xFFDA, 0xFFDB, 0xFFDC, 0xFFDD, 0xFFDE, 0xFFDF, 0xFFE0, 0xFFE1,
+ 0x07F8, 0xFFE2, 0xFFE3, 0xFFE4, 0xFFE5, 0xFFE6, 0xFFE7, 0xFFE8, 0xFFE9, 0xFFEA,
+ 0xFFEB, 0xFFEC, 0xFFED, 0xFFEE, 0xFFEF, 0xFFF0, 0xFFF1, 0xFFF2, 0xFFF3, 0xFFF4,
+ 0xFFF5, 0xFFF6, 0xFFF7, 0xFFF8, 0xFFF9, 0xFFFA, 0xFFFB, 0xFFFC, 0xFFFD, 0xFFFE,
+ 0x07F9
+};
+
+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,
+ 0x0005, 0x0008, 0x000A, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0006, 0x0009, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0006, 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0007, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0008, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0009, 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x000B
+};
+
+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,
+ 0x001A, 0x00F7, 0x03F7, 0x0FF6, 0x7FC2, 0xFF8C, 0xFF8D, 0xFF8E, 0xFF8F, 0xFF90,
+ 0x001B, 0x00F8, 0x03F8, 0x0FF7, 0xFF91, 0xFF92, 0xFF93, 0xFF94, 0xFF95, 0xFF96,
+ 0x003A, 0x01F6, 0xFF97, 0xFF98, 0xFF99, 0xFF9A, 0xFF9B, 0xFF9C, 0xFF9D, 0xFF9E,
+ 0x003B, 0x03F9, 0xFF9F, 0xFFA0, 0xFFA1, 0xFFA2, 0xFFA3, 0xFFA4, 0xFFA5, 0xFFA6,
+ 0x0079, 0x07F7, 0xFFA7, 0xFFA8, 0xFFA9, 0xFFAA, 0xFFAB, 0xFFAC, 0xFFAD, 0xFFAE,
+ 0x007A, 0x07F8, 0xFFAF, 0xFFB0, 0xFFB1, 0xFFB2, 0xFFB3, 0xFFB4, 0xFFB5, 0xFFB6,
+ 0x00F9, 0xFFB7, 0xFFB8, 0xFFB9, 0xFFBA, 0xFFBB, 0xFFBC, 0xFFBD, 0xFFBE, 0xFFBF,
+ 0x01F7, 0xFFC0, 0xFFC1, 0xFFC2, 0xFFC3, 0xFFC4, 0xFFC5, 0xFFC6, 0xFFC7, 0xFFC8,
+ 0x01F8, 0xFFC9, 0xFFCA, 0xFFCB, 0xFFCC, 0xFFCD, 0xFFCE, 0xFFCF, 0xFFD0, 0xFFD1,
+ 0x01F9, 0xFFD2, 0xFFD3, 0xFFD4, 0xFFD5, 0xFFD6, 0xFFD7, 0xFFD8, 0xFFD9, 0xFFDA,
+ 0x01FA, 0xFFDB, 0xFFDC, 0xFFDD, 0xFFDE, 0xFFDF, 0xFFE0, 0xFFE1, 0xFFE2, 0xFFE3,
+ 0x07F9, 0xFFE4, 0xFFE5, 0xFFE6, 0xFFE7, 0xFFE8, 0xFFE9, 0xFFEA, 0xFFEb, 0xFFEC,
+ 0x3FE0, 0xFFED, 0xFFEE, 0xFFEF, 0xFFF0, 0xFFF1, 0xFFF2, 0xFFF3, 0xFFF4, 0xFFF5,
+ 0x7FC3, 0xFFF6, 0xFFF7, 0xFFF8, 0xFFF9, 0xFFFA, 0xFFFB, 0xFFFC, 0xFFFD, 0xFFFE,
+ 0x03FA
+};
+
+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,
+ 0x0005, 0x0008, 0x000A, 0x000C, 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0005, 0x0008, 0x000A, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0006, 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0006, 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0008, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x000E, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010,
+ 0x000A
+};
+
+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,
+ 5, 5, 5, 5, 5, 5, 5, 5,
+ 6, 6, 6, 6, 6, 6, 6, 6,
+ 6, 6, 6, 6, 6, 6, 6, 6,
+ 6, 6, 6, 6, 6, 6, 6, 6,
+ 6, 6, 6, 6, 6, 6, 6, 6,
+ 7, 7, 7, 7, 7, 7, 7, 7,
+ 7, 7, 7, 7, 7, 7, 7, 7,
+ 7, 7, 7, 7, 7, 7, 7, 7,
+ 7, 7, 7, 7, 7, 7, 7, 7,
+ 7, 7, 7, 7, 7, 7, 7, 7,
+ 7, 7, 7, 7, 7, 7, 7, 7,
+ 7, 7, 7, 7, 7, 7, 7, 7,
+ 7, 7, 7, 7, 7, 7, 7, 7,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8,
+ 8, 8, 8, 8, 8, 8, 8, 8
+};
+
+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,
+
+0xFF, 0xC4, 0x00, 0x1F, 0x01, 0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B,
+
+0xFF, 0xC4, 0x00, 0xB5, 0x11, 0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04, 0x07, 0x05, 0x04, 0x04, 0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21, 0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xA1, 0xB1, 0xC1, 0x09, 0x23, 0x33, 0x52, 0xF0, 0x15, 0x62, 0x72, 0xD1, 0x0A, 0x16, 0x24, 0x34, 0xE1, 0x25, 0xF1, 0x17, 0x18, 0x19, 0x1A, 0x26, 0x27, 0x28, 0x29, 0x2A, 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, 0x82, 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, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA,
+};
+
+
+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,
+ 9, 11, 18, 24, 31, 40, 44, 53,
+ 10, 19, 23, 32, 39, 45, 52, 54,
+ 20, 22, 33, 38, 46, 51, 55, 60,
+ 21, 34, 37, 47, 50, 56, 59, 61,
+ 35, 36, 48, 49, 57, 58, 62, 63
+};
+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,
+ 14, 17, 22, 29, 51, 87, 80, 62,
+ 18, 22, 37, 56, 68, 109, 103, 77,
+ 24, 35, 55, 64, 81, 104, 113, 92,
+ 49, 64, 78, 87, 103, 121, 120, 101,
+ 72, 92, 95, 98, 112, 100, 103, 99
+};
+
+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,
+ 47, 66, 99, 99, 99, 99, 99, 99,
+ 99, 99, 99, 99, 99, 99, 99, 99,
+ 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];
+
+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;
+
+
+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)
+{
+ uint16_t mcu_width, mcu_height, bytes_per_pixel;
+
+ if (image_format == FOUR_ZERO_ZERO)
+ {
+ jpeg->mcu_width = mcu_width = 8;
+ jpeg->mcu_height = mcu_height = 8;
+
+ jpeg->horizontal_mcus = (uint16_t) ((image_width + mcu_width - 1) >> 3);
+ jpeg->vertical_mcus = (uint16_t) ((image_height + mcu_height - 1) >> 3);
+
+ bytes_per_pixel = 1;
+ read_format = read_400_format;
+ }
+ else
+ {
+ jpeg->mcu_width = mcu_width = 16;
+ jpeg->horizontal_mcus = (uint16_t) ((image_width + mcu_width - 1) >> 4);
+
+ 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;
+ }
+
+ jpeg->rows_in_bottom_mcus = (uint16_t) (image_height - (jpeg->vertical_mcus - 1) * mcu_height);
+ jpeg->cols_in_right_mcus = (uint16_t) (image_width - (jpeg->horizontal_mcus - 1) * mcu_width);
+
+ jpeg->length_minus_mcu_width = (uint16_t) ((image_width - mcu_width) * bytes_per_pixel);
+ jpeg->length_minus_width = (uint16_t) ((image_width - jpeg->cols_in_right_mcus) * bytes_per_pixel);
+
+ jpeg->mcu_width_size = (uint16_t) (mcu_width * bytes_per_pixel);
+
+ jpeg->offset = (uint16_t) ((image_width * (mcu_height - 1) - (mcu_width - jpeg->cols_in_right_mcus)) * bytes_per_pixel);
+
+ jpeg->ldc1 = 0;
+ jpeg->ldc2 = 0;
+ jpeg->ldc3 = 0;
+ lcode = 0;
+ bitindex = 0;
+}
+
+/////////////////////////////////////////////////////////////
+
+////////////////////////////////////////
+// Q = 0-99
+
+/*
+ * Table K.1 from JPEG spec.
+ */
+static const int jpeg_luma_quantizer[64] = {
+ 16, 11, 10, 16, 24, 40, 51, 61,
+ 12, 12, 14, 19, 26, 58, 60, 55,
+ 14, 13, 16, 24, 40, 57, 69, 56,
+ 14, 17, 22, 29, 51, 87, 80, 62,
+ 18, 22, 37, 56, 68, 109, 103, 77,
+ 24, 35, 55, 64, 81, 104, 113, 92,
+ 49, 64, 78, 87, 103, 121, 120, 101,
+ 72, 92, 95, 98, 112, 100, 103, 99
+};
+
+/*
+ * Table K.2 from JPEG spec.
+ */
+static const int jpeg_chroma_quantizer[64] = {
+ 17, 18, 24, 47, 99, 99, 99, 99,
+ 18, 21, 26, 66, 99, 99, 99, 99,
+ 24, 26, 56, 99, 99, 99, 99, 99,
+ 47, 66, 99, 99, 99, 99, 99, 99,
+ 99, 99, 99, 99, 99, 99, 99, 99,
+ 99, 99, 99, 99, 99, 99, 99, 99,
+ 99, 99, 99, 99, 99, 99, 99, 99,
+ 99, 99, 99, 99, 99, 99, 99, 99
+};
+
+/*
+ * Call MakeTables with the Q factor and two u_char[64] return arrays
+ */
+void MakeTables(int q);
+void MakeTables(int q)
+{
+ int i;
+ int factor = q;
+
+ if (q < 1) factor = 1;
+ if (q > 99) factor = 99;
+ if (q < 50)
+ q = 5000 / factor;
+ else
+ q = 200 - factor*2;
+
+
+ for (i=0; i < 64; i++) {
+ int lq = (jpeg_luma_quantizer[i] * q + 50) / 100;
+ int cq = (jpeg_chroma_quantizer[i] * q + 50) / 100;
+
+ /* Limit the quantizers to 1 <= q <= 255 */
+ if (lq < 1) lq = 1;
+ else if (lq > 255) lq = 255;
+ Lqt [i] = (uint8_t) lq;
+ ILqt [i] = 0x8000 / lq;
+
+ if (cq < 1) cq = 1;
+ else if (cq > 255) cq = 255;
+ Cqt [i] = (uint8_t) cq;
+ //ICqt [i] = DSP_Division (0x8000, value);
+ ICqt [i] = 0x8000 / cq;
+ }
+}
+
+
+
+
+
+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)
+{
+ uint16_t i, j;
+
+ JPEG_ENCODER_STRUCTURE JpegStruct;
+ JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure = &JpegStruct;
+
+ /* Initialization of JPEG control structure */
+ initialization (jpeg_encoder_structure,image_format,image_width,image_height);
+
+ /* Quantization Table Initialization */
+ //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);
+ }
+
+ for (i=1; i<=jpeg_encoder_structure->vertical_mcus; i++)
+ {
+ if (i < jpeg_encoder_structure->vertical_mcus)
+ jpeg_encoder_structure->rows = jpeg_encoder_structure->mcu_height;
+ else
+ jpeg_encoder_structure->rows = jpeg_encoder_structure->rows_in_bottom_mcus;
+
+ for (j=1; j<=jpeg_encoder_structure->horizontal_mcus; j++)
+ {
+ if (j < jpeg_encoder_structure->horizontal_mcus)
+ {
+ jpeg_encoder_structure->cols = jpeg_encoder_structure->mcu_width;
+ jpeg_encoder_structure->incr = jpeg_encoder_structure->length_minus_mcu_width;
+ }
+ else
+ {
+ jpeg_encoder_structure->cols = jpeg_encoder_structure->cols_in_right_mcus;
+ jpeg_encoder_structure->incr = jpeg_encoder_structure->length_minus_width;
+ }
+
+ read_format (jpeg_encoder_structure, input_ptr);
+
+ /* Encode the data in MCU */
+ output_ptr = encodeMCU (jpeg_encoder_structure, image_format, output_ptr);
+
+ input_ptr += jpeg_encoder_structure->mcu_width_size;
+ }
+
+ input_ptr += jpeg_encoder_structure->offset;
+ }
+
+ /* Close Routine */
+ output_ptr = close_bitstream (output_ptr);
+ return output_ptr;
+}
+
+uint8_t* 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);
+
+ if (image_format == FOUR_TWO_TWO)
+ {
+ levelshift (Y2);
+ DCT (Y2);
+ quantization (Y2, ILqt);
+ output_ptr = huffman (jpeg_encoder_structure, 1, output_ptr);
+
+ levelshift (CB);
+ DCT (CB);
+ quantization (CB, ICqt);
+ output_ptr = huffman (jpeg_encoder_structure, 2, output_ptr);
+
+ levelshift (CR);
+ DCT (CR);
+ quantization (CR, ICqt);
+ output_ptr = 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)
+{
+ int16_t i;
+
+ for (i=63; i>=0; i--)
+ data [i] -= 128;
+}
+
+/* DCT for One block(8x8) */
+void DCT (int16_t *data)
+{
+ //_r8x8dct(data, fdct_coeff, fdct_temp);
+
+
+ uint16_t i;
+ int32_t x0, x1, x2, x3, x4, x5, x6, x7, x8;
+
+ static const uint16_t c1=1420; // cos PI/16 * root(2)
+ static const uint16_t c2=1338; // cos PI/8 * root(2)
+ static const uint16_t c3=1204; // cos 3PI/16 * root(2)
+ static const uint16_t c5=805; // cos 5PI/16 * root(2)
+ static const uint16_t c6=554; // cos 3PI/8 * root(2)
+ static const uint16_t c7=283; // cos 7PI/16 * root(2)
+
+ static const uint16_t s1=3;
+ static const uint16_t s2=10;
+ static const uint16_t s3=13;
+
+ for (i=8; i>0; i--)
+ {
+ x8 = data [0] + data [7];
+ x0 = data [0] - data [7];
+
+ x7 = data [1] + data [6];
+ x1 = data [1] - data [6];
+
+ x6 = data [2] + data [5];
+ x2 = data [2] - data [5];
+
+ x5 = data [3] + data [4];
+ x3 = data [3] - data [4];
+
+ x4 = x8 + x5;
+ x8 -= x5;
+
+ x5 = x7 + x6;
+ x7 -= x6;
+
+ data [0] = (int16_t) (x4 + x5);
+ data [4] = (int16_t) (x4 - x5);
+
+ data [2] = (int16_t) ((x8*c2 + x7*c6) >> s2);
+ data [6] = (int16_t) ((x8*c6 - x7*c2) >> s2);
+
+ data [7] = (int16_t) ((x0*c7 - x1*c5 + x2*c3 - x3*c1) >> s2);
+ data [5] = (int16_t) ((x0*c5 - x1*c1 + x2*c7 + x3*c3) >> s2);
+ data [3] = (int16_t) ((x0*c3 - x1*c7 - x2*c1 - x3*c5) >> s2);
+ data [1] = (int16_t) ((x0*c1 + x1*c3 + x2*c5 + x3*c7) >> s2);
+
+ data += 8;
+ }
+
+ data -= 64;
+
+ for (i=8; i>0; i--)
+ {
+ x8 = data [0] + data [56];
+ x0 = data [0] - data [56];
+
+ x7 = data [8] + data [48];
+ x1 = data [8] - data [48];
+
+ x6 = data [16] + data [40];
+ x2 = data [16] - data [40];
+
+ x5 = data [24] + data [32];
+ x3 = data [24] - data [32];
+
+ x4 = x8 + x5;
+ x8 -= x5;
+
+ x5 = x7 + x6;
+ x7 -= x6;
+
+ data [0] = (int16_t) ((x4 + x5) >> s1);
+ data [32] = (int16_t) ((x4 - x5) >> s1);
+
+ data [16] = (int16_t) ((x8*c2 + x7*c6) >> s3);
+ data [48] = (int16_t) ((x8*c6 - x7*c2) >> s3);
+
+ data [56] = (int16_t) ((x0*c7 - x1*c5 + x2*c3 - x3*c1) >> s3);
+ data [40] = (int16_t) ((x0*c5 - x1*c1 + x2*c7 + x3*c3) >> s3);
+ data [24] = (int16_t) ((x0*c3 - x1*c7 - x2*c1 - x3*c5) >> s3);
+ data [8] = (int16_t) ((x0*c1 + x1*c3 + x2*c5 + x3*c7) >> s3);
+
+ data++;
+ }
+}
+
+#define PUTBITS \
+{ \
+ bits_in_next_word = (int16_t) (bitindex + numbits - 32); \
+ if (bits_in_next_word < 0) \
+ { \
+ lcode = (lcode << numbits) | data; \
+ bitindex += numbits; \
+ } \
+ else \
+ { \
+ lcode = (lcode << (32 - bitindex)) | (data >> bits_in_next_word); \
+ if ((*output_ptr++ = (uint8_t)(lcode >> 24)) == 0xff) \
+ *output_ptr++ = 0; \
+ if ((*output_ptr++ = (uint8_t)(lcode >> 16)) == 0xff) \
+ *output_ptr++ = 0; \
+ if ((*output_ptr++ = (uint8_t)(lcode >> 8)) == 0xff) \
+ *output_ptr++ = 0; \
+ if ((*output_ptr++ = (uint8_t) lcode) == 0xff) \
+ *output_ptr++ = 0; \
+ lcode = data; \
+ bitindex = bits_in_next_word; \
+ } \
+}
+
+uint8_t* huffman (JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint16_t component, uint8_t *output_ptr)
+{
+ uint16_t i;
+ uint16_t *DcCodeTable, *DcSizeTable, *AcCodeTable, *AcSizeTable;
+
+ int16_t *Temp_Ptr, Coeff, LastDc;
+ uint16_t AbsCoeff, HuffCode, HuffSize, RunLength=0, DataSize=0, index;
+
+ int16_t bits_in_next_word;
+ uint16_t numbits;
+ uint32_t data;
+
+ Temp_Ptr = Temp;
+ Coeff = *Temp_Ptr++;
+
+ if (component == 1)
+ {
+ DcCodeTable = luminance_dc_code_table;
+ DcSizeTable = luminance_dc_size_table;
+ AcCodeTable = luminance_ac_code_table;
+ AcSizeTable = luminance_ac_size_table;
+
+ LastDc = jpeg_encoder_structure->ldc1;
+ jpeg_encoder_structure->ldc1 = Coeff;
+ }
+ else
+ {
+ DcCodeTable = chrominance_dc_code_table;
+ DcSizeTable = chrominance_dc_size_table;
+ AcCodeTable = chrominance_ac_code_table;
+ AcSizeTable = chrominance_ac_size_table;
+
+ if (component == 2)
+ {
+ LastDc = jpeg_encoder_structure->ldc2;
+ jpeg_encoder_structure->ldc2 = Coeff;
+ }
+ else
+ {
+ LastDc = jpeg_encoder_structure->ldc3;
+ jpeg_encoder_structure->ldc3 = Coeff;
+ }
+ }
+
+ Coeff -= LastDc;
+
+ AbsCoeff = (Coeff < 0) ? -Coeff-- : Coeff;
+
+ while (AbsCoeff != 0)
+ {
+ AbsCoeff >>= 1;
+ DataSize++;
+ }
+
+ HuffCode = DcCodeTable [DataSize];
+ HuffSize = DcSizeTable [DataSize];
+
+ Coeff &= (1 << DataSize) - 1;
+ data = (HuffCode << DataSize) | Coeff;
+ numbits = HuffSize + DataSize;
+
+ PUTBITS
+
+ for (i=63; i>0; i--)
+ {
+ if ((Coeff = *Temp_Ptr++) != 0)
+ {
+ while (RunLength > 15)
+ {
+ RunLength -= 16;
+ data = AcCodeTable [161];
+ numbits = AcSizeTable [161];
+ PUTBITS
+ }
+
+ AbsCoeff = (Coeff < 0) ? -Coeff-- : Coeff;
+
+ if (AbsCoeff >> 8 == 0)
+ DataSize = bitsize [AbsCoeff];
+ else
+ DataSize = bitsize [AbsCoeff >> 8] + 8;
+
+ index = RunLength * 10 + DataSize;
+ HuffCode = AcCodeTable [index];
+ HuffSize = AcSizeTable [index];
+
+ Coeff &= (1 << DataSize) - 1;
+ data = (HuffCode << DataSize) | Coeff;
+ numbits = HuffSize + DataSize;
+
+ PUTBITS
+ RunLength = 0;
+ }
+ else
+ RunLength++;
+ }
+
+ if (RunLength != 0)
+ {
+ data = AcCodeTable [0];
+ numbits = AcSizeTable [0];
+ PUTBITS
+ }
+ return output_ptr;
+}
+
+/* For bit Stuffing and EOI marker */
+uint8_t* close_bitstream (uint8_t *output_ptr)
+{
+ uint16_t i, count;
+ uint8_t *ptr;
+
+ if (bitindex > 0)
+ {
+ lcode <<= (32 - bitindex);
+ //for (i=0; i> 3;
+
+ ptr = (uint8_t *) &lcode + 3;
+
+ for (i=0; i> 8);
+ *output_ptr++ = (uint8_t) header_length;
+
+ // Precision (P)
+ *output_ptr++ = 0x08;
+
+ // image height
+ *output_ptr++ = (uint8_t) (image_height >> 8);
+ *output_ptr++ = (uint8_t) image_height;
+
+ // image width
+ *output_ptr++ = (uint8_t) (image_width >> 8);
+ *output_ptr++ = (uint8_t) image_width;
+
+ // Nf
+ *output_ptr++ = number_of_components;
+
+ if (image_format == FOUR_ZERO_ZERO)
+ {
+ *output_ptr++ = 0x01;
+ *output_ptr++ = 0x11;
+ *output_ptr++ = 0x00;
+ }
+ else
+ {
+ *output_ptr++ = 0x01;
+
+ if (image_format == FOUR_TWO_TWO)
+ *output_ptr++ = 0x21;
+ else
+ *output_ptr++ = 0x11;
+
+ *output_ptr++ = 0x00;
+
+ *output_ptr++ = 0x02;
+ *output_ptr++ = 0x11;
+ *output_ptr++ = 0x01;
+
+ *output_ptr++ = 0x03;
+ *output_ptr++ = 0x11;
+ *output_ptr++ = 0x01;
+ }
+
+ // huffman table(DHT)
+
+ for (i=0; i> 8);
+ *output_ptr++ = (uint8_t) header_length;
+
+ // Ns
+ *output_ptr++ = number_of_components;
+
+ if (image_format == FOUR_ZERO_ZERO)
+ {
+ *output_ptr++ = 0x01;
+ *output_ptr++ = 0x00;
+ }
+ else
+ {
+ *output_ptr++ = 0x01;
+ *output_ptr++ = 0x00;
+
+ *output_ptr++ = 0x02;
+ *output_ptr++ = 0x11;
+
+ *output_ptr++ = 0x03;
+ *output_ptr++ = 0x11;
+ }
+
+ *output_ptr++ = 0x00;
+ *output_ptr++ = 0x3F;
+ *output_ptr++ = 0x00;
+ return output_ptr;
+}
+
+/* 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)
+{
+ uint16_t i, index;
+ uint32_t value;
+
+ if (quality_factor < 1)
+ quality_factor = 1;
+ if (quality_factor > 8)
+ quality_factor = 8;
+ quality_factor = ((quality_factor * 3) - 2) * 128; //converts range[1:8] to [1:22]
+
+ for (i=0; i<64; i++)
+ {
+ index = zigzag_table [i];
+
+ /* luminance quantization table * quality factor */
+ value = luminance_quant_table [i] * quality_factor;
+ value = (value + 0x200) >> 10;
+
+ if (value < 2)
+ value = 2;
+ else if (value > 255)
+ value = 255;
+
+ Lqt [index] = (uint8_t) value;
+ //ILqt [i] = DSP_Division (0x8000, value);
+ ILqt [i] = 0x8000 / value;
+
+ /* chrominance quantization table * quality factor */
+ value = chrominance_quant_table [i] * quality_factor;
+ value = (value + 0x200) >> 10;
+
+ if (value < 2)
+ value = 2;
+ else if (value > 255)
+ value = 255;
+
+ Cqt [index] = (uint8_t) value;
+ //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)
+{
+ int16_t i;
+ int32_t value;
+
+ for (i=63; i>=0; i--)
+ {
+ value = data [i] * quant_table_ptr [i];
+ value = (value + 0x4000) >> 15;
+
+ Temp [zigzag_table [i]] = (int16_t) value;
+ }
+}
+
+void read_400_format (JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr)
+{
+ int32_t i, j;
+ int16_t *Y1_Ptr = Y1;
+
+ uint16_t rows = jpeg_encoder_structure->rows;
+ uint16_t cols = jpeg_encoder_structure->cols;
+ uint16_t incr = jpeg_encoder_structure->incr;
+
+ for (i=rows; i>0; i--)
+ {
+ for (j=cols; j>0; j--)
+ *Y1_Ptr++ = *input_ptr++;
+
+ for (j=8-cols; j>0; j--) {
+ *Y1_Ptr = *(Y1_Ptr - 1);
+ Y1_Ptr++;
+ }
+
+ input_ptr += incr;
+ }
+
+ for (i=8-rows; i>0; i--)
+ {
+ for (j=8; j>0; j--) {
+ *Y1_Ptr = *(Y1_Ptr - 8);
+ Y1_Ptr++;
+ }
+ }
+}
+
+void read_422_format (JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr)
+{
+ int32_t i, j;
+ uint16_t Y1_cols, Y2_cols;
+
+ int16_t *Y1_Ptr = Y1;
+ int16_t *Y2_Ptr = Y2;
+ int16_t *CB_Ptr = CB;
+ int16_t *CR_Ptr = CR;
+
+ uint16_t rows = jpeg_encoder_structure->rows;
+ uint16_t cols = jpeg_encoder_structure->cols;
+ uint16_t incr = jpeg_encoder_structure->incr;
+
+ if (cols <= 8)
+ {
+ Y1_cols = cols;
+ Y2_cols = 0;
+ }
+ else
+ {
+ Y1_cols = 8;
+ Y2_cols = (uint16_t) (cols - 8);
+ }
+
+ for (i=rows; i>0; i--)
+ {
+ for (j=Y1_cols>>1; j>0; j--)
+ {
+ *CB_Ptr++ = *input_ptr++;
+ *Y1_Ptr++ = *input_ptr++;
+ *CR_Ptr++ = *input_ptr++;
+ *Y1_Ptr++ = *input_ptr++;
+ }
+
+ for (j=Y2_cols>>1; j>0; j--)
+ {
+ *CB_Ptr++ = *input_ptr++;
+ *Y2_Ptr++ = *input_ptr++;
+ *CR_Ptr++ = *input_ptr++;
+ *Y2_Ptr++ = *input_ptr++;
+ }
+
+ if (cols <= 8)
+ {
+ for (j=8-Y1_cols; j>0; j--) {
+ *Y1_Ptr = *(Y1_Ptr - 1);
+ Y1_Ptr++;
+ }
+
+ for (j=8-Y2_cols; j>0; j--) {
+ *Y2_Ptr = *(Y1_Ptr - 1);
+ Y2_Ptr++;
+ }
+ }
+ else
+ {
+ for (j=8-Y2_cols; j>0; j--) {
+ *Y2_Ptr = *(Y2_Ptr - 1);
+ Y2_Ptr++;
+ }
+ }
+
+ for (j=(16-cols)>>1; j>0; j--)
+ {
+ *CB_Ptr = *(CB_Ptr-1); CB_Ptr++;
+ *CR_Ptr = *(CR_Ptr-1); CR_Ptr++;
+ }
+
+ input_ptr += incr;
+ }
+
+ for (i=8-rows; i>0; i--)
+ {
+ for (j=8; j>0; j--)
+ {
+ *Y1_Ptr = *(Y1_Ptr - 8); Y1_Ptr++;
+ *Y2_Ptr = *(Y2_Ptr - 8); Y2_Ptr++;
+ *CB_Ptr = *(CB_Ptr - 8); CB_Ptr++;
+ *CR_Ptr = *(CR_Ptr - 8); CR_Ptr++;
+ }
+ }
+}
+
diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h
new file mode 100644
index 0000000000..e275c28fd9
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h
@@ -0,0 +1,45 @@
+
+#ifndef __MY_JPEG_HEADER__
+#define __MY_JPEG_HEADER__
+
+#include
+
+#define FOUR_ZERO_ZERO 0
+#define FOUR_TWO_ZERO 1
+#define FOUR_TWO_TWO 2
+#define FOUR_FOUR_FOUR 3
+#define RGB 4
+
+unsigned char *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
+);
+
+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
+);
+
+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
diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c
new file mode 100644
index 0000000000..aa66af2051
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c
@@ -0,0 +1,196 @@
+
+#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);
+
+// http://www.ietf.org/rfc/rfc3550.txt
+
+#define KJpegCh1ScanDataLen 32
+#define KJpegCh2ScanDataLen 56
+
+// RGB JPEG images as RTP payload - 64x48 pixel
+uint8_t JpegScanDataCh2A[KJpegCh2ScanDataLen] =
+{
+ 0xf8, 0xbe, 0x8a, 0x28, 0xaf, 0xe5, 0x33, 0xfd,
+ 0xfc, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
+};
+uint8_t JpegScanDataCh2B[KJpegCh2ScanDataLen] =
+{
+ 0xf5, 0x8a, 0x28, 0xa2, 0xbf, 0xca, 0xf3, 0xfc,
+ 0x53, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
+ 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
+};
+
+
+
+
+
+void test_rtp_frame(struct UdpSocket *sock)
+{
+ static uint32_t framecounter = 0;
+ static uint32_t timecounter = 0;
+ static uint8_t toggle = 0;
+ toggle = ! toggle;
+
+ uint8_t format_code = 0x01;
+ uint8_t quality_code = 0x54;
+
+ if (toggle)
+ {
+ send_rtp_packet(sock, 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, 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, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
+{
+ static uint32_t packetcounter = 0;
+ static uint32_t timecounter = 0;
+ uint32_t offset = 0;
+
+#define MAX_PACKET_SIZE 1400
+
+ if (delta_t <= 0)
+ {
+ struct timeval tv;
+ gettimeofday(&tv, 0);
+ uint32_t t = (tv.tv_sec % (256*256)) * 90000 + tv.tv_usec * 9 / 100;
+ timecounter = t;
+ }
+
+ // Split frame into packets
+ for (;JpegLen > 0;)
+ {
+ uint32_t len = MAX_PACKET_SIZE;
+ uint8_t lastpacket = 0;
+
+ if (JpegLen <= len)
+ {
+ lastpacket = 1;
+ len = JpegLen;
+ }
+
+ send_rtp_packet(sock, Jpeg,len,packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header);
+
+ JpegLen -= len;
+ Jpeg += len;
+ offset += len;
+ packetcounter++;
+ }
+
+
+ if (delta_t > 0)
+ {
+ // timestamp = 1 / 90 000 seconds
+ timecounter+=delta_t;
+ }
+}
+
+/*
+ * The RTP timestamp is in units of 90000Hz. The same timestamp MUST
+ appear in each fragment of a given frame. The RTP marker bit MUST be
+ set in the last packet of a frame.
+ *
+ */
+
+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)
+{
+
+#define KRtpHeaderSize 12 // size of the RTP header
+#define KJpegHeaderSize 8 // size of the special JPEG payload header
+
+ uint8_t RtpBuf[2048];
+ int RtpPacketSize = JpegLen + KRtpHeaderSize + KJpegHeaderSize;
+
+ memset(RtpBuf,0x00,sizeof(RtpBuf));
+
+ /*
+ The RTP header has the following format:
+
+ 0 1 2 3
+ 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ |V=2|P|X| CC |M| PT | sequence number |
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ | timestamp |
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ | synchronization source (SSRC) identifier |
+ +=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+
+ | contributing source (CSRC) identifiers |
+ | .... |
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * */
+
+ // Prepare the 12 byte RTP header
+ RtpBuf[0] = 0x80; // RTP version
+ RtpBuf[1] = 0x1a + (marker_bit<<7); // JPEG payload (26) and marker bit
+ RtpBuf[2] = m_SequenceNumber >> 8;
+ RtpBuf[3] = m_SequenceNumber & 0x0FF; // each packet is counted with a sequence counter
+ RtpBuf[4] = (m_Timestamp & 0xFF000000) >> 24; // each image gets a timestamp
+ RtpBuf[5] = (m_Timestamp & 0x00FF0000) >> 16;
+ RtpBuf[6] = (m_Timestamp & 0x0000FF00) >> 8;
+ RtpBuf[7] = (m_Timestamp & 0x000000FF);
+ RtpBuf[8] = 0x13; // 4 byte SSRC (sychronization source identifier)
+ RtpBuf[9] = 0xf9; // we just an arbitrary number here to keep it simple
+ RtpBuf[10] = 0x7e;
+ RtpBuf[11] = 0x67;
+
+ /* JPEG header", are as follows:
+ *
+ * http://tools.ietf.org/html/rfc2435
+
+ 0 1 2 3
+ 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ | Type-specific | Fragment Offset |
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ | Type | Q | Width | Height |
+ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+
+ // Prepare the 8 byte payload JPEG header
+ RtpBuf[12] = 0x00; // type specific
+ RtpBuf[13] = (m_offset & 0x00FF0000) >> 16; // 3 byte fragmentation offset for fragmented images
+ RtpBuf[14] = (m_offset & 0x0000FF00) >> 8;
+ RtpBuf[15] = (m_offset & 0x000000FF);
+ RtpBuf[16] = 0x00; // type: 0 422 or 1 421
+ RtpBuf[17] = 60; // quality scale factor
+ RtpBuf[16] = format_code; // type: 0 422 or 1 421
+ if (has_dri_header)
+ RtpBuf[16] |= 0x40; // DRI flag
+ RtpBuf[17] = quality_code; // quality scale factor
+ RtpBuf[18] = w/8; // width / 8 -> 48 pixel
+ RtpBuf[19] = h/8; // height / 8 -> 32 pixel
+ // append the JPEG scan data to the RTP buffer
+ memcpy(&RtpBuf[20],Jpeg,JpegLen);
+
+ udp_write(sock,RtpBuf,RtpPacketSize);
+};
diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.h b/sw/airborne/modules/computer_vision/cv/encoding/rtp.h
new file mode 100644
index 0000000000..66559ed17b
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.h
@@ -0,0 +1,17 @@
+
+
+
+#include
+#include "udp/socket.h"
+
+void send_rtp_frame(
+ struct UdpSocket *sock, // socket
+ uint8_t * Jpeg, uint32_t JpegLen, // jpeg data
+ int w, int h, // width and height
+ uint8_t format_code, // 0=422, 1=421
+ uint8_t quality_code, // 0-99 of 128 for custom (include
+ uint8_t has_dri_header, // Does Jpeg data include Header Info?
+ uint32_t delta_t // time step 90kHz
+);
+
+void test_rtp_frame(struct UdpSocket *sock);
diff --git a/sw/airborne/modules/computer_vision/cv/image.h b/sw/airborne/modules/computer_vision/cv/image.h
new file mode 100644
index 0000000000..cf749ced5b
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/image.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2012-2013
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+
+#ifndef _MY_IMAGE_HEADER_
+#define _MY_IMAGE_HEADER_
+
+
+struct img_struct {
+ int seq;
+ double timestamp;
+ unsigned char *buf;
+ int w;
+ int h;
+};
+
+#endif
diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h
new file mode 100644
index 0000000000..d6f900c1eb
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/resize.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2012-2013
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+
+#include
+#include "image.h"
+
+inline void resize_uyuv(struct img_struct* input, struct img_struct* output, int downsample);
+inline void resize_uyuv(struct img_struct* input, struct img_struct* output, int downsample)
+{
+ uint8_t *source = input->buf;
+ uint8_t *dest = output->buf;
+
+ int pixelskip = downsample-1;
+ for (int y=0;yh;y++)
+ {
+ for (int x=0;xw;x+=2)
+ {
+ // YUYV
+ *dest++ = *source++; // U
+ *dest++ = *source++; // Y
+ // now skip 3 pixels
+ if (pixelskip) source+=(pixelskip+1)*2;
+ *dest++ = *source++; // U
+ *dest++ = *source++; // V
+ if (pixelskip) source+=(pixelskip-1)*2;
+ }
+ // skip 3 rows
+ if (pixelskip) source += pixelskip * input->w * 2;
+ }
+}
+
diff --git a/sw/airborne/modules/computer_vision/image_nc_send.c b/sw/airborne/modules/computer_vision/image_nc_send.c
new file mode 100644
index 0000000000..372b4a2032
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/image_nc_send.c
@@ -0,0 +1,208 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger
+ *
+ * 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/image_nc_send.c
+ *
+ * Capture an image on an ARDrone2 and send it to the ground with netcat (nc)
+ */
+
+// Own header
+#include "modules/computer_vision/image_nc_send.h"
+
+#include
+#include
+#include
+#include
+#include
+
+// Video
+#include "modules/computer_vision/lib/v4l/video.h"
+#include "modules/computer_vision/cv/resize.h"
+#include "modules/computer_vision/cv/encoding/jpeg.h"
+
+// Threaded computer vision
+#include
+
+// Default netcat server IP (destination)
+#ifndef IMAGE_SERVER_IP
+#define IMAGE_SERVER_IP "192.168.1.2"
+#endif
+
+// Default netcat port
+#ifndef IMAGE_SERVER_PORT
+#define IMAGE_SERVER_PORT 4900
+#endif
+
+// Default downsize factor
+#ifndef IMAGE_DOWNSIZE_FACTOR
+#define IMAGE_DOWNSIZE_FACTOR 1
+#endif
+
+// JPEG compression quality factor from 0 to 99 (99=high)
+#ifndef IMAGE_QUALITY_FACTOR
+#define IMAGE_QUALITY_FACTOR 99
+#endif
+
+// Frame Per Second
+#ifndef IMAGE_FPS
+#define IMAGE_FPS (1./6.)
+#endif
+
+// Save images by default
+#ifndef IMAGE_SAVE
+#define IMAGE_SAVE 1
+#endif
+
+void image_nc_send_run(void) {}
+
+
+/////////////////////////////////////////////////////////////////////////
+// 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)
+{
+ // Video Input
+ struct vid_struct vid;
+ vid.device = (char*)"/dev/video1";
+ vid.w=1280;
+ vid.h=720;
+ vid.n_buffers = 4;
+ if (video_init(&vid)<0) {
+ printf("Error initialising video\n");
+ computervision_thread_status = -1;
+ return 0;
+ }
+
+ // Frame Grabbing
+ struct img_struct* img_new = video_create_image(&vid);
+
+ // Frame Resizing
+ uint8_t quality_factor = IMAGE_QUALITY_FACTOR;
+ uint8_t dri_jpeg_header = 1;
+
+ struct img_struct small;
+ small.w = vid.w / IMAGE_DOWNSIZE_FACTOR;
+ small.h = vid.h / IMAGE_DOWNSIZE_FACTOR;
+ small.buf = (uint8_t*)malloc(small.w*small.h*2);
+
+ // Commpressed image buffer
+ uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2);
+
+ // file index (search from 0)
+ int file_index = 0;
+
+ int microsleep = (int)(1000000. / IMAGE_FPS);
+
+ while (computer_vision_thread_command > 0)
+ {
+ usleep(microsleep);
+ video_grab_image(&vid, img_new);
+
+ // Resize
+ resize_uyuv(img_new, &small, IMAGE_DOWNSIZE_FACTOR);
+
+ // 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);
+ uint32_t size = end-(jpegbuf);
+
+#if IMAGE_SAVE
+ FILE* save;
+ char save_name[128];
+ if (system("mkdir -p /data/video/images") == 0) {
+ // search available index (max is 99)
+ for ( ; file_index < 99; file_index++) {
+ printf("search %d\n",file_index);
+ sprintf(save_name,"/data/video/images/img_%02d.jpg",file_index);
+ // test if file exists or not
+ if (access(save_name, F_OK) == -1) {
+ printf("access\n");
+ save = fopen(save_name, "w");
+ if (save != NULL) {
+ fwrite(jpegbuf, sizeof(uint8_t), size, save);
+ fclose(save);
+ }
+ else {
+ printf("Error when opening file %s\n", save_name);
+ }
+ // leave for loop
+ break;
+ }
+ else {printf("file exists\n");}
+ }
+ }
+#endif
+
+ // Fork process
+ int status;
+ pid_t pid = fork();
+
+ if (pid == 0) {
+ // Open process to send using netcat in child process
+ char nc_cmd[64];
+ sprintf(nc_cmd, "nc %s %d", IMAGE_SERVER_IP, IMAGE_SERVER_PORT);
+ FILE* netcat;
+ netcat = popen(nc_cmd, "w");
+ if (netcat != NULL) {
+ fwrite(jpegbuf, sizeof(uint8_t), size, netcat);
+ if (pclose(netcat) == 0) {
+ printf("Sending image succesfully\n");
+ }
+ }
+ else {
+ printf("Fail sending image\n");
+ }
+ exit(0);
+ }
+ else if (pid < 0) {
+ printf("Fork failed\n");
+ }
+ else {
+ // Parent is waiting for child to terminate
+ wait(&status);
+ }
+
+ }
+ printf("Thread Closed\n");
+ video_close(&vid);
+ computervision_thread_status = -100;
+ return 0;
+}
+
+void image_nc_send_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);
+ }
+}
+
+void image_nc_send_stop(void)
+{
+ computer_vision_thread_command = 0;
+}
+
diff --git a/sw/airborne/modules/computer_vision/image_nc_send.h b/sw/airborne/modules/computer_vision/image_nc_send.h
new file mode 100644
index 0000000000..c7bab94da6
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/image_nc_send.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger
+ *
+ * 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/image_nc_send.h
+ *
+ * Capture an image on an ARDrone2 and send it to the ground with netcat (nc)
+ */
+
+#ifndef IMAGE_NC_SEND_H
+#define IMAGE_NC_SEND_H
+
+// Module functions
+extern void image_nc_send_run(void);
+extern void image_nc_send_start(void);
+extern void image_nc_send_stop(void);
+
+#endif /* IMAGE_NC_SEND_H */
+
diff --git a/sw/airborne/modules/computer_vision/image_nc_send_nps.c b/sw/airborne/modules/computer_vision/image_nc_send_nps.c
new file mode 100644
index 0000000000..f2ca1ff16b
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/image_nc_send_nps.c
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+
+// Own header
+#include "image_capture.h"
+
+void image_capture_run(void) {}
+
+void image_capture_start(void) {}
+
+void image_capture_stop(void) {}
+
diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.c b/sw/airborne/modules/computer_vision/lib/udp/socket.c
new file mode 100644
index 0000000000..e0e182f395
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/udp/socket.c
@@ -0,0 +1,103 @@
+#include "socket.h"
+
+#include
+#include
+#include
+#include
+#include
+
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+# define ADDR_SIZE_TYPE socklen_t
+# define SOCKET_ERROR -1
+# define IO_SOCKET ioctl
+
+struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const int port_in, const int broadcast)
+{
+
+ struct UdpSocket* me = malloc(sizeof(struct UdpSocket));
+
+ int so_reuseaddr = 1;
+ me->socket_out = socket( AF_INET, SOCK_DGRAM, 0);
+ setsockopt(me->socket_out, SOL_SOCKET, SO_REUSEADDR,
+ &so_reuseaddr, sizeof(so_reuseaddr));
+
+ /* only set broadcast option if explicitly enabled */
+ if (broadcast)
+ setsockopt(me->socket_out, SOL_SOCKET, SO_BROADCAST,
+ &broadcast, sizeof(broadcast));
+
+ me->addr_out.sin_family = AF_INET;
+ me->addr_out.sin_port = htons(port_out);
+ me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out);
+
+ me->socket_in = socket( AF_INET, SOCK_DGRAM, 0);
+ setsockopt(me->socket_in, SOL_SOCKET, SO_REUSEADDR,
+ &so_reuseaddr, sizeof(so_reuseaddr));
+
+ me->addr_in.sin_family = AF_INET;
+ me->addr_in.sin_port = htons(port_in);
+ me->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
+
+ bind(me->socket_in, (struct sockaddr *)&me->addr_in, sizeof(me->addr_in));
+
+ return me;
+}
+
+#include
+//#define UDP_MODE MSG_DONTWAIT
+#define UDP_MODE 0
+
+int udp_write(struct UdpSocket* me, unsigned char* buf, int len) {
+ sendto(me->socket_out, buf, len, UDP_MODE,
+ (struct sockaddr*)&me->addr_out, sizeof(me->addr_out));
+ //printf("sendto ret=%d\n",ret);
+ return len;
+}
+
+unsigned long MIN(unsigned long a, unsigned long b);
+unsigned long MIN(unsigned long a, unsigned long b)
+{
+ if (a0 in order to start the reading loop
+ int newbytes = 0;
+
+ int status;
+
+ // if socket is connected
+ for (;btr>0;)
+ {
+ // Check Status
+ status = IO_SOCKET(me->socket_in, FIONREAD, &toread);
+ if(status == SOCKET_ERROR) {
+ printf("problem receiving from socket\n");
+ break;
+ }
+
+ //printf("UDP has %d bytes\n", toread);
+ if (toread <= 0)
+ break;
+
+ // If status: ok and new data: read it
+ btr = MIN(toread,(unsigned long)len);
+ recvfrom(me->socket_in, buf, btr, 0, (struct sockaddr*)&me->addr_in, (socklen_t *) sizeof(me->addr_in) );
+ newbytes += btr;
+ }
+ return newbytes;
+}
diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.h b/sw/airborne/modules/computer_vision/lib/udp/socket.h
new file mode 100644
index 0000000000..7260e5cfa5
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/udp/socket.h
@@ -0,0 +1,25 @@
+#ifndef SOCKET_H
+#define SOCKET_H
+
+
+#include
+#include
+
+#define FMS_UNICAST 0
+#define FMS_BROADCAST 1
+
+struct UdpSocket {
+ int socket_in;
+ int socket_out;
+ struct sockaddr_in addr_in;
+ struct sockaddr_in addr_out;
+};
+
+
+extern struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const int port_in, const int broadcast);
+extern int udp_write(struct UdpSocket* me, unsigned char* buf, int len);
+extern int udp_read(struct UdpSocket* me, unsigned char* buf, int len);
+
+
+#endif /* SOCKET_H */
+
diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.c b/sw/airborne/modules/computer_vision/lib/v4l/video.c
new file mode 100644
index 0000000000..b3a090ced4
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/v4l/video.c
@@ -0,0 +1,233 @@
+/*
+ video.c - video driver
+
+ Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
+
+ 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.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ MA 02110-1301 USA.
+ */
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "video.h"
+
+#define CLEAR(x) memset (&(x), 0, sizeof (x))
+
+
+pthread_t video_thread;
+void *video_thread_main(void* data);
+void *video_thread_main(void* data)
+{
+ struct vid_struct* vid = (struct vid_struct*)data;
+ printf("video_thread_main started\n");
+ while (1) {
+ fd_set fds;
+ struct timeval tv;
+ int r;
+
+ FD_ZERO(&fds);
+ FD_SET(vid->fd, &fds);
+
+ tv.tv_sec = 2;
+ tv.tv_usec = 0;
+ r = select(vid->fd + 1, &fds, NULL, NULL, &tv);
+
+ if (-1 == r) {
+ if (EINTR == errno) continue;
+ printf("select err\n");
+ }
+
+ if (0 == r) {
+ fprintf(stderr, "select timeout\n");
+ return 0;
+ }
+
+ struct v4l2_buffer buf;
+
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ if (ioctl(vid->fd, VIDIOC_DQBUF, &buf) < 0) {
+ printf("ioctl() VIDIOC_DQBUF failed.\n");
+ break;
+ }
+
+ assert(buf.index < vid->n_buffers);
+
+ vid->seq++;
+
+ if(vid->trigger) {
+
+ // todo add timestamp again
+ //vid->img->timestamp = util_timestamp();
+ vid->img->seq = vid->seq;
+ memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2);
+ vid->trigger=0;
+ }
+
+ if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) {
+ printf("ioctl() VIDIOC_QBUF failed.\n");
+ break;
+ }
+ }
+ return 0;
+}
+
+int video_init(struct vid_struct *vid)
+{
+ struct v4l2_capability cap;
+ struct v4l2_format fmt;
+ unsigned int i;
+ enum v4l2_buf_type type;
+
+ vid->seq=0;
+ vid->trigger=0;
+ if(vid->n_buffers==0) vid->n_buffers=4;
+
+ vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0);
+
+ if (ioctl(vid->fd, VIDIOC_QUERYCAP, &cap) < 0) {
+ printf("ioctl() VIDIOC_QUERYCAP failed.\n");
+ return -1;
+ }
+
+ //printf("2 driver = %s, card = %s, version = %d, capabilities = 0x%x\n", cap.driver, cap.card, cap.version, cap.capabilities);
+
+ CLEAR(fmt);
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = vid->w;
+ fmt.fmt.pix.height = vid->h;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+
+ if (ioctl(vid->fd, VIDIOC_S_FMT, &fmt) < 0) {
+ printf("ioctl() VIDIOC_S_FMT failed.\n");
+ return -1;
+ }
+
+ //image_size = fmt.fmt.pix.width * fmt.fmt.pix.height *3/2;
+
+ struct v4l2_requestbuffers req;
+
+ CLEAR(req);
+ req.count = vid->n_buffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+
+ if (ioctl(vid->fd, VIDIOC_REQBUFS, &req) < 0) {
+ printf("ioctl() VIDIOC_REQBUFS failed.\n");
+ return -1;
+ }
+
+ printf("Buffer count = %d\n", vid->n_buffers);
+
+ vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct));
+
+ for (i = 0; i < vid->n_buffers; ++i) {
+ struct v4l2_buffer buf;
+
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+
+ if (ioctl(vid->fd, VIDIOC_QUERYBUF, &buf) < 0) {
+ printf("ioctl() VIDIOC_QUERYBUF failed.\n");
+ return -1;
+ }
+
+ vid->buffers[i].length = buf.length;
+ printf("buffer%d.length=%d\n",i,buf.length);
+ vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset);
+
+ if (MAP_FAILED == vid->buffers[i].buf) {
+ printf ("mmap() failed.\n");
+ return -1;
+ }
+ }
+
+ for (i = 0; i < vid->n_buffers; ++i) {
+ struct v4l2_buffer buf;
+
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+
+ if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) {
+ printf("ioctl() VIDIOC_QBUF failed.\n");
+ return -1;
+ }
+ }
+
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) {
+ printf("ioctl() VIDIOC_STREAMON failed.\n");
+ return -1;
+ }
+
+ //start video thread
+ int rc = pthread_create(&video_thread, NULL, video_thread_main, vid);
+ if(rc) {
+ printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
+ return 202;
+ }
+
+ return 0;
+}
+
+void video_close(struct vid_struct *vid)
+{
+ int i;
+ for (i = 0; i < (int)vid->n_buffers; ++i) {
+ if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n");
+ }
+ close(vid->fd);
+}
+
+struct img_struct *video_create_image(struct vid_struct *vid)
+{
+ struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct));
+ img->w=vid->w;
+ img->h=vid->h;
+ img->buf = (unsigned char*)malloc(vid->h*vid->w*2);
+ return img;
+}
+
+pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+void video_grab_image(struct vid_struct *vid, struct img_struct *img) {
+ pthread_mutex_lock(&video_grab_mutex);
+ vid->img = img;
+ vid->trigger=1;
+ // while(vid->trigger) pthread_yield();
+ while(vid->trigger) usleep(1);
+ pthread_mutex_unlock(&video_grab_mutex);
+}
diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h
new file mode 100644
index 0000000000..125892ad1b
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h
@@ -0,0 +1,53 @@
+/*
+ video.h - video driver
+
+ Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
+
+ 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.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ MA 02110-1301 USA.
+ */
+
+#ifndef _VIDEO_H
+#define _VIDEO_H
+
+#include "../../cv/image.h"
+
+struct buffer_struct {
+ void * buf;
+ size_t length;
+};
+
+struct vid_struct {
+ char *device;
+ int w;
+ int h;
+ int seq;
+ unsigned int n_buffers;
+
+ //private members
+ int trigger;
+ struct img_struct *img;
+ struct buffer_struct * buffers;
+ int fd;
+};
+
+
+int video_init(struct vid_struct *vid);
+struct img_struct *video_create_image(struct vid_struct *vid);
+
+void video_grab_image(struct vid_struct *vid, struct img_struct *img);
+void video_close(struct vid_struct *vid);
+
+#endif
diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c
new file mode 100644
index 0000000000..b1b0e7eeab
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/viewvideo.c
@@ -0,0 +1,251 @@
+/*
+ * 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/viewvideo.c
+ *
+ * Get live images from a RTP/UDP stream
+ * and save pictures on internal memory
+ *
+ * Works on Linux platforms
+ */
+
+// Own header
+#include "modules/computer_vision/viewvideo.h"
+
+#include
+#include
+#include
+#include
+#include
+
+// UDP RTP Images
+#include "modules/computer_vision/lib/udp/socket.h"
+// Video
+#include "modules/computer_vision/lib/v4l/video.h"
+#include "modules/computer_vision/cv/resize.h"
+#include "modules/computer_vision/cv/encoding/jpeg.h"
+#include "modules/computer_vision/cv/encoding/rtp.h"
+
+// Threaded computer vision
+#include
+
+// Default broadcast IP
+#ifndef VIDEO_SOCK_IP
+#define VIDEO_SOCK_IP "192.168.1.255"
+#endif
+
+// Output socket can be defined from an offset
+#ifdef VIDEO_SOCK_OUT_OFFSET
+#define VIDEO_SOCK_OUT (5000+VIDEO_SOCK_OUT_OFFSET)
+#endif
+
+#ifndef VIDEO_SOCK_OUT
+#define VIDEO_SOCK_OUT 5000
+#endif
+
+#ifndef VIDEO_SOCK_IN
+#define VIDEO_SOCK_IN 4999
+#endif
+
+// Downsize factor for video stream
+#ifndef VIDEO_DOWNSIZE_FACTOR
+#define VIDEO_DOWNSIZE_FACTOR 4
+#endif
+
+// From 0 to 99 (99=high)
+#ifndef VIDEO_QUALITY_FACTOR
+#define VIDEO_QUALITY_FACTOR 50
+#endif
+
+// Frame Per Seconds
+#ifndef VIDEO_FPS
+#define VIDEO_FPS 4.
+#endif
+
+void viewvideo_run(void) {}
+
+// take shot flag
+int viewvideo_shot = 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)
+{
+ // Video Input
+ struct vid_struct vid;
+ vid.device = (char*)"/dev/video1";
+ vid.w=1280;
+ vid.h=720;
+ vid.n_buffers = 4;
+ if (video_init(&vid)<0) {
+ printf("Error initialising video\n");
+ computervision_thread_status = -1;
+ return 0;
+ }
+
+ // Video Grabbing
+ struct img_struct* img_new = video_create_image(&vid);
+
+ // Video Resizing
+ uint8_t quality_factor = VIDEO_QUALITY_FACTOR;
+ uint8_t dri_jpeg_header = 0;
+ int microsleep = (int)(1000000. / VIDEO_FPS);
+
+ struct img_struct small;
+ small.w = vid.w / VIDEO_DOWNSIZE_FACTOR;
+ small.h = vid.h / VIDEO_DOWNSIZE_FACTOR;
+ small.buf = (uint8_t*)malloc(small.w*small.h*2);
+
+ // Video Compression
+ uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.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);
+ }
+ }
+
+ // file index (search from 0)
+ int file_index = 0;
+
+ // time
+ struct timeval last_time;
+ gettimeofday(&last_time, NULL);
+
+ while (computer_vision_thread_command > 0)
+ {
+ // compute usleep to have a more stable frame rate
+ struct timeval time;
+ gettimeofday(&time, NULL);
+ int dt = (int)(time.tv_sec - last_time.tv_sec)*1000000 + (int)(time.tv_usec - last_time.tv_usec);
+ if (dt < microsleep) usleep(microsleep - dt);
+ last_time = time;
+
+ // Grab new frame
+ video_grab_image(&vid, img_new);
+
+ // Save picture on disk
+ if (computer_vision_thread_command == 2)
+ {
+ uint8_t* end = encode_image (img_new->buf, jpegbuf, 99, FOUR_TWO_TWO, vid.w, vid.h, 1);
+ uint32_t size = end-(jpegbuf);
+ FILE* save;
+ char save_name[128];
+ if (system("mkdir -p /data/video/images") == 0) {
+ // search available index (max is 99)
+ for ( ; file_index < 99; file_index++) {
+ printf("search %d\n",file_index);
+ sprintf(save_name,"/data/video/images/img_%02d.jpg",file_index);
+ // test if file exists or not
+ if (access(save_name, F_OK) == -1) {
+ printf("access\n");
+ save = fopen(save_name, "w");
+ if (save != NULL) {
+ fwrite(jpegbuf, sizeof(uint8_t), size, save);
+ fclose(save);
+ }
+ else {
+ printf("Error when opening file %s\n", save_name);
+ }
+ // leave for loop
+ break;
+ } else {
+ printf("file exists\n");
+ }
+ }
+ }
+ computer_vision_thread_command = 1;
+ viewvideo_shot = 0;
+ }
+
+ // Resize
+ resize_uyuv(img_new, &small, VIDEO_DOWNSIZE_FACTOR);
+
+ // 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);
+ 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
+ 1 // 90kHz time increment
+ );
+ // Extra note: when the time increment is set to 0,
+ // it is automaticaly calculated by the send_rtp_frame function
+ // based on gettimeofday value. This seems to introduce some lag or jitter.
+ // An other way is to compute the time increment and set the correct value.
+ // It seems that a lower value is also working (when the frame is received
+ // the timestamp is always "late" so the frame is displayed immediately).
+ // Here, we set the time increment to the lowest possible value
+ // (1 = 1/90000 s) which is probably stupid but is actually working.
+ }
+ printf("Thread Closed\n");
+ video_close(&vid);
+ computervision_thread_status = -100;
+ return 0;
+}
+
+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);
+ }
+}
+
+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;
+ }
+ return 0;
+}
+
+
diff --git a/sw/airborne/modules/computer_vision/viewvideo.h b/sw/airborne/modules/computer_vision/viewvideo.h
new file mode 100644
index 0000000000..35c67ab778
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/viewvideo.h
@@ -0,0 +1,50 @@
+/*
+ * 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/viewvideo.h
+ *
+ * Get live images from a RTP/UDP stream
+ * and save pictures on internal memory
+ *
+ * Works on Linux platforms
+ */
+
+#ifndef VIEW_VIDEO_H
+#define VIEW_VIDEO_H
+
+// Module functions
+extern void viewvideo_run(void);
+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 int viewvideo_shot;
+#define viewvideo_SaveShot(_v) { \
+ viewvideo_shot = 1; \
+ viewvideo_save_shot(); \
+}
+
+#endif /* VIEW_VIDEO_H */
+
diff --git a/sw/airborne/modules/computer_vision/viewvideo_nps.c b/sw/airborne/modules/computer_vision/viewvideo_nps.c
new file mode 100644
index 0000000000..cc893e165a
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/viewvideo_nps.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2012-2013
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** Dummy C implementation for simulation
+ */
+
+// Own header
+#include "viewvideo.h"
+
+int viewvideo_shot = 0;
+
+void viewvideo_run(void) {}
+void viewvideo_start(void) {}
+void viewvideo_stop(void) {}
+
+int viewvideo_save_shot(void)
+{
+ return 0;
+}
+
+
diff --git a/sw/tools/parrot/ardrone2/dump_images.sh b/sw/tools/parrot/ardrone2/dump_images.sh
new file mode 100755
index 0000000000..2bdb8378e8
--- /dev/null
+++ b/sw/tools/parrot/ardrone2/dump_images.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+
+# base address for ARDrone2
+ADDR_BASE=192.168.1.
+
+# tmp folder for images files (relative path by default)
+IMAGES_DIR=${PAPARAZZI_HOME=../../../..}/var/images_tmp
+
+# test if a complete IP address is passed as first argument or just the last digit
+if [ `grep -c '\.' <<< $1` == 1 ]
+then
+ ADDR=$1
+else
+ ADDR=$ADDR_BASE$1
+fi
+
+# download images folder from ARDrone
+mkdir -p $IMAGES_DIR/$ADDR
+../ardrone2.py --host=$ADDR download_dir $IMAGES_DIR/$ADDR images
+
diff --git a/sw/tools/parrot/ardrone2/start_avplay.sh b/sw/tools/parrot/ardrone2/start_avplay.sh
new file mode 100755
index 0000000000..cb47a59e54
--- /dev/null
+++ b/sw/tools/parrot/ardrone2/start_avplay.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+
+# base address for ARDrone2
+ADDR_BASE=192.168.1.
+
+# test if a complete IP address is passed as first argument or just the last digit
+if [ `grep -c '\.' <<< $1` == 1 ]
+then
+ ADDR=$1
+ PORT=$2
+else
+ ADDR=$ADDR_BASE$1
+ PORT=$((5000+$1))
+fi
+
+pid=0
+
+echo "Start video for $ADDR on port $PORT"
+/usr/bin/avplay -loglevel quiet -max_delay 50 -fflags nobuffer rtp://$ADDR:$PORT
+
diff --git a/sw/tools/parrot/ardrone2/start_mplayer.sh b/sw/tools/parrot/ardrone2/start_mplayer.sh
new file mode 100755
index 0000000000..fa3c09a22e
--- /dev/null
+++ b/sw/tools/parrot/ardrone2/start_mplayer.sh
@@ -0,0 +1,50 @@
+#!/bin/bash
+
+# base address for ARDrone2
+ADDR_BASE=192.168.1.
+
+# tmp folder for sdp files (relative path by default)
+SDP_DIR=${PAPARAZZI_HOME=../../../..}/var/sdp_tmp
+
+# test if a complete IP address is passed as first argument or just the last digit
+if [ `grep -c '\.' <<< $1` == 1 ]
+then
+ ADDR=$1
+else
+ ADDR=$ADDR_BASE$1
+fi
+
+pid=0
+
+function quit {
+ echo "Stop video"
+ if [ $pid > 0 ]
+ then
+ kill -9 $pid
+ fi
+ exit 0
+}
+
+# trap control+c to stop mplayer
+trap quit SIGINT
+
+# fetch sdp file on the ARDrone
+mkdir -p $SDP_DIR/$ADDR
+../ardrone2.py --host=$ADDR download_file $SDP_DIR/$ADDR/x86_config-mjpeg.sdp sdp
+
+if [ ! -f $SDP_DIR/$ADDR/x86_config-mjpeg.sdp ];
+then
+ echo "Unable to download sdp file from $ADDR"
+ exit 0
+fi
+
+# start mplayer and respawn if needed
+echo "Start video"
+while [ 1 ]
+do
+ /usr/bin/mplayer -really-quiet $SDP_DIR/$ADDR/x86_config-mjpeg.sdp&
+ pid=$!
+ wait $pid
+ echo "Restart video"
+done
+