Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_indi_hybrid.c File Reference
+ 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 NAV_MAX_SPEED   (GUIDANCE_INDI_MAX_AIRSPEED + 10.0)
 
#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...
 
static void send_guidance_indi_hybrid (struct transport_tx *trans, struct link_device *dev)
 
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
 
float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED
 
float nav_max_speed = NAV_MAX_SPEED
 
bool take_heading_control = false
 
struct FloatVect3 sp_accel = {0.0,0.0,0.0}
 
float guidance_indi_line_gain = 1.0
 
float inv_eff [4]
 
float lift_pitch_eff = GUIDANCE_INDI_PITCH_LIFT_EFF
 
float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK
 
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

◆ GUIDANCE_INDI_FILTER_CUTOFF

#define GUIDANCE_INDI_FILTER_CUTOFF   3.0

Definition at line 109 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_PITCH_EFF_SCALING

#define GUIDANCE_INDI_PITCH_EFF_SCALING   1.0

◆ GUIDANCE_INDI_POS_GAIN

#define GUIDANCE_INDI_POS_GAIN   0.5

Definition at line 60 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_POS_GAINZ

#define GUIDANCE_INDI_POS_GAINZ   0.5

Definition at line 61 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_SPEED_GAIN

#define GUIDANCE_INDI_SPEED_GAIN   1.8

Definition at line 55 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_SPEED_GAINZ

#define GUIDANCE_INDI_SPEED_GAINZ   1.8

Definition at line 56 of file guidance_indi_hybrid.c.

◆ MAX_DECELERATION

#define MAX_DECELERATION   1.

Definition at line 84 of file guidance_indi_hybrid.c.

◆ NAV_MAX_SPEED

#define NAV_MAX_SPEED   (GUIDANCE_INDI_MAX_AIRSPEED + 10.0)

Definition at line 80 of file guidance_indi_hybrid.c.

Function Documentation

◆ guidance_indi_calcg_wing()

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 502 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:

◆ guidance_indi_enter()

void guidance_indi_enter ( void  )

Call upon entering indi guidance.

Definition at line 202 of file guidance_indi_hybrid.c.

References accely_filt, filt_accel_ned, filter_cutoff, init_butterworth_2_low_pass(), FloatEulers::phi, pitch_filt, roll_filt, stabilization_cmd, stateGetNedToBodyEulers_f(), FloatEulers::theta, thrust_act, thrust_filt, and thrust_in.

+ Here is the call graph for this function:

◆ guidance_indi_get_liftd()

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 544 of file guidance_indi_hybrid.c.

References lift_pitch_eff.

Referenced by guidance_indi_calcg_wing().

+ Here is the caller graph for this function:

◆ guidance_indi_init()

◆ guidance_indi_propagate_filters()

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 481 of file guidance_indi_hybrid.c.

References ACCEL_FLOAT_OF_BFP, accely_filt, eulers_zxy, filt_accel_ned, FloatEulers::phi, pitch_filt, roll_filt, 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:

◆ guidance_indi_run()

void guidance_indi_run ( float *  heading_sp)
Parameters
heading_spthe desired heading [rad]

main indi guidance function

Definition at line 223 of file guidance_indi_hybrid.c.

References HorizontalGuidanceReference::accel, ACCEL_FLOAT_OF_BFP, accely_filt, ANGLE_BFP_OF_REAL, ANGLE_FLOAT_OF_BFP, AP_MODE_NAV, autopilot, BFP_OF_REAL, control_increment, desired_airspeed, euler_cmd, eulers_zxy, filt_accel_ned, 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, force_forward, Ga, Ga_inv, get_sys_time_float(), gih_params, guidance_euler_cmd, guidance_h, 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_specific_force_gain, guidance_indi_speed_gain, guidance_v_z_ref, guidance_v_zd_ref, guidance_v_zdd_ref, guidance_indi_hybrid_params::heading_bank_gain, indi_accel_sp, 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(), nav_heading, SecondOrderLowPass::o, FloatEulers::phi, pitch_filt, 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, roll_filt, sp_accel, HorizontalGuidanceReference::speed, SPEED_FLOAT_OF_BFP, guidance_indi_hybrid_params::speed_gain, guidance_indi_hybrid_params::speed_gainz, speed_sp, stab_att_sp_quat, stabilization_cmd, stateGetAirspeed_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetPositionNed_i(), stateGetSpeedNed_f(), take_heading_control, FloatEulers::theta, thrust_filt, 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:

◆ nav_get_speed_setpoint()

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 567 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:

◆ nav_get_speed_sp_from_go()

struct FloatVect3 nav_get_speed_sp_from_go ( struct EnuCoor_i  target,
float  pos_gain 
)

◆ nav_get_speed_sp_from_line()

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 586 of file guidance_indi_hybrid.c.

References direction, float_vect2_norm(), force_forward, gih_params, guidance_indi_line_gain, 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:

◆ send_guidance_indi_hybrid()

static void send_guidance_indi_hybrid ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 158 of file guidance_indi_hybrid.c.

References dev, euler_cmd, filt_accel_ned, SecondOrderLowPass::o, sp_accel, speed_sp, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by guidance_indi_init().

+ Here is the caller graph for this function:

Variable Documentation

◆ accely_filt

◆ desired_airspeed

struct FloatVect2 desired_airspeed

Definition at line 136 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ euler_cmd

struct FloatVect3 euler_cmd

Definition at line 140 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run(), and send_guidance_indi_hybrid().

◆ eulers_zxy

struct FloatEulers eulers_zxy

◆ filt_accel_ned

◆ filter_cutoff

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 142 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_enter(), and guidance_indi_init().

◆ Ga

struct FloatMat33 Ga

Definition at line 138 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ Ga_inv

struct FloatMat33 Ga_inv

Definition at line 139 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ gih_params

struct guidance_indi_hybrid_params gih_params
Initial value:
= {
.speed_gain = GUIDANCE_INDI_SPEED_GAIN,
.speed_gainz = GUIDANCE_INDI_SPEED_GAINZ,
.heading_bank_gain = GUIDANCE_INDI_HEADING_BANK_GAIN,
}

Definition at line 64 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run(), nav_get_speed_sp_from_go(), and nav_get_speed_sp_from_line().

◆ guidance_euler_cmd

struct FloatEulers guidance_euler_cmd

Definition at line 144 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ guidance_indi_line_gain

float guidance_indi_line_gain = 1.0

Definition at line 116 of file guidance_indi_hybrid.c.

Referenced by nav_get_speed_sp_from_line().

◆ guidance_indi_max_airspeed

float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED

Definition at line 77 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ guidance_indi_max_bank

float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK

Definition at line 124 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ inv_eff

float inv_eff[4]

Definition at line 119 of file guidance_indi_hybrid.c.

◆ lift_pitch_eff

float lift_pitch_eff = GUIDANCE_INDI_PITCH_LIFT_EFF

Definition at line 121 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_get_liftd().

◆ nav_max_speed

float nav_max_speed = NAV_MAX_SPEED

Definition at line 81 of file guidance_indi_hybrid.c.

Referenced by nav_get_speed_sp_from_go(), and nav_get_speed_sp_from_line().

◆ pitch_filt

◆ roll_filt

◆ sp_accel

struct FloatVect3 sp_accel = {0.0,0.0,0.0}

Definition at line 90 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run(), and send_guidance_indi_hybrid().

◆ speed_sp

struct FloatVect3 speed_sp = {0.0, 0.0, 0.0}

◆ take_heading_control

bool take_heading_control = false

Definition at line 88 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ thrust_act

float thrust_act = 0

Definition at line 129 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_enter().

◆ thrust_filt

◆ thrust_in

float thrust_in

Definition at line 145 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_enter(), and guidance_indi_run().

GUIDANCE_INDI_SPEED_GAINZ
#define GUIDANCE_INDI_SPEED_GAINZ
Definition: guidance_indi_hybrid.c:56
GUIDANCE_INDI_POS_GAINZ
#define GUIDANCE_INDI_POS_GAINZ
Definition: guidance_indi_hybrid.c:61
GUIDANCE_INDI_SPEED_GAIN
#define GUIDANCE_INDI_SPEED_GAIN
Definition: guidance_indi_hybrid.c:55
GUIDANCE_INDI_POS_GAIN
#define GUIDANCE_INDI_POS_GAIN
Definition: guidance_indi_hybrid.c:60