From a930eb0f91454c4d67189e024a2ee9e9c113747d Mon Sep 17 00:00:00 2001 From: Badflar Date: Mon, 19 Aug 2024 01:08:29 -0700 Subject: [PATCH 1/7] feat: add basic outline --- RTTComm/__init__.py | 0 RTTComm/rtt_config.py | 30 +++++++++++++ RTTComm/rtt_exceptions.py | 2 + RTTComm/rtt_lora.py | 88 +++++++++++++++++++++++++++++++++++++++ RTTComm/rtt_packet.py | 42 +++++++++++++++++++ poetry.lock | 20 +++++++++ pyproject.toml | 16 +++++++ 7 files changed, 198 insertions(+) create mode 100644 RTTComm/__init__.py create mode 100644 RTTComm/rtt_config.py create mode 100644 RTTComm/rtt_exceptions.py create mode 100644 RTTComm/rtt_lora.py create mode 100644 RTTComm/rtt_packet.py create mode 100644 poetry.lock create mode 100644 pyproject.toml diff --git a/RTTComm/__init__.py b/RTTComm/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/RTTComm/rtt_config.py b/RTTComm/rtt_config.py new file mode 100644 index 0000000..8ffc341 --- /dev/null +++ b/RTTComm/rtt_config.py @@ -0,0 +1,30 @@ +class RTTConfig: + def __init__(self, frequency=433e6, bandwidth=125e3, spreading_factor=10, coding_rate=8, preamble_length=12, sync_word=0x34, power=20): + # Frequency: Center frequency for LoRa transmission (in Hz) + self.frequency = frequency + + # Bandwidth: Signal bandwidth in Hz. Higher bandwidth allows for faster data rates but reduces sensitivity + self.bandwidth = bandwidth + + # Spreading Factor: Number of chirps per symbol. Higher SF increases range but reduces data rate + self.spreading_factor = spreading_factor + + # Coding Rate: Forward error correction rate. Higher rates offer better protection against interference but reduce data rate + self.coding_rate = coding_rate + + # Preamble Length: Number of symbols sent at the start of a packet. Longer preambles improve reception reliability + self.preamble_length = preamble_length + + # Sync Word: Identifier for network separation (devices must have the same sync word to communicate) + self.sync_word = sync_word + + # Transmit Power: Output power for the LoRa radio (in dBm). Higher power increases range but consumes more energy + self.power = power + + def load_from_file(self, config_file): + # Load settings from a config file + pass + + def save_to_file(self, config_file): + # Save current settings to a config file + pass \ No newline at end of file diff --git a/RTTComm/rtt_exceptions.py b/RTTComm/rtt_exceptions.py new file mode 100644 index 0000000..7c63114 --- /dev/null +++ b/RTTComm/rtt_exceptions.py @@ -0,0 +1,2 @@ +class RTTCommError(Exception): + pass diff --git a/RTTComm/rtt_lora.py b/RTTComm/rtt_lora.py new file mode 100644 index 0000000..bfd32ec --- /dev/null +++ b/RTTComm/rtt_lora.py @@ -0,0 +1,88 @@ +import serial +import time +import threading +from .rtt_config import RTTConfig +from .rtt_exceptions import RTTCommError + +class RTTComm: + def __init__(self, config: RTTConfig, port="/dev/tty/USB0", baudrate=9600, timeout=1, send_receive_interval=0.05): + self.config = config + self.serial_port = serial.Serial(port, baudrate, timeout=timeout) + self.send_receive_interval = send_receive_interval + self.running = False + + def initialize(self): + self.set_frequency(self.config.frequency) + self.set_power(self.config.power) + + def set_frequency(self, frequency): + # Command to set the frequency on the LoRa module + pass + + def set_power(self, power_level): + # Command to set the power level on the LoRa module + pass + + def send_packet(self, packet: bytes): + try: + self.serial_port.write(packet) + except serial.SerialException as e: + raise RTTCommError(f"Failed to send packet: {str(e)}") + + def receive_packet(self) -> bytes: + try: + packet = self.serial_port.read(256) + return packet + except serial.SerialException as e: + raise RTTCommError(f"Failed to receive packet: {str(e)}") + + def start_communication(self, send_function, receive_function): + """Start the communicaiton loop for sending and receiving""" + self.running = True + threading.Thread(target=self._send_and_receive, args=(send_function, receive_function)).start() + + def stop_communication(self): + """Stop the communication loop""" + self.running = False + + def _communication_loop(self, send_function, receive_function): + while self.running: + # Send data + send_function() + # Wait for interval + time.sleep(self.send_receive_interval) + # Receive data + received_packet = self.receive_packet() + if received_packet: + receive_function(received_packet) + # Wait for the inverva before the next send/receive cycle + time.sleep(self.send_receive_interval) + + def close(self): + self.serial_port.close() + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/RTTComm/rtt_packet.py b/RTTComm/rtt_packet.py new file mode 100644 index 0000000..16a10e8 --- /dev/null +++ b/RTTComm/rtt_packet.py @@ -0,0 +1,42 @@ +import struct + +class RTTLoRaPacket: + def __init__(self, frequencies=None, amplitudes=None, latitude=0.0, longitude=0.0, payload_id=0): + self.frequencies = frequencies or [] + self.amplitudes = amplitudes or [] + self.latitude = latitude + self.longitude = longitude + self.payload_id = payload_id + + def to_bytes(self) -> bytes: + data = struct.pack('"] +license = "MIT" +readme = "README.md" + +[tool.poetry.dependencies] +python = "^3.12" +pyserial = "^3.5" + + +[build-system] +requires = ["poetry-core"] +build-backend = "poetry.core.masonry.api" From 3bc93ef1fe7da8c1c6550eabed62dbfb3fcde5fc Mon Sep 17 00:00:00 2001 From: Badflar Date: Tue, 20 Aug 2024 00:23:26 -0700 Subject: [PATCH 2/7] feat: added gcs and lora basic structure --- RTTComm/rtt_config.py | 30 --------- RTTComm/rtt_exceptions.py | 2 - RTTComm/rtt_lora.py | 88 ------------------------ RTTComm/rtt_packet.py | 42 ------------ {RTTComm => rttlora}/__init__.py | 0 rttlora/callbacks.py | 0 rttlora/config.py | 0 rttlora/gcs_comms.py | 96 ++++++++++++++++++++++++++ rttlora/lora_radio.py | 112 +++++++++++++++++++++++++++++++ rttlora/mav_comms.py | 0 rttlora/packets.py | 0 rttlora/transport.py | 0 rttlora/utils.py | 0 13 files changed, 208 insertions(+), 162 deletions(-) delete mode 100644 RTTComm/rtt_config.py delete mode 100644 RTTComm/rtt_exceptions.py delete mode 100644 RTTComm/rtt_lora.py delete mode 100644 RTTComm/rtt_packet.py rename {RTTComm => rttlora}/__init__.py (100%) create mode 100644 rttlora/callbacks.py create mode 100644 rttlora/config.py create mode 100644 rttlora/gcs_comms.py create mode 100644 rttlora/lora_radio.py create mode 100644 rttlora/mav_comms.py create mode 100644 rttlora/packets.py create mode 100644 rttlora/transport.py create mode 100644 rttlora/utils.py diff --git a/RTTComm/rtt_config.py b/RTTComm/rtt_config.py deleted file mode 100644 index 8ffc341..0000000 --- a/RTTComm/rtt_config.py +++ /dev/null @@ -1,30 +0,0 @@ -class RTTConfig: - def __init__(self, frequency=433e6, bandwidth=125e3, spreading_factor=10, coding_rate=8, preamble_length=12, sync_word=0x34, power=20): - # Frequency: Center frequency for LoRa transmission (in Hz) - self.frequency = frequency - - # Bandwidth: Signal bandwidth in Hz. Higher bandwidth allows for faster data rates but reduces sensitivity - self.bandwidth = bandwidth - - # Spreading Factor: Number of chirps per symbol. Higher SF increases range but reduces data rate - self.spreading_factor = spreading_factor - - # Coding Rate: Forward error correction rate. Higher rates offer better protection against interference but reduce data rate - self.coding_rate = coding_rate - - # Preamble Length: Number of symbols sent at the start of a packet. Longer preambles improve reception reliability - self.preamble_length = preamble_length - - # Sync Word: Identifier for network separation (devices must have the same sync word to communicate) - self.sync_word = sync_word - - # Transmit Power: Output power for the LoRa radio (in dBm). Higher power increases range but consumes more energy - self.power = power - - def load_from_file(self, config_file): - # Load settings from a config file - pass - - def save_to_file(self, config_file): - # Save current settings to a config file - pass \ No newline at end of file diff --git a/RTTComm/rtt_exceptions.py b/RTTComm/rtt_exceptions.py deleted file mode 100644 index 7c63114..0000000 --- a/RTTComm/rtt_exceptions.py +++ /dev/null @@ -1,2 +0,0 @@ -class RTTCommError(Exception): - pass diff --git a/RTTComm/rtt_lora.py b/RTTComm/rtt_lora.py deleted file mode 100644 index bfd32ec..0000000 --- a/RTTComm/rtt_lora.py +++ /dev/null @@ -1,88 +0,0 @@ -import serial -import time -import threading -from .rtt_config import RTTConfig -from .rtt_exceptions import RTTCommError - -class RTTComm: - def __init__(self, config: RTTConfig, port="/dev/tty/USB0", baudrate=9600, timeout=1, send_receive_interval=0.05): - self.config = config - self.serial_port = serial.Serial(port, baudrate, timeout=timeout) - self.send_receive_interval = send_receive_interval - self.running = False - - def initialize(self): - self.set_frequency(self.config.frequency) - self.set_power(self.config.power) - - def set_frequency(self, frequency): - # Command to set the frequency on the LoRa module - pass - - def set_power(self, power_level): - # Command to set the power level on the LoRa module - pass - - def send_packet(self, packet: bytes): - try: - self.serial_port.write(packet) - except serial.SerialException as e: - raise RTTCommError(f"Failed to send packet: {str(e)}") - - def receive_packet(self) -> bytes: - try: - packet = self.serial_port.read(256) - return packet - except serial.SerialException as e: - raise RTTCommError(f"Failed to receive packet: {str(e)}") - - def start_communication(self, send_function, receive_function): - """Start the communicaiton loop for sending and receiving""" - self.running = True - threading.Thread(target=self._send_and_receive, args=(send_function, receive_function)).start() - - def stop_communication(self): - """Stop the communication loop""" - self.running = False - - def _communication_loop(self, send_function, receive_function): - while self.running: - # Send data - send_function() - # Wait for interval - time.sleep(self.send_receive_interval) - # Receive data - received_packet = self.receive_packet() - if received_packet: - receive_function(received_packet) - # Wait for the inverva before the next send/receive cycle - time.sleep(self.send_receive_interval) - - def close(self): - self.serial_port.close() - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/RTTComm/rtt_packet.py b/RTTComm/rtt_packet.py deleted file mode 100644 index 16a10e8..0000000 --- a/RTTComm/rtt_packet.py +++ /dev/null @@ -1,42 +0,0 @@ -import struct - -class RTTLoRaPacket: - def __init__(self, frequencies=None, amplitudes=None, latitude=0.0, longitude=0.0, payload_id=0): - self.frequencies = frequencies or [] - self.amplitudes = amplitudes or [] - self.latitude = latitude - self.longitude = longitude - self.payload_id = payload_id - - def to_bytes(self) -> bytes: - data = struct.pack(' Tuple[Optional[str], Optional[str]]: + """ + Recieve a message. + + :return: a tuple containing the recieved message, or None if the message is not intended for this device. + """ + with self.__lock: + if self.serial and self.serial.is_open: + try: + data = self.serial.readline().strip() + if b':' in data: + target_addr, message = data.split(b':', 1) + target_addr = target_addr.decode('ascii') + if target_addr == self.device_addr: + print(f"Recieved message for {self.device_addr}: {message.hex()}") + return message + else: + print(f"Message not intended for this device (intended for {target_addr})") + return None + else: + # If no address is provided, assume broadcast and accept the message + print(f"Recieved broadcast message: {data.hex()}") + return data + except Exception as e: + print(f"Error recieving message: {e}") + return None + else: + print(f"Serial connection not open") + return None + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rttlora/mav_comms.py b/rttlora/mav_comms.py new file mode 100644 index 0000000..e69de29 diff --git a/rttlora/packets.py b/rttlora/packets.py new file mode 100644 index 0000000..e69de29 diff --git a/rttlora/transport.py b/rttlora/transport.py new file mode 100644 index 0000000..e69de29 diff --git a/rttlora/utils.py b/rttlora/utils.py new file mode 100644 index 0000000..e69de29 From b2b90cfc53fe0481ed61776c59c812d33a0cc6e1 Mon Sep 17 00:00:00 2001 From: Vasil Bogdev Date: Fri, 13 Dec 2024 09:44:09 -0800 Subject: [PATCH 3/7] heartbeat and set_ping_config packets complete --- rttsik/__init__.py | 0 rttsik/callbacks.py | 0 rttsik/config.py | 0 rttsik/drone_state.py | 70 +++++++++ rttsik/fds_comms.py | 100 +++++++++++++ rttsik/gcs_comms.py | 148 +++++++++++++++++++ rttsik/packets.py | 321 ++++++++++++++++++++++++++++++++++++++++++ rttsik/sik_radio.py | 85 +++++++++++ rttsik/transport.py | 0 rttsik/utils.py | 0 10 files changed, 724 insertions(+) create mode 100644 rttsik/__init__.py create mode 100644 rttsik/callbacks.py create mode 100644 rttsik/config.py create mode 100644 rttsik/drone_state.py create mode 100644 rttsik/fds_comms.py create mode 100644 rttsik/gcs_comms.py create mode 100644 rttsik/packets.py create mode 100644 rttsik/sik_radio.py create mode 100644 rttsik/transport.py create mode 100644 rttsik/utils.py diff --git a/rttsik/__init__.py b/rttsik/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rttsik/callbacks.py b/rttsik/callbacks.py new file mode 100644 index 0000000..e69de29 diff --git a/rttsik/config.py b/rttsik/config.py new file mode 100644 index 0000000..e69de29 diff --git a/rttsik/drone_state.py b/rttsik/drone_state.py new file mode 100644 index 0000000..c547b1e --- /dev/null +++ b/rttsik/drone_state.py @@ -0,0 +1,70 @@ +"""Module for defining drone state-related classes and enums.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from enum import IntEnum + + +class GPSState(IntEnum): + """Enumeration of GPS states.""" + + NOT_READY = 0 + INITIALIZING = 1 + WAITING = 2 + PARTIAL = 3 + READY = 4 + ERRORED = 5 + + +class PingFinderState(IntEnum): + """Enumeration of possible states for the ping finder.""" + + NOT_READY = 0 + READY = 1 + RUNNING = 2 + STOPPED = 3 + ERRORED = 4 + + +@dataclass +class GPSData: + """Represents GPS coordinate data.""" + + latitude: float | None = None + longitude: float | None = None + altitude: float | None = None + heading: float | None = None + + +@dataclass +class DroneState: + """Represents the current state of a drone, including GPS information.""" + + gps_state: GPSState = GPSState.NOT_READY + gps_data: GPSData = field(default_factory=GPSData) + ping_finder_state: PingFinderState = PingFinderState.NOT_READY + + +# Global instance of DroneState +current_state = DroneState() + + +def update_gps_state(state: GPSState) -> None: + """Update the current GPS state.""" + current_state.gps_state = state + + +def update_gps_data(data: GPSData) -> None: + """Update the current state with new GPS data.""" + current_state.gps_data = data + + +def update_ping_finder_state(state: PingFinderState) -> None: + """Update the current ping finder state.""" + current_state.ping_finder_state = state + + +def get_current_state() -> DroneState: + """Return the current state of the drone.""" + return current_state \ No newline at end of file diff --git a/rttsik/fds_comms.py b/rttsik/fds_comms.py new file mode 100644 index 0000000..1ab0c05 --- /dev/null +++ b/rttsik/fds_comms.py @@ -0,0 +1,100 @@ +import struct +import threading +import enum +import time +from rttsik.packets import * +from typing import List, Optional, Callable, Dict +from rttsik.sik_radio import sikRadio +from rttsik.drone_state import * + +class Events(enum.Enum): + SEND_PING_DETECTION = 0x0 + SEND_HEARTBEAT = 0x1 + SEND_TRANSMITTER_LOC_EST = 0x2 + SEND_CONFIG_ACK = 0x3 + SEND_COMMAND_ACK = 0x4 + PING_FINDER_RECIEVED = 0x5 + START_RECIEVED = 0x6 + RESET_RECIEVED = 0x7 + END_RECIEVED = 0x8 + +class FdsComms: + def __init__(self, sik_radio: sikRadio): + """ + Initialize the fds communication interface. + + :param sik_radio: The sik radio interface. + """ + self.sik_radio = sik_radio + self.__receiver_thread = threading.Thread(target=self.__receiver_loop) + self.__receiver_thread.start() + self.__transmitter_thread: Optional(threading.Thread) = None + self.__heartbeat_thread: Optional(threading.Thread) = None + self.__transmitter_queue = [] + self.transmitter_lock = threading.Lock() + self.__packet_map: Dict[int, List[Callable]] = { + evt.value: [] for evt in Events + } + self.__running = False + + self.packet_ctr = 0 + self.drone_latitude_degrees: Optional[float] = None + self.drone_longiude_degrees: Optional[float] = None + self.drone_altitude_meters: Optional[float] = None + self.drone_heading_degrees: Optional[float] = None + self.gps_state = GPSState.NOT_READY + self.ping_finder_state = PingFinderState.NOT_READY + + def start(self): + """ + Start the GCS communication interface. + """ + self.__running = True + + self.__transmitter_thread = threading.Thread(target=self.__transmitter_loop) + self.__heartbeat_thread = threading.Thread(target=self.__heartbeat_loop) + self.__heartbeat_thread.start() + self.__transmitter_thread.start() + print("FdsComms communications started") + + def __receiver_loop(self): + """ + Internal loop to recieve messages from FdsComms. + """ + while True: + message = self.sik_radio.recieve() + if message: + self.__process_message(message) + + time.sleep(0.1) + + def __process_message(self, message: bytes): + packet_type = Packet.get_type(message) + if(Packet.check_crc(message)): + if(packet_type[0] == 0x6): + self.__process_config(message) + + + def __process_config(self, packet: bytes): + config = Set_Ping_Finder_Config_Packet.from_bytes(packet) + + pass + + def __heartbeat_loop(self): + while self.__running: + self.transmitter_lock.acquire() + # add a heartbeat to the transmitter queue + hb_packet = Heartbeat_Packet(self.packet_ctr, self.drone_latitude_degrees, self.drone_longiude_degrees, self.drone_altitude_meters, self.drone_heading_degrees, + self.gps_state, self.ping_finder_state) + self.sik_radio.send(hb_packet.to_packet()) + self.packet_ctr = self.packet_ctr + 1 + self.transmitter_lock.release() + time.sleep(5) + + def __transmitter_loop(self): + while self.__running: + self.transmitter_lock.acquire() + if len(self.__transmitter_queue) > 0: + self.sik_radio.send(self.__transmitter_queue[0]) + self.__transmitter_queue[0].pop(0) + self.transmitter_lock.release() \ No newline at end of file diff --git a/rttsik/gcs_comms.py b/rttsik/gcs_comms.py new file mode 100644 index 0000000..073cafc --- /dev/null +++ b/rttsik/gcs_comms.py @@ -0,0 +1,148 @@ +import threading +import enum +from typing import List, Optional, Callable, Dict +from rttsik.sik_radio import sikRadio +from rttsik.packets import * + +class Events(enum.Enum): + PING_DETECTION_RECIEVED = 0x0 + HEARTBEAT_RECIEVED = 0x1 + TRANSMITTER_LOC_EST_RECIEVED = 0x2 + CONFIG_ACK_RECIEVED = 0x3 + COMMAND_ACK_RECIEVED = 0x4 + SEND_PING_FINDER = 0x5 + SEND_START = 0x6 + SEND_RESET = 0x7 + SEND_END = 0x8 + +class GcsCommications: + def __init__(self, sik_radio: sikRadio): + """ + Initialize the GCS communication interface. + + :param sik_radio: The sik radio interface. + """ + self.sik_radio = sik_radio + self.packet_ctr = 0 + self.fdscomms_list: List[str] = [] # List of known FdsComms addresses + self.__receiver_thread: Optional(threading.Thread) = None + self.__transmitter_thread: Optional(threading.Thread) = None + self.__transmitter_queue = [] + self.__packet_map: Dict[int, List[Callable]] = { + evt.value: [] for evt in Events + } + self.__running = False + self.lock = threading.Lock() + + def add_radio(self, addr): + self.fdscomms_list.append(addr) + + def start(self): + """ + Start the GCS communication interface. + """ + self.__running = True + self.__receiver_thread = threading.Thread(target=self.__receiver_loop) + self.__transmitter_thread = threading.Thread(target=self.__transmitter_loop) + self.__receiver_thread.start() + print("GCS communications started") + + def __receiver_loop(self): + """ + Internal loop to recieve messages from FdsComms. + """ + while self.__running: + message = self.sik_radio.recieve() + if message: + self.__process_message(message) + + def __transmitter_loop(self): + while self.__running: + pass + + def __process_message(self, message: bytes): + """ + Process incoming messages from FdsComms. + + :param message: The message recieved from a FdsComm. + """ + type = Packet.get_type(message) + + # testing to make sure data recieved is correct + print(message.hex()) + if(type[0] == 2): + print("Printing data") + hb_packet = Heartbeat_Packet.from_bytes(message) + print(hb_packet.type) + print(hb_packet.id) + print(hb_packet.drone_latitude_degrees) + print(hb_packet.drone_longitude_degrees) + print(hb_packet.drone_altitude_meters) + print(hb_packet.drone_heading_degrees) + print(hb_packet.gps_state) + print(hb_packet.ping_finder_state) + # Decode header + # Based off of header, accesses callables list from dictionary + # Iterate through list + pass + + + def set_ping_config(self, run_number_id: int, target_frequencies_hz: list[int], sampling_rate_hz: Optional[int] = None, gain_db: Optional[float] = None, + center_frequency_hz: Optional[int] = None, ping_width_ms: Optional[int] = None, ping_min_snr: Optional[int] = None, ping_max_length_multiplier: + Optional[float] = None, ping_min_length_multiplier: Optional[float] = None): + # ACQUIRE LOCK FOR QUEUE + self.lock.acquire() + # create packet and place in transmitter queue + config_packet = Set_Ping_Finder_Config_Packet(self.packet_ctr, run_number_id, target_frequencies_hz, sampling_rate_hz, gain_db, center_frequency_hz, + ping_width_ms, ping_min_snr, ping_max_length_multiplier, ping_min_length_multiplier) + self.packet_ctr = self.packet_ctr + 1 + # send packet + self.send_message(config_packet.to_packet()) + # RELEASE LOCK FOR QUEUE + self.lock.release() + + def send_message(self, message: bytes, target_addr: Optional[str] = None): + """ + Send a message to a specific FdsComm or broadcast to all. + + :param message: The message to send. + :param target_addr: The address of the target FdsComm, or None to broadcast. + """ + if target_addr: + self.sik_radio.send(message, target_addr) + else: + # Broadcast to all known FdsComms + for addr in self.fdscomm_list: + self.sik_radio.send(message, addr) + print(f"GCS sent messsage: {message.hex()} to {'all' if target_addr is None else target_addr}") + + + def stop(self): + """ + Stop the GCS communication interface. + """ + self.__running = False + if self.__receiver_thread: + self.__receiver_thread.join() + print("GCS communications stopped") + + def add_fdscomm(self, fdscomm_addr: str): + """ + Add a MacComm to the list of known FdsComms. + + :param addr: The address of the FdsComm to add. + """ + if fdscomm_addr not in self.fdscomms_list: + self.fdscomms_list.append(fdscomm_addr) + print(f"GCS added FdsComm: {fdscomm_addr}") + + def remove_fdscomm(self, fdscomm_addr: str): + """ + Remove a FdsComm from the list of known FdsComm. + + :param fdscomm_addr: The address of the FdsComm to remove. + """ + if fdscomm_addr in self.fdscomms_list: + self.fdscomms_list.remove(fdscomm_addr) + print(f"GCS removed FdsComm: {fdscomm_addr}") + diff --git a/rttsik/packets.py b/rttsik/packets.py new file mode 100644 index 0000000..218973c --- /dev/null +++ b/rttsik/packets.py @@ -0,0 +1,321 @@ +import binascii +import struct +from typing import Optional +from datetime import datetime + +# placeholder packet class +class Packet: + def __init__(self, id: int, packet_sent_timestamp: datetime): + self.type = 0x0 + self.id = id + self.data_length = 0 + self.packet_sent_timestamp = packet_sent_timestamp + + def to_packet(self): + header = self.get_header() + msg = header + crc = binascii.crc32(msg) + return msg + crc + + #def get_id(self): + # return self.id + + #def get_type(self): + # return self.type + + def get_header(self): + header = struct.pack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Ping_Detection_Packet(id, 0, 0.0, 0.0, 0.0, 0, datetime.now()) + rebuilt_packet.drone_latitude_degrees, rebuilt_packet.drone_longitude_degrees, rebuilt_packet.drone_altitude_meters, rebuilt_packet.transmitter_power_db, rebuilt_packet.transmitter_frequency_hz = struct.unpack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Heartbeat_Packet(id) + rebuilt_packet.drone_latitude_degrees, rebuilt_packet.drone_latitude_degrees_valid, rebuilt_packet.drone_longitude_degrees, rebuilt_packet.drone_longitude_degrees_valid, rebuilt_packet.drone_altitude_meters, rebuilt_packet.drone_altitude_meters_valid, rebuilt_packet.drone_heading_degrees, rebuilt_packet.gps_state, rebuilt_packet.ping_finder_state = struct.unpack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Set_Ping_Finder_Config_Packet(id, 0, [1]) + rebuilt_packet.run_number_id = super().get_run_number_id(packet) + rebuilt_packet.target_frequencies_hz_length = struct.unpack(' Tuple[Optional[str], Optional[str]]: + """ + Recieve a message. + + :return: a tuple containing the recieved message, or None if the message is not intended for this device. + """ + # data = self.serial.readline().strip() + with self.__lock: + if self.serial and self.serial.is_open: + try: + data = self.serial.readline().strip() + if b':' in data: + target_addr, message = data.split(b':', 1) + target_addr = target_addr.decode('ascii') + if target_addr == self.device_addr: + print(f"Recieved message for {self.device_addr}: {message.hex()}") + return message + else: + print(f"Message not intended for this device (intended for {target_addr})") + return None + else: + # If no address is provided, assume broadcast and accept the message + # print(f"Recieved broadcast message: {data.hex()}") + return data + except Exception as e: + print(f"Error recieving message: {e}") + return None + else: + print(f"Serial connection not open") + return None \ No newline at end of file diff --git a/rttsik/transport.py b/rttsik/transport.py new file mode 100644 index 0000000..e69de29 diff --git a/rttsik/utils.py b/rttsik/utils.py new file mode 100644 index 0000000..e69de29 From 523ff4aac4b9b0433e18d1e7f6a601a97c6c695b Mon Sep 17 00:00:00 2001 From: Vasil Bogdev Date: Fri, 13 Dec 2024 14:38:53 -0800 Subject: [PATCH 4/7] tweaked some packets slightly, added a few checks to avoid errors --- pyproject.toml | 2 +- rttsik/fds_comms.py | 7 +++-- rttsik/gcs_comms.py | 10 ++++-- rttsik/packets.py | 76 +++++++++++++++++++++++++++++++++++++++------ test2.py | 25 +++++++++++++++ 5 files changed, 104 insertions(+), 16 deletions(-) create mode 100644 test2.py diff --git a/pyproject.toml b/pyproject.toml index f491066..516c753 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,5 +1,5 @@ [tool.poetry] -name = "rtt-lora-comms-package" +name = "rtt-sik-comms-package" version = "0.1.0" description = "" authors = ["Tyler Flar "] diff --git a/rttsik/fds_comms.py b/rttsik/fds_comms.py index 1ab0c05..ef343d2 100644 --- a/rttsik/fds_comms.py +++ b/rttsik/fds_comms.py @@ -64,7 +64,8 @@ def __receiver_loop(self): while True: message = self.sik_radio.recieve() if message: - self.__process_message(message) + if(len(message) >= 20): # can't do a checksum if you don't have the checksum + self.__process_message(message) time.sleep(0.1) @@ -73,6 +74,8 @@ def __process_message(self, message: bytes): if(Packet.check_crc(message)): if(packet_type[0] == 0x6): self.__process_config(message) + else: + print("fail") def __process_config(self, packet: bytes): @@ -89,7 +92,7 @@ def __heartbeat_loop(self): self.sik_radio.send(hb_packet.to_packet()) self.packet_ctr = self.packet_ctr + 1 self.transmitter_lock.release() - time.sleep(5) + time.sleep(2) def __transmitter_loop(self): while self.__running: diff --git a/rttsik/gcs_comms.py b/rttsik/gcs_comms.py index 073cafc..9761385 100644 --- a/rttsik/gcs_comms.py +++ b/rttsik/gcs_comms.py @@ -54,7 +54,8 @@ def __receiver_loop(self): while self.__running: message = self.sik_radio.recieve() if message: - self.__process_message(message) + if(len(message) >= 20): # can't do a checksum if the packet doesn't contain a checksum + self.__process_message(message) def __transmitter_loop(self): while self.__running: @@ -69,8 +70,10 @@ def __process_message(self, message: bytes): type = Packet.get_type(message) # testing to make sure data recieved is correct + + print("Message:") print(message.hex()) - if(type[0] == 2): + if(Packet.check_crc(message)): print("Printing data") hb_packet = Heartbeat_Packet.from_bytes(message) print(hb_packet.type) @@ -84,7 +87,8 @@ def __process_message(self, message: bytes): # Decode header # Based off of header, accesses callables list from dictionary # Iterate through list - pass + else: + print("failed") def set_ping_config(self, run_number_id: int, target_frequencies_hz: list[int], sampling_rate_hz: Optional[int] = None, gain_db: Optional[float] = None, diff --git a/rttsik/packets.py b/rttsik/packets.py index 218973c..7154598 100644 --- a/rttsik/packets.py +++ b/rttsik/packets.py @@ -4,6 +4,7 @@ from datetime import datetime # placeholder packet class +# packet contains a type, id, payload size and timestamp in the header, plus a 4 byte checksum at the end class Packet: def __init__(self, id: int, packet_sent_timestamp: datetime): self.type = 0x0 @@ -172,10 +173,20 @@ def __init__(self, id: int, transmitter_frequency_hz: int, transmitter_latitude_ def to_packet(self): header = super().get_header() - msg = header + struct.pack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Trx_Loc_Estimation_Packet(id, 0, 0.0, 0.0, 0.0, datetime.now()) + rebuilt_packet.transmitter_frequency_hz, rebuilt_packet.transmitter_latitude_degrees, rebuilt_packet.transmitter_longitude_degrees, rebuilt_packet.transmitter_altitude_meters = struct.unpack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Config_Ack_Packet(id, 0, 0, datetime.now()) + rebuilt_packet.success, rebuilt_packet.message = struct.unpack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Command_Ack_Packet(id, 0, 0, datetime.now()) + rebuilt_packet.success, rebuilt_packet.message = struct.unpack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Config_Ack_Packet(id, 0, 0, datetime.now()) + rebuilt_packet.success = struct.unpack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Command_Ack_Packet(id, 0, 0, datetime.now()) + rebuilt_packet.success, rebuilt_packet.command_type = struct.unpack(' Packet: + id = super().get_id(packet)[0] + payload = super().get_payload(packet) + rebuilt_packet = Command_Ack_Packet(id, 0, 0, datetime.now()) + rebuilt_packet.command_type = struct.unpack(' Date: Tue, 24 Dec 2024 01:40:22 -0800 Subject: [PATCH 5/7] feat: Refactor and enhance radio telemetry tracker drone communications package --- .github/workflows/lint.yml | 40 + .github/workflows/test.yml | 40 + .gitignore | 4 + README.md | 139 ++- poetry.lock | 20 - pyproject.toml | 29 +- .../__init__.py | 42 + .../codec.py | 94 ++ .../data_models.py | 130 +++ .../drone_comms.py | 969 ++++++++++++++++++ .../interfaces.py | 230 +++++ .../proto/__init__.py | 43 + .../proto/compiler.py | 43 + .../proto/packets.proto | 114 +++ .../transceiver.py | 186 ++++ rttlora/__init__.py | 0 rttlora/callbacks.py | 0 rttlora/config.py | 0 rttlora/gcs_comms.py | 96 -- rttlora/lora_radio.py | 112 -- rttlora/mav_comms.py | 0 rttlora/packets.py | 0 rttlora/transport.py | 0 rttlora/utils.py | 0 rttsik/__init__.py | 0 rttsik/callbacks.py | 0 rttsik/config.py | 0 rttsik/drone_state.py | 70 -- rttsik/fds_comms.py | 103 -- rttsik/gcs_comms.py | 152 --- rttsik/packets.py | 377 ------- rttsik/sik_radio.py | 85 -- rttsik/transport.py | 0 rttsik/utils.py | 0 test2.py | 25 - tests/__init__.py | 1 + tests/test_codec.py | 63 ++ tests/test_compiler.py | 86 ++ tests/test_data_models.py | 110 ++ tests/test_drone_comms.py | 138 +++ tests/test_interfaces.py | 92 ++ tests/test_proto_init.py | 39 + tests/test_transceiver.py | 139 +++ tools/__init__.py | 1 + tools/gcs_fds_cli.py | 653 ++++++++++++ 45 files changed, 3421 insertions(+), 1044 deletions(-) create mode 100644 .github/workflows/lint.yml create mode 100644 .github/workflows/test.yml delete mode 100644 poetry.lock create mode 100644 radio_telemetry_tracker_drone_comms_package/__init__.py create mode 100644 radio_telemetry_tracker_drone_comms_package/codec.py create mode 100644 radio_telemetry_tracker_drone_comms_package/data_models.py create mode 100644 radio_telemetry_tracker_drone_comms_package/drone_comms.py create mode 100644 radio_telemetry_tracker_drone_comms_package/interfaces.py create mode 100644 radio_telemetry_tracker_drone_comms_package/proto/__init__.py create mode 100644 radio_telemetry_tracker_drone_comms_package/proto/compiler.py create mode 100644 radio_telemetry_tracker_drone_comms_package/proto/packets.proto create mode 100644 radio_telemetry_tracker_drone_comms_package/transceiver.py delete mode 100644 rttlora/__init__.py delete mode 100644 rttlora/callbacks.py delete mode 100644 rttlora/config.py delete mode 100644 rttlora/gcs_comms.py delete mode 100644 rttlora/lora_radio.py delete mode 100644 rttlora/mav_comms.py delete mode 100644 rttlora/packets.py delete mode 100644 rttlora/transport.py delete mode 100644 rttlora/utils.py delete mode 100644 rttsik/__init__.py delete mode 100644 rttsik/callbacks.py delete mode 100644 rttsik/config.py delete mode 100644 rttsik/drone_state.py delete mode 100644 rttsik/fds_comms.py delete mode 100644 rttsik/gcs_comms.py delete mode 100644 rttsik/packets.py delete mode 100644 rttsik/sik_radio.py delete mode 100644 rttsik/transport.py delete mode 100644 rttsik/utils.py delete mode 100644 test2.py create mode 100644 tests/__init__.py create mode 100644 tests/test_codec.py create mode 100644 tests/test_compiler.py create mode 100644 tests/test_data_models.py create mode 100644 tests/test_drone_comms.py create mode 100644 tests/test_interfaces.py create mode 100644 tests/test_proto_init.py create mode 100644 tests/test_transceiver.py create mode 100644 tools/__init__.py create mode 100644 tools/gcs_fds_cli.py diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml new file mode 100644 index 0000000..8cd62f1 --- /dev/null +++ b/.github/workflows/lint.yml @@ -0,0 +1,40 @@ +name: Run Ruff Linter + +on: + pull_request: + branches: + - main + - dev + +jobs: + lint: + runs-on: ubuntu-latest + + steps: + - name: Check out code + uses: actions/checkout@v3 + + - name: Set up Python 3.12 + uses: actions/setup-python@v4 + with: + python-version: '3.12' + + - name: Cache dependencies + uses: actions/cache@v3 + with: + path: | + ~/.cache/pypoetry + ~/.cache/pip + key: ${{ runner.os }}-poetry-${{ hashFiles('poetry.lock') }} + restore-keys: | + ${{ runner.os }}-poetry- + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install poetry + poetry install --with dev + + - name: Run ruff linter + run: | + poetry run ruff check . \ No newline at end of file diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml new file mode 100644 index 0000000..ffcb89c --- /dev/null +++ b/.github/workflows/test.yml @@ -0,0 +1,40 @@ +name: Run Tests + +on: + pull_request: + branches: + - main + - dev + +jobs: + test: + runs-on: ubuntu-latest + + steps: + - name: Check out code + uses: actions/checkout@v3 + + - name: Set up Python 3.12 + uses: actions/setup-python@v4 + with: + python-version: '3.12' + + - name: Cache dependencies + uses: actions/cache@v3 + with: + path: | + ~/.cache/pypoetry + ~/.cache/pip + key: ${{ runner.os }}-poetry-${{ hashFiles('poetry.lock') }} + restore-keys: | + ${{ runner.os }}-poetry- + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install poetry + poetry install --with dev + + - name: Run tests + run: | + poetry run pytest \ No newline at end of file diff --git a/.gitignore b/.gitignore index 82f9275..0103daf 100644 --- a/.gitignore +++ b/.gitignore @@ -160,3 +160,7 @@ cython_debug/ # and can be added to the global gitignore or merged into this file. For a more nuclear # option (not recommended) you can uncomment the following to ignore the entire idea folder. #.idea/ + +poetry.lock +*_pb2.py +.ruff_cache/ diff --git a/README.md b/README.md index 9d8b17b..b3e883b 100644 --- a/README.md +++ b/README.md @@ -1 +1,138 @@ -# rtt-lora-comms-package \ No newline at end of file +# Radio Telemetry Tracker Drone Communications Package (Comms Package) + +The **Radio Telemetry Tracker Drone Communications Package** is a Python-based library designed to facilitate the transmission, reception, and handling of protobuf-based radio packets between a drone's field device software ([FDS](https://github.com/UCSD-E4E/radio-telemetry-tracker-drone-fds)) and the ground control station ([GCS](https://github.com/UCSD-E4E/radio-telemetry-tracker-drone-gcs)). It implements reliable communication through packet acknowledgements, serialization/deserialization of Protobuf messages, and provides an extendable structure for new message types. + +> Note: This package is not intended for end-user use in standalone mode. Rather, it is a shared component that is consumed by the FDS and GCS repositories. Therefore, detailed user-facing instructions are provided in the respective repositories where this package is integrated. + +## Table of Contents +- [Radio Telemetry Tracker Drone Communications Package (Comms Package)](#radio-telemetry-tracker-drone-communications-package-comms-package) + - [Table of Contents](#table-of-contents) + - [Overview](#overview) + - [Prerequisites](#prerequisites) + - [Installation](#installation) + - [Configuration](#configuration) + - [Usage](#usage) + - [Troubleshooting](#troubleshooting) + - [License](#license) + +## Overview + +This package provides: + +- **Packet Definitions**: Protobuf message definitions for radio telemetry data and requests +- **Codec**: Encoding/decoding logic with framing, CRC checks, and synchronization markers +- **Radio Interfaces**: + - **SerialRadioInterface**: For physical serial (UART) connections + - **SimulatedRadioInterface**: For TCP-based simulation and testing +- **Packet Management**: Reliable transmission with retry and acknowledgment (ACK) mechanisms +- **DroneComms Class**: High-level API with typed handlers and callback registration + +## Prerequisites + +- Python 3.12 or later +- Poetry 1.8 or later +- For serial communication: `pyserial` +- For protocol buffers: `protobuf`, `grpcio-tools` + +## Installation + +1. Add as a dependency to your project: + + ```bash + poetry add git+https://github.com/UCSD-E4E/radio-telemetry-tracker-drone-comms-package.git + ``` + +2. Or clone for development: + ```bash + git clone https://github.com/UCSD-E4E/radio-telemetry-tracker-drone-comms-package.git + cd radio-telemetry-tracker-drone-comms-package + poetry install + ``` + +## Configuration + +The library supports two interface types: + +1. **Serial Interface**: + + ```python + from radio_telemetry_tracker_drone_comms_package import RadioConfig + + config = RadioConfig( + interface_type="serial", + port="/dev/ttyUSB0", # Serial port + baudrate=56700, # Communication speed + ) +``` + +2. **Simulated Interface** (for testing): + + ```python + config = RadioConfig( + interface_type="simulated", + host="localhost", # TCP host + tcp_port=50000, # TCP port + server_mode=False, # Client/server mode + ) + ``` + +## Usage + +Basic usage pattern: + +```python +from radio_telemetry_tracker_drone_comms_package import DroneComms, RadioConfig, GPSData +``` + +1. **Initialize communications**: + ```python + config = RadioConfig(interface_type="serial", port="/dev/ttyUSB0") + comms = DroneComms(radio_config=config) + ``` + +2. **Register handlers for incoming packets**: + ```python + def on_gps_data(data: GPSData): + print(f"GPS: {data.easting}, {data.northing}, {data.altitude}") + +3. **Start communication**: + ```python + comms.start() # Opens the radio interface and starts Rx/Tx threads + ``` + +4. **Send packets as needed**: + ```python + comms.send_sync_request(SyncRequestData()) + ``` + +5. **Clean up when done**: + ```python + comms.stop() # Closes the radio interface and stops threads + ``` + + +## Troubleshooting + +Common issues and solutions: + +- **No Packets Received** + - Check physical connections (serial) or network connectivity (TCP) + - Verify correct port/baudrate settings + - Look for exceptions in logs + +- **CRC Errors** + - Ensure matching Protobuf versions between FDS and GCS + - Check for packet framing issues + - Verify byte order consistency + +- **Timeouts** + - Increase `read_timeout` or `ack_timeout` for slow/unreliable links + - Check for network congestion or interference + - Verify both ends are running and properly configured + +## License + +This project is licensed under the terms specified in the [LICENSE](LICENSE) file. + + + diff --git a/poetry.lock b/poetry.lock deleted file mode 100644 index e54529d..0000000 --- a/poetry.lock +++ /dev/null @@ -1,20 +0,0 @@ -# This file is automatically @generated by Poetry 1.7.1 and should not be changed by hand. - -[[package]] -name = "pyserial" -version = "3.5" -description = "Python Serial Port Extension" -optional = false -python-versions = "*" -files = [ - {file = "pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0"}, - {file = "pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb"}, -] - -[package.extras] -cp2110 = ["hidapi"] - -[metadata] -lock-version = "2.0" -python-versions = "^3.12" -content-hash = "bda6546f3a50848091149f6122b46de5910395fb02333649582bcbd68eeaa576" diff --git a/pyproject.toml b/pyproject.toml index 516c753..0f5022f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,16 +1,39 @@ [tool.poetry] -name = "rtt-sik-comms-package" +name = "radio-telemetry-tracker-drone-comms-package" version = "0.1.0" description = "" authors = ["Tyler Flar "] -license = "MIT" -readme = "README.md" +license = "Other" +packages = [ + { include = "radio_telemetry_tracker_drone_comms_package" }, +] [tool.poetry.dependencies] python = "^3.12" pyserial = "^3.5" +protobuf = "^5.29.2" +grpcio-tools = "^1.68.1" +[tool.poetry.group.dev.dependencies] +ruff = "^0.8.3" +pytest = "^8.3.3" +pytest-mock = "^3.14.0" + +[tool.ruff] +line-length = 120 +exclude = ["radio_telemetry_tracker_drone_comms_package/proto/packets_pb2.py"] + +[tool.ruff.lint] +select = ["ALL"] + +[tool.ruff.lint.pydocstyle] +convention = "google" + +[tool.pytest.ini_options] +testpaths = ["tests"] +addopts = "--maxfail=5 --tb=short" + [build-system] requires = ["poetry-core"] build-backend = "poetry.core.masonry.api" diff --git a/radio_telemetry_tracker_drone_comms_package/__init__.py b/radio_telemetry_tracker_drone_comms_package/__init__.py new file mode 100644 index 0000000..0cd7da1 --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/__init__.py @@ -0,0 +1,42 @@ +"""radio_telemetry_tracker_drone_comms_package. + +Package initialization for the Radio Telemetry Tracker Drone Comms Package. +""" + +__version__ = "0.1.0" + +from radio_telemetry_tracker_drone_comms_package.drone_comms import ( + DroneComms, + RadioConfig, +) +from radio_telemetry_tracker_drone_comms_package.data_models import ( + SyncRequestData, + SyncResponseData, + ConfigRequestData, + ConfigResponseData, + GPSData, + LocEstData, + PingData, + StartRequestData, + StartResponseData, + StopRequestData, + StopResponseData, + ErrorData, +) + +__all__ = [ + "DroneComms", + "RadioConfig", + "SyncRequestData", + "SyncResponseData", + "ConfigRequestData", + "ConfigResponseData", + "GPSData", + "LocEstData", + "PingData", + "StartRequestData", + "StartResponseData", + "StopRequestData", + "StopResponseData", + "ErrorData", +] diff --git a/radio_telemetry_tracker_drone_comms_package/codec.py b/radio_telemetry_tracker_drone_comms_package/codec.py new file mode 100644 index 0000000..5e7852e --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/codec.py @@ -0,0 +1,94 @@ +"""Packet encoding and decoding utilities for radio telemetry communications.""" + +from __future__ import annotations + +import google.protobuf.message + +from radio_telemetry_tracker_drone_comms_package.proto import RadioPacket + +SYNC_MARKER = b"\xAA\x55" +SYNC_MARKER_LENGTH = len(SYNC_MARKER) +LENGTH_FIELD_SIZE = 4 # Size of the length field in bytes +CHECKSUM_SIZE = 2 # Size of the CRC-CCITT checksum in bytes + + +def _calculate_crc16_ccitt(data: bytes) -> int: + """Calculate a 16-bit CRC-CCITT checksum.""" + crc = 0xFFFF + poly = 0x1021 + for b in data: + crc ^= b << 8 + for _ in range(8): + if crc & 0x8000: + crc = (crc << 1) ^ poly + else: + crc <<= 1 + crc &= 0xFFFF + return crc + + +class RadioCodec: + """Provides static methods to encode and decode RadioPacket messages.""" + + @staticmethod + def encode_packet(packet: RadioPacket) -> bytes: + """Encode a RadioPacket into a byte array with the sync marker, length header, and CRC-CCITT.""" + message_data = packet.SerializeToString() + length = len(message_data) + + # data_without_checksum = SYNC_MARKER + LENGTH(4 bytes) + MESSAGE_DATA + header = SYNC_MARKER + length.to_bytes(LENGTH_FIELD_SIZE, byteorder="big") + data_without_checksum = header + message_data + + checksum_val = _calculate_crc16_ccitt(data_without_checksum) + checksum_bytes = checksum_val.to_bytes(CHECKSUM_SIZE, byteorder="big") + return data_without_checksum + checksum_bytes + + @staticmethod + def decode_packet(data: bytes) -> RadioPacket | None: + """Decode a byte array into a RadioPacket, validating sync marker, length, and CRC-CCITT.""" + min_length = ( + SYNC_MARKER_LENGTH + LENGTH_FIELD_SIZE + CHECKSUM_SIZE + ) # sync + length + checksum + if len(data) >= min_length and data[:SYNC_MARKER_LENGTH] == SYNC_MARKER: + length = int.from_bytes( + data[SYNC_MARKER_LENGTH : SYNC_MARKER_LENGTH + LENGTH_FIELD_SIZE], + "big", + ) + expected_length = ( + SYNC_MARKER_LENGTH + LENGTH_FIELD_SIZE + length + CHECKSUM_SIZE + ) + if len(data) == expected_length: + message_data = data[ + SYNC_MARKER_LENGTH + + LENGTH_FIELD_SIZE : SYNC_MARKER_LENGTH + + LENGTH_FIELD_SIZE + + length + ] + checksum_in = int.from_bytes( + data[ + SYNC_MARKER_LENGTH + + LENGTH_FIELD_SIZE + + length : SYNC_MARKER_LENGTH + + LENGTH_FIELD_SIZE + + length + + CHECKSUM_SIZE + ], + "big", + ) + + # Verify checksum + if ( + _calculate_crc16_ccitt( + data[: SYNC_MARKER_LENGTH + LENGTH_FIELD_SIZE + length], + ) + == checksum_in + ): + # Decode Protobuf + try: + packet = RadioPacket() + packet.ParseFromString(message_data) + except google.protobuf.message.DecodeError: + return None + return packet + return None diff --git a/radio_telemetry_tracker_drone_comms_package/data_models.py b/radio_telemetry_tracker_drone_comms_package/data_models.py new file mode 100644 index 0000000..3ac21b8 --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/data_models.py @@ -0,0 +1,130 @@ +"""Data classes for user-facing packet data structures.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass +class SyncRequestData: + """Data container for synchronization request packets.""" + + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class SyncResponseData: + """Data container for synchronization response packets.""" + + success: bool + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class ConfigRequestData: + """Data container for configuration request packets.""" + + gain: float + sampling_rate: int + center_frequency: int + run_num: int + enable_test_data: bool + ping_width_ms: int + ping_min_snr: int + ping_max_len_mult: float + ping_min_len_mult: float + target_frequencies: list[int] + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class ConfigResponseData: + """Data container for configuration response packets.""" + + success: bool + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class GPSData: + """Data container for GPS location packets.""" + + easting: float + northing: float + altitude: float + heading: float + epsg_code: int + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class PingData: + """Data container for radio ping detection packets.""" + + frequency: int + amplitude: float + easting: float + northing: float + altitude: float + epsg_code: int + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class LocEstData: + """Data container for location estimation packets.""" + + frequency: int + easting: float + northing: float + epsg_code: int + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class StartRequestData: + """Data container for start request packets.""" + + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class StartResponseData: + """Data container for start response packets.""" + + success: bool + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class StopRequestData: + """Data container for stop request packets.""" + + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class StopResponseData: + """Data container for stop response packets.""" + + success: bool + packet_id: int | None = None + timestamp: int | None = None + + +@dataclass +class ErrorData: + """Data container for error packets.""" + + packet_id: int | None = None + timestamp: int | None = None diff --git a/radio_telemetry_tracker_drone_comms_package/drone_comms.py b/radio_telemetry_tracker_drone_comms_package/drone_comms.py new file mode 100644 index 0000000..30ad8be --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/drone_comms.py @@ -0,0 +1,969 @@ +"""High-level DroneComms class for user-facing interaction with radio telemetry packet communication.""" + +from __future__ import annotations + +import logging +from dataclasses import dataclass +from typing import Callable, TypeVar + +from radio_telemetry_tracker_drone_comms_package.data_models import ( + ConfigRequestData, + ConfigResponseData, + ErrorData, + GPSData, + LocEstData, + PingData, + StartRequestData, + StartResponseData, + StopRequestData, + StopResponseData, + SyncRequestData, + SyncResponseData, +) +from radio_telemetry_tracker_drone_comms_package.interfaces import ( + SerialRadioInterface, + SimulatedRadioInterface, +) +from radio_telemetry_tracker_drone_comms_package.proto import ( + ConfigRequestPacket, + ConfigResponsePacket, + ErrorPacket, + GPSPacket, + LocEstPacket, + PingPacket, + RadioPacket, + StartRequestPacket, + StartResponsePacket, + StopRequestPacket, + StopResponsePacket, + SyncRequestPacket, + SyncResponsePacket, +) +from radio_telemetry_tracker_drone_comms_package.transceiver import ( + PacketManager, + _current_timestamp_us, +) + +T = TypeVar("T") +logger = logging.getLogger(__name__) + + +@dataclass +class RadioConfig: + """Configuration for radio interface.""" + + interface_type: str = "serial" + port: str | None = None + baudrate: int = 56700 + host: str = "localhost" + tcp_port: int = 50000 + server_mode: bool = False + + +class DroneComms(PacketManager): + """Manages radio telemetry packet communication between drone and base station.""" + + def __init__( + self, + radio_config: RadioConfig = None, + ack_timeout: float = 2.0, + max_retries: int = 5, + on_ack_callback: Callable[[RadioPacket], None] | None = None, + ) -> None: + """Initialize DroneComms instance. + + Args: + radio_config: Configuration for radio interface + ack_timeout: Timeout in seconds for acknowledgment packets + max_retries: Maximum number of packet retransmission attempts + on_ack_callback: Optional callback function when acknowledgment is received + """ + if radio_config is None: + msg = "Radio config is required" + raise ValueError(msg) + + if radio_config.interface_type == "serial": + if radio_config.port is None: + msg = "Serial port must be specified for serial interface" + raise ValueError(msg) + radio_interface = SerialRadioInterface( + port=radio_config.port, + baudrate=radio_config.baudrate, + timeout=ack_timeout, + ) + elif radio_config.interface_type == "simulated": + radio_interface = SimulatedRadioInterface( + host=radio_config.host, + port=radio_config.tcp_port, + server_mode=radio_config.server_mode, + timeout=ack_timeout, + ) + else: + msg = f"Invalid interface type: {radio_config.interface_type}" + raise ValueError(msg) + + super().__init__( + radio_interface=radio_interface, + ack_timeout=ack_timeout, + max_retries=max_retries, + on_ack_timeout=on_ack_callback, + ) + + # Each entry: "proto_field_name": (extractor_function, handler_function) + self._packet_handlers = { + "syn_rqt": (self._extract_sync_request, self._handle_sync_request), + "syn_rsp": (self._extract_sync_response, self._handle_sync_response), + "cfg_rqt": (self._extract_config_request, self._handle_config_request), + "cfg_rsp": (self._extract_config_response, self._handle_config_response), + "gps_pkt": (self._extract_gps_data, self._handle_gps_data), + "ping_pkt": (self._extract_ping_data, self._handle_ping_data), + "loc_pkt": (self._extract_loc_est_data, self._handle_loc_est_data), + "str_rqt": (self._extract_start_request, self._handle_start_request), + "str_rsp": (self._extract_start_response, self._handle_start_response), + "stp_rqt": (self._extract_stop_request, self._handle_stop_request), + "stp_rsp": (self._extract_stop_response, self._handle_stop_response), + "err_pkt": (self._extract_error, self._handle_error), + } + + # Handlers for different packet types: [ (callback, once), ... ] + self._sync_request_handlers: list[ + tuple[Callable[[SyncRequestData], None], bool] + ] = [] + self._sync_response_handlers: list[ + tuple[Callable[[SyncResponseData], None], bool] + ] = [] + self._config_request_handlers: list[ + tuple[Callable[[ConfigRequestData], None], bool] + ] = [] + self._config_response_handlers: list[ + tuple[Callable[[ConfigResponseData], None], bool] + ] = [] + self._gps_handlers: list[tuple[Callable[[GPSData], None], bool]] = [] + self._ping_handlers: list[tuple[Callable[[PingData], None], bool]] = [] + self._loc_est_handlers: list[tuple[Callable[[LocEstData], None], bool]] = [] + self._start_request_handlers: list[ + tuple[Callable[[StartRequestData], None], bool] + ] = [] + self._start_response_handlers: list[ + tuple[Callable[[StartResponseData], None], bool] + ] = [] + self._stop_request_handlers: list[ + tuple[Callable[[StopRequestData], None], bool] + ] = [] + self._stop_response_handlers: list[ + tuple[Callable[[StopResponseData], None], bool] + ] = [] + self._error_handlers: list[tuple[Callable[[ErrorData], None], bool]] = [] + + def on_user_packet_received(self, packet: RadioPacket) -> None: + """Handle received user packets by delegating to typed handlers. Overridden from PacketManager.""" + field = packet.WhichOneof("msg") + handler_entry = self._packet_handlers.get(field) + if not handler_entry: + logger.debug("Received an unhandled packet type: %s", field) + return + + extractor, handler = handler_entry + data_object = extractor(getattr(packet, field)) + handler(data_object) + + # -------------------------------------------------------------------------- + # Handler registration/unregistration (public) + # -------------------------------------------------------------------------- + + def register_sync_request_handler( + self, + callback: Callable[[SyncRequestData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for sync request packets. + + Args: + callback: Function to call when sync request is received + once: If True, handler is removed after first invocation + """ + self._sync_request_handlers.append((callback, once)) + + def unregister_sync_request_handler( + self, + callback: Callable[[SyncRequestData], None], + ) -> bool: + """Unregister a previously registered sync request handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._sync_request_handlers: + if cb == callback: + self._sync_request_handlers.remove((cb, once)) + return True + return False + + def register_sync_response_handler( + self, + callback: Callable[[SyncResponseData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for sync response packets. + + Args: + callback: Function to call when sync response is received + once: If True, handler is removed after first invocation + """ + self._sync_response_handlers.append((callback, once)) + + def unregister_sync_response_handler( + self, + callback: Callable[[SyncResponseData], None], + ) -> bool: + """Unregister a previously registered sync response handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._sync_response_handlers: + if cb == callback: + self._sync_response_handlers.remove((cb, once)) + return True + return False + + def register_config_request_handler( + self, + callback: Callable[[ConfigRequestData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for config request packets. + + Args: + callback: Function to call when config request is received + once: If True, handler is removed after first invocation + """ + self._config_request_handlers.append((callback, once)) + + def unregister_config_request_handler( + self, + callback: Callable[[ConfigRequestData], None], + ) -> bool: + """Unregister a previously registered config request handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._config_request_handlers: + if cb == callback: + self._config_request_handlers.remove((cb, once)) + return True + return False + + def register_config_response_handler( + self, + callback: Callable[[ConfigResponseData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for config response packets. + + Args: + callback: Function to call when config response is received + once: If True, handler is removed after first invocation + """ + self._config_response_handlers.append((callback, once)) + + def unregister_config_response_handler( + self, + callback: Callable[[ConfigResponseData], None], + ) -> bool: + """Unregister a previously registered config response handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._config_response_handlers: + if cb == callback: + self._config_response_handlers.remove((cb, once)) + return True + return False + + def register_gps_handler( + self, + callback: Callable[[GPSData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for GPS data packets. + + Args: + callback: Function to call when GPS data is received + once: If True, handler is removed after first invocation + """ + self._gps_handlers.append((callback, once)) + + def unregister_gps_handler(self, callback: Callable[[GPSData], None]) -> bool: + """Unregister a previously registered GPS data handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._gps_handlers: + if cb == callback: + self._gps_handlers.remove((cb, once)) + return True + return False + + def register_ping_handler( + self, + callback: Callable[[PingData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for ping data packets. + + Args: + callback: Function to call when ping data is received + once: If True, handler is removed after first invocation + """ + self._ping_handlers.append((callback, once)) + + def unregister_ping_handler(self, callback: Callable[[PingData], None]) -> bool: + """Unregister a previously registered ping data handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._ping_handlers: + if cb == callback: + self._ping_handlers.remove((cb, once)) + return True + return False + + def register_loc_est_handler( + self, + callback: Callable[[LocEstData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for location estimation data packets. + + Args: + callback: Function to call when location estimation data is received + once: If True, handler is removed after first invocation + """ + self._loc_est_handlers.append((callback, once)) + + def unregister_loc_est_handler( + self, + callback: Callable[[LocEstData], None], + ) -> bool: + """Unregister a previously registered location estimation data handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._loc_est_handlers: + if cb == callback: + self._loc_est_handlers.remove((cb, once)) + return True + return False + + def register_start_request_handler( + self, + callback: Callable[[StartRequestData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for start request packets. + + Args: + callback: Function to call when start request is received + once: If True, handler is removed after first invocation + """ + self._start_request_handlers.append((callback, once)) + + def unregister_start_request_handler( + self, + callback: Callable[[StartRequestData], None], + ) -> bool: + """Unregister a previously registered start request handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._start_request_handlers: + if cb == callback: + self._start_request_handlers.remove((cb, once)) + return True + return False + + def register_start_response_handler( + self, + callback: Callable[[StartResponseData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for start response packets. + + Args: + callback: Function to call when start response is received + once: If True, handler is removed after first invocation + """ + self._start_response_handlers.append((callback, once)) + + def unregister_start_response_handler( + self, + callback: Callable[[StartResponseData], None], + ) -> bool: + """Unregister a previously registered start response handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._start_response_handlers: + if cb == callback: + self._start_response_handlers.remove((cb, once)) + return True + return False + + def register_stop_request_handler( + self, + callback: Callable[[StopRequestData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for stop request packets. + + Args: + callback: Function to call when stop request is received + once: If True, handler is removed after first invocation + """ + self._stop_request_handlers.append((callback, once)) + + def unregister_stop_request_handler( + self, + callback: Callable[[StopRequestData], None], + ) -> bool: + """Unregister a previously registered stop request handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._stop_request_handlers: + if cb == callback: + self._stop_request_handlers.remove((cb, once)) + return True + return False + + def register_stop_response_handler( + self, + callback: Callable[[StopResponseData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for stop response packets. + + Args: + callback: Function to call when stop response is received + once: If True, handler is removed after first invocation + """ + self._stop_response_handlers.append((callback, once)) + + def unregister_stop_response_handler( + self, + callback: Callable[[StopResponseData], None], + ) -> bool: + """Unregister a previously registered stop response handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._stop_response_handlers: + if cb == callback: + self._stop_response_handlers.remove((cb, once)) + return True + return False + + def register_error_handler( + self, + callback: Callable[[ErrorData], None], + *, + once: bool = False, + ) -> None: + """Register a handler for error packets. + + Args: + callback: Function to call when error packet is received + once: If True, handler is removed after first invocation + """ + self._error_handlers.append((callback, once)) + + def unregister_error_handler(self, callback: Callable[[ErrorData], None]) -> bool: + """Unregister a previously registered error handler. + + Args: + callback: The handler function to remove + + Returns: + bool: True if handler was found and removed, False otherwise + """ + for cb, once in self._error_handlers: + if cb == callback: + self._error_handlers.remove((cb, once)) + return True + return False + + # -------------------------------------------------------------------------- + # Public send methods + # -------------------------------------------------------------------------- + + def send_sync_request(self, _: SyncRequestData) -> tuple[int, bool, int]: + """Send a sync request packet. + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.syn_rqt.base.packet_id = self.generate_packet_id() + packet.syn_rqt.base.need_ack = True + packet.syn_rqt.base.timestamp = _current_timestamp_us() + self.enqueue_packet(packet) + return ( + packet.syn_rqt.base.packet_id, + packet.syn_rqt.base.need_ack, + packet.syn_rqt.base.timestamp, + ) + + def send_sync_response(self, data: SyncResponseData) -> tuple[int, bool, int]: + """Send a sync response packet. + + Args: + data: Sync response data to send + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.syn_rsp.base.packet_id = self.generate_packet_id() + packet.syn_rsp.base.need_ack = False + packet.syn_rsp.base.timestamp = _current_timestamp_us() + packet.syn_rsp.success = data.success + self.enqueue_packet(packet) + return ( + packet.syn_rsp.base.packet_id, + packet.syn_rsp.base.need_ack, + packet.syn_rsp.base.timestamp, + ) + + def send_config_request(self, data: ConfigRequestData) -> tuple[int, bool, int]: + """Send a config request packet. + + Args: + data: Config request data to send + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.cfg_rqt.base.packet_id = self.generate_packet_id() + packet.cfg_rqt.base.need_ack = True + packet.cfg_rqt.base.timestamp = _current_timestamp_us() + packet.cfg_rqt.gain = data.gain + packet.cfg_rqt.sampling_rate = data.sampling_rate + packet.cfg_rqt.center_frequency = data.center_frequency + packet.cfg_rqt.run_num = data.run_num + packet.cfg_rqt.enable_test_data = data.enable_test_data + packet.cfg_rqt.ping_width_ms = data.ping_width_ms + packet.cfg_rqt.ping_min_snr = data.ping_min_snr + packet.cfg_rqt.ping_max_len_mult = data.ping_max_len_mult + packet.cfg_rqt.ping_min_len_mult = data.ping_min_len_mult + packet.cfg_rqt.target_frequencies.extend(data.target_frequencies) + self.enqueue_packet(packet) + return ( + packet.cfg_rqt.base.packet_id, + packet.cfg_rqt.base.need_ack, + packet.cfg_rqt.base.timestamp, + ) + + def send_config_response(self, data: ConfigResponseData) -> tuple[int, bool, int]: + """Send a config response packet. + + Args: + data: Config response data to send + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.cfg_rsp.base.packet_id = self.generate_packet_id() + packet.cfg_rsp.base.need_ack = False + packet.cfg_rsp.base.timestamp = _current_timestamp_us() + packet.cfg_rsp.success = data.success + self.enqueue_packet(packet) + return ( + packet.cfg_rsp.base.packet_id, + packet.cfg_rsp.base.need_ack, + packet.cfg_rsp.base.timestamp, + ) + + def send_gps_data(self, data: GPSData) -> tuple[int, bool, int]: + """Send a GPS data packet. + + Args: + data: GPS data to send + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.gps_pkt.base.packet_id = self.generate_packet_id() + packet.gps_pkt.base.need_ack = False + packet.gps_pkt.base.timestamp = _current_timestamp_us() + packet.gps_pkt.easting = data.easting + packet.gps_pkt.northing = data.northing + packet.gps_pkt.altitude = data.altitude + packet.gps_pkt.heading = data.heading + packet.gps_pkt.epsg_code = data.epsg_code + self.enqueue_packet(packet) + return ( + packet.gps_pkt.base.packet_id, + packet.gps_pkt.base.need_ack, + packet.gps_pkt.base.timestamp, + ) + + def send_ping_data(self, data: PingData) -> tuple[int, bool, int]: + """Send a ping data packet. + + Args: + data: Ping data to send + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.ping_pkt.base.packet_id = self.generate_packet_id() + packet.ping_pkt.base.need_ack = False + packet.ping_pkt.base.timestamp = _current_timestamp_us() + packet.ping_pkt.frequency = data.frequency + packet.ping_pkt.amplitude = data.amplitude + packet.ping_pkt.easting = data.easting + packet.ping_pkt.northing = data.northing + packet.ping_pkt.altitude = data.altitude + packet.ping_pkt.epsg_code = data.epsg_code + self.enqueue_packet(packet) + return ( + packet.ping_pkt.base.packet_id, + packet.ping_pkt.base.need_ack, + packet.ping_pkt.base.timestamp, + ) + + def send_loc_est_data(self, data: LocEstData) -> tuple[int, bool, int]: + """Send a location estimation data packet. + + Args: + data: Location estimation data to send + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.loc_pkt.base.packet_id = self.generate_packet_id() + packet.loc_pkt.base.need_ack = False + packet.loc_pkt.base.timestamp = _current_timestamp_us() + packet.loc_pkt.frequency = data.frequency + packet.loc_pkt.easting = data.easting + packet.loc_pkt.northing = data.northing + packet.loc_pkt.epsg_code = data.epsg_code + self.enqueue_packet(packet) + return ( + packet.loc_pkt.base.packet_id, + packet.loc_pkt.base.need_ack, + packet.loc_pkt.base.timestamp, + ) + + def send_start_request(self, _: StartRequestData) -> tuple[int, bool, int]: + """Send a start request packet. + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.str_rqt.base.packet_id = self.generate_packet_id() + packet.str_rqt.base.need_ack = True + packet.str_rqt.base.timestamp = _current_timestamp_us() + self.enqueue_packet(packet) + return ( + packet.str_rqt.base.packet_id, + packet.str_rqt.base.need_ack, + packet.str_rqt.base.timestamp, + ) + + def send_start_response(self, data: StartResponseData) -> tuple[int, bool, int]: + """Send a start response packet. + + Args: + data: Start response data to send + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.str_rsp.base.packet_id = self.generate_packet_id() + packet.str_rsp.base.need_ack = False + packet.str_rsp.base.timestamp = _current_timestamp_us() + packet.str_rsp.success = data.success + self.enqueue_packet(packet) + return ( + packet.str_rsp.base.packet_id, + packet.str_rsp.base.need_ack, + packet.str_rsp.base.timestamp, + ) + + def send_stop_request(self, _: StopRequestData) -> tuple[int, bool, int]: + """Send a stop request packet. + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.stp_rqt.base.packet_id = self.generate_packet_id() + packet.stp_rqt.base.need_ack = True + packet.stp_rqt.base.timestamp = _current_timestamp_us() + self.enqueue_packet(packet) + return ( + packet.stp_rqt.base.packet_id, + packet.stp_rqt.base.need_ack, + packet.stp_rqt.base.timestamp, + ) + + def send_stop_response(self, data: StopResponseData) -> tuple[int, bool, int]: + """Send a stop response packet. + + Args: + data: Stop response data to send + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.stp_rsp.base.packet_id = self.generate_packet_id() + packet.stp_rsp.base.need_ack = False + packet.stp_rsp.base.timestamp = _current_timestamp_us() + packet.stp_rsp.success = data.success + self.enqueue_packet(packet) + return ( + packet.stp_rsp.base.packet_id, + packet.stp_rsp.base.need_ack, + packet.stp_rsp.base.timestamp, + ) + + def send_error(self, _: ErrorData) -> tuple[int, bool, int]: + """Send an error packet. + + Returns: + tuple: (packet_id, need_ack, timestamp) + """ + packet = RadioPacket() + packet.err_pkt.base.packet_id = self.generate_packet_id() + packet.err_pkt.base.need_ack = False + packet.err_pkt.base.timestamp = _current_timestamp_us() + self.enqueue_packet(packet) + return ( + packet.err_pkt.base.packet_id, + packet.err_pkt.base.need_ack, + packet.err_pkt.base.timestamp, + ) + + # -------------------------------------------------------------------------- + # Private extractor methods + # -------------------------------------------------------------------------- + + def _extract_sync_request(self, packet: SyncRequestPacket) -> SyncRequestData: + return SyncRequestData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + ) + + def _extract_sync_response(self, packet: SyncResponsePacket) -> SyncResponseData: + return SyncResponseData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + success=packet.success, + ) + + def _extract_config_request(self, packet: ConfigRequestPacket) -> ConfigRequestData: + return ConfigRequestData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + gain=packet.gain, + sampling_rate=packet.sampling_rate, + center_frequency=packet.center_frequency, + run_num=packet.run_num, + enable_test_data=packet.enable_test_data, + ping_width_ms=packet.ping_width_ms, + ping_min_snr=packet.ping_min_snr, + ping_max_len_mult=packet.ping_max_len_mult, + ping_min_len_mult=packet.ping_min_len_mult, + target_frequencies=list(packet.target_frequencies), + ) + + def _extract_config_response( + self, + packet: ConfigResponsePacket, + ) -> ConfigResponseData: + return ConfigResponseData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + success=packet.success, + ) + + def _extract_gps_data(self, packet: GPSPacket) -> GPSData: + return GPSData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + easting=packet.easting, + northing=packet.northing, + altitude=packet.altitude, + heading=packet.heading, + epsg_code=packet.epsg_code, + ) + + def _extract_ping_data(self, packet: PingPacket) -> PingData: + return PingData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + frequency=packet.frequency, + amplitude=packet.amplitude, + easting=packet.easting, + northing=packet.northing, + altitude=packet.altitude, + epsg_code=packet.epsg_code, + ) + + def _extract_loc_est_data(self, packet: LocEstPacket) -> LocEstData: + return LocEstData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + frequency=packet.frequency, + easting=packet.easting, + northing=packet.northing, + epsg_code=packet.epsg_code, + ) + + def _extract_start_request(self, packet: StartRequestPacket) -> StartRequestData: + return StartRequestData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + ) + + def _extract_start_response(self, packet: StartResponsePacket) -> StartResponseData: + return StartResponseData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + success=packet.success, + ) + + def _extract_stop_request(self, packet: StopRequestPacket) -> StopRequestData: + return StopRequestData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + ) + + def _extract_stop_response(self, packet: StopResponsePacket) -> StopResponseData: + return StopResponseData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + success=packet.success, + ) + + def _extract_error(self, packet: ErrorPacket) -> ErrorData: + return ErrorData( + packet_id=packet.base.packet_id, + timestamp=packet.base.timestamp, + ) + + # -------------------------------------------------------------------------- + # Private handler methods that call user-registered callbacks + # -------------------------------------------------------------------------- + + def _handle_sync_request(self, data: SyncRequestData) -> None: + self._invoke_handlers(self._sync_request_handlers, data) + + def _handle_sync_response(self, data: SyncResponseData) -> None: + self._invoke_handlers(self._sync_response_handlers, data) + + def _handle_config_request(self, data: ConfigRequestData) -> None: + self._invoke_handlers(self._config_request_handlers, data) + + def _handle_config_response(self, data: ConfigResponseData) -> None: + self._invoke_handlers(self._config_response_handlers, data) + + def _handle_gps_data(self, data: GPSData) -> None: + self._invoke_handlers(self._gps_handlers, data) + + def _handle_ping_data(self, data: PingData) -> None: + self._invoke_handlers(self._ping_handlers, data) + + def _handle_loc_est_data(self, data: LocEstData) -> None: + self._invoke_handlers(self._loc_est_handlers, data) + + def _handle_start_request(self, data: StartRequestData) -> None: + self._invoke_handlers(self._start_request_handlers, data) + + def _handle_start_response(self, data: StartResponseData) -> None: + self._invoke_handlers(self._start_response_handlers, data) + + def _handle_stop_request(self, data: StopRequestData) -> None: + self._invoke_handlers(self._stop_request_handlers, data) + + def _handle_stop_response(self, data: StopResponseData) -> None: + self._invoke_handlers(self._stop_response_handlers, data) + + def _handle_error(self, data: ErrorData) -> None: + self._invoke_handlers(self._error_handlers, data) + + @staticmethod + def _invoke_handlers( + handlers_list: list[tuple[Callable[[T], None], bool]], + data: T, + ) -> None: + to_remove = [] + for i, (callback, once) in enumerate(handlers_list): + callback(data) + if once: + to_remove.append(i) + for i in reversed(to_remove): + handlers_list.pop(i) diff --git a/radio_telemetry_tracker_drone_comms_package/interfaces.py b/radio_telemetry_tracker_drone_comms_package/interfaces.py new file mode 100644 index 0000000..207219f --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/interfaces.py @@ -0,0 +1,230 @@ +"""Radio interface implementations for drone communications. + +This module provides: +- Abstract base class RadioInterface +- SerialRadioInterface +- SimulatedRadioInterface +""" + +from __future__ import annotations + +import abc +import logging +import time +from typing import TYPE_CHECKING + +from radio_telemetry_tracker_drone_comms_package.codec import ( + CHECKSUM_SIZE, + LENGTH_FIELD_SIZE, + SYNC_MARKER, + SYNC_MARKER_LENGTH, + RadioCodec, +) + +if TYPE_CHECKING: + from radio_telemetry_tracker_drone_comms_package.proto.packets_pb2 import ( + RadioPacket, + ) + +logger = logging.getLogger(__name__) + + +class RadioInterface(abc.ABC): + """Abstract base class for radio communication interfaces.""" + + def __init__(self, read_timeout: float = 1.0) -> None: + """Initialize radio interface with specified read timeout. + + Args: + read_timeout: Maximum time in seconds to wait for read operations. + """ + self.read_timeout = read_timeout + + @abc.abstractmethod + def connect(self) -> None: + """Connect to the radio device (real or simulated).""" + + @abc.abstractmethod + def _send_data(self, data: bytes) -> None: + """Send raw bytes through the radio interface.""" + + @abc.abstractmethod + def _read_data(self, max_bytes: int) -> bytes: + """Read raw bytes from the underlying interface.""" + + @abc.abstractmethod + def close(self) -> None: + """Close the radio interface connection.""" + + def send_packet(self, packet: RadioPacket) -> None: + """Encode and send a RadioPacket through the interface.""" + data = RadioCodec.encode_packet(packet) + self._send_data(data) + + def receive_packet(self) -> RadioPacket | None: + """Receive raw bytes from the interface and decode a RadioPacket.""" + # Read the sync marker first + sync = self._read_with_timeout(SYNC_MARKER_LENGTH) + if len(sync) < SYNC_MARKER_LENGTH: + return None + if sync != SYNC_MARKER: + return None + + # Read length field + length_bytes = self._read_with_timeout(LENGTH_FIELD_SIZE) + if len(length_bytes) < LENGTH_FIELD_SIZE: + return None + msg_len = int.from_bytes(length_bytes, byteorder="big") + + # Read message data + message_data = self._read_with_timeout(msg_len) + if len(message_data) < msg_len: + return None + + # Read checksum + checksum_bytes = self._read_with_timeout(CHECKSUM_SIZE) + if len(checksum_bytes) < CHECKSUM_SIZE: + return None + + # Reconstruct the entire packet for decode + raw_packet = sync + length_bytes + message_data + checksum_bytes + return RadioCodec.decode_packet(raw_packet) + + def _read_with_timeout(self, num_bytes: int) -> bytes: + """Read a specified number of bytes with an overall time limit.""" + deadline = time.time() + self.read_timeout + data_collected = bytearray() + + while len(data_collected) < num_bytes: + if time.time() > deadline: + break + chunk = self._read_data(num_bytes - len(data_collected)) + if chunk: + data_collected.extend(chunk) + else: + time.sleep(0.01) + + return bytes(data_collected) + + +class SerialRadioInterface(RadioInterface): + """Radio interface using serial communication.""" + + def __init__(self, port: str, baudrate: int = 56700, timeout: float = 1.0) -> None: + """Initialize serial radio interface. + + Args: + port: Serial port name + baudrate: Communication speed in bits per second + timeout: Read timeout in seconds + """ + super().__init__(read_timeout=timeout) + self.port = port + self.baudrate = baudrate + self.timeout = timeout + self.ser = None + + def connect(self) -> None: + """Initialize and open the serial connection with configured parameters.""" + import serial + + self.ser = serial.Serial(self.port, self.baudrate, timeout=self.timeout) + + def _send_data(self, data: bytes) -> None: + if self.ser and self.ser.is_open: + self.ser.write(data) + + def _read_data(self, max_bytes: int) -> bytes: + if self.ser and self.ser.is_open: + return self.ser.read(max_bytes) + return b"" + + def close(self) -> None: + """Close the serial connection if it is open.""" + if self.ser and self.ser.is_open: + self.ser.close() + + +class SimulatedRadioInterface(RadioInterface): + """Radio interface using TCP/IP for simulation.""" + + def __init__( + self, + host: str = "localhost", + port: int = 50000, + *, + server_mode: bool = False, + timeout: float = 1.0, + ) -> None: + """Initialize simulated radio interface using TCP/IP. + + Args: + host: IP address or hostname to connect/bind to + port: TCP port number + server_mode: If True, act as server; if False, act as client + timeout: Read timeout in seconds + """ + super().__init__(read_timeout=timeout) + self.host = host + self.port = port + self.server_mode = server_mode + self.sock = None + self.conn = None + + def connect(self) -> None: + """Establish TCP/IP connection in either client or server mode.""" + import socket + + self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + + if self.server_mode: + self.sock.bind((self.host, self.port)) + self.sock.listen(1) + logger.info("Waiting for connection on %s:%d...", self.host, self.port) + + # Set socket to non-blocking mode + self.sock.setblocking(False) # noqa: FBT003 + while True: + try: + self.conn, addr = self.sock.accept() + logger.info("Client connected from %s:%d", addr[0], addr[1]) + # Set normal timeout for subsequent operations + self.conn.settimeout(self.read_timeout) + break + except BlockingIOError: + time.sleep(0.1) # Sleep briefly to avoid busy-waiting + except Exception: + logger.exception("Error accepting connection") + self.close() + raise + else: + try: + self.sock.settimeout(10.0) # 10 seconds for client connection + self.sock.connect((self.host, self.port)) + self.conn = self.sock + self.conn.settimeout(self.read_timeout) + logger.info("Connected to server %s:%d", self.host, self.port) + except (socket.timeout, ConnectionRefusedError): + logger.exception("Failed to connect to server") + self.close() + raise + + def _send_data(self, data: bytes) -> None: + if self.conn: + self.conn.sendall(data) + + def _read_data(self, max_bytes: int) -> bytes: + if self.conn: + try: + return self.conn.recv(max_bytes) + except (TimeoutError, ConnectionError, OSError): + return b"" + return b"" + + def close(self) -> None: + """Close both the connection and socket if they exist.""" + if self.conn: + self.conn.close() + if self.sock: + self.sock.close() diff --git a/radio_telemetry_tracker_drone_comms_package/proto/__init__.py b/radio_telemetry_tracker_drone_comms_package/proto/__init__.py new file mode 100644 index 0000000..5511fd5 --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/proto/__init__.py @@ -0,0 +1,43 @@ +"""Protocol buffer module for radio telemetry packet definitions.""" + +from radio_telemetry_tracker_drone_comms_package.proto.compiler import ensure_proto_compiled + +# Ensure proto files are compiled before importing +ensure_proto_compiled() + +# Now import the generated protobuf +from radio_telemetry_tracker_drone_comms_package.proto.packets_pb2 import ( # noqa: E402 + AckPacket, + BasePacket, + ConfigRequestPacket, + ConfigResponsePacket, + ErrorPacket, + GPSPacket, + LocEstPacket, + PingPacket, + RadioPacket, + StartRequestPacket, + StartResponsePacket, + StopRequestPacket, + StopResponsePacket, + SyncRequestPacket, + SyncResponsePacket, +) + +__all__ = [ + "AckPacket", + "BasePacket", + "ConfigRequestPacket", + "ConfigResponsePacket", + "ErrorPacket", + "GPSPacket", + "LocEstPacket", + "PingPacket", + "RadioPacket", + "StartRequestPacket", + "StartResponsePacket", + "StopRequestPacket", + "StopResponsePacket", + "SyncRequestPacket", + "SyncResponsePacket", +] diff --git a/radio_telemetry_tracker_drone_comms_package/proto/compiler.py b/radio_telemetry_tracker_drone_comms_package/proto/compiler.py new file mode 100644 index 0000000..f909508 --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/proto/compiler.py @@ -0,0 +1,43 @@ +"""Proto compiler module for on-demand protobuf compilation.""" + +import logging +from pathlib import Path + +from grpc_tools import protoc + +logger = logging.getLogger(__name__) + + +def ensure_proto_compiled() -> None: + """Compile the protobuf file.""" + + def _handle_error(msg: str) -> None: + """Handle protoc compilation errors. + + Args: + msg: Error message + """ + logger.error(msg) + logger.error("Make sure grpcio-tools is installed:\npoetry add grpcio-tools") + raise RuntimeError(msg) + + proto_dir = Path(__file__).parent + proto_file = proto_dir / "packets.proto" + + try: + # Always compile the proto file + result = protoc.main( + [ + "grpc_tools.protoc", + f"--python_out={proto_dir}", + f"--proto_path={proto_dir}", + str(proto_file), + ], + ) + + if result != 0: + _handle_error(f"protoc returned non-zero status: {result}") + + logger.info("Successfully compiled protobuf file") + except (OSError, ImportError, TypeError) as e: + _handle_error(f"Failed to compile protobuf: {e!s}") diff --git a/radio_telemetry_tracker_drone_comms_package/proto/packets.proto b/radio_telemetry_tracker_drone_comms_package/proto/packets.proto new file mode 100644 index 0000000..a23ad34 --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/proto/packets.proto @@ -0,0 +1,114 @@ +syntax = "proto3"; + +package radio_telemetry_tracker_drone_comms_package.proto; + +// Base fields common to all packets +message BasePacket { + uint32 packet_id = 1; + bool need_ack = 2; + uint64 timestamp = 3; // Microseconds since epoch +} + +// Define packets +message AckPacket { + BasePacket base = 1; + uint32 ack_id = 2; // ID of the packet that this ack is for +} + +message SyncRequestPacket { + BasePacket base = 1; +} + +message SyncResponsePacket { + BasePacket base = 1; + bool success = 2; +} + +message ConfigRequestPacket { + BasePacket base = 1; + float gain = 2; + int32 sampling_rate = 3; + int32 center_frequency = 4; + int32 run_num = 5; + bool enable_test_data = 6; + int32 ping_width_ms = 7; + int32 ping_min_snr = 8; + float ping_max_len_mult = 9; + float ping_min_len_mult = 10; + repeated int32 target_frequencies = 11; +} + +message ConfigResponsePacket { + BasePacket base = 1; + bool success = 2; +} + +message GPSPacket { + BasePacket base = 1; + float easting = 2; + float northing = 3; + float altitude = 4; + float heading = 5; + int32 epsg_code = 6; +} + +message PingPacket { + BasePacket base = 1; + int32 frequency = 2; + float amplitude = 3; + float easting = 4; + float northing = 5; + float altitude = 6; + int32 epsg_code = 7; +} + +message LocEstPacket { + BasePacket base = 1; + int32 frequency = 2; + float easting = 3; + float northing = 4; + int32 epsg_code = 5; +} + +message StartRequestPacket { + BasePacket base = 1; +} + +message StartResponsePacket { + BasePacket base = 1; + bool success = 2; +} + +message StopRequestPacket { + BasePacket base = 1; +} + +message StopResponsePacket { + BasePacket base = 1; + bool success = 2; +} + +message ErrorPacket { + BasePacket base = 1; +} + +// Wrapper message containing one of the packet types +message RadioPacket { + oneof msg { + AckPacket ack_pkt = 1; + SyncRequestPacket syn_rqt = 2; + SyncResponsePacket syn_rsp = 3; + ConfigRequestPacket cfg_rqt = 4; + ConfigResponsePacket cfg_rsp = 5; + GPSPacket gps_pkt = 6; + PingPacket ping_pkt = 7; + LocEstPacket loc_pkt = 8; + StartRequestPacket str_rqt = 9; + StartResponsePacket str_rsp = 10; + StopRequestPacket stp_rqt = 11; + StopResponsePacket stp_rsp = 12; + ErrorPacket err_pkt = 13; + } +} + + diff --git a/radio_telemetry_tracker_drone_comms_package/transceiver.py b/radio_telemetry_tracker_drone_comms_package/transceiver.py new file mode 100644 index 0000000..0a7fdc2 --- /dev/null +++ b/radio_telemetry_tracker_drone_comms_package/transceiver.py @@ -0,0 +1,186 @@ +"""Module for handling radio packet transmission and reception with acknowledgment support.""" + +from __future__ import annotations + +import logging +import queue +import threading +import time +from typing import TYPE_CHECKING, Callable + +from radio_telemetry_tracker_drone_comms_package.proto import BasePacket, RadioPacket + +logger = logging.getLogger(__name__) + +if TYPE_CHECKING: + from radio_telemetry_tracker_drone_comms_package.interfaces import RadioInterface + +MAX_PACKET_ID = 0x7FFFFFFF # Maximum 31-bit signed integer + +def _current_timestamp_us() -> int: + """Return the current time in microseconds since epoch as an integer.""" + return int(time.time() * 1_000_000) + +class PacketManager: + """Manages radio packet transmission and reception with acknowledgment support.""" + + def __init__( + self, + radio_interface: RadioInterface, + ack_timeout: float = 2.0, + max_retries: int = 5, + on_ack_timeout: Callable[[RadioPacket], None] | None = None, + ) -> None: + """Initialize the PacketManager. + + Args: + radio_interface: Interface for radio communication. + ack_timeout: Time in seconds to wait for acknowledgment. + max_retries: Maximum number of retransmission attempts. + on_ack_timeout: Optional callback when packet acknowledgment times out. + """ + self.radio_interface = radio_interface + self.ack_timeout = ack_timeout + self.max_retries = max_retries + self.on_ack_timeout = on_ack_timeout + + self.send_queue: queue.PriorityQueue[tuple[int, float, RadioPacket]] = queue.PriorityQueue() + # Maps packet_id -> { "packet": RadioPacket, "send_time": float, "retries": int } + self.outstanding_acks = {} + + self._next_packet_id = 1 + self._id_lock = threading.Lock() + + # Thread control + self._stop_event = threading.Event() + self._send_thread = threading.Thread(target=self._send_loop, daemon=True) + self._recv_thread = threading.Thread(target=self._recv_loop, daemon=True) + + def start(self) -> None: + """Start send/receive threads and connect the radio interface.""" + self.radio_interface.connect() + self._stop_event.clear() + self._send_thread.start() + self._recv_thread.start() + + def stop(self) -> None: + """Stop send/receive threads and close the radio interface.""" + self._stop_event.set() + self._send_thread.join(timeout=2) + self._recv_thread.join(timeout=2) + self.radio_interface.close() + + def generate_packet_id(self) -> int: + """Generate a unique packet ID between 1 and MAX_PACKET_ID.""" + with self._id_lock: + if self._next_packet_id > MAX_PACKET_ID: + self._next_packet_id = 1 + pid = self._next_packet_id + self._next_packet_id += 1 + return pid + + def enqueue_packet(self, packet: RadioPacket) -> None: + """Enqueue a packet for transmission. + + Priority is 0 if need_ack is True, else 1. + """ + base = self._get_base(packet) + priority = 0 if base.need_ack else 1 + self.send_queue.put((priority, time.time(), packet)) + + def _send_loop(self) -> None: + """Fetch packets from the queue to send, handle ack tracking.""" + while not self._stop_event.is_set(): + try: + priority, enq_time, packet = self.send_queue.get(timeout=0.1) + except queue.Empty: + self._retry_outstanding_packets() + continue + + self.radio_interface.send_packet(packet) + base = self._get_base(packet) + if base.need_ack: + pid = base.packet_id + self.outstanding_acks[pid] = { + "packet": packet, + "send_time": time.time(), + "retries": 0, + } + + def _retry_outstanding_packets(self) -> None: + """Check and retry outstanding packets if ack has not arrived in time.""" + now = time.time() + to_remove = [] + for pid, info in self.outstanding_acks.items(): + if (now - info["send_time"]) >= self.ack_timeout: + if info["retries"] < self.max_retries: + info["retries"] += 1 + info["send_time"] = now + self.radio_interface.send_packet(info["packet"]) + else: + to_remove.append(pid) + + for pid in to_remove: + timed_out_pkt = self.outstanding_acks[pid]["packet"] + del self.outstanding_acks[pid] + if self.on_ack_timeout: + self.on_ack_timeout(timed_out_pkt) + + def _recv_loop(self) -> None: + """Continuously attempt to receive packets and handle them.""" + while not self._stop_event.is_set(): + packet = self.radio_interface.receive_packet() + if packet is None: + continue + self._handle_incoming_packet(packet) + + def _handle_incoming_packet(self, packet: RadioPacket) -> None: + """Process incoming packets, removing from outstanding acks if relevant.""" + if packet.HasField("ack_pkt"): + ack_id = packet.ack_pkt.ack_id + if ack_id in self.outstanding_acks: + del self.outstanding_acks[ack_id] + return + + base = self._get_base(packet) + if base.need_ack: + self._send_ack(base.packet_id) + + # Let subclasses handle domain-specific logic + self.on_user_packet_received(packet) + + def on_user_packet_received(self, packet: RadioPacket) -> None: + """Hook for subclasses to handle a non-ACK packet.""" + + def _send_ack(self, packet_id_to_ack: int) -> None: + """Send an AckPacket for the given packet ID.""" + ack_pkt = RadioPacket() + ack_pkt.ack_pkt.base.packet_id = self.generate_packet_id() + ack_pkt.ack_pkt.base.need_ack = False + ack_pkt.ack_pkt.base.timestamp = _current_timestamp_us() + ack_pkt.ack_pkt.ack_id = packet_id_to_ack + self.enqueue_packet(ack_pkt) + + @staticmethod + def _get_base(packet: RadioPacket) -> BasePacket: + """Return the BasePacket field for the RadioPacket.""" + field = packet.WhichOneof("msg") + packet_types = { + "ack_pkt": lambda p: p.ack_pkt.base, + "syn_rqt": lambda p: p.syn_rqt.base, + "syn_rsp": lambda p: p.syn_rsp.base, + "cfg_rqt": lambda p: p.cfg_rqt.base, + "cfg_rsp": lambda p: p.cfg_rsp.base, + "gps_pkt": lambda p: p.gps_pkt.base, + "ping_pkt": lambda p: p.ping_pkt.base, + "loc_pkt": lambda p: p.loc_pkt.base, + "str_rqt": lambda p: p.str_rqt.base, + "str_rsp": lambda p: p.str_rsp.base, + "stp_rqt": lambda p: p.stp_rqt.base, + "stp_rsp": lambda p: p.stp_rsp.base, + "err_pkt": lambda p: p.err_pkt.base, + } + if field in packet_types: + return packet_types[field](packet) + msg = "Unknown packet type in RadioPacket." + raise ValueError(msg) diff --git a/rttlora/__init__.py b/rttlora/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttlora/callbacks.py b/rttlora/callbacks.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttlora/config.py b/rttlora/config.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttlora/gcs_comms.py b/rttlora/gcs_comms.py deleted file mode 100644 index b944eb9..0000000 --- a/rttlora/gcs_comms.py +++ /dev/null @@ -1,96 +0,0 @@ -import threading -from typing import List, Optional -from lora_radio import LoRaRadio - - -class GcsCommications: - def __init__(self, lora_radio: LoRaRadio): - """ - Initialize the GCS communication interface. - - :param lora_radio: The LoRa radio interface. - """ - self.lora_radio = lora_radio - self.mavcomms_list = List[str] = [] # List of known MavComms addresses - self.__receiver_thread: Optional(threading.Thread) = None - self.__running = False - - def start(self): - """ - Start the GCS communication interface. - """ - self.__running = True - self.__receiver_thread = threading.Thread(target=self.__receiver_loop) - self.__reciever_thread.start() - print("GCS communications started") - - def __receiver_loop(self): - """ - Internal loop to recieve messages from MavComms. - """ - while self.__running: - message = self.lora_radio.recieve() - if message: - self.__process_message(message) - - def __process_message(self, message: bytes): - """ - Process incoming messages from MavComms. - - :param message: The message recieved from a MavComm. - """ - pass - - def send_message(self, message: bytes, target_addr: Optional[str] = None): - """ - Send a message to a specific MavComm or broadcast to all. - - :param message: The message to send. - :param target_addr: The address of the target MavComm, or None to broadcast. - """ - if target_addr: - self.lora_radio.send(message, target_addr) - else: - # Broadcast to all known MavComms - for addr in self.mavcomms_list: - self.lora_radio.send(message, addr) - print(f"GCS sent messsage: {message.hex()} to {'all' if target_addr is None else target_addr}") - - def stop(self): - """ - Stop the GCS communication interface. - """ - self.__running = False - if self.__receiver_thread: - self.__receiver_thread.join() - print("GCS communications stopped") - - def add_mavcomm(self, mavcomm_addr: str): - """ - Add a MacComm to the list of known MavComms. - - :param addr: The address of the MavComm to add. - """ - if mavcomm_addr not in self.mavcomms_list: - self.mavcomms_list.append(mavcomm_addr) - print(f"GCS added MavComm: {mavcomm_addr}") - - def remove_mavcomm(self, mavcomm_addr: str): - """ - Remove a MavComm from the list of known MavComms. - - :param mavcomm_addr: The address of the MavComm to remove. - """ - if mavcomm_addr in self.mavcomms_list: - self.mavcomms_list.remove(mavcomm_addr) - print(f"GCS removed MavComm: {mavcomm_addr}") - - - - - - - - - - diff --git a/rttlora/lora_radio.py b/rttlora/lora_radio.py deleted file mode 100644 index 28ea460..0000000 --- a/rttlora/lora_radio.py +++ /dev/null @@ -1,112 +0,0 @@ -import serial -import threading -from typing import Optional, Tuple - -class LoRaRadio: - def __init__(self, port: str, device_addr: str, baudrate: int = 9600): - """ - Initialize the LoRa radio interface - - :param port: the serial port the LoRa radio is connected to. - :param device_addr: the address of the device. - :param baudrate: the baud rate of the serial port. - """ - self.port = port - self.device_addr = device_addr - self.baudrate = baudrate - self.serial = None - self.__lock = threading.Lock() - - def open(self): - """ - Opens the LoRa radio serial connectoin. - """ - self.serial = serial.Serial(self.port, self.baudrate, timeout=1) - print(f"LoRa radio opened on {self.port} with baudrate {self.baudrate}") - - def close(self): - """ - Closes the LoRa radio serial connection. - """ - if self.serial and self.serial.is_open: - self.serial.close() - print("LoRa radio closed") - - def send(self, message: bytes, addr: Optional[str] = None): - """ - Sends a message to the specified address. - - :param message: the message to send. - :param addr: the address to send the message to. None for broadcast. - """ - with self.__lock: - if addr: - packet = f"{addr:}:".encode('ascii') + message - else: - packet = message - - if self.serial and self.serial.is_open: - self.serial.write(packet) - print(f"Sent message to {'broadcast' if addr is None else addr}: {message.hex()}") - else: - print(f"Serial connection not open") - - def recieve(self) -> Tuple[Optional[str], Optional[str]]: - """ - Recieve a message. - - :return: a tuple containing the recieved message, or None if the message is not intended for this device. - """ - with self.__lock: - if self.serial and self.serial.is_open: - try: - data = self.serial.readline().strip() - if b':' in data: - target_addr, message = data.split(b':', 1) - target_addr = target_addr.decode('ascii') - if target_addr == self.device_addr: - print(f"Recieved message for {self.device_addr}: {message.hex()}") - return message - else: - print(f"Message not intended for this device (intended for {target_addr})") - return None - else: - # If no address is provided, assume broadcast and accept the message - print(f"Recieved broadcast message: {data.hex()}") - return data - except Exception as e: - print(f"Error recieving message: {e}") - return None - else: - print(f"Serial connection not open") - return None - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/rttlora/mav_comms.py b/rttlora/mav_comms.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttlora/packets.py b/rttlora/packets.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttlora/transport.py b/rttlora/transport.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttlora/utils.py b/rttlora/utils.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttsik/__init__.py b/rttsik/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttsik/callbacks.py b/rttsik/callbacks.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttsik/config.py b/rttsik/config.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttsik/drone_state.py b/rttsik/drone_state.py deleted file mode 100644 index c547b1e..0000000 --- a/rttsik/drone_state.py +++ /dev/null @@ -1,70 +0,0 @@ -"""Module for defining drone state-related classes and enums.""" - -from __future__ import annotations - -from dataclasses import dataclass, field -from enum import IntEnum - - -class GPSState(IntEnum): - """Enumeration of GPS states.""" - - NOT_READY = 0 - INITIALIZING = 1 - WAITING = 2 - PARTIAL = 3 - READY = 4 - ERRORED = 5 - - -class PingFinderState(IntEnum): - """Enumeration of possible states for the ping finder.""" - - NOT_READY = 0 - READY = 1 - RUNNING = 2 - STOPPED = 3 - ERRORED = 4 - - -@dataclass -class GPSData: - """Represents GPS coordinate data.""" - - latitude: float | None = None - longitude: float | None = None - altitude: float | None = None - heading: float | None = None - - -@dataclass -class DroneState: - """Represents the current state of a drone, including GPS information.""" - - gps_state: GPSState = GPSState.NOT_READY - gps_data: GPSData = field(default_factory=GPSData) - ping_finder_state: PingFinderState = PingFinderState.NOT_READY - - -# Global instance of DroneState -current_state = DroneState() - - -def update_gps_state(state: GPSState) -> None: - """Update the current GPS state.""" - current_state.gps_state = state - - -def update_gps_data(data: GPSData) -> None: - """Update the current state with new GPS data.""" - current_state.gps_data = data - - -def update_ping_finder_state(state: PingFinderState) -> None: - """Update the current ping finder state.""" - current_state.ping_finder_state = state - - -def get_current_state() -> DroneState: - """Return the current state of the drone.""" - return current_state \ No newline at end of file diff --git a/rttsik/fds_comms.py b/rttsik/fds_comms.py deleted file mode 100644 index ef343d2..0000000 --- a/rttsik/fds_comms.py +++ /dev/null @@ -1,103 +0,0 @@ -import struct -import threading -import enum -import time -from rttsik.packets import * -from typing import List, Optional, Callable, Dict -from rttsik.sik_radio import sikRadio -from rttsik.drone_state import * - -class Events(enum.Enum): - SEND_PING_DETECTION = 0x0 - SEND_HEARTBEAT = 0x1 - SEND_TRANSMITTER_LOC_EST = 0x2 - SEND_CONFIG_ACK = 0x3 - SEND_COMMAND_ACK = 0x4 - PING_FINDER_RECIEVED = 0x5 - START_RECIEVED = 0x6 - RESET_RECIEVED = 0x7 - END_RECIEVED = 0x8 - -class FdsComms: - def __init__(self, sik_radio: sikRadio): - """ - Initialize the fds communication interface. - - :param sik_radio: The sik radio interface. - """ - self.sik_radio = sik_radio - self.__receiver_thread = threading.Thread(target=self.__receiver_loop) - self.__receiver_thread.start() - self.__transmitter_thread: Optional(threading.Thread) = None - self.__heartbeat_thread: Optional(threading.Thread) = None - self.__transmitter_queue = [] - self.transmitter_lock = threading.Lock() - self.__packet_map: Dict[int, List[Callable]] = { - evt.value: [] for evt in Events - } - self.__running = False - - self.packet_ctr = 0 - self.drone_latitude_degrees: Optional[float] = None - self.drone_longiude_degrees: Optional[float] = None - self.drone_altitude_meters: Optional[float] = None - self.drone_heading_degrees: Optional[float] = None - self.gps_state = GPSState.NOT_READY - self.ping_finder_state = PingFinderState.NOT_READY - - def start(self): - """ - Start the GCS communication interface. - """ - self.__running = True - - self.__transmitter_thread = threading.Thread(target=self.__transmitter_loop) - self.__heartbeat_thread = threading.Thread(target=self.__heartbeat_loop) - self.__heartbeat_thread.start() - self.__transmitter_thread.start() - print("FdsComms communications started") - - def __receiver_loop(self): - """ - Internal loop to recieve messages from FdsComms. - """ - while True: - message = self.sik_radio.recieve() - if message: - if(len(message) >= 20): # can't do a checksum if you don't have the checksum - self.__process_message(message) - - time.sleep(0.1) - - def __process_message(self, message: bytes): - packet_type = Packet.get_type(message) - if(Packet.check_crc(message)): - if(packet_type[0] == 0x6): - self.__process_config(message) - else: - print("fail") - - - def __process_config(self, packet: bytes): - config = Set_Ping_Finder_Config_Packet.from_bytes(packet) - - pass - - def __heartbeat_loop(self): - while self.__running: - self.transmitter_lock.acquire() - # add a heartbeat to the transmitter queue - hb_packet = Heartbeat_Packet(self.packet_ctr, self.drone_latitude_degrees, self.drone_longiude_degrees, self.drone_altitude_meters, self.drone_heading_degrees, - self.gps_state, self.ping_finder_state) - self.sik_radio.send(hb_packet.to_packet()) - self.packet_ctr = self.packet_ctr + 1 - self.transmitter_lock.release() - time.sleep(2) - - def __transmitter_loop(self): - while self.__running: - self.transmitter_lock.acquire() - if len(self.__transmitter_queue) > 0: - self.sik_radio.send(self.__transmitter_queue[0]) - self.__transmitter_queue[0].pop(0) - self.transmitter_lock.release() \ No newline at end of file diff --git a/rttsik/gcs_comms.py b/rttsik/gcs_comms.py deleted file mode 100644 index 9761385..0000000 --- a/rttsik/gcs_comms.py +++ /dev/null @@ -1,152 +0,0 @@ -import threading -import enum -from typing import List, Optional, Callable, Dict -from rttsik.sik_radio import sikRadio -from rttsik.packets import * - -class Events(enum.Enum): - PING_DETECTION_RECIEVED = 0x0 - HEARTBEAT_RECIEVED = 0x1 - TRANSMITTER_LOC_EST_RECIEVED = 0x2 - CONFIG_ACK_RECIEVED = 0x3 - COMMAND_ACK_RECIEVED = 0x4 - SEND_PING_FINDER = 0x5 - SEND_START = 0x6 - SEND_RESET = 0x7 - SEND_END = 0x8 - -class GcsCommications: - def __init__(self, sik_radio: sikRadio): - """ - Initialize the GCS communication interface. - - :param sik_radio: The sik radio interface. - """ - self.sik_radio = sik_radio - self.packet_ctr = 0 - self.fdscomms_list: List[str] = [] # List of known FdsComms addresses - self.__receiver_thread: Optional(threading.Thread) = None - self.__transmitter_thread: Optional(threading.Thread) = None - self.__transmitter_queue = [] - self.__packet_map: Dict[int, List[Callable]] = { - evt.value: [] for evt in Events - } - self.__running = False - self.lock = threading.Lock() - - def add_radio(self, addr): - self.fdscomms_list.append(addr) - - def start(self): - """ - Start the GCS communication interface. - """ - self.__running = True - self.__receiver_thread = threading.Thread(target=self.__receiver_loop) - self.__transmitter_thread = threading.Thread(target=self.__transmitter_loop) - self.__receiver_thread.start() - print("GCS communications started") - - def __receiver_loop(self): - """ - Internal loop to recieve messages from FdsComms. - """ - while self.__running: - message = self.sik_radio.recieve() - if message: - if(len(message) >= 20): # can't do a checksum if the packet doesn't contain a checksum - self.__process_message(message) - - def __transmitter_loop(self): - while self.__running: - pass - - def __process_message(self, message: bytes): - """ - Process incoming messages from FdsComms. - - :param message: The message recieved from a FdsComm. - """ - type = Packet.get_type(message) - - # testing to make sure data recieved is correct - - print("Message:") - print(message.hex()) - if(Packet.check_crc(message)): - print("Printing data") - hb_packet = Heartbeat_Packet.from_bytes(message) - print(hb_packet.type) - print(hb_packet.id) - print(hb_packet.drone_latitude_degrees) - print(hb_packet.drone_longitude_degrees) - print(hb_packet.drone_altitude_meters) - print(hb_packet.drone_heading_degrees) - print(hb_packet.gps_state) - print(hb_packet.ping_finder_state) - # Decode header - # Based off of header, accesses callables list from dictionary - # Iterate through list - else: - print("failed") - - - def set_ping_config(self, run_number_id: int, target_frequencies_hz: list[int], sampling_rate_hz: Optional[int] = None, gain_db: Optional[float] = None, - center_frequency_hz: Optional[int] = None, ping_width_ms: Optional[int] = None, ping_min_snr: Optional[int] = None, ping_max_length_multiplier: - Optional[float] = None, ping_min_length_multiplier: Optional[float] = None): - # ACQUIRE LOCK FOR QUEUE - self.lock.acquire() - # create packet and place in transmitter queue - config_packet = Set_Ping_Finder_Config_Packet(self.packet_ctr, run_number_id, target_frequencies_hz, sampling_rate_hz, gain_db, center_frequency_hz, - ping_width_ms, ping_min_snr, ping_max_length_multiplier, ping_min_length_multiplier) - self.packet_ctr = self.packet_ctr + 1 - # send packet - self.send_message(config_packet.to_packet()) - # RELEASE LOCK FOR QUEUE - self.lock.release() - - def send_message(self, message: bytes, target_addr: Optional[str] = None): - """ - Send a message to a specific FdsComm or broadcast to all. - - :param message: The message to send. - :param target_addr: The address of the target FdsComm, or None to broadcast. - """ - if target_addr: - self.sik_radio.send(message, target_addr) - else: - # Broadcast to all known FdsComms - for addr in self.fdscomm_list: - self.sik_radio.send(message, addr) - print(f"GCS sent messsage: {message.hex()} to {'all' if target_addr is None else target_addr}") - - - def stop(self): - """ - Stop the GCS communication interface. - """ - self.__running = False - if self.__receiver_thread: - self.__receiver_thread.join() - print("GCS communications stopped") - - def add_fdscomm(self, fdscomm_addr: str): - """ - Add a MacComm to the list of known FdsComms. - - :param addr: The address of the FdsComm to add. - """ - if fdscomm_addr not in self.fdscomms_list: - self.fdscomms_list.append(fdscomm_addr) - print(f"GCS added FdsComm: {fdscomm_addr}") - - def remove_fdscomm(self, fdscomm_addr: str): - """ - Remove a FdsComm from the list of known FdsComm. - - :param fdscomm_addr: The address of the FdsComm to remove. - """ - if fdscomm_addr in self.fdscomms_list: - self.fdscomms_list.remove(fdscomm_addr) - print(f"GCS removed FdsComm: {fdscomm_addr}") - diff --git a/rttsik/packets.py b/rttsik/packets.py deleted file mode 100644 index 7154598..0000000 --- a/rttsik/packets.py +++ /dev/null @@ -1,377 +0,0 @@ -import binascii -import struct -from typing import Optional -from datetime import datetime - -# placeholder packet class -# packet contains a type, id, payload size and timestamp in the header, plus a 4 byte checksum at the end -class Packet: - def __init__(self, id: int, packet_sent_timestamp: datetime): - self.type = 0x0 - self.id = id - self.data_length = 0 - self.packet_sent_timestamp = packet_sent_timestamp - - def to_packet(self): - header = self.get_header() - msg = header - crc = binascii.crc32(msg) - return msg + crc - - #def get_id(self): - # return self.id - - #def get_type(self): - # return self.type - - def get_header(self): - header = struct.pack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Ping_Detection_Packet(id, 0, 0.0, 0.0, 0.0, 0, datetime.now()) - rebuilt_packet.drone_latitude_degrees, rebuilt_packet.drone_longitude_degrees, rebuilt_packet.drone_altitude_meters, rebuilt_packet.transmitter_power_db, rebuilt_packet.transmitter_frequency_hz = struct.unpack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Heartbeat_Packet(id) - rebuilt_packet.drone_latitude_degrees, rebuilt_packet.drone_latitude_degrees_valid, rebuilt_packet.drone_longitude_degrees, rebuilt_packet.drone_longitude_degrees_valid, rebuilt_packet.drone_altitude_meters, rebuilt_packet.drone_altitude_meters_valid, rebuilt_packet.drone_heading_degrees, rebuilt_packet.gps_state, rebuilt_packet.ping_finder_state = struct.unpack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Trx_Loc_Estimation_Packet(id, 0, 0.0, 0.0, 0.0, datetime.now()) - rebuilt_packet.transmitter_frequency_hz, rebuilt_packet.transmitter_latitude_degrees, rebuilt_packet.transmitter_longitude_degrees, rebuilt_packet.transmitter_altitude_meters = struct.unpack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Config_Ack_Packet(id, 0, 0, datetime.now()) - rebuilt_packet.success, rebuilt_packet.message = struct.unpack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Command_Ack_Packet(id, 0, 0, datetime.now()) - rebuilt_packet.success, rebuilt_packet.message = struct.unpack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Set_Ping_Finder_Config_Packet(id, 0, [1]) - rebuilt_packet.run_number_id = super().get_run_number_id(packet) - rebuilt_packet.target_frequencies_hz_length = struct.unpack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Config_Ack_Packet(id, 0, 0, datetime.now()) - rebuilt_packet.success = struct.unpack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Command_Ack_Packet(id, 0, 0, datetime.now()) - rebuilt_packet.success, rebuilt_packet.command_type = struct.unpack(' Packet: - id = super().get_id(packet)[0] - payload = super().get_payload(packet) - rebuilt_packet = Command_Ack_Packet(id, 0, 0, datetime.now()) - rebuilt_packet.command_type = struct.unpack(' Tuple[Optional[str], Optional[str]]: - """ - Recieve a message. - - :return: a tuple containing the recieved message, or None if the message is not intended for this device. - """ - # data = self.serial.readline().strip() - with self.__lock: - if self.serial and self.serial.is_open: - try: - data = self.serial.readline().strip() - if b':' in data: - target_addr, message = data.split(b':', 1) - target_addr = target_addr.decode('ascii') - if target_addr == self.device_addr: - print(f"Recieved message for {self.device_addr}: {message.hex()}") - return message - else: - print(f"Message not intended for this device (intended for {target_addr})") - return None - else: - # If no address is provided, assume broadcast and accept the message - # print(f"Recieved broadcast message: {data.hex()}") - return data - except Exception as e: - print(f"Error recieving message: {e}") - return None - else: - print(f"Serial connection not open") - return None \ No newline at end of file diff --git a/rttsik/transport.py b/rttsik/transport.py deleted file mode 100644 index e69de29..0000000 diff --git a/rttsik/utils.py b/rttsik/utils.py deleted file mode 100644 index e69de29..0000000 diff --git a/test2.py b/test2.py deleted file mode 100644 index 90a59a6..0000000 --- a/test2.py +++ /dev/null @@ -1,25 +0,0 @@ -import struct -import threading -import time -from typing import List, Optional, Callable, Dict -from rttsik.sik_radio import sikRadio -from rttsik.gcs_comms import GcsCommications -from rttsik.fds_comms import FdsComms - - -rad1 = sikRadio('/dev/ttyUSB0', '0', 57600) -rad2 = sikRadio('/dev/ttyUSB1', '1', 57600) -rad1.open() -rad2.open() - - -time.sleep(1) - -gcs = GcsCommications(rad1) -fds = FdsComms(rad2) -gcs.add_radio('1') - -gcs.start() -fds.start() - -# gcs.set_ping_config(0, [1000]) diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..f2e56a0 --- /dev/null +++ b/tests/__init__.py @@ -0,0 +1 @@ +"""Test package for radio-telemetry-tracker-drone-comms-package.""" diff --git a/tests/test_codec.py b/tests/test_codec.py new file mode 100644 index 0000000..8d6cc67 --- /dev/null +++ b/tests/test_codec.py @@ -0,0 +1,63 @@ +"""Unit tests for the radio codec module.""" + +from radio_telemetry_tracker_drone_comms_package.codec import ( + RadioCodec, + _calculate_crc16_ccitt, +) +from radio_telemetry_tracker_drone_comms_package.proto.packets_pb2 import RadioPacket + +# Test constants +CRC16_CCITT_REF_VALUE = 0x29B1 +TEST_PACKET_ID = 1234 +TEST_TIMESTAMP = 999999 +TEST_ACK_ID = 5678 + + +def test_calculate_crc16_ccitt() -> None: + """Test CRC16-CCITT calculation against known reference value.""" + data = b"123456789" # Classic check data for CRC + assert _calculate_crc16_ccitt(data) == CRC16_CCITT_REF_VALUE # noqa: S101 + + +def test_encode_decode_packet() -> None: + """Test packet encoding and decoding with a simple acknowledgment packet.""" + packet = RadioPacket() + packet.ack_pkt.base.packet_id = TEST_PACKET_ID + packet.ack_pkt.base.need_ack = False + packet.ack_pkt.base.timestamp = TEST_TIMESTAMP + packet.ack_pkt.ack_id = TEST_ACK_ID + + encoded = RadioCodec.encode_packet(packet) + assert encoded.startswith(b"\xAA\x55") # noqa: S101 + + decoded = RadioCodec.decode_packet(encoded) + assert decoded is not None # noqa: S101 + assert decoded.ack_pkt.base.packet_id == TEST_PACKET_ID # noqa: S101 + assert decoded.ack_pkt.ack_id == TEST_ACK_ID # noqa: S101 + + +def test_decode_packet_invalid_sync_marker() -> None: + """Test packet decoding with invalid sync marker returns None.""" + data = b"\x00\x11\x22\x33" + result = RadioCodec.decode_packet(data) + assert result is None # noqa: S101 + + +def test_decode_packet_truncated() -> None: + """Test packet decoding with truncated data returns None.""" + data = b"\xAA\x55\x00\x00\x00\x05" # length=5 but we won't supply the 5 bytes + checksum + result = RadioCodec.decode_packet(data) + assert result is None # noqa: S101 + + +def test_decode_packet_bad_checksum() -> None: + """Test packet decoding with corrupted checksum returns None.""" + packet = RadioPacket() + packet.ping_pkt.base.packet_id = 10 + encoded = RadioCodec.encode_packet(packet) + + # Corrupt the last two bytes (the CRC) + corrupted = encoded[:-2] + b"\x00\x00" + result = RadioCodec.decode_packet(corrupted) + assert result is None # noqa: S101 + diff --git a/tests/test_compiler.py b/tests/test_compiler.py new file mode 100644 index 0000000..922991a --- /dev/null +++ b/tests/test_compiler.py @@ -0,0 +1,86 @@ +"""Unit tests for the protobuf compiler module.""" + +from pathlib import Path + +import pytest +from pytest_mock import MockerFixture + +from radio_telemetry_tracker_drone_comms_package.proto.compiler import ( + ensure_proto_compiled, +) + + +def test_ensure_proto_compiled_success(mocker: MockerFixture, tmp_path: Path) -> None: + """Test that ensure_proto_compiled() calls protoc.main with success.""" + # Mock the Path used in compiler.py so it points to tmp_path + mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.Path", + return_value=tmp_path, + ) + # Simulate a 'packets.proto' file existing + (tmp_path / "packets.proto").write_text('syntax = "proto3";') + + mock_protoc_main = mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.protoc.main", + return_value=0, + ) + mock_logger = mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.logger", + autospec=True, + ) + + ensure_proto_compiled() + + mock_protoc_main.assert_called_once() + mock_logger.info.assert_called_once_with("Successfully compiled protobuf file") + + +def test_ensure_proto_compiled_nonzero_exit(mocker: MockerFixture, tmp_path: Path) -> None: + """Test that ensure_proto_compiled() raises RuntimeError if protoc.main returns non-zero.""" + mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.Path", + return_value=tmp_path, + ) + (tmp_path / "packets.proto").write_text('syntax = "proto3";') + + mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.protoc.main", + return_value=1, + ) + mock_logger = mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.logger", + autospec=True, + ) + + with pytest.raises(RuntimeError) as exc_info: + ensure_proto_compiled() + + assert "protoc returned non-zero status: 1" in str(exc_info.value) # noqa: S101 + mock_logger.error.assert_any_call("protoc returned non-zero status: 1") + + +def test_ensure_proto_compiled_oserror(mocker: MockerFixture, tmp_path: Path) -> None: + """Test that ensure_proto_compiled() raises RuntimeError if protoc.main raises OSError.""" + mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.Path", + return_value=tmp_path, + ) + (tmp_path / "packets.proto").write_text('syntax = "proto3";') + + mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.protoc.main", + side_effect=OSError("protoc not found"), + ) + mock_logger = mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.logger", + autospec=True, + ) + + with pytest.raises(RuntimeError) as exc_info: + ensure_proto_compiled() + + assert "Failed to compile protobuf: protoc not found" in str(exc_info.value) # noqa: S101 + mock_logger.error.assert_any_call( + "Make sure grpcio-tools is installed:\npoetry add grpcio-tools", + ) + mock_logger.error.assert_any_call("Failed to compile protobuf: protoc not found") diff --git a/tests/test_data_models.py b/tests/test_data_models.py new file mode 100644 index 0000000..d322883 --- /dev/null +++ b/tests/test_data_models.py @@ -0,0 +1,110 @@ +"""Unit tests for the data model classes used in radio telemetry packet handling.""" + +from typing import Any + +import pytest + +from radio_telemetry_tracker_drone_comms_package.data_models import ( + ConfigRequestData, + ConfigResponseData, + ErrorData, + GPSData, + LocEstData, + PingData, + StartRequestData, + StartResponseData, + StopRequestData, + StopResponseData, + SyncRequestData, + SyncResponseData, +) + +# Test constants +TEST_GAIN = 1.5 +TEST_SAMPLING_RATE = 48000 +TEST_TARGET_FREQUENCIES = [100, 200, 300] +TEST_SYNC_PACKET_ID = 123 +TEST_SYNC_TIMESTAMP = 456789 + + +def test_sync_request_data() -> None: + """Test SyncRequestData initialization and attribute access.""" + data = SyncRequestData(packet_id=TEST_SYNC_PACKET_ID, timestamp=TEST_SYNC_TIMESTAMP) + assert data.packet_id == TEST_SYNC_PACKET_ID # noqa: S101 + assert data.timestamp == TEST_SYNC_TIMESTAMP # noqa: S101 + + +def test_config_request_data() -> None: + """Test ConfigRequestData initialization with all parameters.""" + data = ConfigRequestData( + packet_id=10, + timestamp=999, + gain=TEST_GAIN, + sampling_rate=TEST_SAMPLING_RATE, + center_frequency=150000, + run_num=1, + enable_test_data=True, + ping_width_ms=10, + ping_min_snr=5, + ping_max_len_mult=2.5, + ping_min_len_mult=1.2, + target_frequencies=TEST_TARGET_FREQUENCIES, + ) + assert data.gain == TEST_GAIN # noqa: S101 + assert data.sampling_rate == TEST_SAMPLING_RATE # noqa: S101 + assert data.target_frequencies == TEST_TARGET_FREQUENCIES # noqa: S101 + + +@pytest.mark.parametrize( + ("klass", "kwargs"), + [ + (SyncResponseData, {"success": True, "packet_id": 1, "timestamp": 1000}), + (ConfigResponseData, {"success": False, "packet_id": 2, "timestamp": 2000}), + ( + GPSData, + { + "packet_id": 3, + "timestamp": 3000, + "easting": 10.0, + "northing": 20.0, + "altitude": 100.0, + "heading": 45.5, + "epsg_code": 4326, + }, + ), + ( + PingData, + { + "packet_id": 4, + "timestamp": 4000, + "frequency": 120, + "amplitude": 0.8, + "easting": 123.4, + "northing": 567.8, + "altitude": 9.0, + "epsg_code": 4326, + }, + ), + ( + LocEstData, + { + "packet_id": 5, + "timestamp": 5000, + "frequency": 180, + "easting": 111.1, + "northing": 222.2, + "epsg_code": 4326, + }, + ), + (StartRequestData, {"packet_id": 6, "timestamp": 6000}), + (StartResponseData, {"packet_id": 7, "timestamp": 7000, "success": True}), + (StopRequestData, {"packet_id": 8, "timestamp": 8000}), + (StopResponseData, {"packet_id": 9, "timestamp": 9000, "success": False}), + (ErrorData, {"packet_id": 10, "timestamp": 10000}), + ], +) +def test_data_classes(klass: type[Any], kwargs: dict[str, Any]) -> None: + """Test initialization and attribute access for all data model classes.""" + instance = klass(**kwargs) + for k, v in kwargs.items(): + assert getattr(instance, k) == v # noqa: S101 diff --git a/tests/test_drone_comms.py b/tests/test_drone_comms.py new file mode 100644 index 0000000..0969c6f --- /dev/null +++ b/tests/test_drone_comms.py @@ -0,0 +1,138 @@ +"""Unit tests for the DroneComms class handling radio communication with the drone.""" +from __future__ import annotations + +from typing import Protocol + +import pytest + +from radio_telemetry_tracker_drone_comms_package.data_models import ( + ConfigRequestData, + ConfigResponseData, + SyncRequestData, +) +from radio_telemetry_tracker_drone_comms_package.drone_comms import ( + DroneComms, + RadioConfig, +) +from radio_telemetry_tracker_drone_comms_package.proto.packets_pb2 import RadioPacket + + +class MockInterface(Protocol): + """Protocol for mock radio interface.""" + + def connect(self) -> None: + """Connect to the radio interface.""" + ... + + def close(self) -> None: + """Close the radio interface connection.""" + ... + + def send_packet(self, packet: RadioPacket) -> None: + """Send a packet over the radio interface.""" + ... + + def receive_packet(self) -> None | RadioPacket: + """Receive a packet from the radio interface.""" + ... + + +@pytest.fixture +def mock_radio_interface() -> MockInterface: + """Create a mock radio interface for testing.""" + + class MockImpl: + def connect(self) -> None: + pass + + def close(self) -> None: + pass + + def send_packet(self, packet: RadioPacket) -> None: + pass + + def receive_packet(self) -> None: + return None + + return MockImpl() + + +@pytest.fixture +def drone_comms(mock_radio_interface: MockInterface) -> DroneComms: + """Create a DroneComms instance with mock radio interface for testing.""" + cfg = RadioConfig(interface_type="serial", port="COM3", baudrate=9600) + comms = DroneComms(radio_config=cfg) + # Inject the mock interface + comms.radio_interface = mock_radio_interface + return comms + + +def test_drone_comms_init_missing_config() -> None: + """Test DroneComms initialization fails when config is missing.""" + with pytest.raises(ValueError, match="Radio config is required"): + DroneComms(None) + + +def test_drone_comms_init_serial_port_missing() -> None: + """Test DroneComms initialization fails when serial port is missing.""" + cfg = RadioConfig(interface_type="serial", port=None) + with pytest.raises(ValueError, match="Serial port must be specified"): + DroneComms(cfg) + + +def test_drone_comms_init_invalid_interface() -> None: + """Test DroneComms initialization fails with invalid interface type.""" + cfg = RadioConfig(interface_type="unknown") + with pytest.raises(ValueError, match="Invalid interface type"): + DroneComms(cfg) + + +def test_drone_comms_send_sync_request(drone_comms: DroneComms) -> None: + """Test sending sync request returns valid packet ID and timestamp.""" + pid, need_ack, timestamp = drone_comms.send_sync_request(SyncRequestData()) + assert need_ack is True # noqa: S101 + assert pid != 0 # noqa: S101 + assert timestamp != 0 # noqa: S101 + + +def test_drone_comms_send_config_request(drone_comms: DroneComms) -> None: + """Test sending config request with all parameters.""" + data = ConfigRequestData( + gain=1.1, + sampling_rate=32000, + center_frequency=1234, + run_num=2, + enable_test_data=True, + ping_width_ms=5, + ping_min_snr=10, + ping_max_len_mult=2.0, + ping_min_len_mult=1.0, + target_frequencies=[100, 200], + ) + pid, need_ack, timestamp = drone_comms.send_config_request(data) + assert need_ack is True # noqa: S101 + assert pid != 0 # noqa: S101 + assert timestamp != 0 # noqa: S101 + + +def test_drone_comms_register_and_invoke_handler(drone_comms: DroneComms) -> None: + """Test registering and invoking config response handler.""" + # Test registering a config response handler, then simulating reception + results = [] + + def on_config_response(resp: ConfigResponseData) -> None: + results.append(resp.success) + + drone_comms.register_config_response_handler(on_config_response) + + # Simulate a RadioPacket containing ConfigResponsePacket + rp = RadioPacket() + rp.cfg_rsp.base.packet_id = 999 + rp.cfg_rsp.base.timestamp = 123456 + rp.cfg_rsp.success = True + + # Simulate that DroneComms receives this packet + drone_comms.on_user_packet_received(rp) + + assert len(results) == 1 # noqa: S101 + assert results[0] is True # noqa: S101 diff --git a/tests/test_interfaces.py b/tests/test_interfaces.py new file mode 100644 index 0000000..27b3a7d --- /dev/null +++ b/tests/test_interfaces.py @@ -0,0 +1,92 @@ +"""Unit tests for radio interface implementations including serial and simulated modes.""" + +import socket +from unittest.mock import MagicMock, call, patch + +import pytest + +from radio_telemetry_tracker_drone_comms_package.interfaces import ( + RadioInterface, + SerialRadioInterface, + SimulatedRadioInterface, +) + +# Test constants +EXPECTED_CLOSE_CALLS = 2 # Both self.sock and self.conn close in client mode + + +def test_radio_interface_abstract() -> None: + """RadioInterface is abstract and cannot be instantiated directly.""" + with pytest.raises(TypeError): + RadioInterface() + + +@patch("serial.Serial") +def test_serial_radio_interface(mock_serial: MagicMock) -> None: + """Test that SerialRadioInterface connect() calls serial.Serial with correct args.""" + interface = SerialRadioInterface(port="COM3", baudrate=9600, timeout=2.0) + interface.connect() + mock_serial.assert_called_once_with("COM3", 9600, timeout=2.0) + + # Mock a read + mock_serial.return_value.read.return_value = b"abc" + data = interface._read_data(3) # noqa: SLF001 + assert data == b"abc" # noqa: S101 + + interface.close() + mock_serial.return_value.close.assert_called_once() + + +@patch("socket.socket") +def test_simulated_radio_interface_client_mode(mock_socket: MagicMock) -> None: + """Test SimulatedRadioInterface in client mode connects to server.""" + mock_sock_instance = mock_socket.return_value + interface = SimulatedRadioInterface(host="testhost", port=12345, server_mode=False) + interface.connect() + + mock_socket.assert_called_once_with(socket.AF_INET, socket.SOCK_STREAM) + assert mock_sock_instance.settimeout.call_args_list == [ # noqa: S101 + call(10.0), # Connection timeout + call(1.0), # Read timeout + ] + mock_sock_instance.connect.assert_called_once_with(("testhost", 12345)) + + interface.close() + # In client mode, both self.sock and self.conn point to the same socket + assert mock_sock_instance.close.call_count == EXPECTED_CLOSE_CALLS # noqa: S101 + + +@patch("socket.socket") +@patch("time.sleep") # Patch sleep to speed up test +def test_simulated_radio_interface_server_mode( + mock_sleep: MagicMock, + mock_socket: MagicMock, +) -> None: + """Test SimulatedRadioInterface in server mode binds and listens.""" + mock_sock_instance = mock_socket.return_value + interface = SimulatedRadioInterface( + host="localhost", + port=50000, + server_mode=True, + timeout=2.0, + ) + + # Simulate connection sequence + mock_sock_instance.accept.side_effect = [ + BlockingIOError(), # First call blocks + (mock_sock_instance, ("clienthost", 9999)), # Second call succeeds + ] + + interface.connect() # Should succeed after retry + + mock_socket.assert_called_once_with(socket.AF_INET, socket.SOCK_STREAM) + mock_sock_instance.bind.assert_called_once_with(("localhost", 50000)) + mock_sock_instance.listen.assert_called_once_with(1) + mock_sock_instance.setblocking.assert_called_once_with(False) # noqa: FBT003 + assert mock_sock_instance.settimeout.call_args_list == [ # noqa: S101 + call(2.0), # Read timeout after connection + ] + mock_sleep.assert_called_once_with(0.1) # Verify sleep was called between retries + + interface.close() + mock_sock_instance.close.assert_called() diff --git a/tests/test_proto_init.py b/tests/test_proto_init.py new file mode 100644 index 0000000..e32f8db --- /dev/null +++ b/tests/test_proto_init.py @@ -0,0 +1,39 @@ +"""Unit tests for protobuf module initialization and message imports.""" + +import sys + +from pytest_mock import MockerFixture + + +def test_proto_init_calls_compiler(mocker: MockerFixture) -> None: + """Test that ensure_proto_compiled() is invoked automatically when the proto/__init__.py module is imported.""" + mock_compiler = mocker.patch( + "radio_telemetry_tracker_drone_comms_package.proto.compiler.ensure_proto_compiled", + autospec=True, + ) + + # Force a reload to trigger the import-time side effect + if "radio_telemetry_tracker_drone_comms_package.proto.__init__" in sys.modules: + del sys.modules["radio_telemetry_tracker_drone_comms_package.proto.__init__"] + + import radio_telemetry_tracker_drone_comms_package.proto.__init__ # noqa: F401 + + mock_compiler.assert_called_once() + + +def test_protobuf_messages_are_imported() -> None: + """Test that protobuf message classes are imported and accessible.""" + from radio_telemetry_tracker_drone_comms_package.proto import ( + AckPacket, + BasePacket, + RadioPacket, + ) + + # Instantiate some classes + ack_packet = AckPacket() + base_packet = BasePacket() + radio_packet = RadioPacket() + + assert ack_packet is not None # noqa: S101 + assert base_packet is not None # noqa: S101 + assert radio_packet is not None # noqa: S101 diff --git a/tests/test_transceiver.py b/tests/test_transceiver.py new file mode 100644 index 0000000..0fdd0c4 --- /dev/null +++ b/tests/test_transceiver.py @@ -0,0 +1,139 @@ +"""Unit tests for the packet transceiver module handling radio packet transmission and acknowledgments.""" + +from __future__ import annotations + +import time +from unittest.mock import MagicMock + +from radio_telemetry_tracker_drone_comms_package.proto.packets_pb2 import RadioPacket +from radio_telemetry_tracker_drone_comms_package.transceiver import ( + PacketManager, + _current_timestamp_us, +) + +# Test constants +DEFAULT_ACK_TIMEOUT = 2.0 +DEFAULT_MAX_RETRIES = 5 +TEST_ACK_TIMEOUT = 0.1 +TEST_MAX_RETRIES = 1 +TEST_PACKET_ID = 100 +TEST_ACK_ID = 101 + + +class MockRadioInterface: + """Mock implementation of radio interface for testing.""" + + def connect(self) -> None: + """Connect to the mock radio interface.""" + + def close(self) -> None: + """Close the mock radio interface connection.""" + + def send_packet(self, packet: RadioPacket) -> None: + """Send a packet through the mock radio interface.""" + + def receive_packet(self) -> None | RadioPacket: + """Receive a packet from the mock radio interface.""" + return None + + +def test_current_timestamp_us() -> None: + """Test that current timestamp function returns valid integer microseconds.""" + ts = _current_timestamp_us() + assert isinstance(ts, int) # noqa: S101 + assert ts > 0 # noqa: S101 + + +def test_packet_manager_initialization() -> None: + """Test PacketManager initialization with default parameters.""" + radio = MockRadioInterface() + manager = PacketManager(radio_interface=radio) + assert manager.radio_interface is radio # noqa: S101 + assert manager.ack_timeout == DEFAULT_ACK_TIMEOUT # noqa: S101 + assert manager.max_retries == DEFAULT_MAX_RETRIES # noqa: S101 + assert manager.on_ack_timeout is None # noqa: S101 + + +def test_packet_manager_start_stop() -> None: + """Test PacketManager start and stop operations.""" + radio = MagicMock() + manager = PacketManager(radio_interface=radio) + manager.start() + radio.connect.assert_called_once() + assert manager._send_thread.is_alive() # noqa: S101, SLF001 + assert manager._recv_thread.is_alive() # noqa: S101, SLF001 + + manager.stop() + radio.close.assert_called_once() + assert not manager._send_thread.is_alive() # noqa: S101, SLF001 + assert not manager._recv_thread.is_alive() # noqa: S101, SLF001 + + +def test_packet_manager_enqueue() -> None: + """Test packet enqueuing with priority based on acknowledgment requirement.""" + radio = MockRadioInterface() + manager = PacketManager(radio_interface=radio) + pkt = RadioPacket() + pkt.ack_pkt.base.need_ack = True + manager.enqueue_packet(pkt) + + # PriorityQueue item => (priority, enq_time, packet) + item = manager.send_queue.get_nowait() + assert item[0] == 0 # Priority 0 for need_ack=True # noqa: S101 + assert item[2] is pkt # noqa: S101 + + +def test_packet_manager_ack_handling() -> None: + """Test acknowledgment handling with timeout and retry logic.""" + radio = MockRadioInterface() + manager = PacketManager( + radio_interface=radio, + ack_timeout=TEST_ACK_TIMEOUT, + max_retries=TEST_MAX_RETRIES, + ) + + # Start the manager + manager.start() + try: + # Enqueue a packet that needs ack + pkt = RadioPacket() + pkt.ack_pkt.base.packet_id = TEST_PACKET_ID + pkt.ack_pkt.base.need_ack = True + manager.enqueue_packet(pkt) + + # Let the send loop run + time.sleep(0.2) + + # Expect that the packet is in outstanding_acks + assert TEST_PACKET_ID in manager.outstanding_acks # noqa: S101 + + # Wait for retry logic + time.sleep(0.2) + + # Because max_retries=1, after 1 retry it should time out + # and remove the packet from outstanding_acks + assert TEST_PACKET_ID not in manager.outstanding_acks # noqa: S101 + finally: + manager.stop() + + +def test_packet_manager_handle_incoming_ack() -> None: + """Test handling of incoming acknowledgment packets.""" + radio = MockRadioInterface() + manager = PacketManager(radio_interface=radio) + pkt = RadioPacket() + pkt.cfg_rqt.base.packet_id = TEST_ACK_ID + pkt.cfg_rqt.base.need_ack = True + + manager.outstanding_acks[TEST_ACK_ID] = { + "packet": pkt, + "send_time": time.time(), + "retries": 0, + } + + # Make an ack packet + ack = RadioPacket() + ack.ack_pkt.ack_id = TEST_ACK_ID + + manager._handle_incoming_packet(ack) # noqa: SLF001 + assert TEST_ACK_ID not in manager.outstanding_acks # noqa: S101 diff --git a/tools/__init__.py b/tools/__init__.py new file mode 100644 index 0000000..e0935c1 --- /dev/null +++ b/tools/__init__.py @@ -0,0 +1 @@ +"""Tools for testing and interacting with the radio telemetry tracker.""" diff --git a/tools/gcs_fds_cli.py b/tools/gcs_fds_cli.py new file mode 100644 index 0000000..65bf800 --- /dev/null +++ b/tools/gcs_fds_cli.py @@ -0,0 +1,653 @@ +"""Command-line interface for testing drone communications.""" + +from __future__ import annotations + +import argparse +import cmd +import logging +import sys +from typing import Callable + +from radio_telemetry_tracker_drone_comms_package import ( + ConfigRequestData, + ConfigResponseData, + DroneComms, + ErrorData, + GPSData, + LocEstData, + PingData, + RadioConfig, + StartRequestData, + StartResponseData, + StopRequestData, + StopResponseData, + SyncRequestData, + SyncResponseData, +) + +logger = logging.getLogger(__name__) + + +class GCSFDSCLI(cmd.Cmd): + """Interactive CLI for testing drone communications.""" + + intro = "Welcome to the GCS/FDS CLI. Type help or ? to list commands.\n" + prompt = "(gcs-fds) " + + def __init__(self, radio_config: RadioConfig) -> None: + """Initialize the CLI with radio configuration. + + Args: + radio_config: Configuration for the radio interface + """ + super().__init__() + self.drone_comms = None + self.radio_config = radio_config + self.started = False + + # Store callback references + self.registered_callbacks = {} + + # Map packet types to their register/unregister method names + self.packet_type_map = { + "sync": ( + "register_sync_request_handler", + "unregister_sync_request_handler", + ), + "sync_response": ( + "register_sync_response_handler", + "unregister_sync_response_handler", + ), + "config": ( + "register_config_request_handler", + "unregister_config_request_handler", + ), + "config_response": ( + "register_config_response_handler", + "unregister_config_response_handler", + ), + "gps": ("register_gps_handler", "unregister_gps_handler"), + "ping": ("register_ping_handler", "unregister_ping_handler"), + "loc": ("register_loc_est_handler", "unregister_loc_est_handler"), + "start": ( + "register_start_request_handler", + "unregister_start_request_handler", + ), + "start_response": ( + "register_start_response_handler", + "unregister_start_response_handler", + ), + "stop": ( + "register_stop_request_handler", + "unregister_stop_request_handler", + ), + "stop_response": ( + "register_stop_response_handler", + "unregister_stop_response_handler", + ), + "error": ("register_error_handler", "unregister_error_handler"), + } + + def do_start(self, arg: str) -> None: # noqa: ARG002 + """Start the drone communications. + + Args: + arg: Command argument (unused) + """ + if self.started: + logger.info("Already started.") + return + + self.drone_comms = DroneComms(radio_config=self.radio_config) + self.drone_comms.start() + self.started = True + logger.info("Started drone communications.") + + def do_stop(self, arg: str) -> None: # noqa: ARG002 + """Stop the drone communications. + + Args: + arg: Command argument (unused) + """ + if not self.started: + logger.info("Not started.") + return + + self.drone_comms.stop() + self.started = False + logger.info("Stopped drone communications.") + + def do_register(self, arg: str) -> None: + """Register a callback for a specific packet type. + + Args: + arg: Command argument + """ + parts = arg.split() + if not parts: + logger.info("Usage: register [once]") + return + pkt_type = parts[0].lower() + once = (len(parts) > 1) and (parts[1].lower() == "once") + + if pkt_type not in self.packet_type_map: + logger.info("Unknown packet type: %s", pkt_type) + return + + register_method_name, _ = self.packet_type_map[pkt_type] + register_method = getattr(self.drone_comms, register_method_name) + + # Create and store callback reference + callback_func = self.make_print_callback(pkt_type) + if pkt_type not in self.registered_callbacks: + self.registered_callbacks[pkt_type] = [] + self.registered_callbacks[pkt_type].append(callback_func) + + register_method(callback_func, once=once) + mode_str = "ONCE" if once else "ALWAYS" + logger.info("Registered print callback for '%s' (%s).", pkt_type, mode_str) + + def do_unregister(self, arg: str) -> None: + """Unregister a callback for a specific packet type. + + Args: + arg: Command argument + """ + pkt_type = arg.strip().lower() + if pkt_type not in self.packet_type_map: + logger.info("Unknown packet type: %s", pkt_type) + return + + if ( + pkt_type not in self.registered_callbacks + or not self.registered_callbacks[pkt_type] + ): + logger.info("No callbacks registered for '%s'.", pkt_type) + return + + _, unregister_method_name = self.packet_type_map[pkt_type] + unregister_method = getattr(self.drone_comms, unregister_method_name) + + # Unregister all callbacks for this packet type + success = False + for callback in self.registered_callbacks[pkt_type]: + if unregister_method(callback): + success = True + + if success: + self.registered_callbacks[pkt_type].clear() + logger.info("Unregistered callbacks for '%s'.", pkt_type) + else: + logger.info("Failed to unregister callbacks (unexpected error).") + + def do_send_sync_request(self, arg: str) -> None: # noqa: ARG002 + """Send a sync request packet. + + Args: + arg: Command argument (unused) + """ + pid, ack, ts = self.drone_comms.send_sync_request(SyncRequestData()) + logger.info( + "Sent SyncRequest (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_sync_response(self, arg: str) -> None: + """Send sync response packet. + + Args: + arg: Command argument + """ + parts = arg.split() + success = True + if parts and parts[0].lower() in ("false", "0", "no"): + success = False + + data = SyncResponseData(success=success) + pid, ack, ts = self.drone_comms.send_sync_response(data) + logger.info( + "Sent SyncResponse (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def _get_default_config(self) -> ConfigRequestData: + """Get default configuration values. + + Returns: + Default ConfigRequestData + """ + return ConfigRequestData( + gain=50.0, + sampling_rate=2_000_000, + center_frequency=150_000_000, + run_num=1, + enable_test_data=False, + ping_width_ms=20, + ping_min_snr=10, + ping_max_len_mult=1.5, + ping_min_len_mult=0.5, + target_frequencies=[150_000_000], + ) + + def _parse_config_request_args(self, args: list[str]) -> ConfigRequestData | None: + """Parse arguments for config request. + + Args: + args: List of argument strings + + Returns: + Parsed ConfigRequestData or None if parsing failed + """ + min_args_for_frequencies = 10 + data = self._get_default_config() + + try: + parsers = [ + (1, lambda x: setattr(data, "gain", float(x))), + (2, lambda x: setattr(data, "sampling_rate", int(x))), + (3, lambda x: setattr(data, "center_frequency", int(x))), + (4, lambda x: setattr(data, "run_num", int(x))), + ( + 5, + lambda x: setattr( + data, + "enable_test_data", + x.lower() in ("true", "1", "yes"), + ), + ), + (6, lambda x: setattr(data, "ping_width_ms", int(x))), + (7, lambda x: setattr(data, "ping_min_snr", int(x))), + (8, lambda x: setattr(data, "ping_max_len_mult", float(x))), + (9, lambda x: setattr(data, "ping_min_len_mult", float(x))), + ] + + for idx, parser in parsers: + if len(args) >= idx: + parser(args[idx - 1]) + + if len(args) >= min_args_for_frequencies: + data.target_frequencies = [int(f) for f in args[9:]] + except ValueError: + logger.exception("Invalid argument format: %s") + return None + return data + + def do_send_config_request(self, arg: str) -> None: + """Send config request packet. + + Args: + arg: Command argument in format: + [gain] [sampling_rate] [center_freq] [run_num] [test_data] + [ping_width] [ping_snr] [max_len_mult] [min_len_mult] [target_freqs...] + """ + parts = arg.split() + data = ( + self._parse_config_request_args(parts) + if parts + else ConfigRequestData( + gain=50.0, + sampling_rate=2_000_000, + center_frequency=150_000_000, + run_num=1, + enable_test_data=False, + ping_width_ms=20, + ping_min_snr=10, + ping_max_len_mult=1.5, + ping_min_len_mult=0.5, + target_frequencies=[150_000_000], + ) + ) + + if data is None: + return + + pid, ack, ts = self.drone_comms.send_config_request(data) + logger.info( + "Sent ConfigRequest (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_config_response(self, arg: str) -> None: + """Send config response packet. + + Args: + arg: Command argument + """ + parts = arg.split() + success = True + if parts and parts[0].lower() in ("false", "0", "no"): + success = False + + data = ConfigResponseData(success=success) + pid, ack, ts = self.drone_comms.send_config_response(data) + logger.info( + "Sent ConfigResponse (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_gps(self, arg: str) -> None: + """Send GPS packet. + + Args: + arg: Command argument in format: [easting] [northing] [altitude] [heading] [epsg_code] + """ + # Default values + data = GPSData( + easting=500000.0, + northing=4000000.0, + altitude=100.0, + heading=45.0, + epsg_code=32610, + ) + + # Parse arguments if provided + parts = arg.split() + if parts: + try: + min_args = { + "easting": 1, + "northing": 2, + "altitude": 3, + "heading": 4, + "epsg_code": 5, + } + if len(parts) >= min_args["easting"]: + data.easting = float(parts[0]) + if len(parts) >= min_args["northing"]: + data.northing = float(parts[1]) + if len(parts) >= min_args["altitude"]: + data.altitude = float(parts[2]) + if len(parts) >= min_args["heading"]: + data.heading = float(parts[3]) + if len(parts) >= min_args["epsg_code"]: + data.epsg_code = int(parts[4]) + except ValueError: + logger.exception("Invalid argument format: %s") + return + + pid, ack, ts = self.drone_comms.send_gps(data) + logger.info( + "Sent GPS (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_ping(self, arg: str) -> None: + """Send ping packet. + + Args: + arg: Command argument in format: [frequency] [amplitude] [easting] [northing] [altitude] [epsg_code] + """ + # Default values + data = PingData( + frequency=150_000_000, + amplitude=1.0, + easting=500000.0, + northing=4000000.0, + altitude=100.0, + epsg_code=32610, + ) + + # Parse arguments if provided + parts = arg.split() + if parts: + try: + min_args = { + "frequency": 1, + "amplitude": 2, + "easting": 3, + "northing": 4, + "altitude": 5, + "epsg_code": 6, + } + if len(parts) >= min_args["frequency"]: + data.frequency = int(parts[0]) + if len(parts) >= min_args["amplitude"]: + data.amplitude = float(parts[1]) + if len(parts) >= min_args["easting"]: + data.easting = float(parts[2]) + if len(parts) >= min_args["northing"]: + data.northing = float(parts[3]) + if len(parts) >= min_args["altitude"]: + data.altitude = float(parts[4]) + if len(parts) >= min_args["epsg_code"]: + data.epsg_code = int(parts[5]) + except ValueError: + logger.exception("Invalid argument format: %s") + return + + pid, ack, ts = self.drone_comms.send_ping(data) + logger.info( + "Sent Ping (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_loc_est(self, arg: str) -> None: + """Send location estimate packet. + + Args: + arg: Command argument in format: [frequency] [easting] [northing] [epsg_code] + """ + # Default values + data = LocEstData( + frequency=150_000_000, + easting=500000.0, + northing=4000000.0, + epsg_code=32610, + ) + + # Parse arguments if provided + parts = arg.split() + if parts: + try: + min_args = { + "frequency": 1, + "easting": 2, + "northing": 3, + "epsg_code": 4, + } + if len(parts) >= min_args["frequency"]: + data.frequency = int(parts[0]) + if len(parts) >= min_args["easting"]: + data.easting = float(parts[1]) + if len(parts) >= min_args["northing"]: + data.northing = float(parts[2]) + if len(parts) >= min_args["epsg_code"]: + data.epsg_code = int(parts[3]) + except ValueError: + logger.exception("Invalid argument format: %s") + return + + pid, ack, ts = self.drone_comms.send_loc_est(data) + logger.info( + "Sent LocEst (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_start_request(self, arg: str) -> None: # noqa: ARG002 + """Send start request packet. + + Args: + arg: Command argument (unused) + """ + pid, ack, ts = self.drone_comms.send_start_request(StartRequestData()) + logger.info( + "Sent StartRequest (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_start_response(self, arg: str) -> None: + """Send start response packet. + + Args: + arg: Command argument + """ + parts = arg.split() + success = True + if parts and parts[0].lower() in ("false", "0", "no"): + success = False + + data = StartResponseData(success=success) + pid, ack, ts = self.drone_comms.send_start_response(data) + logger.info( + "Sent StartResponse (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_stop_request(self, arg: str) -> None: # noqa: ARG002 + """Send stop request packet. + + Args: + arg: Command argument (unused) + """ + pid, ack, ts = self.drone_comms.send_stop_request(StopRequestData()) + logger.info( + "Sent StopRequest (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_stop_response(self, arg: str) -> None: + """Send stop response packet. + + Args: + arg: Command argument + """ + parts = arg.split() + success = True + if parts and parts[0].lower() in ("false", "0", "no"): + success = False + + data = StopResponseData(success=success) + pid, ack, ts = self.drone_comms.send_stop_response(data) + logger.info( + "Sent StopResponse (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_send_error(self, arg: str) -> None: # noqa: ARG002 + """Send error packet. + + Args: + arg: Command argument (unused) + """ + pid, ack, ts = self.drone_comms.send_error(ErrorData()) + logger.info( + "Sent Error (packet_id=%s, need_ack=%s, timestamp=%s)", + pid, + ack, + ts, + ) + + def do_quit(self, arg: str) -> bool: # noqa: ARG002 + """Quit the CLI. + + Args: + arg: Command argument (unused) + + Returns: + bool: True to exit + """ + if self.started: + self.do_stop("") + return True + + def make_print_callback(self, pkt_type: str) -> Callable: + """Create a callback function that prints packet data. + + Args: + pkt_type: Type of packet this callback is for + + Returns: + Callable: The callback function + """ + + def callback(data: object) -> None: + logger.info("Received %s: %s", pkt_type, data) + + return callback + + +def main() -> None: + """Run the CLI.""" + parser = argparse.ArgumentParser(description="GCS/FDS CLI") + parser.add_argument( + "--interface", + choices=["serial", "simulated"], + default="simulated", + help="Radio interface type", + ) + parser.add_argument("--port", help="Serial port (for serial interface)") + parser.add_argument( + "--baudrate", + type=int, + help="Serial baudrate (for serial interface)", + ) + parser.add_argument("--host", help="TCP host (for simulated interface)") + parser.add_argument( + "--tcp-port", + type=int, + help="TCP port (for simulated interface)", + ) + parser.add_argument( + "--server-mode", + action="store_true", + help="Run in server mode (for simulated interface)", + ) + args = parser.parse_args() + + # Configure logging + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s - %(name)s - %(levelname)s - %(message)s", + ) + + # Build radio config from args + config_kwargs = {"interface_type": args.interface} + if args.interface == "serial": + if not args.port: + logger.error("Serial port is required for serial interface") + sys.exit(1) + config_kwargs["port"] = args.port + if args.baudrate: + config_kwargs["baudrate"] = args.baudrate + else: # simulated + if args.host: + config_kwargs["host"] = args.host + if args.tcp_port: + config_kwargs["tcp_port"] = args.tcp_port + if args.server_mode: + config_kwargs["server_mode"] = True + + radio_config = RadioConfig(**config_kwargs) + cli = GCSFDSCLI(radio_config) + + try: + cli.cmdloop() + except KeyboardInterrupt: + if cli.started: + cli.do_stop("") + + +if __name__ == "__main__": + main() From 59bdaf5e36ca0a03d04d7a456aa860d8fb307647 Mon Sep 17 00:00:00 2001 From: Tyler Flar Date: Tue, 24 Dec 2024 14:32:43 -0800 Subject: [PATCH 6/7] docs: added missing handler to README example --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index b3e883b..fcbb3f0 100644 --- a/README.md +++ b/README.md @@ -95,6 +95,9 @@ from radio_telemetry_tracker_drone_comms_package import DroneComms, RadioConfig, def on_gps_data(data: GPSData): print(f"GPS: {data.easting}, {data.northing}, {data.altitude}") + comms.register_gps_handler(on_gps_data) + ``` + 3. **Start communication**: ```python comms.start() # Opens the radio interface and starts Rx/Tx threads From a4a3fcd79560c10d6517e3106ae4ecad6ede3192 Mon Sep 17 00:00:00 2001 From: Tyler Flar Date: Tue, 24 Dec 2024 14:35:30 -0800 Subject: [PATCH 7/7] style: reorganize imports and update __all__ in the drone comms package --- .../__init__.py | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/radio_telemetry_tracker_drone_comms_package/__init__.py b/radio_telemetry_tracker_drone_comms_package/__init__.py index 0cd7da1..75fa731 100644 --- a/radio_telemetry_tracker_drone_comms_package/__init__.py +++ b/radio_telemetry_tracker_drone_comms_package/__init__.py @@ -5,15 +5,10 @@ __version__ = "0.1.0" -from radio_telemetry_tracker_drone_comms_package.drone_comms import ( - DroneComms, - RadioConfig, -) from radio_telemetry_tracker_drone_comms_package.data_models import ( - SyncRequestData, - SyncResponseData, ConfigRequestData, ConfigResponseData, + ErrorData, GPSData, LocEstData, PingData, @@ -21,22 +16,27 @@ StartResponseData, StopRequestData, StopResponseData, - ErrorData, + SyncRequestData, + SyncResponseData, +) +from radio_telemetry_tracker_drone_comms_package.drone_comms import ( + DroneComms, + RadioConfig, ) __all__ = [ - "DroneComms", - "RadioConfig", - "SyncRequestData", - "SyncResponseData", "ConfigRequestData", "ConfigResponseData", + "DroneComms", + "ErrorData", "GPSData", "LocEstData", "PingData", + "RadioConfig", "StartRequestData", "StartResponseData", "StopRequestData", "StopResponseData", - "ErrorData", + "SyncRequestData", + "SyncResponseData", ]