feat: CRSF/ELRS RC integration — 16ch input with failsafe (#Phase2)

Protocol choice: implemented from spec (CRSFforArduino needs Arduino
framework; Betaflight extraction has deep scheduler dependencies).
Protocol verified against Betaflight src/main/rx/crsf.c + CRSF spec.

crsf.c:
- UART4 PA0=TX/PA1=RX (GPIO_AF8_UART4), 420000 baud 8N1, oversampling×8
  APB1=54MHz → BRR=0x101 → 418604 baud (0.33% error, within spec)
- DMA1 Stream2 Channel4, circular 64-byte buffer, IDLE interrupt
  DMA half/complete callbacks drain buffer; IDLE fires at frame boundary
- CRC8 DVB-S2 (polynomial 0xD5) validated on every frame
- Parser state machine: SYNC(0xC8)→LEN→DATA with length sanity check
- 11-bit channel unpack for all 16 channels from 22-byte payload
- RC channels frame (0x16): unpacks 16ch, updates last_rx_ms + armed
- Link stats frame (0x14): captures RSSI dBm, LQ%, SNR dB

crsf.h: added rssi_dbm, link_quality, snr fields to CRSFState

config.h: CRSF_ARM_THRESHOLD=1750, CRSF_STEER_MAX=400, CRSF_FAILSAFE_MS=300

main.c:
- crsf_init() called after motor_driver_init()
- RC failsafe: disarm if (now - last_rx_ms) > CRSF_FAILSAFE_MS, but only
  after RC was first seen (last_rx_ms != 0) — USB-only mode unaffected
- RC arm: CH5 rising edge → safety_arm_start(); falling edge → disarm
  Same ARMING_HOLD_MS interlock as USB arm command
- RC steer: CH1 → crsf_to_range() → ±CRSF_STEER_MAX → motor_driver steer
- RSSI/LQ: appended to JSON when safety_rc_alive() ("rssi","lq" fields)

ui/index.html: hidden RC RSSI row revealed on first packet with rssi/lq

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

include/bmp280.h

@@ -9,10 +9,19 @@
  * Probes I2C1 at 0x76 then 0x77.
  * Returns chip_id (0x58=BMP280, 0x60=BME280) on success, negative if not found.
  * Requires i2c1_init() to have been called first.
+ *
+ * All I2C operations use 100ms timeouts — init will not hang on missing hardware.
  */
 int  bmp280_init(void);
 void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10);
 
+/*
+ * BME280-only humidity readout. Call AFTER bmp280_read() (uses cached t_fine).
+ * Returns humidity in %RH × 10 (e.g. 500 = 50.0 %RH).
+ * Returns -1 if chip is BMP280 (no humidity) or not initialised.
+ */
+int16_t bmp280_read_humidity(void);
+
 /* Convert pressure (Pa) to altitude above sea level (cm), ISA p0=101325 Pa. */
 int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa);
 

include/config.h

@@ -153,4 +153,20 @@
 // --- IMU Calibration ---
 #define GYRO_CAL_SAMPLES        1000    /* gyro bias samples (~1s at 1ms/sample) */
 
+// --- RC / Mode Manager ---
+/* CRSF channel indices (0-based; CRSF range 172-1811, center 992) */
+#define CRSF_CH_SPEED           2       /* CH3 — left stick vertical (fwd/back) */
+#define CRSF_CH_STEER           3       /* CH4 — left stick horizontal (yaw) */
+#define CRSF_CH_ARM             4       /* CH5 — arm switch (2-pos) */
+#define CRSF_CH_MODE            5       /* CH6 — mode switch (3-pos) */
+/* Deadband around CRSF center (992) in raw counts (~2% of range) */
+#define CRSF_DEADBAND           30
+/* CH6 mode thresholds (raw CRSF counts) */
+#define CRSF_MODE_LOW_THRESH    600     /* <= → RC_MANUAL */
+#define CRSF_MODE_HIGH_THRESH   1200    /* >= → AUTONOMOUS */
+/* Max speed bias RC can add to balance PID output (counts, same scale as ESC) */
+#define MOTOR_RC_SPEED_MAX      300
+/* Full blend transition time: MANUAL→AUTO takes this many ms */
+#define MODE_BLEND_MS           500
+
 #endif // CONFIG_H

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 */

include/mode_manager.h

@@ -0,0 +1,74 @@
+#ifndef MODE_MANAGER_H
+#define MODE_MANAGER_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * SaltyLab Mode Manager
+ *
+ * Resolves three operating modes selected by RC CH6 (3-pos switch):
+ *
+ *   RC_MANUAL   — RC steer (CH4) and speed bias (CH3) applied directly.
+ *                 Balance PID remains active for stability.
+ *   RC_ASSISTED — RC inputs blended 50/50 with Jetson autonomous commands.
+ *   AUTONOMOUS  — Jetson commands only; RC CH5 arm switch still kills motors.
+ *
+ * Transitions between modes are smoothed over MODE_BLEND_MS (~500ms) to
+ * prevent jerky handoffs. A single `blend` scalar (0=pure RC, 1=pure auto)
+ * drives all interpolation; adjacent-mode steps take ~250ms each.
+ *
+ * RC safety rule: if RC is alive and CH5 is disarmed, the main loop MUST
+ * disarm regardless of mode. mode_manager only blends commands — kill
+ * authority lives in the main loop.
+ *
+ * Autonomous commands are set by the Jetson serial bridge via
+ * mode_manager_set_auto_cmd(). They default to zero (no motion).
+ */
+
+typedef enum {
+    MODE_RC_MANUAL   = 0,
+    MODE_RC_ASSISTED = 1,
+    MODE_AUTONOMOUS  = 2,
+} robot_mode_t;
+
+typedef struct {
+    robot_mode_t target;         /* Mode requested by CH6 (or fallback) */
+    float        blend;          /* 0.0=pure RC .. 1.0=pure auto, smoothly ramped */
+    bool         rc_alive;       /* Cached RC liveness (set in update) */
+    int16_t      auto_steer;     /* Jetson steer cmd (-1000..+1000) */
+    int16_t      auto_speed_bias;/* Jetson speed bias (-MOTOR_RC_SPEED_MAX..+) */
+} mode_manager_t;
+
+/* Initialise — call once before the main loop */
+void mode_manager_init(mode_manager_t *m);
+
+/*
+ * Call every main-loop tick (1ms) to:
+ *   - read CH6, update target mode
+ *   - cache RC liveness
+ *   - advance blend ramp toward target blend value
+ */
+void mode_manager_update(mode_manager_t *m, uint32_t now);
+
+/* Set autonomous commands from the Jetson serial bridge */
+void mode_manager_set_auto_cmd(mode_manager_t *m,
+                                int16_t steer,
+                                int16_t speed_bias);
+
+/*
+ * Blended steer command to pass to motor_driver_update().
+ * Returns 0 when RC is not alive and no autonomous steer set.
+ */
+int16_t mode_manager_get_steer(const mode_manager_t *m);
+
+/*
+ * Blended speed bias to add to bal.motor_cmd before motor_driver_update().
+ * Returns 0 when RC is not alive and no autonomous speed set.
+ */
+int16_t mode_manager_get_speed_bias(const mode_manager_t *m);
+
+/* Quantised current mode (based on blend position, not target) */
+robot_mode_t mode_manager_active(const mode_manager_t *m);
+
+#endif

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

@@ -1,8 +1,25 @@
-stm32_serial_bridge:
-  ros__parameters:
-    # STM32 USB CDC device node
-    # Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied
-    serial_port: /dev/ttyACM0
-    baud_rate: 921600
-    timeout: 0.1        # serial readline timeout (seconds)
-    reconnect_delay: 2.0  # seconds between reconnect attempts on disconnect
+# saltybot_bridge parameters
+# Used by both serial_bridge_node (RX-only) and saltybot_cmd_node (bidirectional)
+
+# ── Serial ─────────────────────────────────────────────────────────────────────
+# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied.
+serial_port:      /dev/ttyACM0
+baud_rate:        921600
+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():
-    serial_port_arg = DeclareLaunchArgument(
-        "serial_port",
-        default_value="/dev/ttyACM0",
-        description="STM32 USB CDC device node",
-    )
-    baud_rate_arg = DeclareLaunchArgument(
-        "baud_rate",
-        default_value="921600",
-        description="Serial baud rate",
-    )
+def _launch_nodes(context, *args, **kwargs):
+    mode       = LaunchConfiguration("mode").perform(context)
+    port       = LaunchConfiguration("serial_port").perform(context)
+    baud       = LaunchConfiguration("baud_rate").perform(context)
+    spd_scale  = LaunchConfiguration("speed_scale").perform(context)
+    str_scale  = LaunchConfiguration("steer_scale").perform(context)
 
-    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=[
-            {
-                "serial_port": LaunchConfiguration("serial_port"),
-                "baud_rate": LaunchConfiguration("baud_rate"),
-                "timeout": 0.1,
-                "reconnect_delay": 2.0,
-            }
-        ],
-    )
+            parameters=[params],
+        )]
 
-    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/bmp280.c

@@ -3,20 +3,33 @@
  *
  * Probes 0x76 first, then 0x77. Requires i2c1_init() before bmp280_init().
  * Returns chip_id on success (0x58=BMP280, 0x60=BME280), negative if absent.
+ *
+ * All HAL_I2C_Mem_Read/Write calls use 100ms timeouts — init cannot hang
+ * indefinitely even if the I2C bus is stuck or the breakout is absent.
+ *
+ * BME280 (chip_id 0x60): bmp280_read_humidity() returns %RH × 10.
+ * Call bmp280_read() first to refresh t_fine, then bmp280_read_humidity().
  */
 #include "bmp280.h"
 #include "i2c1.h"
 #include <math.h>
 
-/* Shift I2C address for HAL (7-bit left-shifted) */
 static uint16_t s_addr;
+static int      s_chip_id;  /* 0x58=BMP280, 0x60=BME280, 0=none */
 
-/* Calibration data */
+/* Shared temp/pressure calibration */
 static uint16_t dig_T1;
 static int16_t  dig_T2, dig_T3;
 static uint16_t dig_P1;
 static int16_t  dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
-static int32_t  t_fine;
+static int32_t  t_fine;  /* updated by bmp280_read(); used by bmp280_read_humidity() */
+
+/* BME280-only humidity calibration (chip_id 0x60) */
+static uint8_t  dig_H1;
+static int16_t  dig_H2;
+static uint8_t  dig_H3;
+static int16_t  dig_H4, dig_H5;
+static int8_t   dig_H6;
 
 static uint8_t i2c_read(uint8_t reg) {
     uint8_t val = 0;
@@ -36,8 +49,9 @@ static int try_init(uint16_t addr) {
     s_addr = addr;
     uint8_t id = i2c_read(0xD0);
     if (id != 0x58 && id != 0x60) return -(int)id;
+    s_chip_id = (int)id;
 
-    /* Read calibration */
+    /* Temp/pressure calibration (0x88–0x9D, identical layout on BMP280 and BME280) */
     uint8_t cal[26];
     i2c_read_burst(0x88, cal, 26);
     dig_T1 = (uint16_t)(cal[1] << 8 | cal[0]);
@@ -53,7 +67,28 @@ static int try_init(uint16_t addr) {
     dig_P8 = (int16_t) (cal[21] << 8 | cal[20]);
     dig_P9 = (int16_t) (cal[23] << 8 | cal[22]);
 
-    /* Normal mode, ×16 oversampling temp+press, 0.5 ms standby */
+    if (id == 0x60) {
+        /* BME280: humidity calibration.
+         *   dig_H1 : 0xA1         (uint8)
+         *   dig_H2 : 0xE1–0xE2   (int16,  LSB first)
+         *   dig_H3 : 0xE3         (uint8)
+         *   dig_H4 : 0xE4[7:0] | 0xE5[3:0]  (int12)
+         *   dig_H5 : 0xE5[7:4]  | 0xE6[7:0] (int12)
+         *   dig_H6 : 0xE7         (int8)
+         */
+        dig_H1 = i2c_read(0xA1);
+        uint8_t hcal[7];
+        i2c_read_burst(0xE1, hcal, 7);
+        dig_H2 = (int16_t)((hcal[1] << 8) | hcal[0]);
+        dig_H3 = hcal[2];
+        dig_H4 = (int16_t)((hcal[3] << 4) | (hcal[4] & 0x0F));
+        dig_H5 = (int16_t)((hcal[5] << 4) | (hcal[4] >> 4));
+        dig_H6 = (int8_t)hcal[6];
+
+        /* ctrl_hum (0xF2) MUST be written before ctrl_meas (0xF4) — hardware req */
+        i2c_write(0xF2, 0x05);  /* osrs_h = ×16 */
+    }
+
     i2c_write(0xF5, 0x00);  /* config: standby=0.5ms, filter=off */
     i2c_write(0xF4, 0xB7);  /* ctrl_meas: osrs_t=×16, osrs_p=×16, normal mode */
 
@@ -73,7 +108,7 @@ void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) {
     int32_t adc_P = (int32_t)((buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4));
     int32_t adc_T = (int32_t)((buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4));
 
-    /* Temperature compensation (BMP280 datasheet Section 4.2.3) */
+    /* Temperature compensation (BME280/BMP280 datasheet Section 4.2.3) */
     int32_t v1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
     int32_t v2 = (((((adc_T >> 4) - (int32_t)dig_T1) *
                     ((adc_T >> 4) - (int32_t)dig_T1)) >> 12) * (int32_t)dig_T3) >> 14;
@@ -95,6 +130,37 @@ void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) {
     *pressure_pa = (int32_t)(((p + p1 + p2) >> 8) + ((int64_t)dig_P7 << 4)) / 256;
 }
 
+/*
+ * BME280-only humidity readout. MUST be called after bmp280_read() (uses t_fine).
+ *
+ * Compensation: BME280 datasheet section 4.2.3 integer formula.
+ * Result is Q22.10 fixed-point: 1024 units = 1 %RH.
+ *
+ * Returns humidity in %RH × 10 (e.g. 500 = 50.0 %RH).
+ * Returns -1 if chip is BMP280 (no humidity sensor).
+ */
+int16_t bmp280_read_humidity(void) {
+    if (s_chip_id != 0x60) return -1;
+
+    uint8_t hbuf[2];
+    i2c_read_burst(0xFD, hbuf, 2);
+    int32_t adc_H = (int32_t)((hbuf[0] << 8) | hbuf[1]);
+
+    /* BME280 datasheet section 4.2.3 — floating-point compensation.
+     * Single-precision float is hardware-accelerated on STM32F7 (FPv5-SP FPU).
+     * Called at 50 Hz — negligible overhead.
+     */
+    float var_H = ((float)t_fine) - 76800.0f;
+    var_H = (adc_H - (((float)dig_H4) * 64.0f + ((float)dig_H5) / 16384.0f * var_H)) *
+            (((float)dig_H2) / 65536.0f *
+             (1.0f + ((float)dig_H6) / 67108864.0f * var_H *
+              (1.0f + ((float)dig_H3) / 67108864.0f * var_H)));
+    var_H *= (1.0f - (float)dig_H1 * var_H / 524288.0f);
+    if (var_H > 100.0f) var_H = 100.0f;
+    if (var_H < 0.0f)   var_H = 0.0f;
+    return (int16_t)(var_H * 10.0f + 0.5f);  /* %RH × 10, rounded */
+}
+
 int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa) {
     /* Barometric formula: h = 44330 * (1 - (p/p0)^(1/5.255)) metres */
     float ratio = (float)pressure_pa / 101325.0f;

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;
+}

src/main.c

@@ -7,6 +7,7 @@
 #include "balance.h"
 #include "hoverboard.h"
 #include "motor_driver.h"
+#include "mode_manager.h"
 #include "config.h"
 #include "status.h"
 #include "safety.h"
@@ -14,6 +15,7 @@
 #include "i2c1.h"
 #include "bmp280.h"
 #include "mag.h"
+#include "jetson_cmd.h"
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
@@ -132,11 +134,16 @@ int main(void) {
     /* Init CRSF/ELRS receiver on UART4 (PA0/PA1) with DMA */
     crsf_init();
 
+    /* Init mode manager (RC/autonomous blend; CH6 mode switch) */
+    mode_manager_t mode;
+    mode_manager_init(&mode);
+
     /* Probe I2C1 for optional sensors — skip gracefully if not found */
-    int baro_ok = 0;
+    int baro_chip = 0;  /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
     mag_type_t mag_type = MAG_NONE;
     if (i2c1_init() == 0) {
-        baro_ok  = (bmp280_init() > 0) ? 1 : 0;
+        int chip = bmp280_init();
+        baro_chip = (chip > 0) ? chip : 0;
         mag_type = mag_init();
     }
 
@@ -165,6 +172,15 @@ int main(void) {
         /* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */
         safety_refresh();
 
+        /* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
+        mode_manager_update(&mode, now);
+
+        /* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off.
+         * Applies regardless of active mode (CH5 always has kill authority). */
+        if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
+            safety_arm_cancel();
+            balance_disarm(&bal);
+        }
         /* RC failsafe: disarm if signal lost AFTER RC was previously alive.
          * Prevents auto-disarm in USB-only mode (crsf_state.last_rx_ms == 0). */
         if (bal.state == BALANCE_ARMED &&
@@ -224,10 +240,19 @@ int main(void) {
             CDC_Transmit((uint8_t *)reply, strlen(reply));
         }
 
-        /* Balance PID at 1kHz */
+        /* Handle Jetson C<speed>,<steer> command (parsed here, not in ISR) */
+        if (jetson_cmd_ready) {
+            jetson_cmd_ready = 0;
+            jetson_cmd_process();
+        }
+
+        /* Balance PID at 1kHz — apply Jetson speed offset when active */
         if (imu_ret == 0 && now - balance_tick >= 1) {
             balance_tick = now;
+            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;
         }
 
         /* Latch estop on tilt fault or disarm */
@@ -237,18 +262,22 @@ int main(void) {
             motor_driver_estop_clear(&motors);
         }
 
-        /* RC steering from CH1 (mapped to ±CRSF_STEER_MAX motor counts) */
-        int16_t rc_steer = 0;
-        if (safety_rc_alive(now)) {
-            rc_steer = crsf_to_range(crsf_state.channels[0],
-                                     -CRSF_STEER_MAX, CRSF_STEER_MAX);
-        }
+        /* Feed autonomous steer from Jetson into mode manager.
+         * mode_manager_get_steer() blends it with RC steer per active mode. */
+        if (jetson_cmd_is_active(now))
+            mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);
 
         /* Send to hoverboard ESC at 50Hz (every 20ms) */
         if (now - esc_tick >= 20) {
             esc_tick = now;
             if (bal.state == BALANCE_ARMED) {
-                motor_driver_update(&motors, bal.motor_cmd, rc_steer, now);
+                /* Blended steer (RC ↔ auto per mode) + RC speed bias */
+                int16_t steer      = mode_manager_get_steer(&mode);
+                int16_t spd_bias   = mode_manager_get_speed_bias(&mode);
+                int32_t speed      = (int32_t)bal.motor_cmd + spd_bias;
+                if (speed >  1000) speed =  1000;
+                if (speed < -1000) speed = -1000;
+                motor_driver_update(&motors, (int16_t)speed, steer, now);
             } else {
                 /* Always send zero while disarmed to prevent ESC timeout */
                 motor_driver_update(&motors, 0, 0, now);
@@ -274,6 +303,9 @@ int main(void) {
                     (int)bal.state,
                     (int)(imu.yaw * 10));        /* yaw degrees x10 (gyro-integrated) */
                 p += n; rem -= n;
+                n = snprintf(p, rem, ",\"md\":%d",
+                    (int)mode_manager_active(&mode));  /* 0=MANUAL,1=ASSISTED,2=AUTO */
+                p += n; rem -= n;
                 if (mag_type != MAG_NONE) {
                     int16_t hd = mag_read_heading();
                     if (hd >= 0)
@@ -282,12 +314,21 @@ int main(void) {
                         n = snprintf(p, rem, ",\"hd\":-1");      /* not ready */
                     p += n; rem -= n;
                 }
-                if (baro_ok) {
+                if (baro_chip) {
                     int32_t pres_pa; int16_t temp_x10;
                     bmp280_read(&pres_pa, &temp_x10);
                     int32_t alt_cm = bmp280_pressure_to_alt_cm(pres_pa);
-                    n = snprintf(p, rem, ",\"alt\":%ld", (long)alt_cm); /* cm */
+                    /* alt cm, temp °C×10, pressure hPa×10 (Pa÷10 = hPa×10) */
+                    n = snprintf(p, rem, ",\"alt\":%ld,\"t\":%d,\"pa\":%ld",
+                        (long)alt_cm, (int)temp_x10, (long)(pres_pa / 10));
                     p += n; rem -= n;
+                    if (baro_chip == 0x60) {  /* BME280: add humidity %RH×10 */
+                        int16_t hum_x10 = bmp280_read_humidity();
+                        if (hum_x10 >= 0) {
+                            n = snprintf(p, rem, ",\"h\":%d", (int)hum_x10);
+                            p += n; rem -= n;
+                        }
+                    }
                 }
                 if (safety_rc_alive(now)) {
                     /* RSSI in dBm (negative), link quality 0-100% */

src/mode_manager.c

@@ -0,0 +1,129 @@
+#include "mode_manager.h"
+#include "crsf.h"
+#include "config.h"
+
+/* -----------------------------------------------------------------------
+ * Internal helpers
+ * --------------------------------------------------------------------- */
+
+static int16_t clamp16(int32_t v, int16_t lo, int16_t hi) {
+    if (v < lo) return lo;
+    if (v > hi) return hi;
+    return (int16_t)v;
+}
+
+static float clampf(float v, float lo, float hi) {
+    if (v < lo) return lo;
+    if (v > hi) return hi;
+    return v;
+}
+
+/*
+ * Map a CRSF raw value to [-out_max, +out_max] with a symmetric deadband
+ * around center (992). Within ±CRSF_DEADBAND counts of center → returns 0.
+ * Outside deadband the remaining range is rescaled linearly to ±out_max.
+ */
+static int16_t crsf_stick(uint16_t raw, int16_t out_max) {
+    int32_t centered = (int32_t)raw - 992;
+    if (centered >  CRSF_DEADBAND)  centered -= CRSF_DEADBAND;
+    else if (centered < -CRSF_DEADBAND) centered += CRSF_DEADBAND;
+    else return 0;
+    /* CRSF half-range from centre ≈ 820 counts; subtract deadband */
+    const int32_t half_range = 820 - CRSF_DEADBAND;
+    int32_t out = centered * out_max / half_range;
+    return clamp16(out, -out_max, out_max);
+}
+
+/* Blend target values for each mode (0=pure RC, 1=pure autonomous) */
+static const float k_blend_target[3] = {
+    [MODE_RC_MANUAL]   = 0.0f,
+    [MODE_RC_ASSISTED] = 0.5f,
+    [MODE_AUTONOMOUS]  = 1.0f,
+};
+
+/* Blend advance rate: 1/MODE_BLEND_MS per ms → full 0..1 transition in
+ * MODE_BLEND_MS. Adjacent mode steps (covering 0.5 of range) take 250ms. */
+#define BLEND_RATE  (1.0f / (float)MODE_BLEND_MS)
+
+/* -----------------------------------------------------------------------
+ * Public API
+ * --------------------------------------------------------------------- */
+
+void mode_manager_init(mode_manager_t *m) {
+    m->target          = MODE_RC_MANUAL;
+    m->blend           = 0.0f;
+    m->rc_alive        = false;
+    m->auto_steer      = 0;
+    m->auto_speed_bias = 0;
+}
+
+void mode_manager_update(mode_manager_t *m, uint32_t now) {
+    static uint32_t s_last_tick = 0;
+
+    /* Delta-time (cap at 100ms for first call / resume after gap) */
+    int32_t dt_ms = (int32_t)(now - s_last_tick);
+    if (dt_ms > 100) dt_ms = 100;
+    s_last_tick = now;
+
+    /* Cache RC liveness — checked by main loop too, but needed by getters */
+    m->rc_alive = (crsf_state.last_rx_ms != 0) &&
+                  ((now - crsf_state.last_rx_ms) < RC_TIMEOUT_MS);
+
+    /* Determine mode target from CH6 */
+    if (m->rc_alive) {
+        uint16_t ch6 = crsf_state.channels[CRSF_CH_MODE];
+        if (ch6 <= CRSF_MODE_LOW_THRESH)
+            m->target = MODE_RC_MANUAL;
+        else if (ch6 >= CRSF_MODE_HIGH_THRESH)
+            m->target = MODE_AUTONOMOUS;
+        else
+            m->target = MODE_RC_ASSISTED;
+    }
+    /* If RC is not alive, keep existing target — don't flap to MANUAL just
+     * because the stub returns zeros; kill authority is a separate concern. */
+
+    /* Advance blend toward target value */
+    float target_blend = k_blend_target[m->target];
+    float step = BLEND_RATE * (float)dt_ms;
+    if (m->blend < target_blend)
+        m->blend = clampf(m->blend + step, 0.0f, target_blend);
+    else
+        m->blend = clampf(m->blend - step, target_blend, 1.0f);
+}
+
+void mode_manager_set_auto_cmd(mode_manager_t *m,
+                                int16_t steer,
+                                int16_t speed_bias) {
+    m->auto_steer      = clamp16(steer,      -1000, 1000);
+    m->auto_speed_bias = clamp16(speed_bias,
+                                 -MOTOR_RC_SPEED_MAX,
+                                  MOTOR_RC_SPEED_MAX);
+}
+
+int16_t mode_manager_get_steer(const mode_manager_t *m) {
+    int16_t rc_steer = 0;
+    if (m->rc_alive)
+        rc_steer = crsf_stick(crsf_state.channels[CRSF_CH_STEER], 1000);
+
+    /* lerp: rc_steer → auto_steer over blend */
+    int32_t mixed = (int32_t)rc_steer +
+                    (int32_t)((float)(m->auto_steer - rc_steer) * m->blend);
+    return clamp16(mixed, -1000, 1000);
+}
+
+int16_t mode_manager_get_speed_bias(const mode_manager_t *m) {
+    int16_t rc_bias = 0;
+    if (m->rc_alive)
+        rc_bias = crsf_stick(crsf_state.channels[CRSF_CH_SPEED],
+                             MOTOR_RC_SPEED_MAX);
+
+    int32_t mixed = (int32_t)rc_bias +
+                    (int32_t)((float)(m->auto_speed_bias - rc_bias) * m->blend);
+    return clamp16(mixed, -MOTOR_RC_SPEED_MAX, MOTOR_RC_SPEED_MAX);
+}
+
+robot_mode_t mode_manager_active(const mode_manager_t *m) {
+    if (m->blend < 0.25f) return MODE_RC_MANUAL;
+    if (m->blend > 0.75f) return MODE_AUTONOMOUS;
+    return MODE_RC_ASSISTED;
+}

ui/index.html

@@ -53,6 +53,9 @@
   <div class="stat"><span class="label">MOTOR</span> <span class="val" id="v-motor">--</span></div>
   <div class="stat" id="row-hdg" style="display:none"><span class="label">HEADING</span> <span class="val" id="v-hdg">--</span>°</div>
   <div class="stat" id="row-alt" style="display:none"><span class="label">ALT</span> <span class="val" id="v-alt">--</span> m</div>
+  <div class="stat" id="row-temp" style="display:none"><span class="label">TEMP</span> <span class="val" id="v-temp">--</span> °C</div>
+  <div class="stat" id="row-hum" style="display:none"><span class="label">HUMIDITY</span> <span class="val" id="v-hum">--</span> %</div>
+  <div class="stat" id="row-pres" style="display:none"><span class="label">PRESSURE</span> <span class="val" id="v-pres">--</span> hPa</div>
   <div class="stat" id="row-rc" style="display:none">
     <span class="label">RC RSSI</span> <span class="val" id="v-rssi">--</span> dBm
     &nbsp;&nbsp;<span class="label" style="width:auto">LQ</span> <span class="val" id="v-lq">--</span>%
@@ -216,6 +219,18 @@ window.updateIMU = function(data) {
     document.getElementById('row-alt').style.display = '';
     document.getElementById('v-alt').textContent = (data.alt / 100.0).toFixed(1);
   }
+  if (data.t !== undefined) {
+    document.getElementById('row-temp').style.display = '';
+    document.getElementById('v-temp').textContent = (data.t / 10.0).toFixed(1);
+  }
+  if (data.h !== undefined) {
+    document.getElementById('row-hum').style.display = '';
+    document.getElementById('v-hum').textContent = (data.h / 10.0).toFixed(1);
+  }
+  if (data.pa !== undefined) {
+    document.getElementById('row-pres').style.display = '';
+    document.getElementById('v-pres').textContent = (data.pa / 10.0).toFixed(1);
+  }
   if (data.rssi !== undefined) {
     document.getElementById('row-rc').style.display = '';
     document.getElementById('v-rssi').textContent = data.rssi;