From 82780e33b9bff9b7f6284ce5f4c39866c21b1e89 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 4 Jul 2018 14:34:46 +1000
Subject: [PATCH] mc_pos_control: Add parameter support for terrain hold
 function

This new mode of altitude control uses terrain following when holding position and normal altitude control when moving.
---
 .../mc_pos_control/mc_pos_control_params.c    | 32 +++++++++++++++----
 1 file changed, 26 insertions(+), 6 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 95af2fcf76..eb6c42f0ea 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -504,20 +504,40 @@ PARAM_DEFINE_FLOAT(MPC_JERK_MIN, 1.0f);
 /**
  * Altitude control mode.
  *
- * Set to 1 to control height above ground instead of height above origin.
- * Note: If optical flow is being used as the only source of navigation then the height above ground
- * will be selected automatically and maximum height will be limited to the value set by MPC_MAX_FLOW_HGT.
- * Note: The height controller will revert to using height above origin if the distance to ground estimate
- * becomes invalid as indicated by the local_position.distance_bottom_valid message being false.
+ * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in
+ * flight due to sensor drift.
+ * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down
+ * with terrain height variation. Requires a distance to ground sensor. The height controller will
+ * revert to using height above origin if the distance to ground estimate becomes invalid as indicated
+ * by the local_position.distance_bottom_valid message being false.
+ * Set to 2 to control height relative to earth frame origin when stationary and relative to ground
+ * distance when moving horizontally. The speed threshold is controlled by the MPC_ALT_MODE_SPD parameter.
  *
  * @min 0
- * @max 1
+ * @max 2
  * @value 0 Altitude following
  * @value 1 Terrain following
+ * @value 2 Terrain hold
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_INT32(MPC_ALT_MODE, 0);
 
+/**
+ * Terrain hold speed threshold.
+ *
+ * Controls the horizontal speed threshold used when MPC_ALT_MODE = 2. When MPC_ALT_MODE = 2,
+ * the distance to ground will not be used to control height when horizontal speed is greater
+ * than MPC_ALT_MODE_SPD.
+ *
+ * @unit m/s
+ * @min 0.2
+ * @max 2.0
+ * @increment 0.1
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_ALT_MODE_SPD, 0.5f);
+
 /**
  * Manual control stick exponential curve sensitivity attenuation with small velocity setpoints
  *