#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 "generated/modules.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"

Include dependency graph for oneloop_andi.c:

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.

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

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

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.

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

void	oneloop_andi_propagate_filters (void)
	Propagate the filters.

void	init_filter (void)
	Initialize the filters.

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

void	reinit_controller (void)
	Re-Init function of controller variables.

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

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

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

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

void	float_vect_diff_euler (float err[3], float a[3], float b[3])
	Calculate Error between two 3D euler arrays with the psi normalized.

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

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

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

float	bound_v_from_a_vect (float e_x[], float v_bound, float a_bound, int n)
	Calculate velocity limit based on acceleration limit for a vector From SUVAT we have that v^2 = u^2 + 2as.

float	bound_v_from_a (float e_x, float v_bound, float a_bound)
	Calculate velocity limit based on acceleration limit for a float From SUVAT we have that v^2 = u^2 + 2as.

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], struct OneloopStabilizationRef bounds)
	Reference Model Definition for 3rd order system with attitude conversion functions.

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.

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.

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.

void	ec_3rd_att (float y_4d[3], float x_des[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], struct OneloopStabilizationRef bounds, float fb[3])
	Error Controller Definition for 3rd order system specific to attitude.

void	ec_3rd_pos (float y_4d[], float x_des[], 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 fb[], int n)
	Error Controller Definition for 3rd order system.

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

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.

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	oneloop_axis_effectiveness_calc (void)

void	oneloop_andi_bound_disturbance (void)

void	oneloop_calc_model_disturbance (bool in_flight)
	Function that calculates the model disturbance (Control Effort) The control effort is equalt to : EFF_MAT*u_filt where EFF_MAT is what is inverted and u_filt is the filtered actuator state.

void	dynFilter_init (struct Oneloop_DynFilt_t *mu, float varepsilon, float sigma)

void	dynFilter_run (struct Oneloop_DynFilt_t *mu, float u_c, float sigma)

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.

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

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 void	Pos_PID (float x_des[3], float x[3], float x_dot[3], float k_P, float k_I, float k_D, float a[3])

static void	shape_vector (float a[3])

static void	eul_of_acc (float a[3], float psi)

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

static void	init_filter_on_type (struct LP_t *filter, float x0)
	Initialize a filter based on its type.

static void	update_filter_on_type (struct LP_t *filter, float input)
	Update a filter based on its type.

static void	init_LP (struct LP_t *LP, float fc)
	Initialize the Low Pass Filter Struct.

static void	reinit_LP_synchronous (struct LP_t LP, struct LP_t f, bool reinit)
	Reinitialize Low Pass filter if new frequency setting or if forced.

static void	reinit_LP (struct LP_t *LP, bool reinit)

static void	init_all_LP (void)
	Initialize all Low Pass Filters.

static void	reinit_all_LP (bool reinit)
	Reinitialize all the Low Pass Filters.

void	oneloop_andi_init (void)
	Init function of Oneloop ANDI controller

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

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.

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.

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

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

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

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

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

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.

void	guidance_set_min_max_airspeed (float min_airspeed, float max_airspeed)

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

void	guidance_set_max_climb_speed (float max_climb_speed_quad, float max_climb_speed_fwd)

void	guidance_set_max_descend_speed (float max_descend_speed_quad, float max_descend_speed_fwd)

Variables
struct OneloopGeneral	oneloop_andi

static float	dt_1l = 1./PERIODIC_FREQUENCY

static float	g = 9.81

float	oneloop_andi_filt_cutoff = 2.0

float	oneloop_andi_filt_cutoff_a = 2.0

float	oneloop_andi_filt_cutoff_v = 30.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

struct Oneloop_LP_t	LP

struct Oneloop_LP_t	oneloop_andi_model_filt

struct Oneloop_DynFilt_t	mu_mL

struct Oneloop_DynFilt_t	mu_mR

bool	use_dyn_filter = false

float	oneloop_andi_sigma = 29.0

float	oneloop_andi_sigma_max = 29.0

float	oneloop_andi_sigma_min = 8.0

static Butterworth2LowPass	filt_veloc [3]

static Butterworth2LowPass	accely_filt

static Butterworth2LowPass	airspeed_filt

static Butterworth2LowPass	u_filt [ANDI_NUM_ACT_TOT]

float	num_thrusters_oneloop = 4.0

bool	actuator_is_servo [ANDI_NUM_ACT_TOT] = {0}

float	act_dynamics [ANDI_NUM_ACT_TOT] = = {1}

float	act_dyn_ctrl [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	andi_u [ANDI_NUM_ACT_TOT]

float	andi_du [ANDI_NUM_ACT_TOT]

static float	andi_u_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_TOT]

float	max_r = RadOfDeg(120.0)

float	oneloop_andi_yaw_dist_limit = 99999.f

bool	heading_manual = false

bool	yaw_stick_in_auto = false

struct OneloopStabilizationRef	sta_bounds

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}

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

float	fwd_sideslip_gain = 0.2

float	max_a_nav = 4.0

float	max_j_lin = 500.0

float	max_v_nav = 5.0

float	max_v_nav_v = 1.5

float	max_as = 19.0f

float	min_as = 0.0f

float	xdot_lim_sf = 0.8

float	ec_headroom = 1.5

static float	nav_target [3]

static float	nav_target_new [3]

float	gi_unbounded_airspeed_sp = 0.0

float	Wu_quad_motors_fwd = 6.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}

static float	a_thrust = 0.0

static float	g2_ff = 0.0

bool	ctrl_off = false

static float	pitch_pref = 0

float *	bwls_1l [ANDI_OUTPUTS]

float	EFF_MAT_G [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

float	n_array [ANDI_OUTPUTS]

float	m_array [ANDI_NUM_ACT_TOT]

float	coupling_factor [ANDI_OUTPUTS]

float	oneloop_andi_model [ANDI_OUTPUTS]

float	oneloop_andi_dist_bound [ANDI_OUTPUTS]

float	ratio_u_un [ANDI_NUM_ACT_TOT]

float	ratio_vn_v [ANDI_OUTPUTS]

bool	drop_yaw = false

bool	drop_roll = false

bool	drop_pitch = false

bool	drop_aN = false

bool	drop_aE = false

bool	drop_aD = false

struct PolePlacement	p_att_e

struct PolePlacement	p_roll_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	k1_NE_tune = 0.6

float	k2_NE_tune = 1.85

float	k_P = 0.6

float	k_D = 1.2

float	k_I = 9.0e-4

float	temp_P_error [3]

float	temp_D_error [3]

float	temp_I_error [3]

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

float	f1_chirp = 0.2

float	t_chirp = 0.65

float	A_chirp = 250.0

int8_t	chirp_axis = 6

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

int16_t	chirp_n_repeat = 10

int16_t	chirp_n_counter = 0

int16_t	firing_counter = 0

int16_t	delay_counter = 0

bool	multiple_chirp_run = false

Macro Definition Documentation

◆ ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD

#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0

Definition at line 372 of file oneloop_andi.c.

◆ ONELOOP_ANDI_DEBUG_MODE

#define ONELOOP_ANDI_DEBUG_MODE FALSE

Definition at line 129 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_BANK

#define ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL]

Definition at line 265 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_PHI

#define ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL]

Definition at line 266 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_THETA

#define ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH]

Definition at line 267 of file oneloop_andi.c.

◆ USE_BW2

#define USE_BW2

Definition at line 138 of file oneloop_andi.c.

◆ USE_ND_VELOCITIES

#define USE_ND_VELOCITIES

Definition at line 405 of file oneloop_andi.c.

◆ USE_PID_NUMDIFF_DERIVATIVE

#define USE_PID_NUMDIFF_DERIVATIVE

Definition at line 198 of file oneloop_andi.c.

◆ USE_PID_OUTERLOOP

#define USE_PID_OUTERLOOP

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

References float_vect_norm(), foo, 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 2852 of file oneloop_andi.c.

◆ bound_v_from_a()

float bound_v_from_a	(	float	e_x,
		float	v_bound,
		float	a_bound
	)

Calculate velocity limit based on acceleration limit for a float From SUVAT we have that v^2 = u^2 + 2*a*s.

Assuming a is constant we can calculate the velocity bound given the position error and the acceleration limit. This bounding also assumes that we want to arrive at the waypoint with zero velocity which might be untrue.

Parameters

e_x	Error in Position (always wrt the Desired)
v_bound	Safety bound on velocity
a_bound	Acceleration limit
n	Size of vector

Definition at line 931 of file oneloop_andi.c.

References foo, and xdot_lim_sf.

Referenced by ec_3rd_att(), and rm_3rd_attitude().

Here is the caller graph for this function:

◆ bound_v_from_a_vect()

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

Calculate velocity limit based on acceleration limit for a vector From SUVAT we have that v^2 = u^2 + 2*a*s.

Assuming a is constant we can calculate the velocity bound given the position error and the acceleration limit. This bounding also assumes that we want to arrive at the waypoint with zero velocity which might be untrue.

Parameters

e_x	Error in Position (always wrt the Desired)
v_bound	Safety bound on velocity
a_bound	Acceleration limit
n	Size of vector

Definition at line 915 of file oneloop_andi.c.

References float_vect_norm(), foo, and xdot_lim_sf.

Referenced by ec_3rd_pos(), and rm_3rd_pos().

Here is the call graph for this function:

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 2509 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, OneloopStabilizationRef::att_3d, dt_1l, foo, max_j_lin, MAX_PPRZ, nu_norm_max, positive_non_zero(), ratio_u_un, ratio_vn_v, RW_aD, RW_aE, RW_aN, RW_ap, RW_aq, RW_ar, and sta_bounds.

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

References A, chirp_first_call, chirp_on, chirp_pos(), drop_aD, drop_aE, drop_aN, drop_pitch, drop_roll, drop_yaw, float_vect_zero(), foo, get_sys_time_float(), 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 2676 of file oneloop_andi.c.

References A, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, chirp_pos_a_ref(), chirp_pos_j_ref(), chirp_pos_p_ref(), chirp_pos_v_ref(), drop_aE, drop_aN, foo, OneloopGeneral::gui_state, oneloop_andi, oneloop_andi_sigma, oneloop_andi_sigma_max, oneloop_andi_sigma_min, p_ref_0, pitch_pref, OneloopGuidanceState::pos, positive_non_zero(), psi_des_rad, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, t_chirp, theta_pref_max, and OneloopGuidanceState::vel.

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

References A, and foo.

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

References A, and foo.

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

References A, and foo.

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

References A, and foo.

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

References dev, and foo.

Referenced by send_oneloop_debug().

Here is the caller graph for this function:

◆ dynFilter_init()

void dynFilter_init	(	struct Oneloop_DynFilt_t *	mu,
		float	varepsilon,
		float	sigma
	)

Definition at line 3093 of file oneloop_andi.c.

References foo, Oneloop_DynFilt_t::fs, Oneloop_DynFilt_t::mu_c, Oneloop_DynFilt_t::mu_c_0, Oneloop_DynFilt_t::sigma, Oneloop_DynFilt_t::u_c, Oneloop_DynFilt_t::u_c_0, and Oneloop_DynFilt_t::varepsilon.

Referenced by oneloop_andi_init().

Here is the caller graph for this function:

◆ dynFilter_run()

void dynFilter_run	(	struct Oneloop_DynFilt_t *	mu,
		float	u_c,
		float	sigma
	)

Definition at line 3077 of file oneloop_andi.c.

References foo, Oneloop_DynFilt_t::fs, Oneloop_DynFilt_t::mu_c, Oneloop_DynFilt_t::mu_c_0, Oneloop_DynFilt_t::sigma, Oneloop_DynFilt_t::u_c, Oneloop_DynFilt_t::u_c_0, and Oneloop_DynFilt_t::varepsilon.

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_des[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],
		struct OneloopStabilizationRef	bounds,
		float	fb[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 1131 of file oneloop_andi.c.

References bound_v_from_a(), ec_headroom, err_sum_nd(), float_rates_of_euler_dot_vec(), float_vect_diff(), float_vect_diff_euler(), float_vect_sum(), and foo.

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_des[],
		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	fb[],
		int	n
	)

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

References bound_v_from_a_vect(), ec_headroom, err_sum_nd(), float_vect_diff(), foo, 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:

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

References b.

Referenced by 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 706 of file oneloop_andi.c.

References b.

Referenced by ec_3rd_att(), and ec_3rd_pos().

Here is the caller graph for this function:

◆ eul_of_acc()

static void eul_of_acc	(	float	a[3],
		float	psi
	)

static

Definition at line 803 of file oneloop_andi.c.

References eulers_zxy_des, foo, FloatEulers::phi, and FloatEulers::theta.

Referenced by oneloop_andi_run().

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

Attitude Euler to Rates Conversion Function ZXY

Definition at line 866 of file oneloop_andi.c.

References foo.

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

Attitude Rates to Euler Conversion Function ZXY

Definition at line 838 of file oneloop_andi.c.

References foo.

Referenced by ec_3rd_att(), and rm_3rd_attitude().

Here is the caller graph for this function:

◆ float_vect_diff_euler()

void float_vect_diff_euler	(	float	err[3],
		float	a[3],
		float	b[3]
	)

Calculate Error between two 3D euler arrays with the psi normalized.

Definition at line 715 of file oneloop_andi.c.

References b, and foo.

Referenced by ec_3rd_att(), and 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 2419 of file oneloop_andi.c.

References act_dyn_ctrl, airspeed_filt, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, COMMAND_PITCH, COMMAND_ROLL, CTRL_ANDI, CTRL_INDI, ctrl_off, drop_yaw, EFF_MAT_G, EFF_MAT_RW, ELE_MIN_AS, foo, rotwing_state_t::hover_motors_enabled, SecondOrderLowPass::o, oneloop_axis_effectiveness_calc(), 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 2348 of file oneloop_andi.c.

References act_dynamics_d, act_max, act_min, actuator_state_1l, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, andi_u, OneloopStabilizationState::att, autopilot_get_motors_on(), foo, oneloop_andi, and OneloopGeneral::sta_state.

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ guidance_set_max_bank_angle()

void guidance_set_max_bank_angle ( float max_bank )

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

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

Definition at line 3108 of file oneloop_andi.c.

References foo, and max_bank.

◆ guidance_set_max_climb_speed()

void guidance_set_max_climb_speed	(	float	max_climb_speed_quad,
		float	max_climb_speed_fwd
	)

Definition at line 3113 of file oneloop_andi.c.

References foo.

◆ guidance_set_max_descend_speed()

void guidance_set_max_descend_speed	(	float	max_descend_speed_quad,
		float	max_descend_speed_fwd
	)

Definition at line 3119 of file oneloop_andi.c.

References foo.

◆ guidance_set_min_max_airspeed()

void guidance_set_min_max_airspeed	(	float	min_airspeed,
		float	max_airspeed
	)

Definition at line 2925 of file oneloop_andi.c.

References max_airspeed, max_as, and min_as.

◆ init_all_LP()

static void init_all_LP ( void )

inlinestatic

Initialize all Low Pass Filters.

Definition at line 1480 of file oneloop_andi.c.

References Oneloop_LP_t::ax, Oneloop_LP_t::ay, Oneloop_LP_t::az, init_LP(), LP, oneloop_andi_model_filt, Oneloop_LP_t::p, Oneloop_LP_t::p_dot, Oneloop_LP_t::q, Oneloop_LP_t::q_dot, Oneloop_LP_t::r, and Oneloop_LP_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_controller_gains()

void init_controller_gains ( void )

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

Definition at line 1244 of file oneloop_andi.c.

References act_dynamics, COMMAND_PITCH, COMMAND_ROLL, foo, Gains3rdOrder::k1, k1_NE_tune, Gains3rdOrder::k2, k2_NE_tune, Gains3rdOrder::k3, k_att_e, k_att_e_indi, k_att_rm, 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, p_roll_e, 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 1516 of file oneloop_andi.c.

References accely_filt, airspeed_filt, ANDI_NUM_ACT_TOT, filt_veloc, foo, init_butterworth_2_low_pass(), SecondOrderLowPass::o, oneloop_andi_filt_cutoff, oneloop_andi_filt_cutoff_v, and u_filt.

Referenced by oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_filter_on_type()

static void init_filter_on_type	(	struct LP_t *	filter,
		float	x0
	)

inlinestatic

Initialize a filter based on its type.

Definition at line 1388 of file oneloop_andi.c.

References LP_t::bandwidth, BUTTERWORTH_2, BUTTERWORTH_4, LP_t::filter_type, foo, LP_t::freq, init_butterworth_2_low_pass(), init_butterworth_4_low_pass(), init_first_order_low_pass(), LOWPASS_1, LP_t::meas_filt, NOTCH, notch_filter_init(), and LP_t::tau.

Referenced by init_LP(), reinit_LP(), and reinit_LP_synchronous().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_LP()

static void init_LP	(	struct LP_t *	LP,
		float	fc
	)

inlinestatic

Initialize the Low Pass Filter Struct.

Definition at line 1440 of file oneloop_andi.c.

References BUTTERWORTH_2, BUTTERWORTH_4, foo, init_filter_on_type(), LOWPASS_1, and LP.

Referenced by init_all_LP().

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

References act_dynamics, COMMAND_PITCH, COMMAND_ROLL, 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, p_roll_e, slow_pole, 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:

◆ integrate_nd()

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

Integrate in time 3D array.

Definition at line 723 of file oneloop_andi.c.

References foo.

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

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

References positive_non_zero().

Referenced by init_controller_gains().

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

References positive_non_zero().

Referenced by init_controller_gains().

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

References positive_non_zero().

Referenced by init_controller_gains().

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 2555 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_bound_disturbance()

void oneloop_andi_bound_disturbance ( void )

Definition at line 3049 of file oneloop_andi.c.

References OneloopGuidanceState::acc, OneloopStabilizationState::att_2d, foo, OneloopGeneral::gui_state, oneloop_andi, oneloop_andi_dist_bound, oneloop_andi_model, RW_aD, RW_aE, RW_aN, RW_ap, RW_aq, RW_ar, and OneloopGeneral::sta_state.

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

References ANDI_OUTPUTS, bwls_1l, calc_normalization(), OneloopGeneral::ctrl_type, EFF_MAT_G, ele_min, eulers_zxy, foo, G1G2_oneloop(), OneloopGeneral::half_loop, init_controller_gains(), multiple_chirp_run, oneloop_andi, FloatEulers::psi, psi_des_deg, psi_des_rad, and reinit_controller().

Referenced by 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 1621 of file oneloop_andi.c.

References act_dynamics, actuator_state_1l, andi_du, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, andi_u_n, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, calc_normalization(), CTRL_ANDI, OneloopGeneral::ctrl_type, DefaultPeriodic, dynFilter_init(), EFF_MAT_G, eulers_zxy_des, float_vect_zero(), foo, G1G2_oneloop(), OneloopGeneral::half_loop, init_all_LP(), init_controller_gains(), init_filter(), init_poles(), mu_mL, mu_mR, 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 1538 of file oneloop_andi.c.

References ACCEL_FLOAT_OF_BFP, accely_filt, airspeed_filt, Oneloop_LP_t::ax, Oneloop_LP_t::ay, Oneloop_LP_t::az, foo, LP, LP_t::meas, LP_t::meas_prev, Oneloop_LP_t::p, Oneloop_LP_t::p_dot, Oneloop_LP_t::q, Oneloop_LP_t::q_dot, Oneloop_LP_t::r, Oneloop_LP_t::r_dot, reinit_all_LP(), stateGetAccelBody_i(), stateGetAccelNed_f(), stateGetAirspeed_f(), stateGetBodyRates_f(), stateGetSpeedNed_f(), update_butterworth_2_low_pass(), update_filter_on_type(), 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,
		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 1724 of file oneloop_andi.c.

References A_chirp, a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, act_dyn_ctrl, ANDI_NUM_ACT, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, chirp_axis, chirp_call(), chirp_first_call, chirp_n_counter, chirp_n_repeat, chirp_on, CTRL_ANDI, OneloopGeneral::ctrl_type, delay_counter, drop_yaw, dt_1l, EFF_MAT_RW, eulers_zxy, eulers_zxy_des, f0_chirp, f1_chirp, firing_counter, float_vect_copy(), float_vect_zero(), foo, 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_lin, MAX_PPRZ, max_r, max_v_nav, max_v_nav_v, multiple_chirp_run, nav_target, nav_target_new, oneloop_andi, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, oneloop_andi_model, oneloop_andi_sideslip(), p_ref_0, FloatEulers::phi, phi_vec, 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, reshape_wind(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), rm_3rd_pos(), RW_aD, sta_bounds, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, t_0_chirp, t_chirp, FloatEulers::theta, time_elapsed_chirp, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, WLS_one_p, WLS_t::Wv, Wv_backup, and yaw_stick_in_auto.

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

References a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, act_dyn_ctrl, act_dynamics, act_max, act_min, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, andi_u_n, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, autopilot, Oneloop_LP_t::ax, Oneloop_LP_t::ay, Oneloop_LP_t::az, bwls_1l, calc_normalization(), chirp_on, Stabilization::cmd, COMMAND_PITCH, COMMAND_ROLL, commands, CTRL_ANDI, CTRL_INDI, ctrl_off, OneloopGeneral::ctrl_type, RW_skew::deg, drop_aD, drop_aE, drop_aN, drop_pitch, drop_roll, drop_yaw, dynFilter_run(), ec_3rd_att(), ec_3rd_pos(), EFF_MAT_G, RW_Model::ele_pref, eul_of_acc(), eulers_zxy, eulers_zxy_des, rotwing_state_t::fail_pusher_motor, foo, 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_gains(), OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_e, k_att_e_indi, k_D, k_I, k_P, k_pos_e, k_pos_e_indi, LP, max_a_nav, MAX_PPRZ, max_v_nav, Oneloop_DynFilt_t::mu_c, mu_mL, mu_mR, nav, RotorcraftNavigation::nav_altitude, normalize_nu(), nu, num_thrusters_oneloop, SecondOrderLowPass::o, oneloop_andi, oneloop_andi_bound_disturbance(), ONELOOP_ANDI_DEBUG_MODE, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, oneloop_andi_model, oneloop_andi_model_filt, oneloop_andi_propagate_filters(), oneloop_andi_RM(), oneloop_andi_sigma, oneloop_calc_model_disturbance(), LP_t::out, Oneloop_LP_t::p, Oneloop_LP_t::p_dot, FloatEulers::phi, pitch_pref, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, Pos_PID(), FloatEulers::psi, psi_des_deg, psi_des_rad, Oneloop_LP_t::q, Oneloop_LP_t::q_dot, Oneloop_LP_t::r, Oneloop_LP_t::r_dot, RADIO_AUX4, RADIO_AUX5, radio_control, radio_control_get(), RADIO_THROTTLE, ratio_u_un, reinit_controller(), rotwing_state, RW, RW_aD, RW_aE, RW_aN, RW_ap, RW_aq, RW_ar, shape_vector(), RW_Model::skew, sta_bounds, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, stabilization, stateGetNedToBodyEulersZxy_f(), stateGetPositionNed_f(), stateGetSpeedNed_f(), sys_id_doublet_running(), RotorcraftNavigation::target, FloatEulers::theta, theta_pref_max, pprz_autopilot::throttle, WLS_t::u, u_filt, WLS_t::u_max, WLS_t::u_min, u_pref, WLS_t::u_pref, use_dyn_filter, RadioControl::values, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, wls_alloc(), WLS_one_p, WLS_t::Wu, Wu_backup, Wu_quad_motors_fwd, NedCoor_f::x, EnuCoor_f::x, NedCoor_f::y, EnuCoor_f::y, and NedCoor_f::z.

Referenced by guidance_oneloop_run_mode(), 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 2609 of file oneloop_andi.c.

References accely_filt, airspeed_filt, eulers_zxy, foo, fwd_sideslip_gain, g, max_phi, 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_axis_effectiveness_calc()

void oneloop_axis_effectiveness_calc ( void )

Definition at line 2931 of file oneloop_andi.c.

References ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, COMMAND_PITCH, COMMAND_ROLL, coupling_factor, EFF_MAT_G, RW_Model::ele_pref, foo, m_array, MAX_PPRZ, n_array, positive_non_zero(), ratio_u_un, ratio_vn_v, and RW.

Referenced by G1G2_oneloop().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_calc_model_disturbance()

void oneloop_calc_model_disturbance ( bool in_flight )

Function that calculates the model disturbance (Control Effort) The control effort is equalt to : EFF_MAT*u_filt where EFF_MAT is what is inverted and u_filt is the filtered actuator state.

Definition at line 2981 of file oneloop_andi.c.

References actuator_state_1l, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, OneloopStabilizationState::att, Oneloop_LP_t::ax, Oneloop_LP_t::ay, Oneloop_LP_t::az, EFF_MAT_G, float_vect_zero(), foo, Gains3rdOrder::k3, k_att_e, SecondOrderLowPass::o, oneloop_andi, oneloop_andi_model, oneloop_andi_model_filt, Oneloop_LP_t::p_dot, positive_non_zero(), Oneloop_LP_t::q_dot, Oneloop_LP_t::r_dot, ratio_u_un, ratio_vn_v, RW_aD, RW_aE, RW_aN, RW_ap, RW_aq, RW_ar, OneloopGeneral::sta_state, u_filt, update_butterworth_2_low_pass(), and update_filter_on_type().

Referenced by oneloop_andi_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 2568 of file oneloop_andi.c.

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

Here is the call graph for this function:

◆ Pos_PID()

static void Pos_PID	(	float	x_des[3],
		float	x[3],
		float	x_dot[3],
		float	k_P,
		float	k_I,
		float	k_D,
		float	a[3]
	)

static

Definition at line 766 of file oneloop_andi.c.

References dt_1l, filt_veloc, foo, k_D, k_I, k_P, SecondOrderLowPass::o, temp_D_error, temp_I_error, temp_P_error, update_butterworth_2_low_pass(), 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:

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

References foo.

Referenced by acc_body_bound(), calc_normalization(), chirp_pos(), k_rm_1_3_f(), k_rm_2_3_f(), k_rm_3_3_f(), oneloop_andi_init(), oneloop_axis_effectiveness_calc(), oneloop_calc_model_disturbance(), 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(), accel_cb(), accel_cb(), actuators_dshot_arch_init(), gyro_cb(), gyro_cb(), gyro_cb(), gyro_cb(), gyro_cb(), gyro_cb(), gyro_cb(), gyro_cb(), gyro_cb(), mag_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_LP()

static void reinit_all_LP ( bool reinit )

inlinestatic

Reinitialize all the Low Pass Filters.

Definition at line 1501 of file oneloop_andi.c.

References Oneloop_LP_t::ax, Oneloop_LP_t::ay, Oneloop_LP_t::az, foo, LP, oneloop_andi_model_filt, Oneloop_LP_t::p, Oneloop_LP_t::p_dot, Oneloop_LP_t::q, Oneloop_LP_t::q_dot, Oneloop_LP_t::r, Oneloop_LP_t::r_dot, reinit_LP(), and reinit_LP_synchronous().

Referenced by oneloop_andi_propagate_filters().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reinit_controller()

void reinit_controller ( void )

Re-Init function of controller variables.

Definition at line 1585 of file oneloop_andi.c.

References OneloopGuidanceRef::acc, OneloopGuidanceState::acc, andi_u, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, COMMAND_PITCH, COMMAND_ROLL, eulers_zxy_des, float_vect_copy(), float_vect_zero(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, OneloopGuidanceRef::jer, oneloop_andi, FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, FloatEulers::theta, OneloopGuidanceRef::vel, and OneloopGuidanceState::vel.

Referenced by oneloop_andi_enter(), and oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reinit_LP()

static void reinit_LP	(	struct LP_t *	LP,
		bool	reinit
	)

inlinestatic

Definition at line 1469 of file oneloop_andi.c.

References foo, init_filter_on_type(), and LP.

Referenced by reinit_all_LP().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reinit_LP_synchronous()

static void reinit_LP_synchronous	(	struct LP_t *	LP,
		struct LP_t *	f,
		bool	reinit
	)

inlinestatic

Reinitialize Low Pass filter if new frequency setting or if forced.

Definition at line 1459 of file oneloop_andi.c.

References f, foo, init_filter_on_type(), and LP.

Referenced by reinit_all_LP().

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

References acc_body_bound(), airspeed_filt, eulers_zxy, FLOAT_ANGLE_NORMALIZE, FLOAT_VECT2_NORM, foo, 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(), and OneloopGuidanceState::vel.

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

References err_nd(), float_vect_copy(), foo, 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_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 1051 of file oneloop_andi.c.

References err_nd(), float_vect_copy(), foo, 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_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],
		struct OneloopStabilizationRef	bounds
	)

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

Parameters

dt	Delta time [s]
x_ref	Reference signal 1st order (Attitude)
x_d_ref	Reference signal 2nd order (Angular Rate)
x_2d_ref	Reference signal 3rd order (Angular Acceleration)
x_3d_ref	Reference signal 4th order (Angular Jerk)
x_des	Desired 1st order signal (Desired Attitude)
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
bounds	Bounds for the Reference Model

Definition at line 956 of file oneloop_andi.c.

References bound_v_from_a(), err_nd(), float_euler_dot_of_rates_vec(), float_rates_of_euler_dot_vec(), float_vect_copy(), float_vect_diff_euler(), foo, 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_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 1020 of file oneloop_andi.c.

References bound_v_from_a_vect(), err_nd(), float_vect_copy(), foo, 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 611 of file oneloop_andi.c.

References ANDI_NUM_ACT_TOT, dev, EFF_MAT_RW, and foo.

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

References ANDI_NUM_ACT, dev, EFF_MAT_RW, foo, and G2_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 632 of file oneloop_andi.c.

References OneloopGuidanceRef::acc, OneloopGuidanceState::acc, dev, foo, 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 618 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, foo, 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 663 of file oneloop_andi.c.

References debug_vect(), dev, foo, temp_D_error, temp_I_error, and temp_P_error.

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 598 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 594 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:

◆ shape_vector()

static void shape_vector ( float a[3] )

static

Definition at line 799 of file oneloop_andi.c.

References vect_bound_nd().

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_filter_on_type()

static void update_filter_on_type	(	struct LP_t *	filter,
		float	input
	)

inlinestatic

Update a filter based on its type.

Definition at line 1410 of file oneloop_andi.c.

References BUTTERWORTH_2, BUTTERWORTH_4, LP_t::filter_type, LOWPASS_1, LP_t::meas_filt, NOTCH, notch_filter_update(), LP_t::out, update_butterworth_2_low_pass(), update_butterworth_4_low_pass(), and update_first_order_low_pass().

Referenced by oneloop_andi_propagate_filters(), and oneloop_calc_model_disturbance().

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

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

Referenced by ec_3rd_pos(), Pos_PID(), reshape_wind(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_pos(), and shape_vector().

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

References foo, and positive_non_zero().

Referenced by init_controller_gains(), and init_poles().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ A_chirp

float A_chirp = 250.0

Definition at line 524 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ a_thrust

float a_thrust = 0.0

static

Definition at line 458 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ accely_filt

Butterworth2LowPass accely_filt

static

Definition at line 202 of file oneloop_andi.c.

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

◆ act_dyn_ctrl

float act_dyn_ctrl[ANDI_NUM_ACT_TOT] = = {1}

Definition at line 226 of file oneloop_andi.c.

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

◆ act_dynamics

float act_dynamics[ANDI_NUM_ACT_TOT] = = {1}

Definition at line 225 of file oneloop_andi.c.

Referenced by calc_normalization(), init_controller_gains(), init_poles(), oneloop_andi_init(), and oneloop_andi_run().

◆ act_dynamics_d

float act_dynamics_d[ANDI_NUM_ACT_TOT]

static

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

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

◆ act_max_norm

float act_max_norm[ANDI_NUM_ACT_TOT] = = {1.0}

Definition at line 244 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ act_min

float act_min[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 238 of file oneloop_andi.c.

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

◆ act_min_norm

float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 250 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ actuator_is_servo

bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0}

Definition at line 217 of file oneloop_andi.c.

◆ actuator_state_1l

float actuator_state_1l[ANDI_NUM_ACT_TOT]

Definition at line 290 of file oneloop_andi.c.

Referenced by calc_all_thrust_curve(), calc_G1_G2_RW(), get_act_state_oneloop(), oneloop_andi_init(), oneloop_calc_model_disturbance(), and send_oneloop_andi().

◆ airspeed_filt

Butterworth2LowPass airspeed_filt

static

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

Referenced by oneloop_andi_init().

◆ andi_u

float andi_u[ANDI_NUM_ACT_TOT]

Definition at line 284 of file oneloop_andi.c.

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

◆ andi_u_n

float andi_u_n[ANDI_NUM_ACT_TOT]

static

Definition at line 286 of file oneloop_andi.c.

Referenced by oneloop_andi_init(), and oneloop_andi_run().

◆ bwls_1l

float* bwls_1l[ANDI_OUTPUTS]

Definition at line 463 of file oneloop_andi.c.

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

◆ chirp_axis

int8_t chirp_axis = 6

Definition at line 525 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ chirp_first_call

bool chirp_first_call = true

Definition at line 518 of file oneloop_andi.c.

Referenced by chirp_call(), and oneloop_andi_RM().

◆ chirp_n_counter

int16_t chirp_n_counter = 0

Definition at line 528 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ chirp_n_repeat

int16_t chirp_n_repeat = 10

Definition at line 527 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ chirp_on

bool chirp_on = false

Definition at line 517 of file oneloop_andi.c.

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

◆ coupling_factor

float coupling_factor[ANDI_OUTPUTS]

Definition at line 467 of file oneloop_andi.c.

Referenced by oneloop_axis_effectiveness_calc().

◆ ctrl_off

bool ctrl_off = false

Definition at line 460 of file oneloop_andi.c.

Referenced by G1G2_oneloop(), and oneloop_andi_run().

◆ delay_counter

int16_t delay_counter = 0

Definition at line 530 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ drop_aD

bool drop_aD = false

Definition at line 480 of file oneloop_andi.c.

Referenced by chirp_call(), and oneloop_andi_run().

◆ drop_aE

bool drop_aE = false

Definition at line 479 of file oneloop_andi.c.

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

◆ drop_aN

bool drop_aN = false

Definition at line 478 of file oneloop_andi.c.

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

◆ drop_pitch

bool drop_pitch = false

Definition at line 477 of file oneloop_andi.c.

Referenced by chirp_call(), and oneloop_andi_run().

◆ drop_roll

bool drop_roll = false

Definition at line 476 of file oneloop_andi.c.

Referenced by chirp_call(), and oneloop_andi_run().

◆ drop_yaw

bool drop_yaw = false

Definition at line 475 of file oneloop_andi.c.

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

◆ dt_1l

float dt_1l = 1./PERIODIC_FREQUENCY

static

Definition at line 133 of file oneloop_andi.c.

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

◆ ec_headroom

float ec_headroom = 1.5

Definition at line 410 of file oneloop_andi.c.

Referenced by ec_3rd_att(), and ec_3rd_pos().

◆ EFF_MAT_G

float EFF_MAT_G[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

Definition at line 464 of file oneloop_andi.c.

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

◆ eulers_zxy

struct FloatEulers eulers_zxy

Definition at line 363 of file oneloop_andi.c.

Referenced by 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 362 of file oneloop_andi.c.

Referenced by eul_of_acc(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), reinit_controller(), and send_oneloop_andi().

◆ f0_chirp

float f0_chirp = 0.2

Definition at line 521 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ f1_chirp

float f1_chirp = 0.2

Definition at line 522 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ filt_veloc

Butterworth2LowPass filt_veloc[3]

static

Definition at line 200 of file oneloop_andi.c.

Referenced by init_filter(), and Pos_PID().

◆ firing_counter

int16_t firing_counter = 0

Definition at line 529 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ fwd_sideslip_gain

float fwd_sideslip_gain = 0.2

Definition at line 376 of file oneloop_andi.c.

Referenced by oneloop_andi_sideslip().

◆ g

float g = 9.81

static

Definition at line 134 of file oneloop_andi.c.

Referenced by image_calculate_g(), ins_flow_update(), light_scheduler_set(), light_ws2812_arch_set(), oneloop_andi_sideslip(), pprz_circfit_wei_float(), send_ins_flow(), snake_gate_detection(), xgeqrf(), and xgeqrf_f().

◆ g2_ff

float g2_ff = 0.0

static

Definition at line 459 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ gi_unbounded_airspeed_sp

float gi_unbounded_airspeed_sp = 0.0

Definition at line 413 of file oneloop_andi.c.

Referenced by reshape_wind().

◆ heading_manual

bool heading_manual = false

Definition at line 309 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ k1_NE_tune

float k1_NE_tune = 0.6

Definition at line 505 of file oneloop_andi.c.

Referenced by init_controller_gains().

◆ k2_NE_tune

float k2_NE_tune = 1.85

Definition at line 506 of file oneloop_andi.c.

Referenced by init_controller_gains().

◆ k_att_e

struct Gains3rdOrder k_att_e

Definition at line 498 of file oneloop_andi.c.

Referenced by init_controller_gains(), oneloop_andi_run(), and oneloop_calc_model_disturbance().

◆ k_att_e_indi

struct Gains3rdOrder k_att_e_indi

Definition at line 503 of file oneloop_andi.c.

Referenced by init_controller_gains(), and oneloop_andi_run().

◆ k_att_rm

struct Gains3rdOrder k_att_rm

Definition at line 499 of file oneloop_andi.c.

Referenced by generate_reference_attitude(), init_controller_gains(), and oneloop_andi_RM().

◆ k_D

float k_D = 1.2

Definition at line 509 of file oneloop_andi.c.

Referenced by oneloop_andi_run(), and Pos_PID().

◆ k_I

float k_I = 9.0e-4

Definition at line 510 of file oneloop_andi.c.

Referenced by oneloop_andi_run(), and Pos_PID().

◆ k_P

float k_P = 0.6

Definition at line 508 of file oneloop_andi.c.

Referenced by oneloop_andi_run(), and Pos_PID().

◆ k_pos_e

struct Gains3rdOrder k_pos_e

Definition at line 500 of file oneloop_andi.c.

Referenced by init_controller_gains(), and oneloop_andi_run().

◆ k_pos_e_indi

struct Gains3rdOrder k_pos_e_indi

Definition at line 504 of file oneloop_andi.c.

Referenced by init_controller_gains(), and oneloop_andi_run().

◆ k_pos_rm

struct Gains3rdOrder k_pos_rm

Definition at line 501 of file oneloop_andi.c.

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

◆ LP

struct Oneloop_LP_t LP

Definition at line 189 of file oneloop_andi.c.

Referenced by init_all_LP(), init_LP(), oneloop_andi_propagate_filters(), oneloop_andi_run(), reinit_all_LP(), reinit_LP(), and reinit_LP_synchronous().

◆ m_array

float m_array[ANDI_NUM_ACT_TOT]

Definition at line 466 of file oneloop_andi.c.

Referenced by oneloop_axis_effectiveness_calc().

◆ max_a_nav

float max_a_nav = 4.0

Definition at line 384 of file oneloop_andi.c.

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

◆ max_as

float max_as = 19.0f

Definition at line 407 of file oneloop_andi.c.

Referenced by guidance_set_min_max_airspeed(), and reshape_wind().

◆ max_j_lin

float max_j_lin = 500.0

Definition at line 390 of file oneloop_andi.c.

Referenced by calc_normalization(), and oneloop_andi_RM().

◆ max_r

float max_r = RadOfDeg(120.0)

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

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

◆ max_v_nav_v

float max_v_nav_v = 1.5

Definition at line 402 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ min_as

float min_as = 0.0f

Definition at line 408 of file oneloop_andi.c.

Referenced by guidance_set_min_max_airspeed(), and reshape_wind().

◆ mu_mL

struct Oneloop_DynFilt_t mu_mL

Definition at line 191 of file oneloop_andi.c.

Referenced by oneloop_andi_init(), and oneloop_andi_run().

◆ mu_mR

struct Oneloop_DynFilt_t mu_mR

Definition at line 192 of file oneloop_andi.c.

Referenced by oneloop_andi_init(), and oneloop_andi_run().

◆ multiple_chirp_run

bool multiple_chirp_run = false

Definition at line 531 of file oneloop_andi.c.

Referenced by oneloop_andi_enter(), and oneloop_andi_RM().

◆ n_array

float n_array[ANDI_OUTPUTS]

Definition at line 465 of file oneloop_andi.c.

Referenced by oneloop_axis_effectiveness_calc().

◆ nav_target

float nav_target[3]

static

Definition at line 411 of file oneloop_andi.c.

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

◆ nav_target_new

float nav_target_new[3]

static

Definition at line 412 of file oneloop_andi.c.

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

◆ nu

float nu[ANDI_OUTPUTS]

Definition at line 287 of file oneloop_andi.c.

Referenced by control_error_attitude(), control_error_rate(), control_error_thrust(), normalize_nu(), oneloop_andi_init(), and oneloop_andi_run().

◆ nu_n

float nu_n[ANDI_OUTPUTS]

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

Referenced by calc_normalization().

◆ num_thrusters_oneloop

float num_thrusters_oneloop = 4.0

Definition at line 209 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ oneloop_andi

struct OneloopGeneral oneloop_andi

Definition at line 132 of file oneloop_andi.c.

Referenced by chirp_pos(), get_act_state_oneloop(), guidance_oneloop_run_mode(), oneloop_andi_bound_disturbance(), oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), oneloop_calc_model_disturbance(), oneloop_from_nav(), reinit_controller(), reshape_wind(), send_guidance_oneloop_andi(), send_oneloop_andi(), and stabilization_attitude_run().

◆ oneloop_andi_dist_bound

float oneloop_andi_dist_bound[ANDI_OUTPUTS]

Definition at line 469 of file oneloop_andi.c.

Referenced by oneloop_andi_bound_disturbance().

◆ oneloop_andi_filt_cutoff

float oneloop_andi_filt_cutoff = 2.0

Definition at line 143 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_andi_filt_cutoff_a

float oneloop_andi_filt_cutoff_a = 2.0

Definition at line 149 of file oneloop_andi.c.

◆ oneloop_andi_filt_cutoff_p

float oneloop_andi_filt_cutoff_p = 20.0

Definition at line 162 of file oneloop_andi.c.

◆ oneloop_andi_filt_cutoff_q

float oneloop_andi_filt_cutoff_q = 20.0

Definition at line 169 of file oneloop_andi.c.

◆ oneloop_andi_filt_cutoff_r

float oneloop_andi_filt_cutoff_r = 20.0

Definition at line 176 of file oneloop_andi.c.

◆ oneloop_andi_filt_cutoff_v

float oneloop_andi_filt_cutoff_v = 30.0

Definition at line 155 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_andi_model

float oneloop_andi_model[ANDI_OUTPUTS]

Definition at line 468 of file oneloop_andi.c.

Referenced by oneloop_andi_bound_disturbance(), oneloop_andi_RM(), oneloop_andi_run(), and oneloop_calc_model_disturbance().

◆ oneloop_andi_model_filt

struct Oneloop_LP_t oneloop_andi_model_filt

Definition at line 190 of file oneloop_andi.c.

Referenced by init_all_LP(), oneloop_andi_run(), oneloop_calc_model_disturbance(), and reinit_all_LP().

◆ oneloop_andi_sigma

float oneloop_andi_sigma = 29.0

Definition at line 194 of file oneloop_andi.c.

Referenced by chirp_pos(), and oneloop_andi_run().

◆ oneloop_andi_sigma_max

float oneloop_andi_sigma_max = 29.0

Definition at line 195 of file oneloop_andi.c.

Referenced by chirp_pos().

◆ oneloop_andi_sigma_min

float oneloop_andi_sigma_min = 8.0

Definition at line 196 of file oneloop_andi.c.

Referenced by chirp_pos().

◆ oneloop_andi_yaw_dist_limit

float oneloop_andi_yaw_dist_limit = 99999.f

Definition at line 303 of file oneloop_andi.c.

◆ p_alt_e

struct PolePlacement p_alt_e

Definition at line 492 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ p_alt_rm

struct PolePlacement p_alt_rm

Definition at line 493 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ p_att_e

struct PolePlacement p_att_e

Definition at line 485 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ p_att_rm

struct PolePlacement p_att_rm

Definition at line 487 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ p_head_e

struct PolePlacement p_head_e

Definition at line 495 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ p_head_rm

struct PolePlacement p_head_rm

Definition at line 496 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ p_pos_e

struct PolePlacement p_pos_e

Definition at line 489 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ p_pos_rm

struct PolePlacement p_pos_rm

Definition at line 490 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ p_ref_0

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

Definition at line 526 of file oneloop_andi.c.

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

◆ p_roll_e

struct PolePlacement p_roll_e

Definition at line 486 of file oneloop_andi.c.

Referenced by init_controller_gains(), and init_poles().

◆ phi_vec

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

static

Definition at line 367 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ pitch_pref

float pitch_pref = 0

static

Definition at line 461 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 365 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 364 of file oneloop_andi.c.

Referenced by chirp_pos(), 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 366 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_nB_RM().

◆ ratio_u_un

float ratio_u_un[ANDI_NUM_ACT_TOT]

Definition at line 470 of file oneloop_andi.c.

Referenced by calc_normalization(), G1G2_oneloop(), oneloop_andi_run(), oneloop_axis_effectiveness_calc(), and oneloop_calc_model_disturbance().

◆ ratio_vn_v

float ratio_vn_v[ANDI_OUTPUTS]

Definition at line 471 of file oneloop_andi.c.

Referenced by calc_normalization(), G1G2_oneloop(), normalize_nu(), oneloop_axis_effectiveness_calc(), and oneloop_calc_model_disturbance().

◆ sta_bounds

struct OneloopStabilizationRef sta_bounds

Definition at line 318 of file oneloop_andi.c.

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

◆ t_0_chirp

float t_0_chirp = 0.0

Definition at line 520 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ t_chirp

float t_chirp = 0.65

Definition at line 523 of file oneloop_andi.c.

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

◆ temp_D_error

float temp_D_error[3]

Definition at line 512 of file oneloop_andi.c.

Referenced by Pos_PID(), and send_oneloop_debug().

◆ temp_I_error

float temp_I_error[3]

Definition at line 513 of file oneloop_andi.c.

Referenced by Pos_PID(), and send_oneloop_debug().

◆ temp_P_error

float temp_P_error[3]

Definition at line 511 of file oneloop_andi.c.

Referenced by Pos_PID(), and send_oneloop_debug().

◆ theta_pref_max

float theta_pref_max = RadOfDeg(20.0)

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

Referenced by oneloop_andi_RM().

◆ u_filt

Butterworth2LowPass u_filt[ANDI_NUM_ACT_TOT]

static

Definition at line 204 of file oneloop_andi.c.

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

◆ u_pref

float u_pref[ANDI_NUM_ACT_TOT] = {0.0}

static

Definition at line 262 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ use_dyn_filter

bool use_dyn_filter = false

Definition at line 193 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ WLS_one_p

struct WLS_t WLS_one_p

Initial value:

= {
  .nu        = ANDI_NUM_ACT_TOT,
  .nv        = ANDI_OUTPUTS,
  .gamma_sq  = 1000.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 423 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 455 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ Wu_quad_motors_fwd

float Wu_quad_motors_fwd = 6.0

Definition at line 418 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ Wv_backup

float Wv_backup[ANDI_OUTPUTS] = {1.0}

static

Definition at line 449 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ xdot_lim_sf

float xdot_lim_sf = 0.8

Definition at line 409 of file oneloop_andi.c.

Referenced by bound_v_from_a(), and bound_v_from_a_vect().

◆ yaw_stick_in_auto

bool yaw_stick_in_auto = false

Definition at line 315 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

Macros
#define	ONELOOP_ANDI_DEBUG_MODE FALSE

#define	USE_BW2

#define	USE_PID_NUMDIFF_DERIVATIVE

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

#define	USE_PID_OUTERLOOP

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

◆ USE_BW2

◆ USE_ND_VELOCITIES

◆ USE_PID_NUMDIFF_DERIVATIVE

◆ USE_PID_OUTERLOOP

Function Documentation

◆ acc_body_bound()

◆ autopilot_in_flight_end_detection()

◆ bound_v_from_a()

◆ bound_v_from_a_vect()

◆ calc_normalization()

◆ chirp_call()

◆ chirp_pos()

◆ chirp_pos_a_ref()

◆ chirp_pos_j_ref()

◆ chirp_pos_p_ref()

◆ chirp_pos_v_ref()

◆ debug_vect()

◆ dynFilter_init()

◆ dynFilter_run()

◆ ec_3rd_att()

◆ ec_3rd_pos()

◆ err_nd()

◆ err_sum_nd()

◆ eul_of_acc()

◆ float_euler_dot_of_rates_vec()

◆ float_rates_of_euler_dot_vec()

◆ float_vect_diff_euler()

◆ G1G2_oneloop()

◆ get_act_state_oneloop()

◆ guidance_set_max_bank_angle()

◆ guidance_set_max_climb_speed()

◆ guidance_set_max_descend_speed()

◆ guidance_set_min_max_airspeed()

◆ init_all_LP()

◆ init_controller_gains()

◆ init_filter()

◆ init_filter_on_type()

◆ init_LP()

◆ init_poles()

◆ integrate_nd()

◆ k_rm_1_3_f()

◆ k_rm_2_3_f()

◆ k_rm_3_3_f()

◆ normalize_nu()

◆ oneloop_andi_bound_disturbance()

◆ oneloop_andi_enter()

◆ oneloop_andi_init()

◆ oneloop_andi_propagate_filters()

◆ oneloop_andi_RM()

◆ oneloop_andi_run()

◆ oneloop_andi_sideslip()

◆ oneloop_axis_effectiveness_calc()

◆ oneloop_calc_model_disturbance()

◆ oneloop_from_nav()

◆ Pos_PID()

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

◆ reinit_controller()

◆ reinit_LP()

◆ reinit_LP_synchronous()

◆ reshape_wind()

◆ rm_1st_pos()

◆ rm_2nd_pos()

◆ rm_3rd_attitude()

◆ rm_3rd_pos()

◆ send_eff_mat_guid_oneloop_andi()

◆ send_eff_mat_stab_oneloop_andi()