From 2d1745ab6a70a3a3c2e020ceb33d3f9e22b7152c Mon Sep 17 00:00:00 2001
From: Grant Morphett <grant@gmorph.com>
Date: Fri, 30 Oct 2015 16:41:06 +1100
Subject: [PATCH] Rover: Implemented the HOME state update from Plane

---
 APMrover2/APMrover2.cpp        |  4 ++++
 APMrover2/Log.cpp              |  2 +-
 APMrover2/Rover.h              |  5 +++--
 APMrover2/commands.cpp         | 17 ++++++++++++++++-
 APMrover2/commands_logic.cpp   |  2 +-
 APMrover2/commands_process.cpp |  2 +-
 APMrover2/navigation.cpp       |  2 +-
 7 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index e2cab03d27..955b211a97 100644
--- a/APMrover2/APMrover2.cpp
+++ b/APMrover2/APMrover2.cpp
@@ -382,6 +382,10 @@ void Rover::update_GPS_10Hz(void)
             do_take_picture();
         }
 #endif
+
+        if (!hal.util->get_soft_armed()) {
+            update_home();
+        }
     }
 }
 
diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp
index e52766b188..c09cab0612 100644
--- a/APMrover2/Log.cpp
+++ b/APMrover2/Log.cpp
@@ -363,7 +363,7 @@ void Rover::Log_Write_Home_And_Origin()
 #endif
 
     // log ahrs home if set
-    if (home_is_set) {
+    if (home_is_set != HOME_UNSET) {
         DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
     }
 }
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index a65972d40f..1a25281cac 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -314,8 +314,9 @@ private:
     // The home location used for RTL.  The location is set when we first get stable GPS lock
     const struct Location &home;
 
-    // Flag for if we have gps lock and have set the home location
-    bool home_is_set;
+    // Flag for if we have g_gps lock and have set the home location in AHRS
+    enum HomeState home_is_set = HOME_UNSET;
+
     // The location of the previous waypoint.  Used for track following and altitude ramp calculations
     struct Location prev_WP;
     // The location of the current/active waypoint.  Used for track following
diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp
index f912ca14a8..bf521de9d3 100644
--- a/APMrover2/commands.cpp
+++ b/APMrover2/commands.cpp
@@ -66,7 +66,7 @@ void Rover::init_home()
 	gcs_send_text(MAV_SEVERITY_INFO, "init home");
 
     ahrs.set_home(gps.location());
-	home_is_set = true;
+	home_is_set = HOME_SET_NOT_LOCKED;
 	Log_Write_Home_And_Origin();
     GCS_MAVLINK::send_home_all(gps.location());
 
@@ -88,3 +88,18 @@ void Rover::restart_nav()
     prev_WP = current_loc;
     mission.start_or_resume();
 }
+
+/*
+  update home location from GPS
+  this is called as long as we have 3D lock and the arming switch is
+  not pushed
+*/
+void Rover::update_home()
+{
+    if (home_is_set == HOME_SET_NOT_LOCKED) {
+        ahrs.set_home(gps.location());
+        Log_Write_Home_And_Origin();
+        GCS_MAVLINK::send_home_all(gps.location());
+    }
+    barometer.update_calibration();
+}
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index 02eb9f44f2..6d527b94bf 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -324,7 +324,7 @@ void Rover::do_set_home(const AP_Mission::Mission_Command& cmd)
 		init_home();
 	} else {
         ahrs.set_home(cmd.content.location);
-		home_is_set = true;
+		home_is_set = HOME_SET_NOT_LOCKED;
 		Log_Write_Home_And_Origin();
         GCS_MAVLINK::send_home_all(cmd.content.location);
 	}
diff --git a/APMrover2/commands_process.cpp b/APMrover2/commands_process.cpp
index c263f56efd..1688d51320 100644
--- a/APMrover2/commands_process.cpp
+++ b/APMrover2/commands_process.cpp
@@ -7,7 +7,7 @@
 void Rover::update_commands(void)
 {
     if(control_mode == AUTO) {
-        if(home_is_set == true && mission.num_commands() > 1) {
+        if (home_is_set != HOME_UNSET && mission.num_commands() > 1) {
             mission.update();
         }
     }
diff --git a/APMrover2/navigation.cpp b/APMrover2/navigation.cpp
index c409336fdc..d8e9064dd0 100644
--- a/APMrover2/navigation.cpp
+++ b/APMrover2/navigation.cpp
@@ -13,7 +13,7 @@ void Rover::navigate()
 		return;
 	}
 
-	if ((next_WP.lat == 0)||(home_is_set==false)){
+	if ((next_WP.lat == 0) || (home_is_set==HOME_UNSET)){
 		return;
 	}