From 390728230990e5afddabe52c8e8ead62b2fcb5ab Mon Sep 17 00:00:00 2001
From: KonradIT <github@chernowii.com>
Date: Thu, 16 Jan 2025 00:12:19 +0100
Subject: [PATCH] No need for JSON parsing if we get the full dump

---
 platformio.ini          |  4 +--
 scripts/rpi-proxy-fc.py | 80 +++++++++++++++++++++++++++++++----------
 2 files changed, 63 insertions(+), 21 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index e9b391f..1d2bf23 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -240,8 +240,8 @@ build_flags =
     -DSAMPLES_RSSI=5
 	-DUSING_LR1121
 	-DINIT_FREQ=900
-    -DFREQ_BEGIN=900
-	-DFREQ_END=1300
+    -DFREQ_BEGIN=830
+	-DFREQ_END=945
 	-DFREQ_RX=2440
 	-DARDUINO_ARCH_ESP32
 	-DARDUINO_USB_CDC_ON_BOOT=1
diff --git a/scripts/rpi-proxy-fc.py b/scripts/rpi-proxy-fc.py
index c336086..87960ab 100644
--- a/scripts/rpi-proxy-fc.py
+++ b/scripts/rpi-proxy-fc.py
@@ -13,10 +13,32 @@ from yamspy import MSPy
 INAV_KONRAD_MAX_NAME_LENGTH = 16
 INAV_KONRAD_SET_PILOT_NAME = 0x5000
 
-LORA_SA_PORT = "/dev/ttyS0"
-DRONE_PORT = "/dev/ttyACM0"
+LORA_SA_PORT = "COM6"
+DRONE_PORT = "COM4"
 
 
+# lifted from SpectrumScan.py
+
+def parse_line(line):
+    """Parse a JSON line from the serial input."""
+
+    line = line[line.index('SCAN_RESULT '):]  # support garbage interleaving with the string
+    _, count, rest = line.split(' ', 2)
+    return int(count), json.loads(rest.replace('(', '[').replace(')', ']'))
+
+POLY = 0x1021
+def crc16(s, c):
+    c = c ^ 0xffff
+    for ch in s:
+        c = c ^ (ord(ch) << 8)
+        for i in range(8):
+            if c & 0x8000:
+                c = ((c << 1) ^ POLY) & 0xffff
+            else:
+                c = (c << 1) & 0xffff
+
+    return c ^ 0xffff
+
 def get_heading(board: MSPy) -> float:
     board.fast_read_analog()
     board.fast_read_attitude()
@@ -32,6 +54,14 @@ def str2osd(pilot_name):
     )
     return pilot_name_bytes
 
+def get_candidates(data):
+    highest_rssi = -100
+    highest_freq = 100
+    for item in data:
+        if item[1] > highest_rssi:
+            highest_rssi = item[1]
+            highest_freq = item[0]
+    return [highest_freq, highest_rssi]
 
 with MSPy(device=DRONE_PORT, loglevel="WARNING", baudrate=115200) as board:
     if board == 1:
@@ -52,29 +82,41 @@ with MSPy(device=DRONE_PORT, loglevel="WARNING", baudrate=115200) as board:
                 time.sleep(2)
             sys.exit(1)
         board.send_RAW_msg(INAV_KONRAD_SET_PILOT_NAME, str2osd("! lora sa !"))
-        time.sleep(20)
+        time.sleep(5)
+        lora.write(f"SCAN -1 -1\n".encode("utf-8"))
         while True:
-            # tick every:
-            time.sleep(0.5)
-
             # read json data from lora sa:
-            line = lora.readline()
+            try:
+                line = lora.readline().decode('utf-8')
+            except UnicodeDecodeError:
+                continue
+
             if len(line) == 0:
                 continue
 
-            print(line)
+            if 'WRAP ' in line:
+                try:
+                    _, c, rest = line.split(' ', 2)
+                    checksum = int(c, 16)
+                    so_far = crc16(rest, 0)
+                except Exception as e:
+                    continue
 
-            try:
-                parsed = json.loads(line)
-            except:
-                time.sleep(0.5)
-                continue
-            finally:
-                if parsed is not None:
-                    low = parsed.get("low_range_freq")
-                    high = parsed.get("high_range_freq")
-                    rssi = parsed.get("value")
-                    osd_text = str2osd(f"{low}-{high}:{rssi}")
+            if 'SCAN_RESULT ' in line:
+                if checksum == -1:
+                    continue
+
+                c16 = crc16(line, so_far)
+                if checksum != c16:
+                    continue
+                try:
+                    count, data = parse_line(line)
+                    data.sort()
+                    candidate = get_candidates(data)
+                    print(candidate)
+                    osd_text = str2osd(f"{candidate[0]}: {candidate[1]}")
                     board.send_RAW_msg(INAV_KONRAD_SET_PILOT_NAME, osd_text)
                     heading = get_heading(board)
                     lora.write(f"HEADING {heading}\n".encode("utf-8"))
+                except json.JSONDecodeError:
+                    continue