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

#include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "optical_flow_landing.h"
#include "generated/airframe.h"
#include "paparazzi.h"
#include "modules/core/abi.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#include "autopilot.h"
#include "modules/nav/common_flight_plan.h"
#include "modules/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
#include "math/pprz_stat.h"
#include <stdio.h>
#include "modules/computer_vision/textons.h"
#include "math/pprz_matrix_decomp_float.h"
#include "math/pprz_simple_matrix.h"

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_run (bool in_flight)
	Run the optical flow landing module. More...

void	optical_flow_landing_init (void)
	Initialize the optical flow landing module. More...

void	guidance_module_enter (void)
	Entering the module (user switched to module) More...

void	guidance_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 214 of file optical_flow_landing.c.

◆ HORIZONTAL_RATE_CONTROL

#define HORIZONTAL_RATE_CONTROL 0

Definition at line 210 of file optical_flow_landing.c.

◆ INCREASE_GAIN_PER_SECOND

#define INCREASE_GAIN_PER_SECOND 0.10

Definition at line 222 of file optical_flow_landing.c.

◆ MINIMUM_GAIN

#define MINIMUM_GAIN 0.1

Definition at line 219 of file optical_flow_landing.c.

◆ n_ts

#define n_ts 10

Definition at line 261 of file optical_flow_landing.c.

◆ OFL_ACTIVE_MOTION

#define OFL_ACTIVE_MOTION 0

Definition at line 201 of file optical_flow_landing.c.

◆ OFL_AGL_ID

#define OFL_AGL_ID ABI_BROADCAST

Definition at line 99 of file optical_flow_landing.c.

◆ OFL_CLOSE_TO_EDGE

#define OFL_CLOSE_TO_EDGE 0.025

Definition at line 176 of file optical_flow_landing.c.

◆ OFL_CONTROL_METHOD

#define OFL_CONTROL_METHOD 0

Definition at line 143 of file optical_flow_landing.c.

◆ OFL_COV_LANDING_LIMIT

#define OFL_COV_LANDING_LIMIT 2.2

Definition at line 156 of file optical_flow_landing.c.

◆ OFL_COV_METHOD

#define OFL_COV_METHOD 0

Definition at line 147 of file optical_flow_landing.c.

◆ OFL_COV_SETPOINT

#define OFL_COV_SETPOINT -0.0075

Definition at line 160 of file optical_flow_landing.c.

◆ OFL_COV_WINDOW_SIZE

#define OFL_COV_WINDOW_SIZE 30

Definition at line 152 of file optical_flow_landing.c.

◆ OFL_DGAIN

#define OFL_DGAIN 0.0

Definition at line 119 of file optical_flow_landing.c.

◆ OFL_DGAIN_ADAPTIVE

#define OFL_DGAIN_ADAPTIVE 0.50

Definition at line 189 of file optical_flow_landing.c.

◆ OFL_ELC_OSCILLATE

#define OFL_ELC_OSCILLATE true

Definition at line 172 of file optical_flow_landing.c.

◆ OFL_FRONT_DIV_THRESHOLD

#define OFL_FRONT_DIV_THRESHOLD 0.3

Definition at line 205 of file optical_flow_landing.c.

◆ OFL_IGAIN

#define OFL_IGAIN 0.01

Definition at line 115 of file optical_flow_landing.c.

◆ OFL_IGAIN_ADAPTIVE

#define OFL_IGAIN_ADAPTIVE 0.50

Definition at line 185 of file optical_flow_landing.c.

◆ OFL_IGAIN_HORIZONTAL_FACTOR

#define OFL_IGAIN_HORIZONTAL_FACTOR 0.1

Definition at line 127 of file optical_flow_landing.c.

◆ OFL_LP_CONST

#define OFL_LP_CONST 0.02

Definition at line 164 of file optical_flow_landing.c.

◆ OFL_OMEGA_FB

#define OFL_OMEGA_FB 0.0

Definition at line 197 of file optical_flow_landing.c.

◆ OFL_OMEGA_LR

#define OFL_OMEGA_LR 0.0

Definition at line 193 of file optical_flow_landing.c.

◆ OFL_OPTICAL_FLOW_ID

#define OFL_OPTICAL_FLOW_ID ABI_BROADCAST

Definition at line 105 of file optical_flow_landing.c.

◆ OFL_P_LAND_THRESHOLD

#define OFL_P_LAND_THRESHOLD 0.15

Definition at line 168 of file optical_flow_landing.c.

◆ OFL_PGAIN

#define OFL_PGAIN 0.40

Definition at line 111 of file optical_flow_landing.c.

◆ OFL_PGAIN_ADAPTIVE

#define OFL_PGAIN_ADAPTIVE 0.50

Definition at line 181 of file optical_flow_landing.c.

◆ OFL_PGAIN_HORIZONTAL_FACTOR

#define OFL_PGAIN_HORIZONTAL_FACTOR 0.05

Definition at line 123 of file optical_flow_landing.c.

◆ OFL_PITCH_TRIM

#define OFL_PITCH_TRIM 0.0f

Definition at line 135 of file optical_flow_landing.c.

◆ OFL_ROLL_TRIM

#define OFL_ROLL_TRIM 0.0f

Definition at line 131 of file optical_flow_landing.c.

◆ OFL_VISION_METHOD

#define OFL_VISION_METHOD 1

Definition at line 139 of file optical_flow_landing.c.

◆ RECURSIVE_LEARNING

#define RECURSIVE_LEARNING 0

Definition at line 257 of file optical_flow_landing.c.

◆ TEXTON_DISTRIBUTION_PATH

#define TEXTON_DISTRIBUTION_PATH /data/ftp/internal_000

Definition at line 264 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]	targets	The target values
[in]	samples	The samples / feature vectors
[in]	D	The dimensionality of the samples
[in]	count	The number of samples
[out]	parameters*	Parameters of the linear fit
[out]	fit_error*	Total error of the fit

Definition at line 1415 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_module_enter()

void guidance_module_enter ( void )

Entering the module (user switched to module)

Definition at line 1169 of file optical_flow_landing.c.

References Stabilization::cmd, MAX_PPRZ, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, reset_all_vars(), stabilization, and thrust_set.

Here is the call graph for this function:

◆ guidance_module_run()

void guidance_module_run ( bool in_flight )

Definition at line 1178 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 1251 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 1213 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 1521 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:

◆ optical_flow_landing_init()

void optical_flow_landing_init ( void )

Initialize the optical flow landing module.

Definition at line 334 of file optical_flow_landing.c.

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_point	The desired divergence
[in]	P	P-gain
[in]	I	I-gain
[in]	D	D-gain
[in]	dt	time 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(), 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 1486 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 1340 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, 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]	targets	The target values
[in]	samples	The samples / feature vectors
[in]	D	The dimensionality of the samples
[in]	count	The 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 1290 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()

static void reset_all_vars ( void )

static

Reset all variables:

Definition at line 448 of file optical_flow_landing.c.

Referenced by guidance_module_enter(), and optical_flow_landing_init().

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 1186 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 1503 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

Definition at line 300 of file optical_flow_landing.c.

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

Referenced by optical_flow_landing_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] thrust the 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]	err	the error of the divergence and divergence setpoint
[in]	dt	time 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 optical_flow_landing_init().

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.

Referenced by guidance_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
	)