oneloop_andi.c

Go to the documentation of this file.

/*
 * Copyright (C) 2023 Tomaso De Ponti <tmldeponti@tudelft.nl>
 *
 * This file is part of paparazzi
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
 
//====================================================================================================================================
//====================================================================================================================================
// Enter functions change the state of the oneloop controller
// Example of execution of the oneloop controller for two different states in the state machine
//====================================================================================================================================
/* Include necessary header files */
#include "firmwares/rotorcraft/oneloop/oneloop_andi.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "generated/airframe.h"
#include "modules/radio_control/radio_control.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"
#include "filters/notch_filter_float.h"
#include "math/wls/wls_alloc.h"
#include "modules/nav/nav_rotorcraft_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
#include "modules/rotwing_drone/rotwing_state.h"
#endif
#include "modules/core/commands.h"
// #include "modules/ctrl/eff_scheduling_rotwing_V2.h"
#include "generated/modules.h"
#ifdef MODULE_SYS_ID_DOUBLET_ID
#include "modules/system_identification/sys_id_doublet.h"
#endif
#include <stdio.h>
#if INS_EXT_POSE
#include "modules/ins/ins_ext_pose.h"
#endif
 
#ifdef ONELOOP_ANDI_CONSTANT_EFFECTIVENESS
/* Quadrotor without an effectiveness scheduling module: the effectiveness matrix is built here
 * from the airframe rows ONELOOP_ANDI_G1_THRUST/ROLL/PITCH/YAW and ONELOOP_ANDI_G2 (scaled by ANDI_G_SCALING).
 * Do not combine with an effectiveness scheduling module, those provide EFF_MAT_RW themselves.
 */
#define RW_aN 0 // North axis (linear acceleration)
#define RW_aE 1 // East axis (linear acceleration)
#define RW_aD 2 // Down axis (linear acceleration)
#define RW_ap 3 // X body axis (angular acceleration)
#define RW_aq 4 // Y body axis (angular acceleration)
#define RW_ar 5 // Z body axis (angular acceleration)
float EFF_MAT_RW[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT];
float G2_RW[ANDI_NUM_ACT_TOT];
static void oneloop_andi_constant_eff(void);
#endif
//====================================================================================================================================
//====================================================================================================================================
// Define configuration variables with default values if not defined in the airframe file
//====================================================================================================================================
//====================================================================================================================================
// GENERAL VARIABLES 
//====================================================================================================================================
#ifndef ONELOOP_ANDI_DEBUG_MODE                                             // Debug mode, sets in_flight to FALSE     
#define ONELOOP_ANDI_DEBUG_MODE  FALSE
#endif
 
struct OneloopGeneral oneloop_andi;                                         // Define general struct of the Oneloop ANDI controller   
static float          dt_1l = 1./PERIODIC_FREQUENCY;                        // Time step of the oneloop controller [s]   
static float          g = 9.81;                                             // [m/s^2] Gravitational Acceleration
//====================================================================================================================================
// FILTERING VARIABLES 
//====================================================================================================================================
#define USE_BW2
 
#ifdef ONELOOP_ANDI_FILT_CUTOFF
float oneloop_andi_filt_cutoff = ONELOOP_ANDI_FILT_CUTOFF;
#else
float oneloop_andi_filt_cutoff = 2.0;
#endif
 
#ifdef ONELOOP_ANDI_FILT_CUTOFF_ACC
float oneloop_andi_filt_cutoff_a = ONELOOP_ANDI_FILT_CUTOFF_ACC;
#else
float oneloop_andi_filt_cutoff_a = 2.0;
#endif
 
#ifdef ONELOOP_ANDI_FILT_CUTOFF_VEL
float oneloop_andi_filt_cutoff_v = ONELOOP_ANDI_FILT_CUTOFF_VEL;
#else
float  oneloop_andi_filt_cutoff_v = 30.0;
#endif
 
#ifdef ONELOOP_ANDI_FILT_CUTOFF_P
#define ONELOOP_ANDI_FILTER_ROLL_RATE TRUE
float oneloop_andi_filt_cutoff_p = ONELOOP_ANDI_FILT_CUTOFF_P;
#else
float oneloop_andi_filt_cutoff_p = 20.0;
#endif
 
#ifdef ONELOOP_ANDI_FILT_CUTOFF_Q
#define ONELOOP_ANDI_FILTER_PITCH_RATE TRUE
float oneloop_andi_filt_cutoff_q = ONELOOP_ANDI_FILT_CUTOFF_Q;
#else
float oneloop_andi_filt_cutoff_q = 20.0;
#endif
 
#ifdef ONELOOP_ANDI_FILT_CUTOFF_R
#define ONELOOP_ANDI_FILTER_YAW_RATE TRUE
float oneloop_andi_filt_cutoff_r = ONELOOP_ANDI_FILT_CUTOFF_R;
#else
float oneloop_andi_filt_cutoff_r = 20.0;
#endif
 
PRINT_CONFIG_MSG("============================================================")
PRINT_CONFIG_MSG("%%% ONELOOP FILTERING VARIABLES %%%")
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_ACC)
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_VEL)
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF);
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_Q);
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_P);
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_R);
PRINT_CONFIG_MSG("============================================================")
 
struct Oneloop_LP_t         LP;
struct Oneloop_LP_t         oneloop_andi_model_filt;
struct Oneloop_DynFilt_t    mu_mL;
struct Oneloop_DynFilt_t    mu_mR;  
bool                        use_dyn_filter = false; 
float                       oneloop_andi_sigma = 29.0;
float                       oneloop_andi_sigma_max = 29.0;
float                       oneloop_andi_sigma_min = 8.0;
 
#define USE_PID_NUMDIFF_DERIVATIVE
#ifdef USE_PID_NUMDIFF_DERIVATIVE
static Butterworth2LowPass  filt_veloc[3];
#endif
static Butterworth2LowPass  accely_filt;                  // FIXME (check if condenseable) Low pass filter for acceleration in y direction   
static Butterworth2LowPass  airspeed_filt;                // FIXME (check if condenseable) Low pass filter for airspeed                            
static Butterworth2LowPass  u_filt[ANDI_NUM_ACT_TOT];     // FIXME (check if condenseable) Low pass filter for actuators for synchronous filtering      
//====================================================================================================================================
// ACTUATOR VARIABLES 
//====================================================================================================================================
#ifndef ONELOOP_ANDI_NUM_THRUSTERS                                              // Number of motors used for thrust
float num_thrusters_oneloop = 4.0; 
#else
float num_thrusters_oneloop = ONELOOP_ANDI_NUM_THRUSTERS;
#endif
 
#ifdef ONELOOP_ANDI_ACT_IS_SERVO                                                // Unused but kept for future developments
bool   actuator_is_servo[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_ACT_IS_SERVO;
#else
bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0};
#endif
 
#ifdef ONELOOP_ANDI_ACT_DYN                                                     // Actuator dynamics (first order lag) corner frequency [rad/s]
float  act_dynamics[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_ACT_DYN;
float  act_dyn_ctrl[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_ACT_DYN;
#else
#error "You must specify the actuator dynamics"
float act_dynamics[ANDI_NUM_ACT_TOT] = = {1};
float act_dyn_ctrl[ANDI_NUM_ACT_TOT] = = {1};
#endif
 
#ifdef ONELOOP_ANDI_ACT_MAX                                                     // Maximum Paparazzi actuator command 
float  act_max[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_ACT_MAX;
#else
float act_max[ANDI_NUM_ACT_TOT] = = {MAX_PPRZ};
#endif
 
#ifdef ONELOOP_ANDI_ACT_MIN                                                     // Minimum Paparazzi actuator command
float  act_min[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_ACT_MIN;
#else
float act_min[ANDI_NUM_ACT_TOT] = = {0.0};
#endif
 
#ifdef ONELOOP_ANDI_ACT_MAX_NORM                                                // Maximum normalized actuator command 
float  act_max_norm[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_ACT_MAX_NORM;
#else
float act_max_norm[ANDI_NUM_ACT_TOT] = = {1.0};
#endif
 
#ifdef ONELOOP_ANDI_ACT_MIN_NORM                                               // Minimum normalized actuator command
float  act_min_norm[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_ACT_MIN_NORM;
#else
float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0};
#endif
 
#ifdef ONELOOP_ANDI_NU_NORM_MAX                                                // Maximum norm of the normalized pseudo control vector
float  nu_norm_max = ONELOOP_ANDI_NU_NORM_MAX;
#else
float nu_norm_max = 1.0;
#endif
 
#ifdef ONELOOP_ANDI_U_PREF                                                     // Preferred Paparazzi actuator command 
static float u_pref[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_U_PREF;
#else
static float u_pref[ANDI_NUM_ACT_TOT] = {0.0};
#endif
 
#define ONELOOP_ANDI_MAX_BANK  act_max[COMMAND_ROLL]                            // Max Bank (can be different from Max Roll) assuming abs of max and min is the same
#define ONELOOP_ANDI_MAX_PHI   act_max[COMMAND_ROLL]                            // Max Roll assuming abs of max and min is the same
#define ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH]                           // Max Pitch assuming abs of max and min is the same
 
#ifndef ONELOOP_THETA_PREF_MAX                                                  // Max preferred pitch angles
float theta_pref_max = RadOfDeg(20.0);
#else
float theta_pref_max = RadOfDeg(ONELOOP_THETA_PREF_MAX);
#endif
 
#if ANDI_NUM_ACT_TOT != WLS_N_U_MAX
#error Matrix-WLS_N_U_MAX is not equal to the number of actuators: define WLS_N_U_MAX == ANDI_NUM_ACT_TOT in airframe file
#define WLS_N_U_MAX == ANDI_NUM_ACT_TOT
#endif
#if ANDI_OUTPUTS != WLS_N_V_MAX
#error Matrix-WLS_N_V_MAX is not equal to the number of controlled axis: define WLS_N_V_MAX == ANDI_OUTPUTS in airframe file
#define WLS_N_V_MAX == ANDI_OUTPUTS
#endif
 
float         andi_u[ANDI_NUM_ACT_TOT];                                         // Control command vector   
float         andi_du[ANDI_NUM_ACT_TOT];                                        // Control command increment vector
static float  andi_u_n[ANDI_NUM_ACT_TOT];                                       // Control command vector (normalized)
float         nu[ANDI_OUTPUTS];                                                 // Pseudo control vector
float         nu_n[ANDI_OUTPUTS];                                               // Pseudo control vector (normalized)
static float  act_dynamics_d[ANDI_NUM_ACT_TOT];                                 // Actuator dynamics (first order lag) corner frequency [rad/s]
float         actuator_state_1l[ANDI_NUM_ACT_TOT];                              // Actuator state vector (including virtual actuators)
//====================================================================================================================================
// STABILIZATION VARIABLES
//====================================================================================================================================
#ifndef MAX_R                                                                     // Max Yaw rate with the sticks  
float max_r = RadOfDeg(120.0);
#else
float max_r = RadOfDeg(MAX_R);
#endif
 
#ifdef ONELOOP_ANDI_YAW_DISTURBANCE_LIMIT                                         // Yaw disturbance rejection limit     
float oneloop_andi_yaw_dist_limit = ONELOOP_ANDI_YAW_DISTURBANCE_LIMIT;
#else 
float oneloop_andi_yaw_dist_limit = 99999.f;                                      // High value to disable the feature
#endif
 
#if ONELOOP_ANDI_HEADING_MANUAL                                                   // Specify heading in NAV with a slider
bool heading_manual = true;
#else
bool heading_manual = false;
#endif
 
#if ONELOOP_ANDI_YAW_STICK_IN_AUTO                                                // Specify heading in NAV with the yaw stick
bool yaw_stick_in_auto = true;
#else
bool yaw_stick_in_auto = false;
#endif
 
struct OneloopStabilizationRef sta_bounds = {                                     // Stabilization bounds specified per axis
  #ifdef ONELOOP_ANDI_MAX_ANGULAR_JERK
  .att_3d[0] = ONELOOP_ANDI_MAX_ANGULAR_JERK,
  .att_3d[1] = ONELOOP_ANDI_MAX_ANGULAR_JERK,
  #else
  .att_3d[0] = RadOfDeg(100000.0),
  .att_3d[1] = RadOfDeg(100000.0),
  #endif
 
#ifdef ONELOOP_ANDI_MAX_ANGULAR_JERK_YAW
    .att_3d[2] = ONELOOP_ANDI_MAX_ANGULAR_JERK_YAW,
#else
    .att_3d[2] = RadOfDeg(1520.0),
#endif
 
  #ifdef ONELOOP_ANDI_MAX_ANGULAR_ACCEL
  .att_2d[0] = ONELOOP_ANDI_MAX_ANGULAR_ACCEL,
  .att_2d[1] = ONELOOP_ANDI_MAX_ANGULAR_ACCEL,
  #else
  .att_2d[0] = RadOfDeg(700.0),
  .att_2d[1] = RadOfDeg(480.0),
  #endif
 
#ifdef ONELOOP_ANDI_MAX_ANGULAR_ACCEL_YAW
    .att_2d[2] = ONELOOP_ANDI_MAX_ANGULAR_ACCEL_YAW,
#else
    .att_2d[2] = RadOfDeg(130.0),
#endif
 
#ifdef ONELOOP_ANDI_MAX_ANGULAR_VEL
    .att_d[0] = ONELOOP_ANDI_MAX_ANGULAR_VEL,
    .att_d[1] = ONELOOP_ANDI_MAX_ANGULAR_VEL,
#else
    .att_d[0] = RadOfDeg(10000.0),
    .att_d[1] = RadOfDeg(10000.0),
#endif
 
#ifdef ONELOOP_ANDI_MAX_ANGULAR_VEL_YAW
    .att_d[2] = ONELOOP_ANDI_MAX_ANGULAR_VEL_YAW,
#else
    .att_d[2] = RadOfDeg(30.0),
#endif
};
 
struct FloatEulers  eulers_zxy_des;                                               // Desired Euler angles in ZXY sequence
struct FloatEulers  eulers_zxy;                                                   // Actual Euler angles in ZXY sequence
float               psi_des_rad = 0.0;                                            // Desired Yaw angle [rad]
float               psi_des_deg = 0.0;                                            // Desired Yaw angle [deg]
static float        psi_vec[4]  = {0.0, 0.0, 0.0, 0.0};                           // Vector to overwrite the Yaw reference in the attitude controller
static float        phi_vec[4]  = {0.0, 0.0, 0.0, 0.0};                           // Vector to overwrite the Roll reference in the attitude controller
//====================================================================================================================================
// GUIDANCE VARIABLES 
//====================================================================================================================================
#ifndef ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD                                    // Airspeed threshold to switch to coordinated turn behavior    
#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0
#endif
 
#ifndef FWD_SIDESLIP_GAIN                                                         // Gain to reduce sideslip when flying forward 
float fwd_sideslip_gain = 0.2;
#else
float fwd_sideslip_gain = FWD_SIDESLIP_GAIN;
#endif
 
#ifdef NAV_HYBRID_MAX_DECELERATION                                                // Max (ac)/(de)celeration. Can be overwritte nby NAV HYBRID   
float max_a_nav = NAV_HYBRID_MAX_DECELERATION;
#else
float max_a_nav = 4.0;  
#endif
 
#ifdef ONELOOP_ANDI_MAX_LINEAR_JERK                                               // Max linear jerk [m/s^3]    
float max_j_lin = ONELOOP_ANDI_MAX_LINEAR_JERK;
#else
float max_j_lin = 500.0; 
#endif
 
#ifdef NAV_HYBRID_MAX_AIRSPEED
float max_v_nav = NAV_HYBRID_MAX_AIRSPEED;                                        // Max horizontal speed. Can be overwritten by NAV HYBRID
#else
float max_v_nav = 5.0;
#endif
 
#ifdef NAV_HYBRID_MAX_SPEED_V                                                     // Max vertical speed
float max_v_nav_v = NAV_HYBRID_MAX_SPEED_V;
#else
float max_v_nav_v = 1.5;
#endif
 
#define USE_ND_VELOCITIES                                                         // Use NumDiff of velocities instead of Accelerometer.
 
float         max_as = 19.0f;                                                     // Max airspeed [m/s]    
float         min_as = 0.0f;                                                      // Min airspeed [m/s] 
float         xdot_lim_sf = 0.8;                                                  // Safety Factor on SUVAT velocity limit
float         ec_headroom = 1.5;                                                  // Extra headroom for the EC wrt the RM  
static float  nav_target[3];                                                      // Can be a position, speed or acceleration depending on the guidance H mode
static float  nav_target_new[3];                                                  // Wind triangle reshaped NAV target
float         gi_unbounded_airspeed_sp = 0.0;                                     // Unbounded airspeed setpoint [m/s] (mimics guidance_indi_hybrid)                                          
//====================================================================================================================================
// CONTROL ALLOCATION VARIABLES
//====================================================================================================================================
#ifndef ONELOOP_ANDI_WU_QUAD_MOTORS_FWD
float Wu_quad_motors_fwd = 6.0;
#else
float Wu_quad_motors_fwd = ONELOOP_ANDI_WU_QUAD_MOTORS_FWD;
#endif
 
struct WLS_t WLS_one_p = {
  .nu        = ANDI_NUM_ACT_TOT,
  .nv        = ANDI_OUTPUTS,
  .gamma_sq  = 1000.0,
  .v         = {0.0},
#ifdef ONELOOP_ANDI_WV // {ax_dot,ay_dot,az_dot,p_ddot,q_ddot,r_ddot}  
  .Wv        = ONELOOP_ANDI_WV,
#else
  .Wv        = {1.0},
#endif
#ifdef ONELOOP_ANDI_WU // {de,dr,daL,daR,mF,mB,mL,mR,mP,phi,theta}  
  .Wu        = ONELOOP_ANDI_WU,
#else
  .Wu        = {1.0},
#endif
  .u_pref    = {0.0},
  .u_min     = {0.0},
  .u_max     = {0.0},
  .PC        = 0.0,
  .SC        = 0.0,
  .iter      = 0
};
 
#ifdef ONELOOP_ANDI_WV // {ax_dot,ay_dot,az_dot,p_ddot,q_ddot,r_ddot}
static float Wv_backup[ANDI_OUTPUTS] = ONELOOP_ANDI_WV;
#else
static float Wv_backup[ANDI_OUTPUTS] = {1.0};
#endif
 
#ifdef ONELOOP_ANDI_WU // {mF,mR,mB,mL,mP,de,dr,da,df,phi,theta}
static float Wu_backup[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_WU;
#else
static float Wu_backup[ANDI_NUM_ACT_TOT] = {1.0};
#endif
 
static float a_thrust = 0.0;                                                      // Virtual z-Acceleration to command a Thrust level[m/s^2]
static float g2_ff = 0.0;                                                         // Feedforward G2 term for Yaw control
bool         ctrl_off = false;                                                    // Turn off stabilization with the quad motors
static float pitch_pref = 0;                                                      // Preferred pitch angle for the control allocation
 
float         *bwls_1l[ANDI_OUTPUTS];                                             // Pointer to the WLS Effectiveness matrix  
float         EFF_MAT_G[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT];                          // Intermidiate variable to save Effectiveness matrix 
float         n_array[ANDI_OUTPUTS];
float         m_array[ANDI_NUM_ACT_TOT];
float         coupling_factor[ANDI_OUTPUTS];
float         oneloop_andi_model[ANDI_OUTPUTS];
float         oneloop_andi_dist_bound[ANDI_OUTPUTS];
float         ratio_u_un[ANDI_NUM_ACT_TOT];
float         ratio_vn_v[ANDI_OUTPUTS];
//====================================================================================================================================
// CONTROL LAW VARIABLES
//====================================================================================================================================
bool          drop_yaw              = false;                                      // Drop the control of the Yaw axis
bool          drop_roll             = false;                                      // Drop the control of the Roll axis
bool          drop_pitch            = false;                                      // Drop the control of the Pitch axis
bool          drop_aN               = false;                                      // Drop the control of the aN axis
bool          drop_aE               = false;                                      // Drop the control of the aE axis
bool          drop_aD               = false;                                      // Drop the control of the aD axis
//====================================================================================================================================
// Error Controller and Reference Model VARIABLES
//====================================================================================================================================
/*Declaration of Reference Model and Error Controller Gains*/
struct PolePlacement p_att_e;
struct PolePlacement p_roll_e;
struct PolePlacement p_att_rm;
/*Position Loop*/
struct PolePlacement p_pos_e;
struct PolePlacement p_pos_rm;
/*Altitude Loop*/
struct PolePlacement p_alt_e;
struct PolePlacement p_alt_rm;
/*Heading Loop*/
struct PolePlacement p_head_e;
struct PolePlacement p_head_rm;
/*Gains of EC and RM ANDI*/
struct Gains3rdOrder k_att_e;
struct Gains3rdOrder k_att_rm;
struct Gains3rdOrder k_pos_e;
struct Gains3rdOrder k_pos_rm;
/*Gains of EC and RM INDI*/
struct Gains3rdOrder k_att_e_indi;
struct Gains3rdOrder k_pos_e_indi;
float k1_NE_tune = 0.6;
float k2_NE_tune = 1.85;
/* PID */
float k_P = 0.6;
float k_D = 1.2;
float k_I = 9.0e-4;
float temp_P_error[3];
float temp_D_error[3];
float temp_I_error[3];
//====================================================================================================================================
// System Identification VARIABLES
//====================================================================================================================================
bool    chirp_on            = false;
bool    chirp_first_call    = true;
float   time_elapsed_chirp  = 0.0;
float   t_0_chirp           = 0.0;
float   f0_chirp            = 0.2;                // Start Frequency [Hz]
float   f1_chirp            = 0.2;                // End Frequency [Hz]
float   t_chirp             = 0.65 ;              // Time length of the chirp [s]
float   A_chirp             = 250.0;              // Amplitude of the chirp
int8_t  chirp_axis          = 6   ;               // Axis on which the chirp is applied
float   p_ref_0[3]          = {0.0, 0.0, 0.0};
int16_t chirp_n_repeat      = 10;
int16_t chirp_n_counter     = 0;
int16_t firing_counter      = 0;
int16_t delay_counter       = 0;
bool    multiple_chirp_run  = false;
//====================================================================================================================================
// Function Declaration Section
//====================================================================================================================================
void  init_poles(void);
void  calc_normalization(void);
void  normalize_nu(void);
void  G1G2_oneloop(int ctrl_type);
void  get_act_state_oneloop(void);
void  oneloop_andi_propagate_filters(void);
void  init_filter(void);
void  init_controller_gains(void);
void  reinit_controller(void);
void  float_rates_of_euler_dot_vec(float r[3], float e[3], float edot[3]);
void  float_euler_dot_of_rates_vec(float r[3], float e[3], float edot[3]);
void  err_nd(float err[], float a[], float b[], float k[], int n);
void  err_sum_nd(float err[], float a[], float b[], float k[], float c[], int n);
void  float_vect_diff_euler(float err[3], float a[3], float b[3]);
void  integrate_nd(float dt, float a[], float a_dot[], int n);
void  vect_bound_nd(float vect[], float bound, int n);
void  acc_body_bound(struct FloatVect2* vect, float bound);
float bound_v_from_a_vect(float e_x[], float v_bound, float a_bound, int n);
float bound_v_from_a(float e_x, float v_bound, float a_bound);
void  rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3], struct OneloopStabilizationRef bounds);
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);
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);
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);
void  ec_3rd_att(float y_4d[3], float x_des[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3], struct OneloopStabilizationRef bounds, float fb[3]);
void  ec_3rd_pos(float y_4d[], float x_des[], float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x[], float x_d[], float x_2d[], float k1_e[], float k2_e[], float k3_e[], float x_d_bound, float x_2d_bound, float fb[], int n);
float oneloop_andi_sideslip(void);
void  reshape_wind(void);
void  chirp_pos(float time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[]);
void  chirp_call(bool* chirp_on, bool* chirp_first_call, float* t_0_chirp, float* time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[]);
void  oneloop_axis_effectiveness_calc(void); // FIXME check if used
void  oneloop_andi_bound_disturbance(void); // FIXME add flag to turn on and off
void  oneloop_calc_model_disturbance(bool in_flight);
void  dynFilter_init(struct Oneloop_DynFilt_t *mu, float varepsilon, float sigma);
void  dynFilter_run(struct Oneloop_DynFilt_t *mu, float u_c, float sigma);
//====================================================================================================================================
// Telemetry Section
//====================================================================================================================================
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
// static void send_cf_oneloop(struct transport_tx *trans, struct link_device *dev)
// {
//   float temp_cf_p[5] = {cf.p.model,cf.p.model_filt.o[0],cf.p.feedback,cf.p.feedback_filt.o[0],cf.p.out};
//   float temp_cf_q[5] = {cf.q.model,cf.q.model_filt.o[0],cf.q.feedback,cf.q.feedback_filt.o[0],cf.q.out};
//   float temp_cf_r[5] = {cf.r.model,cf.r.model_filt.o[0],cf.r.feedback,cf.r.feedback_filt.o[0],cf.r.out};
//   float temp_cf_p_dot[5] = {cf.p_dot.model,cf.p_dot.model_filt.lp2.o[0],cf.p_dot.feedback,cf.p_dot.feedback_filt.lp2.o[0],cf.p_dot.out};
//   float temp_cf_q_dot[5] = {cf.q_dot.model,cf.q_dot.model_filt.lp2.o[0],cf.q_dot.feedback,cf.q_dot.feedback_filt.lp2.o[0],cf.q_dot.out};
//   float temp_cf_r_dot[5] = {cf.r_dot.model,cf.r_dot.model_filt.o[0],cf.r_dot.feedback,cf.r_dot.feedback_filt.o[0],cf.r_dot.out};
//   //float temp_cf_ax[5] = {cf.ax.model,cf.ax.model_filt.o[0],cf.ax.feedback,cf.ax.feedback_filt.o[0],cf.ax.out};
//   //float temp_cf_ay[5] = {cf.ay.model,cf.ay.model_filt.o[0],cf.ay.feedback,cf.ay.feedback_filt.o[0],cf.ay.out};
//   float temp_cf_az[5] = {cf.az.model,cf.az.model_filt.o[0],cf.az.feedback,cf.az.feedback_filt.o[0],cf.az.out};
//   pprz_msg_send_COMPLEMENTARY_FILTER(trans, dev, AC_ID, 
//                 5, temp_cf_p,
//                 5, temp_cf_q,
//                 5, temp_cf_r,
//                 5, temp_cf_p_dot,
//                 5, temp_cf_q_dot,
//                 5, temp_cf_r_dot,
//                 5, temp_cf_az);
// }
static void send_wls_v_oneloop(struct transport_tx *trans, struct link_device *dev)
{
  send_wls_v("one", &WLS_one_p, trans, dev);
}
static void send_wls_u_oneloop(struct transport_tx *trans, struct link_device *dev)
{
  send_wls_u("one", &WLS_one_p, trans, dev);
}
static void send_eff_mat_stab_oneloop_andi(struct transport_tx *trans, struct link_device *dev)
{
  float zero = 0.0;
  pprz_msg_send_EFF_MAT_STAB(trans, dev, AC_ID, 
                ANDI_NUM_ACT, EFF_MAT_RW[3],
                ANDI_NUM_ACT, EFF_MAT_RW[4],
                ANDI_NUM_ACT, EFF_MAT_RW[5], 
                      ANDI_NUM_ACT, G2_RW);
}
static void send_eff_mat_guid_oneloop_andi(struct transport_tx *trans, struct link_device *dev)
{
  pprz_msg_send_EFF_MAT_GUID(trans, dev, AC_ID,
                             ANDI_NUM_ACT_TOT, EFF_MAT_RW[0],
                             ANDI_NUM_ACT_TOT, EFF_MAT_RW[1],
                             ANDI_NUM_ACT_TOT, EFF_MAT_RW[2]);
}
static void send_oneloop_andi(struct transport_tx *trans, struct link_device *dev)
{
  float temp_eulers_zxy_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, eulers_zxy_des.psi};
  pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
                              3, temp_eulers_zxy_des,
                              3, oneloop_andi.sta_state.att,
                              3, oneloop_andi.sta_ref.att,
                              3, oneloop_andi.sta_state.att_d,
                              3, oneloop_andi.sta_ref.att_d,
                              3, oneloop_andi.sta_state.att_2d,
                              3, oneloop_andi.sta_ref.att_2d,
                              3, oneloop_andi.sta_ref.att_3d,
                              ANDI_NUM_ACT, actuator_state_1l);
}
static void send_guidance_oneloop_andi(struct transport_tx *trans, struct link_device *dev)
{
  pprz_msg_send_GUIDANCE(trans, dev, AC_ID,
                         &oneloop_andi.gui_ref.pos[0],
                         &oneloop_andi.gui_ref.pos[1],
                         &oneloop_andi.gui_ref.pos[2],
                         &oneloop_andi.gui_state.pos[0],
                         &oneloop_andi.gui_state.pos[1],
                         &oneloop_andi.gui_state.pos[2],
                         &oneloop_andi.gui_ref.vel[0],
                         &oneloop_andi.gui_ref.vel[1],
                         &oneloop_andi.gui_ref.vel[2],
                         &oneloop_andi.gui_state.vel[0],
                         &oneloop_andi.gui_state.vel[1],
                         &oneloop_andi.gui_state.vel[2],
                         &oneloop_andi.gui_ref.acc[0],
                         &oneloop_andi.gui_ref.acc[1],
                         &oneloop_andi.gui_ref.acc[2],
                         &oneloop_andi.gui_state.acc[0],
                         &oneloop_andi.gui_state.acc[1],
                         &oneloop_andi.gui_state.acc[2],
                         &oneloop_andi.gui_ref.jer[0],
                         &oneloop_andi.gui_ref.jer[1],
                         &oneloop_andi.gui_ref.jer[2]);
}
static void debug_vect(struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize)
{
  pprz_msg_send_DEBUG_VECT(trans, dev, AC_ID,
                           strlen(name), name,
                           datasize, data);
}
static void send_oneloop_debug(struct transport_tx *trans, struct link_device *dev)
{
  float temp_debug_vect[9];
  //temp_debug_vect[0] = oneloop_andi_model[0];
  //temp_debug_vect[1] = oneloop_andi_model[1];
  //temp_debug_vect[2] = oneloop_andi_model[2];
  //temp_debug_vect[3] = oneloop_andi_model[3];
  //temp_debug_vect[4] = oneloop_andi_model[4];
  //temp_debug_vect[5] = oneloop_andi_model[5];
  temp_debug_vect[0] = temp_P_error[0];
  temp_debug_vect[1] = temp_P_error[1];
  temp_debug_vect[2] = temp_P_error[2];
  temp_debug_vect[3] = temp_D_error[0];
  temp_debug_vect[4] = temp_D_error[1];
  temp_debug_vect[5] = temp_D_error[2];
  temp_debug_vect[6] = temp_I_error[0];
  temp_debug_vect[7] = temp_I_error[1];
  temp_debug_vect[8] = temp_I_error[2];  
  debug_vect(trans, dev, "APF", temp_debug_vect, 9);
}
#endif
//====================================================================================================================================
// Functions Definition Section
//====================================================================================================================================
// General Mathematical Functions
static float positive_non_zero(float input)
{
  if (input < FLT_EPSILON)
  {
    input = 0.00001;
  }
  return input;
}
void err_nd(float err[], float a[], float b[], float k[], int n)
{
  int8_t i;
  for (i = 0; i < n; i++) {
    err[i] = k[i] * (a[i] - b[i]);
  }
}
void err_sum_nd(float err[], float a[], float b[], float k[], float c[], int n)
{
  int8_t i;
  for (i = 0; i < n; i++) {
    err[i] = k[i] * (a[i] - b[i]);
    err[i] += c[i];
  }
}
void float_vect_diff_euler(float err[3], float a[3], float b[3])
{
  err[0] = (a[0] - b[0]);
  err[1] = (a[1] - b[1]);
  err[2] = (a[2] - b[2]);
  NormRadAngle(err[2]);
}
void integrate_nd(float dt, float a[], float a_dot[], int n)
{
  int8_t i;
  for (i = 0; i < n; i++) {
    a[i] = a[i] + dt * a_dot[i];
  }
}
void vect_bound_nd(float vect[], float bound, int n) {
  float norm = float_vect_norm(vect,n);
  norm = positive_non_zero(norm);
  if((norm-bound) > FLT_EPSILON) {
    float scale = bound/norm;
    int8_t i;
    for(i = 0; i < n; i++) {
      vect[i] *= scale;
    }
  }
}
//====================================================================================================================================
// Reference Model Gain Design Functions
static float k_rm_1_3_f(float omega_n, float zeta, float p1) {
    omega_n = positive_non_zero(omega_n);
    zeta    = positive_non_zero(zeta);
    p1      = positive_non_zero(p1);
    return (omega_n * omega_n * p1) / (omega_n * omega_n + omega_n * p1 * zeta * 2.0);
}
static float k_rm_2_3_f(float omega_n, float zeta, float p1) {
    omega_n = positive_non_zero(omega_n);
    zeta    = positive_non_zero(zeta);
    p1      = positive_non_zero(p1);
    return (omega_n * omega_n + omega_n * p1 * zeta * 2.0) / (p1 + omega_n * zeta * 2.0);
}
static float k_rm_3_3_f(float omega_n, float zeta, float p1) {
    omega_n = positive_non_zero(omega_n);
    zeta    = positive_non_zero(zeta);
    p1      = positive_non_zero(p1);
    return p1 + omega_n * zeta * 2.0;
}
 
// 3-axis position PID controller
 
static void Pos_PID(float x_des[3], float x[3],float x_dot[3], float k_P, float k_I, float k_D,float a[3])
{
    static float prev_pos[3]  = {0.0f, 0.0f, 0.0f};
    static float integral[3]  = {0.0f, 0.0f, 0.0f};
    for (int i = 0; i < 3; i++)
    {
        float err = x_des[i] - x[i];
        integral[i] += err * dt_1l;
        // Optional anti-windup
        const float I_MAX = 0.4f;
        if (integral[i] >  I_MAX) integral[i] =  I_MAX;
        if (integral[i] < -I_MAX) integral[i] = -I_MAX;
#ifdef USE_PID_NUMDIFF_DERIVATIVE
        // Numerical derivative  
        //static float prev_err[3]  = {0.0f, 0.0f, 0.0f};
        //float derivative = (err - prev_err[i]) / dt_1l; 
        //prev_err[i] = err;
        float derivative_nd = (x[i] - prev_pos[i]) / dt_1l;  
        prev_pos[i] = x[i];     
        update_butterworth_2_low_pass(&filt_veloc[i], derivative_nd);   
        float derivative = -filt_veloc[i].o[0];  
#else
        float derivative = -x_dot[i];
#endif
 
        // Temp debug
        temp_P_error[i] = k_P * err;
        temp_D_error[i] = k_D * derivative;
        temp_I_error[i] = k_I * integral[i];
        a[i] = k_P * err+ k_I * integral[i]+ k_D * derivative;
    }
    vect_bound_nd(a,0.5*9.81,3);
}
static void shape_vector(float a[3]){
  a[2] += -9.81;
  vect_bound_nd(a,1.0,3);
}
static void eul_of_acc(float a[3], float psi){
  float phi_des;
  float theta_des;
 
  float spsi = sinf(psi);
  float cpsi = cosf(psi);
  //printf("psi:%f \n",DegOfRad(psi));
  float aX = cpsi*a[0]+spsi*a[1];
  float aY =-spsi*a[0]+cpsi*a[1];
  //printf("[aX,aY]=[%f,%f] \n",aX,aY);
  theta_des = asinf(-aX);
  float ctheta_des = cosf(theta_des);
  if (fabs(ctheta_des) < FLT_EPSILON){ctheta_des = FLT_EPSILON;}
  phi_des   = asinf(aY/ctheta_des);
  //printf("[phi_des,theta_des]=[%f,%f] \n",DegOfRad(phi_des),DegOfRad(theta_des));
  BoundAbs(phi_des, M_PI_6); // Limit to 30 deg
  BoundAbs(theta_des, M_PI_6); // Limit to 30 deg
  eulers_zxy_des.phi   = phi_des;
  eulers_zxy_des.theta = theta_des;
}
//====================================================================================================================================
// Attitude Conversion Functions
// void float_rates_of_euler_dot_vec(float r[3], float e[3], float edot[3])
// {
//   float sphi = sinf(e[0]);
//   float cphi = cosf(e[0]);
//   float stheta = sinf(e[1]);
//   float ctheta = cosf(e[1]);
//   r[0] = edot[0] - stheta * edot[2];
//   r[1] = cphi * edot[1] + sphi * ctheta * edot[2];
//   r[2] = -sphi * edot[1] + cphi * ctheta * edot[2];
// }
 
void float_rates_of_euler_dot_vec(float r[3], float e[3], float edot[3])
{
    float sphi   = sinf(e[0]);
    float cphi   = cosf(e[0]);
    float stheta = sinf(e[1]);
    float ctheta = cosf(e[1]);
    r[0] =  ctheta * edot[0] - stheta * cphi * edot[2]; 
    r[1] =  edot[1] + sphi * edot[2];
    r[2] =  stheta * edot[0] + ctheta *cphi * edot[2]; 
}
 
// void float_euler_dot_of_rates_vec(float r[3], float e[3], float edot[3])
// {
//   float sphi = sinf(e[0]);
//   float cphi = cosf(e[0]);
//   float stheta = sinf(e[1]);
//   float ctheta = cosf(e[1]);
//   if (fabs(ctheta) < FLT_EPSILON)
//   {
//     ctheta = FLT_EPSILON;
//   }
//   edot[0] = r[0] + sphi * stheta / ctheta * r[1] + cphi * stheta / ctheta * r[2];
//   edot[1] = cphi * r[1] - sphi * r[2];
//   edot[2] = sphi / ctheta * r[1] + cphi / ctheta * r[2];
// }
 
void float_euler_dot_of_rates_vec(float r[3], float e[3], float edot[3])
{
  float sphi = sinf(e[0]);
  float cphi = cosf(e[0]);
  float stheta = sinf(e[1]);
  float ctheta = cosf(e[1]);
  float spsi = sinf(e[2]);
   float cpsi = cosf(e[2]);
 
  if (fabs(ctheta) < FLT_EPSILON)
  {
    ctheta = FLT_EPSILON;
  }
  edot[0] = r[0] + spsi * stheta / ctheta * r[1] + cpsi * stheta / ctheta * r[2];
  edot[1] = cphi * r[1] - sphi * r[2];
  edot[2] = sphi * r[1] + cphi * r[2];
}
//====================================================================================================================================
// Bounding Functions
void acc_body_bound(struct FloatVect2 *vect, float bound)
{
  int n = 2;
  float v[2] = {vect->x, vect->y};
  float sign_v0 = (v[0] > 0.f) ? 1.f : (v[0] < 0.f) ? -1.f                                               : 0.f;
  float sign_v1 = (v[1] > 0.f) ? 1.f : (v[1] < 0.f) ? -1.f                                               : 0.f;
  float norm = float_vect_norm(v, n);
  v[0] = fabsf(v[0]);
  v[1] = fabsf(v[1]);
  norm = positive_non_zero(norm);
  if ((norm - bound) > FLT_EPSILON)
  {
    v[0] = Min(v[0], bound);
    float acc_b_y_2 = bound * bound - v[0] * v[0];
    acc_b_y_2 = positive_non_zero(acc_b_y_2);
    v[1] = sqrtf(acc_b_y_2);
  }
  vect->x = sign_v0 * v[0];
  vect->y = sign_v1 * v[1];
}
float bound_v_from_a_vect(float e_x[], float v_bound, float a_bound, int n)
{
  float norm = float_vect_norm(e_x, n);
  norm = fmaxf(norm, 1.0);
  float v_bound_a = sqrtf(fabs(2.0 * a_bound * norm * xdot_lim_sf));
  return fminf(v_bound, v_bound_a);
}
float bound_v_from_a(float e_x, float v_bound, float a_bound)
{
  float norm;
  norm = fabsf(e_x);
  //norm = fmaxf(norm, 1.0);
  float v_bound_a  = sqrtf(fabs(2.0 * a_bound * norm * xdot_lim_sf));
  return fminf(v_bound, v_bound_a);
}
//====================================================================================================================================
// Reference Model Functions
void rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3], struct OneloopStabilizationRef bounds){
  
  float e_x[3];                // Attitude Error
  float x_d_fw[3];             // Forward Signal Euler Dot
  float x_d_fw_rates[3];       // Forward Signal Angular Rates
  float x_2d_fw[3];            // Forward Signal Angular Acceleration
  float x_3d_fw[3];            // Forward Signal Angular Jerk
  float bounds_att_d[3];       // Bounds on Angular Rate (SUVAT based)
  float x_d_eul_ref[3];        // Euler angle Reference
  // Attitude error --------------------------------------------------------------------------------------------------
  float_vect_diff_euler(e_x, x_des, x_ref);                    // Calculate Attitude Error
  x_d_fw[0] = e_x[0] * k1_rm[0];                               // Calculate Forward Signal Euler Dot
  x_d_fw[1] = e_x[1] * k1_rm[1];                               // Calculate Forward Signal Euler Dot
  x_d_fw[2] = e_x[2] * k1_rm[2];                               // Calculate Forward Signal Euler Dot
  float_rates_of_euler_dot_vec(x_d_fw_rates, x_ref, x_d_fw);   // Convert Euler Dot to Angular Rates
  bounds_att_d[0] =  bound_v_from_a(e_x[0], bounds.att_d[0], bounds.att_2d[0]);
  bounds_att_d[1] =  bound_v_from_a(e_x[1], bounds.att_d[1], bounds.att_2d[1]);
  bounds_att_d[2] =  bound_v_from_a(e_x[2], bounds.att_d[2], bounds.att_2d[2]);
  BoundAbs(x_d_fw_rates[0], bounds_att_d[0]);
  BoundAbs(x_d_fw_rates[1], bounds_att_d[1]);
  BoundAbs(x_d_fw_rates[2], bounds_att_d[2]);
  // Angular Rate error ----------------------------------------------------------------------------------------------
  err_nd(x_2d_fw, x_d_fw_rates, x_d_ref, k2_rm, 3);
  BoundAbs(x_2d_fw[0], bounds.att_2d[0]);
  BoundAbs(x_2d_fw[1], bounds.att_2d[1]);
  BoundAbs(x_2d_fw[2], bounds.att_2d[2]);
  // Angular Acceleration error -----------------------------------------
  err_nd(x_3d_fw, x_2d_fw, x_2d_ref, k3_rm, 3);
  BoundAbs(x_3d_fw[0], bounds.att_3d[0]);
  BoundAbs(x_3d_fw[1], bounds.att_3d[1]);
  BoundAbs(x_3d_fw[2], bounds.att_3d[2]);
  // Angular Jerk Reference ---------------------------------------------
  float_vect_copy(x_3d_ref, x_3d_fw, 3);
  if (ow_psi){x_3d_ref[2] = psi_overwrite[3];}
  // Angular Acceleration Reference -------------------------------------
  integrate_nd(dt, x_2d_ref, x_3d_ref, 3);
  if (ow_psi){x_2d_ref[2] = psi_overwrite[2];}
  // Angular Rate Reference ---------------------------------------------
  integrate_nd(dt, x_d_ref, x_2d_ref, 3);
  if (ow_psi){x_d_ref[2] = psi_overwrite[1];}
  // Attitude Reference ------------------------------------------------
  float_euler_dot_of_rates_vec(x_d_ref, x_ref, x_d_eul_ref);
  integrate_nd(dt, x_ref, x_d_eul_ref, 3);
  if (ow_psi){x_ref[2] = psi_overwrite[0];}
  NormRadAngle(x_ref[2]);
}
 
 
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)
{
  float e_x[n];
  float e_x_d[n];
  float e_x_2d[n];
  err_nd(e_x, x_des, x_ref, k1_rm, n);
  float max_x_d = bound_v_from_a_vect(e_x, x_d_bound, x_2d_bound, n);
  vect_bound_nd(e_x, max_x_d, n);
  err_nd(e_x_d, e_x, x_d_ref, k2_rm, n);
  vect_bound_nd(e_x_d, x_2d_bound, n);
  err_nd(e_x_2d, e_x_d, x_2d_ref, k3_rm, n);
  float_vect_copy(x_3d_ref, e_x_2d, n);
  vect_bound_nd(x_3d_ref, x_3d_bound, n);
  integrate_nd(dt, x_2d_ref, x_3d_ref, n);
  integrate_nd(dt, x_d_ref, x_2d_ref, n);
  integrate_nd(dt, x_ref, x_d_ref, n);
}
 
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)
{
  float e_x_d[n];
  float e_x_2d[n];
  err_nd(e_x_d, x_d_des, x_d_ref, k2_rm, n);
  vect_bound_nd(e_x_d, x_2d_bound, n);
  err_nd(e_x_2d, e_x_d, x_2d_ref, k3_rm, n);
  float_vect_copy(x_3d_ref, e_x_2d, n);
  vect_bound_nd(x_3d_ref, x_3d_bound, n);
  integrate_nd(dt, x_2d_ref, x_3d_ref, n);
  integrate_nd(dt, x_d_ref, x_2d_ref, n);
}
 
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)
{
  float e_x_2d[n];
  err_nd(e_x_2d, x_2d_des, x_2d_ref, k3_rm, n);
  float_vect_copy(x_3d_ref, e_x_2d, n);
  vect_bound_nd(x_3d_ref, x_3d_bound, n);
  integrate_nd(dt, x_2d_ref, x_3d_ref, n);
}
//====================================================================================================================================
// Error Controller Functions
void ec_3rd_pos(float y_4d[], float x_des[], float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x[], float x_d[], float x_2d[], float k1_e[], float k2_e[], float k3_e[], float x_d_bound, float x_2d_bound, float fb[], int n)
{
  float e_x_d[n];
  float e_x_2d[n];
  float e_x_des[n];
  float_vect_diff(e_x_des, x_des, x, n);
  float max_x_d = bound_v_from_a_vect(e_x_des, x_d_bound * ec_headroom, x_2d_bound * ec_headroom, n);
  err_sum_nd(e_x_d, x_ref, x, k1_e, x_d_ref, n);
  vect_bound_nd(e_x_d, max_x_d, n);
  err_sum_nd(e_x_2d, e_x_d, x_d, k2_e, x_2d_ref, n);
  vect_bound_nd(e_x_2d, x_2d_bound * ec_headroom, n);
  // Calculate and bound distrubance --------------------
  float dist[3];
  float_vect_diff(dist, x_2d, fb, 3);
  err_sum_nd(y_4d, e_x_2d, dist, k3_e, x_3d_ref, n);
}
 
void ec_3rd_att(float y_4d[3], float x_des[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3], struct OneloopStabilizationRef bounds, float fb[3])
{
  float e_x[3];              // Attitude Error on Reference
  float e_x_des[3];          // Attitude Error on Desired
  float x_d_fw[3];           // Forward Signal Euler Dot
  float x_d_fw_rates[3];     // Forward Signal Angular Rates
  float x_d_fw_rates_rm[3];  // Forward Signal Angular Rates plus RM
  float x_2d_fw[3];          // Forward Signal Angular Acceleration
  float bounds_att_d[3];
  // Attitude Error and Heading conversion --------------------------------
  float_vect_diff_euler(e_x,x_ref,x);                    // Calculate the Attitude Error
  x_d_fw[0] = e_x[0]*k1_e[0];                            // Calculate Forward Signal Euler Dot
  x_d_fw[1] = e_x[1]*k1_e[1];                            // Calculate Forward Signal Euler Dot
  x_d_fw[2] = e_x[2]*k1_e[2];                            // Calculate Forward Signal Euler Dot
  float_rates_of_euler_dot_vec(x_d_fw_rates, x, x_d_fw); // Euler dot to rates conversion
  float_vect_sum(x_d_fw_rates_rm, x_d_ref, x_d_fw_rates, 3);// Add body rates reference signal
  float_vect_diff_euler(e_x_des, x_des, x);
  bounds_att_d[0] = bound_v_from_a(e_x_des[0], bounds.att_d[0] * ec_headroom, bounds.att_2d[0] * ec_headroom);
  bounds_att_d[1] = bound_v_from_a(e_x_des[1], bounds.att_d[1] * ec_headroom, bounds.att_2d[1] * ec_headroom);
  bounds_att_d[2] = bound_v_from_a(e_x_des[2], bounds.att_d[2] * ec_headroom, bounds.att_2d[2] * ec_headroom);
  BoundAbs(x_d_fw_rates_rm[0], bounds_att_d[0]);
  BoundAbs(x_d_fw_rates_rm[1], bounds_att_d[1]);
  BoundAbs(x_d_fw_rates_rm[2], bounds_att_d[2]);
  // Angular Rate Error ---------------------------------------------------
  x_2d_fw[0] = (x_d_fw_rates_rm[0] - x_d[0]) * k2_e[0] + x_2d_ref[0];
  x_2d_fw[1] = (x_d_fw_rates_rm[1] - x_d[1]) * k2_e[1] + x_2d_ref[1];
  x_2d_fw[2] = (x_d_fw_rates_rm[2] - x_d[2]) * k2_e[2] + x_2d_ref[2];
  BoundAbs(x_2d_fw[0], bounds.att_2d[0] * ec_headroom);
  BoundAbs(x_2d_fw[1], bounds.att_2d[1] * ec_headroom);
  BoundAbs(x_2d_fw[2], bounds.att_2d[2] * ec_headroom);
  //  Calculate and bound distrubance --------------------------------------
  float dist[3];
  float_vect_diff(dist, x_2d, fb, 3); // The Disturbance is THe difference between the measurment and the Model
  //BoundAbs(dist[2], oneloop_andi_yaw_dist_limit); //FIXME UNCOMMENT ME TO HAVE MAX YAW CONTROL EFFORT
  // Angular Acceleration Error -------------------------------------------
  err_sum_nd(y_4d, x_2d_fw, dist, k3_e, x_3d_ref, 3);
}
 
static float w_approx(float p1, float p2, float p3, float rm_k)
{
  p1 = positive_non_zero(p1);
  p2 = positive_non_zero(p2);
  p3 = positive_non_zero(p3);
  rm_k = positive_non_zero(rm_k);
  float tao = (p1 * p2 + p1 * p3 + p2 * p3) / (p1 * p2 * p3) / (rm_k);
  tao = positive_non_zero(tao);
  return 1.0 / tao;
}
 
void init_poles(void)
{
 
  // Attitude Controller Poles----------------------------------------------------------
  float slow_pole = 22.0; // Pole of the slowest dynamics used in the attitude controller
 
  p_att_e.omega_n = slow_pole / 3.0;
  p_att_e.zeta = 1.0;
  p_att_e.p3 = p_att_e.omega_n;
 
  p_roll_e.omega_n = slow_pole/3.0;
  p_roll_e.zeta    = 1.0;
  p_roll_e.p3      = p_roll_e.omega_n;
 
  p_att_rm.omega_n = p_att_e.omega_n * 0.8;
  p_att_rm.zeta = 1.0;
  p_att_rm.p3 = p_att_rm.omega_n;
 
  p_head_e.omega_n = slow_pole / 3.0;
  p_head_e.zeta = 1.0;
  p_head_e.p3 = p_head_e.omega_n;
 
  p_head_rm.omega_n = p_head_e.omega_n * 0.8;
  p_head_rm.zeta = 1.0;
  p_head_rm.p3 = p_head_rm.omega_n;
 
  act_dynamics[COMMAND_ROLL] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0);
  act_dynamics[COMMAND_PITCH] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0);
 
  // Position Controller Poles----------------------------------------------------------
  slow_pole = act_dynamics[COMMAND_ROLL]; // Pole of the slowest dynamics used in the position controller
 
  p_pos_e.omega_n = 1.19; // slow_pole/3.0;
  p_pos_e.zeta = 0.5;
  p_pos_e.p3 = p_pos_e.omega_n;
 
  p_pos_rm.omega_n = p_pos_e.omega_n * 0.8;
  p_pos_rm.zeta = 0.5;
  p_pos_rm.p3 = p_pos_rm.omega_n;
 
  p_alt_e.omega_n = 1.19; // slow_pole/3.0*2.0;
  p_alt_e.zeta = 0.5;
  p_alt_e.p3 = p_alt_e.omega_n;
 
  p_alt_rm.omega_n = p_alt_e.omega_n * 0.8;
  p_alt_rm.zeta = 0.5;
  p_alt_rm.p3 = p_alt_rm.omega_n;
}
 
void init_controller_gains(void)
{
  /*Register a variable from nav_hybrid. Should be improved when nav hybrid is final.*/
  float max_wind = 20.0;
  max_v_nav = nav_max_speed + max_wind;
  max_a_nav = nav_max_acceleration_sp;
  /*Some calculations in case new poles have been specified*/
  p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta;
  p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta;
  p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta;
  p_head_rm.p3 = p_head_rm.omega_n * p_head_rm.zeta;
 
  //--ANDI Controller gains --------------------------------------------------------------------------------
  /*Attitude Loop*/
  k_att_e.k1[0] = k_rm_1_3_f(p_roll_e.omega_n, p_roll_e.zeta, p_roll_e.p3); // k_rm_1_3_f(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3);
  k_att_e.k2[0] = k_rm_2_3_f(p_roll_e.omega_n, p_roll_e.zeta, p_roll_e.p3); // k_rm_2_3_f(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3);
  k_att_e.k3[0] = k_rm_3_3_f(p_roll_e.omega_n, p_roll_e.zeta, p_roll_e.p3); // k_rm_3_3_f(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3);
 
  k_att_e.k1[1] = k_rm_1_3_f(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3); // k_att_e.k1[0];
  k_att_e.k2[1] = k_rm_2_3_f(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3); // k_att_e.k2[0];
  k_att_e.k3[1] = k_rm_3_3_f(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3); // k_att_e.k3[0];
 
  k_att_rm.k1[0] = k_rm_1_3_f(p_att_rm.omega_n, p_att_rm.zeta, p_att_rm.p3);
  k_att_rm.k2[0] = k_rm_2_3_f(p_att_rm.omega_n, p_att_rm.zeta, p_att_rm.p3);
  k_att_rm.k3[0] = k_rm_3_3_f(p_att_rm.omega_n, p_att_rm.zeta, p_att_rm.p3);
  k_att_rm.k1[1] = k_att_rm.k1[0];
  k_att_rm.k2[1] = k_att_rm.k2[0];
  k_att_rm.k3[1] = k_att_rm.k3[0];
  /*Heading Loop NAV*/
  k_att_e.k1[2] = k_rm_1_3_f(p_head_e.omega_n, p_head_e.zeta, p_head_e.p3);
  k_att_e.k2[2] = k_rm_2_3_f(p_head_e.omega_n, p_head_e.zeta, p_head_e.p3);
  k_att_e.k3[2] = k_rm_3_3_f(p_head_e.omega_n, p_head_e.zeta, p_head_e.p3);
 
  k_att_rm.k1[2] = k_rm_1_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3);
  k_att_rm.k2[2] = k_rm_2_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3);
  k_att_rm.k3[2] = k_rm_3_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3);
 
  // Temporary overrirde of gains
  k_att_e.k1[0]   = 4.19;
  k_att_e.k2[0]   = 10.01;
  k_att_e.k3[0]   = 22.0;
  k_att_e.k1[1]   = k_att_e.k1[0];
  k_att_e.k2[1]   = k_att_e.k2[0];
  k_att_e.k3[1]   = k_att_e.k3[0];
 
  //k_att_rm.k1[0]  = 4.19;
  //k_att_rm.k2[0]  = 10.01;
  //k_att_rm.k3[0]  = 22.0;
//
  //k_pos_e.k1[0]  = 0.6539;
  //k_pos_e.k2[0]  = 1.795;
  //k_pos_e.k3[0]  = XXX;
 
  // Print INNERLOOP ANDI controller gains
  // printf("Attitude RM poles, omega_n: %f, zeta: %f, p3: %f\n", p_att_rm.omega_n, p_att_rm.zeta, p_att_rm.p3);
  // printf("Attitude EC poles, omega_n: %f, zeta: %f, p3: %f\n", p_att_e.omega_n, p_att_e.zeta, p_att_e.p3);
  // printf("Heading  RM poles, omega_n: %f, zeta: %f, p3: %f\n", p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3);
  // printf("Heading  EC poles, omega_n: %f, zeta: %f, p3: %f\n", p_head_e.omega_n, p_head_e.zeta, p_head_e.p3);
  // printf("ANDI Attitude RM Gains: %f %f %f\n", k_att_rm.k1[0], k_att_rm.k2[0], k_att_rm.k3[0]);
  // printf("ANDI Attitude EC Gains: %f %f %f\n", k_att_e.k1[0], k_att_e.k2[0], k_att_e.k3[0]);
  // printf("ANDI Heading  RM Gains: %f %f %f\n", k_att_rm.k1[2], k_att_rm.k2[2], k_att_rm.k3[2]);
  // printf("ANDI Heading  EC Gains: %f %f %f\n", k_att_e.k1[2], k_att_e.k2[2], k_att_e.k3[2]);
 
  /*Position Loop*/
  k_pos_e.k1[0] = k1_NE_tune;//k_rm_1_3_f(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); // 0.595;//
  k_pos_e.k2[0] = k2_NE_tune;//k_rm_2_3_f(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); // 1.190;//
  k_pos_e.k3[0] = k_rm_3_3_f(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); // 2.380;//
  k_pos_e.k1[1] = k_pos_e.k1[0];
  k_pos_e.k2[1] = k_pos_e.k2[0];
  k_pos_e.k3[1] = k_pos_e.k3[0];
 
  k_pos_rm.k1[0] = k_rm_1_3_f(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); // 0.595;
  k_pos_rm.k2[0] = k_rm_2_3_f(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); // 1.190;
  k_pos_rm.k3[0] = k_rm_3_3_f(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); // 2.380;
  k_pos_rm.k1[1] = k_pos_rm.k1[0];
  k_pos_rm.k2[1] = k_pos_rm.k2[0];
  k_pos_rm.k3[1] = k_pos_rm.k3[0];
  nav_hybrid_pos_gain = k_pos_rm.k1[0];
  nav_hybrid_max_bank = ONELOOP_ANDI_MAX_BANK;
 
  /*Altitude Loop*/
  k_pos_e.k1[2] = k_rm_1_3_f(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); // 0.595;
  k_pos_e.k2[2] = k_rm_2_3_f(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); // 1.190;
  k_pos_e.k3[2] = 22;                                                    // k_rm_3_3_f(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); //2.380;
 
  k_pos_rm.k1[2] = k_rm_1_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); // 0.595;
  k_pos_rm.k2[2] = k_rm_2_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); // 1.190;
  k_pos_rm.k3[2] = k_rm_3_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); // 2.380;
 
  // Print OUTERLOOP ANDI controller gains
  //printf("Position NE RM poles, omega_n: %f, zeta: %f, p3: %f\n", p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3);
  //printf("Position NE EC poles, omega_n: %f, zeta: %f, p3: %f\n", p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3);
  //printf("Position D  RM poles, omega_n: %f, zeta: %f, p3: %f\n", p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3);
  //printf("Position D  EC poles, omega_n: %f, zeta: %f, p3: %f\n", p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3);
  //printf("Position N  RM Gains: %f %f %f\n", k_pos_rm.k1[0], k_pos_rm.k2[0], k_pos_rm.k3[0]);
  //printf("Position N  EC Gains: %f %f %f\n", k_pos_e.k1[0], k_pos_e.k2[0], k_pos_e.k3[0]);
  //printf("Position D  RM Gains: %f %f %f\n", k_pos_rm.k1[2], k_pos_rm.k2[2], k_pos_rm.k3[2]);
  //printf("Position D  EC Gains: %f %f %f\n", k_pos_e.k1[2], k_pos_e.k2[2], k_pos_e.k3[2]);
  //--INDI Controller gains --------------------------------------------------------------------------------
    /*Attitude Loop*/
  k_att_e_indi.k1[0]  = k_att_e.k1[0];
  k_att_e_indi.k2[0]  = k_att_e.k2[0];
  k_att_e_indi.k3[0]  = 1.0;
  k_att_e_indi.k1[1]  = k_att_e_indi.k1[0]; 
  k_att_e_indi.k2[1]  = k_att_e_indi.k2[0]; 
  k_att_e_indi.k3[1]  = k_att_e_indi.k3[0]; 
 
  /*Heading Loop NAV*/
  k_att_e_indi.k1[2]  = k_att_e.k1[2];
  k_att_e_indi.k2[2]  = k_att_e.k2[2];
  k_att_e_indi.k3[2]  = 1.0;
  
  // Print INDI INNERLOOP controller gains
  // printf("INDI Attitude EC gains: %f %f %f\n", k_att_e_indi.k1[0], k_att_e_indi.k2[0], k_att_e_indi.k3[0]);
  // printf("INDI Heading  EC gains: %f %f %f\n", k_att_e_indi.k1[2], k_att_e_indi.k2[2], k_att_e_indi.k3[2]);
 
  /*Position Loop*/
  k_pos_e_indi.k1[0]  = k_pos_e.k1[0];
  k_pos_e_indi.k2[0]  = k_pos_e.k2[0];
  k_pos_e_indi.k3[0]  = 1.0;
  k_pos_e_indi.k1[1]  = k_pos_e_indi.k1[0];  
  k_pos_e_indi.k2[1]  = k_pos_e_indi.k2[0];  
  k_pos_e_indi.k3[1]  = k_pos_e_indi.k3[0]; 
 
  /*Altitude Loop*/
  k_pos_e_indi.k1[2]  = k_pos_e.k1[2];
  k_pos_e_indi.k2[2]  = k_pos_e.k2[2];
  k_pos_e_indi.k3[2]  = 1.0;
  
  // Print INDI OUTERLOOP controller gains
  // printf("INDI Position NE EC gains: %f %f %f\n", k_pos_e_indi.k1[0], k_pos_e_indi.k2[0], k_pos_e_indi.k3[0]);
  // printf("INDI Position D  EC gains: %f %f %f\n", k_pos_e_indi.k1[2], k_pos_e_indi.k2[2], k_pos_e_indi.k3[2]);
 
  //------------------------------------------------------------------------------------------
  /*Approximated Dynamics*/
  act_dynamics[COMMAND_ROLL] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0);
  act_dynamics[COMMAND_PITCH] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0);
  // printf("Act Dynamics: %f %f\n", act_dynamics[COMMAND_ROLL], act_dynamics[COMMAND_PITCH]);
}
// -----------------------------------------------------------------------------------------
// Filter Functions ------------------------------------------------------------------------
// -----------------------------------------------------------------------------------------
 
static inline void init_filter_on_type(struct LP_t *filter, float x0)
{
  switch (filter->filter_type)
  {
  case LOWPASS_1:
    init_first_order_low_pass(&filter->meas_filt.lp1, filter->tau, 1.0 / PERIODIC_FREQUENCY, x0);
    break;
  case BUTTERWORTH_2:
    init_butterworth_2_low_pass(&filter->meas_filt.bw2, filter->tau, 1.0 / PERIODIC_FREQUENCY, x0);
    break;
  case BUTTERWORTH_4:
    init_butterworth_4_low_pass(&filter->meas_filt.bw4, filter->tau, 1.0 / PERIODIC_FREQUENCY, x0);
    break;
  case NOTCH:
    notch_filter_init(&filter->meas_filt.notch, filter->freq, filter->bandwidth, PERIODIC_FREQUENCY);
    break;
  default:
    // Handle unexpected filter type
    break;
  }
}
static inline void update_filter_on_type(struct LP_t *filter, float input)
{
  // filter->meas_prev = filter->meas;
  // filter->meas = input;
  switch (filter->filter_type)
  {
  case LOWPASS_1:
    update_first_order_low_pass(&filter->meas_filt.lp1, input);
    filter->out = filter->meas_filt.lp1.last_out;
    break;
  case BUTTERWORTH_2:
    update_butterworth_2_low_pass(&filter->meas_filt.bw2, input);
    filter->out = filter->meas_filt.bw2.o[0];
    break;
  case BUTTERWORTH_4:
    update_butterworth_4_low_pass(&filter->meas_filt.bw4, input);
    filter->out = filter->meas_filt.bw4.lp2.o[0];
    break;
  case NOTCH:
  {
    notch_filter_update(&filter->meas_filt.notch, &input, &filter->out);
    break;
  }
  default:
    filter->out = 0.0;
    break;
    // Handle unexpected filter type
  }
}
static inline void init_LP(struct LP_t *LP, float fc)
{
  LP->freq = fc;
  LP->freq_set = fc;
  LP->tau = 1 / (2 * M_PI * LP->freq);
#ifdef USE_LP1
  LP->filter_type = LOWPASS_1;
#elif defined(USE_BW2)
  LP->filter_type = BUTTERWORTH_2;
#elif defined(USE_YAW_LP4)
  LP->filter_type = BUTTERWORTH_4;
#endif
  init_filter_on_type(LP, 0.0);
  // init_first_order_low_pass(&LP->meas_filt, LP->tau, 1.0 / PERIODIC_FREQUENCY, 0.0);
  LP->meas = 0.0;
  LP->meas_prev = 0.0;
  LP->out = 0.0;
}
static inline void reinit_LP_synchronous(struct LP_t *LP, struct LP_t *f, bool reinit)
{
  if (LP->freq != LP->freq_set || reinit)
  {
    LP->freq = LP->freq_set;
    LP->tau = 1 / (2 * M_PI * LP->freq);
    init_filter_on_type(LP, LP->out);
    init_filter_on_type(f, f->out);
  }
}
static inline void reinit_LP(struct LP_t *LP, bool reinit)
{
  if (LP->freq != LP->freq_set || reinit)
  {
    LP->freq = LP->freq_set;
    LP->tau = 1 / (2 * M_PI * LP->freq);
    init_filter_on_type(LP, LP->out);
  }
}
 
static inline void init_all_LP(void)
{
  init_LP(&LP.ax, 2.0); // oneloop_andi_filt_cutoff_a
  init_LP(&LP.ay, 2.0); // oneloop_andi_filt_cutoff_a
  init_LP(&LP.az, 2.0); // oneloop_andi_filt_cutoff_a
  init_LP(&LP.p_dot, 2.0);
  init_LP(&LP.q_dot, 2.0);
  init_LP(&LP.r_dot, 2.0);
  init_LP(&LP.p, 15.0); // oneloop_andi_filt_cutoff_p
  init_LP(&LP.q, 15.0); // oneloop_andi_filt_cutoff_q
  init_LP(&LP.r, 15.0); // oneloop_andi_filt_cutoff_r
 
  init_LP(&oneloop_andi_model_filt.ax, 2.0); // oneloop_andi_filt_cutoff_a
  init_LP(&oneloop_andi_model_filt.ay, 2.0); // oneloop_andi_filt_cutoff_a
  init_LP(&oneloop_andi_model_filt.az, 2.0); // oneloop_andi_filt_cutoff_a
  init_LP(&oneloop_andi_model_filt.p_dot, 2.0);
  init_LP(&oneloop_andi_model_filt.q_dot, 2.0);
  init_LP(&oneloop_andi_model_filt.r_dot, 2.0);
}
 
static inline void reinit_all_LP(bool reinit)
{
  reinit_LP_synchronous(&LP.ax, &oneloop_andi_model_filt.ax, reinit);
  reinit_LP_synchronous(&LP.ay, &oneloop_andi_model_filt.ay, reinit);
  reinit_LP_synchronous(&LP.az, &oneloop_andi_model_filt.az, reinit);
  reinit_LP_synchronous(&LP.p_dot, &oneloop_andi_model_filt.p_dot, reinit);
  reinit_LP_synchronous(&LP.q_dot, &oneloop_andi_model_filt.q_dot, reinit);
  reinit_LP_synchronous(&LP.r_dot, &oneloop_andi_model_filt.r_dot, reinit);
  reinit_LP(&LP.p, reinit);
  reinit_LP(&LP.q, reinit);
  reinit_LP(&LP.r, reinit);
}
//------------------------------------------------------------------------------------------
 
void init_filter(void)
{
  // Filtering of the velocities
  float tau = 1.0 / (2.0 * M_PI * oneloop_andi_filt_cutoff);
  float tau_v = 1.0 / (2.0 * M_PI * oneloop_andi_filt_cutoff_v);
  float tau_2 = 1.0 / (2.0 * M_PI * 2.0);
  // printf("tau: %f tau_v: %f\n", tau, tau_v);
  // printf("initializing filters\n");
  init_butterworth_2_low_pass(&accely_filt, tau, 1.0 / PERIODIC_FREQUENCY, accely_filt.o[0]);
  init_butterworth_2_low_pass(&airspeed_filt, tau, 1.0 / PERIODIC_FREQUENCY, airspeed_filt.o[0]);
#ifdef USE_PID_NUMDIFF_DERIVATIVE  
  for (int i = 0; i < 3; i++){
    init_butterworth_2_low_pass(&filt_veloc[i], tau_v, 1.0 / PERIODIC_FREQUENCY, filt_veloc[i].o[0]);
  }
#endif  
  for (int i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    init_butterworth_2_low_pass(&u_filt[i], tau_2, 1.0 / PERIODIC_FREQUENCY, 0.0);
  }
}
 
void oneloop_andi_propagate_filters(void)
{
  reinit_all_LP(false); 
  struct NedCoor_f *veloc = stateGetSpeedNed_f();
  struct FloatRates *body_rates = stateGetBodyRates_f();
#ifdef USE_ND_VELOCITIES  
  static bool vel_inited = false;
  static struct NedCoor_f veloc_prev;
  if (!vel_inited) { veloc_prev = *veloc; vel_inited = true; }
  LP.ax.meas      = (veloc->x - veloc_prev.x) * PERIODIC_FREQUENCY;
  LP.ay.meas      = (veloc->y - veloc_prev.y) * PERIODIC_FREQUENCY;
  LP.az.meas      = (veloc->z - veloc_prev.z) * PERIODIC_FREQUENCY;
  veloc_prev      = *veloc;
#else
  struct NedCoor_f *accel = stateGetAccelNed_f();
  LP.ax.meas      = accel->x;
  LP.ay.meas      = accel->y;
  LP.az.meas      = accel->z;
#endif  
  LP.p.meas_prev  = LP.p.meas;
  LP.q.meas_prev  = LP.q.meas;
  LP.r.meas_prev  = LP.r.meas;
  LP.p.meas       = body_rates->p;
  LP.q.meas       = body_rates->q;
  LP.r.meas       = body_rates->r;
  LP.p_dot.meas   = (LP.p.meas - LP.p.meas_prev) * PERIODIC_FREQUENCY;
  LP.q_dot.meas   = (LP.q.meas - LP.q.meas_prev) * PERIODIC_FREQUENCY;
  LP.r_dot.meas   = (LP.r.meas - LP.r.meas_prev) * PERIODIC_FREQUENCY;
  // Update Filters of Feedbacks
  update_filter_on_type(&LP.ax, LP.ax.meas);
  update_filter_on_type(&LP.ay, LP.ay.meas);
  update_filter_on_type(&LP.az, LP.az.meas);
  update_filter_on_type(&LP.p_dot, LP.p_dot.meas);
  update_filter_on_type(&LP.q_dot, LP.q_dot.meas);
  update_filter_on_type(&LP.r_dot, LP.r_dot.meas);
  update_filter_on_type(&LP.p, LP.p.meas);
  update_filter_on_type(&LP.q, LP.q.meas);
  update_filter_on_type(&LP.r, LP.r.meas);
  // Propagate filter for sideslip correction
  float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
  update_butterworth_2_low_pass(&accely_filt, accely);
  float airspeed_meas = stateGetAirspeed_f();
  Bound(airspeed_meas, 0.0, 30.0);
  update_butterworth_2_low_pass(&airspeed_filt, airspeed_meas);
}
//------------------------------------------------------------------------------------------
void reinit_controller(void)
{
  //printf("Re-INIT \n");
  // store float version of commands
  // float commands_float[ANDI_NUM_ACT_TOT];
  // for (int i = 0; i < ANDI_NUM_ACT; i++) {
  //   commands_float[i] = (float)commands[i];
  // }
  // Actuators
  // float_vect_copy(andi_u, commands_float, ANDI_NUM_ACT);
  andi_u[COMMAND_ROLL] = oneloop_andi.sta_state.att[0];
  andi_u[COMMAND_PITCH] = oneloop_andi.sta_state.att[1];
 
  // float_vect_zero(andi_du, ANDI_NUM_ACT_TOT);  // Not used
  // float_vect_zero(andi_u_n, ANDI_NUM_ACT_TOT); // Not used
  // float_vect_copy(actuator_state_1l, commands_float, ANDI_NUM_ACT);
  //  Stabilization
  float_vect_copy(oneloop_andi.sta_ref.att, oneloop_andi.sta_state.att, 3);
  float_vect_copy(oneloop_andi.sta_ref.att_d, oneloop_andi.sta_state.att_d, 3);
  float_vect_copy(oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_state.att_2d, 3);
  float_vect_zero(oneloop_andi.sta_ref.att_3d, 3);
  eulers_zxy_des.phi = oneloop_andi.sta_state.att[0];
  eulers_zxy_des.theta = oneloop_andi.sta_state.att[1];
  eulers_zxy_des.psi = oneloop_andi.sta_state.att[2];
  // Guidance
  float_vect_copy(oneloop_andi.gui_ref.pos, oneloop_andi.gui_state.pos, 3);
  float_vect_copy(oneloop_andi.gui_ref.vel, oneloop_andi.gui_state.vel, 3);
  float_vect_copy(oneloop_andi.gui_ref.acc, oneloop_andi.gui_state.acc, 3);
  float_vect_zero(oneloop_andi.gui_ref.jer, 3);
  // Controller Inputs
  // float_vect_zero(nu, ANDI_OUTPUTS); // Not used
  // float_vect_zero(nu_n, ANDI_OUTPUTS); // Not used
  // float_vect_zero(nav_target,3); // Not used
  // float_vect_zero(nav_target_new,3); // Not used
}
void oneloop_andi_init(void)
{
  //printf("INIT \n");
  oneloop_andi.half_loop = true;
  oneloop_andi.ctrl_type = CTRL_ANDI;
  init_poles();
  // Make sure that the dynamics are positive and non-zero
  int8_t i;
  for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    act_dynamics[i] = positive_non_zero(act_dynamics[i]);
  }
  dynFilter_init(&mu_mR, act_dynamics[COMMAND_MOTOR_RIGHT], act_dynamics[COMMAND_MOTOR_RIGHT]);
  dynFilter_init(&mu_mL, act_dynamics[COMMAND_MOTOR_LEFT], act_dynamics[COMMAND_MOTOR_LEFT]);
  // Initialize Effectiveness matrix
  calc_normalization();
  G1G2_oneloop(oneloop_andi.ctrl_type);
  for (i = 0; i < ANDI_OUTPUTS; i++)
  {
    bwls_1l[i] = EFF_MAT_G[i];
  }
  // Initialize filters and other variables
  init_all_LP();
  init_filter();
  init_controller_gains();
  float_vect_zero(andi_u, ANDI_NUM_ACT_TOT);
  float_vect_zero(andi_du, ANDI_NUM_ACT_TOT);
  float_vect_zero(andi_u_n, ANDI_NUM_ACT_TOT);
  float_vect_zero(actuator_state_1l, ANDI_NUM_ACT_TOT);
  float_vect_zero(oneloop_andi.sta_ref.att, 3);
  float_vect_zero(oneloop_andi.sta_ref.att_d, 3);
  float_vect_zero(oneloop_andi.sta_ref.att_2d, 3);
  float_vect_zero(oneloop_andi.sta_ref.att_3d, 3);
  float_vect_zero(nu, ANDI_OUTPUTS);
  float_vect_zero(nu_n, ANDI_OUTPUTS);
  float_vect_zero(nav_target, 3);
  float_vect_zero(nav_target_new, 3);
  eulers_zxy_des.phi = 0.0;
  eulers_zxy_des.theta = 0.0;
  eulers_zxy_des.psi = 0.0;
// Start telemetry
#if PERIODIC_TELEMETRY
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_oneloop_andi);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_STAB, send_eff_mat_stab_oneloop_andi);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_GUID, send_eff_mat_guid_oneloop_andi);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE, send_guidance_oneloop_andi);
  // register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATOR_STATE, send_oneloop_actuator_state);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DEBUG_VECT, send_oneloop_debug);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_V, send_wls_v_oneloop);
  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_U, send_wls_u_oneloop);
#endif
}
 
void oneloop_andi_enter(bool half_loop_sp, int ctrl_type)
{
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
  ele_min = 0.0;
#endif
  oneloop_andi.half_loop = half_loop_sp;
  oneloop_andi.ctrl_type = ctrl_type;
  psi_des_rad = eulers_zxy.psi;
  psi_des_deg = DegOfRad(eulers_zxy.psi);
  multiple_chirp_run = false;
  // if (oneloop_andi.half_loop){
  //   printf("HALF LOOP\n");
  // } else {
  //   printf("FULL LOOP\n");
  // }
  // if (oneloop_andi.ctrl_type == CTRL_ANDI){
  //   printf("ANDI\n");
  // } else if (oneloop_andi.ctrl_type == CTRL_INDI){
  //   printf("INDI\n");
  // } else {
  //   printf("CTRL_TYPE NOT SET\n");
  // }
  calc_normalization();
  G1G2_oneloop(oneloop_andi.ctrl_type);
  int8_t i;
  for (i = 0; i < ANDI_OUTPUTS; i++)
  {
    bwls_1l[i] = EFF_MAT_G[i];
  }
  // reinit_all_LP(true); // Not used
  // init_filter();
  init_controller_gains();
  /* Stabilization Reset */
  /*Guidance Reset*/
  reinit_controller();
}
 
void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop)
{
  // Initialize some variables
  a_thrust = 0.0;
  nav_target[0] = PSA_des.x;
  nav_target[1] = PSA_des.y;
  nav_target[2] = PSA_des.z;
  float thrust_cmd_1l = 0.0;
  float des_r = 0.0;
  // Generate reference signals with reference model
  if (half_loop)
  {
    // Disregard X and Y jerk objectives
    WLS_one_p.Wv[0] = 0.0;
    WLS_one_p.Wv[1] = 0.0;
    if (drop_yaw)
    {
      WLS_one_p.Wv[5] = 0.0;
    }
    else
    {
      WLS_one_p.Wv[5] = Wv_backup[5];
    }
    // Overwrite references with actual signals (for consistent plotting)
    float_vect_copy(oneloop_andi.gui_ref.pos, oneloop_andi.gui_state.pos, 3);
    float_vect_copy(oneloop_andi.gui_ref.vel, oneloop_andi.gui_state.vel, 3);
    float_vect_copy(oneloop_andi.gui_ref.acc, oneloop_andi.gui_state.acc, 3);
    float_vect_zero(oneloop_andi.gui_ref.jer, 3);
    // Set desired attitude with stick input
    eulers_zxy_des.phi = (float)(radio_control_get(RADIO_ROLL)) / MAX_PPRZ * ONELOOP_ANDI_MAX_PHI;
    eulers_zxy_des.theta = (float)(radio_control_get(RADIO_PITCH)) / MAX_PPRZ * ONELOOP_ANDI_MAX_THETA;
    // Set desired Yaw rate with stick input
    des_r = (float)(radio_control_get(RADIO_YAW)) / MAX_PPRZ * max_r; // Get yaw rate from stick
    BoundAbs(des_r, max_r);                                           // Bound yaw rate
    float delta_psi_des_rad = des_r * dt_1l;                          // Integrate desired Yaw rate to get desired change in yaw
    float delta_psi_rad = eulers_zxy_des.psi - eulers_zxy.psi;        // Calculate current yaw difference between des and actual
    NormRadAngle(delta_psi_rad);                                      // Normalize the difference
    if (fabs(delta_psi_rad) > RadOfDeg(30.0))
    { // If difference is bigger than 10 deg do not further increment desired
      delta_psi_des_rad = 0.0;
    }
    psi_des_rad += delta_psi_des_rad; // Incrementdesired yaw
    NormRadAngle(psi_des_rad);
    // Register Attitude Setpoints from previous loop
    if (!in_flight_oneloop)
    {
      psi_des_rad = eulers_zxy.psi;
    }
    eulers_zxy_des.psi = psi_des_rad;
    float att_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, eulers_zxy_des.psi};
    // Create commands adhoc to get actuators to the wanted level
    thrust_cmd_1l = (float)radio_control_get(RADIO_THROTTLE);
    Bound(thrust_cmd_1l, 0.0, MAX_PPRZ);
    int8_t i;
    // To calculate the nu corrsponding to the Thrust command, plug it in the control law.
    for (i = 0; i < ANDI_NUM_ACT; i++)
    {
      if (oneloop_andi.ctrl_type == CTRL_ANDI)
      {
        a_thrust += (thrust_cmd_1l)*EFF_MAT_RW[RW_aD][i] * act_dyn_ctrl[i];
      }
      else
      {
        a_thrust += (thrust_cmd_1l)*EFF_MAT_RW[RW_aD][i];
      }
    }
    a_thrust = a_thrust - oneloop_andi_model[RW_aD]; // oneloop_andi_model_filt.az.out;
    rm_3rd_attitude(dt_1l, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, att_des, false, psi_vec, k_att_rm.k1, k_att_rm.k2, k_att_rm.k3, sta_bounds);
  }
  else
  {
    // Make sure X and Y jerk objectives are active
    WLS_one_p.Wv[0] = Wv_backup[0];
    WLS_one_p.Wv[1] = Wv_backup[1];
    // Generate Reference signals for positioning using RM
    if (rm_order_h == 3)
    {
      rm_3rd_pos(dt_1l, oneloop_andi.gui_ref.pos, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target, k_pos_rm.k1, k_pos_rm.k2, k_pos_rm.k3, max_v_nav, max_a_nav, max_j_lin, 2);
    }
    else if (rm_order_h == 2)
    {
      float_vect_copy(oneloop_andi.gui_ref.pos, oneloop_andi.gui_state.pos, 2);
      float_vect_copy(oneloop_andi.gui_ref.vel, oneloop_andi.gui_state.vel, 2);
      reshape_wind(); // returns accel sp as nav target new
      rm_1st_pos(dt_1l, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target_new, k_pos_rm.k3, max_j_lin, 2);
      // rm_2nd_pos(dt_1l, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target_new, k_pos_rm.k2, k_pos_rm.k3, max_a_nav, max_j_lin, 2);
    }
    else if (rm_order_h == 1)
    {
      float_vect_copy(oneloop_andi.gui_ref.pos, oneloop_andi.gui_state.pos, 2);
      float_vect_copy(oneloop_andi.gui_ref.vel, oneloop_andi.gui_state.vel, 2);
      rm_1st_pos(dt_1l, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target, k_pos_rm.k3, max_j_lin, 2);
    }
    // Update desired Heading (psi_des_rad) based on previous loop or changed setting
    if (heading_manual)
    {
      psi_des_rad = RadOfDeg(psi_des_deg);
      if (yaw_stick_in_auto)
      {
        psi_des_rad += (float)(radio_control_get(RADIO_YAW)) / MAX_PPRZ * max_r * dt_1l;
      }
    }
    else
    {
      psi_des_rad += oneloop_andi_sideslip() * dt_1l;
      NormRadAngle(psi_des_rad);
    }
    // Register Attitude Setpoints from previous loop
    if (!in_flight_oneloop)
    {
      psi_des_rad = eulers_zxy.psi;
    }
    eulers_zxy_des.psi = psi_des_rad;
    float att_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, eulers_zxy_des.psi};
    // The RM functions want an array as input. Create a single entry array and write the vertical guidance entries.
    float single_value_ref[1] = {oneloop_andi.gui_ref.pos[2]};
    float single_value_d_ref[1] = {oneloop_andi.gui_ref.vel[2]};
    float single_value_2d_ref[1] = {oneloop_andi.gui_ref.acc[2]};
    float single_value_3d_ref[1] = {oneloop_andi.gui_ref.jer[2]};
    float single_value_nav_target[1] = {nav_target[2]};
    float single_value_k1_rm[1] = {k_pos_rm.k1[2]};
    float single_value_k2_rm[1] = {k_pos_rm.k2[2]};
    float single_value_k3_rm[1] = {k_pos_rm.k3[2]};
    if (rm_order_v == 3)
    {
      rm_3rd_pos(dt_1l, single_value_ref, single_value_d_ref, single_value_2d_ref, single_value_3d_ref, single_value_nav_target, single_value_k1_rm, single_value_k2_rm, single_value_k3_rm, max_v_nav_v, max_a_nav, max_j_lin, 1);
      oneloop_andi.gui_ref.pos[2] = single_value_ref[0];
      oneloop_andi.gui_ref.vel[2] = single_value_d_ref[0];
      oneloop_andi.gui_ref.acc[2] = single_value_2d_ref[0];
      oneloop_andi.gui_ref.jer[2] = single_value_3d_ref[0];
    }
    else if (rm_order_v == 2)
    {
      rm_2nd_pos(dt_1l, single_value_d_ref, single_value_2d_ref, single_value_3d_ref, single_value_nav_target, single_value_k2_rm, single_value_k3_rm, max_a_nav, max_j_lin, 1);
      oneloop_andi.gui_ref.pos[2] = oneloop_andi.gui_state.pos[2];
      oneloop_andi.gui_ref.vel[2] = single_value_d_ref[0];
      oneloop_andi.gui_ref.acc[2] = single_value_2d_ref[0];
      oneloop_andi.gui_ref.jer[2] = single_value_3d_ref[0];
    }
    else if (rm_order_v == 1)
    {
      rm_1st_pos(dt_1l, single_value_2d_ref, single_value_3d_ref, single_value_nav_target, single_value_k3_rm, max_j_lin, 1);
      oneloop_andi.gui_ref.pos[2] = oneloop_andi.gui_state.pos[2];
      oneloop_andi.gui_ref.vel[2] = oneloop_andi.gui_state.vel[2];
      oneloop_andi.gui_ref.acc[2] = single_value_2d_ref[0];
      oneloop_andi.gui_ref.jer[2] = single_value_3d_ref[0];
    }
    // Run chirp test if turnerd on (overwrite the guidance references)
    if (chirp_on && !multiple_chirp_run)
    {
      multiple_chirp_run = true;
      chirp_on = false;
    }
    if (multiple_chirp_run && !chirp_on)
    {
      if (chirp_n_counter < chirp_n_repeat)
      {
        delay_counter++;
        int16_t idx_acc = 0;
        float trigger_acc = 0.0;
        switch (chirp_axis)
        {
        case 5: // Yaw
          idx_acc = 2;
          trigger_acc = 5.0;
          break;
        case 6: // Roll
          idx_acc = 0;
          trigger_acc = 15.0;
          break;
        }
        if (fabsf(oneloop_andi.sta_state.att_2d[idx_acc]) < RadOfDeg(trigger_acc) && delay_counter >= 2500 && fabsf(oneloop_andi.sta_state.att_d[idx_acc]) < RadOfDeg(5.0))
        {
          firing_counter++;
        }
        else
        {
          firing_counter = 0;
        }
        if (firing_counter >= 250)
        {
          chirp_n_counter++;
          chirp_on = true;
          firing_counter = 0;
          delay_counter = 0;
        }
      }
      else
      {
        multiple_chirp_run = false;
        chirp_n_counter = 0;
      }
    }
 
    chirp_call(&chirp_on, &chirp_first_call, &t_0_chirp, &time_elapsed_chirp, f0_chirp, f1_chirp, t_chirp, A_chirp, chirp_axis, att_des[2], oneloop_andi.gui_ref.pos, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, p_ref_0);
    // Generate Reference signals for attitude using RM
    // FIX ME ow not yet defined, will be useful in the future to have accurate psi tracking in NAV functions
    bool ow_psi = false;
    if (chirp_on && (chirp_axis == 4 || chirp_axis == 5))
    {
      ow_psi = true;
      psi_vec[0] = oneloop_andi.sta_ref.att[2];
      psi_vec[1] = oneloop_andi.sta_ref.att_d[2];
      psi_vec[2] = oneloop_andi.sta_ref.att_2d[2];
      psi_vec[3] = oneloop_andi.sta_ref.att_3d[2];
      att_des[0] = 0.0;
      att_des[1] = 0.0;
      eulers_zxy_des.phi = 0.0;
      eulers_zxy_des.theta = 0.0;
    }
    bool ow_phi = false;
    if (chirp_on && chirp_axis == 6)
    {
      ow_phi = true;
      phi_vec[0] = oneloop_andi.sta_ref.att[0];
      phi_vec[1] = oneloop_andi.sta_ref.att_d[0];
      phi_vec[2] = oneloop_andi.sta_ref.att_2d[0];
      phi_vec[3] = oneloop_andi.sta_ref.att_3d[0];
      eulers_zxy_des.theta = 1.7;
    }
    rm_3rd_attitude(dt_1l, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, att_des, ow_psi, psi_vec, k_att_rm.k1, k_att_rm.k2, k_att_rm.k3, sta_bounds);
    if (ow_phi)
    {
      oneloop_andi.sta_ref.att[0] = phi_vec[0];
      oneloop_andi.sta_ref.att_d[0] = phi_vec[1];
      oneloop_andi.sta_ref.att_2d[0] = phi_vec[2];
      oneloop_andi.sta_ref.att_3d[0] = phi_vec[3];
    }
  }
}
 
void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
{
  //printf("start [phi_des,theta_des]=[%f,%f] \n",DegOfRad(eulers_zxy_des.phi),DegOfRad(eulers_zxy_des.theta));
  // At beginnig of the loop: (1) Register Attitude, (2) Initialize gains of RM and EC, (3) Calculate Normalization of Actuators Signals, (4) Propagate Actuator Model, (5) Update effectiveness matrix
  eulers_zxy = *stateGetNedToBodyEulersZxy_f();
  init_controller_gains();
  calc_normalization();
  get_act_state_oneloop();
  G1G2_oneloop(oneloop_andi.ctrl_type);
  int8_t i;
  for (i = 0; i < ANDI_OUTPUTS; i++)
  {
    bwls_1l[i] = EFF_MAT_G[i];
  }
  for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    act_dyn_ctrl[i] = act_dynamics[i];
  }
 
  // Need to save and convert desired position for bounding. Not sure NAV always updates this correctly.
  float pos_des[3];
  pos_des[0] = POS_FLOAT_OF_BFP(POS_BFP_OF_REAL(nav.target.y));
  pos_des[1] = POS_FLOAT_OF_BFP(POS_BFP_OF_REAL(nav.target.x));
  pos_des[2] = POS_FLOAT_OF_BFP(-POS_BFP_OF_REAL(nav.nav_altitude));
 
  // If drone is not on the ground use incremental law
  bool  in_flight_oneloop = false;
  if(in_flight) {
    in_flight_oneloop = true;
  }
  if (ONELOOP_ANDI_DEBUG_MODE)
  {
    in_flight_oneloop = false;
  }
  oneloop_calc_model_disturbance(in_flight);
  // Register the state of the drone in the variables used in RM and EC
  // (1) Attitude related
  oneloop_andi.sta_state.att[0] = eulers_zxy.phi;
  oneloop_andi.sta_state.att[1] = eulers_zxy.theta;
  oneloop_andi.sta_state.att[2] = eulers_zxy.psi;
  oneloop_andi_propagate_filters(); // needs to be after update of attitude vector
  oneloop_andi.sta_state.att_d[0] = LP.p.out;
  oneloop_andi.sta_state.att_d[1] = LP.q.out;
  oneloop_andi.sta_state.att_d[2] = LP.r.out;
  oneloop_andi.sta_state.att_2d[0] = LP.p_dot.out;
  oneloop_andi.sta_state.att_2d[1] = LP.q_dot.out;
  oneloop_andi.sta_state.att_2d[2] = LP.r_dot.out;
  // (2) Position related
  oneloop_andi.gui_state.pos[0] = stateGetPositionNed_f()->x;
  oneloop_andi.gui_state.pos[1] = stateGetPositionNed_f()->y;
  oneloop_andi.gui_state.pos[2] = stateGetPositionNed_f()->z;
  oneloop_andi.gui_state.vel[0] = stateGetSpeedNed_f()->x;
  oneloop_andi.gui_state.vel[1] = stateGetSpeedNed_f()->y;
  oneloop_andi.gui_state.vel[2] = stateGetSpeedNed_f()->z;
  oneloop_andi.gui_state.acc[0] = LP.ax.out;
  oneloop_andi.gui_state.acc[1] = LP.ay.out;
  oneloop_andi.gui_state.acc[2] = LP.az.out;
  // Calculated feedforward signal for yaw control
  g2_ff = 0.0;
  for (i = 0; i < ANDI_NUM_ACT; i++)
  {
    if (oneloop_andi.ctrl_type == CTRL_ANDI)
    {
      g2_ff += G2_RW[i] * act_dyn_ctrl[i] * (andi_u[i] - u_filt[i].o[0]);
      // printf("i: %d\n", i);
      // printf("andi_u: %f\n", andi_u[i]);
      // printf("actuator_state_1l: %f\n", actuator_state_1l[i]);
      // printf("act_dynamics: %f\n", act_dynamics[i]);
      // printf("G2_RW: %f\n", G2_RW[i]);
      // printf("g2_ff: %f\n", g2_ff);
    }
    else if (oneloop_andi.ctrl_type == CTRL_INDI)
    {
      g2_ff += G2_RW[i] * (andi_u[i] - u_filt[i].o[0]);
    }
  }
  //printf("pre rm [phi_des,theta_des]=[%f,%f] \n",DegOfRad(eulers_zxy_des.phi),DegOfRad(eulers_zxy_des.theta));
  // Run the Reference Model (RM)
  oneloop_andi_RM(half_loop, PSA_des, rm_order_h, rm_order_v, in_flight_oneloop);
  //printf("post rm [phi_des,theta_des]=[%f,%f] \n",DegOfRad(eulers_zxy_des.phi),DegOfRad(eulers_zxy_des.theta));
  // Run Distrubance Bounder
  oneloop_andi_bound_disturbance(); // Fixme, can be removed
  // Guidance Pseudo Control Vector (nu) based on error controller
  if (half_loop)
  {
    nu[0] = 0.0;
    nu[1] = 0.0;
    nu[2] = a_thrust;
    ctrl_off = false;
  }
  else
  {
    if (oneloop_andi.ctrl_type == CTRL_ANDI)
    {
      // float temp_dist_bound_gui[3] = {oneloop_andi_model[0], oneloop_andi_model[1], oneloop_andi_model[2]};
      float temp_dist_bound_gui[3] = {0.0, 0.0, 0.0};
      ec_3rd_pos(nu, pos_des, oneloop_andi.gui_ref.pos, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, oneloop_andi.gui_state.pos, oneloop_andi.gui_state.vel, oneloop_andi.gui_state.acc, k_pos_e.k1, k_pos_e.k2, k_pos_e.k3, max_v_nav, max_a_nav, temp_dist_bound_gui, 3);
    }
    else if (oneloop_andi.ctrl_type == CTRL_INDI)
    {
      float dummy1[3] = {0.0, 0.0, 0.0};
      ec_3rd_pos(nu, pos_des, oneloop_andi.gui_ref.pos, oneloop_andi.gui_ref.vel, oneloop_andi.gui_ref.acc, dummy1, oneloop_andi.gui_state.pos, oneloop_andi.gui_state.vel, oneloop_andi.gui_state.acc, k_pos_e_indi.k1, k_pos_e_indi.k2, k_pos_e_indi.k3, max_v_nav, max_a_nav, dummy1, 3);
      // nu[0] = ec_3rd(oneloop_andi.gui_ref.pos[0], oneloop_andi.gui_ref.vel[0], oneloop_andi.gui_ref.acc[0], 0.0, oneloop_andi.gui_state.pos[0], oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.acc[0], k_pos_e_indi.k1[0], k_pos_e_indi.k2[0], k_pos_e_indi.k3[0]);
      // nu[1] = ec_3rd(oneloop_andi.gui_ref.pos[1], oneloop_andi.gui_ref.vel[1], oneloop_andi.gui_ref.acc[1], 0.0, oneloop_andi.gui_state.pos[1], oneloop_andi.gui_state.vel[1], oneloop_andi.gui_state.acc[1], k_pos_e_indi.k1[1], k_pos_e_indi.k2[1], k_pos_e_indi.k3[1]);
      // nu[2] = ec_3rd(oneloop_andi.gui_ref.pos[2], oneloop_andi.gui_ref.vel[2], oneloop_andi.gui_ref.acc[2], 0.0, oneloop_andi.gui_state.pos[2], oneloop_andi.gui_state.vel[2], oneloop_andi.gui_state.acc[2], k_pos_e_indi.k1[2], k_pos_e_indi.k2[2], k_pos_e_indi.k3[2]);
    }
#define USE_PID_OUTERLOOP
#ifdef USE_PID_OUTERLOOP
    float acc_des[3];
    Pos_PID(pos_des, oneloop_andi.gui_state.pos, oneloop_andi.gui_state.vel, k_P, k_I, k_D,acc_des);
    //printf("[%f,%f,%f]\n",acc_des[0],acc_des[1],acc_des[2]);
    nu[2] = (acc_des[2]-oneloop_andi.gui_state.acc[2])*k_pos_e.k3[2];
    shape_vector(acc_des);
    eul_of_acc(acc_des, eulers_zxy.psi);
    //printf("Right after [phi_des,theta_des]=[%f,%f] \n",DegOfRad(eulers_zxy_des.phi),DegOfRad(eulers_zxy_des.theta));
  
#endif
  }
  // Attitude Pseudo Control Vector (nu) based on error controller
  float y_4d_att[3];
  float att_des_4_ec[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, eulers_zxy_des.psi};
  if (oneloop_andi.ctrl_type == CTRL_ANDI)
  {
    // float temp_dist_bound_sta[3] = {oneloop_andi_model_filt.p_dot.out, oneloop_andi_model_filt.q_dot.out, oneloop_andi_model_filt.r_dot.out};
    float temp_dist_bound_sta[3] = {oneloop_andi_model[RW_ap], oneloop_andi_model[RW_aq], oneloop_andi_model[RW_ar]};
    ec_3rd_att(y_4d_att, att_des_4_ec, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, oneloop_andi.sta_ref.att_3d, oneloop_andi.sta_state.att, oneloop_andi.sta_state.att_d, oneloop_andi.sta_state.att_2d, k_att_e.k1, k_att_e.k2, k_att_e.k3, sta_bounds, temp_dist_bound_sta);
  }
  else if (oneloop_andi.ctrl_type == CTRL_INDI)
  {
    float dummy0[3] = {0.0, 0.0, 0.0};
    float temp_dist_bound_sta_INDI[3] = {oneloop_andi_model[RW_ap] * k_att_e.k3[0], oneloop_andi_model[RW_aq] * k_att_e.k3[1], oneloop_andi_model[RW_ar] * k_att_e.k3[2]};
    ec_3rd_att(y_4d_att, att_des_4_ec, oneloop_andi.sta_ref.att, oneloop_andi.sta_ref.att_d, oneloop_andi.sta_ref.att_2d, dummy0, oneloop_andi.sta_state.att, oneloop_andi.sta_state.att_d, oneloop_andi.sta_state.att_2d, k_att_e_indi.k1, k_att_e_indi.k2, k_att_e_indi.k3, sta_bounds, temp_dist_bound_sta_INDI);
  }
  if (half_loop && radio_control_get(RADIO_THROTTLE) < 200)
  {
    nu[3] = 0.0;
    nu[4] = 0.0;
    nu[5] = 0.0;
  }
  else
  {
    nu[3] = y_4d_att[0];
    nu[4] = y_4d_att[1];
    nu[5] = y_4d_att[2] + g2_ff;
  }
 
  // temp restructuring------------------
  nu[0] = nu[0] + oneloop_andi_model[RW_aN]; // oneloop_andi_model_filt.ax.out;
  nu[1] = nu[1] + oneloop_andi_model[RW_aE]; // oneloop_andi_model_filt.ay.out;
  nu[2] = nu[2] + oneloop_andi_model[RW_aD]; // oneloop_andi_model_filt.az.out;
  // nu[3] = nu[3] + oneloop_andi_model[3];
  // nu[4] = nu[4] + oneloop_andi_model[4];
  // nu[5] = nu[5] + oneloop_andi_model[5];
  // BoundAbs(nu[5], n_array[5]*coupling_factor[5]); //FIXME UNCOMMENT ME
  if (drop_yaw)
  {
    // nu[0] = oneloop_andi_model_filt.ax.out;
    // nu[1] = oneloop_andi_model_filt.ay.out;
    // nu[2] = oneloop_andi_model_filt.az.out;
    // nu[3] = oneloop_andi_model_filt.p_dot.out*k_att_e.k3[0];
    // nu[4] = oneloop_andi_model_filt.q_dot.out*k_att_e.k3[1];
    nu[5] = oneloop_andi_model_filt.r_dot.out * k_att_e.k3[2];
  }
  if (drop_roll)
  {
    nu[3] = oneloop_andi_model_filt.p_dot.out * k_att_e.k3[0];
  }
  if (drop_pitch)
  {
    nu[4] = oneloop_andi_model_filt.q_dot.out * k_att_e.k3[1];
  }
  if (drop_aN)
  {
    nu[0] = oneloop_andi_model_filt.ax.out;
  }
  if (drop_aE)
  {
    nu[1] = oneloop_andi_model_filt.ay.out;
  }
  if (drop_aD)
  {
    nu[2] = oneloop_andi_model_filt.az.out;
  }
 
  //------------------------------------
 
  if (!chirp_on)
  {
    pitch_pref = radio_control.values[RADIO_AUX5];
    pitch_pref = pitch_pref / MAX_PPRZ * theta_pref_max;
    Bound(pitch_pref, 0.0, theta_pref_max);
  }
  u_pref[COMMAND_PITCH] = 0.0; // pitch_pref; // FIXME
  // Calculate the min and max increments
  for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    // printf("Act_dyn[%d]: %f\n",i,act_dynamics[i]);
    switch (i)
    {
    case COMMAND_MOTOR_FRONT:
    case COMMAND_MOTOR_RIGHT:
    case COMMAND_MOTOR_BACK:
    case COMMAND_MOTOR_LEFT:
    {
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
      float skew_bound = RW.skew.deg;
      Bound(skew_bound, 70.0, 90.0);
      float Wu_sched = (Wu_quad_motors_fwd - Wu_backup[i]) / (90.0 - 70.0) * (skew_bound - 70) + Wu_backup[i];
#else
      float Wu_sched = Wu_backup[i];
#endif
      WLS_one_p.Wu[i] = Wu_sched;
      WLS_one_p.u_min[i] = (act_min[i]) / ratio_u_un[i];
      WLS_one_p.u_pref[i] = (u_pref[i]) / ratio_u_un[i];
      WLS_one_p.u_max[i] = (act_max[i]) / ratio_u_un[i];
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
      if (rotwing_state.hover_motors_enabled)
      {
#else
      if (true)
      {
#endif
        WLS_one_p.u_max[i] = (act_max[i]) / ratio_u_un[i];
      }
      else
      {
        WLS_one_p.u_max[i] = (0.0) / ratio_u_un[i];
      }
      break;
    }
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
    case COMMAND_MOTOR_PUSHER:
    case COMMAND_RUDDER:
      WLS_one_p.u_min[i] = (act_min[i]) / ratio_u_un[i];
      WLS_one_p.u_max[i] = (act_max[i]) / ratio_u_un[i];
      WLS_one_p.u_pref[i] = (u_pref[i]) / ratio_u_un[i];
      break;
    case COMMAND_AILERONS:
      if (RW.skew.deg > 25.0)
      {
        WLS_one_p.u_min[i] = (act_min[i]) / ratio_u_un[i];
        WLS_one_p.u_max[i] = (act_max[i]) / ratio_u_un[i];
      }
      else
      {
        WLS_one_p.u_min[i] = (0.0) / ratio_u_un[i];
        WLS_one_p.u_max[i] = (0.0) / ratio_u_un[i];
      }
      WLS_one_p.u_pref[i] = (u_pref[i]) / ratio_u_un[i];
      break;
    case COMMAND_FLAPS:
      if (RW.skew.deg > 50.0)
      {
        WLS_one_p.u_min[i] = (act_min[i]) / ratio_u_un[i];
        WLS_one_p.u_max[i] = (act_max[i]) / ratio_u_un[i];
      }
      else
      {
        WLS_one_p.u_min[i] = (0.0) / ratio_u_un[i];
        WLS_one_p.u_max[i] = (0.0) / ratio_u_un[i];
      }
      WLS_one_p.u_pref[i] = (u_pref[i]) / ratio_u_un[i];
      break;
    case COMMAND_ELEVATOR:
      WLS_one_p.u_min[i] = (act_min[i]) / ratio_u_un[i];
      WLS_one_p.u_max[i] = (act_max[i]) / ratio_u_un[i];
      u_pref[i] = RW.ele_pref;
      WLS_one_p.u_pref[i] = (u_pref[i]) / ratio_u_un[i];
      break;
#endif
    case COMMAND_ROLL:
      WLS_one_p.u_min[i] = (act_min[i]) / ratio_u_un[i];
      WLS_one_p.u_max[i] = (act_max[i]) / ratio_u_un[i];
      WLS_one_p.u_pref[i] = (u_pref[i]) / ratio_u_un[i];
      break;
    case COMMAND_PITCH:
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
      if (RW.skew.deg > 50.0)
      {
#else
      if (false)
      {
#endif
        WLS_one_p.u_min[i] = (RadOfDeg(-17.0)) / ratio_u_un[i];
        WLS_one_p.u_max[i] = (RadOfDeg(17.0)) / ratio_u_un[i];
      }
      else
      {
        WLS_one_p.u_min[i] = (act_min[i]) / ratio_u_un[i];
        WLS_one_p.u_max[i] = (act_max[i]) / ratio_u_un[i];
      }
      WLS_one_p.u_pref[i] = (u_pref[i]) / ratio_u_un[i];
      break;
    }
  }
  //printf("Pre alloc [phi_des,theta_des]=[%f,%f] \n",DegOfRad(eulers_zxy_des.phi),DegOfRad(eulers_zxy_des.theta));
  // WLS Control Allocator
  normalize_nu();
  wls_alloc(&WLS_one_p, bwls_1l, 0, 0, 10);
 
  // Keep the previous command while a doublet runs (only when the sys_id_doublet module is loaded)
#ifdef MODULE_SYS_ID_DOUBLET_ID
  bool doublet_running = sys_id_doublet_running();
#else
  bool doublet_running = false;
#endif
  if (!doublet_running)
  {
    for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
    {
      andi_u_n[i] = WLS_one_p.u[i];
      andi_u[i] = (float)(andi_u_n[i] * ratio_u_un[i]);
    }
  }
  else
  {
    reinit_controller();
  }
#ifdef COMMAND_MOTOR_PUSHER
  if ((half_loop))
  {
    andi_u[COMMAND_MOTOR_PUSHER] = radio_control.values[RADIO_AUX4];
  }
#endif
  // Run the dynamics substitution filter
  dynFilter_run(&mu_mR, andi_u[COMMAND_MOTOR_RIGHT], oneloop_andi_sigma);
  dynFilter_run(&mu_mL, andi_u[COMMAND_MOTOR_LEFT], oneloop_andi_sigma);
  if (use_dyn_filter)
  {
    andi_u[COMMAND_MOTOR_RIGHT] = mu_mR.mu_c;
    andi_u[COMMAND_MOTOR_LEFT] = mu_mL.mu_c;
  }
  // TODO : USE THE PROVIDED MAX AND MIN and change limits for phi and theta
  // Bound the inputs to the actuators
  for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    Bound(andi_u[i], act_min[i], act_max[i]);
  }
  /*Commit the actuator command*/
  for (i = 0; i < ANDI_NUM_ACT; i++)
  {
    commands[i] = (int16_t)andi_u[i];
  }
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
  if (rotwing_state.fail_pusher_motor)
  {
    commands[COMMAND_MOTOR_PUSHER] = -9600; // Min(1000,andi_u[COMMAND_MOTOR_PUSHER]);
  }
#endif
  // if (manual_all){
  //   for(i=0; i<4; i++){
  //     if (i==chirp_axis){
  //       commands[i] = radio_control_get(RADIO_THROTTLE);
  //     }
  //     commands[i] = 0.0;
  //   }
  //&& radio_control_get(RADIO_THROTTLE)>200
  // commands[COMMAND_MOTOR_RIGHT] = 0.0;//radio_control_get(RADIO_THROTTLE);
  // commands[COMMAND_MOTOR_LEFT]  = 0.0;//radio_control_get(RADIO_THROTTLE);
  // commands[COMMAND_MOTOR_FRONT] = 0.0;//radio_control_get(RADIO_THROTTLE);
  // commands[COMMAND_MOTOR_BACK]  = 0.0;//radio_control_get(RADIO_THROTTLE);
  //  }
  commands[COMMAND_THRUST] = (commands[COMMAND_MOTOR_FRONT] + commands[COMMAND_MOTOR_RIGHT] + commands[COMMAND_MOTOR_BACK] + commands[COMMAND_MOTOR_LEFT]) / num_thrusters_oneloop;
  autopilot.throttle = commands[COMMAND_THRUST];
  stabilization.cmd[COMMAND_THRUST] = commands[COMMAND_THRUST];
#ifndef USE_PID_OUTERLOOP  
  if (!half_loop)
  {
    eulers_zxy_des.phi = andi_u[COMMAND_ROLL];
    eulers_zxy_des.theta = andi_u[COMMAND_PITCH];
  }
#endif
 
  if (heading_manual)
  {
    psi_des_deg = DegOfRad(psi_des_rad);
  }
 
  stabilization.cmd[COMMAND_ROLL] = (int16_t)(DegOfRad(eulers_zxy_des.phi) * MAX_PPRZ / DegOfRad(ONELOOP_ANDI_MAX_PHI));
  stabilization.cmd[COMMAND_PITCH] = (int16_t)(DegOfRad(eulers_zxy_des.theta) * MAX_PPRZ / DegOfRad(ONELOOP_ANDI_MAX_THETA));
  stabilization.cmd[COMMAND_YAW] = (int16_t)(psi_des_deg * MAX_PPRZ / 180.0);
  //printf("end run [phi_des,theta_des]=[%f,%f] \n",DegOfRad(eulers_zxy_des.phi),DegOfRad(eulers_zxy_des.theta));
}
 
void get_act_state_oneloop(void)
{
  int8_t i;
  float prev_actuator_state_1l;
  for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    if (i < ANDI_NUM_ACT)
    {
      prev_actuator_state_1l = actuator_state_1l[i];
      actuator_state_1l[i] = prev_actuator_state_1l + act_dynamics_d[i] * (andi_u[i] - prev_actuator_state_1l);
      if (!autopilot_get_motors_on())
      {
        actuator_state_1l[i] = 0.0;
      }
      Bound(actuator_state_1l[i], act_min[i], act_max[i]);
    }
    else
    {
      actuator_state_1l[i] = oneloop_andi.sta_state.att[i - ANDI_NUM_ACT];
    }
  }
}
 
#ifdef ONELOOP_ANDI_CONSTANT_EFFECTIVENESS
static void oneloop_andi_constant_eff(void)
{
  static const float g1_thrust[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G1_THRUST;
  static const float g1_roll[ANDI_NUM_ACT_TOT]   = ONELOOP_ANDI_G1_ROLL;
  static const float g1_pitch[ANDI_NUM_ACT_TOT]  = ONELOOP_ANDI_G1_PITCH;
  static const float g1_yaw[ANDI_NUM_ACT_TOT]    = ONELOOP_ANDI_G1_YAW;
  static const float g2[ANDI_NUM_ACT_TOT]        = ONELOOP_ANDI_G2;
  struct FloatEulers *eul = stateGetNedToBodyEulersZxy_f();
  float sphi = sinf(eul->phi), cphi = cosf(eul->phi);
  float stheta = sinf(eul->theta), ctheta = cosf(eul->theta);
  float spsi = sinf(eul->psi), cpsi = cosf(eul->psi);
  float T = 9.81f; // hover thrust specific force
  int i;
  for (i = 0; i < ANDI_NUM_ACT; i++) {
    float gz = g1_thrust[i] / ANDI_G_SCALING;
    G2_RW[i] = g2[i] / ANDI_G_SCALING;
    EFF_MAT_RW[RW_aN][i] = (cpsi * stheta + ctheta * sphi * spsi) * gz;
    EFF_MAT_RW[RW_aE][i] = (spsi * stheta - cpsi * ctheta * sphi) * gz;
    EFF_MAT_RW[RW_aD][i] = (cphi * ctheta) * gz;
    EFF_MAT_RW[RW_ap][i] = g1_roll[i] / ANDI_G_SCALING;
    EFF_MAT_RW[RW_aq][i] = g1_pitch[i] / ANDI_G_SCALING;
    EFF_MAT_RW[RW_ar][i] = g1_yaw[i] / ANDI_G_SCALING + G2_RW[i];
  }
  for (i = ANDI_NUM_ACT; i < ANDI_NUM_ACT_TOT; i++) {
    G2_RW[i] = 0.0f;
    EFF_MAT_RW[RW_ap][i] = 0.0f;
    EFF_MAT_RW[RW_aq][i] = 0.0f;
    EFF_MAT_RW[RW_ar][i] = 0.0f;
  }
  EFF_MAT_RW[RW_aN][COMMAND_ROLL]  = -cphi * ctheta * spsi * T;
  EFF_MAT_RW[RW_aE][COMMAND_ROLL]  =  cphi * ctheta * cpsi * T;
  EFF_MAT_RW[RW_aD][COMMAND_ROLL]  =  sphi * ctheta * T;
  EFF_MAT_RW[RW_aN][COMMAND_PITCH] = -(ctheta * cpsi - sphi * stheta * spsi) * T;
  EFF_MAT_RW[RW_aE][COMMAND_PITCH] = -(ctheta * spsi + sphi * stheta * cpsi) * T;
  EFF_MAT_RW[RW_aD][COMMAND_PITCH] =  stheta * cphi * T;
}
#endif
 
void G1G2_oneloop(int ctrl_type)
{
  int i = 0;
  float scaler = 1.0;
#ifdef ONELOOP_ANDI_CONSTANT_EFFECTIVENESS
  oneloop_andi_constant_eff();
#endif
  for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    // printf("act_dyn_ctrl[%d] = %f\n", i, act_dyn_ctrl[i]);
    switch (i)
    {
    case (COMMAND_MOTOR_FRONT):
    case (COMMAND_MOTOR_RIGHT):
    case (COMMAND_MOTOR_BACK):
    case (COMMAND_MOTOR_LEFT):
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
    case (COMMAND_MOTOR_PUSHER):
    case (COMMAND_ELEVATOR):
    case (COMMAND_RUDDER):
    case (COMMAND_AILERONS):
    case (COMMAND_FLAPS):
#endif
      if (ctrl_type == CTRL_ANDI)
      {
        scaler = act_dyn_ctrl[i] * ratio_u_un[i];
      }
      else if (ctrl_type == CTRL_INDI)
      {
        scaler = ratio_u_un[i];
      }
      break;
    case (COMMAND_ROLL):
    case (COMMAND_PITCH):
      if (ctrl_type == CTRL_ANDI)
      {
        scaler = act_dyn_ctrl[i] * ratio_u_un[i];
      }
      else if (ctrl_type == CTRL_INDI)
      {
        scaler = ratio_u_un[i];
      }
      break;
    default:
      break;
    }
    int j = 0;
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
    bool turn_quad_off = ((!rotwing_state.hover_motors_enabled || !rotwing_state_hover_motors_running()) && rotwing_state.state != ROTWING_STATE_FORCE_HOVER);
#else
    bool turn_quad_off = false;
#endif
    for (j = 0; j < ANDI_OUTPUTS; j++)
    {
      EFF_MAT_G[j][i] = EFF_MAT_RW[j][i] * scaler * ratio_vn_v[j];
      if (drop_yaw)
      {
        EFF_MAT_G[5][i] = 0.0;
      }
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
      if (airspeed_filt.o[0] < ELE_MIN_AS && i == COMMAND_ELEVATOR)
      {
        EFF_MAT_G[j][i] = 0.0;
      }
#endif
      if (turn_quad_off && i < 4)
      {
        EFF_MAT_G[j][i] = 0.0;
      }
      if (ctrl_off && i < 4 && j == 5)
      { // hack test
        EFF_MAT_G[j][i] = 0.0;
      }
      if (ctrl_off && i < 4 && j == 4)
      { // hack test
        EFF_MAT_G[j][i] = 0.0;
      }
      if (ctrl_off && i < 4 && j == 3)
      { // hack test
        EFF_MAT_G[j][i] = 0.0;
      }
      // if (j < 2 && i < 4){
      //   EFF_MAT_G[j][i] = 0.0;
      // }
    }
  }
  oneloop_axis_effectiveness_calc();
}
 
void calc_normalization(void)
{
  int8_t i;
  for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    act_dynamics_d[i] = 1.0 - exp(-act_dynamics[i] * dt_1l);
    Bound(act_dynamics_d[i], 0.00001, 1.0);
    Bound(act_max[i], 0, MAX_PPRZ);
    Bound(act_min[i], -MAX_PPRZ, 0);
    float ratio_numerator = act_max[i] - act_min[i];
    ratio_numerator = positive_non_zero(ratio_numerator); // make sure numerator is non-zero
    float ratio_denominator = act_max_norm[i] - act_min_norm[i];
    ratio_denominator = positive_non_zero(ratio_denominator); // make sure denominator is non-zero
    ratio_u_un[i] = ratio_numerator / ratio_denominator;
    ratio_u_un[i] = positive_non_zero(ratio_u_un[i]); // make sure ratio is not zero
    // printf("ratio_u_un[%d]= %f \n",i,ratio_u_un[i]);
  }
  for (i = 0; i < ANDI_OUTPUTS; i++)
  {
    float ratio_numerator = positive_non_zero(nu_norm_max);
    float ratio_denominator = 1.0;
    switch (i)
    {
    case (RW_aN):
    case (RW_aE):
    case (RW_aD):
      ratio_denominator = positive_non_zero(max_j_lin);
      ratio_vn_v[i] = ratio_numerator / ratio_denominator;
      break;
    case (RW_ap):
      ratio_denominator = positive_non_zero(sta_bounds.att_3d[0]);
      ratio_vn_v[i] = ratio_numerator / ratio_denominator;
      break;
    case (RW_aq):
      ratio_denominator = positive_non_zero(sta_bounds.att_3d[1]);
      ratio_vn_v[i] = ratio_numerator / ratio_denominator;
      break;
    case (RW_ar):
      ratio_denominator = positive_non_zero(sta_bounds.att_3d[2]);
      ratio_vn_v[i] = ratio_numerator / ratio_denominator;
      break;
    }
  }
}
 
void normalize_nu(void)
{
  int8_t i;
  for (i = 0; i < ANDI_OUTPUTS; i++)
  {
    // printf("ratio_vn_v[%d] = %f \n",i,ratio_vn_v[i]);
    nu_n[i] = nu[i] * ratio_vn_v[i];
    WLS_one_p.v[i] = nu_n[i];
  }
}
 
 
void oneloop_from_nav(bool in_flight)
{
  if (!in_flight)
  {
    oneloop_andi_enter(false, oneloop_andi.ctrl_type);
  }
  struct FloatVect3 PSA_des;
  PSA_des.x = stateGetPositionNed_f()->x;
  PSA_des.y = stateGetPositionNed_f()->y;
  PSA_des.z = stateGetPositionNed_f()->z;
  int rm_order_h = 3;
  int rm_order_v = 3;
  // Oneloop controller wants desired targets and handles reference generation internally
  switch (nav.setpoint_mode)
  {
  case NAV_SETPOINT_MODE_POS:
    PSA_des.x = POS_FLOAT_OF_BFP(POS_BFP_OF_REAL(nav.target.y));
    PSA_des.y = POS_FLOAT_OF_BFP(POS_BFP_OF_REAL(nav.target.x));
    rm_order_h = 3;
    break;
  case NAV_SETPOINT_MODE_SPEED:
    PSA_des.x = SPEED_FLOAT_OF_BFP(SPEED_BFP_OF_REAL(nav.speed.y));
    PSA_des.y = SPEED_FLOAT_OF_BFP(SPEED_BFP_OF_REAL(nav.speed.x));
    rm_order_h = 2;
    break;
  }
  switch (nav.vertical_mode)
  {
  case NAV_VERTICAL_MODE_ALT:
    PSA_des.z = POS_FLOAT_OF_BFP(-POS_BFP_OF_REAL(nav.nav_altitude));
    rm_order_v = 3;
    break;
  case NAV_VERTICAL_MODE_CLIMB:
    PSA_des.z = SPEED_FLOAT_OF_BFP(-SPEED_BFP_OF_REAL(nav.climb));
    rm_order_v = 2;
    break;
  }
  oneloop_andi_run(in_flight, false, PSA_des, rm_order_h, rm_order_v);
}
 
float oneloop_andi_sideslip(void)
{
  // printf("Calculating the sideslip correction\n");
  //  Coordinated turn
  //  feedforward estimate angular rotation omega = g*tan(phi)/v
  float omega = 0.0;
  const float max_phi = ONELOOP_ANDI_MAX_BANK; // RadOfDeg(ONELOOP_ANDI_MAX_BANK);
  float airspeed_turn = airspeed_filt.o[0];
  Bound(airspeed_turn, 1.0f, 30.0f);
  // Use the current roll angle to determine the corresponding heading rate of change.
  float coordinated_turn_roll = eulers_zxy.phi;
  // Prevent flipping
  if ((eulers_zxy.theta > 0.0f) && (fabs(eulers_zxy.phi) < eulers_zxy.theta))
  {
    // printf("Preventing flipping\n");
    coordinated_turn_roll = ((eulers_zxy.phi > 0.0f) - (eulers_zxy.phi < 0.0f)) * eulers_zxy.theta;
  }
  BoundAbs(coordinated_turn_roll, max_phi);
  omega = g / airspeed_turn * tanf(coordinated_turn_roll);
// printf("Omega: %f\n", omega);
// printf("Coordinated turn roll: %f\n", DegOfRad(coordinated_turn_roll));
// printf("phi: %f\n", DegOfRad(eulers_zxy.phi));
// printf("Airspeed: %f\n", airspeed_turn);
#ifdef FWD_SIDESLIP_GAIN
  // Add sideslip correction
  omega -= accely_filt.o[0] * fwd_sideslip_gain;
// printf("Omega ay: %f\n", omega);
#endif
  return omega;
}
 
static float chirp_pos_p_ref(float delta_t, float f0, float k, float A)
{
  float p_ref_fun = sinf(delta_t * M_PI * (f0 + k * delta_t) * 2.0);
  return (A * p_ref_fun);
}
static float chirp_pos_v_ref(float delta_t, float f0, float k, float A)
{
  float v_ref_fun = cosf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * (M_PI * (f0 + k * delta_t) * 2.0 + k * delta_t * M_PI * 2.0);
  return (A * v_ref_fun);
}
static float chirp_pos_a_ref(float delta_t, float f0, float k, float A)
{
  float a_ref_fun = -sinf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * pow((M_PI * (f0 + k * delta_t) * 2.0 + k * delta_t * M_PI * 2.0), 2) + k * M_PI * cosf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * 4.0;
  return (A * a_ref_fun);
}
static float chirp_pos_j_ref(float delta_t, float f0, float k, float A)
{
  float j_ref_fun = -cosf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * pow((M_PI * (f0 + k * delta_t) * 2.0 + k * delta_t * M_PI * 2.0), 3) - k * M_PI * sinf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * (M_PI * (f0 + k * delta_t) * 2.0 + k * delta_t * M_PI * 2.0) * 1.2e+1;
  return (A * j_ref_fun);
}
 
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[])
{
  float A_backup = A;
  f0 = positive_non_zero(f0);
  f1 = positive_non_zero(f1);
  t_chirp = positive_non_zero(t_chirp);
  A = positive_non_zero(A);
  if ((f1 - f0) < -FLT_EPSILON)
  {
    f1 = f0;
  }
  // 0 body x, 1 body y, 2 body z, 3 pitch pref, 4 Yaw
  if (n > 6)
  {
    n = 0;
  }
  if (n < 0)
  {
    n = 0;
  }
  // I think there should not be a problem with f1 being equal to f0
  float k = (f1 - f0) / t_chirp;
  float p_ref_chirp = chirp_pos_p_ref(time_elapsed, f0, k, A);
  float v_ref_chirp = chirp_pos_v_ref(time_elapsed, f0, k, A);
  float a_ref_chirp = chirp_pos_a_ref(time_elapsed, f0, k, A);
  float j_ref_chirp = chirp_pos_j_ref(time_elapsed, f0, k, A);
 
  float spsi = sinf(psi);
  float cpsi = cosf(psi);
  float mult_0 = 0.0;
  float mult_1 = 0.0;
  float mult_2 = 0.0;
  switch (n)
  {
  case 0:
    mult_0 = cpsi;
    mult_1 = spsi;
    mult_2 = 0.0;
 
    p_ref[0] = p_ref_0[0] + p_ref_chirp * mult_0;
    p_ref[1] = p_ref_0[1] + p_ref_chirp * mult_1;
    v_ref[0] = v_ref_chirp * mult_0;
    v_ref[1] = v_ref_chirp * mult_1;
    a_ref[0] = a_ref_chirp * mult_0;
    a_ref[1] = a_ref_chirp * mult_1;
    j_ref[0] = j_ref_chirp * mult_0;
    j_ref[1] = j_ref_chirp * mult_1;
    break;
  case 1:
    mult_0 = -spsi;
    mult_1 = cpsi;
    mult_2 = 0.0;
 
    p_ref[0] = p_ref_0[0] + p_ref_chirp * mult_0;
    p_ref[1] = p_ref_0[1] + p_ref_chirp * mult_1;
    v_ref[0] = v_ref_chirp * mult_0;
    v_ref[1] = v_ref_chirp * mult_1;
    a_ref[0] = a_ref_chirp * mult_0;
    a_ref[1] = a_ref_chirp * mult_1;
    j_ref[0] = j_ref_chirp * mult_0;
    j_ref[1] = j_ref_chirp * mult_1;
    break;
  case 2:
    mult_0 = 0.0;
    mult_1 = 0.0;
    mult_2 = 1.0;
    p_ref[2] = p_ref_0[2] + p_ref_chirp * mult_2;
    v_ref[2] = v_ref_chirp * mult_2;
    a_ref[2] = a_ref_chirp * mult_2;
    j_ref[2] = j_ref_chirp * mult_2;
    break;
  case 3:
    // Pitch preferred chirp
    pitch_pref = p_ref_chirp;
    pitch_pref = (pitch_pref / A + 1.0) * (theta_pref_max / 2.0);
    float pitch_offset = RadOfDeg(5.0);
    pitch_pref = pitch_pref + pitch_offset;
    Bound(pitch_pref, 0.0, 25.0);
    break;
  case 4:
    // Do a yaw acceleration chirp
    oneloop_andi.sta_ref.att[2] = psi_des_rad + p_ref_chirp * M_PI / 180.0; // oneloop_andi.sta_state.att[2];
    NormRadAngle(oneloop_andi.sta_ref.att[2]);
    oneloop_andi.sta_ref.att_d[2] = v_ref_chirp * M_PI / 180.0;  // oneloop_andi.sta_state.att_d[2];
    oneloop_andi.sta_ref.att_2d[2] = a_ref_chirp * M_PI / 180.0; // p_ref_chirp;
    oneloop_andi.sta_ref.att_3d[2] = j_ref_chirp * M_PI / 180.0; // v_ref_chirp;
    // Include change in dynamics
    oneloop_andi_sigma = (oneloop_andi_sigma_min - oneloop_andi_sigma_max) / t_chirp * time_elapsed + oneloop_andi_sigma_max;
    Bound(oneloop_andi_sigma, oneloop_andi_sigma_min, oneloop_andi_sigma_max);
    break;
  case 5:
    oneloop_andi.sta_ref.att[2] = oneloop_andi.sta_state.att[2];
    oneloop_andi.sta_ref.att_d[2] = oneloop_andi.sta_state.att_d[2];
    if (time_elapsed < (t_chirp / 4.0) || (time_elapsed > (3.0 * t_chirp / 4.0)))
    {
      oneloop_andi.sta_ref.att_2d[2] = 0.0;
    }
    else
    {
      oneloop_andi.sta_ref.att_2d[2] = A_backup * M_PI / 180.0;
    }
    oneloop_andi.sta_ref.att_3d[2] = 0.0;
    float S_psi = 0.5 * A_backup * M_PI / 180.0 * t_chirp * t_chirp;
    BoundAbs(S_psi, 0.9 * M_PI_2)
        psi_des_rad = oneloop_andi.sta_state.att[2] + S_psi;
    NormRadAngle(psi_des_rad);
    // Set the EC to track a 0 acceleration in Z
    p_ref[2] = oneloop_andi.gui_state.pos[2];
    v_ref[2] = oneloop_andi.gui_state.vel[2];
    a_ref[2] = 0.0;
    drop_aN = true;
    drop_aE = true;
 
    break;
  case 6:
    // Do a doublet on the roll acceleration
    oneloop_andi.sta_ref.att[0] = oneloop_andi.sta_state.att[0];
    oneloop_andi.sta_ref.att_d[0] = oneloop_andi.sta_state.att_d[0];
    // oneloop_andi.sta_ref.att_2d[0] = (time_elapsed < (t_chirp/2.0)) ? A*M_PI/180.0 : -A*M_PI/180.0;
    if (time_elapsed < (0.5))
    {
      oneloop_andi.sta_ref.att_2d[0] = 0.0;
    }
    else
    {
      oneloop_andi.sta_ref.att_2d[0] = -A * M_PI / 180.0;
    }
    oneloop_andi.sta_ref.att_3d[0] = 0.0;
    drop_aN = true;
    drop_aE = true;
    break;
  }
}
 
void chirp_call(bool *chirp_on, bool *chirp_first_call, float *t_0, 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[])
{
  if (*chirp_on)
  {
    if (*chirp_first_call)
    {
      *time_elapsed = 0.0;
      *chirp_first_call = false;
      *t_0 = get_sys_time_float();
      p_ref_0[0] = p_ref[0];
      p_ref_0[1] = p_ref[1];
      p_ref_0[2] = p_ref[2];
    }
    if (*time_elapsed < t_chirp)
    {
      *time_elapsed = get_sys_time_float() - *t_0;
      chirp_pos(*time_elapsed, f0, f1, t_chirp, A, n, psi, p_ref, v_ref, a_ref, j_ref, p_ref_0);
    }
    else
    {
      *chirp_on = false;
      *chirp_first_call = true;
      *time_elapsed = 0.0;
      *t_0 = 0.0;
      p_ref_0[0] = p_ref[0];
      p_ref_0[1] = p_ref[1];
      p_ref_0[2] = p_ref[2];
      float_vect_zero(v_ref, 3);
      float_vect_zero(a_ref, 3);
      float_vect_zero(j_ref, 3);
      drop_yaw = false;
      drop_roll = false;
      drop_pitch = false;
      drop_aN = false;
      drop_aE = false;
      drop_aD = false;
      // oneloop_andi_enter(false, oneloop_andi.ctrl_type);
    }
  }
}
 
bool autopilot_in_flight_end_detection(bool motors_on UNUSED)
{
  return !motors_on;
}
 
void reshape_wind(void)
{
  float psi = eulers_zxy.psi;
  float cpsi = cosf(psi);
  float spsi = sinf(psi);
  float airspeed = airspeed_filt.o[0];
  struct FloatVect2 NT_v_NE = {nav_target[0], nav_target[1]}; // Nav target in North and East frame
  struct FloatVect2 airspeed_v = {cpsi * airspeed, spsi * airspeed};
  struct FloatVect2 windspeed;
  struct FloatVect2 groundspeed = {oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.vel[1]};
  struct FloatVect2 des_as_NE;
  struct FloatVect2 des_as_B;
  struct FloatVect2 des_acc_B;
  VECT2_DIFF(windspeed, groundspeed, airspeed_v); // Wind speed in North and East frame
  VECT2_DIFF(des_as_NE, NT_v_NE, windspeed);      // Desired airspeed in North and East frame
  float norm_des_as = FLOAT_VECT2_NORM(des_as_NE);
  gi_unbounded_airspeed_sp = norm_des_as;
  // Check if some minimum airspeed is desired (e.g. to prevent stall)
  if (norm_des_as < min_as)
  {
    norm_des_as = min_as;
  }
  nav_target_new[0] = NT_v_NE.x;
  nav_target_new[1] = NT_v_NE.y;
  // if the desired airspeed is larger than the max airspeed or we are in force forward reshape gs des to cancel wind and fly at max airspeed
  if ((norm_des_as > max_as) || (force_forward))
  {
    float groundspeed_factor = 0.0f;
    if (FLOAT_VECT2_NORM(windspeed) < max_as)
    {
      float av = NT_v_NE.x * NT_v_NE.x + NT_v_NE.y * NT_v_NE.y; // norm squared of nav target
      float bv = -2.f * (windspeed.x * NT_v_NE.x + windspeed.y * NT_v_NE.y);
      float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - max_as * max_as;
      float dv = bv * bv - 4.0f * av * cv;
      // dv can only be positive, but just in case
      if (dv < 0.0f)
      {
        dv = fabsf(dv);
      }
      float d_sqrt = sqrtf(dv);
      groundspeed_factor = (-bv + d_sqrt) / (2.0f * av);
    }
    des_as_NE.x = groundspeed_factor * NT_v_NE.x - windspeed.x;
    des_as_NE.y = groundspeed_factor * NT_v_NE.y - windspeed.y;
    NT_v_NE.x = groundspeed_factor * NT_v_NE.x;
    NT_v_NE.y = groundspeed_factor * NT_v_NE.y;
    norm_des_as = max_as;
  }
  des_as_B.x = norm_des_as; // Desired airspeed in body x frame
  des_as_B.y = 0.0;         // Desired airspeed in body y frame
  if (((airspeed > ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD) && (norm_des_as > (ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD + 2.0f))) || (force_forward))
  {
    float delta_psi = atan2f(des_as_NE.y, des_as_NE.x) - psi;
    FLOAT_ANGLE_NORMALIZE(delta_psi);
    des_acc_B.y = delta_psi * 5.0;                          // gih_params.heading_bank_gain;
    des_acc_B.x = (des_as_B.x - airspeed) * k_pos_rm.k2[0]; // gih_params.speed_gain;
    acc_body_bound(&des_acc_B, max_a_nav);                  // Scale down side acceleration if norm is too large
    nav_target_new[0] = cpsi * des_acc_B.x - spsi * des_acc_B.y;
    nav_target_new[1] = spsi * des_acc_B.x + cpsi * des_acc_B.y;
  }
  else
  {
    nav_target_new[0] = (NT_v_NE.x - groundspeed.x) * k_pos_rm.k2[0];
    nav_target_new[1] = (NT_v_NE.y - groundspeed.y) * k_pos_rm.k2[1];
  }
  vect_bound_nd(nav_target_new, max_a_nav, 2);
}
 
void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed)
{
  min_as = min_airspeed;
  max_as = max_airspeed;
}
 
void oneloop_axis_effectiveness_calc(void)
{
  // Reset the effectiveness matrix
  memset(n_array, 0, sizeof(n_array));
  memset(coupling_factor, 0, sizeof(coupling_factor));
  memset(m_array, 0, sizeof(m_array));
  float u_trim[ANDI_NUM_ACT_TOT];
  u_trim[COMMAND_MOTOR_FRONT] = 4800.0;
  u_trim[COMMAND_MOTOR_RIGHT] = 4800.0;
  u_trim[COMMAND_MOTOR_BACK] = 4800.0;
  u_trim[COMMAND_MOTOR_LEFT] = 4800.0;
#ifndef ONELOOP_ANDI_SIMPLE_QUAD
  u_trim[COMMAND_MOTOR_PUSHER] = 0.0;
  u_trim[COMMAND_ELEVATOR] = RW.ele_pref;
  u_trim[COMMAND_RUDDER] = 0.0;
  u_trim[COMMAND_AILERONS] = 0.0;
  u_trim[COMMAND_FLAPS] = 0.0;
#endif
  u_trim[COMMAND_ROLL] = 0.0;
  u_trim[COMMAND_PITCH] = 0.0;
 
  for (int i = 0; i < ANDI_OUTPUTS; i++)
  {
    for (int j = 0; j < ANDI_NUM_ACT_TOT; j++)
    {
      float eff = positive_non_zero(fabsf(EFF_MAT_G[i][j]));
      float den = positive_non_zero(ratio_u_un[j] * ratio_vn_v[i]);
      float u_range = MAX_PPRZ - u_trim[j];
      eff = (eff / den) * u_range;
      n_array[i] += eff;
      m_array[j] += eff;
    }
  }
  for (int i = 0; i < ANDI_OUTPUTS; i++)
  {
    for (int j = 0; j < ANDI_NUM_ACT_TOT; j++)
    {
      float eff = positive_non_zero(fabsf(EFF_MAT_G[i][j]));
      float den = positive_non_zero(ratio_u_un[j] * ratio_vn_v[i]);
      float u_range = MAX_PPRZ - u_trim[j];
      eff = (eff / den) * u_range;
      float eff2 = eff * eff;
      coupling_factor[i] += eff2 / (n_array[i] * m_array[j]);
    }
  }
  Bound(coupling_factor[5], 0.2, 0.8);
  // printf("Coupling factor: %f %f %f %f %f %f \n", coupling_factor[0], coupling_factor[1], coupling_factor[2], coupling_factor[3], coupling_factor[4], coupling_factor[5]);
  // printf("Axis effectiven: %f %f %f %f %f %f \n", n_array[0], n_array[1], n_array[2], n_array[3], n_array[4], n_array[5]);
}
 
void oneloop_calc_model_disturbance(bool in_flight)
{
  int8_t i;
  int8_t j;
  for (i = 0; i < ANDI_NUM_ACT_TOT; i++)
  {
    if (i < ANDI_NUM_ACT)
    {
      update_butterworth_2_low_pass(&u_filt[i], actuator_state_1l[i]);
    }
    else
    {
      update_butterworth_2_low_pass(&u_filt[i], oneloop_andi.sta_state.att[i - ANDI_NUM_ACT]);
    }
  }
  // Store the distrubance
  float k3;
  if (in_flight)
  {
    for (i = 0; i < ANDI_OUTPUTS; i++)
    { // For loop for prediction of acceleration
      oneloop_andi_model[i] = 0.0;
      switch (i)
      {
      case (RW_aN):
        k3 = 1.0; // k_pos_e.k3[0];
        break;
      case (RW_aE):
        k3 = 1.0; // k_pos_e.k3[1];
        break;
      case (RW_aD):
        k3 = 1.0; // k_pos_e.k3[2];
        break;
      case (RW_ap):
        k3 = k_att_e.k3[0];
        break;
      case (RW_aq):
        k3 = k_att_e.k3[1];
        break;
      case (RW_ar):
        k3 = k_att_e.k3[2];
        break;
      }
      k3 = positive_non_zero(k3);
      if (i == RW_aN)
      {
        // printf("start oneloop_andi_model             : %f \n",oneloop_andi_model[i]);
      }
      for (j = 0; j < ANDI_NUM_ACT_TOT; j++)
      {
        float den = positive_non_zero(ratio_u_un[j] * ratio_vn_v[i]);
        float num = u_filt[j].o[0] * EFF_MAT_G[i][j];
        oneloop_andi_model[i] += num / den;
      }
      oneloop_andi_model[i] = oneloop_andi_model[i] / k3;
    }
    update_filter_on_type(&oneloop_andi_model_filt.ax, oneloop_andi_model[RW_aN]);
    update_filter_on_type(&oneloop_andi_model_filt.ay, oneloop_andi_model[RW_aE]);
    update_filter_on_type(&oneloop_andi_model_filt.az, oneloop_andi_model[RW_aD]);
    update_filter_on_type(&oneloop_andi_model_filt.p_dot, oneloop_andi_model[RW_ap]);
    update_filter_on_type(&oneloop_andi_model_filt.q_dot, oneloop_andi_model[RW_aq]);
    update_filter_on_type(&oneloop_andi_model_filt.r_dot, oneloop_andi_model[RW_ar]);
  }
  else
  {
    float_vect_zero(oneloop_andi_model, ANDI_OUTPUTS);
  }
}
void oneloop_andi_bound_disturbance(void)
{
  oneloop_andi_dist_bound[RW_aN] = oneloop_andi.gui_state.acc[0] - oneloop_andi_model[RW_aN];
  BoundAbs(oneloop_andi_dist_bound[RW_aN], 99999.f); // Large value to not bound
  oneloop_andi_dist_bound[RW_aE] = oneloop_andi.gui_state.acc[1] - oneloop_andi_model[RW_aE];
  BoundAbs(oneloop_andi_dist_bound[RW_aE], 99999.f); // Large value to not bound
  oneloop_andi_dist_bound[RW_aD] = oneloop_andi.gui_state.acc[2] - oneloop_andi_model[RW_aD];
  BoundAbs(oneloop_andi_dist_bound[RW_aD], 99999.f); // Large value to not bound
  oneloop_andi_dist_bound[RW_ap] = oneloop_andi.sta_state.att_2d[0] - oneloop_andi_model[RW_ap];
  BoundAbs(oneloop_andi_dist_bound[RW_ap], 99999.f); // Large value to not bound
  oneloop_andi_dist_bound[RW_aq] = oneloop_andi.sta_state.att_2d[1] - oneloop_andi_model[RW_aq];
  BoundAbs(oneloop_andi_dist_bound[RW_aq], 99999.f); // Large value to not bound
  oneloop_andi_dist_bound[RW_ar] = oneloop_andi.sta_state.att_2d[2] - oneloop_andi_model[RW_ar];
  BoundAbs(oneloop_andi_dist_bound[RW_ar], 99999.f); // Large value to not bound
                                                     // BoundAbs(oneloop_andi_dist_bound[RW_ar], oneloop_andi_yaw_dist_limit); // Bound from INDI controller
}
 
// void dynFilter_run(struct Oneloop_DynFilt_t *mu, float u_c, float sigma){
//   mu->mu_c_0      = mu->mu_c;
//   mu->u_c_0       = mu->u_c;
//   mu->u_c         = u_c;
//   mu->sigma       = sigma;
//   float c_den     = (mu->fs+mu->sigma)*mu->varepsilon;
//   float c_u_c     = (mu->fs*mu->sigma+mu->varepsilon*mu->sigma)/c_den;
//   float c_u_c_0   = (-mu->fs*mu->sigma)/c_den;
//   float c_mu_c_0  = (mu->fs*mu->varepsilon)/c_den;
//   mu->mu_c        = c_u_c * mu->u_c + c_u_c_0 * mu->u_c_0 + c_mu_c_0 * mu->mu_c_0;
// }
void dynFilter_run(struct Oneloop_DynFilt_t *mu, float u_c, float sigma)
{
  mu->mu_c_0 = mu->mu_c;
  mu->u_c_0 = mu->u_c;
  mu->u_c = u_c;
  mu->sigma = sigma;
 
  // Calculate ZOH-based coefficients
  float exp_term = expf(-mu->sigma / mu->fs); // Use expf for float precision
  float c_u_c = mu->sigma / mu->varepsilon;
  float c_u_c_0 = (-mu->sigma + mu->varepsilon - mu->varepsilon * exp_term) / mu->varepsilon;
  float c_mu_c_0 = exp_term;
 
  mu->mu_c = c_u_c * mu->u_c + c_u_c_0 * mu->u_c_0 + c_mu_c_0 * mu->mu_c_0;
}
 
void dynFilter_init(struct Oneloop_DynFilt_t *mu, float varepsilon, float sigma)
{
  mu->fs = (float)PERIODIC_FREQUENCY;
  mu->varepsilon = varepsilon;
  mu->sigma = sigma;
  mu->mu_c = 0.0;
  mu->u_c = 0.0;
  mu->mu_c_0 = 0.0;
  mu->u_c_0 = 0.0;
  // printf("DynFilter initialized with varepsilon: %f, sigma: %f, fs: %f\n", mu->varepsilon, mu->sigma, mu->fs);
}
 
void guidance_set_max_bank_angle(float max_bank)
{
  (void)max_bank;
}
 
void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd)
{
  (void)max_climb_speed_quad;
  (void)max_climb_speed_fwd;
}
 
void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd)
{
  (void)max_descend_speed_quad;
  (void)max_descend_speed_fwd;
}