From 3c40aa0d9c7023740b08a89cd7e8fd4993915c13 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Wed, 7 Feb 2018 13:19:34 +0100
Subject: [PATCH] PositionControl: if valid velocity and position setpoint
 available, always prioritize velocity setpoint generated from position error
 over velocity feedforward

---
 .../mc_pos_control/PositionControl.cpp        | 17 +++---
 .../mc_pos_control/Utility/ControlMath.cpp    | 53 +++++++++++++++++++
 .../mc_pos_control/Utility/ControlMath.hpp    |  1 +
 3 files changed, 61 insertions(+), 10 deletions(-)

diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index abfc8aabe8..f4f9580d37 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -174,18 +174,15 @@ void PositionControl::_positionController()
 	/* Generate desired velocity setpoint */
 
 	/* P-controller */
-	_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;
+	Vector3f vel_sp_position = (_pos_sp - _pos).emult(Pp);
+	_vel_sp = vel_sp_position + _vel_sp;
 
-	/* Make sure velocity setpoint is constrained in all directions (xyz). */
-	Vector2f vel_sp_xy(&_vel_sp(0));
 
-	if (vel_sp_xy.length() >= _VelMaxXY) {
-		_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_sp_xy.length();
-		_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_sp_xy.length();
-	}
-
-	/* Saturate velocity in D-direction */
-	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ.up, _VelMaxZ.down);
+	Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)), Vector2f(&(_vel_sp - vel_sp_position)(0)),
+			     _VelMaxXY);
+	_vel_sp(0) = vel_sp_xy(0);
+	_vel_sp(1) = vel_sp_xy(1);
+	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
 }
 
 void PositionControl::_velocityController(const float &dt)
diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp
index 9235907b7c..df5a1b5216 100644
--- a/src/modules/mc_pos_control/Utility/ControlMath.cpp
+++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp
@@ -113,4 +113,57 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
 
 	return att_sp;
 }
+
+/* The sum of two vectors are constraints such that v0 has priority over v1.
+ * This means that if the length of v0+v1 exceeds max, then it is constraint such
+ * that that v0 has priority.
+ * Inputs:
+ * @max => maximum magnitude of vector (v0 + v1)
+ * @v0 => vector that is prioritized
+ * @v1 => vector that is scaled such that max is not exceeded
+ *
+ * Output:
+ * vector that is the sum of v1 and v0 with v0 prioritized.
+ *
+ * vf = final vector with ||vf|| <= max
+ * s = scaling factor
+ * u1 = unit of v1
+ * vf = v0 + v1 = v0 + s * u1
+ * constraint: ||vf|| <= max
+ * solve for s: ||vf|| = ||v0 + s * u1|| <= max
+ */
+matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f v1, const float max)
+{
+
+	if (matrix::Vector2f(v0 + v1).norm() <= max) {
+		/* Vector does not exceed maximum magnitude */
+		return v0 + v1;
+
+	} else if (v0.length() >= max) {
+		/* The magnitude along v0, which has priority, already exceeds maximum.*/
+		return v0;
+
+	} else if (fabsf(matrix::Vector2f(v1 - v0).norm()) < 0.001f) {
+		/* The two vectors are equal. */
+		return v0.normalized() * max;
+
+	} else if (v1.length() < 0.001f) {
+		/* The second vector is 0. */
+		return v0.normalized() * max;
+
+	} else {
+		/* Constrain but prioritize v0. */
+		float s = 0.0f;
+		matrix::Vector2f u1 = v1.normalized();
+		float det = sqrtf(max * max * u1.length() * u1.length() - (v0(0) * u1(1) - v0(1) * u1(0)) * (v0(0) * u1(1) - v0(1) * u1(
+					  0)));
+		float b = v0(0) * u1(0) + v0(1) * u1(1);
+
+		if (det >= b) {
+			s = (det - b) / (u1.length() * u1.length());
+		}
+
+		return v0 + u1 * s;
+	}
+}
 }
diff --git a/src/modules/mc_pos_control/Utility/ControlMath.hpp b/src/modules/mc_pos_control/Utility/ControlMath.hpp
index c82b5285d0..1018cc3e40 100644
--- a/src/modules/mc_pos_control/Utility/ControlMath.hpp
+++ b/src/modules/mc_pos_control/Utility/ControlMath.hpp
@@ -47,4 +47,5 @@
 namespace ControlMath
 {
 vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp);
+matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f v1, const float max);
 }