From c4c865b93814591fe5c65c2707c6aa97635cb46d Mon Sep 17 00:00:00 2001
From: Oleksiy Protas <elfy@ecognize.me>
Date: Thu, 19 Sep 2024 20:36:42 +0300
Subject: [PATCH] Noise reporting in MavLink (#370)

---
 wfb_ng/protocols.py        | 13 ++++++++-----
 wfb_ng/proxy.py            |  6 +++---
 wfb_ng/tests/test_proxy.py |  4 ++--
 3 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/wfb_ng/protocols.py b/wfb_ng/protocols.py
index ddf18ba8..5ab51ec2 100644
--- a/wfb_ng/protocols.py
+++ b/wfb_ng/protocols.py
@@ -241,7 +241,8 @@ def aggregate_stats(self):
         packet_stats_by_rx = dict((rx_id, packet_stats) for rx_id, (ant_stats, packet_stats) in cur_stats.items())
 
         stats_agg = self._stats_agg_by_freq_and_rxid(ant_stats_by_rx)
-        card_rssi_l = list(rssi_avg
+        # (rssi,noise) tuples
+        card_rssi_l = list((rssi_avg, rssi_avg - snr_avg)
                            for pkt_s,
                                rssi_min, rssi_avg, rssi_max,
                                snr_min, snr_avg, snr_max
@@ -258,17 +259,19 @@ def aggregate_stats(self):
 
             if not card_rssi_l:
                 flags |= WFBFlags.LINK_LOST
+                mav_rssi, mav_noise = -128, -128
 
-            elif bad_packets > 0:
-                flags |= WFBFlags.LINK_JAMMED
+            else:
+                if bad_packets > 0:
+                    flags |= WFBFlags.LINK_JAMMED
+                mav_rssi, mav_noise = max(card_rssi_l)
 
             rx_errors = sum(p['dec_err'][_idx] + p['bad'][_idx] + p['lost'][_idx] for p in packet_stats_by_rx.values())
             rx_fec = sum(p['fec_rec'][_idx] for p in packet_stats_by_rx.values())
-            mav_rssi = (max(card_rssi_l) if card_rssi_l else -128) % 256
 
             for rssi_cb in self.rssi_cb_l:
                 try:
-                    rssi_cb(mav_rssi, min(rx_errors, 65535), min(rx_fec, 65535), flags)
+                    rssi_cb(mav_rssi, mav_noise, min(rx_errors, 65535), min(rx_fec, 65535), flags)
                 except Exception:
                     log.err()
 
diff --git a/wfb_ng/proxy.py b/wfb_ng/proxy.py
index 450b540f..0eae53d4 100644
--- a/wfb_ng/proxy.py
+++ b/wfb_ng/proxy.py
@@ -84,7 +84,7 @@ def messageReceived(self, data):
             self.agg_queue_timer = reactor.callLater(self.agg_timeout, self.flush_queue)
 
     # inject radio rssi info
-    def send_rssi(self, rssi, rx_errors, rx_fec, flags):
+    def send_rssi(self, rssi, noise, rx_errors, rx_fec, flags):
         pass
 
 
@@ -125,11 +125,11 @@ def __init__(self, agg_max_size, agg_timeout,
         else:
             self.radio_mav = None
 
-    def send_rssi(self, rssi, rx_errors, rx_fec, flags):
+    def send_rssi(self, rssi, noise, rx_errors, rx_fec, flags):
         # Send flags as remnoise, because txbuf value is used by PX4 to throttle bandwidth
         # use self.write to send mavlink message
         if self.radio_mav is not None:
-            self.radio_mav.radio_status_send(rssi, rssi, 100, 0, flags, rx_errors, rx_fec)
+            self.radio_mav.radio_status_send(rssi % 256, rssi % 256, 100, noise % 256, flags, rx_errors, rx_fec)
 
 
 class MavlinkUDPProxyProtocol(DatagramProtocol, MavlinkProxyProtocol):
diff --git a/wfb_ng/tests/test_proxy.py b/wfb_ng/tests/test_proxy.py
index 8cd12160..120b5efa 100644
--- a/wfb_ng/tests/test_proxy.py
+++ b/wfb_ng/tests/test_proxy.py
@@ -82,10 +82,10 @@ def test_rssi_injection(self):
         yield df_sleep(0.1)
 
         try:
-            self.p1.send_rssi(1, 2, 3, 4)
+            self.p1.send_rssi(1, 2, 3, 4, 5)
             ts = time.time()
             _replies = yield p.df
-            _expected = [(b'\xfd\t\x00\x00\x00\x03\xf2m\x00\x00\x02\x00\x03\x00\x01\x01d\x00\x04\xa8\xad', addr)]
+            _expected = [(b'\xfd\t\x00\x00\x00\x03\xf2m\x00\x00\x03\x00\x04\x00\x01\x01d\x02\x05\xe1\xb1', addr)]
             self.assertLess(time.time() - ts, 1.0)
             self.assertEqual(_replies, _expected)
         finally: