From 221ddb545eb9b6b851044d5fd1f14b9f2719ee83 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sun, 5 Jul 2015 17:25:28 +0900
Subject: [PATCH] Copter: log ahrs home and ekf origin

---
 ArduCopter/Copter.h     |  1 +
 ArduCopter/Log.cpp      | 15 +++++++++++++++
 ArduCopter/commands.cpp |  3 +++
 3 files changed, 19 insertions(+)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index f360e5637d..48a17cd9de 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -597,6 +597,7 @@ private:
     void Log_Write_Error(uint8_t sub_system, uint8_t error_code);
     void Log_Write_Baro(void);
     void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high);
+    void Log_Write_Home_And_Origin();
 #if FRAME_CONFIG == HELI_FRAME
     void Log_Write_Heli(void);
 #endif
diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
index 8b4f67b4a4..9f57e0b7ea 100644
--- a/ArduCopter/Log.cpp
+++ b/ArduCopter/Log.cpp
@@ -631,6 +631,21 @@ void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t
     DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune));
 }
 
+// log EKF origin and ahrs home to dataflash
+void Copter::Log_Write_Home_And_Origin()
+{
+    // log ekf origin if set
+    Location ekf_orig;
+    if (ahrs.get_NavEKF_const().getOriginLLH(ekf_orig)) {
+        DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig);
+    }
+
+    // log ahrs home if set
+    if (ap.home_state != HOME_UNSET) {
+        DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
+    }
+}
+
 struct PACKED log_Heli {
     LOG_PACKET_HEADER;
     uint64_t time_us;
diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp
index e7de4fa0d0..f79c50aef1 100644
--- a/ArduCopter/commands.cpp
+++ b/ArduCopter/commands.cpp
@@ -101,6 +101,9 @@ bool Copter::set_home(const Location& loc)
         }
     }
 
+    // log ahrs home and ekf origin dataflash
+    Log_Write_Home_And_Origin();
+
     // return success
     return true;
 }