FlightTaskAuto: method for computing heading from 2D vector

src/lib/FlightTasks/tasks/FlightTaskAuto.cpp

@@ -450,6 +450,23 @@ void FlightTaskAuto::_updateInternalWaypoints()
 	}
 }
 
+bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, matrix::Vector2f v)
+{
+	if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) {
+		v.normalize();
+		// To find yaw: take dot product of x = (1,0) and v
+		// and multiply by the sign given of cross product of x and v.
+		// Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0)
+		// Cross product: x(0)*v(1) - v(0)*x(1) = v(1)
+		heading =  math::sign(v(1)) * wrap_pi(acosf(v(0)));
+		return true;
+	}
+
+	// heading unknown and therefore do not change heading
+	return false;
+}
+
+
 float FlightTaskAuto::_getVelocityFromAngle(const float angle)
 {
 	// minimum cruise speed when passing waypoint

src/lib/FlightTasks/tasks/FlightTaskAuto.hpp

@@ -83,6 +83,7 @@ protected:
 	float _getMaxCruiseSpeed() {return MPC_XY_CRUISE.get();} /**< getter for default cruise speed */
 	matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
 	void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
+	bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v);
 
 	matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
 	matrix::Vector3f _prev_wp{}; /**< Previous waypoint  (local frame). If no previous triplet is available, the prev_wp is set to current position. */