mirror of https://github.com/ArduPilot/ardupilot
HAL_ChibiOS: added save/restore of attitude in backup registers
This commit is contained in:
parent
f7026b854e
commit
508aae8f2a
|
@ -361,3 +361,19 @@ bool Util::get_backup_home_state(int32_t &lat, int32_t &lon, int32_t &alt_cm) co
|
||||||
}
|
}
|
||||||
return false;
|
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;
|
||||||
|
}
|
||||||
|
|
|
@ -79,6 +79,12 @@ public:
|
||||||
|
|
||||||
// backup home state for restore on watchdog reset
|
// 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;
|
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:
|
private:
|
||||||
#ifdef HAL_PWM_ALARM
|
#ifdef HAL_PWM_ALARM
|
||||||
|
|
|
@ -46,7 +46,7 @@
|
||||||
|
|
||||||
#define BKP_IDX_FLAGS 0x01
|
#define BKP_IDX_FLAGS 0x01
|
||||||
#define BKP_IDX_HOME 0x02
|
#define BKP_IDX_HOME 0x02
|
||||||
|
#define BKP_IDX_RPY 0x05
|
||||||
|
|
||||||
typedef struct
|
typedef struct
|
||||||
{
|
{
|
||||||
|
@ -61,7 +61,7 @@ typedef struct
|
||||||
|
|
||||||
static bool was_watchdog_reset;
|
static bool was_watchdog_reset;
|
||||||
static bool watchdog_enabled;
|
static bool watchdog_enabled;
|
||||||
static uint32_t boot_backup_state[5];
|
static uint32_t boot_backup_state[8];
|
||||||
|
|
||||||
/*
|
/*
|
||||||
setup the watchdog
|
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];
|
*lon = (int32_t)boot_backup_state[BKP_IDX_HOME+1];
|
||||||
*alt_cm = (int32_t)boot_backup_state[BKP_IDX_HOME+2];
|
*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];
|
||||||
|
}
|
||||||
|
|
|
@ -58,6 +58,16 @@ void stm32_set_backup_home(int32_t lat, int32_t lon, int32_t alt_cm);
|
||||||
get home state from backup
|
get home state from backup
|
||||||
*/
|
*/
|
||||||
void stm32_get_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
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue