From dbfe6b801989692d128b693318c099c70a0b6233 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 9 May 2019 17:49:32 +1000 Subject: [PATCH] HAL_ChibiOS: implement persistent_data for watchdog this is a much simpler approach to persistent data --- .../AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 19 ++-- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 6 +- libraries/AP_HAL_ChibiOS/Util.cpp | 53 ---------- libraries/AP_HAL_ChibiOS/Util.h | 23 +---- .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c | 30 +++--- .../AP_HAL_ChibiOS/hwdef/common/stm32_util.h | 8 +- .../AP_HAL_ChibiOS/hwdef/common/watchdog.c | 98 ++----------------- .../AP_HAL_ChibiOS/hwdef/common/watchdog.h | 38 +------ 8 files changed, 50 insertions(+), 225 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 5cfc416a2e..ddbfc692c1 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -193,6 +193,11 @@ static void main_loop() schedulerInstance.hal_initialized(); + if (stm32_was_watchdog_reset()) { + // load saved watchdog data + stm32_watchdog_load((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4); + } + g_callbacks->setup(); hal.scheduler->system_initialized(); @@ -217,6 +222,8 @@ static void main_loop() stm32_watchdog_init(); #endif + uint32_t last_watchdog_save = AP_HAL::millis(); + while (true) { g_callbacks->loop(); @@ -235,14 +242,12 @@ static void main_loop() #endif stm32_watchdog_pat(); -#if 0 - // simple method to test watchdog functionality - static bool done_pause; - if (!done_pause && AP_HAL::millis() > 20000) { - done_pause = true; - while (AP_HAL::millis() < 22200) ; + uint32_t now = AP_HAL::millis(); + if (now - last_watchdog_save >= 100) { + // save persistent data every 100ms + last_watchdog_save = now; + stm32_watchdog_save((uint32_t *)&utilInstance.persistent_data, (sizeof(utilInstance.persistent_data)+3)/4); } -#endif } thread_running = false; diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index db89ba53f5..27c22b7388 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1389,10 +1389,8 @@ AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void) 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); + if (!hal.util->was_watchdog_reset()) { + hal.util->persistent_data.safety_state = safety_state; } return safety_state; } diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 499dbaaf5b..286fff50d6 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -322,56 +322,3 @@ 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); -} - -// backup home state for restore on watchdog reset -void Util::set_backup_home_state(int32_t lat, int32_t lon, int32_t alt_cm) const -{ - stm32_set_backup_home(lat, lon, alt_cm); -} - -// backup home state for restore on watchdog reset -bool Util::get_backup_home_state(int32_t &lat, int32_t &lon, int32_t &alt_cm) const -{ - if (was_watchdog_reset()) { - stm32_get_backup_home(&lat, &lon, &alt_cm); - return true; - } - return false; -} - -// backup atttude for restore on watchdog reset -void Util::set_backup_attitude(int32_t roll_cd, int32_t pitch_cd, int32_t yaw_cd) const -{ - stm32_set_attitude(roll_cd, pitch_cd, yaw_cd); -} - -// get watchdog reset attitude -bool Util::get_backup_attitude(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_cd) const -{ - if (was_watchdog_reset()) { - stm32_get_attitude(&roll_cd, &pitch_cd, &yaw_cd); - return true; - } - return false; -} diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 8595ddfe1d..b86ba417c7 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -68,24 +68,6 @@ public: // return true if the reason for the reboot was a 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 true if vehicle was armed and this was a watchdog reset - bool was_watchdog_armed() const override; - - // backup home state for restore on watchdog reset - void set_backup_home_state(int32_t lat, int32_t lon, int32_t alt_cm) const override; - - // backup home state for restore on watchdog reset - bool get_backup_home_state(int32_t &lat, int32_t &lon, int32_t &alt_cm) const override; - - // backup atttude for restore on watchdog reset - void set_backup_attitude(int32_t roll_cd, int32_t pitch_cd, int32_t yaw_cd) const override; - - // get watchdog reset attitude - bool get_backup_attitude(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_cd) const override; - private: #ifdef HAL_PWM_ALARM struct ToneAlarmPwmGroup { @@ -126,6 +108,7 @@ private: static memory_heap_t scripting_heap; #endif // ENABLE_HEAP - void set_soft_armed(const bool b) override; - + // stm32F4 and F7 have 20 total RTC backup registers. We use the first one for boot type + // flags, so 19 available for persistent data + static_assert(sizeof(persistent_data) <= 19*4, "watchdog persistent data too large"); }; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c index d419fb4d68..bce91867a3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c @@ -211,14 +211,16 @@ uint32_t get_fattime() #if !defined(NO_FASTBOOT) -// get RTC backup register -uint32_t get_rtc_backup(uint8_t n) +// get RTC backup registers starting at given idx +void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n) { - return ((__IO uint32_t *)&RTC->BKP0R)[n]; + while (n--) { + *v++ = ((__IO uint32_t *)&RTC->BKP0R)[idx++]; + } } -// set RTC backup register 0 -void set_rtc_backup(uint8_t n, uint32_t v) +// set n RTC backup registers starting at given idx +void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n) { if ((RCC->BDCR & RCC_BDCR_RTCEN) == 0) { RCC->BDCR |= STM32_RTCSEL; @@ -229,29 +231,35 @@ void set_rtc_backup(uint8_t n, uint32_t v) #else PWR->CR1 |= PWR_CR1_DBP; #endif - ((__IO uint32_t *)&RTC->BKP0R)[n] = v; + while (n--) { + ((__IO uint32_t *)&RTC->BKP0R)[idx++] = *v++; + } } // see if RTC registers is setup for a fast reboot enum rtc_boot_magic check_fast_reboot(void) { - return (enum rtc_boot_magic)get_rtc_backup(0); + uint32_t v; + get_rtc_backup(0, &v, 1); + return (enum rtc_boot_magic)v; } // set RTC register for a fast reboot void set_fast_reboot(enum rtc_boot_magic v) { - set_rtc_backup(0, v); + uint32_t vv = (uint32_t)v; + set_rtc_backup(0, &vv, 1); } #else // NO_FASTBOOT -// set RTC backup register 1 -void set_rtc_backup(uint8_t n, uint32_t v) +// set n RTC backup registers starting at given idx +void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n) { } -uint32_t get_rtc_backup(uint8_t n) +// get RTC backup registers starting at given idx +void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n) { return 0; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h index 622314e1c1..623d66424a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h @@ -80,11 +80,11 @@ void malloc_init(void); iomode_t palReadLineMode(ioline_t line); #endif -// get RTC backup register -uint32_t get_rtc_backup(uint8_t n); +// set n RTC backup registers starting at given idx +void set_rtc_backup(uint8_t idx, const uint32_t *v, uint8_t n); -// set RTC backup register -void set_rtc_backup(uint8_t n, uint32_t v); +// get RTC backup registers starting at given idx +void get_rtc_backup(uint8_t idx, uint32_t *v, uint8_t n); #ifdef __cplusplus } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c index 6a96f547e4..b73d5bb5a4 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c @@ -41,13 +41,6 @@ #error "Unsupported IWDG MCU config" #endif -#define WDG_SAFETY_BIT 0x01 -#define WDG_ARMED_BIT 0x02 - -#define BKP_IDX_FLAGS 0x01 -#define BKP_IDX_HOME 0x02 -#define BKP_IDX_RPY 0x05 - typedef struct { __IO uint32_t KR; /*!< IWDG Key register, Address offset: 0x00 */ @@ -61,7 +54,6 @@ typedef struct static bool was_watchdog_reset; static bool watchdog_enabled; -static uint32_t boot_backup_state[8]; /* setup the watchdog @@ -94,10 +86,6 @@ void stm32_watchdog_save_reason(void) { if (WDG_RESET_STATUS & WDG_RESET_IS_IWDG) { was_watchdog_reset = true; - uint8_t i; - for (i=0; i