Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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_LIFTD_ASQ   0.20
 
#define GUIDANCE_INDI_MAX_PUSHER_INCREMENT   MAX_PPRZ
 
#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 TURN_AIRSPEED_TH   10.0
 
#define GUIDANCE_INDI_FILTER_CUTOFF   3.0
 
#define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF   0.5
 
#define GUIDANCE_INDI_CLIMB_SPEED_FWD   4.0
 
#define GUIDANCE_INDI_DESCEND_SPEED_FWD   -4.0
 
#define GUIDANCE_INDI_MAX_LAT_ACCEL   9.81
 
#define GUIDANCE_INDI_VEL_SP_ID   ABI_BROADCAST
 

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 send_guidance_indi_hybrid (struct transport_tx *trans, struct link_device *dev)
 
static void send_guidance_indi_debug (struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
 
void guidance_indi_init (void)
 Init function. More...
 
void guidance_indi_enter (void)
 Call upon entering indi guidance. More...
 
struct StabilizationSetpoint guidance_indi_run (struct FloatVect3 *accel_sp, float heading_sp)
 
static struct FloatVect3 compute_accel_from_speed_sp (void)
 
static float bound_vz_sp (float vz_sp)
 
struct StabilizationSetpoint guidance_indi_run_mode (bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode)
 
float WEAK guidance_indi_get_liftd (float airspeed, float theta)
 Get the derivative of lift w.r.t. More...
 

Variables

struct guidance_indi_hybrid_params gih_params
 
float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED
 
float guidance_indi_pitch_pref_deg = 0
 
bool take_heading_control = false
 
bool force_forward = false
 forward flight for hybrid nav More...
 
bool guidance_indi_airspeed_filtering = false
 
struct FloatVect3 sp_accel = {0.0,0.0,0.0}
 
float climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD
 
float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_SPEED_FWD
 
float inv_eff [4]
 
float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK
 
float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH
 
struct FloatEulers eulers_zxy
 state eulers in zxy order More...
 
float thrust_dyn = 0.f
 
float thrust_act = 0.f
 
Butterworth2LowPass filt_accel_ned [3]
 
Butterworth2LowPass roll_filt
 
Butterworth2LowPass pitch_filt
 
Butterworth2LowPass thrust_filt
 
Butterworth2LowPass accely_filt
 
Butterworth2LowPass guidance_indi_airspeed_filt
 
struct FloatVect2 desired_airspeed
 
float Ga [GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U]
 
struct FloatVect3 euler_cmd
 
float du_gih [GUIDANCE_INDI_HYBRID_U]
 
float v_gih [3]
 
float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF
 
float guidance_indi_airspeed_filt_cutoff = GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF
 
float guidance_indi_hybrid_heading_sp = 0.f
 
struct FloatEulers guidance_euler_cmd
 
struct ThrustSetpoint thrust_sp
 
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_AIRSPEED_FILT_CUTOFF

#define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF   0.5

Definition at line 158 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_CLIMB_SPEED_FWD

#define GUIDANCE_INDI_CLIMB_SPEED_FWD   4.0

Definition at line 162 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_DESCEND_SPEED_FWD

#define GUIDANCE_INDI_DESCEND_SPEED_FWD   -4.0

Definition at line 166 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_FILTER_CUTOFF

#define GUIDANCE_INDI_FILTER_CUTOFF   3.0

Definition at line 153 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_LIFTD_ASQ

#define GUIDANCE_INDI_LIFTD_ASQ   0.20

Definition at line 58 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_LIFTD_P50

#define GUIDANCE_INDI_LIFTD_P50   (GUIDANCE_INDI_LIFTD_P80/2)

Definition at line 71 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_LIFTD_P80

#define GUIDANCE_INDI_LIFTD_P80   (GUIDANCE_INDI_LIFTD_ASQ*12*12)

Definition at line 70 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_MAX_LAT_ACCEL

#define GUIDANCE_INDI_MAX_LAT_ACCEL   9.81

Definition at line 170 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_MAX_PUSHER_INCREMENT

#define GUIDANCE_INDI_MAX_PUSHER_INCREMENT   MAX_PPRZ

Definition at line 62 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_POS_GAIN

#define GUIDANCE_INDI_POS_GAIN   0.5

Definition at line 53 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_POS_GAINZ

#define GUIDANCE_INDI_POS_GAINZ   0.5

Definition at line 54 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_SPEED_GAIN

#define GUIDANCE_INDI_SPEED_GAIN   1.8

Definition at line 48 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_SPEED_GAINZ

#define GUIDANCE_INDI_SPEED_GAINZ   1.8

Definition at line 49 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_VEL_SP_ID

#define GUIDANCE_INDI_VEL_SP_ID   ABI_BROADCAST

Definition at line 234 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_ZERO_AIRSPEED

#define GUIDANCE_INDI_ZERO_AIRSPEED   FALSE

Definition at line 111 of file guidance_indi_hybrid.c.

◆ TURN_AIRSPEED_TH

#define TURN_AIRSPEED_TH   10.0

Definition at line 116 of file guidance_indi_hybrid.c.

Function Documentation

◆ bound_vz_sp()

static float bound_vz_sp ( float  vz_sp)
static

◆ compute_accel_from_speed_sp()

◆ guidance_indi_enter()

void guidance_indi_enter ( void  )

Call upon entering indi guidance.

Definition at line 352 of file guidance_indi_hybrid.c.

◆ guidance_indi_get_liftd()

float WEAK guidance_indi_get_liftd ( float  airspeed,
float  theta 
)

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 829 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()

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

References ACCEL_FLOAT_OF_BFP, accely_filt, eulers_zxy, filt_accel_ned, guidance_indi_airspeed_filt, FloatEulers::phi, pitch_filt, roll_filt, stateGetAccelBody_i(), stateGetAccelNed_f(), stateGetAirspeed_f(), FloatEulers::theta, update_butterworth_2_low_pass(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

+ Here is the call graph for this function:

◆ guidance_indi_run()

struct StabilizationSetpoint guidance_indi_run ( struct FloatVect3 accel_sp,
float  heading_sp 
)
Parameters
accel_spaccel setpoint in NED frame [m/s^2]
heading_spthe desired heading [rad]
Returns
stabilization setpoint structure

main indi guidance function

Definition at line 352 of file guidance_indi_hybrid.c.

◆ guidance_indi_run_mode()

struct StabilizationSetpoint guidance_indi_run_mode ( bool in_flight  UNUSED,
struct HorizontalGuidance gh,
struct VerticalGuidance gv,
enum GuidanceIndiHybrid_HMode  h_mode,
enum GuidanceIndiHybrid_VMode  v_mode 
)

Definition at line 696 of file guidance_indi_hybrid.c.

◆ send_guidance_indi_debug()

static void send_guidance_indi_debug ( struct transport_tx *trans  UNUSED,
struct link_device *dev  UNUSED 
)
static

Definition at line 300 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init().

+ 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 245 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 861 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

◆ climb_vspeed_fwd

float climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD

Definition at line 173 of file guidance_indi_hybrid.c.

Referenced by bound_vz_sp().

◆ descend_vspeed_fwd

float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_SPEED_FWD

Definition at line 174 of file guidance_indi_hybrid.c.

Referenced by bound_vz_sp().

◆ desired_airspeed

struct FloatVect2 desired_airspeed

Definition at line 192 of file guidance_indi_hybrid.c.

Referenced by rotwing_state_free_processor().

◆ du_gih

float du_gih[GUIDANCE_INDI_HYBRID_U]

Definition at line 199 of file guidance_indi_hybrid.c.

◆ euler_cmd

struct FloatVect3 euler_cmd

Definition at line 196 of file guidance_indi_hybrid.c.

Referenced by send_guidance_indi_hybrid().

◆ eulers_zxy

◆ filt_accel_ned

◆ filter_cutoff

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 223 of file guidance_indi_hybrid.c.

Referenced by compute_accel_from_speed_sp(), and guidance_indi_init().

◆ force_forward

bool force_forward = false

forward flight for hybrid nav

Definition at line 122 of file guidance_indi_hybrid.c.

Referenced by rotwing_state_set_state_settings().

◆ Ga

float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U]

Definition at line 196 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init().

◆ gi_speed_sp

struct FloatVect3 gi_speed_sp = {0.0, 0.0, 0.0}

Definition at line 229 of file guidance_indi_hybrid.c.

Referenced by 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().

◆ guidance_euler_cmd

struct FloatEulers guidance_euler_cmd

Definition at line 226 of file guidance_indi_hybrid.c.

◆ guidance_indi_airspeed_filt

Butterworth2LowPass guidance_indi_airspeed_filt

◆ guidance_indi_airspeed_filt_cutoff

float guidance_indi_airspeed_filt_cutoff = GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF

Definition at line 224 of file guidance_indi_hybrid.c.

Referenced by compute_accel_from_speed_sp(), and guidance_indi_init().

◆ guidance_indi_airspeed_filtering

bool guidance_indi_airspeed_filtering = false

Definition at line 124 of file guidance_indi_hybrid.c.

◆ guidance_indi_hybrid_heading_sp

float guidance_indi_hybrid_heading_sp = 0.f

Definition at line 226 of file guidance_indi_hybrid.c.

Referenced by compute_accel_from_speed_sp().

◆ guidance_indi_max_airspeed

float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED

Definition at line 90 of file guidance_indi_hybrid.c.

Referenced by rotwing_state_free_processor().

◆ guidance_indi_max_bank

float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK

Definition at line 179 of file guidance_indi_hybrid.c.

◆ guidance_indi_min_pitch

float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH

Definition at line 180 of file guidance_indi_hybrid.c.

◆ guidance_indi_pitch_pref_deg

float guidance_indi_pitch_pref_deg = 0

Definition at line 93 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_hybrid_set_wls_settings().

◆ indi_vel_sp

struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0}

Definition at line 237 of file guidance_indi_hybrid.c.

Referenced by vel_sp_cb().

◆ inv_eff

float inv_eff[4]

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

Referenced by send_guidance_indi_hybrid().

◆ take_heading_control

bool take_heading_control = false

Definition at line 120 of file guidance_indi_hybrid.c.

◆ thrust_act

float thrust_act = 0.f

Definition at line 186 of file guidance_indi_hybrid.c.

Referenced by compute_accel_from_speed_sp().

◆ thrust_dyn

float thrust_dyn = 0.f

Definition at line 185 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init().

◆ thrust_filt

Butterworth2LowPass thrust_filt

Definition at line 190 of file guidance_indi_hybrid.c.

Referenced by compute_accel_from_speed_sp(), and guidance_indi_init().

◆ thrust_in

float thrust_in

Definition at line 229 of file guidance_indi_hybrid.c.

Referenced by compute_accel_from_speed_sp().

◆ thrust_sp

struct ThrustSetpoint thrust_sp

Definition at line 226 of file guidance_indi_hybrid.c.

◆ time_of_vel_sp

float time_of_vel_sp = 0.0

Definition at line 239 of file guidance_indi_hybrid.c.

Referenced by vel_sp_cb().

◆ v_gih

float v_gih[3]

Definition at line 220 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_calcg_wing().

◆ vel_sp_ev

abi_event vel_sp_ev

Definition at line 236 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init().