mirror of https://github.com/ArduPilot/ardupilot
Plane: reset steering integrator on mode change and when not moving
this prevents an old integrator from causing problems on takeoff
This commit is contained in:
parent
e9a9e33280
commit
daa32f9b62
|
@ -364,6 +364,11 @@ static void stabilize()
|
|||
rollController.reset_I();
|
||||
pitchController.reset_I();
|
||||
yawController.reset_I();
|
||||
|
||||
// if moving very slowly also zero the steering integrator
|
||||
if (gps.ground_speed() < 1) {
|
||||
steerController.reset_I();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -390,6 +390,7 @@ static void set_mode(enum FlightMode mode)
|
|||
rollController.reset_I();
|
||||
pitchController.reset_I();
|
||||
yawController.reset_I();
|
||||
steerController.reset_I();
|
||||
}
|
||||
|
||||
// exit_mode - perform any cleanup required when leaving a flight mode
|
||||
|
|
Loading…
Reference in New Issue