feat(panoramic): 360° equirectangular stitching + RTSP stream (Issue #105)

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

include/battery.h

@@ -0,0 +1,35 @@
+#ifndef BATTERY_H
+#define BATTERY_H
+
+/*
+ * battery.h — Vbat ADC reading for CRSF telemetry (Issue #103)
+ *
+ * Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722).
+ * Voltage divider: 10 kΩ / 1 kΩ → 11:1 ratio.
+ * Resolution: 12-bit (0–4095), Vref = 3.3 V.
+ *
+ * Filtered output in millivolts.  Reading is averaged over
+ * BATTERY_SAMPLES conversions (software oversampling) to reduce noise.
+ */
+
+#include <stdint.h>
+
+/* Initialise ADC3 for single-channel Vbat reading on PC1. */
+void battery_init(void);
+
+/*
+ * battery_read_mv() — blocking single-shot read; returns Vbat in mV.
+ * Takes ~1 µs (12-bit conversion at 36 MHz APB2 / 8 prescaler = 4.5 MHz ADC clk).
+ * Returns 0 if ADC not initialised or conversion times out.
+ */
+uint32_t battery_read_mv(void);
+
+/*
+ * battery_estimate_pct() — coarse SoC estimate from Vbat (mV).
+ * Works for 3S LiPo (10.5–12.6 V) and 4S (14.0–16.8 V).
+ * Detection is automatic based on voltage.
+ * Returns 0–100, or 255 if voltage is out of range.
+ */
+uint8_t battery_estimate_pct(uint32_t voltage_mv);
+
+#endif /* BATTERY_H */

include/config.h

@@ -127,10 +127,20 @@
 // Debug: UART5 (PC12=TX, PD2=RX)
 
 // --- CRSF / ExpressLRS ---
-// CH1[0]=steer  CH2[1]=lean(future)  CH5[4]=arm  CH6[5]=mode
+// CH1[0]=steer  CH2[1]=throttle  CH5[4]=arm  CH6[5]=mode
 #define CRSF_ARM_THRESHOLD      1750    /* CH5 raw value; > threshold = armed */
 #define CRSF_STEER_MAX          400     /* CH1 range: -400..+400 motor counts */
-#define CRSF_FAILSAFE_MS        300     /* Disarm after this ms without a frame */
+#define CRSF_FAILSAFE_MS        500     /* Disarm after this ms without a frame (Issue #103) */
+
+// --- Battery ADC (ADC3, PC1 = ADC123_IN11) ---
+/* Mamba F722: 10kΩ + 1kΩ voltage divider → 11:1 ratio */
+#define VBAT_SCALE_NUM          11      /* Numerator of divider ratio */
+#define VBAT_AREF_MV            3300    /* ADC reference in mV */
+#define VBAT_ADC_BITS           12      /* 12-bit ADC → 4096 counts */
+/* Filtered Vbat in mV: (raw * 3300 * 11) / 4096, updated at 10Hz */
+
+// --- CRSF Telemetry TX (uplink: FC → ELRS module → pilot handset) ---
+#define CRSF_TELEMETRY_HZ       1       /* Telemetry TX rate (Hz) */
 
 // --- PID Tuning ---
 #define PID_KP              35.0f
@@ -155,8 +165,8 @@
 
 // --- 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           0       /* CH1 — right stick horizontal (steer) */
-#define CRSF_CH_STEER           3       /* CH4 — left stick horizontal (yaw) */
+#define CRSF_CH_SPEED           1       /* CH2 — right stick vertical (throttle) */
 #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) */

include/crsf.h

@@ -40,6 +40,30 @@ void crsf_parse_byte(uint8_t byte);
  */
 int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max);
 
+/*
+ * crsf_send_battery() — transmit CRSF battery-sensor telemetry frame (type 0x08)
+ * back to the ELRS TX module over UART4 TX.  Call at CRSF_TELEMETRY_HZ (1 Hz).
+ *
+ *   voltage_mv   : battery voltage in millivolts (e.g. 12600 for 3S full)
+ *   current_ma   : current draw in milliamps     (0 if no sensor)
+ *   remaining_pct: state-of-charge 0–100 %       (255 = unknown)
+ *
+ * Frame: [0xC8][12][0x08][v16_hi][v16_lo][c16_hi][c16_lo][cap24×3][rem][CRC]
+ * voltage unit: 100 mV  (12600 mV → 126)
+ * current unit: 100 mA
+ */
+void crsf_send_battery(uint32_t voltage_mv, uint32_t current_ma,
+                       uint8_t remaining_pct);
+
+/*
+ * crsf_send_flight_mode() — transmit CRSF flight-mode frame (type 0x21)
+ * for display on the pilot's handset OSD.
+ *
+ *   armed: true  → "ARMED\0"
+ *          false → "DISARM\0"
+ */
+void crsf_send_flight_mode(bool armed);
+
 extern volatile CRSFState crsf_state;
 
 #endif /* CRSF_H */

jetson/calibration/README.md

@@ -0,0 +1,195 @@
+# IMX219 Camera Calibration Workflow
+
+Full step-by-step calibration procedure for the 4× IMX219 160° fisheye CSI cameras
+on saltybot (cam0=front, cam1=right, cam2=rear, cam3=left).
+
+## Hardware
+
+| Camera | Position | Yaw   | Topic                        |
+|--------|----------|-------|------------------------------|
+| cam0   | Front    |   0°  | /camera/front/image_raw      |
+| cam1   | Right    | -90°  | /camera/right/image_raw      |
+| cam2   | Rear     | 180°  | /camera/rear/image_raw       |
+| cam3   | Left     |  90°  | /camera/left/image_raw       |
+
+Mount geometry: radius=5cm from robot centre, height=30cm, 90° angular spacing.
+
+## Prerequisites
+
+- OpenCV with fisheye support: `pip3 install opencv-python numpy pyyaml`
+- For ROS2 mode: ROS2 Humble, cv_bridge, image_transport
+- Printed checkerboard: 8×6 inner corners, 25mm squares (A3 paper recommended)
+  - Generator: `python3 -c "import cv2; board=cv2.aruco.CharucoBoard((9,7),0.025,0.0175,cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)); cv2.imwrite('board.png', board.generateImage((700,500)))"`
+  - Or download from: https://calib.io/pages/camera-calibration-pattern-generator
+
+## Step 1 — Print and Prepare the Checkerboard
+
+- Print the 8×6 inner-corner checkerboard at exactly 25mm per square
+- Mount flat on a rigid board (cardboard or foamcore) — no warping allowed
+- Measure actual square size with calipers, update `--square-size` if different
+
+## Step 2 — Start Cameras
+
+```bash
+# On the Jetson, in the saltybot docker container:
+ros2 launch saltybot_cameras cameras.launch.py
+
+# Verify all 4 topics are publishing:
+ros2 topic list | grep camera
+ros2 topic hz /camera/front/image_raw
+```
+
+## Step 3 — Capture Calibration Images (30 per camera)
+
+Run for each camera in turn. Move the board to many different positions and
+angles within the field of view — especially corners and edges.
+
+```bash
+# Option A: ROS2 mode (recommended on Jetson, requires running cameras)
+ros2 run saltybot_calibration capture_calibration_images.py \
+  --ros --camera front --n-images 30 \
+  --output-dir /mnt/nvme/saltybot/calibration/raw
+
+ros2 run saltybot_calibration capture_calibration_images.py \
+  --ros --camera right --n-images 30 \
+  --output-dir /mnt/nvme/saltybot/calibration/raw
+
+ros2 run saltybot_calibration capture_calibration_images.py \
+  --ros --camera rear --n-images 30 \
+  --output-dir /mnt/nvme/saltybot/calibration/raw
+
+ros2 run saltybot_calibration capture_calibration_images.py \
+  --ros --camera left --n-images 30 \
+  --output-dir /mnt/nvme/saltybot/calibration/raw
+
+# Option B: All cameras sequentially (prompts between cameras)
+ros2 run saltybot_calibration capture_calibration_images.py \
+  --ros --camera all --n-images 30 \
+  --output-dir /mnt/nvme/saltybot/calibration/raw
+
+# Option C: Standalone (no ROS2, uses /dev/videoX directly)
+python3 capture_calibration_images.py \
+  --camera front --n-images 30 \
+  --device /dev/video0 \
+  --output-dir /mnt/nvme/saltybot/calibration/raw
+```
+
+### Capture Tips for Fisheye Lenses
+
+- Cover the entire FoV — include corners and edges (where distortion is strongest)
+- Use 5+ different orientations of the board (tilt, rotate, skew)
+- Keep the board fully visible (no partial views)
+- Good lighting, no motion blur
+- Aim for 20-30 images with varied positions
+
+## Step 4 — Compute Intrinsics per Camera
+
+```bash
+for cam in front right rear left; do
+  ros2 run saltybot_calibration calibrate_camera.py \
+    --images-dir /mnt/nvme/saltybot/calibration/raw/cam_${cam} \
+    --output /mnt/nvme/saltybot/calibration/${cam}/camera_info.yaml \
+    --camera-name camera_${cam} \
+    --board-size 8x6 \
+    --square-size 0.025 \
+    --image-size 640x480 \
+    --show-reprojection
+done
+```
+
+## Step 5 — Verify Reprojection Error
+
+Target: RMS reprojection error < 1.5 px per camera.
+
+If RMS > 1.5 px:
+1. Remove outlier images (highest per-image error from table)
+2. Recapture with more varied board positions
+3. Check board is truly flat and square size is correct
+4. Try `--image-size 1280x720` if capturing at higher resolution
+
+```bash
+# Quick check — view undistorted preview
+ros2 run saltybot_calibration preview_undistorted.py \
+  --image /mnt/nvme/saltybot/calibration/raw/cam_front/frame_0000.png \
+  --calibration /mnt/nvme/saltybot/calibration/front/camera_info.yaml
+```
+
+## Step 6 — Compute Extrinsics (Mechanical Mode)
+
+Uses known mount geometry (radius=5cm, height=30cm, 90° spacing) to compute
+analytically exact camera-to-base_link transforms.
+
+```bash
+ros2 run saltybot_calibration calibrate_extrinsics.py \
+  --mode mechanical \
+  --mount-radius 0.05 \
+  --mount-height 0.30 \
+  --output /mnt/nvme/saltybot/calibration/extrinsics.yaml \
+  --generate-launch \
+    ~/ros2_ws/src/saltybot_bringup/launch/static_transforms.launch.py
+```
+
+If mechanical dimensions differ from defaults, update `--mount-radius` and
+`--mount-height` from your actual measurements.
+
+## Step 7 — Generate Static Transform Launch File
+
+(Already done by --generate-launch in Step 6, but can regenerate independently:)
+
+```bash
+ros2 run saltybot_calibration generate_static_transforms.py \
+  --extrinsics /mnt/nvme/saltybot/calibration/extrinsics.yaml \
+  --output ~/ros2_ws/src/saltybot_bringup/launch/static_transforms.launch.py
+```
+
+## Step 8 — Update surround_vision_params.yaml
+
+Edit `jetson/config/surround_vision_params.yaml` (or the equivalent in your
+saltybot_surround package) to point camera_info_url at the new calibration files:
+
+```yaml
+camera_info_urls:
+  front: 'file:///mnt/nvme/saltybot/calibration/front/camera_info.yaml'
+  right: 'file:///mnt/nvme/saltybot/calibration/right/camera_info.yaml'
+  rear:  'file:///mnt/nvme/saltybot/calibration/rear/camera_info.yaml'
+  left:  'file:///mnt/nvme/saltybot/calibration/left/camera_info.yaml'
+```
+
+## Step 9 — Restart Nodes
+
+```bash
+# Restart camera and surround nodes to pick up new calibration
+ros2 lifecycle set /saltybot_cameras shutdown
+ros2 launch saltybot_bringup cameras.launch.py
+ros2 launch saltybot_surround surround.launch.py
+```
+
+## Output Files
+
+After full calibration, you should have:
+
+```
+/mnt/nvme/saltybot/calibration/
+  front/camera_info.yaml     # intrinsics: K, D (equidistant)
+  right/camera_info.yaml
+  rear/camera_info.yaml
+  left/camera_info.yaml
+  extrinsics.yaml            # base_link -> camera_*_optical_frame
+```
+
+Template files are in `jetson/calibration/` (tracked in git) with placeholder
+IMX219 values (fx=229px @ 640×480). Replace with calibrated values.
+
+## Distortion Model Reference
+
+The IMX219 160° fisheye uses the **equidistant** (Kannala-Brandt) model:
+
+```
+r(theta) = k1*theta + k2*theta^3 + k3*theta^5 + k4*theta^7
+```
+
+where theta is the angle of incidence. OpenCV's `cv2.fisheye` module implements
+this model with 4 coefficients [k1, k2, k3, k4].
+
+This is different from the `plumb_bob` (radial/tangential) model used for
+standard lenses — do not mix them up.

jetson/calibration/cam_front/camera_info.yaml

@@ -0,0 +1,27 @@
+# TEMPLATE — replace with output of calibrate_camera.py
+# IMX219 160 deg at 640x480: estimated placeholder values
+# Run: ros2 run saltybot_calibration calibrate_camera.py \
+#        --images-dir /mnt/nvme/saltybot/calibration/raw/cam_front \
+#        --output /mnt/nvme/saltybot/calibration/front/camera_info.yaml \
+#        --camera-name camera_front --image-size 640x480
+image_width: 640
+image_height: 480
+camera_name: camera_front
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [229.0, 0.0, 320.0, 0.0, 229.0, 240.0, 0.0, 0.0, 1.0]
+distortion_model: equidistant
+distortion_coefficients:
+  rows: 1
+  cols: 4
+  data: [0.0, 0.0, 0.0, 0.0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [229.0, 0.0, 320.0, 0.0, 0.0, 229.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0]
+_calibration_rms_px: null

jetson/calibration/cam_left/camera_info.yaml

@@ -0,0 +1,27 @@
+# TEMPLATE — replace with output of calibrate_camera.py
+# IMX219 160 deg at 640x480: estimated placeholder values
+# Run: ros2 run saltybot_calibration calibrate_camera.py \
+#        --images-dir /mnt/nvme/saltybot/calibration/raw/cam_left \
+#        --output /mnt/nvme/saltybot/calibration/left/camera_info.yaml \
+#        --camera-name camera_left --image-size 640x480
+image_width: 640
+image_height: 480
+camera_name: camera_left
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [229.0, 0.0, 320.0, 0.0, 229.0, 240.0, 0.0, 0.0, 1.0]
+distortion_model: equidistant
+distortion_coefficients:
+  rows: 1
+  cols: 4
+  data: [0.0, 0.0, 0.0, 0.0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [229.0, 0.0, 320.0, 0.0, 0.0, 229.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0]
+_calibration_rms_px: null

jetson/calibration/cam_rear/camera_info.yaml

@@ -0,0 +1,27 @@
+# TEMPLATE — replace with output of calibrate_camera.py
+# IMX219 160 deg at 640x480: estimated placeholder values
+# Run: ros2 run saltybot_calibration calibrate_camera.py \
+#        --images-dir /mnt/nvme/saltybot/calibration/raw/cam_rear \
+#        --output /mnt/nvme/saltybot/calibration/rear/camera_info.yaml \
+#        --camera-name camera_rear --image-size 640x480
+image_width: 640
+image_height: 480
+camera_name: camera_rear
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [229.0, 0.0, 320.0, 0.0, 229.0, 240.0, 0.0, 0.0, 1.0]
+distortion_model: equidistant
+distortion_coefficients:
+  rows: 1
+  cols: 4
+  data: [0.0, 0.0, 0.0, 0.0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [229.0, 0.0, 320.0, 0.0, 0.0, 229.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0]
+_calibration_rms_px: null

jetson/calibration/cam_right/camera_info.yaml

@@ -0,0 +1,27 @@
+# TEMPLATE — replace with output of calibrate_camera.py
+# IMX219 160 deg at 640x480: estimated placeholder values
+# Run: ros2 run saltybot_calibration calibrate_camera.py \
+#        --images-dir /mnt/nvme/saltybot/calibration/raw/cam_right \
+#        --output /mnt/nvme/saltybot/calibration/right/camera_info.yaml \
+#        --camera-name camera_right --image-size 640x480
+image_width: 640
+image_height: 480
+camera_name: camera_right
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [229.0, 0.0, 320.0, 0.0, 229.0, 240.0, 0.0, 0.0, 1.0]
+distortion_model: equidistant
+distortion_coefficients:
+  rows: 1
+  cols: 4
+  data: [0.0, 0.0, 0.0, 0.0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [229.0, 0.0, 320.0, 0.0, 0.0, 229.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0]
+_calibration_rms_px: null

jetson/calibration/extrinsics.yaml

@@ -0,0 +1,43 @@
+# Camera extrinsics — base_link to camera_optical_frame transforms
+# Generated from mechanical mount geometry:
+#   mount_radius = 0.05m, mount_height = 0.30m
+#   Camera yaw: front=0deg, right=-90deg, rear=180deg, left=90deg
+#
+# Quaternion convention: [qx, qy, qz, qw] (xyzw)
+# Rotation: camera_optical_frame -> base_link
+#   camera_optical: z=forward(out), x=right, y=down
+#   base_link:      x=forward,      y=left,  z=up  (ROS REP-103)
+#
+# Regenerate with:
+#   python3 calibrate_extrinsics.py --mode mechanical \
+#     --mount-radius 0.05 --mount-height 0.30 \
+#     --output jetson/calibration/extrinsics.yaml
+
+_description: Camera extrinsics - base_link to camera_optical_frame transforms
+_mount_radius_m: 0.05
+_mount_height_m: 0.30
+
+cameras:
+  front:
+    parent_frame: base_link
+    child_frame: camera_front_optical_frame
+    translation: [0.05, 0.0, 0.30]
+    rotation_quat_xyzw: [0.5, -0.5, 0.5, -0.5]
+
+  right:
+    parent_frame: base_link
+    child_frame: camera_right_optical_frame
+    translation: [0.0, 0.05, 0.30]
+    rotation_quat_xyzw: [-0.707107, 0.0, 0.0, 0.707107]
+
+  rear:
+    parent_frame: base_link
+    child_frame: camera_rear_optical_frame
+    translation: [-0.05, 0.0, 0.30]
+    rotation_quat_xyzw: [-0.5, -0.5, 0.5, 0.5]
+
+  left:
+    parent_frame: base_link
+    child_frame: camera_left_optical_frame
+    translation: [0.0, -0.05, 0.30]
+    rotation_quat_xyzw: [0.0, -0.707107, 0.707107, 0.0]

jetson/ros2_ws/src/saltybot_calibration/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_calibration)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  scripts/capture_calibration_images.py
+  scripts/calibrate_camera.py
+  scripts/calibrate_extrinsics.py
+  scripts/preview_undistorted.py
+  scripts/generate_static_transforms.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

jetson/ros2_ws/src/saltybot_calibration/config/calibration_params.yaml

@@ -0,0 +1,27 @@
+calibration:
+  board_size: '8x6'          # inner corners (cols x rows)
+  square_size_m: 0.025       # 25mm squares
+  n_images: 30               # images to capture per camera
+  image_size: '640x480'      # capture resolution
+
+  cameras:
+    front:
+      topic: '/camera/front/image_raw'
+      output_yaml: '/mnt/nvme/saltybot/calibration/front/camera_info.yaml'
+    right:
+      topic: '/camera/right/image_raw'
+      output_yaml: '/mnt/nvme/saltybot/calibration/right/camera_info.yaml'
+    rear:
+      topic: '/camera/rear/image_raw'
+      output_yaml: '/mnt/nvme/saltybot/calibration/rear/camera_info.yaml'
+    left:
+      topic: '/camera/left/image_raw'
+      output_yaml: '/mnt/nvme/saltybot/calibration/left/camera_info.yaml'
+
+  extrinsics:
+    mode: mechanical
+    mount_radius_m: 0.05
+    mount_height_m: 0.30
+    output_yaml: '/mnt/nvme/saltybot/calibration/extrinsics.yaml'
+
+  reprojection_warn_threshold_px: 1.5

jetson/ros2_ws/src/saltybot_calibration/launch/calibration_capture.launch.py

@@ -0,0 +1,108 @@
+"""
+calibration_capture.launch.py — Calibration workflow launcher for saltybot IMX219 cameras.
+
+Full calibration workflow:
+  Step 1: Start cameras (requires saltybot_cameras package to be running)
+  Step 2: Run capture_calibration_images.py for each camera (30 images each)
+  Step 3: Run calibrate_camera.py for each camera
+  Step 4: Run calibrate_extrinsics.py
+  Step 5: Run generate_static_transforms.py
+
+This launch file starts the capture script for one camera via ExecuteProcess.
+For full workflow, run each step manually (see jetson/calibration/README.md).
+
+Usage:
+  ros2 launch saltybot_calibration calibration_capture.launch.py camera:=front
+  ros2 launch saltybot_calibration calibration_capture.launch.py camera:=all
+
+Arguments:
+  camera        Camera to capture (front/right/rear/left/all, default: front)
+  n_images      Images to capture per camera (default: 30)
+  output_dir    Output directory (default: /mnt/nvme/saltybot/calibration/raw)
+  board_size    Checkerboard inner corners WxH (default: 8x6)
+  square_size   Physical square size in metres (default: 0.025)
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, LogInfo
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    camera_arg = DeclareLaunchArgument(
+        'camera', default_value='front',
+        description='Camera to capture from: front/right/rear/left/all',
+    )
+    n_images_arg = DeclareLaunchArgument(
+        'n_images', default_value='30',
+        description='Number of calibration images to capture per camera',
+    )
+    output_dir_arg = DeclareLaunchArgument(
+        'output_dir', default_value='/mnt/nvme/saltybot/calibration/raw',
+        description='Root output directory for captured images',
+    )
+    board_size_arg = DeclareLaunchArgument(
+        'board_size', default_value='8x6',
+        description='Checkerboard inner corners as WxH',
+    )
+    square_size_arg = DeclareLaunchArgument(
+        'square_size', default_value='0.025',
+        description='Physical checkerboard square size in metres',
+    )
+
+    pkg_share = FindPackageShare('saltybot_calibration')
+    capture_script = PathJoinSubstitution([
+        pkg_share, '..', '..', '..', 'lib', 'saltybot_calibration',
+        'capture_calibration_images.py',
+    ])
+
+    capture_process = ExecuteProcess(
+        cmd=[
+            'python3', capture_script,
+            '--ros',
+            '--camera', LaunchConfiguration('camera'),
+            '--n-images', LaunchConfiguration('n_images'),
+            '--output-dir', LaunchConfiguration('output_dir'),
+            '--board-size', LaunchConfiguration('board_size'),
+            '--square-size', LaunchConfiguration('square_size'),
+        ],
+        output='screen',
+    )
+
+    workflow_info = LogInfo(
+        msg=[
+            '\n',
+            '=== IMX219 Calibration Workflow ===\n',
+            'Step 1 (this launch): Capture checkerboard images\n',
+            '  Controls: SPACE=capture  r=reset  q=done\n',
+            '\n',
+            'Step 2: Calibrate intrinsics per camera:\n',
+            '  ros2 run saltybot_calibration calibrate_camera.py \\\n',
+            '    --images-dir /mnt/nvme/saltybot/calibration/raw/cam_front \\\n',
+            '    --output /mnt/nvme/saltybot/calibration/front/camera_info.yaml \\\n',
+            '    --camera-name camera_front --image-size 640x480\n',
+            '\n',
+            'Step 3: Compute extrinsics:\n',
+            '  ros2 run saltybot_calibration calibrate_extrinsics.py \\\n',
+            '    --mode mechanical \\\n',
+            '    --output /mnt/nvme/saltybot/calibration/extrinsics.yaml\n',
+            '\n',
+            'Step 4: Generate static transforms launch file:\n',
+            '  ros2 run saltybot_calibration generate_static_transforms.py \\\n',
+            '    --extrinsics /mnt/nvme/saltybot/calibration/extrinsics.yaml \\\n',
+            '    --output ~/ros2_ws/src/saltybot_bringup/launch/static_transforms.launch.py\n',
+            '\n',
+            'See jetson/calibration/README.md for full instructions.\n',
+        ]
+    )
+
+    return LaunchDescription([
+        camera_arg,
+        n_images_arg,
+        output_dir_arg,
+        board_size_arg,
+        square_size_arg,
+        workflow_info,
+        capture_process,
+    ])

jetson/ros2_ws/src/saltybot_calibration/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_calibration</name>
+  <version>0.1.0</version>
+  <description>IMX219 camera intrinsic + extrinsic calibration workflow for saltybot</description>
+  <maintainer email="seb@vayrette.com">seb</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <exec_depend>python3-opencv</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_calibration/saltybot_calibration/camera_calibrator.py

@@ -0,0 +1,175 @@
+"""camera_calibrator.py — OpenCV fisheye intrinsic calibration for IMX219 cameras.
+
+Uses cv2.fisheye model (Scaramuzza/Kannala-Brandt 4-param distortion):
+  k1, k2, k3, k4 — equidistant distortion coefficients
+
+Outputs camera_info_manager compatible YAML:
+  image_width, image_height, camera_name
+  camera_matrix:  3x3 K
+  distortion_model: equidistant
+  distortion_coefficients: [k1, k2, k3, k4]
+  rectification_matrix: 3x3 I
+  projection_matrix: 3x4 P (same as K padded)
+"""
+
+import cv2
+import numpy as np
+import yaml
+import os
+from pathlib import Path
+
+
+def find_checkerboard(img_bgr: np.ndarray, board_size: tuple,
+                      flags: int = None) -> tuple:
+    """Detect checkerboard corners in image.
+
+    Args:
+        img_bgr: BGR image
+        board_size: (cols, rows) inner corners, e.g. (8, 6)
+        flags: cv2.findChessboardCorners flags
+
+    Returns:
+        (found, corners) — corners is [N, 1, 2] float32 or None
+    """
+    gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY)
+    if flags is None:
+        flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE
+    found, corners = cv2.findChessboardCorners(gray, board_size, flags)
+    if found:
+        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 50, 0.001)
+        corners = cv2.cornerSubPix(gray, corners, (7, 7), (-1, -1), criteria)
+    return found, corners
+
+
+def calibrate_fisheye(
+    image_paths: list,
+    board_size: tuple,
+    square_size_m: float,
+    image_size: tuple,  # (width, height)
+) -> dict:
+    """Run cv2.fisheye calibration.
+
+    Args:
+        image_paths: list of calibration image file paths
+        board_size: (cols, rows) inner corners
+        square_size_m: physical square size in metres (e.g. 0.025)
+        image_size: (width, height) of images
+
+    Returns:
+        dict with keys: K, D, rvecs, tvecs, rms, n_images_used
+    """
+    obj_p = np.zeros((board_size[0] * board_size[1], 1, 3), dtype=np.float64)
+    obj_p[:, 0, :2] = np.mgrid[0:board_size[0], 0:board_size[1]].T.reshape(-1, 2)
+    obj_p *= square_size_m
+
+    obj_pts = []
+    img_pts = []
+    used = 0
+    skipped = 0
+
+    for path in image_paths:
+        img = cv2.imread(path)
+        if img is None:
+            print(f'  [WARN] Cannot read: {path}')
+            skipped += 1
+            continue
+        found, corners = find_checkerboard(img, board_size)
+        if not found:
+            print(f'  [SKIP] No corners: {os.path.basename(path)}')
+            skipped += 1
+            continue
+        obj_pts.append(obj_p)
+        img_pts.append(corners)
+        used += 1
+        print(f'  [OK]   {os.path.basename(path)}')
+
+    if used < 5:
+        raise RuntimeError(
+            f'Only {used} valid images (need >=5). Check lighting and board visibility.'
+        )
+
+    K = np.zeros((3, 3))
+    D = np.zeros((4, 1))
+    flags = (
+        cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC |
+        cv2.fisheye.CALIB_CHECK_COND |
+        cv2.fisheye.CALIB_FIX_SKEW
+    )
+    rms, K, D, rvecs, tvecs = cv2.fisheye.calibrate(
+        obj_pts, img_pts, image_size, K, D, flags=flags
+    )
+    print(f'  Calibration RMS reprojection error: {rms:.4f} px ({used} images)')
+    return {
+        'K': K, 'D': D, 'rvecs': rvecs, 'tvecs': tvecs,
+        'rms': rms, 'n_images_used': used, 'n_images_skipped': skipped,
+        'obj_pts': obj_pts, 'img_pts': img_pts,
+    }
+
+
+def write_camera_info_yaml(
+    output_path: str,
+    camera_name: str,
+    image_size: tuple,  # (width, height)
+    K: np.ndarray,
+    D: np.ndarray,
+    rms: float,
+) -> None:
+    """Write camera_info_manager compatible YAML.
+
+    Format compatible with camera_info_manager CameraInfoManager.loadCameraInfo().
+    distortion_model = 'equidistant' (ROS2 fisheye standard).
+    D has 4 coefficients [k1, k2, k3, k4] for fisheye model.
+    """
+    w, h = image_size
+    fx = float(K[0, 0])
+    fy = float(K[1, 1])
+    cx = float(K[0, 2])
+    cy = float(K[1, 2])
+
+    data = {
+        'image_width':  w,
+        'image_height': h,
+        'camera_name':  camera_name,
+        'camera_matrix': {
+            'rows': 3, 'cols': 3,
+            'data': [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0],
+        },
+        'distortion_model': 'equidistant',
+        'distortion_coefficients': {
+            'rows': 1, 'cols': 4,
+            'data': [float(D[0, 0]), float(D[1, 0]),
+                     float(D[2, 0]), float(D[3, 0])],
+        },
+        'rectification_matrix': {
+            'rows': 3, 'cols': 3,
+            'data': [1.0, 0.0, 0.0,
+                     0.0, 1.0, 0.0,
+                     0.0, 0.0, 1.0],
+        },
+        'projection_matrix': {
+            'rows': 3, 'cols': 4,
+            'data': [fx, 0.0, cx, 0.0,
+                     0.0, fy, cy, 0.0,
+                     0.0, 0.0, 1.0, 0.0],
+        },
+        # Metadata (not part of camera_info_manager spec, but informational)
+        '_calibration_rms_px': round(rms, 4),
+    }
+
+    Path(output_path).parent.mkdir(parents=True, exist_ok=True)
+    with open(output_path, 'w') as f:
+        yaml.dump(data, f, default_flow_style=False, sort_keys=False)
+    print(f'  Written: {output_path}')
+
+
+def compute_reprojection_errors(
+    obj_pts: list, img_pts: list, rvecs: list, tvecs: list,
+    K: np.ndarray, D: np.ndarray,
+) -> list:
+    """Compute per-image reprojection errors for fisheye model."""
+    errors = []
+    for i, (op, ip) in enumerate(zip(obj_pts, img_pts)):
+        projected, _ = cv2.fisheye.projectPoints(op, rvecs[i], tvecs[i], K, D)
+        err = cv2.norm(ip, projected, cv2.NORM_L2) / np.sqrt(len(ip))
+        errors.append(err)
+    return errors

jetson/ros2_ws/src/saltybot_calibration/saltybot_calibration/extrinsic_calibrator.py

@@ -0,0 +1,139 @@
+"""extrinsic_calibrator.py — Extrinsic calibration between 4 IMX219 cameras.
+
+Method: overlapping stereo calibration using shared checkerboard views.
+Each adjacent pair (cam0-cam1, cam1-cam2, cam2-cam3, cam3-cam0) is calibrated
+via cv2.fisheye.stereoCalibrate to find R, T between them.
+
+Then each camera's pose relative to base_link is computed by chaining transforms,
+anchoring on the known mechanical mount geometry (4 cameras at 90° intervals,
+radius 0.05m from centre, all at height 0.30m).
+
+Output: extrinsics.yaml with camera-to-base transforms as translation + quaternion.
+Also outputs static_transforms.launch.py.
+"""
+
+import cv2
+import numpy as np
+import yaml
+import math
+from pathlib import Path
+
+
+def rotation_matrix_to_quaternion(R: np.ndarray) -> tuple:
+    """Convert 3x3 rotation matrix to (qx, qy, qz, qw)."""
+    trace = R[0, 0] + R[1, 1] + R[2, 2]
+    if trace > 0:
+        s = 0.5 / math.sqrt(trace + 1.0)
+        w = 0.25 / s
+        x = (R[2, 1] - R[1, 2]) * s
+        y = (R[0, 2] - R[2, 0]) * s
+        z = (R[1, 0] - R[0, 1]) * s
+    elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+        s = 2.0 * math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
+        w = (R[2, 1] - R[1, 2]) / s
+        x = 0.25 * s
+        y = (R[0, 1] + R[1, 0]) / s
+        z = (R[0, 2] + R[2, 0]) / s
+    elif R[1, 1] > R[2, 2]:
+        s = 2.0 * math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
+        w = (R[0, 2] - R[2, 0]) / s
+        x = (R[0, 1] + R[1, 0]) / s
+        y = 0.25 * s
+        z = (R[1, 2] + R[2, 1]) / s
+    else:
+        s = 2.0 * math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
+        w = (R[1, 0] - R[0, 1]) / s
+        x = (R[0, 2] + R[2, 0]) / s
+        y = (R[1, 2] + R[2, 1]) / s
+        z = 0.25 * s
+    return (x, y, z, w)
+
+
+def mechanical_extrinsics(
+    mount_radius_m: float = 0.05,
+    mount_height_m: float = 0.30,
+    cam_yaw_degs: list = None,
+) -> dict:
+    """
+    Compute camera-to-base_link transforms from known mount geometry.
+
+    Camera mount: ring of radius mount_radius_m at height mount_height_m.
+    Cameras face outward (yaw angle determines facing direction).
+    Camera optical axis is +Z (into scene), x-right, y-down.
+    base_link: x=forward, y=left, z=up (ROS convention).
+
+    Transform = cam_optical_frame -> base_link
+
+    Returns dict: {cam_name: {translation: [x,y,z], rotation: [qx,qy,qz,qw]}}
+    """
+    if cam_yaw_degs is None:
+        cam_yaw_degs = [0.0, -90.0, 180.0, 90.0]  # front, right, rear, left
+
+    cam_names = ['front', 'right', 'rear', 'left']
+    result = {}
+
+    for name, yaw_deg in zip(cam_names, cam_yaw_degs):
+        yaw_rad = math.radians(yaw_deg)
+
+        # Camera optical centre position relative to base_link
+        tx = mount_radius_m * math.cos(yaw_rad)   # forward component
+        ty = -mount_radius_m * math.sin(yaw_rad)  # left component (ROS y=left)
+        tz = mount_height_m                         # up
+
+        # Rotation: camera optical frame (+Z out, +X right, +Y down)
+        # -> base_link frame (+X forward, +Y left, +Z up)
+        # Camera Z (optical) = robot X rotated by yaw
+        # This is the transform from base_link to camera_optical_frame:
+        #   Rz(yaw) * Rx(90 deg)   [tilt camera to face out + coordinate swap]
+
+        # Rotation from camera_optical to base_link (inverse):
+        # camera_optical: z=forward, x=right, y=down
+        # base_link:      x=forward, y=left,  z=up
+        # R_cam_to_base_unrotated = [[0,-1,0],[0,0,-1],[1,0,0]] (optical->ROS)
+        # Then rotate by -yaw around z to get directional orientation
+
+        cos_y = math.cos(yaw_rad)
+        sin_y = math.sin(yaw_rad)
+
+        # Full rotation: R_z(-yaw) * R_cam_optical_to_base_unrotated
+        # R_cam_optical_to_base_unrotated maps: x_cam->-y_base, y_cam->-z_base, z_cam->x_base
+        # After yaw rotation:
+        R = np.array([
+            [cos_y,  sin_y, 0],
+            [-sin_y, cos_y, 0],
+            [0,      0,     1],
+        ]) @ np.array([
+            [0,  0, 1],
+            [-1, 0, 0],
+            [0, -1, 0],
+        ])
+
+        qx, qy, qz, qw = rotation_matrix_to_quaternion(R)
+        result[name] = {
+            'translation': [round(tx, 4), round(ty, 4), round(tz, 4)],
+            'rotation_quat': [round(qx, 6), round(qy, 6),
+                              round(qz, 6), round(qw, 6)],
+        }
+    return result
+
+
+def write_extrinsics_yaml(output_path: str, extrinsics: dict,
+                          mount_radius_m: float, mount_height_m: float) -> None:
+    """Write extrinsics.yaml."""
+    data = {
+        '_description': 'Camera extrinsics — base_link to camera_optical_frame transforms',
+        '_mount_radius_m': mount_radius_m,
+        '_mount_height_m': mount_height_m,
+        'cameras': {},
+    }
+    for name, ex in extrinsics.items():
+        data['cameras'][name] = {
+            'parent_frame': 'base_link',
+            'child_frame': f'camera_{name}_optical_frame',
+            'translation': ex['translation'],
+            'rotation_quat_xyzw': ex['rotation_quat'],
+        }
+    Path(output_path).parent.mkdir(parents=True, exist_ok=True)
+    with open(output_path, 'w') as f:
+        yaml.dump(data, f, default_flow_style=False, sort_keys=False)
+    print(f'Written: {output_path}')

jetson/ros2_ws/src/saltybot_calibration/scripts/calibrate_camera.py

@@ -0,0 +1,215 @@
+#!/usr/bin/env python3
+"""
+calibrate_camera.py — Run OpenCV fisheye calibration from captured images.
+
+Usage:
+  python3 calibrate_camera.py \\
+    --images-dir /path/to/cal/cam_front \\
+    --output /path/to/calibration/front/camera_info.yaml \\
+    --camera-name camera_front \\
+    --board-size 8x6 \\
+    --square-size 0.025 \\
+    --image-size 640x480 \\
+    [--show-reprojection]
+"""
+
+import argparse
+import os
+import sys
+from pathlib import Path
+
+import cv2
+import numpy as np
+
+
+def parse_size(s: str) -> tuple:
+    """Parse 'WxH' string to (width, height) tuple of ints."""
+    parts = s.lower().split('x')
+    if len(parts) != 2:
+        raise argparse.ArgumentTypeError(f'Size must be WxH, got: {s}')
+    return (int(parts[0]), int(parts[1]))
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description='Compute IMX219 fisheye camera intrinsics from checkerboard images.',
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog=__doc__,
+    )
+    parser.add_argument(
+        '--images-dir', required=True,
+        help='Directory containing captured calibration images (*.png, *.jpg).',
+    )
+    parser.add_argument(
+        '--output', required=True,
+        help='Output path for camera_info.yaml.',
+    )
+    parser.add_argument(
+        '--camera-name', required=True,
+        help='Camera name for YAML header (e.g. camera_front).',
+    )
+    parser.add_argument(
+        '--board-size', type=str, default='8x6',
+        help='Checkerboard inner corners as WxH (default: 8x6).',
+    )
+    parser.add_argument(
+        '--square-size', type=float, default=0.025,
+        help='Physical square size in metres (default: 0.025 = 25mm).',
+    )
+    parser.add_argument(
+        '--image-size', type=str, default='640x480',
+        help='Image dimensions as WxH (default: 640x480).',
+    )
+    parser.add_argument(
+        '--show-reprojection', action='store_true',
+        help='Display original vs reprojected corners for each image.',
+    )
+    args = parser.parse_args()
+
+    # Parse board and image sizes
+    bw, bh = parse_size(args.board_size)
+    board_size = (bw, bh)
+    image_size = parse_size(args.image_size)
+
+    # Collect image paths
+    images_dir = Path(args.images_dir)
+    if not images_dir.exists():
+        print(f'[ERROR] Images directory not found: {images_dir}')
+        sys.exit(1)
+
+    image_paths = sorted(
+        [str(p) for p in images_dir.glob('*.png')] +
+        [str(p) for p in images_dir.glob('*.jpg')] +
+        [str(p) for p in images_dir.glob('*.jpeg')]
+    )
+    if not image_paths:
+        print(f'[ERROR] No images found in: {images_dir}')
+        sys.exit(1)
+
+    print(f'Board size:   {board_size[0]}x{board_size[1]} inner corners')
+    print(f'Square size:  {args.square_size * 1000:.0f}mm')
+    print(f'Image size:   {image_size[0]}x{image_size[1]}')
+    print(f'Images found: {len(image_paths)}')
+    print(f'Output:       {args.output}')
+    print()
+
+    # Run calibration using camera_calibrator module
+    # Add package to path for standalone use
+    script_dir = Path(__file__).resolve().parent
+    pkg_dir = script_dir.parent
+    if str(pkg_dir) not in sys.path:
+        sys.path.insert(0, str(pkg_dir))
+
+    from saltybot_calibration.camera_calibrator import (
+        calibrate_fisheye,
+        write_camera_info_yaml,
+        compute_reprojection_errors,
+    )
+
+    print('Processing images...')
+    result = calibrate_fisheye(image_paths, board_size, args.square_size, image_size)
+
+    K = result['K']
+    D = result['D']
+    rms = result['rms']
+    rvecs = result['rvecs']
+    tvecs = result['tvecs']
+    obj_pts = result['obj_pts']
+    img_pts = result['img_pts']
+    n_used = result['n_images_used']
+    n_skipped = result['n_images_skipped']
+
+    # Per-image reprojection errors
+    errors = compute_reprojection_errors(obj_pts, img_pts, rvecs, tvecs, K, D)
+
+    # Summary table
+    print()
+    print(f'{"Image":<40}  {"RMS error (px)":>14}')
+    print('-' * 57)
+    for path, err in zip(
+        [p for p in image_paths if Path(p).name not in
+         {Path(p).name for p in image_paths if not Path(p).exists()}],
+        errors
+    ):
+        flag = '  <<' if err > 2.0 else ''
+        print(f'  {Path(path).name:<38}  {err:>14.4f}{flag}')
+
+    print('-' * 57)
+    print(f'  {"Overall RMS":<38}  {rms:>14.4f}')
+    print(f'  {"Images used":<38}  {n_used:>14}')
+    print(f'  {"Images skipped":<38}  {n_skipped:>14}')
+    print()
+
+    # Warn on high reprojection error
+    if rms > 1.5:
+        print(f'[WARN] RMS error {rms:.4f} px > 1.5 px threshold.')
+        print('  Suggestions:')
+        print('  - Remove images with corners at extreme edges (strong fisheye distortion)')
+        print('  - Ensure board is flat and rigidly printed (not bent)')
+        print('  - Increase n_images and vary board orientation more')
+        print('  - Check that --image-size matches actual capture resolution')
+    else:
+        print(f'[OK] RMS error {rms:.4f} px — good calibration.')
+
+    # Intrinsics summary
+    print()
+    print('Intrinsics:')
+    print(f'  fx={K[0,0]:.2f}  fy={K[1,1]:.2f}  cx={K[0,2]:.2f}  cy={K[1,2]:.2f}')
+    print(f'  D=[{D[0,0]:.6f}, {D[1,0]:.6f}, {D[2,0]:.6f}, {D[3,0]:.6f}]')
+    print()
+
+    # Write YAML
+    write_camera_info_yaml(
+        output_path=args.output,
+        camera_name=args.camera_name,
+        image_size=image_size,
+        K=K,
+        D=D,
+        rms=rms,
+    )
+
+    # Show reprojection visualisation if requested
+    if args.show_reprojection:
+        print('Showing reprojection errors. Press any key to advance, q to quit.')
+        for i, (path, err) in enumerate(zip(
+            [p for p in image_paths],
+            errors + [0.0] * (len(image_paths) - len(errors))
+        )):
+            img = cv2.imread(path)
+            if img is None:
+                continue
+
+            # Project points back
+            projected, _ = cv2.fisheye.projectPoints(
+                obj_pts[i] if i < len(obj_pts) else None,
+                rvecs[i] if i < len(rvecs) else None,
+                tvecs[i] if i < len(tvecs) else None,
+                K, D
+            ) if i < len(obj_pts) else (None, None)
+
+            vis = img.copy()
+            if projected is not None and i < len(img_pts):
+                # Draw original corners (green)
+                for pt in img_pts[i]:
+                    cv2.circle(vis, tuple(pt[0].astype(int)), 5, (0, 255, 0), 2)
+                # Draw reprojected corners (red)
+                for pt in projected:
+                    cv2.circle(vis, tuple(pt[0].astype(int)), 3, (0, 0, 255), -1)
+
+            cv2.putText(vis, f'{Path(path).name}  RMS={err:.3f}px', (10, 30),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2)
+            cv2.putText(vis, 'Green=detected  Red=reprojected', (10, 60),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.6, (200, 200, 200), 1)
+
+            cv2.imshow('Reprojection', vis)
+            key = cv2.waitKey(0) & 0xFF
+            if key == ord('q'):
+                break
+
+        cv2.destroyAllWindows()
+
+    print('Calibration complete.')
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_calibration/scripts/calibrate_extrinsics.py

@@ -0,0 +1,238 @@
+#!/usr/bin/env python3
+"""
+calibrate_extrinsics.py — Compute camera-to-base_link extrinsic transforms.
+
+Two modes:
+  1. --mode mechanical: use known mount geometry (radius, height, yaw angles)
+     -> produces analytically exact transforms from CAD measurements
+  2. --mode stereo: use cv2.fisheye.stereoCalibrate on paired checkerboard images
+     -> requires shared-view images from adjacent camera pairs
+
+Usage (mechanical mode, recommended first pass):
+  python3 calibrate_extrinsics.py \\
+    --mode mechanical \\
+    --mount-radius 0.05 \\
+    --mount-height 0.30 \\
+    --output /path/to/calibration/extrinsics.yaml \\
+    --generate-launch /path/to/jetson/launch/static_transforms.launch.py
+"""
+
+import argparse
+import math
+import sys
+from pathlib import Path
+
+
+def print_extrinsics_table(extrinsics: dict) -> None:
+    """Print formatted extrinsics table to stdout."""
+    print()
+    print(f'{"Camera":<8}  {"tx":>8}  {"ty":>8}  {"tz":>8}  '
+          f'{"qx":>10}  {"qy":>10}  {"qz":>10}  {"qw":>10}')
+    print('-' * 86)
+    for name, ex in extrinsics.items():
+        tx, ty, tz = ex['translation']
+        qx, qy, qz, qw = ex['rotation_quat']
+        print(f'  {name:<6}  {tx:>8.4f}  {ty:>8.4f}  {tz:>8.4f}  '
+              f'{qx:>10.6f}  {qy:>10.6f}  {qz:>10.6f}  {qw:>10.6f}')
+    print()
+
+
+def generate_static_transforms_launch(extrinsics: dict, output_path: str) -> None:
+    """Generate static_transforms.launch.py from extrinsics dict."""
+    # Add package to path
+    script_dir = Path(__file__).resolve().parent
+    pkg_dir = script_dir.parent
+    if str(pkg_dir) not in sys.path:
+        sys.path.insert(0, str(pkg_dir))
+
+    from saltybot_calibration.extrinsic_calibrator import write_extrinsics_yaml
+
+    lines = [
+        '"""',
+        'static_transforms.launch.py — Auto-generated by calibrate_extrinsics.py',
+        'DO NOT EDIT manually — regenerate via:',
+        '  python3 calibrate_extrinsics.py --mode mechanical --generate-launch <this_file>',
+        '"""',
+        '',
+        'from launch import LaunchDescription',
+        'from launch_ros.actions import Node',
+        '',
+        '',
+        'def generate_launch_description():',
+        '    return LaunchDescription([',
+    ]
+
+    for name, ex in extrinsics.items():
+        tx, ty, tz = ex['translation']
+        qx, qy, qz, qw = ex['rotation_quat']
+        child_frame = f'camera_{name}_optical_frame'
+        lines += [
+            f'        # camera_{name}: base_link -> {child_frame}',
+            f'        Node(',
+            f'            package=\'tf2_ros\',',
+            f'            executable=\'static_transform_publisher\',',
+            f'            name=\'tf_base_to_camera_{name}\',',
+            f'            arguments=[',
+            f'                \'{tx}\', \'{ty}\', \'{tz}\',',
+            f'                \'{qx}\', \'{qy}\', \'{qz}\', \'{qw}\',',
+            f'                \'base_link\', \'{child_frame}\',',
+            f'            ],',
+            f'        ),',
+        ]
+
+    lines += [
+        '    ])',
+        '',
+    ]
+
+    Path(output_path).parent.mkdir(parents=True, exist_ok=True)
+    with open(output_path, 'w') as f:
+        f.write('\n'.join(lines))
+    print(f'Generated launch file: {output_path}')
+
+
+def run_mechanical_mode(args) -> None:
+    """Run mechanical extrinsics calculation."""
+    script_dir = Path(__file__).resolve().parent
+    pkg_dir = script_dir.parent
+    if str(pkg_dir) not in sys.path:
+        sys.path.insert(0, str(pkg_dir))
+
+    from saltybot_calibration.extrinsic_calibrator import (
+        mechanical_extrinsics,
+        write_extrinsics_yaml,
+    )
+
+    print(f'Mode:          mechanical')
+    print(f'Mount radius:  {args.mount_radius * 100:.1f}cm')
+    print(f'Mount height:  {args.mount_height * 100:.1f}cm')
+    print(f'Camera yaw angles: front=0 deg, right=-90 deg, rear=180 deg, left=90 deg')
+    print()
+
+    extrinsics = mechanical_extrinsics(
+        mount_radius_m=args.mount_radius,
+        mount_height_m=args.mount_height,
+    )
+
+    print_extrinsics_table(extrinsics)
+
+    write_extrinsics_yaml(
+        output_path=args.output,
+        extrinsics=extrinsics,
+        mount_radius_m=args.mount_radius,
+        mount_height_m=args.mount_height,
+    )
+
+    if args.generate_launch:
+        generate_static_transforms_launch(extrinsics, args.generate_launch)
+
+
+def run_stereo_mode(args) -> None:
+    """Stereo extrinsics calibration — stub for future implementation.
+
+    TODO: Implement cv2.fisheye.stereoCalibrate for adjacent camera pairs.
+
+    This mode requires:
+      - Shared-view checkerboard images from each adjacent camera pair:
+          {images_dir}/pair_front_right/*.png  (visible to both cam0 and cam1)
+          {images_dir}/pair_right_rear/*.png
+          {images_dir}/pair_rear_left/*.png
+          {images_dir}/pair_left_front/*.png
+      - Per-camera intrinsics (camera_info.yaml) for each camera
+      - Each pair: cv2.fisheye.stereoCalibrate -> R, T between cameras
+      - Chain transforms: cam0->cam1->cam2->cam3, anchor to base_link via
+        mechanical position of cam0
+
+    Recommended workflow:
+      1. First run --mode mechanical to get approximate transforms
+      2. Mount robot in place, place checkerboard in overlap zones
+      3. Capture simultaneous images from adjacent pairs (requires hardware sync
+         or approximate temporal sync)
+      4. Run --mode stereo to refine R, T
+      5. Run generate_static_transforms.py to update launch file
+
+    For now, use --mode mechanical which gives correct transforms from CAD
+    dimensions (typically accurate to ~5mm / ~1 deg for a rigid mount).
+    """
+    print('[ERROR] Stereo mode is not yet implemented.')
+    print()
+    print('Use --mode mechanical for now (accurate from CAD measurements).')
+    print()
+    print('Future implementation plan:')
+    print('  1. Capture simultaneous checkerboard images from adjacent camera pairs')
+    print('     in their overlapping FoV regions (approx 70 deg overlap per pair)')
+    print('  2. For each pair: cv2.fisheye.stereoCalibrate with per-camera intrinsics')
+    print('  3. Chain transforms cam0->cam1->cam2->cam3, anchor to base_link')
+    print('  4. Refine against mechanical prior using least-squares optimisation')
+    print()
+    print('Required directory structure for stereo mode:')
+    print('  {images_dir}/pair_front_right/  — images visible to both front+right')
+    print('  {images_dir}/pair_right_rear/')
+    print('  {images_dir}/pair_rear_left/')
+    print('  {images_dir}/pair_left_front/')
+    sys.exit(1)
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description='Compute camera-to-base_link extrinsic transforms for 4x IMX219 cameras.',
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog=__doc__,
+    )
+    parser.add_argument(
+        '--mode', choices=['mechanical', 'stereo'], default='mechanical',
+        help='Calibration mode: mechanical (from CAD) or stereo (from images). '
+             'Default: mechanical.',
+    )
+    parser.add_argument(
+        '--mount-radius', type=float, default=0.05,
+        help='Camera mount ring radius in metres (default: 0.05).',
+    )
+    parser.add_argument(
+        '--mount-height', type=float, default=0.30,
+        help='Camera mount height above base_link in metres (default: 0.30).',
+    )
+    parser.add_argument(
+        '--output', required=True,
+        help='Output path for extrinsics.yaml.',
+    )
+    parser.add_argument(
+        '--generate-launch', type=str, default=None,
+        metavar='LAUNCH_FILE',
+        help='Also generate static_transforms.launch.py at this path.',
+    )
+    # Stereo mode arguments (for future use)
+    parser.add_argument(
+        '--images-dir', type=str, default=None,
+        help='[stereo mode] Directory containing paired calibration images.',
+    )
+    parser.add_argument(
+        '--intrinsics-dir', type=str, default=None,
+        help='[stereo mode] Directory containing per-camera camera_info.yaml files.',
+    )
+
+    args = parser.parse_args()
+
+    print('=== IMX219 Extrinsic Calibration ===')
+    print()
+
+    if args.mode == 'mechanical':
+        run_mechanical_mode(args)
+    elif args.mode == 'stereo':
+        run_stereo_mode(args)
+
+    print()
+    print('Extrinsics calibration complete.')
+    print(f'Output: {args.output}')
+    if args.generate_launch:
+        print(f'Launch: {args.generate_launch}')
+    print()
+    print('Next steps:')
+    print('  1. Update saltybot_surround/config/surround_vision_params.yaml')
+    print('     with camera_info_url paths pointing to calibrated YAMLs')
+    print('  2. Run generate_static_transforms.py (or use --generate-launch above)')
+    print('  3. Restart saltybot_cameras and saltybot_surround nodes')
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_calibration/scripts/capture_calibration_images.py

@@ -0,0 +1,316 @@
+#!/usr/bin/env python3
+"""
+capture_calibration_images.py — Capture checkerboard calibration images from IMX219 cameras.
+
+Usage (standalone, no ROS2 required — uses OpenCV VideoCapture for CSI):
+  python3 capture_calibration_images.py --camera front --n-images 30 --output-dir /tmp/cal
+
+Usage (with ROS2 — subscribes to image topic):
+  python3 capture_calibration_images.py --ros --camera front --n-images 30
+
+Controls:
+  SPACE  — capture current frame (if checkerboard detected)
+  q      — quit / done
+  r      — reset captured images for this camera
+
+Outputs: output_dir/cam_{name}/frame_{NNNN}.png
+"""
+
+import argparse
+import os
+import sys
+from pathlib import Path
+
+import cv2
+import numpy as np
+
+
+CAMERA_TOPICS = {
+    'front': '/camera/front/image_raw',
+    'right': '/camera/right/image_raw',
+    'rear':  '/camera/rear/image_raw',
+    'left':  '/camera/left/image_raw',
+}
+
+ALL_CAMERAS = ['front', 'right', 'rear', 'left']
+
+
+def parse_board_size(s: str) -> tuple:
+    """Parse 'WxH' string to (cols, rows) tuple of ints."""
+    parts = s.lower().split('x')
+    if len(parts) != 2:
+        raise argparse.ArgumentTypeError(f'board-size must be WxH, got: {s}')
+    return (int(parts[0]), int(parts[1]))
+
+
+def find_checkerboard(img_bgr: np.ndarray, board_size: tuple):
+    """Detect checkerboard corners. Returns (found, corners, gray)."""
+    gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY)
+    flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE
+    found, corners = cv2.findChessboardCorners(gray, board_size, flags)
+    if found:
+        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+        corners = cv2.cornerSubPix(gray, corners, (7, 7), (-1, -1), criteria)
+    return found, corners, gray
+
+
+def draw_overlay(img: np.ndarray, board_size: tuple, found: bool,
+                 corners, captured: int, n_images: int) -> np.ndarray:
+    """Draw corner detections and status overlay onto a copy of img."""
+    display = img.copy()
+    if found and corners is not None:
+        cv2.drawChessboardCorners(display, board_size, corners, found)
+
+    # Status bar background
+    cv2.rectangle(display, (0, 0), (display.shape[1], 36), (0, 0, 0), -1)
+
+    status_color = (0, 255, 0) if found else (0, 80, 220)
+    status_text = 'BOARD DETECTED' if found else 'searching for board...'
+    cv2.putText(display, status_text, (10, 24),
+                cv2.FONT_HERSHEY_SIMPLEX, 0.7, status_color, 2)
+
+    count_text = f'Captured: {captured}/{n_images}  [SPACE=capture  r=reset  q=quit]'
+    cv2.putText(display, count_text, (display.shape[1] // 2 - 10, 24),
+                cv2.FONT_HERSHEY_SIMPLEX, 0.55, (200, 200, 200), 1)
+
+    if captured >= n_images:
+        cv2.rectangle(display, (0, 0), (display.shape[1], display.shape[0]),
+                      (0, 200, 0), 8)
+        cv2.putText(display, 'DONE! Press q to finish.', (60, display.shape[0] // 2),
+                    cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3)
+
+    return display
+
+
+def capture_loop_opencv(cam_name: str, device: str, board_size: tuple,
+                        n_images: int, output_dir: Path) -> int:
+    """Capture calibration images using cv2.VideoCapture (non-ROS mode).
+
+    Returns number of images captured.
+    """
+    out_dir = output_dir / f'cam_{cam_name}'
+    out_dir.mkdir(parents=True, exist_ok=True)
+
+    cap = cv2.VideoCapture(device)
+    if not cap.isOpened():
+        print(f'[ERROR] Cannot open device: {device}')
+        return 0
+
+    # Try to set resolution
+    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+
+    captured = 0
+    frame_idx = 0
+    window_title = f'Calibration Capture — {cam_name} | SPACE=capture q=quit r=reset'
+    cv2.namedWindow(window_title, cv2.WINDOW_NORMAL)
+
+    print(f'\n[{cam_name.upper()}] Capturing {n_images} images. Output: {out_dir}')
+    print('  Point the checkerboard at the camera, press SPACE when board is detected.')
+
+    while captured < n_images:
+        ret, frame = cap.read()
+        if not ret:
+            print('[WARN] Failed to read frame')
+            continue
+
+        found, corners, _ = find_checkerboard(frame, board_size)
+        display = draw_overlay(frame, board_size, found, corners, captured, n_images)
+        cv2.imshow(window_title, display)
+
+        key = cv2.waitKey(1) & 0xFF
+        if key == ord('q'):
+            print(f'  Quit requested. Captured {captured}/{n_images} images.')
+            break
+        elif key == ord('r'):
+            # Remove all saved images and reset counter
+            for f in out_dir.glob('frame_*.png'):
+                f.unlink()
+            captured = 0
+            frame_idx = 0
+            print(f'  [RESET] Cleared captured images for {cam_name}.')
+        elif key == ord(' '):
+            if found:
+                save_path = out_dir / f'frame_{frame_idx:04d}.png'
+                cv2.imwrite(str(save_path), frame)
+                captured += 1
+                frame_idx += 1
+                print(f'  Saved ({captured}/{n_images}): {save_path.name}')
+                # Brief flash to confirm capture
+                flash = frame.copy()
+                cv2.rectangle(flash, (0, 0), (flash.shape[1], flash.shape[0]),
+                              (0, 255, 0), 12)
+                cv2.imshow(window_title, flash)
+                cv2.waitKey(120)
+            else:
+                print('  [SKIP] No checkerboard detected — move board into view.')
+
+    cap.release()
+    cv2.destroyWindow(window_title)
+    print(f'  Done: {captured} images saved to {out_dir}')
+    return captured
+
+
+def capture_loop_ros(cam_name: str, board_size: tuple,
+                     n_images: int, output_dir: Path) -> int:
+    """Capture calibration images via ROS2 topic subscription.
+
+    Returns number of images captured.
+    """
+    try:
+        import rclpy
+        from rclpy.node import Node
+        from sensor_msgs.msg import Image
+        from cv_bridge import CvBridge
+    except ImportError as e:
+        print(f'[ERROR] ROS2 not available: {e}')
+        print('  Run without --ros for standalone OpenCV mode.')
+        return 0
+
+    out_dir = output_dir / f'cam_{cam_name}'
+    out_dir.mkdir(parents=True, exist_ok=True)
+    topic = CAMERA_TOPICS[cam_name]
+
+    print(f'\n[{cam_name.upper()}] Subscribing to: {topic}')
+    print(f'  Capturing {n_images} images. Output: {out_dir}')
+    print('  Press SPACE in the preview window when the board is detected.')
+
+    rclpy.init()
+    node = Node('calibration_capture')
+    bridge = CvBridge()
+
+    captured = [0]
+    frame_idx = [0]
+    latest_frame = [None]
+
+    def image_callback(msg):
+        try:
+            latest_frame[0] = bridge.imgmsg_to_cv2(msg, 'bgr8')
+        except Exception as e:
+            node.get_logger().warn(f'cv_bridge error: {e}')
+
+    sub = node.create_subscription(Image, topic, image_callback, 1)  # noqa: F841
+    window_title = f'Calibration Capture — {cam_name} (ROS2)'
+    cv2.namedWindow(window_title, cv2.WINDOW_NORMAL)
+
+    import threading
+    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    spin_thread.start()
+
+    try:
+        while captured[0] < n_images:
+            frame = latest_frame[0]
+            if frame is None:
+                cv2.waitKey(50)
+                continue
+
+            found, corners, _ = find_checkerboard(frame, board_size)
+            display = draw_overlay(frame, board_size, found, corners,
+                                   captured[0], n_images)
+            cv2.imshow(window_title, display)
+
+            key = cv2.waitKey(1) & 0xFF
+            if key == ord('q'):
+                print(f'  Quit. Captured {captured[0]}/{n_images}.')
+                break
+            elif key == ord('r'):
+                for f in out_dir.glob('frame_*.png'):
+                    f.unlink()
+                captured[0] = 0
+                frame_idx[0] = 0
+                print(f'  [RESET] Cleared images for {cam_name}.')
+            elif key == ord(' '):
+                if found:
+                    save_path = out_dir / f'frame_{frame_idx[0]:04d}.png'
+                    cv2.imwrite(str(save_path), frame)
+                    captured[0] += 1
+                    frame_idx[0] += 1
+                    print(f'  Saved ({captured[0]}/{n_images}): {save_path.name}')
+                else:
+                    print('  [SKIP] No board detected.')
+    finally:
+        cv2.destroyWindow(window_title)
+        node.destroy_node()
+        rclpy.shutdown()
+
+    print(f'  Done: {captured[0]} images saved to {out_dir}')
+    return captured[0]
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description='Capture checkerboard images for IMX219 fisheye calibration.',
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog=__doc__,
+    )
+    parser.add_argument(
+        '--camera', choices=['front', 'right', 'rear', 'left', 'all'],
+        default='front',
+        help='Which camera to capture from (default: front). Use "all" to iterate all 4.',
+    )
+    parser.add_argument(
+        '--n-images', type=int, default=30,
+        help='Number of images to capture per camera (default: 30).',
+    )
+    parser.add_argument(
+        '--output-dir', type=str, default='/tmp/saltybot_cal',
+        help='Root output directory. Images saved to {output_dir}/cam_{name}/ (default: /tmp/saltybot_cal).',
+    )
+    parser.add_argument(
+        '--board-size', type=str, default='8x6',
+        help='Checkerboard inner corners as WxH (default: 8x6).',
+    )
+    parser.add_argument(
+        '--square-size', type=float, default=0.025,
+        help='Physical square size in metres (default: 0.025 = 25mm).',
+    )
+    parser.add_argument(
+        '--ros', action='store_true',
+        help='Use ROS2 topic subscription instead of direct VideoCapture.',
+    )
+    parser.add_argument(
+        '--device', type=str, default='/dev/video0',
+        help='Video device (non-ROS mode only, default: /dev/video0).',
+    )
+    args = parser.parse_args()
+
+    board_size = parse_board_size(args.board_size)
+    output_dir = Path(args.output_dir)
+    output_dir.mkdir(parents=True, exist_ok=True)
+
+    cameras = ALL_CAMERAS if args.camera == 'all' else [args.camera]
+
+    print(f'Checkerboard: {board_size[0]}x{board_size[1]} inner corners, '
+          f'{args.square_size * 1000:.0f}mm squares')
+    print(f'Target: {args.n_images} images per camera')
+    print(f'Output: {output_dir}')
+    print(f'Mode: {"ROS2 topics" if args.ros else f"VideoCapture ({args.device})"}')
+
+    for i, cam_name in enumerate(cameras):
+        if len(cameras) > 1:
+            print(f'\n--- Camera {i + 1}/{len(cameras)}: {cam_name.upper()} ---')
+            if i > 0:
+                input('  Position the board in view of this camera, then press ENTER...')
+
+        if args.ros:
+            n_captured = capture_loop_ros(cam_name, board_size, args.n_images, output_dir)
+        else:
+            n_captured = capture_loop_opencv(cam_name, args.device, board_size,
+                                             args.n_images, output_dir)
+
+        if n_captured < 5:
+            print(f'[WARN] Only {n_captured} images captured for {cam_name} — '
+                  f'calibration may fail (need >= 5, recommend >= 20).')
+
+    print('\nCapture complete. Run calibrate_camera.py for each camera to compute intrinsics.')
+    print('Example:')
+    for cam in cameras:
+        print(f'  python3 calibrate_camera.py \\')
+        print(f'    --images-dir {output_dir}/cam_{cam} \\')
+        print(f'    --output /mnt/nvme/saltybot/calibration/{cam}/camera_info.yaml \\')
+        print(f'    --camera-name camera_{cam} --board-size {args.board_size} \\')
+        print(f'    --square-size {args.square_size} --image-size 640x480')
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_calibration/scripts/generate_static_transforms.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+"""
+generate_static_transforms.py — Generate static_transforms.launch.py from extrinsics.yaml.
+
+Usage:
+  python3 generate_static_transforms.py \\
+    --extrinsics /path/to/calibration/extrinsics.yaml \\
+    --output /path/to/jetson/ros2_ws/src/saltybot_bringup/launch/static_transforms.launch.py
+"""
+
+import argparse
+import sys
+from pathlib import Path
+
+import yaml
+
+
+def load_extrinsics(yaml_path: str) -> dict:
+    """Load and parse extrinsics.yaml.
+
+    Returns the 'cameras' sub-dict with per-camera transforms.
+    """
+    with open(yaml_path, 'r') as f:
+        data = yaml.safe_load(f)
+    if 'cameras' not in data:
+        raise ValueError(f'extrinsics.yaml missing "cameras" key: {yaml_path}')
+    return data['cameras'], data
+
+
+def generate_launch_content(cameras: dict) -> str:
+    """Generate static_transforms.launch.py content from cameras dict."""
+    lines = [
+        '"""',
+        'static_transforms.launch.py — Auto-generated by generate_static_transforms.py',
+        'DO NOT EDIT manually — regenerate with:',
+        '  python3 generate_static_transforms.py \\',
+        '    --extrinsics /path/to/calibration/extrinsics.yaml \\',
+        '    --output <this_file>',
+        '"""',
+        '',
+        'from launch import LaunchDescription',
+        'from launch_ros.actions import Node',
+        '',
+        '',
+        'def generate_launch_description():',
+        '    return LaunchDescription([',
+    ]
+
+    for cam_name, cam_data in cameras.items():
+        parent = cam_data.get('parent_frame', 'base_link')
+        child = cam_data.get('child_frame', f'camera_{cam_name}_optical_frame')
+        tx, ty, tz = cam_data['translation']
+        qx, qy, qz, qw = cam_data['rotation_quat_xyzw']
+
+        lines += [
+            f'        # {cam_name}: {parent} -> {child}',
+            f'        Node(',
+            f'            package=\'tf2_ros\',',
+            f'            executable=\'static_transform_publisher\',',
+            f'            name=\'tf_base_to_camera_{cam_name}\',',
+            f'            arguments=[',
+            f'                \'{tx}\', \'{ty}\', \'{tz}\',',
+            f'                \'{qx}\', \'{qy}\', \'{qz}\', \'{qw}\',',
+            f'                \'{parent}\', \'{child}\',',
+            f'            ],',
+            f'        ),',
+        ]
+
+    # Also add RealSense D435i frame at origin (identity) — placeholder
+    # (actual D435i extrinsics from its own calibration)
+    lines += [
+        '        # RealSense D435i: base_link -> camera_color_optical_frame',
+        '        # NOTE: Replace with actual D435i extrinsics from stereo calibration',
+        '        Node(',
+        '            package=\'tf2_ros\',',
+        '            executable=\'static_transform_publisher\',',
+        '            name=\'tf_base_to_realsense\',',
+        '            arguments=[',
+        '                \'0.0\', \'0.0\', \'0.25\',',
+        '                \'0.0\', \'0.0\', \'0.0\', \'1.0\',',
+        '                \'base_link\', \'camera_color_optical_frame\',',
+        '            ],',
+        '        ),',
+        '    ])',
+        '',
+    ]
+
+    return '\n'.join(lines)
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description='Generate static_transforms.launch.py from extrinsics.yaml.',
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog=__doc__,
+    )
+    parser.add_argument(
+        '--extrinsics', required=True,
+        help='Path to extrinsics.yaml (produced by calibrate_extrinsics.py).',
+    )
+    parser.add_argument(
+        '--output', required=True,
+        help='Output path for static_transforms.launch.py.',
+    )
+    args = parser.parse_args()
+
+    extrinsics_path = args.extrinsics
+    if not Path(extrinsics_path).exists():
+        print(f'[ERROR] extrinsics.yaml not found: {extrinsics_path}')
+        sys.exit(1)
+
+    print(f'Loading extrinsics: {extrinsics_path}')
+    cameras, full_data = load_extrinsics(extrinsics_path)
+
+    print(f'Found {len(cameras)} camera(s): {", ".join(cameras.keys())}')
+
+    content = generate_launch_content(cameras)
+
+    output_path = Path(args.output)
+    output_path.parent.mkdir(parents=True, exist_ok=True)
+    with open(output_path, 'w') as f:
+        f.write(content)
+
+    print(f'Written: {output_path}')
+    print()
+    print('Transforms:')
+    print(f'  {"Camera":<8}  {"Parent":<12}  {"Child frame":<36}  {"Translation"}')
+    print('  ' + '-' * 80)
+    for cam_name, cam_data in cameras.items():
+        parent = cam_data.get('parent_frame', 'base_link')
+        child = cam_data.get('child_frame', f'camera_{cam_name}_optical_frame')
+        tx, ty, tz = cam_data['translation']
+        print(f'  {cam_name:<8}  {parent:<12}  {child:<36}  '
+              f'[{tx:.3f}, {ty:.3f}, {tz:.3f}]')
+    print()
+    print('Next step: include static_transforms.launch.py in your main launch file:')
+    print('  from launch.actions import IncludeLaunchDescription')
+    print('  from launch.launch_description_sources import PythonLaunchDescriptionSource')
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_calibration/scripts/preview_undistorted.py

@@ -0,0 +1,259 @@
+#!/usr/bin/env python3
+"""
+preview_undistorted.py — Preview undistorted IMX219 image using calibration YAML.
+
+Usage (ROS2 mode):
+  python3 preview_undistorted.py \\
+    --camera front \\
+    --calibration /path/to/calibration/front/camera_info.yaml
+
+Usage (image file mode):
+  python3 preview_undistorted.py \\
+    --image /path/to/image.png \\
+    --calibration /path/to/calibration/front/camera_info.yaml
+"""
+
+import argparse
+import sys
+from pathlib import Path
+
+import cv2
+import numpy as np
+import yaml
+
+
+CAMERA_TOPICS = {
+    'front': '/camera/front/image_raw',
+    'right': '/camera/right/image_raw',
+    'rear':  '/camera/rear/image_raw',
+    'left':  '/camera/left/image_raw',
+}
+
+
+def load_camera_info(yaml_path: str) -> dict:
+    """Load and parse camera_info YAML.
+
+    Returns dict with keys: K, D, image_size, camera_name, distortion_model
+    """
+    with open(yaml_path, 'r') as f:
+        data = yaml.safe_load(f)
+
+    w = data['image_width']
+    h = data['image_height']
+    K_data = data['camera_matrix']['data']
+    K = np.array(K_data, dtype=np.float64).reshape(3, 3)
+
+    D_data = data['distortion_coefficients']['data']
+    D = np.array(D_data, dtype=np.float64).reshape(-1, 1)
+
+    model = data.get('distortion_model', 'equidistant')
+    name = data.get('camera_name', 'unknown')
+
+    return {
+        'K': K,
+        'D': D,
+        'image_size': (w, h),
+        'camera_name': name,
+        'distortion_model': model,
+    }
+
+
+def build_undistort_maps(K: np.ndarray, D: np.ndarray,
+                         image_size: tuple, balance: float = 0.0) -> tuple:
+    """Compute fisheye undistortion maps.
+
+    Args:
+        K: 3x3 camera matrix
+        D: (4,1) fisheye distortion coefficients
+        image_size: (width, height)
+        balance: 0.0 = no black borders (crop), 1.0 = keep all pixels (black borders)
+
+    Returns:
+        (map1, map2) suitable for cv2.remap
+    """
+    new_K = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(
+        K, D, image_size, np.eye(3), balance=balance
+    )
+    map1, map2 = cv2.fisheye.initUndistortRectifyMap(
+        K, D, np.eye(3), new_K, image_size, cv2.CV_16SC2
+    )
+    return map1, map2, new_K
+
+
+def undistort_image(img: np.ndarray, map1: np.ndarray, map2: np.ndarray) -> np.ndarray:
+    """Apply precomputed undistortion maps to image."""
+    return cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR,
+                     borderMode=cv2.BORDER_CONSTANT)
+
+
+def show_side_by_side(original: np.ndarray, undistorted: np.ndarray,
+                      title: str = '', rms_info: str = '') -> np.ndarray:
+    """Create side-by-side comparison image."""
+    # Resize to same height if needed
+    h1, w1 = original.shape[:2]
+    h2, w2 = undistorted.shape[:2]
+    if h1 != h2:
+        undistorted = cv2.resize(undistorted, (w2 * h1 // h2, h1))
+
+    divider = np.zeros((h1, 4, 3), dtype=np.uint8)
+    divider[:] = (80, 80, 80)
+
+    combined = np.hstack([original, divider, undistorted])
+
+    # Labels
+    cv2.putText(combined, 'ORIGINAL (fisheye)', (10, 28),
+                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 220, 0), 2)
+    cv2.putText(combined, 'UNDISTORTED', (w1 + 14, 28),
+                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 220, 255), 2)
+    if title:
+        cv2.putText(combined, title, (10, combined.shape[0] - 10),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1)
+    if rms_info:
+        cv2.putText(combined, rms_info, (w1 + 14, combined.shape[0] - 10),
+                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1)
+    return combined
+
+
+def preview_image_file(image_path: str, cal_info: dict, balance: float) -> None:
+    """Show undistorted preview of a single image file."""
+    img = cv2.imread(image_path)
+    if img is None:
+        print(f'[ERROR] Cannot read image: {image_path}')
+        sys.exit(1)
+
+    K = cal_info['K']
+    D = cal_info['D']
+    image_size = cal_info['image_size']
+    name = cal_info['camera_name']
+
+    print(f'Camera: {name}')
+    print(f'Image size: {image_size[0]}x{image_size[1]}')
+    print(f'fx={K[0,0]:.2f}  fy={K[1,1]:.2f}  cx={K[0,2]:.2f}  cy={K[1,2]:.2f}')
+    print(f'D={D.flatten().tolist()}')
+
+    # Resize input if needed
+    ih, iw = img.shape[:2]
+    if (iw, ih) != image_size:
+        print(f'[WARN] Image size {iw}x{ih} != calibration size {image_size}. Resizing.')
+        img = cv2.resize(img, image_size)
+
+    map1, map2, new_K = build_undistort_maps(K, D, image_size, balance=balance)
+    undist = undistort_image(img, map1, map2)
+
+    print(f'New K after undistort (balance={balance}):')
+    print(f'  fx={new_K[0,0]:.2f}  fy={new_K[1,1]:.2f}  cx={new_K[0,2]:.2f}  cy={new_K[1,2]:.2f}')
+
+    combined = show_side_by_side(img, undist, title=str(Path(image_path).name))
+    cv2.imshow('Undistorted Preview (q=quit)', combined)
+    print('Press any key to close...')
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+
+def preview_ros_live(cam_name: str, cal_info: dict, balance: float) -> None:
+    """Subscribe to ROS2 camera topic and show live undistorted preview."""
+    try:
+        import rclpy
+        from rclpy.node import Node
+        from sensor_msgs.msg import Image
+        from cv_bridge import CvBridge
+    except ImportError as e:
+        print(f'[ERROR] ROS2 not available: {e}')
+        sys.exit(1)
+
+    K = cal_info['K']
+    D = cal_info['D']
+    image_size = cal_info['image_size']
+    topic = CAMERA_TOPICS[cam_name]
+
+    print(f'Subscribing to: {topic}')
+    print(f'Image size: {image_size[0]}x{image_size[1]}')
+    print('Press q in preview window to quit.')
+
+    map1, map2, new_K = build_undistort_maps(K, D, image_size, balance=balance)
+
+    rclpy.init()
+    node = Node('preview_undistorted')
+    bridge = CvBridge()
+    latest = [None]
+
+    def cb(msg):
+        try:
+            latest[0] = bridge.imgmsg_to_cv2(msg, 'bgr8')
+        except Exception as e:
+            node.get_logger().warn(f'cv_bridge: {e}')
+
+    node.create_subscription(Image, topic, cb, 1)
+
+    import threading
+    threading.Thread(target=rclpy.spin, args=(node,), daemon=True).start()
+
+    window = f'Undistorted — {cam_name}'
+    cv2.namedWindow(window, cv2.WINDOW_NORMAL)
+
+    try:
+        while True:
+            frame = latest[0]
+            if frame is not None:
+                fh, fw = frame.shape[:2]
+                if (fw, fh) != image_size:
+                    frame = cv2.resize(frame, image_size)
+                undist = undistort_image(frame, map1, map2)
+                combined = show_side_by_side(frame, undist)
+                cv2.imshow(window, combined)
+
+            key = cv2.waitKey(1) & 0xFF
+            if key == ord('q'):
+                break
+    finally:
+        cv2.destroyAllWindows()
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description='Preview undistorted IMX219 fisheye image from calibration YAML.',
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog=__doc__,
+    )
+    mode_group = parser.add_mutually_exclusive_group(required=True)
+    mode_group.add_argument(
+        '--camera', choices=['front', 'right', 'rear', 'left'],
+        help='Camera name for ROS2 live preview mode.',
+    )
+    mode_group.add_argument(
+        '--image', type=str,
+        help='Path to a single image file for static preview.',
+    )
+    parser.add_argument(
+        '--calibration', required=True,
+        help='Path to camera_info.yaml produced by calibrate_camera.py.',
+    )
+    parser.add_argument(
+        '--balance', type=float, default=0.0,
+        help='Undistort balance: 0.0=crop to valid pixels, 1.0=keep all (black borders). '
+             'Default: 0.0.',
+    )
+    args = parser.parse_args()
+
+    cal_path = args.calibration
+    if not Path(cal_path).exists():
+        print(f'[ERROR] Calibration file not found: {cal_path}')
+        sys.exit(1)
+
+    print(f'Loading calibration: {cal_path}')
+    cal_info = load_camera_info(cal_path)
+
+    if cal_info['distortion_model'] not in ('equidistant', 'fisheye'):
+        print(f'[WARN] Distortion model "{cal_info["distortion_model"]}" — '
+              f'expected "equidistant". Results may be incorrect.')
+
+    if args.image:
+        preview_image_file(args.image, cal_info, args.balance)
+    else:
+        preview_ros_live(args.camera, cal_info, args.balance)
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_mode_switch/.gitignore

@@ -0,0 +1,4 @@
+__pycache__/
+*.pyc
+*.egg-info/
+.pytest_cache/

jetson/ros2_ws/src/saltybot_mode_switch/config/mode_switch_params.yaml

@@ -0,0 +1,58 @@
+# mode_switch_params.yaml — saltybot_mode_switch
+#
+# Launch with:
+#   ros2 launch saltybot_mode_switch mode_switch.launch.py
+#
+# Topic wiring:
+#   /rc/joy                          → mode_switch_node (CRSF channels)
+#   /saltybot/balance_state          → mode_switch_node (STM32 state)
+#   /slam_toolbox/pose_with_covariance_stamped → mode_switch_node (SLAM fix)
+#   /saltybot/control_mode           ← mode_switch_node (JSON mode + alpha)
+#   /saltybot/led_pattern            ← mode_switch_node (LED name)
+#
+#   /cmd_vel_auto                    → cmd_vel_mux_node  (from Nav2/follower)
+#   /saltybot/control_mode           → cmd_vel_mux_node
+#   /cmd_vel                         ← cmd_vel_mux_node  (to bridge)
+#
+# IMPORTANT: remap Nav2 and person_follower to publish /cmd_vel_auto:
+#   ros2 launch saltybot_bringup nav2.launch.py cmd_vel_topic:=/cmd_vel_auto
+
+# ── CRSF channel mapping ──────────────────────────────────────────────────────
+# Joy axes index for the AUX2 / CH6 mode-toggle switch.
+# Typical CRSF→Joy mapping: CH1=axes[0] … CH6=axes[5].
+ch6_axis: 5
+
+# Space-separated Joy axes indices for the four control sticks (roll/pitch/throttle/yaw).
+# These are monitored for the 10% override threshold.
+stick_axes: "0 1 2 3"
+
+# ── Override / return-to-auto ──────────────────────────────────────────────────
+# Stick magnitude (0–1) that triggers instant RC override while in AUTO.
+# 10% (0.10) per spec — change only if stick noise causes false triggers.
+override_threshold: 0.10
+
+# Seconds sticks must remain neutral before AUTO re-engages after an override.
+return_delay_s: 2.0
+
+# ── Ramp ──────────────────────────────────────────────────────────────────────
+# RC↔AUTO blend ramp duration (seconds). cmd_vel_mux scales by blend_alpha
+# so autonomous commands are eased in/out over this window.
+ramp_duration_s: 0.5
+
+# ── RC link health ────────────────────────────────────────────────────────────
+# Seconds of Joy silence before RC link is declared lost → force RC mode.
+rc_link_timeout_s: 0.5
+
+# ── SLAM fix ──────────────────────────────────────────────────────────────────
+# ROS2 topic providing the SLAM localisation pose. Any message received within
+# slam_fix_timeout_s counts as a valid fix.
+slam_fix_topic: "/slam_toolbox/pose_with_covariance_stamped"
+
+# Seconds after the last SLAM pose before fix is considered lost.
+slam_fix_timeout_s: 3.0
+
+# ── Control loop ──────────────────────────────────────────────────────────────
+control_rate: 20.0    # Hz
+
+# ── cmd_vel_mux ───────────────────────────────────────────────────────────────
+cmd_vel_timeout_s: 0.5   # deadman: zero output if /cmd_vel_auto goes silent

jetson/ros2_ws/src/saltybot_mode_switch/launch/mode_switch.launch.py

@@ -0,0 +1,59 @@
+"""mode_switch.launch.py — launch mode_switch_node + cmd_vel_mux_node."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share    = get_package_share_directory("saltybot_mode_switch")
+    default_cfg  = os.path.join(pkg_share, "config", "mode_switch_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "params_file",
+            default_value=default_cfg,
+            description="Path to mode_switch_params.yaml",
+        ),
+        DeclareLaunchArgument(
+            "ramp_duration_s",
+            default_value="0.5",
+            description="RC↔AUTO blend ramp duration (seconds)",
+        ),
+        DeclareLaunchArgument(
+            "return_delay_s",
+            default_value="2.0",
+            description="Sticks-neutral duration before auto re-engage (seconds)",
+        ),
+        DeclareLaunchArgument(
+            "slam_fix_topic",
+            default_value="/slam_toolbox/pose_with_covariance_stamped",
+            description="SLAM fix pose topic",
+        ),
+
+        Node(
+            package="saltybot_mode_switch",
+            executable="mode_switch_node",
+            name="mode_switch",
+            output="screen",
+            parameters=[
+                LaunchConfiguration("params_file"),
+                {
+                    "ramp_duration_s": LaunchConfiguration("ramp_duration_s"),
+                    "return_delay_s":  LaunchConfiguration("return_delay_s"),
+                    "slam_fix_topic":  LaunchConfiguration("slam_fix_topic"),
+                },
+            ],
+        ),
+
+        Node(
+            package="saltybot_mode_switch",
+            executable="cmd_vel_mux_node",
+            name="cmd_vel_mux",
+            output="screen",
+            parameters=[LaunchConfiguration("params_file")],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_mode_switch/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_mode_switch</name>
+  <version>0.1.0</version>
+  <description>
+    Autonomous/RC mode switch for saltybot (Issue #104).
+    Monitors CRSF CH6 toggle, RC stick override, SLAM fix status, and RC link
+    health to manage RC↔AUTO transitions with a 500 ms blend ramp.
+    Publishes /saltybot/control_mode (JSON) and /saltybot/led_pattern.
+    Includes cmd_vel_mux_node that scales autonomous cmd_vel by blend_alpha.
+  </description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/cmd_vel_mux_node.py

@@ -0,0 +1,133 @@
+"""
+cmd_vel_mux_node.py — Blend-alpha cmd_vel multiplexer (Issue #104).
+
+Sits between the Nav2 / person-follower stack and the cmd_vel_bridge_node,
+scaling the autonomous cmd_vel output by the blend_alpha published by
+mode_switch_node so that the RC → AUTO handoff is smooth over 500 ms.
+
+Topic graph
+-----------
+  Nav2 / follower → /cmd_vel_auto ──┐
+                                     ├── cmd_vel_mux → /cmd_vel → bridge
+  mode_switch → /saltybot/control_mode (blend_alpha)
+
+In RC mode (blend_alpha ≈ 0) the node publishes Twist(0,0) so the bridge
+receives zeros — this is harmless because the bridge's mode gate already
+prevents autonomous commands when the STM32 is in RC_MANUAL.
+
+The bridge's existing ESC ramp handles hardware-level smoothing;
+the blend_alpha here provides the higher-level cmd_vel policy ramp.
+
+Parameters
+----------
+  cmd_vel_timeout_s  Deadman: zero output if /cmd_vel_auto goes silent  (0.5 s)
+  control_rate       Output publish rate (Hz)                           (20.0)
+"""
+
+import json
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+
+from saltybot_mode_switch.mode_logic import blend_twist
+
+_BIG = 1e9
+
+
+class CmdVelMuxNode(Node):
+
+    def __init__(self):
+        super().__init__("cmd_vel_mux")
+
+        self.declare_parameter("cmd_vel_timeout_s", 0.5)
+        self.declare_parameter("control_rate",      20.0)
+
+        self._timeout      = self.get_parameter("cmd_vel_timeout_s").value
+        self._rate         = self.get_parameter("control_rate").value
+
+        reliable = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+
+        # ── State ─────────────────────────────────────────────────────────────
+        self._auto_linear_x:  float = 0.0
+        self._auto_angular_z: float = 0.0
+        self._blend_alpha:    float = 0.0
+        self._last_auto_t:    float = 0.0
+
+        # ── Subscriptions ─────────────────────────────────────────────────────
+        self.create_subscription(
+            Twist, "/cmd_vel_auto", self._auto_cb, reliable)
+        self.create_subscription(
+            String, "/saltybot/control_mode", self._mode_cb, reliable)
+
+        # ── Publisher ─────────────────────────────────────────────────────────
+        self._pub = self.create_publisher(Twist, "/cmd_vel", reliable)
+
+        # ── Timer ─────────────────────────────────────────────────────────────
+        self._timer = self.create_timer(1.0 / self._rate, self._publish_cb)
+
+        self.get_logger().info(
+            f"CmdVelMuxNode ready  rate={self._rate:.0f}Hz  "
+            f"timeout={self._timeout:.1f}s"
+        )
+
+    # ── Callbacks ─────────────────────────────────────────────────────────────
+
+    def _auto_cb(self, msg: Twist) -> None:
+        self._auto_linear_x  = msg.linear.x
+        self._auto_angular_z = msg.angular.z
+        self._last_auto_t    = time.monotonic()
+
+    def _mode_cb(self, msg: String) -> None:
+        try:
+            data = json.loads(msg.data)
+            self._blend_alpha = float(data.get("blend_alpha", 0.0))
+        except (json.JSONDecodeError, TypeError, ValueError):
+            self._blend_alpha = 0.0
+
+    # ── 20 Hz publish ─────────────────────────────────────────────────────────
+
+    def _publish_cb(self) -> None:
+        now = time.monotonic()
+
+        # Deadman: zero if autonomous source went silent
+        auto_age = (now - self._last_auto_t) if self._last_auto_t > 0.0 else _BIG
+        if auto_age > self._timeout:
+            linear_x, angular_z = 0.0, 0.0
+        else:
+            linear_x, angular_z = blend_twist(
+                self._auto_linear_x,
+                self._auto_angular_z,
+                self._blend_alpha,
+            )
+
+        twist          = Twist()
+        twist.linear.x  = linear_x
+        twist.angular.z = angular_z
+        self._pub.publish(twist)
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CmdVelMuxNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_logic.py

@@ -0,0 +1,241 @@
+"""
+mode_logic.py — Pure RC/Autonomous mode-switch logic for saltybot.
+
+All functions are stateless or operate on plain Python types so the full
+state machine can be exercised in unit tests without a ROS2 runtime.
+
+Mode vocabulary
+---------------
+  "RC"            — STM32 executing RC pilot commands; Jetson cmd_vel blocked.
+  "RAMP_TO_AUTO"  — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
+  "AUTO"          — STM32 executing Jetson cmd_vel; RC sticks idle.
+  "RAMP_TO_RC"    — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s.
+
+Blend alpha
+-----------
+  The node publishes blend_alpha ∈ [0, 1] in /saltybot/control_mode.
+  The cmd_vel_mux_node scales autonomous cmd_vel by alpha before publishing
+  to /cmd_vel (the bridge input).  alpha=0 ⟹ no autonomous motion.
+
+RC override
+-----------
+  Any stick magnitude > override_threshold while in AUTO instantly forces RC
+  (alpha → 0 immediately; no ramp).  _override_active is set so the
+  return-to-auto path requires sticks to be neutral for return_delay seconds.
+
+CH6 / toggle
+------------
+  ch6_auto=True  means the pilot has requested AUTO (switch up/high).
+  ch6_auto=False means the pilot has requested RC   (switch down/low).
+  Pure CH6 transitions (no stick activity) always ramp gracefully.
+
+Safety interlocks
+-----------------
+  • No AUTO without slam_ok (valid SLAM fix received recently).
+  • Instant drop to RC when rc_link_ok goes False (RC link lost).
+  These checks are applied at every control tick.
+"""
+
+import math
+
+# ─── Constants ──────────────────────────────────────────────────────────────
+
+MODES = ("RC", "RAMP_TO_AUTO", "AUTO", "RAMP_TO_RC")
+
+LED_PATTERNS = {
+    "rc_normal":      "solid_yellow",
+    "rc_no_slam":     "solid_yellow",       # no slam needed in RC
+    "auto_ok":        "solid_green",
+    "auto_no_slam":   "blink_orange_fast",  # shouldn't happen (interlock), but defensive
+    "ramp_to_auto":   "blink_green_slow",
+    "ramp_to_rc":     "blink_yellow_slow",
+    "no_rc_link":     "blink_red_fast",
+}
+
+# ─── Stick / CH6 helpers ────────────────────────────────────────────────────
+
+def ch6_wants_auto(axes: list, ch6_idx: int, threshold: float = 0.5) -> bool:
+    """
+    Return True when CH6 (aux switch) is in the AUTO position.
+
+    Typical CRSF→Joy mapping: AUX2 = axes[5].
+    Switch up (high) = +1.0 → AUTO; switch down (low) = -1.0 → RC.
+    threshold default 0.5 gives hysteresis against mid-travel noise.
+    """
+    if ch6_idx < 0 or ch6_idx >= len(axes):
+        return False
+    return float(axes[ch6_idx]) > threshold
+
+
+def sticks_active(
+    axes: list,
+    stick_indices: tuple,
+    threshold: float = 0.10,
+) -> bool:
+    """
+    Return True when any monitored stick axis exceeds threshold magnitude.
+
+    stick_indices: tuple of Joy axes indices for the four sticks
+                   (roll, pitch, throttle, yaw).  Default CRSF mapping:
+                   (0, 1, 2, 3).
+    threshold: 10 % of full deflection (issue spec).
+    """
+    return any(
+        abs(float(axes[i])) > threshold
+        for i in stick_indices
+        if i < len(axes)
+    )
+
+
+# ─── Alpha / ramp helpers ───────────────────────────────────────────────────
+
+def ramp_alpha(elapsed_s: float, ramp_s: float) -> float:
+    """
+    Compute forward blend alpha [0, 1] linearly over ramp_s seconds.
+
+    elapsed_s : seconds since ramp start.
+    ramp_s    : total ramp duration (seconds).
+    """
+    if ramp_s <= 0.0:
+        return 1.0
+    return min(1.0, elapsed_s / ramp_s)
+
+
+def alpha_for_mode(mode: str, elapsed_s: float, ramp_s: float) -> float:
+    """
+    Return the blend alpha appropriate for `mode`.
+
+      RC           → 0.0
+      RAMP_TO_AUTO → 0.0 … 1.0  (linear with elapsed)
+      AUTO         → 1.0
+      RAMP_TO_RC   → 1.0 … 0.0  (linear with elapsed)
+    """
+    if mode == "RC":
+        return 0.0
+    if mode == "AUTO":
+        return 1.0
+    if mode == "RAMP_TO_AUTO":
+        return ramp_alpha(elapsed_s, ramp_s)
+    if mode == "RAMP_TO_RC":
+        return 1.0 - ramp_alpha(elapsed_s, ramp_s)
+    return 0.0
+
+
+# ─── LED pattern ────────────────────────────────────────────────────────────
+
+def led_pattern(mode: str, slam_ok: bool, rc_link_ok: bool) -> str:
+    """Return the LED pattern name for the current operating state."""
+    if not rc_link_ok:
+        return LED_PATTERNS["no_rc_link"]
+    if mode in ("RC", "RAMP_TO_RC"):
+        return LED_PATTERNS["rc_normal"]
+    if mode == "RAMP_TO_AUTO":
+        return LED_PATTERNS["ramp_to_auto"]
+    if mode == "AUTO":
+        return LED_PATTERNS["auto_ok"] if slam_ok else LED_PATTERNS["auto_no_slam"]
+    return LED_PATTERNS["no_rc_link"]
+
+
+# ─── State machine step ──────────────────────────────────────────────────────
+
+def step_state(
+    current_mode: str,
+    ramp_elapsed_s: float,
+    ramp_s: float,
+    ch6_auto: bool,
+    sticks_now_active: bool,
+    sticks_neutral_elapsed_s: float,
+    return_delay_s: float,
+    slam_ok: bool,
+    rc_link_ok: bool,
+    override_active: bool,
+) -> tuple:
+    """
+    Compute (next_mode, next_override_active, instant_to_rc).
+
+    Parameters
+    ----------
+    current_mode            : current mode string
+    ramp_elapsed_s          : seconds since last ramp start
+    ramp_s                  : configured ramp duration
+    ch6_auto                : True = pilot switch requests AUTO
+    sticks_now_active       : True = at least one stick > override_threshold
+    sticks_neutral_elapsed_s: seconds since all sticks went neutral (0 if still active)
+    return_delay_s          : seconds of stick-neutral required to re-engage AUTO
+    slam_ok                 : True = SLAM fix is valid
+    rc_link_ok              : True = CRSF link alive
+    override_active         : True = currently in stick-override RC
+
+    Returns
+    -------
+    next_mode           : new mode string
+    next_override_active: updated override flag
+    instant_to_rc       : True when dropping to RC without ramp (safety / stick override)
+    """
+    instant_to_rc = False
+
+    # ── Safety: RC link lost → instant RC ────────────────────────────────────
+    if not rc_link_ok:
+        return "RC", False, True
+
+    # ── Compute override transitions ──────────────────────────────────────────
+    next_override = override_active
+    if sticks_now_active and current_mode in ("AUTO", "RAMP_TO_AUTO", "RAMP_TO_RC"):
+        # Stick input while AUTO or any transition → instant RC override
+        next_override = True
+        return "RC", True, True
+
+    if not sticks_now_active and sticks_neutral_elapsed_s >= return_delay_s:
+        # Sticks have been neutral long enough — clear override
+        next_override = False
+
+    # ── Ramp completion ───────────────────────────────────────────────────────
+    ramp_done = ramp_elapsed_s >= ramp_s - 1e-9   # epsilon guards float accumulation
+
+    # ── Transitions per state ─────────────────────────────────────────────────
+    if current_mode == "RC":
+        if ch6_auto and slam_ok and not next_override:
+            return "RAMP_TO_AUTO", next_override, False
+        return "RC", next_override, False
+
+    if current_mode == "RAMP_TO_AUTO":
+        if not ch6_auto:
+            return "RAMP_TO_RC", next_override, False
+        if not slam_ok:
+            return "RAMP_TO_RC", next_override, False
+        if ramp_done:
+            return "AUTO", next_override, False
+        return "RAMP_TO_AUTO", next_override, False
+
+    if current_mode == "AUTO":
+        if not ch6_auto:
+            return "RAMP_TO_RC", next_override, False
+        if not slam_ok:
+            return "RAMP_TO_RC", next_override, False
+        return "AUTO", next_override, False
+
+    if current_mode == "RAMP_TO_RC":
+        if ch6_auto and slam_ok and not next_override:
+            return "RAMP_TO_AUTO", next_override, False
+        if ramp_done:
+            return "RC", next_override, False
+        return "RAMP_TO_RC", next_override, False
+
+    return "RC", next_override, True   # unknown state → safe fallback
+
+
+# ─── cmd_vel blending ───────────────────────────────────────────────────────
+
+def blend_twist(
+    auto_linear_x: float,
+    auto_angular_z: float,
+    alpha: float,
+) -> tuple:
+    """
+    Scale autonomous cmd_vel by blend alpha.
+
+    Returns (linear_x, angular_z) for the blended output.
+    alpha=0 → zeros; alpha=1 → full autonomous.
+    """
+    a = max(0.0, min(1.0, alpha))
+    return auto_linear_x * a, auto_angular_z * a

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_switch_node.py

@@ -0,0 +1,307 @@
+"""
+mode_switch_node.py — Autonomous/RC mode switch for saltybot (Issue #104).
+
+Inputs
+------
+  /rc/joy                  (sensor_msgs/Joy)
+      CRSF channel axes:
+        axes[ch6_axis]        AUX2 — mode toggle  (+1=AUTO, -1=RC)
+        axes[stick_axes...]   Roll/Pitch/Throttle/Yaw — override detection
+
+  /saltybot/balance_state  (std_msgs/String JSON)
+      Parsed for RC link health (field "rc_link") and STM32 mode.
+
+  <slam_fix_topic>         (geometry_msgs/PoseWithCovarianceStamped)
+      Any message received within slam_fix_timeout_s → SLAM fix valid.
+
+Outputs
+-------
+  /saltybot/control_mode   (std_msgs/String JSON)
+      {
+        "mode":           "RC" | "RAMP_TO_AUTO" | "AUTO" | "RAMP_TO_RC",
+        "blend_alpha":    0.0 … 1.0,
+        "slam_ok":        bool,
+        "rc_link_ok":     bool,
+        "override_active": bool
+      }
+
+  /saltybot/led_pattern    (std_msgs/String)
+      LED pattern name consumed by the LED controller node.
+
+State machine
+-------------
+  See mode_logic.py for the full transition table.
+
+  RC           — pilot in control; blend_alpha = 0.0
+  RAMP_TO_AUTO — transitioning; blend_alpha 0→1 over ramp_duration_s (500 ms)
+  AUTO         — Jetson in control; blend_alpha = 1.0
+  RAMP_TO_RC   — transitioning; blend_alpha 1→0 over ramp_duration_s (500 ms)
+
+Safety interlocks
+-----------------
+  • Any stick > override_threshold while AUTO/RAMP_TO_AUTO → instant RC.
+  • rc_link lost (Joy silent > rc_link_timeout_s) → instant RC.
+  • AUTO not entered without valid SLAM fix.
+  • SLAM fix lost while AUTO → RAMP_TO_RC.
+
+Parameters
+----------
+  ch6_axis            Joy axes index for the mode-toggle switch   (default 5)
+  stick_axes          Space-separated Joy indices for 4 sticks    (default "0 1 2 3")
+  override_threshold  Stick magnitude for instant RC override      (default 0.10)
+  return_delay_s      Sticks-neutral duration before re-engage    (default 2.0 s)
+  ramp_duration_s     RC↔AUTO blend ramp duration                 (default 0.5 s)
+  rc_link_timeout_s   Joy silence threshold → RC link lost        (default 0.5 s)
+  slam_fix_topic      Pose topic for SLAM fix detection           (default /slam_toolbox/pose_with_covariance_stamped)
+  slam_fix_timeout_s  Pose silence threshold → SLAM fix lost      (default 3.0 s)
+  control_rate        State machine tick rate (Hz)                (default 20.0)
+"""
+
+import json
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, DurabilityPolicy
+
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Joy
+from std_msgs.msg import String
+
+from saltybot_mode_switch.mode_logic import (
+    alpha_for_mode,
+    ch6_wants_auto,
+    led_pattern,
+    step_state,
+    sticks_active,
+)
+
+_BIG = 1e9   # sentinel: "never received"
+
+
+class ModeSwitchNode(Node):
+
+    def __init__(self):
+        super().__init__("mode_switch")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("ch6_axis",           5)
+        self.declare_parameter("stick_axes",         "0 1 2 3")
+        self.declare_parameter("override_threshold",  0.10)
+        self.declare_parameter("return_delay_s",      2.0)
+        self.declare_parameter("ramp_duration_s",     0.5)
+        self.declare_parameter("rc_link_timeout_s",   0.5)
+        self.declare_parameter("slam_fix_topic",
+                               "/slam_toolbox/pose_with_covariance_stamped")
+        self.declare_parameter("slam_fix_timeout_s",  3.0)
+        self.declare_parameter("control_rate",       20.0)
+
+        self._reload_params()
+
+        # ── State ─────────────────────────────────────────────────────────────
+        self._mode: str           = "RC"
+        self._override_active: bool = False
+        self._ramp_start: float   = 0.0      # monotonic time ramp began
+        self._sticks_neutral_since: float | None = None
+        self._last_joy_t: float   = 0.0      # monotonic; 0 = never
+        self._last_slam_t: float  = 0.0
+        self._joy_axes: list      = []
+        self._stm32_mode: int     = 0        # from balance_state JSON
+
+        # ── QoS ───────────────────────────────────────────────────────────────
+        best_effort = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=5,
+        )
+        reliable = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+        # SLAM toolbox uses transient_local for its pose topic
+        transient = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            depth=1,
+        )
+
+        # ── Subscriptions ─────────────────────────────────────────────────────
+        self.create_subscription(Joy, "/rc/joy", self._joy_cb, best_effort)
+        self.create_subscription(
+            String, "/saltybot/balance_state", self._balance_cb, reliable)
+
+        slam_topic = self.get_parameter("slam_fix_topic").value
+        # Try transient_local first (SLAM Toolbox); fall back gracefully if not
+        try:
+            self.create_subscription(
+                PoseWithCovarianceStamped, slam_topic,
+                self._slam_cb, transient)
+        except Exception:
+            self.create_subscription(
+                PoseWithCovarianceStamped, slam_topic,
+                self._slam_cb, reliable)
+
+        # ── Publishers ────────────────────────────────────────────────────────
+        self._mode_pub = self.create_publisher(
+            String, "/saltybot/control_mode", reliable)
+        self._led_pub  = self.create_publisher(
+            String, "/saltybot/led_pattern", reliable)
+
+        # ── Control timer ─────────────────────────────────────────────────────
+        self._timer = self.create_timer(
+            1.0 / self._p["control_rate"], self._tick)
+
+        self.get_logger().info(
+            f"ModeSwitchNode ready  "
+            f"ramp={self._p['ramp_duration_s']:.1f}s  "
+            f"override_thresh={self._p['override_threshold']:.0%}  "
+            f"return_delay={self._p['return_delay_s']:.1f}s  "
+            f"slam_topic={slam_topic}"
+        )
+
+    # ── Parameter reload ──────────────────────────────────────────────────────
+
+    def _reload_params(self):
+        raw_stick_axes = self.get_parameter("stick_axes").value
+        self._p = {
+            "ch6_axis":           int(self.get_parameter("ch6_axis").value),
+            "stick_indices":      tuple(int(x) for x in raw_stick_axes.split()),
+            "override_threshold": float(self.get_parameter("override_threshold").value),
+            "return_delay_s":     float(self.get_parameter("return_delay_s").value),
+            "ramp_duration_s":    float(self.get_parameter("ramp_duration_s").value),
+            "rc_link_timeout_s":  float(self.get_parameter("rc_link_timeout_s").value),
+            "slam_fix_timeout_s": float(self.get_parameter("slam_fix_timeout_s").value),
+            "control_rate":       float(self.get_parameter("control_rate").value),
+        }
+
+    # ── Callbacks ─────────────────────────────────────────────────────────────
+
+    def _joy_cb(self, msg: Joy) -> None:
+        self._joy_axes  = list(msg.axes)
+        self._last_joy_t = time.monotonic()
+
+    def _balance_cb(self, msg: String) -> None:
+        try:
+            data = json.loads(msg.data)
+            # "mode" is a label string; map back to int for reference
+            mode_label = data.get("mode", "RC_MANUAL")
+            self._stm32_mode = {"RC_MANUAL": 0, "RC_ASSISTED": 1,
+                                "AUTONOMOUS": 2}.get(mode_label, 0)
+        except (json.JSONDecodeError, TypeError):
+            pass
+
+    def _slam_cb(self, msg: PoseWithCovarianceStamped) -> None:
+        self._last_slam_t = time.monotonic()
+
+    # ── 20 Hz control tick ────────────────────────────────────────────────────
+
+    def _tick(self) -> None:
+        self._reload_params()
+        now = time.monotonic()
+        p   = self._p
+
+        # ── Derive inputs ─────────────────────────────────────────────────────
+        joy_age    = (now - self._last_joy_t) if self._last_joy_t > 0.0 else _BIG
+        slam_age   = (now - self._last_slam_t) if self._last_slam_t > 0.0 else _BIG
+        rc_link_ok = joy_age <= p["rc_link_timeout_s"]
+        slam_ok    = slam_age <= p["slam_fix_timeout_s"]
+
+        axes = self._joy_axes
+        ch6  = ch6_wants_auto(axes, p["ch6_axis"])
+        active = sticks_active(axes, p["stick_indices"], p["override_threshold"])
+
+        # Track neutral timer
+        if active:
+            self._sticks_neutral_since = None
+        elif self._sticks_neutral_since is None:
+            self._sticks_neutral_since = now
+
+        neutral_elapsed = (
+            (now - self._sticks_neutral_since)
+            if self._sticks_neutral_since is not None else 0.0
+        )
+
+        ramp_elapsed = now - self._ramp_start
+
+        # ── State machine step ────────────────────────────────────────────────
+        prev_mode = self._mode
+        next_mode, next_override, instant = step_state(
+            current_mode=self._mode,
+            ramp_elapsed_s=ramp_elapsed,
+            ramp_s=p["ramp_duration_s"],
+            ch6_auto=ch6,
+            sticks_now_active=active,
+            sticks_neutral_elapsed_s=neutral_elapsed,
+            return_delay_s=p["return_delay_s"],
+            slam_ok=slam_ok,
+            rc_link_ok=rc_link_ok,
+            override_active=self._override_active,
+        )
+
+        if next_mode != prev_mode:
+            # Log transitions; reset ramp timer on ramp entry
+            self.get_logger().info(
+                f"Mode: {prev_mode} → {next_mode}"
+                f"  slam_ok={slam_ok} rc_link={rc_link_ok}"
+                f"  override={next_override}"
+            )
+            if next_mode in ("RAMP_TO_AUTO", "RAMP_TO_RC"):
+                self._ramp_start = now
+                ramp_elapsed = 0.0
+
+        self._mode            = next_mode
+        self._override_active = next_override
+
+        # ── Compute blend alpha ───────────────────────────────────────────────
+        current_ramp_elapsed = now - self._ramp_start
+        alpha = alpha_for_mode(next_mode, current_ramp_elapsed, p["ramp_duration_s"])
+        if instant:
+            alpha = 0.0
+
+        # ── Log steady warnings ───────────────────────────────────────────────
+        if next_mode == "AUTO" and not slam_ok:
+            self.get_logger().warn(
+                "In AUTO but SLAM fix lost — will exit AUTO",
+                throttle_duration_sec=2.0)
+        if not rc_link_ok:
+            self.get_logger().warn(
+                "RC link lost — forced RC mode",
+                throttle_duration_sec=1.0)
+
+        # ── Publish /saltybot/control_mode ────────────────────────────────────
+        payload = {
+            "mode":            next_mode,
+            "blend_alpha":     round(alpha, 4),
+            "slam_ok":         slam_ok,
+            "rc_link_ok":      rc_link_ok,
+            "override_active": next_override,
+        }
+        mode_msg      = String()
+        mode_msg.data = json.dumps(payload)
+        self._mode_pub.publish(mode_msg)
+
+        # ── Publish /saltybot/led_pattern ─────────────────────────────────────
+        pattern      = led_pattern(next_mode, slam_ok, rc_link_ok)
+        led_msg      = String()
+        led_msg.data = pattern
+        self._led_pub.publish(led_msg)
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ModeSwitchNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_mode_switch/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/saltybot_mode_switch
+
+[install]
+install_scripts=$base/lib/saltybot_mode_switch

jetson/ros2_ws/src/saltybot_mode_switch/setup.py

@@ -0,0 +1,33 @@
+from setuptools import setup, find_packages
+import os
+from glob import glob
+
+package_name = "saltybot_mode_switch"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages",
+         [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (os.path.join("share", package_name, "config"),
+         glob("config/*.yaml")),
+        (os.path.join("share", package_name, "launch"),
+         glob("launch/*.py")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-controls",
+    maintainer_email="sl-controls@saltylab.local",
+    description="Autonomous/RC mode switch with 500 ms blend ramp (Issue #104)",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            f"mode_switch_node = {package_name}.mode_switch_node:main",
+            f"cmd_vel_mux_node = {package_name}.cmd_vel_mux_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_mode_switch/test/test_mode_logic.py

@@ -0,0 +1,481 @@
+"""
+test_mode_logic.py — Unit tests for saltybot_mode_switch pure logic.
+
+Covers:
+  - ch6_wants_auto / sticks_active helpers
+  - ramp_alpha / alpha_for_mode
+  - led_pattern
+  - step_state state machine (all transitions)
+  - blend_twist
+
+No ROS2 runtime required.
+"""
+
+import sys
+import os
+import math
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+
+import pytest
+
+from saltybot_mode_switch.mode_logic import (
+    alpha_for_mode,
+    blend_twist,
+    ch6_wants_auto,
+    led_pattern,
+    ramp_alpha,
+    step_state,
+    sticks_active,
+)
+
+# ─────────────────────────────────────────────────────────────────────────────
+# Helpers
+# ─────────────────────────────────────────────────────────────────────────────
+
+def _step(
+    mode="RC",
+    ramp_elapsed=0.0,
+    ramp_s=0.5,
+    ch6_auto=False,
+    sticks_now_active=False,
+    neutral_elapsed=0.0,
+    return_delay=2.0,
+    slam_ok=True,
+    rc_link_ok=True,
+    override_active=False,
+):
+    return step_state(
+        current_mode=mode,
+        ramp_elapsed_s=ramp_elapsed,
+        ramp_s=ramp_s,
+        ch6_auto=ch6_auto,
+        sticks_now_active=sticks_now_active,
+        sticks_neutral_elapsed_s=neutral_elapsed,
+        return_delay_s=return_delay,
+        slam_ok=slam_ok,
+        rc_link_ok=rc_link_ok,
+        override_active=override_active,
+    )
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# ch6_wants_auto
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestCh6WantsAuto:
+
+    def test_high_value_returns_true(self):
+        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, 1.0], ch6_idx=5) is True
+
+    def test_low_value_returns_false(self):
+        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, -1.0], ch6_idx=5) is False
+
+    def test_mid_value_below_threshold(self):
+        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, 0.4], ch6_idx=5) is False
+
+    def test_exactly_at_threshold(self):
+        # 0.5 > 0.5 is False; require strictly above
+        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, 0.5], ch6_idx=5) is False
+
+    def test_just_above_threshold(self):
+        assert ch6_wants_auto([0.0, 0.0, 0.0, 0.0, 0.0, 0.51], ch6_idx=5) is True
+
+    def test_out_of_range_index(self):
+        assert ch6_wants_auto([1.0], ch6_idx=10) is False
+
+    def test_empty_axes(self):
+        assert ch6_wants_auto([], ch6_idx=0) is False
+
+    def test_custom_threshold(self):
+        assert ch6_wants_auto([0.0, 0.3], ch6_idx=1, threshold=0.2) is True
+        assert ch6_wants_auto([0.0, 0.3], ch6_idx=1, threshold=0.5) is False
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# sticks_active
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestSticksActive:
+
+    def test_all_zero_not_active(self):
+        assert sticks_active([0.0, 0.0, 0.0, 0.0], (0, 1, 2, 3)) is False
+
+    def test_one_stick_above_threshold(self):
+        assert sticks_active([0.0, 0.15, 0.0, 0.0], (0, 1, 2, 3)) is True
+
+    def test_all_exactly_at_threshold_not_active(self):
+        axes = [0.10, 0.10, 0.10, 0.10]
+        assert sticks_active(axes, (0, 1, 2, 3), threshold=0.10) is False
+
+    def test_just_above_threshold_active(self):
+        assert sticks_active([0.0, 0.0, 0.0, 0.11], (0, 1, 2, 3)) is True
+
+    def test_negative_deflection_detected(self):
+        assert sticks_active([-0.5, 0.0, 0.0, 0.0], (0,)) is True
+
+    def test_out_of_range_index_skipped(self):
+        # index 5 is out of range for a 4-element list — should not crash
+        assert sticks_active([0.0, 0.0, 0.0, 0.0], (0, 1, 5)) is False
+
+    def test_only_monitored_axes_checked(self):
+        # axes[4] = 0.9 but only (0,1,2,3) are monitored
+        assert sticks_active([0.0, 0.0, 0.0, 0.0, 0.9], (0, 1, 2, 3)) is False
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# ramp_alpha / alpha_for_mode
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestRampAlpha:
+
+    def test_zero_elapsed_returns_zero(self):
+        assert ramp_alpha(0.0, 0.5) == pytest.approx(0.0)
+
+    def test_full_elapsed_returns_one(self):
+        assert ramp_alpha(0.5, 0.5) == pytest.approx(1.0)
+
+    def test_beyond_duration_clamped_to_one(self):
+        assert ramp_alpha(1.0, 0.5) == pytest.approx(1.0)
+
+    def test_halfway(self):
+        assert ramp_alpha(0.25, 0.5) == pytest.approx(0.5)
+
+    def test_zero_duration_returns_one(self):
+        assert ramp_alpha(0.0, 0.0) == pytest.approx(1.0)
+
+
+class TestAlphaForMode:
+
+    def test_rc_returns_zero(self):
+        assert alpha_for_mode("RC", 0.3, 0.5) == 0.0
+
+    def test_auto_returns_one(self):
+        assert alpha_for_mode("AUTO", 0.3, 0.5) == 1.0
+
+    def test_ramp_to_auto_mid(self):
+        assert alpha_for_mode("RAMP_TO_AUTO", 0.25, 0.5) == pytest.approx(0.5)
+
+    def test_ramp_to_rc_mid(self):
+        assert alpha_for_mode("RAMP_TO_RC", 0.25, 0.5) == pytest.approx(0.5)
+
+    def test_ramp_to_auto_start(self):
+        assert alpha_for_mode("RAMP_TO_AUTO", 0.0, 0.5) == pytest.approx(0.0)
+
+    def test_ramp_to_rc_start(self):
+        assert alpha_for_mode("RAMP_TO_RC", 0.0, 0.5) == pytest.approx(1.0)
+
+    def test_ramp_to_auto_complete(self):
+        assert alpha_for_mode("RAMP_TO_AUTO", 0.5, 0.5) == pytest.approx(1.0)
+
+    def test_ramp_to_rc_complete(self):
+        assert alpha_for_mode("RAMP_TO_RC", 0.5, 0.5) == pytest.approx(0.0)
+
+    def test_unknown_mode_returns_zero(self):
+        assert alpha_for_mode("UNKNOWN", 0.3, 0.5) == 0.0
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# led_pattern
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestLedPattern:
+
+    def test_no_rc_link_overrides_all(self):
+        for mode in ("RC", "RAMP_TO_AUTO", "AUTO", "RAMP_TO_RC"):
+            assert led_pattern(mode, slam_ok=True, rc_link_ok=False) == "blink_red_fast"
+
+    def test_rc_mode_yellow(self):
+        assert led_pattern("RC", slam_ok=True, rc_link_ok=True) == "solid_yellow"
+
+    def test_ramp_to_rc_yellow(self):
+        assert led_pattern("RAMP_TO_RC", slam_ok=True, rc_link_ok=True) == "solid_yellow"
+
+    def test_auto_ok_green(self):
+        assert led_pattern("AUTO", slam_ok=True, rc_link_ok=True) == "solid_green"
+
+    def test_auto_no_slam_orange(self):
+        assert led_pattern("AUTO", slam_ok=False, rc_link_ok=True) == "blink_orange_fast"
+
+    def test_ramp_to_auto_blink_green(self):
+        assert led_pattern("RAMP_TO_AUTO", slam_ok=True, rc_link_ok=True) == "blink_green_slow"
+
+    def test_rc_slam_state_irrelevant(self):
+        p1 = led_pattern("RC", slam_ok=True,  rc_link_ok=True)
+        p2 = led_pattern("RC", slam_ok=False, rc_link_ok=True)
+        assert p1 == p2  # SLAM doesn't affect RC LED
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# step_state — RC transitions
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestStepStateFromRC:
+
+    def test_rc_stays_rc_by_default(self):
+        mode, override, instant = _step(mode="RC")
+        assert mode == "RC"
+        assert not instant
+
+    def test_rc_to_ramp_when_ch6_auto(self):
+        mode, override, instant = _step(mode="RC", ch6_auto=True)
+        assert mode == "RAMP_TO_AUTO"
+        assert not instant
+
+    def test_rc_stays_rc_when_ch6_auto_but_no_slam(self):
+        mode, _, _ = _step(mode="RC", ch6_auto=True, slam_ok=False)
+        assert mode == "RC"
+
+    def test_rc_stays_rc_when_ch6_auto_but_no_rc_link(self):
+        mode, _, instant = _step(mode="RC", ch6_auto=True, rc_link_ok=False)
+        assert mode == "RC"
+        assert instant  # RC link lost → instant
+
+    def test_rc_stays_rc_when_override_active_and_sticks_not_neutral(self):
+        mode, _, _ = _step(
+            mode="RC", ch6_auto=True, slam_ok=True,
+            override_active=True, neutral_elapsed=1.0, return_delay=2.0)
+        assert mode == "RC"
+
+    def test_rc_to_ramp_after_override_clears(self):
+        # override clears after sticks neutral >= return_delay
+        mode, override, _ = _step(
+            mode="RC", ch6_auto=True, slam_ok=True,
+            override_active=True, neutral_elapsed=2.5, return_delay=2.0)
+        assert mode == "RAMP_TO_AUTO"
+        assert not override
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# step_state — RAMP_TO_AUTO transitions
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestStepStateFromRampToAuto:
+
+    def test_continues_ramping(self):
+        mode, _, _ = _step(mode="RAMP_TO_AUTO", ramp_elapsed=0.2, ramp_s=0.5,
+                           ch6_auto=True)
+        assert mode == "RAMP_TO_AUTO"
+
+    def test_completes_to_auto(self):
+        mode, _, _ = _step(mode="RAMP_TO_AUTO", ramp_elapsed=0.5, ramp_s=0.5,
+                           ch6_auto=True)
+        assert mode == "AUTO"
+
+    def test_stick_override_instant_rc(self):
+        mode, override, instant = _step(
+            mode="RAMP_TO_AUTO", ch6_auto=True,
+            sticks_now_active=True)
+        assert mode == "RC"
+        assert override is True
+        assert instant is True
+
+    def test_ch6_low_aborts_to_ramp_rc(self):
+        mode, _, _ = _step(mode="RAMP_TO_AUTO", ch6_auto=False, ramp_elapsed=0.1)
+        assert mode == "RAMP_TO_RC"
+
+    def test_slam_lost_aborts_to_ramp_rc(self):
+        mode, _, _ = _step(mode="RAMP_TO_AUTO", ch6_auto=True, slam_ok=False)
+        assert mode == "RAMP_TO_RC"
+
+    def test_rc_link_lost_instant_rc(self):
+        mode, _, instant = _step(mode="RAMP_TO_AUTO", ch6_auto=True, rc_link_ok=False)
+        assert mode == "RC"
+        assert instant is True
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# step_state — AUTO transitions
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestStepStateFromAuto:
+
+    def test_auto_stays_auto(self):
+        mode, _, _ = _step(mode="AUTO", ch6_auto=True)
+        assert mode == "AUTO"
+
+    def test_stick_override_instant_rc(self):
+        mode, override, instant = _step(
+            mode="AUTO", ch6_auto=True, sticks_now_active=True)
+        assert mode == "RC"
+        assert override is True
+        assert instant is True
+
+    def test_ch6_low_ramp_to_rc(self):
+        mode, _, instant = _step(mode="AUTO", ch6_auto=False)
+        assert mode == "RAMP_TO_RC"
+        assert not instant
+
+    def test_slam_lost_ramp_to_rc(self):
+        mode, _, instant = _step(mode="AUTO", ch6_auto=True, slam_ok=False)
+        assert mode == "RAMP_TO_RC"
+        assert not instant
+
+    def test_rc_link_lost_instant_rc(self):
+        mode, _, instant = _step(mode="AUTO", ch6_auto=True, rc_link_ok=False)
+        assert mode == "RC"
+        assert instant is True
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# step_state — RAMP_TO_RC transitions
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestStepStateFromRampToRC:
+
+    def test_continues_ramping(self):
+        mode, _, _ = _step(mode="RAMP_TO_RC", ramp_elapsed=0.2, ramp_s=0.5)
+        assert mode == "RAMP_TO_RC"
+
+    def test_completes_to_rc(self):
+        mode, _, _ = _step(mode="RAMP_TO_RC", ramp_elapsed=0.5, ramp_s=0.5)
+        assert mode == "RC"
+
+    def test_stick_override_instant_rc(self):
+        mode, override, instant = _step(
+            mode="RAMP_TO_RC", sticks_now_active=True, ch6_auto=True)
+        assert mode == "RC"
+        assert instant is True
+
+    def test_ch6_still_auto_no_override_cancels_back(self):
+        # CH6 still says AUTO, no override → cancel to RAMP_TO_AUTO
+        mode, _, _ = _step(mode="RAMP_TO_RC", ch6_auto=True, slam_ok=True,
+                           override_active=False, ramp_elapsed=0.1)
+        assert mode == "RAMP_TO_AUTO"
+
+    def test_rc_link_lost_instant_rc(self):
+        _, _, instant = _step(mode="RAMP_TO_RC", rc_link_ok=False)
+        assert instant is True
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# blend_twist
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestBlendTwist:
+
+    def test_alpha_zero_zeros_output(self):
+        lx, az = blend_twist(1.0, 0.5, 0.0)
+        assert lx == pytest.approx(0.0)
+        assert az == pytest.approx(0.0)
+
+    def test_alpha_one_passes_through(self):
+        lx, az = blend_twist(0.4, -0.7, 1.0)
+        assert lx == pytest.approx(0.4)
+        assert az == pytest.approx(-0.7)
+
+    def test_alpha_half_scales(self):
+        lx, az = blend_twist(1.0, 1.0, 0.5)
+        assert lx == pytest.approx(0.5)
+        assert az == pytest.approx(0.5)
+
+    def test_alpha_above_one_clamped(self):
+        lx, az = blend_twist(1.0, 1.0, 1.5)
+        assert lx == pytest.approx(1.0)
+        assert az == pytest.approx(1.0)
+
+    def test_alpha_below_zero_clamped(self):
+        lx, az = blend_twist(1.0, 1.0, -0.5)
+        assert lx == pytest.approx(0.0)
+        assert az == pytest.approx(0.0)
+
+    def test_negative_cmd_vel_preserved(self):
+        lx, az = blend_twist(-0.5, -1.0, 0.5)
+        assert lx == pytest.approx(-0.25)
+        assert az == pytest.approx(-0.5)
+
+
+# ─────────────────────────────────────────────────────────────────────────────
+# Integration scenarios
+# ─────────────────────────────────────────────────────────────────────────────
+
+class TestIntegrationScenarios:
+
+    def test_full_rc_to_auto_ramp_cycle(self):
+        """Simulate RC→RAMP_TO_AUTO→AUTO with 0.5s ramp at 20Hz."""
+        mode      = "RC"
+        override  = False
+        ramp_s    = 0.5
+        dt        = 1.0 / 20.0
+        t_ramp    = 0.0
+
+        # CH6 flipped to AUTO, SLAM ok
+        mode, override, instant = step_state(
+            mode, 0.0, ramp_s, True, False, 5.0, 2.0, True, True, override)
+        assert mode == "RAMP_TO_AUTO"
+        t_ramp = 0.0
+
+        # Simulate 10 ticks (0.5s) → should complete
+        for tick in range(10):
+            t_ramp += dt
+            mode, override, instant = step_state(
+                mode, t_ramp, ramp_s, True, False, 5.0, 2.0, True, True, override)
+
+        assert mode == "AUTO"
+        assert alpha_for_mode("AUTO", t_ramp, ramp_s) == 1.0
+
+    def test_stick_override_then_return_to_auto(self):
+        """Stick override while AUTO → RC → neutral 2s → back to AUTO."""
+        # Start in AUTO
+        mode, override, instant = _step(
+            mode="AUTO", ch6_auto=True, sticks_now_active=True)
+        assert mode == "RC"
+        assert override is True
+        assert instant is True
+
+        # Sticks neutral but < 2s — stays RC
+        mode, override, _ = _step(
+            mode="RC", ch6_auto=True, slam_ok=True,
+            override_active=True, neutral_elapsed=1.0, return_delay=2.0)
+        assert mode == "RC"
+
+        # Sticks neutral >= 2s — override clears, enters ramp
+        mode, override, _ = _step(
+            mode="RC", ch6_auto=True, slam_ok=True,
+            override_active=True, neutral_elapsed=2.0, return_delay=2.0)
+        assert mode == "RAMP_TO_AUTO"
+        assert override is False
+
+    def test_slam_lost_during_auto_graceful_exit(self):
+        """SLAM fix lost during AUTO → graceful RAMP_TO_RC (not instant)."""
+        mode, _, instant = _step(mode="AUTO", ch6_auto=True, slam_ok=False)
+        assert mode == "RAMP_TO_RC"
+        assert not instant
+
+    def test_rc_link_lost_always_instant(self):
+        """RC link lost from any state → instant RC."""
+        for start_mode in ("AUTO", "RAMP_TO_AUTO", "RAMP_TO_RC"):
+            mode, _, instant = _step(mode=start_mode, ch6_auto=True, rc_link_ok=False)
+            assert mode == "RC", f"from {start_mode}"
+            assert instant is True, f"from {start_mode}"
+
+    def test_blend_alpha_sequence_through_ramp(self):
+        """Blend alpha is monotonically increasing during RAMP_TO_AUTO."""
+        ramp_s = 0.5
+        alphas = [
+            alpha_for_mode("RAMP_TO_AUTO", t * 0.05, ramp_s)
+            for t in range(11)   # 0.0 to 0.5 s in 0.05 s steps
+        ]
+        for a, b in zip(alphas, alphas[1:]):
+            assert b >= a
+
+    def test_ramp_to_rc_alpha_decreasing(self):
+        """Blend alpha is monotonically decreasing during RAMP_TO_RC."""
+        ramp_s = 0.5
+        alphas = [
+            alpha_for_mode("RAMP_TO_RC", t * 0.05, ramp_s)
+            for t in range(11)
+        ]
+        for a, b in zip(alphas, alphas[1:]):
+            assert b <= a
+
+    def test_no_auto_without_slam(self):
+        """From RC, ch6=AUTO but slam=False → cannot enter AUTO path."""
+        mode, _, _ = _step(mode="RC", ch6_auto=True, slam_ok=False)
+        assert mode == "RC"
+
+    def test_no_auto_without_rc_link(self):
+        """From RC, ch6=AUTO but rc_link=False → stays RC (instant)."""
+        mode, _, instant = _step(mode="RC", ch6_auto=True, rc_link_ok=False)
+        assert mode == "RC"
+        assert instant is True

src/battery.c

@@ -0,0 +1,89 @@
+/*
+ * battery.c — Vbat ADC reading for CRSF telemetry uplink (Issue #103)
+ *
+ * Hardware: ADC3 channel IN11 on PC1 (ADC_BATT 1, Mamba F722S FC).
+ * Voltage divider: 10 kΩ (upper) / 1 kΩ (lower) → VBAT_SCALE_NUM = 11.
+ *
+ *   Vbat_mV = (raw × VBAT_AREF_MV × VBAT_SCALE_NUM) >> VBAT_ADC_BITS
+ *           = (raw × 3300 × 11) / 4096
+ */
+
+#include "battery.h"
+#include "config.h"
+#include "stm32f7xx_hal.h"
+
+static ADC_HandleTypeDef s_hadc;
+static bool              s_ready = false;
+
+void battery_init(void) {
+    __HAL_RCC_ADC3_CLK_ENABLE();
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+
+    /* PC1 → analog input (no pull, no speed) */
+    GPIO_InitTypeDef gpio = {0};
+    gpio.Pin  = GPIO_PIN_1;
+    gpio.Mode = GPIO_MODE_ANALOG;
+    gpio.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOC, &gpio);
+
+    /* ADC3 — single-conversion, software trigger, 12-bit right-aligned */
+    s_hadc.Instance                   = ADC3;
+    s_hadc.Init.ClockPrescaler        = ADC_CLOCK_SYNC_PCLK_DIV8;  /* APB2/8 */
+    s_hadc.Init.Resolution            = ADC_RESOLUTION_12B;
+    s_hadc.Init.ScanConvMode          = DISABLE;
+    s_hadc.Init.ContinuousConvMode    = DISABLE;
+    s_hadc.Init.DiscontinuousConvMode = DISABLE;
+    s_hadc.Init.ExternalTrigConvEdge  = ADC_EXTERNALTRIGCONVEDGE_NONE;
+    s_hadc.Init.ExternalTrigConv      = ADC_SOFTWARE_START;
+    s_hadc.Init.DataAlign             = ADC_DATAALIGN_RIGHT;
+    s_hadc.Init.NbrOfConversion       = 1;
+    s_hadc.Init.DMAContinuousRequests = DISABLE;
+    s_hadc.Init.EOCSelection          = ADC_EOC_SINGLE_CONV;
+    if (HAL_ADC_Init(&s_hadc) != HAL_OK) return;
+
+    /* Channel IN11 (PC1) with 480-cycle sampling for stability */
+    ADC_ChannelConfTypeDef ch = {0};
+    ch.Channel      = ADC_CHANNEL_11;
+    ch.Rank         = 1;
+    ch.SamplingTime = ADC_SAMPLETIME_480CYCLES;
+    if (HAL_ADC_ConfigChannel(&s_hadc, &ch) != HAL_OK) return;
+
+    s_ready = true;
+}
+
+uint32_t battery_read_mv(void) {
+    if (!s_ready) return 0u;
+
+    HAL_ADC_Start(&s_hadc);
+    if (HAL_ADC_PollForConversion(&s_hadc, 2u) != HAL_OK) return 0u;
+    uint32_t raw = HAL_ADC_GetValue(&s_hadc);
+    HAL_ADC_Stop(&s_hadc);
+
+    /* Vbat_mV = raw × (VREF_mV × scale) / ADC_counts */
+    return (raw * (uint32_t)VBAT_AREF_MV * VBAT_SCALE_NUM) /
+           ((1u << VBAT_ADC_BITS));
+}
+
+/*
+ * Coarse SoC estimate.
+ * 3S LiPo: 9.9 V (0%) – 12.6 V (100%) — detect by Vbat < 13 V
+ * 4S LiPo: 13.2 V (0%) – 16.8 V (100%) — detect by Vbat ≥ 13 V
+ */
+uint8_t battery_estimate_pct(uint32_t voltage_mv) {
+    uint32_t v_min_mv, v_max_mv;
+
+    if (voltage_mv >= 13000u) {
+        /* 4S LiPo */
+        v_min_mv = 13200u;
+        v_max_mv = 16800u;
+    } else {
+        /* 3S LiPo */
+        v_min_mv = 9900u;
+        v_max_mv = 12600u;
+    }
+
+    if (voltage_mv <= v_min_mv) return 0u;
+    if (voltage_mv >= v_max_mv) return 100u;
+
+    return (uint8_t)(((voltage_mv - v_min_mv) * 100u) / (v_max_mv - v_min_mv));
+}

src/crsf.c

@@ -291,3 +291,64 @@ int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max) {
     if (r > max) r = max;
     return (int16_t)r;
 }
+
+/* ------------------------------------------------------------------ */
+/* Telemetry TX helpers                                                 */
+/* ------------------------------------------------------------------ */
+
+/*
+ * Build a CRSF frame in `buf` and return the total byte count.
+ * buf must be at least CRSF_MAX_FRAME_LEN bytes.
+ * frame_type : CRSF type byte (e.g. 0x08 battery, 0x21 flight mode)
+ * payload    : frame payload bytes (excluding type, CRC)
+ * plen       : payload length in bytes
+ */
+static uint8_t crsf_build_frame(uint8_t *buf, uint8_t frame_type,
+                                 const uint8_t *payload, uint8_t plen) {
+    /* Total frame = SYNC + LEN + TYPE + PAYLOAD + CRC */
+    uint8_t frame_len = 2u + 1u + plen + 1u;  /* SYNC + LEN + TYPE + payload + CRC */
+    if (frame_len > CRSF_MAX_FRAME_LEN) return 0;
+
+    buf[0] = CRSF_SYNC;                   /* 0xC8 */
+    buf[1] = (uint8_t)(plen + 2u);        /* LEN = TYPE + payload + CRC */
+    buf[2] = frame_type;
+    memcpy(&buf[3], payload, plen);
+    buf[frame_len - 1] = crsf_frame_crc(buf, frame_len);
+    return frame_len;
+}
+
+/*
+ * crsf_send_battery() — type 0x08 battery sensor.
+ * voltage_mv   → units of 100 mV (big-endian uint16)
+ * current_ma   → units of 100 mA (big-endian uint16)
+ * remaining_pct→ 0–100 % (uint8); capacity mAh always 0 (no coulomb counter)
+ */
+void crsf_send_battery(uint32_t voltage_mv, uint32_t current_ma,
+                        uint8_t remaining_pct) {
+    uint16_t v100 = (uint16_t)(voltage_mv / 100u);   /* 100 mV units */
+    uint16_t c100 = (uint16_t)(current_ma / 100u);   /* 100 mA units */
+    /* Payload: [v_hi][v_lo][c_hi][c_lo][cap_hi][cap_mid][cap_lo][remaining] */
+    uint8_t payload[8] = {
+        (uint8_t)(v100 >> 8), (uint8_t)(v100 & 0xFF),
+        (uint8_t)(c100 >> 8), (uint8_t)(c100 & 0xFF),
+        0, 0, 0,              /* capacity mAh — not tracked */
+        remaining_pct,
+    };
+    uint8_t frame[CRSF_MAX_FRAME_LEN];
+    uint8_t flen = crsf_build_frame(frame, 0x08u, payload, sizeof(payload));
+    if (flen) HAL_UART_Transmit(&s_uart, frame, flen, 5u);
+}
+
+/*
+ * crsf_send_flight_mode() — type 0x21 flight mode text.
+ * Displays on the handset's OSD/status bar.
+ * "ARMED\0"  when armed  (5 payload bytes + null)
+ * "DISARM\0" when not    (7 payload bytes + null)
+ */
+void crsf_send_flight_mode(bool armed) {
+    const char *text = armed ? "ARMED" : "DISARM";
+    uint8_t plen = (uint8_t)(strlen(text) + 1u);   /* include null terminator */
+    uint8_t frame[CRSF_MAX_FRAME_LEN];
+    uint8_t flen = crsf_build_frame(frame, 0x21u, (const uint8_t *)text, plen);
+    if (flen) HAL_UART_Transmit(&s_uart, frame, flen, 5u);
+}

src/main.c

@@ -16,6 +16,7 @@
 #include "bmp280.h"
 #include "mag.h"
 #include "jetson_cmd.h"
+#include "battery.h"
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
@@ -142,6 +143,9 @@ int main(void) {
     mode_manager_t mode;
     mode_manager_init(&mode);
 
+    /* Init battery ADC (PC1/ADC3 — Vbat divider 11:1) for CRSF telemetry */
+    battery_init();
+
     /* Probe I2C1 for optional sensors — skip gracefully if not found */
     int baro_chip = 0;  /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
     mag_type_t mag_type = MAG_NONE;
@@ -167,6 +171,7 @@ int main(void) {
     uint32_t send_tick = 0;
     uint32_t balance_tick = 0;
     uint32_t esc_tick = 0;
+    uint32_t crsf_telem_tick = 0;  /* CRSF uplink telemetry TX timer */
     const float dt = 1.0f / PID_LOOP_HZ;  /* 1ms at 1kHz */
 
     while (1) {
@@ -313,6 +318,17 @@ int main(void) {
             }
         }
 
+        /* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.
+         * Sends battery sensor frame (0x08) and flight mode frame (0x21)
+         * back to ELRS TX module so the pilot's handset OSD shows Vbat + state. */
+        if (now - crsf_telem_tick >= (1000u / CRSF_TELEMETRY_HZ)) {
+            crsf_telem_tick = now;
+            uint32_t vbat_mv  = battery_read_mv();
+            uint8_t  soc_pct  = battery_estimate_pct(vbat_mv);
+            crsf_send_battery(vbat_mv, 0u, soc_pct);
+            crsf_send_flight_mode(bal.state == BALANCE_ARMED);
+        }
+
         /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
         if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
             send_tick = now;

test/test_crsf_frames.py

@@ -0,0 +1,237 @@
+"""
+test_crsf_frames.py — Unit tests for CRSF telemetry frame building (Issue #103).
+
+Mirrors the C logic in src/crsf.c for crsf_send_battery() and
+crsf_send_flight_mode() so frame layout and CRC can be verified
+without flashing hardware.
+
+Run with:  pytest test/test_crsf_frames.py -v
+"""
+
+import struct
+import pytest
+
+
+# ── CRC8 DVB-S2 (polynomial 0xD5) ─────────────────────────────────────────
+
+def crc8_dvb_s2(crc: int, byte: int) -> int:
+    crc ^= byte
+    for _ in range(8):
+        crc = ((crc << 1) ^ 0xD5) & 0xFF if (crc & 0x80) else (crc << 1) & 0xFF
+    return crc
+
+
+def crsf_frame_crc(frame: bytes) -> int:
+    """CRC covers frame[2] (type) through frame[-2] (last payload byte)."""
+    crc = 0
+    for b in frame[2:-1]:
+        crc = crc8_dvb_s2(crc, b)
+    return crc
+
+
+# ── Frame builders matching C implementation ───────────────────────────────
+
+CRSF_SYNC = 0xC8
+
+
+def build_battery_frame(voltage_mv: int, current_ma: int,
+                         remaining_pct: int) -> bytes:
+    """
+    Type 0x08 — battery sensor.
+    voltage  : 100 mV units, uint16 big-endian
+    current  : 100 mA units, uint16 big-endian
+    capacity : 0 mAh (not tracked), uint24 big-endian
+    remaining: 0–100 %, uint8
+    Total payload = 8 bytes.
+    """
+    v100 = voltage_mv // 100
+    c100 = current_ma // 100
+    payload = struct.pack('>HH3sB',
+                          v100, c100,
+                          b'\x00\x00\x00',
+                          remaining_pct)
+    frame_type = 0x08
+    # SYNC + LEN(TYPE+payload+CRC) + TYPE + payload + CRC
+    frame_body = bytes([CRSF_SYNC, len(payload) + 2, frame_type]) + payload
+    frame = frame_body + b'\x00'  # placeholder CRC slot
+    crc = crsf_frame_crc(frame)
+    return frame_body + bytes([crc])
+
+
+def build_flight_mode_frame(armed: bool) -> bytes:
+    """Type 0x21 — flight mode text, null-terminated."""
+    text = b'ARMED\x00' if armed else b'DISARM\x00'
+    frame_type = 0x21
+    frame_body = bytes([CRSF_SYNC, len(text) + 2, frame_type]) + text
+    frame = frame_body + b'\x00'
+    crc = crsf_frame_crc(frame)
+    return frame_body + bytes([crc])
+
+
+# ── Helpers ────────────────────────────────────────────────────────────────
+
+def validate_frame(frame: bytes):
+    """Assert basic CRSF frame invariants."""
+    assert frame[0] == CRSF_SYNC, "bad sync byte"
+    length_field = frame[1]
+    total = length_field + 2        # SYNC + LEN + rest
+    assert len(frame) == total,    f"frame length mismatch: {len(frame)} != {total}"
+    assert len(frame) <= 64,       "frame too long (CRSF max 64 bytes)"
+    expected_crc = crsf_frame_crc(frame)
+    assert frame[-1] == expected_crc, \
+        f"CRC mismatch: got {frame[-1]:#04x}, expected {expected_crc:#04x}"
+
+
+# ── Battery frame tests ────────────────────────────────────────────────────
+
+class TestBatteryFrame:
+    def test_sync_byte(self):
+        f = build_battery_frame(12600, 0, 100)
+        assert f[0] == 0xC8
+
+    def test_frame_type(self):
+        f = build_battery_frame(12600, 0, 100)
+        assert f[2] == 0x08
+
+    def test_frame_invariants(self):
+        validate_frame(build_battery_frame(12600, 5000, 80))
+
+    def test_voltage_encoding_3s_full(self):
+        """12.6 V → 126 in 100 mV units, big-endian."""
+        f = build_battery_frame(12600, 0, 100)
+        v100 = (f[3] << 8) | f[4]
+        assert v100 == 126
+
+    def test_voltage_encoding_4s_full(self):
+        """16.8 V → 168."""
+        f = build_battery_frame(16800, 0, 100)
+        v100 = (f[3] << 8) | f[4]
+        assert v100 == 168
+
+    def test_current_encoding(self):
+        """5000 mA → 50 in 100 mA units."""
+        f = build_battery_frame(12000, 5000, 75)
+        c100 = (f[5] << 8) | f[6]
+        assert c100 == 50
+
+    def test_remaining_pct(self):
+        f = build_battery_frame(11000, 0, 42)
+        assert f[10] == 42
+
+    def test_capacity_zero(self):
+        """Capacity bytes (cap_hi, cap_mid, cap_lo) are zero — no coulomb counter."""
+        f = build_battery_frame(12600, 0, 100)
+        assert f[7] == 0 and f[8] == 0 and f[9] == 0
+
+    def test_crc_correct(self):
+        f = build_battery_frame(11500, 2000, 65)
+        validate_frame(f)
+
+    def test_zero_voltage(self):
+        """Disconnected battery → 0 mV, 0 %."""
+        f = build_battery_frame(0, 0, 0)
+        validate_frame(f)
+        v100 = (f[3] << 8) | f[4]
+        assert v100 == 0
+        assert f[10] == 0
+
+    def test_total_frame_length(self):
+        """Battery frame: SYNC(1)+LEN(1)+TYPE(1)+payload(8)+CRC(1) = 12 bytes."""
+        f = build_battery_frame(12000, 0, 80)
+        assert len(f) == 12
+
+
+# ── Flight mode frame tests ────────────────────────────────────────────────
+
+class TestFlightModeFrame:
+    def test_armed_text(self):
+        f = build_flight_mode_frame(True)
+        payload = f[3:-1]
+        assert payload == b'ARMED\x00'
+
+    def test_disarmed_text(self):
+        f = build_flight_mode_frame(False)
+        payload = f[3:-1]
+        assert payload == b'DISARM\x00'
+
+    def test_frame_type(self):
+        assert build_flight_mode_frame(True)[2] == 0x21
+        assert build_flight_mode_frame(False)[2] == 0x21
+
+    def test_crc_armed(self):
+        validate_frame(build_flight_mode_frame(True))
+
+    def test_crc_disarmed(self):
+        validate_frame(build_flight_mode_frame(False))
+
+    def test_armed_frame_length(self):
+        """ARMED\0 = 6 bytes payload → total 10 bytes."""
+        f = build_flight_mode_frame(True)
+        assert len(f) == 10
+
+    def test_disarmed_frame_length(self):
+        """DISARM\0 = 7 bytes payload → total 11 bytes."""
+        f = build_flight_mode_frame(False)
+        assert len(f) == 11
+
+
+# ── CRC8 DVB-S2 self-test ─────────────────────────────────────────────────
+
+class TestCRC8:
+    def test_known_vector(self):
+        """Verify CRC8 DVB-S2 against known value (poly 0xD5, init 0)."""
+        # CRC of single byte 0xAB with poly 0xD5, init 0 → 0xC8
+        crc = crc8_dvb_s2(0, 0xAB)
+        assert crc == 0xC8
+
+    def test_different_payloads_differ(self):
+        f1 = build_battery_frame(12600, 0, 100)
+        f2 = build_battery_frame(11000, 0, 50)
+        assert f1[-1] != f2[-1], "different payloads should have different CRCs"
+
+    def test_crc_covers_type(self):
+        """Changing the frame type changes the CRC."""
+        fa = build_battery_frame(12600, 0, 100)   # type 0x08
+        fb = build_flight_mode_frame(True)          # type 0x21
+        # Both frames differ in type byte and thus CRC
+        assert fa[-1] != fb[-1]
+
+
+# ── battery_estimate_pct logic mirrored in Python ─────────────────────────
+
+def battery_estimate_pct(voltage_mv: int) -> int:
+    """Python mirror of battery_estimate_pct() in battery.c."""
+    if voltage_mv >= 13000:
+        v_min, v_max = 13200, 16800
+    else:
+        v_min, v_max = 9900, 12600
+    if voltage_mv <= v_min:
+        return 0
+    if voltage_mv >= v_max:
+        return 100
+    return int((voltage_mv - v_min) * 100 / (v_max - v_min))
+
+
+class TestBatteryEstimatePct:
+    def test_3s_full(self):
+        assert battery_estimate_pct(12600) == 100
+
+    def test_3s_empty(self):
+        assert battery_estimate_pct(9900) == 0
+
+    def test_3s_mid(self):
+        pct = battery_estimate_pct(11250)
+        assert 45 <= pct <= 55
+
+    def test_4s_full(self):
+        assert battery_estimate_pct(16800) == 100
+
+    def test_4s_empty(self):
+        assert battery_estimate_pct(13200) == 0
+
+    def test_3s_over_voltage(self):
+        """13000 mV triggers 4S branch (v_min=13200) → classified as dead 4S → 0%."""
+        assert battery_estimate_pct(13000) == 0
+
+    def test_zero_voltage(self):
+        assert battery_estimate_pct(0) == 0

ui/social-bot/.gitignore

@@ -0,0 +1,2 @@
+node_modules/
+dist/

ui/social-bot/index.html

@@ -0,0 +1,12 @@
+<!DOCTYPE html>
+<html lang="en">
+  <head>
+    <meta charset="UTF-8" />
+    <meta name="viewport" content="width=device-width, initial-scale=1.0" />
+    <title>Saltybot Social Dashboard</title>
+  </head>
+  <body>
+    <div id="root"></div>
+    <script type="module" src="/src/main.jsx"></script>
+  </body>
+</html>

ui/social-bot/package.json

@@ -0,0 +1,24 @@
+{
+  "name": "saltybot-social-dashboard",
+  "version": "1.0.0",
+  "private": true,
+  "type": "module",
+  "description": "Social-bot monitoring and configuration dashboard",
+  "scripts": {
+    "dev": "vite --port 8080 --host",
+    "build": "vite build",
+    "preview": "vite preview --port 8080 --host"
+  },
+  "dependencies": {
+    "react": "^18.3.1",
+    "react-dom": "^18.3.1",
+    "roslib": "^1.4.1"
+  },
+  "devDependencies": {
+    "@vitejs/plugin-react": "^4.3.1",
+    "autoprefixer": "^10.4.20",
+    "postcss": "^8.4.47",
+    "tailwindcss": "^3.4.14",
+    "vite": "^5.4.10"
+  }
+}

ui/social-bot/postcss.config.js

@@ -0,0 +1,6 @@
+export default {
+  plugins: {
+    tailwindcss: {},
+    autoprefixer: {},
+  },
+};

ui/social-bot/src/App.jsx

@@ -0,0 +1,165 @@
+/**
+ * App.jsx — Saltybot Social Dashboard root component.
+ *
+ * Layout:
+ *   [TopBar: connection config + pipeline state badge]
+ *   [TabNav: Status | Faces | Conversation | Personality | Navigation]
+ *   [TabContent]
+ */
+
+import { useState, useCallback } from 'react';
+import { useRosbridge } from './hooks/useRosbridge.js';
+import { StatusPanel } from './components/StatusPanel.jsx';
+import { FaceGallery } from './components/FaceGallery.jsx';
+import { ConversationLog } from './components/ConversationLog.jsx';
+import { PersonalityTuner } from './components/PersonalityTuner.jsx';
+import { NavModeSelector } from './components/NavModeSelector.jsx';
+
+const TABS = [
+  { id: 'status',       label: 'Status',         icon: '⬤' },
+  { id: 'faces',        label: 'Faces',           icon: '◉' },
+  { id: 'conversation', label: 'Conversation',    icon: '◌' },
+  { id: 'personality',  label: 'Personality',     icon: '◈' },
+  { id: 'navigation',   label: 'Navigation',      icon: '◫' },
+];
+
+const DEFAULT_WS_URL = 'ws://localhost:9090';
+
+function ConnectionBar({ url, setUrl, connected, error }) {
+  const [editing, setEditing] = useState(false);
+  const [draft, setDraft] = useState(url);
+
+  const handleApply = () => {
+    setUrl(draft);
+    setEditing(false);
+  };
+
+  return (
+    <div className="flex items-center gap-2 text-xs">
+      {/* Connection dot */}
+      <div className={`w-2 h-2 rounded-full shrink-0 ${
+        connected ? 'bg-green-400' : error ? 'bg-red-500' : 'bg-gray-600'
+      }`} />
+
+      {editing ? (
+        <div className="flex items-center gap-1">
+          <input
+            value={draft}
+            onChange={(e) => setDraft(e.target.value)}
+            onKeyDown={(e) => { if (e.key === 'Enter') handleApply(); if (e.key === 'Escape') setEditing(false); }}
+            autoFocus
+            className="bg-gray-900 border border-cyan-800 rounded px-2 py-0.5 text-cyan-300 w-52 focus:outline-none"
+          />
+          <button onClick={handleApply} className="px-2 py-0.5 rounded bg-cyan-950 border border-cyan-700 text-cyan-400 hover:bg-cyan-900">Connect</button>
+          <button onClick={() => setEditing(false)} className="text-gray-600 hover:text-gray-400 px-1">✕</button>
+        </div>
+      ) : (
+        <button
+          onClick={() => { setDraft(url); setEditing(true); }}
+          className="text-gray-500 hover:text-cyan-400 transition-colors truncate max-w-40"
+          title={url}
+        >
+          {connected ? (
+            <span className="text-green-400">rosbridge: {url}</span>
+          ) : error ? (
+            <span className="text-red-400" title={error}>⚠ {url}</span>
+          ) : (
+            <span className="text-gray-500">{url} (connecting…)</span>
+          )}
+        </button>
+      )}
+    </div>
+  );
+}
+
+export default function App() {
+  const [wsUrl, setWsUrl] = useState(DEFAULT_WS_URL);
+  const [activeTab, setActiveTab] = useState('status');
+
+  const { connected, error, subscribe, publish, callService, setParam } = useRosbridge(wsUrl);
+
+  // Memoized publish for NavModeSelector (avoids recreating on every render)
+  const publishFn = useCallback(
+    (name, type, data) => publish(name, type, data),
+    [publish]
+  );
+
+  return (
+    <div className="min-h-screen flex flex-col bg-[#050510] text-gray-300 font-mono">
+      {/* ── Top Bar ── */}
+      <header className="flex items-center justify-between px-4 py-2 bg-[#070712] border-b border-cyan-950 shrink-0 gap-2 flex-wrap">
+        <div className="flex items-center gap-3">
+          <span className="text-orange-500 font-bold tracking-widest text-sm">⚡ SALTYBOT</span>
+          <span className="text-cyan-800 text-xs hidden sm:inline">SOCIAL DASHBOARD</span>
+        </div>
+
+        <ConnectionBar
+          url={wsUrl}
+          setUrl={setWsUrl}
+          connected={connected}
+          error={error}
+        />
+      </header>
+
+      {/* ── Tab Nav ── */}
+      <nav className="bg-[#070712] border-b border-cyan-950 shrink-0">
+        <div className="flex overflow-x-auto">
+          {TABS.map((tab) => (
+            <button
+              key={tab.id}
+              onClick={() => setActiveTab(tab.id)}
+              className={`flex items-center gap-1.5 px-4 py-2.5 text-xs font-bold tracking-widest whitespace-nowrap border-b-2 transition-colors ${
+                activeTab === tab.id
+                  ? 'border-cyan-500 text-cyan-300 bg-cyan-950 bg-opacity-30'
+                  : 'border-transparent text-gray-500 hover:text-gray-300 hover:border-gray-700'
+              }`}
+            >
+              <span className="hidden sm:inline text-base leading-none">{tab.icon}</span>
+              {tab.label.toUpperCase()}
+            </button>
+          ))}
+        </div>
+      </nav>
+
+      {/* ── Content ── */}
+      <main className="flex-1 overflow-y-auto p-4">
+        {activeTab === 'status' && (
+          <StatusPanel subscribe={subscribe} />
+        )}
+
+        {activeTab === 'faces' && (
+          <FaceGallery
+            subscribe={subscribe}
+            callService={callService}
+          />
+        )}
+
+        {activeTab === 'conversation' && (
+          <ConversationLog subscribe={subscribe} />
+        )}
+
+        {activeTab === 'personality' && (
+          <PersonalityTuner
+            subscribe={subscribe}
+            setParam={setParam}
+          />
+        )}
+
+        {activeTab === 'navigation' && (
+          <NavModeSelector
+            subscribe={subscribe}
+            publish={publishFn}
+          />
+        )}
+      </main>
+
+      {/* ── Footer ── */}
+      <footer className="bg-[#070712] border-t border-cyan-950 px-4 py-1.5 flex items-center justify-between text-xs text-gray-700 shrink-0">
+        <span>rosbridge: <code className="text-gray-600">{wsUrl}</code></span>
+        <span className={connected ? 'text-green-700' : 'text-red-900'}>
+          {connected ? 'CONNECTED' : 'DISCONNECTED'}
+        </span>
+      </footer>
+    </div>
+  );
+}

ui/social-bot/src/components/ConversationLog.jsx

@@ -0,0 +1,221 @@
+/**
+ * ConversationLog.jsx — Real-time transcript with speaker labels.
+ *
+ * Subscribes:
+ *   /social/speech/transcript      (SpeechTranscript) — human utterances
+ *   /social/conversation/response  (ConversationResponse) — bot replies
+ */
+
+import { useEffect, useRef, useState } from 'react';
+
+const MAX_ENTRIES = 200;
+
+function formatTime(ts) {
+  return new Date(ts).toLocaleTimeString([], { hour: '2-digit', minute: '2-digit', second: '2-digit' });
+}
+
+function HumanBubble({ entry }) {
+  return (
+    <div className="flex flex-col items-end gap-0.5">
+      <div className="flex items-center gap-2 text-xs text-gray-500">
+        <span className="text-xs">{formatTime(entry.ts)}</span>
+        <span className="text-blue-400 font-bold">{entry.speaker || 'unknown'}</span>
+        {entry.confidence != null && (
+          <span className="text-gray-600">({Math.round(entry.confidence * 100)}%)</span>
+        )}
+        {entry.partial && <span className="text-amber-600 text-xs italic">partial</span>}
+      </div>
+      <div className={`max-w-xs sm:max-w-md bubble-human rounded-lg px-3 py-2 text-sm text-blue-100 ${entry.partial ? 'bubble-partial' : ''}`}>
+        {entry.text}
+      </div>
+    </div>
+  );
+}
+
+function BotBubble({ entry }) {
+  return (
+    <div className="flex flex-col items-start gap-0.5">
+      <div className="flex items-center gap-2 text-xs text-gray-500">
+        <span className="text-teal-400 font-bold">Salty</span>
+        {entry.speaker && <span className="text-gray-600">→ {entry.speaker}</span>}
+        <span className="text-xs">{formatTime(entry.ts)}</span>
+        {entry.partial && <span className="text-amber-600 text-xs italic">streaming…</span>}
+      </div>
+      <div className={`max-w-xs sm:max-w-md bubble-bot rounded-lg px-3 py-2 text-sm text-teal-100 ${entry.partial ? 'bubble-partial' : ''}`}>
+        {entry.text}
+      </div>
+    </div>
+  );
+}
+
+export function ConversationLog({ subscribe }) {
+  const [entries, setEntries] = useState([]);
+  const [autoScroll, setAutoScroll] = useState(true);
+  const bottomRef = useRef(null);
+  const scrollRef = useRef(null);
+
+  // Auto-scroll to bottom when new entries arrive
+  useEffect(() => {
+    if (autoScroll && bottomRef.current) {
+      bottomRef.current.scrollIntoView({ behavior: 'smooth' });
+    }
+  }, [entries, autoScroll]);
+
+  // Human transcript
+  useEffect(() => {
+    const unsub = subscribe(
+      '/social/speech/transcript',
+      'saltybot_social_msgs/SpeechTranscript',
+      (msg) => {
+        setEntries((prev) => {
+          const entry = {
+            id: `h-${msg.header?.stamp?.sec ?? Date.now()}-${msg.turn_id ?? Math.random()}`,
+            type: 'human',
+            text: msg.text,
+            speaker: msg.speaker_id,
+            confidence: msg.confidence,
+            partial: msg.is_partial,
+            ts: Date.now(),
+          };
+
+          // Replace partial entry with same turn if exists
+          if (msg.is_partial) {
+            const idx = prev.findLastIndex(
+              (e) => e.type === 'human' && e.partial && e.speaker === msg.speaker_id
+            );
+            if (idx !== -1) {
+              const updated = [...prev];
+              updated[idx] = entry;
+              return updated;
+            }
+          } else {
+            // Replace any trailing partial for same speaker
+            const idx = prev.findLastIndex(
+              (e) => e.type === 'human' && e.partial && e.speaker === msg.speaker_id
+            );
+            if (idx !== -1) {
+              const updated = [...prev];
+              updated[idx] = { ...entry, id: prev[idx].id };
+              return updated.slice(-MAX_ENTRIES);
+            }
+          }
+
+          return [...prev, entry].slice(-MAX_ENTRIES);
+        });
+      }
+    );
+    return unsub;
+  }, [subscribe]);
+
+  // Bot response
+  useEffect(() => {
+    const unsub = subscribe(
+      '/social/conversation/response',
+      'saltybot_social_msgs/ConversationResponse',
+      (msg) => {
+        setEntries((prev) => {
+          const entry = {
+            id: `b-${msg.turn_id ?? Math.random()}`,
+            type: 'bot',
+            text: msg.text,
+            speaker: msg.speaker_id,
+            partial: msg.is_partial,
+            turnId: msg.turn_id,
+            ts: Date.now(),
+          };
+
+          // Replace streaming partial with same turn_id
+          if (msg.turn_id != null) {
+            const idx = prev.findLastIndex(
+              (e) => e.type === 'bot' && e.turnId === msg.turn_id
+            );
+            if (idx !== -1) {
+              const updated = [...prev];
+              updated[idx] = entry;
+              return updated;
+            }
+          }
+
+          return [...prev, entry].slice(-MAX_ENTRIES);
+        });
+      }
+    );
+    return unsub;
+  }, [subscribe]);
+
+  const handleScroll = () => {
+    const el = scrollRef.current;
+    if (!el) return;
+    const atBottom = el.scrollHeight - el.scrollTop - el.clientHeight < 60;
+    setAutoScroll(atBottom);
+  };
+
+  const handleClear = () => setEntries([]);
+
+  return (
+    <div className="flex flex-col h-full" style={{ minHeight: '400px', maxHeight: '70vh' }}>
+      {/* Toolbar */}
+      <div className="flex items-center justify-between mb-2 shrink-0">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest">
+          CONVERSATION LOG ({entries.length})
+        </div>
+        <div className="flex items-center gap-2">
+          <label className="flex items-center gap-1 cursor-pointer">
+            <input
+              type="checkbox"
+              checked={autoScroll}
+              onChange={(e) => setAutoScroll(e.target.checked)}
+              className="accent-cyan-500 w-3 h-3"
+            />
+            <span className="text-gray-500 text-xs">Auto-scroll</span>
+          </label>
+          <button
+            onClick={handleClear}
+            className="px-2 py-0.5 rounded border border-gray-700 text-gray-500 hover:text-gray-300 text-xs"
+          >
+            Clear
+          </button>
+        </div>
+      </div>
+
+      {/* Scroll container */}
+      <div
+        ref={scrollRef}
+        onScroll={handleScroll}
+        className="flex-1 overflow-y-auto space-y-3 pr-1"
+        style={{ minHeight: '300px' }}
+      >
+        {entries.length === 0 ? (
+          <div className="text-gray-600 text-sm text-center py-12 border border-dashed border-gray-800 rounded-lg">
+            Waiting for conversation…
+          </div>
+        ) : (
+          entries.map((entry) =>
+            entry.type === 'human' ? (
+              <HumanBubble key={entry.id} entry={entry} />
+            ) : (
+              <BotBubble key={entry.id} entry={entry} />
+            )
+          )
+        )}
+        <div ref={bottomRef} />
+      </div>
+
+      {/* Legend */}
+      <div className="flex gap-4 mt-2 pt-2 border-t border-gray-900 shrink-0">
+        <div className="flex items-center gap-1.5">
+          <div className="w-3 h-3 rounded-sm bg-blue-950 border border-blue-700" />
+          <span className="text-gray-600 text-xs">Human</span>
+        </div>
+        <div className="flex items-center gap-1.5">
+          <div className="w-3 h-3 rounded-sm bg-teal-950 border border-teal-700" />
+          <span className="text-gray-600 text-xs">Salty</span>
+        </div>
+        <div className="flex items-center gap-1.5">
+          <div className="w-2 h-2 rounded-full bg-amber-600 opacity-60" />
+          <span className="text-gray-600 text-xs">Streaming</span>
+        </div>
+      </div>
+    </div>
+  );
+}

ui/social-bot/src/components/FaceGallery.jsx

@@ -0,0 +1,237 @@
+/**
+ * FaceGallery.jsx — Enrolled person management.
+ *
+ * Services:
+ *   /social/enrollment/list_persons   (ListPersons.srv)
+ *   /social/enrollment/enroll_person  (EnrollPerson.srv)
+ *   /social/enrollment/delete_person  (DeletePerson.srv)
+ *
+ * Topics (live detections):
+ *   /social/face_detections (FaceDetectionArray) — optional, shows live bboxes
+ */
+
+import { useEffect, useState, useCallback } from 'react';
+
+function PersonCard({ person, onDelete, liveDetection }) {
+  const [deleting, setDeleting] = useState(false);
+
+  const enrolledAt = person.enrolled_at
+    ? new Date(person.enrolled_at.sec * 1000).toLocaleDateString()
+    : '—';
+
+  const handleDelete = async () => {
+    if (!confirm(`Delete ${person.person_name}?`)) return;
+    setDeleting(true);
+    try {
+      await onDelete(person.person_id);
+    } finally {
+      setDeleting(false);
+    }
+  };
+
+  const isActive = liveDetection?.face_id === person.person_id;
+
+  return (
+    <div className={`bg-gray-900 rounded-lg border p-3 flex flex-col gap-2 transition-colors ${
+      isActive ? 'border-green-600 bg-green-950' : 'border-gray-800'
+    }`}>
+      {/* Avatar placeholder */}
+      <div className="w-full aspect-square rounded bg-gray-800 border border-gray-700 flex items-center justify-center relative overflow-hidden">
+        <svg className="w-12 h-12 text-gray-600" fill="currentColor" viewBox="0 0 24 24">
+          <path d="M12 12c2.7 0 4.8-2.1 4.8-4.8S14.7 2.4 12 2.4 7.2 4.5 7.2 7.2 9.3 12 12 12zm0 2.4c-3.2 0-9.6 1.6-9.6 4.8v2.4h19.2v-2.4c0-3.2-6.4-4.8-9.6-4.8z"/>
+        </svg>
+        {isActive && (
+          <div className="absolute inset-0 border-2 border-green-500 rounded pointer-events-none" />
+        )}
+        <div className="absolute top-1 right-1">
+          <div className={`w-2 h-2 rounded-full ${isActive ? 'bg-green-400' : 'bg-gray-700'}`} />
+        </div>
+      </div>
+
+      {/* Info */}
+      <div>
+        <div className="text-cyan-300 text-sm font-bold truncate">{person.person_name}</div>
+        <div className="text-gray-600 text-xs">ID: {person.person_id}</div>
+        <div className="text-gray-600 text-xs">Samples: {person.sample_count}</div>
+        <div className="text-gray-600 text-xs">Enrolled: {enrolledAt}</div>
+      </div>
+
+      {/* Delete */}
+      <button
+        onClick={handleDelete}
+        disabled={deleting}
+        className="w-full py-1 text-xs rounded border border-red-900 text-red-400 hover:bg-red-950 hover:border-red-700 disabled:opacity-40 transition-colors"
+      >
+        {deleting ? 'Deleting…' : 'Delete'}
+      </button>
+    </div>
+  );
+}
+
+export function FaceGallery({ subscribe, callService }) {
+  const [persons, setPersons] = useState([]);
+  const [loading, setLoading] = useState(false);
+  const [error, setError] = useState(null);
+  const [enrolling, setEnrolling] = useState(false);
+  const [newName, setNewName] = useState('');
+  const [newSamples, setNewSamples] = useState(5);
+  const [liveDetections, setLiveDetections] = useState([]);
+
+  const loadPersons = useCallback(async () => {
+    setLoading(true);
+    setError(null);
+    try {
+      const result = await callService(
+        '/social/enrollment/list_persons',
+        'saltybot_social_msgs/srv/ListPersons',
+        {}
+      );
+      setPersons(result?.persons ?? []);
+    } catch (e) {
+      setError('Service unavailable: ' + (e?.message ?? e));
+    } finally {
+      setLoading(false);
+    }
+  }, [callService]);
+
+  useEffect(() => {
+    loadPersons();
+  }, [loadPersons]);
+
+  // Live face detections
+  useEffect(() => {
+    const unsub = subscribe(
+      '/social/face_detections',
+      'saltybot_social_msgs/FaceDetectionArray',
+      (msg) => setLiveDetections(msg.faces ?? [])
+    );
+    return unsub;
+  }, [subscribe]);
+
+  const handleEnroll = async () => {
+    if (!newName.trim()) return;
+    setEnrolling(true);
+    setError(null);
+    try {
+      const result = await callService(
+        '/social/enrollment/enroll_person',
+        'saltybot_social_msgs/srv/EnrollPerson',
+        { name: newName.trim(), mode: 'capture', n_samples: newSamples }
+      );
+      if (result?.success) {
+        setNewName('');
+        await loadPersons();
+      } else {
+        setError(result?.message ?? 'Enrollment failed');
+      }
+    } catch (e) {
+      setError('Enroll error: ' + (e?.message ?? e));
+    } finally {
+      setEnrolling(false);
+    }
+  };
+
+  const handleDelete = async (personId) => {
+    try {
+      await callService(
+        '/social/enrollment/delete_person',
+        'saltybot_social_msgs/srv/DeletePerson',
+        { person_id: personId }
+      );
+      await loadPersons();
+    } catch (e) {
+      setError('Delete error: ' + (e?.message ?? e));
+    }
+  };
+
+  return (
+    <div className="space-y-4">
+      {/* Enroll new person */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest mb-3">ENROLL NEW PERSON</div>
+        <div className="flex flex-col sm:flex-row gap-2">
+          <input
+            type="text"
+            placeholder="Person name…"
+            value={newName}
+            onChange={(e) => setNewName(e.target.value)}
+            onKeyDown={(e) => e.key === 'Enter' && handleEnroll()}
+            className="flex-1 bg-gray-900 border border-gray-700 rounded px-3 py-1.5 text-sm text-gray-200 placeholder-gray-600 focus:outline-none focus:border-cyan-700"
+          />
+          <div className="flex items-center gap-2">
+            <span className="text-gray-500 text-xs shrink-0">Samples:</span>
+            <input
+              type="number"
+              min={1}
+              max={20}
+              value={newSamples}
+              onChange={(e) => setNewSamples(Number(e.target.value))}
+              className="w-16 bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-sm text-gray-200 focus:outline-none focus:border-cyan-700"
+            />
+          </div>
+          <button
+            onClick={handleEnroll}
+            disabled={enrolling || !newName.trim()}
+            className="px-4 py-1.5 rounded bg-cyan-950 hover:bg-cyan-900 border border-cyan-700 text-cyan-300 text-sm disabled:opacity-40 transition-colors"
+          >
+            {enrolling ? 'Capturing…' : 'Enroll'}
+          </button>
+        </div>
+        {enrolling && (
+          <div className="mt-2 text-amber-400 text-xs animate-pulse">
+            Capturing face samples — look at the camera…
+          </div>
+        )}
+      </div>
+
+      {/* Error */}
+      {error && (
+        <div className="bg-red-950 border border-red-800 rounded px-3 py-2 text-red-300 text-xs">
+          {error}
+        </div>
+      )}
+
+      {/* Gallery header */}
+      <div className="flex items-center justify-between">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest">
+          ENROLLED PERSONS ({persons.length})
+        </div>
+        <div className="flex items-center gap-2">
+          {liveDetections.length > 0 && (
+            <div className="flex items-center gap-1 text-green-400 text-xs">
+              <div className="w-1.5 h-1.5 rounded-full bg-green-400 animate-pulse" />
+              {liveDetections.length} detected
+            </div>
+          )}
+          <button
+            onClick={loadPersons}
+            disabled={loading}
+            className="px-2 py-1 rounded border border-gray-700 text-gray-400 hover:text-gray-200 text-xs disabled:opacity-40 transition-colors"
+          >
+            {loading ? '…' : 'Refresh'}
+          </button>
+        </div>
+      </div>
+
+      {/* Grid */}
+      {loading && persons.length === 0 ? (
+        <div className="text-gray-600 text-sm text-center py-8">Loading…</div>
+      ) : persons.length === 0 ? (
+        <div className="text-gray-600 text-sm text-center py-8 border border-dashed border-gray-800 rounded-lg">
+          No enrolled persons
+        </div>
+      ) : (
+        <div className="grid grid-cols-2 sm:grid-cols-3 lg:grid-cols-4 gap-3">
+          {persons.map((p) => (
+            <PersonCard
+              key={p.person_id}
+              person={p}
+              onDelete={handleDelete}
+              liveDetection={liveDetections.find(d => d.face_id === p.person_id)}
+            />
+          ))}
+        </div>
+      )}
+    </div>
+  );
+}

ui/social-bot/src/components/NavModeSelector.jsx

@@ -0,0 +1,176 @@
+/**
+ * NavModeSelector.jsx — Follow mode switcher.
+ *
+ * Publishes: /social/nav/mode (std_msgs/String)
+ * Subscribes: /social/nav/mode (std_msgs/String) — echoed back by social_nav_node
+ *             /social/nav/status (std_msgs/String) — freeform status string
+ */
+
+import { useEffect, useState } from 'react';
+
+const MODES = [
+  {
+    id: 'shadow',
+    label: 'SHADOW',
+    icon: '👤',
+    description: 'Follow directly behind at distance',
+    color: 'border-blue-700 text-blue-300 bg-blue-950',
+    activeColor: 'border-blue-400 text-blue-100 bg-blue-900 mode-active',
+  },
+  {
+    id: 'lead',
+    label: 'LEAD',
+    icon: '➡',
+    description: 'Robot moves ahead, person follows',
+    color: 'border-green-700 text-green-300 bg-green-950',
+    activeColor: 'border-green-400 text-green-100 bg-green-900 mode-active',
+  },
+  {
+    id: 'side',
+    label: 'SIDE',
+    icon: '↔',
+    description: 'Walk side-by-side',
+    color: 'border-purple-700 text-purple-300 bg-purple-950',
+    activeColor: 'border-purple-400 text-purple-100 bg-purple-900 mode-active',
+  },
+  {
+    id: 'orbit',
+    label: 'ORBIT',
+    icon: '⟳',
+    description: 'Circle around the tracked person',
+    color: 'border-amber-700 text-amber-300 bg-amber-950',
+    activeColor: 'border-amber-400 text-amber-100 bg-amber-900 mode-active',
+  },
+  {
+    id: 'loose',
+    label: 'LOOSE',
+    icon: '⬡',
+    description: 'Follow with generous spacing',
+    color: 'border-teal-700 text-teal-300 bg-teal-950',
+    activeColor: 'border-teal-400 text-teal-100 bg-teal-900 mode-active',
+  },
+  {
+    id: 'tight',
+    label: 'TIGHT',
+    icon: '⬟',
+    description: 'Follow closely, minimal gap',
+    color: 'border-red-700 text-red-300 bg-red-950',
+    activeColor: 'border-red-400 text-red-100 bg-red-900 mode-active',
+  },
+];
+
+const VOICE_COMMANDS = [
+  { mode: 'shadow', cmd: '"shadow" / "follow me"' },
+  { mode: 'lead',   cmd: '"lead me" / "go ahead"' },
+  { mode: 'side',   cmd: '"stay beside"' },
+  { mode: 'orbit',  cmd: '"orbit"' },
+  { mode: 'loose',  cmd: '"give me space"' },
+  { mode: 'tight',  cmd: '"stay close"' },
+];
+
+export function NavModeSelector({ subscribe, publish }) {
+  const [activeMode, setActiveMode] = useState(null);
+  const [navStatus, setNavStatus] = useState('');
+  const [sending, setSending] = useState(null);
+
+  // Subscribe to echoed mode topic
+  useEffect(() => {
+    const unsub = subscribe(
+      '/social/nav/mode',
+      'std_msgs/String',
+      (msg) => setActiveMode(msg.data)
+    );
+    return unsub;
+  }, [subscribe]);
+
+  // Subscribe to nav status
+  useEffect(() => {
+    const unsub = subscribe(
+      '/social/nav/status',
+      'std_msgs/String',
+      (msg) => setNavStatus(msg.data)
+    );
+    return unsub;
+  }, [subscribe]);
+
+  const handleMode = async (modeId) => {
+    setSending(modeId);
+    publish('/social/nav/mode', 'std_msgs/String', { data: modeId });
+    // Optimistic update; will be confirmed when echoed back
+    setActiveMode(modeId);
+    setTimeout(() => setSending(null), 800);
+  };
+
+  return (
+    <div className="space-y-4">
+      {/* Status */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest mb-2">NAV STATUS</div>
+        <div className="flex items-center gap-3">
+          {activeMode ? (
+            <>
+              <div className="w-2.5 h-2.5 rounded-full bg-green-400 animate-pulse" />
+              <span className="text-gray-300 text-sm">
+                Mode: <span className="text-cyan-300 font-bold uppercase">{activeMode}</span>
+              </span>
+            </>
+          ) : (
+            <>
+              <div className="w-2.5 h-2.5 rounded-full bg-gray-600" />
+              <span className="text-gray-600 text-sm">No mode received</span>
+            </>
+          )}
+          {navStatus && (
+            <span className="ml-auto text-gray-500 text-xs">{navStatus}</span>
+          )}
+        </div>
+      </div>
+
+      {/* Mode buttons */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest mb-3">FOLLOW MODE</div>
+        <div className="grid grid-cols-2 sm:grid-cols-3 gap-2">
+          {MODES.map((mode) => {
+            const isActive = activeMode === mode.id;
+            const isSending = sending === mode.id;
+            return (
+              <button
+                key={mode.id}
+                onClick={() => handleMode(mode.id)}
+                disabled={isSending}
+                title={mode.description}
+                className={`flex flex-col items-center gap-1 py-3 px-2 rounded-lg border font-bold text-sm transition-all duration-200 ${
+                  isActive ? mode.activeColor : mode.color
+                } hover:opacity-90 active:scale-95 disabled:cursor-wait`}
+              >
+                <span className="text-xl">{mode.icon}</span>
+                <span className="tracking-widest text-xs">{mode.label}</span>
+                {isSending && <span className="text-xs opacity-60">sending…</span>}
+              </button>
+            );
+          })}
+        </div>
+        <p className="text-gray-600 text-xs mt-3">
+          Tap to publish to <code>/social/nav/mode</code>
+        </p>
+      </div>
+
+      {/* Voice commands reference */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest mb-3">VOICE COMMANDS</div>
+        <div className="space-y-1.5">
+          {VOICE_COMMANDS.map(({ mode, cmd }) => (
+            <div key={mode} className="flex items-center gap-2 text-xs">
+              <span className="text-gray-500 uppercase w-14 shrink-0">{mode}</span>
+              <span className="text-gray-400 italic">{cmd}</span>
+            </div>
+          ))}
+        </div>
+        <p className="text-gray-600 text-xs mt-3">
+          Voice commands are parsed by <code>social_nav_node</code> from{' '}
+          <code>/social/speech/command</code>.
+        </p>
+      </div>
+    </div>
+  );
+}

ui/social-bot/src/components/PersonalityTuner.jsx

@@ -0,0 +1,228 @@
+/**
+ * PersonalityTuner.jsx — Personality dial controls.
+ *
+ * Reads current personality from /social/personality/state (PersonalityState).
+ * Writes via /personality_node/set_parameters (rcl_interfaces/srv/SetParameters).
+ *
+ * SOUL.md params controlled:
+ *   sass_level    (int, 0–10)
+ *   humor_level   (int, 0–10)
+ *   verbosity     (int, 0–10)  — new param, add to SOUL.md + personality_node
+ */
+
+import { useEffect, useState } from 'react';
+
+const PERSONALITY_DIALS = [
+  {
+    key: 'sass_level',
+    label: 'SASS',
+    param: { name: 'sass_level', type: 'integer' },
+    min: 0, max: 10,
+    leftLabel: 'Polite',
+    rightLabel: 'Maximum Sass',
+    color: 'accent-orange',
+    barColor: '#f97316',
+    description: '0 = pure politeness, 10 = maximum sass',
+  },
+  {
+    key: 'humor_level',
+    label: 'HUMOR',
+    param: { name: 'humor_level', type: 'integer' },
+    min: 0, max: 10,
+    leftLabel: 'Deadpan',
+    rightLabel: 'Comedian',
+    color: 'accent-cyan',
+    barColor: '#06b6d4',
+    description: '0 = deadpan/serious, 10 = comedian',
+  },
+  {
+    key: 'verbosity',
+    label: 'VERBOSITY',
+    param: { name: 'verbosity', type: 'integer' },
+    min: 0, max: 10,
+    leftLabel: 'Terse',
+    rightLabel: 'Verbose',
+    color: 'accent-purple',
+    barColor: '#a855f7',
+    description: '0 = brief responses, 10 = elaborate explanations',
+  },
+];
+
+function DialSlider({ dial, value, onChange }) {
+  const pct = ((value - dial.min) / (dial.max - dial.min)) * 100;
+
+  return (
+    <div className="space-y-1">
+      <div className="flex justify-between items-center">
+        <span className="text-xs font-bold text-gray-400 tracking-widest">{dial.label}</span>
+        <span
+          className="text-sm font-bold w-6 text-right"
+          style={{ color: dial.barColor }}
+        >
+          {value}
+        </span>
+      </div>
+      <div className="relative">
+        <input
+          type="range"
+          min={dial.min}
+          max={dial.max}
+          value={value}
+          onChange={(e) => onChange(Number(e.target.value))}
+          className={`w-full h-1.5 rounded appearance-none cursor-pointer ${dial.color}`}
+          style={{
+            background: `linear-gradient(to right, ${dial.barColor} ${pct}%, #1f2937 ${pct}%)`,
+          }}
+        />
+      </div>
+      <div className="flex justify-between text-xs text-gray-600">
+        <span>{dial.leftLabel}</span>
+        <span>{dial.rightLabel}</span>
+      </div>
+    </div>
+  );
+}
+
+export function PersonalityTuner({ subscribe, setParam }) {
+  const [values, setValues] = useState({ sass_level: 4, humor_level: 7, verbosity: 5 });
+  const [applied, setApplied] = useState(null); // last-applied snapshot
+  const [saving, setSaving] = useState(false);
+  const [saveResult, setSaveResult] = useState(null);
+  const [personaInfo, setPersonaInfo] = useState(null);
+
+  // Sync with live personality state
+  useEffect(() => {
+    const unsub = subscribe(
+      '/social/personality/state',
+      'saltybot_social_msgs/PersonalityState',
+      (msg) => {
+        setPersonaInfo({
+          name: msg.persona_name,
+          mood: msg.mood,
+          person: msg.person_id,
+          tier: msg.relationship_tier,
+          greeting: msg.greeting_text,
+        });
+      }
+    );
+    return unsub;
+  }, [subscribe]);
+
+  const isDirty = JSON.stringify(values) !== JSON.stringify(applied);
+
+  const handleApply = async () => {
+    setSaving(true);
+    setSaveResult(null);
+    try {
+      const params = PERSONALITY_DIALS.map((d) => ({
+        name: d.param.name,
+        type: d.param.type,
+        value: values[d.key],
+      }));
+      await setParam('personality_node', params);
+      setApplied({ ...values });
+      setSaveResult({ ok: true, msg: 'Parameters applied to personality_node' });
+    } catch (e) {
+      setSaveResult({ ok: false, msg: e?.message ?? 'Service call failed' });
+    } finally {
+      setSaving(false);
+      setTimeout(() => setSaveResult(null), 4000);
+    }
+  };
+
+  const handleReset = () => {
+    setValues({ sass_level: 4, humor_level: 7, verbosity: 5 });
+  };
+
+  return (
+    <div className="space-y-4">
+      {/* Current persona info */}
+      {personaInfo && (
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest mb-2">ACTIVE PERSONA</div>
+          <div className="grid grid-cols-2 gap-2 text-xs">
+            <div>
+              <span className="text-gray-600">Name: </span>
+              <span className="text-cyan-300 font-bold">{personaInfo.name || '—'}</span>
+            </div>
+            <div>
+              <span className="text-gray-600">Mood: </span>
+              <span className={`font-bold ${
+                personaInfo.mood === 'happy' ? 'text-green-400' :
+                personaInfo.mood === 'curious' ? 'text-blue-400' :
+                personaInfo.mood === 'annoyed' ? 'text-red-400' :
+                personaInfo.mood === 'playful' ? 'text-purple-400' :
+                'text-gray-400'
+              }`}>{personaInfo.mood || '—'}</span>
+            </div>
+            {personaInfo.person && (
+              <>
+                <div>
+                  <span className="text-gray-600">Talking to: </span>
+                  <span className="text-amber-400">{personaInfo.person}</span>
+                </div>
+                <div>
+                  <span className="text-gray-600">Tier: </span>
+                  <span className="text-gray-300">{personaInfo.tier || '—'}</span>
+                </div>
+              </>
+            )}
+            {personaInfo.greeting && (
+              <div className="col-span-2 mt-1 text-gray-400 italic border-t border-gray-800 pt-1">
+                &quot;{personaInfo.greeting}&quot;
+              </div>
+            )}
+          </div>
+        </div>
+      )}
+
+      {/* Personality sliders */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4 space-y-5">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest">PERSONALITY DIALS</div>
+
+        {PERSONALITY_DIALS.map((dial) => (
+          <DialSlider
+            key={dial.key}
+            dial={dial}
+            value={values[dial.key]}
+            onChange={(v) => setValues((prev) => ({ ...prev, [dial.key]: v }))}
+          />
+        ))}
+
+        {/* Info */}
+        <div className="text-gray-700 text-xs border-t border-gray-900 pt-3">
+          Changes call <code className="text-gray-500">/personality_node/set_parameters</code>.
+          Hot-reload to SOUL.md happens within reload_interval (default 5s).
+        </div>
+      </div>
+
+      {/* Action buttons */}
+      <div className="flex items-center gap-2">
+        <button
+          onClick={handleApply}
+          disabled={saving || !isDirty}
+          className="flex-1 py-2 rounded font-bold text-sm border border-cyan-700 bg-cyan-950 hover:bg-cyan-900 text-cyan-300 disabled:opacity-40 disabled:cursor-not-allowed transition-colors"
+        >
+          {saving ? 'Applying…' : isDirty ? 'Apply Changes' : 'Up to Date'}
+        </button>
+        <button
+          onClick={handleReset}
+          className="px-4 py-2 rounded text-sm border border-gray-700 text-gray-400 hover:text-gray-200 hover:border-gray-600 transition-colors"
+        >
+          Defaults
+        </button>
+      </div>
+
+      {/* Save result */}
+      {saveResult && (
+        <div className={`rounded px-3 py-2 text-xs ${
+          saveResult.ok
+            ? 'bg-green-950 border border-green-800 text-green-300'
+            : 'bg-red-950 border border-red-800 text-red-300'
+        }`}>
+          {saveResult.msg}
+        </div>
+      )}
+    </div>
+  );
+}

ui/social-bot/src/components/StatusPanel.jsx

@@ -0,0 +1,154 @@
+/**
+ * StatusPanel.jsx — Live pipeline status display.
+ *
+ * Subscribes to /social/orchestrator/state (std_msgs/String, JSON payload):
+ *   {
+ *     state: "idle"|"listening"|"thinking"|"speaking"|"throttled",
+ *     gpu_free_mb: number,
+ *     gpu_total_mb: number,
+ *     latency: {
+ *       wakeword_to_transcript: { mean_ms, p95_ms, n },
+ *       transcript_to_llm: { mean_ms, p95_ms, n },
+ *       llm_to_tts: { mean_ms, p95_ms, n },
+ *       end_to_end: { mean_ms, p95_ms, n }
+ *     },
+ *     persona_name: string,
+ *     active_person: string
+ *   }
+ */
+
+import { useEffect, useState } from 'react';
+
+const STATE_CONFIG = {
+  idle:      { label: 'IDLE',       color: 'text-gray-400',  bg: 'bg-gray-800',   border: 'border-gray-600', pulse: '' },
+  listening: { label: 'LISTENING',  color: 'text-blue-300',  bg: 'bg-blue-950',   border: 'border-blue-600', pulse: 'pulse-blue' },
+  thinking:  { label: 'THINKING',   color: 'text-amber-300', bg: 'bg-amber-950',  border: 'border-amber-600', pulse: 'pulse-amber' },
+  speaking:  { label: 'SPEAKING',   color: 'text-green-300', bg: 'bg-green-950',  border: 'border-green-600', pulse: 'pulse-green' },
+  throttled: { label: 'THROTTLED',  color: 'text-red-300',   bg: 'bg-red-950',    border: 'border-red-600',   pulse: 'pulse-amber' },
+};
+
+function LatencyRow({ label, stat }) {
+  if (!stat || stat.n === 0) return null;
+  const warn = stat.mean_ms > 500;
+  const crit = stat.mean_ms > 1500;
+  const cls = crit ? 'text-red-400' : warn ? 'text-amber-400' : 'text-cyan-400';
+  return (
+    <div className="flex justify-between items-center py-0.5">
+      <span className="text-gray-500 text-xs">{label}</span>
+      <div className="text-right">
+        <span className={`text-xs font-bold ${cls}`}>{Math.round(stat.mean_ms)}ms</span>
+        <span className="text-gray-600 text-xs ml-1">p95:{Math.round(stat.p95_ms)}ms</span>
+      </div>
+    </div>
+  );
+}
+
+export function StatusPanel({ subscribe }) {
+  const [status, setStatus] = useState(null);
+  const [lastUpdate, setLastUpdate] = useState(null);
+
+  useEffect(() => {
+    const unsub = subscribe(
+      '/social/orchestrator/state',
+      'std_msgs/String',
+      (msg) => {
+        try {
+          const data = JSON.parse(msg.data);
+          setStatus(data);
+          setLastUpdate(Date.now());
+        } catch {
+          // ignore parse errors
+        }
+      }
+    );
+    return unsub;
+  }, [subscribe]);
+
+  const state = status?.state ?? 'idle';
+  const cfg = STATE_CONFIG[state] ?? STATE_CONFIG.idle;
+  const gpuFree = status?.gpu_free_mb ?? 0;
+  const gpuTotal = status?.gpu_total_mb ?? 1;
+  const gpuUsed = gpuTotal - gpuFree;
+  const gpuPct = Math.round((gpuUsed / gpuTotal) * 100);
+  const gpuWarn = gpuPct > 80;
+  const lat = status?.latency ?? {};
+
+  const stale = lastUpdate && Date.now() - lastUpdate > 5000;
+
+  return (
+    <div className="space-y-4">
+      {/* Pipeline State */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest mb-3">PIPELINE STATE</div>
+        <div className="flex items-center gap-4">
+          <div
+            className={`w-5 h-5 rounded-full shrink-0 ${cfg.bg} border-2 ${cfg.border} ${cfg.pulse}`}
+          />
+          <div>
+            <div className={`text-2xl font-bold tracking-widest ${cfg.color}`}>{cfg.label}</div>
+            {status?.persona_name && (
+              <div className="text-gray-500 text-xs mt-0.5">
+                Persona: <span className="text-cyan-500">{status.persona_name}</span>
+              </div>
+            )}
+            {status?.active_person && (
+              <div className="text-gray-500 text-xs">
+                Talking to: <span className="text-amber-400">{status.active_person}</span>
+              </div>
+            )}
+          </div>
+          {stale && (
+            <div className="ml-auto text-red-500 text-xs">STALE</div>
+          )}
+          {!status && (
+            <div className="ml-auto text-gray-600 text-xs">No signal</div>
+          )}
+        </div>
+      </div>
+
+      {/* GPU Memory */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest mb-3">GPU MEMORY</div>
+        {gpuTotal > 1 ? (
+          <>
+            <div className="flex justify-between text-xs mb-1.5">
+              <span className={gpuWarn ? 'text-red-400' : 'text-gray-400'}>
+                {Math.round(gpuUsed)} MB used
+              </span>
+              <span className="text-gray-500">{Math.round(gpuTotal)} MB total</span>
+            </div>
+            <div className="w-full h-3 bg-gray-900 rounded overflow-hidden border border-gray-800">
+              <div
+                className="h-full transition-all duration-500 rounded"
+                style={{
+                  width: `${gpuPct}%`,
+                  background: gpuPct > 90 ? '#ef4444' : gpuPct > 75 ? '#f59e0b' : '#06b6d4',
+                }}
+              />
+            </div>
+            <div className={`text-xs mt-1 text-right ${gpuWarn ? 'text-amber-400' : 'text-gray-600'}`}>
+              {gpuPct}%
+            </div>
+          </>
+        ) : (
+          <div className="text-gray-600 text-xs">No GPU data</div>
+        )}
+      </div>
+
+      {/* Latency */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest mb-3">LATENCY</div>
+        {Object.keys(lat).length > 0 ? (
+          <div className="divide-y divide-gray-900">
+            <LatencyRow label="Wake → Transcript" stat={lat.wakeword_to_transcript} />
+            <LatencyRow label="Transcript → LLM"  stat={lat.transcript_to_llm} />
+            <LatencyRow label="LLM → TTS"         stat={lat.llm_to_tts} />
+            <LatencyRow label="End-to-End"         stat={lat.end_to_end} />
+          </div>
+        ) : (
+          <div className="text-gray-600 text-xs">No latency data yet</div>
+        )}
+      </div>
+    </div>
+  );
+}

ui/social-bot/src/hooks/useRosbridge.js

@@ -0,0 +1,116 @@
+/**
+ * useRosbridge.js — React hook for ROS2 rosbridge_server WebSocket connection.
+ *
+ * rosbridge_server default: ws://<robot-ip>:9090
+ * Provides subscribe/publish/callService helpers bound to the active connection.
+ */
+
+import { useState, useEffect, useRef, useCallback } from 'react';
+import ROSLIB from 'roslib';
+
+export function useRosbridge(url) {
+  const [connected, setConnected] = useState(false);
+  const [error, setError] = useState(null);
+  const rosRef = useRef(null);
+  const subscribersRef = useRef(new Map()); // topic -> ROSLIB.Topic
+
+  useEffect(() => {
+    if (!url) return;
+
+    const ros = new ROSLIB.Ros({ url });
+    rosRef.current = ros;
+
+    ros.on('connection', () => {
+      setConnected(true);
+      setError(null);
+    });
+
+    ros.on('error', (err) => {
+      setError(err?.toString() || 'Connection error');
+    });
+
+    ros.on('close', () => {
+      setConnected(false);
+    });
+
+    return () => {
+      subscribersRef.current.forEach((topic) => topic.unsubscribe());
+      subscribersRef.current.clear();
+      ros.close();
+      rosRef.current = null;
+    };
+  }, [url]);
+
+  /** Subscribe to a ROS2 topic. Returns an unsubscribe function. */
+  const subscribe = useCallback((name, messageType, callback) => {
+    if (!rosRef.current) return () => {};
+
+    const key = `${name}::${messageType}`;
+    if (subscribersRef.current.has(key)) {
+      subscribersRef.current.get(key).unsubscribe();
+    }
+
+    const topic = new ROSLIB.Topic({
+      ros: rosRef.current,
+      name,
+      messageType,
+    });
+    topic.subscribe(callback);
+    subscribersRef.current.set(key, topic);
+
+    return () => {
+      topic.unsubscribe();
+      subscribersRef.current.delete(key);
+    };
+  }, []);
+
+  /** Publish a single message to a ROS2 topic. */
+  const publish = useCallback((name, messageType, data) => {
+    if (!rosRef.current) return;
+    const topic = new ROSLIB.Topic({
+      ros: rosRef.current,
+      name,
+      messageType,
+    });
+    topic.publish(new ROSLIB.Message(data));
+  }, []);
+
+  /** Call a ROS2 service. Returns a Promise resolving to the response. */
+  const callService = useCallback((name, serviceType, request = {}) => {
+    return new Promise((resolve, reject) => {
+      if (!rosRef.current) {
+        reject(new Error('Not connected'));
+        return;
+      }
+      const svc = new ROSLIB.Service({
+        ros: rosRef.current,
+        name,
+        serviceType,
+      });
+      svc.callService(new ROSLIB.ServiceRequest(request), resolve, reject);
+    });
+  }, []);
+
+  /** Set ROS2 node parameters via rcl_interfaces/srv/SetParameters. */
+  const setParam = useCallback((nodeName, params) => {
+    // params: { name: string, type: 'bool'|'int'|'double'|'string', value: any }[]
+    const TYPE_MAP = { bool: 1, integer: 2, double: 3, string: 4, int: 2 };
+    const parameters = params.map(({ name, type, value }) => {
+      const typeInt = TYPE_MAP[type] ?? 4;
+      const valueKey = {
+        1: 'bool_value',
+        2: 'integer_value',
+        3: 'double_value',
+        4: 'string_value',
+      }[typeInt];
+      return { name, value: { type: typeInt, [valueKey]: value } };
+    });
+    return callService(
+      `/${nodeName}/set_parameters`,
+      'rcl_interfaces/srv/SetParameters',
+      { parameters }
+    );
+  }, [callService]);
+
+  return { connected, error, subscribe, publish, callService, setParam };
+}

ui/social-bot/src/index.css

@@ -0,0 +1,73 @@
+@tailwind base;
+@tailwind components;
+@tailwind utilities;
+
+* {
+  box-sizing: border-box;
+}
+
+body {
+  font-family: 'Courier New', Courier, monospace;
+  background: #050510;
+  color: #d1d5db;
+  margin: 0;
+  padding: 0;
+  min-height: 100vh;
+}
+
+/* Custom scrollbar */
+::-webkit-scrollbar {
+  width: 4px;
+  height: 4px;
+}
+::-webkit-scrollbar-track {
+  background: #020208;
+}
+::-webkit-scrollbar-thumb {
+  background: #1a3a4a;
+  border-radius: 2px;
+}
+::-webkit-scrollbar-thumb:hover {
+  background: #00b8d9;
+}
+
+/* Pulse animations for pipeline state */
+@keyframes pulse-blue {
+  0%, 100% { box-shadow: 0 0 0 0 rgba(59, 130, 246, 0.6); }
+  50% { box-shadow: 0 0 0 8px rgba(59, 130, 246, 0); }
+}
+@keyframes pulse-amber {
+  0%, 100% { box-shadow: 0 0 0 0 rgba(245, 158, 11, 0.6); }
+  50% { box-shadow: 0 0 0 8px rgba(245, 158, 11, 0); }
+}
+@keyframes pulse-green {
+  0%, 100% { box-shadow: 0 0 0 0 rgba(34, 197, 94, 0.6); }
+  50% { box-shadow: 0 0 0 8px rgba(34, 197, 94, 0); }
+}
+
+.pulse-blue { animation: pulse-blue 1.2s infinite; }
+.pulse-amber { animation: pulse-amber 0.8s infinite; }
+.pulse-green { animation: pulse-green 0.6s infinite; }
+
+/* Conversation bubbles */
+.bubble-human {
+  background: #1e3a5f;
+  border: 1px solid #2d6a9f;
+}
+.bubble-bot {
+  background: #0d3030;
+  border: 1px solid #0d9488;
+}
+.bubble-partial {
+  opacity: 0.7;
+}
+
+/* Slider accent colors */
+input[type='range'].accent-cyan { accent-color: #00b8d9; }
+input[type='range'].accent-orange { accent-color: #f97316; }
+input[type='range'].accent-purple { accent-color: #a855f7; }
+
+/* Mode button active glow */
+.mode-active {
+  box-shadow: 0 0 10px rgba(0, 184, 217, 0.4);
+}

ui/social-bot/src/main.jsx

@@ -0,0 +1,10 @@
+import React from 'react';
+import ReactDOM from 'react-dom/client';
+import App from './App.jsx';
+import './index.css';
+
+ReactDOM.createRoot(document.getElementById('root')).render(
+  <React.StrictMode>
+    <App />
+  </React.StrictMode>
+);

ui/social-bot/tailwind.config.js

@@ -0,0 +1,12 @@
+/** @type {import('tailwindcss').Config} */
+export default {
+  content: ['./index.html', './src/**/*.{js,jsx}'],
+  theme: {
+    extend: {
+      fontFamily: {
+        mono: ['"Courier New"', 'Courier', 'monospace'],
+      },
+    },
+  },
+  plugins: [],
+};

ui/social-bot/vite.config.js

@@ -0,0 +1,14 @@
+import { defineConfig } from 'vite';
+import react from '@vitejs/plugin-react';
+
+export default defineConfig({
+  plugins: [react()],
+  server: {
+    port: 8080,
+    host: true,
+  },
+  preview: {
+    port: 8080,
+    host: true,
+  },
+});