From ccea9c9e6f3db3fe414d27d350145dcbaa023db3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 1 Aug 2016 11:18:36 +0200
Subject: [PATCH] fmu: flash safety button correctly even disabled (#5189)

If the circuit breaker to disable IO safety is engaged, the safety
button should still blink in the appropriate pattern: double flash for
safety off, solid on for armed.
---
 src/drivers/px4fmu/fmu.cpp | 16 +++++++++++++---
 1 file changed, 13 insertions(+), 3 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 691f31d971..abb3a36898 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -276,6 +276,7 @@ private:
 	void rc_io_invert();
 	void rc_io_invert(bool invert);
 	void safety_check_button(void);
+	void flash_safety_button(void);
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] =	BOARD_FMU_GPIO_TAB;
@@ -465,7 +466,15 @@ PX4FMU:: safety_check_button(void)
 		counter = 0;
 	}
 
-	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
+#endif
+}
+
+void
+PX4FMU::flash_safety_button()
+{
+#ifdef GPIO_BTN_SAFETY
+
+	/* Select the appropriate LED flash pattern depending on the current arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
 	/* cycle the blink state machine at 10Hz */
@@ -1083,8 +1092,6 @@ PX4FMU::cycle()
 		struct safety_s safety = {};
 
 		if (_safety_disabled) {
-			/* safety switch disabled, turn LED on solid */
-			px4_arch_gpiowrite(GPIO_LED_SAFETY, 0);
 			_safety_off = true;
 
 		} else {
@@ -1092,6 +1099,9 @@ PX4FMU::cycle()
 			safety_check_button();
 		}
 
+		/* Make the safety button flash anyway, no matter if it's used or not. */
+		flash_safety_button();
+
 		safety.timestamp = hrt_absolute_time();
 
 		if (_safety_off) {