From 7dcc5dd7acec412b287380b700bf5355f7fc49db Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 9 Jul 2021 14:18:28 +0900
Subject: [PATCH] Copter: add TERRAIN_MARGIN parameter

---
 ArduCopter/Copter.h        |  1 +
 ArduCopter/Parameters.cpp  | 10 ++++++++++
 ArduCopter/Parameters.h    |  3 +++
 ArduCopter/mode_guided.cpp |  4 +---
 ArduCopter/terrain.cpp     | 10 ++++++++++
 5 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index adcf2a3b37..1b6b30e093 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -884,6 +884,7 @@ private:
     // terrain.cpp
     void terrain_update();
     void terrain_logging();
+    float get_terrain_margin() const;
 
     // tuning.cpp
     void tuning();
diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index c5e580e301..6f4133b65f 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -1079,6 +1079,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0),
 #endif
 
+#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
+    // @Param: TERRAIN_MARGIN
+    // @DisplayName: Terrain following altitude margin
+    // @Description: Terrain following altitude margin.  Vehicle will stop if distance from target altitude is larger than this margin (in meters)
+    // @Units: m
+    // @Range: 0.1 100
+    // @User: Advanced
+    AP_GROUPINFO("TERRAIN_MARGIN", 47, ParametersG2, terrain_margin, 10.0),
+#endif
+
     AP_GROUPEND
 };
 
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 2432455a26..9e91f52240 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -653,6 +653,9 @@ public:
     AP_Float guided_timeout;
 #endif
 
+#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
+    AP_Float terrain_margin;
+#endif
 };
 
 extern const AP_Param::Info        var_info[];
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 7f70ad9f12..848251babd 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -551,11 +551,9 @@ void ModeGuided::pos_control_run()
     guided_accel_target_cmss.zero();
     guided_vel_target_cms.zero();
 
-    // todo: Randy to convert to parameter TERRAIN_MARGIN (in m)
-    float TERRAIN_MARGIN = 10;
     float pos_offset_z_buffer = 0.0; // Vertical buffer size in m
     if (guided_pos_terrain_alt) {
-        pos_offset_z_buffer = MIN(TERRAIN_MARGIN * 100.0, 0.5 * fabsf(guided_pos_target_cm.z));
+        pos_offset_z_buffer = MIN(copter.get_terrain_margin() * 100.0, 0.5 * fabsf(guided_pos_target_cm.z));
     }
     pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset, pos_offset_z_buffer);
 
diff --git a/ArduCopter/terrain.cpp b/ArduCopter/terrain.cpp
index 09649eb26f..43ea931f60 100644
--- a/ArduCopter/terrain.cpp
+++ b/ArduCopter/terrain.cpp
@@ -26,3 +26,13 @@ void Copter::terrain_logging()
     }
 #endif
 }
+
+// return terrain following altitude margin.  Vehicle will stop if distance from target altitude is larger than this margin (in meters)
+float Copter::get_terrain_margin() const
+{
+#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
+    return MAX(g2.terrain_margin.get(), 0.1);
+#else
+    return 10;
+#endif
+}