From f7026b854e73649c42c4c35a054fe8e8bbeadf99 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 21 Apr 2019 13:08:29 +1000
Subject: [PATCH] AP_AHRS: added save/restore of attitude in backup registers

---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 26 +++++++++++++++++++++++++-
 libraries/AP_AHRS/AP_AHRS_DCM.h   |  4 ++++
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index a13b349ca5..402e10f0f1 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -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 {
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index f0c54c9505..42e7d28e18 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -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;
 };