From d45eb33980c2abd98eaa8324e4643510825c66d5 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 18 Sep 2017 14:38:26 +0900
Subject: [PATCH] Tracker: support SET_GPS_GLOBAL_ORIGIN message

---
 AntennaTracker/GCS_Mavlink.cpp | 20 ++++++++++++++++++++
 AntennaTracker/Tracker.h       |  1 +
 AntennaTracker/system.cpp      | 27 +++++++++++++++++++++++++++
 3 files changed, 48 insertions(+)

diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index 05e9ffc1a6..1522fafb05 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/AntennaTracker/GCS_Mavlink.cpp
@@ -434,6 +434,22 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
 {
     switch (msg->msgid) {
 
+    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;
+        tracker.set_ekf_origin(ekf_origin);
+        break;
+    }
+
     // If we are currently operating as a proxy for a remote, 
     // alas we have to look inside each packet to see if it's for us or for the remote
     case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
@@ -526,6 +542,10 @@ void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg)
 
             case MAV_CMD_GET_HOME_POSITION:
                 send_home(tracker.ahrs.get_home());
+                Location ekf_origin;
+                if (tracker.ahrs.get_origin(ekf_origin)) {
+                    send_ekf_origin(ekf_origin);
+                }
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index 950382336e..04a3a9ada6 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -250,6 +250,7 @@ private:
     bool get_home_eeprom(struct Location &loc);
     void set_home_eeprom(struct Location temp);
     void set_home(struct Location temp);
+    void set_ekf_origin(const Location& loc);
     void arm_servos();
     void disarm_servos();
     void prepare_servos();
diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
index 6f33462227..97d5467413 100644
--- a/AntennaTracker/system.cpp
+++ b/AntennaTracker/system.cpp
@@ -166,6 +166,33 @@ void Tracker::set_home(struct Location temp)
     set_home_eeprom(temp);
     current_loc = temp;
     gcs().send_home(temp);
+    Location ekf_origin;
+    if (ahrs.get_origin(ekf_origin)) {
+        gcs().send_ekf_origin(ekf_origin);
+    }
+}
+
+// sets ekf_origin if it has not been set.
+//  should only be used when there is no GPS to provide an absolute position
+void Tracker::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;
+    }
+
+    // send ekf origin to GCS
+    gcs().send_ekf_origin(loc);
 }
 
 void Tracker::arm_servos()