From 482e0d47873803b0ae3146431e9cb5cd1c86a8c8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 26 Oct 2019 12:46:22 +1100
Subject: [PATCH] AP_Periph: implement 30s mark of good firmware

---
 Tools/AP_Periph/AP_Periph.cpp | 27 ++++++++++++++++++++++++++-
 Tools/AP_Periph/AP_Periph.h   | 11 +----------
 Tools/AP_Periph/can.cpp       | 16 ++++++++++++++++
 3 files changed, 43 insertions(+), 11 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 84381a932f..07f4aedf8c 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -25,6 +25,7 @@
 #include "hal.h"
 #include <stdio.h>
 #include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
+#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -47,18 +48,42 @@ void loop(void)
 
 static uint32_t start_ms;
 
-const struct app_descriptor app_descriptor __attribute__((section(".app_descriptor")));;
+/*
+  declare constant app_descriptor in flash
+ */
+const struct app_descriptor app_descriptor __attribute__((section(".app_descriptor"))) = {
+    .sig = { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 },
+    .image_crc1 = 0,
+    .image_crc2 = 0,
+    .image_size = 0,
+    .git_hash = 0,
+    .version_major = AP::fwversion().major,
+    .version_minor = AP::fwversion().minor,
+    .board_id = APJ_BOARD_ID,
+    .reserved = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
+};
 
 void AP_Periph_FW::init()
 {
+    // always run with watchdog enabled. This should have already been
+    // setup by the bootloader, but if not then enable now
+    stm32_watchdog_init();
+
+    stm32_watchdog_pat();
+
     hal.uartA->begin(AP_SERIALMANAGER_CONSOLE_BAUD, 32, 128);
     hal.uartB->begin(115200, 32, 128);
 
     load_parameters();
+
+    stm32_watchdog_pat();
+
     can_start();
 
     serial_manager.init();
 
+    stm32_watchdog_pat();
+
 #ifdef HAL_BOARD_AP_PERIPH_ZUBAXGNSS
     // setup remapping register for ZubaxGNSS
     uint32_t mapr = AFIO->MAPR;
diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index fa696a2c10..7518f709b9 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -6,6 +6,7 @@
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <AP_Common/AP_FWVersion.h>
+#include "../AP_Bootloader/app_comms.h"
 
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED)
 #define AP_PERIPH_HAVE_LED
@@ -17,16 +18,6 @@
 /*
   app descriptor compatible with MissionPlanner
  */
-struct app_descriptor {
-    uint8_t sig[8] = { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 };
-    uint32_t image_crc1 = 0;
-    uint32_t image_crc2 = 0;
-    uint32_t image_size = 0;
-    uint32_t git_hash = 0;
-    uint8_t  version_major = AP::fwversion().major;
-    uint8_t version_minor = AP::fwversion().minor;
-    uint8_t reserved[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-};
 extern const struct app_descriptor app_descriptor;
 
 class AP_Periph_FW {
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index aa3f422870..59a0f4f933 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -853,6 +853,20 @@ static void process1HzTasks(uint64_t timestamp_usec)
 #endif
 
     node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
+
+#if 0
+    // test code for watchdog reset
+    if (AP_HAL::millis() > 15000) {
+        while (true) ;
+    }
+#endif
+
+    if (AP_HAL::millis() > 30000) {
+        // use RTC to mark that we have been running fine for
+        // 30s. This is used along with watchdog resets to ensure the
+        // user has a chance to load a fixed firmware
+        set_fast_reboot(RTC_BOOT_FWOK);
+    }
 }
 
 /*
@@ -866,6 +880,8 @@ static void can_wait_node_id(void)
     {
         printf("Waiting for dynamic node ID allocation... (pool %u)\n", pool_peak_percent());
 
+        stm32_watchdog_pat();
+
         send_next_node_id_allocation_request_at_ms =
             AP_HAL::millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
             (uint32_t)(getRandomFloat() * UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);