From f22964a350f563465921349e2a94e4c0161a9b32 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 13 Apr 2018 16:17:08 +1000
Subject: [PATCH] AP_IOMCU: implement BRD_SAFETYOPTION

---
 libraries/AP_IOMCU/AP_IOMCU.cpp | 36 +++++++++++++++++++++++++++++++++
 libraries/AP_IOMCU/AP_IOMCU.h   |  5 +++++
 2 files changed, 41 insertions(+)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index 1e0449e8af..4d437a16f8 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -11,6 +11,7 @@
 
 #include <AP_Math/AP_Math.h>
 #include <AP_Math/crc.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -86,6 +87,8 @@ enum ioevents {
 #define P_SETUP_ARMING_IO_ARM_OK (1<<0)
 #define P_SETUP_ARMING_FMU_ARMED (1<<1)
 #define P_SETUP_ARMING_RC_HANDLING_DISABLED (1<<6)
+#define P_SETUP_ARMING_SAFETY_DISABLE_ON	(1 << 11) // disable use of safety button for safety off->on
+#define P_SETUP_ARMING_SAFETY_DISABLE_OFF	(1 << 12) // disable use of safety button for safety on->off
 
 #define PAGE_REG_SETUP_PWM_RATE_MASK 2
 #define PAGE_REG_SETUP_DEFAULTRATE   3
@@ -246,6 +249,11 @@ void AP_IOMCU::thread_main(void)
             print_debug();
             last_debug_ms = AP_HAL::millis();
         }
+
+        if (now - last_safety_option_check_ms > 1000) {
+            update_safety_options();
+            last_safety_option_check_ms = now;
+        }
     }
 }
 
@@ -573,4 +581,32 @@ void AP_IOMCU::set_oneshot_mode(void)
     trigger_event(IOEVENT_SET_ONESHOT_ON);
 }
 
+// handling of BRD_SAFETYOPTION parameter
+void AP_IOMCU::update_safety_options(void)
+{
+    AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance();
+    if (!boardconfig) {
+        return;
+    }
+    uint16_t desired_options = 0;
+    uint16_t options = boardconfig->get_safety_button_options();
+    if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
+        desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_OFF;
+    }
+    if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
+        desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_ON;
+    }
+    if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && hal.util->get_soft_armed()) {
+        desired_options |= (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF);
+    }
+    if (last_safety_options != desired_options) {
+        uint16_t mask = (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF);
+		uint32_t bits_to_set = desired_options & mask;
+		uint32_t bits_to_clear = (~desired_options) & mask;
+        if (modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, bits_to_clear, bits_to_set)) {
+            last_safety_options = desired_options;
+        }
+    }
+}
+
 #endif // HAL_WITH_IO_MCU
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index 89148f1e58..f85e2cc786 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -106,6 +106,10 @@ private:
     uint32_t last_rc_read_ms;
     uint32_t last_servo_read_ms;
     uint32_t last_debug_ms;
+    uint32_t last_safety_option_check_ms;
+
+    // last value of safety options
+    uint16_t last_safety_options = 0xFFFF;
 
     void send_servo_out(void);
     void read_rc_input(void);
@@ -114,6 +118,7 @@ private:
     void print_debug(void);
     void discard_input(void);
     void event_failed(uint8_t event);
+    void update_safety_options(void);
     
     // PAGE_STATUS values
     struct PACKED {