Simplify Autorotation Mode Switching and Consolidate Autorotation State in RSC

ArduCopter/Copter.h

@@ -390,7 +390,6 @@ class Copter : public AP_Vehicle {
     // This is the state of the flight control system
     // There are multiple states defined such as STABILIZE, ACRO,
     Mode *flightmode;
-    Mode::Number prev_control_mode;
 
     RCMapper rcmap;
 

ArduCopter/heli.cpp

@@ -189,29 +189,25 @@ void Copter::heli_update_rotor_speed_targets()
 // to autorotation flight mode if manual collective is not being used.
 void Copter::heli_update_autorotation()
 {
-    // check if flying and interlock disengaged
-    if (!ap.land_complete && !motors->get_interlock()) {
+    bool in_autorotation_mode = false;
 #if MODE_AUTOROTATE_ENABLED
-        if (g2.arot.is_enable()) {
-            if (!flightmode->has_manual_throttle()) {
-                // set autonomous autorotation flight mode
-                set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START);
-            }
-            // set flag to facilitate both auto and manual autorotations
-            motors->set_in_autorotation(true);
-            motors->set_enable_bailout(true);
-        }
+    in_autorotation_mode = flightmode == &mode_autorotate;
 #endif
-        if (flightmode->has_manual_throttle() && motors->arot_man_enabled()) {
-            // set flag to facilitate both auto and manual autorotations
-            motors->set_in_autorotation(true);
-            motors->set_enable_bailout(true);
-        }
-    } else {
-        motors->set_in_autorotation(false);
-        motors->set_enable_bailout(false);
+
+    // If we have landed then we do not want to be in autorotation and we do not want to via the bailout state
+    if (ap.land_complete || ap.land_complete_maybe) {
+        motors->force_deactivate_autorotation();
+        return;
     }
 
+    // if we got this far we are flying, check for conditions to set autorotation state
+    if (!motors->get_interlock() && (flightmode->has_manual_throttle() || in_autorotation_mode)) {
+        // set state in motors to facilitate manual and assisted autorotations
+        motors->set_autorotation_active(true);
+    } else {
+        // deactivate the autorotation state via the bailout case
+        motors->set_autorotation_active(false);
+    }
 }
 
 // update collective low flag.  Use a debounce time of 400 milliseconds.

ArduCopter/mode.cpp

@@ -292,20 +292,9 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 #if FRAME_CONFIG == HELI_FRAME
     // do not allow helis to enter a non-manual throttle mode if the
     // rotor runup is not complete
-    if (!ignore_checks && !new_flightmode->has_manual_throttle() &&
-        (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
-        #if MODE_AUTOROTATE_ENABLED
-            //if the mode being exited is the autorotation mode allow mode change despite rotor not being at
-            //full speed.  This will reduce altitude loss on bail-outs back to non-manual throttle modes
-            bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate);
-        #else
-            bool in_autorotation_check = false;
-        #endif
-
-        if (!in_autorotation_check) {
-            mode_change_failed(new_flightmode, "runup not complete");
-            return false;
-        }
+    if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()) {
+        mode_change_failed(new_flightmode, "runup not complete");
+        return false;
     }
 #endif
 
@@ -369,9 +358,6 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
     // perform any cleanup required by previous flight mode
     exit_mode(flightmode, new_flightmode);
 
-    // store previous flight mode (only used by tradeheli's autorotation)
-    prev_control_mode = flightmode->mode_number();
-
     // update flight mode
     flightmode = new_flightmode;
     control_mode_reason = reason;

ArduCopter/mode.h

@@ -1976,18 +1976,14 @@ class ModeAutorotate : public Mode {
     int32_t _pitch_target;          // Target pitch attitude to pass to attitude controller
     uint32_t _entry_time_start_ms;  // Time remaining until entry phase moves on to glide phase
     float _hs_decay;                // The head accerleration during the entry phase
-    float _bail_time;               // Timer for exiting the bail out phase (s)
-    uint32_t _bail_time_start_ms;   // Time at start of bail out
-    float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase
-    float _target_pitch_adjust;     // Target pitch rate used during bail out phase
 
     enum class Autorotation_Phase {
         ENTRY,
         SS_GLIDE,
         FLARE,
         TOUCH_DOWN,
-        BAIL_OUT } phase_switch;
-        
+        LANDED } phase_switch;
+
     enum class Navigation_Decision {
         USER_CONTROL_STABILISED,
         STRAIGHT_AHEAD,
@@ -2000,10 +1996,10 @@ class ModeAutorotate : public Mode {
             bool ss_glide_initial          : 1;
             bool flare_initial             : 1;
             bool touch_down_initial        : 1;
+            bool landed_initial            : 1;
             bool straight_ahead_initial    : 1;
             bool level_initial             : 1;
             bool break_initial             : 1;
-            bool bail_out_initial          : 1;
             bool bad_rpm                   : 1;
     } _flags;
 

ArduCopter/mode_autorotate.cpp

@@ -14,8 +14,8 @@
 #if MODE_AUTOROTATE_ENABLED
 
 #define AUTOROTATE_ENTRY_TIME          2.0f    // (s) number of seconds that the entry phase operates for
-#define BAILOUT_MOTOR_RAMP_TIME        1.0f    // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single
 #define HEAD_SPEED_TARGET_RATIO        1.0f    // Normalised target main rotor head speed (unit: -)
+#define AUTOROTATION_MIN_MOVING_SPEED  100.0    // (cm/s) minimum speed used for "is moving" check
 
 bool ModeAutorotate::init(bool ignore_checks)
 {
@@ -24,15 +24,16 @@ bool ModeAutorotate::init(bool ignore_checks)
     return false;
 #endif
 
-    // Check that mode is enabled
+    // Check that mode is enabled, make sure this is the first check as this is the most
+    // important thing for users to fix if they are planning to use autorotation mode
     if (!g2.arot.is_enable()) {
-        gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Not Enabled");
+        gcs().send_text(MAV_SEVERITY_WARNING, "Autorot Mode Not Enabled");
         return false;
     }
 
-    // Check that interlock is disengaged
-    if (motors->get_interlock()) {
-        gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Change Fail: Interlock Engaged");
+    // Must be armed to use mode, prevent triggering state machine on the ground
+    if (!motors->armed() || copter.ap.land_complete || copter.ap.land_complete_maybe) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Autorot: Must be Armed and Flying");
         return false;
     }
 
@@ -52,10 +53,10 @@ bool ModeAutorotate::init(bool ignore_checks)
     _flags.ss_glide_initial = true;
     _flags.flare_initial = true;
     _flags.touch_down_initial = true;
+    _flags.landed_initial = true;
     _flags.level_initial = true;
     _flags.break_initial = true;
     _flags.straight_ahead_initial = true;
-    _flags.bail_out_initial = true;
     _msg_flags.bad_rpm = true;
 
     // Setting default starting switches
@@ -74,20 +75,9 @@ bool ModeAutorotate::init(bool ignore_checks)
 
 void ModeAutorotate::run()
 {
-    // Check if interlock becomes engaged
-    if (motors->get_interlock() && !copter.ap.land_complete) {
-        phase_switch = Autorotation_Phase::BAIL_OUT;
-    } else if (motors->get_interlock() && copter.ap.land_complete) {
-        // Aircraft is landed and no need to bail out
-        set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT);
-    }
-
     // Current time
     uint32_t now = millis(); //milliseconds
 
-    // Initialise internal variables
-    float curr_vel_z = inertial_nav.get_velocity_z_up_cms();   // Current vertical descent
-
     //----------------------------------------------------------------
     //                  State machine logic
     //----------------------------------------------------------------
@@ -97,12 +87,22 @@ void ModeAutorotate::run()
 
     // Timer from entry phase to progress to glide phase
     if (phase_switch == Autorotation_Phase::ENTRY){
-
         if ((now - _entry_time_start_ms)/1000.0f > AUTOROTATE_ENTRY_TIME) {
             // Flight phase can be progressed to steady state glide
             phase_switch = Autorotation_Phase::SS_GLIDE;
         }
+    }
+
+    // Check if we believe we have landed. We need the landed state to zero all controls and make sure that the copter landing detector will trip
+    bool speed_check = inertial_nav.get_velocity_z_up_cms() < AUTOROTATION_MIN_MOVING_SPEED &&
+                     inertial_nav.get_speed_xy_cms() < AUTOROTATION_MIN_MOVING_SPEED;
+    if (motors->get_below_land_min_coll() && AP::ins().is_still() && speed_check) {
+        phase_switch = Autorotation_Phase::LANDED;
+    }
 
+    // Check if we are bailing out and need to re-set the spool state
+    if (motors->autorotation_bailout()) {
+        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     }
 
 
@@ -199,79 +199,22 @@ void ModeAutorotate::run()
         {
             break;
         }
-
-        case Autorotation_Phase::BAIL_OUT:
+        case Autorotation_Phase::LANDED:
         {
-        if (_flags.bail_out_initial == true) {
-                // Functions and settings to be done once are done here.
+            // Entry phase functions to be run only once
+            if (_flags.landed_initial == true) {
 
                 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-                    gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation");
+                    gcs().send_text(MAV_SEVERITY_INFO, "Landed");
                 #endif
-
-                // Set bail out timer remaining equal to the parameter value, bailout time 
-                // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME.
-                _bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f);
-
-                // Set bail out start time
-                _bail_time_start_ms = now;
-
-                // Set initial target vertical speed
-                _desired_v_z = curr_vel_z;
-
-                // Initialise position and desired velocity
-                if (!pos_control->is_active_z()) {
-                    pos_control->relax_z_controller(g2.arot.get_last_collective());
-                }
-
-                // Get pilot parameter limits
-                const float pilot_spd_dn = -get_pilot_speed_dn();
-                const float pilot_spd_up = g.pilot_speed_up;
-
-                float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
-                pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up);
-
-                // Calculate target climb rate adjustment to transition from bail out descent speed to requested climb rate on stick.
-                _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time
-
-                // Calculate pitch target adjustment rate to return to level
-                _target_pitch_adjust = _pitch_target/_bail_time;
-
-                // set vertical speed and acceleration limits
-                pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust));
-                pos_control->set_correction_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust));
-
-                motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-
-                _flags.bail_out_initial = false;
+                _flags.landed_initial = false;
             }
-
-        if ((now - _bail_time_start_ms)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) {
-            // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed
-            _desired_v_z -= _target_climb_rate_adjust*G_Dt;
-            _pitch_target -= _target_pitch_adjust*G_Dt;
-        }
-        // Set position controller
-        pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z);
-
-        // Update controllers
-        pos_control->update_z_controller();
-
-        if ((now - _bail_time_start_ms)/1000.0f >= _bail_time) {
-            // Bail out timer complete.  Change flight mode. Do not revert back to auto. Prevent aircraft
-            // from continuing mission and potentially flying further away after a power failure.
-            if (copter.prev_control_mode == Mode::Number::AUTO) {
-                set_mode(Mode::Number::ALT_HOLD, ModeReason::AUTOROTATION_BAILOUT);
-            } else {
-                set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT);
-            }
-        }
-
-        break;
+            // don't allow controller to continually ask for more pitch to build speed when we are on the ground, decay to zero smoothly
+            _pitch_target *= 0.95;
+            break;
         }
     }
 
-
     switch (nav_pos_switch) {
 
         case Navigation_Decision::USER_CONTROL_STABILISED:

ArduSub/mode.cpp

@@ -112,7 +112,7 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason)
     // perform any cleanup required by previous flight mode
     exit_mode(flightmode, new_flightmode);
 
-    // store previous flight mode (only used by tradeheli's autorotation)
+    // store previous flight mode
     prev_control_mode = control_mode;
 
     // update flight mode