diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 06db0a2f94..a96a21b055 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" @@ -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); +} diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 2ccaf94cfd..4f26a777f5 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -20,7 +20,6 @@ #include "AP_HAL_ChibiOS_Namespace.h" #include "AP_HAL_ChibiOS.h" #include -#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; + }; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c index d6fc8da871..3cee16947d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c +++ b/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 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 }