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