From d4c68da76e1f999a519d717f94a0cd11655ee899 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 24 Apr 2019 11:33:42 +1000
Subject: [PATCH] AP_IOMCU: added IOMCU time since boot to protocol

and optional watchdog testing using safety switch
(compile time option)
---
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 21 ++++++++++++++++++++
 libraries/AP_IOMCU/iofirmware/ioprotocol.h   |  3 +--
 2 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index afeb72c429..85c33c84ba 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -25,6 +25,7 @@
 #include <AP_HAL_ChibiOS/RCOutput.h>
 #include "analog.h"
 #include "rc.h"
+#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -40,6 +41,9 @@ void loop();
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
+// enable testing of IOMCU watchdog using safety switch
+#define IOMCU_ENABLE_WATCHDOG_TEST 0
+
 // pending events on the main thread
 enum ioevents {
     IOEVENT_PWM=1,
@@ -207,6 +211,7 @@ void AP_IOMCU_FW::update()
     }
 
     uint32_t now = last_ms;
+    reg_status.timestamp_ms = last_ms;
 
     // output SBUS if enabled
     if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) &&
@@ -672,6 +677,22 @@ void AP_IOMCU_FW::safety_update(void)
         }
     }
 
+#if IOMCU_ENABLE_WATCHDOG_TEST
+    if (safety_button_counter == 50) {
+        // deliberate lockup of IOMCU on 5s button press, for testing
+        // watchdog
+        while (true) {
+            hal.scheduler->delay(50);
+            palToggleLine(HAL_GPIO_PIN_SAFETY_LED);
+            if (palReadLine(HAL_GPIO_PIN_SAFETY_INPUT)) {
+                // only trigger watchdog on button release, so we
+                // don't end up stuck in the bootloader
+                stm32_watchdog_pat();
+            }
+        }
+    }
+#endif
+
     led_counter = (led_counter+1) % 16;
     const uint16_t led_pattern = reg_status.flag_safety_off?0xFFFF:0x5500;
     palWriteLine(HAL_GPIO_PIN_SAFETY_LED, (led_pattern & (1U << led_counter))?0:1);
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index df31795aa4..dd50bfe358 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -125,8 +125,7 @@ struct PACKED page_reg_status {
     uint16_t flag_rc_sumd_srxl:1;
 
     uint16_t alarms;
-    uint16_t vbatt;
-    uint16_t ibatt;
+    uint32_t timestamp_ms;
     uint16_t vservo;
     uint16_t vrssi;
     uint16_t prssi;