Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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]dttime difference since last update
[in]*of_hover_ctrlOpticalFlowHoverControl 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]desiredangle
[in]dttime difference since last update
[in]*of_hover_ctrlOpticalFlowHoverControl 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]thrustthe 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