Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: implement persistent_data for watchdog

Browse Source 
this is a much simpler approach to persistent data
This commit is contained in:
Andrew Tridgell 2019-05-09 17:49:32 +10:00
parent 11c123e4da
commit dbfe6b8019
8 changed files with 50 additions and 225 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
View File

@ -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;

6
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -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;
}

53
libraries/AP_HAL_ChibiOS/Util.cpp
View File

@ -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;
}

23
libraries/AP_HAL_ChibiOS/Util.h
View File

@ -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");
};

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

@ -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;
}

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

@ -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
}

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

@ -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<sizeof(boot_backup_state)/sizeof(boot_backup_state[0]); i++) {
boot_backup_state[i] = get_rtc_backup(i);
}
}
}
@ -107,10 +95,6 @@ void stm32_watchdog_save_reason(void)
void stm32_watchdog_clear_reason(void)
{
WDG_RESET_STATUS = WDG_RESET_CLEAR;
uint8_t i;
for (i=1; i<sizeof(boot_backup_state)/sizeof(boot_backup_state[0]); i++) {
set_rtc_backup(i, 0);
}
}
/*
@ -122,87 +106,17 @@ bool stm32_was_watchdog_reset(void)
}
/*
set the safety state in backup register
This is stored so that the safety state can be restored after a
watchdog reset
save persistent watchdog data
*/
void stm32_set_backup_safety_state(bool safety_on)
void stm32_watchdog_save(const uint32_t *data, uint32_t nwords)
{
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_backup(BKP_IDX_FLAGS, v2);
}
set_rtc_backup(1, data, nwords);
}
/*
get the safety state in backup register from initial boot
*/
bool stm32_get_boot_backup_safety_state(void)
{
return (boot_backup_state[BKP_IDX_FLAGS] & 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
load persistent watchdog data
*/
void stm32_set_backup_armed(bool armed)
void stm32_watchdog_load(uint32_t *data, uint32_t nwords)
{
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_backup(BKP_IDX_FLAGS, v2);
}
}
/*
get the armed state in backup register from initial boot
*/
bool stm32_get_boot_backup_armed(void)
{
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];
}
/*
set attitude in backup
*/
void stm32_set_attitude(int32_t roll_cd, int32_t pitch_cd, int32_t yaw_cd)
{
set_rtc_backup(BKP_IDX_RPY, (uint32_t)roll_cd);
set_rtc_backup(BKP_IDX_RPY+1, (uint32_t)pitch_cd);
set_rtc_backup(BKP_IDX_RPY+2, (uint32_t)yaw_cd);
}
/*
get attitude from backup
*/
void stm32_get_attitude(int32_t *roll_cd, int32_t *pitch_cd, int32_t *yaw_cd)
{
*roll_cd = (int32_t)boot_backup_state[BKP_IDX_RPY];
*pitch_cd = (int32_t)boot_backup_state[BKP_IDX_RPY+1];
*yaw_cd = (int32_t)boot_backup_state[BKP_IDX_RPY+2];
get_rtc_backup(1, data, nwords);
}

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

@ -30,44 +30,14 @@ void stm32_watchdog_save_reason(void);
void stm32_watchdog_clear_reason(void);
/*
set the safety state in backup register
save persistent watchdog data
*/
void stm32_set_backup_safety_state(bool safety_on);
void stm32_watchdog_save(const uint32_t *data, uint32_t nwords);
/*
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
load persistent watchdog data
*/
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);
/*
set attitude in backup
*/
void stm32_set_attitude(int32_t roll_cd, int32_t pitch_cd, int32_t yaw_cd);
/*
get attitude from backup
*/
void stm32_get_attitude(int32_t *roll_cd, int32_t *pitch_cd, int32_t *yaw_cd);
void stm32_watchdog_load(uint32_t *data, uint32_t nwords);
#ifdef __cplusplus
}