Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-03 14:38:44 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: save/restore home position in backup registers

Browse Source
This commit is contained in:
Andrew Tridgell 2019-04-20 19:49:46 +10:00
parent 186f36a5bd
commit 723a7bb647
6 changed files with 84 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_HAL_ChibiOS/Util.cpp
View File

@ -296,3 +296,19 @@ 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;
}

6
libraries/AP_HAL_ChibiOS/Util.h
View File

@ -69,6 +69,12 @@ public:
// 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;
private:
#ifdef HAL_PWM_ALARM
struct ToneAlarmPwmGroup {

35
libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
View File

@ -219,14 +219,14 @@ uint32_t get_fattime()
return fattime;
}
// get RTC backup register 0
static uint32_t get_rtc_backup0(void)
// get RTC backup register
uint32_t get_rtc_backup(uint8_t n)
{
return RTC->BKP0R;
return ((__IO uint32_t *)&RTC->BKP0R)[n];
}
// set RTC backup register 0
static void set_rtc_backup0(uint32_t v)
void set_rtc_backup(uint8_t n, uint32_t v)
{
if ((RCC->BDCR & RCC_BDCR_RTCEN) == 0) {
RCC->BDCR |= STM32_RTCSEL;
@ -237,40 +237,19 @@ static void set_rtc_backup0(uint32_t v)
#else
PWR->CR1 |= PWR_CR1_DBP;
#endif
RTC->BKP0R = v;
((__IO uint32_t *)&RTC->BKP0R)[n] = 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_backup0();
return (enum rtc_boot_magic)get_rtc_backup(0);
}
// set RTC register for a fast reboot
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;
set_rtc_backup(0, v);
}
/*

10
libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
View File

@ -79,12 +79,12 @@ void malloc_init(void);
iomode_t palReadLineMode(ioline_t line);
#endif
// get RTC backup register 1
uint32_t get_rtc_backup1(void);
// get RTC backup register
uint32_t get_rtc_backup(uint8_t n);
// set RTC backup register
void set_rtc_backup(uint8_t n, uint32_t v);
// set RTC backup register 1
void set_rtc_backup1(uint32_t v);
#ifdef __cplusplus
}
#endif

52
libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c
View File

@ -41,6 +41,13 @@
#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
typedef struct
{
__IO uint32_t KR; /*!< IWDG Key register, Address offset: 0x00 */
@ -54,7 +61,7 @@ typedef struct
static bool was_watchdog_reset;
static bool watchdog_enabled;
static uint32_t boot_backup1_state;
static uint32_t boot_backup_state[5];
/*
setup the watchdog
@ -87,7 +94,9 @@ void stm32_watchdog_save_reason(void)
{
if (WDG_RESET_STATUS & WDG_RESET_IS_IWDG) {
was_watchdog_reset = true;
boot_backup1_state = get_rtc_backup1();
for (uint8_t i=0; i<sizeof(boot_backup_state)/sizeof(boot_backup_state[0]); i++) {
boot_backup_state[i] = get_rtc_backup(i);
}
}
}
@ -97,7 +106,9 @@ void stm32_watchdog_save_reason(void)
void stm32_watchdog_clear_reason(void)
{
WDG_RESET_STATUS = WDG_RESET_CLEAR;
set_rtc_backup1(0);
for (uint8_t i=1; i<sizeof(boot_backup_state)/sizeof(boot_backup_state[0]); i++) {
set_rtc_backup(i, 0);
}
}
/*
@ -108,9 +119,6 @@ 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
@ -119,10 +127,10 @@ bool stm32_was_watchdog_reset(void)
*/
void stm32_set_backup_safety_state(bool safety_on)
{
uint32_t v = get_rtc_backup1();
uint32_t v = get_rtc_backup(BKP_IDX_FLAGS);
uint32_t v2 = safety_on?(v | WDG_SAFETY_BIT):(v & ~WDG_SAFETY_BIT);
if (v != v2) {
set_rtc_backup1(v2);
set_rtc_backup(BKP_IDX_FLAGS, v2);
}
}
@ -131,7 +139,7 @@ void stm32_set_backup_safety_state(bool safety_on)
*/
bool stm32_get_boot_backup_safety_state(void)
{
return (boot_backup1_state & WDG_SAFETY_BIT) != 0;
return (boot_backup_state[BKP_IDX_FLAGS] & WDG_SAFETY_BIT) != 0;
}
/*
@ -142,10 +150,10 @@ bool stm32_get_boot_backup_safety_state(void)
*/
void stm32_set_backup_armed(bool armed)
{
uint32_t v = get_rtc_backup1();
uint32_t v = get_rtc_backup(BKP_IDX_FLAGS);
uint32_t v2 = armed?(v | WDG_ARMED_BIT): (v & ~WDG_ARMED_BIT);
if (v != v2) {
set_rtc_backup1(v2);
set_rtc_backup(BKP_IDX_FLAGS, v2);
}
}
@ -154,5 +162,25 @@ void stm32_set_backup_armed(bool armed)
*/
bool stm32_get_boot_backup_armed(void)
{
return (boot_backup1_state & WDG_ARMED_BIT) != 0;
return (boot_backup_state[BKP_IDX_FLAGS] & WDG_ARMED_BIT) != 0;
}
/*
set home state in backup
*/
void stm32_set_backup_home(int32_t lat, int32_t lon, int32_t alt_cm)
{
set_rtc_backup(BKP_IDX_HOME, (uint32_t)lat);
set_rtc_backup(BKP_IDX_HOME+1, (uint32_t)lon);
set_rtc_backup(BKP_IDX_HOME+2, (uint32_t)alt_cm);
}
/*
get home state from backup
*/
void stm32_get_backup_home(int32_t *lat, int32_t *lon, int32_t *alt_cm)
{
*lat = (int32_t)boot_backup_state[BKP_IDX_HOME];
*lon = (int32_t)boot_backup_state[BKP_IDX_HOME+1];
*alt_cm = (int32_t)boot_backup_state[BKP_IDX_HOME+2];
}

10
libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h
View File

@ -48,6 +48,16 @@ void stm32_set_backup_armed(bool armed);
get the armed state in backup register from initial boot
*/
bool stm32_get_boot_backup_armed(void);
/*
set home state in backup
*/
void stm32_set_backup_home(int32_t lat, int32_t lon, int32_t alt_cm);
/*
get home state from backup
*/
void stm32_get_backup_home(int32_t *lat, int32_t *lon, int32_t *alt_cm);
#ifdef __cplusplus
}