From 4edd270bd91caceeebb86673db366345484ff0d6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Apr 2019 13:59:39 +1000 Subject: [PATCH] HAL_ChibiOS: added ability to restore safety state on watchdog reset # Conflicts: # libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 8 ++++- libraries/AP_HAL_ChibiOS/Util.h | 5 +++ .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c | 23 ++++++++++++ .../AP_HAL_ChibiOS/hwdef/common/stm32_util.h | 6 ++++ .../AP_HAL_ChibiOS/hwdef/common/watchdog.c | 35 +++++++++++++++++++ .../AP_HAL_ChibiOS/hwdef/common/watchdog.h | 16 +++++++++ 6 files changed, 92 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index dfefb3fdd1..f8d2abdd04 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -20,6 +20,7 @@ #include #include "GPIO.h" #include "hwdef/common/stm32_util.h" +#include "hwdef/common/watchdog.h" #if HAL_USE_PWM == TRUE @@ -1330,9 +1331,14 @@ AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void) { #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { - return iomcu.get_safety_switch_state(); + safety_state = iomcu.get_safety_switch_state(); } #endif + if (safety_state == AP_HAL::Util::SAFETY_ARMED) { + stm32_set_backup_safety_state(false); + } else if (safety_state == AP_HAL::Util::SAFETY_DISARMED) { + stm32_set_backup_safety_state(true); + } return safety_state; } diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index a4764bdc26..7870ea9c88 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -63,6 +63,11 @@ public: // return true if the reason for the reboot was a watchdog reset bool was_watchdog_reset() const override { return stm32_was_watchdog_reset(); } + + // 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; + } private: #ifdef HAL_PWM_ALARM diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c index 064a89437a..dd4c36207e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c @@ -252,6 +252,29 @@ void set_fast_reboot(enum rtc_boot_magic v) set_rtc_backup0(v); } +// get RTC backup register 1 +uint32_t get_rtc_backup1(void) +{ + return RTC->BKP1R; +} + +// set RTC backup register 1 +void set_rtc_backup1(uint32_t v) +{ + if ((RCC->BDCR & RCC_BDCR_RTCEN) == 0) { + RCC->BDCR |= STM32_RTCSEL; + RCC->BDCR |= RCC_BDCR_RTCEN; + } +#ifdef PWR_CR_DBP + PWR->CR |= PWR_CR_DBP; +#else + PWR->CR1 |= PWR_CR1_DBP; +#endif + 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/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h index 9aa6c961a8..5a2e6119e9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h @@ -79,6 +79,12 @@ void malloc_init(void); iomode_t palReadLineMode(ioline_t line); #endif +// get RTC backup register 1 +uint32_t get_rtc_backup1(void); + +// set RTC backup register 1 +void set_rtc_backup1(uint32_t v); + #ifdef __cplusplus } #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c index 8dd7918e64..3b3eeee17e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c @@ -4,6 +4,7 @@ #include "hal.h" #include "watchdog.h" +#include "stm32_util.h" #ifndef IWDG_BASE #if defined(STM32H7) @@ -53,6 +54,7 @@ typedef struct static bool was_watchdog_reset; static bool watchdog_enabled; +static bool boot_safety_state; /* setup the watchdog @@ -85,6 +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(); } } @@ -104,3 +107,35 @@ bool stm32_was_watchdog_reset(void) return was_watchdog_reset; } +/* + set the safety state in backup register + + This is stored so that the safety state can be restored after a + watchdog reset + */ +void stm32_set_backup_safety_state(bool safety_on) +{ + uint32_t v = get_rtc_backup1(); + uint32_t v2 = safety_on?(v|1):(v&~1); + 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; +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h index 489bf40ccb..3ea810ea31 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h @@ -28,6 +28,22 @@ void stm32_watchdog_save_reason(void); clear reset reason code */ void stm32_watchdog_clear_reason(void); + +/* + set the safety state in backup register + */ +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); #ifdef __cplusplus }