|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
General stabilization interface for rotorcrafts. More...
#include "firmwares/rotorcraft/stabilization.h"#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"#include "firmwares/rotorcraft/stabilization/stabilization_direct.h"#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"#include "modules/radio_control/radio_control.h"#include "modules/core/abi.h"#include "autopilot.h"#include "state.h"#include "modules/datalink/telemetry.h"Go to the source code of this file.
Macros | |
| #define | STABILIZATION_RC_ID ABI_BROADCAST |
| #define | TRANSITION_TO_HOVER false |
| Transition from 0 to 100%, used for pitch offset of hybrids. | |
| #define | TRANSITION_TO_FORWARD true |
| #define | TRANSITION_TIME 3.f |
Variables | |
| struct Stabilization | stabilization |
| static abi_event | rc_ev |
| static const float | transition_increment = 1.f / (TRANSITION_TIME * PERIODIC_FREQUENCY) |
General stabilization interface for rotorcrafts.
Definition in file stabilization.c.
| #define STABILIZATION_RC_ID ABI_BROADCAST |
Definition at line 66 of file stabilization.c.
Referenced by stabilization_init().
| #define TRANSITION_TIME 3.f |
Definition at line 224 of file stabilization.c.
| #define TRANSITION_TO_FORWARD true |
Definition at line 221 of file stabilization.c.
Referenced by control_mixing_heewing_attitude_plane(), control_mixing_heewing_manual(), control_mixing_heewing_nav_run(), and stabilization_run().
| #define TRANSITION_TO_HOVER false |
Transition from 0 to 100%, used for pitch offset of hybrids.
Definition at line 220 of file stabilization.c.
Referenced by control_mixing_heewing_attitude_direct(), control_mixing_heewing_nav_run(), and stabilization_run().
|
static |
Definition at line 184 of file stabilization.c.
References autopilot_in_flight(), FALSE, stabilization, STABILIZATION_ATT_SUBMODE_CARE_FREE, STABILIZATION_ATT_SUBMODE_FORWARD, STABILIZATION_ATT_SUBMODE_HEADING, stabilization_attitude_read_rc(), stabilization_direct_read_rc(), STABILIZATION_MODE_ATTITUDE, STABILIZATION_MODE_DIRECT, STABILIZATION_MODE_RATE, stabilization_rate_read_rc(), TRUE, and UNUSED.
|
static |
Definition at line 75 of file stabilization.c.
References COMMAND_PITCH, COMMAND_ROLL, dev, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_YAW, stabilization, stateGetNedToBodyEulers_i(), and UNUSED.
Referenced by stabilization_init().
| struct StabilizationSetpoint stab_sp_from_eulers_f | ( | struct FloatEulers * | eulers | ) |
Definition at line 638 of file stabilization.c.
References StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_EULERS, StabilizationSetpoint::STAB_SP_FLOAT, and stab_sp_from_eulers_f().
Referenced by control_mixing_heewing_attitude_plane(), control_mixing_heewing_nav_run(), guidance_h_from_nav(), guidance_set_rc_h_att(), stab_sp_from_eulers_f(), and stabilization_get_failsafe_sp().
| struct StabilizationSetpoint stab_sp_from_eulers_i | ( | struct Int32Eulers * | eulers | ) |
Definition at line 628 of file stabilization.c.
References StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_EULERS, stab_sp_from_eulers_i(), and StabilizationSetpoint::STAB_SP_INT.
Referenced by guidance_module_run(), stab_sp_from_eulers_i(), and vertical_ctrl_module_run().
| struct StabilizationSetpoint stab_sp_from_ltp_f | ( | struct FloatVect2 * | vect, |
| float | heading ) |
Definition at line 659 of file stabilization.c.
References heading, StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_FLOAT, stab_sp_from_ltp_f(), and StabilizationSetpoint::STAB_SP_LTP.
Referenced by stab_sp_from_ltp_f().
| struct StabilizationSetpoint stab_sp_from_ltp_i | ( | struct Int32Vect2 * | vect, |
| int32_t | heading ) |
Definition at line 648 of file stabilization.c.
References heading, StabilizationSetpoint::sp, stab_sp_from_ltp_i(), StabilizationSetpoint::STAB_SP_INT, and StabilizationSetpoint::STAB_SP_LTP.
Referenced by guidance_pid_h_run(), and stab_sp_from_ltp_i().
| struct StabilizationSetpoint stab_sp_from_quat_f | ( | struct FloatQuat * | quat | ) |
Definition at line 607 of file stabilization.c.
References StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_FLOAT, stab_sp_from_quat_f(), and StabilizationSetpoint::STAB_SP_QUAT.
Referenced by guidance_indi_run(), stab_sp_from_quat_f(), stabilization_attitude_read_rc(), and stabilization_attitude_reset_rc().
| struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f | ( | struct FloatQuat * | quat, |
| struct FloatRates * | rates ) |
Definition at line 617 of file stabilization.c.
References StabilizationSetpoint::r_sp, StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_FLOAT, stab_sp_from_quat_ff_rates_f(), and StabilizationSetpoint::STAB_SP_QUAT_FF_RATE.
Referenced by guidance_indi_run(), and stab_sp_from_quat_ff_rates_f().
| struct StabilizationSetpoint stab_sp_from_quat_i | ( | struct Int32Quat * | quat | ) |
Definition at line 597 of file stabilization.c.
References StabilizationSetpoint::sp, stab_sp_from_quat_i(), StabilizationSetpoint::STAB_SP_INT, and StabilizationSetpoint::STAB_SP_QUAT.
Referenced by guidance_hybrid_set_cmd_i(), and stab_sp_from_quat_i().
| struct StabilizationSetpoint stab_sp_from_rates_f | ( | struct FloatRates * | rates | ) |
Definition at line 680 of file stabilization.c.
References StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_FLOAT, stab_sp_from_rates_f(), and StabilizationSetpoint::STAB_SP_RATES.
Referenced by stab_sp_from_rates_f(), stabilization_indi_attitude_run(), and stabilization_rate_read_rc().
| struct StabilizationSetpoint stab_sp_from_rates_i | ( | struct Int32Rates * | rates | ) |
Definition at line 670 of file stabilization.c.
References StabilizationSetpoint::sp, stab_sp_from_rates_i(), StabilizationSetpoint::STAB_SP_INT, and StabilizationSetpoint::STAB_SP_RATES.
Referenced by stab_sp_from_rates_i().
|
static |
Definition at line 301 of file stabilization.c.
References heading, FloatEulers::phi, FloatEulers::psi, stab_sp_rotate_f(), stateGetNedToBodyEulers_f(), and FloatEulers::theta.
Referenced by stab_sp_rotate_f(), stab_sp_to_eulers_f(), and stab_sp_to_eulers_i().
|
static |
Definition at line 287 of file stabilization.c.
References heading, INT32_TRIG_FRAC, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, stab_sp_rotate_i(), stateGetNedToBodyEulers_i(), and Int32Eulers::theta.
Referenced by stab_sp_rotate_i(), stab_sp_to_eulers_f(), and stab_sp_to_eulers_i().
| struct FloatEulers stab_sp_to_eulers_f | ( | struct StabilizationSetpoint * | sp | ) |
Definition at line 454 of file stabilization.c.
References EULERS_FLOAT_OF_BFP, float_eulers_of_quat(), QUAT_FLOAT_OF_BFP, stab_sp_rotate_f(), stab_sp_rotate_i(), and stab_sp_to_eulers_f().
Referenced by guidance_set_rc_h_att(), stab_sp_to_eulers_f(), stabilization_attitude_plane_pid_run(), stabilization_attitude_run(), and stabilization_attitude_run().
| struct Int32Eulers stab_sp_to_eulers_i | ( | struct StabilizationSetpoint * | sp | ) |
Definition at line 415 of file stabilization.c.
References EULERS_BFP_OF_REAL, int32_eulers_of_quat(), QUAT_BFP_OF_REAL, stab_sp_rotate_f(), stab_sp_rotate_i(), and stab_sp_to_eulers_i().
Referenced by reset_horizontal_vars(), stab_sp_to_eulers_i(), stabilization_attitude_run(), stabilization_attitude_run(), and stabilization_indi_attitude_run().
| struct FloatQuat stab_sp_to_quat_f | ( | struct StabilizationSetpoint * | sp | ) |
Definition at line 373 of file stabilization.c.
References EULERS_FLOAT_OF_BFP, float_quat_identity(), float_quat_of_eulers(), QUAT_FLOAT_OF_BFP, quat_from_earth_cmd_f(), quat_from_earth_cmd_i(), and stab_sp_to_quat_f().
Referenced by stab_sp_to_quat_f(), stabilization_andi_run(), stabilization_attitude_run(), and stabilization_indi_attitude_run().
| struct Int32Quat stab_sp_to_quat_i | ( | struct StabilizationSetpoint * | sp | ) |
Definition at line 331 of file stabilization.c.
References EULERS_BFP_OF_REAL, int32_quat_identity(), int32_quat_of_eulers(), QUAT_BFP_OF_REAL, quat_from_earth_cmd_f(), quat_from_earth_cmd_i(), and stab_sp_to_quat_i().
Referenced by stab_sp_to_quat_i(), stabilization_attitude_run(), stabilization_attitude_run(), and stabilization_indi_attitude_run().
| struct FloatRates stab_sp_to_rates_f | ( | struct StabilizationSetpoint * | sp | ) |
Definition at line 510 of file stabilization.c.
References RATES_FLOAT_OF_BFP, and stab_sp_to_rates_f().
Referenced by stab_sp_to_rates_f(), stabilization_andi_run(), stabilization_indi_attitude_run(), stabilization_indi_rate_run(), and stabilization_rate_run().
| struct Int32Rates stab_sp_to_rates_i | ( | struct StabilizationSetpoint * | sp | ) |
Definition at line 493 of file stabilization.c.
References RATES_BFP_OF_REAL, and stab_sp_to_rates_i().
Referenced by stab_sp_to_rates_i().
| struct StabilizationSetpoint WEAK stabilization_attitude_read_rc | ( | bool | in_flight, |
| bool | carefree, | ||
| bool | coordinated_turn, | ||
| struct RadioControl * | rc ) |
Retrun attitude setpoint from RC as euler angles.
weak function that can be re-implemeted if needed
| [in] | in_flight | true if in flight |
| [in] | in_carefree | true if in carefree mode |
| [in] | coordinated_turn | true if in horizontal mode forward |
| [in] | rc | pointer to radio control structure |
Definition at line 172 of file stabilization.c.
References stab_sp_from_quat_f(), stabilization, stabilization_attitude_read_rc(), stabilization_attitude_read_rc_setpoint(), and stabilization_attitude_read_rc_setpoint_earth_bound().
Referenced by rc_cb(), stabilization_attitude_init(), and stabilization_attitude_read_rc().
|
static |
Definition at line 132 of file stabilization.c.
References stab_sp_from_quat_f(), stabilization, stabilization_attitude_reset_rc_setpoint(), and stateGetNedToBodyQuat_f().
Referenced by stabilization_mode_changed().
| void stabilization_filter_commands | ( | void | ) |
Command filter for vibrating airframes.
Definition at line 314 of file stabilization.c.
References COMMAND_PITCH, COMMAND_ROLL, MAX_PPRZ, stabilization, and update_second_order_low_pass_int().
Referenced by stabilization_run().
| struct StabilizationSetpoint stabilization_get_failsafe_sp | ( | void | ) |
Get stabilization setpoint for failsafe.
Definition at line 275 of file stabilization.c.
References FloatEulers::psi, stab_sp_from_eulers_f(), stabilization_get_failsafe_sp(), and stateGetNedToBodyEulers_f().
Referenced by autopilot_static_periodic(), and stabilization_get_failsafe_sp().
| void stabilization_init | ( | void | ) |
Init function.
Definition at line 94 of file stabilization.c.
References DefaultPeriodic, SecondOrderLowPass_int::i, init_second_order_low_pass_int(), rc_cb(), rc_ev, register_periodic_telemetry(), send_tune_hover(), STAB_SP_SET_EULERS_ZERO, stabilization, STABILIZATION_ATT_SUBMODE_HEADING, stabilization_attitude_rc_setpoint_init(), STABILIZATION_MODE_NONE, and STABILIZATION_RC_ID.
Check mode change.
Definition at line 138 of file stabilization.c.
References stabilization, STABILIZATION_ATT_SUBMODE_CARE_FREE, stabilization_attitude_enter(), stabilization_attitude_reset_care_free_heading(), stabilization_attitude_reset_rc(), stabilization_direct_enter(), STABILIZATION_MODE_ATTITUDE, STABILIZATION_MODE_DIRECT, STABILIZATION_MODE_NONE, STABILIZATION_MODE_RATE, and stabilization_rate_enter().
Referenced by autopilot_static_set_mode(), control_mixing_heewing_attitude_direct_enter(), control_mixing_heewing_attitude_plane_enter(), and control_mixing_heewing_nav_enter().
| void stabilization_run | ( | bool | in_flight, |
| struct StabilizationSetpoint * | sp, | ||
| struct ThrustSetpoint * | thrust, | ||
| int32_t * | cmd ) |
Call default stabilization control.
| [in] | in_flight | true if rotorcraft is flying |
| [in] | sp | pointer to the stabilization setpoint, computed in guidance or from RC |
| [in] | thrust | pointer to thrust setpoint computed by vertical guidance |
| [out] | cmd | pointer to the output command vector |
Definition at line 242 of file stabilization.c.
References StabilizationSetpoint::sp, stabilization, STABILIZATION_ATT_SUBMODE_FORWARD, stabilization_attitude_run(), stabilization_direct_run(), stabilization_filter_commands(), STABILIZATION_MODE_ATTITUDE, STABILIZATION_MODE_DIRECT, STABILIZATION_MODE_RATE, stabilization_rate_run(), transition_run(), TRANSITION_TO_FORWARD, and TRANSITION_TO_HOVER.
Referenced by autopilot_static_periodic(), control_mixing_heewing_attitude_direct(), control_mixing_heewing_attitude_plane(), control_mixing_heewing_nav_run(), and guidance_module_run().
| struct ThrustSetpoint th_sp_from_incr_f | ( | float | th_increment, |
| uint8_t | axis ) |
Definition at line 720 of file stabilization.c.
References ThrustSetpoint::sp, th_sp_from_incr_f(), ThrustSetpoint::THRUST_INCR_SP, and ThrustSetpoint::THRUST_SP_FLOAT.
Referenced by th_sp_from_incr_f().
| struct ThrustSetpoint th_sp_from_incr_i | ( | int32_t | th_increment, |
| uint8_t | axis ) |
Definition at line 710 of file stabilization.c.
References ThrustSetpoint::sp, th_sp_from_incr_i(), ThrustSetpoint::THRUST_INCR_SP, and ThrustSetpoint::THRUST_SP_INT.
Referenced by th_sp_from_incr_i().
| struct ThrustSetpoint th_sp_from_incr_vect_f | ( | float | th_increment[3] | ) |
Definition at line 766 of file stabilization.c.
References ThrustSetpoint::sp, th_sp_from_incr_vect_f(), ThrustSetpoint::THRUST_INCR_SP, and ThrustSetpoint::THRUST_SP_FLOAT.
Referenced by guidance_indi_run(), and th_sp_from_incr_vect_f().
| struct ThrustSetpoint th_sp_from_incr_vect_i | ( | int32_t | th_increment[3] | ) |
Definition at line 754 of file stabilization.c.
References ThrustSetpoint::sp, th_sp_from_incr_vect_i(), ThrustSetpoint::THRUST_INCR_SP, and ThrustSetpoint::THRUST_SP_INT.
Referenced by th_sp_from_incr_vect_i().
| struct ThrustSetpoint th_sp_from_thrust_f | ( | float | thrust, |
| uint8_t | axis ) |
Definition at line 700 of file stabilization.c.
References ThrustSetpoint::sp, th_sp_from_thrust_f(), ThrustSetpoint::THRUST_SP, and ThrustSetpoint::THRUST_SP_FLOAT.
Referenced by th_sp_from_thrust_f().
| struct ThrustSetpoint th_sp_from_thrust_i | ( | int32_t | thrust, |
| uint8_t | axis ) |
Definition at line 690 of file stabilization.c.
References ThrustSetpoint::sp, th_sp_from_thrust_i(), ThrustSetpoint::THRUST_SP, and ThrustSetpoint::THRUST_SP_INT.
Referenced by autopilot_static_periodic(), control_mixing_heewing_attitude_plane(), guidance_hybrid_vertical(), guidance_indi_run(), guidance_module_run(), guidance_pid_v_run(), guidance_v_from_nav(), guidance_v_guided_run(), guidance_v_run(), th_sp_from_thrust_i(), vertical_ctrl_module_run(), and vertical_ctrl_module_run().
| struct ThrustSetpoint th_sp_from_thrust_vect_f | ( | float | thrust[3] | ) |
Definition at line 742 of file stabilization.c.
References ThrustSetpoint::sp, th_sp_from_thrust_vect_f(), ThrustSetpoint::THRUST_SP, and ThrustSetpoint::THRUST_SP_FLOAT.
Referenced by guidance_set_rc_h_thrust(), and th_sp_from_thrust_vect_f().
| struct ThrustSetpoint th_sp_from_thrust_vect_i | ( | int32_t | thrust[3] | ) |
Definition at line 730 of file stabilization.c.
References ThrustSetpoint::sp, th_sp_from_thrust_vect_i(), ThrustSetpoint::THRUST_SP, and ThrustSetpoint::THRUST_SP_INT.
Referenced by th_sp_from_thrust_vect_i().
| float th_sp_to_incr_f | ( | struct ThrustSetpoint * | th, |
| int32_t | thrust, | ||
| uint8_t | axis ) |
Definition at line 579 of file stabilization.c.
References ThrustSetpoint::format, MAX_PPRZ, ThrustSetpoint::sp, and ThrustSetpoint::type.
Referenced by stabilization_indi_rate_run().
| int32_t th_sp_to_incr_i | ( | struct ThrustSetpoint * | th, |
| int32_t | thrust, | ||
| uint8_t | axis ) |
Definition at line 562 of file stabilization.c.
References ThrustSetpoint::format, MAX_PPRZ, ThrustSetpoint::sp, and ThrustSetpoint::type.
| float th_sp_to_thrust_f | ( | struct ThrustSetpoint * | th, |
| int32_t | thrust, | ||
| uint8_t | axis ) |
Definition at line 544 of file stabilization.c.
References ThrustSetpoint::format, MAX_PPRZ, ThrustSetpoint::sp, and ThrustSetpoint::type.
Referenced by guidance_set_rc_h_thrust(), and stabilization_andi_run().
| int32_t th_sp_to_thrust_i | ( | struct ThrustSetpoint * | th, |
| int32_t | thrust, | ||
| uint8_t | axis ) |
Definition at line 527 of file stabilization.c.
References ThrustSetpoint::format, MAX_PPRZ, ThrustSetpoint::sp, and ThrustSetpoint::type.
Referenced by guidance_hybrid_vertical(), stabilization_attitude_plane_pid_run(), stabilization_attitude_run(), stabilization_attitude_run(), stabilization_direct_run(), stabilization_indi_rate_run(), and stabilization_rate_run().
|
inlinestatic |
Definition at line 229 of file stabilization.c.
References stabilization, and transition_increment.
Referenced by stabilization_run().
|
static |
Definition at line 69 of file stabilization.c.
| struct Stabilization stabilization |
Definition at line 41 of file stabilization.c.
Referenced by autopilot_check_in_flight(), autopilot_in_flight_end_detection(), autopilot_static_periodic(), autopilot_static_set_mode(), control_mixing_heewing_attitude_direct(), control_mixing_heewing_attitude_plane(), control_mixing_heewing_nav_run(), ctrl_module_run(), ctrl_module_run(), draw_osd(), eff_scheduling_generic_periodic(), eff_scheduling_periodic_a(), guidance_flip_run(), guidance_indi_enter(), guidance_indi_hybrid_set_wls_settings(), guidance_indi_init_filters(), guidance_indi_run(), guidance_module_enter(), guidance_module_run(), guidance_pid_h_run(), guidance_v_thrust_adapt(), ins_flow_update(), logger_file_write_row(), nav_heli_spinup_run(), nav_heli_spinup_setup(), oneloop_andi_run(), oneloop_nB_run(), rc_cb(), reset_horizontal_vars(), rotwing_state_hover_motors_idling(), rotwing_state_periodic(), send_att(), send_att(), send_att(), send_att(), send_att(), send_bebop_actuators(), send_rate(), send_rate(), send_tune_hover(), send_windtunnel_meas(), stabilization_attitude_read_rc(), stabilization_attitude_reset_rc(), stabilization_filter_commands(), stabilization_init(), stabilization_mode_changed(), stabilization_run(), transition_run(), and vertical_ctrl_module_run().
|
static |
Definition at line 227 of file stabilization.c.
Referenced by transition_run(), and transition_run().