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 "filters/quaternion_filter.h"
#include "modules/core/abi.h"
#include "modules/datalink/telemetry.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.

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.

◆ GUIDANCE_INDI_FILTER_CUTOFF

#define GUIDANCE_INDI_FILTER_CUTOFF 3.0

Definition at line 101 of file guidance_indi.c.

Function Documentation

◆ accel_sp_cb()

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_indi_controller()

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
	)

Definition at line 352 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_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.

Referenced by guidance_indi_run_mode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_indi_enter()

void guidance_indi_enter ( void )

Call upon entering indi guidance.

Definition at line 214 of file guidance_indi.c.

References Stabilization::cmd, filt_accel_ned, filter_cutoff, foo, RotorcraftNavigation::heading, init_butterworth_2_low_pass(), init_quat_butterworth_low_pass(), nav, FloatEulers::psi, quat_filt, stabilization, stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), thrust_act, thrust_dyn, thrust_filt, and thrust_in.

Here is the call graph for this function:

◆ guidance_indi_init()

void guidance_indi_init ( void )

Init function.

Definition at line 199 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.

Here is the call graph for this function:

◆ 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 456 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()

struct StabilizationSetpoint guidance_indi_run	(	struct FloatVect3 *	accel_sp,
		float	heading_sp
	)

Parameters

accel_sp	accel setpoint in NED frame [m/s^2]
heading_sp	the desired heading [rad]

Returns: stabilization setpoint structure

main indi guidance function

Definition at line 246 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, Gmat, guidance_euler_cmd, guidance_indi_calcG(), guidance_indi_max_bank, GUIDANCE_INDI_NU, GUIDANCE_INDI_NV, guidance_indi_propagate_filters(), guidance_indi_set_wls_settings(), guidance_indi_specific_force_gain, MAT33_INV, MAT33_VECT3_MUL, MAX_PPRZ, SecondOrderLowPass::o, FloatEulers::phi, FloatEulers::psi, QuatSecondOrderLowPass::quat, quat_filt, RMAT_ELMT, sp_accel, stab_sp_from_quat_f(), th_sp_from_incr_vect_f(), th_sp_from_thrust_i(), FloatEulers::theta, THRUST_AXIS_Z, thrust_filt, thrust_in, thrust_sp, thrust_vect, wls_alloc(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by guidance_indi_run_mode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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
	)