HAL_ChibiOS: implement was_watchdog_armed()

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -21,6 +21,7 @@
 #include <ch.h>
 #include "RCOutput.h"
 #include "hwdef/common/stm32_util.h"
+#include "hwdef/common/watchdog.h"
 #include "hwdef/common/flash.h"
 #include <AP_ROMFS/AP_ROMFS.h>
 #include "sdcard.h"
@@ -310,3 +311,30 @@ bool Util::fs_init(void)
     return sdcard_retry();
 }
 #endif
+
+// return true if the reason for the reboot was a watchdog reset
+bool Util::was_watchdog_reset() const
+{
+    return stm32_was_watchdog_reset();
+}
+
+// return true if safety was off and this was a watchdog reset
+bool Util::was_watchdog_safety_off() const
+{
+    return stm32_was_watchdog_reset() && stm32_get_boot_backup_safety_state() == false;
+}
+
+// return true if vehicle was armed and this was a watchdog reset
+bool Util::was_watchdog_armed() const
+{
+    return stm32_was_watchdog_reset() && stm32_get_boot_backup_armed() == true;
+}
+
+/*
+  change armed state
+ */
+void Util::set_soft_armed(const bool b)
+{
+    AP_HAL::Util::set_soft_armed(b);
+    stm32_set_backup_armed(b);
+}

libraries/AP_HAL_ChibiOS/Util.h

@@ -20,7 +20,6 @@
 #include "AP_HAL_ChibiOS_Namespace.h"
 #include "AP_HAL_ChibiOS.h"
 #include <ch.h>
-#include "hwdef/common/watchdog.h"
 
 class ChibiOS::Util : public AP_HAL::Util {
 public:
@@ -67,13 +66,14 @@ public:
 #endif
 
     // return true if the reason for the reboot was a watchdog reset
-    bool was_watchdog_reset() const override { return stm32_was_watchdog_reset(); }
+    bool was_watchdog_reset() const override;
 
     // return true if safety was off and this was a watchdog reset
-    bool was_watchdog_safety_off() const override {
-        return stm32_was_watchdog_reset() && stm32_get_boot_backup_safety_state() == false;
-    }
-    
+    bool was_watchdog_safety_off() const override;
+
+    // return true if vehicle was armed and this was a watchdog reset
+    bool was_watchdog_armed() const override;
+
 private:
 #ifdef HAL_PWM_ALARM
     struct ToneAlarmPwmGroup {
@@ -114,4 +114,6 @@ private:
     static memory_heap_t scripting_heap;
 #endif // ENABLE_HEAP
 
+    void set_soft_armed(const bool b) override;
+
 };

libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c

@@ -264,7 +264,18 @@ void set_rtc_backup1(uint32_t v)
     RTC->BKP1R = v;
 }
 
-#endif //NO_FASTBOOT
+#else // NO_FASTBOOT
+
+// set RTC backup register 1
+void set_rtc_backup1(uint32_t v)
+{
+}
+
+uint32_t get_rtc_backup1(void)
+{
+    return 0;
+}
+#endif // NO_FASTBOOT
 
 /*
   enable peripheral power if needed This is done late to prevent

libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c

@@ -54,7 +54,7 @@ typedef struct
 
 static bool was_watchdog_reset;
 static bool watchdog_enabled;
-static bool boot_safety_state;
+static uint32_t boot_backup1_state;
 
 /*
   setup the watchdog
@@ -87,7 +87,7 @@ void stm32_watchdog_save_reason(void)
 {
     if (WDG_RESET_STATUS & WDG_RESET_IS_IWDG) {
         was_watchdog_reset = true;
-        boot_safety_state = stm32_get_backup_safety_state();
+        boot_backup1_state = get_rtc_backup1();
     }
 }
 
@@ -97,6 +97,7 @@ void stm32_watchdog_save_reason(void)
 void stm32_watchdog_clear_reason(void)
 {
     WDG_RESET_STATUS = WDG_RESET_CLEAR;
+    set_rtc_backup1(0);
 }
 
 /*
@@ -107,6 +108,9 @@ bool stm32_was_watchdog_reset(void)
     return was_watchdog_reset;
 }
 
+#define WDG_SAFETY_BIT  0x01
+#define WDG_ARMED_BIT   0x02
+
 /*
   set the safety state in backup register
 
@@ -116,26 +120,39 @@ bool stm32_was_watchdog_reset(void)
 void stm32_set_backup_safety_state(bool safety_on)
 {
     uint32_t v = get_rtc_backup1();
-    uint32_t v2 = safety_on?(v|1):(v&~1);
+    uint32_t v2 = safety_on?(v | WDG_SAFETY_BIT):(v & ~WDG_SAFETY_BIT);
     if (v != v2) {
         set_rtc_backup1(v2);
     }
 }
 
-/*
-  get the safety state in backup register
-  return true if safety is marked as safety on
-*/
-bool stm32_get_backup_safety_state(void)
-{
-    uint32_t v = get_rtc_backup1();
-    return (v&1) != 0;
-}
-
 /*
   get the safety state in backup register from initial boot
 */
 bool stm32_get_boot_backup_safety_state(void)
 {
-    return boot_safety_state;
+    return (boot_backup1_state & WDG_SAFETY_BIT) != 0;
+}
+
+/*
+  set the armed state in backup register
+
+  This is stored so that the armed state can be restored after a
+  watchdog reset
+ */
+void stm32_set_backup_armed(bool armed)
+{
+    uint32_t v = get_rtc_backup1();
+    uint32_t v2 = armed?(v | WDG_ARMED_BIT): (v & ~WDG_ARMED_BIT);
+    if (v != v2) {
+        set_rtc_backup1(v2);
+    }
+}
+
+/*
+  get the armed state in backup register from initial boot
+*/
+bool stm32_get_boot_backup_armed(void)
+{
+    return (boot_backup1_state & WDG_ARMED_BIT) != 0;
 }

libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h

@@ -34,16 +34,20 @@ void stm32_watchdog_clear_reason(void);
  */
 void stm32_set_backup_safety_state(bool safety_on);
 
-/*
-  get the safety state in backup register
-  return true if safety is marked as safety on
- */
-bool stm32_get_backup_safety_state(void);
-    
 /*
   get the safety state in backup register from initial boot
 */
 bool stm32_get_boot_backup_safety_state(void);
+
+/*
+  set the armed state in backup register
+ */
+void stm32_set_backup_armed(bool armed);
+
+/*
+  get the armed state in backup register from initial boot
+*/
+bool stm32_get_boot_backup_armed(void);
     
 #ifdef __cplusplus
 }