Paparazzi UAS  v7.0_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. 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_PGAIN_HORIZONTAL_FACTOR   0.05
 
#define OFL_IGAIN_HORIZONTAL_FACTOR   0.1
 
#define OFL_ROLL_TRIM   0.0f
 
#define OFL_PITCH_TRIM   0.0f
 
#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 OFL_CLOSE_TO_EDGE   0.025
 
#define OFL_PGAIN_ADAPTIVE   0.50
 
#define OFL_IGAIN_ADAPTIVE   0.50
 
#define OFL_DGAIN_ADAPTIVE   0.50
 
#define OFL_OMEGA_LR   0.0
 
#define OFL_OMEGA_FB   0.0
 
#define OFL_ACTIVE_MOTION   0
 
#define OFL_FRONT_DIV_THRESHOLD   0.3
 
#define HORIZONTAL_RATE_CONTROL   0
 
#define ACTIVE_RATES   0
 
#define MINIMUM_GAIN   0.1
 
#define INCREASE_GAIN_PER_SECOND   0.10
 
#define RECURSIVE_LEARNING   0
 
#define n_ts   10
 
#define TEXTON_DISTRIBUTION_PATH   /data/ftp/internal_000
 

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_h_module_init (void)
 
void guidance_h_module_enter (void)
 Entering the horizontal module (user switched to module) More...
 
void guidance_h_module_run (bool UNUSED in_flight)
 
void guidance_h_module_read_rc (void)
 
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)
 
void save_texton_distribution (void)
 
void load_texton_distribution (void)
 
void learn_from_file (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, UNUSED float *params)
 
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. More...
 
float predict_gain (float *distribution)
 
void save_weights (void)
 
void load_weights (void)
 

Variables

float * text_dists [MAX_SAMPLES_LEARNING]
 
float sonar_OF [MAX_SAMPLES_LEARNING]
 
float gains [MAX_SAMPLES_LEARNING]
 
float cov_divs_log [MAX_SAMPLES_LEARNING]
 
float * weights
 
float cov_div
 
float pstate
 
float divergence_vision
 
float old_flow_time
 
float optical_flow_x
 
float optical_flow_y
 
float lp_flow_x
 
float lp_flow_y
 
float lp_divergence_front
 
float sum_roll_error
 
float sum_pitch_error
 
float divergence_vision_dt
 
float normalized_thrust
 
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
 
float divergence_front
 
float texton_distribution_stereoboard [n_ts]
 
float ** P_RLS
 
float lambda
 
static FILE * distribution_logger = NULL
 
static FILE * weights_file = NULL
 
unsigned int n_read_samples
 
float module_active_time_sec
 
float module_enter_time
 
float previous_divergence_setpoint
 
int ramp
 
float delta_setpoint
 
float ramp_start_time
 
float start_setpoint_ramp
 
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

◆ ACTIVE_RATES

#define ACTIVE_RATES   0

Definition at line 213 of file optical_flow_landing.c.

◆ HORIZONTAL_RATE_CONTROL

#define HORIZONTAL_RATE_CONTROL   0

Definition at line 209 of file optical_flow_landing.c.

◆ INCREASE_GAIN_PER_SECOND

#define INCREASE_GAIN_PER_SECOND   0.10

Definition at line 221 of file optical_flow_landing.c.

◆ MINIMUM_GAIN

#define MINIMUM_GAIN   0.1

Definition at line 218 of file optical_flow_landing.c.

◆ n_ts

#define n_ts   10

Definition at line 260 of file optical_flow_landing.c.

◆ OFL_ACTIVE_MOTION

#define OFL_ACTIVE_MOTION   0

Definition at line 200 of file optical_flow_landing.c.

◆ OFL_AGL_ID

#define OFL_AGL_ID   ABI_BROADCAST

Definition at line 98 of file optical_flow_landing.c.

◆ OFL_CLOSE_TO_EDGE

#define OFL_CLOSE_TO_EDGE   0.025

Definition at line 175 of file optical_flow_landing.c.

◆ OFL_CONTROL_METHOD

#define OFL_CONTROL_METHOD   0

Definition at line 142 of file optical_flow_landing.c.

◆ OFL_COV_LANDING_LIMIT

#define OFL_COV_LANDING_LIMIT   2.2

Definition at line 155 of file optical_flow_landing.c.

◆ OFL_COV_METHOD

#define OFL_COV_METHOD   0

Definition at line 146 of file optical_flow_landing.c.

◆ OFL_COV_SETPOINT

#define OFL_COV_SETPOINT   -0.0075

Definition at line 159 of file optical_flow_landing.c.

◆ OFL_COV_WINDOW_SIZE

#define OFL_COV_WINDOW_SIZE   30

Definition at line 151 of file optical_flow_landing.c.

◆ OFL_DGAIN

#define OFL_DGAIN   0.0

Definition at line 118 of file optical_flow_landing.c.

◆ OFL_DGAIN_ADAPTIVE

#define OFL_DGAIN_ADAPTIVE   0.50

Definition at line 188 of file optical_flow_landing.c.

◆ OFL_ELC_OSCILLATE

#define OFL_ELC_OSCILLATE   true

Definition at line 171 of file optical_flow_landing.c.

◆ OFL_FRONT_DIV_THRESHOLD

#define OFL_FRONT_DIV_THRESHOLD   0.3

Definition at line 204 of file optical_flow_landing.c.

◆ OFL_IGAIN

#define OFL_IGAIN   0.01

Definition at line 114 of file optical_flow_landing.c.

◆ OFL_IGAIN_ADAPTIVE

#define OFL_IGAIN_ADAPTIVE   0.50

Definition at line 184 of file optical_flow_landing.c.

◆ OFL_IGAIN_HORIZONTAL_FACTOR

#define OFL_IGAIN_HORIZONTAL_FACTOR   0.1

Definition at line 126 of file optical_flow_landing.c.

◆ OFL_LP_CONST

#define OFL_LP_CONST   0.02

Definition at line 163 of file optical_flow_landing.c.

◆ OFL_OMEGA_FB

#define OFL_OMEGA_FB   0.0

Definition at line 196 of file optical_flow_landing.c.

◆ OFL_OMEGA_LR

#define OFL_OMEGA_LR   0.0

Definition at line 192 of file optical_flow_landing.c.

◆ OFL_OPTICAL_FLOW_ID

#define OFL_OPTICAL_FLOW_ID   ABI_BROADCAST

Definition at line 104 of file optical_flow_landing.c.

◆ OFL_P_LAND_THRESHOLD

#define OFL_P_LAND_THRESHOLD   0.15

Definition at line 167 of file optical_flow_landing.c.

◆ OFL_PGAIN

#define OFL_PGAIN   0.40

Definition at line 110 of file optical_flow_landing.c.

◆ OFL_PGAIN_ADAPTIVE

#define OFL_PGAIN_ADAPTIVE   0.50

Definition at line 180 of file optical_flow_landing.c.

◆ OFL_PGAIN_HORIZONTAL_FACTOR

#define OFL_PGAIN_HORIZONTAL_FACTOR   0.05

Definition at line 122 of file optical_flow_landing.c.

◆ OFL_PITCH_TRIM

#define OFL_PITCH_TRIM   0.0f

Definition at line 134 of file optical_flow_landing.c.

◆ OFL_ROLL_TRIM

#define OFL_ROLL_TRIM   0.0f

Definition at line 130 of file optical_flow_landing.c.

◆ OFL_VISION_METHOD

#define OFL_VISION_METHOD   1

Definition at line 138 of file optical_flow_landing.c.

◆ RECURSIVE_LEARNING

#define RECURSIVE_LEARNING   0

Definition at line 256 of file optical_flow_landing.c.

◆ TEXTON_DISTRIBUTION_PATH

#define TEXTON_DISTRIBUTION_PATH   /data/ftp/internal_000

Definition at line 263 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 998 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:

◆ 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)

Definition at line 1171 of file optical_flow_landing.c.

◆ guidance_h_module_init()

void guidance_h_module_init ( void  )

Definition at line 1166 of file optical_flow_landing.c.

◆ guidance_h_module_read_rc()

void guidance_h_module_read_rc ( void  )

Definition at line 1181 of file optical_flow_landing.c.

◆ guidance_h_module_run()

void guidance_h_module_run ( bool UNUSED  in_flight)

Definition at line 1176 of file optical_flow_landing.c.

◆ 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 1194 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 1186 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 1203 of file optical_flow_landing.c.

References vertical_ctrl_module_run().

+ Here is the call graph for this function:

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

◆ 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 1051 of file optical_flow_landing.c.

References D, OpticalFlowLanding::d_err, delta_setpoint, OpticalFlowLanding::divergence, divergence_vision_dt, get_sys_time_float(), I, MAX_PPRZ, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, P, previous_divergence_setpoint, ramp, OpticalFlowLanding::ramp_duration, ramp_start_time, set_cov_div(), start_setpoint_ramp, 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:

◆ 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,
UNUSED float *  params 
)

Definition at line 1363 of file optical_flow_landing.c.

References float_mat_div_scalar(), float_mat_mul_scalar(), lambda, MAKE_MATRIX_PTR, MAT_MUL, MAT_SUB, P, P_RLS, Int32Eulers::phi, predict_gain(), target, and weights.

Referenced by recursive_least_squares_batch().

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

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

◆ reset_all_vars()

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

◆ send_divergence()

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

◆ 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 1014 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 1109 of file optical_flow_landing.c.

References OpticalFlowLanding::d_err, OpticalFlowLanding::lp_const, lp_factor, 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 1121 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 334 of file optical_flow_landing.c.

References OpticalFlowLanding::active_motion, OpticalFlowLanding::agl, agl_ev, OpticalFlowLanding::agl_lp, OpticalFlowLanding::close_to_edge, OpticalFlowLanding::CONTROL_METHOD, OpticalFlowLanding::cov_limit, OpticalFlowLanding::COV_METHOD, OpticalFlowLanding::cov_set_point, OpticalFlowLanding::d_err, DefaultPeriodic, OpticalFlowLanding::delay_steps, delta_setpoint, OpticalFlowLanding::dgain, OpticalFlowLanding::dgain_adaptive, OpticalFlowLanding::divergence, divergence_front, OpticalFlowLanding::divergence_setpoint, OpticalFlowLanding::elc_oscillate, OpticalFlowLanding::front_div_threshold, get_sys_time_float(), guidance_v, if(), OpticalFlowLanding::igain, OpticalFlowLanding::igain_adaptive, OpticalFlowLanding::igain_horizontal_factor, OpticalFlowLanding::lp_const, OpticalFlowLanding::lp_cov_div_factor, lp_divergence_front, OpticalFlowLanding::lp_factor_prediction, lp_flow_x, lp_flow_y, MAX_PPRZ, module_enter_time, n_textons, VerticalGuidance::nominal_throttle, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, OFL_ACTIVE_MOTION, OFL_AGL_ID, OFL_CLOSE_TO_EDGE, OFL_CONTROL_METHOD, OFL_COV_LANDING_LIMIT, OFL_COV_METHOD, OFL_COV_SETPOINT, OFL_COV_WINDOW_SIZE, OFL_DGAIN, OFL_DGAIN_ADAPTIVE, OFL_ELC_OSCILLATE, OFL_FRONT_DIV_THRESHOLD, OFL_IGAIN, OFL_IGAIN_ADAPTIVE, OFL_IGAIN_HORIZONTAL_FACTOR, OFL_LP_CONST, OFL_OMEGA_FB, OFL_OMEGA_LR, OFL_OPTICAL_FLOW_ID, OFL_P_LAND_THRESHOLD, OFL_PGAIN, OFL_PGAIN_ADAPTIVE, OFL_PGAIN_HORIZONTAL_FACTOR, OFL_PITCH_TRIM, OFL_ROLL_TRIM, OFL_VISION_METHOD, old_flow_time, OpticalFlowLanding::omega_FB, OpticalFlowLanding::omega_LR, optical_flow_ev, OpticalFlowLanding::p_land_threshold, P_RLS, OpticalFlowLanding::pgain, OpticalFlowLanding::pgain_adaptive, OpticalFlowLanding::pgain_horizontal_factor, OpticalFlowLanding::pitch_trim, OpticalFlowLanding::previous_err, ramp, OpticalFlowLanding::ramp_duration, ramp_start_time, OpticalFlowLanding::reduction_factor_elc, register_periodic_telemetry(), reset_all_vars(), OpticalFlowLanding::roll_trim, send_divergence(), start_setpoint_ramp, OpticalFlowLanding::sum_err, OpticalFlowLanding::t_transition, OpticalFlowLanding::use_bias, OpticalFlowLanding::vel, vertical_ctrl_agl_cb(), vertical_ctrl_optical_flow_cb(), OpticalFlowLanding::VISION_METHOD, weights, 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)

Run the optical flow landing module.

Definition at line 510 of file optical_flow_landing.c.

References OpticalFlowLanding::active_motion, ACTIVE_RATES, OpticalFlowLanding::agl, OpticalFlowLanding::agl_lp, ANGLE_BFP_OF_REAL, attitude, OpticalFlowLanding::close_to_edge, OpticalFlowLanding::CONTROL_METHOD, count_covdiv, cov_div, OpticalFlowLanding::cov_limit, OpticalFlowLanding::COV_METHOD, OpticalFlowLanding::cov_set_point, OpticalFlowLanding::dgain, OpticalFlowLanding::divergence, divergence_front, OpticalFlowLanding::divergence_setpoint, divergence_vision, divergence_vision_dt, dstate, elc_d_gain_start, elc_i_gain_start, OpticalFlowLanding::elc_oscillate, elc_p_gain_start, elc_phase, elc_time_start, final_landing_procedure(), OpticalFlowLanding::front_div_threshold, get_sys_time_float(), HORIZONTAL_RATE_CONTROL, if(), OpticalFlowLanding::igain, OpticalFlowLanding::igain_adaptive, OpticalFlowLanding::igain_horizontal_factor, INCREASE_GAIN_PER_SECOND, istate, landing, learn_from_file(), OpticalFlowLanding::learn_gains, load_weights(), OpticalFlowLanding::load_weights, OpticalFlowLanding::lp_const, lp_cov_div, OpticalFlowLanding::lp_cov_div_factor, lp_divergence_front, lp_factor, OpticalFlowLanding::lp_factor_prediction, lp_flow_x, lp_flow_y, MAX_PPRZ, MINIMUM_GAIN, module_active_time_sec, module_enter_time, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, OpticalFlowLanding::omega_FB, OpticalFlowLanding::omega_LR, optical_flow_x, optical_flow_y, OpticalFlowLanding::p_land_threshold, OpticalFlowLanding::pgain, OpticalFlowLanding::pgain_adaptive, OpticalFlowLanding::pgain_horizontal_factor, Int32Eulers::phi, PID_divergence_control(), OpticalFlowLanding::pitch_trim, predict_gain(), prev_vision_time, previous_divergence_setpoint, pstate, pused, OpticalFlowLanding::reduction_factor_elc, OpticalFlowLanding::roll_trim, save_texton_distribution(), stabilization_attitude_run(), stabilization_cmd, stabilization_indi_set_rpy_setpoint_i(), stateGetNedToBodyEulers_f(), stateGetPositionEnu_f(), stateGetSpeedEnu_f(), OpticalFlowLanding::sum_err, sum_pitch_error, sum_roll_error, OpticalFlowLanding::t_transition, texton_distribution, thrust_set, OpticalFlowLanding::vel, OpticalFlowLanding::VISION_METHOD, vision_time, and EnuCoor_f::z.

Referenced by guidance_v_module_run().

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

◆ vertical_ctrl_optical_flow_cb()

void vertical_ctrl_optical_flow_cb ( uint8_t  sender_id,
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 1126 of file optical_flow_landing.c.

References divergence_front, divergence_vision, FLOW_OPTICFLOW_ID, get_sys_time_float(), old_flow_time, optical_flow_x, optical_flow_y, and vision_time.

Referenced by vertical_ctrl_module_init().

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

Variable Documentation

◆ agl_ev

abi_event agl_ev
static

The altitude ABI event.

Definition at line 292 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

◆ count_covdiv

int32_t count_covdiv

Definition at line 289 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 326 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and set_cov_div().

◆ cov_div

◆ cov_divs_log

float cov_divs_log[MAX_SAMPLES_LEARNING]

Definition at line 70 of file optical_flow_landing.c.

Referenced by load_texton_distribution().

◆ delta_setpoint

float delta_setpoint

◆ distribution_logger

FILE* distribution_logger = NULL
static

Definition at line 268 of file optical_flow_landing.c.

Referenced by load_texton_distribution(), and save_texton_distribution().

◆ divergence_front

float divergence_front

◆ divergence_history

float divergence_history[OFL_COV_WINDOW_SIZE]

Definition at line 323 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and set_cov_div().

◆ divergence_setpoint

float divergence_setpoint

Definition at line 249 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 244 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 288 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

◆ elc_i_gain_start

float elc_i_gain_start

Definition at line 288 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

◆ elc_p_gain_start

float elc_p_gain_start

Definition at line 288 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

◆ elc_phase

int32_t elc_phase

Definition at line 286 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 287 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ gains

◆ ind_hist

uint32_t ind_hist

Definition at line 325 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and set_cov_div().

◆ istate

float istate

Definition at line 243 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ lambda

◆ landing

bool landing

◆ lp_cov_div

float lp_cov_div

Definition at line 290 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ lp_divergence_front

float lp_divergence_front

◆ lp_flow_x

float lp_flow_x

◆ lp_flow_y

float lp_flow_y

◆ module_active_time_sec

float module_active_time_sec

Definition at line 275 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

◆ module_enter_time

float module_enter_time

Definition at line 276 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init(), and vertical_ctrl_module_run().

◆ n_read_samples

unsigned int n_read_samples

Definition at line 270 of file optical_flow_landing.c.

Referenced by learn_from_file(), and load_texton_distribution().

◆ normalized_thrust

float normalized_thrust

Definition at line 239 of file optical_flow_landing.c.

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

◆ of_landing_ctrl

◆ old_flow_time

float old_flow_time

◆ optical_flow_ev

abi_event optical_flow_ev
static

Definition at line 293 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

◆ optical_flow_x

float optical_flow_x

◆ optical_flow_y

float optical_flow_y

◆ P_RLS

◆ past_divergence_history

float past_divergence_history[OFL_COV_WINDOW_SIZE]

Definition at line 324 of file optical_flow_landing.c.

Referenced by set_cov_div().

◆ prev_vision_time

float prev_vision_time

Definition at line 245 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 247 of file optical_flow_landing.c.

Referenced by reset_all_vars().

◆ previous_divergence_setpoint

float previous_divergence_setpoint

◆ pstate

◆ pused

float pused

◆ ramp

int ramp

◆ ramp_start_time

float ramp_start_time

◆ sonar_OF

float sonar_OF[MAX_SAMPLES_LEARNING]

Definition at line 68 of file optical_flow_landing.c.

Referenced by load_texton_distribution().

◆ start_setpoint_ramp

float start_setpoint_ramp

◆ sum_pitch_error

float sum_pitch_error

Definition at line 235 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ sum_roll_error

float sum_roll_error

Definition at line 234 of file optical_flow_landing.c.

Referenced by reset_all_vars(), and vertical_ctrl_module_run().

◆ text_dists

float* text_dists[MAX_SAMPLES_LEARNING]

Definition at line 67 of file optical_flow_landing.c.

Referenced by learn_from_file(), and load_texton_distribution().

◆ texton_distribution_stereoboard

float texton_distribution_stereoboard[n_ts]

Definition at line 261 of file optical_flow_landing.c.

◆ thrust_history

float thrust_history[OFL_COV_WINDOW_SIZE]

Definition at line 322 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

◆ weights

◆ weights_file

FILE* weights_file = NULL
static

Definition at line 269 of file optical_flow_landing.c.

Referenced by load_weights(), and save_weights().