diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 85fba4f729..49488c6748 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -2682,6 +2682,20 @@ bool AP_AHRS::set_home(const Location &loc) return true; } +/* if this was a watchdog reset then get home from backup registers */ +void AP_AHRS::load_watchdog_home() +{ + const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; + if (hal.util->was_watchdog_reset() && (pd.home_lat != 0 || pd.home_lon != 0)) { + _home.lat = pd.home_lat; + _home.lng = pd.home_lon; + _home.set_alt_cm(pd.home_alt_cm, Location::AltFrame::ABSOLUTE); + _home_is_set = true; + _home_locked = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Restored watchdog home"); + } +} + // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in metres and a validity flag // this is used to limit height during optical flow navigation // it will return false when no limiting is required diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index d01264fb4c..386083b31d 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -51,22 +51,6 @@ AP_AHRS_DCM::reset_gyro_drift(void) _omega_I_sum_time = 0; } - -/* if this was a watchdog reset then get home from backup registers */ -void AP_AHRS::load_watchdog_home() -{ - const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; - if (hal.util->was_watchdog_reset() && (pd.home_lat != 0 || pd.home_lon != 0)) { - _home.lat = pd.home_lat; - _home.lng = pd.home_lon; - _home.set_alt_cm(pd.home_alt_cm, Location::AltFrame::ABSOLUTE); - _home_is_set = true; - _home_locked = true; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Restored watchdog home"); - } -} - - // run a full DCM update round void AP_AHRS_DCM::update()