Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_indi_hybrid.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_hybrid.c:

Go to the source code of this file.

Macros

#define GUIDANCE_INDI_SPEED_GAIN   1.8
 
#define GUIDANCE_INDI_SPEED_GAINZ   1.8
 
#define GUIDANCE_INDI_POS_GAIN   = 0.5;
 
#define GUIDANCE_INDI_POS_GAINZ   = 0.5;
 
#define MAX_DECELERATION   1.
 
#define GUIDANCE_INDI_FILTER_CUTOFF   3.0
 
#define GUIDANCE_INDI_PITCH_EFF_SCALING   1.0
 

Functions

void guidance_indi_propagate_filters (void)
 Low pass the accelerometer measurements to remove noise from vibrations. More...
 
static void guidance_indi_calcg_wing (struct FloatMat33 *Gmat)
 Calculate the matrix of partial derivatives of the roll, pitch and thrust w.r.t. More...
 
static float guidance_indi_get_liftd (float airspeed, float theta)
 Get the derivative of lift w.r.t. More...
 
struct FloatVect3 nav_get_speed_sp_from_go (struct EnuCoor_i target, float pos_gain)
 Go to a waypoint in the shortest way. More...
 
struct FloatVect3 nav_get_speed_sp_from_line (struct FloatVect2 line_v_enu, struct FloatVect2 to_end_v_enu, struct EnuCoor_i target, float pos_gain)
 follow a line. More...
 
struct FloatVect3 nav_get_speed_setpoint (float pos_gain)
 function that returns a speed setpoint based on flight plan. More...
 
void guidance_indi_init (void)
 Init function. More...
 
void guidance_indi_enter (void)
 Call upon entering indi guidance. More...
 
void guidance_indi_run (float *heading_sp)
 

Variables

struct guidance_indi_hybrid_params gih_params
 
struct FloatVect3 sp_accel = {0.0,0.0,0.0}
 
float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED
 
float inv_eff [4]
 
float lift_pitch_eff = GUIDANCE_INDI_PITCH_LIFT_EFF
 
struct FloatEulers eulers_zxy
 state eulers in zxy order More...
 
float thrust_act = 0
 
Butterworth2LowPass filt_accel_ned [3]
 
Butterworth2LowPass roll_filt
 
Butterworth2LowPass pitch_filt
 
Butterworth2LowPass thrust_filt
 
Butterworth2LowPass accely_filt
 
struct FloatVect2 desired_airspeed
 
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
 
struct FloatVect3 speed_sp = {0.0, 0.0, 0.0}
 

Detailed Description

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

Definition in file guidance_indi_hybrid.c.

Macro Definition Documentation

#define GUIDANCE_INDI_FILTER_CUTOFF   3.0

Definition at line 100 of file guidance_indi_hybrid.c.

#define GUIDANCE_INDI_PITCH_EFF_SCALING   1.0
#define GUIDANCE_INDI_POS_GAIN   = 0.5;

Definition at line 60 of file guidance_indi_hybrid.c.

#define GUIDANCE_INDI_POS_GAINZ   = 0.5;

Definition at line 61 of file guidance_indi_hybrid.c.

#define GUIDANCE_INDI_SPEED_GAIN   1.8

Definition at line 55 of file guidance_indi_hybrid.c.

#define GUIDANCE_INDI_SPEED_GAINZ   1.8

Definition at line 56 of file guidance_indi_hybrid.c.

#define MAX_DECELERATION   1.

Definition at line 78 of file guidance_indi_hybrid.c.

Referenced by nav_get_speed_sp_from_go().

Function Documentation

void guidance_indi_calcg_wing ( struct FloatMat33 Gmat)
static

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

the NED accelerations, taking into account the lift of a wing that is horizontal at -90 degrees pitch

Parameters
Gmatarray to write the matrix to [3x3]

Definition at line 441 of file guidance_indi_hybrid.c.

References eulers_zxy, guidance_indi_get_liftd(), GUIDANCE_INDI_PITCH_EFF_SCALING, FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, stateGetAirspeed_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 165 of file guidance_indi_hybrid.c.

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

+ Here is the call graph for this function:

float guidance_indi_get_liftd ( float  airspeed,
float  theta 
)
static

Get the derivative of lift w.r.t.

pitch.

Parameters
airspeedThe airspeed says most about the flight condition
Returns
The derivative of lift w.r.t. pitch

Definition at line 483 of file guidance_indi_hybrid.c.

References lift_pitch_eff.

Referenced by guidance_indi_calcg_wing().

+ Here is the caller graph for this function:

void guidance_indi_init ( void  )

Init function.

Definition at line 146 of file guidance_indi_hybrid.c.

References accel_sp_cb(), filter_cutoff, GUIDANCE_INDI_ACCEL_SP_ID, and init_butterworth_2_low_pass().

+ Here is the call graph for this function:

void guidance_indi_propagate_filters ( void  )

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 Called as a periodic function with PERIODIC_FREQ

Definition at line 420 of file guidance_indi_hybrid.c.

References ACCEL_FLOAT_OF_BFP, eulers_zxy, FloatEulers::phi, stateGetAccelBody_i(), stateGetAccelNed_f(), FloatEulers::theta, 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 guidance_indi_run ( float *  heading_sp)
Parameters
heading_spthe desired heading [rad]

main indi guidance function

Definition at line 176 of file guidance_indi_hybrid.c.

References ANGLE_BFP_OF_REAL, AP_MODE_NAV, autopilot, BFP_OF_REAL, control_increment, desired_airspeed, euler_cmd, eulers_zxy, FLOAT_ANGLE_NORMALIZE, float_eulers_of_quat_yxz(), float_eulers_of_quat_zxy(), float_quat_normalize(), float_quat_of_eulers_yxz(), float_quat_of_eulers_zxy(), FLOAT_VECT2_NORM, Ga, Ga_inv, get_sys_time_float(), guidance_euler_cmd, guidance_h, GUIDANCE_H_MAX_BANK, guidance_indi_calcg_wing(), guidance_indi_calcG_yxz(), guidance_indi_max_airspeed, guidance_indi_max_bank, guidance_indi_pos_gain, guidance_indi_propagate_filters(), guidance_indi_speed_gain, guidance_v_z_ref, indi_accel_sp_set_2d, indi_accel_sp_set_3d, INT32_ANGLE_FRAC, INT32_PERCENTAGE_FRAC, INT_MULT_RSHIFT, MAT33_INV, MAT33_VECT3_MUL, Min, pprz_autopilot::mode, nav_get_speed_setpoint(), SecondOrderLowPass::o, FloatEulers::phi, HorizontalGuidanceReference::pos, POS_FLOAT_OF_BFP, guidance_indi_hybrid_params::pos_gain, guidance_indi_hybrid_params::pos_gainz, FloatEulers::psi, QUAT_BFP_OF_REAL, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, HorizontalGuidance::ref, guidance_indi_hybrid_params::speed_gain, guidance_indi_hybrid_params::speed_gainz, stab_att_sp_quat, stabilization_cmd, stateGetAirspeed_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetPositionNed_i(), stateGetSpeedNed_f(), FloatEulers::theta, thrust_in, THRUST_INCREMENT_ID, time_of_accel_sp_2d, time_of_accel_sp_3d, transition_percentage, transition_theta_offset, RadioControl::values, VECT2_DIFF, vect_bound_in_2d(), FloatVect2::x, FloatVect3::x, NedCoor_f::x, Int32Vect2::x, FloatVect2::y, FloatVect3::y, NedCoor_f::y, Int32Vect2::y, FloatVect3::z, and NedCoor_f::z.

+ Here is the call graph for this function:

struct FloatVect3 nav_get_speed_setpoint ( float  pos_gain)

function that returns a speed setpoint based on flight plan.

The routines are meant for a hybrid UAV and assume measurement of airspeed. Makes the vehicle track a vector field with a sink at a waypoint. Use force_forward to maintain airspeed and fly 'through' waypoints.

Returns
desired speed setpoint FloatVect3

Definition at line 506 of file guidance_indi_hybrid.c.

References horizontal_mode, HORIZONTAL_MODE_ROUTE, nav_get_speed_sp_from_go(), nav_get_speed_sp_from_line(), navigation_target, speed_sp, and to_end_vect.

Referenced by guidance_indi_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

struct FloatVect3 nav_get_speed_sp_from_go ( struct EnuCoor_i  target,
float  pos_gain 
)
struct FloatVect3 nav_get_speed_sp_from_line ( struct FloatVect2  line_v_enu,
struct FloatVect2  to_end_v_enu,
struct EnuCoor_i  target,
float  pos_gain 
)

follow a line.

Parameters
line_v_enu2d vector from beginning (0) line to end in enu
to_end_v_enu2d vector from current position to end in enu
targetend waypoint in enu
Returns
desired speed setpoint FloatVect3

Definition at line 525 of file guidance_indi_hybrid.c.

References float_vect2_norm(), force_forward, guidance_v_mode, GUIDANCE_V_MODE_NAV, guidance_v_zd_sp, nav_climb_vspeed, nav_descend_vspeed, nav_max_speed, POS_FLOAT_OF_BFP, guidance_indi_hybrid_params::pos_gainz, SPEED_FLOAT_OF_BFP, stateGetAirspeed_f(), stateGetPositionNed_f(), target, VECT2_ADD, VECT2_ASSIGN, VECT2_SMUL, VECT3_ASSIGN, vertical_mode, VERTICAL_MODE_CLIMB, FloatVect2::x, FloatVect2::y, FloatVect3::z, and NedCoor_f::z.

Referenced by nav_get_speed_setpoint().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

Butterworth2LowPass accely_filt

Definition at line 121 of file guidance_indi_hybrid.c.

struct FloatVect2 desired_airspeed

Definition at line 123 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

struct FloatVect3 euler_cmd

Definition at line 127 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

struct FloatEulers eulers_zxy

state eulers in zxy order

Definition at line 114 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_calcg_wing(), guidance_indi_propagate_filters(), guidance_indi_run(), and send_fp().

Butterworth2LowPass filt_accel_ned[3]

Definition at line 117 of file guidance_indi_hybrid.c.

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 129 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init().

struct FloatMat33 Ga

Definition at line 125 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

struct FloatMat33 Ga_inv

Definition at line 126 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

struct guidance_indi_hybrid_params gih_params
Initial value:
= {
.speed_gain = GUIDANCE_INDI_SPEED_GAIN,
.speed_gainz = GUIDANCE_INDI_SPEED_GAINZ,
}
#define GUIDANCE_INDI_SPEED_GAINZ
#define GUIDANCE_INDI_SPEED_GAIN
#define GUIDANCE_INDI_POS_GAIN
#define GUIDANCE_INDI_POS_GAINZ

Definition at line 64 of file guidance_indi_hybrid.c.

struct FloatEulers guidance_euler_cmd

Definition at line 131 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED

Definition at line 107 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

float inv_eff[4]

Definition at line 109 of file guidance_indi_hybrid.c.

float lift_pitch_eff = GUIDANCE_INDI_PITCH_LIFT_EFF

Definition at line 111 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_get_liftd().

Butterworth2LowPass pitch_filt

Definition at line 119 of file guidance_indi_hybrid.c.

Definition at line 118 of file guidance_indi_hybrid.c.

struct FloatVect3 sp_accel = {0.0,0.0,0.0}

Definition at line 81 of file guidance_indi_hybrid.c.

struct FloatVect3 speed_sp = {0.0, 0.0, 0.0}
float thrust_act = 0

Definition at line 116 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_enter().

Butterworth2LowPass thrust_filt

Definition at line 120 of file guidance_indi_hybrid.c.

float thrust_in

Definition at line 132 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_enter(), and guidance_indi_run().