From ecfe06b9e4973617d68ca5cc69f47cc59e6e029c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 11 May 2018 11:43:28 +1000
Subject: [PATCH] AP_IOMCU: implement safety LED and switch in iofirmware

---
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 36 ++++++++++++++++++++
 libraries/AP_IOMCU/iofirmware/iofirmware.h   |  4 +++
 2 files changed, 40 insertions(+)

diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 4481f57a71..8d900f033f 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -139,6 +139,7 @@ void AP_IOMCU_FW::update()
     pwm_out_update();
     heater_update();
     rcin_update();
+    safety_update();
 }
 
 void AP_IOMCU_FW::pwm_out_update()
@@ -416,4 +417,39 @@ void AP_IOMCU_FW::calculate_fw_crc(void)
 }
 
 
+/*
+  update safety state
+ */
+void AP_IOMCU_FW::safety_update(void)
+{
+    uint32_t now = AP_HAL::millis();
+    if (now - safety_update_ms < 100) {
+        // update safety at 10Hz
+        return;
+    }
+    safety_update_ms = now;
+
+    bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
+    if (safety_pressed) {
+        if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) {
+            safety_pressed = false;
+        } else if ((!reg_status.flag_safety_off) && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_OFF)) {
+            safety_pressed = false;
+        }
+    }
+    if (safety_pressed) {
+        safety_button_counter++;
+    } else {
+        safety_button_counter = 0;
+    }
+    if (safety_button_counter == 10) {
+        // safety has been pressed for 1 second, change state
+        reg_status.flag_safety_off = !reg_status.flag_safety_off;
+    }
+
+    led_counter = (led_counter+1) % 16;
+    const uint16_t led_pattern = reg_status.flag_safety_off?0xFFFF:0x5500;
+    palWriteLine(HAL_GPIO_PIN_SAFETY_LED, (led_pattern & (1U << led_counter))?0:1);
+}
+
 AP_HAL_MAIN();
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index e3064c6dc4..f2c5882ba9 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -26,6 +26,7 @@ private:
     bool handle_code_write();
     bool handle_code_read();
     void schedule_reboot(uint32_t time_ms);
+    void safety_update();
 
     struct PACKED {
         /* default to RSSI ADC functionality */
@@ -87,5 +88,8 @@ private:
     bool update_rcout_freq;
     bool has_heater;
     uint32_t last_blue_led_ms;
+    uint32_t safety_update_ms;
+    uint32_t safety_button_counter;
+    uint8_t led_counter;
 };