diff --git a/Parser/create_configs.py b/Parser/create_configs.py index fbeb091..da0bb23 100644 --- a/Parser/create_configs.py +++ b/Parser/create_configs.py @@ -84,22 +84,18 @@ def appa_data_bitmask() -> DataBitmask: name="state_estim", value=Toggle.ENABLED, sensors=[ - BaseSensor("rollDeg", "Roll Body Angle", 4, float, "deg"), - BaseSensor("pitchDeg", "Pitch Body Angle", 4, float, "deg"), - BaseSensor("yawDeg", "Yaw Body Angle", 4, float, "deg"), - BaseSensor("rollRate", "Roll Body Rate", 4, float, "deg/s"), - BaseSensor("pitchRate", "Pitch Body Rate", 4, float, "deg/s"), - BaseSensor("yawRate", "Yaw Body Rate", 4, float, "deg/s"), + BaseSensor("quat_w", "Unit Quaternion W", 4, float, ""), + BaseSensor("quat_x", "Unit Quaternion X", 4, float, ""), + BaseSensor("quat_y", "Unit Quaternion Y", 4, float, ""), + BaseSensor("quat_z", "Unit Quaternion Z", 4, float, ""), + BaseSensor("roll_rate", "Roll Body Rate", 4, float, "deg/s"), BaseSensor("velo", "Velocity", 4, float, "m/s"), BaseSensor("velo_x", "Velo X", 4, float, "m/s"), BaseSensor("velo_y", "Velo Y", 4, float, "m/s"), BaseSensor("velo_z", "Velo Z", 4, float, "m/s"), - - BaseSensor("pos", "Position", 4, float, "m"), BaseSensor("alt", "Barometric Altitude", 4, float, "m"), - BaseSensor("bvelo", "Barometric Velocity", 4, float, "m/s") ] ), Data( diff --git a/Parser/telemetry.py b/Parser/telemetry.py index b55d9ae..f6b910f 100644 --- a/Parser/telemetry.py +++ b/Parser/telemetry.py @@ -12,6 +12,7 @@ import enum import struct import math +import time from dataclasses import dataclass, asdict from typing import ClassVar from SDECv2.SerialController import SerialObj @@ -21,11 +22,11 @@ from typing import Any UID_SIZE = 12 -LORA_INTERNAL_HEADER_SIZE = 20 -LORA_PAYLOAD_SIZE = 76 +LORA_INTERNAL_HEADER_SIZE = 8 +LORA_PAYLOAD_SIZE = 40 LORA_MESSAGE_SIZE = LORA_INTERNAL_HEADER_SIZE + LORA_PAYLOAD_SIZE FLIGHT_ID_SIZE = 16 -DASHBOARD_DUMP_TYPE_SIZE = 72 +DASHBOARD_DUMP_TYPE_SIZE = 36 class LoRaMessageTypes(enum.IntEnum): """LORA_MESSAGE_TYPES (uint32 on wire).""" @@ -38,11 +39,10 @@ class LoRaMessageTypes(enum.IntEnum): @dataclass(frozen=True) class LoRaInternalHeaderType: - """LORA_INTERNAL_HEADER_TYPE — uid, mid, timestamp (packed, 20 bytes).""" + """LORA_INTERNAL_HEADER_TYPE — mid, timestamp (packed, 8 bytes).""" - STRUCT: ClassVar[str] = f"<{UID_SIZE}sII" + STRUCT: ClassVar[str] = f" LoRaInternalHeaderType: f"LoRaInternalHeaderType.parse expects {LORA_INTERNAL_HEADER_SIZE} bytes, " f"got {len(data)}" ) - uid, mid_u32, ts = struct.unpack(cls.STRUCT, data[:LORA_INTERNAL_HEADER_SIZE]) - return cls(bytes(uid), mid_u32, ts) + mid_u32, ts = struct.unpack(cls.STRUCT, data[:LORA_INTERNAL_HEADER_SIZE]) + return cls(mid_u32, ts) @dataclass(frozen=True) class DashboardDumpType: - """DASHBOARD_DUMP_TYPE — 18 × float32, 72 bytes.""" - - STRUCT: ClassVar[str] = "<18f" - - accXconv: float - accYconv: float - accZconv: float - gyroXconv: float - gyroYconv: float - gyroZconv: float - rollDeg: float - pitchDeg: float - yawDeg: float - rollRate: float - pitchRate: float - yawRate: float - pres: float - temp: float + """DASHBOARD_DUMP_TYPE — 9 × float32, 36 bytes.""" + + STRUCT: ClassVar[str] = "<9f" + + quat_w: float + quat_x: float + quat_y: float + quat_z: float alt: float - bvelo: float long: float lat: float + acc_z: float + roll_rate: float @classmethod def parse(cls, data: bytes) -> DashboardDumpType: - if len(data) < DASHBOARD_DUMP_TYPE_SIZE: + if len(data) != DASHBOARD_DUMP_TYPE_SIZE: raise ParserError( f"DashboardDumpType.parse expects {DASHBOARD_DUMP_TYPE_SIZE} bytes, got {len(data)}" ) @@ -114,8 +105,9 @@ def to_json(self) -> dict[str, Any]: class LoRaMsgVehicleIdType: """LORA_MSG_VEHICLE_ID_TYPE — trailing explicit padding on the wire is discarded.""" - STRUCT: ClassVar[str] = " LoRaMsgVehicleIdType: raise ParserError( f"LoRaMsgVehicleIdType.parse expects {LORA_PAYLOAD_SIZE} bytes, got {len(data)}" ) - hw, fw, ver, fid = struct.unpack(cls.STRUCT, data[: 6 + FLIGHT_ID_SIZE]) + uid, hw, fw, ver, fid = struct.unpack(cls.STRUCT, data[: 18 + FLIGHT_ID_SIZE]) flight_id = fid.split(b"\x00", 1)[0].decode("utf-8", errors="replace") - return cls(hw, fw, ver, flight_id) + return cls(uid, hw, fw, ver, flight_id) @dataclass(frozen=True) @@ -152,7 +144,7 @@ def parse(cls, data: bytes) -> LoRaMsgDashboardDumpType: @dataclass(frozen=True) class LoRaMsgTextMessageType: - """LORA_MSG_TEXT_MESSAGE_TYPE — TEXT_MESSAGE occupies the full 76-byte slot.""" + """LORA_MSG_TEXT_MESSAGE_TYPE — TEXT_MESSAGE occupies the full 36-byte slot.""" msg: str @@ -239,6 +231,22 @@ def __init__(self): "status": "Connecting..." } self.last_msg_time = None + self.msg_rx_callback = None + + def __put_to_rx_callback(self, msgType: str, timestamp: float, msg: dict[str, Any]): + if self.msg_rx_callback is None: + return + self.msg_rx_callback(msgType, timestamp, msg) + + def register_rx_callback(self, msg_rx_callback: callable[str, float, dict[str, Any]]): + """ + Register the receive callback for this telemetry object. + The callback takes the following parameters and returns nothing: + - messageType (as a string) + - timestamp (as a float, timebase is seconds) + - message contents (json/dict) + """ + self.msg_rx_callback = msg_rx_callback def dashboard_dump(self, serial_connection: SerialObj): """ @@ -250,12 +258,15 @@ def dashboard_dump(self, serial_connection: SerialObj): serial_connection.send(b'\x30') if( serial_connection.target == None ): print("Warning: The serial connection does not have an associated hardware/firmware platform. Report this.") + return elif( serial_connection.target.controller.id == b'\x05'): # Flight Computer: Parse dashboard dump data = serial_connection.read(DASHBOARD_DUMP_TYPE_SIZE) parsed = DashboardDumpType.parse(data) with( self.telem_lock ): self.last_dashboard_dump = parsed + self.__put_to_rx_callback(LoRaMessageTypes.DASHBOARD_DATA.name, time.time(), parsed.to_json()) + return elif( serial_connection.target.controller.id == b'\x10'): # Ground station: Interpret message & save data = serial_connection.read(LORA_MESSAGE_SIZE) @@ -273,6 +284,7 @@ def dashboard_dump(self, serial_connection: SerialObj): # Update message if( parsed.header.mid_enum == LoRaMessageTypes.DASHBOARD_DATA ): self.last_dashboard_dump = parsed.dashboard_dump.data + self.__put_to_rx_callback(parsed.header.mid_enum.name, parsed.header.timestamp / 1000.0, self.last_dashboard_dump.to_json()) elif( parsed.header.mid_enum == LoRaMessageTypes.VEHICLE_ID ): hw = parsed.vehicle_id.hw_opcode.to_bytes(1) fw = parsed.vehicle_id.fw_opcode.to_bytes(1) @@ -284,10 +296,11 @@ def dashboard_dump(self, serial_connection: SerialObj): "sig_strength": 0, "status": "OK" } + self.__put_to_rx_callback(parsed.header.mid_enum.name, parsed.header.timestamp / 1000.0, self.last_wireless_stats) except (ValueError, ParserError, TypeError, AttributeError) as e: print("Malformed vehicle ID message received. Discarding.") print(f"HW: {hw}; FW: {fw};") - + return def get_latest_dashboard_dump(self) -> dict[str, Any] | None: with( self.telem_lock ): diff --git a/Sensor/create_sensors.py b/Sensor/create_sensors.py index a6da845..07043d0 100644 --- a/Sensor/create_sensors.py +++ b/Sensor/create_sensors.py @@ -23,74 +23,31 @@ def flight_computer_rev2_sensors() -> List[Sensor]: (b"\x07", "magYconv", "Pre-converted Mag Y", 4, float, "µT", None), (b"\x08", "magZconv", "Pre-converted Mag Z", 4, float, "µT", None), - (b"\x09", "rollDeg", "Roll Body Angle", 4, float, "deg", None), - (b"\x0A", "pitchDeg", "Pitch Body Angle", 4, float, "deg", None), - (b"\x0B", "yawDeg", "Yaw Body Angle", 4, float, "deg", None), - (b"\x0C", "rollRate", "Roll Body Rate", 4, float, "deg/s", None), - (b"\x0D", "pitchRate", "Pitch Body Rate", 4, float, "deg/s", None), - (b"\x0E", "yawRate", "Yaw Body Rate", 4, float, "deg/s", None), - - (b"\x0F", "velo", "Velocity", 4, float, "m/s", None), - (b"\x10", "velo_x", "Velo X", 4, float, "m/s", None), - (b"\x11", "velo_y", "Velo Y", 4, float, "m/s", None), - (b"\x12", "velo_z", "Velo Z", 4, float, "m/s", None), - - (b"\x13", "pos", "Position", 4, float, "m", None), - (b"\x14", "pres", "Barometric Pressure", 4, float, "Pa", None), - (b"\x15", "temp", "Barometric Temperature", 4, float, "C", None), - (b"\x16", "alt", "Barometric Altitude", 4, float, "m", None), - (b"\x17", "bvelo", "Barometric Velocity", 4, float, "m/s", None), - - (b"\x18", "altg", "GPS Altitude (ft)", 4, float, "ft", None), - (b"\x19", "speedg", "GPS Speed (KmH)", 4, float, "km/h", None), - (b"\x1A", "utc_time", "GPS UTC Time", 4, float, "s", None), - (b"\x1B", "long", "GPS Longitude (deg)", 4, float, "deg", None), - (b"\x1C", "lat", "GPS Latitude (deg)", 4, float, "deg", None), - - (b"\x1D", "ns", "GPS North/South", 1, str, "N/S", None), - (b"\x1E", "ew", "GPS East/West", 1, str, "E/W", None), - (b"\x1F", "gll_s", "GPS GLL Status", 1, str, "", None), - (b"\x20", "rmc_s", "GPS RMC Status", 1, str, "", None), - ] - - sensors: List[Sensor] = [] - offset = 0 - - for poll_code, short_name, name, size, data_type, unit, conv_func in sensor_tuples: - sensors.append(Sensor(short_name, name, size, data_type, unit, conv_func, poll_code, offset)) - offset += size - - return sensors - -def rev2_dashboard_dump_sensors() -> List[Sensor]: - """ - Create and return a list of dashbaord specific sensors for the Flight Computer Rev 2.0. - - Returns: - List[Sensor]: List of configured dashboard specific sensors for the Flight Computer Rev 2.0. - """ - sensor_tuples = [ - (b"\x00", "accXconv", "Pre-converted Accel X", 4, float, "m/s/s", None), - (b"\x01", "accYconv", "Pre-converted Accel Y", 4, float, "m/s/s", None), - (b"\x02", "accZconv", "Pre-converted Accel Z", 4, float, "m/s/s", None), - (b"\x03", "gyroXconv", "Pre-converted Gyro X", 4, float, "deg/s", None), - (b"\x04", "gyroYconv", "Pre-converted Gyro Y", 4, float, "deg/s", None), - (b"\x05", "gyroZconv", "Pre-converted Gyro Z", 4, float, "deg/s", None), - - (b"\x09", "rollDeg", "Roll Body Angle", 4, float, "deg", None), - (b"\x0A", "pitchDeg", "Pitch Body Angle", 4, float, "deg", None), - (b"\x0B", "yawDeg", "Yaw Body Angle", 4, float, "deg", None), - (b"\x0C", "rollRate", "Roll Body Rate", 4, float, "deg/s", None), - (b"\x0D", "pitchRate", "Pitch Body Rate", 4, float, "deg/s", None), - (b"\x0E", "yawRate", "Yaw Body Rate", 4, float, "deg/s", None), - - (b"\x14", "pres", "Barometric Pressure", 4, float, "kPa", baro_press), - (b"\x15", "temp", "Barometric Temperature", 4, float, "C", None), - (b"\x16", "alt", "Barometric Altitude", 4, float, "m", None), - (b"\x17", "bvelo", "Barometric Velocity", 4, float, "m/s", None), - - (b"\x1B", "long", "GPS Longitude (deg)", 4, float, "deg", None), - (b"\x1C", "lat", "GPS Latitude (deg)", 4, float, "deg", None), + (b"\x09", "quat_w", "Unit Quaternion W", 4, float, "", None), + (b"\x0A", "quat_x", "Unit Quaternion X", 4, float, "", None), + (b"\x0B", "quat_y", "Unit Quaternion Y", 4, float, "", None), + (b"\x0C", "quat_z", "Unit Quaternion Z", 4, float, "", None), + (b"\x0D", "roll_rate", "Roll Rate", 4, float, "deg/s", None), + + (b"\x0E", "velo", "Velocity", 4, float, "m/s", None), + (b"\x0F", "velo_x", "Velo X", 4, float, "m/s", None), + (b"\x10", "velo_y", "Velo Y", 4, float, "m/s", None), + (b"\x11", "velo_z", "Velo Z", 4, float, "m/s", None), + + (b"\x12", "pres", "Barometric Pressure", 4, float, "Pa", None), + (b"\x13", "temp", "Barometric Temperature", 4, float, "C", None), + (b"\x14", "alt", "Barometric Altitude", 4, float, "m", None), + + (b"\x15", "altg", "GPS Altitude (ft)", 4, float, "ft", None), + (b"\x16", "speedg", "GPS Speed (KmH)", 4, float, "km/h", None), + (b"\x17", "utc_time", "GPS UTC Time", 4, float, "s", None), + (b"\x18", "long", "GPS Longitude (deg)", 4, float, "deg", None), + (b"\x19", "lat", "GPS Latitude (deg)", 4, float, "deg", None), + + (b"\x20", "ns", "GPS North/South", 1, str, "N/S", None), + (b"\x21", "ew", "GPS East/West", 1, str, "E/W", None), + (b"\x22", "gll_s", "GPS GLL Status", 1, str, "", None), + (b"\x23", "rmc_s", "GPS RMC Status", 1, str, "", None), ] sensors: List[Sensor] = [] diff --git a/Sensor/sensor_sentry.py b/Sensor/sensor_sentry.py index 7552409..648e4e2 100644 --- a/Sensor/sensor_sentry.py +++ b/Sensor/sensor_sentry.py @@ -7,7 +7,6 @@ from typing import List, Callable, Dict, Optional, Generator -from .create_sensors import rev2_dashboard_dump_sensors from .sensor import Sensor from .util import bytes_to_float, bytes_to_int, process_data_bytes from SDECv2.SerialController import SerialObj @@ -179,43 +178,6 @@ def dump(self, serial_connection: SerialObj) -> Dict[Sensor, float | int | None] sensor_dump[sensor] = converted_number return sensor_dump - - @classmethod - def dashboard_dump(cls, serial_connection: SerialObj) -> Dict[Sensor, float | int | None]: - """ - Dashboard dump opcode. - - Args: - serial_connection (SerialObj): Serial connection to the Flight Computer. - - Returns: - Dict[Sensor, float | int | None]: Sensor data. - """ - # Dashboard dump opcode - serial_connection.send(b"\x30") - - # Avoid creating a serial_sentry to reduce overhead - sensors = rev2_dashboard_dump_sensors() - sensors_size = sum(sensor.size for sensor in sensors) - - data_bytes = serial_connection.read(sensors_size) - - sensor_dump = {} - for sensor in sensors: - # Read and convert raw bytes to the readout - sensor_data_bytes = data_bytes[sensor.offset:sensor.offset+sensor.size] - converted_number = process_data_bytes(sensor_data_bytes, sensor.data_type, sensor.convert_data) - - # Convert Python inf and NaN into JSON readable values - if converted_number is not None: - if math.isinf(converted_number): - converted_number = 999999 - elif math.isnan(converted_number): - converted_number = None - - sensor_dump[sensor] = converted_number - - return sensor_dump def pretty_print(self, indent=0): """