Paparazzi UAS  v5.12_stable-4-g9b43e9b
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.

Macros

#define GUIDANCE_INDI_FILTER_CUTOFF   3.0
 

Functions

static void guidance_indi_propagate_filters (void)
 Low pass the accelerometer measurements to remove noise from vibrations. More...
 
static void guidance_indi_calcG (struct FloatMat33 *Gmat)
 
void guidance_indi_enter (void)
 Call upon entering indi guidance. More...
 
void guidance_indi_run (bool in_flight, float heading_sp)
 
void stabilization_attitude_set_setpoint_rp_quat_f (struct FloatEulers *indi_rp_cmd, bool in_flight, float 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}
 
float thrust_act = 0
 
Butterworth2LowPass filt_accel_ned [3]
 
Butterworth2LowPass roll_filt
 
Butterworth2LowPass pitch_filt
 
Butterworth2LowPass thrust_filt
 
struct FloatMat33 Ga
 
struct FloatMat33 Ga_inv
 
struct FloatVect3 euler_cmd
 
float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF
 
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.

Macro Definition Documentation

#define GUIDANCE_INDI_FILTER_CUTOFF   3.0

Definition at line 83 of file guidance_indi.c.

Function Documentation

void guidance_indi_calcG ( struct FloatMat33 Gmat)
static
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 250 of file guidance_indi.c.

References FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, stateGetNedToBodyEulers_f(), mesonh.mesonh_atmosphere::T, 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  )

Call upon entering indi guidance.

Definition at line 109 of file guidance_indi.c.

References filter_cutoff, init_butterworth_2_low_pass(), FloatEulers::phi, stabilization_cmd, stateGetNedToBodyEulers_f(), FloatEulers::theta, thrust_act, and thrust_in.

Referenced by guidance_h_mode_changed().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void guidance_indi_propagate_filters ( void  )
static

Low pass the accelerometer measurements to remove noise from vibrations.

The roll and pitch also need to be filtered to synchronize them with the acceleration

Definition at line 234 of file guidance_indi.c.

References stateGetAccelNed_f(), stateGetNedToBodyEulers_f(), update_butterworth_2_low_pass(), NedCoor_f::x, NedCoor_f::y, 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 stabilization_attitude_set_setpoint_rp_quat_f ( struct FloatEulers indi_rp_cmd,
bool  in_flight,
float  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 281 of file guidance_indi.c.

References 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 95 of file guidance_indi.c.

Referenced by guidance_indi_run().

Butterworth2LowPass filt_accel_ned[3]

Definition at line 88 of file guidance_indi.c.

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 97 of file guidance_indi.c.

Referenced by guidance_indi_enter().

struct FloatMat33 Ga

Definition at line 93 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatMat33 Ga_inv

Definition at line 94 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatEulers guidance_euler_cmd

Definition at line 99 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_pos_gain = 0.5

Definition at line 55 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_speed_gain = 1.8

Definition at line 61 of file guidance_indi.c.

Referenced by guidance_indi_run().

Butterworth2LowPass pitch_filt

Definition at line 90 of file guidance_indi.c.

Definition at line 89 of file guidance_indi.c.

struct FloatVect3 sp_accel = {0.0,0.0,0.0}

Definition at line 64 of file guidance_indi.c.

float thrust_act = 0

Definition at line 87 of file guidance_indi.c.

Referenced by guidance_indi_enter().

Butterworth2LowPass thrust_filt

Definition at line 91 of file guidance_indi.c.

float thrust_in

Definition at line 100 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_run().