From 986a1eb72f1670a915f1915ec2b947067e830a30 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 4 Sep 2018 12:16:19 +1000
Subject: [PATCH] AP_IOMCU: cope with IOMCU reset in flight

this copes with IOMCU reset when we have safety forced off
---
 libraries/AP_IOMCU/AP_IOMCU.cpp | 22 ++++++++++++++++++++++
 libraries/AP_IOMCU/AP_IOMCU.h   |  3 +++
 2 files changed, 25 insertions(+)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index f7c0797e9f..9c0fc62cec 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -323,6 +323,26 @@ void AP_IOMCU::read_status()
 {
     uint16_t *r = (uint16_t *)&reg_status;
     read_registers(PAGE_STATUS, 0, sizeof(reg_status)/2, r);
+
+    if (reg_status.flag_safety_off == 0) {
+        // if the IOMCU is indicating that safety is on, then force a
+        // re-check of the safety options. This copes with a IOMCU reset
+        last_safety_options = 0xFFFF;
+
+        // also check if the safety should be definately off.
+        AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance();
+        if (!boardconfig) {
+            return;
+        }
+        uint16_t options = boardconfig->get_safety_button_options();
+        if (safety_forced_off && (options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON) == 0) {
+            // the safety has been forced off, and the user has asked
+            // that the button can never be used, so there should be
+            // no way for the safety to be on except a IOMCU
+            // reboot. Force safety off again
+            force_safety_off();
+        }
+    }
 }
 
 /*
@@ -519,6 +539,7 @@ AP_HAL::Util::safety_state AP_IOMCU::get_safety_switch_state(void) const
 bool AP_IOMCU::force_safety_on(void)
 {
     trigger_event(IOEVENT_FORCE_SAFETY_ON);
+    safety_forced_off = false;
     return true;
 }
 
@@ -526,6 +547,7 @@ bool AP_IOMCU::force_safety_on(void)
 void AP_IOMCU::force_safety_off(void)
 {
     trigger_event(IOEVENT_FORCE_SAFETY_OFF);
+    safety_forced_off = true;
 }
 
 // read from one channel
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index 1216404909..11058c4779 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -119,6 +119,9 @@ private:
     // last value of safety options
     uint16_t last_safety_options = 0xFFFF;
 
+    // have we forced the safety off?
+    bool safety_forced_off;
+
     void send_servo_out(void);
     void read_rc_input(void);
     void read_servo(void);