Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: fixed time wrap bug in is_active_xy() 

this failed at 70 minutes
This commit is contained in:
Andrew Tridgell 2023-01-08 12:49:53 +11:00 committed by Randy Mackay
parent fe3ae67b73
commit bd9f3ef696
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -589,7 +589,8 @@ void AC_PosControl::stop_vel_xy_stabilisation()
// is_active_xy - returns true if the xy position controller has bee n run in the previous 5 loop times
bool AC_PosControl::is_active_xy() const
{
return ((AP::ins().get_last_update_usec() - _last_update_xy_us) <= _dt * 1500000.0);
const uint32_t dt_us = AP::ins().get_last_update_usec() - _last_update_xy_us;
return dt_us <= _dt * 1500000.0;
}
/// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors.
@ -903,7 +904,8 @@ void AC_PosControl::update_pos_offset_z(float pos_offset_z)
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
bool AC_PosControl::is_active_z() const
{
return ((AP::ins().get_last_update_usec() - _last_update_z_us) <= _dt * 1500000.0);
const uint32_t dt_us = AP::ins().get_last_update_usec() - _last_update_z_us;
return dt_us <= _dt * 1500000.0;
}
/// update_z_controller - runs the vertical position controller correcting position, velocity and acceleration errors.