fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 16:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC_auto: avoid yaw step if previous setpoint is NAN

Browse Source 
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.
This commit is contained in:
bresch
2023-05-10 16:19:55 +02:00
parent a15b0d3c6a
commit 20ab4ccaca
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+3 -3
View File
@@ -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