![]() |
Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
#include "generated/airframe.h"
Go to the source code of this file.
Data Structures | |
struct | guidance_indi_hybrid_params |
struct | OneloopGuidanceRef |
struct | OneloopGuidanceState |
struct | OneloopStabilizationRef |
struct | OneloopStabilizationState |
struct | OneloopGeneral |
struct | PolePlacement |
struct | Gains3rdOrder |
struct | Gains2ndOrder |
Macros | |
#define | ANDI_NUM_ACT COMMANDS_NB_REAL |
#define | ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL |
#define | ANDI_NUM_VIRTUAL_ACT 2 |
#define | ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT) |
#define | ANDI_OUTPUTS 6 |
#define | ANDI_G_SCALING 1000.0f |
#define | CTRL_ANDI 0 |
Control types. More... | |
#define | CTRL_INDI 1 |
Functions | |
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_set_failsafe_setpoint (void) |
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_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_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn) |
void | oneloop_from_nav (bool in_flight) |
Function that maps navigation inputs to the oneloop controller for the generated autopilot. More... | |
Variables | |
bool | ctrl_off |
float | act_state_filt_vect_1l [ANDI_NUM_ACT] |
float | actuator_state_1l [ANDI_NUM_ACT] |
float | nu [6] |
float | g1g2_1l [ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] |
float | andi_u [ANDI_NUM_ACT_TOT] |
float | andi_du [ANDI_NUM_ACT_TOT] |
float | psi_des_deg |
bool | heading_manual |
bool | yaw_stick_in_auto |
float | fwd_sideslip_gain |
struct FloatEulers | eulers_zxy_des |
float | psi_des_rad |
float | k_as |
float | max_as |
bool | chirp_on |
float | f0_chirp |
float | f1_chirp |
float | t_chirp |
float | A_chirp |
int8_t | chirp_axis |
struct guidance_indi_hybrid_params | gih_params |
struct OneloopGeneral | oneloop_andi |
int16_t | temp_pitch |
struct PolePlacement | p_att_e |
struct PolePlacement | p_att_rm |
struct PolePlacement | p_pos_e |
struct PolePlacement | p_pos_rm |
struct PolePlacement | p_alt_e |
struct PolePlacement | p_alt_rm |
struct PolePlacement | p_head_e |
struct PolePlacement | p_head_rm |
struct Gains3rdOrder | k_att_e |
struct Gains3rdOrder | k_att_rm |
struct Gains2ndOrder | k_head_e |
struct Gains2ndOrder | k_head_rm |
struct Gains3rdOrder | k_pos_e |
struct Gains3rdOrder | k_pos_rm |
Definition in file oneloop_andi.h.
struct guidance_indi_hybrid_params |
Definition at line 74 of file guidance_indi_hybrid.h.
Data Fields | ||
---|---|---|
float | heading_bank_gain | |
float | liftd_asq | |
float | liftd_p50 | |
float | liftd_p80 | |
float | pos_gain | |
float | pos_gainz | |
float | speed_gain | |
float | speed_gainz |
struct OneloopGuidanceRef |
Definition at line 98 of file oneloop_andi.h.
Data Fields | ||
---|---|---|
float | acc[3] | |
float | jer[3] | |
float | pos[3] | |
float | vel[3] |
struct OneloopGuidanceState |
Definition at line 105 of file oneloop_andi.h.
Data Fields | ||
---|---|---|
float | acc[3] | |
float | pos[3] | |
float | vel[3] |
struct OneloopStabilizationRef |
Definition at line 111 of file oneloop_andi.h.
Data Fields | ||
---|---|---|
float | att[3] | |
float | att_2d[3] | |
float | att_3d[3] | |
float | att_d[3] |
struct OneloopStabilizationState |
Definition at line 118 of file oneloop_andi.h.
Data Fields | ||
---|---|---|
float | att[3] | |
float | att_2d[3] | |
float | att_d[3] |
struct OneloopGeneral |
Definition at line 123 of file oneloop_andi.h.
Data Fields | ||
---|---|---|
int | ctrl_type | |
struct OneloopGuidanceRef | gui_ref | |
struct OneloopGuidanceState | gui_state | |
bool | half_loop | |
struct OneloopStabilizationRef | sta_ref | |
struct OneloopStabilizationState | sta_state |
struct PolePlacement |
Definition at line 135 of file oneloop_andi.h.
Data Fields | ||
---|---|---|
float | omega_n | |
float | p3 | |
float | zeta |
struct Gains3rdOrder |
Definition at line 140 of file oneloop_andi.h.
Data Fields | ||
---|---|---|
float | k1[3] | |
float | k2[3] | |
float | k3[3] |
struct Gains2ndOrder |
Definition at line 145 of file oneloop_andi.h.
Data Fields | ||
---|---|---|
float | k2 | |
float | k3 |
#define ANDI_G_SCALING 1000.0f |
Definition at line 54 of file oneloop_andi.h.
#define ANDI_NUM_ACT COMMANDS_NB_REAL |
Definition at line 35 of file oneloop_andi.h.
#define ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT) |
Definition at line 48 of file oneloop_andi.h.
#define ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL |
Definition at line 45 of file oneloop_andi.h.
#define ANDI_NUM_VIRTUAL_ACT 2 |
Definition at line 45 of file oneloop_andi.h.
#define ANDI_OUTPUTS 6 |
Definition at line 52 of file oneloop_andi.h.
#define CTRL_ANDI 0 |
Control types.
Definition at line 57 of file oneloop_andi.h.
#define CTRL_INDI 1 |
Definition at line 58 of file oneloop_andi.h.
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 1212 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 1157 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(), OneloopGeneral::sta_ref, and FloatEulers::theta.
void oneloop_andi_read_rc | ( | bool | in_flight, |
bool | in_carefree, | ||
bool | coordinated_turn | ||
) |
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 1251 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(), OneloopGeneral::sta_ref, t_0_chirp, t_chirp, FloatEulers::theta, time_elapsed_chirp, use_increment, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, Wv, Wv_wls, FloatVect3::x, FloatVect3::y, yaw_stick_in_auto, and FloatVect3::z.
Referenced by oneloop_andi_run().
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 1388 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, du_max_1l, du_min_1l, du_pref_1l, 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, gamma_wls, 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, nu_n, num_thrusters_oneloop, number_iter, 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, u_pref, use_increment, RadioControl::values, OneloopGuidanceRef::vel, OneloopGuidanceState::vel, wls_alloc(), Wu, Wv_wls, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.
Referenced by oneloop_from_nav(), and stabilization_attitude_run().
void oneloop_andi_set_failsafe_setpoint | ( | void | ) |
void oneloop_from_nav | ( | bool | in_flight | ) |
Function that maps navigation inputs to the oneloop controller for the generated autopilot.
Definition at line 1753 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.
|
extern |
Definition at line 382 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
extern |
|
extern |
Definition at line 343 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().
|
extern |
Definition at line 338 of file oneloop_andi.c.
Referenced by oneloop_andi_init(), and oneloop_andi_run().
|
extern |
Definition at line 337 of file oneloop_andi.c.
Referenced by get_act_state_oneloop(), oneloop_andi_init(), and oneloop_andi_run().
|
extern |
Definition at line 383 of file oneloop_andi.c.
|
extern |
Definition at line 375 of file oneloop_andi.c.
Referenced by chirp_call(), oneloop_andi_RM(), and oneloop_andi_run().
|
extern |
Definition at line 358 of file oneloop_andi.c.
Referenced by G1G2_oneloop(), and oneloop_andi_run().
|
extern |
Definition at line 345 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_init(), oneloop_andi_RM(), oneloop_andi_run(), and send_oneloop_andi().
|
extern |
Definition at line 379 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
extern |
Definition at line 380 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().
|
extern |
Definition at line 287 of file oneloop_andi.c.
Referenced by oneloop_andi_sideslip().
|
extern |
|
extern |
Definition at line 1 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_get_liftd().
|
extern |
Definition at line 356 of file oneloop_andi.c.
Referenced by oneloop_andi_RM(), and oneloop_andi_run().
|
extern |
Definition at line 333 of file oneloop_andi.c.
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_RM().
|
extern |
|
extern |
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and oneloop_andi_run().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), oneloop_andi_RM(), and reshape_wind().
|
extern |
Definition at line 279 of file oneloop_andi.c.
Referenced by reshape_wind().
|
extern |
Definition at line 340 of file oneloop_andi.c.
Referenced by normalize_nu(), oneloop_andi_init(), and oneloop_andi_run().
|
extern |
Definition at line 322 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().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
|
extern |
Definition at line 384 of file oneloop_andi.c.
Referenced by init_controller(), and init_poles().
|
extern |
Definition at line 354 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_RM(), and oneloop_andi_run().
|
extern |
Definition at line 353 of file oneloop_andi.c.
Referenced by oneloop_andi_enter(), oneloop_andi_RM(), and oneloop_andi_run().
|
extern |
Definition at line 381 of file oneloop_andi.c.
Referenced by chirp_call(), chirp_pos(), and oneloop_andi_RM().
|
extern |
Definition at line 334 of file oneloop_andi.c.
Referenced by indi_apply_actuator_models().
|
extern |
Definition at line 357 of file oneloop_andi.c.
Referenced by oneloop_andi_RM().