From 6c0acf8e13dcc38c2f7eb6e03d1b251621fdf451 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 16 Dec 2017 09:30:37 +0100 Subject: [PATCH] FlightTaskManual: enable non-carrot velocity method also for yaw --- src/lib/FlightTasks/tasks/FlightTaskManual.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 89b1f1a404..f4f718b361 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -76,7 +76,7 @@ bool FlightTaskManual::activate() { bool ret = FlightTask::activate(); _hold_position = Vector3f(NAN, NAN, NAN); - _hold_yaw = _yaw; + _hold_yaw = NAN; return ret; } @@ -150,7 +150,15 @@ void FlightTaskManual::_updateYaw() const float yaw_speed = _sticks(3) * math::radians(_man_yaw_max.get()); _setYawspeedSetpoint(yaw_speed); - _hold_yaw += yaw_speed * _deltatime; + const bool stick_yaw_zero = yaw_speed <= FLT_EPSILON; + + if (stick_yaw_zero && !PX4_ISFINITE(_hold_position(2))) { + _hold_yaw = _yaw; + + } else if (!stick_yaw_zero) { + _hold_yaw = NAN; + } + _setYawSetpoint(_hold_yaw); }