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

AP_AHRS: added save/restore of attitude in backup registers

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

26
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -116,6 +116,21 @@ AP_AHRS_DCM::update(bool skip_ins_update)
// update AOA and SSA
update_AOA_SSA();
backup_attitude();
}
/*
backup attitude to stm32 registers at 3Hz
*/
void AP_AHRS_DCM::backup_attitude(void)
{
uint32_t now = AP_HAL::millis();
if (now - _last_backup_ms < 333) {
return;
}
_last_backup_ms = now;
hal.util->set_backup_attitude(roll_sensor, pitch_sensor, yaw_sensor);
}
// update the DCM matrix using only the gyros
@ -175,7 +190,16 @@ AP_AHRS_DCM::reset(bool recover_eulers)
// if the caller wants us to try to recover to the current
// attitude then calculate the dcm matrix from the current
// roll/pitch/yaw values
if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
if (hal.util->was_watchdog_reset() &&
hal.util->get_backup_attitude(roll_sensor, pitch_sensor, yaw_sensor) &&
AP_HAL::millis() < 10000) {
roll = radians(roll_sensor*0.01f);
pitch = radians(pitch_sensor*0.01f);
yaw = radians(yaw_sensor*0.01f);
_dcm_matrix.from_euler(roll, pitch, yaw);
gcs().send_text(MAV_SEVERITY_INFO, "Restored watchdog attitude %d %d %d",
roll_sensor/100, pitch_sensor/100, yaw_sensor/100);
} else if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
_dcm_matrix.from_euler(roll, pitch, yaw);
} else {

4
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -120,6 +120,7 @@ private:
bool have_gps(void) const;
bool use_fast_gains(void) const;
void load_watchdog_home();
void backup_attitude(void);
// primary representation of attitude of board used for all inertial calculations
Matrix3f _dcm_matrix;
@ -194,4 +195,7 @@ private:
// time when DCM was last reset
uint32_t _last_startup_ms;
// time when DCM was last backed up to stm32 backup registers
uint32_t _last_backup_ms;
};