From 65fadfa2fe224d31dca8d0cc3e8e78ca723b4422 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 11 Feb 2023 10:44:09 +1100
Subject: [PATCH] AP_IOMCU: fixed an issue with double reset of IOMCU

if the IOMCU resets twice in quick succession then the code that
restores the safety state while flying can fail, leading to the
aircraft trying to continue flying with safety on

This results from two issues:

- a race in handling the last_safety_off variable
- the fact that plane sets the soft_armed state based on safety state
---
 libraries/AP_IOMCU/AP_IOMCU.cpp | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index a7fefc12e0..d143be8f07 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -18,6 +18,7 @@
 #include <AP_RCProtocol/AP_RCProtocol.h>
 #include <AP_InternalError/AP_InternalError.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_Arming/AP_Arming.h>
 #include <ch.h>
 
 extern const AP_HAL::HAL &hal;
@@ -785,7 +786,7 @@ void AP_IOMCU::update_safety_options(void)
     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()) {
+    if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && AP::arming().is_armed()) {
         desired_options |= (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF);
     }
     if (last_safety_options != desired_options) {
@@ -915,7 +916,7 @@ void AP_IOMCU::shutdown(void)
  */
 void AP_IOMCU::bind_dsm(uint8_t mode)
 {
-    if (!is_chibios_backend || hal.util->get_soft_armed()) {
+    if (!is_chibios_backend || AP::arming().is_armed()) {
         // only with ChibiOS IO firmware, and disarmed
         return;
     }
@@ -1021,7 +1022,9 @@ void AP_IOMCU::check_iomcu_reset(void)
         return;
     }
     uint32_t dt_ms = reg_status.timestamp_ms - last_iocmu_timestamp_ms;
-    uint32_t ts1 = last_iocmu_timestamp_ms;
+#if IOMCU_DEBUG_ENABLE
+    const uint32_t ts1 = last_iocmu_timestamp_ms;
+#endif
     // when we are in an expected delay allow for a larger time
     // delta. This copes with flash erase, such as bootloader update
     const uint32_t max_delay = hal.scheduler->in_expected_delay()?5000:500;
@@ -1034,19 +1037,23 @@ void AP_IOMCU::check_iomcu_reset(void)
     }
     detected_io_reset = true;
     INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset);
-    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));
+    debug("IOMCU reset t=%u %u %u dt=%u\n",
+          unsigned(AP_HAL::millis()), unsigned(ts1), unsigned(reg_status.timestamp_ms), unsigned(dt_ms));
 
-    if (last_safety_off && !reg_status.flag_safety_off && hal.util->get_soft_armed()) {
+    bool have_forced_off = false;
+    if (last_safety_off && !reg_status.flag_safety_off && AP::arming().is_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
+            have_forced_off = true;
             force_safety_off();
         }
     }
-    last_safety_off = reg_status.flag_safety_off;
+    if (!have_forced_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