Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "firmwares/rotorcraft/oneloop/oneloop_andi.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "generated/airframe.h"
#include "modules/radio_control/radio_control.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"
#include "math/wls/wls_alloc.h"
#include "modules/nav/nav_rotorcraft_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
#include "modules/rotwing_drone/rotwing_state_V2.h"
#include "modules/core/commands.h"
#include "modules/ctrl/eff_scheduling_rotwing_V2.h"
#include <stdio.h>
#include "modules/gps/gps.h"
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Macros | |
#define | ONELOOP_ANDI_SCHEDULING FALSE |
#define | ONELOOP_ANDI_DEBUG_MODE FALSE |
#define | ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL] |
#define | ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL] |
#define | ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH] |
#define | ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0 |
Functions | |
void | init_poles (void) |
Initialize Position of Poles. More... | |
void | calc_normalization (void) |
Calculate Normalization of actuators and discrete actuator dynamics More... | |
void | normalize_nu (void) |
Function to normalize the pseudo control vector. More... | |
void | G1G2_oneloop (int ctrl_type) |
Function that samples and scales the effectiveness matrix FIXME: make this function into a for loop to make it more adaptable to different configurations. More... | |
void | get_act_state_oneloop (void) |
Function to reconstruct actuator state using first order dynamics. More... | |
void | oneloop_andi_propagate_filters (void) |
Propagate the filters. More... | |
void | init_filter (void) |
Initialize the filters. More... | |
void | init_controller (void) |
Initialize Controller Gains FIXME: Calculate the gains dynamically for transition. More... | |
void | float_rates_of_euler_dot_vec (float r[3], float e[3], float edot[3]) |
Attitude Rates to Euler Conversion Function. More... | |
void | float_euler_dot_of_rates_vec (float r[3], float e[3], float edot[3]) |
Attitude Euler to Rates Conversion Function. More... | |
void | err_nd (float err[], float a[], float b[], float k[], int n) |
Calculate Scaled Error between two 3D arrays. More... | |
void | err_sum_nd (float err[], float a[], float b[], float k[], float c[], int n) |
Calculate Scaled Error between two 3D arrays. More... | |
void | integrate_nd (float dt, float a[], float a_dot[], int n) |
Integrate in time 3D array. More... | |
void | vect_bound_nd (float vect[], float bound, int n) |
Scale a 3D array to within a 3D bound. More... | |
void | acc_body_bound (struct FloatVect2 *vect, float bound) |
Scale a 3D array to within a 3D bound. More... | |
float | bound_v_from_a (float e_x[], float v_bound, float a_bound, int n) |
Calculate velocity limit based on acceleration limit. More... | |
void | rm_2nd (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float x_des, float k1_rm, float k2_rm) |
Reference Model Definition for 2nd order system. More... | |
void | rm_3rd (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float *x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm) |
Reference Model Definition for 3rd order system. More... | |
void | rm_3rd_head (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float *x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm) |
Reference Model Definition for 3rd order system specific to the heading angle. More... | |
void | rm_3rd_attitude (float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3], float max_ang_jerk) |
Reference Model Definition for 3rd order system with attitude conversion functions. More... | |
void | rm_3rd_pos (float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_des[], float k1_rm[], float k2_rm[], float k3_rm[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n) |
Reference Model Definition for 3rd order system specific to positioning with bounds. More... | |
void | rm_2nd_pos (float dt, float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_d_des[], float k2_rm[], float k3_rm[], float x_2d_bound, float x_3d_bound, int n) |
Reference Model Definition for 3rd order system specific to positioning with bounds. More... | |
void | rm_1st_pos (float dt, float x_2d_ref[], float x_3d_ref[], float x_2d_des[], float k3_rm[], float x_3d_bound, int n) |
Reference Model Definition for 3rd order system specific to positioning with bounds. More... | |
void | ec_3rd_att (float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3], float max_ang_jerk) |
Error Controller Definition for 3rd order system specific to attitude. More... | |
void | ec_3rd_pos (float y_4d[], float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x[], float x_d[], float x_2d[], float k1_e[], float k2_e[], float k3_e[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n) |
void | calc_model (void) |
Function that calculates the model prediction for the complementary filter. More... | |
float | oneloop_andi_sideslip (void) |
Function to calculate corrections for sideslip. More... | |
void | reshape_wind (void) |
void | chirp_pos (float time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[]) |
Reference Model Definition for 3rd order system specific to positioning with bounds. More... | |
void | chirp_call (bool *chirp_on, bool *chirp_first_call, float *t_0_chirp, float *time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[]) |
static void | send_wls_v_oneloop (struct transport_tx *trans, struct link_device *dev) |
static void | send_wls_u_oneloop (struct transport_tx *trans, struct link_device *dev) |
static void | send_eff_mat_stab_oneloop_andi (struct transport_tx *trans, struct link_device *dev) |
static void | send_eff_mat_guid_oneloop_andi (struct transport_tx *trans, struct link_device *dev) |
static void | send_oneloop_andi (struct transport_tx *trans, struct link_device *dev) |
static void | send_guidance_oneloop_andi (struct transport_tx *trans, struct link_device *dev) |
static void | debug_vect (struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize) |
static void | send_oneloop_debug (struct transport_tx *trans, struct link_device *dev) |
static float | positive_non_zero (float input) |
Function to make sure that inputs are positive non zero vaues. More... | |
static float | k_e_1_2_f_v2 (float omega, float zeta) |
Error Controller Gain Design. More... | |
static float | k_e_2_2_f_v2 (float omega, float zeta) |
static float | k_e_1_3_f_v2 (float omega_n, UNUSED float zeta, float p1) |
static float | k_e_2_3_f_v2 (float omega_n, float zeta, float p1) |
static float | k_e_3_3_f_v2 (float omega_n, float zeta, float p1) |
static float | k_rm_1_3_f (float omega_n, float zeta, float p1) |
Reference Model Gain Design. More... | |
static float | k_rm_2_3_f (float omega_n, float zeta, float p1) |
static float | k_rm_3_3_f (float omega_n, float zeta, float p1) |
static float | ec_3rd (float x_ref, float x_d_ref, float x_2d_ref, float x_3d_ref, float x, float x_d, float x_2d, float k1_e, float k2_e, float k3_e) |
Error Controller Definition for 3rd order system. More... | |
static float | w_approx (float p1, float p2, float p3, float rm_k) |
Third Order to First Order Dynamics Approximation. More... | |
void | oneloop_andi_init (void) |
Init function of Oneloop ANDI controller More... | |
void | oneloop_andi_enter (bool half_loop_sp, int ctrl_type) |
Function that resets important values upon engaging Oneloop ANDI. More... | |
void | oneloop_andi_RM (bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop) |
Function to generate the reference signals for the oneloop controller. More... | |
void | oneloop_andi_run (bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v) |
Main function that runs the controller and performs control allocation. More... | |
void | oneloop_from_nav (bool in_flight) |
Function that maps navigation inputs to the oneloop controller for the generated autopilot. More... | |
static float | chirp_pos_p_ref (float delta_t, float f0, float k, float A) |
Function to calculate the position reference during the chirp. More... | |
static float | chirp_pos_v_ref (float delta_t, float f0, float k, float A) |
Function to calculate the velocity reference during the chirp. More... | |
static float | chirp_pos_a_ref (float delta_t, float f0, float k, float A) |
Function to calculate the acceleration reference during the chirp. More... | |
static float | chirp_pos_j_ref (float delta_t, float f0, float k, float A) |
Function to calculate the jerk reference during the chirp. More... | |
bool | autopilot_in_flight_end_detection (bool motors_on UNUSED) |
Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts with all thrust off. More... | |
#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0 |
Definition at line 238 of file oneloop_andi.c.
#define ONELOOP_ANDI_DEBUG_MODE FALSE |
Definition at line 213 of file oneloop_andi.c.
#define ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL] |
Definition at line 217 of file oneloop_andi.c.
#define ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL] |
Definition at line 218 of file oneloop_andi.c.
#define ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH] |
Definition at line 220 of file oneloop_andi.c.
#define ONELOOP_ANDI_SCHEDULING FALSE |
Definition at line 109 of file oneloop_andi.c.
void acc_body_bound | ( | struct FloatVect2 * | vect, |
float | bound | ||
) |
Scale a 3D array to within a 3D bound.
Definition at line 677 of file oneloop_andi.c.
References float_vect_norm(), Min, positive_non_zero(), FloatVect2::x, and FloatVect2::y.
Referenced by reshape_wind().
bool autopilot_in_flight_end_detection | ( | bool motors_on | UNUSED | ) |
Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts with all thrust off.
Definition at line 1974 of file oneloop_andi.c.
float bound_v_from_a | ( | float | e_x[], |
float | v_bound, | ||
float | a_bound, | ||
int | n | ||
) |
Calculate velocity limit based on acceleration limit.
Definition at line 693 of file oneloop_andi.c.
References float_vect_norm().
Referenced by rm_3rd_pos().
void calc_model | ( | void | ) |
Function that calculates the model prediction for the complementary filter.
Definition at line 1762 of file oneloop_andi.c.
References actuator_state_1l, ANDI_NUM_ACT, ANDI_OUTPUTS, EFF_MAT_RW, RW_Model::ele_pref, eulers_zxy, g, wing_model::L, RW_Model::m, model_pred, RW_Model::P, P, FloatEulers::phi, FloatEulers::psi, RW, RW_Model::T, mesonh.mesonh_atmosphere::T, FloatEulers::theta, and RW_Model::wing.
Referenced by oneloop_andi_propagate_filters().
void calc_normalization | ( | void | ) |
Calculate Normalization of actuators and discrete actuator dynamics
Definition at line 1718 of file oneloop_andi.c.
References act_dynamics, act_dynamics_d, act_max, act_max_norm, act_min, act_min_norm, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, dt_1l, max_j_ang, max_j_nav, MAX_PPRZ, nu_norm_max, positive_non_zero(), ratio_u_un, ratio_vn_v, RW_aD, RW_aE, RW_aN, RW_ap, RW_aq, and RW_ar.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().
void chirp_call | ( | bool * | chirp_on, |
bool * | chirp_first_call, | ||
float * | t_0_chirp, | ||
float * | time_elapsed, | ||
float | f0, | ||
float | f1, | ||
float | t_chirp, | ||
float | A, | ||
int8_t | n, | ||
float | psi, | ||
float | p_ref[], | ||
float | v_ref[], | ||
float | a_ref[], | ||
float | j_ref[], | ||
float | p_ref_0[] | ||
) |
Definition at line 1950 of file oneloop_andi.c.
References A, chirp_first_call, chirp_on, chirp_pos(), OneloopGeneral::ctrl_type, get_sys_time_float(), oneloop_andi, oneloop_andi_enter(), p_ref_0, and t_chirp.
Referenced by oneloop_andi_RM().
void chirp_pos | ( | float | time_elapsed, |
float | f0, | ||
float | f1, | ||
float | t_chirp, | ||
float | A, | ||
int8_t | n, | ||
float | psi, | ||
float | p_ref[], | ||
float | v_ref[], | ||
float | a_ref[], | ||
float | j_ref[], | ||
float | p_ref_0[] | ||
) |
Reference Model Definition for 3rd order system specific to positioning with bounds.
dt | [s] time passed since start of the chirp |
f0 | [Hz] initial frequency of the chirp |
f1 | [Hz] final frequency of the chirp |
t_chirp | [s] duration of the chirp |
p_ref | [m] position reference |
v_ref | [m/s] velocity reference |
a_ref | [m/s2] acceleration reference |
j_ref | [m/s3] jerk reference |
Definition at line 1886 of file oneloop_andi.c.
References A, chirp_pos_a_ref(), chirp_pos_j_ref(), chirp_pos_p_ref(), chirp_pos_v_ref(), p_ref_0, pitch_pref, positive_non_zero(), t_chirp, and theta_pref_max.
Referenced by chirp_call().
|
static |
Function to calculate the acceleration reference during the chirp.
Definition at line 1865 of file oneloop_andi.c.
References A.
Referenced by chirp_pos().
|
static |
Function to calculate the jerk reference during the chirp.
Definition at line 1870 of file oneloop_andi.c.
References A.
Referenced by chirp_pos().
|
static |
Function to calculate the position reference during the chirp.
Definition at line 1855 of file oneloop_andi.c.
References A.
Referenced by chirp_pos().
|
static |
Function to calculate the velocity reference during the chirp.
Definition at line 1860 of file oneloop_andi.c.
References A.
Referenced by chirp_pos().
|
static |
Definition at line 521 of file oneloop_andi.c.
References dev.
Referenced by send_oneloop_debug().
|
static |
Error Controller Definition for 3rd order system.
dt | Delta time [s] |
x_ref | Reference signal 1st order |
x_d_ref | Reference signal 2nd order |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_des | Desired 1st order signal |
x | Current 1st order signal |
x_d | Current 2nd order signal |
x_2d | Current 3rd order signal |
k1_e | Error Controller Gain 1st order signal |
k2_e | Error Controller Gain 2nd order signal |
k3_e | Error Controller Gain 3rd order signal |
Definition at line 901 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
void ec_3rd_att | ( | float | y_4d[3], |
float | x_ref[3], | ||
float | x_d_ref[3], | ||
float | x_2d_ref[3], | ||
float | x_3d_ref[3], | ||
float | x[3], | ||
float | x_d[3], | ||
float | x_2d[3], | ||
float | k1_e[3], | ||
float | k2_e[3], | ||
float | k3_e[3], | ||
float | max_ang_jerk | ||
) |
Error Controller Definition for 3rd order system specific to attitude.
dt | Delta time [s] |
x_ref | Reference signal 1st order |
x_d_ref | Reference signal 2nd order |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_des | Desired 1st order signal |
x | Current 1st order signal |
x_d | Current 2nd order signal |
x_2d | Current 3rd order signal |
k1_e | Error Controller Gain 1st order signal |
k2_e | Error Controller Gain 2nd order signal |
k3_e | Error Controller Gain 3rd order signal |
Definition at line 933 of file oneloop_andi.c.
References err_nd(), float_vect_copy(), float_vect_sum(), and vect_bound_nd().
Referenced by oneloop_andi_run().
void ec_3rd_pos | ( | float | y_4d[], |
float | x_ref[], | ||
float | x_d_ref[], | ||
float | x_2d_ref[], | ||
float | x_3d_ref[], | ||
float | x[], | ||
float | x_d[], | ||
float | x_2d[], | ||
float | k1_e[], | ||
float | k2_e[], | ||
float | k3_e[], | ||
float | x_d_bound, | ||
float | x_2d_bound, | ||
float | x_3d_bound, | ||
int | n | ||
) |
Definition at line 905 of file oneloop_andi.c.
References err_sum_nd(), and vect_bound_nd().
Referenced by oneloop_andi_run().
void err_nd | ( | float | err[], |
float | a[], | ||
float | b[], | ||
float | k[], | ||
int | n | ||
) |
Calculate Scaled Error between two 3D arrays.
Definition at line 636 of file oneloop_andi.c.
References b.
Referenced by ec_3rd_att(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), and rm_3rd_pos().
void err_sum_nd | ( | float | err[], |
float | a[], | ||
float | b[], | ||
float | k[], | ||
float | c[], | ||
int | n | ||
) |
Calculate Scaled Error between two 3D arrays.
Definition at line 645 of file oneloop_andi.c.
References b.
Referenced by ec_3rd_pos().
void float_euler_dot_of_rates_vec | ( | float | r[3], |
float | e[3], | ||
float | edot[3] | ||
) |
Attitude Euler to Rates Conversion Function.
Definition at line 621 of file oneloop_andi.c.
Referenced by rm_3rd_attitude().
void float_rates_of_euler_dot_vec | ( | float | r[3], |
float | e[3], | ||
float | edot[3] | ||
) |
Attitude Rates to Euler Conversion Function.
Definition at line 609 of file oneloop_andi.c.
Referenced by rm_3rd_attitude().
void G1G2_oneloop | ( | int | ctrl_type | ) |
Function that samples and scales the effectiveness matrix FIXME: make this function into a for loop to make it more adaptable to different configurations.
Definition at line 1663 of file oneloop_andi.c.
References act_dynamics, airspeed_filt, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, CTRL_ANDI, CTRL_INDI, ctrl_off, EFF_MAT_G, EFF_MAT_RW, ELE_MIN_AS, RotWingStateSettings::hover_motors_active, SecondOrderLowPass::o, ratio_u_un, ratio_vn_v, and rotwing_state_settings.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().
void get_act_state_oneloop | ( | void | ) |
Function to reconstruct actuator state using first order dynamics.
Definition at line 1645 of file oneloop_andi.c.
References act_dynamics_d, act_max, act_min, actuator_state_1l, ANDI_NUM_ACT, andi_u, and autopilot_get_motors_on().
Referenced by oneloop_andi_run().
void init_controller | ( | void | ) |
Initialize Controller Gains FIXME: Calculate the gains dynamically for transition.
Definition at line 1019 of file oneloop_andi.c.
References act_dynamics, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_e, k_att_e_indi, k_att_rm, k_e_1_2_f_v2(), k_e_1_3_f_v2(), k_e_2_2_f_v2(), k_e_2_3_f_v2(), k_e_3_3_f_v2(), k_pos_e, k_pos_e_indi, k_pos_rm, k_rm_1_3_f(), k_rm_2_3_f(), k_rm_3_3_f(), max_a_nav, max_v_nav, nav_hybrid_max_bank, nav_hybrid_pos_gain, nav_max_acceleration_sp, nav_max_speed, PolePlacement::omega_n, ONELOOP_ANDI_MAX_BANK, PolePlacement::p3, p_alt_e, p_alt_rm, p_att_e, p_att_rm, p_head_e, p_head_rm, p_pos_e, p_pos_rm, w_approx(), and PolePlacement::zeta.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().
void init_filter | ( | void | ) |
Initialize the filters.
Definition at line 1124 of file oneloop_andi.c.
References accely_filt, airspeed_filt, ang_rate_lowpass_filters, filt_accel_ned, filt_veloc_ned, init_butterworth_2_low_pass(), model_pred_aa_filt, model_pred_ar_filt, model_pred_la_filt, oneloop_andi_filt_cutoff, oneloop_andi_filt_cutoff_a, oneloop_andi_filt_cutoff_p, oneloop_andi_filt_cutoff_q, oneloop_andi_filt_cutoff_r, oneloop_andi_filt_cutoff_v, p, rates_filt_bt, and stateGetBodyRates_f().
Referenced by oneloop_andi_enter(), and oneloop_andi_init().
void init_poles | ( | void | ) |
Initialize Position of Poles.
Definition at line 972 of file oneloop_andi.c.
References act_dynamics, PolePlacement::omega_n, PolePlacement::p3, p_alt_e, p_alt_rm, p_att_e, p_att_rm, p_head_e, p_head_rm, p_pos_e, p_pos_rm, w_approx(), and PolePlacement::zeta.
Referenced by oneloop_andi_init().
void integrate_nd | ( | float | dt, |
float | a[], | ||
float | a_dot[], | ||
int | n | ||
) |
Integrate in time 3D array.
Definition at line 655 of file oneloop_andi.c.
Referenced by rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), and rm_3rd_pos().
|
static |
Error Controller Gain Design.
Definition at line 553 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 565 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 559 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 571 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 578 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Reference Model Gain Design.
Definition at line 587 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 594 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 601 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
void normalize_nu | ( | void | ) |
Function to normalize the pseudo control vector.
Definition at line 1753 of file oneloop_andi.c.
References ANDI_OUTPUTS, nu, nu_n, ratio_vn_v, WLS_t::v, and WLS_one_p.
Referenced by oneloop_andi_run().
void oneloop_andi_enter | ( | bool | half_loop_sp, |
int | ctrl_type | ||
) |
Function that resets important values upon engaging Oneloop ANDI.
FIXME: Ideally we should distinguish between the "stabilization" and "guidance" needs because it is unlikely to switch stabilization in flight, and there are multiple modes that use (the same) stabilization. Resetting the controller is not so nice when you are flying.
Definition at line 1250 of file oneloop_andi.c.
References ANDI_OUTPUTS, ang_acc, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, calc_normalization(), OneloopGeneral::ctrl_type, EFF_MAT_G, ele_min, eulers_zxy, eulers_zxy_des, float_vect_zero(), G1G2_oneloop(), OneloopGeneral::half_loop, init_controller(), init_filter(), lin_acc, model_pred, model_pred_ar, nav_target, nav_target_new, oneloop_andi, FloatEulers::phi, FloatEulers::psi, psi_des_deg, psi_des_rad, OneloopGeneral::sta_ref, and FloatEulers::theta.
Referenced by chirp_call(), guidance_h_run_enter(), oneloop_from_nav(), and stabilization_attitude_enter().
void oneloop_andi_init | ( | void | ) |
Init function of Oneloop ANDI controller
Definition at line 1192 of file oneloop_andi.c.
References act_dynamics, actuator_state_1l, andi_du, andi_du_n, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, ang_acc, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, calc_normalization(), CTRL_ANDI, OneloopGeneral::ctrl_type, DefaultPeriodic, EFF_MAT_G, eulers_zxy_des, float_vect_zero(), G1G2_oneloop(), OneloopGeneral::half_loop, init_controller(), init_filter(), init_poles(), lin_acc, model_pred, model_pred_ar, nav_target, nav_target_new, nu, nu_n, oneloop_andi, FloatEulers::phi, positive_non_zero(), FloatEulers::psi, register_periodic_telemetry(), send_eff_mat_guid_oneloop_andi(), send_eff_mat_stab_oneloop_andi(), send_guidance_oneloop_andi(), send_oneloop_andi(), send_oneloop_debug(), send_wls_u_oneloop(), send_wls_v_oneloop(), OneloopGeneral::sta_ref, and FloatEulers::theta.
void oneloop_andi_propagate_filters | ( | void | ) |
Propagate the filters.
Definition at line 1158 of file oneloop_andi.c.
References ACCEL_FLOAT_OF_BFP, accely_filt, airspeed_filt, ang_acc, ang_rate, ang_rate_lowpass_filters, calc_model(), filt_accel_ned, filt_veloc_ned, lin_acc, model_pred, model_pred_aa_filt, model_pred_ar, model_pred_ar_filt, model_pred_la_filt, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, rates_filt_bt, stateGetAccelBody_i(), stateGetAccelNed_f(), stateGetAirspeed_f(), stateGetBodyRates_f(), stateGetSpeedNed_f(), update_butterworth_2_low_pass(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.
Referenced by oneloop_andi_run().
void oneloop_andi_RM | ( | bool | half_loop, |
struct FloatVect3 | PSA_des, | ||
int | rm_order_h, | ||
int | rm_order_v, | ||
bool | in_flight_oneloop | ||
) |
Function to generate the reference signals for the oneloop controller.
half_loop | In half-loop mode the controller is used for stabilization only |
PSA_des | Desired position/speed/acceleration |
rm_order_h | Order of the reference model for horizontal guidance |
rm_order_v | Order of the reference model for vertical guidance |
Definition at line 1289 of file oneloop_andi.c.
References A_chirp, a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, actuator_state_1l, ANDI_NUM_ACT, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, chirp_axis, chirp_call(), chirp_first_call, chirp_on, dt_1l, eulers_zxy, eulers_zxy_des, f0_chirp, f1_chirp, float_vect_copy(), float_vect_norm(), float_vect_zero(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, heading_manual, OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_rm, k_pos_rm, max_a_nav, max_j_ang, max_j_nav, MAX_PPRZ, max_r, max_v_nav, max_v_nav_v, nav_target, nav_target_new, oneloop_andi, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, oneloop_andi_sideslip(), p_ref_0, FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, psi_vec, radio_control_get(), RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, ratio_u_un, ratio_vn_v, reshape_wind(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), rm_3rd_pos(), RW_aD, OneloopGeneral::sta_ref, t_0_chirp, t_chirp, FloatEulers::theta, time_elapsed_chirp, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, WLS_one_p, WLS_t::Wv, Wv_backup, FloatVect3::x, FloatVect3::y, yaw_stick_in_auto, and FloatVect3::z.
Referenced by oneloop_andi_run().
void oneloop_andi_run | ( | bool | in_flight, |
bool | half_loop, | ||
struct FloatVect3 | PSA_des, | ||
int | rm_order_h, | ||
int | rm_order_v | ||
) |
Main function that runs the controller and performs control allocation.
half_loop | In half-loop mode the controller is used for stabilization only |
in_flight | The drone is in flight |
PSA_des | Desired position/speed/acceleration |
rm_order_h | Order of the reference model for horizontal guidance |
rm_order_v | Order of the reference model for vertical guidance |
Definition at line 1426 of file oneloop_andi.c.
References a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, act_dynamics, act_max, act_min, actuator_state_1l, andi_du, andi_du_n, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, ang_acc, ang_rate, AP_MODE_ATTITUDE_DIRECT, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, autopilot, bwls_1l, calc_normalization(), chirp_on, Stabilization::cmd, commands, CTRL_ANDI, CTRL_INDI, ctrl_off, OneloopGeneral::ctrl_type, RW_skew::deg, ec_3rd(), ec_3rd_att(), ec_3rd_pos(), EFF_MAT_G, RW_Model::ele_pref, eulers_zxy, eulers_zxy_des, filt_veloc_ned, float_eulers_of_quat_zxy(), float_vect_copy(), float_vect_sum(), G1G2_oneloop(), g2_ff, G2_RW, get_act_state_oneloop(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, heading_manual, RotWingStateSettings::hover_motors_active, init_controller(), OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, k_att_e, k_att_e_indi, k_pos_e, k_pos_e_indi, lin_acc, max_a_nav, max_j_ang, max_j_nav, MAX_PPRZ, max_v_nav, pprz_autopilot::mode, normalize_nu(), nu, num_thrusters_oneloop, SecondOrderLowPass::o, oneloop_andi, ONELOOP_ANDI_DEBUG_MODE, ONELOOP_ANDI_MAX_PHI, ONELOOP_ANDI_MAX_THETA, oneloop_andi_propagate_filters(), oneloop_andi_RM(), FloatEulers::phi, pitch_pref, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, RADIO_AUX4, RADIO_AUX5, radio_control, ratio_u_un, rotwing_state_settings, RW, RW_Model::skew, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, stabilization, RotWingStateSettings::stall_protection, stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), FloatEulers::theta, theta_pref_max, pprz_autopilot::throttle, WLS_t::u, WLS_t::u_max, WLS_t::u_min, u_pref, WLS_t::u_pref, use_increment, RadioControl::values, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, wls_alloc(), WLS_one_p, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.
Referenced by oneloop_from_nav(), and stabilization_attitude_run().
float oneloop_andi_sideslip | ( | void | ) |
Function to calculate corrections for sideslip.
Definition at line 1831 of file oneloop_andi.c.
References accely_filt, airspeed_filt, eulers_zxy, fwd_sideslip_gain, g, SecondOrderLowPass::o, ONELOOP_ANDI_MAX_BANK, FloatEulers::phi, and FloatEulers::theta.
Referenced by oneloop_andi_RM().
void oneloop_from_nav | ( | bool | in_flight | ) |
Function that maps navigation inputs to the oneloop controller for the generated autopilot.
Definition at line 1793 of file oneloop_andi.c.
References RotorcraftNavigation::climb, OneloopGeneral::ctrl_type, nav, RotorcraftNavigation::nav_altitude, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, NAV_VERTICAL_MODE_ALT, NAV_VERTICAL_MODE_CLIMB, oneloop_andi, oneloop_andi_enter(), oneloop_andi_run(), POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, RotorcraftNavigation::setpoint_mode, RotorcraftNavigation::speed, SPEED_BFP_OF_REAL, SPEED_FLOAT_OF_BFP, stateGetPositionNed_f(), RotorcraftNavigation::target, RotorcraftNavigation::vertical_mode, FloatVect3::x, NedCoor_f::x, EnuCoor_f::x, FloatVect3::y, NedCoor_f::y, EnuCoor_f::y, FloatVect3::z, and NedCoor_f::z.
|
static |
Function to make sure that inputs are positive non zero vaues.
Definition at line 543 of file oneloop_andi.c.
Referenced by acc_body_bound(), calc_normalization(), chirp_pos(), k_e_1_2_f_v2(), k_e_1_3_f_v2(), k_e_2_2_f_v2(), k_e_2_3_f_v2(), k_e_3_3_f_v2(), k_rm_1_3_f(), k_rm_2_3_f(), k_rm_3_3_f(), oneloop_andi_init(), vect_bound_nd(), and w_approx().
void reshape_wind | ( | void | ) |
Definition at line 1979 of file oneloop_andi.c.
References acc_body_bound(), airspeed_filt, eulers_zxy, FLOAT_ANGLE_NORMALIZE, FLOAT_VECT2_NORM, RotWingStateSettings::force_forward, OneloopGeneral::gui_state, Gains3rdOrder::k2, k_pos_rm, max_a_nav, max_as, nav_target, nav_target_new, SecondOrderLowPass::o, oneloop_andi, ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD, FloatEulers::psi, rotwing_state_settings, VECT2_DIFF, vect_bound_nd(), OneloopGuidanceState::vel, FloatVect2::x, and FloatVect2::y.
Referenced by oneloop_andi_RM().
void rm_1st_pos | ( | float | dt, |
float | x_2d_ref[], | ||
float | x_3d_ref[], | ||
float | x_2d_des[], | ||
float | k3_rm[], | ||
float | x_3d_bound, | ||
int | n | ||
) |
Reference Model Definition for 3rd order system specific to positioning with bounds.
dt | Delta time [s] |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_2d_des | Desired 3rd order signal |
k3_rm | Reference Model Gain 3rd order signal |
x_3d_bound | Bound for the 4th order reference signal |
n | Number of dimensions |
Definition at line 857 of file oneloop_andi.c.
References err_nd(), float_vect_copy(), integrate_nd(), and vect_bound_nd().
Referenced by oneloop_andi_RM().
void rm_2nd | ( | float | dt, |
float * | x_ref, | ||
float * | x_d_ref, | ||
float * | x_2d_ref, | ||
float | x_des, | ||
float | k1_rm, | ||
float | k2_rm | ||
) |
Reference Model Definition for 2nd order system.
dt | Delta time [s] |
x_ref | Reference signal 1st order |
x_d_ref | Reference signal 2nd order |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_des | Desired 1st order signal |
k1_rm | Reference Model Gain 1st order signal |
k2_rm | Reference Model Gain 2nd order signal |
k3_rm | Reference Model Gain 3rd order signal |
Definition at line 878 of file oneloop_andi.c.
void rm_2nd_pos | ( | float | dt, |
float | x_d_ref[], | ||
float | x_2d_ref[], | ||
float | x_3d_ref[], | ||
float | x_d_des[], | ||
float | k2_rm[], | ||
float | k3_rm[], | ||
float | x_2d_bound, | ||
float | x_3d_bound, | ||
int | n | ||
) |
Reference Model Definition for 3rd order system specific to positioning with bounds.
dt | Delta time [s] |
x_d_ref | Reference signal 2nd order |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_d_des | Desired 2nd order signal |
k2_rm | Reference Model Gain 2nd order signal |
k3_rm | Reference Model Gain 3rd order signal |
x_2d_bound | Bound for the 3rd order reference signal |
x_3d_bound | Bound for the 4th order reference signal |
n | Number of dimensions |
Definition at line 835 of file oneloop_andi.c.
References err_nd(), float_vect_copy(), integrate_nd(), and vect_bound_nd().
Referenced by oneloop_andi_RM().
void rm_3rd | ( | float | dt, |
float * | x_ref, | ||
float * | x_d_ref, | ||
float * | x_2d_ref, | ||
float * | x_3d_ref, | ||
float | x_des, | ||
float | k1_rm, | ||
float | k2_rm, | ||
float | k3_rm | ||
) |
Reference Model Definition for 3rd order system.
dt | Delta time [s] |
x_ref | Reference signal 1st order |
x_d_ref | Reference signal 2nd order |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_des | Desired 1st order signal |
k1_rm | Reference Model Gain 1st order signal |
k2_rm | Reference Model Gain 2nd order signal |
k3_rm | Reference Model Gain 3rd order signal |
Definition at line 755 of file oneloop_andi.c.
void rm_3rd_attitude | ( | float | dt, |
float | x_ref[3], | ||
float | x_d_ref[3], | ||
float | x_2d_ref[3], | ||
float | x_3d_ref[3], | ||
float | x_des[3], | ||
bool | ow_psi, | ||
float | psi_overwrite[4], | ||
float | k1_rm[3], | ||
float | k2_rm[3], | ||
float | k3_rm[3], | ||
float | max_ang_jerk | ||
) |
Reference Model Definition for 3rd order system with attitude conversion functions.
dt | Delta time [s] |
x_ref | Reference signal 1st order |
x_d_ref | Reference signal 2nd order |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_des | Desired 1st order signal |
ow_psi | Overwrite psi (for navigation functions) [bool] |
psi_overwrite | Overwrite psi (for navigation functions) [values] |
k1_rm | Reference Model Gain 1st order signal |
k2_rm | Reference Model Gain 2nd order signal |
k3_rm | Reference Model Gain 3rd order signal |
Definition at line 714 of file oneloop_andi.c.
References err_nd(), float_euler_dot_of_rates_vec(), float_rates_of_euler_dot_vec(), float_vect_copy(), integrate_nd(), and vect_bound_nd().
Referenced by oneloop_andi_RM().
void rm_3rd_head | ( | float | dt, |
float * | x_ref, | ||
float * | x_d_ref, | ||
float * | x_2d_ref, | ||
float * | x_3d_ref, | ||
float | x_des, | ||
float | k1_rm, | ||
float | k2_rm, | ||
float | k3_rm | ||
) |
Reference Model Definition for 3rd order system specific to the heading angle.
dt | Delta time [s] |
x_ref | Reference signal 1st order |
x_d_ref | Reference signal 2nd order |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_des | Desired 1st order signal |
k1_rm | Reference Model Gain 1st order signal |
k2_rm | Reference Model Gain 2nd order signal |
k3_rm | Reference Model Gain 3rd order signal |
Definition at line 777 of file oneloop_andi.c.
void rm_3rd_pos | ( | float | dt, |
float | x_ref[], | ||
float | x_d_ref[], | ||
float | x_2d_ref[], | ||
float | x_3d_ref[], | ||
float | x_des[], | ||
float | k1_rm[], | ||
float | k2_rm[], | ||
float | k3_rm[], | ||
float | x_d_bound, | ||
float | x_2d_bound, | ||
float | x_3d_bound, | ||
int | n | ||
) |
Reference Model Definition for 3rd order system specific to positioning with bounds.
dt | Delta time [s] |
x_ref | Reference signal 1st order |
x_d_ref | Reference signal 2nd order |
x_2d_ref | Reference signal 3rd order |
x_3d_ref | Reference signal 4th order |
x_des | Desired 1st order signal |
k1_rm | Reference Model Gain 1st order signal |
k2_rm | Reference Model Gain 2nd order signal |
k3_rm | Reference Model Gain 3rd order signal |
x_d_bound | Bound for the 2nd order reference signal |
x_2d_bound | Bound for the 3rd order reference signal |
x_3d_bound | Bound for the 4th order reference signal |
n | Number of dimensions |
Definition at line 805 of file oneloop_andi.c.
References bound_v_from_a(), err_nd(), float_vect_copy(), integrate_nd(), and vect_bound_nd().
Referenced by oneloop_andi_RM().
|
static |
Definition at line 468 of file oneloop_andi.c.
References ANDI_NUM_ACT_TOT, dev, and EFF_MAT_G.
Referenced by oneloop_andi_init().
|
static |
Definition at line 457 of file oneloop_andi.c.
References ANDI_NUM_ACT, dev, and EFF_MAT_G.
Referenced by oneloop_andi_init().
|
static |
Definition at line 495 of file oneloop_andi.c.
References OneloopGuidanceRef::acc, OneloopGuidanceState::acc, dev, OneloopGeneral::gui_ref, OneloopGeneral::gui_state, OneloopGuidanceRef::jer, oneloop_andi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, OneloopGuidanceRef::vel, and OneloopGuidanceState::vel.
Referenced by oneloop_andi_init().
|
static |
Definition at line 475 of file oneloop_andi.c.
References actuator_state_1l, ANDI_NUM_ACT, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, dev, eulers_zxy_des, oneloop_andi, FloatEulers::phi, FloatEulers::psi, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, and FloatEulers::theta.
Referenced by oneloop_andi_init().
|
static |
Definition at line 528 of file oneloop_andi.c.
References RW_Model::as, debug_vect(), dev, model_pred, and RW.
Referenced by oneloop_andi_init().
|
static |
Definition at line 453 of file oneloop_andi.c.
References dev, send_wls_u(), and WLS_one_p.
Referenced by oneloop_andi_init().
|
static |
Definition at line 449 of file oneloop_andi.c.
References dev, send_wls_v(), and WLS_one_p.
Referenced by oneloop_andi_init().
void vect_bound_nd | ( | float | vect[], |
float | bound, | ||
int | n | ||
) |
Scale a 3D array to within a 3D bound.
Definition at line 664 of file oneloop_andi.c.
References float_vect_norm(), positive_non_zero(), and scale.
Referenced by ec_3rd_att(), ec_3rd_pos(), reshape_wind(), rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), and rm_3rd_pos().
|
static |
Third Order to First Order Dynamics Approximation.
p1 | Pole 1 |
p2 | Pole 2 |
p3 | Pole 3 |
rm_k | Reference Model Gain |
Definition at line 958 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller(), and init_poles().
float A_chirp = 1.0 |
Definition at line 404 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
static |
Definition at line 334 of file oneloop_andi.c.
Referenced by oneloop_andi_RM(), and oneloop_andi_run().
|
static |
Definition at line 443 of file oneloop_andi.c.
Referenced by init_filter(), oneloop_andi_propagate_filters(), and oneloop_andi_sideslip().
float act_dynamics[ANDI_NUM_ACT_TOT] = = {1} |
Definition at line 173 of file oneloop_andi.c.
Referenced by calc_normalization(), G1G2_oneloop(), init_controller(), init_poles(), oneloop_andi_init(), and oneloop_andi_run().
|
static |
Definition at line 332 of file oneloop_andi.c.
Referenced by calc_normalization(), and get_act_state_oneloop().
float act_max[ANDI_NUM_ACT_TOT] = = {MAX_PPRZ} |
Definition at line 179 of file oneloop_andi.c.
Referenced by calc_normalization(), get_act_state_oneloop(), and oneloop_andi_run().
float act_max_norm[ANDI_NUM_ACT_TOT] = = {1.0} |
Definition at line 191 of file oneloop_andi.c.
Referenced by calc_normalization().
float act_min[ANDI_NUM_ACT_TOT] = = {0.0} |
Definition at line 185 of file oneloop_andi.c.
Referenced by calc_normalization(), get_act_state_oneloop(), and oneloop_andi_run().
float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0} |
Definition at line 197 of file oneloop_andi.c.
Referenced by calc_normalization().
bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0} |
Definition at line 166 of file oneloop_andi.c.
float actuator_state_1l[ANDI_NUM_ACT] |
Definition at line 333 of file oneloop_andi.c.
Referenced by calc_G1_G2_RW(), calc_model(), get_act_state_oneloop(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), and send_oneloop_andi().
|
static |
Definition at line 444 of file oneloop_andi.c.
Referenced by G1G2_oneloop(), init_filter(), oneloop_andi_propagate_filters(), oneloop_andi_sideslip(), and reshape_wind().
float andi_du[ANDI_NUM_ACT_TOT] |
Definition at line 328 of file oneloop_andi.c.
Referenced by oneloop_andi_init(), and oneloop_andi_run().
|
static |
Definition at line 329 of file oneloop_andi.c.
Referenced by oneloop_andi_init(), and oneloop_andi_run().
float andi_u[ANDI_NUM_ACT_TOT] |
Definition at line 327 of file oneloop_andi.c.
Referenced by get_act_state_oneloop(), oneloop_andi_init(), and oneloop_andi_run().
|
static |
Definition at line 392 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_propagate_filters(), and oneloop_andi_run().
|
static |
Definition at line 393 of file oneloop_andi.c.
Referenced by oneloop_andi_propagate_filters(), and oneloop_andi_run().
|
static |
Definition at line 440 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
float* bwls_1l[ANDI_OUTPUTS] |
Definition at line 430 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), and oneloop_andi_run().
int8_t chirp_axis = 0 |
Definition at line 405 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
bool chirp_first_call = true |
Definition at line 398 of file oneloop_andi.c.
Referenced by chirp_call(), and oneloop_andi_RM().
bool chirp_on = false |
Definition at line 397 of file oneloop_andi.c.
Referenced by chirp_call(), oneloop_andi_RM(), and oneloop_andi_run().
bool ctrl_off = false |
Definition at line 357 of file oneloop_andi.c.
Referenced by G1G2_oneloop(), and oneloop_andi_run().
|
static |
Definition at line 321 of file oneloop_andi.c.
Referenced by calc_normalization(), and oneloop_andi_RM().
float EFF_MAT_G[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] |
Definition at line 431 of file oneloop_andi.c.
Referenced by G1G2_oneloop(), oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_run(), send_eff_mat_guid_oneloop_andi(), and send_eff_mat_stab_oneloop_andi().
struct FloatEulers eulers_zxy |
Definition at line 335 of file oneloop_andi.c.
Referenced by calc_model(), oneloop_andi_enter(), oneloop_andi_RM(), oneloop_andi_run(), oneloop_andi_sideslip(), and reshape_wind().
struct FloatEulers eulers_zxy_des |
Definition at line 335 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), and send_oneloop_andi().
float f0_chirp = 0.8 / (2.0 * M_PI) |
Definition at line 401 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
float f1_chirp = 0.8 / (2.0 * M_PI) |
Definition at line 402 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
static |
Definition at line 436 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
|
static |
Definition at line 437 of file oneloop_andi.c.
Referenced by init_filter(), oneloop_andi_propagate_filters(), and oneloop_andi_run().
float fwd_sideslip_gain = 0.2 |
Definition at line 277 of file oneloop_andi.c.
Referenced by oneloop_andi_sideslip().
|
static |
Definition at line 322 of file oneloop_andi.c.
Referenced by calc_model(), and oneloop_andi_sideslip().
|
static |
Definition at line 335 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
bool heading_manual = false |
Definition at line 350 of file oneloop_andi.c.
Referenced by oneloop_andi_RM(), and oneloop_andi_run().
float k_as = 2.0 |
Definition at line 323 of file oneloop_andi.c.
struct Gains3rdOrder k_att_e |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
struct Gains3rdOrder k_att_e_indi |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
struct Gains3rdOrder k_att_rm |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_RM().
struct Gains3rdOrder k_pos_e |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
struct Gains3rdOrder k_pos_e_indi |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
struct Gains3rdOrder k_pos_rm |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), oneloop_andi_RM(), and reshape_wind().
|
static |
Definition at line 394 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_propagate_filters(), and oneloop_andi_run().
float max_a_nav = 4.0 |
Definition at line 249 of file oneloop_andi.c.
Referenced by init_controller(), oneloop_andi_RM(), oneloop_andi_run(), and reshape_wind().
float max_as = 19.0f |
Definition at line 269 of file oneloop_andi.c.
Referenced by reshape_wind().
float max_j_ang = 1000.0 |
Definition at line 261 of file oneloop_andi.c.
Referenced by calc_normalization(), oneloop_andi_RM(), and oneloop_andi_run().
float max_j_nav = 500.0 |
Definition at line 255 of file oneloop_andi.c.
Referenced by calc_normalization(), oneloop_andi_RM(), and oneloop_andi_run().
float max_r = RadOfDeg(120.0) |
Definition at line 158 of file oneloop_andi.c.
Referenced by attitude_ref_quat_int_set_max_r(), and oneloop_andi_RM().
float max_v_nav = 5.0 |
Definition at line 267 of file oneloop_andi.c.
Referenced by init_controller(), oneloop_andi_RM(), and oneloop_andi_run().
float max_v_nav_v = 1.5 |
Definition at line 274 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
static |
Definition at line 390 of file oneloop_andi.c.
Referenced by calc_model(), oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_propagate_filters(), and send_oneloop_debug().
|
static |
Definition at line 441 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
|
static |
Definition at line 391 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_propagate_filters().
|
static |
Definition at line 442 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
|
static |
Definition at line 439 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
|
static |
Definition at line 319 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), and reshape_wind().
|
static |
Definition at line 320 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), and reshape_wind().
float nu[ANDI_OUTPUTS] |
Definition at line 330 of file oneloop_andi.c.
Referenced by normalize_nu(), oneloop_andi_init(), and oneloop_andi_run().
float nu_n[ANDI_OUTPUTS] |
Definition at line 331 of file oneloop_andi.c.
Referenced by normalize_nu(), and oneloop_andi_init().
float nu_norm_max = 1.0 |
Definition at line 203 of file oneloop_andi.c.
Referenced by calc_normalization().
float num_thrusters_oneloop = 4.0 |
Definition at line 103 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
struct OneloopGeneral oneloop_andi |
Definition at line 312 of file oneloop_andi.c.
Referenced by chirp_call(), oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), oneloop_from_nav(), reshape_wind(), send_guidance_oneloop_andi(), send_oneloop_andi(), and stabilization_attitude_run().
float oneloop_andi_filt_cutoff = 2.0 |
Definition at line 115 of file oneloop_andi.c.
Referenced by init_filter().
float oneloop_andi_filt_cutoff_a = 2.0 |
Definition at line 121 of file oneloop_andi.c.
Referenced by init_filter().
float oneloop_andi_filt_cutoff_p = 20.0 |
Definition at line 140 of file oneloop_andi.c.
Referenced by init_filter().
float oneloop_andi_filt_cutoff_pos = 2.0 |
Definition at line 133 of file oneloop_andi.c.
float oneloop_andi_filt_cutoff_q = 20.0 |
Definition at line 147 of file oneloop_andi.c.
Referenced by init_filter().
float oneloop_andi_filt_cutoff_r = 20.0 |
Definition at line 154 of file oneloop_andi.c.
Referenced by init_filter().
float oneloop_andi_filt_cutoff_v = 2.0 |
Definition at line 127 of file oneloop_andi.c.
Referenced by init_filter().
struct PolePlacement p_alt_e |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_alt_rm |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_att_e |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_att_rm |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_head_e |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_head_rm |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_pos_e |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_pos_rm |
Definition at line 406 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
float p_ref_0[3] = {0.0, 0.0, 0.0} |
Definition at line 406 of file oneloop_andi.c.
Referenced by chirp_call(), chirp_pos(), and oneloop_andi_RM().
|
static |
Definition at line 360 of file oneloop_andi.c.
Referenced by chirp_pos(), and oneloop_andi_run().
float psi_des_deg = 0.0 |
Definition at line 344 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_RM(), and oneloop_andi_run().
float psi_des_rad = 0.0 |
Definition at line 343 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_RM(), and oneloop_andi_run().
|
static |
Definition at line 345 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
static |
Definition at line 438 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
float ratio_u_un[ANDI_NUM_ACT_TOT] |
Definition at line 432 of file oneloop_andi.c.
Referenced by calc_normalization(), G1G2_oneloop(), oneloop_andi_RM(), and oneloop_andi_run().
float ratio_vn_v[ANDI_OUTPUTS] |
Definition at line 433 of file oneloop_andi.c.
Referenced by calc_normalization(), G1G2_oneloop(), normalize_nu(), and oneloop_andi_RM().
struct Int32Eulers stab_att_sp_euler_1l |
Definition at line 335 of file oneloop_andi.c.
struct Int32Quat stab_att_sp_quat_1l |
Definition at line 335 of file oneloop_andi.c.
float t_0_chirp = 0.0 |
Definition at line 400 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
float t_chirp = 45.0 |
Definition at line 403 of file oneloop_andi.c.
Referenced by chirp_call(), chirp_pos(), and oneloop_andi_RM().
int16_t temp_pitch = 0 |
Definition at line 324 of file oneloop_andi.c.
Referenced by indi_apply_actuator_models().
float theta_pref_max = RadOfDeg(20.0) |
Definition at line 223 of file oneloop_andi.c.
Referenced by chirp_pos(), and oneloop_andi_run().
float time_elapsed_chirp = 0.0 |
Definition at line 399 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
static |
Definition at line 209 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
|
static |
Definition at line 318 of file oneloop_andi.c.
Referenced by oneloop_andi_RM(), and oneloop_andi_run().
struct WLS_t WLS_one_p |
Definition at line 360 of file oneloop_andi.c.
Referenced by normalize_nu(), oneloop_andi_RM(), oneloop_andi_run(), send_wls_u_oneloop(), and send_wls_v_oneloop().
|
static |
Definition at line 387 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
bool yaw_stick_in_auto = false |
Definition at line 355 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().