Tracker: support SET_GPS_GLOBAL_ORIGIN message

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;
 

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();

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()