feat(arch): align CAN/UART bridges with SAUL-TEE-SYSTEM-REFERENCE.md spec
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:
parent
cfd5a15b3e
commit
f66035cf81
@ -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` |
|
||||
|
||||
|
||||
## FC UART Summary (MAMBA F722S)
|
||||
## FC UART Summary (MAMBA F722S — OBSOLETE)
|
||||
|
||||
| UART | Pins | Baud | Assignment | Notes |
|
||||
|------|------|------|------------|-------|
|
||||
|
||||
@ -1,30 +1,16 @@
|
||||
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
|
||||
# Binary-framed Jetson↔STM32 bridge at 921600 baud.
|
||||
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge)
|
||||
# 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 ────────────────────────────────────────────────────────────────
|
||||
# Use /dev/stm32-bridge if the udev rule is applied:
|
||||
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
|
||||
# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
|
||||
serial_port: /dev/ttyACM0
|
||||
baud_rate: 921600
|
||||
reconnect_delay: 2.0 # seconds between USB reconnect attempts
|
||||
# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md).
|
||||
# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed.
|
||||
serial_port: /dev/esp32-io
|
||||
baud_rate: 460800
|
||||
reconnect_delay: 2.0 # seconds between reconnect attempts
|
||||
|
||||
# ── Heartbeat ─────────────────────────────────────────────────────────────────
|
||||
# HEARTBEAT frame sent every heartbeat_period seconds.
|
||||
# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
|
||||
heartbeat_period: 0.2 # 200ms → well within 500ms STM32 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
|
||||
# HEARTBEAT (0x20) sent every heartbeat_period.
|
||||
# ESP32 IO watchdog fires if no heartbeat for ~500 ms.
|
||||
heartbeat_period: 0.2 # 200 ms → well within 500 ms watchdog
|
||||
|
||||
@ -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:
|
||||
# Default (binary protocol, bidirectional):
|
||||
ros2 launch saltybot_bridge stm32_cmd.launch.py
|
||||
|
||||
# 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
|
||||
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
|
||||
"""
|
||||
|
||||
import os
|
||||
@ -24,11 +24,8 @@ def generate_launch_description() -> LaunchDescription:
|
||||
params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
||||
DeclareLaunchArgument("baud_rate", default_value="921600"),
|
||||
DeclareLaunchArgument("speed_scale", default_value="1000.0"),
|
||||
DeclareLaunchArgument("steer_scale", default_value="-500.0"),
|
||||
DeclareLaunchArgument("watchdog_timeout", default_value="0.5"),
|
||||
DeclareLaunchArgument("serial_port", default_value="/dev/esp32-io"),
|
||||
DeclareLaunchArgument("baud_rate", default_value="460800"),
|
||||
DeclareLaunchArgument("heartbeat_period", default_value="0.2"),
|
||||
|
||||
Node(
|
||||
@ -42,9 +39,6 @@ def generate_launch_description() -> LaunchDescription:
|
||||
{
|
||||
"serial_port": LaunchConfiguration("serial_port"),
|
||||
"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"),
|
||||
},
|
||||
],
|
||||
|
||||
@ -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
|
||||
framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
|
||||
Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
|
||||
inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).
|
||||
|
||||
TX commands (Jetson → STM32):
|
||||
SPEED_STEER — 50 Hz from /cmd_vel subscription
|
||||
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
|
||||
This node is NOT the primary drive path (that is CAN via can_bridge_node).
|
||||
It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.
|
||||
|
||||
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):
|
||||
IMU → /saltybot/imu (sensor_msgs/Imu)
|
||||
BATTERY → /saltybot/telemetry/battery (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)
|
||||
RX from ESP32 IO:
|
||||
RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON)
|
||||
SENSORS (0x02) → /saltybot/sensors (std_msgs/String JSON)
|
||||
|
||||
Auto-reconnect: USB disconnect is detected when serial.read() raises; node
|
||||
continuously retries at reconnect_delay interval.
|
||||
|
||||
This node owns /dev/ttyACM0 exclusively — do NOT run alongside
|
||||
serial_bridge_node or saltybot_cmd_node on the same port.
|
||||
TX to ESP32 IO:
|
||||
LED_CMD (0x10) ← /saltybot/leds (std_msgs/String JSON)
|
||||
OUTPUT_CMD (0x11) ← /saltybot/outputs (std_msgs/String JSON)
|
||||
HEARTBEAT (0x20) — sent every heartbeat_period (keep IO watchdog alive)
|
||||
|
||||
Parameters (config/stm32_cmd_params.yaml):
|
||||
serial_port /dev/ttyACM0
|
||||
baud_rate 921600
|
||||
reconnect_delay 2.0 (seconds)
|
||||
heartbeat_period 0.2 (seconds)
|
||||
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)
|
||||
serial_port /dev/esp32-io
|
||||
baud_rate 460800
|
||||
reconnect_delay 2.0
|
||||
heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
|
||||
@ -50,119 +37,69 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
import serial
|
||||
|
||||
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_srvs.srv import SetBool, Trigger
|
||||
|
||||
from .stm32_protocol import (
|
||||
BAUD_RATE,
|
||||
FrameParser,
|
||||
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
|
||||
encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
|
||||
encode_pid_update,
|
||||
RcChannels,
|
||||
SensorData,
|
||||
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):
|
||||
"""Binary-framed Jetson↔STM32 bridge node."""
|
||||
"""Orin ↔ ESP32-S3 IO auxiliary bridge node."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("stm32_cmd_node")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/ttyACM0")
|
||||
self.declare_parameter("baud_rate", 921600)
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/esp32-io")
|
||||
self.declare_parameter("baud_rate", BAUD_RATE)
|
||||
self.declare_parameter("reconnect_delay", 2.0)
|
||||
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
|
||||
baud = self.get_parameter("baud_rate").value
|
||||
self._port_name = self.get_parameter("serial_port").value
|
||||
self._baud = self.get_parameter("baud_rate").value
|
||||
self._reconnect_delay = self.get_parameter("reconnect_delay").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 ───────────────────────────────────────────────────────────────
|
||||
sensor_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST, depth=10,
|
||||
)
|
||||
# ── QoS ───────────────────────────────────────────────────────────
|
||||
rel_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST, depth=10,
|
||||
)
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────────
|
||||
self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos)
|
||||
self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", 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)
|
||||
# ── Publishers ────────────────────────────────────────────────────
|
||||
self._rc_pub = self.create_publisher(String, "/saltybot/rc_channels", rel_qos)
|
||||
self._sens_pub = self.create_publisher(String, "/saltybot/sensors", rel_qos)
|
||||
self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos)
|
||||
|
||||
# ── Subscribers ───────────────────────────────────────────────────────
|
||||
self._cmd_vel_sub = self.create_subscription(
|
||||
Twist, "/cmd_vel", self._on_cmd_vel, rel_qos,
|
||||
)
|
||||
self._pid_sub = self.create_subscription(
|
||||
String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
|
||||
)
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(String, "/saltybot/leds", self._on_leds, rel_qos)
|
||||
self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)
|
||||
|
||||
# ── Services ──────────────────────────────────────────────────────────
|
||||
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
|
||||
# ── Serial state ──────────────────────────────────────────────────
|
||||
self._ser: serial.Serial | None = None
|
||||
self._ser_lock = threading.Lock()
|
||||
self._parser = FrameParser()
|
||||
self._rx_count = 0
|
||||
|
||||
# ── TX state ──────────────────────────────────────────────────────────
|
||||
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 ──────────────────────────────────────
|
||||
# ── Open serial and start timers ──────────────────────────────────
|
||||
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)
|
||||
# Heartbeat TX
|
||||
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.get_logger().info(
|
||||
f"stm32_cmd_node started — {port} @ {baud} baud | "
|
||||
f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
|
||||
f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
|
||||
)
|
||||
|
||||
# ── Serial management ─────────────────────────────────────────────────────
|
||||
# ── Serial management ─────────────────────────────────────────────────
|
||||
|
||||
def _open_serial(self) -> bool:
|
||||
with self._ser_lock:
|
||||
@ -170,7 +107,7 @@ class Stm32CmdNode(Node):
|
||||
self._ser = serial.Serial(
|
||||
port=self._port_name,
|
||||
baudrate=self._baud,
|
||||
timeout=0.005, # non-blocking reads
|
||||
timeout=0.005,
|
||||
write_timeout=0.1,
|
||||
)
|
||||
self._ser.reset_input_buffer()
|
||||
@ -185,17 +122,7 @@ class Stm32CmdNode(Node):
|
||||
self._ser = None
|
||||
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:
|
||||
"""Thread-safe serial write. Returns False if port is not open."""
|
||||
with self._ser_lock:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
return False
|
||||
@ -207,16 +134,15 @@ class Stm32CmdNode(Node):
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
# ── RX — read callback ────────────────────────────────────────────────────
|
||||
# ── RX ────────────────────────────────────────────────────────────────
|
||||
|
||||
def _read_cb(self) -> None:
|
||||
"""Read bytes from serial and feed them to the frame parser."""
|
||||
raw: bytes | None = None
|
||||
reconnect_needed = False
|
||||
reconnect = False
|
||||
|
||||
with self._ser_lock:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
reconnect_needed = True
|
||||
reconnect = True
|
||||
else:
|
||||
try:
|
||||
n = self._ser.in_waiting
|
||||
@ -225,9 +151,9 @@ class Stm32CmdNode(Node):
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Serial read error: {exc}")
|
||||
self._ser = None
|
||||
reconnect_needed = True
|
||||
reconnect = True
|
||||
|
||||
if reconnect_needed:
|
||||
if reconnect:
|
||||
self.get_logger().warn(
|
||||
"Serial disconnected — will retry",
|
||||
throttle_duration_sec=self._reconnect_delay,
|
||||
@ -240,230 +166,105 @@ class Stm32CmdNode(Node):
|
||||
return
|
||||
|
||||
for byte in raw:
|
||||
frame = self._parser.feed(byte)
|
||||
if frame is not None:
|
||||
self._rx_frame_count += 1
|
||||
self._dispatch_frame(frame)
|
||||
msg = self._parser.feed(byte)
|
||||
if msg is not None:
|
||||
self._rx_count += 1
|
||||
self._dispatch(msg)
|
||||
|
||||
def _dispatch_frame(self, frame) -> None:
|
||||
"""Route a decoded frame to the appropriate publisher."""
|
||||
def _dispatch(self, msg) -> None:
|
||||
now = self.get_clock().now().to_msg()
|
||||
ts = f"{now.sec}.{now.nanosec:09d}"
|
||||
|
||||
if isinstance(frame, ImuFrame):
|
||||
self._publish_imu(frame, now)
|
||||
if isinstance(msg, RcChannels):
|
||||
out = String()
|
||||
out.data = json.dumps({
|
||||
"channels": msg.channels,
|
||||
"source": msg.source,
|
||||
"ts": ts,
|
||||
})
|
||||
self._rc_pub.publish(out)
|
||||
|
||||
elif isinstance(frame, BatteryFrame):
|
||||
self._publish_battery(frame, now)
|
||||
elif isinstance(msg, SensorData):
|
||||
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):
|
||||
self._publish_motor_rpm(frame, now)
|
||||
elif isinstance(msg, tuple):
|
||||
type_code, _ = msg
|
||||
self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
|
||||
|
||||
elif isinstance(frame, ArmStateFrame):
|
||||
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,
|
||||
)
|
||||
# ── TX ────────────────────────────────────────────────────────────────
|
||||
|
||||
def _heartbeat_cb(self) -> None:
|
||||
"""Send HEARTBEAT every heartbeat_period (default 200ms)."""
|
||||
self._write(encode_heartbeat())
|
||||
|
||||
def _watchdog_cb(self) -> None:
|
||||
"""Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
|
||||
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."""
|
||||
def _on_leds(self, msg: String) -> None:
|
||||
"""Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
|
||||
try:
|
||||
data = json.loads(msg.data)
|
||||
kp = float(data["kp"])
|
||||
ki = float(data["ki"])
|
||||
kd = float(data["kd"])
|
||||
except (ValueError, KeyError, json.JSONDecodeError) as exc:
|
||||
self.get_logger().error(f"Bad PID update JSON: {exc}")
|
||||
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'}"
|
||||
d = json.loads(msg.data)
|
||||
frame = encode_led_cmd(
|
||||
int(d.get("pattern", 0)),
|
||||
int(d.get("r", 0)),
|
||||
int(d.get("g", 0)),
|
||||
int(d.get("b", 0)),
|
||||
)
|
||||
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):
|
||||
"""SetBool: data maps to mode byte (True=1, False=0)."""
|
||||
mode = 1 if request.data else 0
|
||||
frame = encode_set_mode(mode)
|
||||
ok = self._write(frame)
|
||||
response.success = ok
|
||||
response.message = f"mode={mode}" if ok else "serial not open"
|
||||
return response
|
||||
def _on_outputs(self, msg: String) -> None:
|
||||
"""Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
|
||||
try:
|
||||
d = json.loads(msg.data)
|
||||
frame = encode_output_cmd(
|
||||
bool(d.get("horn", False)),
|
||||
bool(d.get("buzzer", False)),
|
||||
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:
|
||||
diag = DiagnosticArray()
|
||||
diag.header.stamp = self.get_clock().now().to_msg()
|
||||
|
||||
status = DiagnosticStatus()
|
||||
status.name = "saltybot/stm32_cmd_node"
|
||||
status.hardware_id = "stm32f722"
|
||||
|
||||
status.name = "saltybot/esp32_io_bridge"
|
||||
status.hardware_id = "esp32-s3-io"
|
||||
port_ok = self._ser is not None and self._ser.is_open
|
||||
if port_ok:
|
||||
status.level = DiagnosticStatus.OK
|
||||
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.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
|
||||
status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
|
||||
status.values = [
|
||||
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="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="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)
|
||||
self._diag_pub.publish(diag)
|
||||
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
# Send zero-speed + disarm on shutdown
|
||||
self._write(encode_speed_steer(0, 0))
|
||||
self._write(encode_arm(False))
|
||||
self._close_serial()
|
||||
self._write(encode_heartbeat(state=0))
|
||||
with self._ser_lock:
|
||||
if self._ser and self._ser.is_open:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
|
||||
@ -1,26 +1,34 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
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
|
||||
-------------
|
||||
/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
|
||||
------------
|
||||
/can/imu sensor_msgs/Imu from FC_STATUS (0x400) pitch
|
||||
/can/battery sensor_msgs/BatteryState from FC_STATUS (0x400) vbat_mv
|
||||
/can/vesc/left/state std_msgs/Float32MultiArray from FC_VESC (0x401)
|
||||
/can/vesc/right/state std_msgs/Float32MultiArray from FC_VESC (0x401)
|
||||
/saltybot/attitude std_msgs/String (JSON) pitch, speed, yaw_rate, state
|
||||
/saltybot/balance_state std_msgs/String (JSON) alias of /saltybot/attitude
|
||||
/can/battery sensor_msgs/BatteryState vbat_mv, fault, rssi
|
||||
/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"
|
||||
|
||||
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 time
|
||||
from typing import Optional
|
||||
@ -29,42 +37,32 @@ import can
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.node import Node
|
||||
|
||||
from sensor_msgs.msg import BatteryState, Imu
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||
|
||||
from saltybot_can_bridge.mamba_protocol import (
|
||||
# Orin → BALANCE command IDs
|
||||
ORIN_CMD_DRIVE,
|
||||
ORIN_CMD_MODE,
|
||||
ORIN_CMD_ARM,
|
||||
ORIN_CMD_ESTOP,
|
||||
# BALANCE → Orin telemetry IDs
|
||||
FC_STATUS,
|
||||
FC_VESC,
|
||||
# VESC node IDs
|
||||
ESP32_TELEM_ATTITUDE,
|
||||
ESP32_TELEM_BATTERY,
|
||||
VESC_LEFT_ID,
|
||||
VESC_RIGHT_ID,
|
||||
VESC_STATUS_1,
|
||||
# Mode constants
|
||||
MODE_DRIVE,
|
||||
MODE_ESTOP,
|
||||
MODE_IDLE,
|
||||
# Encoders
|
||||
encode_drive_cmd,
|
||||
encode_mode_cmd,
|
||||
encode_arm_cmd,
|
||||
encode_estop_cmd,
|
||||
encode_led_cmd,
|
||||
# Decoders
|
||||
decode_fc_status,
|
||||
decode_fc_vesc,
|
||||
decode_attitude,
|
||||
decode_battery,
|
||||
decode_vesc_status1,
|
||||
decode_vesc_can_id,
|
||||
)
|
||||
|
||||
# Reconnect attempt interval when CAN bus is lost
|
||||
_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
|
||||
|
||||
|
||||
@ -76,37 +74,37 @@ class CanBridgeNode(Node):
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────
|
||||
self.declare_parameter("can_interface", "slcan0")
|
||||
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID) # 56
|
||||
self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) # 68
|
||||
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID)
|
||||
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._iface: str = self.get_parameter("can_interface").value
|
||||
self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
|
||||
self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
|
||||
self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
|
||||
self._iface = self.get_parameter("can_interface").value
|
||||
self._left_vesc_id = self.get_parameter("left_vesc_can_id").value
|
||||
self._right_vesc_id = self.get_parameter("right_vesc_can_id").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 ─────────────────────────────────────────────────────────
|
||||
self._bus: Optional[can.BusABC] = None
|
||||
self._connected: bool = False
|
||||
self._last_cmd_time: float = time.monotonic()
|
||||
self._lock = threading.Lock() # protects _bus / _connected
|
||||
self._lock = threading.Lock()
|
||||
|
||||
# ── 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_vesc_left = self.create_publisher(
|
||||
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_left = self.create_publisher(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)
|
||||
|
||||
# ── Subscriptions ─────────────────────────────────────────────────
|
||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
||||
self.create_subscription(Bool, "/estop", self._estop_cb, 10)
|
||||
self.create_subscription(Bool, "/saltybot/arm", self._arm_cb, 10)
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────
|
||||
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
||||
@ -124,21 +122,17 @@ class CanBridgeNode(Node):
|
||||
self.get_logger().info(
|
||||
f"can_bridge_node ready — iface={self._iface} "
|
||||
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 ──────────────────────────────────────────────
|
||||
|
||||
def _try_connect(self) -> None:
|
||||
"""Attempt to open the CAN interface; silently skip if already connected."""
|
||||
with self._lock:
|
||||
if self._connected:
|
||||
return
|
||||
try:
|
||||
bus = can.interface.Bus(
|
||||
channel=self._iface,
|
||||
bustype="socketcan",
|
||||
)
|
||||
self._bus = bus
|
||||
self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
|
||||
self._connected = True
|
||||
self.get_logger().info(f"CAN bus connected: {self._iface}")
|
||||
self._publish_status("connected")
|
||||
@ -151,12 +145,10 @@ class CanBridgeNode(Node):
|
||||
self._publish_status("disconnected")
|
||||
|
||||
def _reconnect_cb(self) -> None:
|
||||
"""Periodic timer: try to reconnect when disconnected."""
|
||||
if not self._connected:
|
||||
self._try_connect()
|
||||
|
||||
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}")
|
||||
with self._lock:
|
||||
if self._bus is not None:
|
||||
@ -171,59 +163,51 @@ class CanBridgeNode(Node):
|
||||
# ── ROS callbacks ─────────────────────────────────────────────────────
|
||||
|
||||
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()
|
||||
|
||||
if not self._connected:
|
||||
return
|
||||
|
||||
# Differential drive decomposition — individual wheel speeds in m/s.
|
||||
# The VESC nodes interpret linear velocity directly; angular is handled
|
||||
# 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")
|
||||
speed = int(max(-1000.0, min(1000.0, msg.linear.x * self._speed_scale)))
|
||||
steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale)))
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel")
|
||||
|
||||
def _estop_cb(self, msg: Bool) -> None:
|
||||
"""Forward /estop to ESP32 BALANCE over CAN."""
|
||||
if not self._connected:
|
||||
return
|
||||
self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(stop=msg.data), "estop")
|
||||
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")
|
||||
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 ──────────────────────────────────────────────────────────
|
||||
|
||||
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:
|
||||
return
|
||||
elapsed = time.monotonic() - self._last_cmd_time
|
||||
if elapsed > self._cmd_timeout:
|
||||
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")
|
||||
if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")
|
||||
|
||||
# ── CAN send helper ───────────────────────────────────────────────────
|
||||
|
||||
def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
|
||||
"""Send a standard CAN frame; handle errors gracefully."""
|
||||
def _send_can(self, arb_id: int, data: bytes, context: str,
|
||||
extended: bool = False) -> None:
|
||||
with self._lock:
|
||||
if not self._connected or self._bus is None:
|
||||
return
|
||||
bus = self._bus
|
||||
|
||||
msg = can.Message(
|
||||
arbitration_id=arb_id,
|
||||
data=data,
|
||||
is_extended_id=False,
|
||||
)
|
||||
msg = can.Message(arbitration_id=arb_id, data=data,
|
||||
is_extended_id=extended)
|
||||
try:
|
||||
bus.send(msg, timeout=0.05)
|
||||
except can.CanError as exc:
|
||||
@ -232,59 +216,41 @@ class CanBridgeNode(Node):
|
||||
# ── Background CAN reader ─────────────────────────────────────────────
|
||||
|
||||
def _reader_loop(self) -> None:
|
||||
"""
|
||||
Blocking CAN read loop executed in a daemon thread.
|
||||
Dispatches incoming frames to the appropriate handler.
|
||||
"""
|
||||
while rclpy.ok():
|
||||
with self._lock:
|
||||
connected = self._connected
|
||||
bus = self._bus
|
||||
|
||||
connected, bus = self._connected, self._bus
|
||||
if not connected or bus is None:
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
|
||||
try:
|
||||
frame = bus.recv(timeout=0.5)
|
||||
except can.CanError as exc:
|
||||
self._handle_can_error(exc, "reader_loop recv")
|
||||
continue
|
||||
|
||||
if frame is None:
|
||||
# Timeout — no frame within 0.5 s, loop again
|
||||
continue
|
||||
|
||||
self._dispatch_frame(frame)
|
||||
|
||||
def _dispatch_frame(self, frame: can.Message) -> None:
|
||||
"""Route an incoming CAN frame to the correct publisher."""
|
||||
arb_id = frame.arbitration_id
|
||||
data = bytes(frame.data)
|
||||
|
||||
# VESC STATUS_1 CAN IDs: (VESC_STATUS_1 << 8) | node_id
|
||||
_vesc_left_status1 = (VESC_STATUS_1 << 8) | self._left_vesc_id
|
||||
_vesc_right_status1 = (VESC_STATUS_1 << 8) | self._right_vesc_id
|
||||
|
||||
vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
|
||||
vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
|
||||
try:
|
||||
if arb_id == FC_STATUS:
|
||||
self._handle_fc_status(data)
|
||||
|
||||
elif arb_id == FC_VESC:
|
||||
self._handle_fc_vesc(data)
|
||||
|
||||
elif arb_id == _vesc_left_status1:
|
||||
telem = decode_vesc_status1(self._left_vesc_id, data)
|
||||
msg = Float32MultiArray()
|
||||
msg.data = [telem.erpm, telem.duty, 0.0, telem.current]
|
||||
self._pub_vesc_left.publish(msg)
|
||||
|
||||
elif arb_id == _vesc_right_status1:
|
||||
telem = decode_vesc_status1(self._right_vesc_id, data)
|
||||
msg = Float32MultiArray()
|
||||
msg.data = [telem.erpm, telem.duty, 0.0, telem.current]
|
||||
self._pub_vesc_right.publish(msg)
|
||||
|
||||
if arb_id == ESP32_TELEM_ATTITUDE:
|
||||
self._handle_attitude(data)
|
||||
elif arb_id == ESP32_TELEM_BATTERY:
|
||||
self._handle_battery(data)
|
||||
elif arb_id == vesc_l:
|
||||
t = decode_vesc_status1(self._left_vesc_id, data)
|
||||
m = Float32MultiArray()
|
||||
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||
self._pub_vesc_left.publish(m)
|
||||
elif arb_id == vesc_r:
|
||||
t = decode_vesc_status1(self._right_vesc_id, data)
|
||||
m = Float32MultiArray()
|
||||
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||
self._pub_vesc_right.publish(m)
|
||||
except Exception as exc:
|
||||
self.get_logger().warning(
|
||||
f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
|
||||
@ -292,38 +258,35 @@ class CanBridgeNode(Node):
|
||||
|
||||
# ── Frame handlers ────────────────────────────────────────────────────
|
||||
|
||||
def _handle_fc_status(self, data: bytes) -> None:
|
||||
"""FC_STATUS (0x400): pitch, motor_cmd, vbat_mv, state, flags."""
|
||||
telem = decode_fc_status(data)
|
||||
_STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
|
||||
|
||||
# Publish pitch as IMU (orientation only — yaw/roll unknown from FC_STATUS)
|
||||
imu_msg = Imu()
|
||||
imu_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
imu_msg.header.frame_id = "imu_link"
|
||||
# Only pitch is available; publish as angular velocity placeholder
|
||||
imu_msg.angular_velocity.y = telem.pitch_deg # degrees, not rad/s
|
||||
imu_msg.orientation_covariance[0] = -1.0 # covariance unknown
|
||||
self._pub_imu.publish(imu_msg)
|
||||
def _handle_attitude(self, data: bytes) -> None:
|
||||
"""ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
|
||||
t = decode_attitude(data)
|
||||
now = self.get_clock().now().to_msg()
|
||||
payload = {
|
||||
"pitch_deg": round(t.pitch_deg, 2),
|
||||
"speed_mps": round(t.speed, 3),
|
||||
"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)
|
||||
bat_msg = BatteryState()
|
||||
bat_msg.header.stamp = imu_msg.header.stamp
|
||||
bat_msg.voltage = telem.vbat_mv / 1000.0
|
||||
bat_msg.present = True
|
||||
bat_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
self._pub_battery.publish(bat_msg)
|
||||
|
||||
def _handle_fc_vesc(self, data: bytes) -> None:
|
||||
"""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)
|
||||
def _handle_battery(self, data: bytes) -> None:
|
||||
"""BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
|
||||
t = decode_battery(data)
|
||||
msg = BatteryState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.voltage = t.vbat_mv / 1000.0
|
||||
msg.present = True
|
||||
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||
self._pub_battery.publish(msg)
|
||||
|
||||
# ── Status helper ─────────────────────────────────────────────────────
|
||||
|
||||
@ -335,11 +298,10 @@ class CanBridgeNode(Node):
|
||||
# ── Shutdown ──────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
"""Send zero velocity and shut down the CAN bus cleanly."""
|
||||
if self._connected and self._bus is not None:
|
||||
try:
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "shutdown")
|
||||
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown")
|
||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
|
||||
self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "shutdown")
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
@ -349,8 +311,6 @@ class CanBridgeNode(Node):
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = CanBridgeNode()
|
||||
|
||||
@ -1,31 +1,29 @@
|
||||
#!/usr/bin/env python3
|
||||
"""mamba_protocol.py — CAN frame codec for SAUL-TEE ESP32 BALANCE + VESC.
|
||||
|
||||
Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 & §8 (2026-04-04)
|
||||
"""mamba_protocol.py — CAN frame codec for Orin ↔ ESP32-S3 BALANCE.
|
||||
|
||||
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
|
||||
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) ───────────────────────────────────────────
|
||||
0x300 DRIVE 8 B [left_mps f32 LE][right_mps f32 LE] velocity setpoint
|
||||
0x301 MODE 1 B [mode u8] 0=idle 1=drive 2=estop
|
||||
0x302 ESTOP 1 B [flags u8] bit0=stop bit1=clear
|
||||
0x303 LED 4 B [pattern u8][r u8][g u8][b u8]
|
||||
0x300 DRIVE 8 B [speed:i16 BE][steer:i16 BE][mode:u8][flags:u8][_:u16]
|
||||
0x301 ARM 1 B [arm:u8] 0x00=DISARM 0x01=ARM
|
||||
0x302 PID_SET 8 B [kp:f16 BE][ki:f16 BE][kd:f16 BE][_:u16]
|
||||
0x303 ESTOP 1 B [0xE5] magic byte — cuts all motors immediately
|
||||
|
||||
── ESP32 BALANCE → Orin (telemetry) ──────────────────────────────────────────
|
||||
0x400 FC_STATUS 8 B [pitch_x10 i16][motor_cmd i16][vbat_mv u16][state u8][flags u8] 10 Hz
|
||||
0x401 FC_VESC 8 B [l_rpm_x10 i16][r_rpm_x10 i16][l_cur_x10 i16][r_cur_x10 i16] 10 Hz
|
||||
0x400 ATTITUDE 8 B [pitch:f16 BE][speed:f16 BE][yaw_rate:f16 BE][state:u8][flags:u8]
|
||||
0x401 BATTERY 4 B [vbat_mv:u16 BE][fault_code:u8][rssi:i8]
|
||||
|
||||
── VESC standard CAN frames (29-bit extended IDs) ────────────────────────────
|
||||
VESC node IDs: Left = 56, Right = 68
|
||||
ID = (packet_type << 8) | node_id
|
||||
speed/steer range: −1000..+1000 (motor units). f16 = IEEE 754 half-precision.
|
||||
|
||||
SET_RPM cmd=3 payload: rpm i32 big-endian
|
||||
STATUS cmd=9 payload: erpm i32, current i16, duty i16
|
||||
STATUS_4 cmd=16 payload: temp_fet×10 i16, temp_motor×10 i16,
|
||||
current_in×10 i16, pid_pos×50 i16
|
||||
STATUS_5 cmd=27 payload: tacho i32, vbat×10 i16
|
||||
VESC standard extended CAN (29-bit IDs = packet_type<<8 | node_id):
|
||||
Left VESC node ID = 56 (0x38)
|
||||
Right VESC node ID = 68 (0x44)
|
||||
STATUS_1 cmd=9 erpm i32 BE, current i16 (/10 A), duty i16 (/1000)
|
||||
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
|
||||
@ -35,60 +33,76 @@ from dataclasses import dataclass
|
||||
|
||||
# Orin → ESP32 BALANCE
|
||||
ORIN_CMD_DRIVE: int = 0x300
|
||||
ORIN_CMD_MODE: int = 0x301
|
||||
ORIN_CMD_ESTOP: int = 0x302
|
||||
ORIN_CMD_LED: int = 0x303
|
||||
ORIN_CMD_ARM: int = 0x301
|
||||
ORIN_CMD_PID: int = 0x302
|
||||
ORIN_CMD_ESTOP: int = 0x303
|
||||
|
||||
# ESP32 BALANCE → Orin
|
||||
FC_STATUS: int = 0x400
|
||||
FC_VESC: int = 0x401
|
||||
ESP32_TELEM_ATTITUDE: int = 0x400
|
||||
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_RIGHT_ID: int = 68
|
||||
|
||||
# VESC CAN packet types (standard VESC protocol)
|
||||
VESC_CMD_SET_RPM: int = 3
|
||||
# VESC packet types
|
||||
VESC_STATUS_1: int = 9
|
||||
VESC_STATUS_4: int = 16
|
||||
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 ──────────────────────────────────────────────────────────────
|
||||
|
||||
@dataclass
|
||||
class DriveCmd:
|
||||
left_mps: float = 0.0 # m/s, positive = forward
|
||||
right_mps: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
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
|
||||
class AttitudeTelemetry:
|
||||
"""Decoded ATTITUDE (0x400) from ESP32 BALANCE."""
|
||||
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
|
||||
state: int = 0 # 0=IDLE 1=RUNNING 2=FAULT
|
||||
flags: int = 0 # error bitmask
|
||||
|
||||
|
||||
@dataclass
|
||||
class FcVesc:
|
||||
"""Decoded FC_VESC (0x401) VESC aggregate telemetry."""
|
||||
left_rpm: float = 0.0 # left_rpm_x10 / 10.0
|
||||
right_rpm: float = 0.0
|
||||
left_cur: float = 0.0 # left_cur_x10 / 10.0 (amps)
|
||||
right_cur: float = 0.0
|
||||
class BatteryTelemetry:
|
||||
"""Decoded BATTERY (0x401) from ESP32 BALANCE."""
|
||||
vbat_mv: int = 0 # millivolts
|
||||
fault_code: int = 0 # 0 = OK
|
||||
rssi: int = 0 # RC signal dBm (signed)
|
||||
|
||||
|
||||
@dataclass
|
||||
class PidGains:
|
||||
"""Balance PID gains."""
|
||||
kp: float = 0.0
|
||||
ki: float = 0.0
|
||||
kd: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class VescStatus1:
|
||||
"""Decoded VESC STATUS (cmd=9) frame."""
|
||||
"""Decoded VESC STATUS (cmd=9) — direct from VESC."""
|
||||
node_id: int = 0
|
||||
erpm: float = 0.0
|
||||
current: float = 0.0 # A
|
||||
@ -97,7 +111,7 @@ class VescStatus1:
|
||||
|
||||
@dataclass
|
||||
class VescStatus4:
|
||||
"""Decoded VESC STATUS_4 (cmd=16) frame."""
|
||||
"""Decoded VESC STATUS_4 (cmd=16)."""
|
||||
node_id: int = 0
|
||||
temp_fet_c: float = 0.0
|
||||
temp_motor_c: float = 0.0
|
||||
@ -106,112 +120,98 @@ class VescStatus4:
|
||||
|
||||
@dataclass
|
||||
class VescStatus5:
|
||||
"""Decoded VESC STATUS_5 (cmd=27) frame."""
|
||||
"""Decoded VESC STATUS_5 (cmd=27)."""
|
||||
node_id: int = 0
|
||||
tacho: int = 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 ───────────────────────────────────────────────────
|
||||
|
||||
def encode_drive_cmd(left_mps: float, right_mps: float) -> bytes:
|
||||
"""Encode ORIN_CMD_DRIVE (0x300) — 8 bytes, float32 LE."""
|
||||
return struct.pack("<ff", float(left_mps), float(right_mps))
|
||||
def encode_drive_cmd(speed: int, steer: int,
|
||||
mode: int = MODE_DRIVE, flags: int = 0) -> bytes:
|
||||
"""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:
|
||||
"""Encode ORIN_CMD_MODE (0x301) — 1 byte."""
|
||||
if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
|
||||
raise ValueError(f"Invalid mode {mode!r}")
|
||||
return struct.pack("B", mode)
|
||||
def encode_arm_cmd(arm: bool) -> bytes:
|
||||
"""Encode ORIN_CMD_ARM (0x301) — 1 byte."""
|
||||
return struct.pack("B", 0x01 if arm else 0x00)
|
||||
|
||||
|
||||
def encode_estop_cmd(stop: bool = True, clear: bool = False) -> bytes:
|
||||
"""Encode ORIN_CMD_ESTOP (0x302) — 1 byte flags (bit0=stop, bit1=clear)."""
|
||||
flags = (0x01 if stop else 0) | (0x02 if clear else 0)
|
||||
return struct.pack("B", flags)
|
||||
def encode_pid_cmd(kp: float, ki: float, kd: float) -> bytes:
|
||||
"""Encode ORIN_CMD_PID (0x302) — 8 bytes (3× half-float + 2-byte pad)."""
|
||||
return struct.pack(_FMT_PID, float(kp), float(ki), float(kd), 0)
|
||||
|
||||
|
||||
def encode_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes:
|
||||
"""Encode ORIN_CMD_LED (0x303) — 4 bytes."""
|
||||
return struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF)
|
||||
|
||||
|
||||
# ── 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))
|
||||
def encode_estop_cmd() -> bytes:
|
||||
"""Encode ORIN_CMD_ESTOP (0x303) — 1 byte magic 0xE5."""
|
||||
return struct.pack("B", _ESTOP_MAGIC)
|
||||
|
||||
|
||||
# ── BALANCE → Orin decoders ───────────────────────────────────────────────────
|
||||
|
||||
def decode_fc_status(data: bytes) -> FcStatus:
|
||||
"""Decode FC_STATUS (0x400) — 8 bytes."""
|
||||
def decode_attitude(data: bytes) -> AttitudeTelemetry:
|
||||
"""Decode ATTITUDE (0x400) — 8 bytes."""
|
||||
if len(data) < 8:
|
||||
raise ValueError(f"FC_STATUS expects 8 bytes, got {len(data)}")
|
||||
pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hHHBB", data)
|
||||
return FcStatus(
|
||||
pitch_deg=pitch_x10 / 10.0,
|
||||
motor_cmd=motor_cmd,
|
||||
vbat_mv=vbat_mv,
|
||||
state=state,
|
||||
flags=flags,
|
||||
)
|
||||
raise ValueError(f"ATTITUDE expects ≥8 bytes, got {len(data)}")
|
||||
pitch, speed, yaw_rate, state, flags = struct.unpack_from(_FMT_ATTITUDE, data)
|
||||
return AttitudeTelemetry(pitch_deg=pitch, speed=speed, yaw_rate=yaw_rate,
|
||||
state=state, flags=flags)
|
||||
|
||||
|
||||
def decode_fc_vesc(data: bytes) -> FcVesc:
|
||||
"""Decode FC_VESC (0x401) — 8 bytes."""
|
||||
if len(data) < 8:
|
||||
raise ValueError(f"FC_VESC expects 8 bytes, got {len(data)}")
|
||||
l_rpm, r_rpm, l_cur, r_cur = struct.unpack_from(">hhhh", data)
|
||||
return FcVesc(
|
||||
left_rpm=l_rpm / 10.0,
|
||||
right_rpm=r_rpm / 10.0,
|
||||
left_cur=l_cur / 10.0,
|
||||
right_cur=r_cur / 10.0,
|
||||
)
|
||||
def decode_battery(data: bytes) -> BatteryTelemetry:
|
||||
"""Decode BATTERY (0x401) — 4 bytes."""
|
||||
if len(data) < 4:
|
||||
raise ValueError(f"BATTERY expects ≥4 bytes, got {len(data)}")
|
||||
vbat, fault, rssi = struct.unpack_from(_FMT_BATTERY, data)
|
||||
return BatteryTelemetry(vbat_mv=vbat, fault_code=fault, rssi=rssi)
|
||||
|
||||
|
||||
# ── 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)."""
|
||||
return (can_id >> 8) & 0xFF, can_id & 0xFF
|
||||
|
||||
|
||||
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])
|
||||
return VescStatus1(
|
||||
node_id=node_id,
|
||||
erpm=float(erpm),
|
||||
current=cur_x10 / 10.0,
|
||||
duty=duty_x1000 / 1000.0,
|
||||
)
|
||||
return VescStatus1(node_id=node_id, erpm=float(erpm),
|
||||
current=cur_x10 / 10.0, duty=duty_x1000 / 1000.0)
|
||||
|
||||
|
||||
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])
|
||||
return VescStatus4(
|
||||
node_id=node_id,
|
||||
temp_fet_c=tfet / 10.0,
|
||||
temp_motor_c=tmot / 10.0,
|
||||
current_in=cur_in / 10.0,
|
||||
)
|
||||
return VescStatus4(node_id=node_id, 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:
|
||||
"""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])
|
||||
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))
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user