From f5e170c76b58ef52186de1bff6a2a3905db4e976 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Apr 2019 14:01:25 +1000 Subject: [PATCH] HAL_ChibiOS: implement was_watchdog_armed() --- libraries/AP_HAL_ChibiOS/Util.cpp | 28 ++++++++++++ libraries/AP_HAL_ChibiOS/Util.h | 13 +++--- .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c | 2 - .../AP_HAL_ChibiOS/hwdef/common/watchdog.c | 45 +++++++++++++------ .../AP_HAL_ChibiOS/hwdef/common/watchdog.h | 16 ++++--- 5 files changed, 76 insertions(+), 28 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 94cc8ad990..76897186f7 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -21,6 +21,7 @@ #include #include "RCOutput.h" #include "hwdef/common/stm32_util.h" +#include "hwdef/common/watchdog.h" #include "hwdef/common/flash.h" #include #include "sdcard.h" @@ -261,3 +262,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); +} diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 7870ea9c88..de63cd71d4 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -21,7 +21,6 @@ #include "Semaphores.h" #include "AP_HAL_ChibiOS.h" #include -#include "hwdef/common/watchdog.h" class ChibiOS::Util : public AP_HAL::Util { public: @@ -62,13 +61,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 { @@ -103,4 +103,5 @@ private: uint64_t get_hw_rtc() const override; bool flash_bootloader() override; + void set_soft_armed(const bool b) override; }; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c index dd4c36207e..a91d389f6e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c @@ -273,8 +273,6 @@ void set_rtc_backup1(uint32_t v) RTC->BKP1R = v; } -#endif //NO_FASTBOOT - /* enable peripheral power if needed This is done late to prevent problems with CTS causing SiK radios to stay in the bootloader. A diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c index 3b3eeee17e..a5320c5aab 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c +++ b/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; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h index 3ea810ea31..390d9e1684 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h +++ b/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 }