Ref /issues/214

svpcom · Apr 14, 2022 · 6bc5a53 · 6bc5a53
1 parent bbeb1ae
commit 6bc5a53
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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