diff --git a/conf/airframes/tudelft/ardrone2_OF_hover.xml b/conf/airframes/tudelft/ardrone2_OF_hover.xml
new file mode 100644
index 0000000000..7fddba5f7c
--- /dev/null
+++ b/conf/airframes/tudelft/ardrone2_OF_hover.xml
@@ -0,0 +1,252 @@
+
+
+
+
+ This airframe uses the optical flow module that uses monocular vision height estimation to stabilize a quadrotor using only a downwards
+ facing camera and an IMU. For additional documentation please visit the module xml file.
+
+ The settings in this airframe are calibrated for performance on an ARDrone2 specifically.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/tudelft/bebop_OF_hover.xml b/conf/airframes/tudelft/bebop_OF_hover.xml
new file mode 100644
index 0000000000..5e95fd5de1
--- /dev/null
+++ b/conf/airframes/tudelft/bebop_OF_hover.xml
@@ -0,0 +1,229 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/optical_flow_hover.xml b/conf/modules/optical_flow_hover.xml
new file mode 100644
index 0000000000..696c42c88a
--- /dev/null
+++ b/conf/modules/optical_flow_hover.xml
@@ -0,0 +1,95 @@
+
+
+
+
+
+ This module implements monocular vision height estimation and uses this
+ estimate to stabilize a quadrotor using only a downwards
+ facing camera and an IMU. For scientific background please visit:
+ https://repository.tudelft.nl/islandora/object/uuid%3A6e3ce742-a974-491d-97c2-1cafc090b3d9
+
+ The gain height relationship as first described in
+ http://iopscience.iop.org/article/10.1088/1748-3190/11/1/016004 is
+ calibrated
+ in a controlled environment so that the resulting relationship can be
+ used to determine the height at which the quadrotor is flying.
+ In addition to the vertical axis the same principle is applied to the
+ horizontal axes.
+
+ A short guide on how to determine the slope:
+ Picking a relatively high OFH_RAMP* with a very high negative
+ OFH_COV*_SETPOINT (i.e. -6000000),
+ looking at when the quadrotor starts oscillating, the respective message
+ (covariances.*) should be used to determine a suitable
+ threshold value for OFH_COV*_SETPOINT. The slope can then be decreased to gain
+ accuracy in reaching the oscillations.
+ Finally when flying the quadrotor on different heights the gain at which
+ instability occurs can be noted to determine the slope of the
+ gain-height relationship.
+
+ Important settings:
+ -OFH_HOVER_METHOD/hover_method:
+ 0, the method is applied to all 3 axes in the following order: Z - X - Y
+ 1, the method is applied to all 3 axes at the same time
+ 2, the method is applied to the Z axis and the estimated height is used
+ to determine the proper gains
+ for the horizontal axes using the predetermined relationship.
+
+ -XY_SYMMETRICAL is set to 1 if the drone is roughly symmetrical,
+ causing the found gains for the X axis is also used for the Y axis.
+ -OFH_*GAIN* can be used to set starting gains to prevent initial drift
+ while still estimating height and appropriate gains.
+ -OFH_REDUCTION* is used to reduce the gain after determining the oscillation to
+ stabilize the quadrotor.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index cd56f46813..bf52194b31 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -27,6 +27,7 @@
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 387dbc49e5..9f4135e646 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -1,4 +1,16 @@
+
+
.
+ */
+
+#include "optical_flow_functions.h"
+#include "paparazzi.h"
+#include "math/pprz_stat.h"
+#include "std.h"
+
+// The maximum bank angle the algorithm can steer
+#ifndef OFH_MAXBANK
+#define OFH_MAXBANK 10.f
+#endif
+
+// The low pass filter constant for updating the vision measurements in the algorithm
+#ifndef OF_LP_CONST
+#define OF_LP_CONST 0.5 // Average between 0.4 Bebop value and 0.6 ARDrone value
+#endif
+
+// In case the autocovariance is used, select half the window size as the delay
+#ifndef OF_COV_DELAY_STEPS
+#define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2
+#endif
+
+uint32_t ind_histXY;
+uint8_t cov_array_filledXY;
+uint32_t ind_histZ;
+uint8_t cov_array_filledZ;
+
+
+/**
+ * Set the covariance of the divergence and the thrust / past divergence
+ * This funciton should only be called once per time step
+ * @param[in] thrust: the current thrust value
+ */
+float set_cov_div(bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs)
+{
+ float cov_div = 0;
+ // histories and cov detection:
+
+ history->OF[ind_histZ] = of_hover.divergence;
+
+ float normalized_thrust = (float)(100.0 * inputs->thrust / MAX_PPRZ);
+ history->input[ind_histZ] = normalized_thrust;
+
+ int ind_past = ind_histZ - OF_COV_DELAY_STEPS;
+ while (ind_past < 0) { ind_past += COV_WINDOW_SIZE; }
+ history->past_OF[ind_histZ] = history->OF[ind_past];
+
+ // determine the covariance for hover detection:
+ // only take covariance into account if there are enough samples in the histories:
+ if (cov_method == 0 && cov_array_filledZ > 0) {
+ // TODO: step in hover set point causes an incorrectly perceived covariance
+ cov_div = covariance_f(history->input, history->OF, COV_WINDOW_SIZE);
+ } else if (cov_method == 1 && cov_array_filledZ > 1) {
+ // todo: delay steps should be invariant to the run frequency
+ cov_div = covariance_f(history->past_OF, history->OF, COV_WINDOW_SIZE);
+ }
+
+ if (cov_array_filledZ < 2 && ind_histZ + 1 == COV_WINDOW_SIZE) {
+ cov_array_filledZ++;
+ }
+ ind_histZ = (ind_histZ + 1) % COV_WINDOW_SIZE;
+
+ return cov_div;
+}
+
+
+/**
+ * Set the covariance of the flow and past flow / desired angle
+ * This funciton should only be called once per time step
+ */
+void set_cov_flow(bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY, struct DesiredInputs *inputs,
+ struct FloatVect3 *covs)
+{
+ // histories and cov detection:
+ historyX->OF[ind_histXY] = of_hover.flowX;
+ historyY->OF[ind_histXY] = of_hover.flowY;
+
+ int ind_past = ind_histXY - OF_COV_DELAY_STEPS;
+ while (ind_past < 0) { ind_past += COV_WINDOW_SIZE; }
+ historyX->past_OF[ind_histXY] = historyX->OF[ind_past];
+ historyY->past_OF[ind_histXY] = historyY->OF[ind_past];
+ float normalized_phi = (float)(100.0 * inputs->phi / OFH_MAXBANK);
+ float normalized_theta = (float)(100.0 * inputs->theta / OFH_MAXBANK);
+ historyX->input[ind_histXY] = normalized_phi;
+ historyY->input[ind_histXY] = normalized_theta;
+
+ // determine the covariance for hover detection:
+ // only take covariance into account if there are enough samples in the histories:
+ if (cov_method == 0 && cov_array_filledXY > 0) {
+ // // TODO: step in hover set point causes an incorrectly perceived covariance
+ covs->x = covariance_f(historyX->input, historyX->OF, COV_WINDOW_SIZE);
+ covs->y = covariance_f(historyY->input, historyY->OF, COV_WINDOW_SIZE);
+ } else if (cov_method == 1 && cov_array_filledXY > 1) {
+ if (cov_array_filledXY > 1) {
+ // todo: delay steps should be invariant to the run frequency
+ covs->x = covariance_f(historyX->past_OF, historyX->OF, COV_WINDOW_SIZE);
+ covs->y = covariance_f(historyY->past_OF, historyY->OF, COV_WINDOW_SIZE);
+ }
+ }
+
+ if (cov_array_filledXY < 2 && ind_histXY + 1 == COV_WINDOW_SIZE) {
+ cov_array_filledXY++;
+ }
+ ind_histXY = (ind_histXY + 1) % COV_WINDOW_SIZE;
+}
+
+
+/**
+ * Determine and set the desired angle for constant flow control
+ * @param[out] desired angle
+ * @param[in] dt: time difference since last update
+ * @param[in] *of_hover_ctrl: OpticalFlowHoverControl structure
+ */
+float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
+{
+ // update the controller errors:
+ float lp_factor = dt / OF_LP_CONST;
+ Bound(lp_factor, 0.f, 1.f);
+
+ // maintain the controller errors:
+ of_hover_ctrl->PID.sum_err += of_hover_ctrl->PID.err;
+ of_hover_ctrl->PID.d_err += (((of_hover_ctrl->PID.err - of_hover_ctrl->PID.previous_err) / dt) -
+ of_hover_ctrl->PID.d_err) * lp_factor;
+ of_hover_ctrl->PID.previous_err = of_hover_ctrl->PID.err;
+
+ // compute the desired angle
+ float des_angle = of_hover_ctrl->PID.P * of_hover_ctrl->PID.err + of_hover_ctrl->PID.I *
+ of_hover_ctrl->PID.sum_err + of_hover_ctrl->PID.D * of_hover_ctrl->PID.d_err;
+
+ // Bound angle:
+ Bound(des_angle, -OFH_MAXBANK, OFH_MAXBANK);
+
+ return des_angle;
+}
+
+/**
+ * Determine and set the thrust for constant divergence control
+ * @param[out] thrust
+ * @param[in] dt: time difference since last update
+ * @param[in] *of_hover_ctrl: OpticalFlowHoverControl structure
+ */
+int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
+{
+ // update the controller errors:
+ float lp_factor = dt / OF_LP_CONST;
+ Bound(lp_factor, 0.f, 1.f);
+
+ // maintain the controller errors:
+ of_hover_ctrl->PID.sum_err += of_hover_ctrl->PID.err;
+ of_hover_ctrl->PID.d_err += (((of_hover_ctrl->PID.err - of_hover_ctrl->PID.previous_err) / dt) -
+ of_hover_ctrl->PID.d_err) * lp_factor;
+ of_hover_ctrl->PID.previous_err = of_hover_ctrl->PID.err;
+
+ // PID control:
+ int32_t thrust = (of_hover_ctrl->nominal_value
+ + of_hover_ctrl->PID.P * of_hover_ctrl->PID.err
+ + of_hover_ctrl->PID.I * of_hover_ctrl->PID.sum_err
+ + of_hover_ctrl->PID.D * of_hover_ctrl->PID.d_err) * MAX_PPRZ;
+
+ // bound thrust:
+ Bound(thrust, 0.25 * of_hover_ctrl->nominal_value * MAX_PPRZ, MAX_PPRZ);
+
+ return thrust;
+}
diff --git a/sw/airborne/modules/ctrl/optical_flow_functions.h b/sw/airborne/modules/ctrl/optical_flow_functions.h
new file mode 100644
index 0000000000..8c63f6530d
--- /dev/null
+++ b/sw/airborne/modules/ctrl/optical_flow_functions.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2018
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+#ifndef OPTICAL_FLOW_FUNCTIONS_H_
+#define OPTICAL_FLOW_FUNCTIONS_H_
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+
+struct GainsPID {
+ float P; ///< P-gain for control
+ float I; ///< I-gain for control
+ float D; ///< D-gain for control
+ float err; ///< Current tracking error
+ float previous_err; ///< Previous tracking error
+ float sum_err; ///< integration of the error for I-gain
+ float d_err; ///< difference of error for the D-gain
+};
+
+struct OFhistory {
+ float input[COV_WINDOW_SIZE];
+ float OF[COV_WINDOW_SIZE];
+ float past_OF[COV_WINDOW_SIZE];
+};
+
+struct OpticalFlowHoverControl {
+ struct GainsPID PID; ///< The struct with the PID gains
+
+ float nominal_value; ///< The nominal value of thrust, phi or theta depending on Z, Y, X
+
+ float ramp; ///< The ramp pused is increased with per dt
+
+ float reduction_factor; ///< Reduce the gain by this factor when oscillating
+
+ float setpoint; ///< setpoint for constant divergence/flow
+ float cov_setpoint; ///< for adaptive gain control, setpoint of the covariance (oscillations)
+};
+
+struct OpticalFlowHover {
+ float divergence; ///< Divergence estimate
+ float flowX; ///< Flow estimate in X direction
+ float flowY; ///< Flow estimate in Y direction
+};
+
+struct DesiredInputs {
+ float phi;
+ float theta;
+ int32_t thrust;
+};
+
+extern uint32_t ind_histXY;
+extern uint8_t cov_array_filledXY;
+extern uint32_t ind_histZ;
+extern uint8_t cov_array_filledZ;
+
+struct OpticalFlowHover of_hover;
+
+extern float set_cov_div(bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs);
+extern void set_cov_flow(bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY,
+ struct DesiredInputs *inputs, struct FloatVect3 *covs);
+extern float PID_flow_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl);
+extern int32_t PID_divergence_control(float dt, struct OpticalFlowHoverControl *of_hover_ctrl);
+#endif /* OPTICAL_FLOW_FUNCTIONS_H_ */
diff --git a/sw/airborne/modules/ctrl/optical_flow_hover.c b/sw/airborne/modules/ctrl/optical_flow_hover.c
new file mode 100644
index 0000000000..8534d608a8
--- /dev/null
+++ b/sw/airborne/modules/ctrl/optical_flow_hover.c
@@ -0,0 +1,756 @@
+/*
+ * Copyright (C) 2018
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+#include "optical_flow_hover.h"
+#include "optical_flow_functions.h"
+
+#include "generated/airframe.h"
+#include "paparazzi.h"
+#include "subsystems/abi.h"
+#include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "subsystems/electrical.h"
+#include
+
+#include "subsystems/datalink/telemetry.h"
+//
+//// for measuring time
+#include "mcu_periph/sys_time.h"
+//
+// Additional math functions
+#include "math/pprz_stat.h"
+
+/* Use optical flow estimates */
+#ifndef OFH_OPTICAL_FLOW_ID
+#define OFH_OPTICAL_FLOW_ID ABI_BROADCAST
+#endif
+PRINT_CONFIG_VAR(OFH_OPTICAL_FLOW_ID)
+
+// Set the standard method (1) to apply the algorithm to all 3 axes at the same time
+#ifndef OFH_HOVER_METHOD
+#define OFH_HOVER_METHOD 1
+#endif
+
+// Set the standard method to assume the quadrotor is not (0) symmetrical
+#ifndef XY_SYMMETRICAL
+#define XY_SYMMETRICAL 0
+#endif
+
+// Set the standard method to use vision signal and the known input and signal (0) optionally one can use the autocovariance (1)
+#ifndef OFH_COV_METHOD
+#define OFH_COV_METHOD 0
+#endif
+
+// number of time steps used for calculating the covariance (oscillations) and the delay steps
+#ifndef COV_WINDOW_SIZE
+#define COV_WINDOW_SIZE (10*30)
+#endif
+
+// In case the autocovariance is used, select half the window size as the delay
+#ifndef OF_COV_DELAY_STEPS
+#define OF_COV_DELAY_STEPS COV_WINDOW_SIZE/2
+#endif
+
+// The low pass filter constant for updating the vision measurements in the algorithm
+#ifndef OF_LP_CONST
+#define OF_LP_CONST 0.5
+#endif
+
+// Whether the algorithm should be applied to the phi angle(1) or not(0)
+#ifndef OFH_OSCPHI
+#define OFH_OSCPHI 1
+#endif
+
+// Whether the algorithm should be applied to the theta angle(1) or not(0)
+#ifndef OFH_OSCTHETA
+#define OFH_OSCTHETA 1
+#endif
+
+// Use default PID gains
+#ifndef OFH_PGAINZ
+#define OFH_PGAINZ 0.4
+#endif
+
+#ifndef OFH_IGAINZ
+#define OFH_IGAINZ 0.f
+#endif
+
+#ifndef OFH_DGAINZ
+#define OFH_DGAINZ 0.0
+#endif
+
+// Default slope at which the gain is increased
+#ifndef OFH_RAMPZ
+#define OFH_RAMPZ 0.15
+#endif
+
+// Default reduction factor to stabilize the quad after oscillating
+#ifndef OFH_REDUCTIONZ
+#define OFH_REDUCTIONZ 0.45
+#endif
+
+// The threshold value for the covariance oscillation measurement
+#ifndef OFH_COVDIV_SETPOINT
+#define OFH_COVDIV_SETPOINT -0.02
+#endif
+
+// Use default PID gains
+#ifndef OFH_PGAINX
+#define OFH_PGAINX 0.f
+#endif
+
+#ifndef OFH_IGAINX
+#define OFH_IGAINX 0.00002
+#endif
+
+#ifndef OFH_DGAINX
+#define OFH_DGAINX 0.f
+#endif
+
+#ifndef OFH_PGAINY
+#define OFH_PGAINY 0.f
+#endif
+
+#ifndef OFH_IGAINY
+#define OFH_IGAINY 0.00002
+#endif
+
+#ifndef OFH_DGAINY
+#define OFH_DGAINY 0.f
+#endif
+
+// Default slope at which the gain is increased
+#ifndef OFH_RAMPXY
+#define OFH_RAMPXY 0.0008
+#endif
+
+// Default reduction factor to stabilize the quad after oscillating
+#ifndef OFH_REDUCTIONXY
+#define OFH_REDUCTIONXY 0.3
+#endif
+
+// The threshold value for the covariance oscillation measurement
+#ifndef OFH_COVFLOW_SETPOINT
+#define OFH_COVFLOW_SETPOINT -500.f
+#endif
+
+// The default slopes for the height-gain relationships
+#ifndef OFH_VER_SLOPE_A
+#define OFH_VER_SLOPE_A 0.5
+#endif
+
+#ifndef OFH_VER_SLOPE_B
+#define OFH_VER_SLOPE_B 0.25
+#endif
+
+#ifndef OFH_HOR_X_SLOPE_A
+#define OFH_HOR_X_SLOPE_A 2.f
+#endif
+
+#ifndef OFH_HOR_X_SLOPE_B
+#define OFH_HOR_X_SLOPE_B 0.5
+#endif
+
+#ifndef OFH_HOR_Y_SLOPE_A
+#define OFH_HOR_Y_SLOPE_A OFH_HOR_X_SLOPE_A
+#endif
+
+#ifndef OFH_HOR_Y_SLOPE_B
+#define OFH_HOR_Y_SLOPE_B OFH_HOR_X_SLOPE_B
+#endif
+
+
+struct DesiredInputs des_inputs;
+struct FloatVect3 covariances;
+
+// variables retained between module calls
+float vision_time, prev_vision_timeXY, prev_vision_timeZ;
+
+bool oscillatingX;
+bool oscillatingY;
+int16_t flowX;
+int16_t flowY;
+struct OFhistory historyX;
+struct OFhistory historyY;
+
+// Stabilizing commands
+struct Int32Eulers ofh_sp_eu;
+
+bool oscillatingZ;
+float divergence_vision;
+
+struct OFhistory historyZ;
+
+// The optical flow ABI event
+static abi_event optical_flow_ev;
+
+// struct containing most relevant parameters
+struct OpticalFlowHoverControl of_hover_ctrl_X;
+struct OpticalFlowHoverControl of_hover_ctrl_Y;
+struct OpticalFlowHoverControl of_hover_ctrl_Z;
+
+// variables used in the GCS
+bool oscphi;
+bool osctheta;
+bool derotated;
+bool cov_method;
+uint8_t hover_method;
+
+
+/// Function definitions
+// Callback function of the optical flow estimate:
+void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, int16_t flow_x, int16_t flow_y,
+ int16_t flow_der_x, int16_t flow_der_y, float quality, float size_div);
+
+// resetting all variables to be called for instance when starting up / re-entering module
+static void reset_horizontal_vars(void);
+static void reset_vertical_vars(void);
+void vertical_ctrl_module_init(void);
+void vertical_ctrl_module_run(bool in_flight);
+void horizontal_ctrl_module_init(void);
+void horizontal_ctrl_module_run(bool in_flight);
+
+// Compute OptiTrack stabilization for 1/2 axes
+void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu);
+#ifndef GH_GAIN_SCALE
+#define GH_GAIN_SCALE 2
+#endif
+#ifndef MAX_POS_ERR
+#define MAX_POS_ERR POS_BFP_OF_REAL(16.)
+#endif
+#ifndef MAX_SPEED_ERR
+#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
+#endif
+struct Int32Vect2 of_hover_pos_err;
+struct Int32Vect2 of_hover_speed_err;
+struct Int32Vect2 of_hover_ref_pos;
+struct Int32Vect2 of_hover_trim_att_integrator;
+struct Int32Vect2 of_hover_cmd_earth;
+
+
+// sending the divergence message to the ground station:
+static void send_optical_flow_hover(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_OPTICAL_FLOW_HOVER(trans, dev, AC_ID, &of_hover.flowX, &of_hover.flowY, &of_hover.divergence,
+ &covariances.x, &covariances.y, &covariances.z, &of_hover_ctrl_X.PID.P, &of_hover_ctrl_Y.PID.P, &of_hover_ctrl_Z.PID.P,
+ &of_hover_ctrl_X.PID.sum_err, &of_hover_ctrl_Y.PID.sum_err, &of_hover_ctrl_Z.PID.sum_err,
+ &des_inputs.thrust, &des_inputs.phi, &des_inputs.theta);
+}
+
+
+// Init the optical flow hover module
+void optical_flow_hover_init()
+{
+ of_hover_ctrl_X.setpoint = 0.0f;
+ of_hover_ctrl_X.cov_setpoint = OFH_COVFLOW_SETPOINT;
+ of_hover_ctrl_X.PID.P = OFH_PGAINX;
+ of_hover_ctrl_X.PID.I = OFH_IGAINX;
+ of_hover_ctrl_X.PID.D = OFH_DGAINX;
+ of_hover_ctrl_X.ramp = OFH_RAMPXY;
+ of_hover_ctrl_X.reduction_factor = OFH_REDUCTIONXY;
+
+ of_hover_ctrl_Y.setpoint = 0.0f;
+ of_hover_ctrl_Y.cov_setpoint = OFH_COVFLOW_SETPOINT;
+ of_hover_ctrl_Y.PID.P = OFH_PGAINY;
+ of_hover_ctrl_Y.PID.I = OFH_IGAINY;
+ of_hover_ctrl_Y.PID.D = OFH_DGAINY;
+ of_hover_ctrl_Y.ramp = OFH_RAMPXY;
+ of_hover_ctrl_Y.reduction_factor = OFH_REDUCTIONXY;
+
+ of_hover_ctrl_Z.setpoint = 0.0f;
+ of_hover_ctrl_Z.cov_setpoint = OFH_COVDIV_SETPOINT;
+ of_hover_ctrl_Z.PID.P = OFH_PGAINZ;
+ of_hover_ctrl_Z.PID.I = OFH_IGAINZ;
+ of_hover_ctrl_Z.PID.D = OFH_DGAINZ;
+ of_hover_ctrl_Z.ramp = OFH_RAMPZ;
+ of_hover_ctrl_Z.reduction_factor = OFH_REDUCTIONZ;
+
+ oscphi = OFH_OSCPHI;
+ osctheta = OFH_OSCTHETA;
+
+ cov_method = OFH_COV_METHOD;
+ hover_method = OFH_HOVER_METHOD;
+
+ reset_horizontal_vars();
+ reset_vertical_vars();
+
+ // Subscribe to the optical flow estimator:
+ AbiBindMsgOPTICAL_FLOW(OFH_OPTICAL_FLOW_ID, &optical_flow_ev, ofh_optical_flow_cb);
+
+ // register telemetry:
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_OPTICAL_FLOW_HOVER, send_optical_flow_hover);
+}
+
+/**
+ * Initialize the vertical optical flow hover module
+ */
+void vertical_ctrl_module_init(void)
+{
+ // filling the of_hover_ctrl struct with default values:
+ reset_vertical_vars();
+}
+
+/**
+ * Initialize the horizontal optical flow hover module
+ */
+void horizontal_ctrl_module_init(void)
+{
+ // filling the of_hover_ctrl struct with default values:
+ reset_horizontal_vars();
+}
+
+/**
+ * Reset all horizontal variables:
+ */
+static void reset_horizontal_vars(void)
+{
+ // Take the current angles as neutral
+ struct Int32Eulers tempangle;
+ int32_eulers_of_quat(&tempangle, &stab_att_sp_quat);
+ of_hover_ctrl_X.nominal_value = DegOfRad(FLOAT_OF_BFP(tempangle.phi, INT32_ANGLE_FRAC));
+ of_hover_ctrl_Y.nominal_value = DegOfRad(FLOAT_OF_BFP(tempangle.theta, INT32_ANGLE_FRAC));
+
+ des_inputs.phi = 0;
+ des_inputs.theta = 0;
+
+ if ((hover_method == 0) && (GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE)) {
+ // Z - X - Y Order
+ oscillatingX = 1;
+ oscillatingY = 1;
+ of_hover_ctrl_X.PID.P = OFH_PGAINX;
+ of_hover_ctrl_Y.PID.P = OFH_PGAINX;
+ } else if ((hover_method == 2) && (GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE)) {
+ // Z Set XY
+ oscillatingX = 1;
+ oscillatingY = 1;
+ of_hover_ctrl_X.PID.P = OFH_PGAINX;
+ of_hover_ctrl_Y.PID.P = OFH_PGAINY;
+ of_hover_ctrl_X.PID.I = OFH_IGAINX / 4; // Have a slighly lower I gain during Z
+ of_hover_ctrl_Y.PID.I = OFH_IGAINY / 4; // Have a slighly lower I gain during Z
+ } else {
+ // if V is in NAV or hover_method = 1
+ //All axes
+ oscillatingX = 0;
+ oscillatingY = 0;
+ of_hover_ctrl_X.PID.P = OFH_PGAINX;
+ of_hover_ctrl_Y.PID.P = OFH_PGAINX;
+ }
+
+ flowX = 0;
+ flowY = 0;
+
+ of_hover_ctrl_X.PID.sum_err = 0.0f;
+ of_hover_ctrl_X.PID.d_err = 0.0f;
+ of_hover_ctrl_X.PID.previous_err = 0.0f;
+
+ of_hover_ctrl_Y.PID.sum_err = 0.0f;
+ of_hover_ctrl_Y.PID.d_err = 0.0f;
+ of_hover_ctrl_Y.PID.previous_err = 0.0f;
+
+ ofh_sp_eu.phi = of_hover_ctrl_X.nominal_value;
+ ofh_sp_eu.theta = of_hover_ctrl_Y.nominal_value;
+
+ ind_histXY = 0;
+ cov_array_filledXY = 0;
+
+ covariances.x = 0.0f;
+ covariances.y = 0.0f;
+
+ for (uint16_t i = 0; i < COV_WINDOW_SIZE; i++) {
+ historyX.OF[i] = 0.0f;
+ historyY.OF[i] = 0.0f;
+ historyX.past_OF[i] = 0.0f;
+ historyY.past_OF[i] = 0.0f;
+ historyX.input[i] = 0.0f;
+ historyY.input[i] = 0.0f;
+ }
+
+ of_hover.flowX = 0;
+ of_hover.flowY = 0;
+
+ vision_time = get_sys_time_float();
+ prev_vision_timeXY = vision_time;
+}
+
+/**
+ * Reset all vertical variables:
+ */
+static void reset_vertical_vars(void)
+{
+ oscillatingZ = 0;
+
+ divergence_vision = 0;
+ of_hover.divergence = 0;
+
+ for (uint16_t i = 0; i < COV_WINDOW_SIZE; i++) {
+ historyZ.OF[i] = 0.0f;
+ historyZ.past_OF[i] = 0.0f;
+ historyZ.input[i] = 0.0f;
+ }
+
+ ind_histZ = 0;
+
+ covariances.z = 0.0f;
+ cov_array_filledZ = 0;
+
+ of_hover_ctrl_Z.PID.P = OFH_PGAINZ;
+
+ of_hover_ctrl_Z.PID.sum_err = 0.0f;
+ of_hover_ctrl_Z.PID.d_err = 0.0f;
+ of_hover_ctrl_Z.PID.previous_err = 0.0f;
+
+ vision_time = get_sys_time_float();
+ prev_vision_timeZ = vision_time;
+}
+
+// Read H RC
+void guidance_h_module_read_rc(void) {}
+
+/**
+ * Run the horizontal optical flow hover module
+ */
+void horizontal_ctrl_module_run(bool in_flight)
+{
+ /***********
+ * TIME
+ ***********/
+ float ventral_factor = -1.28f; // magic number comprising field of view etc.
+
+ float dt = vision_time - prev_vision_timeXY;
+
+ // check if new measurement received
+ if (dt <= 1e-5f) {
+ return;
+ }
+
+ /***********
+ * VISION
+ ***********/
+
+ float lp_factor = dt / OF_LP_CONST;
+ Bound(lp_factor, 0.f, 1.f);
+
+ float new_flowX = (flowX * ventral_factor) / dt;
+ float new_flowY = (flowY * ventral_factor) / dt;
+
+ //TODO: deal with (unlikely) fast changes in Flow?
+
+ // low-pass filter the divergence:
+ of_hover.flowX += (new_flowX - of_hover.flowX) * lp_factor;
+ of_hover.flowY += (new_flowY - of_hover.flowY) * lp_factor;
+
+
+ /***********
+ * CONTROL
+ ***********/
+
+ if (!oscillatingX) {
+ // if not oscillating, increase gain
+ of_hover_ctrl_X.PID.P += of_hover_ctrl_X.ramp * dt;
+ }
+ if (!oscillatingY) {
+ // if not oscillating, increase gain
+ of_hover_ctrl_Y.PID.P += of_hover_ctrl_Y.ramp * dt;
+ }
+
+ // set desired pitch en roll
+ if (oscphi) {
+ of_hover_ctrl_X.PID.err = of_hover_ctrl_X.setpoint - of_hover.flowX;
+ des_inputs.phi = of_hover_ctrl_X.nominal_value + PID_flow_control(dt, &of_hover_ctrl_X);
+ }
+ if (osctheta) {
+ of_hover_ctrl_Y.PID.err = of_hover_ctrl_Y.setpoint - of_hover.flowY;
+ des_inputs.theta = of_hover_ctrl_Y.nominal_value + PID_flow_control(dt, &of_hover_ctrl_Y);
+ }
+
+ // update covariance
+ set_cov_flow(cov_method, &historyX, &historyY, &des_inputs, &covariances);
+ ofh_sp_eu.phi = BFP_OF_REAL(RadOfDeg(des_inputs.phi * oscphi), INT32_ANGLE_FRAC);
+ ofh_sp_eu.theta = BFP_OF_REAL(RadOfDeg(des_inputs.theta * osctheta), INT32_ANGLE_FRAC);
+
+ // Check for oscillations
+ if ((covariances.x < of_hover_ctrl_X.cov_setpoint) && (!oscillatingX)) {
+ oscillatingX = 1;
+
+ if (hover_method == 0) {
+ //Start the Y axis
+ oscillatingY = 0;
+ of_hover_ctrl_Y.PID.P = OFH_PGAINY;
+ }
+ of_hover_ctrl_X.PID.P = of_hover_ctrl_X.PID.P * of_hover_ctrl_X.reduction_factor;
+
+ if (XY_SYMMETRICAL) {
+ // also set Y
+ oscillatingY = 1;
+ of_hover_ctrl_Y.PID.P = of_hover_ctrl_X.PID.P;
+ }
+ }
+ if ((covariances.y < of_hover_ctrl_Y.cov_setpoint) && (!oscillatingY)) {
+ oscillatingY = 1;
+ of_hover_ctrl_Y.PID.P = of_hover_ctrl_Y.PID.P * of_hover_ctrl_Y.reduction_factor;
+ }
+
+ // Compute 0, 1 or 2 horizontal axes with optitrack
+ computeOptiTrack(!oscphi, !osctheta, &ofh_sp_eu);
+ // Run the stabilization mode
+ stabilization_attitude_set_rpy_setpoint_i(&ofh_sp_eu);
+
+ prev_vision_timeXY = vision_time;
+}
+
+/**
+ * Run the vertical optical flow hover module
+ */
+void vertical_ctrl_module_run(bool in_flight)
+{
+ /***********
+ * TIME
+ ***********/
+
+ float div_factor; // factor that maps divergence in pixels as received from vision to 1 / frame
+
+ float dt = vision_time - prev_vision_timeZ;
+
+ // check if new measurement received
+ if (dt <= 1e-5f) {
+ return;
+ }
+
+ /***********
+ * VISION
+ ***********/
+
+ float lp_factor = dt / OF_LP_CONST;
+ Bound(lp_factor, 0.f, 1.f);
+
+ // Vision
+ div_factor = -1.28f; // magic number comprising field of view etc.
+ float new_divergence = (divergence_vision * div_factor) / dt;
+
+ // deal with (unlikely) fast changes in divergence:
+ static const float max_div_dt = 0.20f;
+ if (fabsf(new_divergence - of_hover.divergence) > max_div_dt) {
+ if (new_divergence < of_hover.divergence) { new_divergence = of_hover.divergence - max_div_dt; }
+ else { new_divergence = of_hover.divergence + max_div_dt; }
+ }
+ // low-pass filter the divergence:
+
+ of_hover.divergence += (new_divergence - of_hover.divergence) * lp_factor;
+ prev_vision_timeZ = vision_time;
+
+ /***********
+ * CONTROL
+ ***********/
+ if (!oscillatingZ) {
+ // if not oscillating, increase gain
+ of_hover_ctrl_Z.PID.P += of_hover_ctrl_Z.ramp * dt;
+ }
+
+ // use the divergence for control:
+ of_hover_ctrl_Z.PID.err = of_hover_ctrl_Z.setpoint - of_hover.divergence;
+ des_inputs.thrust = PID_divergence_control(dt, &of_hover_ctrl_Z);
+
+ covariances.z = set_cov_div(cov_method, &historyZ, &des_inputs);
+
+ // Check for oscillations
+ if (covariances.z < of_hover_ctrl_Z.cov_setpoint && (!oscillatingZ)) {
+ float estimatedHeight = of_hover_ctrl_Z.PID.P * OFH_VER_SLOPE_A + OFH_VER_SLOPE_B; // Vertical slope Z
+ oscillatingZ = 1;
+ of_hover_ctrl_Z.setpoint = 0.0f;
+ of_hover_ctrl_Z.PID.P = of_hover_ctrl_Z.PID.P * of_hover_ctrl_Z.reduction_factor;
+
+ if (hover_method == 0) {
+ // Z- X - Y Order
+ // Start the X axis
+ oscillatingX = 0;
+ of_hover_ctrl_X.PID.P = OFH_PGAINX;
+ } else if (hover_method == 2) {
+ // Start XY axes with computed slope
+ of_hover_ctrl_X.PID.sum_err = 0.0f;
+ of_hover_ctrl_Y.PID.sum_err = 0.0f;
+ of_hover_ctrl_X.PID.I = OFH_IGAINX;
+ of_hover_ctrl_Y.PID.I = OFH_IGAINY;
+ of_hover_ctrl_X.PID.P = OFH_REDUCTIONXY * (estimatedHeight * OFH_HOR_X_SLOPE_A - OFH_HOR_X_SLOPE_B); // Horizontal Slope X
+ of_hover_ctrl_Y.PID.P = OFH_REDUCTIONXY * (estimatedHeight * OFH_HOR_Y_SLOPE_A - OFH_HOR_Y_SLOPE_B); // Horizontal Slope Y
+ } else {
+ // hover_method = 1
+ //All axes
+ }
+ }
+
+ stabilization_cmd[COMMAND_THRUST] = des_inputs.thrust;
+}
+
+void ofh_optical_flow_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, int16_t flow_x, int16_t flow_y,
+ int16_t flow_der_x, int16_t flow_der_y, float quality, float size_div)
+{
+ if (!derotated) {
+ flowX = flow_x;
+ flowY = flow_y;
+ } else {
+ flowX = flow_der_x;
+ flowY = flow_der_y;
+ }
+
+ divergence_vision = size_div;
+
+ vision_time = ((float)stamp) / 1e6;
+}
+
+////////////////////////////////////////////////////////////////////
+// Call our vertical controller
+void guidance_v_module_init(void)
+{
+ vertical_ctrl_module_init();
+}
+
+// Call our horizontal controller
+void guidance_h_module_init(void)
+{
+ horizontal_ctrl_module_init();
+}
+
+/**
+ * Entering the vertical module (user switched to module)
+ */
+void guidance_v_module_enter(void)
+{
+ reset_vertical_vars();
+
+ // adaptive estimation - assume hover condition when entering the module
+ of_hover_ctrl_Z.nominal_value = (float) stabilization_cmd[COMMAND_THRUST] / MAX_PPRZ;
+ des_inputs.thrust = (int32_t) of_hover_ctrl_Z.nominal_value * MAX_PPRZ;
+}
+
+/**
+ * Entering the horizontal module (user switched to module)
+ */
+void guidance_h_module_enter(void)
+{
+ // Set current psi as heading
+ ofh_sp_eu.psi = stateGetNedToBodyEulers_i()->psi;
+
+ VECT2_COPY(of_hover_ref_pos, *stateGetPositionNed_i());
+ reset_horizontal_vars();
+
+}
+
+// Run the veritcal controller
+void guidance_v_module_run(bool in_flight)
+{
+ if (electrical.bat_low) {
+ autopilot_static_set_mode(AP_MODE_NAV);
+ } else {
+ // your vertical controller goes here
+ vertical_ctrl_module_run(in_flight);
+ }
+}
+
+// Run the horizontal controller
+void guidance_h_module_run(bool in_flight)
+{
+
+ if (electrical.bat_low) {
+ autopilot_static_set_mode(AP_MODE_NAV);
+ } else {
+ horizontal_ctrl_module_run(in_flight);
+ stabilization_attitude_run(in_flight);
+ }
+}
+
+/**
+ * Get the desired Euler angles for optitrack stabilization
+ * @param[in] Boolean whether to Phi or not
+ * @param[in] Boolean whether to Theta or not
+ * @param[out] The desired Euler angles
+ */
+void computeOptiTrack(bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
+{
+
+ bool optiVelOnly;
+ optiVelOnly = 0;
+
+ int32_t psi = stateGetNedToBodyEulers_i()->psi;
+
+ struct NedCoor_i vel_from_GPS;
+ struct NedCoor_i pos_from_GPS;
+
+ vel_from_GPS = *stateGetSpeedNed_i();
+ pos_from_GPS = *stateGetPositionNed_i();
+
+ /* maximum bank angle: default 20 deg, max 40 deg*/
+ static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
+ BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
+ static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
+
+ /* compute position error */
+ VECT2_DIFF(of_hover_pos_err, of_hover_ref_pos, pos_from_GPS);
+ /* saturate it */
+ VECT2_STRIM(of_hover_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
+
+ struct Int32Vect2 ref_speed;
+ ref_speed.x = 0;
+ ref_speed.y = 0;
+
+ /* compute speed error */
+ VECT2_DIFF(of_hover_speed_err, ref_speed, vel_from_GPS);
+ /* saturate it */
+ VECT2_STRIM(of_hover_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+
+ if (optiVelOnly) {
+ of_hover_pos_err.x = 0;
+ of_hover_pos_err.y = 0;
+ }
+
+ /* run PID */
+ of_hover_cmd_earth.x =
+ ((GUIDANCE_H_PGAIN * of_hover_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
+ ((GUIDANCE_H_DGAIN * (of_hover_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
+ of_hover_cmd_earth.y =
+ ((GUIDANCE_H_PGAIN * of_hover_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
+ ((GUIDANCE_H_DGAIN * (of_hover_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
+
+ /* trim max bank angle from PD */
+ VECT2_STRIM(of_hover_cmd_earth, -traj_max_bank, traj_max_bank);
+
+ of_hover_trim_att_integrator.x += (GUIDANCE_H_IGAIN * of_hover_cmd_earth.x);
+ of_hover_trim_att_integrator.y += (GUIDANCE_H_IGAIN * of_hover_cmd_earth.y);
+ /* saturate it */
+ VECT2_STRIM(of_hover_trim_att_integrator, -(traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)),
+ (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)));
+
+ /* add it to the command */
+ of_hover_cmd_earth.x += (of_hover_trim_att_integrator.x >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2));
+ of_hover_cmd_earth.y += (of_hover_trim_att_integrator.y >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2));
+
+ VECT2_STRIM(of_hover_cmd_earth, -total_max_bank, total_max_bank);
+
+ // Compute Angle Setpoints - Taken from Stab_att_quat
+ int32_t s_psi, c_psi;
+ PPRZ_ITRIG_SIN(s_psi, psi);
+ PPRZ_ITRIG_COS(c_psi, psi);
+
+ if (phi) {
+ opti_sp_eu->phi = (-s_psi * of_hover_cmd_earth.x + c_psi * of_hover_cmd_earth.y) >> INT32_TRIG_FRAC;
+ }
+ if (theta) {
+ opti_sp_eu->theta = -(c_psi * of_hover_cmd_earth.x + s_psi * of_hover_cmd_earth.y) >> INT32_TRIG_FRAC;
+ }
+}
diff --git a/sw/airborne/modules/ctrl/optical_flow_hover.h b/sw/airborne/modules/ctrl/optical_flow_hover.h
new file mode 100644
index 0000000000..e11c601908
--- /dev/null
+++ b/sw/airborne/modules/ctrl/optical_flow_hover.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2018
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+#ifndef OPTICAL_FLOW_HOVER_H_
+#define OPTICAL_FLOW_HOVER_H_
+
+#include "std.h"
+// Without optitrack set to: GUIDANCE_V/H_MODE_ATTITUDE
+// With optitrack set to: GUIDANCE_V/H_MODE_NAV
+// To use the Optical Flow Hover module use GUIDANCE_V/H_MODE_MODULE
+
+#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
+// #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_NAV
+
+#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
+//#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_NAV
+
+extern bool oscphi;
+extern bool osctheta;
+extern bool derotated;
+extern bool cov_method; ///< method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past / flow and past flow(1)
+extern uint8_t hover_method; ///< Method used to hover 0 = All axis after each other; 1 = all axis at the same time; 2 = vertical only, use relation to set horizontal
+
+extern struct OpticalFlowHoverControl of_hover_ctrl_X;
+extern struct OpticalFlowHoverControl of_hover_ctrl_Y;
+extern struct OpticalFlowHoverControl of_hover_ctrl_Z;
+
+// The module functions
+extern void optical_flow_hover_init(void);
+
+// Vertical loops
+extern void guidance_v_module_init(void);
+extern void guidance_v_module_enter(void);
+extern void guidance_v_module_run(bool in_flight);
+
+// Horizontal loops
+extern void guidance_h_module_init(void);
+extern void guidance_h_module_enter(void);
+extern void guidance_h_module_run(bool in_flight);
+extern void guidance_h_module_read_rc(void);
+
+#endif /* OPTICAL_FLOW_LANDING_H_ */