diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index eb0e6b01f8..329a601b6d 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -577,8 +577,12 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, if ((_armed_time != 0) && (time_us < _armed_time + static_cast(_param_com_spoolup_time.get() * 1_s)) ) { - CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, ActionOptions(Action::Disarm).cannotBeDeferred()); - CHECK_FAILSAFE(status_flags, battery_unhealthy, ActionOptions(Action::Disarm).cannotBeDeferred()); + _last_state_fd_esc_arming = checkFailsafe(_caller_id_fd_esc_arming, _last_state_fd_esc_arming, + status_flags.fd_esc_arming_failure, + ActionOptions(Action::Disarm).cannotBeDeferred()); + _last_state_battery_unhealthy_spoolup = checkFailsafe(_caller_id_battery_unhealthy_spoolup, + _last_state_battery_unhealthy_spoolup, status_flags.battery_unhealthy, + ActionOptions(Action::Disarm).cannotBeDeferred()); } // Handle fails during the early takeoff phase diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 7865c207db..71308c9d6f 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -179,6 +179,10 @@ private: bool _last_state_battery_warning_critical{false}; const int _caller_id_battery_warning_emergency{genCallerId()}; bool _last_state_battery_warning_emergency{false}; + const int _caller_id_fd_esc_arming{genCallerId()}; + bool _last_state_fd_esc_arming{false}; + const int _caller_id_battery_unhealthy_spoolup{genCallerId()}; + bool _last_state_battery_unhealthy_spoolup{false}; hrt_abstime _armed_time{0}; bool _was_armed{false};