Copter: Added autorotation flight mode and support

ArduCopter/Copter.cpp

@@ -250,6 +250,9 @@ void Copter::fast_loop()
 
 #if FRAME_CONFIG == HELI_FRAME
     update_heli_control_dynamics();
+    #if MODE_AUTOROTATE_ENABLED == ENABLED
+        heli_update_autorotation();
+    #endif
 #endif //HELI_FRAME
 
     // Inertial Nav
@@ -403,6 +406,13 @@ void Copter::twentyfive_hz_logging()
     // log output
     Log_Write_Precland();
 #endif
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
+        //update autorotation log
+        g2.arot.Log_Write_Autorotation();
+    }
+#endif
 }
 
 // three_hz_loop - 3.3hz loop

ArduCopter/Copter.h

@@ -84,6 +84,10 @@
  #define MOTOR_CLASS AP_MotorsMulticopter
 #endif
 
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+ #include <AC_Autorotation/AC_Autorotation.h> // Autorotation controllers
+#endif
+
 #include "RC_Channel.h"         // RC Channel Library
 
 #include "GCS_Mavlink.h"
@@ -95,6 +99,7 @@
 #if BEACON_ENABLED == ENABLED
  #include <AP_Beacon/AP_Beacon.h>
 #endif
+
 #if AC_AVOID_ENABLED == ENABLED
  #include <AC_Avoidance/AC_Avoid.h>
 #endif
@@ -230,6 +235,7 @@ public:
     friend class ModeSystemId;
     friend class ModeThrow;
     friend class ModeZigZag;
+    friend class ModeAutorotate;
 
     Copter(void);
 
@@ -477,6 +483,7 @@ private:
     AC_PosControl *pos_control;
     AC_WPNav *wp_nav;
     AC_Loiter *loiter_nav;
+
 #if MODE_CIRCLE_ENABLED == ENABLED
     AC_Circle *circle_nav;
 #endif
@@ -575,6 +582,7 @@ private:
     typedef struct {
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
         uint8_t inverted_flight         : 1;    // 1   // true for inverted flight mode
+        uint8_t in_autorotation         : 1;    // 2   // true when heli is in autorotation
     } heli_flags_t;
     heli_flags_t heli_flags;
 
@@ -751,7 +759,10 @@ private:
     void update_heli_control_dynamics(void);
     void heli_update_landing_swash();
     void heli_update_rotor_speed_targets();
-
+    void heli_update_autorotation();
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    void heli_set_autorotation(bool autotrotation);
+#endif
     // inertia.cpp
     void read_inertia();
 
@@ -980,6 +991,9 @@ private:
 #if MODE_ZIGZAG_ENABLED == ENABLED
     ModeZigZag mode_zigzag;
 #endif
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    ModeAutorotate mode_autorotate;
+#endif
 
     // mode.cpp
     Mode *mode_from_mode_num(const Mode::Number mode);

ArduCopter/Parameters.cpp

@@ -262,42 +262,42 @@ const AP_Param::Info Copter::var_info[] = {
     // @Param: FLTMODE1
     // @DisplayName: Flight Mode 1
     // @Description: Flight mode when Channel 5 pwm is <= 1230
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
     // @User: Standard
     GSCALAR(flight_mode1, "FLTMODE1",               (uint8_t)FLIGHT_MODE_1),
 
     // @Param: FLTMODE2
     // @DisplayName: Flight Mode 2
     // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
     // @User: Standard
     GSCALAR(flight_mode2, "FLTMODE2",               (uint8_t)FLIGHT_MODE_2),
 
     // @Param: FLTMODE3
     // @DisplayName: Flight Mode 3
     // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
     // @User: Standard
     GSCALAR(flight_mode3, "FLTMODE3",               (uint8_t)FLIGHT_MODE_3),
 
     // @Param: FLTMODE4
     // @DisplayName: Flight Mode 4
     // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
     // @User: Standard
     GSCALAR(flight_mode4, "FLTMODE4",               (uint8_t)FLIGHT_MODE_4),
 
     // @Param: FLTMODE5
     // @DisplayName: Flight Mode 5
     // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
     // @User: Standard
     GSCALAR(flight_mode5, "FLTMODE5",               (uint8_t)FLIGHT_MODE_5),
 
     // @Param: FLTMODE6
     // @DisplayName: Flight Mode 6
     // @Description: Flight mode when Channel 5 pwm is >=1750
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
     // @User: Standard
     GSCALAR(flight_mode6, "FLTMODE6",               (uint8_t)FLIGHT_MODE_6),
 
@@ -712,7 +712,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Path: ../libraries/AP_OSD/AP_OSD.cpp
     GOBJECT(osd, "OSD", AP_OSD),
 #endif
-    
+
     // @Group:
     // @Path: Parameters.cpp
     GOBJECT(g2, "",  ParametersG2),
@@ -955,6 +955,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0),
 
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    // @Group: AROT_
+    // @Path: ../libraries/AC_Autorotation/AC_Autorotation.cpp
+    AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation),
+#endif
+
+
+
     AP_GROUPEND
 };
 
@@ -1041,6 +1049,9 @@ ParametersG2::ParametersG2(void)
 #if MODE_SYSTEMID_ENABLED == ENABLED
     ,mode_systemid_ptr(&copter.mode_systemid)
 #endif
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    ,arot(copter.inertial_nav)
+#endif
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

ArduCopter/Parameters.h

@@ -615,6 +615,11 @@ public:
 
     // Failsafe options bitmask #36
     AP_Int32 fs_options;
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    // Autonmous autorotation
+    AC_Autorotation arot;
+#endif
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/config.h

@@ -372,6 +372,22 @@
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////
+// Autorotate - autonomous auto-rotation - helicopters only
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    #if FRAME_CONFIG == HELI_FRAME
+        #ifndef MODE_AUTOROTATE_ENABLED
+        # define MODE_AUTOROTATE_ENABLED !HAL_MINIMIZE_FEATURES
+        #endif
+    #else
+        # define MODE_AUTOROTATE_ENABLED DISABLED
+    #endif
+#else
+    # define MODE_AUTOROTATE_ENABLED DISABLED
+#endif
+//////////////////////////////////////////////////////////////////////////////
+
 // Beacon support - support for local positioning systems
 #ifndef BEACON_ENABLED
 # define BEACON_ENABLED !HAL_MINIMIZE_FEATURES

ArduCopter/crash_check.cpp

@@ -35,6 +35,14 @@ void Copter::crash_check()
         return;
     }
 
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    //return immediately if in autorotation mode
+    if (control_mode == Mode::Number::AUTOROTATE) {
+        crash_counter = 0;
+        return;
+    }
+#endif
+
     // vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
     if (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
         crash_counter = 0;

ArduCopter/heli.cpp

@@ -180,4 +180,38 @@ void Copter::heli_update_rotor_speed_targets()
     rotor_runup_complete_last = motors->rotor_runup_complete();
 }
 
+
+// heli_update_autorotation - determines if aircraft is in autorotation and sets motors flag and switches
+// to autorotation flight mode if manual collective is not being used.
+void Copter::heli_update_autorotation()
+{
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    //set autonomous autorotation flight mode
+    if (!ap.land_complete && !motors->get_interlock() && !flightmode->has_manual_throttle() && g2.arot.is_enable()) {
+        heli_flags.in_autorotation = true;
+        set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START);
+    } else {
+        heli_flags.in_autorotation = false;
+    }
+
+    // sets autorotation flags through out libraries
+    heli_set_autorotation(heli_flags.in_autorotation);
+    if (!ap.land_complete && g2.arot.is_enable()) {
+        motors->set_enable_bailout(true);
+    } else {
+        motors->set_enable_bailout(false);
+    }
+#else
+    heli_flags.in_autorotation = false;
+    motors->set_enable_bailout(false);
+#endif
+}
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+// heli_set_autorotation - set the autorotation f`lag throughout libraries
+void Copter::heli_set_autorotation(bool autorotation)
+{
+    motors->set_in_autorotation(autorotation);
+}
+#endif
 #endif  // FRAME_CONFIG == HELI_FRAME

ArduCopter/mode.cpp

@@ -165,6 +165,12 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
             break;
 #endif
 
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+        case Mode::Number::AUTOROTATE:
+            ret = &mode_autorotate;
+            break;
+#endif
+
         default:
             break;
     }
@@ -198,11 +204,29 @@ 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 (!ignore_checks && 
-        gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
+    !new_flightmode->has_manual_throttle() && 
-        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
+    (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
-        return false;
+        #if MODE_AUTOROTATE_ENABLED == 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) {
+            gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
+            AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
+            return false;
+        }
     }
+
+    #if MODE_AUTOROTATE_ENABLED == ENABLED
+        // If changing to autorotate flight mode from a non-manual throttle mode, store the previous flight mode
+        // to exit back to it when interlock is re-engaged
+        prev_control_mode = control_mode;
+    #endif
 #endif
 
 #if FRAME_CONFIG != HELI_FRAME

ArduCopter/mode.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include "Copter.h"
-
 class Parameters;
 class ParametersG2;
 
@@ -36,6 +35,7 @@ public:
         FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
         ZIGZAG    =    24,  // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
         SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
+        AUTOROTATE =   26,  // Autonomous autorotation
     };
 
     // constructor
@@ -1348,7 +1348,7 @@ class ModeZigZag : public Mode {
 
 public:
 
-    // inherit constructor
+    // Inherit constructor
     using Mode::Mode;
 
     bool init(bool ignore_checks) override;
@@ -1388,3 +1388,83 @@ private:
 
     uint32_t reach_wp_time_ms = 0;  // time since vehicle reached destination (or zero if not yet reached)
 };
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+class ModeAutorotate : public Mode {
+
+public:
+
+    // inherit constructor
+    using Mode::Mode;
+
+    bool init(bool ignore_checks) override;
+    void run() override;
+
+    bool is_autopilot() const override { return true; }
+    bool requires_GPS() const override { return false; }
+    bool has_manual_throttle() const override { return false; }
+    bool allows_arming(bool from_gcs) const override { return false; };
+
+    static const struct AP_Param::GroupInfo  var_info[];
+
+protected:
+
+    const char *name() const override { return "AUTOROTATE"; }
+    const char *name4() const override { return "AROT"; }
+
+private:
+
+    // --- Internal variables ---
+    float _initial_rpm;             // Head speed recorded at initiation of flight mode (RPM)
+    float _target_head_speed;       // The terget head main rotor head speed.  Normalised by main rotor set point
+    float _fwd_speed_target;        // Target forward speed (cm/s)
+    float _desired_v_z;             // Desired vertical
+    int32_t _pitch_target;          // Target pitch attitude to pass to attitude controller
+    float _collective_aggression;   // The 'aggresiveness' of collective appliction
+    float _z_touch_down_start;      // The height in cm that the touch down phase began
+    float _t_touch_down_initiate;   // The time in ms that the touch down phase began
+    float now;                      // Current time in millis
+    float _entry_time_start;        // 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)
+    float _bail_time_start;         // Time at start of bail out
+    float _des_z;                   // Desired vertical position
+    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
+    uint16_t log_counter;           // Used to reduce the data flash logging rate
+
+    enum class Autorotation_Phase {
+        ENTRY,
+        SS_GLIDE,
+        FLARE,
+        TOUCH_DOWN,
+        BAIL_OUT } phase_switch;
+        
+    enum class Navigation_Decision {
+        USER_CONTROL_STABILISED,
+        STRAIGHT_AHEAD,
+        INTO_WIND,
+        NEAREST_RALLY} nav_pos_switch;
+
+    // --- Internal flags ---
+    struct controller_flags {
+            bool entry_initial             : 1;
+            bool ss_glide_initial          : 1;
+            bool flare_initial             : 1;
+            bool touch_down_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;
+
+    struct message_flags {
+            bool bad_rpm                   : 1;
+    } _msg_flags;
+
+    //--- Internal functions ---
+    void warning_message(uint8_t message_n);    //Handles output messages to the terminal
+
+};
+#endif

ArduCopter/mode_autorotate.cpp

@@ -0,0 +1,325 @@
+/* -----------------------------------------------------------------------------------------
+    This is currently a SITL only function until the project is complete.
+    To trial this in SITL you will need to use Real Flight 8.
+    Instructions for how to set this up in SITL can be found here:
+    https://discuss.ardupilot.org/t/autonomous-autorotation-gsoc-project-blog/42139
+ -----------------------------------------------------------------------------------------*/
+
+#include "Copter.h"
+#include <AC_Autorotation/AC_Autorotation.h>
+#include "mode.h"
+
+#include <utility>
+
+#if MODE_AUTOROTATE_ENABLED == 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: -)
+
+bool ModeAutorotate::init(bool ignore_checks)
+{
+#if FRAME_CONFIG != HELI_FRAME
+    // Only allow trad heli to use autorotation mode
+    return false;
+#endif
+
+    // Check that mode is enabled
+    if (!g2.arot.is_enable()) {
+        gcs().send_text(MAV_SEVERITY_INFO, "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");
+        return false;
+    }
+
+    // Initialise controllers
+    // This must be done before RPM value is fetched
+    g2.arot.init_hs_controller();
+    g2.arot.init_fwd_spd_controller();
+
+    // Retrive rpm and start rpm sensor health checks
+    _initial_rpm = g2.arot.get_rpm(true);
+
+    // Display message 
+    gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated");
+
+     // Set all inial flags to on
+    _flags.entry_initial = 1;
+    _flags.ss_glide_initial = 1;
+    _flags.flare_initial = 1;
+    _flags.touch_down_initial = 1;
+    _flags.level_initial = 1;
+    _flags.break_initial = 1;
+    _flags.straight_ahead_initial = 1;
+    _flags.bail_out_initial = 1;
+    _msg_flags.bad_rpm = true;
+
+    // Setting default starting switches
+    phase_switch = Autorotation_Phase::ENTRY;
+
+    // Set entry timer
+    _entry_time_start = millis();
+
+    // The decay rate to reduce the head speed from the current to the target
+    _hs_decay = ((_initial_rpm/g2.arot.get_hs_set_point()) - HEAD_SPEED_TARGET_RATIO) / AUTOROTATE_ENTRY_TIME;
+
+    return true;
+}
+
+
+
+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
+    now = millis(); //milliseconds
+
+    // Initialise internal variables
+    float curr_vel_z = inertial_nav.get_velocity().z;   // Current vertical descent
+
+    //----------------------------------------------------------------
+    //                  State machine logic
+    //----------------------------------------------------------------
+
+     // Setting default phase switch positions
+     nav_pos_switch = Navigation_Decision::USER_CONTROL_STABILISED;
+
+    // Timer from entry phase to progress to glide phase
+    if (phase_switch == Autorotation_Phase::ENTRY){
+
+        if ((now - _entry_time_start)/1000.0f > AUTOROTATE_ENTRY_TIME) {
+            // Flight phase can be progressed to steady state glide
+            phase_switch = Autorotation_Phase::SS_GLIDE;
+        }
+
+    }
+
+
+    //----------------------------------------------------------------
+    //                  State machine actions
+    //----------------------------------------------------------------
+    switch (phase_switch) {
+
+        case Autorotation_Phase::ENTRY:
+        {
+            // Entry phase functions to be run only once
+            if (_flags.entry_initial == 1) {
+
+                #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+                    gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase");
+                #endif
+
+                // Set following trim low pass cut off frequency
+                g2.arot.set_col_cutoff_freq(g2.arot.get_col_entry_freq());
+
+                // Target head speed is set to rpm at initiation to prevent unwanted changes in attitude
+                _target_head_speed = _initial_rpm/g2.arot.get_hs_set_point();
+
+                // Set desired forward speed target
+                g2.arot.set_desired_fwd_speed();
+
+                // Prevent running the initial entry functions again
+                _flags.entry_initial = 0;
+
+            }
+
+            // Slowly change the target head speed until the target head speed matches the parameter defined value
+            if (g2.arot.get_rpm() > HEAD_SPEED_TARGET_RATIO*1.005f  ||  g2.arot.get_rpm() < HEAD_SPEED_TARGET_RATIO*0.995f) {
+                _target_head_speed -= _hs_decay*G_Dt;
+            } else {
+                _target_head_speed = HEAD_SPEED_TARGET_RATIO;
+            }
+
+            // Set target head speed in head speed controller
+            g2.arot.set_target_head_speed(_target_head_speed);
+
+            // Run airspeed/attitude controller
+            g2.arot.set_dt(G_Dt);
+            g2.arot.update_forward_speed_controller();
+
+            // Retrieve pitch target
+            _pitch_target = g2.arot.get_pitch();
+
+            // Update controllers
+            _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); //run head speed/ collective controller
+
+            break;
+        }
+
+        case Autorotation_Phase::SS_GLIDE:
+        {
+            // Steady state glide functions to be run only once
+            if (_flags.ss_glide_initial == 1) {
+
+                #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+                    gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase");
+                #endif
+
+                // Set following trim low pass cut off frequency
+                g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq());
+
+                // Set desired forward speed target
+                g2.arot.set_desired_fwd_speed();
+
+                // Set target head speed in head speed controller
+                _target_head_speed = HEAD_SPEED_TARGET_RATIO;  //Ensure target hs is set to glide incase hs hasent reached target for glide
+                g2.arot.set_target_head_speed(_target_head_speed);
+
+                // Prevent running the initial glide functions again
+                _flags.ss_glide_initial = 0;
+            }
+
+            // Run airspeed/attitude controller
+            g2.arot.set_dt(G_Dt);
+            g2.arot.update_forward_speed_controller();
+
+            // Retrieve pitch target 
+            _pitch_target = g2.arot.get_pitch();
+
+            // Update head speed/ collective controller
+            _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); 
+            // Attitude controller is updated in navigation switch-case statements
+
+            break;
+        }
+
+        case Autorotation_Phase::FLARE:
+        case Autorotation_Phase::TOUCH_DOWN:
+        {
+            break;
+        }
+
+        case Autorotation_Phase::BAIL_OUT:
+        {
+        if (_flags.bail_out_initial == 1) {
+                // Functions and settings to be done once are done here.
+
+                #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+                    gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation");
+                #endif
+
+                // Set bail out timer remaining equal to the paramter 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 = 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_alt_hold_controllers(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;
+
+                // Set speed limit
+                pos_control->set_max_speed_z(curr_vel_z, pilot_spd_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 decent 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 acceleration limit
+                pos_control->set_max_accel_z(abs(_target_climb_rate_adjust));
+
+                motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+                _flags.bail_out_initial = 0;
+            }
+
+        if ((now - _bail_time_start)/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_alt_target_from_climb_rate(_desired_v_z, G_Dt, false);
+
+        // Update controllers
+        pos_control->update_z_controller();
+
+        if ((now - _bail_time_start)/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;
+        }
+    }
+
+
+    switch (nav_pos_switch) {
+
+        case Navigation_Decision::USER_CONTROL_STABILISED:
+        {
+            // Operator is in control of roll and yaw.  Controls act as if in stabilise flight mode.  Pitch 
+            // is controlled by speed-height controller.
+            float pilot_roll, pilot_pitch;
+            get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
+
+            // Get pilot's desired yaw rate
+            float pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+
+            // Pitch target is calculated in autorotation phase switch above
+            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pilot_roll, _pitch_target, pilot_yaw_rate);
+            break;
+        }
+
+        case Navigation_Decision::STRAIGHT_AHEAD:
+        case Navigation_Decision::INTO_WIND:
+        case Navigation_Decision::NEAREST_RALLY:
+        {
+            break;
+        }
+    }
+
+    // Output warning messaged if rpm signal is bad
+    if (_flags.bad_rpm) {
+        warning_message(1);
+    }
+
+} // End function run()
+
+void ModeAutorotate::warning_message(uint8_t message_n)
+{
+    switch (message_n) {
+        case 1:
+        {
+            if (_msg_flags.bad_rpm) {
+                // Bad rpm sensor health.
+                gcs().send_text(MAV_SEVERITY_INFO, "Warning: Poor RPM Sensor Health");
+                gcs().send_text(MAV_SEVERITY_INFO, "Action: Minimum Collective Applied");
+                _msg_flags.bad_rpm = false;
+            }
+            break;
+        }
+    }
+}
+
+#endif

ArduCopter/wscript

@@ -11,6 +11,7 @@ def build(bld):
             'AC_InputManager',
             'AC_PrecLand',
             'AC_Sprayer',
+            'AC_Autorotation',
             'AC_WPNav',
             'AP_Camera',
             'AP_IRLock',