temp save

ArduCopter/Copter.h

@@ -234,6 +234,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
@@ -273,6 +277,8 @@ class Copter : public AP_Vehicle {
         float terrain_offset_cm;    // filtered terrain offset (e.g. terrain's height above EKF origin)
     } rangefinder_state, rangefinder_up_state;
 
+    uint32_t rangefinder_timeout_ms;
+
     // return rangefinder height interpolated using inertial altitude
     bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;
 
@@ -820,6 +826,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/heli.cpp

@@ -211,6 +211,17 @@ void Copter::heli_update_autorotation()
     // 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

ArduCopter/mode.h

@@ -1907,7 +1907,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; }

ArduCopter/mode_autorotate.cpp

@@ -45,14 +45,17 @@ bool ModeAutorotate::init(bool ignore_checks)
     // Display message
     gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated");
 
-    // Set all intial flags to on
+    // Set all intial flags
     _flags.entry_init = false;
     _flags.hover_entry_init = false;
     _flags.ss_glide_init = false;
     _flags.flare_init = false;
     _flags.touch_down_init = false;
     _flags.bail_out_init = false;
 
+    // Extend rangefinder time out for use in the autorotation
+    set_rangefinder_timeout(10000)
+
     // Check if we have sufficient speed or height to do a full autorotation otherwise we have to do one from the hover
     // Note: This must be called after arot.init()
     _hover_autorotation = (inertial_nav.get_speed_xy_cms() < 250.0f) && !g2.arot.above_flare_height();
@@ -364,4 +367,9 @@ void ModeAutorotate::run()
 
 } // End function run()
 
+
+void ModeAutorotate::exit(void) {
+    reset_rangefinder_timeout()
+}
+
 #endif

ArduCopter/sensors.cpp

@@ -52,8 +52,10 @@ void Copter::read_rangefinder(void)
         // tilt corrected but unfiltered, not glitch protected alt
         rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient);
 
-        // remember inertial alt to allow us to interpolate rangefinder
-        rf_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm();
+        // remember inertial alt to allow us to interpolate rangefinder from last healthy measurement
+        if (rf_state.alt_healthy) {
+            rf_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm();
+        }
 
         // glitch handling.  rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
         // are considered a glitch and glitch_count becomes non-zero
@@ -161,12 +163,17 @@ void Copter::update_rangefinder_terrain_offset()
   recorded whenever we update the rangefinder height, then we use the
   difference between the inertial height at that time and the current
   inertial height to give us interpolation of height from rangefinder
+  time_out arg: this gives us the chance to use the inertially interpolated
+  range finder height for a short time after it goes out of range. This
+  is needed to prevent jumps in hagl when the rangefinder comes close to
+  the ground in an autorotation landing
  */
 bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) const
 {
-    if (!rangefinder_alt_ok()) {
+    if (!rangefinder_alt_ok() && ((rangefinder_timeout_ms == 0) || (AP_HAL::millis() - rangefinder_state.last_healthy_ms > rangefinder_timeout_ms))) {
         return false;
     }
+
     ret = rangefinder_state.alt_cm_filt.get();
     float inertial_alt_cm = inertial_nav.get_position_z_up_cm();
     ret += inertial_alt_cm - rangefinder_state.inertial_alt_cm;

libraries/AC_Autorotation/AC_Autorotation.cpp

@@ -159,7 +159,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Autorotation options
     // @Description: Bitmask for autorotation options.
-    // @Bitmask: 0: Use stabilize-like controls (roll angle, yaw rate), 1: Print inital flare calc
+    // @Bitmask: 0: Use stabilize-like controls (roll angle, yaw rate), 1: Print inital flare calc, 2: Test rangefinder timeout
     AP_GROUPINFO("OPTIONS", 17, AC_Autorotation, _options, 0),
 
     // @Param: CL_ALPHA

libraries/AC_Autorotation/AC_Autorotation.h

@@ -97,7 +97,9 @@ class AC_Autorotation
     // Estimate the time to impact and compare it with the touchdown time param
     bool should_begin_touchdown(void);
 
-    bool use_stabilise_controls(void) const { return _options.get() & int32_t(OPTION::STABILISE_CONTROLS); }
+    bool use_stabilise_controls(void) const { return int32_t(_options.get()) & int32_t(OPTION::STABILISE_CONTROLS); }
+
+    bool use_autorotation_config(void) const { return int32_t(_options.get()) & int32_t(OPTION::TEST_RANGEFINDER_TIMEOUT); }
 
     void run_flare_prelim_calc(void);
 
@@ -175,6 +177,7 @@ class AC_Autorotation
     enum class OPTION {
         STABILISE_CONTROLS = (1<<0),
         PRINT_FLARE_ESTIMATES = (1<<1),
+        TEST_RANGEFINDER_TIMEOUT = (1<<2),
     };
 
     //--------Internal Functions--------