Better filtering of MAVLink components

MAVProxy/modules/lib/mp_module.py

@@ -205,9 +205,11 @@ def link_label(link):
             label = str(link.linknum+1)
         return label
 
-    def is_primary_vehicle(self, msg):
+    def message_is_from_primary_vehicle(self, msg):
         '''see if a msg is from our primary vehicle'''
         sysid = msg.get_srcSystem()
-        if self.target_system == 0 or self.target_system == sysid:
+        compid = msg.get_srcComponent()
+        if ((self.target_system == 0 or self.target_system == sysid) and
+           (self.target_component == 0 or self.target_component == compid)):
             return True
         return False

MAVProxy/modules/mavproxy_console.py

@@ -336,7 +336,7 @@ def mavlink_packet(self, msg):
         if type == 'SYS_STATUS':
             self.check_critical_error(msg)
 
-        if not self.is_primary_vehicle(msg):
+        if not self.message_is_from_primary_vehicle(msg):
             # don't process msgs from other than primary vehicle, other than
             # updating vehicle list
             return
@@ -540,13 +540,6 @@ def mavlink_packet(self, msg):
             self.console.set_status('PWR', status, fg=fg)
             self.console.set_status('Srv', 'Srv %.2f' % (msg.Vservo*0.001), fg='green')
         elif type in ['HEARTBEAT', 'HIGH_LATENCY2']:
-            if msg.get_srcComponent() in [mavutil.mavlink.MAV_COMP_ID_ADSB,
-                                          mavutil.mavlink.MAV_COMP_ID_ODID_TXRX_1,
-                                          mavutil.mavlink.MAV_COMP_ID_ODID_TXRX_2,
-                                          mavutil.mavlink.MAV_COMP_ID_ODID_TXRX_3]:
-                # ignore these
-                return
-
             fmode = master.flightmode
             if self.settings.vehicle_name:
                 fmode = self.settings.vehicle_name + ':' + fmode

MAVProxy/modules/mavproxy_fence.py

@@ -218,7 +218,7 @@ def handle_sys_status(self, m):
             self.console.set_status('Fence', 'FEN', row=0, fg='red')
 
     def mavlink_packet(self, m):
-        if m.get_type() == 'SYS_STATUS':
+        if m.get_type() == 'SYS_STATUS' and self.message_is_from_primary_vehicle(m):
             self.handle_sys_status(m)
         super(FenceModule, self).mavlink_packet(m)
 

MAVProxy/modules/mavproxy_link.py

@@ -615,7 +615,7 @@ def heartbeat_is_from_autopilot(self, m):
 
     def master_msg_handling(self, m, master):
         '''link message handling for an upstream link'''
-        if self.settings.target_system != 0 and m.get_srcSystem() != self.settings.target_system:
+        if not self.message_is_from_primary_vehicle(m):
             # don't process messages not from our target
             if m.get_type() == "BAD_DATA":
                 if self.mpstate.settings.shownoise and mavutil.all_printable(m.data):
@@ -927,12 +927,8 @@ def master_callback(self, m, master):
                 if mtype_instance not in self.status.msg_count:
                     self.status.msg_count[mtype_instance] = 0
                 self.status.msg_count[mtype_instance] += 1
-
-        if m.get_srcComponent() == mavutil.mavlink.MAV_COMP_ID_GIMBAL and mtype == 'HEARTBEAT':
-            # silence gimbal heartbeat packets for now
-            return
 
-        if getattr(m, 'time_boot_ms', None) is not None and self.settings.target_system == m.get_srcSystem():
+        if getattr(m, 'time_boot_ms', None) is not None and self.message_is_from_primary_vehicle(m):
             # update link_delayed attribute
             self.handle_msec_timestamp(m, master)
 
@@ -967,6 +963,9 @@ def master_callback(self, m, master):
             for (mod,pm) in self.mpstate.modules:
                 if not hasattr(mod, 'mavlink_packet'):
                     continue
+                # Do not send other-system-or-component heartbeat packets to non-multi-vehicle modules
+                if not self.message_is_from_primary_vehicle(m) and not mod.multi_vehicle and mtype == 'HEARTBEAT':
+                    continue
                 # sysid 51/'3' is used by SiK radio for the injected RADIO/RADIO_STATUS mavlink frames.
                 # In order to be able to pass these to e.g. the graph module, which is not multi-vehicle,
                 # special handling is needed, so that the module gets both RADIO_STATUS and (single) target 

MAVProxy/modules/mavproxy_map/__init__.py

@@ -959,7 +959,7 @@ def mavlink_packet(self, m):
                     else:
                         label = None
                     self.map.set_position('Pos' + vehicle, (lat, lon), rotation=heading, label=label, colour=(255,255,255))
-                    self.map.set_follow_object('Pos' + vehicle, self.is_primary_vehicle(m))
+                    self.map.set_follow_object('Pos' + vehicle, self.message_is_from_primary_vehicle(m))
 
         elif mtype == "HIGH_LATENCY2" and self.map_settings.showahrspos:
             (lat, lon) = (m.latitude*1.0e-7, m.longitude*1.0e-7)
@@ -972,7 +972,7 @@ def mavlink_packet(self, m):
                 else:
                     label = None
                 self.map.set_position('Pos' + vehicle, (lat, lon), rotation=cog, label=label, colour=(255,255,255))
-                self.map.set_follow_object('Pos' + vehicle, self.is_primary_vehicle(m))
+                self.map.set_follow_object('Pos' + vehicle, self.message_is_from_primary_vehicle(m))
 
         elif mtype == 'HOME_POSITION':
             (lat, lon) = (m.latitude*1.0e-7, m.longitude*1.0e-7)
@@ -1016,7 +1016,7 @@ def mavlink_packet(self, m):
             else:
                 self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))
 
-        if not self.is_primary_vehicle(m):
+        if not self.message_is_from_primary_vehicle(m):
             # the rest should only be done for the primary vehicle
             return