Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
oneloop_andi.c File Reference
+ Include dependency graph for oneloop_andi.c:

Go to the source code of this file.

Macros

#define ONELOOP_ANDI_SCHEDULING   FALSE
 
#define ONELOOP_ANDI_DEBUG_MODE   FALSE;
 
#define ONELOOP_ANDI_AC_HAS_PUSHER   FALSE;
 
#define ONELOOP_ANDI_PUSHER_IDX   0
 
#define ONELOOP_ANDI_PHI_IDX   ANDI_NUM_ACT
 
#define ONELOOP_ANDI_MAX_BANK   act_max[ONELOOP_ANDI_PHI_IDX]
 
#define ONELOOP_ANDI_MAX_PHI   act_max[ONELOOP_ANDI_PHI_IDX]
 
#define ONELOOP_ANDI_THETA_IDX   ANDI_NUM_ACT+1
 
#define ONELOOP_ANDI_MAX_THETA   act_max[ONELOOP_ANDI_THETA_IDX]
 

Functions

void init_poles (void)
 Initialize Position of Poles. More...
 
void calc_normalization (void)
 Calculate Normalization of actuators and discrete actuator dynamics
More...
 
void sum_g1g2_1l (int ctrl_type)
 Function that sums g1 and g2 to obtain the g1_g2 matrix. More...
 
void get_act_state_oneloop (void)
 Function to reconstruct actuator state using first order dynamics. More...
 
void oneloop_andi_propagate_filters (void)
 Propagate the filters. More...
 
void init_filter (void)
 Initialize the filters. More...
 
void init_controller (void)
 Initialize Controller Gains FIXME: Calculate the gains dynamically for transition. More...
 
void float_rates_of_euler_dot_vec (float r[3], float e[3], float edot[3])
 Attitude Rates to Euler Conversion Function. More...
 
void float_euler_dot_of_rates_vec (float r[3], float e[3], float edot[3])
 Attitude Euler to Rates Conversion Function. More...
 
void err_nd (float err[], float a[], float b[], float k[], int n)
 Calculate Scaled Error between two 3D arrays. More...
 
void integrate_nd (float dt, float a[], float a_dot[], int n)
 Integrate in time 3D array. More...
 
void vect_bound_nd (float vect[], float bound, int n)
 Scale a 3D array to within a 3D bound. More...
 
void rm_2nd (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float x_des, float k1_rm, float k2_rm)
 Reference Model Definition for 2nd order system. More...
 
void rm_3rd (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float *x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm)
 Reference Model Definition for 3rd order system. More...
 
void rm_3rd_head (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float *x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm)
 Reference Model Definition for 3rd order system specific to the heading angle. More...
 
void rm_3rd_attitude (float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3])
 Reference Model Definition for 3rd order system with attitude conversion functions. More...
 
void rm_3rd_pos (float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_des[], float k1_rm[], float k2_rm[], float k3_rm[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n)
 Reference Model Definition for 3rd order system specific to positioning with bounds. More...
 
void rm_2nd_pos (float dt, float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_d_des[], float k2_rm[], float k3_rm[], float x_2d_bound, float x_3d_bound, int n)
 Reference Model Definition for 3rd order system specific to positioning with bounds. More...
 
void rm_1st_pos (float dt, float x_2d_ref[], float x_3d_ref[], float x_2d_des[], float k3_rm[], float x_3d_bound, int n)
 Reference Model Definition for 3rd order system specific to positioning with bounds. More...
 
void ec_3rd_att (float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3])
 Error Controller Definition for 3rd order system specific to attitude. More...
 
void calc_model (int ctrl_type)
 Function that calculates the model prediction for the complementary filter. More...
 
float oneloop_andi_sideslip (void)
 Function to calculate corrections for sideslip. More...
 
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[])
 Function to perform position and attitude chirps. 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_g_oneloop_andi (struct transport_tx *trans, struct link_device *dev)
 
static void send_oneloop_andi (struct transport_tx *trans, struct link_device *dev)
 
static void send_guidance_oneloop_andi (struct transport_tx *trans, struct link_device *dev)
 
static 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)
 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...
 

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}
 
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_v_nav = 5.0
 
float fwd_sideslip_gain = 1.0
 
struct OneloopGeneral oneloop_andi
 
static float use_increment = 0.0
 
static float nav_target [3]
 
static float dt_1l = 1./PERIODIC_FREQUENCY
 
static float g = 9.81
 
float andi_u [ANDI_NUM_ACT_TOT]
 
float andi_du [ANDI_NUM_ACT_TOT]
 
static float andi_du_n [ANDI_NUM_ACT_TOT]
 
float nu [ANDI_OUTPUTS]
 
static float act_dynamics_d [ANDI_NUM_ACT_TOT]
 
float actuator_state_1l [ANDI_NUM_ACT]
 
static float a_thrust = 0.0
 
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 = true
 
bool yaw_stick_in_auto = false
 
static float gamma_wls = 1000.0
 
static float du_min_1l [ANDI_NUM_ACT_TOT]
 
static float du_max_1l [ANDI_NUM_ACT_TOT]
 
static float du_pref_1l [ANDI_NUM_ACT_TOT]
 
static float pitch_pref = 0
 
static int number_iter = 0
 
static float model_pred [ANDI_OUTPUTS]
 
static float ang_acc [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 g2_1l [ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2
 
float g1_1l [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] = {ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_THRUST, ONELOOP_ANDI_G1_ROLL, ONELOOP_ANDI_G1_PITCH, ONELOOP_ANDI_G1_YAW}
 
float g1g2_1l [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]
 
float * bwls_1l [ANDI_OUTPUTS]
 
float ratio_u_un [ANDI_NUM_ACT_TOT]
 
float ratio_vn_v [ANDI_NUM_ACT_TOT]
 
static 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 att_dot_meas_lowpass_filters [3]
 
static Butterworth2LowPass model_pred_aa_filt [3]
 
static Butterworth2LowPass accely_filt
 
static Butterworth2LowPass airspeed_filt
 

Macro Definition Documentation

◆ ONELOOP_ANDI_AC_HAS_PUSHER

#define ONELOOP_ANDI_AC_HAS_PUSHER   FALSE;

Definition at line 223 of file oneloop_andi.c.

◆ ONELOOP_ANDI_DEBUG_MODE

#define ONELOOP_ANDI_DEBUG_MODE   FALSE;

Definition at line 218 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_BANK

#define ONELOOP_ANDI_MAX_BANK   act_max[ONELOOP_ANDI_PHI_IDX]

Definition at line 240 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_PHI

#define ONELOOP_ANDI_MAX_PHI   act_max[ONELOOP_ANDI_PHI_IDX]

Definition at line 241 of file oneloop_andi.c.

◆ ONELOOP_ANDI_MAX_THETA

#define ONELOOP_ANDI_MAX_THETA   act_max[ONELOOP_ANDI_THETA_IDX]

Definition at line 247 of file oneloop_andi.c.

◆ ONELOOP_ANDI_PHI_IDX

#define ONELOOP_ANDI_PHI_IDX   ANDI_NUM_ACT

Definition at line 237 of file oneloop_andi.c.

◆ ONELOOP_ANDI_PUSHER_IDX

#define ONELOOP_ANDI_PUSHER_IDX   0

Definition at line 227 of file oneloop_andi.c.

◆ ONELOOP_ANDI_SCHEDULING

#define ONELOOP_ANDI_SCHEDULING   FALSE

Definition at line 105 of file oneloop_andi.c.

◆ ONELOOP_ANDI_THETA_IDX

#define ONELOOP_ANDI_THETA_IDX   ANDI_NUM_ACT+1

Definition at line 244 of file oneloop_andi.c.

Function Documentation

◆ calc_model()

void calc_model ( int  ctrl_type)

Function that calculates the model prediction for the complementary filter.

Definition at line 1574 of file oneloop_andi.c.

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

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

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

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

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

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

Function to perform position and attitude chirps.

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 1700 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 1679 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 1684 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 1669 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 1674 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 478 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
dtDelta time [s]
x_refReference signal 1st order
x_d_refReference signal 2nd order
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_desDesired 1st order signal
xCurrent 1st order signal
x_dCurrent 2nd order signal
x_2dCurrent 3rd order signal
k1_eError Controller Gain 1st order signal
k2_eError Controller Gain 2nd order signal
k3_eError Controller Gain 3rd order signal

Definition at line 814 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

+ Here is the caller graph for this function:

◆ ec_3rd_att()

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

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

Parameters
dtDelta time [s]
x_refReference signal 1st order
x_d_refReference signal 2nd order
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_desDesired 1st order signal
xCurrent 1st order signal
x_dCurrent 2nd order signal
x_2dCurrent 3rd order signal
k1_eError Controller Gain 1st order signal
k2_eError Controller Gain 2nd order signal
k3_eError Controller Gain 3rd order signal

Definition at line 834 of file oneloop_andi.c.

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

Referenced by oneloop_andi_run().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ err_nd()

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

Calculate Scaled Error between two 3D arrays.

Definition at line 588 of file oneloop_andi.c.

References b.

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

+ Here is the caller graph for this function:

◆ float_euler_dot_of_rates_vec()

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

Attitude Euler to Rates Conversion Function.

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

Referenced by rm_3rd_attitude().

+ Here is the caller graph for this function:

◆ get_act_state_oneloop()

void get_act_state_oneloop ( void  )

Function to reconstruct actuator state using first order dynamics.

Definition at line 1441 of file oneloop_andi.c.

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

Referenced by oneloop_andi_run().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ init_controller()

◆ init_filter()

◆ init_poles()

void init_poles ( void  )

Initialize Position of Poles.

Definition at line 872 of file oneloop_andi.c.

References act_dynamics, PolePlacement::omega_n, ONELOOP_ANDI_PHI_IDX, ONELOOP_ANDI_THETA_IDX, 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 597 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 505 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 517 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 511 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 523 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 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_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 539 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ k_rm_2_3_f()

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

Definition at line 546 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ k_rm_3_3_f()

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

Definition at line 553 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ oneloop_andi_enter()

void oneloop_andi_enter ( bool  half_loop_sp,
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 1126 of file oneloop_andi.c.

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

Referenced by 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()

◆ oneloop_andi_propagate_filters()

◆ oneloop_andi_RM()

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

Function to generate the reference signals for the oneloop controller.

Parameters
half_loopIn half-loop mode the controller is used for stabilization only
PSA_desDesired position/speed/acceleration
rm_order_hOrder of the reference model for horizontal guidance
rm_order_vOrder of the reference model for vertical guidance

Definition at line 1158 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_nav, MAX_PPRZ, max_r, max_v_nav, nav_target, 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, 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_loopIn half-loop mode the controller is used for stabilization only
in_flightThe drone is in flight
PSA_desDesired position/speed/acceleration
rm_order_hOrder of the reference model for horizontal guidance
rm_order_vOrder of the reference model for vertical guidance

Definition at line 1279 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_G_SCALING, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, ang_acc, AP_MODE_ATTITUDE_DIRECT, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, autopilot, bwls_1l, calc_normalization(), chirp_on, Stabilization::cmd, CTRL_ANDI, CTRL_INDI, OneloopGeneral::ctrl_type, du_max_1l, du_min_1l, du_pref_1l, ec_3rd(), ec_3rd_att(), eulers_zxy, eulers_zxy_des, filt_veloc_ned, float_eulers_of_quat_zxy(), float_vect_copy(), float_vect_sum(), g1g2_1l, g2_1l, g2_ff, gamma_wls, get_act_state_oneloop(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, heading_manual, 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_PPRZ, pprz_autopilot::mode, nu, num_thrusters_oneloop, number_iter, SecondOrderLowPass::o, oneloop_andi, ONELOOP_ANDI_AC_HAS_PUSHER, ONELOOP_ANDI_DEBUG_MODE, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, ONELOOP_ANDI_PHI_IDX, oneloop_andi_propagate_filters(), ONELOOP_ANDI_PUSHER_IDX, oneloop_andi_RM(), ONELOOP_ANDI_THETA_IDX, FloatEulers::phi, pitch_pref, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, RADIO_AUX4, RADIO_AUX5, radio_control, rates_filt_bt, ratio_u_un, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, stabilization, stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), sum_g1g2_1l(), 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 1645 of file oneloop_andi.c.

References accely_filt, airspeed_filt, andi_u, eulers_zxy, fwd_sideslip_gain, g, SecondOrderLowPass::o, ONELOOP_ANDI_MAX_BANK, ONELOOP_ANDI_PHI_IDX, ONELOOP_ANDI_THETA_IDX, and FloatEulers::phi.

Referenced by oneloop_andi_RM().

+ Here is the caller graph for this function:

◆ oneloop_from_nav()

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

Referenced by 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:

◆ 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
dtDelta time [s]
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_2d_desDesired 3rd order signal
k3_rmReference Model Gain 3rd order signal
x_3d_boundBound for the 4th order reference signal
nNumber of dimensions

Definition at line 770 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
dtDelta time [s]
x_refReference signal 1st order
x_d_refReference signal 2nd order
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_desDesired 1st order signal
k1_rmReference Model Gain 1st order signal
k2_rmReference Model Gain 2nd order signal
k3_rmReference Model Gain 3rd order signal

Definition at line 791 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
dtDelta time [s]
x_d_refReference signal 2nd order
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_d_desDesired 2nd order signal
k2_rmReference Model Gain 2nd order signal
k3_rmReference Model Gain 3rd order signal
x_2d_boundBound for the 3rd order reference signal
x_3d_boundBound for the 4th order reference signal
nNumber of dimensions

Definition at line 748 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
dtDelta time [s]
x_refReference signal 1st order
x_d_refReference signal 2nd order
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_desDesired 1st order signal
k1_rmReference Model Gain 1st order signal
k2_rmReference Model Gain 2nd order signal
k3_rmReference Model Gain 3rd order signal

Definition at line 669 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] 
)

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

Parameters
dtDelta time [s]
x_refReference signal 1st order
x_d_refReference signal 2nd order
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_desDesired 1st order signal
ow_psiOverwrite psi (for navigation functions) [bool]
psi_overwriteOverwrite psi (for navigation functions) [values]
k1_rmReference Model Gain 1st order signal
k2_rmReference Model Gain 2nd order signal
k3_rmReference Model Gain 3rd order signal

Definition at line 632 of file oneloop_andi.c.

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

Referenced by oneloop_andi_RM().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ rm_3rd_head()

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

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

Parameters
dtDelta time [s]
x_refReference signal 1st order
x_d_refReference signal 2nd order
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_desDesired 1st order signal
k1_rmReference Model Gain 1st order signal
k2_rmReference Model Gain 2nd order signal
k3_rmReference Model Gain 3rd order signal

Definition at line 691 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
dtDelta time [s]
x_refReference signal 1st order
x_d_refReference signal 2nd order
x_2d_refReference signal 3rd order
x_3d_refReference signal 4th order
x_desDesired 1st order signal
k1_rmReference Model Gain 1st order signal
k2_rmReference Model Gain 2nd order signal
k3_rmReference Model Gain 3rd order signal
x_d_boundBound for the 2nd order reference signal
x_2d_boundBound for the 3rd order reference signal
x_3d_boundBound for the 4th order reference signal
nNumber of dimensions

Definition at line 719 of file oneloop_andi.c.

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

Referenced by oneloop_andi_RM().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ send_eff_mat_g_oneloop_andi()

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

Definition at line 414 of file oneloop_andi.c.

References ANDI_NUM_ACT_TOT, dev, and g1g2_1l.

Referenced by oneloop_andi_init().

+ Here is the caller graph for this function:

◆ send_guidance_oneloop_andi()

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

◆ send_oneloop_andi()

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

◆ send_oneloop_debug()

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

Definition at line 485 of file oneloop_andi.c.

References andi_u, debug_vect(), dev, ONELOOP_ANDI_THETA_IDX, and pitch_pref.

Referenced by oneloop_andi_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ sum_g1g2_1l()

void sum_g1g2_1l ( int  ctrl_type)

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

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

Definition at line 1459 of file oneloop_andi.c.

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

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

+ Here is the caller graph for this function:

◆ vect_bound_nd()

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

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

Definition at line 606 of file oneloop_andi.c.

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

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

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ w_approx()

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

Third Order to First Order Dynamics Approximation.

Parameters
p1Pole 1
p2Pole 2
p3Pole 3
rm_kReference Model Gain

Definition at line 858 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller(), and init_poles().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

Variable Documentation

◆ A_chirp

float A_chirp = 1.0

Definition at line 367 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ a_thrust

float a_thrust = 0.0
static

Definition at line 331 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ accely_filt

Butterworth2LowPass accely_filt
static

◆ act_dynamics

◆ act_dynamics_d

float act_dynamics_d[ANDI_NUM_ACT_TOT]
static

Definition at line 329 of file oneloop_andi.c.

Referenced by calc_normalization(), and get_act_state_oneloop().

◆ act_max

float act_max[ANDI_NUM_ACT_TOT] = = {MAX_PPRZ}

Definition at line 175 of file oneloop_andi.c.

Referenced by calc_normalization(), and oneloop_andi_run().

◆ act_max_norm

float act_max_norm[ANDI_NUM_ACT_TOT] = = {1.0}

Definition at line 187 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ act_min

float act_min[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 181 of file oneloop_andi.c.

Referenced by calc_normalization(), and oneloop_andi_run().

◆ act_min_norm

float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0}

Definition at line 193 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ actuator_is_servo

bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0}

Definition at line 162 of file oneloop_andi.c.

◆ actuator_state_1l

◆ airspeed_filt

Butterworth2LowPass airspeed_filt
static

◆ andi_du

float andi_du[ANDI_NUM_ACT_TOT]

Definition at line 326 of file oneloop_andi.c.

Referenced by oneloop_andi_init(), and oneloop_andi_run().

◆ andi_du_n

float andi_du_n[ANDI_NUM_ACT_TOT]
static

Definition at line 327 of file oneloop_andi.c.

Referenced by oneloop_andi_init(), and oneloop_andi_run().

◆ andi_u

◆ ang_acc

float ang_acc[3]
static

◆ att_dot_meas_lowpass_filters

Butterworth2LowPass att_dot_meas_lowpass_filters[3]
static

Definition at line 405 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ bwls_1l

float* bwls_1l[ANDI_OUTPUTS]

Definition at line 396 of file oneloop_andi.c.

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

◆ chirp_axis

int8_t chirp_axis = 0

Definition at line 368 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ chirp_first_call

bool chirp_first_call = true

Definition at line 361 of file oneloop_andi.c.

Referenced by chirp_call(), and oneloop_andi_RM().

◆ chirp_on

bool chirp_on = false

Definition at line 360 of file oneloop_andi.c.

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

◆ dt_1l

float dt_1l = 1./PERIODIC_FREQUENCY
static

Definition at line 321 of file oneloop_andi.c.

Referenced by calc_normalization(), and oneloop_andi_RM().

◆ du_max_1l

float du_max_1l[ANDI_NUM_ACT_TOT]
static

Definition at line 349 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ du_min_1l

float du_min_1l[ANDI_NUM_ACT_TOT]
static

Definition at line 348 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ du_pref_1l

float du_pref_1l[ANDI_NUM_ACT_TOT]
static

Definition at line 350 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ eulers_zxy

◆ eulers_zxy_des

struct FloatEulers eulers_zxy_des

◆ f0_chirp

float f0_chirp = 0.8 / (2.0 * M_PI)

Definition at line 364 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ f1_chirp

float f1_chirp = 0.8 / (2.0 * M_PI)

Definition at line 365 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ filt_accel_ned

Butterworth2LowPass filt_accel_ned[3]
static

Definition at line 401 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ filt_veloc_ned

Butterworth2LowPass filt_veloc_ned[3]
static

Definition at line 402 of file oneloop_andi.c.

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

◆ fwd_sideslip_gain

float fwd_sideslip_gain = 1.0

Definition at line 284 of file oneloop_andi.c.

Referenced by oneloop_andi_sideslip().

◆ g

float g = 9.81
static

Definition at line 322 of file oneloop_andi.c.

Referenced by calc_model(), oneloop_andi_sideslip(), and sum_g1g2_1l().

◆ g1_1l

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

Definition at line 394 of file oneloop_andi.c.

Referenced by calc_model(), and sum_g1g2_1l().

◆ g1g2_1l

◆ g2_1l

float g2_1l[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2

Definition at line 393 of file oneloop_andi.c.

Referenced by oneloop_andi_run(), and sum_g1g2_1l().

◆ g2_ff

float g2_ff = 0.0
static

Definition at line 332 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ gamma_wls

float gamma_wls = 1000.0
static

Definition at line 347 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ heading_manual

bool heading_manual = true

Definition at line 343 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ k_att_e

struct Gains3rdOrder k_att_e

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_att_e_indi

struct Gains3rdOrder k_att_e_indi

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_att_rm

struct Gains3rdOrder k_att_rm

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_RM().

◆ k_pos_e

struct Gains3rdOrder k_pos_e

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_pos_e_indi

struct Gains3rdOrder k_pos_e_indi

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_pos_rm

struct Gains3rdOrder k_pos_rm

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_RM().

◆ lin_acc

float lin_acc[3]
static

◆ max_a_nav

float max_a_nav = 4.0

Definition at line 268 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ max_j_nav

float max_j_nav = 500.0

Definition at line 274 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ max_r

float max_r = RadOfDeg(120.0)

Definition at line 154 of file oneloop_andi.c.

Referenced by attitude_ref_quat_int_set_max_r(), and oneloop_andi_RM().

◆ max_v_nav

float max_v_nav = 5.0

Definition at line 280 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_RM().

◆ model_pred

float model_pred[ANDI_OUTPUTS]
static

◆ model_pred_aa_filt

Butterworth2LowPass model_pred_aa_filt[3]
static

Definition at line 406 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ model_pred_la_filt

Butterworth2LowPass model_pred_la_filt[3]
static

Definition at line 404 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ nav_target

float nav_target[3]
static

Definition at line 320 of file oneloop_andi.c.

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

◆ nu

float nu[ANDI_OUTPUTS]

Definition at line 328 of file oneloop_andi.c.

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

◆ num_thrusters_oneloop

float num_thrusters_oneloop = 4.0

Definition at line 99 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ number_iter

int number_iter = 0
static

Definition at line 352 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ oneloop_andi

◆ oneloop_andi_filt_cutoff

float oneloop_andi_filt_cutoff = 2.0

Definition at line 111 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_andi_filt_cutoff_a

float oneloop_andi_filt_cutoff_a = 2.0

Definition at line 117 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_andi_filt_cutoff_p

float oneloop_andi_filt_cutoff_p = 20.0

Definition at line 136 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_andi_filt_cutoff_pos

float oneloop_andi_filt_cutoff_pos = 2.0

Definition at line 129 of file oneloop_andi.c.

◆ oneloop_andi_filt_cutoff_q

float oneloop_andi_filt_cutoff_q = 20.0

Definition at line 143 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_andi_filt_cutoff_r

float oneloop_andi_filt_cutoff_r = 20.0

Definition at line 150 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_andi_filt_cutoff_v

float oneloop_andi_filt_cutoff_v = 2.0

Definition at line 123 of file oneloop_andi.c.

Referenced by init_filter().

◆ p_alt_e

struct PolePlacement p_alt_e

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_alt_rm

struct PolePlacement p_alt_rm

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_att_e

struct PolePlacement p_att_e

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_att_rm

struct PolePlacement p_att_rm

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_head_e

struct PolePlacement p_head_e

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_head_rm

struct PolePlacement p_head_rm

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_pos_e

struct PolePlacement p_pos_e

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_pos_rm

struct PolePlacement p_pos_rm

Definition at line 369 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_ref_0

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

Definition at line 369 of file oneloop_andi.c.

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

◆ pitch_pref

float pitch_pref = 0
static

Definition at line 351 of file oneloop_andi.c.

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

◆ psi_des_deg

float psi_des_deg = 0.0

Definition at line 341 of file oneloop_andi.c.

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

◆ psi_des_rad

float psi_des_rad = 0.0

Definition at line 340 of file oneloop_andi.c.

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

◆ psi_vec

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

Definition at line 342 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ rates_filt_bt

Butterworth2LowPass rates_filt_bt[3]
static

Definition at line 403 of file oneloop_andi.c.

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

◆ ratio_u_un

float ratio_u_un[ANDI_NUM_ACT_TOT]

◆ ratio_vn_v

float ratio_vn_v[ANDI_NUM_ACT_TOT]

Definition at line 398 of file oneloop_andi.c.

Referenced by calc_model(), calc_normalization(), oneloop_andi_RM(), and sum_g1g2_1l().

◆ stab_att_sp_euler_1l

struct Int32Eulers stab_att_sp_euler_1l

Definition at line 332 of file oneloop_andi.c.

◆ stab_att_sp_quat_1l

struct Int32Quat stab_att_sp_quat_1l

Definition at line 332 of file oneloop_andi.c.

◆ t_0_chirp

float t_0_chirp = 0.0

Definition at line 363 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ t_chirp

float t_chirp = 45.0

Definition at line 366 of file oneloop_andi.c.

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

◆ theta_pref_max

float theta_pref_max = RadOfDeg(20.0)

Definition at line 250 of file oneloop_andi.c.

Referenced by chirp_pos(), and oneloop_andi_run().

◆ time_elapsed_chirp

float time_elapsed_chirp = 0.0

Definition at line 362 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ u_pref

float u_pref[ANDI_NUM_ACT_TOT] = {0.0}
static

Definition at line 213 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ use_increment

float use_increment = 0.0
static

Definition at line 319 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ Wu

float Wu[ANDI_NUM_ACT_TOT] = {1.0}
static

Definition at line 207 of file oneloop_andi.c.

Referenced by oneloop_andi_run(), and wls_alloc().

◆ Wv

float Wv[ANDI_OUTPUTS] = {1.0}
static

Definition at line 200 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and wls_alloc().

◆ Wv_wls

float Wv_wls[ANDI_OUTPUTS] = {1.0}
static

Definition at line 201 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ yaw_stick_in_auto

bool yaw_stick_in_auto = false

Definition at line 344 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().