From 45d82887b3b927c715db017b1a84715477d9722b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 22 May 2021 07:04:43 +1000
Subject: [PATCH] AP_IOMCU: fixed a safety reset case for IOMCU reset

if IOMCU resets in flight when user had disabled the safety switch
using the button then the IOCMU force safety code was not called
---
 libraries/AP_IOMCU/AP_IOMCU.cpp | 14 ++++++++++----
 libraries/AP_IOMCU/AP_IOMCU.h   |  3 +++
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index de27451f97..5a518c6ad5 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -1038,6 +1038,7 @@ void AP_IOMCU::check_iomcu_reset(void)
 
     if (dt_ms < max_delay) {
         // all OK
+        last_safety_off = reg_status.flag_safety_off;
         return;
     }
     detected_io_reset = true;
@@ -1045,11 +1046,16 @@ void AP_IOMCU::check_iomcu_reset(void)
     hal.console->printf("IOMCU reset t=%u %u %u dt=%u\n",
                         unsigned(AP_HAL::millis()), unsigned(ts1), unsigned(reg_status.timestamp_ms), unsigned(dt_ms));
 
-    if (safety_forced_off && !reg_status.flag_safety_off && hal.util->get_soft_armed()) {
-        // IOMCU has reset while armed with safety off - force it off
-        // again so we can keep flying
-        force_safety_off();
+    if (last_safety_off && !reg_status.flag_safety_off && hal.util->get_soft_armed()) {
+        AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
+        uint16_t options = boardconfig?boardconfig->get_safety_button_options():0;
+        if (safety_forced_off || (options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) == 0) {
+            // IOMCU has reset while armed with safety off - force it off
+            // again so we can keep flying
+            force_safety_off();
+        }
     }
+    last_safety_off = reg_status.flag_safety_off;
 
     // we need to ensure the mixer data and the rates are sent over to
     // the IOMCU
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index df604a00eb..3a47e2da40 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -154,6 +154,9 @@ private:
     // have we forced the safety off?
     bool safety_forced_off;
 
+    // was safety off on last status?
+    bool last_safety_off;
+
     void send_servo_out(void);
     void read_rc_input(void);
     void read_servo(void);