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

Include dependency graph for oneloop_nB.c:

Macros
#define	ONELOOP_NB_DEBUG_MODE FALSE
#define	IDX_aD RW_aD - 2
#define	IDX_ap RW_ap - 2
#define	IDX_aq RW_aq - 2
#define	IDX_ar RW_ar - 2
#define	USE_BW2
#define	ONELOOP_NB_MAX_FAULT_MOT 3000.0
#define	ONELOOP_NB_AIRSPEED_SWITCH_THRESHOLD 10.0
#define	USE_ND_VELOCITIES
#define	ONELOOP_NB_FAULT_AILERONS TRUE
#define	ONELOOP_NB_SPIN_PROT_MAX_RATE 34.0
#define	ONELOOP_NB_SPIN_PROT_START_RATE 28.0
#define	ONELOOP_NB_SPIN_PROT_MAX_CMD 4800.0
#define	ONELOOP_NB_SPIN_TRIM_ON TRUE
#define	ONELOOP_NB_SPIN_TRIM_RATE 1000.0
#define	ONELOOP_NB_SPIN_TRIM_BLEED_RATE 1000.0
#define	ONELOOP_NB_SPIN_TRIM_RELEASE_RATE 24.0
#define	ONELOOP_NB_SPIN_PROT_MIN_GAP 1.0
#define	ONELOOP_NB_SPIN_MAN_RAMP_RATE 2.0
#define	ONELOOP_NB_SPIN_MAN_BLEND_TIME 1.0
#define	ONELOOP_NB_SPIN_MAN_TRIM_RATE 300.0
#define	ONELOOP_NB_SPIN_MAN_DONE_RATE 0.5
#define	SPIN_MAN_PAIR_PITCH 0
#define	SPIN_MAN_PAIR_ROLL 1
#define	ONELOOP_NB_SPIN_MAN_PAIR SPIN_MAN_PAIR_PITCH
#define	ONELOOP_NB_SPIN_MAN_PITCH_DIR 1.0
#define	ONELOOP_NB_EC_K3 22.0
#define	ONELOOP_NB_PID_A_MAX 0.6
#define	ONELOOP_NB_PID_V_MAX_MANUAL 3.0
#define	ONELOOP_NB_PID_V_MAX_NAV 3.0
#define	STREAM_BWLS
#define	OVERRIDE_ATT_RM

Enumerations
enum	spin_man_state_t { SPIN_MAN_IDLE , SPIN_MAN_UP_RAMP , SPIN_MAN_UP_BLEND , SPIN_MAN_DOWN_SLOW , SPIN_MAN_DOWN_BLEND , SPIN_MAN_DOWN_RAMP }

Functions
	PRINT_CONFIG_VAR (ONELOOP_NB_FILT_CUTOFF)
	PRINT_CONFIG_VAR (ONELOOP_NB_FILT_CUTOFF_Q)
	PRINT_CONFIG_VAR (ONELOOP_NB_FILT_CUTOFF_P)
	PRINT_CONFIG_VAR (ONELOOP_NB_FILT_CUTOFF_R)
	PRINT_CONFIG_VAR (ONELOOP_NB_MAX_THETA)
void	init_poles (void)
	Initialize Position of Poles.
void	init_poles_att (void)
	Initialize Position of Poles.
void	init_poles_pos (void)
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_nB_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	rm_3rd_nI (float dt, struct FloatVect3 x_ref, struct FloatVect3 x_d_ref, struct FloatVect3 x_2d_ref, struct FloatVect3 x_3d_ref, const struct FloatVect3 *x_des, const float k1_rm[3], const float k2_rm[3], const float k3_rm[3])
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_nB_sideslip (void)
	Function to calculate corrections for sideslip DEPRECIATED.
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_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	oneloop_nB_state_compensation (bool state_compensation_on)
	Function that performs state compensation for gyroscopic effects.
void	set_WLS_settings (void)
	Function that sets the WLS settings for oneloop controller.
void	drop_axis (void)
	Function that drops control along selected axes.
void	skew_symmetric (struct FloatRMat out, const struct FloatVect3 in)
void	oneloop_nB_calc_nB_states (void)
	Function which estimates the nB states.
void	nB_EC (struct FloatVect3 nB, struct FloatVect3 nB_d, struct FloatVect3 nB_2d, struct FloatVect3 mu_B, float k1_e[3], float k2_e[3], float k3_e[3], float dist[3], float nB_nu[3])
void	calc_HB_matrix (float HB[3][3], struct FloatVect3 nB)
void	SpinQuad_overwrite (float gain, float ce_model, float *nu_stab_2)
float	cos_2n (float psi, float xi, int n)
float	phase_first_order (float r, float w)
float	xi_fun (float r, float w, float aN_des, float aE_des)
float	pusher_cmd_fun (float aN_des, float aE_des, float r, float psi, float w, int n)
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_nB (struct transport_tx trans, struct link_device dev)
static void	send_eff_mat_stab_thrust_oneloop_nB (struct transport_tx trans, struct link_device dev)
static void	send_eff_mat_guid_oneloop_nB (struct transport_tx trans, struct link_device dev)
static void	send_oneloop_nB (struct transport_tx trans, struct link_device dev)
static void	send_oneloop_nB_ctrl (struct transport_tx trans, struct link_device dev)
static void	send_guidance_oneloop_nB (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 void	spin_prot_bound_params (void)
	Enforce valid spin protection / spin trim parameters (they can be changed in flight from the settings).
static void	spin_man_reset (void)
	Stop the spin manoeuvre and clear its GCS triggers (fault flags are left as they are).
static void	spin_man_set_pair (uint8_t pair)
	Select the motors of a pair (pitch: FRONT/BACK, roll: RIGHT/LEFT) for the blend.
static void	spin_man_start_blend (void)
	Start a bumpless transfer of the selected pair from the average of its last applied commands, so that both motors of the pair are commanded the same during the blend.
static void	spin_man_ramp_ref (float target)
	Move the yaw rate reference towards target at spin_man_ramp_rate.
static bool	spin_man_rate_ctrl (void)
	Yaw rate tracking is used by the manoeuvre (no fault active).
static void	spin_man_update (bool in_flight, bool nb_ctrl)
	Spin transition manoeuvre state machine.
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 bool	vec3_saturated (const float u_unsat[3], const float u_sat[3], float eps)
static void	ARW_ClampVec3 (const float err[3], const float u_unsat[3], const float u_sat[3], const float I_prev[3], const float I_cand[3], float I_next[3])
static void	Vel_PID_ARW (float x_dot_des[3], const float x_dot[3], float k_P, float k_I, float k_D, float a_sp[3], float a_max, float v_max)
static void	Pos_KPID_ARW (const float x_des[3], const float x[3], const float x_dot[3], float k_K, float k_P, float k_I, float k_D, float a_sp[3], float a_max, float v_max)
static void	shape_vector (float a[3])
static void	eul_of_acc (float a[3], float psi)
static float	ec_poles (float p_rm, float slow_pole, float k)
	Calculate EC poles given RM poles.
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_nB_init (void)
	Init function of Oneloop ANDI controller.
void	oneloop_nB_enter (bool half_loop_sp, int ctrl_type)
	Function that resets important values upon engaging Oneloop ANDI.
void	oneloop_nB_RM (bool half_loop, struct FloatVect3 PSA_des, bool in_flight_oneloop)
	Function to generate the reference signals for the oneloop controller.
void	oneloop_nB_run (bool in_flight, bool half_loop, struct FloatVect3 PSA_des)
	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.
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
int8_t	TestMotorIDX = 0
struct OneloopGeneral	oneloop_nB
static float	dt_1l = 1. / PERIODIC_FREQUENCY
static float	g = 9.81
float	oneloop_nB_filt_cutoff = 2.0
float	oneloop_nB_filt_cutoff_a = 2.0
float	oneloop_nB_filt_cutoff_v = 30.0
float	oneloop_nB_filt_cutoff_p = 20.0
float	oneloop_nB_filt_cutoff_q = 20.0
float	oneloop_nB_filt_cutoff_r = 20.0
struct Oneloop_LP_t	LP
static Butterworth2LowPass	KPID_vel_filt [3]
static Butterworth2LowPass	KPID_filt [3]
static Butterworth2LowPass	push_PID_vel [2]
static Butterworth2LowPass	push_PID_vel_d [2]
static Butterworth2LowPass	accely_filt
static Butterworth2LowPass	airspeed_filt
static Butterworth2LowPass	u_filt [ANDI_NUM_ACT_TOT]
static Butterworth2LowPass	nB_filt [3]
static Butterworth2LowPass	nB_2d_filt [3]
static Butterworth2LowPass	nB_3d_state_filt [2]
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	max_bank = M_PI_6
float	max_phi = M_PI_6
float	max_theta = M_PI_6
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	nB_jerk_des [3]
float	SQ_r = 0.0
float	max_fault_mot = ONELOOP_NB_MAX_FAULT_MOT
float	max_r = RadOfDeg(180.0)
float	oneloop_nB_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
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	Wu_backup [ANDI_NUM_ACT_TOT] = {1.0}
static float	Wv_backup [ANDI_OUTPUTS] = {1.0}
static float	a_thrust = 0.0
static float	g2_ff = 0.0
bool	ctrl_off = false
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	ctrl_effort_model [ANDI_OUTPUTS]
float	ratio_u_un [ANDI_NUM_ACT_TOT]
float	ratio_vn_v [ANDI_OUTPUTS]
float	SpinQuadRate = 0.0
bool	SpinQuad = false
bool	fault_pitch_motors = false
bool	fault_roll_motors = false
bool	fault_ailerons = ONELOOP_NB_FAULT_AILERONS
bool	drop_yaw = false
bool	drop_roll = false
bool	drop_pitch = false
bool	drop_aD = false
bool	state_compensation_on = false
bool	use_push_PID = false
bool	use_push_Position = false
bool	radio_body_ctrl = false
bool	oneloop_nB_Z_hold = false
bool	vel_ctrl_in_manual = true
bool	use_safety_killer = false
bool	safety_killer_trigger = false
bool	auto_fault_cmd = false
float	temp_acc_des [3]
float	safety_killer_cutoff = 7500.0
float	xi = 0.0
float	max_pusher_cmd = 7500
float	debug_state [3]
float	state_term [3]
float	delta_fault = 1000.0
float	spin_prot_max_rate = ONELOOP_NB_SPIN_PROT_MAX_RATE
float	spin_prot_start_rate = ONELOOP_NB_SPIN_PROT_START_RATE
float	spin_prot_max_cmd = ONELOOP_NB_SPIN_PROT_MAX_CMD
float	spin_prot_ratio = 0.0
bool	spin_trim_on = ONELOOP_NB_SPIN_TRIM_ON
float	spin_trim_rate = ONELOOP_NB_SPIN_TRIM_RATE
float	spin_trim_bleed_rate = ONELOOP_NB_SPIN_TRIM_BLEED_RATE
float	spin_trim_release_rate = ONELOOP_NB_SPIN_TRIM_RELEASE_RATE
float	spin_trim = 0.0
float	spin_prot_min_gap = ONELOOP_NB_SPIN_PROT_MIN_GAP
float	ec_k3 = ONELOOP_NB_EC_K3
float	pid_a_max = ONELOOP_NB_PID_A_MAX
float	pid_v_max_manual = ONELOOP_NB_PID_V_MAX_MANUAL
float	pid_v_max_nav = ONELOOP_NB_PID_V_MAX_NAV
bool	spin_man_up = false
bool	spin_man_down = false
uint8_t	spin_man_pair = ONELOOP_NB_SPIN_MAN_PAIR
float	spin_man_pitch_dir = ONELOOP_NB_SPIN_MAN_PITCH_DIR
float	spin_man_ramp_rate = ONELOOP_NB_SPIN_MAN_RAMP_RATE
float	spin_man_blend_time = ONELOOP_NB_SPIN_MAN_BLEND_TIME
float	spin_man_trim_rate = ONELOOP_NB_SPIN_MAN_TRIM_RATE
float	spin_man_done_rate = ONELOOP_NB_SPIN_MAN_DONE_RATE
uint8_t	spin_man_state = SPIN_MAN_IDLE
float	spin_man_r_ref = 0.0
static float	spin_man_dir = 1.0
static float	spin_man_alpha = 1.0
static uint8_t	spin_man_mot [2] = {COMMAND_MOTOR_FRONT, COMMAND_MOTOR_BACK}
static float	spin_man_u_hold [2] = {0.0, 0.0}
static float	spin_man_u_last [ANDI_NUM_ACT] = {0.0}
float	slow_pole = 22.0
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_K = 0.6
float	k_P = 1.8
float	k_I = 0.4
float	k_D = 0.2
float	temp_P_error [3]
float	temp_D_error [3]
float	temp_I_error [3]

Macro Definition Documentation

◆ IDX_aD

#define IDX_aD RW_aD - 2

Definition at line 118 of file oneloop_nB.c.

Referenced by calc_normalization(), drop_axis(), oneloop_nB_RM(), oneloop_nB_run(), and set_WLS_settings().

◆ IDX_ap

#define IDX_ap RW_ap - 2

Definition at line 119 of file oneloop_nB.c.

Referenced by calc_normalization(), drop_axis(), G1G2_oneloop(), oneloop_nB_run(), oneloop_nB_state_compensation(), and set_WLS_settings().

◆ IDX_aq

#define IDX_aq RW_aq - 2

Definition at line 120 of file oneloop_nB.c.

Referenced by calc_normalization(), drop_axis(), G1G2_oneloop(), oneloop_nB_run(), oneloop_nB_state_compensation(), and set_WLS_settings().

◆ IDX_ar

#define IDX_ar RW_ar - 2

Definition at line 121 of file oneloop_nB.c.

Referenced by calc_normalization(), drop_axis(), G1G2_oneloop(), oneloop_nB_run(), send_oneloop_debug(), and set_WLS_settings().

◆ ONELOOP_NB_AIRSPEED_SWITCH_THRESHOLD

#define ONELOOP_NB_AIRSPEED_SWITCH_THRESHOLD 10.0

Definition at line 395 of file oneloop_nB.c.

Referenced by reshape_wind().

◆ ONELOOP_NB_DEBUG_MODE

#define ONELOOP_NB_DEBUG_MODE FALSE

Definition at line 111 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ ONELOOP_NB_EC_K3

#define ONELOOP_NB_EC_K3 22.0

Definition at line 619 of file oneloop_nB.c.

◆ ONELOOP_NB_FAULT_AILERONS

#define ONELOOP_NB_FAULT_AILERONS TRUE

Definition at line 499 of file oneloop_nB.c.

◆ ONELOOP_NB_MAX_FAULT_MOT

#define ONELOOP_NB_MAX_FAULT_MOT 3000.0

Definition at line 310 of file oneloop_nB.c.

◆ ONELOOP_NB_PID_A_MAX

#define ONELOOP_NB_PID_A_MAX 0.6

Definition at line 626 of file oneloop_nB.c.

◆ ONELOOP_NB_PID_V_MAX_MANUAL

#define ONELOOP_NB_PID_V_MAX_MANUAL 3.0

Definition at line 629 of file oneloop_nB.c.

◆ ONELOOP_NB_PID_V_MAX_NAV

#define ONELOOP_NB_PID_V_MAX_NAV 3.0

Definition at line 632 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_MAN_BLEND_TIME

#define ONELOOP_NB_SPIN_MAN_BLEND_TIME 1.0

Definition at line 589 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_MAN_DONE_RATE

#define ONELOOP_NB_SPIN_MAN_DONE_RATE 0.5

Definition at line 595 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_MAN_PAIR

#define ONELOOP_NB_SPIN_MAN_PAIR SPIN_MAN_PAIR_PITCH

Definition at line 600 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_MAN_PITCH_DIR

#define ONELOOP_NB_SPIN_MAN_PITCH_DIR 1.0

Definition at line 603 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_MAN_RAMP_RATE

#define ONELOOP_NB_SPIN_MAN_RAMP_RATE 2.0

Definition at line 586 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_MAN_TRIM_RATE

#define ONELOOP_NB_SPIN_MAN_TRIM_RATE 300.0

Definition at line 592 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_PROT_MAX_CMD

#define ONELOOP_NB_SPIN_PROT_MAX_CMD 4800.0

Definition at line 537 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_PROT_MAX_RATE

#define ONELOOP_NB_SPIN_PROT_MAX_RATE 34.0

Definition at line 531 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_PROT_MIN_GAP

#define ONELOOP_NB_SPIN_PROT_MIN_GAP 1.0

Definition at line 565 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_PROT_START_RATE

#define ONELOOP_NB_SPIN_PROT_START_RATE 28.0

Definition at line 534 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_TRIM_BLEED_RATE

#define ONELOOP_NB_SPIN_TRIM_BLEED_RATE 1000.0

Definition at line 554 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_TRIM_ON

#define ONELOOP_NB_SPIN_TRIM_ON TRUE

Definition at line 548 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_TRIM_RATE

#define ONELOOP_NB_SPIN_TRIM_RATE 1000.0

Definition at line 551 of file oneloop_nB.c.

◆ ONELOOP_NB_SPIN_TRIM_RELEASE_RATE

#define ONELOOP_NB_SPIN_TRIM_RELEASE_RATE 24.0

Definition at line 557 of file oneloop_nB.c.

◆ OVERRIDE_ATT_RM

#define OVERRIDE_ATT_RM

◆ SPIN_MAN_PAIR_PITCH

#define SPIN_MAN_PAIR_PITCH 0

Definition at line 597 of file oneloop_nB.c.

Referenced by spin_man_update(), and spin_prot_bound_params().

◆ SPIN_MAN_PAIR_ROLL

#define SPIN_MAN_PAIR_ROLL 1

Definition at line 598 of file oneloop_nB.c.

Referenced by spin_man_set_pair(), spin_man_update(), and spin_prot_bound_params().

◆ STREAM_BWLS

#define STREAM_BWLS

◆ USE_BW2

#define USE_BW2

Definition at line 125 of file oneloop_nB.c.

◆ USE_ND_VELOCITIES

#define USE_ND_VELOCITIES

Definition at line 428 of file oneloop_nB.c.

Enumeration Type Documentation

◆ spin_man_state_t

enum spin_man_state_t

Enumerator
SPIN_MAN_IDLE
SPIN_MAN_UP_RAMP
SPIN_MAN_UP_BLEND
SPIN_MAN_DOWN_SLOW
SPIN_MAN_DOWN_BLEND
SPIN_MAN_DOWN_RAMP

Definition at line 642 of file oneloop_nB.c.

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

References float_vect_norm(), Min, positive_non_zero(), WLS_t::v, 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:

◆ ARW_ClampVec3()

void ARW_ClampVec3	(	const float	err[3],
		const float	u_unsat[3],
		const float	u_sat[3],
		const float	I_prev[3],
		const float	I_cand[3],
		float	I_next[3] )

static

Definition at line 1237 of file oneloop_nB.c.

References vec3_saturated().

Referenced by Pos_KPID_ARW(), and Vel_PID_ARW().

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

References UNUSED.

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

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

References float_vect_norm(), 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_HB_matrix()

void calc_HB_matrix	(	float	HB[3][3],
		struct FloatVect3	nB )

Definition at line 3736 of file oneloop_nB.c.

References FloatVect3::x, FloatVect3::y, and FloatVect3::z.

◆ calc_normalization()

void calc_normalization ( void )

Calculate Normalization of actuators and discrete actuator dynamics.

Definition at line 3051 of file oneloop_nB.c.

References act_dynamics, act_dynamics_d, act_max, act_max_norm, act_min, act_min_norm, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, dt_1l, IDX_aD, IDX_ap, IDX_aq, IDX_ar, max_j_lin, MAX_PPRZ, nu_norm_max, positive_non_zero(), ratio_u_un, ratio_vn_v, and sta_bounds.

Referenced by oneloop_nB_enter(), oneloop_nB_init(), and oneloop_nB_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(), 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, chirp_pos_a_ref(), chirp_pos_j_ref(), chirp_pos_p_ref(), chirp_pos_v_ref(), drop_aE, drop_aN, oneloop_andi, oneloop_andi_sigma, oneloop_andi_sigma_max, oneloop_andi_sigma_min, p_ref_0, pitch_pref, positive_non_zero(), psi_des_rad, t_chirp, and theta_pref_max.

Referenced by chirp_call().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ cos_2n()

float cos_2n	(	float	psi,
		float	xi,
		int	n )

Definition at line 3795 of file oneloop_nB.c.

References c2, xi, and FloatVect3::y.

Referenced by pusher_cmd_fun().

Here is the caller graph for this function:

◆ debug_vect()

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

static

Definition at line 894 of file oneloop_nB.c.

References dev.

Referenced by send_oneloop_debug().

Here is the caller graph for this function:

◆ drop_axis()

void drop_axis ( void )

Function that drops control along selected axes.

Definition at line 3326 of file oneloop_nB.c.

References ctrl_effort_model, drop_aD, drop_pitch, drop_roll, drop_yaw, IDX_aD, IDX_ap, IDX_aq, IDX_ar, and nu.

Referenced by oneloop_nB_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 1746 of file oneloop_nB.c.

References OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_d, bound_v_from_a(), ec_headroom, err_sum_nd(), float_rates_of_euler_dot_vec(), float_vect_diff(), float_vect_diff_euler(), and float_vect_sum().

Referenced by oneloop_nB_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 1714 of file oneloop_nB.c.

References bound_v_from_a_vect(), ec_headroom, err_sum_nd(), float_vect_diff(), and vect_bound_nd().

Here is the call graph for this function:

◆ ec_poles()

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

static

Calculate EC poles given RM poles.

Parameters

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

Definition at line 1796 of file oneloop_nB.c.

References positive_non_zero(), and slow_pole.

Referenced by init_poles_att(), and init_poles_pos().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ err_nd()

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

Calculate Scaled Error between two 3D arrays.

Definition at line 1118 of file oneloop_nB.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 1127 of file oneloop_nB.c.

References b.

Referenced by ec_3rd_att(), and ec_3rd_pos().

Here is the caller graph for this function:

◆ eul_of_acc()

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

static

Definition at line 1403 of file oneloop_nB.c.

References eulers_zxy_des, max_phi, and max_theta.

Referenced by oneloop_nB_RM().

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 1467 of file oneloop_nB.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 ZYX.

Attitude Rates to Euler Conversion Function ZXY

Definition at line 1439 of file oneloop_nB.c.

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

References b.

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

References act_dyn_ctrl, act_dynamics, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, bwls_1l, CTRL_ANDI, CTRL_INDI, CTRL_NB_ANDI, CTRL_NB_INDI, EFF_MAT_G, EFF_MAT_RW, fault_ailerons, fault_pitch_motors, fault_roll_motors, IDX_ap, IDX_aq, IDX_ar, nB_filt, ratio_u_un, and ratio_vn_v.

Referenced by oneloop_nB_enter(), oneloop_nB_init(), and oneloop_nB_run().

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

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

Referenced by oneloop_nB_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 3774 of file oneloop_nB.c.

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

◆ guidance_set_max_descend_speed()

void guidance_set_max_descend_speed	(	float	max_descend_speed_quad,
		float	max_descend_speed_fwd )

Definition at line 3787 of file oneloop_nB.c.

◆ guidance_set_min_max_airspeed()

void guidance_set_min_max_airspeed	(	float	min_airspeed,
		float	max_airspeed )

Definition at line 3251 of file oneloop_nB.c.

References max_airspeed, max_as, and min_as.

◆ init_all_LP()

void init_all_LP ( void )

inlinestatic

Initialize all Low Pass Filters.

Definition at line 2109 of file oneloop_nB.c.

References init_LP(), LP, and oneloop_nB_filt_cutoff.

Referenced by oneloop_nB_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 1872 of file oneloop_nB.c.

References ec_k3, k1_NE_tune, k2_NE_tune, 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_bank, max_v_nav, nav_hybrid_max_bank, nav_hybrid_pos_gain, nav_max_acceleration_sp, nav_max_speed, p_alt_e, p_alt_rm, p_att_e, p_att_rm, p_head_e, p_head_rm, p_pos_e, p_pos_rm, and p_roll_e.

Referenced by oneloop_nB_enter(), oneloop_nB_init(), and oneloop_nB_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 2176 of file oneloop_nB.c.

References accely_filt, airspeed_filt, ANDI_NUM_ACT_TOT, init_butterworth_2_low_pass(), KPID_filt, KPID_vel_filt, nB_2d_filt, nB_3d_state_filt, nB_filt, oneloop_nB_filt_cutoff, oneloop_nB_filt_cutoff_v, push_PID_vel, push_PID_vel_d, and u_filt.

Referenced by oneloop_nB_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_filter_on_type()

void init_filter_on_type	(	struct LP_t *	filter,
		float	x0 )

inlinestatic

Initialize a filter based on its type.

Definition at line 2017 of file oneloop_nB.c.

References LP_t::bandwidth, BUTTERWORTH_2, BUTTERWORTH_4, LP_t::filter_type, 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()

void init_LP	(	struct LP_t *	LP,
		float	fc )

inlinestatic

Initialize the Low Pass Filter Struct.

Definition at line 2069 of file oneloop_nB.c.

References BUTTERWORTH_2, BUTTERWORTH_4, 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 1825 of file oneloop_nB.c.

References 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, and slow_pole.

Referenced by oneloop_nB_init().

Here is the caller graph for this function:

◆ init_poles_att()

void init_poles_att ( void )

Initialize Position of Poles.

Definition at line 1810 of file oneloop_nB.c.

References ec_poles(), p_att_e, p_att_rm, p_head_e, p_head_rm, and slow_pole.

Here is the call graph for this function:

◆ init_poles_pos()

void init_poles_pos ( void )

Definition at line 1815 of file oneloop_nB.c.

References ec_poles(), p_alt_e, p_alt_rm, p_pos_e, p_pos_rm, and slow_pole.

Here is the call 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 1145 of file oneloop_nB.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_rm_1_3_f()

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

static

Reference Model Gain Design.

Definition at line 1171 of file oneloop_nB.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()

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

static

Definition at line 1178 of file oneloop_nB.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()

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

static

Definition at line 1185 of file oneloop_nB.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:

◆ nB_EC()

void nB_EC	(	struct FloatVect3	nB,
		struct FloatVect3	nB_d,
		struct FloatVect3	nB_2d,
		struct FloatVect3	mu_B,
		float	k1_e[3],
		float	k2_e[3],
		float	k3_e[3],
		float	dist[3],
		float	nB_nu[3] )

Definition at line 3648 of file oneloop_nB.c.

References FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by oneloop_nB_run().

Here is the caller graph for this function:

◆ normalize_nu()

void normalize_nu ( void )

Function to normalize the pseudo control vector.

Definition at line 3095 of file oneloop_nB.c.

References ANDI_OUTPUTS, nu, nu_n, ratio_vn_v, and WLS_one_p.

Referenced by oneloop_nB_run().

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

References actuator_state_1l, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, ctrl_effort_model, EFF_MAT_G, float_vect_zero(), positive_non_zero(), ratio_u_un, ratio_vn_v, u_filt, and update_butterworth_2_low_pass().

Referenced by oneloop_nB_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 3108 of file oneloop_nB.c.

References nav, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, NAV_VERTICAL_MODE_ALT, NAV_VERTICAL_MODE_CLIMB, oneloop_nB, oneloop_nB_enter(), oneloop_nB_run(), POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, SPEED_BFP_OF_REAL, SPEED_FLOAT_OF_BFP, stateGetPositionNed_f(), FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Here is the call graph for this function:

◆ oneloop_nB_calc_nB_states()

void oneloop_nB_calc_nB_states ( void )

Function which estimates the nB states.

Definition at line 3471 of file oneloop_nB.c.

References A, B, D, debug_state, float_rmat_vmult(), FLOAT_VECT3_ZERO, LP, nB_2d_filt, nB_3d_state_filt, nB_filt, oneloop_nB, RW, skew_symmetric(), state_term, stateGetNedToBodyRMat_f(), t1, update_butterworth_2_low_pass(), VECT3_ADD, VECT3_SUB, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by oneloop_nB_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_nB_enter()

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

References calc_normalization(), eulers_zxy, G1G2_oneloop(), init_controller_gains(), oneloop_nB, psi_des_deg, psi_des_rad, reinit_controller(), safety_killer_trigger, and spin_man_reset().

Referenced by oneloop_from_nav().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_nB_init()

void oneloop_nB_init ( void )

Init function of Oneloop ANDI controller.

Definition at line 2291 of file oneloop_nB.c.

References act_dynamics, actuator_state_1l, andi_du, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, andi_u_n, calc_normalization(), CTRL_ANDI, DefaultPeriodic, eulers_zxy_des, float_vect_zero(), G1G2_oneloop(), init_all_LP(), init_controller_gains(), init_filter(), init_poles(), nav_target, nav_target_new, nu, nu_n, oneloop_nB, positive_non_zero(), register_periodic_telemetry(), safety_killer_trigger, send_eff_mat_guid_oneloop_nB(), send_eff_mat_stab_oneloop_nB(), send_eff_mat_stab_thrust_oneloop_nB(), send_guidance_oneloop_nB(), send_oneloop_debug(), send_oneloop_nB(), send_oneloop_nB_ctrl(), send_wls_u_oneloop(), and send_wls_v_oneloop().

Here is the call graph for this function:

◆ oneloop_nB_propagate_filters()

void oneloop_nB_propagate_filters ( void )

Propagate the filters.

Definition at line 2210 of file oneloop_nB.c.

References ACCEL_FLOAT_OF_BFP, accely_filt, airspeed_filt, LP, FloatRates::p, FloatRates::q, FloatRates::r, 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_nB_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_nB_RM()

void oneloop_nB_RM	(	bool	half_loop,
		struct FloatVect3	PSA_des,
		bool	in_flight_oneloop )

Function to generate the reference signals for the oneloop controller.

Parameters

half_loop	In half-loop mode the controller is used for stabilization only
PSA_des	Desired position/speed/acceleration

Definition at line 2371 of file oneloop_nB.c.

References a_thrust, ANDI_NUM_ACT, commands, CTRL_ANDI, ctrl_effort_model, CTRL_INDI, CTRL_NB_ANDI, CTRL_NB_INDI, ctrl_off, dt_1l, EFF_MAT_G, eul_of_acc(), eulers_zxy, eulers_zxy_des, fault_pitch_motors, fault_roll_motors, float_vect_zero(), heading_manual, IDX_aD, k_att_rm, k_D, k_I, k_K, k_P, k_pos_e, max_phi, MAX_PPRZ, max_pusher_cmd, max_r, max_theta, nav, nav_target, nu, oneloop_nB, oneloop_nB_sideslip(), oneloop_nB_Z_hold, pid_a_max, pid_v_max_manual, pid_v_max_nav, POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, Pos_KPID_ARW(), positive_non_zero(), psi_des_deg, psi_des_rad, psi_vec, push_PID_vel, push_PID_vel_d, pusher_cmd_fun(), radio_body_ctrl, radio_control_get(), RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, ratio_u_un, ratio_vn_v, rm_3rd_attitude(), rm_3rd_nI(), RW, shape_vector(), SPIN_MAN_IDLE, spin_man_state, SpinQuad, sta_bounds, stateGetSpeedNed_f(), temp_acc_des, update_butterworth_2_low_pass(), use_push_PID, use_push_Position, vel_ctrl_in_manual, Vel_PID_ARW(), FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, yaw_stick_in_auto, and FloatVect3::z.

Referenced by oneloop_nB_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_nB_run()

void oneloop_nB_run	(	bool	in_flight,
		bool	half_loop,
		struct FloatVect3	PSA_des )

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

Definition at line 2649 of file oneloop_nB.c.

References act_dyn_ctrl, act_max, act_min, actuator_state_1l, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, andi_u, andi_u_n, auto_fault_cmd, autopilot, bwls_1l, calc_normalization(), COMMAND_PITCH, COMMAND_ROLL, commands, CTRL_ANDI, ctrl_effort_model, CTRL_INDI, CTRL_NB_ANDI, CTRL_NB_INDI, delta_fault, drop_axis(), ec_3rd_att(), eulers_zxy, eulers_zxy_des, fault_pitch_motors, fault_roll_motors, float_eulers_of_quat_zxy(), G1G2_oneloop(), g2_ff, G2_RW, get_act_state_oneloop(), heading_manual, IDX_aD, IDX_ap, IDX_aq, IDX_ar, init_controller_gains(), k_att_e, LP, max_fault_mot, max_phi, MAX_PPRZ, max_theta, nB_2d_filt, nB_3d_state_filt, nB_EC(), nB_jerk_des, normalize_nu(), nu, num_thrusters_oneloop, oneloop_calc_model_disturbance(), oneloop_nB, oneloop_nB_calc_nB_states(), ONELOOP_NB_DEBUG_MODE, oneloop_nB_propagate_filters(), oneloop_nB_RM(), oneloop_nB_state_compensation(), psi_des_deg, psi_des_rad, radio_control_get(), RADIO_THROTTLE, ratio_u_un, safety_killer_cutoff, safety_killer_trigger, set_WLS_settings(), spin_man_alpha, SPIN_MAN_DOWN_SLOW, spin_man_mot, spin_man_state, spin_man_trim_rate, spin_man_u_hold, spin_man_u_last, spin_man_update(), spin_prot_bound_params(), spin_prot_max_cmd, spin_prot_max_rate, spin_prot_ratio, spin_prot_start_rate, spin_trim, spin_trim_bleed_rate, spin_trim_on, spin_trim_rate, spin_trim_release_rate, SpinQuad_overwrite(), sta_bounds, stabilization, state_compensation_on, stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetSpeedNed_f(), TestMotorIDX, u_filt, use_safety_killer, wls_alloc(), WLS_one_p, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by guidance_oneloop_nB_run_mode(), and oneloop_from_nav().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_nB_sideslip()

float oneloop_nB_sideslip ( void )

Function to calculate corrections for sideslip DEPRECIATED.

Definition at line 3150 of file oneloop_nB.c.

References accely_filt, airspeed_filt, eulers_zxy, fwd_sideslip_gain, g, and max_phi.

Referenced by oneloop_nB_RM().

Here is the caller graph for this function:

◆ oneloop_nB_state_compensation()

void oneloop_nB_state_compensation ( bool state_compensation_on )

Function that performs state compensation for gyroscopic effects.

Definition at line 3309 of file oneloop_nB.c.

References IDX_ap, IDX_aq, nu, oneloop_nB, p, RW, and state_compensation_on.

Referenced by oneloop_nB_run().

Here is the caller graph for this function:

◆ phase_first_order()

float phase_first_order	(	float	r,
		float	w )

Definition at line 3808 of file oneloop_nB.c.

Referenced by xi_fun().

Here is the caller graph for this function:

◆ Pos_KPID_ARW()

void Pos_KPID_ARW	(	const float	x_des[3],
		const float	x[3],
		const float	x_dot[3],
		float	k_K,
		float	k_P,
		float	k_I,
		float	k_D,
		float	a_sp[3],
		float	a_max,
		float	v_max )

static

Definition at line 1333 of file oneloop_nB.c.

References ARW_ClampVec3(), dt_1l, k_D, k_I, k_K, k_P, KPID_filt, KPID_vel_filt, update_butterworth_2_low_pass(), v_max, and vect_bound_nd().

Referenced by oneloop_nB_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ positive_non_zero()

float positive_non_zero ( float input )

static

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

Definition at line 1109 of file oneloop_nB.c.

Referenced by acc_body_bound(), calc_normalization(), ec_poles(), k_rm_1_3_f(), k_rm_2_3_f(), k_rm_3_3_f(), oneloop_calc_model_disturbance(), oneloop_nB_init(), oneloop_nB_RM(), and vect_bound_nd().

Here is the caller graph for this function:

◆ PRINT_CONFIG_VAR() [1/5]

PRINT_CONFIG_VAR ( ONELOOP_NB_FILT_CUTOFF )

References PRINT_CONFIG_VAR().

Here is the call graph for this function:

◆ PRINT_CONFIG_VAR() [2/5]

PRINT_CONFIG_VAR ( ONELOOP_NB_FILT_CUTOFF_P )

References PRINT_CONFIG_VAR().

Here is the call graph for this function:

◆ PRINT_CONFIG_VAR() [3/5]

PRINT_CONFIG_VAR ( ONELOOP_NB_FILT_CUTOFF_Q )

References PRINT_CONFIG_VAR().

Here is the call graph for this function:

◆ PRINT_CONFIG_VAR() [4/5]

PRINT_CONFIG_VAR ( ONELOOP_NB_FILT_CUTOFF_R )

References ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_NUM_VIRTUAL_ACT, ANDI_OUTPUTS, PRINT_CONFIG_MSG(), and PRINT_CONFIG_VAR().

Here is the call graph for this function:

◆ PRINT_CONFIG_VAR() [5/5]

PRINT_CONFIG_VAR ( ONELOOP_NB_MAX_THETA )

References max_bank, PRINT_CONFIG_MSG(), and PRINT_CONFIG_VAR().

Here is the call graph for this function:

◆ pusher_cmd_fun()

float pusher_cmd_fun	(	float	aN_des,
		float	aE_des,
		float	r,
		float	psi,
		float	w,
		int	n )

Definition at line 3823 of file oneloop_nB.c.

References cos_2n(), xi, and xi_fun().

Referenced by oneloop_nB_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reinit_all_LP()

void reinit_all_LP ( bool reinit )

inlinestatic

Reinitialize all the Low Pass Filters.

Definition at line 2126 of file oneloop_nB.c.

References ANDI_NUM_ACT_TOT, init_butterworth_2_low_pass(), LP, nB_2d_filt, nB_3d_state_filt, nB_filt, oneloop_nB_filt_cutoff, reinit_LP(), state_term, and u_filt.

Referenced by oneloop_nB_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 2270 of file oneloop_nB.c.

References eulers_zxy_des, float_vect_copy(), float_vect_zero(), and oneloop_nB.

Referenced by oneloop_nB_enter().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reinit_LP()

void reinit_LP	(	struct LP_t *	LP,
		bool	reinit )

inlinestatic

Definition at line 2098 of file oneloop_nB.c.

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

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

References f, init_filter_on_type(), and LP.

Here is the call graph for this function:

◆ reshape_wind()

void reshape_wind ( void )

Definition at line 3183 of file oneloop_nB.c.

References acc_body_bound(), airspeed_filt, eulers_zxy, FLOAT_ANGLE_NORMALIZE, FLOAT_VECT2_NORM, force_forward, gi_unbounded_airspeed_sp, k_pos_rm, max_a_nav, max_as, min_as, nav_target, nav_target_new, oneloop_nB, ONELOOP_NB_AIRSPEED_SWITCH_THRESHOLD, VECT2_DIFF, vect_bound_nd(), FloatVect2::x, and FloatVect2::y.

Here is the call 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 1689 of file oneloop_nB.c.

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

Here is the call 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 1666 of file oneloop_nB.c.

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

Here is the call 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 1559 of file oneloop_nB.c.

References OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, 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(), and integrate_nd().

Referenced by oneloop_nB_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_3rd_nI()

void rm_3rd_nI	(	float	dt,
		struct FloatVect3 *	x_ref,
		struct FloatVect3 *	x_d_ref,
		struct FloatVect3 *	x_2d_ref,
		struct FloatVect3 *	x_3d_ref,
		const struct FloatVect3 *	x_des,
		const float	k1_rm[3],
		const float	k2_rm[3],
		const float	k3_rm[3] )

Definition at line 3681 of file oneloop_nB.c.

References FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by oneloop_nB_RM().

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

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

Here is the call graph for this function:

◆ send_eff_mat_guid_oneloop_nB()

void send_eff_mat_guid_oneloop_nB	(	struct transport_tx *	trans,
		struct link_device *	dev )

static

Definition at line 801 of file oneloop_nB.c.

References ANDI_NUM_ACT_TOT, dev, and EFF_MAT_RW.

Referenced by oneloop_nB_init().

Here is the caller graph for this function:

◆ send_eff_mat_stab_oneloop_nB()

void send_eff_mat_stab_oneloop_nB	(	struct transport_tx *	trans,
		struct link_device *	dev )

static

Definition at line 768 of file oneloop_nB.c.

References ANDI_NUM_ACT, bwls_1l, dev, EFF_MAT_RW, and G2_RW.

Referenced by oneloop_nB_init().

Here is the caller graph for this function:

◆ send_eff_mat_stab_thrust_oneloop_nB()

void send_eff_mat_stab_thrust_oneloop_nB	(	struct transport_tx *	trans,
		struct link_device *	dev )

static

Definition at line 785 of file oneloop_nB.c.

References ANDI_NUM_ACT, bwls_1l, dev, and EFF_MAT_RW.

Referenced by oneloop_nB_init().

Here is the caller graph for this function:

◆ send_guidance_oneloop_nB()

void send_guidance_oneloop_nB	(	struct transport_tx *	trans,
		struct link_device *	dev )

static

Definition at line 846 of file oneloop_nB.c.

References dev, and oneloop_nB.

Referenced by oneloop_nB_init().

Here is the caller graph for this function:

◆ send_oneloop_debug()

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

static

Definition at line 900 of file oneloop_nB.c.

References andi_u, debug_vect(), dev, eulers_zxy, fault_pitch_motors, fault_roll_motors, IDX_ar, LP, nu, oneloop_nB, psi_des_rad, radio_control_get(), RADIO_THROTTLE, spin_man_alpha, spin_man_down, spin_man_r_ref, spin_man_state, spin_man_u_hold, spin_man_up, spin_prot_ratio, spin_trim, and SQ_r.

Here is the call graph for this function:

◆ send_oneloop_nB()

void send_oneloop_nB	(	struct transport_tx *	trans,
		struct link_device *	dev )

static

Definition at line 808 of file oneloop_nB.c.

References actuator_state_1l, ANDI_NUM_ACT, dev, eulers_zxy_des, and oneloop_nB.

Referenced by oneloop_nB_init().

Here is the caller graph for this function:

◆ send_oneloop_nB_ctrl()

void send_oneloop_nB_ctrl	(	struct transport_tx *	trans,
		struct link_device *	dev )

static

Definition at line 822 of file oneloop_nB.c.

References dev, and oneloop_nB.

Referenced by oneloop_nB_init().

Here is the caller graph for this function:

◆ send_wls_u_oneloop()

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

static

Definition at line 764 of file oneloop_nB.c.

References dev, send_wls_u(), and WLS_one_p.

Here is the call graph for this function:

◆ send_wls_v_oneloop()

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

static

Definition at line 760 of file oneloop_nB.c.

References dev, send_wls_v(), and WLS_one_p.

Here is the call graph for this function:

◆ set_WLS_settings()

void set_WLS_settings ( void )

Function that sets the WLS settings for oneloop controller.

Definition at line 3348 of file oneloop_nB.c.

References act_max, act_min, ANDI_NUM_ACT_TOT, CTRL_NB_ANDI, CTRL_NB_INDI, drop_aD, drop_pitch, drop_roll, drop_yaw, fault_ailerons, fault_pitch_motors, fault_roll_motors, IDX_aD, IDX_ap, IDX_aq, IDX_ar, oneloop_nB, ratio_u_un, RW, u_pref, WLS_one_p, Wu_backup, and Wv_backup.

Referenced by oneloop_nB_run().

Here is the caller graph for this function:

◆ shape_vector()

void shape_vector ( float a[3] )

static

Definition at line 1398 of file oneloop_nB.c.

References vect_bound_nd().

Referenced by oneloop_nB_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ skew_symmetric()

void skew_symmetric	(	struct FloatRMat *	out,
		const struct FloatVect3 *	in )

Definition at line 3665 of file oneloop_nB.c.

References FloatRMat::m, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by oneloop_nB_calc_nB_states().

Here is the caller graph for this function:

◆ spin_man_ramp_ref()

void spin_man_ramp_ref ( float target )

static

Move the yaw rate reference towards target at spin_man_ramp_rate.

Definition at line 1000 of file oneloop_nB.c.

References dt_1l, spin_man_r_ref, spin_man_ramp_rate, step, and target.

Referenced by spin_man_update().

Here is the caller graph for this function:

◆ spin_man_rate_ctrl()

bool spin_man_rate_ctrl ( void )

static

Yaw rate tracking is used by the manoeuvre (no fault active).

Definition at line 1007 of file oneloop_nB.c.

References SPIN_MAN_DOWN_BLEND, SPIN_MAN_DOWN_RAMP, spin_man_state, and SPIN_MAN_UP_RAMP.

Referenced by SpinQuad_overwrite().

Here is the caller graph for this function:

◆ spin_man_reset()

void spin_man_reset ( void )

static

Stop the spin manoeuvre and clear its GCS triggers (fault flags are left as they are).

Definition at line 976 of file oneloop_nB.c.

References spin_man_alpha, spin_man_down, SPIN_MAN_IDLE, spin_man_state, and spin_man_up.

Referenced by oneloop_nB_enter(), and spin_man_update().

Here is the caller graph for this function:

◆ spin_man_set_pair()

void spin_man_set_pair ( uint8_t pair )

static

Select the motors of a pair (pitch: FRONT/BACK, roll: RIGHT/LEFT) for the blend.

Definition at line 984 of file oneloop_nB.c.

References spin_man_mot, and SPIN_MAN_PAIR_ROLL.

Referenced by spin_man_update().

Here is the caller graph for this function:

◆ spin_man_start_blend()

void spin_man_start_blend ( void )

static

Start a bumpless transfer of the selected pair from the average of its last applied commands, so that both motors of the pair are commanded the same during the blend.

Definition at line 992 of file oneloop_nB.c.

References spin_man_alpha, spin_man_mot, spin_man_u_hold, and spin_man_u_last.

Referenced by spin_man_update().

Here is the caller graph for this function:

◆ spin_man_update()

void spin_man_update	(	bool	in_flight,
		bool	nb_ctrl )

static

Spin transition manoeuvre state machine.

Sets the fault flags and the yaw rate reference. Spin up: UP_RAMP -> (|r| >= release) fault spin_man_pair -> UP_BLEND -> IDLE Spin down: DOWN_SLOW -> (|r| < release - gap) unfault the faulted pair -> DOWN_BLEND -> DOWN_RAMP -> (|r| < done) IDLE

Definition at line 1016 of file oneloop_nB.c.

References dt_1l, fault_pitch_motors, fault_roll_motors, oneloop_nB, spin_man_alpha, spin_man_blend_time, spin_man_dir, spin_man_done_rate, spin_man_down, SPIN_MAN_DOWN_BLEND, SPIN_MAN_DOWN_RAMP, SPIN_MAN_DOWN_SLOW, SPIN_MAN_IDLE, spin_man_pair, SPIN_MAN_PAIR_PITCH, SPIN_MAN_PAIR_ROLL, spin_man_pitch_dir, spin_man_r_ref, spin_man_ramp_ref(), spin_man_reset(), spin_man_set_pair(), spin_man_start_blend(), spin_man_state, spin_man_up, SPIN_MAN_UP_BLEND, SPIN_MAN_UP_RAMP, spin_prot_min_gap, spin_prot_start_rate, and spin_trim_release_rate.

Referenced by oneloop_nB_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ spin_prot_bound_params()

void spin_prot_bound_params ( void )

static

Enforce valid spin protection / spin trim parameters (they can be changed in flight from the settings).

Order enforced: 0 <= release <= start - gap, start <= max - gap. The max rate is the safety limit and is never raised to fix an inconsistent setting, the lower rates are pushed down instead.

Definition at line 959 of file oneloop_nB.c.

References MAX_PPRZ, spin_man_blend_time, spin_man_done_rate, spin_man_pair, SPIN_MAN_PAIR_PITCH, SPIN_MAN_PAIR_ROLL, spin_man_pitch_dir, spin_man_ramp_rate, spin_man_trim_rate, spin_prot_max_cmd, spin_prot_max_rate, spin_prot_min_gap, spin_prot_start_rate, spin_trim_bleed_rate, spin_trim_rate, and spin_trim_release_rate.

Referenced by oneloop_nB_run().

Here is the caller graph for this function:

◆ SpinQuad_overwrite()

void SpinQuad_overwrite	(	float	gain,
		float	ce_model,
		float *	nu_stab_2 )

Definition at line 3749 of file oneloop_nB.c.

References k_att_e, MAX_PPRZ, oneloop_nB, RADIO_AUX4, radio_control, spin_man_r_ref, spin_man_rate_ctrl(), SpinQuad, SQ_r, and sta_bounds.

Referenced by oneloop_nB_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_filter_on_type()

void update_filter_on_type	(	struct LP_t *	filter,
		float	input )

inlinestatic

Update a filter based on its type.

Definition at line 2039 of file oneloop_nB.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_nB_propagate_filters().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ vec3_saturated()

bool vec3_saturated	(	const float	u_unsat[3],
		const float	u_sat[3],
		float	eps )

inlinestatic

Definition at line 1229 of file oneloop_nB.c.

Referenced by ARW_ClampVec3().

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

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ Vel_PID_ARW()

void Vel_PID_ARW	(	float	x_dot_des[3],
		const float	x_dot[3],
		float	k_P,
		float	k_I,
		float	k_D,
		float	a_sp[3],
		float	a_max,
		float	v_max )

static

Definition at line 1275 of file oneloop_nB.c.

References ARW_ClampVec3(), dt_1l, k_D, k_I, k_P, KPID_filt, KPID_vel_filt, update_butterworth_2_low_pass(), v_max, and vect_bound_nd().

Referenced by oneloop_nB_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ xi_fun()

float xi_fun	(	float	r,
		float	w,
		float	aN_des,
		float	aE_des )

Definition at line 3815 of file oneloop_nB.c.

References oneloop_nB, and phase_first_order().

Referenced by pusher_cmd_fun().

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

◆ accely_filt

Butterworth2LowPass accely_filt

static

Definition at line 188 of file oneloop_nB.c.

◆ act_dyn_ctrl

float act_dyn_ctrl[ANDI_NUM_ACT_TOT] = = {1}

Definition at line 215 of file oneloop_nB.c.

◆ act_dynamics

float act_dynamics[ANDI_NUM_ACT_TOT] = = {1}

Definition at line 214 of file oneloop_nB.c.

◆ act_dynamics_d

float act_dynamics_d[ANDI_NUM_ACT_TOT]

static

Definition at line 305 of file oneloop_nB.c.

◆ act_max

float act_max[ANDI_NUM_ACT_TOT] = = {MAX_PPRZ}

Definition at line 221 of file oneloop_nB.c.

◆ act_max_norm

float act_max_norm[ANDI_NUM_ACT_TOT] = = {1.0}

Definition at line 233 of file oneloop_nB.c.

◆ act_min

float act_min[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 227 of file oneloop_nB.c.

◆ act_min_norm

float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 239 of file oneloop_nB.c.

◆ actuator_is_servo

bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0}

Definition at line 206 of file oneloop_nB.c.

◆ actuator_state_1l

float actuator_state_1l[ANDI_NUM_ACT_TOT]

Definition at line 306 of file oneloop_nB.c.

◆ airspeed_filt

Butterworth2LowPass airspeed_filt

static

Definition at line 189 of file oneloop_nB.c.

◆ andi_du

float andi_du[ANDI_NUM_ACT_TOT]

Definition at line 301 of file oneloop_nB.c.

◆ andi_u

float andi_u[ANDI_NUM_ACT_TOT]

Definition at line 300 of file oneloop_nB.c.

◆ andi_u_n

float andi_u_n[ANDI_NUM_ACT_TOT]

static

Definition at line 302 of file oneloop_nB.c.

◆ auto_fault_cmd

bool auto_fault_cmd = false

Definition at line 514 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ bwls_1l

float* bwls_1l[ANDI_OUTPUTS]

Definition at line 484 of file oneloop_nB.c.

◆ ctrl_effort_model

float ctrl_effort_model[ANDI_OUTPUTS]

Definition at line 488 of file oneloop_nB.c.

Referenced by drop_axis(), oneloop_calc_model_disturbance(), oneloop_nB_RM(), and oneloop_nB_run().

◆ ctrl_off

bool ctrl_off = false

Definition at line 482 of file oneloop_nB.c.

◆ debug_state

float debug_state[3]

Definition at line 519 of file oneloop_nB.c.

Referenced by oneloop_nB_calc_nB_states().

◆ delta_fault

float delta_fault = 1000.0

Definition at line 524 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ drop_aD

bool drop_aD = false

Definition at line 505 of file oneloop_nB.c.

◆ drop_pitch

bool drop_pitch = false

Definition at line 504 of file oneloop_nB.c.

◆ drop_roll

bool drop_roll = false

Definition at line 503 of file oneloop_nB.c.

◆ drop_yaw

bool drop_yaw = false

Definition at line 502 of file oneloop_nB.c.

◆ dt_1l

float dt_1l = 1. / PERIODIC_FREQUENCY

static

Definition at line 115 of file oneloop_nB.c.

◆ ec_headroom

float ec_headroom = 1.5

Definition at line 433 of file oneloop_nB.c.

◆ ec_k3

float ec_k3 = ONELOOP_NB_EC_K3

Definition at line 622 of file oneloop_nB.c.

Referenced by init_controller_gains().

◆ EFF_MAT_G

float EFF_MAT_G[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

Definition at line 485 of file oneloop_nB.c.

◆ eulers_zxy

struct FloatEulers eulers_zxy

Definition at line 388 of file oneloop_nB.c.

◆ eulers_zxy_des

struct FloatEulers eulers_zxy_des

Definition at line 387 of file oneloop_nB.c.

◆ fault_ailerons

bool fault_ailerons = ONELOOP_NB_FAULT_AILERONS

Definition at line 501 of file oneloop_nB.c.

Referenced by G1G2_oneloop(), and set_WLS_settings().

◆ fault_pitch_motors

bool fault_pitch_motors = false

Definition at line 496 of file oneloop_nB.c.

Referenced by G1G2_oneloop(), oneloop_nB_RM(), oneloop_nB_run(), send_oneloop_debug(), set_WLS_settings(), and spin_man_update().

◆ fault_roll_motors

bool fault_roll_motors = false

Definition at line 497 of file oneloop_nB.c.

Referenced by G1G2_oneloop(), oneloop_nB_RM(), oneloop_nB_run(), send_oneloop_debug(), set_WLS_settings(), and spin_man_update().

◆ fwd_sideslip_gain

float fwd_sideslip_gain = 0.2

Definition at line 399 of file oneloop_nB.c.

◆ g

float g = 9.81

static

Definition at line 116 of file oneloop_nB.c.

◆ g2_ff

float g2_ff = 0.0

static

Definition at line 481 of file oneloop_nB.c.

◆ gi_unbounded_airspeed_sp

float gi_unbounded_airspeed_sp = 0.0

Definition at line 436 of file oneloop_nB.c.

◆ heading_manual

bool heading_manual = false

Definition at line 333 of file oneloop_nB.c.

◆ k1_NE_tune

float k1_NE_tune = 0.6

Definition at line 697 of file oneloop_nB.c.

◆ k2_NE_tune

float k2_NE_tune = 1.85

Definition at line 698 of file oneloop_nB.c.

◆ k_att_e

struct Gains3rdOrder k_att_e

Definition at line 690 of file oneloop_nB.c.

◆ k_att_e_indi

struct Gains3rdOrder k_att_e_indi

Definition at line 695 of file oneloop_nB.c.

◆ k_att_rm

struct Gains3rdOrder k_att_rm

Definition at line 691 of file oneloop_nB.c.

◆ k_D

float k_D = 0.2

Definition at line 703 of file oneloop_nB.c.

◆ k_I

float k_I = 0.4

Definition at line 702 of file oneloop_nB.c.

◆ k_K

float k_K = 0.6

Definition at line 700 of file oneloop_nB.c.

Referenced by oneloop_nB_RM(), and Pos_KPID_ARW().

◆ k_P

float k_P = 1.8

Definition at line 701 of file oneloop_nB.c.

◆ k_pos_e

struct Gains3rdOrder k_pos_e

Definition at line 692 of file oneloop_nB.c.

◆ k_pos_e_indi

struct Gains3rdOrder k_pos_e_indi

Definition at line 696 of file oneloop_nB.c.

◆ k_pos_rm

struct Gains3rdOrder k_pos_rm

Definition at line 693 of file oneloop_nB.c.

◆ KPID_filt

Butterworth2LowPass KPID_filt[3]

static

Definition at line 185 of file oneloop_nB.c.

Referenced by init_filter(), Pos_KPID_ARW(), and Vel_PID_ARW().

◆ KPID_vel_filt

Butterworth2LowPass KPID_vel_filt[3]

static

Definition at line 184 of file oneloop_nB.c.

Referenced by init_filter(), Pos_KPID_ARW(), and Vel_PID_ARW().

◆ LP

struct Oneloop_LP_t LP

Definition at line 182 of file oneloop_nB.c.

◆ m_array

float m_array[ANDI_NUM_ACT_TOT]

Definition at line 487 of file oneloop_nB.c.

◆ max_a_nav

float max_a_nav = 4.0

Definition at line 407 of file oneloop_nB.c.

◆ max_as

float max_as = 19.0f

Definition at line 430 of file oneloop_nB.c.

◆ max_bank

float max_bank = M_PI_6

Definition at line 264 of file oneloop_nB.c.

Referenced by guidance_set_max_bank_angle(), init_controller_gains(), oneloop_andi_read_rc(), oneloop_nB_read_rc(), and PRINT_CONFIG_VAR().

◆ max_fault_mot

float max_fault_mot = ONELOOP_NB_MAX_FAULT_MOT

Definition at line 314 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ max_j_lin

float max_j_lin = 500.0

Definition at line 413 of file oneloop_nB.c.

◆ max_phi

float max_phi = M_PI_6

Definition at line 270 of file oneloop_nB.c.

Referenced by eul_of_acc(), guidance_indi_run(), oneloop_andi_sideslip(), oneloop_nB_RM(), oneloop_nB_run(), oneloop_nB_sideslip(), stabilization_attitude_read_rc_setpoint_eulers(), and stabilization_attitude_read_rc_setpoint_eulers_f().

◆ max_pusher_cmd

float max_pusher_cmd = 7500

Definition at line 518 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ max_r

float max_r = RadOfDeg(180.0)

Definition at line 319 of file oneloop_nB.c.

◆ max_theta

float max_theta = M_PI_6

Definition at line 276 of file oneloop_nB.c.

Referenced by eul_of_acc(), oneloop_nB_RM(), and oneloop_nB_run().

◆ max_v_nav

float max_v_nav = 5.0

Definition at line 419 of file oneloop_nB.c.

◆ max_v_nav_v

float max_v_nav_v = 1.5

Definition at line 425 of file oneloop_nB.c.

◆ min_as

float min_as = 0.0f

Definition at line 431 of file oneloop_nB.c.

◆ n_array

float n_array[ANDI_OUTPUTS]

Definition at line 486 of file oneloop_nB.c.

◆ nav_target

float nav_target[3]

static

Definition at line 434 of file oneloop_nB.c.

◆ nav_target_new

float nav_target_new[3]

static

Definition at line 435 of file oneloop_nB.c.

◆ nB_2d_filt

Butterworth2LowPass nB_2d_filt[3]

static

Definition at line 192 of file oneloop_nB.c.

Referenced by init_filter(), oneloop_nB_calc_nB_states(), oneloop_nB_run(), and reinit_all_LP().

◆ nB_3d_state_filt

Butterworth2LowPass nB_3d_state_filt[2]

static

Definition at line 193 of file oneloop_nB.c.

Referenced by init_filter(), oneloop_nB_calc_nB_states(), oneloop_nB_run(), and reinit_all_LP().

◆ nB_filt

Butterworth2LowPass nB_filt[3]

static

Definition at line 191 of file oneloop_nB.c.

Referenced by G1G2_oneloop(), init_filter(), oneloop_nB_calc_nB_states(), and reinit_all_LP().

◆ nB_jerk_des

float nB_jerk_des[3]

Definition at line 307 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ nu

float nu[ANDI_OUTPUTS]

Definition at line 303 of file oneloop_nB.c.

◆ nu_n

float nu_n[ANDI_OUTPUTS]

Definition at line 304 of file oneloop_nB.c.

◆ nu_norm_max

float nu_norm_max = 1.0

Definition at line 245 of file oneloop_nB.c.

◆ num_thrusters_oneloop

float num_thrusters_oneloop = 4.0

Definition at line 198 of file oneloop_nB.c.

◆ oneloop_nB

struct OneloopGeneral oneloop_nB

◆ oneloop_nB_filt_cutoff

float oneloop_nB_filt_cutoff = 2.0

Definition at line 130 of file oneloop_nB.c.

Referenced by eff_scheduling_rotwing_init(), init_all_LP(), init_filter(), init_RW_Model(), reinit_all_LP(), and update_attitude().

◆ oneloop_nB_filt_cutoff_a

float oneloop_nB_filt_cutoff_a = 2.0

Definition at line 136 of file oneloop_nB.c.

◆ oneloop_nB_filt_cutoff_p

float oneloop_nB_filt_cutoff_p = 20.0

Definition at line 149 of file oneloop_nB.c.

◆ oneloop_nB_filt_cutoff_q

float oneloop_nB_filt_cutoff_q = 20.0

Definition at line 156 of file oneloop_nB.c.

◆ oneloop_nB_filt_cutoff_r

float oneloop_nB_filt_cutoff_r = 20.0

Definition at line 163 of file oneloop_nB.c.

◆ oneloop_nB_filt_cutoff_v

float oneloop_nB_filt_cutoff_v = 30.0

Definition at line 142 of file oneloop_nB.c.

Referenced by init_filter().

◆ oneloop_nB_yaw_dist_limit

float oneloop_nB_yaw_dist_limit = 99999.f

Definition at line 327 of file oneloop_nB.c.

◆ oneloop_nB_Z_hold

bool oneloop_nB_Z_hold = false

Definition at line 510 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ p_alt_e

struct PolePlacement p_alt_e

Definition at line 684 of file oneloop_nB.c.

◆ p_alt_rm

struct PolePlacement p_alt_rm

Definition at line 685 of file oneloop_nB.c.

◆ p_att_e

struct PolePlacement p_att_e

Definition at line 677 of file oneloop_nB.c.

◆ p_att_rm

struct PolePlacement p_att_rm

Definition at line 679 of file oneloop_nB.c.

◆ p_head_e

struct PolePlacement p_head_e

Definition at line 687 of file oneloop_nB.c.

◆ p_head_rm

struct PolePlacement p_head_rm

Definition at line 688 of file oneloop_nB.c.

◆ p_pos_e

struct PolePlacement p_pos_e

Definition at line 681 of file oneloop_nB.c.

◆ p_pos_rm

struct PolePlacement p_pos_rm

Definition at line 682 of file oneloop_nB.c.

◆ p_roll_e

struct PolePlacement p_roll_e

Definition at line 678 of file oneloop_nB.c.

◆ pid_a_max

float pid_a_max = ONELOOP_NB_PID_A_MAX

Definition at line 639 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ pid_v_max_manual

float pid_v_max_manual = ONELOOP_NB_PID_V_MAX_MANUAL

Definition at line 640 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ pid_v_max_nav

float pid_v_max_nav = ONELOOP_NB_PID_V_MAX_NAV

Definition at line 641 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ psi_des_deg

float psi_des_deg = 0.0

Definition at line 390 of file oneloop_nB.c.

◆ psi_des_rad

float psi_des_rad = 0.0

Definition at line 389 of file oneloop_nB.c.

◆ push_PID_vel

Butterworth2LowPass push_PID_vel[2]

static

Definition at line 186 of file oneloop_nB.c.

Referenced by init_filter(), and oneloop_nB_RM().

◆ push_PID_vel_d

Butterworth2LowPass push_PID_vel_d[2]

static

Definition at line 187 of file oneloop_nB.c.

Referenced by init_filter(), and oneloop_nB_RM().

◆ radio_body_ctrl

bool radio_body_ctrl = false

Definition at line 509 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ ratio_u_un

float ratio_u_un[ANDI_NUM_ACT_TOT]

Definition at line 489 of file oneloop_nB.c.

◆ ratio_vn_v

float ratio_vn_v[ANDI_OUTPUTS]

Definition at line 490 of file oneloop_nB.c.

◆ safety_killer_cutoff

float safety_killer_cutoff = 7500.0

Definition at line 516 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ safety_killer_trigger

bool safety_killer_trigger = false

Definition at line 513 of file oneloop_nB.c.

Referenced by oneloop_nB_enter(), oneloop_nB_init(), and oneloop_nB_run().

◆ slow_pole

float slow_pole = 22.0

Definition at line 672 of file oneloop_nB.c.

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

◆ spin_man_alpha

float spin_man_alpha = 1.0

static

Definition at line 662 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), send_oneloop_debug(), spin_man_reset(), spin_man_start_blend(), and spin_man_update().

◆ spin_man_blend_time

float spin_man_blend_time = ONELOOP_NB_SPIN_MAN_BLEND_TIME

Definition at line 656 of file oneloop_nB.c.

Referenced by spin_man_update(), and spin_prot_bound_params().

◆ spin_man_dir

float spin_man_dir = 1.0

static

Definition at line 661 of file oneloop_nB.c.

Referenced by spin_man_update().

◆ spin_man_done_rate

float spin_man_done_rate = ONELOOP_NB_SPIN_MAN_DONE_RATE

Definition at line 658 of file oneloop_nB.c.

Referenced by spin_man_update(), and spin_prot_bound_params().

◆ spin_man_down

bool spin_man_down = false

Definition at line 652 of file oneloop_nB.c.

Referenced by send_oneloop_debug(), spin_man_reset(), and spin_man_update().

◆ spin_man_mot

uint8_t spin_man_mot[2] = {COMMAND_MOTOR_FRONT, COMMAND_MOTOR_BACK}

static

Definition at line 663 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), spin_man_set_pair(), and spin_man_start_blend().

◆ spin_man_pair

uint8_t spin_man_pair = ONELOOP_NB_SPIN_MAN_PAIR

Definition at line 653 of file oneloop_nB.c.

Referenced by spin_man_update(), and spin_prot_bound_params().

◆ spin_man_pitch_dir

float spin_man_pitch_dir = ONELOOP_NB_SPIN_MAN_PITCH_DIR

Definition at line 654 of file oneloop_nB.c.

Referenced by spin_man_update(), and spin_prot_bound_params().

◆ spin_man_r_ref

float spin_man_r_ref = 0.0

Definition at line 660 of file oneloop_nB.c.

Referenced by send_oneloop_debug(), spin_man_ramp_ref(), spin_man_update(), and SpinQuad_overwrite().

◆ spin_man_ramp_rate

float spin_man_ramp_rate = ONELOOP_NB_SPIN_MAN_RAMP_RATE

Definition at line 655 of file oneloop_nB.c.

Referenced by spin_man_ramp_ref(), and spin_prot_bound_params().

◆ spin_man_state

uint8_t spin_man_state = SPIN_MAN_IDLE

Definition at line 659 of file oneloop_nB.c.

Referenced by oneloop_nB_RM(), oneloop_nB_run(), send_oneloop_debug(), spin_man_rate_ctrl(), spin_man_reset(), and spin_man_update().

◆ spin_man_trim_rate

float spin_man_trim_rate = ONELOOP_NB_SPIN_MAN_TRIM_RATE

Definition at line 657 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and spin_prot_bound_params().

◆ spin_man_u_hold

float spin_man_u_hold[2] = {0.0, 0.0}

static

Definition at line 664 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), send_oneloop_debug(), and spin_man_start_blend().

◆ spin_man_u_last

float spin_man_u_last[ANDI_NUM_ACT] = {0.0}

static

Definition at line 665 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and spin_man_start_blend().

◆ spin_man_up

bool spin_man_up = false

Definition at line 651 of file oneloop_nB.c.

Referenced by send_oneloop_debug(), spin_man_reset(), and spin_man_update().

◆ spin_prot_max_cmd

float spin_prot_max_cmd = ONELOOP_NB_SPIN_PROT_MAX_CMD

Definition at line 541 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and spin_prot_bound_params().

◆ spin_prot_max_rate

float spin_prot_max_rate = ONELOOP_NB_SPIN_PROT_MAX_RATE

Definition at line 539 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and spin_prot_bound_params().

◆ spin_prot_min_gap

float spin_prot_min_gap = ONELOOP_NB_SPIN_PROT_MIN_GAP

Definition at line 567 of file oneloop_nB.c.

Referenced by spin_man_update(), and spin_prot_bound_params().

◆ spin_prot_ratio

float spin_prot_ratio = 0.0

Definition at line 542 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and send_oneloop_debug().

◆ spin_prot_start_rate

float spin_prot_start_rate = ONELOOP_NB_SPIN_PROT_START_RATE

Definition at line 540 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), spin_man_update(), and spin_prot_bound_params().

◆ spin_trim

float spin_trim = 0.0

Definition at line 563 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and send_oneloop_debug().

◆ spin_trim_bleed_rate

float spin_trim_bleed_rate = ONELOOP_NB_SPIN_TRIM_BLEED_RATE

Definition at line 561 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and spin_prot_bound_params().

◆ spin_trim_on

bool spin_trim_on = ONELOOP_NB_SPIN_TRIM_ON

Definition at line 559 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ spin_trim_rate

float spin_trim_rate = ONELOOP_NB_SPIN_TRIM_RATE

Definition at line 560 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and spin_prot_bound_params().

◆ spin_trim_release_rate

float spin_trim_release_rate = ONELOOP_NB_SPIN_TRIM_RELEASE_RATE

Definition at line 562 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), spin_man_update(), and spin_prot_bound_params().

◆ SpinQuad

bool SpinQuad = false

Definition at line 495 of file oneloop_nB.c.

Referenced by oneloop_nB_RM(), and SpinQuad_overwrite().

◆ SpinQuadRate

float SpinQuadRate = 0.0

Definition at line 494 of file oneloop_nB.c.

◆ SQ_r

float SQ_r = 0.0

Definition at line 308 of file oneloop_nB.c.

Referenced by send_oneloop_debug(), and SpinQuad_overwrite().

◆ sta_bounds

struct OneloopStabilizationRef sta_bounds

Definition at line 342 of file oneloop_nB.c.

◆ state_compensation_on

bool state_compensation_on = false

Definition at line 506 of file oneloop_nB.c.

Referenced by oneloop_nB_run(), and oneloop_nB_state_compensation().

◆ state_term

float state_term[3]

Definition at line 520 of file oneloop_nB.c.

Referenced by oneloop_nB_calc_nB_states(), and reinit_all_LP().

◆ temp_acc_des

float temp_acc_des[3]

Definition at line 515 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ temp_D_error

float temp_D_error[3]

Definition at line 705 of file oneloop_nB.c.

◆ temp_I_error

float temp_I_error[3]

Definition at line 706 of file oneloop_nB.c.

◆ temp_P_error

float temp_P_error[3]

Definition at line 704 of file oneloop_nB.c.

◆ TestMotorIDX

int8_t TestMotorIDX = 0

Definition at line 113 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ theta_pref_max

float theta_pref_max = RadOfDeg(20.0)

Definition at line 280 of file oneloop_nB.c.

◆ u_filt

Butterworth2LowPass u_filt[ANDI_NUM_ACT_TOT]

static

Definition at line 190 of file oneloop_nB.c.

◆ u_pref

float u_pref[ANDI_NUM_ACT_TOT] = {0.0}

static

Definition at line 251 of file oneloop_nB.c.

◆ use_push_PID

bool use_push_PID = false

Definition at line 507 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ use_push_Position

bool use_push_Position = false

Definition at line 508 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

◆ use_safety_killer

bool use_safety_killer = false

Definition at line 512 of file oneloop_nB.c.

Referenced by oneloop_nB_run().

◆ vel_ctrl_in_manual

bool vel_ctrl_in_manual = true

Definition at line 511 of file oneloop_nB.c.

Referenced by oneloop_nB_RM().

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

◆ Wu_backup

float Wu_backup[ANDI_NUM_ACT_TOT] = {1.0}

static

Definition at line 471 of file oneloop_nB.c.

◆ Wu_quad_motors_fwd

float Wu_quad_motors_fwd = 6.0

Definition at line 441 of file oneloop_nB.c.

◆ Wv_backup

float Wv_backup[ANDI_OUTPUTS] = {1.0}

static

Definition at line 477 of file oneloop_nB.c.

◆ xdot_lim_sf

float xdot_lim_sf = 0.8

Definition at line 432 of file oneloop_nB.c.

◆ xi

float xi = 0.0

Definition at line 517 of file oneloop_nB.c.

Referenced by cos_2n(), pusher_cmd_fun(), and RungeKutta().

◆ yaw_stick_in_auto

bool yaw_stick_in_auto = false

Definition at line 339 of file oneloop_nB.c.

Macros

Enumerations

Functions

Variables

Macro Definition Documentation

◆ IDX_aD

◆ IDX_ap

◆ IDX_aq

◆ IDX_ar

◆ ONELOOP_NB_AIRSPEED_SWITCH_THRESHOLD

◆ ONELOOP_NB_DEBUG_MODE

◆ ONELOOP_NB_EC_K3

◆ ONELOOP_NB_FAULT_AILERONS

◆ ONELOOP_NB_MAX_FAULT_MOT

◆ ONELOOP_NB_PID_A_MAX

◆ ONELOOP_NB_PID_V_MAX_MANUAL

◆ ONELOOP_NB_PID_V_MAX_NAV

◆ ONELOOP_NB_SPIN_MAN_BLEND_TIME

◆ ONELOOP_NB_SPIN_MAN_DONE_RATE

◆ ONELOOP_NB_SPIN_MAN_PAIR

◆ ONELOOP_NB_SPIN_MAN_PITCH_DIR

◆ ONELOOP_NB_SPIN_MAN_RAMP_RATE

◆ ONELOOP_NB_SPIN_MAN_TRIM_RATE

◆ ONELOOP_NB_SPIN_PROT_MAX_CMD

◆ ONELOOP_NB_SPIN_PROT_MAX_RATE

◆ ONELOOP_NB_SPIN_PROT_MIN_GAP

◆ ONELOOP_NB_SPIN_PROT_START_RATE

◆ ONELOOP_NB_SPIN_TRIM_BLEED_RATE

◆ ONELOOP_NB_SPIN_TRIM_ON

◆ ONELOOP_NB_SPIN_TRIM_RATE

◆ ONELOOP_NB_SPIN_TRIM_RELEASE_RATE

◆ OVERRIDE_ATT_RM

◆ SPIN_MAN_PAIR_PITCH

◆ SPIN_MAN_PAIR_ROLL

◆ STREAM_BWLS

◆ USE_BW2

◆ USE_ND_VELOCITIES

Enumeration Type Documentation

◆ spin_man_state_t

Function Documentation

◆ acc_body_bound()

◆ ARW_ClampVec3()

◆ autopilot_in_flight_end_detection()

◆ bound_v_from_a()

◆ bound_v_from_a_vect()

◆ calc_HB_matrix()

◆ calc_normalization()

◆ chirp_call()

◆ chirp_pos()

◆ cos_2n()

◆ debug_vect()

◆ drop_axis()

◆ ec_3rd_att()

◆ ec_3rd_pos()

◆ ec_poles()

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

◆ init_poles_att()

◆ init_poles_pos()

◆ integrate_nd()

◆ k_rm_1_3_f()

◆ k_rm_2_3_f()

◆ k_rm_3_3_f()

◆ nB_EC()