From 9a45940c25787cab9b2f85c284b38321418cae3c Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 18 Sep 2017 10:35:12 +0900
Subject: [PATCH] GCS_MAVLink: add send_ekf_origin

---
 libraries/GCS_MAVLink/GCS.h          |  2 ++
 libraries/GCS_MAVLink/GCS_Common.cpp | 27 +++++++++++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 24ce234c68..548a7dd1d1 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -158,6 +158,8 @@ public:
     void send_vibration(const AP_InertialSensor &ins) const;
     void send_home(const Location &home) const;
     static void send_home_all(const Location &home);
+    void send_ekf_origin(const Location &ekf_origin) const;
+    static void send_ekf_origin_all(const Location &ekf_origin);
     void send_heartbeat(uint8_t type, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status);
     void send_servo_output_raw(bool hil);
     static void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 21a606d640..431a22979e 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1416,6 +1416,33 @@ void GCS_MAVLINK::send_home_all(const Location &home)
     }
 }
 
+void GCS_MAVLINK::send_ekf_origin(const Location &ekf_origin) const
+{
+    if (HAVE_PAYLOAD_SPACE(chan, GPS_GLOBAL_ORIGIN)) {
+        mavlink_msg_gps_global_origin_send(
+            chan,
+            ekf_origin.lat,
+            ekf_origin.lng,
+            ekf_origin.alt * 10);
+    }
+}
+
+void GCS_MAVLINK::send_ekf_origin_all(const Location &ekf_origin)
+{
+    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
+        if ((1U<<i) & mavlink_active) {
+            mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
+            if (HAVE_PAYLOAD_SPACE(chan, GPS_GLOBAL_ORIGIN)) {
+                mavlink_msg_gps_global_origin_send(
+                    chan,
+                    ekf_origin.lat,
+                    ekf_origin.lng,
+                    ekf_origin.alt * 10);
+            }
+        }
+    }
+}
+
 /*
   wrapper for sending heartbeat
  */