Paparazzi UAS  v5.15_devel-111-g8fb4629
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
optical_flow_landing.c File Reference

This module implements optical flow landings in which the divergence is kept constant. 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, int16_t flow_x, int16_t flow_y, int16_t flow_der_x, int16_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

#define INCREASE_GAIN_PER_SECOND   0.02

Definition at line 130 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

#define MINIMUM_GAIN   0.1

Definition at line 127 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

#define OFL_AGL_ID   ABI_BROADCAST

Definition at line 64 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_CONTROL_METHOD   0

Definition at line 92 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_COV_LANDING_LIMIT   2.2

Definition at line 105 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_COV_METHOD   0

Definition at line 96 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_COV_SETPOINT   -0.0075

Definition at line 109 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_COV_WINDOW_SIZE   30

Definition at line 101 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_init().

#define OFL_DGAIN   0.0

Definition at line 84 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_ELC_OSCILLATE   true

Definition at line 121 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_IGAIN   0.01

Definition at line 80 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_LP_CONST   0.02

Definition at line 113 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_OPTICAL_FLOW_ID   ABI_BROADCAST

Definition at line 70 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_P_LAND_THRESHOLD   0.15

Definition at line 117 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_PGAIN   0.40

Definition at line 76 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OFL_VISION_METHOD   1

Definition at line 88 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

Function Documentation

uint32_t final_landing_procedure ( void  )
static

Execute a final landing procedure.

Definition at line 502 of file optical_flow_landing.c.

References MAX_PPRZ, OpticalFlowLanding::nominal_thrust, and of_landing_ctrl.

Referenced by vertical_ctrl_module_run().

+ Here is the caller graph for this function:

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(), and stabilization_cmd.

+ Here is the call graph for this function:

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:

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:

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 OpticalFlowLanding::d_err, OpticalFlowLanding::divergence, MAX_PPRZ, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, 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:

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, OpticalFlowLanding::divergence, and of_landing_ctrl.

Referenced by vertical_ctrl_module_init().

+ Here is the caller graph for this function:

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 OpticalFlowLanding::COV_METHOD, covariance_f(), OpticalFlowLanding::delay_steps, OpticalFlowLanding::divergence, ind_hist, MAX_PPRZ, normalized_thrust, of_landing_ctrl, 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:

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, 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:

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:

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, 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, 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:

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

Definition at line 600 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

+ Here is the caller graph for this function:

Variable Documentation

abi_event agl_ev
static

The altitude ABI event.

Definition at line 153 of file optical_flow_landing.c.

int32_t count_covdiv

Definition at line 150 of file optical_flow_landing.c.

uint8_t cov_array_filled

Definition at line 187 of file optical_flow_landing.c.

float cov_div

Definition at line 136 of file optical_flow_landing.c.

Referenced by set_cov_div(), and vertical_ctrl_module_run().

float divergence_history[OFL_COV_WINDOW_SIZE]

Definition at line 184 of file optical_flow_landing.c.

float divergence_setpoint

Definition at line 144 of file optical_flow_landing.c.

float divergence_vision

Definition at line 133 of file optical_flow_landing.c.

float divergence_vision_dt

Definition at line 134 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

float dstate

Definition at line 139 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

float elc_d_gain_start

Definition at line 149 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

float elc_i_gain_start

Definition at line 149 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

float elc_p_gain_start

Definition at line 149 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

int32_t elc_phase

Definition at line 147 of file optical_flow_landing.c.

uint32_t elc_time_start

Definition at line 148 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

uint32_t ind_hist

Definition at line 186 of file optical_flow_landing.c.

Referenced by set_cov_div().

float istate

Definition at line 138 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

bool landing

Definition at line 141 of file optical_flow_landing.c.

float lp_cov_div

Definition at line 151 of file optical_flow_landing.c.

float normalized_thrust

Definition at line 135 of file optical_flow_landing.c.

Referenced by set_cov_div().

abi_event optical_flow_ev
static

Definition at line 154 of file optical_flow_landing.c.

float past_divergence_history[OFL_COV_WINDOW_SIZE]

Definition at line 185 of file optical_flow_landing.c.

float prev_vision_time

Definition at line 140 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

float previous_cov_err

Definition at line 142 of file optical_flow_landing.c.

float pstate

Definition at line 137 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

float pused

Definition at line 137 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

float thrust_history[OFL_COV_WINDOW_SIZE]

Definition at line 183 of file optical_flow_landing.c.

int32_t thrust_set

Definition at line 143 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

float vision_time

Definition at line 140 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().