guidance_indi.c File Reference

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

#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
#include "subsystems/ins/ins_int.h"
#include "subsystems/radio_control.h"
#include "state.h"
#include "subsystems/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 "filters/low_pass_filter.h"
#include "subsystems/abi.h"

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. More...
static void	guidance_indi_propagate_filters (struct FloatEulers *eulers)
	Low pass the accelerometer measurements to remove noise from vibrations. More...
static void	guidance_indi_calcG (struct FloatMat33 *Gmat)
static void	guidance_indi_calcG_yxz (struct FloatMat33 *Gmat, struct FloatEulers *euler_yxz)
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
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.0, 0.0, 0.0}
bool	indi_accel_sp_set_2d = false
bool	indi_accel_sp_set_3d = false
struct FloatVect3	sp_accel = {0.0, 0.0, 0.0}
float	thrust_act = 0
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
float	thrust_in

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

#define GUIDANCE_INDI_ACCEL_SP_ID   ABI_BROADCAST

Definition at line 68 of file guidance_indi.c.

Referenced by guidance_indi_init().

#define GUIDANCE_INDI_FILTER_CUTOFF   3.0

Definition at line 95 of file guidance_indi.c.

Function Documentation

static 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 362 of file guidance_indi.c.

References get_sys_time_float(), 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:

UNUSED void guidance_indi_calcG ( struct FloatMat33 *  Gmat )

static

Parameters

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 333 of file guidance_indi.c.

References FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, stateGetNedToBodyEulers_f(), mesonh.mesonh_atmosphere::T, and FloatEulers::theta.

Here is the call graph for this function:

void guidance_indi_calcG_yxz ( struct FloatMat33 *  Gmat, struct FloatEulers *  euler_yxz  )

static

Parameters

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 305 of file guidance_indi.c.

References FloatEulers::phi, RMAT_ELMT, mesonh.mesonh_atmosphere::T, and FloatEulers::theta.

Referenced by guidance_indi_run().

Here is the caller graph for this function:

void guidance_indi_enter

(

void

)

Call upon entering indi guidance.

Definition at line 134 of file guidance_indi.c.

References filter_cutoff, init_butterworth_2_low_pass(), FloatEulers::phi, stabilization_cmd, stateGetNedToBodyEulers_f(), FloatEulers::theta, thrust_act, and thrust_in.

Referenced by guidance_h_mode_changed().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_indi_init

(

void

)

Init function.

Definition at line 125 of file guidance_indi.c.

References accel_sp_cb(), and GUIDANCE_INDI_ACCEL_SP_ID.

Here is the call graph for this function:

void guidance_indi_propagate_filters ( struct FloatEulers *  eulers )

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 287 of file guidance_indi.c.

References FloatEulers::phi, stateGetAccelNed_f(), FloatEulers::theta, update_butterworth_2_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:

void guidance_indi_run

(

float

heading_sp

)

Parameters

heading_sp

the desired heading [rad]

main indi guidance function

Definition at line 154 of file guidance_indi.c.

References control_increment, float_eulers_of_quat_yxz(), float_quat_of_eulers_yxz(), Ga, Ga_inv, get_sys_time_float(), guidance_euler_cmd, guidance_h, guidance_indi_calcG_yxz(), guidance_indi_max_bank, guidance_indi_pos_gain, guidance_indi_propagate_filters(), guidance_indi_speed_gain, guidance_v_z_ref, indi_accel_sp_set_2d, indi_accel_sp_set_3d, MAT33_INV, MAT33_VECT3_MUL, SecondOrderLowPass::o, FloatEulers::phi, HorizontalGuidanceReference::pos, POS_FLOAT_OF_BFP, FloatEulers::psi, QUAT_BFP_OF_REAL, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, HorizontalGuidance::ref, stab_att_sp_quat, stabilization_cmd, stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetPositionNed_i(), stateGetSpeedNed_f(), FloatEulers::theta, thrust_in, THRUST_INCREMENT_ID, time_of_accel_sp_2d, time_of_accel_sp_3d, RadioControl::values, FloatVect3::x, NedCoor_f::x, Int32Vect2::x, FloatVect3::y, NedCoor_f::y, Int32Vect2::y, FloatVect3::z, and NedCoor_f::z.

Referenced by guidance_h_from_nav(), and guidance_h_guided_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

abi_event accel_sp_ev

Definition at line 70 of file guidance_indi.c.

struct FloatVect3 control_increment

Definition at line 107 of file guidance_indi.c.

Referenced by guidance_indi_run().

Butterworth2LowPass filt_accel_ned[3]

Definition at line 100 of file guidance_indi.c.

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 109 of file guidance_indi.c.

Referenced by guidance_indi_enter().

struct FloatMat33 Ga

Definition at line 105 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatMat33 Ga_inv

Definition at line 106 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatEulers guidance_euler_cmd

Definition at line 115 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK

Definition at line 110 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_pos_gain = 0.5

Definition at line 58 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_speed_gain = 1.8

Definition at line 64 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatVect3 indi_accel_sp = {0.0, 0.0, 0.0}

Definition at line 72 of file guidance_indi.c.

bool indi_accel_sp_set_2d = false

Definition at line 73 of file guidance_indi.c.

Referenced by accel_sp_cb(), and guidance_indi_run().

bool indi_accel_sp_set_3d = false

Definition at line 74 of file guidance_indi.c.

Referenced by accel_sp_cb(), and guidance_indi_run().

Butterworth2LowPass pitch_filt

Definition at line 102 of file guidance_indi.c.

Butterworth2LowPass roll_filt

Definition at line 101 of file guidance_indi.c.

struct FloatVect3 sp_accel = {0.0, 0.0, 0.0}

Definition at line 76 of file guidance_indi.c.

float thrust_act = 0

Definition at line 99 of file guidance_indi.c.

Referenced by guidance_indi_enter().

Butterworth2LowPass thrust_filt

Definition at line 103 of file guidance_indi.c.

float thrust_in

Definition at line 116 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_run().

float time_of_accel_sp_2d = 0.0

Definition at line 112 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 113 of file guidance_indi.c.

Referenced by accel_sp_cb(), and guidance_indi_run().