ControlMath: adding limitTilt() helper function

which takes care of limiting the lilt angle of a "body" vector with
respect to a "world" vector. Both vectors have to be unit length!

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -50,6 +50,23 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitu
 	att_sp.thrust_body[2] = -thr_sp.length();
 }
 
+void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle)
+{
+	// determine tilt
+	const float dot_product_unit = body_unit.dot(world_unit);
+	float angle = acosf(dot_product_unit);
+	// limit tilt
+	angle = math::min(angle, max_angle);
+	Vector3f rejection = body_unit - (dot_product_unit * world_unit);
+
+	// corner case exactly parallel vectors
+	if (rejection.norm_squared() < FLT_EPSILON) {
+		rejection(0) = 1.f;
+	}
+
+	body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit();
+}
+
 void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
 {
 	// zero vector, no direction, set safe level value

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -52,6 +52,15 @@ namespace ControlMath
  * @param att_sp attitude setpoint to fill
  */
 void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+
+/**
+ * Limits the tilt angle between two unit vectors
+ * @param body_unit unit vector that will get adjusted if angle is too big
+ * @param world_unit fixed vector to measure the angle against
+ * @param max_angle maximum tilt angle between vectors in radians
+ */
+void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit, const float max_angle);
+
 /**
  * Converts a body z vector and yaw set-point to a desired attitude.
  * @param body_z a world frame 3D vector in direction of the desired body z axis

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -38,6 +38,65 @@
 using namespace matrix;
 using namespace ControlMath;
 
+TEST(ControlMathTest, LimitTiltUnchanged)
+{
+	Vector3f body = Vector3f(0.f, 0.f, 1.f).normalized();
+	Vector3f body_before = body;
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	EXPECT_EQ(body, body_before);
+
+	body = Vector3f(0.f, .1f, 1.f).normalized();
+	body_before = body;
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	EXPECT_EQ(body, body_before);
+}
+
+TEST(ControlMathTest, LimitTiltOpposite)
+{
+	Vector3f body = Vector3f(0.f, 0.f, -1.f).normalized();
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f);
+	EXPECT_FLOAT_EQ(body.length(), 1.f);
+}
+
+TEST(ControlMathTest, LimitTiltAlmostOpposite)
+{
+	// This case doesn't trigger corner case handling but is very close to it
+	Vector3f body = Vector3f(0.001f, 0.f, -1.f).normalized();
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f);
+	EXPECT_FLOAT_EQ(body.length(), 1.f);
+}
+
+TEST(ControlMathTest, LimitTilt45degree)
+{
+	Vector3f body = Vector3f(1.f, 0.f, 0.f);
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	EXPECT_EQ(body, Vector3f(M_SQRT1_2_F, 0, M_SQRT1_2_F));
+
+	body = Vector3f(0.f, 1.f, 0.f);
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	EXPECT_EQ(body, Vector3f(0.f, M_SQRT1_2_F, M_SQRT1_2_F));
+}
+
+TEST(ControlMathTest, LimitTilt10degree)
+{
+	Vector3f body = Vector3f(1.f, 1.f, .1f).normalized();
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f);
+	float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f);
+	EXPECT_FLOAT_EQ(body.length(), 1.f);
+	EXPECT_FLOAT_EQ(body(0), body(1));
+
+	body = Vector3f(1, 2, .2f);
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f);
+	angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f);
+	EXPECT_FLOAT_EQ(body.length(), 1.f);
+	EXPECT_FLOAT_EQ(2.f * body(0), body(1));
+}
 
 TEST(ControlMathTest, ThrottleAttitudeMapping)
 {
@@ -76,12 +135,12 @@ TEST(ControlMathTest, ConstrainXYPriorities)
 {
 	const float max = 5.f;
 	// v0 already at max
-	Vector2f v0(max, 0);
+	Vector2f v0(max, 0.f);
 	Vector2f v1(v0(1), -v0(0));
 
 	Vector2f v_r = constrainXY(v0, v1, max);
 	EXPECT_FLOAT_EQ(v_r(0), max);
-	EXPECT_FLOAT_EQ(v_r(1), 0);
+	EXPECT_FLOAT_EQ(v_r(1), 0.f);
 
 	// norm of v1 exceeds max but v0 is zero
 	v0.zero();
@@ -100,7 +159,7 @@ TEST(ControlMathTest, ConstrainXYPriorities)
 	v1 = Vector2f(0.f, -4.f);
 	v_r = constrainXY(v0, v1, max);
 	EXPECT_FLOAT_EQ(v_r(0), v0(0));
-	EXPECT_GT(v_r(0), 0);
+	EXPECT_GT(v_r(0), 0.f);
 	const float remaining = sqrtf(max * max - (v0(0) * v0(0)));
 	EXPECT_FLOAT_EQ(v_r(1), -remaining);
 }