#include "firmwares/rotorcraft/oneloop/oneloop_andi.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "generated/airframe.h"
#include "modules/radio_control/radio_control.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"
#include "filters/notch_filter_float.h"
#include "math/wls/wls_alloc.h"
#include "modules/nav/nav_rotorcraft_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
#include "modules/rotwing_drone/rotwing_state.h"
#include "modules/core/commands.h"
#include "modules/ctrl/eff_scheduling_rotwing_V2.h"
#include <stdio.h>
#include "modules/gps/gps.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for oneloop_andi.c:

Macros
#define	ONELOOP_ANDI_SCHEDULING FALSE

#define	ONELOOP_ANDI_DEBUG_MODE FALSE

#define	ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL]

#define	ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL]

#define	ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH]

#define	ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0

#define	USE_YAW_NOTCH

Functions
	PRINT_CONFIG_VAR (ONELOOP_ANDI_FILT_CUTOFF)

	PRINT_CONFIG_VAR (ONELOOP_ANDI_FILT_CUTOFF_Q)

	PRINT_CONFIG_VAR (ONELOOP_ANDI_FILT_CUTOFF_P)

	PRINT_CONFIG_VAR (ONELOOP_ANDI_FILT_CUTOFF_R)

void	init_poles (void)
	Initialize Position of Poles. More...

void	init_poles_att (void)
	Initialize Position of Poles. More...

void	init_poles_pos (void)

void	calc_normalization (void)
	Calculate Normalization of actuators and discrete actuator dynamics More...

void	normalize_nu (void)
	Function to normalize the pseudo control vector. More...

void	G1G2_oneloop (int ctrl_type)
	Function that samples and scales the effectiveness matrix FIXME: make this function into a for loop to make it more adaptable to different configurations. More...

void	get_act_state_oneloop (void)
	Function to reconstruct actuator state using first order dynamics. More...

void	oneloop_andi_propagate_filters (void)
	Propagate the filters. More...

void	init_filter (void)
	Initialize the filters. More...

void	init_controller (void)
	Initialize Controller Gains FIXME: Calculate the gains dynamically for transition. More...

void	float_rates_of_euler_dot_vec (float r[3], float e[3], float edot[3])
	Attitude Rates to Euler Conversion Function. More...

void	float_euler_dot_of_rates_vec (float r[3], float e[3], float edot[3])
	Attitude Euler to Rates Conversion Function. More...

void	err_nd (float err[], float a[], float b[], float k[], int n)
	Calculate Scaled Error between two 3D arrays. More...

void	err_sum_nd (float err[], float a[], float b[], float k[], float c[], int n)
	Calculate Scaled Error between two 3D arrays. More...

void	integrate_nd (float dt, float a[], float a_dot[], int n)
	Integrate in time 3D array. More...

void	vect_bound_nd (float vect[], float bound, int n)
	Scale a 3D array to within a 3D bound. More...

void	acc_body_bound (struct FloatVect2 *vect, float bound)
	Scale a 3D array to within a 3D bound. More...

float	bound_v_from_a (float e_x[], float v_bound, float a_bound, int n)
	Calculate velocity limit based on acceleration limit. More...

void	rm_2nd (float dt, float x_ref, float x_d_ref, float *x_2d_ref, float x_des, float k1_rm, float k2_rm)
	Reference Model Definition for 2nd order system. More...

void	rm_3rd (float dt, float x_ref, float x_d_ref, float x_2d_ref, float x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm)
	Reference Model Definition for 3rd order system. More...

void	rm_3rd_head (float dt, float x_ref, float x_d_ref, float x_2d_ref, float x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm)
	Reference Model Definition for 3rd order system specific to the heading angle. More...

void	rm_3rd_attitude (float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3], float max_ang_jerk)
	Reference Model Definition for 3rd order system with attitude conversion functions. More...

void	rm_3rd_pos (float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_des[], float k1_rm[], float k2_rm[], float k3_rm[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n)
	Reference Model Definition for 3rd order system specific to positioning with bounds. More...

void	rm_2nd_pos (float dt, float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_d_des[], float k2_rm[], float k3_rm[], float x_2d_bound, float x_3d_bound, int n)
	Reference Model Definition for 3rd order system specific to positioning with bounds. More...

void	rm_1st_pos (float dt, float x_2d_ref[], float x_3d_ref[], float x_2d_des[], float k3_rm[], float x_3d_bound, int n)
	Reference Model Definition for 3rd order system specific to positioning with bounds. More...

void	ec_3rd_att (float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3], float max_ang_jerk)
	Error Controller Definition for 3rd order system specific to attitude. More...

void	ec_3rd_pos (float y_4d[], float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x[], float x_d[], float x_2d[], float k1_e[], float k2_e[], float k3_e[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n)

void	calc_model (void)
	Function that calculates the model prediction for the complementary filter. More...

float	oneloop_andi_sideslip (void)
	Function to calculate corrections for sideslip. More...

void	reshape_wind (void)

void	chirp_pos (float time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[])
	Reference Model Definition for 3rd order system specific to positioning with bounds. More...

void	chirp_call (bool chirp_on, bool chirp_first_call, float t_0_chirp, float time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[])

void	init_cf2 (struct CF2_t *cf, float fc)
	Initialize the Complementary Filters 2nd Order Butterworth. More...

void	init_cf4 (struct CF4_t *cf, float fc)
	Initialize the Complementary Filters 4th Order Butterworth. More...

void	init_all_cf (void)
	Initialize all the Complementary Filters. More...

void	reinit_cf2 (struct CF2_t *cf, bool reinit)
	Reinitialize 2nd Order CF if new frequency setting or if forced. More...

void	reinit_cf4 (struct CF4_t *cf, bool reinit)
	Reinitialize 4th Order CF if new frequency setting or if forced. More...

void	reinit_all_cf (bool reinit)
	Reinitialize all the Complementary Filters. More...

static void	send_wls_v_oneloop (struct transport_tx trans, struct link_device dev)

static void	send_wls_u_oneloop (struct transport_tx trans, struct link_device dev)

static void	send_eff_mat_stab_oneloop_andi (struct transport_tx trans, struct link_device dev)

static void	send_eff_mat_guid_oneloop_andi (struct transport_tx trans, struct link_device dev)

static void	send_oneloop_andi (struct transport_tx trans, struct link_device dev)

static void	send_guidance_oneloop_andi (struct transport_tx trans, struct link_device dev)

static void	debug_vect (struct transport_tx trans, struct link_device dev, char name, float data, int datasize)

static void	send_oneloop_debug (struct transport_tx trans, struct link_device dev)

static float	positive_non_zero (float input)
	Function to make sure that inputs are positive non zero vaues. More...

static float	k_e_1_2_f_v2 (float omega, float zeta)
	Error Controller Gain Design. More...

static float	k_e_2_2_f_v2 (float omega, float zeta)

static float	k_e_1_3_f_v2 (float omega_n, UNUSED float zeta, float p1)

static float	k_e_2_3_f_v2 (float omega_n, float zeta, float p1)

static float	k_e_3_3_f_v2 (float omega_n, float zeta, float p1)

static float	k_rm_1_3_f (float omega_n, float zeta, float p1)
	Reference Model Gain Design. More...

static float	k_rm_2_3_f (float omega_n, float zeta, float p1)

static float	k_rm_3_3_f (float omega_n, float zeta, float p1)

static float	ec_3rd (float x_ref, float x_d_ref, float x_2d_ref, float x_3d_ref, float x, float x_d, float x_2d, float k1_e, float k2_e, float k3_e)
	Error Controller Definition for 3rd order system. More...

static float	w_approx (float p1, float p2, float p3, float rm_k)
	Third Order to First Order Dynamics Approximation. More...

static float	ec_poles (float p_rm, float slow_pole, float k)
	Calculate EC poles given RM poles. More...

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_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_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_from_nav (bool in_flight)
	Function that maps navigation inputs to the oneloop controller for the generated autopilot. More...

static float	chirp_pos_p_ref (float delta_t, float f0, float k, float A)
	Function to calculate the position reference during the chirp. More...

static float	chirp_pos_v_ref (float delta_t, float f0, float k, float A)
	Function to calculate the velocity reference during the chirp. More...

static float	chirp_pos_a_ref (float delta_t, float f0, float k, float A)
	Function to calculate the acceleration reference during the chirp. More...

static float	chirp_pos_j_ref (float delta_t, float f0, float k, float A)
	Function to calculate the jerk reference during the chirp. More...

bool	autopilot_in_flight_end_detection (bool motors_on UNUSED)
	Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts with all thrust off. More...

void	guidance_set_min_max_airspeed (float min_airspeed, float max_airspeed)

Variables
float	num_thrusters_oneloop = 4.0

float	oneloop_andi_filt_cutoff = 2.0

float	oneloop_andi_filt_cutoff_a = 2.0

float	oneloop_andi_filt_cutoff_v = 2.0

float	oneloop_andi_filt_cutoff_pos = 2.0

float	oneloop_andi_filt_cutoff_p = 20.0

float	oneloop_andi_filt_cutoff_q = 20.0

float	oneloop_andi_filt_cutoff_r = 20.0

float	max_r = RadOfDeg(120.0)

bool	actuator_is_servo [ANDI_NUM_ACT_TOT] = {0}

float	act_dynamics [ANDI_NUM_ACT_TOT] = = {1}

float	act_max [ANDI_NUM_ACT_TOT] = = {MAX_PPRZ}

float	act_min [ANDI_NUM_ACT_TOT] = = {0.0}

float	act_max_norm [ANDI_NUM_ACT_TOT] = = {1.0}

float	act_min_norm [ANDI_NUM_ACT_TOT] = = {0.0}

float	nu_norm_max = 1.0

static float	u_pref [ANDI_NUM_ACT_TOT] = {0.0}

float	theta_pref_max = RadOfDeg(20.0)

float	max_a_nav = 4.0

float	max_j_nav = 500.0

float	max_j_ang = 1000.0

float	max_v_nav = 5.0

float	max_as = 19.0f

float	min_as = 0.0f

float	max_v_nav_v = 1.5

float	fwd_sideslip_gain = 0.2

float	Wu_quad_motors_fwd = 6.0

struct OneloopGeneral	oneloop_andi

static float	use_increment = 0.0

static float	nav_target [3]

static float	nav_target_new [3]

static float	dt_1l = 1./PERIODIC_FREQUENCY

static float	g = 9.81

float	k_as = 2.0

int16_t	temp_pitch = 0

float	gi_unbounded_airspeed_sp = 0.0

float	andi_u [ANDI_NUM_ACT_TOT]

float	andi_du [ANDI_NUM_ACT_TOT]

static float	andi_du_n [ANDI_NUM_ACT_TOT]

float	nu [ANDI_OUTPUTS]

float	nu_n [ANDI_OUTPUTS]

static float	act_dynamics_d [ANDI_NUM_ACT_TOT]

float	actuator_state_1l [ANDI_NUM_ACT]

static float	a_thrust = 0.0

static float	g2_ff = 0.0

struct Int32Eulers	stab_att_sp_euler_1l

struct Int32Quat	stab_att_sp_quat_1l

struct FloatEulers	eulers_zxy_des

struct FloatEulers	eulers_zxy

float	psi_des_rad = 0.0

float	psi_des_deg = 0.0

static float	psi_vec [4] = {0.0, 0.0, 0.0, 0.0}

bool	heading_manual = false

bool	yaw_stick_in_auto = false

bool	ctrl_off = false

static float	pitch_pref = 0

struct WLS_t	WLS_one_p

static float	Wv_backup [ANDI_OUTPUTS] = {1.0}

static float	Wu_backup [ANDI_NUM_ACT_TOT] = {1.0}

struct Oneloop_CF_t	cf

static struct Oneloop_notch_t	oneloop_notch

bool	chirp_on = false

bool	chirp_first_call = true

float	time_elapsed_chirp = 0.0

float	t_0_chirp = 0.0

float	f0_chirp = 0.8 / (2.0 * M_PI)

float	f1_chirp = 0.8 / (2.0 * M_PI)

float	t_chirp = 45.0

float	A_chirp = 1.0

int8_t	chirp_axis = 0

float	p_ref_0 [3] = {0.0, 0.0, 0.0}

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 Gains3rdOrder	k_pos_e

struct Gains3rdOrder	k_pos_rm

struct Gains3rdOrder	k_att_e_indi

struct Gains3rdOrder	k_pos_e_indi

float *	bwls_1l [ANDI_OUTPUTS]

float	EFF_MAT_G [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

float	ratio_u_un [ANDI_NUM_ACT_TOT]

float	ratio_vn_v [ANDI_OUTPUTS]

static Butterworth2LowPass	filt_veloc_N

static Butterworth2LowPass	filt_veloc_E

static Butterworth2LowPass	filt_veloc_D

static Butterworth2LowPass	accely_filt

static Butterworth2LowPass	airspeed_filt

Macro Definition Documentation

◆ ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD

#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0

Definition at line 243 of file oneloop_andi.c.

◆ ONELOOP_ANDI_DEBUG_MODE

#define ONELOOP_ANDI_DEBUG_MODE FALSE

Definition at line 218 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_BANK

#define ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL]

Definition at line 222 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_PHI

#define ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL]

Definition at line 223 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_THETA

#define ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH]

Definition at line 225 of file oneloop_andi.c.

◆ ONELOOP_ANDI_SCHEDULING

#define ONELOOP_ANDI_SCHEDULING FALSE

Definition at line 110 of file oneloop_andi.c.

◆ USE_YAW_NOTCH

#define USE_YAW_NOTCH

Function Documentation

◆ acc_body_bound()

void acc_body_bound	(	struct FloatVect2 *	vect,
		float	bound
	)

Scale a 3D array to within a 3D bound.

Definition at line 717 of file oneloop_andi.c.

References float_vect_norm(), Min, positive_non_zero(), FloatVect2::x, and FloatVect2::y.

Referenced by reshape_wind().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ autopilot_in_flight_end_detection()

bool autopilot_in_flight_end_detection ( bool motors_on UNUSED )

Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts with all thrust off.

Definition at line 2185 of file oneloop_andi.c.

◆ bound_v_from_a()

float bound_v_from_a	(	float	e_x[],
		float	v_bound,
		float	a_bound,
		int	n
	)

Calculate velocity limit based on acceleration limit.

Definition at line 737 of file oneloop_andi.c.

References float_vect_norm().

Referenced by rm_3rd_pos().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calc_model()

void calc_model ( void )

Function that calculates the model prediction for the complementary filter.

Definition at line 1970 of file oneloop_andi.c.

References actuator_state_1l, ANDI_NUM_ACT, Oneloop_CF_t::ax, Oneloop_CF_t::ay, Oneloop_CF_t::az, cf, EFF_MAT_RW, RW_Model::ele_pref, eulers_zxy, g, wing_model::L, RW_Model::m, CF4_t::model, CF2_t::model, RW_Model::P, P, Oneloop_CF_t::p_dot, FloatEulers::phi, FloatEulers::psi, Oneloop_CF_t::q_dot, Oneloop_CF_t::r_dot, RW, RW_Model::T, mesonh.mesonh_atmosphere::T, FloatEulers::theta, and RW_Model::wing.

Referenced by oneloop_andi_propagate_filters().

Here is the caller graph for this function:

◆ calc_normalization()

void calc_normalization ( void )

Calculate Normalization of actuators and discrete actuator dynamics

Definition at line 1926 of file oneloop_andi.c.

References act_dynamics, act_dynamics_d, act_max, act_max_norm, act_min, act_min_norm, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, dt_1l, max_j_ang, max_j_nav, MAX_PPRZ, nu_norm_max, positive_non_zero(), ratio_u_un, ratio_vn_v, RW_aD, RW_aE, RW_aN, RW_ap, RW_aq, and RW_ar.

Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ chirp_call()

void chirp_call	(	bool *	chirp_on,
		bool *	chirp_first_call,
		float *	t_0_chirp,
		float *	time_elapsed,
		float	f0,
		float	f1,
		float	t_chirp,
		float	A,
		int8_t	n,
		float	psi,
		float	p_ref[],
		float	v_ref[],
		float	a_ref[],
		float	j_ref[],
		float	p_ref_0[]
	)

Definition at line 2161 of file oneloop_andi.c.

References A, chirp_first_call, chirp_on, chirp_pos(), OneloopGeneral::ctrl_type, get_sys_time_float(), oneloop_andi, oneloop_andi_enter(), p_ref_0, and t_chirp.

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ chirp_pos()

void chirp_pos	(	float	time_elapsed,
		float	f0,
		float	f1,
		float	t_chirp,
		float	A,
		int8_t	n,
		float	psi,
		float	p_ref[],
		float	v_ref[],
		float	a_ref[],
		float	j_ref[],
		float	p_ref_0[]
	)

Reference Model Definition for 3rd order system specific to positioning with bounds.

Parameters

dt	[s] time passed since start of the chirp
f0	[Hz] initial frequency of the chirp
f1	[Hz] final frequency of the chirp
t_chirp	[s] duration of the chirp
p_ref	[m] position reference
v_ref	[m/s] velocity reference
a_ref	[m/s2] acceleration reference
j_ref	[m/s3] jerk reference

Definition at line 2097 of file oneloop_andi.c.

References A, chirp_pos_a_ref(), chirp_pos_j_ref(), chirp_pos_p_ref(), chirp_pos_v_ref(), p_ref_0, pitch_pref, positive_non_zero(), t_chirp, and theta_pref_max.

Referenced by chirp_call().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ chirp_pos_a_ref()

static float chirp_pos_a_ref	(	float	delta_t,
		float	f0,
		float	k,
		float	A
	)

static

Function to calculate the acceleration reference during the chirp.

Definition at line 2076 of file oneloop_andi.c.

References A.

Referenced by chirp_pos().

Here is the caller graph for this function:

◆ chirp_pos_j_ref()

static float chirp_pos_j_ref	(	float	delta_t,
		float	f0,
		float	k,
		float	A
	)

static

Function to calculate the jerk reference during the chirp.

Definition at line 2081 of file oneloop_andi.c.

References A.

Referenced by chirp_pos().

Here is the caller graph for this function:

◆ chirp_pos_p_ref()

static float chirp_pos_p_ref	(	float	delta_t,
		float	f0,
		float	k,
		float	A
	)

static

Function to calculate the position reference during the chirp.

Definition at line 2066 of file oneloop_andi.c.

References A.

Referenced by chirp_pos().

Here is the caller graph for this function:

◆ chirp_pos_v_ref()

static float chirp_pos_v_ref	(	float	delta_t,
		float	f0,
		float	k,
		float	A
	)

static

Function to calculate the velocity reference during the chirp.

Definition at line 2071 of file oneloop_andi.c.

References A.

Referenced by chirp_pos().

Here is the caller graph for this function:

◆ debug_vect()

static void debug_vect	(	struct transport_tx *	trans,
		struct link_device *	dev,
		char *	name,
		float *	data,
		int	datasize
	)

static

Definition at line 559 of file oneloop_andi.c.

References dev.

Referenced by send_oneloop_debug().

Here is the caller graph for this function:

◆ ec_3rd()

static float ec_3rd	(	float	x_ref,
		float	x_d_ref,
		float	x_2d_ref,
		float	x_3d_ref,
		float	x,
		float	x_d,
		float	x_2d,
		float	k1_e,
		float	k2_e,
		float	k3_e
	)

static

Error Controller Definition for 3rd order system.

Parameters

dt	Delta time [s]
x_ref	Reference signal 1st order
x_d_ref	Reference signal 2nd order
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_des	Desired 1st order signal
x	Current 1st order signal
x_d	Current 2nd order signal
x_2d	Current 3rd order signal
k1_e	Error Controller Gain 1st order signal
k2_e	Error Controller Gain 2nd order signal
k3_e	Error Controller Gain 3rd order signal

Definition at line 945 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

Here is the caller graph for this function:

◆ ec_3rd_att()

void ec_3rd_att	(	float	y_4d[3],
		float	x_ref[3],
		float	x_d_ref[3],
		float	x_2d_ref[3],
		float	x_3d_ref[3],
		float	x[3],
		float	x_d[3],
		float	x_2d[3],
		float	k1_e[3],
		float	k2_e[3],
		float	k3_e[3],
		float	max_ang_jerk
	)

Error Controller Definition for 3rd order system specific to attitude.

Parameters

dt	Delta time [s]
x_ref	Reference signal 1st order
x_d_ref	Reference signal 2nd order
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_des	Desired 1st order signal
x	Current 1st order signal
x_d	Current 2nd order signal
x_2d	Current 3rd order signal
k1_e	Error Controller Gain 1st order signal
k2_e	Error Controller Gain 2nd order signal
k3_e	Error Controller Gain 3rd order signal

Definition at line 977 of file oneloop_andi.c.

References err_nd(), float_vect_copy(), float_vect_sum(), and vect_bound_nd().

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ec_3rd_pos()

void ec_3rd_pos	(	float	y_4d[],
		float	x_ref[],
		float	x_d_ref[],
		float	x_2d_ref[],
		float	x_3d_ref[],
		float	x[],
		float	x_d[],
		float	x_2d[],
		float	k1_e[],
		float	k2_e[],
		float	k3_e[],
		float	x_d_bound,
		float	x_2d_bound,
		float	x_3d_bound,
		int	n
	)

Definition at line 949 of file oneloop_andi.c.

References err_sum_nd(), and vect_bound_nd().

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ec_poles()

static float ec_poles	(	float	p_rm,
		float	slow_pole,
		float	k
	)

static

Calculate EC poles given RM poles.

Parameters

p_rm	Reference Model Pole (3 coincident poles)
slow_pole	Pole of the slowest dynamics
k	EC / RM ratio
omega_n	Natural Frequency

Definition at line 1019 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_poles(), init_poles_att(), and init_poles_pos().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ err_nd()

void err_nd	(	float	err[],
		float	a[],
		float	b[],
		float	k[],
		int	n
	)

Calculate Scaled Error between two 3D arrays.

Definition at line 676 of file oneloop_andi.c.

References b.

Referenced by ec_3rd_att(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), and rm_3rd_pos().

Here is the caller graph for this function:

◆ err_sum_nd()

void err_sum_nd	(	float	err[],
		float	a[],
		float	b[],
		float	k[],
		float	c[],
		int	n
	)

Calculate Scaled Error between two 3D arrays.

Definition at line 685 of file oneloop_andi.c.

References b.

Referenced by ec_3rd_pos().

Here is the caller graph for this function:

◆ float_euler_dot_of_rates_vec()

void float_euler_dot_of_rates_vec	(	float	r[3],
		float	e[3],
		float	edot[3]
	)

Attitude Euler to Rates Conversion Function.

Definition at line 661 of file oneloop_andi.c.

Referenced by rm_3rd_attitude().

Here is the caller graph for this function:

◆ float_rates_of_euler_dot_vec()

void float_rates_of_euler_dot_vec	(	float	r[3],
		float	e[3],
		float	edot[3]
	)

Attitude Rates to Euler Conversion Function.

Definition at line 649 of file oneloop_andi.c.

Referenced by rm_3rd_attitude().

Here is the caller graph for this function:

◆ G1G2_oneloop()

void G1G2_oneloop ( int ctrl_type )

Function that samples and scales the effectiveness matrix FIXME: make this function into a for loop to make it more adaptable to different configurations.

Definition at line 1870 of file oneloop_andi.c.

References act_dynamics, airspeed_filt, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, CTRL_ANDI, CTRL_INDI, ctrl_off, EFF_MAT_G, EFF_MAT_RW, ELE_MIN_AS, rotwing_state_t::hover_motors_enabled, SecondOrderLowPass::o, ratio_u_un, ratio_vn_v, rotwing_state, ROTWING_STATE_FORCE_HOVER, rotwing_state_hover_motors_running(), and rotwing_state_t::state.

Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_act_state_oneloop()

void get_act_state_oneloop ( void )

Function to reconstruct actuator state using first order dynamics.

Definition at line 1852 of file oneloop_andi.c.

References act_dynamics_d, act_max, act_min, actuator_state_1l, ANDI_NUM_ACT, andi_u, and autopilot_get_motors_on().

Referenced by oneloop_andi_run().

Here is the call graph for this function:

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
	)

Definition at line 2251 of file oneloop_andi.c.

References max_airspeed, max_as, and min_as.

◆ init_all_cf()

void init_all_cf ( void )

Initialize all the Complementary Filters.

Definition at line 1231 of file oneloop_andi.c.

References Oneloop_CF_t::ax, Oneloop_CF_t::ay, Oneloop_CF_t::az, cf, init_cf2(), init_cf4(), oneloop_andi_filt_cutoff, oneloop_andi_filt_cutoff_a, oneloop_andi_filt_cutoff_p, oneloop_andi_filt_cutoff_q, oneloop_andi_filt_cutoff_r, Oneloop_CF_t::p, Oneloop_CF_t::p_dot, Oneloop_CF_t::q, Oneloop_CF_t::q_dot, Oneloop_CF_t::r, and Oneloop_CF_t::r_dot.

Referenced by oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_cf2()

void init_cf2	(	struct CF2_t *	cf,
		float	fc
	)

Initialize the Complementary Filters 2nd Order Butterworth.

Definition at line 1209 of file oneloop_andi.c.

References cf, and init_butterworth_2_low_pass().

Referenced by init_all_cf().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_cf4()

void init_cf4	(	struct CF4_t *	cf,
		float	fc
	)

Initialize the Complementary Filters 4th Order Butterworth.

Definition at line 1220 of file oneloop_andi.c.

References cf, and init_butterworth_4_low_pass().

Referenced by init_all_cf().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_controller()

void init_controller ( void )

Initialize Controller Gains FIXME: Calculate the gains dynamically for transition.

Definition at line 1102 of file oneloop_andi.c.

References act_dynamics, init_poles_att(), init_poles_pos(), Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_e, k_att_e_indi, k_att_rm, k_e_1_2_f_v2(), k_e_1_3_f_v2(), k_e_2_2_f_v2(), k_e_2_3_f_v2(), k_e_3_3_f_v2(), k_pos_e, k_pos_e_indi, k_pos_rm, k_rm_1_3_f(), k_rm_2_3_f(), k_rm_3_3_f(), max_a_nav, max_v_nav, nav_hybrid_max_bank, nav_hybrid_pos_gain, nav_max_acceleration_sp, nav_max_speed, PolePlacement::omega_n, ONELOOP_ANDI_MAX_BANK, PolePlacement::p3, p_alt_e, p_alt_rm, p_att_e, p_att_rm, p_head_e, p_head_rm, p_pos_e, p_pos_rm, w_approx(), and PolePlacement::zeta.

Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_filter()

void init_filter ( void )

Initialize the filters.

Definition at line 1274 of file oneloop_andi.c.

References accely_filt, airspeed_filt, notch_axis_t::bandwidth, filt_veloc_D, filt_veloc_E, filt_veloc_N, notch_axis_t::filter, notch_axis_t::freq, init_butterworth_2_low_pass(), notch_filter_init(), oneloop_andi_filt_cutoff, oneloop_andi_filt_cutoff_v, oneloop_notch, Oneloop_notch_t::pitch, Oneloop_notch_t::roll, and Oneloop_notch_t::yaw.

Referenced by oneloop_andi_enter(), and oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_poles()

void init_poles ( void )

Initialize Position of Poles.

Definition at line 1048 of file oneloop_andi.c.

References act_dynamics, ec_poles(), PolePlacement::omega_n, PolePlacement::p3, p_alt_e, p_alt_rm, p_att_e, p_att_rm, p_head_e, p_head_rm, p_pos_e, p_pos_rm, w_approx(), and PolePlacement::zeta.

Referenced by oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_poles_att()

void init_poles_att ( void )

Initialize Position of Poles.

Definition at line 1031 of file oneloop_andi.c.

References ec_poles(), PolePlacement::omega_n, p_att_e, p_att_rm, p_head_e, and p_head_rm.

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_poles_pos()

void init_poles_pos ( void )

Definition at line 1036 of file oneloop_andi.c.

References act_dynamics, ec_poles(), PolePlacement::omega_n, PolePlacement::p3, p_alt_e, p_alt_rm, p_att_rm, p_pos_e, p_pos_rm, and w_approx().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ integrate_nd()

void integrate_nd	(	float	dt,
		float	a[],
		float	a_dot[],
		int	n
	)

Integrate in time 3D array.

Definition at line 695 of file oneloop_andi.c.

Referenced by rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), and rm_3rd_pos().

Here is the caller graph for this function:

◆ k_e_1_2_f_v2()

static float k_e_1_2_f_v2	(	float	omega,
		float	zeta
	)

static

Error Controller Gain Design.

Definition at line 593 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_e_1_3_f_v2()

static float k_e_1_3_f_v2	(	float	omega_n,
		UNUSED float	zeta,
		float	p1
	)

static

Definition at line 605 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_e_2_2_f_v2()

static float k_e_2_2_f_v2	(	float	omega,
		float	zeta
	)

static

Definition at line 599 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_e_2_3_f_v2()

static float k_e_2_3_f_v2	(	float	omega_n,
		float	zeta,
		float	p1
	)

static

Definition at line 611 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_e_3_3_f_v2()

static float k_e_3_3_f_v2	(	float	omega_n,
		float	zeta,
		float	p1
	)

static

Definition at line 618 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_rm_1_3_f()

static float k_rm_1_3_f	(	float	omega_n,
		float	zeta,
		float	p1
	)

static

Reference Model Gain Design.

Definition at line 627 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_rm_2_3_f()

static float k_rm_2_3_f	(	float	omega_n,
		float	zeta,
		float	p1
	)

static

Definition at line 634 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_rm_3_3_f()

static float k_rm_3_3_f	(	float	omega_n,
		float	zeta,
		float	p1
	)

static

Definition at line 641 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ normalize_nu()

void normalize_nu ( void )

Function to normalize the pseudo control vector.

Definition at line 1961 of file oneloop_andi.c.

References ANDI_OUTPUTS, nu, nu_n, ratio_vn_v, WLS_t::v, and WLS_one_p.

Referenced by oneloop_andi_run().

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_propagate_filters()

void oneloop_andi_propagate_filters ( void )

Propagate the filters.

Definition at line 1313 of file oneloop_andi.c.

References ACCEL_FLOAT_OF_BFP, accely_filt, airspeed_filt, Oneloop_CF_t::ax, Oneloop_CF_t::ay, Oneloop_CF_t::az, calc_model(), cf, CF4_t::feedback, CF2_t::feedback, CF4_t::feedback_filt, CF2_t::feedback_filt, filt_veloc_D, filt_veloc_E, filt_veloc_N, notch_axis_t::filter, Butterworth4LowPass::lp2, CF4_t::model, CF2_t::model, CF4_t::model_filt, CF2_t::model_filt, notch_filter_update(), SecondOrderLowPass::o, oneloop_notch, CF4_t::out, CF2_t::out, Oneloop_CF_t::p, FloatRates::p, Oneloop_CF_t::p_dot, Oneloop_notch_t::pitch, Oneloop_CF_t::q, FloatRates::q, Oneloop_CF_t::q_dot, Oneloop_CF_t::r, FloatRates::r, Oneloop_CF_t::r_dot, reinit_all_cf(), Oneloop_notch_t::roll, stateGetAccelBody_i(), stateGetAccelNed_f(), stateGetAirspeed_f(), stateGetBodyRates_f(), stateGetSpeedNed_f(), update_butterworth_2_low_pass(), update_butterworth_4_low_pass(), NedCoor_f::x, NedCoor_f::y, Oneloop_notch_t::yaw, and NedCoor_f::z.

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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

Definition at line 1629 of file oneloop_andi.c.

References a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, act_dynamics, act_max, act_min, actuator_state_1l, andi_du, andi_du_n, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, AP_MODE_ATTITUDE_DIRECT, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, autopilot, Oneloop_CF_t::ax, Oneloop_CF_t::ay, Oneloop_CF_t::az, bwls_1l, calc_normalization(), cf, chirp_on, Stabilization::cmd, commands, CTRL_ANDI, CTRL_INDI, ctrl_off, OneloopGeneral::ctrl_type, RW_skew::deg, ec_3rd(), ec_3rd_att(), ec_3rd_pos(), EFF_MAT_G, RW_Model::ele_pref, eulers_zxy, eulers_zxy_des, rotwing_state_t::fail_pusher_motor, filt_veloc_D, filt_veloc_E, filt_veloc_N, float_eulers_of_quat_zxy(), float_vect_copy(), float_vect_sum(), G1G2_oneloop(), g2_ff, G2_RW, get_act_state_oneloop(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, heading_manual, rotwing_state_t::hover_motors_enabled, init_controller(), OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_e, k_att_e_indi, k_pos_e, k_pos_e_indi, max_a_nav, max_j_ang, max_j_nav, MAX_PPRZ, max_v_nav, pprz_autopilot::mode, normalize_nu(), nu, num_thrusters_oneloop, SecondOrderLowPass::o, oneloop_andi, ONELOOP_ANDI_DEBUG_MODE, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, oneloop_andi_propagate_filters(), oneloop_andi_RM(), CF4_t::out, CF2_t::out, Oneloop_CF_t::p, Oneloop_CF_t::p_dot, FloatEulers::phi, pitch_pref, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, Oneloop_CF_t::q, Oneloop_CF_t::q_dot, Oneloop_CF_t::r, Oneloop_CF_t::r_dot, RADIO_AUX4, RADIO_AUX5, radio_control, ratio_u_un, rotwing_state, RW, RW_Model::skew, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, stabilization, stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), FloatEulers::theta, theta_pref_max, pprz_autopilot::throttle, WLS_t::u, WLS_t::u_max, WLS_t::u_min, u_pref, WLS_t::u_pref, use_increment, RadioControl::values, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, wls_alloc(), WLS_one_p, WLS_t::Wu, Wu_backup, Wu_quad_motors_fwd, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by oneloop_from_nav(), and stabilization_attitude_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_andi_sideslip()

float oneloop_andi_sideslip ( void )

Function to calculate corrections for sideslip.

Definition at line 2042 of file oneloop_andi.c.

References accely_filt, airspeed_filt, eulers_zxy, fwd_sideslip_gain, g, SecondOrderLowPass::o, ONELOOP_ANDI_MAX_BANK, FloatEulers::phi, and FloatEulers::theta.

Referenced by oneloop_andi_RM().

Here is the caller graph for this function:

◆ oneloop_from_nav()

void oneloop_from_nav ( bool in_flight )

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

Definition at line 2004 of file oneloop_andi.c.

References RotorcraftNavigation::climb, OneloopGeneral::ctrl_type, 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(), 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, FloatVect3::y, NedCoor_f::y, EnuCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Here is the call graph for this function:

◆ positive_non_zero()

static float positive_non_zero ( float input )

static

Function to make sure that inputs are positive non zero vaues.

Definition at line 583 of file oneloop_andi.c.

Referenced by acc_body_bound(), calc_normalization(), chirp_pos(), ec_poles(), k_e_1_2_f_v2(), k_e_1_3_f_v2(), k_e_2_2_f_v2(), k_e_2_3_f_v2(), k_e_3_3_f_v2(), k_rm_1_3_f(), k_rm_2_3_f(), k_rm_3_3_f(), oneloop_andi_init(), vect_bound_nd(), and w_approx().

Here is the caller graph for this function:

◆ PRINT_CONFIG_VAR() [1/4]

PRINT_CONFIG_VAR ( ONELOOP_ANDI_FILT_CUTOFF )

Referenced by accel_cb(), actuators_dshot_arch_init(), gyro_cb(), mag_cb(), nps_autopilot_run_step(), and superbitrf_event().

Here is the caller graph for this function:

◆ PRINT_CONFIG_VAR() [2/4]

PRINT_CONFIG_VAR ( ONELOOP_ANDI_FILT_CUTOFF_P )

◆ PRINT_CONFIG_VAR() [3/4]

PRINT_CONFIG_VAR ( ONELOOP_ANDI_FILT_CUTOFF_Q )

◆ PRINT_CONFIG_VAR() [4/4]

PRINT_CONFIG_VAR ( ONELOOP_ANDI_FILT_CUTOFF_R )

◆ reinit_all_cf()

void reinit_all_cf ( bool reinit )

Reinitialize all the Complementary Filters.

Definition at line 1261 of file oneloop_andi.c.

References Oneloop_CF_t::ax, Oneloop_CF_t::ay, Oneloop_CF_t::az, cf, Oneloop_CF_t::p, Oneloop_CF_t::p_dot, Oneloop_CF_t::q, Oneloop_CF_t::q_dot, Oneloop_CF_t::r, Oneloop_CF_t::r_dot, reinit_cf2(), and reinit_cf4().

Referenced by oneloop_andi_enter(), and oneloop_andi_propagate_filters().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reinit_cf2()

void reinit_cf2	(	struct CF2_t *	cf,
		bool	reinit
	)

Reinitialize 2nd Order CF if new frequency setting or if forced.

Definition at line 1243 of file oneloop_andi.c.

References cf, get_butterworth_2_low_pass(), and init_butterworth_2_low_pass().

Referenced by reinit_all_cf().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reinit_cf4()

void reinit_cf4	(	struct CF4_t *	cf,
		bool	reinit
	)

Reinitialize 4th Order CF if new frequency setting or if forced.

Definition at line 1252 of file oneloop_andi.c.

References cf, get_butterworth_4_low_pass(), and init_butterworth_4_low_pass().

Referenced by reinit_all_cf().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reshape_wind()

void reshape_wind ( void )

Definition at line 2190 of file oneloop_andi.c.

References acc_body_bound(), airspeed_filt, eulers_zxy, FLOAT_ANGLE_NORMALIZE, FLOAT_VECT2_NORM, force_forward, gi_unbounded_airspeed_sp, OneloopGeneral::gui_state, Gains3rdOrder::k2, k_pos_rm, max_a_nav, max_as, min_as, nav_target, nav_target_new, SecondOrderLowPass::o, oneloop_andi, ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD, FloatEulers::psi, VECT2_DIFF, vect_bound_nd(), OneloopGuidanceState::vel, FloatVect2::x, and FloatVect2::y.

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_1st_pos()

void rm_1st_pos	(	float	dt,
		float	x_2d_ref[],
		float	x_3d_ref[],
		float	x_2d_des[],
		float	k3_rm[],
		float	x_3d_bound,
		int	n
	)

Reference Model Definition for 3rd order system specific to positioning with bounds.

Parameters

dt	Delta time [s]
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_2d_des	Desired 3rd order signal
k3_rm	Reference Model Gain 3rd order signal
x_3d_bound	Bound for the 4th order reference signal
n	Number of dimensions

Definition at line 901 of file oneloop_andi.c.

References err_nd(), float_vect_copy(), integrate_nd(), and vect_bound_nd().

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_2nd()

void rm_2nd	(	float	dt,
		float *	x_ref,
		float *	x_d_ref,
		float *	x_2d_ref,
		float	x_des,
		float	k1_rm,
		float	k2_rm
	)

Reference Model Definition for 2nd order system.

Parameters

dt	Delta time [s]
x_ref	Reference signal 1st order
x_d_ref	Reference signal 2nd order
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_des	Desired 1st order signal
k1_rm	Reference Model Gain 1st order signal
k2_rm	Reference Model Gain 2nd order signal
k3_rm	Reference Model Gain 3rd order signal

Definition at line 922 of file oneloop_andi.c.

◆ rm_2nd_pos()

void rm_2nd_pos	(	float	dt,
		float	x_d_ref[],
		float	x_2d_ref[],
		float	x_3d_ref[],
		float	x_d_des[],
		float	k2_rm[],
		float	k3_rm[],
		float	x_2d_bound,
		float	x_3d_bound,
		int	n
	)

Reference Model Definition for 3rd order system specific to positioning with bounds.

Parameters

dt	Delta time [s]
x_d_ref	Reference signal 2nd order
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_d_des	Desired 2nd order signal
k2_rm	Reference Model Gain 2nd order signal
k3_rm	Reference Model Gain 3rd order signal
x_2d_bound	Bound for the 3rd order reference signal
x_3d_bound	Bound for the 4th order reference signal
n	Number of dimensions

Definition at line 879 of file oneloop_andi.c.

References err_nd(), float_vect_copy(), integrate_nd(), and vect_bound_nd().

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_3rd()

void rm_3rd	(	float	dt,
		float *	x_ref,
		float *	x_d_ref,
		float *	x_2d_ref,
		float *	x_3d_ref,
		float	x_des,
		float	k1_rm,
		float	k2_rm,
		float	k3_rm
	)

Reference Model Definition for 3rd order system.

Parameters

dt	Delta time [s]
x_ref	Reference signal 1st order
x_d_ref	Reference signal 2nd order
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_des	Desired 1st order signal
k1_rm	Reference Model Gain 1st order signal
k2_rm	Reference Model Gain 2nd order signal
k3_rm	Reference Model Gain 3rd order signal

Definition at line 799 of file oneloop_andi.c.

◆ rm_3rd_attitude()

void rm_3rd_attitude	(	float	dt,
		float	x_ref[3],
		float	x_d_ref[3],
		float	x_2d_ref[3],
		float	x_3d_ref[3],
		float	x_des[3],
		bool	ow_psi,
		float	psi_overwrite[4],
		float	k1_rm[3],
		float	k2_rm[3],
		float	k3_rm[3],
		float	max_ang_jerk
	)

Reference Model Definition for 3rd order system with attitude conversion functions.

Parameters

dt	Delta time [s]
x_ref	Reference signal 1st order
x_d_ref	Reference signal 2nd order
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_des	Desired 1st order signal
ow_psi	Overwrite psi (for navigation functions) [bool]
psi_overwrite	Overwrite psi (for navigation functions) [values]
k1_rm	Reference Model Gain 1st order signal
k2_rm	Reference Model Gain 2nd order signal
k3_rm	Reference Model Gain 3rd order signal

Definition at line 758 of file oneloop_andi.c.

References err_nd(), float_euler_dot_of_rates_vec(), float_rates_of_euler_dot_vec(), float_vect_copy(), integrate_nd(), and vect_bound_nd().

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_3rd_head()

void rm_3rd_head	(	float	dt,
		float *	x_ref,
		float *	x_d_ref,
		float *	x_2d_ref,
		float *	x_3d_ref,
		float	x_des,
		float	k1_rm,
		float	k2_rm,
		float	k3_rm
	)

Reference Model Definition for 3rd order system specific to the heading angle.

Parameters

dt	Delta time [s]
x_ref	Reference signal 1st order
x_d_ref	Reference signal 2nd order
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_des	Desired 1st order signal
k1_rm	Reference Model Gain 1st order signal
k2_rm	Reference Model Gain 2nd order signal
k3_rm	Reference Model Gain 3rd order signal

Definition at line 821 of file oneloop_andi.c.

◆ rm_3rd_pos()

void rm_3rd_pos	(	float	dt,
		float	x_ref[],
		float	x_d_ref[],
		float	x_2d_ref[],
		float	x_3d_ref[],
		float	x_des[],
		float	k1_rm[],
		float	k2_rm[],
		float	k3_rm[],
		float	x_d_bound,
		float	x_2d_bound,
		float	x_3d_bound,
		int	n
	)

Reference Model Definition for 3rd order system specific to positioning with bounds.

Parameters

dt	Delta time [s]
x_ref	Reference signal 1st order
x_d_ref	Reference signal 2nd order
x_2d_ref	Reference signal 3rd order
x_3d_ref	Reference signal 4th order
x_des	Desired 1st order signal
k1_rm	Reference Model Gain 1st order signal
k2_rm	Reference Model Gain 2nd order signal
k3_rm	Reference Model Gain 3rd order signal
x_d_bound	Bound for the 2nd order reference signal
x_2d_bound	Bound for the 3rd order reference signal
x_3d_bound	Bound for the 4th order reference signal
n	Number of dimensions

Definition at line 849 of file oneloop_andi.c.

References bound_v_from_a(), err_nd(), float_vect_copy(), integrate_nd(), and vect_bound_nd().

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_eff_mat_guid_oneloop_andi()

static void send_eff_mat_guid_oneloop_andi	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 506 of file oneloop_andi.c.

References ANDI_NUM_ACT_TOT, dev, and EFF_MAT_RW.

Referenced by oneloop_andi_init().

Here is the caller graph for this function:

◆ send_eff_mat_stab_oneloop_andi()

static void send_eff_mat_stab_oneloop_andi	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 495 of file oneloop_andi.c.

References ANDI_NUM_ACT, dev, and EFF_MAT_RW.

Referenced by oneloop_andi_init().

Here is the caller graph for this function:

◆ send_guidance_oneloop_andi()

static void send_guidance_oneloop_andi	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 533 of file oneloop_andi.c.

References OneloopGuidanceRef::acc, OneloopGuidanceState::acc, dev, OneloopGeneral::gui_ref, OneloopGeneral::gui_state, OneloopGuidanceRef::jer, oneloop_andi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, OneloopGuidanceRef::vel, and OneloopGuidanceState::vel.

Referenced by oneloop_andi_init().

Here is the caller graph for this function:

◆ send_oneloop_andi()

static void send_oneloop_andi	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 513 of file oneloop_andi.c.

References actuator_state_1l, ANDI_NUM_ACT, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, dev, eulers_zxy_des, oneloop_andi, FloatEulers::phi, FloatEulers::psi, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, and FloatEulers::theta.

Referenced by oneloop_andi_init().

Here is the caller graph for this function:

◆ send_oneloop_debug()

static void send_oneloop_debug	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 566 of file oneloop_andi.c.

References Oneloop_CF_t::ax, Oneloop_CF_t::ay, Oneloop_CF_t::az, cf, debug_vect(), dev, CF4_t::model, CF2_t::model, Oneloop_CF_t::p, Oneloop_CF_t::p_dot, Oneloop_CF_t::q, Oneloop_CF_t::q_dot, Oneloop_CF_t::r, and Oneloop_CF_t::r_dot.

Referenced by oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_wls_u_oneloop()

static void send_wls_u_oneloop	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 491 of file oneloop_andi.c.

References dev, send_wls_u(), and WLS_one_p.

Referenced by oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_wls_v_oneloop()

static void send_wls_v_oneloop	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 487 of file oneloop_andi.c.

References dev, send_wls_v(), and WLS_one_p.

Referenced by oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ vect_bound_nd()

void vect_bound_nd	(	float	vect[],
		float	bound,
		int	n
	)

Scale a 3D array to within a 3D bound.

Definition at line 704 of file oneloop_andi.c.

References float_vect_norm(), positive_non_zero(), and scale.

Referenced by ec_3rd_att(), ec_3rd_pos(), reshape_wind(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), and rm_3rd_pos().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ w_approx()

static float w_approx	(	float	p1,
		float	p2,
		float	p3,
		float	rm_k
	)

static

Third Order to First Order Dynamics Approximation.

Parameters

p1	Pole 1
p2	Pole 2
p3	Pole 3
rm_k	Reference Model Gain

Definition at line 1002 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller(), init_poles(), and init_poles_pos().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ A_chirp

float A_chirp = 1.0

Definition at line 427 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ a_thrust

float a_thrust = 0.0

static

Definition at line 353 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ accely_filt

Butterworth2LowPass accely_filt

static

Definition at line 462 of file oneloop_andi.c.

Referenced by init_filter(), oneloop_andi_propagate_filters(), and oneloop_andi_sideslip().

◆ act_dynamics

float act_dynamics[ANDI_NUM_ACT_TOT] = = {1}

Definition at line 178 of file oneloop_andi.c.

Referenced by calc_normalization(), G1G2_oneloop(), init_controller(), init_poles(), init_poles_pos(), oneloop_andi_init(), and oneloop_andi_run().

◆ act_dynamics_d

float act_dynamics_d[ANDI_NUM_ACT_TOT]

static

Definition at line 351 of file oneloop_andi.c.

Referenced by calc_normalization(), and get_act_state_oneloop().

◆ act_max

float act_max[ANDI_NUM_ACT_TOT] = = {MAX_PPRZ}

Definition at line 184 of file oneloop_andi.c.

Referenced by calc_normalization(), get_act_state_oneloop(), and oneloop_andi_run().

◆ act_max_norm

float act_max_norm[ANDI_NUM_ACT_TOT] = = {1.0}

Definition at line 196 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ act_min

float act_min[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 190 of file oneloop_andi.c.

Referenced by calc_normalization(), get_act_state_oneloop(), and oneloop_andi_run().

◆ act_min_norm

float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 202 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ actuator_is_servo

bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0}

Definition at line 171 of file oneloop_andi.c.

◆ actuator_state_1l

float actuator_state_1l[ANDI_NUM_ACT]

Definition at line 352 of file oneloop_andi.c.

Referenced by calc_G1_G2_RW(), calc_model(), get_act_state_oneloop(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), and send_oneloop_andi().

◆ airspeed_filt

Butterworth2LowPass airspeed_filt

static

Definition at line 463 of file oneloop_andi.c.

Referenced by G1G2_oneloop(), init_filter(), oneloop_andi_propagate_filters(), oneloop_andi_sideslip(), and reshape_wind().

◆ andi_du

float andi_du[ANDI_NUM_ACT_TOT]

Definition at line 347 of file oneloop_andi.c.

Referenced by oneloop_andi_init(), and oneloop_andi_run().

◆ andi_du_n

float andi_du_n[ANDI_NUM_ACT_TOT]

static

Definition at line 348 of file oneloop_andi.c.

Referenced by oneloop_andi_init(), and oneloop_andi_run().

◆ andi_u

float andi_u[ANDI_NUM_ACT_TOT]

Definition at line 346 of file oneloop_andi.c.

Referenced by get_act_state_oneloop(), oneloop_andi_init(), and oneloop_andi_run().

◆ bwls_1l

float* bwls_1l[ANDI_OUTPUTS]

Definition at line 453 of file oneloop_andi.c.

Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), and oneloop_andi_run().

◆ cf

struct Oneloop_CF_t cf

Definition at line 412 of file oneloop_andi.c.

Referenced by calc_model(), init_all_cf(), init_cf2(), init_cf4(), oneloop_andi_propagate_filters(), oneloop_andi_run(), reinit_all_cf(), reinit_cf2(), reinit_cf4(), and send_oneloop_debug().

◆ chirp_axis

int8_t chirp_axis = 0

Definition at line 428 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ chirp_first_call

bool chirp_first_call = true

Definition at line 421 of file oneloop_andi.c.

Referenced by chirp_call(), and oneloop_andi_RM().

◆ chirp_on

bool chirp_on = false

Definition at line 420 of file oneloop_andi.c.

Referenced by chirp_call(), oneloop_andi_RM(), and oneloop_andi_run().

◆ ctrl_off

bool ctrl_off = false

Definition at line 376 of file oneloop_andi.c.

Referenced by G1G2_oneloop(), and oneloop_andi_run().

◆ dt_1l

float dt_1l = 1./PERIODIC_FREQUENCY

static

Definition at line 339 of file oneloop_andi.c.

Referenced by calc_normalization(), and oneloop_andi_RM().

◆ EFF_MAT_G

float EFF_MAT_G[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

Definition at line 454 of file oneloop_andi.c.

Referenced by G1G2_oneloop(), oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().

◆ eulers_zxy

struct FloatEulers eulers_zxy

Definition at line 354 of file oneloop_andi.c.

Referenced by calc_model(), oneloop_andi_enter(), oneloop_andi_RM(), oneloop_andi_run(), oneloop_andi_sideslip(), and reshape_wind().

◆ eulers_zxy_des

struct FloatEulers eulers_zxy_des

Definition at line 354 of file oneloop_andi.c.

Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), and send_oneloop_andi().

◆ f0_chirp

float f0_chirp = 0.8 / (2.0 * M_PI)

Definition at line 424 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ f1_chirp

float f1_chirp = 0.8 / (2.0 * M_PI)

Definition at line 425 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ filt_veloc_D

Butterworth2LowPass filt_veloc_D

static

Definition at line 461 of file oneloop_andi.c.

Referenced by init_filter(), oneloop_andi_propagate_filters(), and oneloop_andi_run().

◆ filt_veloc_E

Butterworth2LowPass filt_veloc_E

static

Definition at line 460 of file oneloop_andi.c.

Referenced by init_filter(), oneloop_andi_propagate_filters(), and oneloop_andi_run().

◆ filt_veloc_N

Butterworth2LowPass filt_veloc_N

static

Definition at line 459 of file oneloop_andi.c.

Referenced by init_filter(), oneloop_andi_propagate_filters(), and oneloop_andi_run().

◆ fwd_sideslip_gain

float fwd_sideslip_gain = 0.2

Definition at line 280 of file oneloop_andi.c.

Referenced by oneloop_andi_sideslip().

◆ g

float g = 9.81

static

Definition at line 340 of file oneloop_andi.c.

Referenced by calc_model(), and oneloop_andi_sideslip().

◆ g2_ff

float g2_ff = 0.0

static

Definition at line 354 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ gi_unbounded_airspeed_sp

float gi_unbounded_airspeed_sp = 0.0

Definition at line 343 of file oneloop_andi.c.

Referenced by reshape_wind().

◆ heading_manual

bool heading_manual = false

Definition at line 369 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ k_as

float k_as = 2.0

Definition at line 341 of file oneloop_andi.c.

◆ k_att_e

struct Gains3rdOrder k_att_e

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_att_e_indi

struct Gains3rdOrder k_att_e_indi

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_att_rm

struct Gains3rdOrder k_att_rm

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_RM().

◆ k_pos_e

struct Gains3rdOrder k_pos_e

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_pos_e_indi

struct Gains3rdOrder k_pos_e_indi

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_pos_rm

struct Gains3rdOrder k_pos_rm

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), oneloop_andi_RM(), and reshape_wind().

◆ max_a_nav

float max_a_nav = 4.0

Definition at line 250 of file oneloop_andi.c.

Referenced by init_controller(), oneloop_andi_RM(), oneloop_andi_run(), and reshape_wind().

◆ max_as

float max_as = 19.0f

Definition at line 270 of file oneloop_andi.c.

Referenced by guidance_set_min_max_airspeed(), and reshape_wind().

◆ max_j_ang

float max_j_ang = 1000.0

Definition at line 262 of file oneloop_andi.c.

Referenced by calc_normalization(), oneloop_andi_RM(), and oneloop_andi_run().

◆ max_j_nav

float max_j_nav = 500.0

Definition at line 256 of file oneloop_andi.c.

Referenced by calc_normalization(), oneloop_andi_RM(), and oneloop_andi_run().

◆ max_r

float max_r = RadOfDeg(120.0)

Definition at line 163 of file oneloop_andi.c.

Referenced by attitude_ref_quat_int_set_max_r(), and oneloop_andi_RM().

◆ max_v_nav

float max_v_nav = 5.0

Definition at line 268 of file oneloop_andi.c.

Referenced by init_controller(), oneloop_andi_RM(), and oneloop_andi_run().

◆ max_v_nav_v

float max_v_nav_v = 1.5

Definition at line 276 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ min_as

float min_as = 0.0f

Definition at line 271 of file oneloop_andi.c.

Referenced by guidance_set_min_max_airspeed(), and reshape_wind().

◆ nav_target

float nav_target[3]

static

Definition at line 337 of file oneloop_andi.c.

Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), and reshape_wind().

◆ nav_target_new

float nav_target_new[3]

static

Definition at line 338 of file oneloop_andi.c.

Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), and reshape_wind().

◆ nu

float nu[ANDI_OUTPUTS]

Definition at line 349 of file oneloop_andi.c.

Referenced by normalize_nu(), oneloop_andi_init(), and oneloop_andi_run().

◆ nu_n

float nu_n[ANDI_OUTPUTS]

Definition at line 350 of file oneloop_andi.c.

Referenced by normalize_nu(), and oneloop_andi_init().

◆ nu_norm_max

float nu_norm_max = 1.0

Definition at line 208 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ num_thrusters_oneloop

float num_thrusters_oneloop = 4.0

Definition at line 104 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ oneloop_andi

struct OneloopGeneral oneloop_andi

Definition at line 330 of file oneloop_andi.c.

Referenced by chirp_call(), oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), oneloop_from_nav(), reshape_wind(), send_guidance_oneloop_andi(), send_oneloop_andi(), and stabilization_attitude_run().

◆ oneloop_andi_filt_cutoff

float oneloop_andi_filt_cutoff = 2.0

Definition at line 116 of file oneloop_andi.c.

Referenced by init_all_cf(), and init_filter().

◆ oneloop_andi_filt_cutoff_a

float oneloop_andi_filt_cutoff_a = 2.0

Definition at line 122 of file oneloop_andi.c.

Referenced by init_all_cf().

◆ oneloop_andi_filt_cutoff_p

float oneloop_andi_filt_cutoff_p = 20.0

Definition at line 141 of file oneloop_andi.c.

Referenced by init_all_cf().

◆ oneloop_andi_filt_cutoff_pos

float oneloop_andi_filt_cutoff_pos = 2.0

Definition at line 134 of file oneloop_andi.c.

◆ oneloop_andi_filt_cutoff_q

float oneloop_andi_filt_cutoff_q = 20.0

Definition at line 148 of file oneloop_andi.c.

Referenced by init_all_cf().

◆ oneloop_andi_filt_cutoff_r

float oneloop_andi_filt_cutoff_r = 20.0

Definition at line 155 of file oneloop_andi.c.

Referenced by init_all_cf().

◆ oneloop_andi_filt_cutoff_v

float oneloop_andi_filt_cutoff_v = 2.0

Definition at line 128 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_notch

struct Oneloop_notch_t oneloop_notch

static

Definition at line 412 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ p_alt_e

struct PolePlacement p_alt_e

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), init_poles(), and init_poles_pos().

◆ p_alt_rm

struct PolePlacement p_alt_rm

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), init_poles(), and init_poles_pos().

◆ p_att_e

struct PolePlacement p_att_e

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), init_poles(), and init_poles_att().

◆ p_att_rm

struct PolePlacement p_att_rm

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), init_poles(), init_poles_att(), and init_poles_pos().

◆ p_head_e

struct PolePlacement p_head_e

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), init_poles(), and init_poles_att().

◆ p_head_rm

struct PolePlacement p_head_rm

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), init_poles(), and init_poles_att().

◆ p_pos_e

struct PolePlacement p_pos_e

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), init_poles(), and init_poles_pos().

◆ p_pos_rm

struct PolePlacement p_pos_rm

Definition at line 429 of file oneloop_andi.c.

Referenced by init_controller(), init_poles(), and init_poles_pos().

◆ p_ref_0

float p_ref_0[3] = {0.0, 0.0, 0.0}

Definition at line 429 of file oneloop_andi.c.

Referenced by chirp_call(), chirp_pos(), and oneloop_andi_RM().

◆ pitch_pref

float pitch_pref = 0

static

Definition at line 379 of file oneloop_andi.c.

Referenced by chirp_pos(), and oneloop_andi_run().

◆ psi_des_deg

float psi_des_deg = 0.0

Definition at line 363 of file oneloop_andi.c.

Referenced by oneloop_andi_enter(), oneloop_andi_RM(), and oneloop_andi_run().

◆ psi_des_rad

float psi_des_rad = 0.0

Definition at line 362 of file oneloop_andi.c.

Referenced by oneloop_andi_enter(), oneloop_andi_RM(), and oneloop_andi_run().

◆ psi_vec

float psi_vec[4] = {0.0, 0.0, 0.0, 0.0}

static

Definition at line 364 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ ratio_u_un

float ratio_u_un[ANDI_NUM_ACT_TOT]

Definition at line 455 of file oneloop_andi.c.

Referenced by calc_normalization(), G1G2_oneloop(), oneloop_andi_RM(), and oneloop_andi_run().

◆ ratio_vn_v

float ratio_vn_v[ANDI_OUTPUTS]

Definition at line 456 of file oneloop_andi.c.

Referenced by calc_normalization(), G1G2_oneloop(), normalize_nu(), and oneloop_andi_RM().

◆ stab_att_sp_euler_1l

struct Int32Eulers stab_att_sp_euler_1l

Definition at line 354 of file oneloop_andi.c.

◆ stab_att_sp_quat_1l

struct Int32Quat stab_att_sp_quat_1l

Definition at line 354 of file oneloop_andi.c.

◆ t_0_chirp

float t_0_chirp = 0.0

Definition at line 423 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ t_chirp

float t_chirp = 45.0

Definition at line 426 of file oneloop_andi.c.

Referenced by chirp_call(), chirp_pos(), and oneloop_andi_RM().

◆ temp_pitch

int16_t temp_pitch = 0

Definition at line 342 of file oneloop_andi.c.

Referenced by indi_apply_actuator_models().

◆ theta_pref_max

float theta_pref_max = RadOfDeg(20.0)

Definition at line 228 of file oneloop_andi.c.

Referenced by chirp_pos(), and oneloop_andi_run().

◆ time_elapsed_chirp

float time_elapsed_chirp = 0.0

Definition at line 422 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ u_pref

float u_pref[ANDI_NUM_ACT_TOT] = {0.0}

static

Definition at line 214 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ use_increment

float use_increment = 0.0

static

Definition at line 336 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ WLS_one_p

struct WLS_t WLS_one_p

Initial value:

= {
  .nu        = ANDI_NUM_ACT_TOT,
  .nv        = ANDI_OUTPUTS,
  .gamma_sq  = 100.0,
  .v         = {0.0},
 
 
 
  .Wv        = {1.0},
 
 
 
 
  .Wu        = {1.0},
 
  .u_pref    = {0.0},
  .u_min     = {0.0},
  .u_max     = {0.0},
  .PC        = 0.0,
  .SC        = 0.0,
  .iter      = 0
}

Definition at line 379 of file oneloop_andi.c.

Referenced by normalize_nu(), oneloop_andi_RM(), oneloop_andi_run(), send_wls_u_oneloop(), and send_wls_v_oneloop().

◆ Wu_backup

float Wu_backup[ANDI_NUM_ACT_TOT] = {1.0}

static

Definition at line 412 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ Wu_quad_motors_fwd

float Wu_quad_motors_fwd = 6.0

Definition at line 286 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ Wv_backup

float Wv_backup[ANDI_OUTPUTS] = {1.0}

static

Definition at line 406 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ yaw_stick_in_auto

bool yaw_stick_in_auto = false

Definition at line 374 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

Macros

Functions

Variables

Macro Definition Documentation

◆ ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD

◆ ONELOOP_ANDI_DEBUG_MODE

◆ ONELOOP_ANDI_MAX_BANK

◆ ONELOOP_ANDI_MAX_PHI

◆ ONELOOP_ANDI_MAX_THETA

◆ ONELOOP_ANDI_SCHEDULING

◆ USE_YAW_NOTCH

Function Documentation

◆ acc_body_bound()

◆ autopilot_in_flight_end_detection()

◆ bound_v_from_a()

◆ calc_model()

◆ calc_normalization()

◆ chirp_call()

◆ chirp_pos()

◆ chirp_pos_a_ref()

◆ chirp_pos_j_ref()

◆ chirp_pos_p_ref()

◆ chirp_pos_v_ref()

◆ debug_vect()

◆ ec_3rd()

◆ ec_3rd_att()

◆ ec_3rd_pos()

◆ ec_poles()

◆ err_nd()

◆ err_sum_nd()

◆ float_euler_dot_of_rates_vec()

◆ float_rates_of_euler_dot_vec()

◆ G1G2_oneloop()

◆ get_act_state_oneloop()

◆ guidance_set_min_max_airspeed()

◆ init_all_cf()

◆ init_cf2()

◆ init_cf4()

◆ init_controller()

◆ init_filter()

◆ init_poles()

◆ init_poles_att()

◆ init_poles_pos()

◆ integrate_nd()

◆ k_e_1_2_f_v2()

◆ k_e_1_3_f_v2()

◆ k_e_2_2_f_v2()

◆ k_e_2_3_f_v2()

◆ k_e_3_3_f_v2()

◆ k_rm_1_3_f()

◆ k_rm_2_3_f()

◆ k_rm_3_3_f()

◆ normalize_nu()

◆ oneloop_andi_enter()

◆ oneloop_andi_init()

◆ oneloop_andi_propagate_filters()

◆ oneloop_andi_RM()

◆ oneloop_andi_run()

◆ oneloop_andi_sideslip()

◆ oneloop_from_nav()

◆ positive_non_zero()

◆ PRINT_CONFIG_VAR() [1/4]

◆ PRINT_CONFIG_VAR() [2/4]

◆ PRINT_CONFIG_VAR() [3/4]

◆ PRINT_CONFIG_VAR() [4/4]

◆ reinit_all_cf()

◆ reinit_cf2()

◆ reinit_cf4()

◆ reshape_wind()

◆ rm_1st_pos()

◆ rm_2nd()

◆ rm_2nd_pos()

◆ rm_3rd()

◆ rm_3rd_attitude()

◆ rm_3rd_head()

◆ rm_3rd_pos()

◆ send_eff_mat_guid_oneloop_andi()

◆ send_eff_mat_stab_oneloop_andi()

◆ send_guidance_oneloop_andi()

◆ send_oneloop_andi()