From b8605c9ee6dd5d834cb2177f734bd7fb21e8fec5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 20 Apr 2019 20:02:46 +1000
Subject: [PATCH] AP_AHRS: save/restore home to backup registers

restore on watchdog reset

# Conflicts:
#	libraries/AP_AHRS/AP_AHRS_DCM.cpp
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 23 +++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_DCM.h   |  1 +
 2 files changed, 24 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 8c1a35f030..a13b349ca5 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -46,6 +46,23 @@ 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_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.set_alt_cm(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(bool skip_ins_update)
@@ -57,6 +74,7 @@ AP_AHRS_DCM::update(bool skip_ins_update)
 
     if (_last_startup_ms == 0) {
         _last_startup_ms = AP_HAL::millis();
+        load_watchdog_home();
     }
 
     if (!skip_ins_update) {
@@ -192,6 +210,9 @@ AP_AHRS_DCM::reset(bool recover_eulers)
 
     }
 
+    if (_last_startup_ms == 0) {
+        load_watchdog_home();
+    }
     _last_startup_ms = AP_HAL::millis();
 }
 
@@ -1057,6 +1078,8 @@ bool AP_AHRS_DCM::set_home(const Location &loc)
     gcs().send_message(MSG_HOME);
     gcs().send_message(MSG_ORIGIN);
 
+    hal.util->set_backup_home_state(loc.lat, loc.lng, loc.alt);
+
     return true;
 }
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index a66fb44e5d..f0c54c9505 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -119,6 +119,7 @@ private:
     void            euler_angles(void);
     bool            have_gps(void) const;
     bool            use_fast_gains(void) const;
+    void            load_watchdog_home();
 
     // primary representation of attitude of board used for all inertial calculations
     Matrix3f _dcm_matrix;