![]() |
Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
A guidance mode based on Incremental Nonlinear Dynamic Inversion. More...
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "mcu_periph/sys_time.h"
#include "state.h"
#include "autopilot.h"
#include "filters/low_pass_filter.h"
#include "modules/core/abi.h"
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Macros | |
#define | GUIDANCE_INDI_ACCEL_SP_ID ABI_BROADCAST |
#define | GUIDANCE_INDI_FILTER_CUTOFF 3.0 |
Functions | |
static void | accel_sp_cb (uint8_t sender_id, uint8_t flag, struct FloatVect3 *accel_sp) |
ABI callback that obtains the acceleration setpoint from telemetry flag: 0 -> 2D, 1 -> 3D. | |
static void | guidance_indi_propagate_filters (struct FloatEulers *eulers) |
Low pass the accelerometer measurements to remove noise from vibrations. | |
static void | guidance_indi_calcG (struct FloatMat33 *Gmat) |
static void | guidance_indi_calcG_yxz (struct FloatMat33 *Gmat, struct FloatEulers *euler_yxz) |
static void | send_indi_guidance (struct transport_tx *trans, struct link_device *dev) |
void | guidance_indi_init (void) |
Init function. | |
void | guidance_indi_enter (void) |
Call upon entering indi guidance. | |
struct StabilizationSetpoint | guidance_indi_run (struct FloatVect3 *accel_sp, float heading_sp) |
struct StabilizationSetpoint | guidance_indi_run_mode (bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode) |
Variables | |
float | guidance_indi_pos_gain = 0.5 |
float | guidance_indi_speed_gain = 1.8 |
abi_event | accel_sp_ev |
struct FloatVect3 | indi_accel_sp = {0.0f, 0.0f, 0.0f} |
bool | indi_accel_sp_set_2d = false |
bool | indi_accel_sp_set_3d = false |
struct FloatVect3 | speed_sp = {0.0f, 0.0f, 0.0f} |
struct FloatVect3 | sp_accel = {0.0f, 0.0f, 0.0f} |
float | thrust_dyn = 0.f |
float | thrust_act = 0.f |
Butterworth2LowPass | filt_accel_ned [3] |
Butterworth2LowPass | roll_filt |
Butterworth2LowPass | pitch_filt |
Butterworth2LowPass | thrust_filt |
struct FloatMat33 | Ga |
struct FloatMat33 | Ga_inv |
struct FloatVect3 | control_increment |
float | filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF |
float | guidance_indi_max_bank = GUIDANCE_H_MAX_BANK |
float | time_of_accel_sp_2d = 0.0 |
float | time_of_accel_sp_3d = 0.0 |
struct FloatEulers | guidance_euler_cmd |
struct ThrustSetpoint | thrust_sp |
float | thrust_in |
A guidance mode based on Incremental Nonlinear Dynamic Inversion.
Based on the papers: Cascaded Incremental Nonlinear Dynamic Inversion Control for MAV Disturbance Rejection https://www.researchgate.net/publication/312907985_Cascaded_Incremental_Nonlinear_Dynamic_Inversion_Control_for_MAV_Disturbance_Rejection
Gust Disturbance Alleviation with Incremental Nonlinear Dynamic Inversion https://www.researchgate.net/publication/309212603_Gust_Disturbance_Alleviation_with_Incremental_Nonlinear_Dynamic_Inversion
Definition in file guidance_indi.c.
#define GUIDANCE_INDI_ACCEL_SP_ID ABI_BROADCAST |
Definition at line 64 of file guidance_indi.c.
#define GUIDANCE_INDI_FILTER_CUTOFF 3.0 |
Definition at line 101 of file guidance_indi.c.
|
static |
ABI callback that obtains the acceleration setpoint from telemetry flag: 0 -> 2D, 1 -> 3D.
Definition at line 460 of file guidance_indi.c.
References foo, get_sys_time_float(), indi_accel_sp, indi_accel_sp_set_2d, indi_accel_sp_set_3d, time_of_accel_sp_2d, time_of_accel_sp_3d, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by guidance_indi_init().
|
static |
Gmat | array to write the matrix to [3x3] |
Calculate the matrix of partial derivatives of the roll, pitch and thrust. w.r.t. the NED accelerations for ZYX eulers ddx = G*[dtheta,dphi,dT]
Definition at line 431 of file guidance_indi.c.
References foo, FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, stateGetNedToBodyEulers_f(), and FloatEulers::theta.
|
static |
Gmat | array to write the matrix to [3x3] |
Calculate the matrix of partial derivatives of the pitch, roll and thrust. w.r.t. the NED accelerations for YXZ eulers ddx = G*[dtheta,dphi,dT]
Definition at line 403 of file guidance_indi.c.
References foo, and RMAT_ELMT.
Referenced by guidance_indi_run().
Call upon entering indi guidance.
Definition at line 168 of file guidance_indi.c.
References Stabilization::cmd, filt_accel_ned, filter_cutoff, foo, RotorcraftNavigation::heading, init_butterworth_2_low_pass(), nav, pitch_filt, FloatEulers::psi, roll_filt, stabilization, stateGetNedToBodyEulers_f(), thrust_act, thrust_dyn, thrust_filt, and thrust_in.
Init function.
Definition at line 153 of file guidance_indi.c.
References accel_sp_cb(), accel_sp_ev, DefaultPeriodic, FLOAT_EULERS_ZERO, foo, guidance_euler_cmd, GUIDANCE_INDI_ACCEL_SP_ID, register_periodic_telemetry(), send_indi_guidance(), thrust_sp, and THRUST_SP_SET_ZERO.
|
static |
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
Definition at line 385 of file guidance_indi.c.
References filt_accel_ned, FloatEulers::phi, pitch_filt, roll_filt, 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 201 of file guidance_indi.c.
References control_increment, filt_accel_ned, float_eulers_of_quat_yxz(), float_quat_of_eulers_yxz(), foo, Ga, Ga_inv, get_sys_time_float(), guidance_euler_cmd, guidance_indi_calcG_yxz(), guidance_indi_max_bank, guidance_indi_propagate_filters(), guidance_indi_specific_force_gain, guidance_indi_speed_gain, indi_accel_sp, indi_accel_sp_set_2d, indi_accel_sp_set_3d, MAT33_INV, MAT33_VECT3_MUL, SecondOrderLowPass::o, FloatEulers::phi, pitch_filt, FloatEulers::psi, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, roll_filt, sp_accel, speed_sp, stab_sp_from_quat_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), stateGetSpeedNed_f(), th_sp_from_incr_vect_f(), th_sp_from_thrust_i(), FloatEulers::theta, THRUST_AXIS_Z, thrust_filt, thrust_in, thrust_sp, time_of_accel_sp_2d, time_of_accel_sp_3d, RadioControl::values, FloatVect3::x, FloatVect3::y, FloatVect3::z, and NedCoor_f::z.
Referenced by guidance_indi_run_mode().
struct StabilizationSetpoint guidance_indi_run_mode | ( | bool in_flight | UNUSED, |
struct HorizontalGuidance * | gh, | ||
struct VerticalGuidance * | gv, | ||
enum GuidanceIndi_HMode | h_mode, | ||
enum GuidanceIndi_VMode | v_mode | ||
) |
Definition at line 314 of file guidance_indi.c.
References ACCEL_FLOAT_OF_BFP, foo, GUIDANCE_INDI_H_ACCEL, GUIDANCE_INDI_H_SPEED, guidance_indi_pos_gain, guidance_indi_run(), guidance_indi_speed_gain, GUIDANCE_INDI_V_ACCEL, GUIDANCE_INDI_V_SPEED, POS_FLOAT_OF_BFP, SPEED_FLOAT_OF_BFP, speed_sp, stateGetPositionNed_f(), stateGetSpeedNed_f(), FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.
|
static |
Definition at line 132 of file guidance_indi.c.
References control_increment, dev, filt_accel_ned, foo, SecondOrderLowPass::o, sp_accel, speed_sp, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by guidance_indi_init().
abi_event accel_sp_ev |
Definition at line 66 of file guidance_indi.c.
Referenced by guidance_indi_init().
struct FloatVect3 control_increment |
Definition at line 114 of file guidance_indi.c.
Referenced by guidance_indi_run(), and send_indi_guidance().
Butterworth2LowPass filt_accel_ned[3] |
Definition at line 107 of file guidance_indi.c.
Referenced by guidance_indi_enter(), guidance_indi_propagate_filters(), guidance_indi_run(), and send_indi_guidance().
float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF |
Definition at line 116 of file guidance_indi.c.
Referenced by guidance_indi_enter().
struct FloatMat33 Ga |
Definition at line 112 of file guidance_indi.c.
Referenced by guidance_indi_run().
struct FloatMat33 Ga_inv |
Definition at line 113 of file guidance_indi.c.
Referenced by guidance_indi_run().
struct FloatEulers guidance_euler_cmd |
Definition at line 122 of file guidance_indi.c.
Referenced by guidance_indi_init(), and guidance_indi_run().
float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK |
Definition at line 117 of file guidance_indi.c.
Referenced by guidance_indi_hybrid_set_wls_settings(), and guidance_indi_run().
float guidance_indi_pos_gain = 0.5 |
Definition at line 54 of file guidance_indi.c.
Referenced by guidance_indi_run_mode().
float guidance_indi_speed_gain = 1.8 |
Definition at line 60 of file guidance_indi.c.
Referenced by guidance_indi_run(), and guidance_indi_run_mode().
struct FloatVect3 indi_accel_sp = {0.0f, 0.0f, 0.0f} |
Definition at line 68 of file guidance_indi.c.
Referenced by accel_sp_cb(), and guidance_indi_run().
Definition at line 69 of file guidance_indi.c.
Referenced by accel_sp_cb(), and guidance_indi_run().
Definition at line 70 of file guidance_indi.c.
Referenced by accel_sp_cb(), and guidance_indi_run().
Butterworth2LowPass pitch_filt |
Definition at line 109 of file guidance_indi.c.
Referenced by guidance_indi_enter(), guidance_indi_propagate_filters(), and guidance_indi_run().
Butterworth2LowPass roll_filt |
Definition at line 108 of file guidance_indi.c.
Referenced by guidance_indi_enter(), guidance_indi_propagate_filters(), and guidance_indi_run().
struct FloatVect3 sp_accel = {0.0f, 0.0f, 0.0f} |
Definition at line 74 of file guidance_indi.c.
Referenced by guidance_indi_run(), and send_indi_guidance().
struct FloatVect3 speed_sp = {0.0f, 0.0f, 0.0f} |
Definition at line 73 of file guidance_indi.c.
Referenced by gh_saturate_speed(), gh_update_ref_from_speed_sp(), guidance_indi_run(), guidance_indi_run_mode(), nav_hybrid_goto(), orange_avoider_guided_periodic(), read_rc_setpoint_speed_i(), rotwing_state_periodic(), and send_indi_guidance().
Definition at line 106 of file guidance_indi.c.
Referenced by guidance_indi_enter().
Definition at line 105 of file guidance_indi.c.
Referenced by guidance_indi_enter().
Butterworth2LowPass thrust_filt |
Definition at line 110 of file guidance_indi.c.
Referenced by guidance_indi_enter(), guidance_indi_run(), and stabilization_indi_rate_run().
float thrust_in |
Definition at line 124 of file guidance_indi.c.
Referenced by guidance_indi_enter(), and guidance_indi_run().
struct ThrustSetpoint thrust_sp |
Definition at line 123 of file guidance_indi.c.
Referenced by autopilot_static_periodic(), guidance_indi_init(), and guidance_indi_run().
float time_of_accel_sp_2d = 0.0 |
Definition at line 119 of file guidance_indi.c.
Referenced by accel_sp_cb(), and guidance_indi_run().
float time_of_accel_sp_3d = 0.0 |
Definition at line 120 of file guidance_indi.c.
Referenced by accel_sp_cb(), and guidance_indi_run().