Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: prevent panic on SITL when no lock-step scheduling

Browse Source 
this disables the internal error for bad timing on SITL types without
lock-step scheduling, such as RealFlight
This commit is contained in:
Andrew Tridgell 2021-05-18 11:24:41 +10:00
parent bf1f27af32
commit 8213fc5277
2 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -573,8 +573,10 @@ void AC_PosControl::update_xy_controller()
// Check for position control time out
if ( !is_active_xy() ) {
init_xy_controller();
// call internal error because initialisation has not been done
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
if (has_good_timing()) {
// call internal error because initialisation has not been done
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
_last_update_xy_us = AP_HAL::micros64();
@ -863,8 +865,10 @@ void AC_PosControl::update_z_controller()
// Check for z_controller time out
if (!is_active_z()) {
init_z_controller();
// call internal error because initialisation has not been done
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
if (has_good_timing()) {
// call internal error because initialisation has not been done
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
_last_update_z_us = AP_HAL::micros64();
@ -1222,3 +1226,16 @@ bool AC_PosControl::pre_arm_checks(const char *param_prefix,
return true;
}
// return true if on a real vehicle or SITL with lock-step scheduling
bool AC_PosControl::has_good_timing(void) const
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
auto *sitl = AP::sitl();
if (sitl) {
return sitl->state.is_lock_step_scheduled;
}
#endif
// real boards are assumed to have good timing
return true;
}

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -431,4 +431,7 @@ protected:
// high vibration handling
bool _vibe_comp_enabled; // true when high vibration compensation is on
// return true if on a real vehicle or SITL with lock-step scheduling
bool has_good_timing(void) const;
};