From 56a269a09f7321759d8abe74f4041c81e96d9528 Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Sun, 13 Nov 2022 14:18:01 +1100 Subject: [PATCH] Modules: Better per-component filtering --- MAVProxy/modules/lib/mp_module.py | 4 +++- MAVProxy/modules/mavproxy_console.py | 7 ------- MAVProxy/modules/mavproxy_link.py | 18 +++++------------- 3 files changed, 8 insertions(+), 21 deletions(-) diff --git a/MAVProxy/modules/lib/mp_module.py b/MAVProxy/modules/lib/mp_module.py index 06d6dca3c3..9291643632 100644 --- a/MAVProxy/modules/lib/mp_module.py +++ b/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 diff --git a/MAVProxy/modules/mavproxy_console.py b/MAVProxy/modules/mavproxy_console.py index a7c78cd26d..7609367268 100644 --- a/MAVProxy/modules/mavproxy_console.py +++ b/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 diff --git a/MAVProxy/modules/mavproxy_link.py b/MAVProxy/modules/mavproxy_link.py index 5648d76c68..1778c782f4 100644 --- a/MAVProxy/modules/mavproxy_link.py +++ b/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