feat: update SLAM stack for Jetson Orin Nano Super (67 TOPS, JetPack 6)

Platform upgrade: Jetson Nano 4GB → Orin Nano Super 8GB (March 1, 2026)
All Nano-era constraints removed — power/rate/resolution limits obsolete.

Dockerfile: l4t-jetpack:r36.2.0 (JetPack 6 / Ubuntu 22.04 / CUDA 12.x),
  ROS2 Humble via native apt, added ros-humble-rtabmap-ros,
  ros-humble-v4l2-camera for future IMX219 CSI (Phase 2c)

New: slam_rtabmap.launch.py — Orin primary SLAM entry point
  RTAB-Map with subscribe_scan (RPLIDAR) + subscribe_rgbd (D435i)
  Replaces slam_toolbox as docker-compose default

New: config/rtabmap_params.yaml — Orin-optimized
  DetectionRate 10Hz, MaxFeatures 1000, Grid/3D true,
  TimeThr 0 (no limit), Mem/STMSize 0 (unlimited)

Updated: config/realsense_d435i.yaml — 848x480x30, pointcloud enabled
Updated: config/slam_toolbox_params.yaml — 10Hz rate, 1s map interval
Updated: SLAM-SETUP-PLAN.md — full rewrite for Orin: arch diagram,
  Phase 2c IMX219 plan (4x 160° CSI surround), 25W power budget

docker-compose.yml: image tag jetson-orin, default → slam_rtabmap.launch.py

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

@@ -147,4 +147,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/Dockerfile

@@ -1,12 +1,15 @@
-# Jetson Nano — ROS2 Humble dev container
+# Jetson Orin Nano Super — ROS2 Humble dev container
-# Base: JetPack 4.6 (L4T R32.6.1) + CUDA 10.2
+# Base: JetPack 6 (L4T R36.2.0) + CUDA 12.x / Ubuntu 22.04
-# Arch: ARM64 (aarch64)
+#
+# Hardware: Jetson Orin Nano Super 8GB (67 TOPS, 1024-core Ampere)
+# Previous: Jetson Nano 4GB (JetPack 4.6 / L4T R32.6.1) — see git history
 
-FROM nvcr.io/nvidia/l4t-base:r32.6.1
+FROM nvcr.io/nvidia/l4t-jetpack:r36.2.0
 
-LABEL maintainer="sl-jetson <saltylab>"
+LABEL maintainer="sl-perception <saltylab>"
-LABEL description="ROS2 Humble + SLAM stack for Jetson Nano self-balancing robot"
+LABEL description="ROS2 Humble + SLAM stack for Jetson Orin Nano Super self-balancing robot"
-LABEL jetpack="4.6"
+LABEL jetpack="6.0"
+LABEL l4t="r36.2.0"
 LABEL ros_distro="humble"
 
 ENV DEBIAN_FRONTEND=noninteractive
@@ -15,32 +18,29 @@ ENV ROS_ROOT=/opt/ros/${ROS_DISTRO}
 ENV PYTHONDONTWRITEBYTECODE=1
 ENV PYTHONUNBUFFERED=1
 
-# ── System deps ────────────────────────────────────────────────────────────────
+# ── Locale ─────────────────────────────────────────────────────────────────────
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    locales tzdata \
+    && locale-gen en_US.UTF-8 \
+    && rm -rf /var/lib/apt/lists/*
+ENV LANG=en_US.UTF-8
+
+# ── System deps ─────────────────────────────────────────────────────────────────
 RUN apt-get update && apt-get install -y --no-install-recommends \
-    # Build tools
     build-essential cmake git wget curl \
-    # Python
     python3-dev python3-pip python3-setuptools python3-wheel \
-    # Serial / I2C / SPI
     i2c-tools libi2c-dev python3-smbus \
     picocom minicom setserial \
-    # USB
     usbutils libusb-1.0-0-dev \
-    # Misc
+    htop tmux nano \
-    locales tzdata htop tmux nano \
-    # Networking
     net-tools iputils-ping \
     && rm -rf /var/lib/apt/lists/*
 
-RUN locale-gen en_US.UTF-8
+# ── ROS2 Humble (Ubuntu 22.04 native — standard apt, no ARM64 workarounds) ─────
-ENV LANG=en_US.UTF-8
-
-# ── ROS2 Humble (from ROS2 apt repo — ARM64 build) ─────────────────────────────
-# Note: official humble debs for ARM64/L4T are provided via NVIDIA Isaac ROS
-# or via ros2-apt-source for 20.04 focal.
 RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc \
-        | apt-key add - && \
+        | gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg && \
-    echo "deb [arch=arm64] http://packages.ros.org/ros2/ubuntu focal main" \
+    echo "deb [arch=arm64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
+        http://packages.ros.org/ros2/ubuntu jammy main" \
         > /etc/apt/sources.list.d/ros2.list && \
     apt-get update && apt-get install -y --no-install-recommends \
         ros-humble-ros-base \
@@ -51,50 +51,45 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc \
     && rosdep init && rosdep update \
     && rm -rf /var/lib/apt/lists/*
 
-# ── Nav / SLAM / sensor packages ──────────────────────────────────────────────
+# ── SLAM / Nav / Sensor packages ───────────────────────────────────────────────
 RUN apt-get update && apt-get install -y --no-install-recommends \
-    ros-humble-nav2-bringup \
+    ros-humble-rtabmap-ros \
     ros-humble-slam-toolbox \
+    ros-humble-nav2-bringup \
     ros-humble-robot-localization \
     ros-humble-rplidar-ros \
     ros-humble-realsense2-camera \
     ros-humble-realsense2-description \
     ros-humble-tf2-tools \
+    ros-humble-robot-state-publisher \
     ros-humble-rqt \
     ros-humble-rqt-common-plugins \
     ros-humble-rviz2 \
     ros-humble-rosbridge-server \
+    ros-humble-image-transport-plugins \
+    ros-humble-v4l2-camera \
     && rm -rf /var/lib/apt/lists/*
 
-# ── GPIO / serial Python libs ──────────────────────────────────────────────────
+# ── GPIO / serial Python libs ───────────────────────────────────────────────────
 RUN pip3 install --no-cache-dir \
-    Jetson.GPIO \
+    Jetson.GPIO pyserial smbus2 numpy scipy
-    pyserial \
-    smbus2 \
-    adafruit-blinka \
-    RPi.GPIO \
-    numpy \
-    scipy
 
-# ── RealSense SDK (librealsense2 ARM64) ───────────────────────────────────────
+# ── RealSense SDK (librealsense2 ARM64 for JetPack 6) ─────────────────────────
-# Pre-built for L4T — install from Jetson Hacks script or apt source
 RUN apt-get update && apt-get install -y --no-install-recommends \
     libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev \
     && rm -rf /var/lib/apt/lists/*
 
-# librealsense2 ARM64 wheel (NVIDIA-patched for L4T)
 RUN pip3 install --no-cache-dir pyrealsense2
 
-# ── Workspace setup ───────────────────────────────────────────────────────────
+# ── Workspace setup ────────────────────────────────────────────────────────────
 RUN mkdir -p /ros2_ws/src
 WORKDIR /ros2_ws
 
-# Source ROS2 in every shell
 RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /root/.bashrc && \
     echo "source /ros2_ws/install/local_setup.bash 2>/dev/null || true" >> /root/.bashrc && \
     echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /root/.bashrc
 
-# ── Entrypoint ────────────────────────────────────────────────────────────────
+# ── Entrypoint ─────────────────────────────────────────────────────────────────
 COPY scripts/entrypoint.sh /entrypoint.sh
 RUN chmod +x /entrypoint.sh
 

jetson/config/realsense_d435i.yaml

@@ -1,53 +1,33 @@
-# RealSense D435i configuration — Jetson Nano (power-budget tuned)
+# RealSense D435i configuration — Jetson Orin Nano Super
 #
-# Profile format: WxHxFPS
+# Previous Nano 4GB settings (640x480x15, no point cloud) replaced.
-# Constraint: ~10W total budget. 640x480x15 saves ~0.4W vs 30fps.
+# Orin Nano Super has ample Ampere GPU headroom for 848x480x30fps + point cloud.
 #
-# Reference topics at these settings:
+# Reference topics:
-#   /camera/color/image_raw               640x480  15 Hz   RGB8
+#   /camera/color/image_raw               848x480  30 Hz   RGB8
-#   /camera/depth/image_rect_raw          640x480  15 Hz   Z16
+#   /camera/depth/image_rect_raw          848x480  30 Hz   Z16
-#   /camera/aligned_depth_to_color/image_raw  640x480  15 Hz  Z16
+#   /camera/aligned_depth_to_color/image_raw  848x480  30 Hz  Z16
-#   /camera/imu                           6-axis   ~200 Hz  (accel@100Hz + gyro@400Hz fused)
+#   /camera/color/camera_info             848x480  30 Hz
-#   /camera/color/camera_info             640x480  15 Hz
+#   /camera/depth/camera_info             848x480  30 Hz
-#   /camera/depth/camera_info             640x480  15 Hz
+#   /camera/imu                           ~400 Hz  (gyro@400Hz + accel@100Hz fused)
-#
+#   /camera/points                        PointCloud2  30 Hz
-# Hardware IMU: Bosch BMI055
-#   Accelerometer native rate: 100 Hz
-#   Gyroscope native rate:     400 Hz
-#   unite_imu_method=2: linearly interpolates accel to match gyro timestamps
 
 camera:
   ros__parameters:
-    # ── Streams ──────────────────────────────────────────────────────────────
+    depth_module.profile: "848x480x30"
-    depth_module.profile: "640x480x15"
+    rgb_camera.profile:   "848x480x30"
-    rgb_camera.profile:   "640x480x15"
 
     enable_depth:   true
     enable_color:   true
-    enable_infra1:  false    # not needed for RGB-D SLAM
+    enable_infra1:  false
     enable_infra2:  false
 
-    # ── IMU ──────────────────────────────────────────────────────────────────
     enable_gyro:          true
     enable_accel:         true
-    # 0=none  1=copy  2=linear_interpolation
+    unite_imu_method:     2    # linear_interpolation
-    # Use 2: aligns accel timestamps to gyro rate, required for sensor fusion
-    unite_imu_method:     2
 
-    # ── Alignment ────────────────────────────────────────────────────────────
-    # Projects depth pixels into RGB frame — required for rtabmap_ros rgbd input
     align_depth.enable:   true
+    pointcloud.enable:    true    # Orin Ampere GPU handles this without issue
 
-    # ── Point cloud ──────────────────────────────────────────────────────────
-    # Disabled: rtabmap_ros generates its own from aligned depth.
-    # Maxwell GPU cannot handle both simultaneously at budget.
-    pointcloud.enable:    false
-
-    # ── TF ───────────────────────────────────────────────────────────────────
     publish_tf:       true
-    tf_publish_rate:  0.0    # 0 = publish static transforms only (no redundant timer)
+    tf_publish_rate:  0.0
-
-    # ── Device ───────────────────────────────────────────────────────────────
-    # Leave serial_no empty to auto-select first found device
-    # serial_no: ''
-    # device_type: d435i

jetson/config/rtabmap_params.yaml

@@ -0,0 +1,78 @@
+# RTAB-Map configuration — Jetson Orin Nano Super
+#
+# Hardware context: 67 TOPS, 1024-core Ampere GPU, 8GB RAM
+# All Nano-era constraints (2Hz cap, 400 features, no 3D) removed.
+#
+# Sensor inputs:
+#   /scan                    LaserScan from RPLIDAR A1M8    ~5.5 Hz
+#   /camera/color/image_raw  RGB from RealSense D435i       30 Hz
+#   /camera/depth/image_rect_raw  Depth from D435i          30 Hz
+#   /camera/color/camera_info    Camera intrinsics          30 Hz
+#
+# Outputs:
+#   /rtabmap/map             OccupancyGrid (2D occupancy)
+#   /rtabmap/cloud_map       PointCloud2   (3D point cloud, enabled)
+#   /rtabmap/odom            Odometry      (visual-inertial odometry)
+#   /rtabmap/mapData         Map data for serialization/loop closure
+#
+# Frame assumptions (match sensors.launch.py static TF):
+#   map → odom → base_link → laser
+#                          → camera_link
+
+rtabmap:
+  ros__parameters:
+    # ── Core rate and timing ───────────────────────────────────────────────────
+    # Process at 10Hz — far below D435i 30fps ceiling, still 5x faster than Nano
+    Rtabmap/DetectionRate:    "10"
+    # No processing time limit — Orin has headroom (Nano was 700ms)
+    Rtabmap/TimeThr:          "0"
+    # Update map every 5cm or ~3° rotation (was 10cm / 5°)
+    RGBD/LinearUpdate:        "0.05"
+    RGBD/AngularUpdate:       "0.05"
+
+    # ── Visual features ────────────────────────────────────────────────────────
+    # 1000 ORB features per frame (Nano was 400)
+    Kp/MaxFeatures:           "1000"
+    Vis/MaxFeatures:          "1000"
+
+    # ── Memory ────────────────────────────────────────────────────────────────
+    # Unlimited short-term memory — 8GB RAM (Nano was 30 keyframes)
+    Mem/STMSize:              "0"
+    # Keep full map in working memory
+    Mem/IncrementalMemory:    "true"
+
+    # ── Map generation ────────────────────────────────────────────────────────
+    # 3D occupancy grid enabled — Ampere GPU handles point cloud generation
+    Grid/3D:                  "true"
+    Grid/CellSize:            "0.05"    # 5cm voxels
+    Grid/RangeMin:            "0.3"     # ignore points closer than 30cm
+    Grid/RangeMax:            "8.0"     # clip at A1M8 reliable range
+
+    # ── Sensor subscriptions ──────────────────────────────────────────────────
+    # Subscribe to both RPLIDAR scan (fast 2D) and D435i depth (3D)
+    subscribe_scan:           "true"
+    subscribe_rgbd:           "true"
+    subscribe_odom_info:      "false"
+
+    # ── Loop closure ──────────────────────────────────────────────────────────
+    # More aggressive loop closure — Orin can handle it
+    RGBD/LoopClosureReextractFeatures: "true"
+    Kp/IncrementalFlann:      "true"
+
+    # ── Odometry ──────────────────────────────────────────────────────────────
+    # Use F2M (frame-to-map) visual odometry — more accurate on Orin
+    Odom/Strategy:            "0"        # 0=F2M, 1=F2F
+    Odom/ResetCountdown:      "1"
+    OdomF2M/MaxSize:          "2000"
+
+    # ── RGBD node input topics ─────────────────────────────────────────────────
+    rgb_topic:              /camera/color/image_raw
+    depth_topic:            /camera/depth/image_rect_raw
+    camera_info_topic:      /camera/color/camera_info
+    depth_camera_info_topic: /camera/depth/camera_info
+    scan_topic:             /scan
+
+    # ── Output ────────────────────────────────────────────────────────────────
+    # Publish 3D point cloud map
+    cloud_output_voxelized: "true"
+    cloud_voxel_size:       "0.05"      # match Grid/CellSize

jetson/config/slam_toolbox_params.yaml

@@ -1,44 +1,37 @@
 # slam_toolbox — online async SLAM configuration
-# Tuned for Jetson Nano 4GB (constrained CPU/RAM, indoor mapping)
+# Jetson Orin Nano Super — all Nano rate caps removed.
+#
+# Primary SLAM is now RTAB-Map (slam_rtabmap.launch.py).
+# slam_toolbox retained for LIDAR-only localization against pre-built maps.
 #
 # Input:  /scan  (LaserScan from RPLIDAR A1M8, ~5.5 Hz)
-# Output: /map   (OccupancyGrid, updated every map_update_interval seconds)
+# Output: /map   (OccupancyGrid)
-#
-# Frame assumptions (must match sensors.launch.py static TF):
-#   map → odom → base_link → laser
-#   (odom not yet published — slam_toolbox handles this via scan matching)
 
 slam_toolbox:
   ros__parameters:
-    # ── Frames ───────────────────────────────────────────────────────────────
     odom_frame:   odom
     map_frame:    map
     base_frame:   base_link
     scan_topic:   /scan
-    mode:         mapping    # 'mapping' or 'localization'
+    mode:         mapping
 
-    # ── Map params ───────────────────────────────────────────────────────────
+    resolution:               0.05
-    resolution:            0.05    # 5 cm/cell — good balance for A1M8 angular res
+    max_laser_range:          8.0
-    max_laser_range:       8.0     # clip to reliable range of A1M8 (spec: 12m)
+    map_update_interval:      1.0     # 1s (was 5s on Nano)
-    map_update_interval:   5.0     # seconds between full map publishes (saves CPU)
+    minimum_travel_distance:  0.2
-    minimum_travel_distance:  0.3  # only update after moving 30 cm
+    minimum_travel_heading:   0.2
-    minimum_travel_heading:   0.3  # or rotating ~17°
 
-    # ── Performance (Nano-specific) ───────────────────────────────────────────
+    minimum_time_interval:  0.1    # ~10Hz processing (was 0.5s on Nano)
-    # Reduce scan processing rate to stay within ~3.5W CPU budget
-    minimum_time_interval:  0.5   # max 2 Hz scan processing (A1M8 is ~5.5 Hz)
     transform_timeout:      0.2
     tf_buffer_duration:     30.0
-    stack_size_to_use:      40000000   # 40 MB stack
+    stack_size_to_use:      40000000
-    enable_interactive_mode: false     # disable interactive editing (saves CPU)
+    enable_interactive_mode: false
 
-    # ── Scan matching ─────────────────────────────────────────────────────────
     use_scan_matching:    true
     use_scan_barycenter:  true
-    scan_buffer_size:     10
+    scan_buffer_size:     20
     scan_buffer_maximum_scan_distance: 10.0
 
-    # ── Loop closure ──────────────────────────────────────────────────────────
     do_loop_closing:                      true
     loop_match_minimum_chain_size:        10
     loop_match_maximum_variance_coarse:   3.0
@@ -46,31 +39,25 @@ slam_toolbox:
     loop_match_minimum_response_fine:     0.45
     loop_search_maximum_distance:         3.0
 
-    # ── Correlation (coarse scan matching) ───────────────────────────────────
+    correlation_search_space_dimension:       0.5
-    correlation_search_space_dimension:      0.5
+    correlation_search_space_resolution:      0.01
-    correlation_search_space_resolution:     0.01
     correlation_search_space_smear_deviation: 0.1
 
-    # ── Loop search space ─────────────────────────────────────────────────────
+    loop_search_space_dimension:       8.0
-    loop_search_space_dimension:      8.0
+    loop_search_space_resolution:      0.05
-    loop_search_space_resolution:     0.05
     loop_search_space_smear_deviation: 0.03
 
-    # ── Response expansion ────────────────────────────────────────────────────
+    link_match_minimum_response_fine: 0.1
-    link_match_minimum_response_fine:  0.1
+    link_scan_maximum_distance:       1.5
-    link_scan_maximum_distance:        1.5
+    use_response_expansion:           true
-    use_response_expansion:            true
+    distance_variance_penalty:        0.5
+    angle_variance_penalty:           1.0
+    fine_search_angle_offset:         0.00349
+    coarse_search_angle_offset:       0.349
+    coarse_angle_resolution:          0.0349
+    minimum_angle_penalty:            0.9
+    minimum_distance_penalty:         0.5
 
-    # ── Penalties (scan matcher quality thresholds) ───────────────────────────
-    distance_variance_penalty: 0.5
-    angle_variance_penalty:    1.0
-    fine_search_angle_offset:  0.00349    # ~0.2°
-    coarse_search_angle_offset: 0.349     # ~20°
-    coarse_angle_resolution:   0.0349     # ~2°
-    minimum_angle_penalty:     0.9
-    minimum_distance_penalty:  0.5
-
-    # ── Solver ────────────────────────────────────────────────────────────────
     solver_plugin:           solver_plugins::CeresSolver
     ceres_linear_solver:     SPARSE_NORMAL_CHOLESKY
     ceres_preconditioner:    SCHUR_JACOBI

jetson/docker-compose.yml

@@ -8,7 +8,7 @@ services:
 
   # ── Core ROS2 + SLAM node ─────────────────────────────────────────────────
   saltybot-ros2:
-    image: saltybot/ros2-humble:jetson-nano
+    image: saltybot/ros2-humble:jetson-orin
     build:
       context: .
       dockerfile: Dockerfile
@@ -51,7 +51,7 @@ services:
     command: >
       bash -c "
         source /opt/ros/humble/setup.bash &&
-        ros2 launch saltybot_bringup slam.launch.py
+        ros2 launch saltybot_bringup slam_rtabmap.launch.py
       "
 
   # ── RPLIDAR driver node ────────────────────────────────────────────────────

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=[params],
+        )]
+
+    # bidirectional (default)
+    params.update({
+        "heartbeat_period": 0.2,
+        "speed_scale":      float(spd_scale),
+        "steer_scale":      float(str_scale),
+    })
+    return [Node(
         package="saltybot_bridge",
-        executable="serial_bridge_node",
+        executable="saltybot_cmd_node",
-        name="stm32_serial_bridge",
+        name="saltybot_cmd",
         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])
+
+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"

jetson/ros2_ws/src/saltybot_bringup/launch/slam_rtabmap.launch.py

@@ -0,0 +1,114 @@
+"""
+slam_rtabmap.launch.py — RTAB-Map RGB-D + LIDAR SLAM (Orin primary entry point)
+
+Replaces slam.launch.py (slam_toolbox) as the docker-compose default on Orin.
+RTAB-Map chosen over slam_toolbox for Orin because:
+  - Native D435i depth + RPLIDAR sensor fusion
+  - 3D point cloud output (Ampere GPU handles it)
+  - Better loop closure with visual features
+  - Outputs occupancy map directly consumable by Nav2
+
+Stack:
+  sensors.launch.py       (RPLIDAR + RealSense D435i + static TF)
+  rtabmap_ros/rtabmap     (RTAB-Map node with RGB-D + scan input)
+
+RTAB-Map input topics:
+  /camera/color/image_raw          30 Hz  (RGB from D435i)
+  /camera/depth/image_rect_raw     30 Hz  (depth from D435i)
+  /camera/color/camera_info        30 Hz
+  /scan                            ~5.5 Hz (RPLIDAR A1M8)
+
+RTAB-Map output topics:
+  /rtabmap/map             OccupancyGrid   (2D — Nav2 input)
+  /rtabmap/cloud_map       PointCloud2     (3D — visualization)
+  /rtabmap/odom            Odometry        (visual-inertial)
+
+Config: /config/rtabmap_params.yaml (mounted from jetson/config/)
+
+Verify:
+  ros2 topic hz /rtabmap/map          # ~1Hz map updates
+  ros2 topic hz /rtabmap/cloud_map    # ~1Hz 3D cloud updates
+  ros2 topic hz /rtabmap/odom         # ~10Hz odometry
+"""
+
+import os
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+RTABMAP_PARAMS_FILE = '/config/rtabmap_params.yaml'
+
+
+def generate_launch_description():
+    use_sim_time_arg = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation clock (set true for rosbag playback)',
+    )
+
+    bringup_share = get_package_share_directory('saltybot_bringup')
+
+    sensors_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(bringup_share, 'launch', 'sensors.launch.py')
+        ),
+    )
+
+    # RTAB-Map node (rtabmap_ros package)
+    # RGB-D + LIDAR mode: subscribe_scan=true, subscribe_rgbd=true
+    rtabmap_node = Node(
+        package='rtabmap_ros',
+        executable='rtabmap',
+        name='rtabmap',
+        output='screen',
+        parameters=[
+            RTABMAP_PARAMS_FILE,
+            {
+                'use_sim_time': LaunchConfiguration('use_sim_time'),
+                # Frame IDs — must match sensors.launch.py static TF
+                'frame_id':       'base_link',
+                'odom_frame_id':  'odom',
+                'map_frame_id':   'map',
+            },
+        ],
+        remappings=[
+            # RGB-D inputs from RealSense
+            ('rgb/image',           '/camera/color/image_raw'),
+            ('rgb/camera_info',     '/camera/color/camera_info'),
+            ('depth/image',         '/camera/depth/image_rect_raw'),
+            # 2D LIDAR from RPLIDAR A1M8
+            ('scan',                '/scan'),
+        ],
+        arguments=['--delete_db_on_start'],   # fresh map each launch
+    )
+
+    # RTAB-Map visualization node (optional — comment out to save CPU)
+    rtabmap_viz_node = Node(
+        package='rtabmap_ros',
+        executable='rtabmapviz',
+        name='rtabmapviz',
+        output='screen',
+        parameters=[{
+            'use_sim_time': LaunchConfiguration('use_sim_time'),
+            'frame_id':     'base_link',
+        }],
+        remappings=[
+            ('rgb/image',       '/camera/color/image_raw'),
+            ('rgb/camera_info', '/camera/color/camera_info'),
+            ('depth/image',     '/camera/depth/image_rect_raw'),
+            ('scan',            '/scan'),
+        ],
+        # Only launch viz if DISPLAY is available
+        condition=None,    # always launch; comment this node out if headless
+    )
+
+    return LaunchDescription([
+        use_sim_time_arg,
+        sensors_launch,
+        rtabmap_node,
+        # rtabmap_viz_node,   # uncomment for rviz-style RTAB-Map visualization
+    ])

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

projects/saltybot/SLAM-SETUP-PLAN.md

@@ -1,264 +1,167 @@
-# bd-wax: Jetson Nano + RealSense D435i + RPLIDAR SLAM Setup — Technical Plan
+# SLAM Setup Plan — Jetson Orin Nano Super
 
-**Bead:** bd-wax
+**Bead:** bd-wax (plan), bd-a2j (sensor drivers done PR #17)
-**Phase:** 2 (lower priority than Phase 1 balance)
+**Phase:** 2 | **Owner:** sl-perception
-**Owner:** sl-perception
+**Updated:** 2026-03-01 — revised for Jetson Orin Nano Super (replaces Nano 4GB)
-**Hardware:** Jetson Nano 4GB + Intel RealSense D435i + RPLIDAR A1M8
+
-**Goal:** Indoor SLAM, mapping, and autonomous navigation (person-following)
+> All Nano-era constraints (10W cap, 2Hz detection, 400 features, no 3D) are obsolete.
 
 ---
 
-## Hardware Summary
+## Hardware
 
 | Component | Specs |
 |-----------|-------|
-| Jetson Nano 4GB | Quad-core ARM Cortex-A57 @ 1.43GHz, 128-core Maxwell GPU, 4GB LPDDR4 |
+| AI Brain | **Jetson Orin Nano Super 8GB** — 6-core A78AE, 1024-core Ampere, **67 TOPS**, JetPack 6 |
-| RealSense D435i | Stereo depth (0.1–10m), 848×480 @ 90fps, BMI055 IMU (accel+gyro) |
+| Depth Cam | Intel RealSense D435i — 848×480 @ 90fps, BMI055 IMU |
-| RPLIDAR A1M8 | 360° 2D LIDAR, 12m range, 8000 samples/s, ~5.5Hz scan rate |
+| LIDAR | RPLIDAR A1M8 — 360° 2D, 12m range, ~5.5 Hz |
+| Wide Cams | 4× IMX219 160° CSI — front/right/rear/left 90° intervals *(arriving)* |
+| FC | STM32F722 — UART bridge `/dev/ttyACM0` @ 921600 |
 
 ---
 
-## 1. OS & ROS2 Environment
+## 1. OS & ROS2
 
-### Recommended: Docker on JetPack 4.6 (Ubuntu 18.04 / L4T 32.x)
+JetPack 6 = Ubuntu 22.04 → ROS2 Humble via **native apt** (no Docker workarounds needed).
 
-Jetson Nano ships with JetPack 4.6 (Ubuntu 18.04). Native ROS2 Humble requires Ubuntu 22.04, so **Docker is the correct approach**:
+```bash
-
+sudo apt install ros-humble-desktop ros-humble-rtabmap-ros \
-- NVIDIA provides official ROS2 containers for Jetson via [dusty-nv/jetson-containers](https://github.com/dusty-nv/jetson-containers)
+  ros-humble-rplidar-ros ros-humble-realsense2-camera \
-- Container: `dustynv/ros:humble-ros-base-l4t-r32.7.1` (arm64, CUDA 10.2)
+  ros-humble-slam-toolbox ros-humble-nav2-bringup
-- Compose file pins container, mounts device nodes (`/dev/video*`, `/dev/ttyUSB*`), and handles GPU access
-
-**Alternative: JetPack 5.x (Ubuntu 20.04)** allows native ROS2 Foxy but requires flashing newer JetPack — only worth it if we need direct hardware access outside Docker.
-
-**Decision:** Use JetPack 4.6 + Docker (Humble). Fastest path, avoids full OS reflash, proven for Nano.
-
----
-
-## 2. SLAM Stack Recommendation
-
-### Recommendation: RTAB-Map (primary) with ORB-SLAM3 as fallback
-
-#### Why RTAB-Map
-
-| Criterion | ORB-SLAM3 | RTAB-Map |
-|-----------|-----------|----------|
-| Sensor fusion (D435i + RPLIDAR) | D435i only | D435i + RPLIDAR natively |
-| Output map type | Sparse 3D point cloud | Dense 2D occupancy + 3D point cloud |
-| Nav2 compatibility | Needs wrapper | Direct occupancy map output |
-| ATE RMSE accuracy | 0.009m | 0.019m |
-| Nano resource usage | Lower | Higher (needs tuning) |
-| Loop closure robustness | Good | Excellent |
-
-RTAB-Map's `subscribe_scan` mode uses the RPLIDAR A1M8 as the primary 2D odometry front-end (fast, low-CPU) with the D435i providing depth for loop closure and 3D reconstruction. This hybrid approach is well-documented for Nano deployments.
-
-**Note:** Cannot simultaneously use `subscribe_scan` (2D LIDAR) and `subscribe_scan_cloud` (3D point cloud) in RTAB-Map — use 2D mode.
-
-#### ORB-SLAM3 Role
-
-Keep ORB-SLAM3 as a lightweight visual odometry alternative if RTAB-Map proves too heavy for real-time operation. Can feed its odometry output into RTAB-Map's map management node.
-
----
-
-## 3. Software Architecture
-
-```
-┌─────────────────────────────────────────────────────┐
-│                  Jetson Nano (Docker)                │
-│                                                     │
-│  ┌───────────────┐    ┌──────────────────────────┐  │
-│  │ realsense2_   │    │     rplidar_ros2          │  │
-│  │ camera node   │    │  /scan (LaserScan)        │  │
-│  │               │    │  5.5Hz, 360°, 12m range   │  │
-│  │ /depth/image  │    └──────────┬───────────────┘  │
-│  │ /color/image  │               │                  │
-│  │ /imu          │               │                  │
-│  └──────┬────────┘               │                  │
-│         │                        │                  │
-│         └──────────┬─────────────┘                  │
-│                    ▼                                 │
-│         ┌──────────────────┐                        │
-│         │   rtabmap_ros    │                        │
-│         │                  │                        │
-│         │ subscribe_scan=true (RPLIDAR)              │
-│         │ subscribe_rgbd=true (D435i depth)          │
-│         │                  │                        │
-│         │ → /map (OccupancyGrid)                    │
-│         │ → /rtabmap/odom                           │
-│         │ → /rtabmap/cloud_map (3D)                 │
-│         └──────────┬────────┘                       │
-│                    │                                │
-│                    ▼                                │
-│         ┌──────────────────┐                        │
-│         │   nav2 stack     │  (Phase 2b)            │
-│         │  (reduced freq)  │                        │
-│         │  5-10Hz costmap  │                        │
-│         └──────────────────┘                        │
-└─────────────────────────────────────────────────────┘
-         │                        │
-    USB3 (D435i)             USB2 (RPLIDAR)
 ```
 
-### ROS2 Node Graph
+Docker still supported for CI. Updated `Dockerfile` uses `nvcr.io/nvidia/l4t-jetpack:r36.2.0`.
-
-| Node | Package | Key Topics |
-|------|---------|------------|
-| `camera/realsense2_camera_node` | `realsense2_camera` | `/camera/depth/image_rect_raw`, `/camera/color/image_raw`, `/camera/imu` |
-| `rplidar_node` | `rplidar_ros` | `/scan` |
-| `rtabmap` | `rtabmap_ros` | `/map`, `/rtabmap/odom`, `/rtabmap/cloud_map` |
-| `robot_state_publisher` | `robot_state_publisher` | `/tf` (static transforms) |
 
 ---
 
-## 4. Implementation Phases
+## 2. SLAM Stack
 
-### Phase 2a — SLAM Bring-up (this bead, bd-wax)
+### Primary: RTAB-Map (RGB-D + 2D LIDAR fusion)
 
-**Deliverables:**
+| Parameter | Nano 4GB (old) | **Orin Nano Super** |
-- [ ] Docker Compose file (`docker/slam/docker-compose.yml`)
+|-----------|---------------|---------------------|
-- [ ] ROS2 Humble container with RTAB-Map + RealSense + RPLIDAR packages
+| Detection rate | 2 Hz | **10 Hz** |
-- [ ] Launch file: `launch/slam.launch.py` (RTAB-Map + both sensor nodes)
+| Visual features | 400 | **1000** |
-- [ ] URDF/static TF: D435i and RPLIDAR positions on robot frame
+| D435i profile | 640×480×15fps | **848×480×30fps** |
-- [ ] `README.md` for Jetson setup instructions
+| 3D point cloud | disabled | **enabled** |
+| Map type | 2D only | **2D + 3D** |
+| Processing time limit | 700ms | **none** |
+| Short-term memory | 30 keyframes | **unlimited** |
 
-**Out of scope (Phase 2b):**
+Fusion: RPLIDAR `/scan` (fast 2D loop closure) + D435i depth (3D reconstruction + visual odometry).
-- Nav2 autonomous navigation
-- Person-following
-- Integration with STM32 motor commands (needs Phase 1 balance complete first)
 
-### Phase 2b — Nav2 Integration (separate bead)
+### Secondary: slam_toolbox (LIDAR-only localization / pre-built map mode)
-
-- Nav2 stack with pre-built occupancy maps
-- 5–10Hz costmap updates (reduced from desktop default 20Hz)
-- Velocity command bridge: Nav2 `/cmd_vel` → STM32 via serial/ROS2 bridge
-- Person detection (YOLOv5 on Nano, or TensorRT-optimized)
 
 ---
 
-## 5. Key Configuration Parameters
+## 3. Architecture
 
-### RTAB-Map Tuning for Jetson Nano
+```
+Jetson Orin Nano Super (Ubuntu 22.04 / JetPack 6 / CUDA 12.x)
+
+  realsense2_camera          rplidar_ros
+  848×480×30fps              /scan ~5.5Hz 360°
+  /camera/color              │
+  /camera/depth              │
+  /camera/imu ~400Hz         │
+        │                    │
+        └──────────┬─────────┘
+                   ▼
+            rtabmap_ros
+            10Hz | 3D cloud | 1000 features
+            → /rtabmap/map         (OccupancyGrid)
+            → /rtabmap/cloud_map   (PointCloud2)
+            → /rtabmap/odom        (Odometry)
+                   │
+                   ▼
+             Nav2 stack            (Phase 2b)
+             20Hz costmap
+             /cmd_vel → STM32
+
+  4× IMX219 CSI               (Phase 2c — pending hardware)
+  front/right/rear/left 160°
+  → panoramic stitch, person tracking
+```
+
+---
+
+## 4. Phases
+
+| Phase | Status | Description |
+|-------|--------|-------------|
+| 2a | ✅ Done (PR #17) | Sensor drivers — `saltybot_bringup` package |
+| 2a+ | This PR | Orin update: Dockerfile JetPack 6, RTAB-Map launch + config |
+| 2b | Pending | Nav2 integration (separate bead, after Phase 1 balance) |
+| 2c | Pending | 4× IMX219 surround vision (new bead, after hardware arrives) |
+
+---
+
+## 5. RTAB-Map Config (Orin)
+
+Full config: `jetson/config/rtabmap_params.yaml`
 
 ```yaml
-# rtabmap_ros params — power/performance tradeoffs for Nano
+Rtabmap/DetectionRate: "10"     # was 2 on Nano
-rtabmap:
+Kp/MaxFeatures:        "1000"   # was 400
-  Rtabmap/DetectionRate: "2"          # Process keyframes at 2Hz (not every frame)
+RGBD/LinearUpdate:     "0.05"   # 5cm  (was 10cm)
-  Rtabmap/TimeThr: "700"              # Max processing time 700ms per iteration
+RGBD/AngularUpdate:    "0.05"   # ~3°  (was 5°)
-  Kp/MaxFeatures: "400"               # Reduce keypoints from default 1000
+Grid/3D:               "true"   # 3D cloud enabled  (was false)
-  Vis/MaxFeatures: "400"
+Rtabmap/TimeThr:       "0"      # no limit  (was 700ms)
-  RGBD/LinearUpdate: "0.1"            # Only update map if moved 10cm
+Mem/STMSize:           "0"      # unlimited  (was 30)
-  RGBD/AngularUpdate: "0.09"          # Or rotated ~5 degrees
-  Mem/STMSize: "30"                   # Short-term memory limit (saves RAM)
-  Grid/3D: "false"                    # Use 2D occupancy only (lighter)
-  Grid/RangeMin: "0.5"                # Ignore points closer than 0.5m
-  Grid/RangeMax: "5.0"                # Limit to 5m (A1M8 is reliable to ~8m)
-```
-
-### RealSense D435i Launch Params
-
-```yaml
-# realsense2_camera — reduce load on Nano
-depth_module.profile: "640x480x15"    # 15fps depth (not 90fps)
-rgb_camera.profile: "640x480x15"      # 15fps color
-enable_gyro: true
-enable_accel: true
-unite_imu_method: 2                   # Publish unified /camera/imu topic
-align_depth.enable: true
-```
-
-### RPLIDAR
-
-```yaml
-serial_port: "/dev/ttyUSB0"
-serial_baudrate: 115200
-scan_mode: "Standard"                 # A1M8 only supports Standard mode
-frame_id: "laser_frame"
 ```
 
 ---
 
-## 6. Static TF / URDF Robot Frame
+## 6. 4× IMX219 Layout (Phase 2c)
-
-Robot coordinate frame (`base_link`) transforms needed:
 
 ```
-base_link
+      FRONT (CSI0) 160°
-├── laser_frame      (RPLIDAR A1M8 — top center of robot)
+LEFT  (CSI3) ×  RIGHT (CSI1)
-├── camera_link      (RealSense D435i — front, ~camera_height above base)
+      REAR  (CSI2) 160°
-│   ├── camera_depth_frame
-│   └── camera_imu_frame
-└── imu_link         (STM32 IMU — FC board location)
 ```
 
-Transforms will be defined in `urdf/saltybot.urdf.xacro` with measured offsets once hardware is physically mounted.
+90° between cameras, 160° FOV → ~70° overlap at each boundary, full 360° coverage.
+
+ROS topics (planned): `/camera/{front,right,rear,left}/image_raw` @ 30Hz,
+`/camera/panoramic/image_raw` @ 15Hz (stitched equirectangular).
 
 ---
 
-## 7. Power Budget Concern
+## 7. Power Budget (Orin Nano Super)
 
-Jetson Nano 4GB maximum draw: **10W** (5V/2A barrel jack mode)
+| Scenario | Total |
+|----------|-------|
+| SLAM active (RTAB-Map + D435i + RPLIDAR) | ~16W |
+| + 4× IMX219 | ~17W |
+| + Nav2 + TensorRT person detection | ~22W |
 
-| Component | Estimated Draw |
+Orin Nano Super TDP: **25W max**. Recommended PSU: 5V 5A (25W) from robot buck converter.
-|-----------|---------------|
+No power gating needed. Run `sudo nvpmodel -m 0 && sudo jetson_clocks` for full performance.
-| Jetson Nano (under SLAM load) | 7–8W |
-| RealSense D435i (USB3) | 1.5W |
-| RPLIDAR A1M8 (USB) | 0.5W |
-| **Total** | **~10W** |
-
-**Risk:** Marginal power budget. Recommend:
-1. Use 5V/4A supply (requires Jetson J48 header, not USB-C)
-2. Disable Jetson display output (`sudo systemctl disable gdm3`)
-3. Use `nvpmodel -m 1` (5W mode) during mapping-only tasks
-4. External powered USB hub for peripherals
 
 ---
 
-## 8. Milestone Checklist
+## 8. Milestones
 
-- [ ] Flash JetPack 4.6 on Nano (or verify existing install)
+- [ ] Flash JetPack 6 on Orin (arriving March 1)
-- [ ] Install Docker + NVIDIA Container Runtime on Nano
+- [ ] `sudo apt install ros-humble-desktop ros-humble-rtabmap-ros ...`
-- [ ] Pull `dustynv/ros:humble-ros-base-l4t-r32.7.1` container
+- [ ] Verify D435i: `lsusb | grep "8086:0b3a"`
-- [ ] Verify D435i recognized: `realsense-viewer` or `rs-enumerate-devices`
+- [ ] Verify RPLIDAR: `ls /dev/rplidar`
-- [ ] Verify RPLIDAR port: `ls /dev/ttyUSB*`
+- [ ] `colcon build --packages-select saltybot_bringup`
-- [ ] Build/pull `realsense2_camera` ROS2 package in container
+- [ ] `ros2 launch saltybot_bringup sensors.launch.py` — verify topics
-- [ ] Build/pull `rplidar_ros` ROS2 package in container
+- [ ] `ros2 launch saltybot_bringup slam_rtabmap.launch.py` — verify `/rtabmap/map`
-- [ ] Build/pull `rtabmap_ros` ROS2 package in container
+- [ ] `ros2 topic hz /rtabmap/cloud_map` — verify 3D cloud
-- [ ] Test individual sensor topics with `ros2 topic echo`
+- [ ] Record rosbag, monitor `tegrastats` for thermal headroom
-- [ ] Run SLAM launch file — verify `/map` published
+- [ ] Update static TF with real mount measurements
-- [ ] Record rosbag for offline tuning
+- [ ] Open bead: Phase 2b Nav2
-- [ ] Document measured TF offsets (physical mount positions)
+- [ ] Open bead: Phase 2c IMX219 (after hardware arrives)
-- [ ] Write Phase 2b bead for Nav2 integration
 
 ---
 
-## 9. Repo Structure (to be created)
+## 9. References
 
-```
+- [Jetson Orin Nano Super](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-orin/)
-saltylab-firmware/
+- [JetPack 6 / L4T R36](https://developer.nvidia.com/embedded/jetpack)
-└── projects/
+- [dusty-nv/jetson-containers JetPack 6](https://github.com/dusty-nv/jetson-containers)
-    └── saltybot/
+- [rtabmap_ros ROS2](https://github.com/introlab/rtabmap_ros)
-        ├── SLAM-SETUP-PLAN.md         ← this file
+- [realsense-ros](https://github.com/IntelRealSense/realsense-ros)
-        ├── SALTYLAB.md                ← main design doc (TBD)
+- [IMX219 / nvarguscamerasrc on Jetson](https://developer.ridgerun.com/wiki/index.php/NVIDIA_Jetson_ISP_Control)
-        └── slam/
-            ├── docker/
-            │   ├── docker-compose.yml
-            │   └── Dockerfile.slam
-            ├── launch/
-            │   └── slam.launch.py
-            ├── config/
-            │   ├── rtabmap_params.yaml
-            │   └── realsense_params.yaml
-            └── urdf/
-                └── saltybot.urdf.xacro
-```
-
----
-
-## 10. References
-
-- [dusty-nv/jetson-containers](https://github.com/dusty-nv/jetson-containers) — official NVIDIA ROS2 Docker containers for Jetson
-- [rtabmap_ros](https://github.com/introlab/rtabmap_ros) — RTAB-Map ROS2 wrapper
-- [realsense-ros](https://github.com/IntelRealSense/realsense-ros) — Intel RealSense ROS2 wrapper
-- [rplidar_ros](https://github.com/Slamtec/rplidar_ros) — Slamtec RPLIDAR ROS2 package
-- [reedjacobp/JetsonBot](https://github.com/reedjacobp/JetsonBot) — Nano + D435i + RPLIDAR reference implementation
-- [RTAB-Map D435 + RPLidar discussion](https://answers.ros.org/question/367019/using-2-d435-cameras-and-an-rplidar-with-rtabmap/)
-- [Comparative SLAM evaluation (2024)](https://arxiv.org/html/2401.02816v1)

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>
@@ -129,11 +131,16 @@ int main(void) {
     motor_driver_t motors;
     motor_driver_init(&motors);
 
+    /* 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();
     }
 
@@ -162,10 +169,19 @@ int main(void) {
         /* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */
         safety_refresh();
 
-        /* RC timeout: disarm if signal lost while armed */
+        /* 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 signal lost while armed — disarm for safety */
         if (bal.state == BALANCE_ARMED && !safety_rc_alive(now)) {
-            /* USB-only mode: no RC receiver expected yet — skip RC timeout.
+            /* Uncomment when CRSF is wired (last_rx_ms stays 0 on stub): */
-             * Uncomment when CRSF is wired: balance_disarm(&bal); */
+            /* balance_disarm(&bal); */
         }
 
         /* Tilt fault buzzer alert (one-shot on fault edge) */
@@ -199,10 +215,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 */
@@ -212,11 +237,22 @@ int main(void) {
             motor_driver_estop_clear(&motors);
         }
 
+        /* Feed autonomous steer from Jetson into mode manager.
+         * mode_manager_get_steer() blends it with RC CH4 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, 0, 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);
@@ -242,6 +278,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)
@@ -250,12 +289,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;
+                        }
+                    }
                 }
                 n = snprintf(p, rem, "}\n");
                 len = (int)(p + n - buf);

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"><span class="label">HZ</span> <span class="val" id="v-hz">--</span></div>
   <div>
     <button class="btn" id="connect-btn" onclick="toggleSerial()">CONNECT USB</button>
@@ -212,6 +215,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);
+  }
 
   // Pitch bar: center at 50%, ±90°
   document.getElementById('bar-pitch').style.width = ((pitch + 90) / 180 * 100) + '%';