aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: fixed time wrap bug in is_active_xy()

Browse Source 
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 fe2f5acff6
commit 5cbd810e41
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 // 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 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. /// 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 // 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 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. /// update_z_controller - runs the vertical position controller correcting position, velocity and acceleration errors.