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 ANDI_NUM_ACT   4
 
#define ANDI_NUM_VIRTUAL_ACT   2
 
#define ANDI_NUM_ACT_TOT   (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)
 
#define ANDI_OUTPUTS   6
 
#define ONELOOP_ANDI_FILT_CUTOFF_P   20.0
 
#define ONELOOP_ANDI_FILT_CUTOFF_Q   20.0
 
#define ONELOOP_ANDI_FILT_CUTOFF_R   20.0
 
#define ONELOOP_ANDI_DEBUG_MODE   FALSE;
 
#define ONELOOP_ANDI_AC_HAS_PUSHER   FALSE;
 
#define ONELOOP_ANDI_PUSHER_IDX   0
 
#define WLS_N_U   == ANDI_NUM_ACT_TOT
 
#define WLS_N_V   == ANDI_OUTPUTS
 
#define GUIDANCE_INDI_SPEED_GAIN   1.8
 
#define GUIDANCE_INDI_SPEED_GAINZ   1.8
 
#define GUIDANCE_INDI_POS_GAIN   0.5
 
#define GUIDANCE_INDI_POS_GAINZ   0.5
 
#define GUIDANCE_INDI_LIFTD_ASQ   0.20
 
#define GUIDANCE_INDI_LIFTD_P80   (GUIDANCE_INDI_LIFTD_ASQ*12*12)
 
#define GUIDANCE_INDI_LIFTD_P50   (GUIDANCE_INDI_LIFTD_P80/2)
 

Functions

void init_poles (void)
 Initialize Position of Poles. More...
 
void calc_normalization (void)
 Calculate Normalization of actuators and discrete actuator dynamics
More...
 
void sum_g1g2_1l (void)
 Function that sums g1 and g2 to obtain the g1_g2 matrix. More...
 
void get_act_state_oneloop (void)
 Function to reconstruct actuator state using first order dynamics. More...
 
void oneloop_andi_propagate_filters (void)
 Propagate the filters. More...
 
void init_filter (void)
 Initialize the filters. More...
 
void init_controller (void)
 Initialize Controller Gains FIXME: Calculate the gains dynamically for transition. More...
 
void float_rates_of_euler_dot_vec (float r[3], float e[3], float edot[3])
 Attitude Rates to Euler Conversion Function. More...
 
void float_euler_dot_of_rates_vec (float r[3], float e[3], float edot[3])
 Attitude Euler to Rates Conversion Function. More...
 
void err_nd (float err[], float a[], float b[], float k[], int n)
 Calculate Scaled Error between two 3D arrays. More...
 
void integrate_nd (float dt, float a[], float a_dot[], int n)
 Integrate in time 3D array. More...
 
void vect_bound_nd (float vect[], float bound, int n)
 Scale a 3D array to within a 3D bound. More...
 
void rm_2nd (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float x_des, float k1_rm, float k2_rm)
 Reference Model Definition for 2nd order system. More...
 
void rm_3rd (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float *x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm)
 Reference Model Definition for 3rd order system. More...
 
void rm_3rd_head (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float *x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm)
 Reference Model Definition for 3rd order system specific to the heading angle. More...
 
void rm_3rd_attitude (float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3])
 Reference Model Definition for 3rd order system with attitude conversion functions. More...
 
void rm_3rd_pos (float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_des[], float k1_rm[], float k2_rm[], float k3_rm[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n)
 Reference Model Definition for 3rd order system specific to positioning with bounds. More...
 
void rm_2nd_pos (float dt, float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_d_des[], float k2_rm[], float k3_rm[], float x_2d_bound, float x_3d_bound, int n)
 Reference Model Definition for 3rd order system specific to positioning with bounds. More...
 
void rm_1st_pos (float dt, float x_2d_ref[], float x_3d_ref[], float x_2d_des[], float k3_rm[], float x_3d_bound, int n)
 Reference Model Definition for 3rd order system specific to positioning with bounds. More...
 
void ec_3rd_att (float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3])
 Error Controller Definition for 3rd order system specific to attitude. More...
 
void calc_model (void)
 Function that calculates the model prediction for the complementary filter. More...
 
static void send_eff_mat_g_oneloop_andi (struct transport_tx *trans, struct link_device *dev)
 
static void send_oneloop_andi (struct transport_tx *trans, struct link_device *dev)
 
static void send_guidance_oneloop_andi (struct transport_tx *trans, struct link_device *dev)
 
static float positive_non_zero (float input)
 Function to make sure that inputs are positive non zero vaues. More...
 
static float k_e_1_3_f (float p1, float p2, float p3)
 Error Controller Gain Design. More...
 
static float k_e_2_3_f (float p1, float p2, float p3)
 
static float k_e_3_3_f (float p1, float p2, float p3)
 
static float k_e_1_2_f (float p1, float p2)
 
static float k_e_2_2_f (float p1, float p2)
 
static float k_e_1_3_f_v2 (float omega_n, UNUSED float zeta, float p1)
 
static float k_e_2_3_f_v2 (float omega_n, float zeta, float p1)
 
static float k_e_3_3_f_v2 (float omega_n, float zeta, float p1)
 
static float k_rm_1_3_f (float omega_n, float zeta, float p1)
 Reference Model Gain Design. More...
 
static float k_rm_2_3_f (float omega_n, float zeta, float p1)
 
static float k_rm_3_3_f (float omega_n, float zeta, float p1)
 
static float k_rm_1_2_f (float omega_n, float zeta)
 
static float k_rm_2_2_f (float omega_n, float zeta)
 
static float ec_3rd (float x_ref, float x_d_ref, float x_2d_ref, float x_3d_ref, float x, float x_d, float x_2d, float k1_e, float k2_e, float k3_e)
 Error Controller Definition for 3rd order system. More...
 
static float ec_2rd (float x_ref, float x_d_ref, float x_2d_ref, float x, float x_d, float k1_e, float k2_e)
 Error Controller Definition for 2rd order system. More...
 
static float w_approx (float p1, float p2, float p3, float rm_k)
 Third Order to First Order Dynamics Approximation. More...
 
void oneloop_andi_init (void)
 Init function of Oneloop ANDI controller
More...
 
void oneloop_andi_enter (bool half_loop_sp)
 Function that resets important values upon engaging Oneloop ANDI. More...
 
void oneloop_andi_RM (bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
 Function to generate the reference signals for the oneloop controller. More...
 
void oneloop_andi_run (bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
 Main function that runs the controller and performs control allocation. More...
 
void oneloop_from_nav (bool in_flight)
 Function that maps navigation inputs to the oneloop controller for the generated autopilot. More...
 

Variables

float num_thrusters_oneloop = 4.0
 
float oneloop_andi_filt_cutoff = 2.0
 
float oneloop_andi_filt_cutoff_a = 2.0
 
float oneloop_andi_filt_cutoff_v = 2.0
 
float oneloop_andi_filt_cutoff_p = 2.0
 
bool actuator_is_servo [ANDI_NUM_ACT_TOT] = {0}
 
float act_dynamics [ANDI_NUM_ACT_TOT] = = {1}
 
float act_max [ANDI_NUM_ACT_TOT] = = {MAX_PPRZ}
 
float act_min [ANDI_NUM_ACT_TOT] = = {0.0}
 
float act_max_norm [ANDI_NUM_ACT_TOT] = = {1.0}
 
float act_min_norm [ANDI_NUM_ACT_TOT] = = {0.0}
 
static float Wv [ANDI_OUTPUTS] = {1.0}
 
static float Wv_wls [ANDI_OUTPUTS] = {1.0}
 
static float Wu [ANDI_NUM_ACT_TOT] = {1.0}
 
static float u_pref [ANDI_NUM_ACT_TOT] = {0.0}
 
float max_a_nav = 4.0
 
float max_j_nav = 500.0
 
float max_v_nav = 5.0
 
struct guidance_indi_hybrid_params gih_params
 
bool force_forward = false
 forward flight for hybrid nav More...
 
struct OneloopGeneral oneloop_andi
 
static float use_increment = 0.0
 
static float nav_target [3]
 
static float dt_1l = 1./PERIODIC_FREQUENCY
 
static float g = 9.81
 
float andi_u [ANDI_NUM_ACT_TOT]
 
float andi_du [ANDI_NUM_ACT_TOT]
 
static float andi_du_n [ANDI_NUM_ACT_TOT]
 
float nu [ANDI_OUTPUTS]
 
static float act_dynamics_d [ANDI_NUM_ACT_TOT]
 
float actuator_state_1l [ANDI_NUM_ACT]
 
static float a_thrust = 0.0
 
struct Int32Eulers stab_att_sp_euler_1l
 
struct Int32Quat stab_att_sp_quat_1l
 
struct FloatEulers eulers_zxy_des
 
struct FloatEulers eulers_zxy
 
static float psi_des_rad = 0.0
 
float psi_des_deg = 0.0
 
static float psi_vec [4] = {0.0, 0.0, 0.0, 0.0}
 
static float gamma_wls = 1000.0
 
static float du_min_1l [ANDI_NUM_ACT_TOT]
 
static float du_max_1l [ANDI_NUM_ACT_TOT]
 
static float du_pref_1l [ANDI_NUM_ACT_TOT]
 
static int number_iter = 0
 
static float model_pred [ANDI_OUTPUTS]
 
static float ang_acc [3]
 
static float lin_acc [3]
 
struct PolePlacement p_att_e
 
struct PolePlacement p_att_rm
 
struct PolePlacement p_pos_e
 
struct PolePlacement p_pos_rm
 
struct PolePlacement p_alt_e
 
struct PolePlacement p_alt_rm
 
struct PolePlacement p_head_e
 
struct PolePlacement p_head_rm
 
struct Gains3rdOrder k_att_e
 
struct Gains3rdOrder k_att_rm
 
struct Gains2ndOrder k_head_e
 
struct Gains2ndOrder k_head_rm
 
struct Gains3rdOrder k_pos_e
 
struct Gains3rdOrder k_pos_rm
 
float g2_1l [ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2
 
float g1_1l [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] = {ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_THRUST, ONELOOP_ANDI_G1_ROLL, ONELOOP_ANDI_G1_PITCH, ONELOOP_ANDI_G1_YAW}
 
float g1g2_1l [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]
 
float * bwls_1l [ANDI_OUTPUTS]
 
float ratio_u_un [ANDI_NUM_ACT_TOT]
 
float ratio_vn_v [ANDI_NUM_ACT_TOT]
 
static struct FirstOrderLowPass filt_accel_ned [3]
 
static struct FirstOrderLowPass rates_filt_fo [3]
 
static struct FirstOrderLowPass model_pred_a_filt [3]
 
static Butterworth2LowPass att_dot_meas_lowpass_filters [3]
 
static Butterworth2LowPass model_pred_filt [ANDI_OUTPUTS]
 

Macro Definition Documentation

◆ ANDI_NUM_ACT

#define ANDI_NUM_ACT   4

Definition at line 100 of file oneloop_andi.c.

◆ ANDI_NUM_ACT_TOT

#define ANDI_NUM_ACT_TOT   (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)

Definition at line 112 of file oneloop_andi.c.

◆ ANDI_NUM_VIRTUAL_ACT

#define ANDI_NUM_VIRTUAL_ACT   2

Definition at line 106 of file oneloop_andi.c.

◆ ANDI_OUTPUTS

#define ANDI_OUTPUTS   6

Definition at line 117 of file oneloop_andi.c.

◆ GUIDANCE_INDI_LIFTD_ASQ

#define GUIDANCE_INDI_LIFTD_ASQ   0.20

Definition at line 286 of file oneloop_andi.c.

◆ GUIDANCE_INDI_LIFTD_P50

#define GUIDANCE_INDI_LIFTD_P50   (GUIDANCE_INDI_LIFTD_P80/2)

Definition at line 295 of file oneloop_andi.c.

◆ GUIDANCE_INDI_LIFTD_P80

#define GUIDANCE_INDI_LIFTD_P80   (GUIDANCE_INDI_LIFTD_ASQ*12*12)

Definition at line 294 of file oneloop_andi.c.

◆ GUIDANCE_INDI_POS_GAIN

#define GUIDANCE_INDI_POS_GAIN   0.5

Definition at line 281 of file oneloop_andi.c.

◆ GUIDANCE_INDI_POS_GAINZ

#define GUIDANCE_INDI_POS_GAINZ   0.5

Definition at line 282 of file oneloop_andi.c.

◆ GUIDANCE_INDI_SPEED_GAIN

#define GUIDANCE_INDI_SPEED_GAIN   1.8

Definition at line 276 of file oneloop_andi.c.

◆ GUIDANCE_INDI_SPEED_GAINZ

#define GUIDANCE_INDI_SPEED_GAINZ   1.8

Definition at line 277 of file oneloop_andi.c.

◆ ONELOOP_ANDI_AC_HAS_PUSHER

#define ONELOOP_ANDI_AC_HAS_PUSHER   FALSE;

Definition at line 232 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_FILT_CUTOFF_P

#define ONELOOP_ANDI_FILT_CUTOFF_P   20.0

Definition at line 153 of file oneloop_andi.c.

◆ ONELOOP_ANDI_FILT_CUTOFF_Q

#define ONELOOP_ANDI_FILT_CUTOFF_Q   20.0

Definition at line 159 of file oneloop_andi.c.

◆ ONELOOP_ANDI_FILT_CUTOFF_R

#define ONELOOP_ANDI_FILT_CUTOFF_R   20.0

Definition at line 165 of file oneloop_andi.c.

◆ ONELOOP_ANDI_PUSHER_IDX

#define ONELOOP_ANDI_PUSHER_IDX   0

Definition at line 236 of file oneloop_andi.c.

◆ WLS_N_U

#define WLS_N_U   == ANDI_NUM_ACT_TOT

Definition at line 246 of file oneloop_andi.c.

◆ WLS_N_V

#define WLS_N_V   == ANDI_OUTPUTS

Definition at line 250 of file oneloop_andi.c.

Function Documentation

◆ calc_model()

void calc_model ( void  )

Function that calculates the model prediction for the complementary filter.

Definition at line 1475 of file oneloop_andi.c.

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

Referenced by oneloop_andi_propagate_filters().

+ Here is the caller graph for this function:

◆ calc_normalization()

void calc_normalization ( void  )

Calculate Normalization of actuators and discrete actuator dynamics

Definition at line 1457 of file oneloop_andi.c.

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

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

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

◆ ec_2rd()

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

Error Controller Definition for 2rd order system.

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

Definition at line 883 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

+ Here is the caller graph for this function:

◆ ec_3rd()

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

Error Controller Definition for 3rd order system.

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

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 854 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 608 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 593 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 581 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 1381 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 909 of file oneloop_andi.c.

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

Referenced by oneloop_andi_init().

+ Here is the caller graph for this function:

◆ integrate_nd()

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

Integrate in time 3D array.

Definition at line 617 of file oneloop_andi.c.

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

+ Here is the caller graph for this function:

◆ k_e_1_2_f()

static float k_e_1_2_f ( float  p1,
float  p2 
)
static

Definition at line 513 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

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

◆ k_e_1_3_f()

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

Error Controller Gain Design.

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

References positive_non_zero().

Referenced by init_controller().

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

◆ k_e_2_2_f()

static float k_e_2_2_f ( float  p1,
float  p2 
)
static

Definition at line 519 of file oneloop_andi.c.

References positive_non_zero().

Referenced by init_controller().

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

◆ k_e_2_3_f()

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

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

References positive_non_zero().

Referenced by init_controller().

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

◆ k_e_3_3_f()

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

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

References positive_non_zero().

Referenced by init_controller().

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

◆ k_rm_1_2_f()

static float k_rm_1_2_f ( float  omega_n,
float  zeta 
)
static

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

References positive_non_zero().

Referenced by init_controller().

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

◆ k_rm_2_2_f()

static float k_rm_2_2_f ( float  omega_n,
float  zeta 
)
static

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

References positive_non_zero().

Referenced by init_controller().

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

◆ oneloop_andi_enter()

void oneloop_andi_enter ( bool  half_loop_sp)

Function that resets important values upon engaging Oneloop ANDI.

FIXME: Ideally we should distinguish between the "stabilization" and "guidance" needs because it is unlikely to switch stabilization in flight, and there are multiple modes that use (the same) stabilization. Resetting the controller is not so nice when you are flying.

Definition at line 1120 of file oneloop_andi.c.

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

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

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

◆ oneloop_andi_init()

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

References a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, actuator_state_1l, ANDI_NUM_ACT, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, dt_1l, eulers_zxy, eulers_zxy_des, float_vect_copy(), float_vect_zero(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains2ndOrder::k2, Gains3rdOrder::k3, Gains2ndOrder::k3, k_att_rm, k_head_rm, k_pos_rm, max_a_nav, max_j_nav, MAX_PPRZ, max_v_nav, nav_target, oneloop_andi, FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_rad, psi_vec, radio_control_get(), RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, ratio_u_un, ratio_vn_v, rm_1st_pos(), rm_2nd(), rm_2nd_pos(), rm_3rd(), rm_3rd_attitude(), rm_3rd_pos(), OneloopGeneral::sta_ref, FloatEulers::theta, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, Wv, Wv_wls, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by oneloop_andi_run().

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

◆ oneloop_andi_run()

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

Main function that runs the controller and performs control allocation.

Parameters
half_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 1253 of file oneloop_andi.c.

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

Referenced by oneloop_from_nav(), and stabilization_attitude_run().

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

◆ oneloop_from_nav()

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

Referenced by calc_normalization(), k_e_1_2_f(), k_e_1_3_f(), k_e_1_3_f_v2(), k_e_2_2_f(), k_e_2_3_f(), k_e_2_3_f_v2(), k_e_3_3_f(), k_e_3_3_f_v2(), k_rm_1_2_f(), k_rm_1_3_f(), k_rm_2_2_f(), k_rm_2_3_f(), k_rm_3_3_f(), oneloop_andi_init(), vect_bound_nd(), and w_approx().

+ Here is the caller graph for this function:

◆ rm_1st_pos()

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

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

Parameters
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 790 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 811 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

+ Here is the caller graph for this function:

◆ rm_2nd_pos()

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

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

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

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

Referenced by oneloop_andi_RM().

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

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

Referenced by oneloop_andi_RM().

+ Here is the caller graph for this function:

◆ rm_3rd_attitude()

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

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

Parameters
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 652 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 711 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 739 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 340 of file oneloop_andi.c.

References ANDI_NUM_ACT_TOT, dev, and g1g2_1l.

Referenced by oneloop_andi_init().

+ Here is the caller graph for this function:

◆ send_guidance_oneloop_andi()

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

Definition at line 378 of file oneloop_andi.c.

Referenced by oneloop_andi_init().

+ Here is the caller graph for this function:

◆ send_oneloop_andi()

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

◆ sum_g1g2_1l()

void sum_g1g2_1l ( void  )

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

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

Definition at line 1399 of file oneloop_andi.c.

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

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

+ Here is the caller graph for this function:

◆ vect_bound_nd()

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

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

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

References positive_non_zero().

Referenced by init_controller().

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

Variable Documentation

◆ a_thrust

float a_thrust = 0.0
static

Definition at line 421 of file oneloop_andi.c.

Referenced by oneloop_andi_RM(), and oneloop_andi_run().

◆ act_dynamics

float act_dynamics[ANDI_NUM_ACT_TOT] = = {1}

◆ act_dynamics_d

float act_dynamics_d[ANDI_NUM_ACT_TOT]
static

Definition at line 419 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 184 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 196 of file oneloop_andi.c.

Referenced by calc_normalization().

◆ act_min

float act_min[ANDI_NUM_ACT_TOT] = = {0.0}

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

Referenced by calc_normalization().

◆ actuator_is_servo

bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0}

Definition at line 171 of file oneloop_andi.c.

◆ actuator_state_1l

◆ andi_du

float andi_du[ANDI_NUM_ACT_TOT]

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

Referenced by oneloop_andi_init(), and oneloop_andi_run().

◆ andi_u

float andi_u[ANDI_NUM_ACT_TOT]

Definition at line 415 of file oneloop_andi.c.

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

◆ ang_acc

float ang_acc[3]
static

◆ att_dot_meas_lowpass_filters

Butterworth2LowPass att_dot_meas_lowpass_filters[3]
static

Definition at line 477 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ bwls_1l

float* bwls_1l[ANDI_OUTPUTS]

Definition at line 469 of file oneloop_andi.c.

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

◆ dt_1l

float dt_1l = 1./PERIODIC_FREQUENCY
static

Definition at line 411 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 435 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 434 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 436 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ eulers_zxy

struct FloatEulers eulers_zxy

◆ eulers_zxy_des

struct FloatEulers eulers_zxy_des

◆ filt_accel_ned

struct FirstOrderLowPass filt_accel_ned[3]
static

Definition at line 471 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ force_forward

bool force_forward = false

forward flight for hybrid nav

Definition at line 310 of file oneloop_andi.c.

◆ g

float g = 9.81
static

Definition at line 412 of file oneloop_andi.c.

Referenced by calc_model().

◆ g1_1l

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

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

Referenced by sum_g1g2_1l().

◆ gamma_wls

float gamma_wls = 1000.0
static

Definition at line 433 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ gih_params

struct guidance_indi_hybrid_params gih_params
Initial value:
= {
.speed_gain = GUIDANCE_INDI_SPEED_GAIN,
.speed_gainz = GUIDANCE_INDI_SPEED_GAINZ,
.heading_bank_gain = 5.0,
.liftd_asq = GUIDANCE_INDI_LIFTD_ASQ,
}
#define GUIDANCE_INDI_SPEED_GAINZ
Definition: oneloop_andi.c:277
#define GUIDANCE_INDI_LIFTD_ASQ
Definition: oneloop_andi.c:286
#define GUIDANCE_INDI_POS_GAIN
Definition: oneloop_andi.c:281
#define GUIDANCE_INDI_LIFTD_P50
Definition: oneloop_andi.c:295
#define GUIDANCE_INDI_POS_GAINZ
Definition: oneloop_andi.c:282
#define GUIDANCE_INDI_LIFTD_P80
Definition: oneloop_andi.c:294
#define GUIDANCE_INDI_SPEED_GAIN
Definition: oneloop_andi.c:276

Definition at line 270 of file oneloop_andi.c.

Referenced by init_controller().

◆ k_att_e

struct Gains3rdOrder k_att_e

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_att_rm

struct Gains3rdOrder k_att_rm

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_RM().

◆ k_head_e

struct Gains2ndOrder k_head_e

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_head_rm

struct Gains2ndOrder k_head_rm

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_RM().

◆ k_pos_e

struct Gains3rdOrder k_pos_e

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_run().

◆ k_pos_rm

struct Gains3rdOrder k_pos_rm

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

Referenced by oneloop_andi_RM().

◆ max_j_nav

float max_j_nav = 500.0

Definition at line 264 of file oneloop_andi.c.

Referenced by oneloop_andi_RM().

◆ max_v_nav

float max_v_nav = 5.0

Definition at line 270 of file oneloop_andi.c.

Referenced by init_controller(), and oneloop_andi_RM().

◆ model_pred

float model_pred[ANDI_OUTPUTS]
static

◆ model_pred_a_filt

struct FirstOrderLowPass model_pred_a_filt[3]
static

Definition at line 471 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ model_pred_filt

Butterworth2LowPass model_pred_filt[ANDI_OUTPUTS]
static

Definition at line 478 of file oneloop_andi.c.

Referenced by init_filter(), and oneloop_andi_propagate_filters().

◆ nav_target

float nav_target[3]
static

Definition at line 410 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 418 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 121 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ number_iter

int number_iter = 0
static

Definition at line 437 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 129 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 135 of file oneloop_andi.c.

Referenced by init_filter().

◆ oneloop_andi_filt_cutoff_p

float oneloop_andi_filt_cutoff_p = 2.0

Definition at line 147 of file oneloop_andi.c.

◆ oneloop_andi_filt_cutoff_v

float oneloop_andi_filt_cutoff_v = 2.0

Definition at line 141 of file oneloop_andi.c.

◆ p_alt_e

struct PolePlacement p_alt_e

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_alt_rm

struct PolePlacement p_alt_rm

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_att_e

struct PolePlacement p_att_e

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_att_rm

struct PolePlacement p_att_rm

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_head_e

struct PolePlacement p_head_e

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_head_rm

struct PolePlacement p_head_rm

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller().

◆ p_pos_e

struct PolePlacement p_pos_e

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ p_pos_rm

struct PolePlacement p_pos_rm

Definition at line 442 of file oneloop_andi.c.

Referenced by init_controller(), and init_poles().

◆ psi_des_deg

float psi_des_deg = 0.0

Definition at line 429 of file oneloop_andi.c.

Referenced by oneloop_andi_enter(), and oneloop_andi_run().

◆ psi_des_rad

float psi_des_rad = 0.0
static

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

Referenced by oneloop_andi_RM().

◆ rates_filt_fo

struct FirstOrderLowPass rates_filt_fo[3]
static

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

◆ stab_att_sp_quat_1l

struct Int32Quat stab_att_sp_quat_1l

Definition at line 421 of file oneloop_andi.c.

◆ u_pref

float u_pref[ANDI_NUM_ACT_TOT] = {0.0}
static

Definition at line 222 of file oneloop_andi.c.

Referenced by oneloop_andi_run().

◆ use_increment

float use_increment = 0.0
static

◆ Wu

float Wu[ANDI_NUM_ACT_TOT] = {1.0}
static

Definition at line 216 of file oneloop_andi.c.

Referenced by oneloop_andi_run(), and wls_alloc().

◆ Wv

float Wv[ANDI_OUTPUTS] = {1.0}
static

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

Referenced by oneloop_andi_RM(), and oneloop_andi_run().