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

#include "optical_flow_landing.h"
#include "generated/airframe.h"
#include "paparazzi.h"
#include "subsystems/abi.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#include "autopilot.h"
#include "subsystems/navigation/common_flight_plan.h"
#include "subsystems/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
#include "math/pprz_stat.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_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, 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_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 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 reset_all_vars ( void )

static

Reset all variables:

Definition at line 243 of file optical_flow_landing.c.

References OpticalFlowLanding::agl, OpticalFlowLanding::agl_lp, OpticalFlowLanding::d_err, OpticalFlowLanding::dgain, OpticalFlowLanding::divergence, get_sys_time_float(), OpticalFlowLanding::igain, MAX_PPRZ, OpticalFlowLanding::nominal_thrust, of_landing_ctrl, OFL_COV_WINDOW_SIZE, OpticalFlowLanding::pgain, OpticalFlowLanding::previous_err, pstate, stateGetPositionEnu_f(), OpticalFlowLanding::sum_err, vision_time, and EnuCoor_f::z.

Referenced by guidance_v_module_enter(), and vertical_ctrl_module_init().

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] thrust the 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]	err	the error of the divergence and divergence setpoint
[in]	dt	time 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,
		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.

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_module_run ( bool in_flight )

Run the optical flow landing module.

Definition at line 288 of file optical_flow_landing.c.

Referenced by guidance_v_module_run().

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
	)