Copter: Autorotation support

ArduCopter/Copter.h

@@ -230,6 +230,10 @@ class Copter : public AP_Vehicle {
 
     Copter(void);
 
+    void set_rangefinder_timeout(uint32_t timeout) { rangefinder_timeout_ms = timeout; }
+
+    void reset_rangefinder_timeout(void) { set_rangefinder_timeout(0); }
+
 private:
 
     // key aircraft parameters passed to multiple libraries
@@ -821,6 +825,8 @@ class Copter : public AP_Vehicle {
     void heli_update_autorotation();
     void update_collective_low_flag(int16_t throttle_control);
 
+    bool arot_rng_finder_set_on;
+
     // inertia.cpp
     void read_inertia();
 

ArduCopter/Parameters.cpp

@@ -1271,7 +1271,7 @@ ParametersG2::ParametersG2(void)
     ,mode_systemid_ptr(&copter.mode_systemid)
 #endif
 #if MODE_AUTOROTATE_ENABLED == ENABLED
-    ,arot()
+    ,arot(copter.inertial_nav, copter.ahrs, copter.motors)
 #endif
 #if HAL_BUTTON_ENABLED
     ,button_ptr(&copter.button)

ArduCopter/config.h

@@ -263,17 +263,13 @@
 //////////////////////////////////////////////////////////////////////////////
 // Autorotate - autonomous auto-rotation - helicopters only
 #ifndef MODE_AUTOROTATE_ENABLED
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    #if FRAME_CONFIG == HELI_FRAME
-        #ifndef MODE_AUTOROTATE_ENABLED
-        # define MODE_AUTOROTATE_ENABLED ENABLED
-        #endif
-    #else
-        # define MODE_AUTOROTATE_ENABLED DISABLED
-    #endif
-#else
-    # define MODE_AUTOROTATE_ENABLED DISABLED
-#endif
+  #if FRAME_CONFIG == HELI_FRAME
+      #ifndef MODE_AUTOROTATE_ENABLED
+      # define MODE_AUTOROTATE_ENABLED ENABLED
+      #endif
+  #else
+      # define MODE_AUTOROTATE_ENABLED DISABLED
+  #endif
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/heli.cpp

@@ -189,15 +189,42 @@ void Copter::heli_update_rotor_speed_targets()
 // to autorotation flight mode if manual collective is not being used.
 void Copter::heli_update_autorotation()
 {
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    // Always update the ground distance to prevent a race on init
+    if (motors->armed() && g2.arot.is_enable()) {
+        // Get height above ground. If using a healthy LiDaR below func will return an interpolated
+        // distance based on inertial measurement. If LiDaR is unhealthy and terrain is available
+        // we will get a terrain database estimate. Otherwise we will get height above home.
+        int32_t gnd_dist = flightmode->get_alt_above_ground_cm();
+
+        // set the height in the autorotation controller
+        g2.arot.set_ground_distance(gnd_dist);
+    }
+
+    // Run the preliminary flare calcs to get prints to the GCS to help users setup the controller.
+    // This function returns early if not configured to do this.
+    g2.arot.run_flare_prelim_calc();
+
+    // See if we need to use autorotation config for preliminary tests
+    if (g2.arot.use_autorotation_config() && !arot_rng_finder_set_on) {
+        gcs().send_text(MAV_SEVERITY_INFO, "arot rngfnd test on");
+        set_rangefinder_timeout(10000);
+        arot_rng_finder_set_on = true;
+    } else if (arot_rng_finder_set_on) {
+        gcs().send_text(MAV_SEVERITY_INFO, "arot rngfnd test off");
+        reset_rangefinder_timeout();
+        arot_rng_finder_set_on = false;
+    }
+#endif
+
     // check if flying and interlock disengaged
-    if (!ap.land_complete && !motors->get_interlock()) {
+    if (!ap.land_complete && !motors->get_interlock() && motors->armed()) {
 #if MODE_AUTOROTATE_ENABLED == 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
+        if (g2.arot.is_enable() && !flightmode->has_manual_throttle()) {
+            // set autonomous autorotation flight mode
+            set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START);
+
+            // set flag to facilitate auto autorotation
             motors->set_in_autorotation(true);
             motors->set_enable_bailout(true);
         }
@@ -211,7 +238,6 @@ void Copter::heli_update_autorotation()
         motors->set_in_autorotation(false);
         motors->set_enable_bailout(false);
     }
-
 }
 
 // update collective low flag.  Use a debounce time of 400 milliseconds.

ArduCopter/mode.cpp

@@ -623,9 +623,6 @@ int32_t Mode::get_alt_above_ground_cm(void)
     if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) {
         return alt_above_ground_cm;
     }
-    if (!pos_control->is_active_xy()) {
-        return copter.current_loc.alt;
-    }
     if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) {
         return alt_above_ground_cm;
     }

ArduCopter/mode.h

@@ -1951,7 +1951,8 @@ class ModeAutorotate : public Mode {
     Number mode_number() const override { return Number::AUTOROTATE; }
 
     bool init(bool ignore_checks) override;
-    void run() override;
+    void run(void) override;
+    void exit(void) override;
 
     bool is_autopilot() const override { return true; }
     bool requires_GPS() const override { return false; }
@@ -1968,49 +1969,31 @@ class ModeAutorotate : public Mode {
 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 _desired_v_z;             // Desired vertical
-    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
+    bool _hover_autorotation;       // bool to determine if we should enter the hover autorotation or not
 
     enum class Autorotation_Phase {
         ENTRY,
+        HOVER_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;
+            bool entry_init             : 1;
+            bool hover_entry_init       : 1;
+            bool ss_glide_init          : 1;
+            bool flare_init             : 1;
+            bool touch_down_init        : 1;
+            bool bail_out_init          : 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