#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.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 "stdio.h"
#include "filters/low_pass_filter.h"
#include "subsystems/abi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/navigation.h"

Include dependency graph for guidance_indi_hybrid.c:

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	MAX_DECELERATION 1.

#define	GUIDANCE_INDI_FILTER_CUTOFF 3.0

#define	GUIDANCE_INDI_PITCH_EFF_SCALING 1.0

Functions
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...

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

struct FloatVect3	sp_accel = {0.0,0.0,0.0}

float	guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED

float	inv_eff [4]

float	lift_pitch_eff = GUIDANCE_INDI_PITCH_LIFT_EFF

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	speed_sp = {0.0, 0.0, 0.0}

Detailed Description

A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to IROS2016 to learn more!

Definition in file guidance_indi_hybrid.c.

Macro Definition Documentation

◆ GUIDANCE_INDI_FILTER_CUTOFF

#define GUIDANCE_INDI_FILTER_CUTOFF 3.0

Definition at line 100 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_PITCH_EFF_SCALING

#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0

◆ GUIDANCE_INDI_POS_GAIN

#define GUIDANCE_INDI_POS_GAIN = 0.5;

Definition at line 60 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_POS_GAINZ

#define GUIDANCE_INDI_POS_GAINZ = 0.5;

Definition at line 61 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_SPEED_GAIN

#define GUIDANCE_INDI_SPEED_GAIN 1.8

Definition at line 55 of file guidance_indi_hybrid.c.

◆ GUIDANCE_INDI_SPEED_GAINZ

#define GUIDANCE_INDI_SPEED_GAINZ 1.8

Definition at line 56 of file guidance_indi_hybrid.c.

◆ MAX_DECELERATION

#define MAX_DECELERATION 1.

Definition at line 78 of file guidance_indi_hybrid.c.

Function Documentation

◆ guidance_indi_calcg_wing()

void guidance_indi_calcg_wing ( struct FloatMat33 * Gmat )

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

Parameters

Gmat	array to write the matrix to [3x3]

Definition at line 441 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().

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 165 of file guidance_indi_hybrid.c.

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

Here is the call graph for this function:

◆ guidance_indi_get_liftd()

float guidance_indi_get_liftd	(	float	airspeed,
		float	theta
	)

static

Get the derivative of lift w.r.t.

pitch.

Parameters

airspeed The airspeed says most about the flight condition

Returns: The derivative of lift w.r.t. pitch

Definition at line 483 of file guidance_indi_hybrid.c.

References lift_pitch_eff.

Referenced by guidance_indi_calcg_wing().

Here is the caller graph for this function:

◆ guidance_indi_init()

void guidance_indi_init ( void )

Init function.

Definition at line 146 of file guidance_indi_hybrid.c.

References accel_sp_cb(), accel_sp_ev, accely_filt, filt_accel_ned, filter_cutoff, GUIDANCE_INDI_ACCEL_SP_ID, init_butterworth_2_low_pass(), pitch_filt, roll_filt, and thrust_filt.

Here is the call graph for this function:

◆ guidance_indi_propagate_filters()

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_indi_run()

void guidance_indi_run ( float * heading_sp )

Parameters

heading_sp the desired heading [rad]

main indi guidance function

Definition at line 176 of file guidance_indi_hybrid.c.

References accely_filt, ANGLE_BFP_OF_REAL, AP_MODE_NAV, autopilot, BFP_OF_REAL, control_increment, desired_airspeed, euler_cmd, eulers_zxy, filt_accel_ned, FLOAT_ANGLE_NORMALIZE, float_eulers_of_quat_yxz(), float_eulers_of_quat_zxy(), float_quat_normalize(), float_quat_of_eulers_yxz(), float_quat_of_eulers_zxy(), FLOAT_VECT2_NORM, Ga, Ga_inv, get_sys_time_float(), gih_params, guidance_euler_cmd, guidance_h, GUIDANCE_H_MAX_BANK, guidance_indi_calcg_wing(), guidance_indi_calcG_yxz(), guidance_indi_max_airspeed, guidance_indi_max_bank, guidance_indi_pos_gain, guidance_indi_propagate_filters(), guidance_indi_speed_gain, guidance_v_z_ref, indi_accel_sp, indi_accel_sp_set_2d, indi_accel_sp_set_3d, INT32_ANGLE_FRAC, INT32_PERCENTAGE_FRAC, INT_MULT_RSHIFT, MAT33_INV, MAT33_VECT3_MUL, Min, pprz_autopilot::mode, nav_get_speed_setpoint(), 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, speed_sp, stab_att_sp_quat, stabilization_cmd, stateGetAirspeed_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetPositionNed_i(), stateGetSpeedNed_f(), FloatEulers::theta, thrust_filt, thrust_in, THRUST_INCREMENT_ID, time_of_accel_sp_2d, time_of_accel_sp_3d, transition_percentage, transition_theta_offset, RadioControl::values, VECT2_DIFF, vect_bound_in_2d(), FloatVect2::x, FloatVect3::x, NedCoor_f::x, Int32Vect2::x, FloatVect2::y, FloatVect3::y, NedCoor_f::y, Int32Vect2::y, FloatVect3::z, and NedCoor_f::z.

Here is the call graph for this function:

◆ nav_get_speed_setpoint()

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.

Returns: desired speed setpoint FloatVect3

Definition at line 506 of file guidance_indi_hybrid.c.

References horizontal_mode, HORIZONTAL_MODE_ROUTE, nav_get_speed_sp_from_go(), nav_get_speed_sp_from_line(), navigation_target, speed_sp, and to_end_vect.

Referenced by guidance_indi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_get_speed_sp_from_go()

struct FloatVect3 nav_get_speed_sp_from_go	(	struct EnuCoor_i	target,
		float	pos_gain
	)

Go to a waypoint in the shortest way.

Parameters

target the target waypoint

Returns: desired speed FloatVect3

Definition at line 611 of file guidance_indi_hybrid.c.

References FLOAT_VECT2_NORM, force_forward, gih_params, guidance_v_mode, GUIDANCE_V_MODE_NAV, guidance_v_zd_sp, MAX_DECELERATION, Min, nav_climb_vspeed, nav_descend_vspeed, nav_max_speed, POS_FLOAT_OF_BFP, guidance_indi_hybrid_params::pos_gainz, SPEED_FLOAT_OF_BFP, stateGetAirspeed_f(), stateGetPositionNed_f(), target, VECT3_ASSIGN, VECT3_DIFF, VECT3_SMUL, vect_bound_in_2d(), vect_scale(), vertical_mode, VERTICAL_MODE_CLIMB, FloatVect2::x, FloatVect2::y, and FloatVect3::z.

Referenced by nav_get_speed_setpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_get_speed_sp_from_line()

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.

Parameters

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

Returns: desired speed setpoint FloatVect3

Definition at line 525 of file guidance_indi_hybrid.c.

References direction, float_vect2_norm(), force_forward, gih_params, guidance_v_mode, GUIDANCE_V_MODE_NAV, guidance_v_zd_sp, nav_climb_vspeed, nav_descend_vspeed, nav_max_speed, POS_FLOAT_OF_BFP, guidance_indi_hybrid_params::pos_gainz, SPEED_FLOAT_OF_BFP, stateGetAirspeed_f(), stateGetPositionNed_f(), target, VECT2_ADD, VECT2_ASSIGN, VECT2_SMUL, VECT3_ASSIGN, vertical_mode, VERTICAL_MODE_CLIMB, FloatVect2::x, FloatVect2::y, FloatVect3::z, and NedCoor_f::z.

Referenced by nav_get_speed_setpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ accely_filt

Butterworth2LowPass accely_filt

Definition at line 121 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init(), guidance_indi_propagate_filters(), and guidance_indi_run().

◆ desired_airspeed

struct FloatVect2 desired_airspeed

Definition at line 123 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ euler_cmd

struct FloatVect3 euler_cmd

Definition at line 127 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ eulers_zxy

struct FloatEulers eulers_zxy

state eulers in zxy order

Definition at line 114 of file guidance_indi_hybrid.c.

Referenced by ctrl_eff_scheduling_periodic_b(), guidance_indi_calcg_wing(), guidance_indi_propagate_filters(), guidance_indi_run(), and send_fp().

◆ filt_accel_ned

Butterworth2LowPass filt_accel_ned[3]

Definition at line 117 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init(), guidance_indi_propagate_filters(), and guidance_indi_run().

◆ filter_cutoff

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 129 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_init().

◆ Ga

struct FloatMat33 Ga

Definition at line 125 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ Ga_inv

struct FloatMat33 Ga_inv

Definition at line 126 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ gih_params

struct guidance_indi_hybrid_params gih_params

Initial value:

= {
  .pos_gain = GUIDANCE_INDI_POS_GAIN,
  .pos_gainz = GUIDANCE_INDI_POS_GAINZ,
 
  .speed_gain = GUIDANCE_INDI_SPEED_GAIN,
  .speed_gainz = GUIDANCE_INDI_SPEED_GAINZ,
}