From 6bc5a530229a0fefcce60b8ee277ef3e2fe51193 Mon Sep 17 00:00:00 2001 From: Vasily Evseenko Date: Thu, 14 Apr 2022 19:02:29 +0300 Subject: [PATCH] Ref svpcom/wifibroadcast/issues/214 --- telemetry/conf/master.cfg | 4 ++++ telemetry/proxy.py | 18 ++++++++++++------ telemetry/server.py | 8 ++++++-- telemetry/tests/test_proxy.py | 2 +- 4 files changed, 23 insertions(+), 9 deletions(-) diff --git a/telemetry/conf/master.cfg b/telemetry/conf/master.cfg index 93bd824c..f9e29b91 100644 --- a/telemetry/conf/master.cfg +++ b/telemetry/conf/master.cfg @@ -35,6 +35,8 @@ mirror = None # mirror = 'connect://127.0.0.1:14551' # mirroring mavlink packets to OSD inject_rssi = True # inject RADIO_STATUS packets +mavlink_sys_id = 3 # for injected rssi packets +mavlink_comp_id = 68 # MAV_COMP_ID_TELEMETRY_RADIO # Radio settings for TX and RX bandwidth = 20 # bandwidth 20 or 40 MHz @@ -77,6 +79,8 @@ peer = 'listen://0.0.0.0:14560' # incoming connection mirror = None inject_rssi = True # inject RADIO_STATUS packets +mavlink_sys_id = 3 # for injected rssi packets +mavlink_comp_id = 68 # MAV_COMP_ID_TELEMETRY_RADIO # Radio settings for TX and RX bandwidth = 20 # bandwidth 20 or 40 MHz diff --git a/telemetry/proxy.py b/telemetry/proxy.py index f17b6364..c4c9856b 100644 --- a/telemetry/proxy.py +++ b/telemetry/proxy.py @@ -59,10 +59,12 @@ def _got_signal(f): class ProxyProtocol: - def __init__(self, agg_max_size=None, agg_timeout=None, inject_rssi=False, arm_proto=None): + def __init__(self, agg_max_size=None, agg_timeout=None, inject_rssi=False, arm_proto=None, + mavlink_sys_id=None, mavlink_comp_id=None): + # use self.write to send mavlink message if inject_rssi: - self.radio_mav = mavlink.MAVLink(self, srcSystem=3, srcComponent=242) # WFB + self.radio_mav = mavlink.MAVLink(self, srcSystem=mavlink_sys_id, srcComponent=mavlink_comp_id) # WFB else: self.radio_mav = None @@ -135,8 +137,10 @@ def send_rssi(self, rssi, rx_errors, rx_fec, flags): class UDPProxyProtocol(DatagramProtocol, ProxyProtocol): noisy = False - def __init__(self, addr=None, agg_max_size=None, agg_timeout=None, inject_rssi=False, mirror=None, arm_proto=None): - ProxyProtocol.__init__(self, agg_max_size, agg_timeout, inject_rssi, arm_proto=arm_proto) + def __init__(self, addr=None, agg_max_size=None, agg_timeout=None, inject_rssi=False, mirror=None, + arm_proto=None, mavlink_sys_id=None, mavlink_comp_id=None): + ProxyProtocol.__init__(self, agg_max_size, agg_timeout, inject_rssi, arm_proto=arm_proto, + mavlink_sys_id=mavlink_sys_id, mavlink_comp_id=mavlink_comp_id) self.reply_addr = addr self.fixed_addr = bool(addr) self.mirror = mirror @@ -202,8 +206,10 @@ def write(self, msg): class SerialProxyProtocol(Protocol, ProxyProtocol): noisy = False - def __init__(self, agg_max_size=None, agg_timeout=None, inject_rssi=False, arm_proto=None): - ProxyProtocol.__init__(self, agg_max_size, agg_timeout, inject_rssi, arm_proto=arm_proto) + def __init__(self, agg_max_size=None, agg_timeout=None, inject_rssi=False, arm_proto=None, + mavlink_sys_id=None, mavlink_comp_id=None): + ProxyProtocol.__init__(self, agg_max_size, agg_timeout, inject_rssi, arm_proto=arm_proto, + mavlink_sys_id=mavlink_sys_id, mavlink_comp_id=mavlink_comp_id) self.mavlink_fsm = self.mavlink_parser() self.mavlink_fsm.send(None) diff --git a/telemetry/server.py b/telemetry/server.py index 4e313e71..c0ae5cc7 100644 --- a/telemetry/server.py +++ b/telemetry/server.py @@ -337,14 +337,18 @@ def init_mavlink(profile, wlans): p_in = SerialProxyProtocol(agg_max_size=settings.common.radio_mtu, agg_timeout=settings.common.mavlink_agg_timeout, inject_rssi=cfg.inject_rssi, - arm_proto=arm_proto) + arm_proto=arm_proto, + mavlink_sys_id=cfg.mavlink_sys_id, + mavlink_comp_id=cfg.mavlink_comp_id) else: # The first argument is not None only if we initiate mavlink connection p_in = UDPProxyProtocol(connect, agg_max_size=settings.common.radio_mtu, agg_timeout=settings.common.mavlink_agg_timeout, inject_rssi=cfg.inject_rssi, mirror=mirror, - arm_proto=arm_proto) + arm_proto=arm_proto, + mavlink_sys_id=cfg.mavlink_sys_id, + mavlink_comp_id=cfg.mavlink_comp_id) p_tx_l = [UDPProxyProtocol(('127.0.0.1', cfg.port_tx + i)) for i, _ in enumerate(wlans)] p_rx = UDPProxyProtocol(arm_proto=arm_proto) diff --git a/telemetry/tests/test_proxy.py b/telemetry/tests/test_proxy.py index 24e3a0ab..f933a1ee 100644 --- a/telemetry/tests/test_proxy.py +++ b/telemetry/tests/test_proxy.py @@ -34,7 +34,7 @@ def datagramReceived(self, data, addr): class UDPProxyTestCase(unittest.TestCase): def setUp(self): - self.p1 = UDPProxyProtocol(agg_max_size=1445, agg_timeout=1, inject_rssi=True) + self.p1 = UDPProxyProtocol(agg_max_size=1445, agg_timeout=1, inject_rssi=True, mavlink_sys_id=3, mavlink_comp_id=242) self.p2 = UDPProxyProtocol(('127.0.0.1', 14553)) self.p1.peer = self.p2 self.p2.peer = self.p1