#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 "math/wls/wls_alloc.h"
#include "modules/nav/nav_rotorcraft_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"

Include dependency graph for oneloop_andi.c:

Macros
#define	ONELOOP_ANDI_FILT_CUTOFF_P 20.0

#define	ONELOOP_ANDI_FILT_CUTOFF_Q 20.0

#define	ONELOOP_ANDI_FILT_CUTOFF_R 20.0

#define	ONELOOP_ANDI_DEBUG_MODE FALSE;

#define	ONELOOP_ANDI_AC_HAS_PUSHER FALSE;

#define	ONELOOP_ANDI_PUSHER_IDX 0

#define	GUIDANCE_INDI_SPEED_GAIN 1.8

#define	GUIDANCE_INDI_SPEED_GAINZ 1.8

#define	GUIDANCE_INDI_POS_GAIN 0.5

#define	GUIDANCE_INDI_POS_GAINZ 0.5

#define	GUIDANCE_INDI_LIFTD_ASQ 0.20

#define	GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ1212)

#define	GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)

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

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

void	sum_g1g2_1l (void)
	Function that sums g1 and g2 to obtain the g1_g2 matrix. 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	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	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])
	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])
	Error Controller Definition for 3rd order system specific to attitude. More...

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

static void	send_eff_mat_g_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 float	positive_non_zero (float input)
	Function to make sure that inputs are positive non zero vaues. More...

static float	k_e_1_3_f (float p1, float p2, float p3)
	Error Controller Gain Design. More...

static float	k_e_2_3_f (float p1, float p2, float p3)

static float	k_e_3_3_f (float p1, float p2, float p3)

static float	k_e_1_2_f (float p1, float p2)

static float	k_e_2_2_f (float p1, float p2)

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	k_rm_1_2_f (float omega_n, float zeta)

static float	k_rm_2_2_f (float omega_n, float zeta)

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	ec_2rd (float x_ref, float x_d_ref, float x_2d_ref, float x, float x_d, float k1_e, float k2_e)
	Error Controller Definition for 2rd order system. More...

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

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

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_p = 2.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}

static float	Wv [ANDI_OUTPUTS] = {1.0}

static float	Wv_wls [ANDI_OUTPUTS] = {1.0}

static float	Wu [ANDI_NUM_ACT_TOT] = {1.0}

static float	u_pref [ANDI_NUM_ACT_TOT] = {0.0}

float	max_a_nav = 4.0

float	max_j_nav = 500.0

float	max_v_nav = 5.0

struct guidance_indi_hybrid_params	gih_params

bool	force_forward = false
	forward flight for hybrid nav More...

struct OneloopGeneral	oneloop_andi

static float	use_increment = 0.0

static float	nav_target [3]

static float	dt_1l = 1./PERIODIC_FREQUENCY

static float	g = 9.81

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]

static float	act_dynamics_d [ANDI_NUM_ACT_TOT]

float	actuator_state_1l [ANDI_NUM_ACT]

static float	a_thrust = 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

static 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}

static float	gamma_wls = 1000.0

static float	du_min_1l [ANDI_NUM_ACT_TOT]

static float	du_max_1l [ANDI_NUM_ACT_TOT]

static float	du_pref_1l [ANDI_NUM_ACT_TOT]

static int	number_iter = 0

static float	model_pred [ANDI_OUTPUTS]

static float	ang_acc [3]

static float	lin_acc [3]

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

float	g2_1l [ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2

float	g1_1l [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] = {ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_THRUST, ONELOOP_ANDI_G1_ROLL, ONELOOP_ANDI_G1_PITCH, ONELOOP_ANDI_G1_YAW}

float	g1g2_1l [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

float *	bwls_1l [ANDI_OUTPUTS]

float	ratio_u_un [ANDI_NUM_ACT_TOT]

float	ratio_vn_v [ANDI_NUM_ACT_TOT]

static struct FirstOrderLowPass	filt_accel_ned [3]

static struct FirstOrderLowPass	rates_filt_fo [3]

static struct FirstOrderLowPass	model_pred_a_filt [3]

static Butterworth2LowPass	att_dot_meas_lowpass_filters [3]

static Butterworth2LowPass	model_pred_filt [ANDI_OUTPUTS]

Macro Definition Documentation

◆ GUIDANCE_INDI_LIFTD_ASQ

#define GUIDANCE_INDI_LIFTD_ASQ 0.20

Definition at line 264 of file oneloop_andi.c.

◆ GUIDANCE_INDI_LIFTD_P50

#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)

Definition at line 273 of file oneloop_andi.c.

◆ GUIDANCE_INDI_LIFTD_P80

#define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12)

Definition at line 272 of file oneloop_andi.c.

◆ GUIDANCE_INDI_POS_GAIN

#define GUIDANCE_INDI_POS_GAIN 0.5

Definition at line 259 of file oneloop_andi.c.

◆ GUIDANCE_INDI_POS_GAINZ

#define GUIDANCE_INDI_POS_GAINZ 0.5

Definition at line 260 of file oneloop_andi.c.

◆ GUIDANCE_INDI_SPEED_GAIN

#define GUIDANCE_INDI_SPEED_GAIN 1.8

Definition at line 254 of file oneloop_andi.c.

◆ GUIDANCE_INDI_SPEED_GAINZ

#define GUIDANCE_INDI_SPEED_GAINZ 1.8

Definition at line 255 of file oneloop_andi.c.

◆ ONELOOP_ANDI_AC_HAS_PUSHER

#define ONELOOP_ANDI_AC_HAS_PUSHER FALSE;

Definition at line 210 of file oneloop_andi.c.

◆ ONELOOP_ANDI_DEBUG_MODE

#define ONELOOP_ANDI_DEBUG_MODE FALSE;

Definition at line 205 of file oneloop_andi.c.

◆ ONELOOP_ANDI_FILT_CUTOFF_P

#define ONELOOP_ANDI_FILT_CUTOFF_P 20.0

Definition at line 131 of file oneloop_andi.c.

◆ ONELOOP_ANDI_FILT_CUTOFF_Q

#define ONELOOP_ANDI_FILT_CUTOFF_Q 20.0

Definition at line 137 of file oneloop_andi.c.

◆ ONELOOP_ANDI_FILT_CUTOFF_R

#define ONELOOP_ANDI_FILT_CUTOFF_R 20.0

Definition at line 143 of file oneloop_andi.c.

◆ ONELOOP_ANDI_PUSHER_IDX

#define ONELOOP_ANDI_PUSHER_IDX 0

Definition at line 214 of file oneloop_andi.c.

Function Documentation

◆ calc_model()

void calc_model ( void )

Function that calculates the model prediction for the complementary filter.

Definition at line 1453 of file oneloop_andi.c.

References act_dynamics, actuator_state_1l, ANDI_G_SCALING, ANDI_NUM_ACT, ANDI_OUTPUTS, eulers_zxy, g, g1_1l, g1g2_1l, model_pred, ONELOOP_ANDI_PUSHER_IDX, P, FloatEulers::phi, FloatEulers::psi, ratio_u_un, ratio_vn_v, mesonh.mesonh_atmosphere::T, and FloatEulers::theta.

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 1435 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, dt_1l, MAX_PPRZ, positive_non_zero(), ratio_u_un, and ratio_vn_v.

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:

◆ ec_2rd()

static float ec_2rd	(	float	x_ref,
		float	x_d_ref,
		float	x_2d_ref,
		float	x,
		float	x_d,
		float	k1_e,
		float	k2_e
	)

static

Error Controller Definition for 2rd 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_des	Desired 1st order signal
x	Current 1st order signal
x_d	Current 2nd order signal
k1_e	Error Controller Gain 1st order signal
k2_e	Error Controller Gain 2nd order signal

Definition at line 861 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

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 812 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]
	)

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

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

Referenced by oneloop_andi_run().

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 586 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:

◆ 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 571 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 559 of file oneloop_andi.c.

Referenced by rm_3rd_attitude().

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

References act_dynamics_d, actuator_state_1l, ANDI_NUM_ACT, andi_u, autopilot_get_motors_on(), and MAX_PPRZ.

Referenced by oneloop_andi_run().

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

References act_dynamics, ANDI_NUM_ACT, gih_params, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains2ndOrder::k2, Gains3rdOrder::k3, Gains2ndOrder::k3, k_att_e, k_att_rm, k_e_1_2_f(), k_e_1_3_f(), k_e_1_3_f_v2(), k_e_2_2_f(), k_e_2_3_f(), k_e_2_3_f_v2(), k_e_3_3_f(), k_e_3_3_f_v2(), k_head_e, k_head_rm, k_pos_e, k_pos_rm, k_rm_1_2_f(), k_rm_1_3_f(), k_rm_2_2_f(), k_rm_2_3_f(), k_rm_3_3_f(), max_v_nav, nav_max_speed, 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, guidance_indi_hybrid_params::pos_gain, guidance_indi_hybrid_params::pos_gainz, guidance_indi_hybrid_params::speed_gain, guidance_indi_hybrid_params::speed_gainz, 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 991 of file oneloop_andi.c.

References ANDI_OUTPUTS, att_dot_meas_lowpass_filters, filt_accel_ned, init_butterworth_2_low_pass(), init_first_order_low_pass(), model_pred_a_filt, model_pred_filt, oneloop_andi_filt_cutoff, oneloop_andi_filt_cutoff_a, ONELOOP_ANDI_FILT_CUTOFF_P, ONELOOP_ANDI_FILT_CUTOFF_Q, ONELOOP_ANDI_FILT_CUTOFF_R, p, rates_filt_fo, and stateGetBodyRates_f().

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

References PolePlacement::omega_n, PolePlacement::p3, p_alt_e, p_alt_rm, p_att_e, p_att_rm, p_head_e, p_pos_e, p_pos_rm, and PolePlacement::zeta.

Referenced by oneloop_andi_init().

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

static float k_e_1_2_f	(	float	p1,
		float	p2
	)

static

Definition at line 491 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()

static float k_e_1_3_f	(	float	p1,
		float	p2,
		float	p3
	)

static

Error Controller Gain Design.

Definition at line 470 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 503 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()

static float k_e_2_2_f	(	float	p1,
		float	p2
	)

static

Definition at line 497 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()

static float k_e_2_3_f	(	float	p1,
		float	p2,
		float	p3
	)

static

Definition at line 477 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 509 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()

static float k_e_3_3_f	(	float	p1,
		float	p2,
		float	p3
	)

static

Definition at line 484 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 516 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_2_f()

static float k_rm_1_2_f	(	float	omega_n,
		float	zeta
	)

static

Definition at line 546 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 525 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_2_f()

static float k_rm_2_2_f	(	float	omega_n,
		float	zeta
	)

static

Definition at line 552 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 532 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 539 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:

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

void oneloop_andi_propagate_filters ( void )

Propagate the filters.

Definition at line 1023 of file oneloop_andi.c.

References ANDI_OUTPUTS, ang_acc, att_dot_meas_lowpass_filters, calc_model(), filt_accel_ned, FirstOrderLowPass::last_out, lin_acc, model_pred, model_pred_a_filt, model_pred_filt, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, rates_filt_fo, stateGetAccelNed_f(), stateGetBodyRates_f(), update_butterworth_2_low_pass(), update_first_order_low_pass(), NedCoor_f::x, NedCoor_f::y, 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
	)

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

Definition at line 1231 of file oneloop_andi.c.

References a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, act_max, act_min, actuator_state_1l, andi_du, andi_du_n, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, ang_acc, AP_MODE_ATTITUDE_DIRECT, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, autopilot, bwls_1l, calc_normalization(), du_max_1l, du_min_1l, du_pref_1l, ec_2rd(), ec_3rd(), ec_3rd_att(), eulers_zxy, eulers_zxy_des, float_eulers_of_quat_zxy(), float_vect_copy(), float_vect_sum(), g1g2_1l, gamma_wls, get_act_state_oneloop(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, init_controller(), OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains2ndOrder::k2, Gains3rdOrder::k3, Gains2ndOrder::k3, k_att_e, k_head_e, k_pos_e, FirstOrderLowPass::last_out, lin_acc, pprz_autopilot::mode, nu, num_thrusters_oneloop, number_iter, oneloop_andi, ONELOOP_ANDI_DEBUG_MODE, oneloop_andi_propagate_filters(), oneloop_andi_RM(), FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, rates_filt_fo, ratio_u_un, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, stabilization_cmd, stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetSpeedNed_f(), sum_g1g2_1l(), FloatEulers::theta, u_pref, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, wls_alloc(), Wu, Wv_wls, 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_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 1479 of file oneloop_andi.c.

References RotorcraftNavigation::carrot, RotorcraftNavigation::climb, nav, RotorcraftNavigation::nav_altitude, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, NAV_VERTICAL_MODE_ALT, NAV_VERTICAL_MODE_CLIMB, 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::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 460 of file oneloop_andi.c.

Referenced by calc_normalization(), k_e_1_2_f(), k_e_1_3_f(), k_e_1_3_f_v2(), k_e_2_2_f(), k_e_2_3_f(), k_e_2_3_f_v2(), k_e_3_3_f(), k_e_3_3_f_v2(), k_rm_1_2_f(), k_rm_1_3_f(), k_rm_2_2_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:

◆ 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 768 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 789 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

Here is the caller graph for this function:

◆ 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 746 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 667 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

Here is the caller graph for this function:

◆ 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]
	)

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

References err_nd(), float_euler_dot_of_rates_vec(), float_rates_of_euler_dot_vec(), float_vect_copy(), and integrate_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 689 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 717 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:

◆ send_eff_mat_g_oneloop_andi()

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

static

Definition at line 318 of file oneloop_andi.c.

References ANDI_NUM_ACT_TOT, dev, and g1g2_1l.

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

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

References actuator_state_1l, ANDI_NUM_ACT, ANDI_OUTPUTS, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, dev, nu, oneloop_andi, OneloopGeneral::sta_ref, and OneloopGeneral::sta_state.

Referenced by oneloop_andi_init().

Here is the caller graph for this function:

◆ sum_g1g2_1l()

void sum_g1g2_1l ( void )

Function that sums g1 and g2 to obtain the g1_g2 matrix.

It also undoes the scaling that was done to make the values readable FIXME: make this function into a for loop to make it more adaptable to different configurations

Definition at line 1377 of file oneloop_andi.c.

References act_dynamics, actuator_state_1l, ANDI_G_SCALING, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, eulers_zxy, g1_1l, g1g2_1l, g2_1l, ONELOOP_ANDI_AC_HAS_PUSHER, ONELOOP_ANDI_PUSHER_IDX, P, FloatEulers::phi, FloatEulers::psi, ratio_u_un, ratio_vn_v, mesonh.mesonh_atmosphere::T, and FloatEulers::theta.

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

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

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

Referenced by rm_1st_pos(), rm_2nd_pos(), 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 874 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:

Variable Documentation

◆ a_thrust

float a_thrust = 0.0

static

Definition at line 399 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ act_dynamics

float act_dynamics[ANDI_NUM_ACT_TOT] = = {1}

Definition at line 156 of file oneloop_andi.c.

Referenced by calc_model(), calc_normalization(), init_controller(), oneloop_andi_init(), and sum_g1g2_1l().

◆ act_dynamics_d

float act_dynamics_d[ANDI_NUM_ACT_TOT]

static

Definition at line 397 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 162 of file oneloop_andi.c.

Referenced by calc_normalization(), and oneloop_andi_run().

◆ act_max_norm

float act_max_norm[ANDI_NUM_ACT_TOT] = = {1.0}

Definition at line 174 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ act_min

float act_min[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 168 of file oneloop_andi.c.

Referenced by calc_normalization(), and oneloop_andi_run().

◆ act_min_norm

float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 180 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ actuator_is_servo

bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0}

Definition at line 149 of file oneloop_andi.c.

◆ actuator_state_1l

float actuator_state_1l[ANDI_NUM_ACT]

Definition at line 398 of file oneloop_andi.c.

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

◆ andi_du

float andi_du[ANDI_NUM_ACT_TOT]

Definition at line 394 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 395 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 393 of file oneloop_andi.c.

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

◆ ang_acc

float ang_acc[3]

static

Definition at line 419 of file oneloop_andi.c.

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

◆ att_dot_meas_lowpass_filters

Butterworth2LowPass att_dot_meas_lowpass_filters[3]

static

Definition at line 455 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ bwls_1l

float* bwls_1l[ANDI_OUTPUTS]

Definition at line 447 of file oneloop_andi.c.

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

◆ dt_1l

float dt_1l = 1./PERIODIC_FREQUENCY

static

Definition at line 389 of file oneloop_andi.c.

Referenced by calc_normalization(), and oneloop_andi_RM().

◆ du_max_1l

float du_max_1l[ANDI_NUM_ACT_TOT]

static

Definition at line 413 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ du_min_1l

float du_min_1l[ANDI_NUM_ACT_TOT]

static

Definition at line 412 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ du_pref_1l

float du_pref_1l[ANDI_NUM_ACT_TOT]

static

Definition at line 414 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ eulers_zxy

struct FloatEulers eulers_zxy

Definition at line 399 of file oneloop_andi.c.

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

◆ eulers_zxy_des

struct FloatEulers eulers_zxy_des

Definition at line 399 of file oneloop_andi.c.

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

◆ filt_accel_ned

struct FirstOrderLowPass filt_accel_ned[3]

static

Definition at line 449 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ force_forward

bool force_forward = false

forward flight for hybrid nav

Definition at line 288 of file oneloop_andi.c.

◆ g

float g = 9.81

static

Definition at line 390 of file oneloop_andi.c.

Referenced by calc_model().

◆ g1_1l

float g1_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] = {ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_THRUST, ONELOOP_ANDI_G1_ROLL, ONELOOP_ANDI_G1_PITCH, ONELOOP_ANDI_G1_YAW}

Definition at line 445 of file oneloop_andi.c.

Referenced by calc_model(), and sum_g1g2_1l().

◆ g1g2_1l

float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

Definition at line 446 of file oneloop_andi.c.

Referenced by calc_model(), oneloop_andi_init(), oneloop_andi_run(), send_eff_mat_g_oneloop_andi(), and sum_g1g2_1l().

◆ g2_1l

float g2_1l[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2

Definition at line 444 of file oneloop_andi.c.

Referenced by sum_g1g2_1l().

◆ gamma_wls

float gamma_wls = 1000.0

static

Definition at line 411 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ gih_params

struct guidance_indi_hybrid_params gih_params

Initial value:

= {
  .pos_gain = GUIDANCE_INDI_POS_GAIN,
  .pos_gainz = GUIDANCE_INDI_POS_GAINZ,
 
  .speed_gain = GUIDANCE_INDI_SPEED_GAIN,
  .speed_gainz = GUIDANCE_INDI_SPEED_GAINZ,
 
  .heading_bank_gain = 5.0,
  .liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, 
  .liftd_p80 = GUIDANCE_INDI_LIFTD_P80,
  .liftd_p50 = GUIDANCE_INDI_LIFTD_P50,
}