mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-08-02 04:58:15 +08:00
FlightTaskAuto: hotfix filter for yawspeed feed-forward
to get rid of derivative spikes when navigator is continuously updating the yaw setpoint in the triplet for a POI but is running at a lower rate. The proper solution is to generate that yaw setpoint with high rate in the flight task and have the triplet just guid to the next waypoint at low rate.
This commit is contained in:
committed by
Daniel Agar
parent
3e6ff2451b
commit
b40dbd3d6f
@@ -109,8 +109,10 @@ void FlightTaskAuto::_limitYawRate()
|
||||
_yaw_setpoint = yaw_setpoint_sat;
|
||||
|
||||
if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
|
||||
// Create a feedforward
|
||||
_yawspeed_setpoint = dyaw / _deltatime;
|
||||
// Create a feedforward using the filtered derivative
|
||||
_yawspeed_filter.setParameters(_deltatime, .2f);
|
||||
_yawspeed_filter.update(dyaw);
|
||||
_yawspeed_setpoint = _yawspeed_filter.getState() / _deltatime;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user