fixed can connector

src/anchor_pkg/anchor_pkg/connector.py

@@ -21,6 +21,16 @@ KNOWN_USBS = [
 ]
 BAUD_RATE = 115200
 
+MCU_IDS = [
+            "broadcast",
+            "core",
+            "arm",
+            "digit",
+            "faerie",
+            "citadel",
+            "libs",
+        ]
+
 
 class NoValidDeviceException(Exception):
     pass
@@ -269,17 +279,9 @@ class CANConnector(Connector):
         data_type_key = (arbitration_id >> 6) & 0b11
         command = arbitration_id & 0x3F
 
-        key_to_mcu: dict[int, str] = {
+        try:
-            1: "broadcast",
+            mcu_name = MCU_IDS[mcu_key]
-            2: "core",
+        except IndexError:
-            3: "arm",
-            4: "digit",
-            5: "faerie",
-            6: "citadel",
-        }
-
-        mcu_name = key_to_mcu.get(mcu_key)
-        if mcu_name is None:
             self.logger.warn(
                 f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}"
             )
@@ -296,7 +298,7 @@ class CANConnector(Connector):
                         f"received double payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
                     return (None, None)
-                (value,) = struct.unpack("<d", data_bytes[:8])
+                (value,) = struct.unpack(">d", data_bytes[:8])
                 data = [float(value)]
             elif data_type_key == 1:
                 if len(data_bytes) < 8:
@@ -304,7 +306,7 @@ class CANConnector(Connector):
                         f"received float32x2 payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
                     return (None, None)
-                v1, v2 = struct.unpack("<ff", data_bytes[:8])
+                v1, v2 = struct.unpack(">ff", data_bytes[:8])
                 data = [float(v1), float(v2)]
             elif data_type_key == 2:
                 if len(data_bytes) < 8:
@@ -312,7 +314,7 @@ class CANConnector(Connector):
                         f"received int16x4 payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
                     return (None, None)
-                i1, i2, i3, i4 = struct.unpack("<hhhh", data_bytes[:8])
+                i1, i2, i3, i4 = struct.unpack(">hhhh", data_bytes[:8])
                 data = [float(i1), float(i2), float(i3), float(i4)]
             else:
                 self.logger.warn(
@@ -344,92 +346,53 @@ class CANConnector(Connector):
         if not self.can_bus:
             raise DeviceClosedException("CAN bus not initialized")
 
-        # Build 11-bit arbitration ID according to the VicCAN scheme:
+        # build 11-bit arbitration ID according to VicCAN spec:
         # bits 10..8: targeted MCU key
         # bits 7..6:  data type key
         # bits 5..0:  command
 
-        # Map MCU name to 3-bit key.
+        # map MCU name to 3-bit key.
-        mcu_name = (msg.mcu_name or "").lower()
+        try:
-        mcu_key_map: dict[str, int] = {
+            mcu_id = MCU_IDS.index((msg.mcu_name or "").lower())
-            "broadcast": 1,
+        except ValueError:
-            "core": 2,
-            "arm": 3,
-            "digit": 4,
-            "faerie": 5,
-            "citadel": 6,
-        }
-
-        if mcu_name not in mcu_key_map:
             self.logger.error(
                 f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message"
             )
             return
 
-        mcu_key = mcu_key_map[mcu_name] & 0b111
+        # determine data type from length:
-
-        # Infer data type key from payload length according to the table:
         # 0: double x1, 1: float32 x2, 2: int16 x4, 3: empty
-        data_len = len(msg.data)
+        match data_len := len(msg.data):
-        if data_len == 0:
+            case 0:
-            data_type_key = 3
+                data_type = 3
-        elif data_len == 1:
+                data = bytes()
-            data_type_key = 0
+            case 1:
-        elif data_len == 2:
+                data_type = 0
-            data_type_key = 1
+                data = struct.pack(">d", *msg.data)
-        elif data_len == 4:
+            case 2:
-            data_type_key = 2
+                data_type = 1
-        else:
+                data = struct.pack(">ff", *msg.data)
-            # Fallback: treat any other non-zero length as float32 x2
+            case 4:
-            self.logger.warn(
+                data_type = 2
-                f"unexpected VicCAN data length {data_len}; encoding as float32 x2 (key=1) and truncating/padding as needed"
+                data = struct.pack(">hhhh", *[ int(x) for x in msg.data])
-            )
+            case _:
-            data_type_key = 1
-
-        # Command is limited to 6 bits.
-        command = int(msg.command_id)
-        if command < 0:
-            self.logger.error(f"invalid negative command_id for CAN frame: {command}")
-            return
-        if command > 0x3F:
-            self.logger.warn(
-                f"command_id 0x{command:X} exceeds 6-bit range; truncating to lower 6 bits"
-            )
-            command &= 0x3F
-
-        arbitration_id = (
-            ((mcu_key & 0b111) << 8) | ((data_type_key & 0b11) << 6) | (command & 0x3F)
-        )
-
-        # Map VicCAN.data (floats) to up to 8 CAN data bytes.
-        raw_bytes: list[int] = []
-        for value in msg.data:
-            try:
-                b = int(round(value))
-            except (TypeError, ValueError):
                 self.logger.error(
-                    f"non-numeric VicCAN data value: {value}; dropping message"
+                    f"unexpected VicCAN data length: {data_len}; dropping message"
                 )
                 return
 
-            if b < 0 or b > 255:
+        # command is limited to 6 bits.
-                self.logger.warn(
+        command = int(msg.command_id)
-                    f"VicCAN data value {value} out of byte range; clamping into [0, 255]"
+        if command < 0 or command > 0x3F:
+            self.logger.error(
+                f"invalid command_id for CAN frame: {command}; dropping message"
             )
-                b = max(0, min(255, b))
+            return
-
-            raw_bytes.append(b)
-
-        if len(raw_bytes) > 8:
-            self.logger.warn(
-                f"VicCAN data too long for single CAN frame ({len(raw_bytes)} > 8); truncating"
-            )
-            raw_bytes = raw_bytes[:8]
 
         try:
             can_message = can.Message(
-                arbitration_id=arbitration_id,
+                arbitration_id=(mcu_id << 8) | (data_type << 6) | command,
-                data=raw_bytes,
+                data=data,
                 is_extended_id=False,
             )
         except Exception as e:

src/core_pkg/core_pkg/core_ptz.py

@@ -262,7 +262,7 @@ class PtzNode(Node):
             f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
         )
         self.debug_pub.publish(msg)
-        self.get_logger().info(message_text)
+        self.get_logger().debug(message_text)
 
     def run_async_func(self, coro):
         """Run an async function in the event loop."""