Modules: Better per-component filtering

MAVProxy/modules/lib/mp_module.py

@@ -208,6 +208,8 @@ def link_label(link):
     def is_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

@@ -490,13 +490,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_link.py

@@ -541,7 +541,7 @@ def emit_accumulated_statustext(self, key, id, pending):
 
     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.is_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):
@@ -569,13 +569,6 @@ def master_msg_handling(self, m, master):
                 for mav in self.mpstate.mav_master:
                     mav.target_system = self.settings.target_system
 
-            if m.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
-
             if self.status.heartbeat_error:
                 self.status.heartbeat_error = False
                 self.say("heartbeat OK")
@@ -853,12 +846,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.is_primary_vehicle(m):
             # update link_delayed attribute
             self.handle_msec_timestamp(m, master)
 
@@ -893,6 +882,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.is_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