From 5ef4f8e90a09e66e2a206906f11c3fa5f771fc3a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 6 Jul 2015 13:17:05 +0900
Subject: [PATCH] Rover: log ahrs home and ekf origin

---
 APMrover2/Log.cpp            | 17 +++++++++++++++++
 APMrover2/Rover.h            |  1 +
 APMrover2/commands.cpp       |  1 +
 APMrover2/commands_logic.cpp |  1 +
 4 files changed, 20 insertions(+)

diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp
index 3ee6b3d1c8..a80e0f4145 100644
--- a/APMrover2/Log.cpp
+++ b/APMrover2/Log.cpp
@@ -359,6 +359,23 @@ void Rover::Log_Write_Baro(void)
     DataFlash.Log_Write_Baro(barometer);
 }
 
+// log ahrs home and EKF origin to dataflash
+void Rover::Log_Write_Home_And_Origin()
+{
+#if AP_AHRS_NAVEKF_AVAILABLE
+    // 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);
+    }
+#endif
+
+    // log ahrs home if set
+    if (home_is_set) {
+        DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
+    }
+}
+
 const LogStructure Rover::log_structure[] PROGMEM = {
     LOG_COMMON_STRUCTURES,
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index fbc3a8436e..334fcd7217 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -405,6 +405,7 @@ private:
     void Log_Write_Attitude();
     void Log_Write_RC(void);
     void Log_Write_Baro(void);
+    void Log_Write_Home_And_Origin();
     void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
     void log_init(void);
     void start_logging() ;
diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp
index ed327e1120..fb4299e335 100644
--- a/APMrover2/commands.cpp
+++ b/APMrover2/commands.cpp
@@ -67,6 +67,7 @@ void Rover::init_home()
 
     ahrs.set_home(gps.location());
 	home_is_set = true;
+	Log_Write_Home_And_Origin();
 
 	// Save Home to EEPROM
 	mission.write_home_to_storage();
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index 51ac3ee25e..825d0b16ff 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -282,6 +282,7 @@ void Rover::do_set_home(const AP_Mission::Mission_Command& cmd)
 	} else {
         ahrs.set_home(cmd.content.location);
 		home_is_set = true;
+		Log_Write_Home_And_Origin();
 	}
 }