Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_indi.c File Reference

A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to IROS2016 to learn more! More...

+ Include dependency graph for guidance_indi.c:

Go to the source code of this file.

Functions

void guidance_indi_enter (void)
 Call upon entering indi guidance. More...
 
void guidance_indi_run (bool in_flight, int32_t heading)
 
void guidance_indi_filter_accel (void)
 low pass the accelerometer measurements with a second order filter to remove noise from vibrations More...
 
void guidance_indi_filter_attitude (void)
 Filter the attitude, such that it corresponds to the filtered measured acceleration. More...
 
void guidance_indi_calcG (struct FloatMat33 *Gmat)
 
void stabilization_attitude_set_setpoint_rp_quat_f (struct FloatEulers *indi_rp_cmd, bool in_flight, int32_t heading)
 

Variables

float guidance_indi_pos_gain = 0.5
 
float guidance_indi_speed_gain = 1.8
 
struct FloatVect3 sp_accel = {0.0,0.0,0.0}
 
struct FloatVect3 filt_accel_ned
 
struct FloatVect3 filt_accel_ned_d
 
struct FloatVect3 filt_accel_ned_dd
 
float filt_accelzbody = 0
 
float filt_accelzbodyd = 0
 
float filt_accelzbodydd = 0
 
float roll_filt = 0
 
float roll_filtd = 0
 
float roll_filtdd = 0
 
float pitch_filt = 0
 
float pitch_filtd = 0
 
float pitch_filtdd = 0
 
float thrust_act = 0
 
float thrust_filt = 0
 
float thrust_filtd = 0
 
float thrust_filtdd = 0
 
struct FloatMat33 Ga
 
struct FloatMat33 Ga_inv
 
struct FloatVect3 euler_cmd
 
float filter_omega = 20.0
 
float filter_zeta = 0.65
 
struct FloatEulers guidance_euler_cmd
 
float thrust_in
 

Detailed Description

A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to IROS2016 to learn more!

Definition in file guidance_indi.c.

Function Documentation

void guidance_indi_calcG ( struct FloatMat33 Gmat)
Parameters
Gmatarray to write the matrix to [3x3]

Calculate the matrix of partial derivatives of the roll, pitch and thrust w.r.t. the NED accelerations

Definition at line 256 of file guidance_indi.c.

References FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, stateGetNedToBodyEulers_f(), and FloatEulers::theta.

Referenced by guidance_indi_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void guidance_indi_enter ( void  )
void guidance_indi_filter_accel ( void  )

low pass the accelerometer measurements with a second order filter to remove noise from vibrations

Definition at line 223 of file guidance_indi.c.

References filt_accel_ned, filt_accel_ned_d, filt_accel_ned_dd, filter_omega, filter_zeta, stateGetAccelNed_f(), VECT3_ADD_SCALED, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by guidance_indi_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void guidance_indi_filter_attitude ( void  )

Filter the attitude, such that it corresponds to the filtered measured acceleration.

Definition at line 238 of file guidance_indi.c.

References filter_omega, filter_zeta, FloatEulers::phi, pitch_filt, pitch_filtd, pitch_filtdd, roll_filt, roll_filtd, roll_filtdd, stateGetNedToBodyEulers_f(), and FloatEulers::theta.

Referenced by guidance_indi_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void stabilization_attitude_set_setpoint_rp_quat_f ( struct FloatEulers indi_rp_cmd,
bool  in_flight,
int32_t  heading 
)
Parameters
indi_rp_cmdroll/pitch command from indi guidance [rad] (float)
in_flightin flight boolean
headingthe desired heading [rad] in BFP with INT32_ANGLE_FRAC

function that creates a quaternion from a roll, pitch and yaw setpoint

Definition at line 287 of file guidance_indi.c.

References ANGLE_FLOAT_OF_BFP, float_quat_comp(), float_quat_comp_inv(), float_quat_comp_norm_shortest(), float_quat_normalize(), float_quat_of_axis_angle(), float_quat_of_eulers(), QUAT_BFP_OF_REAL, QUAT_COPY, stab_att_sp_quat, and stateGetNedToBodyEulers_f().

Referenced by guidance_indi_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

struct FloatVect3 euler_cmd

Definition at line 82 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatVect3 filt_accel_ned
struct FloatVect3 filt_accel_ned_d

Definition at line 64 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_accel().

struct FloatVect3 filt_accel_ned_dd

Definition at line 65 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_accel().

float filt_accelzbody = 0

Definition at line 66 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float filt_accelzbodyd = 0

Definition at line 67 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float filt_accelzbodydd = 0

Definition at line 68 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float filter_omega = 20.0

Definition at line 84 of file guidance_indi.c.

Referenced by guidance_indi_filter_accel(), and guidance_indi_filter_attitude().

float filter_zeta = 0.65

Definition at line 85 of file guidance_indi.c.

Referenced by guidance_indi_filter_accel(), and guidance_indi_filter_attitude().

struct FloatMat33 Ga

Definition at line 80 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatMat33 Ga_inv

Definition at line 81 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatEulers guidance_euler_cmd

Definition at line 87 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_pos_gain = 0.5

Definition at line 46 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_speed_gain = 1.8

Definition at line 47 of file guidance_indi.c.

Referenced by guidance_indi_run().

float pitch_filt = 0
float pitch_filtd = 0

Definition at line 73 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_attitude().

float pitch_filtdd = 0

Definition at line 74 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_attitude().

float roll_filt = 0
float roll_filtd = 0

Definition at line 70 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_attitude().

float roll_filtdd = 0

Definition at line 71 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_attitude().

struct FloatVect3 sp_accel = {0.0,0.0,0.0}

Definition at line 48 of file guidance_indi.c.

float thrust_act = 0

Definition at line 75 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float thrust_filt = 0

Definition at line 76 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_run().

float thrust_filtd = 0

Definition at line 77 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float thrust_filtdd = 0

Definition at line 78 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float thrust_in

Definition at line 88 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_run().