From 165b0e929f0910a567fed1ca4b0033ef501eba5f Mon Sep 17 00:00:00 2001 From: Bron2002 Date: Tue, 21 May 2024 13:56:33 +0300 Subject: [PATCH] AP_GPS: add checking of instance number before update with incoming data --- libraries/AP_GPS/AP_GPS_MAV.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index 68f0ef9576..b9100f5f17 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -47,6 +47,11 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) mavlink_gps_input_t packet; mavlink_msg_gps_input_decode(&msg, &packet); + // check if target instance belongs to incoming gps data. + if (state.instance != packet.gps_id) { + return; + } + bool have_alt = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_ALT) == 0); bool have_hdop = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HDOP) == 0); bool have_vdop = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VDOP) == 0); @@ -145,6 +150,11 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) mavlink_hil_gps_t packet; mavlink_msg_hil_gps_decode(&msg, &packet); + // check if target instance belongs to incoming gps data. + if (state.instance != packet.id) { + return; + } + state.time_week = 0; state.time_week_ms = packet.time_usec/1000; state.status = (AP_GPS::GPS_Status)packet.fix_type;