From 95d79d1b9bc79f5791ca2cb44b41e08f84b61158 Mon Sep 17 00:00:00 2001
From: cameron brown <52760912+cbrxyz@users.noreply.github.com>
Date: Tue, 26 Mar 2024 23:13:49 +0000
Subject: [PATCH] More reliability for `mil_usb_to_can` (#1168)

* Add checksum tests, fix sync chars, add tests for actuator packets

* More tests, thrust + kill board

* Fixing sub9_thrust_and_kill usb_to_can tests

* Fixing checksum test, adding ThrusterId to docs

* Enforce little endian over native byte order
---
 .../sub9_thrust_and_kill_board/__init__.py    |  1 +
 .../sub9_thrust_and_kill_board/packets.py     | 53 ++++++++++---------
 .../test/simulated_board_test.py              | 36 +++++++++++++
 .../sub_actuator_board/packets.py             | 27 ++++++++--
 .../test/simulated_board_test.py              | 23 ++++++++
 docs/subjugator/reference.rst                 |  7 +++
 .../mil_usb_to_can/sub9/packet.py             | 12 ++---
 .../mil_usb_to_can/test/test_packets_sub9.py  | 15 +++++-
 8 files changed, 139 insertions(+), 35 deletions(-)

diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py
index 7a9f7f666..387a49295 100644
--- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py
+++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py
@@ -5,6 +5,7 @@
     KillReceivePacket,
     KillSetPacket,
     KillStatus,
+    ThrusterId,
     ThrustSetPacket,
 )
 from .simulation import ThrusterAndKillBoardSimulation
diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py
index 93d892fd3..82ed2d671 100644
--- a/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py
+++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/packets.py
@@ -18,38 +18,41 @@ class HeartbeatReceivePacket(Packet, msg_id=0x02, subclass_id=0x01, payload_form
     """
 
 
+class ThrusterId(IntEnum):
+    """
+    Enum to represent the thruster ID.
+    """
+
+    #: Front left horizontal thruster.
+    FLH = 0
+    #: Front right horizontal thruster.
+    FRH = 1
+    #: Front left vertical thruster.
+    FLV = 2
+    #: Front right vertical thruster.
+    FRV = 3
+    #: Back left horizontal thruster.
+    BLH = 4
+    #: Back right horizontal thruster.
+    BRH = 5
+    #: Back left vertical thruster.
+    BLV = 6
+    #: Back right vertical thruster.
+    BRV = 7
+
+
 @dataclass
-class ThrustSetPacket(Packet, msg_id=0x02, subclass_id=0x02, payload_format="=Bf"):
+class ThrustSetPacket(Packet, msg_id=0x02, subclass_id=0x02, payload_format="<Bf"):
     """
     Packet to set the speed of a specific thruster.
 
     Attributes:
-        thruster_id (int): The ID of the thruster to set the speed of. The ID of
+        thruster_id (ThrusterId): The ID of the thruster to set the speed of. The ID of
             the thruster corresponds to a specific thruster:
-
-            +--------+------+
-            |  name  |  id  |
-            +========+======+
-            |  FLH   |  0   |
-            +--------+------+
-            |  FRH   |  1   |
-            +--------+------+
-            |  FLV   |  2   |
-            +--------+------+
-            |  FRV   |  3   |
-            +--------+------+
-            |  BLH   |  4   |
-            +--------+------+
-            |  BRH   |  5   |
-            +--------+------+
-            |  BLV   |  6   |
-            +--------+------+
-            |  BRV   |  7   |
-            +--------+------+
         speed (float): The speed to set the thruster to.
     """
 
-    thruster_id: int
+    thruster_id: ThrusterId
     speed: float
 
 
@@ -71,7 +74,7 @@ class KillStatus(IntEnum):
 
 
 @dataclass
-class KillSetPacket(Packet, msg_id=0x02, subclass_id=0x03, payload_format="=BB"):
+class KillSetPacket(Packet, msg_id=0x02, subclass_id=0x03, payload_format="<BB"):
     """
     Packet sent by the motherboard to set/unset the kill.
 
@@ -85,7 +88,7 @@ class KillSetPacket(Packet, msg_id=0x02, subclass_id=0x03, payload_format="=BB")
 
 
 @dataclass
-class KillReceivePacket(Packet, msg_id=0x02, subclass_id=0x04, payload_format="=BB"):
+class KillReceivePacket(Packet, msg_id=0x02, subclass_id=0x04, payload_format="<BB"):
     """
     Packet sent by the motherboard to set/unset the kill.
 
diff --git a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py
index 4af9813d7..439699b9b 100755
--- a/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py
+++ b/SubjuGator/drivers/sub9_thrust_and_kill_board/test/simulated_board_test.py
@@ -4,6 +4,15 @@
 import rospy
 from ros_alarms import AlarmListener
 from std_srvs.srv import SetBool, SetBoolRequest
+from sub9_thrust_and_kill_board.packets import (
+    HeartbeatReceivePacket,
+    HeartbeatSetPacket,
+    KillReceivePacket,
+    KillSetPacket,
+    KillStatus,
+    ThrusterId,
+    ThrustSetPacket,
+)
 
 
 class SimulatedBoardTest(unittest.TestCase):
@@ -28,6 +37,33 @@ def test_correct_response(self):
         self.assertTrue(self.kill_srv(SetBoolRequest(False)).success)
         self.assertTrue(self.hw_alarm_listener.is_raised(False))
 
+    def test_packet(self):
+        # ThrustSetPacket
+        thrust_set_packet = ThrustSetPacket(ThrusterId.FLH, 0.5)
+        self.assertEqual(thrust_set_packet.thruster_id, ThrusterId.FLH)
+        self.assertEqual(
+            bytes(thrust_set_packet),
+            b"7\x01\x02\x02\x05\x00\x00\x00\x00\x00?H\x84",
+        )
+        # HeartbeatSetPacket
+        heartbeat_set_packet = HeartbeatSetPacket()
+        self.assertEqual(bytes(heartbeat_set_packet), b"7\x01\x02\x00\x00\x00\x02\x08")
+        # HeartbeatReceivePacket
+        heartbeat_receive_packet = HeartbeatReceivePacket()
+        self.assertEqual(
+            bytes(heartbeat_receive_packet),
+            b"7\x01\x02\x01\x00\x00\x03\x0b",
+        )
+        # KillSetPacket
+        kill_set_packet = KillSetPacket(True, KillStatus.BATTERY_LOW)
+        self.assertEqual(bytes(kill_set_packet), b"7\x01\x02\x03\x02\x00\x01\x04\x0c)")
+        # KillReceivePacket
+        kill_receive_packet = KillReceivePacket(True, KillStatus.BATTERY_LOW)
+        self.assertEqual(
+            bytes(kill_receive_packet),
+            b"7\x01\x02\x04\x02\x00\x01\x04\r.",
+        )
+
 
 if __name__ == "__main__":
     rospy.init_node("simulated_board_test", anonymous=True)
diff --git a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py
index 2d7f50314..8d387357b 100644
--- a/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py
+++ b/SubjuGator/drivers/sub_actuator_board/sub_actuator_board/packets.py
@@ -9,8 +9,8 @@ class ActuatorPacketId(IntEnum):
     Enumerator representing each controllable actuator.
     """
 
-    #: The dropper actuator.
-    DROPPER = 0
+    #: The gripper actuator.
+    GRIPPER = 0
     #: The torpedo launcher actuator.
     TORPEDO_LAUNCHER = 1
     #: The ball drop actuator. Only one actuator is used for both balls.
@@ -18,7 +18,7 @@ class ActuatorPacketId(IntEnum):
 
 
 @dataclass
-class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="BB"):
+class ActuatorSetPacket(Packet, msg_id=0x03, subclass_id=0x00, payload_format="<BB"):
     """
     Packet used by the actuator board to set a specific valve.
 
@@ -60,3 +60,24 @@ class ActuatorPollResponsePacket(
     """
 
     values: int
+
+    @property
+    def gripper_opened(self) -> bool:
+        """
+        Whether the gripper is opened.
+        """
+        return bool(self.values & (0b0001 << ActuatorPacketId.GRIPPER))
+
+    @property
+    def torpedo_launcher_opened(self) -> bool:
+        """
+        Whether the torpedo launcher is opened.
+        """
+        return bool(self.values & (0b0001 << ActuatorPacketId.TORPEDO_LAUNCHER))
+
+    @property
+    def ball_drop_opened(self) -> bool:
+        """
+        Whether the ball drop is opened.
+        """
+        return bool(self.values & (0b0001 << ActuatorPacketId.BALL_DROP))
diff --git a/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py b/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py
index 014bd090c..0e00479fb 100755
--- a/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py
+++ b/SubjuGator/drivers/sub_actuator_board/test/simulated_board_test.py
@@ -2,6 +2,12 @@
 import unittest
 
 import rospy
+from sub_actuator_board.packets import (
+    ActuatorPacketId,
+    ActuatorPollRequestPacket,
+    ActuatorPollResponsePacket,
+    ActuatorSetPacket,
+)
 from sub_actuator_board.srv import GetValve, GetValveRequest, SetValve, SetValveRequest
 
 
@@ -35,6 +41,23 @@ def test_correct_response(self):
         self.assertFalse(self.get_srv(GetValveRequest(2)).opened)
         self.assertFalse(self.get_srv(GetValveRequest(0)).opened)
 
+    def test_packet(self):
+        """
+        Test that the bytes representation of all packets is correct.
+        """
+        # ActuatorPollRequestPacket
+        packet = ActuatorPollRequestPacket()
+        self.assertEqual(bytes(packet), b"7\x01\x03\x01\x00\x00\x04\x0f")
+        # ActuatorPollResponsePacket
+        packet = ActuatorPollResponsePacket(0b00111)
+        self.assertEqual(packet.ball_drop_opened, True)
+        self.assertEqual(packet.gripper_opened, True)
+        self.assertEqual(packet.torpedo_launcher_opened, True)
+        self.assertEqual(bytes(packet), b"7\x01\x03\x02\x01\x00\x07\r!")
+        # ActuatorSetPacket
+        packet = ActuatorSetPacket(ActuatorPacketId.TORPEDO_LAUNCHER, True)
+        self.assertEqual(bytes(packet), b"7\x01\x03\x00\x02\x00\x01\x01\x07\x1d")
+
 
 if __name__ == "__main__":
     rospy.init_node("simulated_board_test", anonymous=True)
diff --git a/docs/subjugator/reference.rst b/docs/subjugator/reference.rst
index 6d0134f4f..4b5433071 100644
--- a/docs/subjugator/reference.rst
+++ b/docs/subjugator/reference.rst
@@ -188,6 +188,13 @@ HeartbeatReceivePacket
 .. autoclass:: sub9_thrust_and_kill_board.HeartbeatReceivePacket
     :members:
 
+ThrusterId
+^^^^^^^^^^
+.. attributetable:: sub9_thrust_and_kill_board.ThrusterId
+
+.. autoclass:: sub9_thrust_and_kill_board.ThrusterId
+    :members:
+
 ThrustSetPacket
 ^^^^^^^^^^^^^^^
 .. attributetable:: sub9_thrust_and_kill_board.ThrustSetPacket
diff --git a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py
index 06f625b9b..8b123bfc9 100644
--- a/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py
+++ b/mil_common/drivers/mil_usb_to_can/mil_usb_to_can/sub9/packet.py
@@ -6,8 +6,8 @@
 from functools import lru_cache
 from typing import ClassVar, get_type_hints
 
-SYNC_CHAR_1 = ord("3")
-SYNC_CHAR_2 = ord("7")
+SYNC_CHAR_1 = 0x37
+SYNC_CHAR_2 = 0x01
 
 _packet_registry: dict[int, dict[int, type[Packet]]] = {}
 
@@ -117,7 +117,7 @@ def _calculate_checksum(cls, data: bytes) -> tuple[int, int]:
     def __bytes__(self):
         payload = struct.pack(self.payload_format, *self.__dict__.values())
         data = struct.pack(
-            f"=BBBBH{len(payload)}s",
+            f"<BBBBH{len(payload)}s",
             SYNC_CHAR_1,
             SYNC_CHAR_2,
             self.msg_id,
@@ -126,7 +126,7 @@ def __bytes__(self):
             payload,
         )
         checksum = self._calculate_checksum(data[2:])
-        return data + struct.pack("=BB", *checksum)
+        return data + struct.pack("<BB", *checksum)
 
     @classmethod
     def from_bytes(cls, packed: bytes) -> Packet:
@@ -141,12 +141,12 @@ def from_bytes(cls, packed: bytes) -> Packet:
         if msg_id in _packet_registry and subclass_id in _packet_registry[msg_id]:
             subclass = _packet_registry[msg_id][subclass_id]
             payload = packed[6:-2]
-            if struct.unpack("=BB", packed[-2:]) != cls._calculate_checksum(
+            if struct.unpack("<BB", packed[-2:]) != cls._calculate_checksum(
                 packed[2:-2],
             ):
                 raise ChecksumException(
                     subclass,
-                    struct.unpack("=BB", packed[-2:]),
+                    struct.unpack("<BB", packed[-2:]),
                     cls._calculate_checksum(packed[2:-2]),
                 )
             unpacked = struct.unpack(subclass.payload_format, payload)
diff --git a/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py
index c58a8c32c..ba20cd34b 100755
--- a/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py
+++ b/mil_common/drivers/mil_usb_to_can/test/test_packets_sub9.py
@@ -1,4 +1,5 @@
 #! /usr/bin/env python3
+import struct
 import unittest
 from dataclasses import dataclass
 
@@ -16,7 +17,7 @@ class TestPacket(Packet, msg_id=0x47, subclass_id=0x44, payload_format="?Hd"):
 
 class BasicApplicationPacketTest(unittest.IsolatedAsyncioTestCase):
     """
-    Tests basic application packt functionality.
+    Tests basic application packet functionality.
     """
 
     def test_simple_packet(self):
@@ -52,6 +53,18 @@ def test_comparisons(self):
         with self.assertRaises(TypeError):
             packet > packet_two
 
+    def _pack_checksum(self, byte_string: bytes) -> int:
+        checksum = Packet._calculate_checksum(byte_string)
+        return int.from_bytes(struct.pack("<BB", *checksum), byteorder="big")
+
+    def test_checksum(self):
+        self.assertEqual(self._pack_checksum(b"abcde"), 0xF0C8)
+        self.assertEqual(self._pack_checksum(b"abcdefgh"), 0x2706)
+        self.assertEqual(
+            self._pack_checksum(b"abcdeabcdeabcdeabcdeabcde"),
+            0xB4FA,
+        )
+
 
 if __name__ == "__main__":
     packet = TestPacket(False, 42, 3.14)