Merge pull request 'feat: Jetson command protocol — /cmd_vel to STM32 (Phase 2)' (#34) from sl-jetson/command-protocol into main

include/jetson_cmd.h

@@ -0,0 +1,76 @@
+#ifndef JETSON_CMD_H
+#define JETSON_CMD_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * Jetson→STM32 command protocol over USB CDC (bidirectional, same /dev/ttyACM0)
+ *
+ * Commands (newline-terminated ASCII, sent by Jetson):
+ *   H\n              — heartbeat (every 200ms). Must arrive within 500ms or
+ *                      jetson_cmd_is_active() returns false → steer reverts to 0.
+ *   C<spd>,<str>\n   — drive command: speed -1000..+1000, steer -1000..+1000.
+ *                      Also refreshes the heartbeat timer.
+ *
+ * Speed→setpoint:
+ *   Speed is converted to a setpoint offset (degrees) before calling balance_update().
+ *   Positive speed → forward tilt → robot moves forward.
+ *   Max offset is ±JETSON_SPEED_MAX_DEG (see below).
+ *
+ * Steer:
+ *   Passed directly to motor_driver_update() as steer_cmd.
+ *   Motor driver ramps and clamps with balance headroom (see motor_driver.h).
+ *
+ * Integration pattern in main.c (after the cdc_cmd_ready block):
+ *
+ *   // Process buffered C command (parsed here, not in ISR)
+ *   if (jetson_cmd_ready) { jetson_cmd_ready = 0; jetson_cmd_process(); }
+ *
+ *   // Apply setpoint offset and steer when active
+ *   float base_sp = bal.setpoint;
+ *   if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset();
+ *   balance_update(&bal, &imu, dt);
+ *   bal.setpoint = base_sp;
+ *
+ *   // Steer injection in 50Hz ESC block
+ *   int16_t jsteer = jetson_cmd_is_active(now) ? jetson_cmd_steer() : 0;
+ *   motor_driver_update(&motors, bal.motor_cmd, jsteer, now);
+ */
+
+/* Heartbeat timeout: if no H or C within this window, commands deactivate */
+#define JETSON_HB_TIMEOUT_MS    500
+
+/* Max setpoint offset from Jetson speed command (speed=1000 → +N degrees tilt) */
+#define JETSON_SPEED_MAX_DEG    4.0f   /* ±4° → enough for ~0.5 m/s */
+
+/*
+ * jetson_cmd_process()
+ * Call from main loop (NOT ISR) when jetson_cmd_ready is set.
+ * Parses jetson_cmd_buf (the C<spd>,<str> frame) with sscanf.
+ */
+void    jetson_cmd_process(void);
+
+/*
+ * jetson_cmd_is_active(now)
+ * Returns true if a heartbeat (H or C command) arrived within JETSON_HB_TIMEOUT_MS.
+ * If false, main loop should fall back to RC or zero steer.
+ */
+bool    jetson_cmd_is_active(uint32_t now_ms);
+
+/* Current steer command after latest C frame, clamped to ±1000 */
+int16_t jetson_cmd_steer(void);
+
+/* Setpoint offset (degrees) derived from latest speed command. */
+float   jetson_cmd_sp_offset(void);
+
+/*
+ * Externals — declared here, defined in usbd_cdc_if.c alongside the other
+ * CDC volatile flags (cdc_streaming, cdc_arm_request, etc.).
+ * Main loop checks jetson_cmd_ready; ISR sets it.
+ */
+extern volatile uint8_t  jetson_cmd_ready;   /* set by ISR on C frame */
+extern volatile char     jetson_cmd_buf[32]; /* C<spd>,<str>\0 from ISR */
+extern volatile uint32_t jetson_hb_tick;     /* HAL_GetTick() of last H or C */
+
+#endif /* JETSON_CMD_H */

jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml

@@ -1,8 +1,25 @@
-stm32_serial_bridge:
+# saltybot_bridge parameters
-  ros__parameters:
+# Used by both serial_bridge_node (RX-only) and saltybot_cmd_node (bidirectional)
-    # STM32 USB CDC device node
+
-    # Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied
+# ── Serial ─────────────────────────────────────────────────────────────────────
-    serial_port: /dev/ttyACM0
+# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied.
-    baud_rate: 921600
+serial_port:      /dev/ttyACM0
-    timeout: 0.1        # serial readline timeout (seconds)
+baud_rate:        921600
-    reconnect_delay: 2.0  # seconds between reconnect attempts on disconnect
+timeout:          0.05   # serial readline timeout (seconds)
+reconnect_delay:  2.0    # seconds between reconnect attempts on serial disconnect
+
+# ── saltybot_cmd_node (bidirectional) only ─────────────────────────────────────
+
+# Heartbeat: H\n sent every heartbeat_period seconds.
+# STM32 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.
+heartbeat_period: 0.2    # seconds (= 200ms)
+
+# Twist → ESC command 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]
+#
+# Tune speed_scale for max desired forward speed (1 m/s → 1000 ESC units at default).
+# steer_scale is negative because ROS2 +angular.z = CCW but ESC positive steer
+# may mean right-turn — verify on hardware and flip sign if needed.
+speed_scale:  1000.0
+steer_scale:  -500.0

jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py

@@ -1,34 +1,75 @@
+"""
+saltybot_bridge launch file.
+
+Two deployment modes:
+
+  1. Full bidirectional (recommended for Nav2):
+       ros2 launch saltybot_bridge bridge.launch.py mode:=bidirectional
+     Starts saltybot_cmd_node — owns serial port, handles both RX telemetry
+     and TX /cmd_vel → STM32 commands + heartbeat.
+
+  2. RX-only (telemetry monitor, no drive commands):
+       ros2 launch saltybot_bridge bridge.launch.py mode:=rx_only
+     Starts serial_bridge_node — telemetry RX only. Use when you want to
+     observe telemetry without commanding the robot.
+
+Note: never run both nodes simultaneously on the same serial port.
+"""
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def _launch_nodes(context, *args, **kwargs):
-    serial_port_arg = DeclareLaunchArgument(
+    mode       = LaunchConfiguration("mode").perform(context)
-        "serial_port",
+    port       = LaunchConfiguration("serial_port").perform(context)
-        default_value="/dev/ttyACM0",
+    baud       = LaunchConfiguration("baud_rate").perform(context)
-        description="STM32 USB CDC device node",
+    spd_scale  = LaunchConfiguration("speed_scale").perform(context)
-    )
+    str_scale  = LaunchConfiguration("steer_scale").perform(context)
-    baud_rate_arg = DeclareLaunchArgument(
-        "baud_rate",
-        default_value="921600",
-        description="Serial baud rate",
-    )
 
-    bridge_node = Node(
+    params = {
+        "serial_port":      port,
+        "baud_rate":        int(baud),
+        "timeout":          0.05,
+        "reconnect_delay":  2.0,
+    }
+
+    if mode == "rx_only":
+        return [Node(
             package="saltybot_bridge",
             executable="serial_bridge_node",
             name="stm32_serial_bridge",
             output="screen",
-        parameters=[
+            parameters=[params],
-            {
+        )]
-                "serial_port": LaunchConfiguration("serial_port"),
-                "baud_rate": LaunchConfiguration("baud_rate"),
-                "timeout": 0.1,
-                "reconnect_delay": 2.0,
-            }
-        ],
-    )
 
-    return LaunchDescription([serial_port_arg, baud_rate_arg, bridge_node])
+    # bidirectional (default)
+    params.update({
+        "heartbeat_period": 0.2,
+        "speed_scale":      float(spd_scale),
+        "steer_scale":      float(str_scale),
+    })
+    return [Node(
+        package="saltybot_bridge",
+        executable="saltybot_cmd_node",
+        name="saltybot_cmd",
+        output="screen",
+        parameters=[params],
+    )]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument("mode",         default_value="bidirectional",
+                              description="bidirectional | rx_only"),
+        DeclareLaunchArgument("serial_port",  default_value="/dev/ttyACM0",
+                              description="STM32 USB CDC device node"),
+        DeclareLaunchArgument("baud_rate",    default_value="921600"),
+        DeclareLaunchArgument("speed_scale",  default_value="1000.0",
+                              description="m/s → ESC units (linear.x scale)"),
+        DeclareLaunchArgument("steer_scale",  default_value="-500.0",
+                              description="rad/s → ESC units (angular.z scale, neg=flip)"),
+        OpaqueFunction(function=_launch_nodes),
+    ])

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py

@@ -0,0 +1,344 @@
+"""
+saltybot_cmd_node — full bidirectional STM32↔Jetson bridge
+Combines telemetry RX (from serial_bridge_node) with drive command TX.
+
+Owns /dev/ttyACM0 exclusively — do NOT run alongside serial_bridge_node.
+
+RX path (50Hz from STM32):
+  JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics
+
+TX path:
+  /cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n  → STM32
+  Heartbeat timer (200ms)        → H\\n                 → STM32
+
+Protocol:
+  H\\n              — heartbeat. STM32 reverts steer to 0 if gap > 500ms.
+  C<spd>,<str>\\n   — drive command. speed/steer: -1000..+1000 integers.
+                      C command also refreshes STM32 heartbeat timer.
+
+Twist mapping (configurable via ROS2 params):
+  speed = clamp(linear.x  * speed_scale, -1000, 1000)
+  steer = clamp(angular.z * steer_scale, -1000, 1000)
+  Default scales: speed_scale=1000.0 (1 m/s → 1000), steer_scale=-500.0
+  Negative steer_scale because ROS2 +angular.z = CCW but ESC steer convention
+  may differ — tune in config/bridge_params.yaml.
+"""
+
+import json
+import math
+import threading
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+import serial
+
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Imu
+from std_msgs.msg import String
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+
+_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
+IMU_FRAME_ID = "imu_link"
+
+
+def _clamp(v: float, lo: float, hi: float) -> float:
+    return max(lo, min(hi, v))
+
+
+class SaltybotCmdNode(Node):
+    def __init__(self):
+        super().__init__("saltybot_cmd")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",      "/dev/ttyACM0")
+        self.declare_parameter("baud_rate",        921600)
+        self.declare_parameter("timeout",          0.05)
+        self.declare_parameter("reconnect_delay",  2.0)
+        self.declare_parameter("heartbeat_period", 0.2)   # seconds
+        self.declare_parameter("speed_scale",      1000.0) # m/s → ESC units
+        self.declare_parameter("steer_scale",      -500.0) # rad/s → ESC units
+
+        port      = self.get_parameter("serial_port").value
+        baud      = self.get_parameter("baud_rate").value
+        timeout   = self.get_parameter("timeout").value
+        self._reconnect_delay  = self.get_parameter("reconnect_delay").value
+        self._hb_period        = self.get_parameter("heartbeat_period").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)
+        reliable_qos = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST, depth=10)
+
+        # ── Publishers (telemetry RX path) ────────────────────────────────────
+        self._imu_pub     = self.create_publisher(Imu,    "/saltybot/imu",           sensor_qos)
+        self._balance_pub = self.create_publisher(String, "/saltybot/balance_state",  reliable_qos)
+        self._diag_pub    = self.create_publisher(DiagnosticArray, "/diagnostics",    reliable_qos)
+
+        # ── Subscriber (cmd TX path) ──────────────────────────────────────────
+        self._cmd_vel_sub = self.create_subscription(
+            Twist, "/cmd_vel", self._cmd_vel_cb, reliable_qos)
+
+        # ── State ─────────────────────────────────────────────────────────────
+        self._port_name  = port
+        self._baud       = baud
+        self._timeout    = timeout
+        self._ser: serial.Serial | None = None
+        self._ser_lock   = threading.Lock()   # guards _ser for RX/TX threads
+        self._frame_count  = 0
+        self._error_count  = 0
+        self._last_state   = -1
+        self._last_speed   = 0
+        self._last_steer   = 0
+
+        # ── Open serial ───────────────────────────────────────────────────────
+        self._open_serial()
+
+        # ── Timers ────────────────────────────────────────────────────────────
+        # Telemetry read at 100Hz (STM32 sends at 50Hz)
+        self._read_timer = self.create_timer(0.01, self._read_cb)
+        # Heartbeat TX at configured period (default 200ms)
+        self._hb_timer   = self.create_timer(self._hb_period, self._heartbeat_cb)
+
+        self.get_logger().info(
+            f"saltybot_cmd started — {port} @ {baud} baud "
+            f"(HB {int(self._hb_period*1000)}ms, "
+            f"speed_scale={self._speed_scale}, steer_scale={self._steer_scale})"
+        )
+
+    # ── Serial management ─────────────────────────────────────────────────────
+
+    def _open_serial(self) -> bool:
+        with self._ser_lock:
+            try:
+                self._ser = serial.Serial(
+                    port=self._port_name,
+                    baudrate=self._baud,
+                    timeout=self._timeout,
+                )
+                self._ser.reset_input_buffer()
+                self.get_logger().info(f"Serial open: {self._port_name}")
+                return True
+            except serial.SerialException as exc:
+                self.get_logger().error(f"Cannot open {self._port_name}: {exc}")
+                self._ser = None
+                return False
+
+    def _close_serial(self):
+        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 not open."""
+        with self._ser_lock:
+            if self._ser is None or not self._ser.is_open:
+                return False
+            try:
+                self._ser.write(data)
+                return True
+            except serial.SerialException as exc:
+                self.get_logger().error(f"Serial write error: {exc}")
+                self._ser = None
+                return False
+
+    # ── RX — telemetry read ───────────────────────────────────────────────────
+
+    def _read_cb(self):
+        with self._ser_lock:
+            if self._ser is None or not self._ser.is_open:
+                pass
+            else:
+                try:
+                    lines = []
+                    while self._ser.in_waiting:
+                        raw = self._ser.readline()
+                        if raw:
+                            lines.append(raw)
+                except serial.SerialException as exc:
+                    self.get_logger().error(f"Serial read error: {exc}")
+                    self._ser = None
+                    lines = []
+
+        # Parse outside the lock
+        for raw in lines:
+            self._parse_and_publish(raw)
+
+        # Reconnect if port lost
+        with self._ser_lock:
+            if self._ser is None:
+                self.get_logger().warn(
+                    "Serial lost — reconnecting…",
+                    throttle_duration_sec=self._reconnect_delay,
+                )
+        if self._ser is None:
+            self._open_serial()
+
+    def _parse_and_publish(self, raw: bytes):
+        try:
+            text = raw.decode("ascii", errors="ignore").strip()
+            if not text:
+                return
+            data = json.loads(text)
+        except (ValueError, UnicodeDecodeError) as exc:
+            self.get_logger().debug(f"Parse error ({exc}): {raw!r}")
+            self._error_count += 1
+            return
+
+        now = self.get_clock().now().to_msg()
+
+        if "err" in data:
+            self._publish_imu_fault(data["err"], now)
+            return
+
+        required = ("p", "r", "e", "ig", "m", "s", "y")
+        if not all(k in data for k in required):
+            self.get_logger().debug(f"Incomplete frame: {text}")
+            return
+
+        pitch_deg = data["p"] / 10.0
+        roll_deg  = data["r"] / 10.0
+        yaw_deg   = data["y"] / 10.0
+        error_deg = data["e"] / 10.0
+        integral  = data["ig"] / 10.0
+        motor_cmd = float(data["m"])
+        state     = int(data["s"])
+        self._frame_count += 1
+
+        self._publish_imu(pitch_deg, roll_deg, yaw_deg, now)
+        self._publish_balance_state(
+            pitch_deg, roll_deg, yaw_deg,
+            error_deg, integral, motor_cmd, state, now,
+        )
+        if state != self._last_state:
+            self.get_logger().info(
+                f"Balance state → {_STATE_LABEL.get(state, f'UNKNOWN({state})')}"
+            )
+            self._last_state = state
+
+    def _publish_imu(self, pitch_deg, roll_deg, yaw_deg, stamp):
+        msg = Imu()
+        msg.header.stamp = stamp
+        msg.header.frame_id = IMU_FRAME_ID
+        msg.orientation_covariance[0] = -1.0
+        msg.angular_velocity.x = math.radians(pitch_deg)
+        msg.angular_velocity.y = math.radians(roll_deg)
+        msg.angular_velocity.z = math.radians(yaw_deg)
+        cov = math.radians(0.5) ** 2
+        msg.angular_velocity_covariance[0] = cov
+        msg.angular_velocity_covariance[4] = cov
+        msg.angular_velocity_covariance[8] = cov
+        msg.linear_acceleration_covariance[0] = -1.0
+        self._imu_pub.publish(msg)
+
+    def _publish_balance_state(
+        self, pitch, roll, yaw, error, integral, motor_cmd, state, stamp
+    ):
+        state_label = _STATE_LABEL.get(state, f"UNKNOWN({state})")
+        payload = {
+            "stamp":        f"{stamp.sec}.{stamp.nanosec:09d}",
+            "state":        state_label,
+            "pitch_deg":    round(pitch, 1),
+            "roll_deg":     round(roll, 1),
+            "yaw_deg":      round(yaw, 1),
+            "pid_error_deg":round(error, 1),
+            "integral":     round(integral, 1),
+            "motor_cmd":    int(motor_cmd),
+            "jetson_speed": self._last_speed,
+            "jetson_steer": self._last_steer,
+            "frames":       self._frame_count,
+            "parse_errors": self._error_count,
+        }
+        str_msg = String()
+        str_msg.data = json.dumps(payload)
+        self._balance_pub.publish(str_msg)
+
+        diag = DiagnosticArray()
+        diag.header.stamp = stamp
+        status = DiagnosticStatus()
+        status.name = "saltybot/balance_controller"
+        status.hardware_id = "stm32f722"
+        status.message = state_label
+        if state == 1:
+            status.level = DiagnosticStatus.OK
+        elif state == 0:
+            status.level = DiagnosticStatus.WARN
+        else:
+            status.level = DiagnosticStatus.ERROR
+        status.values = [
+            KeyValue(key="pitch_deg",      value=f"{pitch:.1f}"),
+            KeyValue(key="roll_deg",       value=f"{roll:.1f}"),
+            KeyValue(key="yaw_deg",        value=f"{yaw:.1f}"),
+            KeyValue(key="pid_error_deg",  value=f"{error:.1f}"),
+            KeyValue(key="integral",       value=f"{integral:.1f}"),
+            KeyValue(key="motor_cmd",      value=f"{int(motor_cmd)}"),
+            KeyValue(key="jetson_speed",   value=str(self._last_speed)),
+            KeyValue(key="jetson_steer",   value=str(self._last_steer)),
+            KeyValue(key="state",          value=state_label),
+        ]
+        diag.status.append(status)
+        self._diag_pub.publish(diag)
+
+    def _publish_imu_fault(self, errno: int, stamp):
+        diag = DiagnosticArray()
+        diag.header.stamp = stamp
+        status = DiagnosticStatus()
+        status.level = DiagnosticStatus.ERROR
+        status.name = "saltybot/balance_controller"
+        status.hardware_id = "stm32f722"
+        status.message = f"IMU fault errno={errno}"
+        diag.status.append(status)
+        self._diag_pub.publish(diag)
+        self.get_logger().error(f"STM32 IMU fault: errno={errno}")
+
+    # ── TX — command send ─────────────────────────────────────────────────────
+
+    def _cmd_vel_cb(self, msg: Twist):
+        """Convert Twist to C<speed>,<steer>\\n and send over serial."""
+        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
+        frame = f"C{speed},{steer}\n".encode("ascii")
+        if not self._write(frame):
+            self.get_logger().warn(
+                "Cannot send cmd — serial not open",
+                throttle_duration_sec=2.0,
+            )
+
+    def _heartbeat_cb(self):
+        """Send H\\n heartbeat. STM32 reverts steer to 0 if gap > 500ms."""
+        self._write(b"H\n")
+
+    # ── Lifecycle ─────────────────────────────────────────────────────────────
+
+    def destroy_node(self):
+        # Send zero command on shutdown so robot doesn't run away
+        self._write(b"C0,0\n")
+        self._close_serial()
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SaltybotCmdNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py

@@ -115,6 +115,22 @@ class SerialBridgeNode(Node):
                 pass
         self._ser = None
 
+    def write_serial(self, data: bytes) -> bool:
+        """
+        Send raw bytes to STM32 over the open serial port.
+        Returns False if port is not open (caller should handle gracefully).
+        Note: for bidirectional use prefer saltybot_cmd_node which owns TX natively.
+        """
+        if self._ser is None or not self._ser.is_open:
+            return False
+        try:
+            self._ser.write(data)
+            return True
+        except serial.SerialException as exc:
+            self.get_logger().error(f"Serial write error: {exc}")
+            self._close_serial()
+            return False
+
     # ── Read callback ─────────────────────────────────────────────────────────
 
     def _read_cb(self):

jetson/ros2_ws/src/saltybot_bridge/setup.py

@@ -21,7 +21,10 @@ setup(
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
+            # RX-only telemetry bridge (does not send commands)
             "serial_bridge_node = saltybot_bridge.serial_bridge_node:main",
+            # Full bidirectional bridge: telemetry RX + /cmd_vel TX + heartbeat
+            "saltybot_cmd_node  = saltybot_bridge.saltybot_cmd_node:main",
         ],
     },
 )

jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py

@@ -0,0 +1,99 @@
+"""
+Unit tests for Jetson→STM32 command serialization logic.
+Tests Twist→speed/steer conversion and frame formatting.
+Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
+"""
+
+import pytest
+
+
+# ── Minimal stubs (no ROS2 runtime needed) ───────────────────────────────────
+
+def _clamp(v, lo, hi):
+    return max(lo, min(hi, v))
+
+
+def twist_to_frame(linear_x, angular_z, speed_scale=1000.0, steer_scale=-500.0):
+    """Mirror of SaltybotCmdNode._cmd_vel_cb frame building."""
+    speed = int(_clamp(linear_x  * speed_scale, -1000.0, 1000.0))
+    steer = int(_clamp(angular_z * steer_scale, -1000.0, 1000.0))
+    return f"C{speed},{steer}\n".encode("ascii"), speed, steer
+
+
+# ── Frame format tests ────────────────────────────────────────────────────────
+
+def test_zero_twist_produces_zero_cmd():
+    frame, speed, steer = twist_to_frame(0.0, 0.0)
+    assert frame == b"C0,0\n"
+    assert speed == 0
+    assert steer == 0
+
+
+def test_full_forward():
+    frame, speed, steer = twist_to_frame(1.0, 0.0)
+    assert frame == b"C1000,0\n"
+    assert speed == 1000
+
+
+def test_full_reverse():
+    frame, speed, steer = twist_to_frame(-1.0, 0.0)
+    assert frame == b"C-1000,0\n"
+    assert speed == -1000
+
+
+def test_left_turn_positive_angular_z():
+    # Default steer_scale=-500: +angular.z → negative steer
+    frame, speed, steer = twist_to_frame(0.0, 1.0)
+    assert steer == -500
+    assert b"C0,-500\n" == frame
+
+
+def test_right_turn_negative_angular_z():
+    frame, speed, steer = twist_to_frame(0.0, -1.0)
+    assert steer == 500
+    assert b"C0,500\n" == frame
+
+
+def test_speed_clamped_at_max():
+    _, speed, _ = twist_to_frame(5.0, 0.0)   # 5 m/s >> 1 m/s max
+    assert speed == 1000
+
+
+def test_speed_clamped_at_min():
+    _, speed, _ = twist_to_frame(-5.0, 0.0)
+    assert speed == -1000
+
+
+def test_steer_clamped_at_max():
+    # angular.z=-5 rad/s with steer_scale=-500 → +2500 → clamped to +1000
+    _, _, steer = twist_to_frame(0.0, -5.0)
+    assert steer == 1000
+
+
+def test_steer_clamped_at_min():
+    _, _, steer = twist_to_frame(0.0, 5.0)
+    assert steer == -1000
+
+
+def test_combined_motion():
+    frame, speed, steer = twist_to_frame(0.5, -0.4)
+    assert speed == 500
+    assert steer == int(_clamp(-0.4 * -500.0, -1000.0, 1000.0))  # +200
+    assert frame == b"C500,200\n"
+
+
+def test_custom_scales():
+    # speed_scale=500 → 1 m/s = 500 ESC units
+    frame, speed, steer = twist_to_frame(1.0, 0.0, speed_scale=500.0, steer_scale=-250.0)
+    assert speed == 500
+    assert frame == b"C500,0\n"
+
+
+def test_heartbeat_frame():
+    assert b"H\n" == b"H\n"   # constant — just verifies expected bytes
+
+
+def test_zero_cmd_frame():
+    """C0,0\\n must be sent on shutdown."""
+    frame, _, _ = twist_to_frame(0.0, 0.0)
+    assert frame == b"C0,0\n"

lib/USB_CDC/src/usbd_cdc_if.c

@@ -16,6 +16,16 @@ volatile uint8_t cdc_disarm_request = 0; /* set by D command */
 volatile uint8_t  cdc_cmd_ready = 0;
 volatile char     cdc_cmd_buf[32];
 
+/*
+ * Jetson command buffer (bidirectional protocol).
+ * 'H'\n        — heartbeat, ISR updates jetson_hb_tick only (no buf copy needed).
+ * 'C'<s>,<t>\n — drive command: ISR copies to buf, main loop parses with sscanf.
+ * jetson_hb_tick is also refreshed on every C command.
+ */
+volatile uint8_t  jetson_cmd_ready = 0;
+volatile char     jetson_cmd_buf[32];
+volatile uint32_t jetson_hb_tick = 0;  /* HAL_GetTick() of last H or C */
+
 /*
  * USB TX/RX buffers grouped into a single 512-byte aligned struct so that
  * one MPU region (configured in usbd_conf.c) can mark them non-cacheable.
@@ -141,6 +151,23 @@ static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
             cdc_cmd_ready = 1;
             break;
         }
+
+        /* Jetson heartbeat — just refresh the tick, no buffer copy needed */
+        case 'H':
+            jetson_hb_tick = HAL_GetTick();
+            break;
+
+        /* Jetson drive command: C<speed>,<steer>\n
+         * Copy to buffer; main loop parses ints (keeps sscanf out of ISR). */
+        case 'C': {
+            uint32_t copy_len = *len < 31 ? *len : 31;
+            for (uint32_t i = 0; i < copy_len; i++) jetson_cmd_buf[i] = (char)buf[i];
+            jetson_cmd_buf[copy_len] = '\0';
+            jetson_hb_tick = HAL_GetTick(); /* C command also refreshes heartbeat */
+            jetson_cmd_ready = 1;
+            break;
+        }
+
         default: break;
     }
 

src/jetson_cmd.c

@@ -0,0 +1,55 @@
+#include "jetson_cmd.h"
+#include <stdio.h>
+
+/*
+ * Parsed drive state — updated by jetson_cmd_process() in the main loop.
+ * Raw fields are ints parsed from "C<speed>,<steer>\n".
+ */
+static volatile int16_t jcmd_speed = 0;  /* -1000..+1000 */
+static volatile int16_t jcmd_steer = 0;  /* -1000..+1000 */
+
+/* Clamp helper (avoids including math.h) */
+static int16_t clamp16(int v, int lo, int hi) {
+    if (v < lo) return (int16_t)lo;
+    if (v > hi) return (int16_t)hi;
+    return (int16_t)v;
+}
+
+/*
+ * Called from main loop when jetson_cmd_ready is set.
+ * Parses jetson_cmd_buf — safe to use sscanf here (not in ISR).
+ * The ISR only copies bytes and sets the ready flag.
+ */
+void jetson_cmd_process(void) {
+    int speed = 0, steer = 0;
+    /* buf format: "C<speed>,<steer>" — skip leading 'C' */
+    if (sscanf((const char *)jetson_cmd_buf + 1, "%d,%d", &speed, &steer) == 2) {
+        jcmd_speed = clamp16(speed, -1000, 1000);
+        jcmd_steer = clamp16(steer, -1000, 1000);
+    }
+    /* If parse fails, keep previous values — don't zero-out mid-motion */
+}
+
+/*
+ * Returns true if the last heartbeat (H or C command) arrived within
+ * JETSON_HB_TIMEOUT_MS.  jetson_hb_tick is updated in the ISR.
+ */
+bool jetson_cmd_is_active(uint32_t now_ms) {
+    /* jetson_hb_tick == 0 means we've never received any command */
+    if (jetson_hb_tick == 0) return false;
+    return (now_ms - jetson_hb_tick) < JETSON_HB_TIMEOUT_MS;
+}
+
+/* Steer command for motor_driver_update() */
+int16_t jetson_cmd_steer(void) {
+    return jcmd_steer;
+}
+
+/*
+ * Convert speed command to balance setpoint offset (degrees).
+ * Positive speed → lean forward → robot moves forward.
+ * Scaled linearly: speed=1000 → +JETSON_SPEED_MAX_DEG degrees.
+ */
+float jetson_cmd_sp_offset(void) {
+    return ((float)jcmd_speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
+}