AP_GPS: add checking of instance number before update with incoming data

ArduPilot · May 29, 2024 · d699208 · d699208
1 parent 1c9a44c
commit d699208
Showing 1 changed file with 10 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,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;