A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more! More...

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "filters/high_pass_filter.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"

Include dependency graph for guidance_indi_hybrid.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	guidance_indi_hybrid_params

Enumerations
enum	GuidanceIndiHybrid_HMode { GUIDANCE_INDI_HYBRID_H_POS , GUIDANCE_INDI_HYBRID_H_SPEED , GUIDANCE_INDI_HYBRID_H_ACCEL }

enum	GuidanceIndiHybrid_VMode { GUIDANCE_INDI_HYBRID_V_POS , GUIDANCE_INDI_HYBRID_V_SPEED , GUIDANCE_INDI_HYBRID_V_ACCEL }

Functions
void	guidance_indi_init (void)
	Init function.

void	guidance_indi_enter (void)
	Call upon entering indi guidance.

float	guidance_indi_get_liftd (float pitch, float theta)

void	guidance_indi_calcg_wing (float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V])
	Perform WLS.

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

struct StabilizationSetpoint	guidance_indi_run_mode (bool in_flight, struct HorizontalGuidance gh, struct VerticalGuidance gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode)

void	guidance_set_min_max_airspeed (float min_airspeed, float max_airspeed)

Variables
struct FloatVect3	sp_accel

struct FloatVect3	gi_speed_sp

struct guidance_indi_hybrid_params	gih_params

float	guidance_indi_pitch_pref_deg

float	gi_unbounded_airspeed_sp

float	guidance_indi_thrust_z_eff

float	guidance_indi_specific_force_gain

bool	take_heading_control

float	guidance_indi_max_bank

float	guidance_indi_min_pitch

bool	force_forward
	forward flight for hybrid nav

bool	guidance_indi_airspeed_filtering

bool	coordinated_turn_use_accel

Detailed Description

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

Definition in file guidance_indi_hybrid.h.

Data Structure Documentation

◆ guidance_indi_hybrid_params

struct guidance_indi_hybrid_params

Definition at line 75 of file guidance_indi_hybrid.h.

Data Fields
float	climb_vspeed_fwd
float	climb_vspeed_quad
float	descend_vspeed_fwd
float	descend_vspeed_quad
float	heading_bank_gain
float	liftd_asq
float	liftd_p50
float	liftd_p80
float	max_airspeed
float	min_airspeed
float	pos_gain
float	pos_gainz
float	speed_gain
float	speed_gainz
float	stall_protect_gain

Enumeration Type Documentation

◆ GuidanceIndiHybrid_HMode

enum GuidanceIndiHybrid_HMode

Enumerator
GUIDANCE_INDI_HYBRID_H_POS
GUIDANCE_INDI_HYBRID_H_SPEED
GUIDANCE_INDI_HYBRID_H_ACCEL

Definition at line 59 of file guidance_indi_hybrid.h.

◆ GuidanceIndiHybrid_VMode

enum GuidanceIndiHybrid_VMode

Enumerator
GUIDANCE_INDI_HYBRID_V_POS
GUIDANCE_INDI_HYBRID_V_SPEED
GUIDANCE_INDI_HYBRID_V_ACCEL

Definition at line 65 of file guidance_indi_hybrid.h.

Function Documentation

◆ guidance_indi_calcg_wing()

void guidance_indi_calcg_wing	(	float	Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U],
		struct FloatVect3	a_diff,
		float	v_gih[GUIDANCE_INDI_HYBRID_V]
	)

extern

Perform WLS.

Parameters

Gmat	Dynamics matrix
a_diff	acceleration errors in earth frame
body_v	3D vector to write the control objective v

Perform WLS.

the NED accelerations, taking into account the lift of a wing that is horizontal at -90 degrees pitch

Parameters

Gmat	Dynamics matrix
a_diff	acceleration errors in earth frame
body_v	3D vector to write the control objective v

Definition at line 80 of file guidance_indi_hybrid_quadplane.c.

References accel_bodyz_filt, eulers_zxy, float_eulers_of_quat_zxy(), foo, guidance_indi_get_liftd(), GUIDANCE_INDI_PITCH_EFF_SCALING, SecondOrderLowPass::o, FloatEulers::phi, FloatEulers::psi, stateGetAirspeed_f(), stateGetNedToBodyQuat_f(), FloatEulers::theta, and v_gih.

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 )

extern

Call upon entering indi guidance.

Definition at line 168 of file guidance_indi.c.

References accely_filt, Stabilization::cmd, eulers_zxy, filt_accel_ned, filter_cutoff, float_eulers_of_quat_zxy(), foo, guidance_indi_airspeed_filt, guidance_indi_airspeed_filt_cutoff, guidance_indi_hybrid_heading_sp, RotorcraftNavigation::heading, init_butterworth_2_low_pass(), nav, FloatEulers::phi, pitch_filt, FloatEulers::psi, roll_filt, stabilization, stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), FloatEulers::theta, thrust_act, thrust_dyn, thrust_filt, and thrust_in.

Here is the call graph for this function:

◆ guidance_indi_get_liftd()

float guidance_indi_get_liftd	(	float pitch	UNUSED,
		float theta	UNUSED
	)

externinline

Definition at line 468 of file eff_scheduling_rotwing.c.

References eff_scheduling_rotwing_lift_d.

◆ guidance_indi_init()

void guidance_indi_init ( void )

extern

Init function.

Definition at line 153 of file guidance_indi.c.

References accel_sp_cb(), accel_sp_ev, accely_filt, DefaultPeriodic, filt_accel_ned, filter_cutoff, FLOAT_EULERS_ZERO, foo, Ga, guidance_euler_cmd, GUIDANCE_INDI_ACCEL_SP_ID, guidance_indi_airspeed_filt, guidance_indi_airspeed_filt_cutoff, GUIDANCE_INDI_VEL_SP_ID, init_butterworth_2_low_pass(), pitch_filt, register_periodic_telemetry(), roll_filt, send_eff_mat_guid_indi_hybrid(), send_guidance_indi_hybrid(), send_indi_guidance(), thrust_dyn, thrust_filt, thrust_sp, THRUST_SP_SET_ZERO, vel_sp_cb(), and vel_sp_ev.

Here is the call graph for this function:

◆ guidance_indi_run()

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

extern

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

References accely_filt, control_increment, du_gih, euler_cmd, eulers_zxy, filt_accel_ned, FLOAT_ANGLE_NORMALIZE, float_eulers_of_quat_yxz(), float_eulers_of_quat_zxy(), float_mat3_mult(), float_mat_inv_3d(), float_quat_normalize(), float_quat_of_eulers_yxz(), float_quat_of_eulers_zxy(), foo, g1g2, Ga, Ga_inv, get_sys_time_float(), gih_coordinated_turn_max_airspeed, gih_coordinated_turn_min_airspeed, guidance_euler_cmd, guidance_indi_calcg_wing(), guidance_indi_calcG_yxz(), guidance_indi_hybrid_heading_sp, guidance_indi_hybrid_set_wls_settings(), guidance_indi_max_bank, GUIDANCE_INDI_MAX_PITCH, GUIDANCE_INDI_MAX_PUSHER_INCREMENT, guidance_indi_min_pitch, guidance_indi_propagate_filters(), guidance_indi_specific_force_gain, guidance_indi_speed_gain, heading, indi_accel_sp, indi_accel_sp_set_2d, indi_accel_sp_set_3d, MAT33_INV, MAT33_VECT3_MUL, SecondOrderLowPass::o, FloatRates::p, 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(), stab_sp_from_quat_ff_rates_f(), stabilization, stabilization_attitude_get_heading_f(), stateGetAirspeed_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), stateGetSpeedNed_f(), take_heading_control, 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, Stabilization::transition_ratio, v_gih, RadioControl::values, VECT3_DIFF, wls_alloc(), FloatVect3::x, FloatVect3::y, FloatVect3::z, and NedCoor_f::z.

Referenced by guidance_indi_run_mode(), and 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,
		struct HorizontalGuidance *	gh,
		struct VerticalGuidance *	gv,
		enum GuidanceIndiHybrid_HMode	h_mode,
		enum GuidanceIndiHybrid_VMode	v_mode
	)

extern

Definition at line 760 of file guidance_indi_hybrid.c.

References ACCEL_FLOAT_OF_BFP, bound_vz_sp(), compute_accel_from_speed_sp(), foo, get_sys_time_float(), gi_speed_sp, gih_params, GUIDANCE_INDI_HYBRID_H_POS, GUIDANCE_INDI_HYBRID_H_SPEED, GUIDANCE_INDI_HYBRID_V_ACCEL, GUIDANCE_INDI_HYBRID_V_POS, GUIDANCE_INDI_HYBRID_V_SPEED, guidance_indi_run(), indi_vel_sp, POS_FLOAT_OF_BFP, guidance_indi_hybrid_params::pos_gain, guidance_indi_hybrid_params::pos_gainz, SPEED_FLOAT_OF_BFP, guidance_indi_hybrid_params::speed_gain, guidance_indi_hybrid_params::speed_gainz, stateGetPositionNed_f(), stateGetSpeedNed_f(), time_of_vel_sp, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Here is the call graph for this function:

◆ guidance_set_min_max_airspeed()

void guidance_set_min_max_airspeed	(	float	min_airspeed,
		float	max_airspeed
	)

extern

Definition at line 414 of file guidance_indi_hybrid.c.

Variable Documentation

◆ coordinated_turn_use_accel

bool coordinated_turn_use_accel

extern

Definition at line 229 of file guidance_indi_hybrid.c.

◆ force_forward

bool force_forward

extern

forward flight for hybrid nav

Definition at line 160 of file guidance_indi_hybrid.c.

◆ gi_speed_sp

struct FloatVect3 gi_speed_sp

extern

Definition at line 289 of file guidance_indi_hybrid.c.

Referenced by compute_accel_from_speed_sp(), guidance_indi_run_mode(), and send_guidance_indi_hybrid().

◆ gi_unbounded_airspeed_sp

float gi_unbounded_airspeed_sp

extern

Definition at line 244 of file guidance_indi_hybrid.c.

◆ gih_params

struct guidance_indi_hybrid_params gih_params

extern

Definition at line 110 of file guidance_indi_hybrid.c.

◆ guidance_indi_airspeed_filtering

bool guidance_indi_airspeed_filtering

extern

Definition at line 162 of file guidance_indi_hybrid.c.

Referenced by compute_accel_from_speed_sp().

◆ guidance_indi_max_bank

float guidance_indi_max_bank

extern

Definition at line 117 of file guidance_indi.c.

Referenced by guidance_indi_hybrid_set_wls_settings(), and guidance_indi_run().

◆ guidance_indi_min_pitch

float guidance_indi_min_pitch

extern

Definition at line 219 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

◆ guidance_indi_pitch_pref_deg

float guidance_indi_pitch_pref_deg

extern

Definition at line 131 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_hybrid_set_wls_settings().

◆ guidance_indi_specific_force_gain

float guidance_indi_specific_force_gain

extern

◆ guidance_indi_thrust_z_eff

float guidance_indi_thrust_z_eff

extern

Referenced by guidance_indi_hybrid_set_wls_settings().

◆ sp_accel

struct FloatVect3 sp_accel

extern

Definition at line 74 of file guidance_indi.c.

Referenced by guidance_indi_run(), send_guidance_indi_hybrid(), and send_indi_guidance().

◆ take_heading_control

bool take_heading_control

extern

Definition at line 158 of file guidance_indi_hybrid.c.

Referenced by guidance_indi_run().

Data Structures

Enumerations

Functions

Variables

Detailed Description

Data Structure Documentation

◆ guidance_indi_hybrid_params

Enumeration Type Documentation

◆ GuidanceIndiHybrid_HMode

◆ GuidanceIndiHybrid_VMode

Function Documentation

◆ guidance_indi_calcg_wing()

◆ guidance_indi_enter()

◆ guidance_indi_get_liftd()

◆ guidance_indi_init()

◆ guidance_indi_run()

◆ guidance_indi_run_mode()

◆ guidance_set_min_max_airspeed()

Variable Documentation

◆ coordinated_turn_use_accel

◆ force_forward

◆ gi_speed_sp

◆ gi_unbounded_airspeed_sp

◆ gih_params

◆ guidance_indi_airspeed_filtering

◆ guidance_indi_max_bank

◆ guidance_indi_min_pitch

◆ guidance_indi_pitch_pref_deg

◆ guidance_indi_specific_force_gain

◆ guidance_indi_thrust_z_eff

◆ sp_accel

◆ take_heading_control