mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
AC_PosControl: bug fix dt calculation
fixes issue in which now could be earlier than _last_update_xy_ms leading to a large dt value and a sudden lean on takeoff
This commit is contained in:
parent
34352860e7
commit
a1cfd03c9b
@ -548,6 +548,7 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity)
|
||||
uint32_t now = hal.scheduler->millis();
|
||||
if ((now - _last_update_xy_ms) >= POSCONTROL_ACTIVE_TIMEOUT_MS) {
|
||||
init_xy_controller();
|
||||
now = _last_update_xy_ms;
|
||||
}
|
||||
|
||||
// check if xy leash needs to be recalculated
|
||||
|
Loading…
Reference in New Issue
Block a user