Browse Source

Copter: add failsafe_ekf_recheck

this allows modes to retrigger the EKF failsafe if they move from a sub mode that did not require GPS to one that does
apm_2208
Randy Mackay 3 years ago
parent
commit
0f73d705a7
  1. 1
      ArduCopter/Copter.h
  2. 18
      ArduCopter/ekf_check.cpp

1
ArduCopter/Copter.h

@ -720,6 +720,7 @@ private: @@ -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

@ -152,11 +152,6 @@ bool Copter::ekf_over_threshold() @@ -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) @@ -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()
{

Loading…
Cancel
Save