State Estimations & Telemetry Updates

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(

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"<II"
 
-    uid: bytes
     mid: int
     timestamp: int
 
@@ -60,38 +60,29 @@ def parse(cls, data: bytes) -> 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] = "<BBI16s"
+    STRUCT: ClassVar[str] = f"<{UID_SIZE}sBBI16s"
 
+    uid: bytes
     hw_opcode: int
     fw_opcode: int
     version: int
@@ -127,9 +119,9 @@ def parse(cls, data: bytes) -> 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 ):

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] = []

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):
         """