Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
optical_flow_landing.h File Reference

This module implements optical flow landings in which the divergence is kept constant. More...

#include "std.h"
+ Include dependency graph for optical_flow_landing.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  OpticalFlowLanding
 

Macros

#define MAX_SAMPLES_LEARNING   25000
 
#define GUIDANCE_H_MODE_MODULE_SETTING   GUIDANCE_H_MODE_MODULE
 
#define GUIDANCE_V_MODE_MODULE_SETTING   GUIDANCE_V_MODE_MODULE
 

Functions

void guidance_h_module_init (void)
 Initialization of horizontal guidance module. More...
 
void guidance_h_module_enter (void)
 Entering the horizontal module (user switched to module) More...
 
void guidance_h_module_run (bool in_flight)
 Main guidance loop. More...
 
void guidance_h_module_read_rc (void)
 Read the RC commands. More...
 
void guidance_v_module_init (void)
 
void guidance_v_module_enter (void)
 Entering the vertical module (user switched to module) More...
 
void guidance_v_module_run (bool in_flight)
 
void save_texton_distribution (void)
 
void load_texton_distribution (void)
 
void fit_linear_model_OF (float *targets, float **samples, uint8_t D, uint16_t count, float *parameters, float *fit_error)
 Fit a linear model from samples to target values. More...
 
void learn_from_file (void)
 
float predict_gain (float *distribution)
 
void save_weights (void)
 
void load_weights (void)
 
void recursive_least_squares_batch (float *targets, float **samples, uint8_t D, uint16_t count, float *params, float *fit_error)
 Recursively fit a linear model from samples to target values - batch mode, possibly for initialization. More...
 
void recursive_least_squares (float target, float *sample, uint8_t length_sample, float *params)
 

Variables

struct OpticalFlowLanding of_landing_ctrl
 

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

The module also contains code for learning to map the visual appearance of the landing surface with self-supervised learning (SSL) to the corresponding control gains. This leads to smooth optical flow landings where the "height" expressed in the control gain is known and can be used to shut down the motors. This has been published in the following paper:

de Croon, Guido CHE, Christophe De Wagter, and Tobias Seidl. "Enhancing optical-flow-based control by learning visual appearance cues for flying robots." Nature Machine Intelligence 3.1 (2021): 33-41.

Finally, horizontal flow control has been added, so that the drone can hover without optitrack. In fact, this module also formed the basis for the experiments in which the attitude is estimated with only optical flow and gyro measurements (without accelerometers), published in Nature:

De Croon, G.C.H.E., Dupeyroux, J.J., De Wagter, C., Chatterjee, A., Olejnik, D. A., & Ruffier, F. (2022). Accommodating unobservability to control flight attitude with optic flow. Nature, 610(7932), 485-490.

Definition in file optical_flow_landing.h.


Data Structure Documentation

◆ OpticalFlowLanding

struct OpticalFlowLanding

Definition at line 52 of file optical_flow_landing.h.

Data Fields
uint32_t active_motion Method for actively inducing motion.
float agl agl = height from sonar (only used when using "fake" divergence)
float agl_lp low-pass version of agl
float close_to_edge if abs(cov_div - reference cov_div) < close_to_edge, then texton distributions and gains are stored for learning
uint32_t CONTROL_METHOD type of divergence control: 0 = fixed gain, 1 = adaptive gain, 2 = exponential time landing control. 3 = learning control
float cov_limit for fixed gain control, what is the cov limit triggering the landing
uint32_t COV_METHOD method to calculate the covariance: between thrust and div (0) or div and div past (1)
float cov_set_point for adaptive gain control, setpoint of the covariance (oscillations)
float d_err difference of error for the D-gain
uint32_t delay_steps number of delay steps for div past
float dgain D-gain for constant divergence control.
float dgain_adaptive D-gain for adaptive gain control.
float divergence Divergence estimate.
float divergence_setpoint setpoint for constant divergence approach
bool elc_oscillate Whether or not to oscillate at beginning of elc to find optimum gain.
float front_div_threshold Threshold when the front div gets above this value, it will command a horizontal stop.
float igain I-gain for constant divergence control.
float igain_adaptive I-gain for adaptive gain control.
float igain_horizontal_factor factor multiplied with the vertical I-gain for horizontal ventral-flow-based control
volatile bool learn_gains set to true if the robot needs to learn a mapping from texton distributions to the p-gain
bool load_weights load the weights that were learned before
float lp_const low-pass value for divergence
float lp_cov_div_factor low-pass factor for the covariance of divergence in order to trigger the second landing phase in the exponential strategy.
float lp_factor_prediction low-pass value for the predicted P-value
float nominal_thrust nominal thrust around which the PID-control operates
float omega_FB Set point for the front-back ventral flow.
float omega_LR Set point for the left-right ventral flow.
float p_land_threshold if during the exponential landing the gain reaches this value, the final landing procedure is triggered
float pgain P-gain for constant divergence control (from divergence error to thrust)
float pgain_adaptive P-gain for adaptive gain control.
float pgain_horizontal_factor factor multiplied with the vertical P-gain for horizontal ventral-flow-based control
float pitch_trim Pitch trim angle in degrees.
float previous_err Previous divergence tracking error.
float ramp_duration ramp duration in seconds for when the divergence setpoint changes
float reduction_factor_elc reduction factor - after oscillation, how much to reduce the gain...
float roll_trim Roll trim angle in degrees.
bool snapshot if true, besides storing a texton distribution, an image will also be stored (when unstable)
float sum_err integration of the error for I-gain
float t_transition how many seconds the drone has to be oscillating in order to transition to the hover phase with reduced gain
bool use_bias if true, a bias 1.0f will be added to the feature vector (texton distribution) - this typically leads to very large weights
float vel vertical velocity as determined with sonar (only used when using "fake" divergence)
uint32_t VISION_METHOD whether to use vision (1) or Optitrack / sonar (0)
uint32_t window_size number of time steps in "window" used for getting the covariance

Macro Definition Documentation

◆ GUIDANCE_H_MODE_MODULE_SETTING

#define GUIDANCE_H_MODE_MODULE_SETTING   GUIDANCE_H_MODE_MODULE

Definition at line 106 of file optical_flow_landing.h.

◆ GUIDANCE_V_MODE_MODULE_SETTING

#define GUIDANCE_V_MODE_MODULE_SETTING   GUIDANCE_V_MODE_MODULE

Definition at line 109 of file optical_flow_landing.h.

◆ MAX_SAMPLES_LEARNING

#define MAX_SAMPLES_LEARNING   25000

Definition at line 50 of file optical_flow_landing.h.

Function Documentation

◆ fit_linear_model_OF()

void fit_linear_model_OF ( float *  targets,
float **  samples,
uint8_t  D,
uint16_t  count,
float *  params,
float *  fit_error 
)

Fit a linear model from samples to target values.

Parameters
[in]targetsThe target values
[in]samplesThe samples / feature vectors
[in]DThe dimensionality of the samples
[in]countThe number of samples
[out]parameters*Parameters of the linear fit
[out]fit_error*Total error of the fit

Definition at line 1438 of file optical_flow_landing.c.

References D, MAKE_MATRIX_PTR, MAT_MUL, MAT_SUB, n_samples, of_landing_ctrl, parameters, pprz_svd_float(), pprz_svd_solve_float(), and OpticalFlowLanding::use_bias.

Referenced by learn_from_file().

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

◆ guidance_h_module_enter()

void guidance_h_module_enter ( void  )

Entering the horizontal module (user switched to module)

Entering the horizontal module (user switched to module)

Definition at line 85 of file ctrl_module_innerloop_demo.c.

◆ guidance_h_module_init()

void guidance_h_module_init ( void  )

Initialization of horizontal guidance module.

Definition at line 80 of file ctrl_module_innerloop_demo.c.

◆ guidance_h_module_read_rc()

void guidance_h_module_read_rc ( void  )

Read the RC commands.

Definition at line 90 of file ctrl_module_innerloop_demo.c.

◆ guidance_h_module_run()

void guidance_h_module_run ( bool  in_flight)

Main guidance loop.

Parameters
[in]in_flightWhether we are in flight or not

Definition at line 99 of file ctrl_module_innerloop_demo.c.

◆ guidance_v_module_enter()

void guidance_v_module_enter ( void  )

Entering the vertical module (user switched to module)

Entering the vertical module (user switched to module)

Definition at line 111 of file ctrl_module_innerloop_demo.c.

◆ guidance_v_module_init()

void guidance_v_module_init ( void  )

Definition at line 105 of file ctrl_module_innerloop_demo.c.

◆ guidance_v_module_run()

void guidance_v_module_run ( bool  in_flight)

Definition at line 656 of file optical_flow_hover.c.

◆ learn_from_file()

void learn_from_file ( void  )

Definition at line 1274 of file optical_flow_landing.c.

References fit_linear_model_OF(), gains, load_texton_distribution(), n_read_samples, n_textons, RECURSIVE_LEARNING, recursive_least_squares_batch(), save_weights(), text_dists, and weights.

Referenced by vertical_ctrl_module_run().

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

◆ load_texton_distribution()

void load_texton_distribution ( void  )

Definition at line 1236 of file optical_flow_landing.c.

References cov_divs_log, distribution_logger, gains, MAX_SAMPLES_LEARNING, n_read_samples, n_textons, sonar_OF, text_dists, and TEXTON_DISTRIBUTION_PATH.

Referenced by learn_from_file().

+ Here is the caller graph for this function:

◆ load_weights()

void load_weights ( void  )

Definition at line 1544 of file optical_flow_landing.c.

References n_textons, TEXTON_DISTRIBUTION_PATH, weights, and weights_file.

Referenced by vertical_ctrl_module_run().

+ Here is the caller graph for this function:

◆ predict_gain()

float predict_gain ( float *  distribution)

Definition at line 1509 of file optical_flow_landing.c.

References n_textons, of_landing_ctrl, OpticalFlowLanding::use_bias, and weights.

Referenced by recursive_least_squares(), recursive_least_squares_batch(), and vertical_ctrl_module_run().

+ Here is the caller graph for this function:

◆ recursive_least_squares()

void recursive_least_squares ( float  target,
float *  sample,
uint8_t  length_sample,
float *  params 
)

◆ recursive_least_squares_batch()

void recursive_least_squares_batch ( float *  targets,
float **  samples,
uint8_t  D,
uint16_t  count,
float *  params,
float *  fit_error 
)

Recursively fit a linear model from samples to target values - batch mode, possibly for initialization.

Parameters
[in]targetsThe target values
[in]samplesThe samples / feature vectors
[in]DThe dimensionality of the samples
[in]countThe number of samples
[out]parameters*Parameters of the linear fit
[out]fit_error*Total error of the fit // TODO: relevant for RLS?

Definition at line 1313 of file optical_flow_landing.c.

References D, n_textons, of_landing_ctrl, P_RLS, predict_gain(), recursive_least_squares(), OpticalFlowLanding::use_bias, and weights.

Referenced by learn_from_file().

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

◆ save_texton_distribution()

void save_texton_distribution ( void  )

Definition at line 1209 of file optical_flow_landing.c.

References OpticalFlowLanding::agl, cov_div, distribution_logger, n_textons, of_landing_ctrl, pstate, texton_distribution, and TEXTON_DISTRIBUTION_PATH.

Referenced by vertical_ctrl_module_run().

+ Here is the caller graph for this function:

◆ save_weights()

void save_weights ( void  )

Definition at line 1526 of file optical_flow_landing.c.

References n_textons, TEXTON_DISTRIBUTION_PATH, weights, and weights_file.

Referenced by learn_from_file().

+ Here is the caller graph for this function:

Variable Documentation

◆ of_landing_ctrl