Rewrite to generic i2c driver

conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml

@@ -37,10 +37,14 @@
       <define name="VIDEO_THREAD_SHOT_PATH" value="/data/ftp/internal_000/images"/>
     </module>
 
-    <module name="cv_opticflow.xml">
+    <!--module name="cv_opticflow.xml">
 	<define name="MAX_HORIZON" value="2"/>
         <define name="CAMERA_ROTATED_180" value="1"/>
         <define name="OPTICFLOW_DEROTATION" value="0"/>
+    </module-->
+    <module name="video_rtp_stream">
+      <define name="VIEWVIDEO_QUALITY_FACTOR" value="80"/>
+      <define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
     </module>
   </modules>
 

conf/airframes/TUDELFT/tudelft_conf.xml

@@ -95,7 +95,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/rotorcraft_speed.xml settings/control/stabilization_indi.xml"
-   settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/cv_opticflow.xml"
+   settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
    gui_color="#ffffbc3bce5b"
   />
   <aircraft

conf/boards/bebop.makefile

@@ -35,6 +35,7 @@ $(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL -D_GNU_SOURCE
 $(TARGET).srcs +=  $(SRC_BOARD)/board.c
 
 # Compile the video specific parts
+$(TARGET).CFLAGS += -DI2C_BUF_LEN=56 -DUSE_I2C0
 $(TARGET).srcs +=  $(SRC_BOARD)/mt9v117.c $(SRC_BOARD)/mt9f002.c
 
 # Link static (Done for GLIBC)

sw/airborne/arch/linux/mcu_periph/i2c_arch.c

@@ -115,7 +115,7 @@ struct i2c_errors i2c0_errors;
 
 void i2c0_hw_init(void)
 {
-  i2c1.reg_addr = (void *)open("/dev/i2c-0", O_RDWR);
+  i2c0.reg_addr = (void *)open("/dev/i2c-0", O_RDWR);
   i2c0.errors = &i2c0_errors;
 
   /* zeros error counter */

sw/airborne/boards/bebop/board.c

@@ -29,6 +29,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <unistd.h>
+#include "mcu_periph/i2c.h"
 #include "mt9v117.h"
 #include "mt9f002.h"
 #include "mcu.h"
@@ -84,11 +85,20 @@ void board_init(void)
   int ret __attribute__((unused)) = system("killall -q -15 DragonStarter.sh");
   usleep(50000); /* Give DragonStarter 50ms time to end on a busy system */
   kill_gracefull("dragon-prog");
+}
+
+void board_init2(void)
+{
+  /* Initialize MT9V117 chipset (Bottom camera) */
+  struct mt9v117_t mt9v117 = {
+    // Initial values
 
-  // We also try to initialize the video CMOS chips here (Bottom and front)
-  mt9v117_init();
+    // I2C connection port
+    .i2c_periph = &i2c0
+  };
+  mt9v117_init(&mt9v117);
 
-  /* Initialize MT9F002 chipset */
+  /* Initialize MT9F002 chipset (Front camera) */
   struct mt9f002_t mt9f002 = {
     // Precomputed values to go from InputCLK of (26/2)MHz to 96MH
     .interface = MT9F002_PARALLEL,
@@ -114,7 +124,10 @@ void board_init(void)
     .output_height = 480,
     .output_scaler = 1.0,
     .offset_x = 0,
-    .offset_y = 0
+    .offset_y = 0,
+
+    // I2C connection port
+    .i2c_periph = &i2c0
   };
 
   mt9f002_init(&mt9f002);

sw/airborne/boards/bebop/mt9f002.c

@@ -64,7 +64,7 @@ static void write_reg(struct mt9f002_t *mt, uint16_t addr, uint32_t val, uint8_t
   }
 
   // Transmit the buffer
-  i2c_transmit(mt->i2c_periph, &mt->i2c_trans, mt->i2c_trans.slave_addr, len + 2);
+  i2c_transmit(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, len + 2);
 }
 
 /**
@@ -77,7 +77,7 @@ static uint32_t read_reg(struct mt9f002_t *mt, uint16_t addr, uint8_t len)
   mt->i2c_trans.buf[1] = addr & 0xFF;
 
   // Transmit the buffer and receive back
-  i2c_transceive(mt->i2c_periph, &mt->i2c_trans, mt->i2c_trans.slave_addr, 2, len);
+  i2c_transceive(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, 2, len);
 
   /* Fix sigdness */
   for(uint8_t i =0; i < len; i++) {
@@ -89,7 +89,7 @@ static uint32_t read_reg(struct mt9f002_t *mt, uint16_t addr, uint8_t len)
 /**
  * Configure stage 1 for both MiPi and HiSPi connection
  */
-static void mt9f002_mipi_stage1(struct mt9f002_t *mt)
+static inline void mt9f002_mipi_stage1(struct mt9f002_t *mt)
 {
   write_reg(mt, MT9F002_RESET_REGISTER, 0x0118, 2);
   write_reg(mt, MT9F002_MODE_SELECT, 0x00, 1);
@@ -346,15 +346,15 @@ static void mt9f002_mipi_stage1(struct mt9f002_t *mt)
 /**
  * Configure stage 2 for both MiPi and HiSPi connection
  */
-static void mt9f002_mipi_stage2(struct mt9f002_t *mt)
+static inline void mt9f002_mipi_stage2(struct mt9f002_t *mt)
 {
   write_reg(mt, MT9F002_SMIA_TEST, 0x0045, 2);
 }
 
 /**
  * Configure stage 3 for both MiPi and HiSPi connection
  */
-static void mt9f002_mipi_stage3(struct mt9f002_t *mt)
+static inline void mt9f002_mipi_stage3(struct mt9f002_t *mt)
 {
   write_reg(mt, MT9F002_EXTRA_DELAY      , 0x0000, 2);
   write_reg(mt, MT9F002_RESET_REGISTER   , 0x0118, 2);
@@ -365,7 +365,7 @@ static void mt9f002_mipi_stage3(struct mt9f002_t *mt)
 /**
  * Configure stage 1 for parallel connection
  */
-static void mt9f002_parallel_stage1(struct mt9f002_t *mt)
+static inline void mt9f002_parallel_stage1(struct mt9f002_t *mt)
 {
   write_reg(mt, MT9F002_RESET_REGISTER , 0x0010, 2);
   write_reg(mt, MT9F002_GLOBAL_GAIN    , 0x1430, 2);
@@ -462,7 +462,7 @@ static void mt9f002_parallel_stage1(struct mt9f002_t *mt)
 /**
  * Configure stage 2 for parallel connection
  */
-static void mt9f002_parallel_stage2(struct mt9f002_t *mt)
+static inline void mt9f002_parallel_stage2(struct mt9f002_t *mt)
 {
   write_reg(mt, MT9F002_ANALOG_CONTROL4, 0x8000, 2);
   write_reg(mt, MT9F002_READ_MODE, 0x0041, 2);
@@ -503,7 +503,7 @@ static void mt9f002_parallel_stage2(struct mt9f002_t *mt)
 /**
  * Set the PLL registers based on config
  */
-static void mt9f002_set_pll(struct mt9f002_t *mt)
+static inline void mt9f002_set_pll(struct mt9f002_t *mt)
 {
   // Update registers
   write_reg(mt, MT9F002_VT_PIX_CLK_DIV , mt->vt_pix_clk_div, 2);
@@ -689,7 +689,7 @@ static void mt9f002_set_exposure(struct mt9f002_t *mt)
 /**
  *  Calculate the gain based on value of 1.0 -> 63.50
  */
-static inline uint16_t mt9f002_calc_gain(float gain) {
+static uint16_t mt9f002_calc_gain(float gain) {
   // Check if gain is valid
   if(gain < 1.0) {
     gain = 1.0;
@@ -763,7 +763,6 @@ void mt9f002_init(struct mt9f002_t *mt)
   //TODO???
 
   /* Setup i2c transaction */
-  mt->i2c_trans.slave_addr = MT9F002_ADDRESS;
   mt->i2c_trans.status = I2CTransDone;
 
   /* Software reset */

sw/airborne/boards/bebop/mt9f002_regs.h

@@ -1,7 +1,7 @@
 #ifndef MT9F002_REGS_H
 #define MT9F002_REGS_H
 
-#define MT9F002_ADDRESS 0x10
+#define MT9F002_ADDRESS 0x20
 #define MT9F002_NUM_OF_REGISTERS 367
 #define MT9F002_SCALER_N 16
 #define MT9F002_LINE_LENGTH_MAX 0xFFFF