HAL_ChibiOS: added ability to restore safety state on watchdog reset

# Conflicts:
#	libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
This commit is contained in:
Andrew Tridgell 2019-04-20 13:59:39 +10:00 committed by Randy Mackay
parent 92f12deb51
commit 4edd270bd9
6 changed files with 92 additions and 1 deletions

View File

@ -20,6 +20,7 @@
#include <AP_HAL/utility/RingBuffer.h> #include <AP_HAL/utility/RingBuffer.h>
#include "GPIO.h" #include "GPIO.h"
#include "hwdef/common/stm32_util.h" #include "hwdef/common/stm32_util.h"
#include "hwdef/common/watchdog.h"
#if HAL_USE_PWM == TRUE #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 HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) { if (AP_BoardConfig::io_enabled()) {
return iomcu.get_safety_switch_state(); safety_state = iomcu.get_safety_switch_state();
} }
#endif #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; return safety_state;
} }

View File

@ -64,6 +64,11 @@ public:
// return true if the reason for the reboot was a watchdog reset // 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 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: private:
#ifdef HAL_PWM_ALARM #ifdef HAL_PWM_ALARM
struct ToneAlarmPwmGroup { struct ToneAlarmPwmGroup {

View File

@ -252,6 +252,29 @@ void set_fast_reboot(enum rtc_boot_magic v)
set_rtc_backup0(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 enable peripheral power if needed This is done late to prevent
problems with CTS causing SiK radios to stay in the bootloader. A problems with CTS causing SiK radios to stay in the bootloader. A

View File

@ -79,6 +79,12 @@ void malloc_init(void);
iomode_t palReadLineMode(ioline_t line); iomode_t palReadLineMode(ioline_t line);
#endif #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 #ifdef __cplusplus
} }
#endif #endif

View File

@ -4,6 +4,7 @@
#include "hal.h" #include "hal.h"
#include "watchdog.h" #include "watchdog.h"
#include "stm32_util.h"
#ifndef IWDG_BASE #ifndef IWDG_BASE
#if defined(STM32H7) #if defined(STM32H7)
@ -53,6 +54,7 @@ typedef struct
static bool was_watchdog_reset; static bool was_watchdog_reset;
static bool watchdog_enabled; static bool watchdog_enabled;
static bool boot_safety_state;
/* /*
setup the watchdog setup the watchdog
@ -85,6 +87,7 @@ void stm32_watchdog_save_reason(void)
{ {
if (WDG_RESET_STATUS & WDG_RESET_IS_IWDG) { if (WDG_RESET_STATUS & WDG_RESET_IS_IWDG) {
was_watchdog_reset = true; 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; 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;
}

View File

@ -29,6 +29,22 @@ void stm32_watchdog_save_reason(void);
*/ */
void stm32_watchdog_clear_reason(void); 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 #ifdef __cplusplus
} }
#endif #endif