AC_WPNav: support rangefinder for terrain following

libraries/AC_WPNav/AC_WPNav.cpp

@@ -1155,6 +1155,17 @@ void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector
 bool AC_WPNav::get_terrain_offset(float& offset_cm)
 {
 #if AP_TERRAIN_AVAILABLE
+    // use range finder if connected
+    if (_rangefinder_use) {
+        if (_rangefinder_healthy) {
+            offset_cm = _inav.get_altitude() - _rangefinder_alt_cm;
+            return true;
+        } else {
+            return false;
+        }
+    }
+
+    // use terrain database
     float terr_alt = 0.0f;
     if (_terrain != NULL && _terrain->height_above_terrain(terr_alt, true)) {
         offset_cm = _inav.get_altitude() - (terr_alt * 100.0f);

libraries/AC_WPNav/AC_WPNav.h

@@ -40,6 +40,8 @@
 
 #define WPNAV_YAW_DIST_MIN                 200      // minimum track length which will lead to target yaw being updated to point at next waypoint.  Under this distance the yaw target will be frozen at the current heading
 
+#define WPNAV_RANGEFINDER_FILT_Z         0.25f      // range finder distance filtered at 0.25hz
+
 class AC_WPNav
 {
 public:
@@ -57,6 +59,9 @@ public:
     /// provide pointer to terrain database
     void set_terrain(AP_Terrain* terrain_ptr) { _terrain = terrain_ptr; }
 
+    /// provide rangefinder altitude
+    void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_use = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; }
+
     ///
     /// loiter controller
     ///
@@ -356,4 +361,7 @@ protected:
     // terrain following variables
     bool        _terrain_alt = false;   // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
     bool        _ekf_origin_terrain_alt_set = false;
+    bool        _rangefinder_use = false;
+    bool        _rangefinder_healthy = false;
+    float       _rangefinder_alt_cm = 0.0f;
 };