From 1b3cc9289bccf881d7c9e0e015db69ace8eea790 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 18 Sep 2017 10:37:55 +0900
Subject: [PATCH] Copter: support SET_GPS_GLOBAL_ORIGIN

also remove setting of origin from DO_SET_HOME command
initialise ekf_origin location when consuming SET_GPS_GLOBAL_ORIGIN
set_ekf_origin performs sanity check on location
---
 ArduCopter/Copter.h        |  1 +
 ArduCopter/GCS_Mavlink.cpp | 20 ++++++++++++++++++++
 ArduCopter/commands.cpp    | 36 +++++++++++++++++++++++++++++++-----
 3 files changed, 52 insertions(+), 5 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 6aca80cb17..b85c142ead 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -772,6 +772,7 @@ private:
     void set_home_to_current_location_inflight();
     bool set_home_to_current_location(bool lock);
     bool set_home(const Location& loc, bool lock);
+    void set_ekf_origin(const Location& loc);
     bool far_from_EKF_origin(const Location& loc);
     void set_system_time_from_GPS();
     void exit_mission();
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index abfd342be0..02c6520941 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -742,6 +742,22 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
         break;
     }
 
+    case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
+    {
+        mavlink_set_gps_global_origin_t packet;
+        mavlink_msg_set_gps_global_origin_decode(msg, &packet);
+        // sanity check location
+        if (!check_latlng(packet.latitude, packet.longitude)) {
+            break;
+        }
+        Location ekf_origin {};
+        ekf_origin.lat = packet.latitude;
+        ekf_origin.lng = packet.longitude;
+        ekf_origin.alt = packet.altitude / 10;
+        copter.set_ekf_origin(ekf_origin);
+        break;
+    }
+
     case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:    // MAV ID: 66
     {
         handle_request_data_stream(msg, false);
@@ -1051,6 +1067,10 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
         case MAV_CMD_GET_HOME_POSITION:
             if (copter.ap.home_state != HOME_UNSET) {
                 send_home(copter.ahrs.get_home());
+                Location ekf_origin;
+                if (copter.ahrs.get_origin(ekf_origin)) {
+                    send_ekf_origin(ekf_origin);
+                }
                 result = MAV_RESULT_ACCEPTED;
             } else {
                 result = MAV_RESULT_FAILED;
diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp
index d06cc06180..9eae1f572c 100644
--- a/ArduCopter/commands.cpp
+++ b/ArduCopter/commands.cpp
@@ -55,11 +55,10 @@ bool Copter::set_home(const Location& loc, bool lock)
         return false;
     }
 
-    // set EKF origin to home if it hasn't been set yet and vehicle is disarmed
-    // this is required to allowing flying in AUTO and GUIDED with only an optical flow
+    // check EKF origin has been set
     Location ekf_origin;
-    if (!motors->armed() && !ahrs.get_origin(ekf_origin)) {
-        ahrs.set_origin(loc);
+    if (!ahrs.get_origin(ekf_origin)) {
+        return false;
     }
 
     // check home is close to EKF origin
@@ -94,13 +93,40 @@ bool Copter::set_home(const Location& loc, bool lock)
     // log ahrs home and ekf origin dataflash
     Log_Write_Home_And_Origin();
 
-    // send new home location to GCS
+    // send new home and ekf origin to GCS
     gcs().send_home(loc);
+    gcs().send_ekf_origin(ekf_origin);
 
     // return success
     return true;
 }
 
+// sets ekf_origin if it has not been set.
+//  should only be used when there is no GPS to provide an absolute position
+void Copter::set_ekf_origin(const Location& loc)
+{
+    // check location is valid
+    if (!check_latlng(loc)) {
+        return;
+    }
+
+    // check EKF origin has already been set
+    Location ekf_origin;
+    if (ahrs.get_origin(ekf_origin)) {
+        return;
+    }
+
+    if (!ahrs.set_origin(loc)) {
+        return;
+    }
+
+    // log ahrs home and ekf origin dataflash
+    Log_Write_Home_And_Origin();
+
+    // send ekf origin to GCS
+    gcs().send_ekf_origin(loc);
+}
+
 // far_from_EKF_origin - checks if a location is too far from the EKF origin
 //  returns true if too far
 bool Copter::far_from_EKF_origin(const Location& loc)