From 46f0a14c2879fecb43aa7520d8f86e0ede7b02f0 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 3 Jan 2018 10:42:24 +0100 Subject: [PATCH] FlightTaskStabilized: remove yaw prediction --- .../FlightTasks/tasks/FlightTaskManualStabilized.cpp | 10 ++++------ .../FlightTasks/tasks/FlightTaskManualStabilized.hpp | 1 - 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp index 3c866f9c7b..a38906ae2f 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp @@ -55,7 +55,6 @@ bool FlightTaskManualStabilized::activate() { _yaw_sp = _yaw; _yaw_rate_sp = 0.0f; - _sign_speed = 1.0f; return FlightTaskManual::activate(); } @@ -69,17 +68,16 @@ void FlightTaskManualStabilized::_scaleSticks() void FlightTaskManualStabilized::_updateHeadingSetpoints() { - /* Yaw-lock depends on stick input. If locked, - * yawspeed_sp is set to NAN. Otherwise yaw_sp is set - * to NAN.*/ + /* Yaw-lock depends on stick input. If not locked, + * yaw_sp is set to NAN. + * TODO: add yawspeed to get threshold.*/ const bool stick_yaw_zero = fabsf(_sticks(3)) <= _stick_dz.get(); if (stick_yaw_zero && !PX4_ISFINITE(_yaw_sp)) { - _yaw_sp = _wrap_pi(_yaw + _sign_speed * _yaw_rate_scaling.get() * _deltatime); + _yaw_sp = _yaw; } else if (!stick_yaw_zero) { _yaw_sp = NAN; - _sign_speed = math::sign(_yaw_rate_sp); } } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp index f324fa3e2d..37b21c6c3b 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp @@ -62,7 +62,6 @@ protected: virtual void _scaleSticks(); /**< Scales sticks to yaw and thrust. */ private: - float _sign_speed{}; void _updateHeadingSetpoints(); /**< Sets yaw or yaw speed. */ void _updateThrustSetpoints(); /**< Sets thrust setpoint */