From 9793703a760a0ee7128d03123e1191698c32cb46 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 16 May 2018 13:28:31 +1000
Subject: [PATCH] AP_AHRS: move Log_Write_Home_And_Origin into AP_AHRS

---
 libraries/AP_AHRS/AP_AHRS.cpp | 23 +++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS.h   |  2 ++
 2 files changed, 25 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 9e12aea45c..c2c5db7cbb 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -17,6 +17,7 @@
 #include "AP_AHRS.h"
 #include "AP_AHRS_View.h"
 #include <AP_HAL/AP_HAL.h>
+#include <DataFlash/DataFlash.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -457,6 +458,28 @@ Vector2f AP_AHRS::rotate_body_to_earth2D(const Vector2f &bf) const
                     bf.x * _sin_yaw + bf.y * _cos_yaw);
 }
 
+// log ahrs home and EKF origin to dataflash
+void AP_AHRS::Log_Write_Home_And_Origin()
+{
+    DataFlash_Class *df = DataFlash_Class::instance();
+    if (df == nullptr) {
+        return;
+    }
+#if AP_AHRS_NAVEKF_AVAILABLE
+    // log ekf origin if set
+    Location ekf_orig;
+    if (get_origin(ekf_orig)) {
+        df->Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
+    }
+#endif
+
+    // log ahrs home if set
+    if (home_is_set()) {
+        df->Log_Write_Origin(LogOriginType::ahrs_home, _home);
+    }
+}
+
+
 // singleton instance
 AP_AHRS *AP_AHRS::_singleton;
 
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 7bf12dd7b4..cd29d6db28 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -459,6 +459,8 @@ public:
     // returns the inertial navigation origin in lat/lon/alt
     virtual bool get_origin(Location &ret) const { return false; }
 
+    void Log_Write_Home_And_Origin();
+
     // return true if the AHRS object supports inertial navigation,
     // with very accurate position and velocity
     virtual bool have_inertial_nav(void) const {