From 9e740f1affd0f8396c64f107879f4828bc13e9fa Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@px4.io>
Date: Thu, 26 Apr 2018 11:26:19 +0200
Subject: [PATCH] FlightTaskManualAlt/Position/Sport: replace limits with
 constraint member structure

---
 .../tasks/FlightTaskManualAltitude.cpp        |  9 ++++++-
 .../tasks/FlightTaskManualAltitude.hpp        |  3 +--
 .../tasks/FlightTaskManualPosition.cpp        | 24 +++++++++++--------
 .../tasks/FlightTaskManualPosition.hpp        |  4 ++--
 src/lib/FlightTasks/tasks/FlightTaskSport.hpp | 11 +++++----
 5 files changed, 31 insertions(+), 20 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index 4935a8d248..ffe46b0b50 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -40,13 +40,20 @@
 
 using namespace matrix;
 
+bool FlightTaskManualAltitude::activate()
+{
+	bool ret = FlightTaskManualStabilized::activate();
+	_setDefaultConstraints();
+	return ret;
+}
+
 void FlightTaskManualAltitude::_scaleSticks()
 {
 	// reuse same scaling as for stabilized
 	FlightTaskManualStabilized::_scaleSticks();
 
 	// scale horizontal velocity with expo curve stick input
-	const float vel_max_z = (_sticks(2) > 0.0f) ? _limits.speed_dn_max : _limits.speed_up_max;
+	const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
 	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
 }
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
index 430120d880..f1940019d9 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
@@ -45,8 +45,8 @@ class FlightTaskManualAltitude : public FlightTaskManualStabilized
 {
 public:
 	FlightTaskManualAltitude() = default;
-
 	virtual ~FlightTaskManualAltitude() = default;
+	bool activate() override;
 
 protected:
 	void _updateSetpoints() override; /**< updates all setpoints */
@@ -64,7 +64,6 @@ protected:
 					(ParamFloat<px4::params::SENS_FLOW_MINRNG>) SENS_FLOW_MINRNG,
 					(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE
 				       )
-
 private:
 	/**
 	 * Distance to ground during terrain following.
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
index 1de8a825e4..8fb081678a 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
@@ -41,6 +41,19 @@
 
 using namespace matrix;
 
+bool FlightTaskManualPosition::activate()
+{
+
+	bool ret = FlightTaskManualAltitude::activate();
+
+	// set task specific constraint
+	if (_constraints.speed_xy >= MPC_VEL_MANUAL.get()) {
+		_constraints.speed_xy = MPC_VEL_MANUAL.get();
+	}
+
+	return ret;
+}
+
 void FlightTaskManualPosition::_scaleSticks()
 {
 	/* Use same scaling as for FlightTaskManualAltitude */
@@ -56,7 +69,7 @@ void FlightTaskManualPosition::_scaleSticks()
 	}
 
 	// scale velocity to its maximum lmits
-	Vector2f vel_sp_xy = stick_xy * _limits.speed_NE_max;
+	Vector2f vel_sp_xy = stick_xy * _constraints.speed_xy;
 
 	/* Rotate setpoint into local frame. */
 	_rotateIntoHeadingFrame(vel_sp_xy);
@@ -88,12 +101,3 @@ void FlightTaskManualPosition::_updateSetpoints()
 	_thrust_setpoint *= NAN; // don't require any thrust setpoints
 	_updateXYlock(); // check for position lock
 }
-
-void FlightTaskManualPosition::_updateSetpointLimits()
-{
-	FlightTaskManualAltitude::_updateSetpointLimits();
-
-	if (_limits.speed_NE_max >= MPC_VEL_MANUAL.get()) {
-		_limits.speed_NE_max = MPC_VEL_MANUAL.get();
-	}
-}
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
index cbd96eafc9..a9c0abc260 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
@@ -48,12 +48,12 @@ public:
 	FlightTaskManualPosition() = default;
 
 	virtual ~FlightTaskManualPosition() = default;
+	bool activate() override;
 
 protected:
-	void _updateXYlock(); /**< applies positon lock based on stick and velocity */
+	void _updateXYlock(); /**< applies position lock based on stick and velocity */
 	void _updateSetpoints() override;
 	void _scaleSticks() override;
-	void _updateSetpointLimits() override;
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
 					(ParamFloat<px4::params::MPC_VEL_MANUAL>) MPC_VEL_MANUAL,
diff --git a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
index b146a2492a..98b978052b 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
@@ -52,12 +52,13 @@ public:
 
 	virtual ~FlightTaskSport() = default;
 
-protected:
-	void _updateSetpointLimits() override
+	bool activate() override
 	{
-		FlightTaskManualPosition::_updateSetpointLimits();
+		bool ret = FlightTaskManualPosition::activate();
 
-		// for sport mode we just increase horizontal speed to maximum speed
-		_limits.speed_NE_max = MPC_XY_VEL_MAX.get();
+		// default constraints already are the maximum allowed limits
+		_setDefaultConstraints();
+
+		return ret;
 	}
 };