From c7be7d7343ff34d898a0108e91c1e796d9b223ae Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 22 Apr 2019 12:54:00 +1000
Subject: [PATCH] AP_IOMCU: cope with IOMCU reset

---
 libraries/AP_IOMCU/AP_IOMCU.cpp | 26 +++++++++++++++++++++++++-
 libraries/AP_IOMCU/AP_IOMCU.h   |  2 ++
 2 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index 406a999e84..6c881fc821 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -17,6 +17,7 @@
 #include <SRV_Channel/SRV_Channel.h>
 #include <RC_Channel/RC_Channel.h>
 #include <AP_RCProtocol/AP_RCProtocol.h>
+#include <AP_InternalError/AP_InternalError.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -420,6 +421,9 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
         return false;
     }
     memcpy(regs, pkt.regs, count*2);
+    if (protocol_fail_count > 3) {
+        handle_repeated_failures();
+    }
     protocol_fail_count = 0;
     return true;
 }
@@ -485,6 +489,9 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons
         protocol_fail_count++;
         return false;
     }
+    if (protocol_fail_count > 3) {
+        handle_repeated_failures();
+    }
     protocol_fail_count = 0;
     return true;
 }
@@ -786,7 +793,7 @@ void AP_IOMCU::set_safety_mask(uint16_t chmask)
  */
 bool AP_IOMCU::healthy(void)
 {
-    return crc_is_ok && protocol_fail_count == 0;
+    return crc_is_ok && protocol_fail_count == 0 && !detected_io_reset;
 }
 
 /*
@@ -884,4 +891,21 @@ const char *AP_IOMCU::get_rc_protocol(void)
     return AP_RCProtocol::protocol_name_from_protocol((AP_RCProtocol::rcprotocol_t)rc_input.data);
 }
 
+/*
+  we have had a series of repeated protocol failures to the
+  IOMCU. This may indicate that the IOMCU has been reset (possibly due
+  to a watchdog).
+ */
+void AP_IOMCU::handle_repeated_failures(void)
+{
+    detected_io_reset = true;
+    AP::internalerror().error(AP_InternalError::error_t::iomcu_reset);
+    // we need to ensure the mixer data and the rates are sent over to
+    // the IOMCU
+    if (mixing.enabled) {
+        trigger_event(IOEVENT_MIXING);
+    }
+    trigger_event(IOEVENT_SET_RATES);
+}
+
 #endif // HAL_WITH_IO_MCU
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index 57df6a2296..7e03b16960 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -200,6 +200,7 @@ private:
     bool done_shutdown;
 
     bool crc_is_ok;
+    bool detected_io_reset;
     bool initialised;
     bool is_chibios_backend;
 
@@ -226,6 +227,7 @@ private:
     bool reboot();
 
     bool check_crc(void);
+    void handle_repeated_failures();
     
     enum {
         PROTO_NOP               = 0x00,