From 73a7aa75a8b8963eb03087c3df3d4060502bd7c8 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 12 Jul 2018 12:00:59 +1000
Subject: [PATCH] FlightTasks: update terrain hold transition logic.

Use pre-existing MPC_HOLD_MAX_XY parameter for speed threshold.
Use _stick_expo variable for stick movement check.
Update documentation.
---
 .../tasks/FlightTaskManualAltitude.cpp        | 23 ++++++++++++-------
 .../tasks/FlightTaskManualAltitude.hpp        |  2 +-
 2 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index 07087fbb96..168fe33fb0 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -91,25 +91,31 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 	// Depending on stick inputs and velocity, position is locked.
 	// If not locked, altitude setpoint is set to NAN.
 
-	// check if user wants to break
+	// Check if user wants to break
 	const bool apply_brake = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON;
 
-	// check if vehicle has stopped
+	// Check if vehicle has stopped
 	const bool stopped = (MPC_HOLD_MAX_Z.get() < FLT_EPSILON || fabsf(_velocity(2)) < MPC_HOLD_MAX_Z.get());
 
+	// Manage transition between use of distance to ground and distance to local origin
+	// when terrain hold behaviour has been selected.
 	if (MPC_ALT_MODE.get() == 2) {
-		// terrain hold
+		// Use horizontal speed as a transition criteria
 		float spd_xy = Vector2f(&_velocity(0)).length();
-		bool stick_input = Vector2f(&_velocity_setpoint(0)).length() > 0.5f * MPC_ALT_MODE_SPD.get();
+
+		// Use presence of horizontal stick inputs as a transition criteria
+		float stick_xy = Vector2f(&_sticks_expo(0)).length();
+		bool stick_input = stick_xy > 0.001f;
 
 		if (_terrain_hold) {
-			bool too_fast = spd_xy > MPC_ALT_MODE_SPD.get();
+			bool too_fast = spd_xy > MPC_HOLD_MAX_XY.get();
 
 			if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
+				// Stop using distance to ground
 				_terrain_hold = false;
 				_terrain_follow = false;
 
-				// adjust the setpoint to maintain the same height error to reduce control transients
+				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
 					_position_setpoint(2) = _position(2) + (_dist_to_ground_lock - _dist_to_bottom);
 
@@ -119,13 +125,14 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			}
 
 		} else {
-			bool not_moving = spd_xy < 0.5f * MPC_ALT_MODE_SPD.get();
+			bool not_moving = spd_xy < 0.5f * MPC_HOLD_MAX_XY.get();
 
 			if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
+				// Start using distance to ground
 				_terrain_hold = true;
 				_terrain_follow = true;
 
-				// adjust the setpoint to maintain the same height error to reduce control transients
+				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_position_setpoint(2))) {
 					_dist_to_ground_lock = _dist_to_bottom + (_position_setpoint(2) - _position(2));
 				}
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
index 901f6475e0..f7bb7a51d0 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
@@ -63,7 +63,7 @@ protected:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
 					(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
-					(ParamFloat<px4::params::MPC_ALT_MODE_SPD>) MPC_ALT_MODE_SPD,
+					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
 					(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P
 				       )
 private: