optical_flow_functions.c File Reference

#include "optical_flow_functions.h"
#include "paparazzi.h"
#include "math/pprz_stat.h"
#include "std.h"

Include dependency graph for optical_flow_functions.c:

Go to the source code of this file.

Macros
#define	OFH_MAXBANK   10.f
#define	OF_LP_CONST   0.5
#define	OF_COV_DELAY_STEPS   COV_WINDOW_SIZE/2

Functions
float	set_cov_div (bool cov_method, struct OFhistory *history, struct DesiredInputs *inputs)
	Set the covariance of the divergence and the thrust / past divergence This funciton should only be called once per time step. More...
void	set_cov_flow (bool cov_method, struct OFhistory *historyX, struct OFhistory *historyY, struct DesiredInputs *inputs, struct FloatVect3 *covs)
	Set the covariance of the flow and past flow / desired angle This funciton should only be called once per time step. More...
float	PID_flow_control (float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
	Determine and set the desired angle for constant flow control. More...
int32_t	PID_divergence_control (float dt, struct OpticalFlowHoverControl *of_hover_ctrl)
	Determine and set the thrust for constant divergence control. More...

Variables
uint32_t	ind_histXY
uint8_t	cov_array_filledXY
uint32_t	ind_histZ
uint8_t	cov_array_filledZ
struct OpticalFlowHover	of_hover

Macro Definition Documentation

OF_COV_DELAY_STEPS

#define OF_COV_DELAY_STEPS   COV_WINDOW_SIZE/2

Definition at line 38 of file optical_flow_functions.c.

OF_LP_CONST

#define OF_LP_CONST   0.5

Definition at line 33 of file optical_flow_functions.c.

OFH_MAXBANK

#define OFH_MAXBANK   10.f

Definition at line 28 of file optical_flow_functions.c.

Function Documentation

PID_divergence_control()

int32_t PID_divergence_control	(	float	dt,
		struct OpticalFlowHoverControl *	of_hover_ctrl
	)

Determine and set the thrust for constant divergence control.

Parameters

[out]	thrust
[in]	dt	time difference since last update
[in]	*of_hover_ctrl	OpticalFlowHoverControl structure

Definition at line 161 of file optical_flow_functions.c.

References GainsPID::D, GainsPID::d_err, GainsPID::err, GainsPID::I, lp_factor, MAX_PPRZ, OpticalFlowHoverControl::nominal_value, OF_LP_CONST, GainsPID::P, OpticalFlowHoverControl::PID, GainsPID::previous_err, and GainsPID::sum_err.

Referenced by vertical_ctrl_module_run().

Here is the caller graph for this function:

PID_flow_control()

float PID_flow_control	(	float	dt,
		struct OpticalFlowHoverControl *	of_hover_ctrl
	)

Determine and set the desired angle for constant flow control.

Parameters

[out]	desired	angle
[in]	dt	time difference since last update
[in]	*of_hover_ctrl	OpticalFlowHoverControl structure

Definition at line 133 of file optical_flow_functions.c.

References GainsPID::D, GainsPID::d_err, GainsPID::err, GainsPID::I, lp_factor, OF_LP_CONST, OFH_MAXBANK, GainsPID::P, OpticalFlowHoverControl::PID, GainsPID::previous_err, and GainsPID::sum_err.

Referenced by horizontal_ctrl_module_run().

Here is the caller graph for this function:

set_cov_div()

float set_cov_div	(	bool	cov_method,
		struct OFhistory *	history,
		struct DesiredInputs *	inputs
	)

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 53 of file optical_flow_functions.c.

References cov_array_filledZ, cov_div, cov_method, COV_WINDOW_SIZE, covariance_f(), OpticalFlowHover::divergence, ind_histZ, OFhistory::input, MAX_PPRZ, normalized_thrust, OFhistory::OF, OF_COV_DELAY_STEPS, of_hover, OFhistory::past_OF, and DesiredInputs::thrust.

Referenced by vertical_ctrl_module_run().

Here is the call graph for this function:

Here is the caller graph for this function:

set_cov_flow()

void set_cov_flow	(	bool	cov_method,
		struct OFhistory *	historyX,
		struct OFhistory *	historyY,
		struct DesiredInputs *	inputs,
		struct FloatVect3 *	covs
	)

Set the covariance of the flow and past flow / desired angle This funciton should only be called once per time step.

Definition at line 90 of file optical_flow_functions.c.

References cov_array_filledXY, cov_method, COV_WINDOW_SIZE, covariance_f(), OpticalFlowHover::flowX, OpticalFlowHover::flowY, historyX, historyY, ind_histXY, OFhistory::input, OFhistory::OF, OF_COV_DELAY_STEPS, of_hover, OFH_MAXBANK, OFhistory::past_OF, DesiredInputs::phi, DesiredInputs::theta, FloatVect3::x, and FloatVect3::y.

Referenced by horizontal_ctrl_module_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

cov_array_filledXY

uint8_t cov_array_filledXY

Definition at line 42 of file optical_flow_functions.c.

Referenced by reset_horizontal_vars(), and set_cov_flow().

cov_array_filledZ

uint8_t cov_array_filledZ

Definition at line 44 of file optical_flow_functions.c.

Referenced by reset_vertical_vars(), and set_cov_div().

ind_histXY

uint32_t ind_histXY

Definition at line 41 of file optical_flow_functions.c.

Referenced by reset_horizontal_vars(), and set_cov_flow().

ind_histZ

uint32_t ind_histZ

Definition at line 43 of file optical_flow_functions.c.

Referenced by reset_vertical_vars(), and set_cov_div().

of_hover

struct OpticalFlowHover of_hover

Definition at line 44 of file optical_flow_functions.c.

Referenced by horizontal_ctrl_module_run(), reset_horizontal_vars(), reset_vertical_vars(), send_optical_flow_hover(), set_cov_div(), set_cov_flow(), and vertical_ctrl_module_run().