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

restore on watchdog reset
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 24 ++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_DCM.h   |  1 +
 2 files changed, 25 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index c69d1f2343..46b42e8be9 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -22,6 +22,7 @@
  */
 #include "AP_AHRS.h"
 #include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -45,6 +46,24 @@ 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.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
 void
 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) {
         _last_startup_ms = AP_HAL::millis();
+        load_watchdog_home();
     }
 
     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();
 }
 
@@ -1029,6 +1052,7 @@ void AP_AHRS_DCM::set_home(const Location &loc)
     _home = loc;
     _home.options = 0;
     _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
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 8d6f7d634c..94dcd5d970 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -120,6 +120,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;