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

#include "firmwares/rotorcraft/autopilot.h"
#include "subsystems/navigation/common_flight_plan.h"
#include "subsystems/datalink/telemetry.h"
#include <time.h>
#include "modules/ctrl/optical_flow_landing.h"
#include "generated/airframe.h"
#include "paparazzi.h"
#include "subsystems/abi.h"
#include "firmwares/rotorcraft/stabilization.h"

Include dependency graph for optical_flow_landing.c:

Go to the source code of this file.

Macros
#define	MINIMUM_GAIN 0.1

#define	OPTICAL_FLOW_LANDING_AGL_ID ABI_BROADCAST

#define	OPTICAL_FLOW_LANDING_OPTICAL_FLOW_ID ABI_BROADCAST

#define	OPTICAL_FLOW_LANDING_PGAIN 1.0

#define	OPTICAL_FLOW_LANDING_IGAIN 0.0

#define	OPTICAL_FLOW_LANDING_DGAIN 0.0

#define	OPTICAL_FLOW_LANDING_VISION_METHOD 1

#define	OPTICAL_FLOW_LANDING_CONTROL_METHOD 0

#define	OPTICAL_FLOW_LANDING_COV_METHOD 0

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

static void	vertical_ctrl_agl_cb (uint8_t sender_id, float distance)
	Callback function of the ground altitude. More...

static 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, uint8_t quality, float size_divergence, float dist)

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	reset_all_vars ()
	Reset all variables: More...

float	get_mean_array (float *a, int n_elements)
	Get the mean value of an array. More...

float	get_cov (float a, float b, int n_elements)
	Get the covariance of two arrays. 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

float	divergence_vision

float	divergence_vision_dt

float	normalized_thrust

float	cov_div

float	pstate

float	pused

float	dt

int	vision_message_nr

int	previous_message_nr

int	landing

float	previous_err

float	previous_cov_err

long	previous_time

static abi_event	agl_ev
	The altitude ABI event. More...

static abi_event	optical_flow_ev

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

Definition in file optical_flow_landing.c.

Macro Definition Documentation

#define MINIMUM_GAIN 0.1

Definition at line 53 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_run().

#define OPTICAL_FLOW_LANDING_AGL_ID ABI_BROADCAST

Definition at line 80 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OPTICAL_FLOW_LANDING_CONTROL_METHOD 0

Definition at line 108 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OPTICAL_FLOW_LANDING_COV_METHOD 0

Definition at line 112 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OPTICAL_FLOW_LANDING_DGAIN 0.0

Definition at line 100 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OPTICAL_FLOW_LANDING_IGAIN 0.0

Definition at line 96 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OPTICAL_FLOW_LANDING_OPTICAL_FLOW_ID ABI_BROADCAST

Definition at line 86 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OPTICAL_FLOW_LANDING_PGAIN 1.0

Definition at line 92 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

#define OPTICAL_FLOW_LANDING_VISION_METHOD 1

Definition at line 104 of file optical_flow_landing.c.

Referenced by vertical_ctrl_module_init().

Function Documentation

float get_cov	(	float *	a,
		float *	b,
		int	n_elements
	)

Get the covariance of two arrays.

Parameters

[out]	cov	The covariance
[in]	*a	The first array
[in]	*b	The second array
[in]	n	Number of elements in the arrays

Definition at line 459 of file optical_flow_landing.c.

References get_mean_array().

Referenced by vertical_ctrl_module_run().

Here is the call graph for this function:

Here is the caller graph for this function:

float get_mean_array	(	float *	a,
		int	n_elements
	)

Get the mean value of an array.

Parameters

[out]	mean	The mean value
[in]	*a	The array
[in]	n	Number of elements in the array

Definition at line 440 of file optical_flow_landing.c.

Referenced by get_cov().

Here is the caller graph for this function:

void guidance_v_module_enter ( void )

Entering the module (user switched to module)

Definition at line 501 of file optical_flow_landing.c.

References OpticalFlowLanding::agl_lp, cov_div, OpticalFlowLanding::cov_set_point, COV_WINDOW_SIZE, divergence, divergence_history, OpticalFlowLanding::divergence_setpoint, dt, ind_hist, landing, normalized_thrust, of_landing_ctrl, previous_cov_err, previous_err, previous_message_nr, previous_time, OpticalFlowLanding::sum_err, thrust_history, and vision_message_nr.

void guidance_v_module_init ( void )

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

void reset_all_vars ( void )

Reset all variables:

Definition at line 194 of file optical_flow_landing.c.

References OpticalFlowLanding::agl_lp, cov_div, OpticalFlowLanding::cov_set_point, COV_WINDOW_SIZE, divergence, divergence_history, OpticalFlowLanding::divergence_setpoint, dt, ind_hist, landing, normalized_thrust, of_landing_ctrl, previous_cov_err, previous_err, previous_message_nr, previous_time, stabilization_cmd, OpticalFlowLanding::sum_err, thrust_history, and vision_message_nr.

Referenced by vertical_ctrl_module_run().

Here is the caller graph for this function:

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

static

Definition at line 65 of file optical_flow_landing.c.

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

Referenced by vertical_ctrl_module_init().

Here is the caller graph for this function:

static void vertical_ctrl_agl_cb	(	uint8_t	sender_id,
		float	distance
	)

static

Callback function of the ground altitude.

Definition at line 479 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 131 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 224 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:

static 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,
		uint8_t	quality,
		float	size_divergence,
		float	dist
	)