fix: scrub remaining Mamba references in can_bridge and e2e test protocol files

- balance_protocol.py: Mamba→Orin / Mamba→VESC comments → ESP32-S3 BALANCE
- can_bridge_node.py: docstring and inline comments
- __init__.py: package description
- protocol_defs.py: all Mamba references in docstring and comments
- test_fc_vesc_broadcast.py, test_drive_command.py: test comments

Zero Mamba/STM32F722/BlackPill/stm32_protocol/mamba_protocol references
now exist outside legacy/stm32/.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-firmware 2026-04-04 08:42:50 -04:00
parent 291dd689f8
commit 9aed96391f
9 changed files with 31 additions and 31 deletions

View File

@ -102,7 +102,7 @@ balance loop, and drives the hoverboard ESC via UART. Jetson Orin Nano Super
sends velocity commands over UART1. ELRS receiver on UART3 provides RC sends velocity commands over UART1. ELRS receiver on UART3 provides RC
override and kill-switch capability. override and kill-switch capability.
The legacy STM32 firmware (Mamba F722S era) has been archived to The legacy STM32 firmware (STM32 era) has been archived to
`legacy/stm32/` and is no longer built or deployed. `legacy/stm32/` and is no longer built or deployed.
## LED Subsystem (ESP32-C3) ## LED Subsystem (ESP32-C3)

View File

@ -1 +1 @@
"""SaltyBot CAN bridge package — Mamba controller and VESC telemetry via python-can.""" """SaltyBot CAN bridge package — ESP32-S3 BALANCE controller and VESC telemetry via python-can."""

View File

@ -1,16 +1,16 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
balance_protocol.py CAN message encoding/decoding for the Mamba motor controller balance_protocol.py CAN message encoding/decoding for the ESP32-S3 BALANCE motor controller
and VESC telemetry. and VESC telemetry.
CAN message layout CAN message layout
------------------ ------------------
Command frames (Orin Mamba / VESC): Command frames (Orin ESP32-S3 BALANCE / VESC):
MAMBA_CMD_VELOCITY 0x100 8 bytes left_speed (f32, m/s) | right_speed (f32, m/s) MAMBA_CMD_VELOCITY 0x100 8 bytes left_speed (f32, m/s) | right_speed (f32, m/s)
MAMBA_CMD_MODE 0x101 1 byte mode (0=idle, 1=drive, 2=estop) MAMBA_CMD_MODE 0x101 1 byte mode (0=idle, 1=drive, 2=estop)
MAMBA_CMD_ESTOP 0x102 1 byte 0x01 = stop MAMBA_CMD_ESTOP 0x102 1 byte 0x01 = stop
Telemetry frames (Mamba Orin): Telemetry frames (ESP32-S3 BALANCE Orin):
MAMBA_TELEM_IMU 0x200 24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each) MAMBA_TELEM_IMU 0x200 24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each)
MAMBA_TELEM_BATTERY 0x201 8 bytes voltage (f32, V) | current (f32, A) MAMBA_TELEM_BATTERY 0x201 8 bytes voltage (f32, V) | current (f32, A)
@ -56,7 +56,7 @@ MODE_ESTOP: int = 2
@dataclass @dataclass
class ImuTelemetry: class ImuTelemetry:
"""Decoded IMU telemetry from Mamba (MAMBA_TELEM_IMU).""" """Decoded IMU telemetry from ESP32-S3 BALANCE (MAMBA_TELEM_IMU)."""
accel_x: float = 0.0 # m/s² accel_x: float = 0.0 # m/s²
accel_y: float = 0.0 accel_y: float = 0.0
@ -68,7 +68,7 @@ class ImuTelemetry:
@dataclass @dataclass
class BatteryTelemetry: class BatteryTelemetry:
"""Decoded battery telemetry from Mamba (MAMBA_TELEM_BATTERY).""" """Decoded battery telemetry from ESP32-S3 BALANCE (MAMBA_TELEM_BATTERY)."""
voltage: float = 0.0 # V voltage: float = 0.0 # V
current: float = 0.0 # A current: float = 0.0 # A

View File

@ -1,6 +1,6 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
can_bridge_node.py ROS2 node bridging the SaltyBot Orin to the Mamba motor can_bridge_node.py ROS2 node bridging the SaltyBot Orin to the ESP32-S3 BALANCE motor
controller and VESC motor controllers over CAN bus. controller and VESC motor controllers over CAN bus.
The node opens the SocketCAN interface (slcan0 by default), spawns a background The node opens the SocketCAN interface (slcan0 by default), spawns a background
@ -9,12 +9,12 @@ reader thread to process incoming telemetry, and exposes the following interface
Subscriptions Subscriptions
------------- -------------
/cmd_vel geometry_msgs/Twist VESC speed commands (CAN) /cmd_vel geometry_msgs/Twist VESC speed commands (CAN)
/estop std_msgs/Bool Mamba e-stop (CAN) /estop std_msgs/Bool ESP32-S3 BALANCE e-stop (CAN)
Publications Publications
------------ ------------
/can/imu sensor_msgs/Imu Mamba IMU telemetry /can/imu sensor_msgs/Imu ESP32-S3 BALANCE IMU telemetry
/can/battery sensor_msgs/BatteryState Mamba battery telemetry /can/battery sensor_msgs/BatteryState ESP32-S3 BALANCE battery telemetry
/can/vesc/left/state std_msgs/Float32MultiArray Left VESC state /can/vesc/left/state std_msgs/Float32MultiArray Left VESC state
/can/vesc/right/state std_msgs/Float32MultiArray Right VESC state /can/vesc/right/state std_msgs/Float32MultiArray Right VESC state
/can/connection_status std_msgs/String "connected" | "disconnected" /can/connection_status std_msgs/String "connected" | "disconnected"
@ -64,7 +64,7 @@ _WATCHDOG_HZ: float = 10.0
class CanBridgeNode(Node): class CanBridgeNode(Node):
"""CAN bus bridge between Orin ROS2 and Mamba / VESC controllers.""" """CAN bus bridge between Orin ROS2 and ESP32-S3 BALANCE / VESC controllers."""
def __init__(self) -> None: def __init__(self) -> None:
super().__init__("can_bridge_node") super().__init__("can_bridge_node")
@ -214,18 +214,18 @@ class CanBridgeNode(Node):
# Forward left = forward right for pure translation; for rotation # Forward left = forward right for pure translation; for rotation
# left slows and right speeds up (positive angular = CCW = left turn). # left slows and right speeds up (positive angular = CCW = left turn).
# The Mamba velocity command carries both wheels independently. # The ESP32-S3 BALANCE velocity command carries both wheels independently.
left_mps = linear - angular left_mps = linear - angular
right_mps = linear + angular right_mps = linear + angular
payload = encode_velocity_cmd(left_mps, right_mps) payload = encode_velocity_cmd(left_mps, right_mps)
self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel") self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
# Keep Mamba in DRIVE mode while receiving commands # Keep ESP32-S3 BALANCE in DRIVE mode while receiving commands
self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode") self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
def _estop_cb(self, msg: Bool) -> None: def _estop_cb(self, msg: Bool) -> None:
"""Forward /estop to Mamba over CAN.""" """Forward /estop to ESP32-S3 BALANCE over CAN."""
if not self._connected: if not self._connected:
return return
payload = encode_estop_cmd(msg.data) payload = encode_estop_cmd(msg.data)
@ -234,7 +234,7 @@ class CanBridgeNode(Node):
self._send_can( self._send_can(
MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode" MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
) )
self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba") self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32-S3 BALANCE")
# ── Watchdog ────────────────────────────────────────────────────────── # ── Watchdog ──────────────────────────────────────────────────────────

View File

@ -1,28 +1,28 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
protocol_defs.py CAN message ID constants and frame builders/parsers for the protocol_defs.py CAN message ID constants and frame builders/parsers for the
OrinMambaVESC integration test suite. OrinESP32-S3 BALANCEVESC integration test suite.
All IDs and payload formats are derived from: All IDs and payload formats are derived from:
include/orin_can.h OrinFC (Mamba) protocol include/orin_can.h OrinFC (ESP32-S3 BALANCE) protocol
include/vesc_can.h VESC CAN protocol include/vesc_can.h VESC CAN protocol
saltybot_can_bridge/balance_protocol.py existing bridge constants saltybot_can_bridge/balance_protocol.py existing bridge constants
CAN IDs used in tests CAN IDs used in tests
--------------------- ---------------------
Orin FC (Mamba) commands (standard 11-bit, matching orin_can.h): Orin FC (ESP32-S3 BALANCE) commands (standard 11-bit, matching orin_can.h):
ORIN_CMD_HEARTBEAT 0x300 ORIN_CMD_HEARTBEAT 0x300
ORIN_CMD_DRIVE 0x301 int16 speed (1000..+1000), int16 steer (1000..+1000) ORIN_CMD_DRIVE 0x301 int16 speed (1000..+1000), int16 steer (1000..+1000)
ORIN_CMD_MODE 0x302 uint8 mode byte ORIN_CMD_MODE 0x302 uint8 mode byte
ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR) ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR)
FC (Mamba) Orin telemetry (standard 11-bit, matching orin_can.h): FC (ESP32-S3 BALANCE) Orin telemetry (standard 11-bit, matching orin_can.h):
FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t) FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t)
FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t) FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t)
FC_IMU 0x402 8 bytes FC_IMU 0x402 8 bytes
FC_BARO 0x403 8 bytes FC_BARO 0x403 8 bytes
Mamba VESC internal commands (matching balance_protocol.py): ESP32-S3 BALANCE VESC internal commands (matching balance_protocol.py):
MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian
MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop) MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop)
MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop
@ -36,7 +36,7 @@ import struct
from typing import Tuple from typing import Tuple
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Orin → FC (Mamba) command IDs (from orin_can.h) # Orin → FC (ESP32-S3 BALANCE) command IDs (from orin_can.h)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
ORIN_CMD_HEARTBEAT: int = 0x300 ORIN_CMD_HEARTBEAT: int = 0x300
@ -45,7 +45,7 @@ ORIN_CMD_MODE: int = 0x302
ORIN_CMD_ESTOP: int = 0x303 ORIN_CMD_ESTOP: int = 0x303
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# FC (Mamba) → Orin telemetry IDs (from orin_can.h) # FC (ESP32-S3 BALANCE) → Orin telemetry IDs (from orin_can.h)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
FC_STATUS: int = 0x400 FC_STATUS: int = 0x400
@ -54,7 +54,7 @@ FC_IMU: int = 0x402
FC_BARO: int = 0x403 FC_BARO: int = 0x403
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Mamba → VESC internal command IDs (from balance_protocol.py) # ESP32-S3 BALANCE → VESC internal command IDs (from balance_protocol.py)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
MAMBA_CMD_VELOCITY: int = 0x100 MAMBA_CMD_VELOCITY: int = 0x100
@ -136,7 +136,7 @@ def build_estop_cmd(action: int = 1) -> bytes:
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Frame builders — Mamba velocity commands (balance_protocol.py encoding) # Frame builders — ESP32-S3 BALANCE velocity commands (balance_protocol.py encoding)
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes: def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes:

View File

@ -14,7 +14,7 @@ setup(
zip_safe=True, zip_safe=True,
maintainer="sl-jetson", maintainer="sl-jetson",
maintainer_email="sl-jetson@saltylab.local", maintainer_email="sl-jetson@saltylab.local",
description="End-to-end CAN integration tests for Orin↔Mamba↔VESC full loop", description="End-to-end CAN integration tests for Orin↔ESP32-S3 BALANCE↔VESC full loop",
license="MIT", license="MIT",
tests_require=["pytest"], tests_require=["pytest"],
entry_points={ entry_points={

View File

@ -3,7 +3,7 @@
test_drive_command.py Integration tests for the drive command path. test_drive_command.py Integration tests for the drive command path.
Tests verify: Tests verify:
DRIVE cmd Mamba receives velocity command frame mock VESC status response DRIVE cmd ESP32-S3 BALANCE receives velocity command frame mock VESC status response
FC_VESC broadcast contains correct RPMs. FC_VESC broadcast contains correct RPMs.
All tests run without real hardware or a running ROS2 system. All tests run without real hardware or a running ROS2 system.
@ -61,7 +61,7 @@ def _send_drive(bus, left_mps: float, right_mps: float) -> None:
class TestDriveForward: class TestDriveForward:
def test_drive_forward_velocity_frame_sent(self, mock_can_bus): def test_drive_forward_velocity_frame_sent(self, mock_can_bus):
""" """
Inject DRIVE cmd (1.0 m/s, 1.0 m/s) verify Mamba receives Inject DRIVE cmd (1.0 m/s, 1.0 m/s) verify ESP32-S3 BALANCE receives
a MAMBA_CMD_VELOCITY frame with correct payload. a MAMBA_CMD_VELOCITY frame with correct payload.
""" """
_send_drive(mock_can_bus, 1.0, 1.0) _send_drive(mock_can_bus, 1.0, 1.0)
@ -84,7 +84,7 @@ class TestDriveForward:
def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus): def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus):
""" """
Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct. Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct.
(In the real loop Mamba computes RPM from m/s and broadcasts FC_VESC.) (In the real loop ESP32-S3 BALANCE computes RPM from m/s and broadcasts FC_VESC.)
This test checks the FC_VESC frame format and parser. This test checks the FC_VESC frame format and parser.
""" """
# Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics) # Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics)

View File

@ -47,7 +47,7 @@ class VescStatusAggregator:
2. Builds an FC_VESC broadcast payload 2. Builds an FC_VESC broadcast payload
3. Injects the FC_VESC frame onto the mock bus 3. Injects the FC_VESC frame onto the mock bus
This represents the Mamba Orin telemetry path. This represents the ESP32-S3 BALANCE Orin telemetry path.
""" """
def __init__(self, bus: MockCANBus): def __init__(self, bus: MockCANBus):

View File

@ -90,7 +90,7 @@ class HeartbeatSimulator:
def _simulate_estop_on_timeout(bus: MockCANBus) -> None: def _simulate_estop_on_timeout(bus: MockCANBus) -> None:
""" """
Simulate the firmware-side logic: when heartbeat timeout expires, Simulate the firmware-side logic: when heartbeat timeout expires,
the FC sends an e-stop command by setting estop mode on the Mamba bus. the FC sends an e-stop command by setting estop mode on the ESP32-S3 BALANCE bus.
We model this as the bridge sending zero velocity + ESTOP mode. We model this as the bridge sending zero velocity + ESTOP mode.
""" """