diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 56c81b545c..38ab91b5c0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -223,6 +223,7 @@ protected: virtual AP_Rally *get_rally() const = 0; virtual Compass *get_compass() const = 0; virtual AP_ServoRelayEvents *get_servorelayevents() const = 0; + virtual AP_GPS *get_gps() const = 0; bool waypoint_receiving; // currently receiving // the following two variables are only here because of Tracker @@ -253,6 +254,7 @@ protected: void handle_param_request_list(mavlink_message_t *msg); void handle_param_request_read(mavlink_message_t *msg); + void handle_common_gps_message(mavlink_message_t *msg); void handle_common_rally_message(mavlink_message_t *msg); void handle_rally_fetch_point(mavlink_message_t *msg); void handle_rally_point(mavlink_message_t *msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 7ed7b48674..a298297be2 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1744,6 +1744,17 @@ void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg) df->Log_Write_Message(text); } + +void GCS_MAVLINK::handle_common_gps_message(mavlink_message_t *msg) +{ + AP_GPS *gps = get_gps(); + if (gps == nullptr) { + return; + } + + gps->handle_msg(msg); +} + /* handle messages which don't require vehicle specific data */ @@ -1800,6 +1811,14 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) handle_common_mission_message(msg); break; + case MAVLINK_MSG_ID_GPS_RTCM_DATA: + /* fall through */ + case MAVLINK_MSG_ID_GPS_INPUT: + /* fall through */ + case MAVLINK_MSG_ID_HIL_GPS: + handle_common_gps_message(msg); + break; + case MAVLINK_MSG_ID_STATUSTEXT: handle_statustext(msg); break; diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 3e08606032..2e91ceeb34 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -17,6 +17,7 @@ protected: Compass *get_compass() const override { return nullptr; }; AP_Mission *get_mission() override { return nullptr; } AP_Rally *get_rally() const override { return nullptr; }; + AP_GPS *get_gps() const override { return nullptr; }; AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; } uint8_t sysid_my_gcs() const override { return 1; } diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index 3794e20f24..cee53d4b0b 100644 --- a/libraries/GCS_MAVLink/examples/routing/routing.cpp +++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp @@ -25,6 +25,7 @@ protected: AP_Mission *get_mission() override { return nullptr; } AP_Rally *get_rally() const override { return nullptr; } AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; } + AP_GPS *get_gps() const override { return nullptr; }; uint8_t sysid_my_gcs() const override { return 1; } private: