Copter: update ekf terrain height stable setting

setting is true only when taking off or landing and horizontal speed is no more than 0.5m/s

ArduCopter/Copter.cpp

@@ -354,6 +354,7 @@ void Copter::throttle_loop()
 
     // compensate for ground effect (if enabled)
     update_ground_effect_detector();
+    update_ekf_terrain_height_stable();
 }
 
 // update_batt_compass - read battery and compass

ArduCopter/Copter.h

@@ -677,6 +677,7 @@ private:
 
     // baro_ground_effect.cpp
     void update_ground_effect_detector(void);
+    void update_ekf_terrain_height_stable();
 
     // commands.cpp
     void update_home_from_EKF();

ArduCopter/baro_ground_effect.cpp

@@ -1,5 +1,9 @@
 #include "Copter.h"
 
+#ifndef EKF_TERRAIN_HGT_STABLE_SPEED_MAX
+ # define EKF_TERRAIN_HGT_STABLE_SPEED_MAX  0.5f    // terrain height is only considered stable under 0.75m/s.  Note this is not related to terrain following
+#endif
+
 void Copter::update_ground_effect_detector(void)
 {
     if(!g2.gndeffect_comp_enabled || !motors->armed()) {
@@ -67,3 +71,18 @@ void Copter::update_ground_effect_detector(void)
     ahrs.setTakeoffExpected(gndeffect_state.takeoff_expected);
     ahrs.setTouchdownExpected(gndeffect_state.touchdown_expected);
 }
+
+// update ekf terrain height stable setting
+// when set to true, this allows the EKF to stabilize the normally barometer based altitude using a rangefinder
+// this is not related to terrain following
+void Copter::update_ekf_terrain_height_stable()
+{
+    // set to false if no position estimate
+    if (!position_ok() && !optflow_position_ok()) {
+        ahrs.set_terrain_hgt_stable(false);
+    }
+
+    // consider terrain height stable if vehicle is taking off or landing and moving less than 0.5 m/s
+    const bool is_stable = (flightmode->is_taking_off() || flightmode->is_landing()) && (ahrs.groundspeed() <= EKF_TERRAIN_HGT_STABLE_SPEED_MAX);
+    ahrs.set_terrain_hgt_stable(is_stable);
+}