Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add failsafe_ekf_recheck

Browse Source 
this allows modes to retrigger the EKF failsafe if they move from a sub mode that did not require GPS to one that does
This commit is contained in:
Randy Mackay 2022-05-25 21:25:33 +09:00
parent ecad165f86
commit 0f73d705a7
2 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -720,6 +720,7 @@ private:
bool ekf_over_threshold();
void failsafe_ekf_event();
void failsafe_ekf_off_event(void);
void failsafe_ekf_recheck();
void check_ekf_reset();
void check_vibration();

18
ArduCopter/ekf_check.cpp
View File

@ -152,11 +152,6 @@ bool Copter::ekf_over_threshold()
// failsafe_ekf_event - perform ekf failsafe
void Copter::failsafe_ekf_event()
{
// return immediately if ekf failsafe already triggered
if (failsafe.ekf) {
return;
}
// EKF failsafe event has occurred
failsafe.ekf = true;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
@ -213,6 +208,19 @@ void Copter::failsafe_ekf_off_event(void)
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
}
// re-check if the flight mode requires GPS but EKF failsafe is active
// this should be called by flight modes that are changing their submode from one that does NOT require a position estimate to one that does
void Copter::failsafe_ekf_recheck()
{
// return immediately if not in ekf failsafe
if (!failsafe.ekf) {
return;
}
// trigger EKF failsafe action
failsafe_ekf_event();
}
// check for ekf yaw reset and adjust target heading, also log position reset
void Copter::check_ekf_reset()
{