#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 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

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

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

void	oneloop_andi_enter (bool half_loop_sp)
	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)
	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...

Variables
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

struct guidance_indi_hybrid_params	gih_params

bool	force_forward
	forward flight for hybrid nav More...

struct OneloopGeneral	oneloop_andi

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 72 of file guidance_indi_hybrid.h.

Data Fields
float	heading_bank_gain
float	liftd_asq
float	liftd_p50
float	liftd_p80
float	pos_gain
float	pos_gainz
float	speed_gain
float	speed_gainz

◆ OneloopGuidanceRef

struct OneloopGuidanceRef

Definition at line 78 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 85 of file oneloop_andi.h.

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

◆ OneloopStabilizationRef

struct OneloopStabilizationRef

Definition at line 91 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 98 of file oneloop_andi.h.

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

◆ OneloopGeneral

struct OneloopGeneral

Definition at line 103 of file oneloop_andi.h.

Collaboration diagram for OneloopGeneral:

Data Fields
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 114 of file oneloop_andi.h.

Data Fields
float	omega_n
float	p3
float	zeta

◆ Gains3rdOrder

struct Gains3rdOrder

Definition at line 119 of file oneloop_andi.h.

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

◆ Gains2ndOrder

struct Gains2ndOrder

Definition at line 124 of file oneloop_andi.h.

Data Fields
float	k2
float	k3

Macro Definition Documentation

◆ ANDI_G_SCALING

#define ANDI_G_SCALING 1000.0f

Definition at line 54 of file oneloop_andi.h.

◆ ANDI_NUM_ACT

#define ANDI_NUM_ACT COMMANDS_NB_REAL

Definition at line 35 of file oneloop_andi.h.

◆ ANDI_NUM_ACT_TOT

#define ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)

Definition at line 48 of file oneloop_andi.h.

◆ ANDI_NUM_VIRTUAL_ACT [1/2]

#define ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL

Definition at line 45 of file oneloop_andi.h.

◆ ANDI_NUM_VIRTUAL_ACT [2/2]

#define ANDI_NUM_VIRTUAL_ACT 2

Definition at line 45 of file oneloop_andi.h.

◆ ANDI_OUTPUTS

#define ANDI_OUTPUTS 6

Definition at line 52 of file oneloop_andi.h.

Function Documentation

◆ oneloop_andi_enter()

void oneloop_andi_enter ( bool half_loop_sp )

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 1098 of file oneloop_andi.c.

References ANDI_OUTPUTS, ang_acc, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, calc_normalization(), eulers_zxy, eulers_zxy_des, float_vect_zero(), OneloopGeneral::half_loop, init_controller(), init_filter(), lin_acc, model_pred, nav_target, oneloop_andi, FloatEulers::phi, FloatEulers::psi, psi_des_deg, psi_des_rad, OneloopGeneral::sta_ref, sum_g1g2_1l(), and FloatEulers::theta.

Referenced by 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 1048 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, ang_acc, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, calc_normalization(), DefaultPeriodic, eulers_zxy_des, float_vect_zero(), g1g2_1l, OneloopGeneral::half_loop, init_controller(), init_filter(), init_poles(), lin_acc, model_pred, nav_target, nu, oneloop_andi, FloatEulers::phi, positive_non_zero(), FloatEulers::psi, register_periodic_telemetry(), send_eff_mat_g_oneloop_andi(), send_guidance_oneloop_andi(), send_oneloop_andi(), OneloopGeneral::sta_ref, sum_g1g2_1l(), 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
	)

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 1129 of file oneloop_andi.c.

References 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, dt_1l, eulers_zxy, eulers_zxy_des, float_vect_copy(), float_vect_zero(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains2ndOrder::k2, Gains3rdOrder::k3, Gains2ndOrder::k3, k_att_rm, k_head_rm, k_pos_rm, max_a_nav, max_j_nav, MAX_PPRZ, max_v_nav, nav_target, oneloop_andi, FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_rad, psi_vec, radio_control_get(), RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, ratio_u_un, ratio_vn_v, rm_1st_pos(), rm_2nd(), rm_2nd_pos(), rm_3rd(), rm_3rd_attitude(), rm_3rd_pos(), OneloopGeneral::sta_ref, FloatEulers::theta, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, Wv, Wv_wls, FloatVect3::x, FloatVect3::y, 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