add checking of instance number before update with incoming data

ArduPilot · May 21, 2024 · 3482356 · 3482356
1 parent 5e5fde7
commit 3482356
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp
@@ -47,6 +47,10 @@ 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 instatce belong 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);
@@ -144,6 +148,10 @@ 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 instatce belong 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;