diff --git a/conf/airframes/RM/rm_ardrone2.xml b/conf/airframes/RM/rm_ardrone2.xml
index 5f901fec4d..7a81e61bc5 100644
--- a/conf/airframes/RM/rm_ardrone2.xml
+++ b/conf/airframes/RM/rm_ardrone2.xml
@@ -33,7 +33,13 @@
-
+
+
+
+
+
+
+
@@ -223,7 +229,7 @@
-
+
diff --git a/conf/airframes/examples/ardrone2_px4flow_hover.xml b/conf/airframes/examples/ardrone2_px4flow_hover.xml
new file mode 100644
index 0000000000..a72466dff9
--- /dev/null
+++ b/conf/airframes/examples/ardrone2_px4flow_hover.xml
@@ -0,0 +1,241 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/opticflow_hover.xml b/conf/modules/opticflow_hover.xml
index 744d261637..ca87f0ebba 100644
--- a/conf/modules/opticflow_hover.xml
+++ b/conf/modules/opticflow_hover.xml
@@ -25,17 +25,17 @@
-
-
-
-
-
-
+
+
+
+
+
+
- cv_opticflow
+
diff --git a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
index 122493dd85..18cce940bb 100644
--- a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
+++ b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
@@ -151,6 +151,7 @@ void guidance_h_module_run(bool_t in_flight)
static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, float vel_x, float vel_y, float vel_z)
{
+ printf("Stabilisation before %f %f \n", vel_x, vel_y);
/* Check if we are in the correct AP_MODE before setting commands */
if (autopilot_mode != AP_MODE_MODULE) {
return;
@@ -170,6 +171,7 @@ static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unus
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
+// printf("Stabilisation %f %f so we take %f %f\n", vel_x, vel_y, opticflow_stab.cmd.phi, opticflow_stab.cmd.theta);
/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
diff --git a/sw/airborne/modules/optical_flow/px4flow.c b/sw/airborne/modules/optical_flow/px4flow.c
index e842bf33a0..4512a85fe6 100644
--- a/sw/airborne/modules/optical_flow/px4flow.c
+++ b/sw/airborne/modules/optical_flow/px4flow.c
@@ -28,7 +28,7 @@
#include "modules/optical_flow/px4flow.h"
#include "modules/datalink/mavlink_decoder.h"
-#include
+#include "subsystems/abi.h"
struct mavlink_optical_flow optical_flow;
bool_t optical_flow_available;
@@ -40,11 +40,19 @@ bool_t optical_flow_available;
// request struct for mavlink decoder
struct mavlink_msg_req req;
+#define PIX4FLOWSENDER_ID 1333
// callback function on message reception
static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((unused)))
{
optical_flow_available = TRUE;
+ // printf("Stabilisation decoded %d %d height: %f \n", optical_flow.flow_x, optical_flow.flow_y,optical_flow.ground_distance);
+
+ // X and Y negated to get to the body of the drone
+ AbiSendMsgVELOCITY_ESTIMATE(PIX4FLOWSENDER_ID, 0,
+ (optical_flow.flow_x/optical_flow.ground_distance),
+ -1.0*(optical_flow.flow_y/optical_flow.ground_distance),
+ 0.0f);
}
/** Initialization function