Copter: use new surface distance libary

Copter: remove old rangefinder updates brought forward in rebase

Copter: Fix rangefinder time out check

Copter: Move ineritally interpolated height measurement into lib

Copter: Move enabled and healthy checks down into lib

ArduCopter/Copter.h

@@ -71,6 +71,7 @@
 #include <AC_PrecLand/AC_PrecLand_config.h>
 #include <AP_OpticalFlow/AP_OpticalFlow.h>
 #include <AP_Winch/AP_Winch_config.h>
+#include <AC_SurfaceDistance/AC_SurfaceDistance.h>
 
 // Configuration
 #include "defines.h"
@@ -251,20 +252,10 @@ class Copter : public AP_Vehicle {
     AP_Int8 *flight_modes;
     const uint8_t num_flight_modes = 6;
 
-    struct RangeFinderState {
-        bool enabled:1;
-        bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
-        int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
-        float inertial_alt_cm; // inertial alt at time of last rangefinder sample
-        uint32_t last_healthy_ms;
-        LowPassFilterFloat alt_cm_filt; // altitude filter
-        int16_t alt_cm_glitch_protected;    // last glitch protected altitude
-        int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
-        uint32_t glitch_cleared_ms; // system time glitch cleared
-        float terrain_offset_cm;    // filtered terrain offset (e.g. terrain's height above EKF origin)
-    } rangefinder_state, rangefinder_up_state;
-
-    // return rangefinder height interpolated using inertial altitude
+    AC_SurfaceDistance rangefinder_state {ROTATION_PITCH_270, inertial_nav};
+    AC_SurfaceDistance rangefinder_up_state {ROTATION_PITCH_90, inertial_nav};
+
+    // helper function to get inertially interpolated rangefinder height.
     bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;
 
     class SurfaceTracking {

ArduCopter/config.h

@@ -74,14 +74,6 @@
  # define RANGEFINDER_ENABLED ENABLED
 #endif
 
-#ifndef RANGEFINDER_HEALTH_MAX
- # define RANGEFINDER_HEALTH_MAX 3          // number of good reads that indicates a healthy rangefinder
-#endif
-
-#ifndef RANGEFINDER_TIMEOUT_MS
-# define RANGEFINDER_TIMEOUT_MS 1000        // rangefinder filter reset if no updates from sensor in 1 second
-#endif
-
 #ifndef RANGEFINDER_FILT_DEFAULT
  # define RANGEFINDER_FILT_DEFAULT 0.5f     // filter for rangefinder distance
 #endif
@@ -90,18 +82,6 @@
  # define SURFACE_TRACKING_TIMEOUT_MS  1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
 #endif
 
-#ifndef RANGEFINDER_TILT_CORRECTION         // by disable tilt correction for use of range finder data by EKF
- # define RANGEFINDER_TILT_CORRECTION ENABLED
-#endif
-
-#ifndef RANGEFINDER_GLITCH_ALT_CM
- # define RANGEFINDER_GLITCH_ALT_CM  200      // amount of rangefinder change to be considered a glitch
-#endif
-
-#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES
- # define RANGEFINDER_GLITCH_NUM_SAMPLES  3   // number of rangefinder glitches in a row to take new reading
-#endif
-
 #ifndef MAV_SYSTEM_ID
  # define MAV_SYSTEM_ID          1
 #endif

ArduCopter/sensors.cpp

@@ -28,114 +28,26 @@ void Copter::read_rangefinder(void)
 #if RANGEFINDER_ENABLED == ENABLED
     rangefinder.update();
 
-#if RANGEFINDER_TILT_CORRECTION == ENABLED
-    const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
-#else
-    const float tilt_correction = 1.0f;
-#endif
-
-    // iterate through downward and upward facing lidar
-    struct {
-        RangeFinderState &state;
-        enum Rotation orientation;
-    } rngfnd[2] = {{rangefinder_state, ROTATION_PITCH_270}, {rangefinder_up_state, ROTATION_PITCH_90}};
-
-    for (uint8_t i=0; i < ARRAY_SIZE(rngfnd); i++) {
-        // local variables to make accessing simpler
-        RangeFinderState &rf_state = rngfnd[i].state;
-        enum Rotation rf_orient = rngfnd[i].orientation;
-
-        // update health
-        rf_state.alt_healthy = ((rangefinder.status_orient(rf_orient) == RangeFinder::Status::Good) &&
-                                (rangefinder.range_valid_count_orient(rf_orient) >= RANGEFINDER_HEALTH_MAX));
-
-        // 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();
-
-        // 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
-        // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
-        // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
-        const int32_t glitch_cm = rf_state.alt_cm - rf_state.alt_cm_glitch_protected;
-        bool reset_terrain_offset = false;
-        if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
-            rf_state.glitch_count = MAX(rf_state.glitch_count+1, 1);
-        } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
-            rf_state.glitch_count = MIN(rf_state.glitch_count-1, -1);
-        } else {
-            rf_state.glitch_count = 0;
-            rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
-        }
-        if (abs(rf_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
-            // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
-            rf_state.glitch_count = 0;
-            rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
-            rf_state.glitch_cleared_ms = AP_HAL::millis();
-            reset_terrain_offset = true;
-        }
+    rangefinder_state.update();
+    rangefinder_up_state.update();
 
-        // filter rangefinder altitude
-        uint32_t now = AP_HAL::millis();
-        const bool timed_out = now - rf_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
-        if (rf_state.alt_healthy) {
-            if (timed_out) {
-                // reset filter if we haven't used it within the last second
-                rf_state.alt_cm_filt.reset(rf_state.alt_cm);
-                reset_terrain_offset = true;
-
-            } else {
-                rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f);
-            }
-            rf_state.last_healthy_ms = now;
-        }
-
-        // handle reset of terrain offset
-        if (reset_terrain_offset) {
-            if (rf_orient == ROTATION_PITCH_90) {
-                // upward facing
-                rf_state.terrain_offset_cm = rf_state.inertial_alt_cm + rf_state.alt_cm;
-            } else {
-                // assume downward facing
-                rf_state.terrain_offset_cm = rf_state.inertial_alt_cm - rf_state.alt_cm;
-            }
-        }
-
-        // send downward facing lidar altitude and health to the libraries that require it
 #if HAL_PROXIMITY_ENABLED
-        if (rf_orient == ROTATION_PITCH_270) {
-            if (rangefinder_state.alt_healthy || timed_out) {
-                g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
-            }
-        }
+    g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
 #endif
-    }
-
-#else
-    // downward facing rangefinder
-    rangefinder_state.enabled = false;
-    rangefinder_state.alt_healthy = false;
-    rangefinder_state.alt_cm = 0;
 
-    // upward facing rangefinder
-    rangefinder_up_state.enabled = false;
-    rangefinder_up_state.alt_healthy = false;
-    rangefinder_up_state.alt_cm = 0;
 #endif
 }
 
 // return true if rangefinder_alt can be used
 bool Copter::rangefinder_alt_ok() const
 {
-    return (rangefinder_state.enabled && rangefinder_state.alt_healthy);
+    return rangefinder_state.enabled_and_healthy();
 }
 
 // return true if rangefinder_alt can be used
 bool Copter::rangefinder_up_ok() const
 {
-    return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy);
+    return rangefinder_up_state.enabled_and_healthy();
 }
 
 // update rangefinder based terrain offset
@@ -148,27 +60,16 @@ void Copter::update_rangefinder_terrain_offset()
     terrain_offset_cm = rangefinder_up_state.inertial_alt_cm + rangefinder_up_state.alt_cm_glitch_protected;
     rangefinder_up_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_up_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
 
-    if (rangefinder_state.alt_healthy || (AP_HAL::millis() - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS)) {
+    if (rangefinder_state.alt_healthy || rangefinder_state.data_stale()) {
         wp_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
 #if MODE_CIRCLE_ENABLED
         circle_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
 #endif
     }
 }
 
-/*
-  get inertially interpolated rangefinder height. Inertial height is
-  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
- */
+// helper function to get inertially interpolated rangefinder height.
 bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) const
 {
-    if (!rangefinder_alt_ok()) {
-        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;
-    return true;
+    return rangefinder_state.get_rangefinder_height_interpolated_cm(ret);
 }

ArduCopter/surface_tracking.cpp

@@ -13,9 +13,8 @@ void Copter::SurfaceTracking::update_surface_offset()
     if (((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) ||
         ((surface == Surface::CEILING) && copter.rangefinder_up_ok() && (copter.rangefinder_up_state.glitch_count == 0))) {
 
-        // calculate surfaces height above the EKF origin
-        // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m.  curr_surface_alt_above_origin_cm is 7m (or 700cm)
-        RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
+        // Get the appropriate surface distance state, the terrain offset is calculated in the surface distance lib. 
+        AC_SurfaceDistance &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
 
         // update position controller target offset to the surface's alt above the EKF origin
         copter.pos_control->set_pos_offset_target_z_cm(rf_state.terrain_offset_cm);

ArduCopter/wscript

@@ -27,6 +27,7 @@ def build(bld):
             'AP_Devo_Telem',
             'AC_AutoTune',
             'AP_KDECAN',
+            'AC_SurfaceDistance'
         ],
     )
 

libraries/AC_SurfaceDistance/AC_SurfaceDistance.cpp

@@ -60,6 +60,7 @@ void AC_SurfaceDistance::update()
     // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
     // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
     const int32_t glitch_cm = alt_cm - alt_cm_glitch_protected;
+    bool reset_terrain_offset = false;
     if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
         glitch_count = MAX(glitch_count+1, 1);
     } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
@@ -73,6 +74,7 @@ void AC_SurfaceDistance::update()
         glitch_count = 0;
         alt_cm_glitch_protected = alt_cm;
         glitch_cleared_ms = now;
+        reset_terrain_offset = true;
     }
 
     // filter rangefinder altitude
@@ -81,12 +83,24 @@ void AC_SurfaceDistance::update()
         if (timed_out) {
             // reset filter if we haven't used it within the last second
             alt_cm_filt.reset(alt_cm);
+            reset_terrain_offset = true;
         } else {
             alt_cm_filt.apply(alt_cm, 0.05f);
         }
         last_healthy_ms = now;
     }
 
+    // handle reset of terrain offset
+    if (reset_terrain_offset) {
+        if (rotation == ROTATION_PITCH_90) {
+            // upward facing
+            terrain_offset_cm = inertial_alt_cm + alt_cm;
+        } else {
+            // assume downward facing
+            terrain_offset_cm = inertial_alt_cm - alt_cm;
+        }
+    }
+
 }
 
 bool AC_SurfaceDistance::data_stale(void) const

libraries/AC_SurfaceDistance/AC_SurfaceDistance.h

@@ -20,14 +20,15 @@ class AC_SurfaceDistance {
     bool get_rangefinder_height_interpolated_cm(int32_t& ret) const;
 
     bool enabled;
-    bool alt_healthy; // true if we can trust the altitude from the rangefinder
-    int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder
-    float inertial_alt_cm; // inertial alt at time of last rangefinder sample
+    bool alt_healthy;                      // true if we can trust the altitude from the rangefinder
+    int16_t alt_cm;                        // tilt compensated altitude (in cm) from rangefinder
+    float inertial_alt_cm;                 // inertial alt at time of last rangefinder sample
     uint32_t last_healthy_ms;
-    LowPassFilterFloat alt_cm_filt {0.5}; // altitude filter
-    int16_t alt_cm_glitch_protected;    // last glitch protected altitude
-    int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
-    uint32_t glitch_cleared_ms; // system time glitch cleared
+    LowPassFilterFloat alt_cm_filt {0.5};  // altitude filter
+    int16_t alt_cm_glitch_protected;       // last glitch protected altitude
+    int8_t glitch_count;                   // non-zero number indicates rangefinder is glitching
+    uint32_t glitch_cleared_ms;            // system time glitch cleared
+    float terrain_offset_cm;               // filtered terrain offset (e.g. terrain's height above EKF origin)
 
     const AP_InertialNav& inertial_nav;
     const Rotation rotation;