diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp index 37b2f711ae..c671c3a8de 100644 --- a/src/drivers/safety_button/SafetyButton.cpp +++ b/src/drivers/safety_button/SafetyButton.cpp @@ -213,13 +213,18 @@ SafetyButton::Run() if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { FlashButton(); - safety_s safety{}; - safety.timestamp = hrt_absolute_time(); - safety.safety_switch_available = true; - safety.safety_off = _safety_btn_off || _safety_disabled; + const bool safety_off = _safety_btn_off || _safety_disabled; - // publish the safety status - _to_safety.publish(safety); + // publish immediately on change, otherwise at 1 Hz + if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s) + || (_safety.safety_off != safety_off)) { + + _safety.safety_switch_available = true; + _safety.safety_off = safety_off; + _safety.timestamp = hrt_absolute_time(); + + _to_safety.publish(_safety); + } } } diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp index 79177b0265..84bc0bf0f5 100644 --- a/src/drivers/safety_button/SafetyButton.hpp +++ b/src/drivers/safety_button/SafetyButton.hpp @@ -84,6 +84,7 @@ private: uORB::PublicationQueued _to_led_control{ORB_ID(led_control)}; uORB::Publication _to_tune_control{ORB_ID(tune_control)}; + safety_s _safety{}; uint8_t _button_counter{0}; uint8_t _blink_counter{0};