21 #define FLIP_THR_INC 0.20f // throttle increase during Flip_Start stage (under 45deg lean angle) 22 #define FLIP_THR_DEC 0.24f // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll) 23 #define FLIP_ROTATION_RATE 40000 // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec) 24 #define FLIP_TIMEOUT_MS 2500 // timeout after 2.5sec. Vehicle will switch back to original flight mode 25 #define FLIP_RECOVERY_ANGLE 500 // consider successful recovery when roll is back within 5 degrees of original 27 #define FLIP_ROLL_RIGHT 1 // used to set flip_dir 28 #define FLIP_ROLL_LEFT -1 // used to set flip_dir 30 #define FLIP_PITCH_BACK 1 // used to set flip_dir 31 #define FLIP_PITCH_FORWARD -1 // used to set flip_dir 62 if (!
motors->armed() ||
ap.land_complete) {
90 float angle_max =
copter.aparm.angle_max;
103 float recovery_angle;
134 if (flip_angle >= 4500) {
135 if (flip_roll_dir != 0) {
152 if ((flip_angle < 4500) && (flip_angle > -9000)) {
188 if (flip_roll_dir != 0) {
bool init(bool ignore_checks) override
#define FLIP_RECOVERY_ANGLE
void Log_Write_Event(uint8_t id)
Vector3f flip_orig_attitude
AC_AttitudeControl_t *& attitude_control
#define FLIP_PITCH_FORWARD
int16_t get_control_in() const
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid=0.0f)
DESIRED_THROTTLE_UNLIMITED
RC_Channel *& channel_roll
#define ERROR_CODE_FLIP_ABANDONED
#define ERROR_SUBSYSTEM_FLIP
float constrain_float(const float amt, const float low, const float high)
control_mode_t flip_orig_control_mode
AP_HAL_MAIN_CALLBACKS & copter
#define FLIP_ROTATION_RATE
RC_Channel *& channel_throttle
RC_Channel *& channel_pitch