Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: added save/restore of attitude in backup registers

This commit is contained in:
Andrew Tridgell 2019-04-21 13:08:29 +10:00
parent f7026b854e
commit 508aae8f2a
4 changed files with 54 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_HAL_ChibiOS/Util.cpp
View File

@ -361,3 +361,19 @@ bool Util::get_backup_home_state(int32_t &lat, int32_t &lon, int32_t &alt_cm) co
}
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;
}

6
libraries/AP_HAL_ChibiOS/Util.h
View File

@ -80,6 +80,12 @@ public:
// 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 {

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

@ -46,7 +46,7 @@
#define BKP_IDX_FLAGS 0x01
#define BKP_IDX_HOME 0x02
#define BKP_IDX_RPY 0x05
typedef struct
{
@ -61,7 +61,7 @@ typedef struct
static bool was_watchdog_reset;
static bool watchdog_enabled;
static uint32_t boot_backup_state[5];
static uint32_t boot_backup_state[8];
/*
setup the watchdog
@ -186,3 +186,23 @@ void stm32_get_backup_home(int32_t *lat, int32_t *lon, int32_t *alt_cm)
*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];
}

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

@ -59,6 +59,16 @@ void stm32_set_backup_home(int32_t lat, int32_t lon, int32_t alt_cm);
*/
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);
#ifdef __cplusplus
}
#endif