From 94a5a97dccbc9ca9f373948f13ff75cfa6e8723d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 16 Jul 2020 12:30:41 -0400
Subject: [PATCH] drivers/safety_button: only publish safety at 1 Hz or
 immeidately on change

---
 src/drivers/safety_button/SafetyButton.cpp | 17 +++++++++++------
 src/drivers/safety_button/SafetyButton.hpp |  1 +
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp
index 37b2f711aea..c671c3a8de7 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 79177b02656..84bc0bf0f59 100644
--- a/src/drivers/safety_button/SafetyButton.hpp
+++ b/src/drivers/safety_button/SafetyButton.hpp
@@ -84,6 +84,7 @@ private:
 	uORB::PublicationQueued<led_control_s> _to_led_control{ORB_ID(led_control)};
 	uORB::Publication<tune_control_s> _to_tune_control{ORB_ID(tune_control)};
 
+	safety_s _safety{};
 
 	uint8_t				_button_counter{0};
 	uint8_t				_blink_counter{0};