Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_FLIP_ENABLED
       4             : 
       5             : /*
       6             :  * Flight mode which performs a flip, then self-exits.
       7             :  *      original implementation in 2010 by Jose Julio
       8             :  *      Adapted and updated for AC2 in 2011 by Jason Short
       9             :  *
      10             :  *      Flip can be initiated by either a (configured) RC switch or switching modes to FLIP.
      11             :  *      With no pilot input, the flip is in the ROLL_LEFT direction.
      12             :  *      By gently holding the roll/pitch stick in a direction during mode entry, the pilot can select the flip's direction.
      13             :  *      While in FLIP mode, the flip can be aborted by:
      14             :  *      - Pushing either the roll or pitch stick to a 'high magnitude' value.
      15             :  *      - Toggling the (configured) RC switch away from HIGH.
      16             :  *      - Switching mode away from FLIP.
      17             :  *      No matter whether the flip completes or is aborted, the mode will switch away afterwards.
      18             :  *
      19             :  */
      20             : 
      21             : #define FLIP_THR_INC            0.20f           // throttle increase during FlipState::Start stage (under 45deg lean angle)
      22             : #define FLIP_THR_DEC            0.24f           // throttle decrease during FlipState::Roll stage (between 45deg ~ -90deg roll)
      23             : #define FLIP_RECOVERY_ANGLE_RAD radians(5.0)    // consider successful recovery when roll is back within 5 degrees of original
      24             : 
      25             : #define FLIP_ROLL_RIGHT      1      // used to set flip_dir
      26             : #define FLIP_ROLL_LEFT      -1      // used to set flip_dir
      27             : 
      28             : #define FLIP_PITCH_BACK      1      // used to set flip_dir
      29             : #define FLIP_PITCH_FORWARD  -1      // used to set flip_dir
      30             : 
      31             : #define FLIP_RATE_DEFAULT_DPS 400.0f  // default flip rate in deg/s
      32             : 
      33             : const AP_Param::GroupInfo ModeFlip::var_info[] = {
      34             : 
      35             :     // @Param: RATE
      36             :     // @DisplayName: Flip Mode Rotational Rate
      37             :     // @Description: Rotational Rate for Flip Mode in Deg/s.  Be sure to set a rotational rate that the aircraft can achieve.
      38             :     // @Units: deg/s
      39             :     // @Range: 60 1000
      40             :     // @Increment: 1
      41             :     // @User: Standard
      42             :     AP_GROUPINFO("RATE", 1, ModeFlip, flip_rate_dps, FLIP_RATE_DEFAULT_DPS),
      43             : 
      44             :     AP_GROUPEND
      45             : };
      46             : 
      47             : // constructor
      48        1001 : ModeFlip::ModeFlip(void) : Mode()
      49             : {
      50             :     // load parameter defaults
      51        1001 :     AP_Param::setup_object_defaults(this, var_info);
      52        1001 : }
      53             : 
      54             : // flip_init - initialise flip controller
      55          47 : bool ModeFlip::init(bool ignore_checks)
      56             : {
      57             :     // only allow flip from some flight modes, for example ACRO, Stabilize, AltHold or FlowHold flight modes
      58          47 :     if (!copter.flightmode->allows_flip()) {
      59          32 :         return false;
      60             :     }
      61             : 
      62             :     // if in acro or stabilize ensure throttle is above zero
      63          15 :     if (copter.ap.throttle_zero && (copter.flightmode->mode_number() == Mode::Number::ACRO || copter.flightmode->mode_number() == Mode::Number::STABILIZE)) {
      64           0 :         return false;
      65             :     }
      66             : 
      67          15 :     if (input_is_high_magnitude(*channel_roll) || input_is_high_magnitude(*channel_pitch)) {
      68           0 :         return false;
      69             :     }
      70             : 
      71             :     // only allow flip when flying
      72          15 :     if (!motors->armed() || copter.ap.land_complete) {
      73           0 :         return false;
      74             :     }
      75             : 
      76             :     // capture original flight mode so that we can return to it after completion
      77          15 :     orig_control_mode = copter.flightmode->mode_number();
      78             : 
      79             :     // initialise state
      80          15 :     _state = FlipState::Start;
      81          15 :     abandon_requested = false;
      82          15 :     start_time_ms = millis();
      83             : 
      84          15 :     roll_dir = pitch_dir = 0;
      85             : 
      86             :     // choose direction based on pilot's roll and pitch sticks
      87          15 :     if (channel_pitch->get_control_in() > 300) {
      88           1 :         pitch_dir = FLIP_PITCH_BACK;
      89          14 :     } else if (channel_pitch->get_control_in() < -300) {
      90           0 :         pitch_dir = FLIP_PITCH_FORWARD;
      91          14 :     } else if (channel_roll->get_control_in() >= 0) {
      92           4 :         roll_dir = FLIP_ROLL_RIGHT;
      93             :     } else {
      94          10 :         roll_dir = FLIP_ROLL_LEFT;
      95             :     }
      96             : 
      97             :     // log start of flip
      98          15 :     LOGGER_WRITE_EVENT(LogEvent::FLIP_START);
      99             : 
     100             :     // capture current attitude which will be used during the FlipState::Recovery stage
     101          15 :     const float angle_max_rad = attitude_control->lean_angle_max_rad();
     102          15 :     orig_attitude_euler_rad.x = constrain_float(ahrs.get_roll_rad(), -angle_max_rad, angle_max_rad);
     103          15 :     orig_attitude_euler_rad.y = constrain_float(ahrs.get_pitch_rad(), -angle_max_rad, angle_max_rad);
     104          15 :     orig_attitude_euler_rad.z = ahrs.get_yaw_rad();
     105             : 
     106          15 :     return true;
     107             : }
     108             : 
     109             : // run - runs the flip controller
     110             : // should be called at 100hz or more
     111        4313 : void ModeFlip::run()
     112             : {
     113             :     // get flip rotation rate parameter and constrain to range from 60 to 1000 deg/s.
     114        4313 :     const float flip_rate_rads = radians(constrain_float(flip_rate_dps.get(), 60.0f, 1000.0f));
     115             :     // determine timeout for flip based on rotation rate.  Time is doubled to allow for poorly tuned vehicles.
     116        4313 :     const float flip_time_out_ms = 1000.0f * (2.0f * radians(360.0f) / flip_rate_rads);
     117             : 
     118             :     // if pilot inputs roll > 40deg or timeout occurs abandon flip
     119        4313 :     if (abandon_requested || !motors->armed() || input_is_high_magnitude(*channel_roll) || input_is_high_magnitude(*channel_pitch) || ((millis() - start_time_ms) > flip_time_out_ms)) {
     120           2 :         _state = FlipState::Abandon;
     121           2 :         abandon_requested = false;
     122             :     }
     123             : 
     124             :     // get pilot's desired throttle
     125        4313 :     float throttle_out = get_pilot_desired_throttle();
     126             : 
     127             :     // set motors to full range
     128        4313 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     129             : 
     130             :     // get corrected angle based on direction and axis of rotation
     131             :     // we flip the sign of flip_angle to minimize the code repetition
     132             :     float flip_angle_rad;
     133             : 
     134        4313 :     if (roll_dir != 0) {
     135        3879 :         flip_angle_rad = ahrs.get_roll_rad() * roll_dir;
     136             :     } else {
     137         434 :         flip_angle_rad = ahrs.get_pitch_rad() * pitch_dir;
     138             :     }
     139             : 
     140             :     // state machine
     141        4313 :     switch (_state) {
     142             : 
     143        1751 :     case FlipState::Start:
     144             :         // request flip_rate_rads in the nonzero direction
     145        1751 :         attitude_control->input_rate_bf_roll_pitch_yaw_rads(flip_rate_rads * roll_dir, flip_rate_rads * pitch_dir, 0.0);
     146             : 
     147             :         // increase throttle
     148        1751 :         throttle_out += FLIP_THR_INC;
     149             : 
     150             :         // beyond specified lean angle: move to next stage
     151        1751 :         if (flip_angle_rad >= radians(45.0)) {
     152          13 :             if (roll_dir != 0) {
     153             :                 // we are rolling
     154          12 :                 _state = FlipState::Roll;
     155             :             } else {
     156             :                 // we are pitching
     157           1 :                 _state = FlipState::Pitch_A;
     158             :             }
     159             :         }
     160        1751 :         break;
     161             : 
     162        1895 :     case FlipState::Roll:
     163             :         // request flip_rate_rads roll
     164        1895 :         attitude_control->input_rate_bf_roll_pitch_yaw_rads(flip_rate_rads * roll_dir, 0.0, 0.0);
     165             :         // decrease throttle
     166        1895 :         throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
     167             : 
     168             :         // if state transition conditions are met: move on to recovery
     169        1895 :         if ((flip_angle_rad < radians(45.0)) && (flip_angle_rad > -radians(90.0))) {
     170           5 :             _state = FlipState::Recover;
     171             :         }
     172        1895 :         break;
     173             : 
     174          51 :     case FlipState::Pitch_A:
     175             :         // request flip_rate_rads pitch
     176          51 :         attitude_control->input_rate_bf_roll_pitch_yaw_rads(0.0f, flip_rate_rads * pitch_dir, 0.0);
     177             :         // decrease throttle
     178          51 :         throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
     179             : 
     180             :         // check roll for inversion
     181          51 :         if ((fabsf(ahrs.get_roll_rad()) > radians(90.0)) && (flip_angle_rad > radians(45.0))) {
     182           1 :             _state = FlipState::Pitch_B;
     183             :         }
     184          51 :         break;
     185             : 
     186         220 :     case FlipState::Pitch_B:
     187             :         // request flip_rate_rads pitch
     188         220 :         attitude_control->input_rate_bf_roll_pitch_yaw_rads(0.0, flip_rate_rads * pitch_dir, 0.0);
     189             :         // decrease throttle
     190         220 :         throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
     191             : 
     192             :         // check roll for inversion
     193         220 :         if ((fabsf(ahrs.get_roll_rad()) < radians(90.0)) && (flip_angle_rad > -radians(45.0))) {
     194           1 :             _state = FlipState::Recover;
     195             :         }
     196         220 :         break;
     197             : 
     198         394 :     case FlipState::Recover: {
     199             :         // use originally captured earth-frame angle targets to recover
     200         394 :         attitude_control->input_euler_angle_roll_pitch_yaw_rad(orig_attitude_euler_rad.x, orig_attitude_euler_rad.y, orig_attitude_euler_rad.z, false);
     201             : 
     202             :         // increase throttle to gain any lost altitude
     203         394 :         throttle_out += FLIP_THR_INC;
     204             : 
     205             :         float recovery_angle_rad;
     206         394 :         if (roll_dir != 0) {
     207             :             // we are rolling
     208         341 :             recovery_angle_rad = fabsf(orig_attitude_euler_rad.x - ahrs.get_roll_rad());
     209             :         } else {
     210             :             // we are pitching
     211          53 :             recovery_angle_rad = fabsf(orig_attitude_euler_rad.y - ahrs.get_pitch_rad());
     212             :         }
     213             : 
     214             :         // check for successful recovery
     215         394 :         if (fabsf(recovery_angle_rad) <= FLIP_RECOVERY_ANGLE_RAD) {
     216             :             // restore original flight mode
     217           3 :             if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
     218             :                 // this should never happen but just in case
     219           0 :                 copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
     220             :             }
     221             :             // log successful completion
     222           3 :             LOGGER_WRITE_EVENT(LogEvent::FLIP_END);
     223             :         }
     224         394 :         break;
     225             : 
     226             :     }
     227           2 :     case FlipState::Abandon:
     228             :         // restore original flight mode
     229           2 :         if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
     230             :             // this should never happen but just in case
     231           0 :             copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
     232             :         }
     233             :         // log abandoning flip
     234           2 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED);
     235           2 :         break;
     236             :     }
     237             : 
     238             :     // output pilot's throttle without angle boost
     239        4313 :     attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt);
     240        4313 : }
     241             : 
     242           4 : void ModeFlip::abandon_flip()
     243             : {
     244           4 :     abandon_requested = true;
     245           4 : }
     246             : 
     247        8652 : bool ModeFlip::input_is_high_magnitude(RC_Channel &input) const
     248             : {
     249        8652 :     return abs(input.get_control_in()) >= 4000;
     250             : }
     251             : 
     252             : #endif