Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
optical_flow_landing.c File Reference

This module implements optical flow landings in which the divergence is kept constant. When using a fixed gain for control, the covariance between thrust and divergence is tracked, so that the drone knows when it has arrived close to the landing surface. Then, a final landing procedure is triggered. It can also be set to adaptive gain control, where the goal is to continuously gauge the distance to the landing surface. In this mode, the drone will oscillate all the way down to the surface. More...

+ Include dependency graph for optical_flow_landing.c:

Go to the source code of this file.

Macros

#define OFL_AGL_ID   ABI_BROADCAST
 
#define OFL_OPTICAL_FLOW_ID   ABI_BROADCAST
 
#define OFL_PGAIN   0.40
 
#define OFL_IGAIN   0.01
 
#define OFL_DGAIN   0.0
 
#define OFL_VISION_METHOD   1
 
#define OFL_CONTROL_METHOD   0
 
#define OFL_COV_METHOD   0
 
#define OFL_COV_WINDOW_SIZE   30
 
#define OFL_COV_LANDING_LIMIT   2.2
 
#define OFL_COV_SETPOINT   -0.0075
 
#define OFL_LP_CONST   0.02
 
#define OFL_P_LAND_THRESHOLD   0.15
 
#define OFL_ELC_OSCILLATE   true
 
#define MINIMUM_GAIN   0.1
 
#define INCREASE_GAIN_PER_SECOND   0.02
 

Functions

static void send_divergence (struct transport_tx *trans, struct link_device *dev)
 
void vertical_ctrl_agl_cb (uint8_t sender_id, uint32_t stamp, float distance)
 Function definitions Callback function of the ground altitude. More...
 
void vertical_ctrl_optical_flow_cb (uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
 
static void set_cov_div (int32_t thrust)
 Set the covariance of the divergence and the thrust / past divergence This funciton should only be called once per time step. More...
 
static int32_t PID_divergence_control (float setpoint, float P, float I, float D, float dt)
 Determine and set the thrust for constant divergence control. More...
 
static void update_errors (float err, float dt)
 Updates the integral and differential errors for PID control and sets the previous error. More...
 
static uint32_t final_landing_procedure ()
 Execute a final landing procedure. More...
 
static void reset_all_vars (void)
 Reset all variables: More...
 
void vertical_ctrl_module_init (void)
 Initialize the optical flow landing module. More...
 
void vertical_ctrl_module_run (bool in_flight)
 Run the optical flow landing module. More...
 
void guidance_v_module_init (void)
 
void guidance_v_module_enter (void)
 Entering the module (user switched to module) More...
 
void guidance_v_module_run (bool in_flight)
 

Variables

float divergence_vision
 
float divergence_vision_dt
 
float normalized_thrust
 
float cov_div
 
float pstate
 
float pused
 
float istate
 
float dstate
 
float vision_time
 
float prev_vision_time
 
bool landing
 
float previous_cov_err
 
int32_t thrust_set
 
float divergence_setpoint
 
int32_t elc_phase
 
uint32_t elc_time_start
 
float elc_p_gain_start
 
float elc_i_gain_start
 
float elc_d_gain_start
 
int32_t count_covdiv
 
float lp_cov_div
 
static abi_event agl_ev
 The altitude ABI event. More...
 
static abi_event optical_flow_ev
 
struct OpticalFlowLanding of_landing_ctrl
 
float thrust_history [OFL_COV_WINDOW_SIZE]
 
float divergence_history [OFL_COV_WINDOW_SIZE]
 
float past_divergence_history [OFL_COV_WINDOW_SIZE]
 
uint32_t ind_hist
 
uint8_t cov_array_filled
 

Detailed Description

This module implements optical flow landings in which the divergence is kept constant. When using a fixed gain for control, the covariance between thrust and divergence is tracked, so that the drone knows when it has arrived close to the landing surface. Then, a final landing procedure is triggered. It can also be set to adaptive gain control, where the goal is to continuously gauge the distance to the landing surface. In this mode, the drone will oscillate all the way down to the surface.

de Croon, G.C.H.E. (2016). Monocular distance estimation with optical flow maneuvers and efference copies: a stability-based strategy. Bioinspiration & biomimetics, 11(1), 016004. http://iopscience.iop.org/article/10.1088/1748-3190/11/1/016004

Based on the above theory, we have also developed a new strategy for landing that consists of two phases: (1) while hovering, the drone determines the optimal gain by increasing the gain until oscillation (2) the drone starts landing while exponentially decreasing the gain over time

This strategy leads to smooth, high-performance constant divergence landings, as explained in the article: H.W. Ho, G.C.H.E. de Croon, E. van Kampen, Q.P. Chu, and M. Mulder (submitted) Adaptive Control Strategy for Constant Optical Flow Divergence Landing, https://arxiv.org/abs/1609.06767

Definition in file optical_flow_landing.c.

Macro Definition Documentation

◆ INCREASE_GAIN_PER_SECOND

#define INCREASE_GAIN_PER_SECOND   0.02

Definition at line 130 of file optical_flow_landing.c.

◆ MINIMUM_GAIN

#define MINIMUM_GAIN   0.1

Definition at line 127 of file optical_flow_landing.c.

◆ OFL_AGL_ID

#define OFL_AGL_ID   ABI_BROADCAST

Definition at line 64 of file optical_flow_landing.c.

◆ OFL_CONTROL_METHOD

#define OFL_CONTROL_METHOD   0

Definition at line 92 of file optical_flow_landing.c.

◆ OFL_COV_LANDING_LIMIT

#define OFL_COV_LANDING_LIMIT   2.2

Definition at line 105 of file optical_flow_landing.c.

◆ OFL_COV_METHOD

#define OFL_COV_METHOD   0

Definition at line 96 of file optical_flow_landing.c.

◆ OFL_COV_SETPOINT

#define OFL_COV_SETPOINT   -0.0075

Definition at line 109 of file optical_flow_landing.c.

◆ OFL_COV_WINDOW_SIZE

#define OFL_COV_WINDOW_SIZE   30

Definition at line 101 of file optical_flow_landing.c.

◆ OFL_DGAIN

#define OFL_DGAIN   0.0

Definition at line 84 of file optical_flow_landing.c.

◆ OFL_ELC_OSCILLATE

#define OFL_ELC_OSCILLATE   true

Definition at line 121 of file optical_flow_landing.c.

◆ OFL_IGAIN

#define OFL_IGAIN   0.01

Definition at line 80 of file optical_flow_landing.c.

◆ OFL_LP_CONST

#define OFL_LP_CONST   0.02

Definition at line 113 of file optical_flow_landing.c.

◆ OFL_OPTICAL_FLOW_ID

#define OFL_OPTICAL_FLOW_ID   ABI_BROADCAST

Definition at line 70 of file optical_flow_landing.c.

◆ OFL_P_LAND_THRESHOLD

#define OFL_P_LAND_THRESHOLD   0.15

Definition at line 117 of file optical_flow_landing.c.

◆ OFL_PGAIN

#define OFL_PGAIN   0.40

Definition at line 76 of file optical_flow_landing.c.

◆ OFL_VISION_METHOD

#define OFL_VISION_METHOD   1

Definition at line 88 of file optical_flow_landing.c.

Function Documentation

◆ final_landing_procedure()

uint32_t final_landing_procedure ( void  )
static

Execute a final landing procedure.

Definition at line 502 of file optical_flow_landing.c.

References landing, MAX_PPRZ, OpticalFlowLanding::nominal_thrust, and of_landing_ctrl.

Referenced by vertical_ctrl_module_run().

+ Here is the caller graph for this function:

◆ guidance_v_module_enter()

void guidance_v_module_enter ( void  )

Entering the module (user switched to module)

Entering the vertical module (user switched to module)

Definition at line 618 of file optical_flow_landing.c.

References MAX_PPRZ, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, reset_all_vars(), stabilization_cmd, and thrust_set.

+ Here is the call graph for this function:

◆ guidance_v_module_init()

void guidance_v_module_init ( void  )

Definition at line 610 of file optical_flow_landing.c.

References vertical_ctrl_module_init().

+ Here is the call graph for this function:

◆ guidance_v_module_run()

void guidance_v_module_run ( bool  in_flight)

Definition at line 627 of file optical_flow_landing.c.

References vertical_ctrl_module_run().

+ Here is the call graph for this function:

◆ PID_divergence_control()

int32_t PID_divergence_control ( float  setpoint,
float  P,
float  I,
float  D,
float  dt 
)
static

Determine and set the thrust for constant divergence control.

Parameters
[out]thrust
[in]divergence_set_pointThe desired divergence
[in]PP-gain
[in]II-gain
[in]DD-gain
[in]dttime difference since last update

Definition at line 555 of file optical_flow_landing.c.

References D, OpticalFlowLanding::d_err, OpticalFlowLanding::divergence, I, MAX_PPRZ, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, P, set_cov_div(), OpticalFlowLanding::sum_err, and update_errors().

Referenced by vertical_ctrl_module_run().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ reset_all_vars()

◆ send_divergence()

static void send_divergence ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 160 of file optical_flow_landing.c.

References OpticalFlowLanding::agl, cov_div, dev, OpticalFlowLanding::divergence, divergence_vision_dt, normalized_thrust, of_landing_ctrl, pstate, and pused.

Referenced by vertical_ctrl_module_init().

+ Here is the caller graph for this function:

◆ set_cov_div()

void set_cov_div ( int32_t  thrust)
static

Set the covariance of the divergence and the thrust / past divergence This funciton should only be called once per time step.

Parameters
[in]thrustthe current thrust value

Definition at line 518 of file optical_flow_landing.c.

References cov_array_filled, cov_div, OpticalFlowLanding::COV_METHOD, covariance_f(), OpticalFlowLanding::delay_steps, OpticalFlowLanding::divergence, divergence_history, ind_hist, MAX_PPRZ, normalized_thrust, of_landing_ctrl, past_divergence_history, thrust_history, and OpticalFlowLanding::window_size.

Referenced by PID_divergence_control().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ update_errors()

void update_errors ( float  err,
float  dt 
)
static

Updates the integral and differential errors for PID control and sets the previous error.

Parameters
[in]errthe error of the divergence and divergence setpoint
[in]dttime difference since last update

Definition at line 583 of file optical_flow_landing.c.

References OpticalFlowLanding::d_err, f, OpticalFlowLanding::lp_const, of_landing_ctrl, OpticalFlowLanding::previous_err, and OpticalFlowLanding::sum_err.

Referenced by PID_divergence_control().

+ Here is the caller graph for this function:

◆ vertical_ctrl_agl_cb()

static void vertical_ctrl_agl_cb ( uint8_t sender_id  UNUSED,
uint32_t  stamp,
float  distance 
)

Function definitions Callback function of the ground altitude.

Definition at line 595 of file optical_flow_landing.c.

References OpticalFlowLanding::agl, and of_landing_ctrl.

Referenced by vertical_ctrl_module_init().

+ Here is the caller graph for this function:

◆ vertical_ctrl_module_init()

void vertical_ctrl_module_init ( void  )

Initialize the optical flow landing module.

Definition at line 195 of file optical_flow_landing.c.

References OpticalFlowLanding::agl, agl_ev, OpticalFlowLanding::agl_lp, OpticalFlowLanding::CONTROL_METHOD, OpticalFlowLanding::cov_limit, OpticalFlowLanding::COV_METHOD, OpticalFlowLanding::cov_set_point, OpticalFlowLanding::d_err, DefaultPeriodic, OpticalFlowLanding::delay_steps, OpticalFlowLanding::dgain, OpticalFlowLanding::dgain_adaptive, OpticalFlowLanding::divergence, OpticalFlowLanding::divergence_setpoint, OpticalFlowLanding::elc_oscillate, guidance_v_nominal_throttle, OpticalFlowLanding::igain, OpticalFlowLanding::igain_adaptive, OpticalFlowLanding::lp_const, OpticalFlowLanding::lp_cov_div_factor, MAX_PPRZ, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, OFL_AGL_ID, OFL_CONTROL_METHOD, OFL_COV_LANDING_LIMIT, OFL_COV_METHOD, OFL_COV_SETPOINT, OFL_COV_WINDOW_SIZE, OFL_DGAIN, OFL_ELC_OSCILLATE, OFL_IGAIN, OFL_LP_CONST, OFL_OPTICAL_FLOW_ID, OFL_P_LAND_THRESHOLD, OFL_PGAIN, OFL_VISION_METHOD, optical_flow_ev, OpticalFlowLanding::p_land_threshold, OpticalFlowLanding::pgain, OpticalFlowLanding::pgain_adaptive, OpticalFlowLanding::previous_err, OpticalFlowLanding::reduction_factor_elc, register_periodic_telemetry(), reset_all_vars(), send_divergence(), OpticalFlowLanding::sum_err, OpticalFlowLanding::t_transition, OpticalFlowLanding::vel, vertical_ctrl_agl_cb(), vertical_ctrl_optical_flow_cb(), OpticalFlowLanding::VISION_METHOD, and OpticalFlowLanding::window_size.

Referenced by guidance_v_module_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ vertical_ctrl_module_run()

void vertical_ctrl_module_run ( bool  in_flight)

◆ vertical_ctrl_optical_flow_cb()

void vertical_ctrl_optical_flow_cb ( uint8_t sender_id  UNUSED,
uint32_t  stamp,
int32_t flow_x  UNUSED,
int32_t flow_y  UNUSED,
int32_t flow_der_x  UNUSED,
int32_t flow_der_y  UNUSED,
float quality  UNUSED,
float  size_divergence 
)

Definition at line 600 of file optical_flow_landing.c.

References divergence_vision, and vision_time.

Referenced by vertical_ctrl_module_init().

+ Here is the caller graph for this function:

Variable Documentation

◆ agl_ev

abi_event agl_ev
static

The altitude ABI event.

Definition at line 153 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

◆ count_covdiv

int32_t count_covdiv

Definition at line 150 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ cov_array_filled

uint8_t cov_array_filled

Definition at line 187 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and set_cov_div().

◆ cov_div

float cov_div

◆ divergence_history

float divergence_history[OFL_COV_WINDOW_SIZE]

Definition at line 184 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and set_cov_div().

◆ divergence_setpoint

float divergence_setpoint

Definition at line 144 of file optical_flow_landing.c.

Referenced by reset_all_vars().

◆ divergence_vision

float divergence_vision

◆ divergence_vision_dt

float divergence_vision_dt

◆ dstate

float dstate

Definition at line 139 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ elc_d_gain_start

float elc_d_gain_start

Definition at line 149 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

◆ elc_i_gain_start

float elc_i_gain_start

Definition at line 149 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

◆ elc_p_gain_start

float elc_p_gain_start

Definition at line 149 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

◆ elc_phase

int32_t elc_phase

Definition at line 147 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ elc_time_start

uint32_t elc_time_start

Definition at line 148 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ ind_hist

uint32_t ind_hist

Definition at line 186 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and set_cov_div().

◆ istate

float istate

Definition at line 138 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ landing

bool landing

◆ lp_cov_div

float lp_cov_div

Definition at line 151 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ normalized_thrust

float normalized_thrust

Definition at line 135 of file optical_flow_landing.c.

Referenced by reset_all_vars(), send_divergence(), and set_cov_div().

◆ of_landing_ctrl

◆ optical_flow_ev

abi_event optical_flow_ev
static

Definition at line 154 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

◆ past_divergence_history

float past_divergence_history[OFL_COV_WINDOW_SIZE]

Definition at line 185 of file optical_flow_landing.c.

Referenced by set_cov_div().

◆ prev_vision_time

float prev_vision_time

Definition at line 140 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ previous_cov_err

float previous_cov_err

Definition at line 142 of file optical_flow_landing.c.

Referenced by reset_all_vars().

◆ pstate

float pstate

◆ pused

float pused

◆ thrust_history

float thrust_history[OFL_COV_WINDOW_SIZE]

Definition at line 183 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and set_cov_div().

◆ thrust_set

int32_t thrust_set

◆ vision_time

float vision_time