From afada8ddb56560ec5f52c9d949b3500deae9ffc6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 22 Sep 2018 09:12:50 +0200 Subject: [PATCH] FlightTaskManualStabilized: allow attitude control with 0 throttle With minimum throttle set to 0, it was not possible to control the attitude anymore at 0 throttle, the vehicle would just stay level. --- .../tasks/ManualStabilized/FlightTaskManualStabilized.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp index fb11330e31..25b273f577 100644 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp @@ -123,9 +123,10 @@ void FlightTaskManualStabilized::_updateThrustSetpoints() /* The final thrust setpoint is found by rotating the scaled unit vector pointing * upward by the Axis-Angle. + * Make sure that the attitude can be controlled even at 0 throttle. */ Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f); - _thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle; + _thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * math::max(_throttle, 0.0001f); } void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v)