Copter: add TERRAIN_MARGIN parameter

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();

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
 };
 

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[];

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);
 

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
+}