From 9df337e2437e94b9530fa92d272f3db7a47bdf6c Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@px4.io>
Date: Thu, 26 Apr 2018 14:31:52 +0200
Subject: [PATCH] FlightTaskManualPosition: adjust scale based on vehicle
 maximum allowed speed sent by estimator

---
 .../tasks/FlightTaskManualPosition.cpp        | 27 +++++++++++++++++--
 .../tasks/FlightTaskManualPosition.hpp        |  6 +++--
 2 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
index 8fb081678a..9d264a9218 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
@@ -51,6 +51,8 @@ bool FlightTaskManualPosition::activate()
 		_constraints.speed_xy = MPC_VEL_MANUAL.get();
 	}
 
+	_velocity_scale = _constraints.speed_xy;
+
 	return ret;
 }
 
@@ -68,8 +70,29 @@ void FlightTaskManualPosition::_scaleSticks()
 		stick_xy = stick_xy.normalized() * mag;
 	}
 
-	// scale velocity to its maximum lmits
-	Vector2f vel_sp_xy = stick_xy * _constraints.speed_xy;
+	// scale the stick inputs
+	if (_sub_vehicle_local_position->get().vxy_max > 0.001f) {
+		// estimator provides vehicle specific max
+
+		// use the minimum of the estimator and user specified limit
+		_velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position->get().vxy_max);
+		// Allow for a minimum of 0.3 m/s for repositioning
+		_velocity_scale = fmaxf(_velocity_scale, 0.3f);
+
+	} else if (stick_xy.length() > 0.5f) {
+		// raise the limit at a constant rate up to the user specified value
+
+		if (_velocity_scale < _constraints.speed_xy) {
+			_velocity_scale += _deltatime * MPC_ACC_HOR_FLOW.get();
+
+		} else {
+			_velocity_scale = _constraints.speed_xy;
+
+		}
+	}
+
+	// scale velocity to its maximum limits
+	Vector2f vel_sp_xy = stick_xy * _velocity_scale;
 
 	/* Rotate setpoint into local frame. */
 	_rotateIntoHeadingFrame(vel_sp_xy);
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
index a9c0abc260..29dedd6b2f 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
@@ -58,7 +58,9 @@ protected:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
 					(ParamFloat<px4::params::MPC_VEL_MANUAL>) MPC_VEL_MANUAL,
 					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX,
-					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY
+					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
+					(ParamFloat<px4::params::MPC_ACC_HOR_FLOW>) MPC_ACC_HOR_FLOW
 				       )
-
+private:
+	float _velocity_scale; //scales the stick input to velocity
 };