From 882e916fe27d0061aa2316af415b1ae37db8957b Mon Sep 17 00:00:00 2001 From: KonradIT Date: Wed, 15 Jan 2025 23:15:12 +0100 Subject: [PATCH 1/3] add rpi proxy script --- scripts/rpi-proxy-fc.py | 80 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 scripts/rpi-proxy-fc.py diff --git a/scripts/rpi-proxy-fc.py b/scripts/rpi-proxy-fc.py new file mode 100644 index 0000000..c336086 --- /dev/null +++ b/scripts/rpi-proxy-fc.py @@ -0,0 +1,80 @@ +#!/usr/bin/python +import os +import sys +import serial +import time +import json + +from yamspy import MSPy + +## Constants + +# INAV Konrad custom FW +INAV_KONRAD_MAX_NAME_LENGTH = 16 +INAV_KONRAD_SET_PILOT_NAME = 0x5000 + +LORA_SA_PORT = "/dev/ttyS0" +DRONE_PORT = "/dev/ttyACM0" + + +def get_heading(board: MSPy) -> float: + board.fast_read_analog() + board.fast_read_attitude() + board.fast_read_imu() + + return int(board.SENSOR_DATA["kinematics"][2]) # heading value + + +def str2osd(pilot_name): + pilot_name_bytes = pilot_name.encode("utf-8") + pilot_name_bytes = pilot_name_bytes[:INAV_KONRAD_MAX_NAME_LENGTH].ljust( + INAV_KONRAD_MAX_NAME_LENGTH, b" " + ) + return pilot_name_bytes + + +with MSPy(device=DRONE_PORT, loglevel="WARNING", baudrate=115200) as board: + if board == 1: + print("Connecting to the FC... FAILED!") + sys.exit(1) + else: + try: + lora = serial.Serial(LORA_SA_PORT, 115200, timeout=5) + except: + for _ in range(10): + board.send_RAW_msg( + INAV_KONRAD_SET_PILOT_NAME, str2osd("err: lora board") + ) + time.sleep(2) + board.send_RAW_msg( + INAV_KONRAD_SET_PILOT_NAME, str2osd("restart needed") + ) + time.sleep(2) + sys.exit(1) + board.send_RAW_msg(INAV_KONRAD_SET_PILOT_NAME, str2osd("! lora sa !")) + time.sleep(20) + while True: + # tick every: + time.sleep(0.5) + + # read json data from lora sa: + line = lora.readline() + if len(line) == 0: + continue + + print(line) + + 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}") + board.send_RAW_msg(INAV_KONRAD_SET_PILOT_NAME, osd_text) + heading = get_heading(board) + lora.write(f"HEADING {heading}\n".encode("utf-8")) From 390728230990e5afddabe52c8e8ead62b2fcb5ab Mon Sep 17 00:00:00 2001 From: KonradIT Date: Thu, 16 Jan 2025 00:12:19 +0100 Subject: [PATCH 2/3] 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 From 25c214765520fad9453e0ebeb2e42abc7e382e34 Mon Sep 17 00:00:00 2001 From: KonradIT Date: Thu, 16 Jan 2025 00:19:10 +0100 Subject: [PATCH 3/3] fixes --- scripts/rpi-proxy-fc.py | 40 ++++++++++++++++++++++++++++++---------- 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/scripts/rpi-proxy-fc.py b/scripts/rpi-proxy-fc.py index 87960ab..66ce68f 100644 --- a/scripts/rpi-proxy-fc.py +++ b/scripts/rpi-proxy-fc.py @@ -10,15 +10,20 @@ from yamspy import MSPy ## Constants # INAV Konrad custom FW +# https://github.com/KonradIT/inav/tree/set-pilot-name-msp INAV_KONRAD_MAX_NAME_LENGTH = 16 INAV_KONRAD_SET_PILOT_NAME = 0x5000 -LORA_SA_PORT = "COM6" -DRONE_PORT = "COM4" + +LORA_SA_PORT = "/dev/ttyS0" +DRONE_PORT = "/dev/ttyACM0" + +# For testing on Windows: +# LORA_SA_PORT = "COM6" +# DRONE_PORT = "COM4" # lifted from SpectrumScan.py - def parse_line(line): """Parse a JSON line from the serial input.""" @@ -39,12 +44,13 @@ def crc16(s, c): return c ^ 0xffff +# use MSP to get the heading def get_heading(board: MSPy) -> float: board.fast_read_analog() board.fast_read_attitude() board.fast_read_imu() - return int(board.SENSOR_DATA["kinematics"][2]) # heading value + return int(board.SENSOR_DATA["kinematics"][2]) # heading (z) value def str2osd(pilot_name): @@ -54,8 +60,9 @@ def str2osd(pilot_name): ) return pilot_name_bytes +# get the highest reading from the entire scan array def get_candidates(data): - highest_rssi = -100 + highest_rssi = -120 highest_freq = 100 for item in data: if item[1] > highest_rssi: @@ -69,8 +76,10 @@ with MSPy(device=DRONE_PORT, loglevel="WARNING", baudrate=115200) as board: sys.exit(1) else: try: + # Attempt to connect to the Lora ESP SA over serial, use 115200 baudrate lora = serial.Serial(LORA_SA_PORT, 115200, timeout=5) except: + # just some basic error display with the FC OSD for _ in range(10): board.send_RAW_msg( INAV_KONRAD_SET_PILOT_NAME, str2osd("err: lora board") @@ -81,11 +90,21 @@ with MSPy(device=DRONE_PORT, loglevel="WARNING", baudrate=115200) as board: ) time.sleep(2) sys.exit(1) + + # On to sending data: board.send_RAW_msg(INAV_KONRAD_SET_PILOT_NAME, str2osd("! lora sa !")) - time.sleep(5) + time.sleep(20) + + # Tell Lora ESP SA to start sending data over serial lora.write(f"SCAN -1 -1\n".encode("utf-8")) while True: - # read json data from lora sa: + # read incoming WRAP and CRC data from lora sa: + # how it looks: + """ + SCAN_RESULT 258 [ (830000, -110), (830449, -110), (830898, -110), (831347, -110), + (831796, -110), (832245, -110), (832694, -110), (833143, -110), (833592, -110), + (834041, -110), (834490, -110), .... + """ try: line = lora.readline().decode('utf-8') except UnicodeDecodeError: @@ -94,6 +113,7 @@ with MSPy(device=DRONE_PORT, loglevel="WARNING", baudrate=115200) as board: if len(line) == 0: continue + # Lifted from SpectrumScan.py if 'WRAP ' in line: try: _, c, rest = line.split(' ', 2) @@ -111,12 +131,12 @@ with MSPy(device=DRONE_PORT, loglevel="WARNING", baudrate=115200) as board: continue try: count, data = parse_line(line) + except json.JSONDecodeError: + continue + finally: 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