Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
optical_flow_hover.c File Reference
+ Include dependency graph for optical_flow_hover.c:

Go to the source code of this file.

Macros

#define OFH_OPTICAL_FLOW_ID   ABI_BROADCAST
 
#define OFH_HOVER_METHOD   1
 
#define XY_SYMMETRICAL   0
 
#define OFH_COV_METHOD   0
 
#define COV_WINDOW_SIZE   (10*30)
 
#define OF_COV_DELAY_STEPS   COV_WINDOW_SIZE/2
 
#define OF_LP_CONST   0.5
 
#define OFH_OSCPHI   1
 
#define OFH_OSCTHETA   1
 
#define OFH_PGAINZ   0.4
 
#define OFH_IGAINZ   0.f
 
#define OFH_DGAINZ   0.0
 
#define OFH_RAMPZ   0.15
 
#define OFH_REDUCTIONZ   0.45
 
#define OFH_COVDIV_SETPOINT   -0.02
 
#define OFH_PGAINX   0.f
 
#define OFH_IGAINX   0.00002
 
#define OFH_DGAINX   0.f
 
#define OFH_PGAINY   0.f
 
#define OFH_IGAINY   0.00002
 
#define OFH_DGAINY   0.f
 
#define OFH_RAMPXY   0.0008
 
#define OFH_REDUCTIONXY   0.3
 
#define OFH_COVFLOW_SETPOINT   -500.f
 
#define OFH_VER_SLOPE_A   0.5
 
#define OFH_VER_SLOPE_B   0.25
 
#define OFH_HOR_X_SLOPE_A   2.f
 
#define OFH_HOR_X_SLOPE_B   0.5
 
#define OFH_HOR_Y_SLOPE_A   OFH_HOR_X_SLOPE_A
 
#define OFH_HOR_Y_SLOPE_B   OFH_HOR_X_SLOPE_B
 
#define GH_GAIN_SCALE   2
 
#define MAX_POS_ERR   POS_BFP_OF_REAL(16.)
 
#define MAX_SPEED_ERR   SPEED_BFP_OF_REAL(16.)
 

Functions

void ofh_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_div)
 Function definitions. More...
 
static void reset_horizontal_vars (void)
 Reset all horizontal variables: More...
 
static void reset_vertical_vars (void)
 Reset all vertical variables: More...
 
void vertical_ctrl_module_init (void)
 Initialize the vertical optical flow hover module. More...
 
void vertical_ctrl_module_run (bool in_flight)
 Run the vertical optical flow hover module. More...
 
void horizontal_ctrl_module_init (void)
 Initialize the horizontal optical flow hover module. More...
 
void horizontal_ctrl_module_run (bool in_flight)
 Run the horizontal optical flow hover module. More...
 
void computeOptiTrack (bool phi, bool theta, struct Int32Eulers *opti_sp_eu)
 Get the desired Euler angles for optitrack stabilization. More...
 
static void send_optical_flow_hover (struct transport_tx *trans, struct link_device *dev)
 
void optical_flow_hover_init ()
 
void guidance_h_module_read_rc (void)
 
void guidance_v_module_init (void)
 
void guidance_h_module_init (void)
 
void guidance_v_module_enter (void)
 Entering the vertical module (user switched to module) More...
 
void guidance_h_module_enter (void)
 Entering the horizontal module (user switched to module) More...
 
void guidance_v_module_run (bool in_flight)
 
void guidance_h_module_run (bool in_flight)
 

Variables

struct DesiredInputs des_inputs
 
struct FloatVect3 covariances
 
float vision_time
 
float prev_vision_timeXY
 
float prev_vision_timeZ
 
bool oscillatingX
 
bool oscillatingY
 
int16_t flowX
 
int16_t flowY
 
struct OFhistory historyX
 
struct OFhistory historyY
 
struct Int32Eulers ofh_sp_eu
 
bool oscillatingZ
 
float divergence_vision
 
struct OFhistory historyZ
 
static abi_event optical_flow_ev
 
struct OpticalFlowHoverControl of_hover_ctrl_X
 
struct OpticalFlowHoverControl of_hover_ctrl_Y
 
struct OpticalFlowHoverControl of_hover_ctrl_Z
 
bool oscphi
 
bool osctheta
 
bool derotated
 
bool cov_method
 method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past / flow and past flow(1) More...
 
uint8_t hover_method
 Method used to hover 0 = All axis after each other; 1 = all axis at the same time; 2 = vertical only, use relation to set horizontal. More...
 
struct Int32Vect2 of_hover_pos_err
 
struct Int32Vect2 of_hover_speed_err
 
struct Int32Vect2 of_hover_ref_pos
 
struct Int32Vect2 of_hover_trim_att_integrator
 
struct Int32Vect2 of_hover_cmd_earth
 

Macro Definition Documentation

#define COV_WINDOW_SIZE   (10*30)
#define GH_GAIN_SCALE   2

Definition at line 233 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

#define MAX_POS_ERR   POS_BFP_OF_REAL(16.)

Definition at line 236 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

#define MAX_SPEED_ERR   SPEED_BFP_OF_REAL(16.)

Definition at line 239 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

#define OF_COV_DELAY_STEPS   COV_WINDOW_SIZE/2

Definition at line 68 of file optical_flow_hover.c.

#define OF_LP_CONST   0.5

Definition at line 73 of file optical_flow_hover.c.

Referenced by horizontal_ctrl_module_run(), and vertical_ctrl_module_run().

#define OFH_COV_METHOD   0

Definition at line 58 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_COVDIV_SETPOINT   -0.02

Definition at line 111 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_COVFLOW_SETPOINT   -500.f

Definition at line 151 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_DGAINX   0.f

Definition at line 124 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_DGAINY   0.f

Definition at line 136 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_DGAINZ   0.0

Definition at line 96 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_HOR_X_SLOPE_A   2.f

Definition at line 164 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_HOR_X_SLOPE_B   0.5

Definition at line 168 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_HOR_Y_SLOPE_A   OFH_HOR_X_SLOPE_A

Definition at line 172 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_HOR_Y_SLOPE_B   OFH_HOR_X_SLOPE_B

Definition at line 176 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_HOVER_METHOD   1

Definition at line 48 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_IGAINX   0.00002
#define OFH_IGAINY   0.00002
#define OFH_IGAINZ   0.f

Definition at line 92 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_OPTICAL_FLOW_ID   ABI_BROADCAST

Definition at line 42 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_OSCPHI   1

Definition at line 78 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_OSCTHETA   1

Definition at line 83 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_PGAINX   0.f
#define OFH_PGAINY   0.f
#define OFH_PGAINZ   0.4

Definition at line 88 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init(), and reset_vertical_vars().

#define OFH_RAMPXY   0.0008

Definition at line 141 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_RAMPZ   0.15

Definition at line 101 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_REDUCTIONXY   0.3

Definition at line 146 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init(), and vertical_ctrl_module_run().

#define OFH_REDUCTIONZ   0.45

Definition at line 106 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

#define OFH_VER_SLOPE_A   0.5

Definition at line 156 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define OFH_VER_SLOPE_B   0.25

Definition at line 160 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().

#define XY_SYMMETRICAL   0

Definition at line 53 of file optical_flow_hover.c.

Referenced by horizontal_ctrl_module_run().

Function Documentation

void computeOptiTrack ( bool  phi,
bool  theta,
struct Int32Eulers opti_sp_eu 
)

Get the desired Euler angles for optitrack stabilization.

Parameters
[in]Booleanwhether to Phi or not
[in]Booleanwhether to Theta or not
[out]Thedesired Euler angles

Definition at line 684 of file optical_flow_hover.c.

References BFP_OF_REAL, GH_GAIN_SCALE, GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_TRIG_FRAC, MAX_POS_ERR, MAX_SPEED_ERR, Min, of_hover_cmd_earth, of_hover_pos_err, of_hover_ref_pos, of_hover_speed_err, of_hover_trim_att_integrator, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, stateGetNedToBodyEulers_i(), stateGetPositionNed_i(), stateGetSpeedNed_i(), Int32Eulers::theta, VECT2_DIFF, VECT2_STRIM, Int32Vect2::x, and Int32Vect2::y.

Referenced by horizontal_ctrl_module_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void guidance_h_module_enter ( void  )

Entering the horizontal module (user switched to module)

Definition at line 645 of file optical_flow_hover.c.

References of_hover_ref_pos, ofh_sp_eu, Int32Eulers::psi, reset_horizontal_vars(), stateGetNedToBodyEulers_i(), stateGetPositionNed_i(), and VECT2_COPY.

+ Here is the call graph for this function:

void guidance_h_module_init ( void  )

Definition at line 625 of file optical_flow_hover.c.

References horizontal_ctrl_module_init().

+ Here is the call graph for this function:

void guidance_h_module_read_rc ( void  )

Definition at line 424 of file optical_flow_hover.c.

void guidance_h_module_run ( bool  in_flight)

Definition at line 667 of file optical_flow_hover.c.

References AP_MODE_NAV, autopilot_static_set_mode(), Electrical::bat_low, electrical, horizontal_ctrl_module_run(), and stabilization_attitude_run().

+ Here is the call graph for this function:

void guidance_v_module_enter ( void  )

Entering the vertical module (user switched to module)

Definition at line 633 of file optical_flow_hover.c.

References des_inputs, MAX_PPRZ, OpticalFlowHoverControl::nominal_value, of_hover_ctrl_Z, reset_vertical_vars(), stabilization_cmd, and DesiredInputs::thrust.

+ Here is the call graph for this function:

void guidance_v_module_init ( void  )

Definition at line 619 of file optical_flow_hover.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 656 of file optical_flow_hover.c.

void horizontal_ctrl_module_init ( void  )

Initialize the horizontal optical flow hover module.

Definition at line 313 of file optical_flow_hover.c.

References reset_horizontal_vars().

Referenced by guidance_h_module_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ofh_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_div 
)

Function definitions.

Definition at line 601 of file optical_flow_hover.c.

Referenced by optical_flow_hover_init().

+ Here is the caller graph for this function:

static void reset_vertical_vars ( void  )
static
static void send_optical_flow_hover ( struct transport_tx *  trans,
struct link_device *  dev 
)
static
void vertical_ctrl_module_init ( void  )

Initialize the vertical optical flow hover module.

Initialize the optical flow landing module.

Definition at line 304 of file optical_flow_hover.c.

References reset_vertical_vars().

Referenced by guidance_v_module_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

bool cov_method

method to calculate the covariance: between thrust and div / angle and flow (0) or div and div past / flow and past flow(1)

Definition at line 213 of file optical_flow_hover.c.

bool derotated

Definition at line 212 of file optical_flow_hover.c.

float divergence_vision

Definition at line 197 of file optical_flow_hover.c.

int16_t flowX

Definition at line 188 of file optical_flow_hover.c.

int16_t flowY

Definition at line 189 of file optical_flow_hover.c.

struct OFhistory historyX

Definition at line 190 of file optical_flow_hover.c.

Referenced by horizontal_ctrl_module_run(), and reset_horizontal_vars().

struct OFhistory historyY

Definition at line 191 of file optical_flow_hover.c.

Referenced by horizontal_ctrl_module_run(), and reset_horizontal_vars().

struct OFhistory historyZ

Definition at line 199 of file optical_flow_hover.c.

Referenced by reset_vertical_vars(), and vertical_ctrl_module_run().

uint8_t hover_method

Method used to hover 0 = All axis after each other; 1 = all axis at the same time; 2 = vertical only, use relation to set horizontal.

Definition at line 214 of file optical_flow_hover.c.

struct Int32Vect2 of_hover_cmd_earth

Definition at line 245 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

struct Int32Vect2 of_hover_pos_err

Definition at line 241 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

struct Int32Vect2 of_hover_ref_pos

Definition at line 243 of file optical_flow_hover.c.

Referenced by computeOptiTrack(), and guidance_h_module_enter().

struct Int32Vect2 of_hover_speed_err

Definition at line 242 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

struct Int32Vect2 of_hover_trim_att_integrator

Definition at line 244 of file optical_flow_hover.c.

Referenced by computeOptiTrack().

abi_event optical_flow_ev
static

Definition at line 202 of file optical_flow_hover.c.

bool oscillatingX

Definition at line 186 of file optical_flow_hover.c.

bool oscillatingY

Definition at line 187 of file optical_flow_hover.c.

bool oscillatingZ

Definition at line 196 of file optical_flow_hover.c.

bool oscphi

Definition at line 210 of file optical_flow_hover.c.

bool osctheta

Definition at line 211 of file optical_flow_hover.c.

float prev_vision_timeXY

Definition at line 184 of file optical_flow_hover.c.

Referenced by horizontal_ctrl_module_run().

float prev_vision_timeZ

Definition at line 184 of file optical_flow_hover.c.

Referenced by vertical_ctrl_module_run().