Implement assisted autorotation for Heli’s

ArduCopter/Parameters.cpp

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

ArduCopter/config.h

@@ -279,17 +279,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

@@ -196,6 +196,19 @@ 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(false);
+
+        // set the height in the autorotation controller
+        g2.arot.set_ground_distance(gnd_dist);
+    }
+#endif
+
     // check if flying and interlock disengaged
     if (!ap.land_complete && !motors->get_interlock()) {
 #if MODE_AUTOROTATE_ENABLED == ENABLED
@@ -204,7 +217,7 @@ void Copter::heli_update_autorotation()
                 // set autonomous autorotation flight mode
                 set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START);
             }
-            // set flag to facilitate both auto and manual autorotations
+            // set flag to facilitate auto autorotation
             heli_flags.in_autorotation = true;
             motors->set_in_autorotation(heli_flags.in_autorotation);
             motors->set_enable_bailout(true);

ArduCopter/mode.cpp

@@ -609,13 +609,13 @@ void Mode::make_safe_ground_handling(bool force_throttle_unlimited)
 /*
   get a height above ground estimate for landing
  */
-int32_t Mode::get_alt_above_ground_cm(void)
+int32_t Mode::get_alt_above_ground_cm(bool use_psc_case)
 {
     int32_t alt_above_ground_cm;
     if (copter.get_rangefinder_height_interpolated_cm(alt_above_ground_cm)) {
         return alt_above_ground_cm;
     }
-    if (!pos_control->is_active_xy()) {
+    if (!pos_control->is_active_xy() && use_psc_case) {
         return copter.current_loc.alt;
     }
     if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground_cm)) {

ArduCopter/mode.h

@@ -152,7 +152,7 @@ class Mode {
     virtual bool set_speed_up(float speed_xy_cms) {return false;}
     virtual bool set_speed_down(float speed_xy_cms) {return false;}
 
-    int32_t get_alt_above_ground_cm(void);
+    int32_t get_alt_above_ground_cm(bool use_psc_case=true);
 
     // pilot input processing
     void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const;
@@ -1934,39 +1934,29 @@ class ModeAutorotate : public Mode {
     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
+    uint32_t _touchdown_time_ms;
+    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;
+            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
+    uint32_t _last_bad_rpm_ms;
 
 };
 #endif