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 <stdio.h>
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Macros | |
#define | ONELOOP_ANDI_FILT_CUTOFF_P 20.0 |
#define | ONELOOP_ANDI_FILT_CUTOFF_Q 20.0 |
#define | ONELOOP_ANDI_FILT_CUTOFF_R 20.0 |
#define | ONELOOP_ANDI_DEBUG_MODE FALSE; |
#define | ONELOOP_ANDI_AC_HAS_PUSHER FALSE; |
#define | ONELOOP_ANDI_PUSHER_IDX 0 |
#define | GUIDANCE_INDI_SPEED_GAIN 1.8 |
#define | GUIDANCE_INDI_SPEED_GAINZ 1.8 |
#define | GUIDANCE_INDI_POS_GAIN 0.5 |
#define | GUIDANCE_INDI_POS_GAINZ 0.5 |
#define | GUIDANCE_INDI_LIFTD_ASQ 0.20 |
#define | GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12) |
#define | GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2) |
Functions | |
void | init_poles (void) |
Initialize Position of Poles. More... | |
void | calc_normalization (void) |
Calculate Normalization of actuators and discrete actuator dynamics More... | |
void | sum_g1g2_1l (void) |
Function that sums g1 and g2 to obtain the g1_g2 matrix. More... | |
void | get_act_state_oneloop (void) |
Function to reconstruct actuator state using first order dynamics. More... | |
void | oneloop_andi_propagate_filters (void) |
Propagate the filters. More... | |
void | init_filter (void) |
Initialize the filters. More... | |
void | init_controller (void) |
Initialize Controller Gains FIXME: Calculate the gains dynamically for transition. More... | |
void | float_rates_of_euler_dot_vec (float r[3], float e[3], float edot[3]) |
Attitude Rates to Euler Conversion Function. More... | |
void | float_euler_dot_of_rates_vec (float r[3], float e[3], float edot[3]) |
Attitude Euler to Rates Conversion Function. More... | |
void | err_nd (float err[], float a[], float b[], float k[], int n) |
Calculate Scaled Error between two 3D arrays. More... | |
void | integrate_nd (float dt, float a[], float a_dot[], int n) |
Integrate in time 3D array. More... | |
void | vect_bound_nd (float vect[], float bound, int n) |
Scale a 3D array to within a 3D bound. More... | |
void | rm_2nd (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float x_des, float k1_rm, float k2_rm) |
Reference Model Definition for 2nd order system. More... | |
void | rm_3rd (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float *x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm) |
Reference Model Definition for 3rd order system. More... | |
void | rm_3rd_head (float dt, float *x_ref, float *x_d_ref, float *x_2d_ref, float *x_3d_ref, float x_des, float k1_rm, float k2_rm, float k3_rm) |
Reference Model Definition for 3rd order system specific to the heading angle. More... | |
void | rm_3rd_attitude (float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3]) |
Reference Model Definition for 3rd order system with attitude conversion functions. More... | |
void | rm_3rd_pos (float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_des[], float k1_rm[], float k2_rm[], float k3_rm[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n) |
Reference Model Definition for 3rd order system specific to positioning with bounds. More... | |
void | rm_2nd_pos (float dt, float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_d_des[], float k2_rm[], float k3_rm[], float x_2d_bound, float x_3d_bound, int n) |
Reference Model Definition for 3rd order system specific to positioning with bounds. More... | |
void | rm_1st_pos (float dt, float x_2d_ref[], float x_3d_ref[], float x_2d_des[], float k3_rm[], float x_3d_bound, int n) |
Reference Model Definition for 3rd order system specific to positioning with bounds. More... | |
void | ec_3rd_att (float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3]) |
Error Controller Definition for 3rd order system specific to attitude. More... | |
void | calc_model (void) |
Function that calculates the model prediction for the complementary filter. More... | |
static void | send_eff_mat_g_oneloop_andi (struct transport_tx *trans, struct link_device *dev) |
static void | send_oneloop_andi (struct transport_tx *trans, struct link_device *dev) |
static void | send_guidance_oneloop_andi (struct transport_tx *trans, struct link_device *dev) |
static float | positive_non_zero (float input) |
Function to make sure that inputs are positive non zero vaues. More... | |
static float | k_e_1_3_f (float p1, float p2, float p3) |
Error Controller Gain Design. More... | |
static float | k_e_2_3_f (float p1, float p2, float p3) |
static float | k_e_3_3_f (float p1, float p2, float p3) |
static float | k_e_1_2_f (float p1, float p2) |
static float | k_e_2_2_f (float p1, float p2) |
static float | k_e_1_3_f_v2 (float omega_n, UNUSED float zeta, float p1) |
static float | k_e_2_3_f_v2 (float omega_n, float zeta, float p1) |
static float | k_e_3_3_f_v2 (float omega_n, float zeta, float p1) |
static float | k_rm_1_3_f (float omega_n, float zeta, float p1) |
Reference Model Gain Design. More... | |
static float | k_rm_2_3_f (float omega_n, float zeta, float p1) |
static float | k_rm_3_3_f (float omega_n, float zeta, float p1) |
static float | k_rm_1_2_f (float omega_n, float zeta) |
static float | k_rm_2_2_f (float omega_n, float zeta) |
static float | ec_3rd (float x_ref, float x_d_ref, float x_2d_ref, float x_3d_ref, float x, float x_d, float x_2d, float k1_e, float k2_e, float k3_e) |
Error Controller Definition for 3rd order system. More... | |
static float | ec_2rd (float x_ref, float x_d_ref, float x_2d_ref, float x, float x_d, float k1_e, float k2_e) |
Error Controller Definition for 2rd order system. More... | |
static float | w_approx (float p1, float p2, float p3, float rm_k) |
Third Order to First Order Dynamics Approximation. More... | |
void | oneloop_andi_init (void) |
Init function of Oneloop ANDI controller More... | |
void | oneloop_andi_enter (bool half_loop_sp) |
Function that resets important values upon engaging Oneloop ANDI. More... | |
void | oneloop_andi_RM (bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v) |
Function to generate the reference signals for the oneloop controller. More... | |
void | oneloop_andi_run (bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v) |
Main function that runs the controller and performs control allocation. More... | |
void | oneloop_from_nav (bool in_flight) |
Function that maps navigation inputs to the oneloop controller for the generated autopilot. More... | |
#define GUIDANCE_INDI_LIFTD_ASQ 0.20 |
Definition at line 264 of file oneloop_andi.c.
#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2) |
Definition at line 273 of file oneloop_andi.c.
#define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12) |
Definition at line 272 of file oneloop_andi.c.
#define GUIDANCE_INDI_POS_GAIN 0.5 |
Definition at line 259 of file oneloop_andi.c.
#define GUIDANCE_INDI_POS_GAINZ 0.5 |
Definition at line 260 of file oneloop_andi.c.
#define GUIDANCE_INDI_SPEED_GAIN 1.8 |
Definition at line 254 of file oneloop_andi.c.
#define GUIDANCE_INDI_SPEED_GAINZ 1.8 |
Definition at line 255 of file oneloop_andi.c.
#define ONELOOP_ANDI_AC_HAS_PUSHER FALSE; |
Definition at line 210 of file oneloop_andi.c.
#define ONELOOP_ANDI_DEBUG_MODE FALSE; |
Definition at line 205 of file oneloop_andi.c.
#define ONELOOP_ANDI_FILT_CUTOFF_P 20.0 |
Definition at line 131 of file oneloop_andi.c.
#define ONELOOP_ANDI_FILT_CUTOFF_Q 20.0 |
Definition at line 137 of file oneloop_andi.c.
#define ONELOOP_ANDI_FILT_CUTOFF_R 20.0 |
Definition at line 143 of file oneloop_andi.c.
#define ONELOOP_ANDI_PUSHER_IDX 0 |
Definition at line 214 of file oneloop_andi.c.
void calc_model | ( | void | ) |
Function that calculates the model prediction for the complementary filter.
Definition at line 1453 of file oneloop_andi.c.
References act_dynamics, actuator_state_1l, ANDI_G_SCALING, ANDI_NUM_ACT, ANDI_OUTPUTS, eulers_zxy, g, g1_1l, g1g2_1l, model_pred, ONELOOP_ANDI_PUSHER_IDX, P, FloatEulers::phi, FloatEulers::psi, ratio_u_un, ratio_vn_v, mesonh.mesonh_atmosphere::T, and FloatEulers::theta.
Referenced by oneloop_andi_propagate_filters().
void calc_normalization | ( | void | ) |
Calculate Normalization of actuators and discrete actuator dynamics
Definition at line 1435 of file oneloop_andi.c.
References act_dynamics, act_dynamics_d, act_max, act_max_norm, act_min, act_min_norm, ANDI_NUM_ACT_TOT, dt_1l, MAX_PPRZ, positive_non_zero(), ratio_u_un, and ratio_vn_v.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().
|
static |
Error Controller Definition for 2rd 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_des | Desired 1st order signal |
x | Current 1st order signal |
x_d | Current 2nd order signal |
k1_e | Error Controller Gain 1st order signal |
k2_e | Error Controller Gain 2nd order signal |
Definition at line 861 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
|
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 812 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] | ||
) |
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 832 of file oneloop_andi.c.
References err_nd(), float_vect_copy(), and float_vect_sum().
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 586 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 float_euler_dot_of_rates_vec | ( | float | r[3], |
float | e[3], | ||
float | edot[3] | ||
) |
Attitude Euler to Rates Conversion Function.
Definition at line 571 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 559 of file oneloop_andi.c.
Referenced by rm_3rd_attitude().
void get_act_state_oneloop | ( | void | ) |
Function to reconstruct actuator state using first order dynamics.
Definition at line 1359 of file oneloop_andi.c.
References act_dynamics_d, actuator_state_1l, ANDI_NUM_ACT, andi_u, autopilot_get_motors_on(), and MAX_PPRZ.
Referenced by oneloop_andi_run().
void init_controller | ( | void | ) |
Initialize Controller Gains FIXME: Calculate the gains dynamically for transition.
Definition at line 920 of file oneloop_andi.c.
References act_dynamics, ANDI_NUM_ACT, gih_params, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains2ndOrder::k2, Gains3rdOrder::k3, Gains2ndOrder::k3, k_att_e, k_att_rm, k_e_1_2_f(), k_e_1_3_f(), k_e_1_3_f_v2(), k_e_2_2_f(), k_e_2_3_f(), k_e_2_3_f_v2(), k_e_3_3_f(), k_e_3_3_f_v2(), k_head_e, k_head_rm, k_pos_e, k_pos_rm, k_rm_1_2_f(), k_rm_1_3_f(), k_rm_2_2_f(), k_rm_2_3_f(), k_rm_3_3_f(), max_v_nav, nav_max_speed, 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, guidance_indi_hybrid_params::pos_gain, guidance_indi_hybrid_params::pos_gainz, guidance_indi_hybrid_params::speed_gain, guidance_indi_hybrid_params::speed_gainz, 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 991 of file oneloop_andi.c.
References ANDI_OUTPUTS, att_dot_meas_lowpass_filters, filt_accel_ned, init_butterworth_2_low_pass(), init_first_order_low_pass(), model_pred_a_filt, model_pred_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, p, rates_filt_fo, and stateGetBodyRates_f().
Referenced by oneloop_andi_enter(), and oneloop_andi_init().
void init_poles | ( | void | ) |
Initialize Position of Poles.
Definition at line 887 of file oneloop_andi.c.
References PolePlacement::omega_n, PolePlacement::p3, p_alt_e, p_alt_rm, p_att_e, p_att_rm, p_head_e, p_pos_e, p_pos_rm, and PolePlacement::zeta.
Referenced by oneloop_andi_init().
void integrate_nd | ( | float | dt, |
float | a[], | ||
float | a_dot[], | ||
int | n | ||
) |
Integrate in time 3D array.
Definition at line 595 of file oneloop_andi.c.
Referenced by rm_1st_pos(), rm_2nd_pos(), rm_3rd_attitude(), and rm_3rd_pos().
|
static |
Definition at line 491 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Error Controller Gain Design.
Definition at line 470 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 503 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 497 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 477 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 509 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 484 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 516 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 546 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Reference Model Gain Design.
Definition at line 525 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 552 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 532 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 539 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
void oneloop_andi_enter | ( | bool | half_loop_sp | ) |
Function that resets important values upon engaging Oneloop ANDI.
FIXME: Ideally we should distinguish between the "stabilization" and "guidance" needs because it is unlikely to switch stabilization in flight, and there are multiple modes that use (the same) stabilization. Resetting the controller is not so nice when you are flying.
Definition at line 1098 of file oneloop_andi.c.
References ANDI_OUTPUTS, ang_acc, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, calc_normalization(), eulers_zxy, eulers_zxy_des, float_vect_zero(), OneloopGeneral::half_loop, init_controller(), init_filter(), lin_acc, model_pred, nav_target, oneloop_andi, FloatEulers::phi, FloatEulers::psi, psi_des_deg, psi_des_rad, OneloopGeneral::sta_ref, sum_g1g2_1l(), and FloatEulers::theta.
Referenced by guidance_h_run_enter(), oneloop_from_nav(), and stabilization_attitude_enter().
void oneloop_andi_init | ( | void | ) |
Init function of Oneloop ANDI controller
Definition at line 1048 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(), DefaultPeriodic, eulers_zxy_des, float_vect_zero(), g1g2_1l, OneloopGeneral::half_loop, init_controller(), init_filter(), init_poles(), lin_acc, model_pred, nav_target, nu, oneloop_andi, FloatEulers::phi, positive_non_zero(), FloatEulers::psi, register_periodic_telemetry(), send_eff_mat_g_oneloop_andi(), send_guidance_oneloop_andi(), send_oneloop_andi(), OneloopGeneral::sta_ref, sum_g1g2_1l(), and FloatEulers::theta.
void oneloop_andi_propagate_filters | ( | void | ) |
Propagate the filters.
Definition at line 1023 of file oneloop_andi.c.
References ANDI_OUTPUTS, ang_acc, att_dot_meas_lowpass_filters, calc_model(), filt_accel_ned, FirstOrderLowPass::last_out, lin_acc, model_pred, model_pred_a_filt, model_pred_filt, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, rates_filt_fo, stateGetAccelNed_f(), stateGetBodyRates_f(), update_butterworth_2_low_pass(), update_first_order_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 | ||
) |
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 1129 of file oneloop_andi.c.
References a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, actuator_state_1l, ANDI_NUM_ACT, OneloopStabilizationRef::att, OneloopStabilizationRef::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, bwls_1l, dt_1l, eulers_zxy, eulers_zxy_des, float_vect_copy(), float_vect_zero(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains2ndOrder::k2, Gains3rdOrder::k3, Gains2ndOrder::k3, k_att_rm, k_head_rm, k_pos_rm, max_a_nav, max_j_nav, MAX_PPRZ, max_v_nav, nav_target, oneloop_andi, FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_rad, psi_vec, radio_control_get(), RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, RADIO_YAW, ratio_u_un, ratio_vn_v, rm_1st_pos(), rm_2nd(), rm_2nd_pos(), rm_3rd(), rm_3rd_attitude(), rm_3rd_pos(), OneloopGeneral::sta_ref, FloatEulers::theta, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, Wv, Wv_wls, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by oneloop_andi_run().
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 1231 of file oneloop_andi.c.
References a_thrust, OneloopGuidanceRef::acc, OneloopGuidanceState::acc, act_max, act_min, actuator_state_1l, andi_du, andi_du_n, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, ANDI_OUTPUTS, andi_u, ang_acc, AP_MODE_ATTITUDE_DIRECT, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_3d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, autopilot, bwls_1l, calc_normalization(), du_max_1l, du_min_1l, du_pref_1l, ec_2rd(), ec_3rd(), ec_3rd_att(), eulers_zxy, eulers_zxy_des, float_eulers_of_quat_zxy(), float_vect_copy(), float_vect_sum(), g1g2_1l, gamma_wls, get_act_state_oneloop(), OneloopGeneral::gui_ref, OneloopGeneral::gui_state, init_controller(), OneloopGuidanceRef::jer, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains2ndOrder::k2, Gains3rdOrder::k3, Gains2ndOrder::k3, k_att_e, k_head_e, k_pos_e, FirstOrderLowPass::last_out, lin_acc, pprz_autopilot::mode, nu, num_thrusters_oneloop, number_iter, oneloop_andi, ONELOOP_ANDI_DEBUG_MODE, oneloop_andi_propagate_filters(), oneloop_andi_RM(), FloatEulers::phi, OneloopGuidanceRef::pos, OneloopGuidanceState::pos, FloatEulers::psi, psi_des_deg, psi_des_rad, rates_filt_fo, ratio_u_un, OneloopGeneral::sta_ref, OneloopGeneral::sta_state, stabilization_cmd, stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetSpeedNed_f(), sum_g1g2_1l(), FloatEulers::theta, u_pref, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, wls_alloc(), Wu, Wv_wls, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.
Referenced by oneloop_from_nav(), and stabilization_attitude_run().
void oneloop_from_nav | ( | bool | in_flight | ) |
Function that maps navigation inputs to the oneloop controller for the generated autopilot.
Definition at line 1479 of file oneloop_andi.c.
References RotorcraftNavigation::carrot, RotorcraftNavigation::climb, nav, RotorcraftNavigation::nav_altitude, NAV_SETPOINT_MODE_POS, NAV_SETPOINT_MODE_SPEED, NAV_VERTICAL_MODE_ALT, NAV_VERTICAL_MODE_CLIMB, 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::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 460 of file oneloop_andi.c.
Referenced by calc_normalization(), k_e_1_2_f(), k_e_1_3_f(), k_e_1_3_f_v2(), k_e_2_2_f(), k_e_2_3_f(), k_e_2_3_f_v2(), k_e_3_3_f(), k_e_3_3_f_v2(), k_rm_1_2_f(), k_rm_1_3_f(), k_rm_2_2_f(), k_rm_2_3_f(), k_rm_3_3_f(), oneloop_andi_init(), vect_bound_nd(), and w_approx().
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 768 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 789 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
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 746 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 667 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
void rm_3rd_attitude | ( | float | dt, |
float | x_ref[3], | ||
float | x_d_ref[3], | ||
float | x_2d_ref[3], | ||
float | x_3d_ref[3], | ||
float | x_des[3], | ||
bool | ow_psi, | ||
float | psi_overwrite[4], | ||
float | k1_rm[3], | ||
float | k2_rm[3], | ||
float | k3_rm[3] | ||
) |
Reference Model Definition for 3rd order system with attitude conversion functions.
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 630 of file oneloop_andi.c.
References err_nd(), float_euler_dot_of_rates_vec(), float_rates_of_euler_dot_vec(), float_vect_copy(), and integrate_nd().
Referenced by oneloop_andi_RM().
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 689 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 717 of file oneloop_andi.c.
References err_nd(), float_vect_copy(), integrate_nd(), and vect_bound_nd().
Referenced by oneloop_andi_RM().
|
static |
Definition at line 318 of file oneloop_andi.c.
References ANDI_NUM_ACT_TOT, dev, and g1g2_1l.
Referenced by oneloop_andi_init().
|
static |
Definition at line 356 of file oneloop_andi.c.
Referenced by oneloop_andi_init().
|
static |
Definition at line 331 of file oneloop_andi.c.
References actuator_state_1l, ANDI_NUM_ACT, ANDI_OUTPUTS, OneloopStabilizationRef::att, OneloopStabilizationState::att, OneloopStabilizationRef::att_2d, OneloopStabilizationState::att_2d, OneloopStabilizationRef::att_d, OneloopStabilizationState::att_d, dev, nu, oneloop_andi, OneloopGeneral::sta_ref, and OneloopGeneral::sta_state.
Referenced by oneloop_andi_init().
void sum_g1g2_1l | ( | void | ) |
Function that sums g1 and g2 to obtain the g1_g2 matrix.
It also undoes the scaling that was done to make the values readable FIXME: make this function into a for loop to make it more adaptable to different configurations
Definition at line 1377 of file oneloop_andi.c.
References act_dynamics, actuator_state_1l, ANDI_G_SCALING, ANDI_NUM_ACT, ANDI_NUM_ACT_TOT, eulers_zxy, g1_1l, g1g2_1l, g2_1l, ONELOOP_ANDI_AC_HAS_PUSHER, ONELOOP_ANDI_PUSHER_IDX, P, FloatEulers::phi, FloatEulers::psi, ratio_u_un, ratio_vn_v, mesonh.mesonh_atmosphere::T, and FloatEulers::theta.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_run().
void vect_bound_nd | ( | float | vect[], |
float | bound, | ||
int | n | ||
) |
Scale a 3D array to within a 3D bound.
Definition at line 604 of file oneloop_andi.c.
References float_vect_norm(), positive_non_zero(), and scale.
Referenced by rm_1st_pos(), rm_2nd_pos(), and rm_3rd_pos().
|
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 874 of file oneloop_andi.c.
References positive_non_zero().
Referenced by init_controller().
|
static |
Definition at line 399 of file oneloop_andi.c.
Referenced by oneloop_andi_RM(), and oneloop_andi_run().
float act_dynamics[ANDI_NUM_ACT_TOT] = = {1} |
Definition at line 156 of file oneloop_andi.c.
Referenced by calc_model(), calc_normalization(), init_controller(), oneloop_andi_init(), and sum_g1g2_1l().
|
static |
Definition at line 397 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 162 of file oneloop_andi.c.
Referenced by calc_normalization(), and oneloop_andi_run().
float act_max_norm[ANDI_NUM_ACT_TOT] = = {1.0} |
Definition at line 174 of file oneloop_andi.c.
Referenced by calc_normalization().
float act_min[ANDI_NUM_ACT_TOT] = = {0.0} |
Definition at line 168 of file oneloop_andi.c.
Referenced by calc_normalization(), and oneloop_andi_run().
float act_min_norm[ANDI_NUM_ACT_TOT] = = {0.0} |
Definition at line 180 of file oneloop_andi.c.
Referenced by calc_normalization().
bool actuator_is_servo[ANDI_NUM_ACT_TOT] = {0} |
Definition at line 149 of file oneloop_andi.c.
float actuator_state_1l[ANDI_NUM_ACT] |
Definition at line 398 of file oneloop_andi.c.
Referenced by calc_model(), get_act_state_oneloop(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), send_oneloop_andi(), and sum_g1g2_1l().
float andi_du[ANDI_NUM_ACT_TOT] |
Definition at line 394 of file oneloop_andi.c.
Referenced by oneloop_andi_init(), and oneloop_andi_run().
|
static |
Definition at line 395 of file oneloop_andi.c.
Referenced by oneloop_andi_init(), and oneloop_andi_run().
float andi_u[ANDI_NUM_ACT_TOT] |
Definition at line 393 of file oneloop_andi.c.
Referenced by get_act_state_oneloop(), oneloop_andi_init(), and oneloop_andi_run().
|
static |
Definition at line 419 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 455 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
float* bwls_1l[ANDI_OUTPUTS] |
Definition at line 447 of file oneloop_andi.c.
Referenced by oneloop_andi_init(), oneloop_andi_RM(), and oneloop_andi_run().
|
static |
Definition at line 389 of file oneloop_andi.c.
Referenced by calc_normalization(), and oneloop_andi_RM().
|
static |
Definition at line 413 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
|
static |
Definition at line 412 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
|
static |
Definition at line 414 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
struct FloatEulers eulers_zxy |
Definition at line 399 of file oneloop_andi.c.
Referenced by calc_model(), oneloop_andi_enter(), oneloop_andi_RM(), oneloop_andi_run(), and sum_g1g2_1l().
struct FloatEulers eulers_zxy_des |
Definition at line 399 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), and oneloop_andi_run().
|
static |
Definition at line 449 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
bool force_forward = false |
forward flight for hybrid nav
Definition at line 288 of file oneloop_andi.c.
|
static |
Definition at line 390 of file oneloop_andi.c.
Referenced by calc_model().
float g1_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] = {ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_THRUST, ONELOOP_ANDI_G1_ROLL, ONELOOP_ANDI_G1_PITCH, ONELOOP_ANDI_G1_YAW} |
Definition at line 445 of file oneloop_andi.c.
Referenced by calc_model(), and sum_g1g2_1l().
float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] |
Definition at line 446 of file oneloop_andi.c.
Referenced by calc_model(), oneloop_andi_init(), oneloop_andi_run(), send_eff_mat_g_oneloop_andi(), and sum_g1g2_1l().
float g2_1l[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2 |
Definition at line 444 of file oneloop_andi.c.
Referenced by sum_g1g2_1l().
|
static |
Definition at line 411 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
struct guidance_indi_hybrid_params gih_params |
Definition at line 248 of file oneloop_andi.c.
Referenced by init_controller().
struct Gains3rdOrder k_att_e |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
struct Gains3rdOrder k_att_rm |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_RM().
struct Gains2ndOrder k_head_e |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
struct Gains2ndOrder k_head_rm |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_RM().
struct Gains3rdOrder k_pos_e |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
struct Gains3rdOrder k_pos_rm |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_RM().
|
static |
Definition at line 420 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 236 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
float max_j_nav = 500.0 |
Definition at line 242 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
float max_v_nav = 5.0 |
Definition at line 248 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_RM().
|
static |
Definition at line 418 of file oneloop_andi.c.
Referenced by calc_model(), oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_propagate_filters().
|
static |
Definition at line 449 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
|
static |
Definition at line 456 of file oneloop_andi.c.
Referenced by init_filter(), and oneloop_andi_propagate_filters().
|
static |
Definition at line 388 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), and oneloop_andi_RM().
float nu[ANDI_OUTPUTS] |
Definition at line 396 of file oneloop_andi.c.
Referenced by oneloop_andi_init(), oneloop_andi_run(), and send_oneloop_andi().
float num_thrusters_oneloop = 4.0 |
Definition at line 99 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
|
static |
Definition at line 415 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
struct OneloopGeneral oneloop_andi |
Definition at line 356 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), send_oneloop_andi(), and stabilization_attitude_run().
float oneloop_andi_filt_cutoff = 2.0 |
Definition at line 107 of file oneloop_andi.c.
Referenced by init_filter().
float oneloop_andi_filt_cutoff_a = 2.0 |
Definition at line 113 of file oneloop_andi.c.
Referenced by init_filter().
float oneloop_andi_filt_cutoff_p = 2.0 |
Definition at line 125 of file oneloop_andi.c.
float oneloop_andi_filt_cutoff_v = 2.0 |
Definition at line 119 of file oneloop_andi.c.
struct PolePlacement p_alt_e |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_alt_rm |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_att_e |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_att_rm |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_head_e |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_head_rm |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller().
struct PolePlacement p_pos_e |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
struct PolePlacement p_pos_rm |
Definition at line 420 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
float psi_des_deg = 0.0 |
Definition at line 407 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), and oneloop_andi_run().
|
static |
Definition at line 406 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_RM(), and oneloop_andi_run().
|
static |
Definition at line 408 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
static |
Definition at line 449 of file oneloop_andi.c.
Referenced by init_filter(), oneloop_andi_propagate_filters(), and oneloop_andi_run().
float ratio_u_un[ANDI_NUM_ACT_TOT] |
Definition at line 448 of file oneloop_andi.c.
Referenced by calc_model(), calc_normalization(), oneloop_andi_RM(), oneloop_andi_run(), and sum_g1g2_1l().
float ratio_vn_v[ANDI_NUM_ACT_TOT] |
Definition at line 449 of file oneloop_andi.c.
Referenced by calc_model(), calc_normalization(), oneloop_andi_RM(), and sum_g1g2_1l().
struct Int32Eulers stab_att_sp_euler_1l |
Definition at line 399 of file oneloop_andi.c.
struct Int32Quat stab_att_sp_quat_1l |
Definition at line 399 of file oneloop_andi.c.
|
static |
Definition at line 200 of file oneloop_andi.c.
Referenced by oneloop_andi_run().
|
static |
Definition at line 387 of file oneloop_andi.c.
Referenced by oneloop_andi_RM(), and oneloop_andi_run().
|
static |
Definition at line 194 of file oneloop_andi.c.
Referenced by oneloop_andi_run(), and wls_alloc().
|
static |
Definition at line 187 of file oneloop_andi.c.
Referenced by oneloop_andi_RM(), and wls_alloc().
|
static |
Definition at line 188 of file oneloop_andi.c.
Referenced by oneloop_andi_RM(), and oneloop_andi_run().