#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
#include "generated/airframe.h"
#include "filters/low_pass_filter.h"
#include "filters/notch_filter_float.h"

Include dependency graph for oneloop_andi.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

struct	OneloopGuidanceRef

struct	OneloopGuidanceState

struct	OneloopStabilizationRef

struct	OneloopStabilizationState

struct	OneloopGeneral

struct	PolePlacement

struct	Gains3rdOrder

struct	Gains2ndOrder

struct	CF4_t

struct	CF2_t

struct	Oneloop_CF_t

struct	notch_axis_t

struct	Oneloop_notch_t

Macros
#define	ANDI_NUM_ACT COMMANDS_NB_REAL

#define	ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL

#define	ANDI_NUM_VIRTUAL_ACT 2

#define	ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)

#define	ANDI_OUTPUTS 6

#define	ANDI_G_SCALING 1000.0f

#define	CTRL_ANDI 0
	Control types. More...

#define	CTRL_INDI 1

Functions
void	oneloop_andi_init (void)
	Init function of Oneloop ANDI controller More...

void	oneloop_andi_enter (bool half_loop_sp, int ctrl_type)
	Function that resets important values upon engaging Oneloop ANDI. More...

void	oneloop_andi_set_failsafe_setpoint (void)

void	oneloop_andi_run (bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
	Main function that runs the controller and performs control allocation. More...

void	oneloop_andi_RM (bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop)
	Function to generate the reference signals for the oneloop controller. More...

void	oneloop_andi_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn)

void	oneloop_from_nav (bool in_flight)
	Function that maps navigation inputs to the oneloop controller for the generated autopilot. More...

void	guidance_set_min_max_airspeed (float min_airspeed, float max_airspeed)

Variables
bool	ctrl_off

float	act_state_filt_vect_1l [ANDI_NUM_ACT]

float	actuator_state_1l [ANDI_NUM_ACT]

float	nu [6]

float	g1g2_1l [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

float	andi_u [ANDI_NUM_ACT_TOT]

float	andi_du [ANDI_NUM_ACT_TOT]

float	psi_des_deg

bool	heading_manual

bool	yaw_stick_in_auto

float	fwd_sideslip_gain

struct FloatEulers	eulers_zxy_des

float	psi_des_rad

float	k_as

float	max_as

float	gi_unbounded_airspeed_sp

bool	chirp_on

float	f0_chirp

float	f1_chirp

float	t_chirp

float	A_chirp

int8_t	chirp_axis

struct guidance_indi_hybrid_params	gih_params

struct OneloopGeneral	oneloop_andi

struct Oneloop_CF_t	cf

int16_t	temp_pitch

struct PolePlacement	p_att_e

struct PolePlacement	p_att_rm

struct PolePlacement	p_pos_e

struct PolePlacement	p_pos_rm

struct PolePlacement	p_alt_e

struct PolePlacement	p_alt_rm

struct PolePlacement	p_head_e

struct PolePlacement	p_head_rm

struct Gains3rdOrder	k_att_e

struct Gains3rdOrder	k_att_rm

struct Gains2ndOrder	k_head_e

struct Gains2ndOrder	k_head_rm

struct Gains3rdOrder	k_pos_e

struct Gains3rdOrder	k_pos_rm

Detailed Description

Author: Tomaso De Ponti tmlde.nosp@m.pont.nosp@m.i@tud.nosp@m.elft.nosp@m..nl One loop (Guidance + Stabilization) ANDI controller for rotorcrafts; Tomaso De Ponti tmlde.nosp@m.pont.nosp@m.i@tud.nosp@m.elft.nosp@m..nl One loop (Guidance + Stabilization) ANDI controller for the rotating wing drone RW3C

Definition in file oneloop_andi.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

◆ OneloopGuidanceRef

struct OneloopGuidanceRef

Definition at line 101 of file oneloop_andi.h.

Data Fields
float	acc[3]
float	jer[3]
float	pos[3]
float	vel[3]

◆ OneloopGuidanceState

struct OneloopGuidanceState

Definition at line 108 of file oneloop_andi.h.

Data Fields
float	acc[3]
float	pos[3]
float	vel[3]

◆ OneloopStabilizationRef

struct OneloopStabilizationRef

Definition at line 114 of file oneloop_andi.h.

Data Fields
float	att[3]
float	att_2d[3]
float	att_3d[3]
float	att_d[3]

◆ OneloopStabilizationState

struct OneloopStabilizationState

Definition at line 121 of file oneloop_andi.h.

Data Fields
float	att[3]
float	att_2d[3]
float	att_d[3]

◆ OneloopGeneral

struct OneloopGeneral

Definition at line 126 of file oneloop_andi.h.

Collaboration diagram for OneloopGeneral:

Data Fields
int	ctrl_type
struct OneloopGuidanceRef	gui_ref
struct OneloopGuidanceState	gui_state
bool	half_loop
struct OneloopStabilizationRef	sta_ref
struct OneloopStabilizationState	sta_state

◆ PolePlacement

struct PolePlacement

Definition at line 138 of file oneloop_andi.h.

Data Fields
float	omega_n
float	p3
float	zeta

◆ Gains3rdOrder

struct Gains3rdOrder

Definition at line 143 of file oneloop_andi.h.

Data Fields
float	k1[3]
float	k2[3]
float	k3[3]

◆ Gains2ndOrder

struct Gains2ndOrder

Definition at line 148 of file oneloop_andi.h.

Data Fields
float	k2
float	k3

◆ CF4_t

struct CF4_t

Definition at line 153 of file oneloop_andi.h.

Collaboration diagram for CF4_t:

Data Fields
float	feedback
Butterworth4LowPass	feedback_filt
float	freq
float	freq_set
float	model
Butterworth4LowPass	model_filt
float	out
float	tau

◆ CF2_t

struct CF2_t

Definition at line 163 of file oneloop_andi.h.

Collaboration diagram for CF2_t:

Data Fields
float	feedback
Butterworth2LowPass	feedback_filt
float	freq
float	freq_set
float	model
Butterworth2LowPass	model_filt
float	out
float	tau

◆ Oneloop_CF_t

struct Oneloop_CF_t

Definition at line 174 of file oneloop_andi.h.

Collaboration diagram for Oneloop_CF_t:

Data Fields
struct CF2_t	ax
struct CF2_t	ay
struct CF2_t	az
struct CF2_t	p
struct CF4_t	p_dot
struct CF2_t	q
struct CF4_t	q_dot
struct CF2_t	r
struct CF2_t	r_dot

◆ notch_axis_t

struct notch_axis_t

Definition at line 186 of file oneloop_andi.h.

Collaboration diagram for notch_axis_t:

Data Fields
float	bandwidth
struct SecondOrderNotchFilter	filter
float	freq

◆ Oneloop_notch_t

struct Oneloop_notch_t

Definition at line 191 of file oneloop_andi.h.

Collaboration diagram for Oneloop_notch_t:

Data Fields
struct notch_axis_t	pitch
struct notch_axis_t	roll
struct notch_axis_t	yaw

Macro Definition Documentation

◆ ANDI_G_SCALING

#define ANDI_G_SCALING 1000.0f

Definition at line 56 of file oneloop_andi.h.

◆ ANDI_NUM_ACT

#define ANDI_NUM_ACT COMMANDS_NB_REAL

Definition at line 37 of file oneloop_andi.h.

◆ ANDI_NUM_ACT_TOT

#define ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)

Definition at line 50 of file oneloop_andi.h.

◆ ANDI_NUM_VIRTUAL_ACT [1/2]

#define ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL

Definition at line 47 of file oneloop_andi.h.

◆ ANDI_NUM_VIRTUAL_ACT [2/2]

#define ANDI_NUM_VIRTUAL_ACT 2

Definition at line 47 of file oneloop_andi.h.

◆ ANDI_OUTPUTS

#define ANDI_OUTPUTS 6

Definition at line 54 of file oneloop_andi.h.

◆ CTRL_ANDI

#define CTRL_ANDI 0

Control types.

Definition at line 59 of file oneloop_andi.h.

◆ CTRL_INDI

#define CTRL_INDI 1

Definition at line 60 of file oneloop_andi.h.

Function Documentation

◆ guidance_set_min_max_airspeed()

void guidance_set_min_max_airspeed	(	float	min_airspeed,
		float	max_airspeed
	)

Definition at line 414 of file guidance_indi_hybrid.c.

References max_airspeed, max_as, and min_as.

Referenced by rotwing_state_periodic().

Here is the caller graph for this function:

◆ oneloop_andi_enter()

void oneloop_andi_enter	(	bool	half_loop_sp,
		int	ctrl_type
	)

Function that resets important values upon engaging Oneloop ANDI.

FIXME: Ideally we should distinguish between the "stabilization" and "guidance" needs because it is unlikely to switch stabilization in flight, and there are multiple modes that use (the same) stabilization. Resetting the controller is not so nice when you are flying.

Definition at line 1456 of file oneloop_andi.c.

References ANDI_OUTPUTS, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, calc_normalization(), OneloopGeneral::ctrl_type, EFF_MAT_G, ele_min, eulers_zxy, eulers_zxy_des, float_vect_zero(), G1G2_oneloop(), OneloopGeneral::half_loop, init_controller(), init_filter(), nav_target, nav_target_new, oneloop_andi, FloatEulers::phi, FloatEulers::psi, psi_des_deg, psi_des_rad, reinit_all_cf(), OneloopGeneral::sta_ref, and FloatEulers::theta.

Referenced by chirp_call(), guidance_h_run_enter(), oneloop_from_nav(), and stabilization_attitude_enter().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_andi_init()

void oneloop_andi_init ( void )

Init function of Oneloop ANDI controller

Definition at line 1401 of file oneloop_andi.c.

References act_dynamics, actuator_state_1l, andi_du, andi_du_n, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, calc_normalization(), CTRL_ANDI, OneloopGeneral::ctrl_type, DefaultPeriodic, EFF_MAT_G, eulers_zxy_des, float_vect_zero(), G1G2_oneloop(), OneloopGeneral::half_loop, init_all_cf(), init_controller(), init_filter(), init_poles(), nav_target, nav_target_new, nu, nu_n, oneloop_andi, FloatEulers::phi, positive_non_zero(), FloatEulers::psi, register_periodic_telemetry(), send_eff_mat_guid_oneloop_andi(), send_eff_mat_stab_oneloop_andi(), send_guidance_oneloop_andi(), send_oneloop_andi(), send_oneloop_debug(), send_wls_u_oneloop(), send_wls_v_oneloop(), OneloopGeneral::sta_ref, and FloatEulers::theta.

Here is the call graph for this function:

◆ oneloop_andi_read_rc()

void oneloop_andi_read_rc	(	bool	in_flight,
		bool	in_carefree,
		bool	coordinated_turn
	)

◆ oneloop_andi_RM()

void oneloop_andi_RM	(	bool	half_loop,
		struct FloatVect3	PSA_des,
		int	rm_order_h,
		int	rm_order_v,
		bool	in_flight_oneloop
	)

Function to generate the reference signals for the oneloop controller.

Parameters

half_loop	In half-loop mode the controller is used for stabilization only
PSA_des	Desired position/speed/acceleration
rm_order_h	Order of the reference model for horizontal guidance
rm_order_v	Order of the reference model for vertical guidance

Definition at line 1492 of file oneloop_andi.c.

References A_chirp, a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, actuator_state_1l, ANDI_NUM_ACT, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, chirp_axis, chirp_call(), chirp_first_call, chirp_on, dt_1l, eulers_zxy, eulers_zxy_des, f0_chirp, f1_chirp, float_vect_copy(), float_vect_norm(), float_vect_zero(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, heading_manual, OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_rm, k_pos_rm, max_a_nav, max_j_ang, max_j_nav, MAX_PPRZ, max_r, max_v_nav, max_v_nav_v, nav_target, nav_target_new, oneloop_andi, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, oneloop_andi_sideslip(), p_ref_0, FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, psi_vec, radio_control_get(), RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, ratio_u_un, ratio_vn_v, reshape_wind(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), rm_3rd_pos(), RW_aD, OneloopGeneral::sta_ref, t_0_chirp, t_chirp, FloatEulers::theta, time_elapsed_chirp, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, WLS_one_p, WLS_t::Wv, Wv_backup, FloatVect3::x, FloatVect3::y, yaw_stick_in_auto, and FloatVect3::z.

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_andi_run()

void oneloop_andi_run	(	bool	in_flight,
		bool	half_loop,
		struct FloatVect3	PSA_des,
		int	rm_order_h,
		int	rm_order_v
	)

Main function that runs the controller and performs control allocation.

Parameters

half_loop	In half-loop mode the controller is used for stabilization only
in_flight	The drone is in flight
PSA_des	Desired position/speed/acceleration
rm_order_h	Order of the reference model for horizontal guidance
rm_order_v	Order of the reference model for vertical guidance