Browse Source

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.
sbg
Beat Küng 7 years ago committed by Lorenz Meier
parent
commit
afada8ddb5
  1. 3
      src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp

3
src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp

@ -123,9 +123,10 @@ void FlightTaskManualStabilized::_updateThrustSetpoints() @@ -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)

Loading…
Cancel
Save