From ab0768871b6dc636be6688a81d49d97b9a957e0f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 11 Feb 2023 12:40:26 +1100
Subject: [PATCH] AP_IOMCU: add internal errors for unresponsive IOMCU

if IOMCU stops responding completely or stops giving status update
then give an internal error to help with diagnostics
---
 libraries/AP_IOMCU/AP_IOMCU.cpp | 15 +++++++++++++++
 libraries/AP_IOMCU/AP_IOMCU.h   |  1 +
 2 files changed, 16 insertions(+)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index d143be8f07..14aee80e73 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -110,6 +110,13 @@ void AP_IOMCU::thread_main(void)
     trigger_event(IOEVENT_INIT);
 
     while (!do_shutdown) {
+        // check if we have lost contact with the IOMCU
+        const uint32_t now_ms = AP_HAL::millis();
+        if (last_reg_read_ms != 0 && now_ms - last_reg_read_ms > 1000U) {
+            INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset);
+            last_reg_read_ms = 0;
+        }
+
         eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(10));
 
         // check for pending IO events
@@ -323,6 +330,10 @@ void AP_IOMCU::read_status()
     uint16_t *r = (uint16_t *)&reg_status;
     if (!read_registers(PAGE_STATUS, 0, sizeof(reg_status)/2, r)) {
         read_status_errors++;
+        if (read_status_errors == 20 && last_iocmu_timestamp_ms != 0) {
+            // the IOMCU has stopped responding to status requests
+            INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset);
+        }
         return;
     }
     if (read_status_ok == 0) {
@@ -539,6 +550,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
     total_errors += protocol_fail_count;
     protocol_fail_count = 0;
     protocol_count++;
+    last_reg_read_ms = AP_HAL::millis();
     return true;
 }
 
@@ -852,6 +864,9 @@ bool AP_IOMCU::check_crc(void)
     const uint16_t magic = REBOOT_BL_MAGIC;
     write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);
 
+    // avoid internal error on fw upload delay
+    last_reg_read_ms = 0;
+
     if (!upload_fw()) {
         AP_ROMFS::free(fw);
         fw = nullptr;
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index d792731f9a..8e508ebebb 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -161,6 +161,7 @@ private:
     uint32_t last_rc_read_ms;
     uint32_t last_servo_read_ms;
     uint32_t last_safety_option_check_ms;
+    uint32_t last_reg_read_ms;
 
     // last value of safety options
     uint16_t last_safety_options = 0xFFFF;