![]() |
Paparazzi UAS
v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to IROS2016 to learn more! More...
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
#include "modules/radio_control/radio_control.h"
#include "state.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
#include "stdio.h"
#include "filters/low_pass_filter.h"
#include "modules/core/abi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "modules/datalink/telemetry.h"
#include "firmwares/rotorcraft/navigation.h"
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_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_CLIMB_SPEED_FWD 4.0 |
#define | GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0 |
#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... | |
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to IROS2016 to learn more!
Definition in file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_CLIMB_SPEED_FWD 4.0 |
Definition at line 152 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0 |
Definition at line 156 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_FILTER_CUTOFF 3.0 |
Definition at line 147 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_LIFTD_ASQ 0.20 |
Definition at line 59 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2) |
Definition at line 68 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12) |
Definition at line 67 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_POS_GAIN 0.5 |
Definition at line 54 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_POS_GAINZ 0.5 |
Definition at line 55 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_SPEED_GAIN 1.8 |
Definition at line 49 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_SPEED_GAINZ 1.8 |
Definition at line 50 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_VEL_SP_ID ABI_BROADCAST |
Definition at line 215 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_ZERO_AIRSPEED FALSE |
Definition at line 108 of file guidance_indi_hybrid.c.
#define TURN_AIRSPEED_TH 10.0 |
Definition at line 113 of file guidance_indi_hybrid.c.
|
static |
Definition at line 662 of file guidance_indi_hybrid.c.
References RotorcraftNavigation::climb_vspeed, climb_vspeed_fwd, RotorcraftNavigation::descend_vspeed, descend_vspeed_fwd, nav, and stateGetAirspeed_f().
|
static |
Definition at line 330 of file guidance_indi_hybrid.c.
References accely_filt, eulers_zxy, filt_accel_ned, filter_cutoff, float_eulers_of_quat_zxy(), guidance_indi_hybrid_heading_sp, RotorcraftNavigation::heading, init_butterworth_2_low_pass(), nav, FloatEulers::phi, pitch_filt, FloatEulers::psi, roll_filt, stabilization_cmd, stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), FloatEulers::theta, thrust_act, thrust_filt, and thrust_in.
void guidance_indi_enter | ( | void | ) |
Call upon entering indi guidance.
Definition at line 330 of file guidance_indi_hybrid.c.
float WEAK guidance_indi_get_liftd | ( | float | airspeed, |
float | theta | ||
) |
Get the derivative of lift w.r.t.
pitch.
airspeed | The airspeed says most about the flight condition |
Definition at line 791 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().
void guidance_indi_init | ( | void | ) |
Init function.
Definition at line 291 of file guidance_indi_hybrid.c.
References accely_filt, DefaultPeriodic, filt_accel_ned, filter_cutoff, Ga, GUIDANCE_INDI_VEL_SP_ID, init_butterworth_2_low_pass(), pitch_filt, register_periodic_telemetry(), roll_filt, send_guidance_indi_debug(), send_guidance_indi_hybrid(), thrust_dyn, thrust_filt, vel_sp_cb(), and vel_sp_ev.
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 769 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.
struct StabilizationSetpoint guidance_indi_run | ( | struct FloatVect3 * | accel_sp, |
float | heading_sp | ||
) |
accel_sp | accel setpoint in NED frame [m/s^2] |
heading_sp | the desired heading [rad] |
main indi guidance function
Definition at line 330 of file guidance_indi_hybrid.c.
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 662 of file guidance_indi_hybrid.c.
|
static |
Definition at line 281 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_init().
|
static |
Definition at line 226 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().
|
static |
ABI callback that obtains the velocity setpoint from a module.
Definition at line 823 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().
Butterworth2LowPass accely_filt |
Definition at line 177 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp(), guidance_indi_init(), and guidance_indi_propagate_filters().
float climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD |
Definition at line 159 of file guidance_indi_hybrid.c.
Referenced by bound_vz_sp().
float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_SPEED_FWD |
Definition at line 160 of file guidance_indi_hybrid.c.
Referenced by bound_vz_sp().
struct FloatVect2 desired_airspeed |
Definition at line 177 of file guidance_indi_hybrid.c.
struct FloatVect3 euler_cmd |
Definition at line 181 of file guidance_indi_hybrid.c.
Referenced by send_guidance_indi_hybrid().
struct FloatEulers eulers_zxy |
state eulers in zxy order
Definition at line 166 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp(), eff_scheduling_falcon_periodic(), eff_scheduling_periodic_b(), guidance_indi_calcg_wing(), guidance_indi_hybrid_set_wls_settings(), guidance_indi_propagate_filters(), rot_wing_vis_transition(), schdule_control_effectiveness(), and send_fp().
Butterworth2LowPass filt_accel_ned[3] |
Definition at line 173 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp(), guidance_indi_init(), guidance_indi_propagate_filters(), and send_guidance_indi_hybrid().
float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF |
Definition at line 206 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp(), and guidance_indi_init().
bool force_forward = false |
forward flight for hybrid nav
Definition at line 119 of file guidance_indi_hybrid.c.
Referenced by rotwing_state_set_state_settings().
float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U] |
Definition at line 181 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_init().
struct FloatVect3 gi_speed_sp = {0.0, 0.0, 0.0} |
Definition at line 210 of file guidance_indi_hybrid.c.
Referenced by send_guidance_indi_hybrid().
struct guidance_indi_hybrid_params gih_params |
Definition at line 1 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_get_liftd().
struct FloatEulers guidance_euler_cmd |
Definition at line 208 of file guidance_indi_hybrid.c.
float guidance_indi_hybrid_heading_sp = 0.f |
Definition at line 208 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp().
float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED |
Definition at line 87 of file guidance_indi_hybrid.c.
float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK |
Definition at line 165 of file guidance_indi_hybrid.c.
float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH |
Definition at line 166 of file guidance_indi_hybrid.c.
float guidance_indi_pitch_pref_deg = 0 |
Definition at line 90 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_hybrid_set_wls_settings().
struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0} |
Definition at line 218 of file guidance_indi_hybrid.c.
Referenced by vel_sp_cb().
float inv_eff[4] |
Definition at line 162 of file guidance_indi_hybrid.c.
Butterworth2LowPass pitch_filt |
Definition at line 175 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp(), guidance_indi_init(), and guidance_indi_propagate_filters().
Butterworth2LowPass roll_filt |
Definition at line 174 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp(), guidance_indi_init(), and guidance_indi_propagate_filters().
struct FloatVect3 sp_accel = {0.0,0.0,0.0} |
Definition at line 119 of file guidance_indi_hybrid.c.
Referenced by send_guidance_indi_hybrid().
bool take_heading_control = false |
Definition at line 117 of file guidance_indi_hybrid.c.
float thrust_act = 0 |
Definition at line 172 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp().
float thrust_dyn = 0.f |
Definition at line 171 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_init().
Butterworth2LowPass thrust_filt |
Definition at line 176 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp(), and guidance_indi_init().
float thrust_in |
Definition at line 210 of file guidance_indi_hybrid.c.
Referenced by compute_accel_from_speed_sp().
float time_of_vel_sp = 0.0 |
Definition at line 220 of file guidance_indi_hybrid.c.
Referenced by vel_sp_cb().
float v_gih[3] |
Definition at line 203 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_calcg_wing().
abi_event vel_sp_ev |
Definition at line 217 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_init().