ArduCopter/mode_flip.cpp

   1 #include "Copter.h"
   2
   3 #if MODE_FLIP_ENABLED
   4
   5 /*
   6  * Init and run calls for flip flight mode
   7  *      original implementation in 2010 by Jose Julio
   8  *      Adapted and updated for AC2 in 2011 by Jason Short
   9  *
  10  *      Controls:
  11  *          RC7_OPTION - RC12_OPTION parameter must be set to "Flip" (AUXSW_FLIP) which is "2"
  12  *          Pilot switches to Stabilize, Acro or AltHold flight mode and puts ch7/ch8 switch to ON position
  13  *          Vehicle will Roll right by default but if roll or pitch stick is held slightly left, forward or back it will flip in that direction
  14  *          Vehicle should complete the roll within 2.5sec and will then return to the original flight mode it was in before flip was triggered
  15  *          Pilot may manually exit flip by switching off ch7/ch8 or by moving roll stick to >40deg left or right
  16  *
  17  *      State machine approach:
  18  *          FlipState::Start (while copter is leaning <45deg) : roll right at 400deg/sec, increase throttle
  19  *          FlipState::Roll (while copter is between +45deg ~ -90) : roll right at 400deg/sec, reduce throttle
  20  *          FlipState::Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
  21  */
  22
  23 #define FLIP_THR_INC        0.20f   // throttle increase during FlipState::Start stage (under 45deg lean angle)
  24 #define FLIP_THR_DEC        0.24f   // throttle decrease during FlipState::Roll stage (between 45deg ~ -90deg roll)
  25 #define FLIP_ROTATION_RATE  40000   // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec)
  26 #define FLIP_TIMEOUT_MS     2500    // timeout after 2.5sec.  Vehicle will switch back to original flight mode
  27 #define FLIP_RECOVERY_ANGLE 500     // consider successful recovery when roll is back within 5 degrees of original
  28
  29 #define FLIP_ROLL_RIGHT      1      // used to set flip_dir
  30 #define FLIP_ROLL_LEFT      -1      // used to set flip_dir
  31
  32 #define FLIP_PITCH_BACK      1      // used to set flip_dir
  33 #define FLIP_PITCH_FORWARD  -1      // used to set flip_dir
  34
  35 // flip_init - initialise flip controller
  36 bool ModeFlip::init(bool ignore_checks)
  37 {
  38     // only allow flip from some flight modes, for example ACRO, Stabilize, AltHold or FlowHold flight modes
  39     if (!copter.flightmode->allows_flip()) {
  40         return false;
  41     }
  42
  43     // if in acro or stabilize ensure throttle is above zero
  44     if (copter.ap.throttle_zero && (copter.flightmode->mode_number() == Mode::Number::ACRO || copter.flightmode->mode_number() == Mode::Number::STABILIZE)) {
  45         return false;
  46     }
  47
  48     // ensure roll input is less than 40deg
  49     if (abs(channel_roll->get_control_in()) >= 4000) {
  50         return false;
  51     }
  52
  53     // only allow flip when flying
  54     if (!motors->armed() || copter.ap.land_complete) {
  55         return false;
  56     }
  57
  58     // capture original flight mode so that we can return to it after completion
  59     orig_control_mode = copter.flightmode->mode_number();
  60
  61     // initialise state
  62     _state = FlipState::Start;
  63     start_time_ms = millis();
  64
  65     roll_dir = pitch_dir = 0;
  66
  67     // choose direction based on pilot's roll and pitch sticks
  68     if (channel_pitch->get_control_in() > 300) {
  69         pitch_dir = FLIP_PITCH_BACK;
  70     } else if (channel_pitch->get_control_in() < -300) {
  71         pitch_dir = FLIP_PITCH_FORWARD;
  72     } else if (channel_roll->get_control_in() >= 0) {
  73         roll_dir = FLIP_ROLL_RIGHT;
  74     } else {
  75         roll_dir = FLIP_ROLL_LEFT;
  76     }
  77
  78     // log start of flip
  79     LOGGER_WRITE_EVENT(LogEvent::FLIP_START);
  80
  81     // capture current attitude which will be used during the FlipState::Recovery stage
  82     const float angle_max = copter.aparm.angle_max;
  83     orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max);
  84     orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max);
  85     orig_attitude.z = ahrs.yaw_sensor;
  86
  87     return true;
  88 }
  89
  90 // run - runs the flip controller
  91 // should be called at 100hz or more
  92 void ModeFlip::run()
  93 {
  94     // if pilot inputs roll > 40deg or timeout occurs abandon flip
  95     if (!motors->armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - start_time_ms) > FLIP_TIMEOUT_MS)) {
  96         _state = FlipState::Abandon;
  97     }
  98
  99     // get pilot's desired throttle
 100     float throttle_out = get_pilot_desired_throttle();
 101
 102     // set motors to full range
 103     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 104
 105     // get corrected angle based on direction and axis of rotation
 106     // we flip the sign of flip_angle to minimize the code repetition
 107     int32_t flip_angle;
 108
 109     if (roll_dir != 0) {
 110         flip_angle = ahrs.roll_sensor * roll_dir;
 111     } else {
 112         flip_angle = ahrs.pitch_sensor * pitch_dir;
 113     }
 114
 115     // state machine
 116     switch (_state) {
 117
 118     case FlipState::Start:
 119         // under 45 degrees request 400deg/sec roll or pitch
 120         attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * roll_dir, FLIP_ROTATION_RATE * pitch_dir, 0.0);
 121
 122         // increase throttle
 123         throttle_out += FLIP_THR_INC;
 124
 125         // beyond 45deg lean angle move to next stage
 126         if (flip_angle >= 4500) {
 127             if (roll_dir != 0) {
 128                 // we are rolling
 129             _state = FlipState::Roll;
 130             } else {
 131                 // we are pitching
 132                 _state = FlipState::Pitch_A;
 133         }
 134         }
 135         break;
 136
 137     case FlipState::Roll:
 138         // between 45deg ~ -90deg request 400deg/sec roll
 139         attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * roll_dir, 0.0, 0.0);
 140         // decrease throttle
 141         throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
 142
 143         // beyond -90deg move on to recovery
 144         if ((flip_angle < 4500) && (flip_angle > -9000)) {
 145             _state = FlipState::Recover;
 146         }
 147         break;
 148
 149     case FlipState::Pitch_A:
 150         // between 45deg ~ -90deg request 400deg/sec pitch
 151         attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * pitch_dir, 0.0);
 152         // decrease throttle
 153         throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
 154
 155         // check roll for inversion
 156         if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
 157             _state = FlipState::Pitch_B;
 158         }
 159         break;
 160
 161     case FlipState::Pitch_B:
 162         // between 45deg ~ -90deg request 400deg/sec pitch
 163         attitude_control->input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * pitch_dir, 0.0);
 164         // decrease throttle
 165         throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
 166
 167         // check roll for inversion
 168         if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
 169             _state = FlipState::Recover;
 170         }
 171         break;
 172
 173     case FlipState::Recover: {
 174         // use originally captured earth-frame angle targets to recover
 175         attitude_control->input_euler_angle_roll_pitch_yaw(orig_attitude.x, orig_attitude.y, orig_attitude.z, false);
 176
 177         // increase throttle to gain any lost altitude
 178         throttle_out += FLIP_THR_INC;
 179
 180         float recovery_angle;
 181         if (roll_dir != 0) {
 182             // we are rolling
 183             recovery_angle = fabsf(orig_attitude.x - (float)ahrs.roll_sensor);
 184         } else {
 185             // we are pitching
 186             recovery_angle = fabsf(orig_attitude.y - (float)ahrs.pitch_sensor);
 187         }
 188
 189         // check for successful recovery
 190         if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
 191             // restore original flight mode
 192             if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
 193                 // this should never happen but just in case
 194                 copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
 195             }
 196             // log successful completion
 197             LOGGER_WRITE_EVENT(LogEvent::FLIP_END);
 198         }
 199         break;
 200
 201     }
 202     case FlipState::Abandon:
 203         // restore original flight mode
 204         if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
 205             // this should never happen but just in case
 206             copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
 207         }
 208         // log abandoning flip
 209         LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED);
 210         break;
 211     }
 212
 213     // output pilot's throttle without angle boost
 214     attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt);
 215 }
 216
 217 #endif