Plane: support SET_GPS_GLOBAL_ORIGIN message

ArduPlane/GCS_Mavlink.cpp

@@ -895,6 +895,22 @@ void GCS_MAVLINK_Plane::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;
+        plane.set_ekf_origin(ekf_origin);
+        break;
+    }
+
     case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
     {
         handle_request_data_stream(msg, true);
@@ -1185,6 +1201,10 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         case MAV_CMD_GET_HOME_POSITION:
             if (plane.home_is_set != HOME_UNSET) {
                 send_home(plane.ahrs.get_home());
+                Location ekf_origin;
+                if (plane.ahrs.get_origin(ekf_origin)) {
+                    send_ekf_origin(ekf_origin);
+                }
                 result = MAV_RESULT_ACCEPTED;
             } else {
                 result = MAV_RESULT_FAILED;

ArduPlane/Plane.h

@@ -885,6 +885,7 @@ private:
     void set_guided_WP(void);
     void init_home();
     void update_home();
+    void set_ekf_origin(const Location& loc);
     void do_RTL(int32_t alt);
     bool verify_takeoff();
     bool verify_loiter_unlim();

ArduPlane/commands.cpp

@@ -145,3 +145,29 @@ void Plane::update_home()
     }
     barometer.update_calibration();
 }
+
+// sets ekf_origin if it has not been set.
+//  should only be used when there is no GPS to provide an absolute position
+void Plane::set_ekf_origin(const Location& loc)
+{
+    // check location is valid
+    if (!check_latlng(loc)) {
+        return;
+    }
+
+    // check if 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);
+}

ArduPlane/commands_logic.cpp

@@ -906,6 +906,11 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
         home_is_set = HOME_SET_NOT_LOCKED;
         Log_Write_Home_And_Origin();
         gcs().send_home(cmd.content.location);
+        // send ekf origin if set
+        Location ekf_origin;
+        if (ahrs.get_origin(ekf_origin)) {
+            gcs().send_ekf_origin(ekf_origin);
+        }
     }
 }