#include "math/pprz_algebra_float.h"
#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_nB.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	OneloopStabilizationnBState

struct	OneloopPushnB

struct	OneloopGeneral

struct	PolePlacement

struct	Gains3rdOrder

struct	Gains2ndOrder

struct	LP_t

struct	Oneloop_LP_t

struct	Oneloop_DynFilt_t

union	LP_t.meas_filt

Macros
#define	ANDI_NUM_ACT COMMANDS_NB_REAL

#define	COMMANDS_NB_VIRTUAL 0

#define	ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL

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

#define	CTRL_INDI 1

#define	CTRL_NB_ANDI 2

#define	CTRL_NB_INDI 3

Enumerations
enum	FilterType { LOWPASS_1 , BUTTERWORTH_2 , BUTTERWORTH_4 , NOTCH }

Functions
void	oneloop_nB_init (void)
	Init function of Oneloop ANDI controller

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

void	oneloop_nB_set_failsafe_setpoint (void)

void	oneloop_nB_run (bool in_flight, bool half_loop, struct FloatVect3 PSA_des)
	Main function that runs the controller and performs control allocation.

void	oneloop_nB_RM (bool half_loop, struct FloatVect3 PSA_des, bool in_flight_oneloop)
	Function to generate the reference signals for the oneloop controller.

void	oneloop_nB_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.

void	guidance_set_min_max_airspeed (float min_airspeed, float max_airspeed)

void	guidance_set_max_bank_angle (float max_bank)
	Guidance limit setters called by rotwing_state (also implemented by guidance_indi_hybrid).

void	guidance_set_max_climb_speed (float max_climb_speed_quad, float max_climb_speed_fwd)

void	guidance_set_max_descend_speed (float max_descend_speed_quad, float max_descend_speed_fwd)

Variables
bool	auto_fault_cmd

bool	use_safety_killer

bool	safety_killer_trigger

float	safety_killer_cutoff

float	delta_fault

float	oneloop_nB_filt_cutoff

bool	vel_ctrl_in_manual

bool	oneloop_nB_Z_hold

bool	use_push_Position

bool	use_push_PID

float	max_bank

float	max_phi

float	max_theta

int8_t	TestMotorIDX

float	max_pusher_cmd

float	max_fault_mot

float	spin_prot_max_rate

float	spin_prot_start_rate

float	spin_prot_max_cmd

bool	spin_trim_on

float	spin_trim_rate

float	spin_trim_bleed_rate

float	spin_trim_release_rate

float	spin_trim

float	spin_prot_min_gap

float	ec_k3

float	pid_a_max

float	pid_v_max_manual

float	pid_v_max_nav

bool	spin_man_up

bool	spin_man_down

uint8_t	spin_man_pair

float	spin_man_pitch_dir

float	spin_man_ramp_rate

float	spin_man_blend_time

float	spin_man_trim_rate

float	spin_man_done_rate

uint8_t	spin_man_state

float	spin_man_r_ref

float	SpinQuadRate

bool	SpinQuad

bool	fault_pitch_motors

bool	fault_roll_motors

bool	fault_ailerons

float	k_K

float	k_P

float	k_I

float	k_D

float	k1_NE_tune

float	k2_NE_tune

bool	ctrl_off

float	act_state_filt_vect_1l [ANDI_NUM_ACT]

float	actuator_state_1l [ANDI_NUM_ACT_TOT]

float	nu [ANDI_OUTPUTS]

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	max_as

float	gi_unbounded_airspeed_sp

bool	drop_yaw

bool	state_compensation_on

struct guidance_indi_hybrid_params	gih_params

struct OneloopGeneral	oneloop_nB

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

struct Oneloop_LP_t	LP

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) nB controller

Definition in file oneloop_nB.h.

Data Structure Documentation

◆ guidance_indi_hybrid_params

struct guidance_indi_hybrid_params

Definition at line 80 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 110 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 116 of file oneloop_andi.h.

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

◆ OneloopStabilizationRef

struct OneloopStabilizationRef

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

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

◆ OneloopStabilizationnBState

struct OneloopStabilizationnBState

Definition at line 171 of file oneloop_nB.h.

Collaboration diagram for OneloopStabilizationnBState:

Data Fields
struct FloatVect3	mu_B
struct FloatVect3	nB
struct FloatVect3	nB_2d
struct FloatVect3	nB_3d
struct FloatVect3	nB_d
struct FloatVect3	nI
struct FloatVect3	nI_2d
struct FloatVect3	nI_3d
struct FloatVect3	nI_d
struct FloatVect3	nI_des

◆ OneloopPushnB

struct OneloopPushnB

Definition at line 183 of file oneloop_nB.h.

Data Fields
float	max_push_cmd
float	max_v_d
int	n
float	pE
float	pE_d
float	pN
float	pN_d
float	push_cmd
float	varepsilon
float	vE
float	vE_d
float	vE_d_filt
float	vE_filt
float	vN
float	vN_d
float	vN_d_filt
float	vN_filt
float	xi
float	xi_0

◆ OneloopGeneral

struct OneloopGeneral

Definition at line 132 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 OneloopPushnB	push_nB
struct OneloopStabilizationnBState	sta_nB_state
struct OneloopStabilizationRef	sta_ref
struct OneloopStabilizationState	sta_state

◆ PolePlacement

struct PolePlacement

Definition at line 143 of file oneloop_andi.h.

Data Fields
float	omega_n
float	p3
float	zeta

◆ Gains3rdOrder

struct Gains3rdOrder

Definition at line 148 of file oneloop_andi.h.

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

◆ Gains2ndOrder

struct Gains2ndOrder

Definition at line 153 of file oneloop_andi.h.

Data Fields
float	k2
float	k3

◆ LP_t

struct LP_t

Definition at line 184 of file oneloop_andi.h.

Data Fields
float	bandwidth
enum FilterType	filter_type
float	freq
float	freq_set
float	meas
union LP_t	meas_filt
union LP_t.meas_filt	meas_filt
float	meas_prev
float	out
float	tau

◆ Oneloop_LP_t

struct Oneloop_LP_t

Definition at line 200 of file oneloop_andi.h.

Collaboration diagram for Oneloop_LP_t:

Data Fields
struct LP_t	ax
struct LP_t	ay
struct LP_t	az
struct LP_t	p
struct LP_t	p_ddot
struct LP_t	p_dot
struct LP_t	q
struct LP_t	q_ddot
struct LP_t	q_dot
struct LP_t	r
struct LP_t	r_ddot
struct LP_t	r_dot

◆ Oneloop_DynFilt_t

struct Oneloop_DynFilt_t

Definition at line 211 of file oneloop_andi.h.

Data Fields
float	fs
float	mu_c
float	mu_c_0
float	sigma
float	u_c
float	u_c_0
float	varepsilon

◆ LP_t.meas_filt

union LP_t.meas_filt

Definition at line 267 of file oneloop_nB.h.

Data Fields
Butterworth2LowPass	bw2
Butterworth2LowPass	bw2
Butterworth4LowPass	bw4
Butterworth4LowPass	bw4
struct FirstOrderLowPass	lp1
struct FirstOrderLowPass	lp1
struct SecondOrderNotchFilter	notch
struct SecondOrderNotchFilter	notch

Macro Definition Documentation

◆ ANDI_G_SCALING

#define ANDI_G_SCALING 1000.0f

Definition at line 57 of file oneloop_nB.h.

◆ ANDI_NUM_ACT

#define ANDI_NUM_ACT COMMANDS_NB_REAL

Definition at line 40 of file oneloop_nB.h.

◆ ANDI_NUM_ACT_TOT

#define ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)

Definition at line 51 of file oneloop_nB.h.

◆ ANDI_NUM_VIRTUAL_ACT

#define ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL

Definition at line 48 of file oneloop_nB.h.

◆ ANDI_OUTPUTS

#define ANDI_OUTPUTS 6

Definition at line 55 of file oneloop_nB.h.

◆ COMMANDS_NB_VIRTUAL

#define COMMANDS_NB_VIRTUAL 0

Definition at line 44 of file oneloop_nB.h.

◆ CTRL_ANDI

#define CTRL_ANDI 0

Control types.

Definition at line 60 of file oneloop_nB.h.

◆ CTRL_INDI

#define CTRL_INDI 1

Definition at line 61 of file oneloop_nB.h.

◆ CTRL_NB_ANDI

#define CTRL_NB_ANDI 2

Definition at line 62 of file oneloop_nB.h.

◆ CTRL_NB_INDI

#define CTRL_NB_INDI 3

Definition at line 63 of file oneloop_nB.h.

Enumeration Type Documentation

◆ FilterType

enum FilterType

Enumerator
LOWPASS_1
BUTTERWORTH_2
BUTTERWORTH_4
NOTCH

Definition at line 252 of file oneloop_nB.h.

Function Documentation

◆ guidance_set_max_bank_angle()

void guidance_set_max_bank_angle ( float max_bank )

extern

Guidance limit setters called by rotwing_state (also implemented by guidance_indi_hybrid).

The one loop controller uses its own limits, so these are no-ops here.

Definition at line 388 of file guidance_indi_hybrid.c.

References foo, guidance_indi_max_bank, and max_bank.

Referenced by rotwing_state_periodic().

Here is the caller graph for this function:

◆ guidance_set_max_climb_speed()

void guidance_set_max_climb_speed	(	float	max_climb_speed_quad,
		float	max_climb_speed_fwd
	)

extern

Definition at line 392 of file guidance_indi_hybrid.c.

References guidance_indi_hybrid_params::climb_vspeed_fwd, guidance_indi_hybrid_params::climb_vspeed_quad, foo, and gih_params.

Referenced by rotwing_state_periodic().

Here is the caller graph for this function:

◆ guidance_set_max_descend_speed()

void guidance_set_max_descend_speed	(	float	max_descend_speed_quad,
		float	max_descend_speed_fwd
	)

extern

Definition at line 397 of file guidance_indi_hybrid.c.

References guidance_indi_hybrid_params::descend_vspeed_fwd, guidance_indi_hybrid_params::descend_vspeed_quad, foo, and gih_params.

Referenced by rotwing_state_periodic().

Here is the caller 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 383 of file guidance_indi_hybrid.c.

References gih_params, max_airspeed, guidance_indi_hybrid_params::max_airspeed, max_as, guidance_indi_hybrid_params::min_airspeed, and min_as.

Referenced by rotwing_state_periodic().

Here is the caller graph for this function:

◆ oneloop_from_nav()

void oneloop_from_nav ( bool in_flight )

extern

Function that maps navigation inputs to the oneloop controller for the generated autopilot.

Definition at line 2568 of file oneloop_andi.c.

References RotorcraftNavigation::climb, OneloopGeneral::ctrl_type, foo, nav, RotorcraftNavigation::nav_altitude, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, NAV_VERTICAL_MODE_ALT, NAV_VERTICAL_MODE_CLIMB, oneloop_andi, oneloop_andi_enter(), oneloop_andi_run(), oneloop_nB, oneloop_nB_enter(), oneloop_nB_run(), POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, RotorcraftNavigation::setpoint_mode, RotorcraftNavigation::speed, SPEED_BFP_OF_REAL, SPEED_FLOAT_OF_BFP, stateGetPositionNed_f(), RotorcraftNavigation::target, RotorcraftNavigation::vertical_mode, FloatVect3::x, NedCoor_f::x, EnuCoor_f::x, NedCoor_f::y, EnuCoor_f::y, and NedCoor_f::z.

Here is the call graph for this function:

◆ oneloop_nB_enter()

void oneloop_nB_enter	(	bool	half_loop_sp,
		int	ctrl_type
	)

extern

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 2352 of file oneloop_nB.c.

References calc_normalization(), OneloopGeneral::ctrl_type, eulers_zxy, foo, G1G2_oneloop(), OneloopGeneral::half_loop, init_controller_gains(), oneloop_nB, FloatEulers::psi, psi_des_deg, psi_des_rad, reinit_controller(), safety_killer_trigger, and spin_man_reset().