From b84e40a804399b4be7055a4c39718eab5d7d9081 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 19 Sep 2017 12:37:09 +1000 Subject: [PATCH] Rover: move handling of SET_GPS_GLOBAL_ORIGIN up --- APMrover2/GCS_Mavlink.cpp | 21 +++++---------------- APMrover2/GCS_Mavlink.h | 1 + 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 46d0758cff..ebec0ff59a 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -644,22 +644,6 @@ void GCS_MAVLINK_Rover::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; - rover.set_ekf_origin(ekf_origin); - break; - } - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); @@ -1386,3 +1370,8 @@ const AP_FWVersion &GCS_MAVLINK_Rover::get_fwver() const { return rover.fwver; } + +void GCS_MAVLINK_Rover::set_ekf_origin(const Location& loc) +{ + rover.set_ekf_origin(loc); +} diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index 3ef77246af..5da8fa8211 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -24,6 +24,7 @@ protected: AP_GPS *get_gps() const override; AP_AdvancedFailsafe *get_advanced_failsafe() const override; const AP_FWVersion &get_fwver() const override; + void set_ekf_origin(const Location& loc) override; uint8_t sysid_my_gcs() const override;