From 5efb025df7f8a97ddebcc4a7b1e9d1950d85b0bb Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Wed, 18 Dec 2024 20:56:50 -0800
Subject: [PATCH] created dataclasses for new packets

---
 rb_ws/src/buggy/buggy/serial/host_comm.py     | 132 ++++++++++++++----
 .../src/buggy/buggy/serial/ros_to_bnyahaj.py  |   7 -
 2 files changed, 105 insertions(+), 34 deletions(-)

diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py
index e98c23e..76f4859 100644
--- a/rb_ws/src/buggy/buggy/serial/host_comm.py
+++ b/rb_ws/src/buggy/buggy/serial/host_comm.py
@@ -58,28 +58,88 @@ def update(self, b):
 
 MAX_PAYLOAD_LEN = 100
 
-MSG_TYPE_DEBUG    = b'DB'
-MSG_TYPE_ODOMETRY = b'OD'
+
+# firmware --> software
+MSG_TYPE_NAND_DEBUG = b'ND'
+MSG_TYPE_NAND_UKF = b'NU'
+MSG_TYPE_NAND_GPS = b'NG'
+MSG_TYPE_RADIO = b'RA'
+MSG_TYPE_SC_DEBUG = b'SD'
+MSG_TYPE_SC_SENSORS = b'SS'
+MSG_TYPE_ROUNDTRIP_TIMESTAMP = b'RT'
+
+# software --> firmware
 MSG_TYPE_STEERING = b'ST'
-MSG_TYPE_BRAKE    = b'BR'
 MSG_TYPE_ALARM    = b'AL'
-MSG_TYPE_BNYATEL  = b'BT'
+MSG_TYPE_SOFTWARE_TIMESTAMP = b'TM'
+
+
+
+@dataclass
+class NANDDebug:
+    timestamp: int
+    rc_steering_angle: float
+    stepper_steering_angle: float
+    accelerometer: float
+    encoder_angle: float
+    operator_ready: bool
+    brake_status: bool
+    auton_steer: bool
+    rfm69_timeout: bool
+    tx12_state: bool
+    stepper_alarm: bool
+    rc_uplink_quality: int
+
+@dataclass
+class NANDUKF:
+    timestamp: int
+    easting: float
+    northing: float
+    theta: float
+    omega: float
+    velocity: float
 
 @dataclass
-class Odometry:
-    x: float
-    y: float
-    radio_seqnum: int
+class NANDGPS:
     gps_seqnum: int
+    easting: float
+    northing: float
+    gps_fix: int
+    # this is a 2D accuracy value
+    accuracy: int
+    timestamp: int
+
+@dataclass
+class Radio:
+    gps_seqnum: int
+    nand_x_gps: float
+    nand_y_gps: float
+    nand_gps_fix: float
+
+@dataclass
+class SCDebug:
+    timestamp: int
+    rc_steering_angle: float
+    stepper_steering_angle: float
+    # encoder_angle: float
+    operator_ready: bool
+    brake_status: bool
+    auton_steer: bool
+    tx12_state: bool
+    stepper_alarm: bool
+    rc_uplink_quality: int
+
+
+@dataclass
+class SCSensors:
+    steering: float
+    # velocity: float
+    timestamp: int
 
 @dataclass
-class BnyaTelemetry:
-    x: float
-    y: float
-    velocity: float # In METERS / SECOND
-    steering: float # In DEGREES
-    heading: float  # In RADIANS
-    heading_rate: float  # In RADIANS / SECOND
+class RoundtripTimestamp:
+    returned_time: int
+
 
 class IncompletePacket(Exception):
     pass
@@ -115,9 +175,11 @@ def send_steering(self, angle: float):
     def send_alarm(self, status: int):
         self.send_packet_raw(MSG_TYPE_ALARM, struct.pack('<B', status))
 
+    def send_timestamp(self, time: float):
+        self.send_packet_raw(MSG_TYPE_SOFTWARE_TIMESTAMP, struct.pack('<d', time))
+
     def read_packet_raw(self):
         self.rx_buffer += self.port.read_all() #type:ignore
-
         try:
             return self._try_parse_buffer()
         except IncompletePacket:
@@ -184,21 +246,37 @@ def read_packet(self):
             return None
 
         msg_type, payload = packet
-        if msg_type == MSG_TYPE_ODOMETRY:
-            # Odometry message
+        if msg_type == MSG_TYPE_NAND_DEBUG:
+            x, y, radio_seqnum, gps_seqnum = struct.unpack('<Idddd??????B', payload)
+            return NANDDebug(x, y, radio_seqnum, gps_seqnum)
+
+        elif msg_type == MSG_TYPE_NAND_UKF:
             x, y, radio_seqnum, gps_seqnum = struct.unpack('<ddII', payload)
-            return Odometry(x, y, radio_seqnum, gps_seqnum)
-        elif msg_type == MSG_TYPE_DEBUG:
-            # Debug message
-            debug = struct.unpack('<fff??B?BBxx', payload)
-            # print(debug)
-            return debug
-        elif msg_type == MSG_TYPE_BNYATEL:
+            return NANDUKF(x, y, radio_seqnum, gps_seqnum)
+
+        elif msg_type == MSG_TYPE_NAND_GPS:
+            x, y, vel, steering, heading, heading_rate = struct.unpack('<dddddd', payload)
+            return NANDGPS(x, y, vel, steering, heading, heading_rate)
+
+        elif msg_type == MSG_TYPE_RADIO:
             x, y, vel, steering, heading, heading_rate = struct.unpack('<dddddd', payload)
-            return BnyaTelemetry(x, y, vel, steering, heading, heading_rate)
+            return Radio(x, y, vel, steering, heading, heading_rate)
+
+        elif msg_type == MSG_TYPE_SC_DEBUG:
+            x, y, vel, steering, heading, heading_rate = struct.unpack('<dddddd', payload)
+            return SCDebug(x, y, vel, steering, heading, heading_rate)
+
+        elif msg_type == MSG_TYPE_SC_SENSORS:
+            x, y, vel, steering, heading, heading_rate = struct.unpack('<dddddd', payload)
+            return SCSensors(x, y, vel, steering, heading, heading_rate)
+
+        elif msg_type == MSG_TYPE_ROUNDTRIP_TIMESTAMP:
+            x, y, vel, steering, heading, heading_rate = struct.unpack('<dddddd', payload)
+            return RoundtripTimestamp(x, y, vel, steering, heading, heading_rate)
         else:
+            print(f'Unknown packet type {msg_type}')
             return None
-            # print(f'Unknown packet type {msg_type}')
+
 
 
 def main():
diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
index 17f5ae7..4335be4 100644
--- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
+++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py
@@ -18,13 +18,6 @@ class Translator(Node):
     Translates the output from bnyahaj serial (interpreted from host_comm) to ros topics and vice versa.
     Performs reading (from Bnya Serial) and writing (from Ros Topics) on different python threads, so
     be careful of multithreading synchronizaiton issues.
-    SC:
-    (ROS) Self Steering topic --> (Bnyahaj) Stepper Motor
-    (Bnyahaj) NAND Odom --> (ROS) NAND Odom topic
-
-    NAND:
-    (ROS) Self Steering --> (Bnyahaj) Stepper Motor
-    (Bnyahaj) Self Odom from UKF --> (ROS) NAND Odom topic
     """
 
     def __init__(self, self_name, other_name, teensy_name):