From 1f1c9fde01f7703bee532224ca45db43bbd198bb Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 23 Aug 2018 08:25:23 +0200 Subject: [PATCH] FlightTaskAuto: lock yaw once within acceptance radius --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 9 ++++++++- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 1fb231e72f..eb0bb1b950 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -256,13 +256,20 @@ void FlightTaskAuto::_set_heading_from_mode() } if (PX4_ISFINITE(v.length())) { - // We only adjust yaw if vehicle is outside of acceptance radius. + // We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance + // radius, lock yaw to current yaw. // This prevents excessive yawing. if (v.length() > NAV_ACC_RAD.get()) { _compute_heading_from_2D_vector(_yaw_setpoint, v); + _yaw_lock = false; + } else { + if (!_yaw_lock) { + _yaw_setpoint = _yaw; + _yaw_lock = true; } } else { + _yaw_lock = false; _yaw_setpoint = NAN; } } diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 3941aca1ab..90ba24b61e 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -107,7 +107,7 @@ protected: private: matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */ - + bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */ uORB::Subscription *_sub_triplet_setpoint{nullptr}; matrix::Vector3f