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

AP_AHRS: save/restore home to backup registers 

restore on watchdog reset
This commit is contained in:
Andrew Tridgell 2019-04-20 19:50:25 +10:00
parent 723a7bb647
commit 9f0312d744
2 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -22,6 +22,7 @@
*/ */
#include "AP_AHRS.h" #include "AP_AHRS.h"
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -45,6 +46,24 @@ AP_AHRS_DCM::reset_gyro_drift(void)
_omega_I_sum_time = 0; _omega_I_sum_time = 0;
} }
/* if this was a watchdog reset then get home from backup registers */
void AP_AHRS_DCM::load_watchdog_home()
{
int32_t lat, lon, alt_cm;
if (hal.util->was_watchdog_reset() &&
hal.util->get_backup_home_state(lat, lon, alt_cm) &&
(lat != 0 || lon != 0)) {
_home.lat = lat;
_home.lng = lon;
_home.alt = alt_cm;
_home.options = 0;
_home_is_set = true;
_home_locked = true;
gcs().send_text(MAV_SEVERITY_INFO, "Restored watchdog home");
}
}
// run a full DCM update round // run a full DCM update round
void void
AP_AHRS_DCM::update(bool skip_ins_update) AP_AHRS_DCM::update(bool skip_ins_update)
@ -53,6 +72,7 @@ AP_AHRS_DCM::update(bool skip_ins_update)
if (_last_startup_ms == 0) { if (_last_startup_ms == 0) {
_last_startup_ms = AP_HAL::millis(); _last_startup_ms = AP_HAL::millis();
load_watchdog_home();
} }
if (!skip_ins_update) { if (!skip_ins_update) {
@ -185,6 +205,9 @@ AP_AHRS_DCM::reset(bool recover_eulers)
} }
if (_last_startup_ms == 0) {
load_watchdog_home();
}
_last_startup_ms = AP_HAL::millis(); _last_startup_ms = AP_HAL::millis();
} }
@ -1029,6 +1052,7 @@ void AP_AHRS_DCM::set_home(const Location &loc)
_home = loc; _home = loc;
_home.options = 0; _home.options = 0;
_home_is_set = true; _home_is_set = true;
hal.util->set_backup_home_state(loc.lat, loc.lng, loc.alt);
} }
// a relative ground position to home in meters, Down // a relative ground position to home in meters, Down

1
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -120,6 +120,7 @@ private:
void euler_angles(void); void euler_angles(void);
bool have_gps(void) const; bool have_gps(void) const;
bool use_fast_gains(void) const; bool use_fast_gains(void) const;
void load_watchdog_home();
// primary representation of attitude of board used for all inertial calculations // primary representation of attitude of board used for all inertial calculations
Matrix3f _dcm_matrix; Matrix3f _dcm_matrix;