dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
bresch 2023-05-10 16:19:55 +02:00 committed by Mathieu Bresciani
parent f19b81880f
commit 616a71a959
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
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