Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
guidance_indi.c File Reference

A guidance mode based on Incremental Nonlinear Dynamic Inversion. More...

Include dependency graph for guidance_indi.c:

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 (void)
 Low pass the accelerometer measurements to remove noise from vibrations.
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 FloatVect3 WEAK guidance_indi_controller (bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
struct StabilizationSetpoint guidance_indi_run_mode (bool in_flight, 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 thrust_filt
QuatButterworthLowPass quat_filt
static float Gmat [GUIDANCE_INDI_NV][GUIDANCE_INDI_NU]
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
float thrust_vect [3]

Detailed Description

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.

Macro Definition Documentation

◆ GUIDANCE_INDI_ACCEL_SP_ID

#define GUIDANCE_INDI_ACCEL_SP_ID   ABI_BROADCAST

Definition at line 64 of file guidance_indi.c.

Referenced by guidance_indi_init().

◆ GUIDANCE_INDI_FILTER_CUTOFF

#define GUIDANCE_INDI_FILTER_CUTOFF   3.0

Definition at line 94 of file guidance_indi.c.

Function Documentation

◆ accel_sp_cb()

void accel_sp_cb ( uint8_t sender_id,
uint8_t flag,
struct FloatVect3 * accel_sp )
static

ABI callback that obtains the acceleration setpoint from telemetry flag: 0 -> 2D, 1 -> 3D.

Definition at line 458 of file guidance_indi.c.

References 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().

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

◆ guidance_indi_controller()

◆ guidance_indi_enter()

void guidance_indi_enter ( void )

◆ guidance_indi_init()

void guidance_indi_init ( void )

◆ guidance_indi_propagate_filters()

void guidance_indi_propagate_filters ( void )
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 445 of file guidance_indi.c.

References filt_accel_ned, quat_filt, stateGetAccelNed_f(), stateGetNedToBodyQuat_f(), update_butterworth_2_low_pass(), update_quat_butterworth_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()

◆ guidance_indi_run_mode()

struct StabilizationSetpoint guidance_indi_run_mode ( bool in_flight in_flight ,
struct HorizontalGuidance * gh,
struct VerticalGuidance * gv,
enum GuidanceIndi_HMode h_mode,
enum GuidanceIndi_VMode v_mode )

◆ send_indi_guidance()

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

Definition at line 165 of file guidance_indi.c.

References control_increment, dev, filt_accel_ned, sp_accel, and speed_sp.

Referenced by guidance_indi_init().

Here is the caller graph for this function:

Variable Documentation

◆ accel_sp_ev

abi_event accel_sp_ev

Definition at line 66 of file guidance_indi.c.

Referenced by guidance_indi_init().

◆ control_increment

struct FloatVect3 control_increment

Definition at line 144 of file guidance_indi.c.

Referenced by guidance_indi_run(), and send_indi_guidance().

◆ filt_accel_ned

◆ filter_cutoff

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 148 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_init_filters().

◆ Ga

struct FloatMat33 Ga

◆ Ga_inv

struct FloatMat33 Ga_inv

Definition at line 143 of file guidance_indi.c.

Referenced by guidance_indi_run().

◆ Gmat

◆ guidance_euler_cmd

struct FloatEulers guidance_euler_cmd

Definition at line 154 of file guidance_indi.c.

Referenced by guidance_indi_init(), and guidance_indi_run().

◆ guidance_indi_max_bank

◆ guidance_indi_pos_gain

float guidance_indi_pos_gain = 0.5

Definition at line 54 of file guidance_indi.c.

Referenced by guidance_indi_calcG(), and guidance_indi_controller().

◆ guidance_indi_speed_gain

float guidance_indi_speed_gain = 1.8

◆ indi_accel_sp

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_mode().

◆ indi_accel_sp_set_2d

bool indi_accel_sp_set_2d = false

Definition at line 69 of file guidance_indi.c.

Referenced by accel_sp_cb(), and guidance_indi_run_mode().

◆ indi_accel_sp_set_3d

bool indi_accel_sp_set_3d = false

Definition at line 70 of file guidance_indi.c.

Referenced by accel_sp_cb(), and guidance_indi_run_mode().

◆ quat_filt

◆ sp_accel

struct FloatVect3 sp_accel = {0.0f, 0.0f, 0.0f}

◆ speed_sp

◆ thrust_act

float thrust_act = 0.f

Definition at line 99 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_init_filters().

◆ thrust_dyn

float thrust_dyn = 0.f

Definition at line 98 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_init().

◆ thrust_filt

◆ thrust_in

float thrust_in

◆ thrust_sp

◆ thrust_vect

float thrust_vect[3]

Definition at line 157 of file guidance_indi.c.

Referenced by guidance_indi_run(), and quat_from_earth_cmd_f().

◆ time_of_accel_sp_2d

float time_of_accel_sp_2d = 0.0

Definition at line 151 of file guidance_indi.c.

Referenced by accel_sp_cb(), and guidance_indi_run_mode().

◆ time_of_accel_sp_3d

float time_of_accel_sp_3d = 0.0

Definition at line 152 of file guidance_indi.c.

Referenced by accel_sp_cb(), and guidance_indi_run_mode().