diff --git a/conf/telemetry/default_ardrone.xml b/conf/telemetry/default_ardrone.xml
index ab3bc70dc3..7f96c17535 100644
--- a/conf/telemetry/default_ardrone.xml
+++ b/conf/telemetry/default_ardrone.xml
@@ -21,6 +21,7 @@
+
diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c
index c647bcb4f2..6e725a5e62 100644
--- a/sw/airborne/boards/bebop/actuators.c
+++ b/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++;
- }
-}*/
diff --git a/sw/airborne/modules/computer_vision/lib/vision/image.c b/sw/airborne/modules/computer_vision/lib/vision/image.c
index 969dd9a9b8..752afcf2f2 100644
--- a/sw/airborne/modules/computer_vision/lib/vision/image.c
+++ b/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
diff --git a/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c b/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c
index f4742a00c8..cff2aba784 100644
--- a/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c
+++ b/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);
}
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index b1071a5167..654e541673 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/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
}