From 980c1f50207bdd34a7c22812e8ad932a34aab4b0 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 22 Jun 2020 13:00:44 +0900
Subject: [PATCH] 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             |  1 +
 ArduCopter/Copter.h               |  1 +
 ArduCopter/baro_ground_effect.cpp | 19 +++++++++++++++++++
 3 files changed, 21 insertions(+)

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index df74c7bcae..77fd564ae2 100644
--- a/ArduCopter/Copter.cpp
+++ b/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
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index fa58dac91e..4c72ef3703 100644
--- a/ArduCopter/Copter.h
+++ b/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();
diff --git a/ArduCopter/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp
index ef63a9e705..14b0ddfbd6 100644
--- a/ArduCopter/baro_ground_effect.cpp
+++ b/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);
+}