From 694f49c80aa9d9bcb9d7c8df9bb7f126dc5d2e04 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 2 Aug 2018 09:49:15 +0200 Subject: [PATCH] FlightTaskAuto: method for computing heading from 2D vector --- src/lib/FlightTasks/tasks/FlightTaskAuto.cpp | 17 +++++++++++++++++ src/lib/FlightTasks/tasks/FlightTaskAuto.hpp | 1 + 2 files changed, 18 insertions(+) diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index 0628dcfc4d..692ccab413 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/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 diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index 44a451d15c..524cf467dd 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/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. */