From 2b6e356c9175743f4b43c425ad527da4a4e361ab Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 10 Jan 2018 11:41:13 +0100 Subject: [PATCH] FlightTaskManual: fix for the sideways oscillations in fast foward flight when using the vehicle yaw estimate --- src/lib/FlightTasks/tasks/FlightTaskManual.cpp | 11 +++++++++++ src/lib/FlightTasks/tasks/FlightTaskManual.hpp | 2 +- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index d224d48ec4..548475173f 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -145,6 +145,17 @@ bool FlightTaskManual::update() return true; } +float FlightTaskManual::_get_input_frame_yaw() +{ + /* using constant yaw angle from setpoint here to prevent sideways oscillation in fast forward flight */ + if (PX4_ISFINITE(_hold_yaw)) { + return _hold_yaw; + + } else { + return _yaw; + } +} + void FlightTaskManual::_updateYaw() { const float yaw_speed = _sticks(3) * math::radians(_man_yaw_max.get()); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index 4e09faba02..985b40c7df 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -67,7 +67,7 @@ protected: bool _evaluateSticks(); bool _sticks_data_required = true; /**< let sibling task define if it depends on stick data */ - float _get_input_frame_yaw() { return _yaw; } + float _get_input_frame_yaw(); virtual void _scaleVelocity(matrix::Vector3f &velocity); control::BlockParamFloat _z_vel_max_up; /**< maximal vertical velocity when flying upwards with the stick */