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.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