![]() |
Paparazzi UAS
v6.2.0_stable
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/ins/ins_int.h"
#include "modules/radio_control/radio_control.h"
#include "state.h"
#include "modules/imu/imu.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.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 "firmwares/rotorcraft/stabilization.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_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 |
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_FILTER_CUTOFF 3.0 |
Definition at line 144 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_LIFTD_ASQ 0.20 |
Definition at line 70 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2) |
Definition at line 79 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12) |
Definition at line 78 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_MAX_PITCH 25 |
Definition at line 66 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_MIN_PITCH -120 |
Definition at line 65 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_NAV_SPEED_MARGIN 10.0 |
Definition at line 108 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 GUIDANCE_INDI_VEL_SP_ID ABI_BROADCAST |
Definition at line 183 of file guidance_indi_hybrid.c.
#define GUIDANCE_INDI_ZERO_AIRSPEED FALSE |
Definition at line 103 of file guidance_indi_hybrid.c.
#define MAX_DECELERATION 1. |
Definition at line 114 of file guidance_indi_hybrid.c.
#define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN) |
Definition at line 110 of file guidance_indi_hybrid.c.
#define TURN_AIRSPEED_TH 10.0 |
Definition at line 119 of file guidance_indi_hybrid.c.
|
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
Gmat | array 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().
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.
|
static |
Get the derivative of lift w.r.t.
pitch.
airspeed | The airspeed says most about the flight condition |
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().
void guidance_indi_init | ( | void | ) |
Init function.
Definition at line 220 of file guidance_indi_hybrid.c.
References accely_filt, DefaultPeriodic, filt_accel_ned, filter_cutoff, GUIDANCE_INDI_VEL_SP_ID, init_butterworth_2_low_pass(), pitch_filt, register_periodic_telemetry(), roll_filt, send_guidance_indi_hybrid(), 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 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().
void guidance_indi_run | ( | float * | heading_sp | ) |
heading_sp | the 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.
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.
Definition at line 614 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
struct FloatVect3 nav_get_speed_sp_from_go | ( | struct EnuCoor_i | target, |
float | pos_gain | ||
) |
Go to a waypoint in the shortest way.
target | the target waypoint |
Definition at line 614 of file guidance_indi_hybrid.c.
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.
line_v_enu | 2d vector from beginning (0) line to end in enu |
to_end_v_enu | 2d vector from current position to end in enu |
target | end waypoint in enu |
Definition at line 614 of file guidance_indi_hybrid.c.
|
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().
|
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().
Butterworth2LowPass accely_filt |
Definition at line 167 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_enter(), guidance_indi_init(), guidance_indi_propagate_filters(), and guidance_indi_run().
struct FloatVect2 desired_airspeed |
Definition at line 167 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
struct FloatVect3 euler_cmd |
Definition at line 167 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run(), and send_guidance_indi_hybrid().
struct FloatEulers eulers_zxy |
state eulers in zxy order
Definition at line 157 of file guidance_indi_hybrid.c.
Referenced by ctrl_eff_scheduling_periodic_b(), guidance_indi_calcg_wing(), guidance_indi_propagate_filters(), guidance_indi_run(), schdule_control_effectiveness(), and send_fp().
Butterworth2LowPass filt_accel_ned[3] |
Definition at line 163 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_enter(), guidance_indi_init(), guidance_indi_propagate_filters(), guidance_indi_run(), and send_guidance_indi_hybrid().
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().
struct FloatMat33 Ga |
Definition at line 167 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
struct FloatMat33 Ga_inv |
Definition at line 167 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
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().
struct guidance_indi_hybrid_params gih_params |
Definition at line 1 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_get_liftd(), and guidance_indi_run().
struct FloatEulers guidance_euler_cmd |
Definition at line 175 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
float guidance_indi_line_gain = 1.0 |
Definition at line 151 of file guidance_indi_hybrid.c.
float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED |
Definition at line 98 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK |
Definition at line 157 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
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().
float inv_eff[4] |
Definition at line 154 of file guidance_indi_hybrid.c.
float nav_max_speed = NAV_MAX_SPEED |
Definition at line 111 of file guidance_indi_hybrid.c.
Butterworth2LowPass pitch_filt |
Definition at line 165 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_enter(), guidance_indi_init(), guidance_indi_propagate_filters(), and guidance_indi_run().
Butterworth2LowPass roll_filt |
Definition at line 164 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_enter(), guidance_indi_init(), guidance_indi_propagate_filters(), and guidance_indi_run().
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().
bool take_heading_control = false |
Definition at line 123 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
float thrust_act = 0 |
Definition at line 162 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_enter().
Butterworth2LowPass thrust_filt |
Definition at line 166 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_enter(), guidance_indi_init(), and guidance_indi_run().
float thrust_in |
Definition at line 178 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_enter(), and guidance_indi_run().
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().
abi_event vel_sp_ev |
Definition at line 185 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_init().