Paparazzi UAS  v6.2.0_stable
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 GUIDANCE_INDI_MIN_PITCH   -120
 
#define GUIDANCE_INDI_MAX_PITCH   25
 
#define GUIDANCE_INDI_LIFTD_ASQ   0.20
 
#define GUIDANCE_INDI_LIFTD_P80   (GUIDANCE_INDI_LIFTD_ASQ*12*12)
 
#define GUIDANCE_INDI_LIFTD_P50   (GUIDANCE_INDI_LIFTD_P80/2)
 
#define GUIDANCE_INDI_ZERO_AIRSPEED   FALSE
 
#define GUIDANCE_INDI_NAV_SPEED_MARGIN   10.0
 
#define NAV_MAX_SPEED   (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)
 
#define MAX_DECELERATION   1.
 
#define TURN_AIRSPEED_TH   10.0
 
#define GUIDANCE_INDI_FILTER_CUTOFF   3.0
 
#define GUIDANCE_INDI_VEL_SP_ID   ABI_BROADCAST
 
#define GUIDANCE_INDI_PITCH_EFF_SCALING   1.0
 

Functions

static void vel_sp_cb (uint8_t sender_id, struct FloatVect3 *vel_sp)
 ABI callback that obtains the velocity setpoint from a module. More...
 
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 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 gi_speed_sp = {0.0, 0.0, 0.0}
 
abi_event vel_sp_ev
 
struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0}
 
float time_of_vel_sp = 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 144 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_LIFTD_ASQ

#define GUIDANCE_INDI_LIFTD_ASQ   0.20

Definition at line 70 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_LIFTD_P50

#define GUIDANCE_INDI_LIFTD_P50   (GUIDANCE_INDI_LIFTD_P80/2)

Definition at line 79 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_LIFTD_P80

#define GUIDANCE_INDI_LIFTD_P80   (GUIDANCE_INDI_LIFTD_ASQ*12*12)

Definition at line 78 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_MAX_PITCH

#define GUIDANCE_INDI_MAX_PITCH   25

Definition at line 66 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_MIN_PITCH

#define GUIDANCE_INDI_MIN_PITCH   -120

Definition at line 65 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_NAV_SPEED_MARGIN

#define GUIDANCE_INDI_NAV_SPEED_MARGIN   10.0

Definition at line 108 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.

◆ GUIDANCE_INDI_VEL_SP_ID

#define GUIDANCE_INDI_VEL_SP_ID   ABI_BROADCAST

Definition at line 183 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_ZERO_AIRSPEED

#define GUIDANCE_INDI_ZERO_AIRSPEED   FALSE

Definition at line 103 of file guidance_indi_hybrid.c.

◆ MAX_DECELERATION

#define MAX_DECELERATION   1.

Definition at line 114 of file guidance_indi_hybrid.c.

◆ NAV_MAX_SPEED

#define NAV_MAX_SPEED   (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)

Definition at line 110 of file guidance_indi_hybrid.c.

◆ TURN_AIRSPEED_TH

#define TURN_AIRSPEED_TH   10.0

Definition at line 119 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 572 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 244 of file guidance_indi_hybrid.c.

References accely_filt, filt_accel_ned, filter_cutoff, init_butterworth_2_low_pass(), pitch_filt, roll_filt, stabilization_cmd, stateGetNedToBodyEulers_f(), 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 614 of file guidance_indi_hybrid.c.

References gih_params, guidance_indi_hybrid_params::liftd_asq, guidance_indi_hybrid_params::liftd_p50, and guidance_indi_hybrid_params::liftd_p80.

Referenced by guidance_indi_calcg_wing().

+ Here is the caller graph for this function:

◆ guidance_indi_init()

void guidance_indi_init ( void  )

◆ 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 551 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 265 of file guidance_indi_hybrid.c.

References accely_filt, ANGLE_BFP_OF_REAL, ANGLE_FLOAT_OF_BFP, AP_MODE_NAV, autopilot, BFP_OF_REAL, desired_airspeed, euler_cmd, eulers_zxy, filt_accel_ned, FLOAT_ANGLE_NORMALIZE, float_eulers_of_quat_zxy(), float_quat_normalize(), float_quat_of_eulers_zxy(), FLOAT_VECT2_NORM, force_forward, Ga, Ga_inv, get_sys_time_float(), gi_speed_sp, gih_params, guidance_euler_cmd, guidance_h, guidance_indi_calcg_wing(), guidance_indi_max_airspeed, guidance_indi_max_bank, GUIDANCE_INDI_MAX_PITCH, GUIDANCE_INDI_MIN_PITCH, guidance_indi_propagate_filters(), guidance_indi_specific_force_gain, GUIDANCE_INDI_ZERO_AIRSPEED, guidance_v_z_ref, heading, guidance_indi_hybrid_params::heading_bank_gain, indi_vel_sp, 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, guidance_indi_hybrid_params::speed_gain, guidance_indi_hybrid_params::speed_gainz, stab_att_sp_quat, stabilization_attitude_get_heading_f(), stabilization_cmd, stateGetAirspeed_f(), stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetPositionNed_i(), stateGetSpeedNed_f(), take_heading_control, FloatEulers::theta, thrust_filt, thrust_in, THRUST_INCREMENT_ID, time_of_vel_sp, transition_percentage, transition_theta_offset, TURN_AIRSPEED_TH, RadioControl::values, VECT2_DIFF, vect_bound_in_2d(), FloatVect2::x, FloatVect3::x, Int32Vect2::x, NedCoor_f::x, FloatVect2::y, FloatVect3::y, Int32Vect2::y, NedCoor_f::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 614 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

+ 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 
)

Go to a waypoint in the shortest way.

Parameters
targetthe target waypoint
Returns
desired speed FloatVect3

Definition at line 614 of file guidance_indi_hybrid.c.

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

◆ send_guidance_indi_hybrid()

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

Definition at line 199 of file guidance_indi_hybrid.c.

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

Referenced by guidance_indi_init().

+ Here is the caller graph for this function:

◆ vel_sp_cb()

static void vel_sp_cb ( uint8_t  sender_id,
struct FloatVect3 vel_sp 
)
static

ABI callback that obtains the velocity setpoint from a module.

Definition at line 809 of file guidance_indi_hybrid.c.

References get_sys_time_float(), indi_vel_sp, time_of_vel_sp, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by guidance_indi_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

Variable Documentation

◆ accely_filt

◆ desired_airspeed

struct FloatVect2 desired_airspeed

Definition at line 167 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ euler_cmd

struct FloatVect3 euler_cmd

Definition at line 167 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run(), and send_guidance_indi_hybrid().

◆ eulers_zxy

◆ filt_accel_ned

◆ filter_cutoff

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 175 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_enter(), and guidance_indi_init().

◆ Ga

struct FloatMat33 Ga

Definition at line 167 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ Ga_inv

struct FloatMat33 Ga_inv

Definition at line 167 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ gi_speed_sp

struct FloatVect3 gi_speed_sp = {0.0, 0.0, 0.0}

Definition at line 178 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run(), and send_guidance_indi_hybrid().

◆ 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,
.liftd_asq = GUIDANCE_INDI_LIFTD_ASQ,
}
#define GUIDANCE_INDI_SPEED_GAINZ
#define GUIDANCE_INDI_LIFTD_ASQ
#define GUIDANCE_INDI_POS_GAIN
#define GUIDANCE_INDI_LIFTD_P50
#define GUIDANCE_INDI_POS_GAINZ
#define GUIDANCE_INDI_LIFTD_P80
#define GUIDANCE_INDI_SPEED_GAIN

Definition at line 1 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_get_liftd(), and guidance_indi_run().

◆ guidance_euler_cmd

struct FloatEulers guidance_euler_cmd

Definition at line 175 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 151 of file guidance_indi_hybrid.c.

◆ guidance_indi_max_airspeed

float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED

Definition at line 98 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 157 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ indi_vel_sp

struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0}

Definition at line 186 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run(), and vel_sp_cb().

◆ inv_eff

float inv_eff[4]

Definition at line 154 of file guidance_indi_hybrid.c.

◆ nav_max_speed

float nav_max_speed = NAV_MAX_SPEED

Definition at line 111 of file guidance_indi_hybrid.c.

◆ pitch_filt

◆ roll_filt

◆ sp_accel

struct FloatVect3 sp_accel = {0.0,0.0,0.0}

Definition at line 123 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run(), and send_guidance_indi_hybrid().

◆ take_heading_control

bool take_heading_control = false

Definition at line 123 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ thrust_act

float thrust_act = 0

Definition at line 162 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_enter().

◆ thrust_filt

◆ thrust_in

float thrust_in

Definition at line 178 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_enter(), and guidance_indi_run().

◆ time_of_vel_sp

float time_of_vel_sp = 0.0

Definition at line 188 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run(), and vel_sp_cb().

◆ vel_sp_ev

abi_event vel_sp_ev

Definition at line 185 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init().