From 068208e4d716ba89e7a6df0dd424e492d6368068 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Tue, 8 Mar 2022 14:21:27 +0530
Subject: [PATCH] AP_Periph: add debug option for hold in bootloader after 15s

---
 Tools/AP_Periph/AP_Periph.cpp  | 13 +++++++++++--
 Tools/AP_Periph/AP_Periph.h    |  5 +++++
 Tools/AP_Periph/Parameters.cpp |  3 ++-
 3 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 8364ec59fb..2ff9ff446e 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -384,12 +384,21 @@ void AP_Periph_FW::update()
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE
     static uint32_t last_debug_ms;
-    if (g.debug==1 && now - last_debug_ms > 5000) {
+    if ((g.debug&(1<<DEBUG_SHOW_STACK)) && now - last_debug_ms > 5000) {
         last_debug_ms = now;
         show_stack_free();
     }
 #endif
-    
+
+    if ((g.debug&(1<<DEBUG_AUTOREBOOT)) && AP_HAL::millis() > 15000) {
+        // attempt reboot with HOLD after 15s
+        periph.prepare_reboot();
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+        set_fast_reboot((rtc_boot_magic)(RTC_BOOT_HOLD));
+        NVIC_SystemReset();
+#endif
+    }
+
 #ifdef HAL_PERIPH_ENABLE_BATTERY
     if (now - battery.last_read_ms >= 100) {
         // update battery at 10Hz
diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 77262b1634..7454b5ef74 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -247,6 +247,11 @@ public:
 
     static AP_Periph_FW *_singleton;
 
+    enum {
+        DEBUG_SHOW_STACK,
+        DEBUG_AUTOREBOOT
+    };
+
     // show stack as DEBUG msgs
     void show_stack_free();
 
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 2fe378093c..a023723043 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -136,10 +136,11 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Param: DEBUG
     // @DisplayName: Debug
     // @Description: Debug
-    // @Values: 0:Disabled, 1:Show free stack space
+    // @Bitmask: 0:Disabled, 1:Show free stack space, 2:Auto Reboot after 15sec
     // @User: Advanced
     GSCALAR(debug, "DEBUG", 0),
 
+
     // @Param: BRD_SERIAL_NUM
     // @DisplayName: Serial number of device
     // @Description: Non-zero positive values will be shown on the CAN App Name string