diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 54c37e0e81..676ce1dd78 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -926,17 +926,13 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) rtk_age_ms(1)); } -void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan) +void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) { - if (drivers[0] != nullptr && drivers[0]->highest_supported_status() > AP_GPS::GPS_OK_FIX_3D) { - drivers[0]->send_mavlink_gps_rtk(chan); + if (inst >= GPS_MAX_RECEIVERS) { + return; } -} - -void AP_GPS::send_mavlink_gps2_rtk(mavlink_channel_t chan) -{ - if (drivers[1] != nullptr && drivers[1]->highest_supported_status() > AP_GPS::GPS_OK_FIX_3D) { - drivers[1]->send_mavlink_gps_rtk(chan); + if (drivers[inst] != nullptr && drivers[inst]->highest_supported_status() > AP_GPS::GPS_OK_FIX_3D) { + drivers[inst]->send_mavlink_gps_rtk(chan); } } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index f19af60634..b021a6312c 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -363,8 +363,7 @@ public: void send_mavlink_gps_raw(mavlink_channel_t chan); void send_mavlink_gps2_raw(mavlink_channel_t chan); - void send_mavlink_gps_rtk(mavlink_channel_t chan); - void send_mavlink_gps2_rtk(mavlink_channel_t chan); + void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst); // Returns the index of the first unconfigured GPS (returns GPS_ALL_CONFIGURED if all instances report as being configured) uint8_t first_unconfigured_gps(void) const; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 27e233d279..5b76a34166 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -44,9 +44,7 @@ public: virtual void inject_data(const uint8_t *data, uint16_t len); //MAVLink methods - virtual void send_mavlink_gps_rtk(mavlink_channel_t chan) { return ; } - - virtual void send_mavlink_gps2_rtk(mavlink_channel_t chan) { return ; } + virtual void send_mavlink_gps_rtk(mavlink_channel_t chan); virtual void broadcast_configuration_failure_reason(void) const { return ; }