diff --git a/conf/abi.xml b/conf/abi.xml
index 5704993f98..1e451f9dd6 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -55,6 +55,23 @@
+
+
+ Flow in x direction from the camera (in subpixels)
+ Flow in y direction from the camera (in subpixels)
+ The derotated flow calculation in the x direction (in subpixels)
+ The derotated flow calculation in the y direction (in subpixels)
+
+ Distance to ground
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 4b4c7ae7be..765698294f 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -179,6 +179,10 @@ void guidance_h_init(void)
gh_ref_init();
+#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
+ guidance_h_module_init();
+#endif
+
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GUIDANCE_H_INT", send_gh);
register_periodic_telemetry(DefaultPeriodic, "HOVER_LOOP", send_hover_loop);
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
index f0d7cf8aa4..d24eda6d8a 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
@@ -33,6 +33,7 @@
*
* a) Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE
* One must then implement:
+ * - void guidance_h_module_init(void);
* - void guidance_h_module_enter(void);
* - void guidance_h_module_read_rc(void);
* - void guidance_h_module_run(bool_t in_flight);
diff --git a/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c b/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c
index d25f36d850..22f7a8e0f4 100644
--- a/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c
+++ b/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.c
@@ -30,12 +30,20 @@
// Own Header
#include "stabilization_opticflow.h"
+#include "subsystems/abi.h"
+
// Stabilization
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "autopilot.h"
#include "subsystems/datalink/downlink.h"
+/** Default sender to accect VELOCITY_ESTIMATE messages from */
+#ifndef VISION_VELOCITY_ESTIMATE_ID
+#define VISION_VELOCITY_ESTIMATE_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(VISION_VELOCITY_ESTIMATE_ID)
+
#define CMD_OF_SAT 1500 // 40 deg = 2859.1851
#ifndef VISION_PHI_PGAIN
@@ -76,6 +84,8 @@ PRINT_CONFIG_VAR(VISION_DESIRED_VY)
#error "ALL control gains have to be positive!!!"
#endif
+static abi_event velocity_est_ev;
+
/* Initialize the default gains and settings */
struct opticflow_stab_t opticflow_stab = {
.phi_pgain = VISION_PHI_PGAIN,
@@ -86,6 +96,18 @@ struct opticflow_stab_t opticflow_stab = {
.desired_vy = VISION_DESIRED_VY
};
+
+static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
+ uint32_t stamp, float vel_x, float vel_y, float vel_z);
+/**
+ * Initialization of horizontal guidance module.
+ */
+void guidance_h_module_init(void)
+{
+ // Subscribe to the VELOCITY_ESTIMATE ABI message
+ AbiBindMsgVELOCITY_ESTIMATE(VISION_VELOCITY_ESTIMATE_ID, &velocity_est_ev, stabilization_opticflow_vel_cb);
+}
+
/**
* Horizontal guidance mode enter resets the errors
* and starts the controller.
@@ -124,23 +146,19 @@ void guidance_h_module_run(bool_t in_flight)
}
/**
- * Update the controls based on a vision result
- * @param[in] *result The opticflow calculation result used for control
+ * Update the controls on a new VELOCITY_ESTIMATE ABI message.
*/
-void stabilization_opticflow_update(struct opticflow_result_t *result)
+static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
+ uint32_t stamp, float vel_x, float vel_y, float vel_z)
{
/* Check if we are in the correct AP_MODE before setting commands */
if (autopilot_mode != AP_MODE_MODULE) {
return;
}
- /* Calculate the error if we have enough flow */
- float err_vx = 0;
- float err_vy = 0;
- if (result->tracked_cnt > 0) {
- err_vx = opticflow_stab.desired_vx - result->vel_x;
- err_vy = opticflow_stab.desired_vy - result->vel_y;
- }
+ /* Calculate the error */
+ float err_vx = opticflow_stab.desired_vx - vel_x;
+ float err_vy = opticflow_stab.desired_vy - vel_y;
/* Calculate the integrated errors (TODO: bound??) */
opticflow_stab.err_vx_int += err_vx / 100;
diff --git a/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.h b/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.h
index c897c6f5af..881dd93828 100644
--- a/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.h
+++ b/sw/airborne/modules/computer_vision/opticflow/stabilization_opticflow.h
@@ -31,8 +31,6 @@
#define CV_STABILIZATION_OPTICFLOW_H_
#include "std.h"
-#include "lib/v4l/v4l2.h"
-#include "inter_thread_data.h"
#include "math/pprz_algebra_int.h"
/* The opticflow stabilization */
@@ -51,11 +49,9 @@ struct opticflow_stab_t {
extern struct opticflow_stab_t opticflow_stab;
// Implement own Horizontal loops
+extern void guidance_h_module_init(void);
extern void guidance_h_module_enter(void);
extern void guidance_h_module_read_rc(void);
extern void guidance_h_module_run(bool_t in_flight);
-// Update the stabiliztion commands based on a vision result
-void stabilization_opticflow_update(struct opticflow_result_t *vision);
-
#endif /* CV_STABILIZATION_OPTICFLOW_H_ */
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index cf6be7da80..ee45984e23 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -43,6 +43,10 @@
#endif
PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)
+#ifndef OPTICFLOW_SENDER_ID
+#define OPTICFLOW_SENDER_ID 1
+#endif
+
/* The video device */
#ifndef OPTICFLOW_DEVICE
#define OPTICFLOW_DEVICE /dev/video2 ///< The video device
@@ -151,7 +155,21 @@ void opticflow_module_run(void)
// Update the stabilization loops on the current calculation
if (opticflow_got_result) {
- stabilization_opticflow_update(&opticflow_result);
+ uint32_t now_ts = get_sys_time_usec();
+ uint8_t quality = opticflow_result.divergence; // FIXME, scale to some quality measure 0-255
+ AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SENDER_ID, now_ts,
+ opticflow_result.flow_x,
+ opticflow_result.flow_y,
+ opticflow_result.flow_der_x,
+ opticflow_result.flow_der_x,
+ quality,
+ opticflow_state.agl);
+ if (opticflow_result.tracked_cnt > 0) {
+ AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
+ opticflow_result.vel_x,
+ opticflow_result.vel_y,
+ 0.0f);
+ }
opticflow_got_result = FALSE;
}
pthread_mutex_unlock(&opticflow_mutex);
diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.c b/sw/airborne/modules/ctrl/ctrl_module_demo.c
index 1590702235..f76145a0e3 100644
--- a/sw/airborne/modules/ctrl/ctrl_module_demo.c
+++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c
@@ -77,6 +77,11 @@ void ctrl_module_run(bool_t in_flight)
////////////////////////////////////////////////////////////////////
// Call our controller
// Implement own Horizontal loops
+void guidance_h_module_init(void)
+{
+ ctrl_module_init();
+}
+
void guidance_h_module_enter(void)
{
ctrl_module_init();
diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.h b/sw/airborne/modules/ctrl/ctrl_module_demo.h
index acf74f9024..bea8612444 100644
--- a/sw/airborne/modules/ctrl/ctrl_module_demo.h
+++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h
@@ -44,6 +44,7 @@ extern float ctrl_module_demo_y_d_gain;
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
// Implement own Horizontal loops
+extern void guidance_h_module_init(void);
extern void guidance_h_module_enter(void);
extern void guidance_h_module_read_rc(void);
extern void guidance_h_module_run(bool_t in_flight);