[vision] Small fixes and added periodic telemetry

conf/telemetry/default_ardrone.xml

@@ -21,6 +21,7 @@
       <message name="UART_ERRORS"            period="3.1"/>
       <message name="DATALINK_REPORT"        period="5.1"/>
       <message name="STATE_FILTER_STATUS"    period="3.2"/>
+      <message name="OPTIC_FLOW_EST"         period="0.25"/>
     </mode>
 
     <mode name="ppm">

sw/airborne/boards/bebop/actuators.c

@@ -144,36 +144,3 @@ static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size)
 
   return checksum;
 }
-
-/*static void actuators_bebop_saturate(void) {
-  // Find the lowest and highest commands
-  int32_t max_cmd = 9000; // Should be gotton from airframe file per motor
-  int32_t min_cmd = 3000; // Should be gotton from airframe file per motor
-  for(int i = 0; i < 4; i++) {
-    if(actuators_bebop.rpm_ref[i] > max_cmd)
-      max_cmd = actuators_bebop.rpm_ref[i];
-    if(actuators_bebop.rpm_ref[i] < min_cmd)
-      min_cmd = actuators_bebop.rpm_ref[i];
-  }
-
-  // Find the maximum motor command (Saturated motor or either MOTOR_MIXING_MAX_MOTOR)
-  int32_t max_motor = 9000;
-  for(int i = 0; i < 4; i++) {
-    if(actuators_bebop.rpm_obs[i] & (1<<15) && max_cmd > (actuators_bebop.rpm_obs[i] & ~(1<<15)))
-      max_motor = actuators_bebop.rpm_obs[i] & ~(1<<15);
-  }
-
-  // Saturate the offsets
-  if(max_cmd > max_motor) {
-    int32_t saturation_offset = 9000 - max_cmd;
-    for(int i = 0; i < 4; i++)
-      actuators_bebop.rpm_ref[i] += saturation_offset;
-    motor_mixing.nb_saturation++;
-  }
-  else if(min_cmd < 3000) {
-    int32_t saturation_offset = 3000 - min_cmd;
-    for(int i = 0; i < 4; i++)
-      actuators_bebop.rpm_ref[i] += saturation_offset;
-    motor_mixing.nb_saturation++;
-  }
-}*/

sw/airborne/modules/computer_vision/lib/vision/image.c

@@ -476,8 +476,8 @@ void image_draw_line(struct image_t *img, struct point_t *from, struct point_t *
   uint16_t starty = from->y;
 
   /* compute the distances in both directions */
-  int32_t delta_x = from->x - to->x;
-  int32_t delta_y = from->y - to->y;
+  int32_t delta_x = to->x - from->x;
+  int32_t delta_y = to->y - from->y;
 
   /* Compute the direction of the increment,
      an increment of 0 means either a horizontal or vertical

sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c

@@ -150,14 +150,9 @@ void stabilization_opticflow_update(struct opticflow_result_t *result)
   opticflow_stab.cmd.phi   = opticflow_stab.phi_pgain * err_vx / 100
                               + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
   opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
-                              + opticflow_stab.err_vy_int * opticflow_stab.err_vy_int);
+                              + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
 
   /* Bound the roll and pitch commands */
   BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
   BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
-
-  DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice,
-                       &result->fps, &result->corner_cnt, &result->tracked_cnt, &result->flow_x, &result->flow_y,
-                       &result->flow_der_x, &result->flow_der_y, &result->vel_x, &result->vel_y,
-                       &opticflow_stab.cmd.phi, &opticflow_stab.cmd.theta);
 }

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -77,6 +77,26 @@ static pthread_mutex_t opticflow_mutex;             //< Mutex lock fo thread saf
 static void *opticflow_module_calc(void *data);                   //< The main optical flow calculation thread
 static void opticflow_agl_cb(uint8_t sender_id, float distance);  //< Callback function of the ground altitude
 
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+/**
+ * Send optical flow telemetry information
+ * @param[in] *trans The transport structure to send the information over
+ * @param[in] *dev The link to send the data over
+ */
+static void opticflow_telem_send(struct transport_tx *trans, struct link_device *dev)
+{
+  pthread_mutex_lock(&opticflow_mutex);
+  pprz_msg_send_OPTIC_FLOW_EST(trans, dev, AC_ID,
+                       &opticflow_result.fps, &opticflow_result.corner_cnt,
+                       &opticflow_result.tracked_cnt, &opticflow_result.flow_x,
+                       &opticflow_result.flow_y, &opticflow_result.flow_der_x,
+                       &opticflow_result.flow_der_y, &opticflow_result.vel_x,
+                       &opticflow_result.vel_y,
+                       &opticflow_stab.cmd.phi, &opticflow_stab.cmd.theta);
+  pthread_mutex_unlock(&opticflow_mutex);
+}
+#endif
 
 /**
  * Initialize the optical flow module for the bottom camera
@@ -111,6 +131,10 @@ void opticflow_module_init(void)
   if (opticflow_dev == NULL) {
     printf("[opticflow_module] Could not initialize the video device\n");
   }
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, "OPTIC_FLOW_EST", opticflow_telem_send);
+#endif
 }
 
 /**
@@ -174,7 +198,7 @@ static void *opticflow_module_calc(void *data __attribute__((unused))) {
     return 0;
   }
 
-#ifdef OPTICFLOW_DEBUG
+#if OPTICFLOW_DEBUG
   // Create a new JPEG image
   struct image_t img_jpeg;
   image_create(&img_jpeg, opticflow_dev->w, opticflow_dev->h, IMAGE_JPEG);
@@ -202,7 +226,7 @@ static void *opticflow_module_calc(void *data __attribute__((unused))) {
     opticflow_got_result = TRUE;
     pthread_mutex_unlock(&opticflow_mutex);
 
-#ifdef OPTICFLOW_DEBUG
+#if OPTICFLOW_DEBUG
     jpeg_encode_image(&img, &img_jpeg, 70, FALSE);
     rtp_frame_send(
       &VIEWVIDEO_DEV,           // UDP device
@@ -218,7 +242,7 @@ static void *opticflow_module_calc(void *data __attribute__((unused))) {
     v4l2_image_free(opticflow_dev, &img);
   }
 
-#ifdef OPTICFLOW_DEBUG
+#if OPTICFLOW_DEBUG
   image_free(&img_jpeg);
 #endif
 }