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

Include dependency graph for oneloop_andi.c:

Macros
#define	ONELOOP_ANDI_SCHEDULING FALSE

#define	ONELOOP_ANDI_DEBUG_MODE FALSE

#define	ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL]

#define	ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL]

#define	ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH]

#define	ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

void	reshape_wind (void)

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

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

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

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

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

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

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

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

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

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

static float	k_e_2_2_f_v2 (float omega, float zeta)

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

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

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

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

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

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

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

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

void	oneloop_andi_init (void)
	Init function of Oneloop ANDI controller More...

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

void	oneloop_andi_RM (bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop)
	Function to generate the reference signals for the oneloop controller. More...

void	oneloop_andi_run (bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
	Main function that runs the controller and performs control allocation. More...

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

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

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

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

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

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

Variables
float	num_thrusters_oneloop = 4.0

float	oneloop_andi_filt_cutoff = 2.0

float	oneloop_andi_filt_cutoff_a = 2.0

float	oneloop_andi_filt_cutoff_v = 2.0

float	oneloop_andi_filt_cutoff_pos = 2.0

float	oneloop_andi_filt_cutoff_p = 20.0

float	oneloop_andi_filt_cutoff_q = 20.0

float	oneloop_andi_filt_cutoff_r = 20.0

float	max_r = RadOfDeg(120.0)

bool	actuator_is_servo [ANDI_NUM_ACT_TOT] = {0}

float	act_dynamics [ANDI_NUM_ACT_TOT] = = {1}

float	act_max [ANDI_NUM_ACT_TOT] = = {MAX_PPRZ}

float	act_min [ANDI_NUM_ACT_TOT] = = {0.0}

float	act_max_norm [ANDI_NUM_ACT_TOT] = = {1.0}

float	act_min_norm [ANDI_NUM_ACT_TOT] = = {0.0}

float	nu_norm_max = 1.0

static float	Wv [ANDI_OUTPUTS] = {1.0}

static float	Wv_wls [ANDI_OUTPUTS] = {1.0}

static float	Wu [ANDI_NUM_ACT_TOT] = {1.0}

static float	u_pref [ANDI_NUM_ACT_TOT] = {0.0}

float	theta_pref_max = RadOfDeg(20.0)

float	max_a_nav = 4.0

float	max_j_nav = 500.0

float	max_j_ang = 1000.0

float	max_v_nav = 5.0

float	max_as = 19.0f

float	max_v_nav_v = 1.5

float	fwd_sideslip_gain = 0.2

struct OneloopGeneral	oneloop_andi

static float	use_increment = 0.0

static float	nav_target [3]

static float	nav_target_new [3]

static float	dt_1l = 1./PERIODIC_FREQUENCY

static float	g = 9.81

float	k_as = 2.0

int16_t	temp_pitch = 0

float	andi_u [ANDI_NUM_ACT_TOT]

float	andi_du [ANDI_NUM_ACT_TOT]

static float	andi_du_n [ANDI_NUM_ACT_TOT]

float	nu [ANDI_OUTPUTS]

float	nu_n [ANDI_OUTPUTS]

static float	act_dynamics_d [ANDI_NUM_ACT_TOT]

float	actuator_state_1l [ANDI_NUM_ACT]

static float	a_thrust = 0.0

static float	g2_ff = 0.0

struct Int32Eulers	stab_att_sp_euler_1l

struct Int32Quat	stab_att_sp_quat_1l

struct FloatEulers	eulers_zxy_des

struct FloatEulers	eulers_zxy

float	psi_des_rad = 0.0

float	psi_des_deg = 0.0

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

bool	heading_manual = false

bool	yaw_stick_in_auto = false

bool	ctrl_off = false

static float	gamma_wls = 100

static float	du_min_1l [ANDI_NUM_ACT_TOT]

static float	du_max_1l [ANDI_NUM_ACT_TOT]

static float	du_pref_1l [ANDI_NUM_ACT_TOT]

static float	pitch_pref = 0

static int	number_iter = 0

static float	model_pred [ANDI_OUTPUTS]

static float	model_pred_ar [3]

static float	ang_acc [3]

static float	ang_rate [3]

static float	lin_acc [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.8 / (2.0 * M_PI)

float	f1_chirp = 0.8 / (2.0 * M_PI)

float	t_chirp = 45.0

float	A_chirp = 1.0

int8_t	chirp_axis = 0

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

struct PolePlacement	p_att_e

struct PolePlacement	p_att_rm

struct PolePlacement	p_pos_e

struct PolePlacement	p_pos_rm

struct PolePlacement	p_alt_e

struct PolePlacement	p_alt_rm

struct PolePlacement	p_head_e

struct PolePlacement	p_head_rm

struct Gains3rdOrder	k_att_e

struct Gains3rdOrder	k_att_rm

struct Gains3rdOrder	k_pos_e

struct Gains3rdOrder	k_pos_rm

struct Gains3rdOrder	k_att_e_indi

struct Gains3rdOrder	k_pos_e_indi

float *	bwls_1l [ANDI_OUTPUTS]

float	EFF_MAT_G [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]

float	ratio_u_un [ANDI_NUM_ACT_TOT]

float	ratio_vn_v [ANDI_OUTPUTS]

static Butterworth2LowPass	filt_accel_ned [3]

static Butterworth2LowPass	filt_veloc_ned [3]

static Butterworth2LowPass	rates_filt_bt [3]

static Butterworth2LowPass	model_pred_la_filt [3]

static Butterworth2LowPass	ang_rate_lowpass_filters [3]

static Butterworth2LowPass	model_pred_aa_filt [3]

static Butterworth2LowPass	model_pred_ar_filt [3]

static Butterworth2LowPass	accely_filt

static Butterworth2LowPass	airspeed_filt

Macro Definition Documentation

◆ ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD

#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0

Definition at line 252 of file oneloop_andi.c.

◆ ONELOOP_ANDI_DEBUG_MODE

#define ONELOOP_ANDI_DEBUG_MODE FALSE

Definition at line 227 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_BANK

#define ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL]

Definition at line 231 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_PHI

#define ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL]

Definition at line 232 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_THETA

#define ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH]

Definition at line 234 of file oneloop_andi.c.

◆ ONELOOP_ANDI_SCHEDULING

#define ONELOOP_ANDI_SCHEDULING FALSE

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

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

Referenced by reshape_wind().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ autopilot_in_flight_end_detection()

bool autopilot_in_flight_end_detection ( bool motors_on UNUSED )

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

Definition at line 1934 of file oneloop_andi.c.

◆ bound_v_from_a()

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

Calculate velocity limit based on acceleration limit.

Definition at line 658 of file oneloop_andi.c.

References float_vect_norm().

Referenced by rm_3rd_pos().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calc_model()

void calc_model ( void )

Function that calculates the model prediction for the complementary filter.

Definition at line 1722 of file oneloop_andi.c.

References actuator_state_1l, ANDI_NUM_ACT, ANDI_OUTPUTS, EFF_MAT_RW, RW_Model::ele_pref, eulers_zxy, g, wing_model::L, RW_Model::m, model_pred, RW_Model::P, P, FloatEulers::phi, FloatEulers::psi, RW, RW_Model::T, mesonh.mesonh_atmosphere::T, FloatEulers::theta, and RW_Model::wing.

Referenced by oneloop_andi_propagate_filters().

Here is the caller graph for this function:

◆ calc_normalization()

void calc_normalization ( void )

Calculate Normalization of actuators and discrete actuator dynamics

Definition at line 1679 of file oneloop_andi.c.

References act_dynamics, act_dynamics_d, act_max, act_max_norm, act_min, act_min_norm, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, dt_1l, max_j_ang, max_j_nav, MAX_PPRZ, nu_norm_max, positive_non_zero(), ratio_u_un, and ratio_vn_v.

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

Here is the call graph for this function:

Here is the caller graph for this function:

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

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

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ chirp_pos()

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

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

Parameters

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

Definition at line 1846 of file oneloop_andi.c.

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

Referenced by chirp_call().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ chirp_pos_a_ref()

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

static

Function to calculate the acceleration reference during the chirp.

Definition at line 1825 of file oneloop_andi.c.

References A.

Referenced by chirp_pos().

Here is the caller graph for this function:

◆ chirp_pos_j_ref()

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

static

Function to calculate the jerk reference during the chirp.

Definition at line 1830 of file oneloop_andi.c.

References A.

Referenced by chirp_pos().

Here is the caller graph for this function:

◆ chirp_pos_p_ref()

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

static

Function to calculate the position reference during the chirp.

Definition at line 1815 of file oneloop_andi.c.

References A.

Referenced by chirp_pos().

Here is the caller graph for this function:

◆ chirp_pos_v_ref()

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

static

Function to calculate the velocity reference during the chirp.

Definition at line 1820 of file oneloop_andi.c.

References A.

Referenced by chirp_pos().

Here is the caller graph for this function:

◆ debug_vect()

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

static

Definition at line 486 of file oneloop_andi.c.

References dev.

Referenced by send_oneloop_debug().

Here is the caller graph for this function:

◆ ec_3rd()

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

static

Error Controller Definition for 3rd order system.

Parameters

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

Definition at line 866 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

Here is the caller graph for this function:

◆ ec_3rd_att()

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

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

Parameters

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

Definition at line 898 of file oneloop_andi.c.

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

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ec_3rd_pos()

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

Definition at line 870 of file oneloop_andi.c.

References err_sum_nd(), and vect_bound_nd().

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

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

References b.

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

Here is the caller graph for this function:

◆ err_sum_nd()

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

Calculate Scaled Error between two 3D arrays.

Definition at line 610 of file oneloop_andi.c.

References b.

Referenced by ec_3rd_pos().

Here is the caller graph for this function:

◆ float_euler_dot_of_rates_vec()

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

Attitude Euler to Rates Conversion Function.

Definition at line 586 of file oneloop_andi.c.

Referenced by rm_3rd_attitude().

Here is the caller graph for this function:

◆ float_rates_of_euler_dot_vec()

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

Attitude Rates to Euler Conversion Function.

Definition at line 574 of file oneloop_andi.c.

Referenced by rm_3rd_attitude().

Here is the caller graph for this function:

◆ G1G2_oneloop()

void G1G2_oneloop ( int ctrl_type )

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

Definition at line 1624 of file oneloop_andi.c.

References act_dynamics, airspeed_filt, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, CTRL_ANDI, CTRL_INDI, ctrl_off, EFF_MAT_G, EFF_MAT_RW, ELE_MIN_AS, RotWingStateSettings::hover_motors_active, SecondOrderLowPass::o, ratio_u_un, ratio_vn_v, and rotwing_state_settings.

Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_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 1606 of file oneloop_andi.c.

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

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_controller()

void init_controller ( void )

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

Definition at line 984 of file oneloop_andi.c.

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

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_filter()

void init_filter ( void )

Initialize the filters.

Definition at line 1089 of file oneloop_andi.c.

References accely_filt, airspeed_filt, ang_rate_lowpass_filters, filt_accel_ned, filt_veloc_ned, init_butterworth_2_low_pass(), model_pred_aa_filt, model_pred_ar_filt, model_pred_la_filt, oneloop_andi_filt_cutoff, oneloop_andi_filt_cutoff_a, oneloop_andi_filt_cutoff_p, oneloop_andi_filt_cutoff_q, oneloop_andi_filt_cutoff_r, oneloop_andi_filt_cutoff_v, p, rates_filt_bt, and stateGetBodyRates_f().

Referenced by oneloop_andi_enter(), and oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_poles()

void init_poles ( void )

Initialize Position of Poles.

Definition at line 937 of file oneloop_andi.c.

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

Referenced by oneloop_andi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ integrate_nd()

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

Integrate in time 3D array.

Definition at line 620 of file oneloop_andi.c.

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

Here is the caller graph for this function:

◆ k_e_1_2_f_v2()

static float k_e_1_2_f_v2	(	float	omega,
		float	zeta
	)

static

Error Controller Gain Design.

Definition at line 518 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_e_1_3_f_v2()

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

static

Definition at line 530 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_e_2_2_f_v2()

static float k_e_2_2_f_v2	(	float	omega,
		float	zeta
	)

static

Definition at line 524 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_e_2_3_f_v2()

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

static

Definition at line 536 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_e_3_3_f_v2()

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

static

Definition at line 543 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_rm_1_3_f()

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

static

Reference Model Gain Design.

Definition at line 552 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_rm_2_3_f()

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

static

Definition at line 559 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ k_rm_3_3_f()

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

static

Definition at line 566 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ normalize_nu()

void normalize_nu ( void )

Function to normalize the pseudo control vector.

Definition at line 1714 of file oneloop_andi.c.

References ANDI_OUTPUTS, nu, nu_n, and ratio_vn_v.

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

References ANDI_OUTPUTS, ang_acc, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, calc_normalization(), OneloopGeneral::ctrl_type, EFF_MAT_G, ele_min, eulers_zxy, eulers_zxy_des, float_vect_zero(), G1G2_oneloop(), OneloopGeneral::half_loop, init_controller(), init_filter(), lin_acc, model_pred, model_pred_ar, nav_target, nav_target_new, oneloop_andi, FloatEulers::phi, FloatEulers::psi, psi_des_deg, psi_des_rad, OneloopGeneral::sta_ref, and FloatEulers::theta.

Referenced by chirp_call(), guidance_h_run_enter(), oneloop_from_nav(), and stabilization_attitude_enter().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_andi_init()

void oneloop_andi_init ( void )

Init function of Oneloop ANDI controller

Definition at line 1157 of file oneloop_andi.c.

References act_dynamics, actuator_state_1l, andi_du, andi_du_n, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, ang_acc, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, calc_normalization(), CTRL_ANDI, OneloopGeneral::ctrl_type, DefaultPeriodic, EFF_MAT_G, eulers_zxy_des, float_vect_zero(), G1G2_oneloop(), OneloopGeneral::half_loop, init_controller(), init_filter(), init_poles(), lin_acc, model_pred, model_pred_ar, 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(), 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 1123 of file oneloop_andi.c.

References ACCEL_FLOAT_OF_BFP, accely_filt, airspeed_filt, ang_acc, ang_rate, ang_rate_lowpass_filters, calc_model(), filt_accel_ned, filt_veloc_ned, lin_acc, model_pred, model_pred_aa_filt, model_pred_ar, model_pred_ar_filt, model_pred_la_filt, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, rates_filt_bt, stateGetAccelBody_i(), stateGetAccelNed_f(), stateGetAirspeed_f(), stateGetBodyRates_f(), stateGetSpeedNed_f(), update_butterworth_2_low_pass(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_andi_RM()

void oneloop_andi_RM	(	bool	half_loop,
		struct FloatVect3	PSA_des,
		int	rm_order_h,
		int	rm_order_v,
		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 1251 of file oneloop_andi.c.

References A_chirp, a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, actuator_state_1l, ANDI_NUM_ACT, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, chirp_axis, chirp_call(), chirp_first_call, chirp_on, dt_1l, eulers_zxy, eulers_zxy_des, f0_chirp, f1_chirp, float_vect_copy(), float_vect_norm(), float_vect_zero(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, heading_manual, OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_rm, k_pos_rm, max_a_nav, max_j_ang, max_j_nav, MAX_PPRZ, max_r, max_v_nav, max_v_nav_v, nav_target, nav_target_new, oneloop_andi, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, oneloop_andi_sideslip(), p_ref_0, FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, psi_vec, radio_control_get(), RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, ratio_u_un, ratio_vn_v, reshape_wind(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), rm_3rd_pos(), OneloopGeneral::sta_ref, t_0_chirp, t_chirp, FloatEulers::theta, time_elapsed_chirp, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, Wv, Wv_wls, FloatVect3::x, FloatVect3::y, yaw_stick_in_auto, and FloatVect3::z.

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_andi_run()

void oneloop_andi_run	(	bool	in_flight,
		bool	half_loop,
		struct FloatVect3	PSA_des,
		int	rm_order_h,
		int	rm_order_v
	)

Main function that runs the controller and performs control allocation.

Parameters

half_loop	In half-loop mode the controller is used for stabilization only
in_flight	The drone is in flight
PSA_des	Desired position/speed/acceleration
rm_order_h	Order of the reference model for horizontal guidance
rm_order_v	Order of the reference model for vertical guidance

Definition at line 1388 of file oneloop_andi.c.

References a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, act_dynamics, act_max, act_min, actuator_state_1l, andi_du, andi_du_n, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, ang_acc, ang_rate, AP_MODE_ATTITUDE_DIRECT, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, autopilot, bwls_1l, calc_normalization(), chirp_on, Stabilization::cmd, commands, CTRL_ANDI, CTRL_INDI, ctrl_off, OneloopGeneral::ctrl_type, RW_skew::deg, du_max_1l, du_min_1l, du_pref_1l, ec_3rd(), ec_3rd_att(), ec_3rd_pos(), EFF_MAT_G, RW_Model::ele_pref, eulers_zxy, eulers_zxy_des, filt_veloc_ned, float_eulers_of_quat_zxy(), float_vect_copy(), float_vect_sum(), G1G2_oneloop(), g2_ff, G2_RW, gamma_wls, get_act_state_oneloop(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, heading_manual, RotWingStateSettings::hover_motors_active, init_controller(), OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_e, k_att_e_indi, k_pos_e, k_pos_e_indi, lin_acc, max_a_nav, max_j_ang, max_j_nav, MAX_PPRZ, max_v_nav, pprz_autopilot::mode, normalize_nu(), nu, nu_n, num_thrusters_oneloop, number_iter, SecondOrderLowPass::o, oneloop_andi, ONELOOP_ANDI_DEBUG_MODE, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, oneloop_andi_propagate_filters(), oneloop_andi_RM(), FloatEulers::phi, pitch_pref, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, RADIO_AUX4, RADIO_AUX5, radio_control, ratio_u_un, rotwing_state_settings, RW, RW_Model::skew, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, stabilization, RotWingStateSettings::stall_protection, stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), FloatEulers::theta, theta_pref_max, pprz_autopilot::throttle, u_pref, use_increment, RadioControl::values, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, wls_alloc(), Wu, Wv_wls, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.

Referenced by oneloop_from_nav(), and stabilization_attitude_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ oneloop_andi_sideslip()

float oneloop_andi_sideslip ( void )

Function to calculate corrections for sideslip.

Definition at line 1791 of file oneloop_andi.c.

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

Referenced by oneloop_andi_RM().

Here is the caller graph for this function:

◆ oneloop_from_nav()

void oneloop_from_nav ( bool in_flight )

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

Definition at line 1753 of file oneloop_andi.c.

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

Here is the call graph for this function:

◆ positive_non_zero()

static float positive_non_zero ( float input )

static

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

Definition at line 508 of file oneloop_andi.c.

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

Here is the caller graph for this function:

◆ reshape_wind()

void reshape_wind ( void )

Definition at line 1939 of file oneloop_andi.c.

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

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_1st_pos()

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

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

Parameters

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

Definition at line 822 of file oneloop_andi.c.

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

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_2nd()

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

Reference Model Definition for 2nd order system.

Parameters

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

Definition at line 843 of file oneloop_andi.c.

◆ rm_2nd_pos()

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

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

Parameters

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

Definition at line 800 of file oneloop_andi.c.

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

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_3rd()

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

Reference Model Definition for 3rd order system.

Parameters

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

Definition at line 720 of file oneloop_andi.c.

◆ rm_3rd_attitude()

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

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

Parameters

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

Definition at line 679 of file oneloop_andi.c.

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

Referenced by oneloop_andi_RM().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_3rd_head()

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

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

Parameters

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

Definition at line 742 of file oneloop_andi.c.

◆ rm_3rd_pos()

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

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

Parameters

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