From 20ab4ccacac07cf72f7456385d9b1489aa2bf5f7 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 10 May 2023 16:19:55 +0200 Subject: [PATCH] MC_auto: avoid yaw step if previous setpoint is NAN A VTOL plane in MC mode has no yaw setpoint during takeoff because of weather-vane. To align for the front transition, the yaw target jumps and caused a step in the controller, making it reach saturation. With this commit, the previous yaw setpoint is set to the current yaw when no yaw setpoint is sent in order to create a smooth yaw trajectory starting at the current orientation when yaw target is suddenly finite. The yawspeed filter also now contains the yaw speed instead of dyaw in order to prevent chattering due to dt jitter. --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 9431e69d56..6c34aae7a3 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -321,12 +321,12 @@ void FlightTaskAuto::_limitYawRate() if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) { // Create a feedforward using the filtered derivative _yawspeed_filter.setParameters(_deltatime, .2f); - _yawspeed_filter.update(dyaw); - _yawspeed_setpoint = _yawspeed_filter.getState() / _deltatime; + _yawspeed_filter.update(dyaw / _deltatime); + _yawspeed_setpoint = _yawspeed_filter.getState(); } } - _yaw_sp_prev = _yaw_setpoint; + _yaw_sp_prev = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; if (PX4_ISFINITE(_yawspeed_setpoint)) { // The yaw setpoint is aligned when its rate is not saturated