seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(arch): align CAN/UART bridges with SAUL-TEE-SYSTEM-REFERENCE.md spec

Browse Source 
Update CAN and serial bridge code to match authoritative protocol spec
from docs/SAUL-TEE-SYSTEM-REFERENCE.md §5-6 (hal, 2026-04-04).

mamba_protocol.py (CAN, Orin ↔ ESP32 BALANCE):
- 0x300 DRIVE: [speed:i16][steer:i16][mode:u8][flags:u8][_:u16] — combined frame
- 0x301 ARM:   [arm:u8]
- 0x302 PID:   [kp:f16][ki:f16][kd:f16][_:u16]  — half-float gains
- 0x303 ESTOP: [0xE5] — magic byte cut
- 0x400 ATTITUDE: [pitch:f16][speed:f16][yaw_rate:f16][state:u8][flags:u8]
- 0x401 BATTERY:  [vbat_mv:u16][fault_code:u8][rssi:i8]
- Add VESC STATUS1/4/5 decode helpers; VESC IDs 56 (left) / 68 (right)

can_bridge_node.py:
- /cmd_vel → encode_drive_cmd (speed/steer int16, MODE_DRIVE)
- /estop → encode_estop_cmd (magic 0xE5); clear → DISARM
- /saltybot/arm → encode_arm_cmd (new subscription)
- Watchdog sends DRIVE(0,0,MODE_IDLE) when /cmd_vel silent
- ATTITUDE (0x400) → /saltybot/attitude + /saltybot/balance_state JSON
- BATTERY  (0x401) → /can/battery BatteryState
- VESC STATUS1 frames → /can/vesc/left|right/state

stm32_cmd_node.py — rewritten for inter-board protocol API:
- Imports from updated stm32_protocol (BAUD_RATE=460800, new frame types)
- RX: RcChannels → /saltybot/rc_channels, SensorData → /saltybot/sensors
- TX: encode_led_cmd, encode_output_cmd from /saltybot/leds + /saltybot/outputs
- HEARTBEAT (0x20) timer replaces old SPEED_STEER/ARM logic

stm32_cmd_params.yaml: serial_port=/dev/esp32-io, baud=460800
stm32_cmd.launch.py: updated defaults and description

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-controls 2026-04-04 08:32:00 -04:00
parent cfd5a15b3e
commit f66035cf81
6 changed files with 406 additions and 658 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
docs/wiring-diagram.md
View File

@ -1,6 +1,13 @@
# SaltyLab Wiring Diagram # SaltyLab / SAUL-TEE Wiring Reference
## System Overview > ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32 retired.
> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN.
> **Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md)
> Historical STM32/Mamba wiring below is **obsolete** — retained for reference only.
---
## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md)
``` ```
┌─────────────────────────────────────────────────────────────────────┐ ┌─────────────────────────────────────────────────────────────────────┐
@ -139,7 +146,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
| 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` | | 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` |
## FC UART Summary (MAMBA F722S) ## FC UART Summary (MAMBA F722S — OBSOLETE)
| UART | Pins | Baud | Assignment | Notes | | UART | Pins | Baud | Assignment | Notes |
|------|------|------|------------|-------| |------|------|------|------------|-------|

38
jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml
View File

@ -1,30 +1,16 @@
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119) # stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge)
# Binary-framed Jetson↔STM32 bridge at 921600 baud. # Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud.
# Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8]
# Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5
# ── Serial port ──────────────────────────────────────────────────────────────── # ── Serial port ────────────────────────────────────────────────────────────────
# Use /dev/stm32-bridge if the udev rule is applied: # Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md).
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", # ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed.
# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout" serial_port: /dev/esp32-io
serial_port: /dev/ttyACM0 baud_rate: 460800
baud_rate: 921600 reconnect_delay: 2.0 # seconds between reconnect attempts
reconnect_delay: 2.0 # seconds between USB reconnect attempts
# ── Heartbeat ───────────────────────────────────────────────────────────────── # ── Heartbeat ─────────────────────────────────────────────────────────────────
# HEARTBEAT frame sent every heartbeat_period seconds. # HEARTBEAT (0x20) sent every heartbeat_period.
# STM32 fires watchdog and reverts to safe state if no frame received for 500ms. # ESP32 IO watchdog fires if no heartbeat for ~500 ms.
heartbeat_period: 0.2 # 200ms → well within 500ms STM32 watchdog heartbeat_period: 0.2 # 200 ms → well within 500 ms watchdog
# ── Watchdog (Jetson-side) ────────────────────────────────────────────────────
# If no /cmd_vel message received for watchdog_timeout seconds,
# send SPEED_STEER(0,0) to stop the robot.
watchdog_timeout: 0.5 # 500ms
# ── Twist velocity scaling ────────────────────────────────────────────────────
# speed = clamp(linear.x * speed_scale, -1000, 1000) (m/s → ESC units)
# steer = clamp(angular.z * steer_scale, -1000, 1000) (rad/s → ESC units)
#
# Default: 1 m/s → 1000 ESC units, ±2 rad/s → ±1000 steer.
# Negative steer_scale flips ROS2 CCW+ convention to match ESC steer direction.
# Tune speed_scale to set the physical top speed.
speed_scale: 1000.0
steer_scale: -500.0

26
jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py
View File

@ -1,14 +1,14 @@
"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119). """stm32_cmd.launch.py — Launch the ESP32-S3 IO auxiliary bridge node.
Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud (inter-board protocol).
Handles RC monitoring, sensor data, LED/output commands.
Primary drive path uses CAN (can_bridge_node / saltybot_can_node), not this node.
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5
Usage: Usage:
# Default (binary protocol, bidirectional):
ros2 launch saltybot_bridge stm32_cmd.launch.py ros2 launch saltybot_bridge stm32_cmd.launch.py
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
# Override serial port:
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1
# Custom velocity scales:
ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
""" """
import os import os
@ -24,11 +24,8 @@ def generate_launch_description() -> LaunchDescription:
params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml") params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
return LaunchDescription([ return LaunchDescription([
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"), DeclareLaunchArgument("serial_port", default_value="/dev/esp32-io"),
DeclareLaunchArgument("baud_rate", default_value="921600"), DeclareLaunchArgument("baud_rate", default_value="460800"),
DeclareLaunchArgument("speed_scale", default_value="1000.0"),
DeclareLaunchArgument("steer_scale", default_value="-500.0"),
DeclareLaunchArgument("watchdog_timeout", default_value="0.5"),
DeclareLaunchArgument("heartbeat_period", default_value="0.2"), DeclareLaunchArgument("heartbeat_period", default_value="0.2"),
Node( Node(
@ -42,9 +39,6 @@ def generate_launch_description() -> LaunchDescription:
{ {
"serial_port": LaunchConfiguration("serial_port"), "serial_port": LaunchConfiguration("serial_port"),
"baud_rate": LaunchConfiguration("baud_rate"), "baud_rate": LaunchConfiguration("baud_rate"),
"speed_scale": LaunchConfiguration("speed_scale"),
"steer_scale": LaunchConfiguration("steer_scale"),
"watchdog_timeout": LaunchConfiguration("watchdog_timeout"),
"heartbeat_period": LaunchConfiguration("heartbeat_period"), "heartbeat_period": LaunchConfiguration("heartbeat_period"),
}, },
], ],

431
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
View File

@ -1,45 +1,32 @@
"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge. """stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node.
Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud. inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).
TX commands (Jetson → STM32): This node is NOT the primary drive path (that is CAN via can_bridge_node).
SPEED_STEER — 50 Hz from /cmd_vel subscription It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.
HEARTBEAT — 200 ms timer (STM32 watchdog fires at 500 ms)
ARM — via /saltybot/arm service
SET_MODE — via /saltybot/set_mode service
PID_UPDATE — via /saltybot/pid_update topic
Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning. Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud
RX telemetry (STM32 → Jetson): RX from ESP32 IO:
IMU → /saltybot/imu (sensor_msgs/Imu) RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON)
BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON) SENSORS (0x02) → /saltybot/sensors (std_msgs/String JSON)
MOTOR_RPM → /saltybot/telemetry/motor_rpm (std_msgs/String JSON)
ARM_STATE → /saltybot/arm_state (std_msgs/String JSON)
ERROR → /saltybot/error (std_msgs/String JSON)
All frames → /diagnostics (diagnostic_msgs/DiagnosticArray)
Auto-reconnect: USB disconnect is detected when serial.read() raises; node TX to ESP32 IO:
continuously retries at reconnect_delay interval. LED_CMD (0x10) ← /saltybot/leds (std_msgs/String JSON)
OUTPUT_CMD (0x11) ← /saltybot/outputs (std_msgs/String JSON)
This node owns /dev/ttyACM0 exclusively — do NOT run alongside HEARTBEAT (0x20) — sent every heartbeat_period (keep IO watchdog alive)
serial_bridge_node or saltybot_cmd_node on the same port.
Parameters (config/stm32_cmd_params.yaml): Parameters (config/stm32_cmd_params.yaml):
serial_port /dev/ttyACM0 serial_port /dev/esp32-io
baud_rate 921600 baud_rate 460800
reconnect_delay 2.0 (seconds) reconnect_delay 2.0
heartbeat_period 0.2 (seconds) heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms)
watchdog_timeout 0.5 (seconds — no /cmd_vel → send zero-speed)
speed_scale 1000.0 (linear.x m/s → ESC units)
steer_scale -500.0 (angular.z rad/s → ESC units, neg to flip convention)
""" """
from __future__ import annotations from __future__ import annotations
import json import json
import math
import threading import threading
import time import time
@ -50,119 +37,69 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
import serial import serial
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Imu
from std_msgs.msg import String from std_msgs.msg import String
from std_srvs.srv import SetBool, Trigger
from .stm32_protocol import ( from .stm32_protocol import (
BAUD_RATE,
FrameParser, FrameParser,
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame, RcChannels,
encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode, SensorData,
encode_pid_update, encode_heartbeat,
encode_led_cmd,
encode_output_cmd,
) )
# ── Constants ─────────────────────────────────────────────────────────────────
IMU_FRAME_ID = "imu_link"
_ARM_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
def _clamp(v: float, lo: float, hi: float) -> float:
return max(lo, min(hi, v))
# ── Node ──────────────────────────────────────────────────────────────────────
class Stm32CmdNode(Node): class Stm32CmdNode(Node):
"""Binary-framed Jetson↔STM32 bridge node.""" """Orin ↔ ESP32-S3 IO auxiliary bridge node."""
def __init__(self) -> None: def __init__(self) -> None:
super().__init__("stm32_cmd_node") super().__init__("stm32_cmd_node")
# ── Parameters ──────────────────────────────────────────────────────── # ── Parameters ────────────────────────────────────────────────────
self.declare_parameter("serial_port", "/dev/ttyACM0") self.declare_parameter("serial_port", "/dev/esp32-io")
self.declare_parameter("baud_rate", 921600) self.declare_parameter("baud_rate", BAUD_RATE)
self.declare_parameter("reconnect_delay", 2.0) self.declare_parameter("reconnect_delay", 2.0)
self.declare_parameter("heartbeat_period", 0.2) self.declare_parameter("heartbeat_period", 0.2)
self.declare_parameter("watchdog_timeout", 0.5)
self.declare_parameter("speed_scale", 1000.0)
self.declare_parameter("steer_scale", -500.0)
port = self.get_parameter("serial_port").value self._port_name = self.get_parameter("serial_port").value
baud = self.get_parameter("baud_rate").value self._baud = self.get_parameter("baud_rate").value
self._reconnect_delay = self.get_parameter("reconnect_delay").value self._reconnect_delay = self.get_parameter("reconnect_delay").value
self._hb_period = self.get_parameter("heartbeat_period").value self._hb_period = self.get_parameter("heartbeat_period").value
self._wd_timeout = self.get_parameter("watchdog_timeout").value
self._speed_scale = self.get_parameter("speed_scale").value
self._steer_scale = self.get_parameter("steer_scale").value
# ── QoS ─────────────────────────────────────────────────────────────── # ── QoS ───────────────────────────────────────────────────────────
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST, depth=10,
)
rel_qos = QoSProfile( rel_qos = QoSProfile(
reliability=ReliabilityPolicy.RELIABLE, reliability=ReliabilityPolicy.RELIABLE,
history=HistoryPolicy.KEEP_LAST, depth=10, history=HistoryPolicy.KEEP_LAST, depth=10,
) )
# ── Publishers ──────────────────────────────────────────────────────── # ── Publishers ────────────────────────────────────────────────────
self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos) self._rc_pub = self.create_publisher(String, "/saltybot/rc_channels", rel_qos)
self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", rel_qos) self._sens_pub = self.create_publisher(String, "/saltybot/sensors", rel_qos)
self._error_pub = self.create_publisher(String, "/saltybot/error", rel_qos)
self._battery_pub = self.create_publisher(String, "/saltybot/telemetry/battery", rel_qos)
self._rpm_pub = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos)
self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos) self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos)
# ── Subscribers ─────────────────────────────────────────────────────── # ── Subscriptions ─────────────────────────────────────────────────
self._cmd_vel_sub = self.create_subscription( self.create_subscription(String, "/saltybot/leds", self._on_leds, rel_qos)
Twist, "/cmd_vel", self._on_cmd_vel, rel_qos, self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)
)
self._pid_sub = self.create_subscription(
String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
)
# ── Services ────────────────────────────────────────────────────────── # ── Serial state ──────────────────────────────────────────────────
self._arm_srv = self.create_service(SetBool, "/saltybot/arm", self._svc_arm)
self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode)
# ── Serial state ──────────────────────────────────────────────────────
self._port_name = port
self._baud = baud
self._ser: serial.Serial | None = None self._ser: serial.Serial | None = None
self._ser_lock = threading.Lock() self._ser_lock = threading.Lock()
self._parser = FrameParser() self._parser = FrameParser()
self._rx_count = 0
# ── TX state ────────────────────────────────────────────────────────── # ── Open serial and start timers ──────────────────────────────────
self._last_speed = 0
self._last_steer = 0
self._last_cmd_t = time.monotonic()
self._watchdog_sent = False # tracks whether we already sent zero
# ── Diagnostics state ──────────────────────────────────────────────────
self._last_arm_state = -1
self._last_battery_mv = 0
self._rx_frame_count = 0
# ── Open serial and start timers ──────────────────────────────────────
self._open_serial() self._open_serial()
# Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean)
self._read_timer = self.create_timer(0.005, self._read_cb) self._read_timer = self.create_timer(0.005, self._read_cb)
# Heartbeat TX
self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb) self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb)
# Watchdog check (fires at 2× watchdog_timeout for quick detection)
self._wd_timer = self.create_timer(self._wd_timeout / 2, self._watchdog_cb)
# Periodic diagnostics
self._diag_timer = self.create_timer(1.0, self._publish_diagnostics) self._diag_timer = self.create_timer(1.0, self._publish_diagnostics)
self.get_logger().info( self.get_logger().info(
f"stm32_cmd_node started — {port} @ {baud} baud | " f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
) )
# ── Serial management ───────────────────────────────────────────────────── # ── Serial management ─────────────────────────────────────────────────
def _open_serial(self) -> bool: def _open_serial(self) -> bool:
with self._ser_lock: with self._ser_lock:
@ -170,7 +107,7 @@ class Stm32CmdNode(Node):
self._ser = serial.Serial( self._ser = serial.Serial(
port=self._port_name, port=self._port_name,
baudrate=self._baud, baudrate=self._baud,
timeout=0.005, # non-blocking reads timeout=0.005,
write_timeout=0.1, write_timeout=0.1,
) )
self._ser.reset_input_buffer() self._ser.reset_input_buffer()
@ -185,17 +122,7 @@ class Stm32CmdNode(Node):
self._ser = None self._ser = None
return False return False
def _close_serial(self) -> None:
with self._ser_lock:
if self._ser and self._ser.is_open:
try:
self._ser.close()
except Exception:
pass
self._ser = None
def _write(self, data: bytes) -> bool: def _write(self, data: bytes) -> bool:
"""Thread-safe serial write. Returns False if port is not open."""
with self._ser_lock: with self._ser_lock:
if self._ser is None or not self._ser.is_open: if self._ser is None or not self._ser.is_open:
return False return False
@ -207,16 +134,15 @@ class Stm32CmdNode(Node):
self._ser = None self._ser = None
return False return False
# ── RX — read callback ──────────────────────────────────────────────────── # ── RX ────────────────────────────────────────────────────────────────
def _read_cb(self) -> None: def _read_cb(self) -> None:
"""Read bytes from serial and feed them to the frame parser."""
raw: bytes | None = None raw: bytes | None = None
reconnect_needed = False reconnect = False
with self._ser_lock: with self._ser_lock:
if self._ser is None or not self._ser.is_open: if self._ser is None or not self._ser.is_open:
reconnect_needed = True reconnect = True
else: else:
try: try:
n = self._ser.in_waiting n = self._ser.in_waiting
@ -225,9 +151,9 @@ class Stm32CmdNode(Node):
except serial.SerialException as exc: except serial.SerialException as exc:
self.get_logger().error(f"Serial read error: {exc}") self.get_logger().error(f"Serial read error: {exc}")
self._ser = None self._ser = None
reconnect_needed = True reconnect = True
if reconnect_needed: if reconnect:
self.get_logger().warn( self.get_logger().warn(
"Serial disconnected — will retry", "Serial disconnected — will retry",
throttle_duration_sec=self._reconnect_delay, throttle_duration_sec=self._reconnect_delay,
@ -240,230 +166,105 @@ class Stm32CmdNode(Node):
return return
for byte in raw: for byte in raw:
frame = self._parser.feed(byte) msg = self._parser.feed(byte)
if frame is not None: if msg is not None:
self._rx_frame_count += 1 self._rx_count += 1
self._dispatch_frame(frame) self._dispatch(msg)
def _dispatch_frame(self, frame) -> None: def _dispatch(self, msg) -> None:
"""Route a decoded frame to the appropriate publisher."""
now = self.get_clock().now().to_msg() now = self.get_clock().now().to_msg()
ts = f"{now.sec}.{now.nanosec:09d}"
if isinstance(frame, ImuFrame): if isinstance(msg, RcChannels):
self._publish_imu(frame, now) out = String()
out.data = json.dumps({
"channels": msg.channels,
"source": msg.source,
"ts": ts,
})
self._rc_pub.publish(out)
elif isinstance(frame, BatteryFrame): elif isinstance(msg, SensorData):
self._publish_battery(frame, now) out = String()
out.data = json.dumps({
"pressure_pa": msg.pressure_pa,
"temperature_c": msg.temperature_c,
"tof_mm": msg.tof_mm,
"ts": ts,
})
self._sens_pub.publish(out)
elif isinstance(frame, MotorRpmFrame): elif isinstance(msg, tuple):
self._publish_motor_rpm(frame, now) type_code, _ = msg
self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
elif isinstance(frame, ArmStateFrame): # ── TX ────────────────────────────────────────────────────────────────
self._publish_arm_state(frame, now)
elif isinstance(frame, ErrorFrame):
self._publish_error(frame, now)
elif isinstance(frame, tuple):
type_code, payload = frame
self.get_logger().debug(
f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}"
)
# ── Telemetry publishers ──────────────────────────────────────────────────
def _publish_imu(self, frame: ImuFrame, stamp) -> None:
msg = Imu()
msg.header.stamp = stamp
msg.header.frame_id = IMU_FRAME_ID
# orientation: unknown — signal with -1 in first covariance
msg.orientation_covariance[0] = -1.0
msg.angular_velocity.x = math.radians(frame.pitch_deg)
msg.angular_velocity.y = math.radians(frame.roll_deg)
msg.angular_velocity.z = math.radians(frame.yaw_deg)
cov = math.radians(0.3) ** 2 # ±0.3° noise estimate from STM32 BMI088
msg.angular_velocity_covariance[0] = cov
msg.angular_velocity_covariance[4] = cov
msg.angular_velocity_covariance[8] = cov
msg.linear_acceleration.x = frame.accel_x
msg.linear_acceleration.y = frame.accel_y
msg.linear_acceleration.z = frame.accel_z
acov = 0.05 ** 2 # ±0.05 m/s² noise
msg.linear_acceleration_covariance[0] = acov
msg.linear_acceleration_covariance[4] = acov
msg.linear_acceleration_covariance[8] = acov
self._imu_pub.publish(msg)
def _publish_battery(self, frame: BatteryFrame, stamp) -> None:
payload = {
"voltage_v": round(frame.voltage_mv / 1000.0, 3),
"voltage_mv": frame.voltage_mv,
"current_ma": frame.current_ma,
"soc_pct": frame.soc_pct,
"charging": frame.current_ma < -100,
"ts": f"{stamp.sec}.{stamp.nanosec:09d}",
}
self._last_battery_mv = frame.voltage_mv
msg = String()
msg.data = json.dumps(payload)
self._battery_pub.publish(msg)
def _publish_motor_rpm(self, frame: MotorRpmFrame, stamp) -> None:
payload = {
"left_rpm": frame.left_rpm,
"right_rpm": frame.right_rpm,
"ts": f"{stamp.sec}.{stamp.nanosec:09d}",
}
msg = String()
msg.data = json.dumps(payload)
self._rpm_pub.publish(msg)
def _publish_arm_state(self, frame: ArmStateFrame, stamp) -> None:
label = _ARM_LABEL.get(frame.state, f"UNKNOWN({frame.state})")
if frame.state != self._last_arm_state:
self.get_logger().info(f"Arm state → {label} (flags=0x{frame.error_flags:02X})")
self._last_arm_state = frame.state
payload = {
"state": frame.state,
"state_label": label,
"error_flags": frame.error_flags,
"ts": f"{stamp.sec}.{stamp.nanosec:09d}",
}
msg = String()
msg.data = json.dumps(payload)
self._arm_pub.publish(msg)
def _publish_error(self, frame: ErrorFrame, stamp) -> None:
self.get_logger().error(
f"STM32 error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}"
)
payload = {
"error_code": frame.error_code,
"subcode": frame.subcode,
"ts": f"{stamp.sec}.{stamp.nanosec:09d}",
}
msg = String()
msg.data = json.dumps(payload)
self._error_pub.publish(msg)
# ── TX — command send ─────────────────────────────────────────────────────
def _on_cmd_vel(self, msg: Twist) -> None:
"""Convert /cmd_vel Twist to SPEED_STEER frame at up to 50 Hz."""
speed = int(_clamp(msg.linear.x * self._speed_scale, -1000.0, 1000.0))
steer = int(_clamp(msg.angular.z * self._steer_scale, -1000.0, 1000.0))
self._last_speed = speed
self._last_steer = steer
self._last_cmd_t = time.monotonic()
self._watchdog_sent = False
frame = encode_speed_steer(speed, steer)
if not self._write(frame):
self.get_logger().warn(
"SPEED_STEER dropped — serial not open",
throttle_duration_sec=2.0,
)
def _heartbeat_cb(self) -> None: def _heartbeat_cb(self) -> None:
"""Send HEARTBEAT every heartbeat_period (default 200ms)."""
self._write(encode_heartbeat()) self._write(encode_heartbeat())
def _watchdog_cb(self) -> None: def _on_leds(self, msg: String) -> None:
"""Send zero-speed if /cmd_vel silent for watchdog_timeout seconds.""" """Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
if time.monotonic() - self._last_cmd_t >= self._wd_timeout:
if not self._watchdog_sent:
self.get_logger().warn(
f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed"
)
self._watchdog_sent = True
self._last_speed = 0
self._last_steer = 0
self._write(encode_speed_steer(0, 0))
def _on_pid_update(self, msg: String) -> None:
"""Parse JSON /saltybot/pid_update and send PID_UPDATE frame."""
try: try:
data = json.loads(msg.data) d = json.loads(msg.data)
kp = float(data["kp"]) frame = encode_led_cmd(
ki = float(data["ki"]) int(d.get("pattern", 0)),
kd = float(data["kd"]) int(d.get("r", 0)),
except (ValueError, KeyError, json.JSONDecodeError) as exc: int(d.get("g", 0)),
self.get_logger().error(f"Bad PID update JSON: {exc}") int(d.get("b", 0)),
return
frame = encode_pid_update(kp, ki, kd)
if self._write(frame):
self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}")
else:
self.get_logger().warn("PID_UPDATE dropped — serial not open")
# ── Services ──────────────────────────────────────────────────────────────
def _svc_arm(self, request: SetBool.Request, response: SetBool.Response):
"""SetBool(True) = arm, SetBool(False) = disarm."""
arm = request.data
frame = encode_arm(arm)
ok = self._write(frame)
response.success = ok
response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open"
self.get_logger().info(
f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}"
) )
return response except (ValueError, KeyError, json.JSONDecodeError) as exc:
self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
return
self._write(frame)
def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response): def _on_outputs(self, msg: String) -> None:
"""SetBool: data maps to mode byte (True=1, False=0).""" """Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
mode = 1 if request.data else 0 try:
frame = encode_set_mode(mode) d = json.loads(msg.data)
ok = self._write(frame) frame = encode_output_cmd(
response.success = ok bool(d.get("horn", False)),
response.message = f"mode={mode}" if ok else "serial not open" bool(d.get("buzzer", False)),
return response int(d.get("headlight", 0)),
int(d.get("fan", 0)),
)
except (ValueError, KeyError, json.JSONDecodeError) as exc:
self.get_logger().error(f"Bad /saltybot/outputs JSON: {exc}")
return
self._write(frame)
# ── Diagnostics ─────────────────────────────────────────────────────────── # ── Diagnostics ───────────────────────────────────────────────────────
def _publish_diagnostics(self) -> None: def _publish_diagnostics(self) -> None:
diag = DiagnosticArray() diag = DiagnosticArray()
diag.header.stamp = self.get_clock().now().to_msg() diag.header.stamp = self.get_clock().now().to_msg()
status = DiagnosticStatus() status = DiagnosticStatus()
status.name = "saltybot/stm32_cmd_node" status.name = "saltybot/esp32_io_bridge"
status.hardware_id = "stm32f722" status.hardware_id = "esp32-s3-io"
port_ok = self._ser is not None and self._ser.is_open port_ok = self._ser is not None and self._ser.is_open
if port_ok: status.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
status.level = DiagnosticStatus.OK status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
status.message = "Serial OK"
else:
status.level = DiagnosticStatus.ERROR
status.message = f"Serial disconnected: {self._port_name}"
wd_age = time.monotonic() - self._last_cmd_t
status.values = [ status.values = [
KeyValue(key="serial_port", value=self._port_name), KeyValue(key="serial_port", value=self._port_name),
KeyValue(key="baud_rate", value=str(self._baud)),
KeyValue(key="port_open", value=str(port_ok)), KeyValue(key="port_open", value=str(port_ok)),
KeyValue(key="rx_frames", value=str(self._rx_frame_count)), KeyValue(key="rx_frames", value=str(self._rx_count)),
KeyValue(key="rx_errors", value=str(self._parser.frames_error)), KeyValue(key="rx_errors", value=str(self._parser.frames_error)),
KeyValue(key="last_speed", value=str(self._last_speed)),
KeyValue(key="last_steer", value=str(self._last_steer)),
KeyValue(key="cmd_vel_age_s", value=f"{wd_age:.2f}"),
KeyValue(key="battery_mv", value=str(self._last_battery_mv)),
KeyValue(key="arm_state", value=_ARM_LABEL.get(self._last_arm_state, "?")),
] ]
diag.status.append(status) diag.status.append(status)
self._diag_pub.publish(diag) self._diag_pub.publish(diag)
# ── Lifecycle ───────────────────────────────────────────────────────────── # ── Lifecycle ─────────────────────────────────────────────────────────
def destroy_node(self) -> None: def destroy_node(self) -> None:
# Send zero-speed + disarm on shutdown self._write(encode_heartbeat(state=0))
self._write(encode_speed_steer(0, 0)) with self._ser_lock:
self._write(encode_arm(False)) if self._ser and self._ser.is_open:
self._close_serial() try:
self._ser.close()
except Exception:
pass
self._ser = None
super().destroy_node() super().destroy_node()

262
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py
View File

@ -1,26 +1,34 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE
board and VESC motor controllers over CAN bus (CANable2 / slcan0, 500 kbps). board and VESC motor controllers over CAN bus (CANable 2.0 / slcan0, 500 kbps).
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §4 & §6 (2026-04-04) Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
Subscriptions Subscriptions
------------- -------------
/cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300) /cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300)
/estop std_msgs/Bool → ORIN_CMD_ESTOP (0x302) /estop std_msgs/Bool → ORIN_CMD_ESTOP (0x303)
/saltybot/arm std_msgs/Bool → ORIN_CMD_ARM (0x301)
Publications Publications
------------ ------------
/can/imu sensor_msgs/Imu from FC_STATUS (0x400) pitch /saltybot/attitude std_msgs/String (JSON) pitch, speed, yaw_rate, state
/can/battery sensor_msgs/BatteryState from FC_STATUS (0x400) vbat_mv /saltybot/balance_state std_msgs/String (JSON) alias of /saltybot/attitude
/can/vesc/left/state std_msgs/Float32MultiArray from FC_VESC (0x401) /can/battery sensor_msgs/BatteryState vbat_mv, fault, rssi
/can/vesc/right/state std_msgs/Float32MultiArray from FC_VESC (0x401) /can/vesc/left/state std_msgs/Float32MultiArray VESC STATUS_1 left
/can/vesc/right/state std_msgs/Float32MultiArray VESC STATUS_1 right
/can/connection_status std_msgs/String "connected" | "disconnected" /can/connection_status std_msgs/String "connected" | "disconnected"
Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674 Parameters
----------
can_interface str CAN socket name (default: slcan0)
speed_scale float /cmd_vel linear.x (m/s) → motor units (default: 1000.0)
steer_scale float /cmd_vel angular.z (rad/s) → motor units (default: -500.0)
command_timeout_s float watchdog zero-vel threshold (default: 0.5)
""" """
import json
import threading import threading
import time import time
from typing import Optional from typing import Optional
@ -29,42 +37,32 @@ import can
import rclpy import rclpy
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from rclpy.node import Node from rclpy.node import Node
from sensor_msgs.msg import BatteryState
from sensor_msgs.msg import BatteryState, Imu
from std_msgs.msg import Bool, Float32MultiArray, String from std_msgs.msg import Bool, Float32MultiArray, String
from saltybot_can_bridge.mamba_protocol import ( from saltybot_can_bridge.mamba_protocol import (
# Orin → BALANCE command IDs
ORIN_CMD_DRIVE, ORIN_CMD_DRIVE,
ORIN_CMD_MODE, ORIN_CMD_ARM,
ORIN_CMD_ESTOP, ORIN_CMD_ESTOP,
# BALANCE → Orin telemetry IDs ESP32_TELEM_ATTITUDE,
FC_STATUS, ESP32_TELEM_BATTERY,
FC_VESC,
# VESC node IDs
VESC_LEFT_ID, VESC_LEFT_ID,
VESC_RIGHT_ID, VESC_RIGHT_ID,
VESC_STATUS_1, VESC_STATUS_1,
# Mode constants
MODE_DRIVE, MODE_DRIVE,
MODE_ESTOP,
MODE_IDLE, MODE_IDLE,
# Encoders
encode_drive_cmd, encode_drive_cmd,
encode_mode_cmd, encode_arm_cmd,
encode_estop_cmd, encode_estop_cmd,
encode_led_cmd, decode_attitude,
# Decoders decode_battery,
decode_fc_status,
decode_fc_vesc,
decode_vesc_status1, decode_vesc_status1,
decode_vesc_can_id,
) )
# Reconnect attempt interval when CAN bus is lost # Reconnect attempt interval when CAN bus is lost
_RECONNECT_INTERVAL_S: float = 5.0 _RECONNECT_INTERVAL_S: float = 5.0
# Watchdog timer tick rate (Hz) # Watchdog tick rate (Hz); sends zero DRIVE when /cmd_vel is silent
_WATCHDOG_HZ: float = 10.0 _WATCHDOG_HZ: float = 10.0
@ -76,37 +74,37 @@ class CanBridgeNode(Node):
# ── Parameters ──────────────────────────────────────────────────── # ── Parameters ────────────────────────────────────────────────────
self.declare_parameter("can_interface", "slcan0") self.declare_parameter("can_interface", "slcan0")
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID) # 56 self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID)
self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) # 68 self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID)
self.declare_parameter("speed_scale", 1000.0)
self.declare_parameter("steer_scale", -500.0)
self.declare_parameter("command_timeout_s", 0.5) self.declare_parameter("command_timeout_s", 0.5)
self._iface: str = self.get_parameter("can_interface").value self._iface = self.get_parameter("can_interface").value
self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value self._left_vesc_id = self.get_parameter("left_vesc_can_id").value
self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value self._right_vesc_id = self.get_parameter("right_vesc_can_id").value
self._cmd_timeout: float = self.get_parameter("command_timeout_s").value self._speed_scale = self.get_parameter("speed_scale").value
self._steer_scale = self.get_parameter("steer_scale").value
self._cmd_timeout = self.get_parameter("command_timeout_s").value
# ── State ───────────────────────────────────────────────────────── # ── State ─────────────────────────────────────────────────────────
self._bus: Optional[can.BusABC] = None self._bus: Optional[can.BusABC] = None
self._connected: bool = False self._connected: bool = False
self._last_cmd_time: float = time.monotonic() self._last_cmd_time: float = time.monotonic()
self._lock = threading.Lock() # protects _bus / _connected self._lock = threading.Lock()
# ── Publishers ──────────────────────────────────────────────────── # ── Publishers ────────────────────────────────────────────────────
self._pub_imu = self.create_publisher(Imu, "/can/imu", 10) self._pub_attitude = self.create_publisher(String, "/saltybot/attitude", 10)
self._pub_balance = self.create_publisher(String, "/saltybot/balance_state", 10)
self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10) self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
self._pub_vesc_left = self.create_publisher( self._pub_vesc_left = self.create_publisher(Float32MultiArray,"/can/vesc/left/state", 10)
Float32MultiArray, "/can/vesc/left/state", 10 self._pub_vesc_right= self.create_publisher(Float32MultiArray,"/can/vesc/right/state", 10)
) self._pub_status = self.create_publisher(String, "/can/connection_status", 10)
self._pub_vesc_right = self.create_publisher(
Float32MultiArray, "/can/vesc/right/state", 10
)
self._pub_status = self.create_publisher(
String, "/can/connection_status", 10
)
# ── Subscriptions ───────────────────────────────────────────────── # ── Subscriptions ─────────────────────────────────────────────────
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10) self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
self.create_subscription(Bool, "/estop", self._estop_cb, 10) self.create_subscription(Bool, "/estop", self._estop_cb, 10)
self.create_subscription(Bool, "/saltybot/arm", self._arm_cb, 10)
# ── Timers ──────────────────────────────────────────────────────── # ── Timers ────────────────────────────────────────────────────────
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb) self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
@ -124,21 +122,17 @@ class CanBridgeNode(Node):
self.get_logger().info( self.get_logger().info(
f"can_bridge_node ready — iface={self._iface} " f"can_bridge_node ready — iface={self._iface} "
f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} " f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}"
) )
# ── Connection management ────────────────────────────────────────────── # ── Connection management ──────────────────────────────────────────────
def _try_connect(self) -> None: def _try_connect(self) -> None:
"""Attempt to open the CAN interface; silently skip if already connected."""
with self._lock: with self._lock:
if self._connected: if self._connected:
return return
try: try:
bus = can.interface.Bus( self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
channel=self._iface,
bustype="socketcan",
)
self._bus = bus
self._connected = True self._connected = True
self.get_logger().info(f"CAN bus connected: {self._iface}") self.get_logger().info(f"CAN bus connected: {self._iface}")
self._publish_status("connected") self._publish_status("connected")
@ -151,12 +145,10 @@ class CanBridgeNode(Node):
self._publish_status("disconnected") self._publish_status("disconnected")
def _reconnect_cb(self) -> None: def _reconnect_cb(self) -> None:
"""Periodic timer: try to reconnect when disconnected."""
if not self._connected: if not self._connected:
self._try_connect() self._try_connect()
def _handle_can_error(self, exc: Exception, context: str) -> None: def _handle_can_error(self, exc: Exception, context: str) -> None:
"""Mark bus as disconnected on any CAN error."""
self.get_logger().warning(f"CAN error in {context}: {exc}") self.get_logger().warning(f"CAN error in {context}: {exc}")
with self._lock: with self._lock:
if self._bus is not None: if self._bus is not None:
@ -171,59 +163,51 @@ class CanBridgeNode(Node):
# ── ROS callbacks ───────────────────────────────────────────────────── # ── ROS callbacks ─────────────────────────────────────────────────────
def _cmd_vel_cb(self, msg: Twist) -> None: def _cmd_vel_cb(self, msg: Twist) -> None:
"""Convert /cmd_vel Twist to VESC speed commands over CAN.""" """Convert /cmd_vel Twist to ORIN_CMD_DRIVE over CAN."""
self._last_cmd_time = time.monotonic() self._last_cmd_time = time.monotonic()
if not self._connected: if not self._connected:
return return
speed = int(max(-1000.0, min(1000.0, msg.linear.x * self._speed_scale)))
# Differential drive decomposition — individual wheel speeds in m/s. steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale)))
# The VESC nodes interpret linear velocity directly; angular is handled self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel")
# by the sign difference between left and right.
linear = msg.linear.x
angular = msg.angular.z
# Differential drive decomposition (positive angular = CCW = left turn).
left_mps = linear - angular
right_mps = linear + angular
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(left_mps, right_mps), "cmd_vel")
self._send_can(ORIN_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 ESP32 BALANCE over CAN.""" """Forward /estop to ESP32 BALANCE over CAN."""
if not self._connected: if not self._connected:
return return
self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(stop=msg.data), "estop")
if msg.data: if msg.data:
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode") self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop")
self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE") self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE")
else:
# Clear estop: send DISARM then re-ARM (let operator decide to re-arm)
self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "estop_clear")
def _arm_cb(self, msg: Bool) -> None:
"""Forward /saltybot/arm to ORIN_CMD_ARM."""
if not self._connected:
return
self._send_can(ORIN_CMD_ARM, encode_arm_cmd(msg.data), "arm")
self.get_logger().info(f"ARM command: {'ARM' if msg.data else 'DISARM'}")
# ── Watchdog ────────────────────────────────────────────────────────── # ── Watchdog ──────────────────────────────────────────────────────────
def _watchdog_cb(self) -> None: def _watchdog_cb(self) -> None:
"""If no /cmd_vel arrives within the timeout, send zero velocity.""" """If /cmd_vel is silent for command_timeout_s, send zero DRIVE (acts as keepalive)."""
if not self._connected: if not self._connected:
return return
elapsed = time.monotonic() - self._last_cmd_time if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
if elapsed > self._cmd_timeout: self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "watchdog zero-vel")
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle")
# ── CAN send helper ─────────────────────────────────────────────────── # ── CAN send helper ───────────────────────────────────────────────────
def _send_can(self, arb_id: int, data: bytes, context: str) -> None: def _send_can(self, arb_id: int, data: bytes, context: str,
"""Send a standard CAN frame; handle errors gracefully.""" extended: bool = False) -> None:
with self._lock: with self._lock:
if not self._connected or self._bus is None: if not self._connected or self._bus is None:
return return
bus = self._bus bus = self._bus
msg = can.Message(arbitration_id=arb_id, data=data,
msg = can.Message( is_extended_id=extended)
arbitration_id=arb_id,
data=data,
is_extended_id=False,
)
try: try:
bus.send(msg, timeout=0.05) bus.send(msg, timeout=0.05)
except can.CanError as exc: except can.CanError as exc:
@ -232,59 +216,41 @@ class CanBridgeNode(Node):
# ── Background CAN reader ───────────────────────────────────────────── # ── Background CAN reader ─────────────────────────────────────────────
def _reader_loop(self) -> None: def _reader_loop(self) -> None:
"""
Blocking CAN read loop executed in a daemon thread.
Dispatches incoming frames to the appropriate handler.
"""
while rclpy.ok(): while rclpy.ok():
with self._lock: with self._lock:
connected = self._connected connected, bus = self._connected, self._bus
bus = self._bus
if not connected or bus is None: if not connected or bus is None:
time.sleep(0.1) time.sleep(0.1)
continue continue
try: try:
frame = bus.recv(timeout=0.5) frame = bus.recv(timeout=0.5)
except can.CanError as exc: except can.CanError as exc:
self._handle_can_error(exc, "reader_loop recv") self._handle_can_error(exc, "reader_loop recv")
continue continue
if frame is None: if frame is None:
# Timeout — no frame within 0.5 s, loop again
continue continue
self._dispatch_frame(frame) self._dispatch_frame(frame)
def _dispatch_frame(self, frame: can.Message) -> None: def _dispatch_frame(self, frame: can.Message) -> None:
"""Route an incoming CAN frame to the correct publisher."""
arb_id = frame.arbitration_id arb_id = frame.arbitration_id
data = bytes(frame.data) data = bytes(frame.data)
vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
# VESC STATUS_1 CAN IDs: (VESC_STATUS_1 << 8) | node_id vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
_vesc_left_status1 = (VESC_STATUS_1 << 8) | self._left_vesc_id
_vesc_right_status1 = (VESC_STATUS_1 << 8) | self._right_vesc_id
try: try:
if arb_id == FC_STATUS: if arb_id == ESP32_TELEM_ATTITUDE:
self._handle_fc_status(data) self._handle_attitude(data)
elif arb_id == ESP32_TELEM_BATTERY:
elif arb_id == FC_VESC: self._handle_battery(data)
self._handle_fc_vesc(data) elif arb_id == vesc_l:
t = decode_vesc_status1(self._left_vesc_id, data)
elif arb_id == _vesc_left_status1: m = Float32MultiArray()
telem = decode_vesc_status1(self._left_vesc_id, data) m.data = [t.erpm, t.duty, 0.0, t.current]
msg = Float32MultiArray() self._pub_vesc_left.publish(m)
msg.data = [telem.erpm, telem.duty, 0.0, telem.current] elif arb_id == vesc_r:
self._pub_vesc_left.publish(msg) t = decode_vesc_status1(self._right_vesc_id, data)
m = Float32MultiArray()
elif arb_id == _vesc_right_status1: m.data = [t.erpm, t.duty, 0.0, t.current]
telem = decode_vesc_status1(self._right_vesc_id, data) self._pub_vesc_right.publish(m)
msg = Float32MultiArray()
msg.data = [telem.erpm, telem.duty, 0.0, telem.current]
self._pub_vesc_right.publish(msg)
except Exception as exc: except Exception as exc:
self.get_logger().warning( self.get_logger().warning(
f"Error parsing CAN frame 0x{arb_id:03X}: {exc}" f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
@ -292,38 +258,35 @@ class CanBridgeNode(Node):
# ── Frame handlers ──────────────────────────────────────────────────── # ── Frame handlers ────────────────────────────────────────────────────
def _handle_fc_status(self, data: bytes) -> None: _STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
"""FC_STATUS (0x400): pitch, motor_cmd, vbat_mv, state, flags."""
telem = decode_fc_status(data)
# Publish pitch as IMU (orientation only — yaw/roll unknown from FC_STATUS) def _handle_attitude(self, data: bytes) -> None:
imu_msg = Imu() """ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
imu_msg.header.stamp = self.get_clock().now().to_msg() t = decode_attitude(data)
imu_msg.header.frame_id = "imu_link" now = self.get_clock().now().to_msg()
# Only pitch is available; publish as angular velocity placeholder payload = {
imu_msg.angular_velocity.y = telem.pitch_deg # degrees, not rad/s "pitch_deg": round(t.pitch_deg, 2),
imu_msg.orientation_covariance[0] = -1.0 # covariance unknown "speed_mps": round(t.speed, 3),
self._pub_imu.publish(imu_msg) "yaw_rate": round(t.yaw_rate, 3),
"state": t.state,
"state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"),
"flags": t.flags,
"ts": f"{now.sec}.{now.nanosec:09d}",
}
msg = String()
msg.data = json.dumps(payload)
self._pub_attitude.publish(msg)
self._pub_balance.publish(msg) # keep /saltybot/balance_state alive
# Publish battery (vbat_mv → volts) def _handle_battery(self, data: bytes) -> None:
bat_msg = BatteryState() """BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
bat_msg.header.stamp = imu_msg.header.stamp t = decode_battery(data)
bat_msg.voltage = telem.vbat_mv / 1000.0 msg = BatteryState()
bat_msg.present = True msg.header.stamp = self.get_clock().now().to_msg()
bat_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING msg.voltage = t.vbat_mv / 1000.0
self._pub_battery.publish(bat_msg) msg.present = True
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
def _handle_fc_vesc(self, data: bytes) -> None: self._pub_battery.publish(msg)
"""FC_VESC (0x401): left/right RPM and current aggregated by BALANCE."""
telem = decode_fc_vesc(data)
left_msg = Float32MultiArray()
left_msg.data = [telem.left_rpm, 0.0, 0.0, telem.left_cur]
self._pub_vesc_left.publish(left_msg)
right_msg = Float32MultiArray()
right_msg.data = [telem.right_rpm, 0.0, 0.0, telem.right_cur]
self._pub_vesc_right.publish(right_msg)
# ── Status helper ───────────────────────────────────────────────────── # ── Status helper ─────────────────────────────────────────────────────
@ -335,11 +298,10 @@ class CanBridgeNode(Node):
# ── Shutdown ────────────────────────────────────────────────────────── # ── Shutdown ──────────────────────────────────────────────────────────
def destroy_node(self) -> None: def destroy_node(self) -> None:
"""Send zero velocity and shut down the CAN bus cleanly."""
if self._connected and self._bus is not None: if self._connected and self._bus is not None:
try: try:
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "shutdown") self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown") self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "shutdown")
except Exception: except Exception:
pass pass
try: try:
@ -349,8 +311,6 @@ class CanBridgeNode(Node):
super().destroy_node() super().destroy_node()
# ---------------------------------------------------------------------------
def main(args=None) -> None: def main(args=None) -> None:
rclpy.init(args=args) rclpy.init(args=args)
node = CanBridgeNode() node = CanBridgeNode()

242
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py
View File

@ -1,31 +1,29 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
"""mamba_protocol.py — CAN frame codec for SAUL-TEE ESP32 BALANCE + VESC. """mamba_protocol.py — CAN frame codec for Orin ↔ ESP32-S3 BALANCE.
Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 & §8 (2026-04-04)
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
File name retained for import compatibility. File name retained for import compatibility.
CAN bus: 500 kbps, CANable 2.0 (slcan0) on Orin. CAN bus: 500 kbps, standard 11-bit IDs, CANable 2.0 (slcan0 / can0) on Orin.
── Orin → ESP32 BALANCE (commands) ─────────────────────────────────────────── ── Orin → ESP32 BALANCE (commands) ───────────────────────────────────────────
0x300 DRIVE 8 B [left_mps f32 LE][right_mps f32 LE] velocity setpoint 0x300 DRIVE 8 B [speed:i16 BE][steer:i16 BE][mode:u8][flags:u8][_:u16]
0x301 MODE 1 B [mode u8] 0=idle 1=drive 2=estop 0x301 ARM 1 B [arm:u8] 0x00=DISARM 0x01=ARM
0x302 ESTOP 1 B [flags u8] bit0=stop bit1=clear 0x302 PID_SET 8 B [kp:f16 BE][ki:f16 BE][kd:f16 BE][_:u16]
0x303 LED 4 B [pattern u8][r u8][g u8][b u8] 0x303 ESTOP 1 B [0xE5] magic byte — cuts all motors immediately
── ESP32 BALANCE → Orin (telemetry) ────────────────────────────────────────── ── ESP32 BALANCE → Orin (telemetry) ──────────────────────────────────────────
0x400 FC_STATUS 8 B [pitch_x10 i16][motor_cmd i16][vbat_mv u16][state u8][flags u8] 10 Hz 0x400 ATTITUDE 8 B [pitch:f16 BE][speed:f16 BE][yaw_rate:f16 BE][state:u8][flags:u8]
0x401 FC_VESC 8 B [l_rpm_x10 i16][r_rpm_x10 i16][l_cur_x10 i16][r_cur_x10 i16] 10 Hz 0x401 BATTERY 4 B [vbat_mv:u16 BE][fault_code:u8][rssi:i8]
── VESC standard CAN frames (29-bit extended IDs) ──────────────────────────── speed/steer range: −1000..+1000 (motor units). f16 = IEEE 754 half-precision.
VESC node IDs: Left = 56, Right = 68
ID = (packet_type << 8) | node_id
SET_RPM cmd=3 payload: rpm i32 big-endian VESC standard extended CAN (29-bit IDs = packet_type<<8 | node_id):
STATUS cmd=9 payload: erpm i32, current i16, duty i16 Left VESC node ID = 56 (0x38)
STATUS_4 cmd=16 payload: temp_fet×10 i16, temp_motor×10 i16, Right VESC node ID = 68 (0x44)
current_in×10 i16, pid_pos×50 i16 STATUS_1 cmd=9 erpm i32 BE, current i16 (/10 A), duty i16 (/1000)
STATUS_5 cmd=27 payload: tacho i32, vbat×10 i16 STATUS_4 cmd=16 temp_fet i16 (/10 °C), temp_mot i16 (/10 °C), cur_in i16 (/10 A)
STATUS_5 cmd=27 tacho i32, vbat i16 (/10 V)
""" """
import struct import struct
@ -35,60 +33,76 @@ from dataclasses import dataclass
# Orin → ESP32 BALANCE # Orin → ESP32 BALANCE
ORIN_CMD_DRIVE: int = 0x300 ORIN_CMD_DRIVE: int = 0x300
ORIN_CMD_MODE: int = 0x301 ORIN_CMD_ARM: int = 0x301
ORIN_CMD_ESTOP: int = 0x302 ORIN_CMD_PID: int = 0x302
ORIN_CMD_LED: int = 0x303 ORIN_CMD_ESTOP: int = 0x303
# ESP32 BALANCE → Orin # ESP32 BALANCE → Orin
FC_STATUS: int = 0x400 ESP32_TELEM_ATTITUDE: int = 0x400
FC_VESC: int = 0x401 ESP32_TELEM_BATTERY: int = 0x401
# VESC node IDs (per docs/SAUL-TEE-SYSTEM-REFERENCE.md §8) # Backward-compat aliases used by other nodes
FC_STATUS: int = ESP32_TELEM_ATTITUDE
FC_VESC: int = ESP32_TELEM_BATTERY
# VESC node IDs
VESC_LEFT_ID: int = 56 VESC_LEFT_ID: int = 56
VESC_RIGHT_ID: int = 68 VESC_RIGHT_ID: int = 68
# VESC CAN packet types (standard VESC protocol) # VESC packet types
VESC_CMD_SET_RPM: int = 3
VESC_STATUS_1: int = 9 VESC_STATUS_1: int = 9
VESC_STATUS_4: int = 16 VESC_STATUS_4: int = 16
VESC_STATUS_5: int = 27 VESC_STATUS_5: int = 27
# ── Mode constants ──────────────────────────────────────────────────────────── # ── Mode constants (DRIVE frame mode byte) ─────────────────────────────────────
MODE_IDLE: int = 0 # RC passthrough, Orin not injecting
MODE_DRIVE: int = 1 # Orin velocity commands
MODE_AUTONOMOUS: int = 2 # full autonomy
MODE_ESTOP: int = 2 # alias
# ESTOP magic byte
_ESTOP_MAGIC: int = 0xE5
# ── Struct formats (big-endian) ────────────────────────────────────────────────
_FMT_DRIVE = ">hhBBH" # i16 speed, i16 steer, u8 mode, u8 flags, u16 pad
_FMT_PID = ">eeeH" # f16 kp, f16 ki, f16 kd, u16 pad
_FMT_ATTITUDE = ">eeeBB" # f16 pitch, f16 speed, f16 yaw_rate, u8 state, u8 flags
_FMT_BATTERY = ">HBb" # u16 vbat_mv, u8 fault_code, i8 rssi
MODE_IDLE: int = 0
MODE_DRIVE: int = 1
MODE_ESTOP: int = 2
# ── Data classes ────────────────────────────────────────────────────────────── # ── Data classes ──────────────────────────────────────────────────────────────
@dataclass @dataclass
class DriveCmd: class AttitudeTelemetry:
left_mps: float = 0.0 # m/s, positive = forward """Decoded ATTITUDE (0x400) from ESP32 BALANCE."""
right_mps: float = 0.0 pitch_deg: float = 0.0 # degrees, half-float
speed: float = 0.0 # m/s, half-float
yaw_rate: float = 0.0 # rad/s, half-float
@dataclass state: int = 0 # 0=IDLE 1=RUNNING 2=FAULT
class FcStatus:
"""Decoded FC_STATUS (0x400) telemetry from ESP32 BALANCE."""
pitch_deg: float = 0.0 # pitch_x10 / 10.0
motor_cmd: int = 0 # raw motor command output
vbat_mv: int = 0 # battery voltage in mV
state: int = 0 # 0=idle 1=running 2=fault
flags: int = 0 # error bitmask flags: int = 0 # error bitmask
@dataclass @dataclass
class FcVesc: class BatteryTelemetry:
"""Decoded FC_VESC (0x401) VESC aggregate telemetry.""" """Decoded BATTERY (0x401) from ESP32 BALANCE."""
left_rpm: float = 0.0 # left_rpm_x10 / 10.0 vbat_mv: int = 0 # millivolts
right_rpm: float = 0.0 fault_code: int = 0 # 0 = OK
left_cur: float = 0.0 # left_cur_x10 / 10.0 (amps) rssi: int = 0 # RC signal dBm (signed)
right_cur: float = 0.0
@dataclass
class PidGains:
"""Balance PID gains."""
kp: float = 0.0
ki: float = 0.0
kd: float = 0.0
@dataclass @dataclass
class VescStatus1: class VescStatus1:
"""Decoded VESC STATUS (cmd=9) frame.""" """Decoded VESC STATUS (cmd=9) — direct from VESC."""
node_id: int = 0 node_id: int = 0
erpm: float = 0.0 erpm: float = 0.0
current: float = 0.0 # A current: float = 0.0 # A
@ -97,7 +111,7 @@ class VescStatus1:
@dataclass @dataclass
class VescStatus4: class VescStatus4:
"""Decoded VESC STATUS_4 (cmd=16) frame.""" """Decoded VESC STATUS_4 (cmd=16)."""
node_id: int = 0 node_id: int = 0
temp_fet_c: float = 0.0 temp_fet_c: float = 0.0
temp_motor_c: float = 0.0 temp_motor_c: float = 0.0
@ -106,112 +120,98 @@ class VescStatus4:
@dataclass @dataclass
class VescStatus5: class VescStatus5:
"""Decoded VESC STATUS_5 (cmd=27) frame.""" """Decoded VESC STATUS_5 (cmd=27)."""
node_id: int = 0 node_id: int = 0
tacho: int = 0 tacho: int = 0
vbat_v: float = 0.0 vbat_v: float = 0.0
@dataclass
class PidGains:
"""Balance PID gains (Issue #693)."""
kp: float = 0.0
ki: float = 0.0
kd: float = 0.0
# ── Orin → BALANCE encoders ─────────────────────────────────────────────────── # ── Orin → BALANCE encoders ───────────────────────────────────────────────────
def encode_drive_cmd(left_mps: float, right_mps: float) -> bytes: def encode_drive_cmd(speed: int, steer: int,
"""Encode ORIN_CMD_DRIVE (0x300) — 8 bytes, float32 LE.""" mode: int = MODE_DRIVE, flags: int = 0) -> bytes:
return struct.pack("<ff", float(left_mps), float(right_mps)) """Encode ORIN_CMD_DRIVE (0x300) — 8 bytes.
speed: −1000..+1000 motor units (positive = forward)
steer: −1000..+1000 motor units (positive = right)
mode: MODE_IDLE / MODE_DRIVE / MODE_AUTONOMOUS
"""
speed = max(-1000, min(1000, int(speed)))
steer = max(-1000, min(1000, int(steer)))
return struct.pack(_FMT_DRIVE, speed, steer, mode & 0xFF, flags & 0xFF, 0)
def encode_mode_cmd(mode: int) -> bytes: def encode_arm_cmd(arm: bool) -> bytes:
"""Encode ORIN_CMD_MODE (0x301) — 1 byte.""" """Encode ORIN_CMD_ARM (0x301) — 1 byte."""
if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): return struct.pack("B", 0x01 if arm else 0x00)
raise ValueError(f"Invalid mode {mode!r}")
return struct.pack("B", mode)
def encode_estop_cmd(stop: bool = True, clear: bool = False) -> bytes: def encode_pid_cmd(kp: float, ki: float, kd: float) -> bytes:
"""Encode ORIN_CMD_ESTOP (0x302) — 1 byte flags (bit0=stop, bit1=clear).""" """Encode ORIN_CMD_PID (0x302) — 8 bytes (3× half-float + 2-byte pad)."""
flags = (0x01 if stop else 0) | (0x02 if clear else 0) return struct.pack(_FMT_PID, float(kp), float(ki), float(kd), 0)
return struct.pack("B", flags)
def encode_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes: def encode_estop_cmd() -> bytes:
"""Encode ORIN_CMD_LED (0x303) — 4 bytes.""" """Encode ORIN_CMD_ESTOP (0x303) — 1 byte magic 0xE5."""
return struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF) return struct.pack("B", _ESTOP_MAGIC)
# ── VESC encoders ─────────────────────────────────────────────────────────────
def encode_vesc_set_rpm(node_id: int, rpm: int) -> tuple[int, bytes]:
"""Encode VESC SET_RPM command. Returns (can_id, payload)."""
can_id = (VESC_CMD_SET_RPM << 8) | (node_id & 0xFF)
return can_id, struct.pack(">i", int(rpm))
# ── BALANCE → Orin decoders ─────────────────────────────────────────────────── # ── BALANCE → Orin decoders ───────────────────────────────────────────────────
def decode_fc_status(data: bytes) -> FcStatus: def decode_attitude(data: bytes) -> AttitudeTelemetry:
"""Decode FC_STATUS (0x400) — 8 bytes.""" """Decode ATTITUDE (0x400) — 8 bytes."""
if len(data) < 8: if len(data) < 8:
raise ValueError(f"FC_STATUS expects 8 bytes, got {len(data)}") raise ValueError(f"ATTITUDE expects ≥8 bytes, got {len(data)}")
pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hHHBB", data) pitch, speed, yaw_rate, state, flags = struct.unpack_from(_FMT_ATTITUDE, data)
return FcStatus( return AttitudeTelemetry(pitch_deg=pitch, speed=speed, yaw_rate=yaw_rate,
pitch_deg=pitch_x10 / 10.0, state=state, flags=flags)
motor_cmd=motor_cmd,
vbat_mv=vbat_mv,
state=state,
flags=flags,
)
def decode_fc_vesc(data: bytes) -> FcVesc: def decode_battery(data: bytes) -> BatteryTelemetry:
"""Decode FC_VESC (0x401) — 8 bytes.""" """Decode BATTERY (0x401) — 4 bytes."""
if len(data) < 8: if len(data) < 4:
raise ValueError(f"FC_VESC expects 8 bytes, got {len(data)}") raise ValueError(f"BATTERY expects ≥4 bytes, got {len(data)}")
l_rpm, r_rpm, l_cur, r_cur = struct.unpack_from(">hhhh", data) vbat, fault, rssi = struct.unpack_from(_FMT_BATTERY, data)
return FcVesc( return BatteryTelemetry(vbat_mv=vbat, fault_code=fault, rssi=rssi)
left_rpm=l_rpm / 10.0,
right_rpm=r_rpm / 10.0,
left_cur=l_cur / 10.0,
right_cur=r_cur / 10.0,
)
# ── VESC telemetry decoders ─────────────────────────────────────────────────── # Backward-compat aliases
def decode_fc_status(data: bytes) -> AttitudeTelemetry:
return decode_attitude(data)
def decode_vesc_can_id(can_id: int) -> tuple[int, int]:
def decode_fc_vesc(data: bytes) -> BatteryTelemetry:
return decode_battery(data)
# ── VESC CAN helpers ─────────────────────────────────────────────────────────
def decode_vesc_can_id(can_id: int) -> tuple:
"""Split a VESC extended CAN ID into (packet_type, node_id).""" """Split a VESC extended CAN ID into (packet_type, node_id)."""
return (can_id >> 8) & 0xFF, can_id & 0xFF return (can_id >> 8) & 0xFF, can_id & 0xFF
def decode_vesc_status1(node_id: int, data: bytes) -> VescStatus1: def decode_vesc_status1(node_id: int, data: bytes) -> VescStatus1:
"""Decode VESC STATUS (cmd=9) — erpm i32, current i16, duty i16.""" """Decode VESC STATUS (cmd=9): erpm i32, current i16(/10), duty i16(/1000)."""
erpm, cur_x10, duty_x1000 = struct.unpack_from(">ihh", data[:8]) erpm, cur_x10, duty_x1000 = struct.unpack_from(">ihh", data[:8])
return VescStatus1( return VescStatus1(node_id=node_id, erpm=float(erpm),
node_id=node_id, current=cur_x10 / 10.0, duty=duty_x1000 / 1000.0)
erpm=float(erpm),
current=cur_x10 / 10.0,
duty=duty_x1000 / 1000.0,
)
def decode_vesc_status4(node_id: int, data: bytes) -> VescStatus4: def decode_vesc_status4(node_id: int, data: bytes) -> VescStatus4:
"""Decode VESC STATUS_4 (cmd=16) — temp_fet×10, temp_motor×10, current_in×10.""" """Decode VESC STATUS_4 (cmd=16): temp_fet, temp_mot, cur_in (all /10)."""
tfet, tmot, cur_in, _ = struct.unpack_from(">hhhh", data[:8]) tfet, tmot, cur_in, _ = struct.unpack_from(">hhhh", data[:8])
return VescStatus4( return VescStatus4(node_id=node_id, temp_fet_c=tfet / 10.0,
node_id=node_id, temp_motor_c=tmot / 10.0, current_in=cur_in / 10.0)
temp_fet_c=tfet / 10.0,
temp_motor_c=tmot / 10.0,
current_in=cur_in / 10.0,
)
def decode_vesc_status5(node_id: int, data: bytes) -> VescStatus5: def decode_vesc_status5(node_id: int, data: bytes) -> VescStatus5:
"""Decode VESC STATUS_5 (cmd=27) — tacho i32, vbat×10 i16.""" """Decode VESC STATUS_5 (cmd=27): tacho i32, vbat i16 (/10 V)."""
tacho, vbat_x10 = struct.unpack_from(">ih", data[:6]) tacho, vbat_x10 = struct.unpack_from(">ih", data[:6])
return VescStatus5(node_id=node_id, tacho=tacho, vbat_v=vbat_x10 / 10.0) return VescStatus5(node_id=node_id, tacho=tacho, vbat_v=vbat_x10 / 10.0)
def encode_vesc_set_rpm(node_id: int, rpm: int) -> tuple:
"""Encode VESC SET_RPM command. Returns (extended_can_id, payload)."""
can_id = (3 << 8) | (node_id & 0xFF)
return can_id, struct.pack(">i", int(rpm))