feat(bridge): battery management node (Issue #125)

Add battery_node.py:
- Subscribes /saltybot/telemetry/battery (JSON from stm32_cmd_node)
- Publishes sensor_msgs/BatteryState on /saltybot/battery at 1 Hz
- SoC source priority: STM32 fuel gauge soc_pct field → fallback to
  3S LiPo voltage curve (12-point lookup with linear interpolation)
- Charging detection: current_ma < -100 mA threshold
- Alert levels: WARNING (20%)→speed 60%, CRITICAL (10%)→speed 30%,
  EMERGENCY (5%)→zero /cmd_vel + /saltybot/arm(False) disarm
- /saltybot/battery/alert JSON topic on threshold crossings
- /saltybot/speed_limit Float32 (0.0-1.0) for nav speed capping
- SQLite history logging: /var/log/saltybot/battery.db, 7-day retention
- Hourly prune timer to keep DB bounded

Add test_battery.py (70+ tests, no ROS2 runtime):
- SoC lookup: all curve points, interpolation, clamping, 3S/4S packs
- Alert level thresholds and transitions for all levels
- Speed factor assignments per alert level
- Charging detection logic
- sensor_msgs/BatteryState field population
- SQLite insert/retrieve/prune (in-memory and on-disk)
- JSON telemetry parsing: normal, charging, soc_pct=0 fallback

Add battery_params.yaml, battery.launch.py.

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

chassis/saltytank_BOM.md

@@ -0,0 +1,383 @@
+# SaltyTank Chassis — BOM & Assembly Notes
+**Issue: #121  Agent: sl-mechanical  Date: 2026-03-01**
+
+---
+
+## Overview
+
+SaltyTank is the tracked variant of the SaltyLab robot family.
+Rubber or metal continuous tracks replace wheels for rough-terrain capability.
+The electronics bay, RPLIDAR, D435i, stem, and sensor head are **shared with
+SaltyLab and SaltyRover** — no modifications required.
+
+```
+Side view (schematic):
+
+    ←  +Y forward
+                              Sensor head + RPLIDAR
+                                      │
+ ┌──────────────────────────── deck (Z=0) ───────────────────────────┐
+ │              [Electronics Bay]     [Stem collar]                  │
+ └──────────────────────────────────────────────────────────────────┘
+ │ [Side frame - 90 mm tall]                                         │
+ │   ◎ idler (Ø80)       ◎ road wheel  ◎ road wheel   ◎ drive (Ø66)│
+ │───────────────────── track belt (rubber, 80 mm wide) ─────────────│
+ (ground)
+
+Top view (schematic):
+            +Y (forward)
+                 │
+[track]  ┌──────┴───────┐  [track]
+         │  [Electronics │
+         │    Bay +      │
+         │   RPLIDAR]    │
+[track]  └──────┬───────┘  [track]
+                │
+            D435i →
+
+Key dimensions:
+  Deck: 500 mm (L) × 360 mm (W)
+  Track belt centres: 360 + 6 + 80 = 446 mm left-right CL to CL
+  Overall robot width (outer track edge): 446 + 80 = 526 mm
+  Ground clearance (hull between tracks): 90 mm  ← exceeds 50 mm req.
+  Height: deck to RPLIDAR scan plane: ~330 mm (with 550 mm stem option)
+```
+
+---
+
+## File Index
+
+| File | Description | RENDER → Output |
+|------|-------------|-----------------|
+| `saltytank_chassis.scad` | Deck plate + side frames + idler block + CSI/D435i mounts | See table below |
+| `saltytank_skid_plate.scad` | Underside skid plate (bolt-on, sacrificial) | `skid_2d` → DXF; `skid_stl` → STL |
+| `rover_electronics_bay.scad` | Electronics bay body + lid + RPLIDAR tower | **Unchanged** — reuse rover part |
+| `rover_motor_mount.scad` | (Not used — drive sprockets mount directly to side frame) | — |
+| `rover_stem_adapter.scad` | Stem adapter (shared) | Unchanged |
+| `rplidar_mount.scad` | RPLIDAR anti-vibration ring (shared) | Unchanged |
+
+### RENDER map
+
+| RENDER value | Output | Qty | Process |
+|---|---|---|---|
+| `"deck_2d"` | `saltytank_deck.dxf` | 1 | Waterjet / CNC, 8 mm Al |
+| `"side_frame_2d"` | `saltytank_side_frame.dxf` | 2 | CNC / laser, 6 mm Al (cut 2×, flip 1) |
+| `"side_frame_stl"` | `saltytank_side_frame.stl` | 2 | PETG print (prototype) |
+| `"idler_block_stl"` | `saltytank_idler_block.stl` | 2 | PETG print |
+| `"csi_mount_stl"` | `saltytank_csi_mount.stl` | 4 | PETG print |
+| `"d435i_mount_stl"` | `saltytank_d435i_mount.stl` | 1 | PETG print |
+| `"skid_2d"` | `saltytank_skid.dxf` | 1 | Waterjet / CNC, 4 mm HDPE |
+| `"skid_stl"` | `saltytank_skid.stl` | 1 | PETG print (prototype) |
+
+### Export commands
+
+```bash
+# Deck plate DXF:
+openscad saltytank_chassis.scad    -D 'RENDER="deck_2d"'         -o saltytank_deck.dxf
+
+# Side frame DXF (cut 2×, flip one):
+openscad saltytank_chassis.scad    -D 'RENDER="side_frame_2d"'   -o saltytank_side_frame.dxf
+
+# Side frame STL (print 2×, mirror right in slicer):
+openscad saltytank_chassis.scad    -D 'RENDER="side_frame_stl"'  -o saltytank_side_frame.stl
+
+# Idler tensioner block STL (×2):
+openscad saltytank_chassis.scad    -D 'RENDER="idler_block_stl"' -o saltytank_idler_block.stl
+
+# CSI bracket STL (×4):
+openscad saltytank_chassis.scad    -D 'RENDER="csi_mount_stl"'   -o saltytank_csi_mount.stl
+
+# D435i bracket STL (×1):
+openscad saltytank_chassis.scad    -D 'RENDER="d435i_mount_stl"' -o saltytank_d435i_mount.stl
+
+# Skid plate DXF (4 mm HDPE, preferred):
+openscad saltytank_skid_plate.scad -D 'RENDER="skid_2d"'         -o saltytank_skid.dxf
+
+# Skid plate STL (prototype):
+openscad saltytank_skid_plate.scad -D 'RENDER="skid_stl"'        -o saltytank_skid.stl
+```
+
+---
+
+## Part A — Deck Plate
+
+| # | Spec | Qty | Material | Process | Notes |
+|---|------|-----|----------|---------|-------|
+| A1 | 8 mm 5052-H32 Al, 500×360 mm blank | 1 | Aluminium | Waterjet | Preferred |
+| A1-alt | 6 mm 6061-T6 Al | 1 | — | CNC router | Saves 0.3 kg |
+| A1-proto | 8 mm PETG FDM, 2 halves lap-jointed | 1 | — | Print | Prototype only |
+
+**Deck fasteners:**
+
+| # | Spec | Qty | Use |
+|---|------|-----|-----|
+| A-f1 | M5×20 SHCS | 8 | Side frame to deck side edge (4 per side) |
+| A-f2 | M5 T-nut or rivet nut | 8 | Captured in deck edge (installed before frame) |
+| A-f3 | M4×16 FHCS | 4 | Stem collar to deck (4× M4 countersunk) |
+| A-f4 | M3×12 SHCS | 10 | Electronics bay to deck |
+| A-f5 | M4×12 FHCS | 6 | Skid plate to deck (countersunk from below) |
+| A-f6 | M4 rivet-nut | 6 | Installed in deck for skid plate bolts |
+
+---
+
+## Part B — Side Frames (×2)
+
+| # | RENDER | Qty | Material | Process | Notes |
+|---|--------|-----|----------|---------|-------|
+| B1 | `side_frame_2d` | 2 | 6 mm 6061-T6 Al | CNC router or waterjet | Cut 2× same DXF; flip 1 (mirror) |
+| B1-alt | `side_frame_stl` | 2 | PETG | Print | 5 perims, 60% gyroid; prototype only |
+
+> ⚠ **Hub motor flange bolt circle:** Default is 52 mm BC × 4× M5.
+> **Measure your motor hub** before cutting the side frame — adjust `HUB_FLANGE_BC`
+> in `saltytank_chassis.scad` if needed.
+
+**Side frame fasteners:**
+
+| # | Spec | Qty | Use |
+|---|------|-----|-----|
+| B-f1 | M5×20 SHCS | 8 | Frame-to-deck attachment (4 per frame, through slotted holes) |
+| B-f2 | M5 nyloc | 8 | Under deck |
+| B-f3 | M5×16 SHCS | 8 | Hub motor flange to side frame (2× M5 per motor hub face) |
+| B-f4 | M8×60 hex bolt | 2 | Road wheel axle bolts (1 per wheel × 2 per side) |
+| B-f5 | M8 nyloc | 4 | Road wheel axle retention |
+| B-f6 | M8 flat washer | 8 | Both sides of each road wheel |
+
+---
+
+## Part C — Drive Sprockets + Hub Motors
+
+SaltyTank reuses the same hoverboard hub motors as SaltyLab/SaltyRover.
+A laser-cut sprocket plate bolts to the motor hub and engages the track belt.
+
+| # | Part | Qty | Spec | Notes |
+|---|------|-----|------|-------|
+| C1 | Hub motor | 2 | 10×2.125" tire, 36 V, ~350 W; axle OD 16.11 mm (caliper) | **Only 2 motors — rear drive** |
+| C2 | Drive sprocket plate | 2 | 5 mm 6061-T6 Al, laser-cut | 10-tooth, 20 mm pitch, PCD ≈ 64.7 mm; Ø52 mm hub bolt holes (verify) |
+| C3 | Motor phase cable | 2 | 12 AWG, 300 mm, XT30 | Left + right rear |
+| C4 | Hall sensor cable | 2 | 6-pin JST-PH, 300 mm | — |
+
+> **Sprocket plate design** (separate file to be made if needed):
+> 10 teeth, pitch = 20 mm → PCD = 20 / sin(18°) ≈ 64.7 mm → R ≈ 32.4 mm.
+> Hub bolt circle = 52 mm, 4× M5 at 90°.  Verify against actual motor hub.
+> Sprocket tooth profile: ISO/DIN standard for roller chain or custom for belt track.
+
+**Motor fasteners:**
+
+| # | Spec | Qty | Use |
+|---|------|-----|-----|
+| C-f1 | M5×16 SHCS | 8 | Sprocket plate to motor hub flange (4 per sprocket) |
+| C-f2 | Hub motor axle nut | 2 | M16×1.5 (verify axle thread); torque 35–40 N·m |
+| C-f3 | Loctite 243 | — | On axle nut and flange bolts |
+
+---
+
+## Part D — Idler Wheels + Tensioners
+
+| # | RENDER | Qty | Material | Notes |
+|---|--------|-----|----------|-------|
+| D1 | `idler_block_stl` | 2 | PETG | Tensioner sliding block, 1 per side |
+
+| # | Part | Qty | Spec | Notes |
+|---|------|-----|------|-------|
+| D2 | Idler wheel | 2 | Ø80 mm, 80+ mm wide, M8 bore, flanged | Off-shelf: e.g. polyurethane track idler |
+| D3 | M8×100 SHCS | 2 | Stainless | Idler axle through block + wheel + outer bearing |
+| D4 | M8 nyloc nut | 2 | — | Axle retention |
+| D5 | M8 flat washer | 4 | — | Both sides of idler wheel |
+| D6 | M6×40 SHCS | 2 | — | Tensioner adjustment bolt (threads into idler block lug) |
+| D7 | M6 nyloc nut | 2 | — | Captured in block; lock bolt position |
+
+**Track tensioning procedure:**
+1. Loosen idler axle nut (M8) — block free to slide in slot.
+2. Turn tensioner M6 bolt CW to push block rearward → tighten track.
+3. Target: ~10 mm slack on upper track run (finger-press deflects 10 mm).
+4. Tighten M8 axle nut to 12 N·m.  Check tension after first 5-minute run.
+
+---
+
+## Part E — Road Wheels (2 per side, 4 total)
+
+| # | Part | Qty | Spec | Notes |
+|---|------|-----|------|-------|
+| E1 | Road wheel | 4 | Ø60 mm × 80+ mm wide, M8 bore | Polyurethane or HDPE; must fit inside track width |
+| E2 | M8×80 SHCS | 4 | Stainless | Road wheel axle through side frame |
+| E3 | M8 nyloc | 4 | — | — |
+| E4 | M8 flat washer | 8 | — | — |
+
+---
+
+## Part F — Track Belts
+
+| # | Part | Qty | Spec | Notes |
+|---|------|-----|------|-------|
+| F1 | Rubber track belt | 2 | 80 mm wide, 20 mm pitch, ~1040 mm circumference | Circumference: 2 × (sprocket–idler CL) + π × (sprocket_D + idler_D)/2 ≈ 2 × 440 + π × 72 ≈ 1106 mm; order next standard size up |
+| F1-alt | Metal link track | 2 | 80 mm wide, 20 mm pitch, adjustable length | Higher durability; heavier (~0.8 kg/belt); louder |
+
+> **Track circumference calculation:**
+> `C = 2 × span + π × (D_sprocket + D_idler) / 2`
+> `  = 2 × 440 + π × (66 + 80) / 2`
+> `  = 880 + π × 73`
+> `  = 880 + 229 ≈ 1109 mm`
+> Order **1120 mm** (next standard rubber track pitch × N links).
+
+---
+
+## Part G — Skid Plate
+
+| # | RENDER | Qty | Material | Process | Notes |
+|---|--------|-----|----------|---------|-------|
+| G1 | `skid_2d` | 1 | 4 mm HDPE (white) | Waterjet / CNC | Preferred — lightweight, low friction |
+| G1-alt | `skid_2d` | 1 | 2 mm 304 stainless | Laser-cut | Heavy but extreme durability |
+| G1-proto | `skid_stl` | 1 | PETG | Print | Prototype — 2 halves if bed <500 mm |
+
+| # | Spec | Qty | Use |
+|---|------|-----|-----|
+| G-f1 | M4×12 FHCS | 6 | Skid plate to deck (countersunk flush) |
+| G-f2 | M4 rivet-nut (short) | 6 | Pre-installed in deck plate |
+
+---
+
+## Part H — Sensor Brackets
+
+| # | RENDER | Qty | Material | Notes |
+|---|--------|-----|----------|-------|
+| H1 | `csi_mount_stl` | 4 | PETG | CSI corner brackets (45° outward, 20° down) |
+| H2 | `d435i_mount_stl` | 1 | PETG | D435i front arm (8° nose-down) |
+
+| # | Spec | Qty | Use |
+|---|------|-----|-----|
+| H-f1 | M2×6 SHCS | 8 | CSI camera PCB to bracket |
+| H-f2 | M3×8 SHCS | 8 | CSI bracket to deck |
+| H-f3 | M4×14 SHCS | 2 | D435i bracket to deck front face |
+| H-f4 | 1/4-20 UNC hex nut | 1 | Captured in D435i bracket face |
+| H-c1 | 200 mm CSI FPC cable | 4 | IMX219 to Jetson |
+
+---
+
+## Part I — Electronics Bay (reuse from rover)
+
+The `rover_electronics_bay.scad` bay fits the SaltyTank deck directly — the
+deck bolt pattern is identical.  Print or reuse existing bay + lid.
+
+| # | Part | Qty | Notes |
+|---|------|-----|-------|
+| I1 | Electronics bay body | 1 | `rover_electronics_bay.scad` → `bay_stl` |
+| I2 | Electronics bay lid | 1 | Includes RPLIDAR tower |
+| I3 | M3×12 SHCS | 10 | Bay to deck |
+| I4 | M3×8 BHCS | 4 | Lid to bay |
+
+---
+
+## Part J — Stem + Sensor Head (shared, unchanged)
+
+Same stem adapter, RPLIDAR, D435i, and IMX219 sensor head as SaltyLab.
+Recommended stem length for SaltyTank: **500 mm** (lower than SaltyBot mast for
+stability; RPLIDAR at ~635 mm from ground with 98 mm deck height).
+
+---
+
+## Mass Estimate — Frame Only
+
+| Assembly | Material | Est. mass |
+|----------|----------|-----------|
+| Deck plate (8 mm Al, ~45% lightened) | Al | ~1.55 kg |
+| Side frames ×2 (6 mm Al, ~35% lightened) | Al | ~0.52 kg |
+| Skid plate (4 mm HDPE, solid) | HDPE | ~0.56 kg |
+| Idler tensioner blocks ×2 | PETG | ~0.06 kg |
+| CSI brackets ×4 + D435i bracket | PETG | ~0.07 kg |
+| Stem collar | PETG | ~0.04 kg |
+| Fasteners (M4–M8 SS) | Steel | ~0.18 kg |
+| **Frame total** | | **~2.98 kg** ✓ |
+
+> **Just under the 3 kg target.**
+> To save weight: switch side frames from 6 mm to 5 mm Al → saves ~0.09 kg.
+> Set `FRAME_T = 5.0` in `saltytank_chassis.scad` and re-export DXF.
+
+---
+
+## Assembly Sequence
+
+### 1. Fabricate / source parts
+1. Export DXFs; send deck and side frames to waterjet.  Order HDPE skid to DXF.
+2. Print idler blocks, CSI/D435i brackets, stem collar.
+3. Source track belts, idler wheels, road wheels, hub motors.
+4. Install M4 rivet-nuts in deck plate (6× for skid; stem flange positions).
+
+### 2. Side frame preparation
+1. Test-fit hub motor axle D-cut bore in side frame — adjust `SPROCKET_AXLE_D`
+   and `SPROCKET_AXLE_FLAT` if caliper measurement differs.
+2. Press or Loctite M8 flange nuts into road wheel axle positions (optional).
+
+### 3. Frame assembly
+1. Slide side frames into deck side edge slots (inner face flush with deck edge).
+2. Insert M5×20 SHCS from above through deck slots; fit nyloc nuts under deck.
+3. **Snug only** — leave adjustable for wheel alignment (step 6).
+
+### 4. Drive motor installation
+1. Feed hub motor axle through rear bore in side frame (D-cut aligned to flat).
+2. Bolt sprocket plate to motor hub: 4× M5×16 SHCS, 2.5 N·m, Loctite 243.
+3. Fit axle lock nut; Loctite 243; torque 35 N·m.
+4. Route phase + hall cables through deck cable slot.
+
+### 5. Road wheel installation
+1. Slide M8×80 bolt through side frame road wheel bore.
+2. Fit road wheel on axle (M8 flat washers both sides).
+3. Thread M8 nyloc; torque to 12 N·m.
+4. Verify wheel rotates freely.
+
+### 6. Track belt installation (one side at a time)
+1. Install idler block in tensioner slot (tensioner bolt only finger-tight).
+2. Thread track belt around sprocket → road wheels → idler.
+   *(Easier with motor temporarily removed — reinstall after threading.)*
+3. Fit idler axle (M8×100) through idler block + idler wheel; nyloc finger-tight.
+4. Apply tension: tighten M6 tensioner bolt until ~10 mm slack on upper run.
+5. Torque idler axle nut to 12 N·m.
+
+### 7. Geometry verification
+1. Set robot on flat surface; check all 4 road wheels contact ground through track.
+2. Measure track tension on both sides — should match within 2 mm.
+3. Spin motors briefly — verify tracks run straight without walking.
+4. Torque side-frame-to-deck M5 bolts to 4 N·m once alignment confirmed.
+
+### 8. Skid plate
+1. Slide skid plate under deck; align M4 countersunk holes with rivet-nuts.
+2. Fasten 6× M4×12 FHCS from below; tighten evenly to 2.5 N·m.
+
+### 9. Electronics, sensors, stem
+1. Mount electronics bay on deck (10× M3×12); route cables.
+2. Install CSI and D435i brackets on deck corners/front.
+3. Press stem through collar; install stem adapter + clamp.
+4. Mount sensor head + RPLIDAR on stem top.
+
+---
+
+## Critical Dimensions
+
+| Dimension | Nominal | Tolerance |
+|-----------|---------|-----------|
+| Deck length × width | 500 × 360 mm | ±1 mm |
+| Frame height | 90 mm | ±0.5 mm |
+| Frame length | 500 mm | ±0.5 mm |
+| Drive axle bore OD | 16.11 mm | +0.4/0 (round section) |
+| Drive axle flat chord | 13.00 mm | +0.4/0 |
+| Bearing seat recess OD | 37.80 mm | +1.5/0 |
+| Hub motor flange BC | 52 mm | **⚠ verify caliper** |
+| Tensioner slot height | 10.5 mm | ±0.2 mm |
+| Tensioner travel | ±15 mm | — |
+| FC hole pattern | 30.5×30.5 mm | ±0.2 mm |
+| Jetson hole pattern | 58×49 mm | ±0.2 mm |
+| Stem bore | Ø25.5 mm | +0.3/0 |
+| Track belt width | 80 mm | ±1 mm |
+| Sprocket PCD | 64.7 mm | ±0.5 mm |
+| Idler OD | 80 mm | ±1 mm |
+| Road wheel OD | 60 mm | ±1 mm |
+
+---
+
+## OpenSCAD Version
+
+Requires OpenSCAD **2021.01 or newer**.
+
+```bash
+# Full assembly preview:
+openscad saltytank_chassis.scad &
+```

chassis/saltytank_chassis.scad

@@ -0,0 +1,672 @@
+// ============================================================
+// saltytank_chassis.scad — SaltyTank Tracked Chassis
+// Issue: #121  Agent: sl-mechanical  Date: 2026-03-01
+// ============================================================
+//
+// Parametric tank chassis for rubber or metal continuous tracks.
+//
+// Structure (left-to-right cross-section):
+//   [left track belt]
+//     ← sprocket + idler + road wheels on outer face of side frame
+//   [left side frame plate, 8 mm Al]
+//   [deck plate, 8 mm Al]          ← electronics bay on top
+//   [right side frame plate, 8 mm Al]
+//     → sprocket + idler + road wheels
+//   [right track belt]
+//
+// Drive:   hoverboard hub motors (rear) — caliper-verified axle
+//          (16.11 mm OD, D-cut flat 13.00 mm, bearing seat Ø37.8 mm)
+// Idler:   80 mm OD, M8 axle; tensioner slot ±15 mm fore-aft
+// Road wh: 60 mm OD × 2 per side, M8 axle, fixed
+// Tracks:  rubber belt, 80 mm wide, 20 mm pitch (parametric)
+//
+// Electronics bay: reuses rover_electronics_bay.scad (same deck
+//   footprint, FC 30.5 × 30.5 mm M3 + Jetson 58 × 49 mm M3)
+// Sensors: RPLIDAR A1M8 top (bay lid tower), D435i front,
+//          4 × IMX219 / CSI at deck corners
+// Stem:    Ø25 mm (shared with SaltyLab / SaltyRover)
+//
+// Coordinate convention:
+//   Z = 0    deck top face
+//   +Y       forward
+//   +X       right
+//   Ground   Z = -(DECK_T + FRAME_H)   [= −98 mm with defaults]
+//
+// Ground clearance of hull (between tracks): FRAME_H = 90 mm ✓
+//   (exceeds 50 mm requirement with significant margin)
+//
+// Weight estimate — frame only (excl. motors, electronics, battery):
+//   Deck plate (8 mm Al, lightened) ≈ 1.55 kg
+//   Side frames 2 × (6 mm Al)      ≈ 0.52 kg
+//   Skid plate (saltytank_skid_plate.scad, 4 mm HDPE) ≈ 0.56 kg
+//   Brackets + fasteners (PETG + SS) ≈ 0.35 kg
+//   Total ≈ 2.98 kg  ← just under 3 kg target
+//
+// RENDER options:
+//   "assembly"           full 3D preview (default)
+//   "deck_2d"            DXF — deck plate (waterjet / CNC)
+//   "side_frame_2d"      DXF — side frame plate (×2 mirrored; CNC)
+//   "side_frame_stl"     STL — side frame (print 2×, mirror right)
+//   "idler_block_stl"    STL — tensioner idler block (print 2×)
+//   "csi_mount_stl"      STL — CSI corner bracket (print 4×)
+//   "d435i_mount_stl"    STL — D435i front bracket (print 1×)
+//
+// ── Export commands ─────────────────────────────────────────
+//  Deck DXF:
+//    openscad saltytank_chassis.scad -D 'RENDER="deck_2d"' -o saltytank_deck.dxf
+//  Side frame DXF (cut 2×, flip one for mirror):
+//    openscad saltytank_chassis.scad -D 'RENDER="side_frame_2d"' -o saltytank_side_frame.dxf
+//  Side frame STL (print 2×; mirror one in slicer):
+//    openscad saltytank_chassis.scad -D 'RENDER="side_frame_stl"' -o saltytank_side_frame.stl
+//  Idler block STL (×2):
+//    openscad saltytank_chassis.scad -D 'RENDER="idler_block_stl"' -o saltytank_idler_block.stl
+//  CSI bracket STL (×4):
+//    openscad saltytank_chassis.scad -D 'RENDER="csi_mount_stl"' -o saltytank_csi_mount.stl
+//  D435i bracket STL (×1):
+//    openscad saltytank_chassis.scad -D 'RENDER="d435i_mount_stl"' -o saltytank_d435i_mount.stl
+// ============================================================
+
+$fn = 64;
+e   = 0.01;
+
+// ── Deck plate ────────────────────────────────────────────────────────────────
+BODY_L     = 500.0;    // deck fore-aft (Y)
+BODY_W     = 360.0;    // deck left-right (X) — space between inner frame faces
+DECK_T     =   8.0;    // deck plate thickness
+DECK_R     =  15.0;    // corner fillet radius
+
+// ── Side frame geometry ───────────────────────────────────────────────────────
+FRAME_H    =  90.0;    // frame height below deck bottom = hull ground clearance
+FRAME_T    =   6.0;    // frame plate thickness (6 mm Al laser-cut)
+FRAME_R    =  10.0;    // frame corner fillet radius
+
+// ── Track system (rubber or metal belt) ───────────────────────────────────────
+TRACK_WID  =  80.0;    // track belt width
+TRACK_PITCH =  20.0;   // track link pitch (mm) — affects sprocket tooth count
+// Track belt inner face sits at X = ±(BODY_W/2 + FRAME_T) from centre
+// Track CL at X = ±(BODY_W/2 + FRAME_T + TRACK_WID/2) from centre
+
+// ── Drive sprocket (rear) — hoverboard hub motor ──────────────────────────────
+// Caliper-verified axle (matches BOM.md):
+SPROCKET_AXLE_D   =  16.11;   // axle base OD (round section near hub)
+SPROCKET_AXLE_FLAT=  13.00;   // D-cut chord width
+SPROCKET_AXLE_DCUT=  15.95;   // D-cut OD
+BEARING_OD        =  37.80;   // motor bearing-seat collar OD
+BEARING_RECESS    =   8.0;    // bearing seat recess depth in frame
+
+// Sprocket pitch circle: 10 teeth × 20 mm pitch
+// PCD = pitch / sin(π/N) = 20 / sin(18°) ≈ 64.7 mm → R ≈ 32.4 mm
+SPROCKET_R        =  33.0;    // sprocket pitch-circle radius (nominal)
+SPROCKET_POS_Y    = -(BODY_L/2 - SPROCKET_R - 22);  // rear, −Y
+
+// Hub motor flange bolt circle (4× M5 at 90°, BC = 52 mm)
+// ⚠ Verify against your motor flange before fabricating!
+HUB_FLANGE_BC     =  52.0;    // motor hub bolt circle OD (M5 × 4)
+HUB_BOLT_D        =   5.3;    // M5 clearance
+
+// ── Idler wheel (front, adjustable tensioner) ─────────────────────────────────
+IDLER_R           =  40.0;    // idler wheel radius (OD = 80 mm)
+IDLER_AXLE_D      =   8.5;    // M8 axle clearance bore
+IDLER_POS_Y_NOM   = +(BODY_L/2 - IDLER_R - 22);  // front nominal, +Y
+// Tensioner slot: idler can move ±TENS_TRAVEL fore-aft to tension track
+TENS_TRAVEL       =  15.0;    // ±15 mm track tension adjustment
+// Tensioner bolt: M6 runs fore-aft in threaded lug at front of slot
+TENS_BOLT_D       =   6.5;    // M6 clearance
+TENS_BLOCK_L      =  35.0;    // sliding idler block length
+TENS_BLOCK_W      =  TRACK_WID - 4;  // block width (fits inside track)
+TENS_SLOT_H       =  IDLER_AXLE_D + 2.0;  // slot height (clearance for block)
+
+// ── Road wheels (2× per side, between sprocket and idler) ────────────────────
+ROAD_WHEEL_R      =  30.0;    // road wheel radius (OD = 60 mm)
+ROAD_AXLE_D       =   8.5;    // M8 axle clearance bore
+ROAD_Y_1          = -BODY_L/4;   // forward road wheel (−125 mm from centre)
+ROAD_Y_2          = +BODY_L/4;   // rearward road wheel (+125 mm from centre)
+// ⚠ Reversed fore/aft: ROAD_Y_1 is near rear sprocket, ROAD_Y_2 near front idler
+
+// ── Height stack ─────────────────────────────────────────────────────────────
+// Ground Z = 0 (absolute); deck coords: Z=0 at deck top.
+// In deck coords, ground is at Z = -(DECK_T + FRAME_H) = -98 mm.
+//   SPROCKET_CL_Z = -(DECK_T + FRAME_H - SPROCKET_R)  = -(98 - 33) = -65 mm
+//   IDLER_CL_Z    = -(DECK_T + FRAME_H - IDLER_R)     = -(98 - 40) = -58 mm
+//   ROAD_WHEEL_Z  = -(DECK_T + FRAME_H - ROAD_WHEEL_R) = -(98 - 30) = -68 mm
+GROUND_Z          = -(DECK_T + FRAME_H);   // = -98 mm in deck coords
+
+// ── Stem socket (deck centre, shared with SaltyLab / SaltyRover) ─────────────
+STEM_BORE         =  25.5;    // 25 mm tube + 0.5 mm clearance
+STEM_COLLAR_OD    =  50.0;
+STEM_COLLAR_H     =  20.0;    // boss height above deck top
+STEM_FLANGE_BC    =  40.0;    // 4× M4 bolt circle
+
+// ── Electronics bay footprint (rover_electronics_bay.scad) ───────────────────
+// Bay dimensions match rover_electronics_bay.scad exactly.
+// Deck holes match bay floor bolt pattern for drop-in compatibility.
+BAY_L             = 240.0;    // bay length (X on deck = left-right)
+BAY_W             = 200.0;    // bay width  (Y on deck = fore-aft)
+BAY_WALL          =   3.0;    // bay wall thickness
+BAY_BOLT_INSET    =   8.0;    // bay bolt CL from bay exterior corner
+
+// ── FC mount — 30.5 × 30.5 mm M3 (shared SaltyLab pattern) ──────────────────
+FC_PITCH          =  30.5;
+FC_HOLE_D         =   3.2;
+FC_POS_Y          =  BAY_W/2 - 50.0;   // near front edge (inside bay footprint)
+
+// ── Jetson Orin mount — 58 × 49 mm M3 (shared SaltyLab pattern) ─────────────
+ORIN_HOLE_X       =  58.0;
+ORIN_HOLE_Y       =  49.0;
+ORIN_HOLE_D       =   3.2;
+ORIN_POS_Y        = -(BAY_W/2 - 55.0);  // near rear edge
+
+// ── CSI corner camera mounts ──────────────────────────────────────────────────
+CSI_PCB           =  25.0;    // IMX219 PCB square side
+CSI_M2_SPC        =  15.0;    // M2 hole pitch
+CSI_TILT          =  20.0;    // nose-down tilt (degrees)
+
+// ── D435i front bracket ───────────────────────────────────────────────────────
+RS_TILT           =   8.0;    // nose-down tilt (degrees)
+RS_ARM_LEN        =  70.0;    // arm reach forward from deck edge
+RS_BASE_W         =  44.0;    // base plate width
+
+// ── Fasteners ─────────────────────────────────────────────────────────────────
+M2_D  = 2.3;
+M3_D  = 3.3;
+M4_D  = 4.3;
+M5_D  = 5.3;
+M6_D  = 6.5;
+M8_D  = 8.5;
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
+RENDER = "assembly";
+
+if (RENDER == "assembly") {
+    assembly();
+} else if (RENDER == "deck_2d") {
+    projection(cut = true)
+        translate([0, 0, -DECK_T / 2])
+            deck_plate();
+} else if (RENDER == "side_frame_2d") {
+    // Left frame, projected flat (2D profile only — Z/Y plane)
+    projection(cut = true)
+        rotate([90, 0, 0])
+            translate([0, 0, BODY_L / 2])
+                side_frame(-1);
+} else if (RENDER == "side_frame_stl") {
+    side_frame(-1);  // mirror one in slicer for right side
+} else if (RENDER == "idler_block_stl") {
+    idler_block();
+} else if (RENDER == "csi_mount_stl") {
+    csi_corner_bracket();
+} else if (RENDER == "d435i_mount_stl") {
+    d435i_front_bracket();
+}
+
+// ============================================================
+// FULL ASSEMBLY
+// ============================================================
+module assembly() {
+    // Deck plate
+    color("Silver", 0.90) deck_plate();
+
+    // Stem collar
+    color("DimGray", 0.85) stem_collar();
+
+    // Side frames (left = −1, right = +1)
+    color("SteelBlue",  0.80) side_frame(-1);
+    color("SteelBlue",  0.80) side_frame(+1);
+
+    // Idler tensioner blocks (×2)
+    color("LightSlateGray", 0.85)
+        translate([-BODY_W/2 - FRAME_T, IDLER_POS_Y_NOM,
+                   GROUND_Z + IDLER_R])
+            rotate([0, 90, 0]) idler_block();
+    color("LightSlateGray", 0.85)
+        translate([+BODY_W/2, IDLER_POS_Y_NOM,
+                   GROUND_Z + IDLER_R])
+            rotate([0, -90, 0]) idler_block();
+
+    // CSI corner brackets
+    for (sx = [-1, 1])
+    for (sy = [-1, 1])
+        color("Teal", 0.85) csi_bracket_placed(sx, sy);
+
+    // D435i front bracket
+    color("DarkSlateGray", 0.85) d435i_bracket_placed();
+
+    // Phantom: electronics bay (rover_electronics_bay.scad)
+    %color("OliveDrab", 0.25)
+        translate([0, 0, DECK_T])
+            cube([BAY_L + 2*BAY_WALL,
+                  BAY_W + 2*BAY_WALL,
+                  84], center = true);
+
+    // Phantom: track loops (rubber belt cross-section)
+    for (sx = [-1, 1])
+        %color("Black", 0.15)
+            translate([sx * (BODY_W/2 + FRAME_T + TRACK_WID/2), 0, 0])
+                rotate([90, 0, 0])
+                    track_loop_ghost();
+
+    // Phantom: hub motors (rear, outboard)
+    for (sx = [-1, 1])
+        %color("Orange", 0.20)
+            translate([sx * (BODY_W/2 + FRAME_T + 30),
+                       SPROCKET_POS_Y,
+                       GROUND_Z + SPROCKET_R])
+                rotate([0, sx*90, 0])
+                    cylinder(d = BEARING_OD, h = 70, center = false);
+}
+
+// Ghost track loop outline for preview (no geometry output)
+module track_loop_ghost() {
+    span = abs(IDLER_POS_Y_NOM - SPROCKET_POS_Y);
+    hull() {
+        translate([0, IDLER_POS_Y_NOM,  GROUND_Z + IDLER_R])
+            rotate([90, 0, 0]) cylinder(d = IDLER_R*2, h = 1);
+        translate([0, SPROCKET_POS_Y,   GROUND_Z + SPROCKET_R])
+            rotate([90, 0, 0]) cylinder(d = SPROCKET_R*2, h = 1);
+    }
+}
+
+// ============================================================
+// DECK PLATE  (Part A — laser-cut 8 mm 5052-H32 aluminium)
+// ============================================================
+// Rectangular plate spanning the gap between the two side frames.
+// All electronics and sensor mounts attach to the deck top face.
+// Side frames bolt to deck side edges (M5 × 4 per side).
+// Weight estimate: 500×360×8 mm Al, ~45% lightened ≈ 1.55 kg
+module deck_plate() {
+    difference() {
+        // ── Outer profile ─────────────────────────────────────────────
+        linear_extrude(DECK_T)
+            minkowski() {
+                square([BODY_L - 2*DECK_R, BODY_W - 2*DECK_R],
+                        center = true);
+                circle(r = DECK_R);
+            }
+
+        // ── Side frame attachment slots (M5 × 4 per side) ─────────────
+        // Slots run fore-aft (Y) for ±10 mm lateral alignment adjustment
+        for (sx = [-1, 1])
+        for (py = [-BODY_L/4, BODY_L/4]) {
+            hull() {
+                translate([sx*(BODY_W/2 - 8), py - 12, -e])
+                    cylinder(d = M5_D, h = DECK_T + 2*e);
+                translate([sx*(BODY_W/2 - 8), py + 12, -e])
+                    cylinder(d = M5_D, h = DECK_T + 2*e);
+            }
+        }
+
+        // ── Stem bore ─────────────────────────────────────────────────
+        translate([0, 0, -e])
+            cylinder(d = STEM_BORE, h = DECK_T + 2*e);
+
+        // ── Stem flange bolts (4× M4 at 90°) ─────────────────────────
+        for (a = [0, 90, 180, 270])
+            rotate([0, 0, a])
+                translate([STEM_FLANGE_BC/2, 0, -e])
+                    cylinder(d = M4_D, h = DECK_T + 2*e);
+
+        // ── Electronics bay footprint bolt holes (10× M3) ─────────────
+        // Matches rover_electronics_bay.scad floor flange pattern exactly
+        for (sx = [-1, 1])
+        for (sy = [-1, 1]) {
+            bx = sx * (BAY_L/2 + BAY_WALL - BAY_BOLT_INSET);
+            by = sy * (BAY_W/2 + BAY_WALL - BAY_BOLT_INSET);
+            translate([bx, by, -e])
+                cylinder(d = M3_D, h = DECK_T + 2*e);
+        }
+        // Centre long-wall bolts (2×)
+        for (sy = [-1, 1])
+            translate([0, sy*(BAY_W/2 + BAY_WALL - BAY_BOLT_INSET), -e])
+                cylinder(d = M3_D, h = DECK_T + 2*e);
+
+        // ── FC mount holes — 30.5×30.5 M3 (shared SaltyLab) ──────────
+        for (dx = [-FC_PITCH/2, FC_PITCH/2])
+        for (dy = [-FC_PITCH/2, FC_PITCH/2])
+            translate([dx, FC_POS_Y + dy, -e])
+                cylinder(d = FC_HOLE_D, h = DECK_T + 2*e);
+
+        // ── Jetson Orin mount holes — 58×49 M3 (shared SaltyLab) ─────
+        for (dx = [-ORIN_HOLE_X/2, ORIN_HOLE_X/2])
+        for (dy = [-ORIN_HOLE_Y/2, ORIN_HOLE_Y/2])
+            translate([dx, ORIN_POS_Y + dy, -e])
+                cylinder(d = ORIN_HOLE_D, h = DECK_T + 2*e);
+
+        // ── Lightening holes (between bay footprint and deck edges) ───
+        for (sx = [-1, 1])
+        for (sy = [-1, 1]) {
+            lx = sx * (BAY_L/2 + 40);
+            ly = sy * (BODY_L/4 + 10);
+            translate([lx, ly, -e])
+                cylinder(d = 50, h = DECK_T + 2*e);
+        }
+        // Centre pair flanking stem
+        for (sx = [-1, 1])
+            translate([sx * 65, 0, -e])
+                cylinder(d = 38, h = DECK_T + 2*e);
+
+        // ── Cable routing slots (motor phase + sensor harness) ─────────
+        // 4× slots near side frame attachment points
+        for (sx = [-1, 1])
+        for (sy = [-1, 1])
+            hull() {
+                translate([sx*(BODY_W/2 - 30), sy*(BODY_L/4 - 8), -e])
+                    cylinder(d = 14, h = DECK_T + 2*e);
+                translate([sx*(BODY_W/2 - 30), sy*(BODY_L/4 + 8), -e])
+                    cylinder(d = 14, h = DECK_T + 2*e);
+            }
+
+        // ── Skid plate attachment holes (M4 × 8, through deck underside)
+        // These align with saltytank_skid_plate.scad bolt pattern
+        for (sx = [-1, 1])
+        for (sy = [-1, 0, 1]) {
+            bx = sx * (BODY_W/2 - 20);
+            by = sy * (BODY_L/3);
+            translate([bx, by, -e])
+                cylinder(d = M4_D, h = DECK_T + 2*e);
+        }
+
+        // ── CSI corner bracket attachment holes (M3 × 2 per corner) ───
+        for (sx = [-1, 1])
+        for (sy = [-1, 1])
+        for (dd = [-12, 12])
+            translate([sx*(BODY_W/2 - 18),
+                       sy*(BODY_L/2 - 18) + dd*sy, -e])
+                cylinder(d = M3_D, h = DECK_T + 2*e);
+    }
+}
+
+// ── Deck-top stem collar ─────────────────────────────────────────────────────
+module stem_collar() {
+    translate([0, 0, DECK_T])
+        difference() {
+            cylinder(d = STEM_COLLAR_OD, h = STEM_COLLAR_H);
+            translate([0, 0, -e])
+                cylinder(d = STEM_BORE, h = STEM_COLLAR_H + 2*e);
+            for (a = [0, 90, 180, 270])
+                rotate([0, 0, a])
+                    translate([STEM_FLANGE_BC/2, 0, -e])
+                        cylinder(d = M4_D, h = STEM_COLLAR_H + 2*e);
+        }
+}
+
+// ============================================================
+// SIDE FRAME  (Part B — laser-cut 6 mm 6061-T6 aluminium)
+// ============================================================
+// Vertical plate running the full body length.
+// Inner face bolts to deck plate side edge.
+// Outer face mounts: drive sprocket (rear) + idler slot (front)
+//                   + road wheels (middle).
+//
+// `side` = -1 (left / −X) or +1 (right / +X)
+// The right frame is a mirror of the left — cut 2× from same DXF,
+// flip one face-down before mounting.
+//
+// Weight estimate: 500×90×6 mm Al, ~35% lightened ≈ 0.26 kg each
+// ============================================================
+module side_frame(side = -1) {
+    sx = side;  // −1 = left, +1 = right
+    // Frame inner face at X = sx * BODY_W/2
+    // Frame outer face at X = sx * (BODY_W/2 + FRAME_T)
+    frame_x = sx * BODY_W/2;  // inner face X position
+
+    // In deck coords:  sprocket / idler CL Z values
+    spr_z  = GROUND_Z + SPROCKET_R;   // = -(DECK_T + FRAME_H) + SPROCKET_R
+    idl_z  = GROUND_Z + IDLER_R;
+    rw_z   = GROUND_Z + ROAD_WHEEL_R;
+
+    translate([frame_x, 0, 0])
+    rotate([0, sx > 0 ? 180 : 0, 0])  // mirror right frame in X
+    translate([0, 0, 0]) {
+        difference() {
+            // ── Outer profile of side frame ───────────────────────────
+            // Frame plate in the Y-Z plane, FRAME_T thick in X
+            translate([0, -BODY_L/2, GROUND_Z])
+                linear_extrude(FRAME_T)
+                    minkowski() {
+                        square([BODY_L - 2*FRAME_R,
+                                FRAME_H + DECK_T - 2*FRAME_R],
+                                center = false);
+                        circle(r = FRAME_R);
+                    }
+
+            // ── Deck attachment slots (2× M5 per side, front + rear) ──
+            // Slots allow ±10 mm vertical adjustment for frame height
+            for (py = [-BODY_L/4, BODY_L/4])
+                hull() {
+                    translate([e, py - 12, -DECK_T / 2])
+                        rotate([0, 90, 0])
+                            cylinder(d = M5_D, h = FRAME_T + 2*e);
+                    translate([e, py + 12, -DECK_T / 2])
+                        rotate([0, 90, 0])
+                            cylinder(d = M5_D, h = FRAME_T + 2*e);
+                }
+
+            // ── Drive sprocket bore — D-cut (rear) ────────────────────
+            // Round section (base, near hub)
+            translate([e, SPROCKET_POS_Y, spr_z])
+                rotate([0, 90, 0])
+                    cylinder(d = SPROCKET_AXLE_D + 0.4, h = FRAME_T + 2*e);
+
+            // D-cut flat (anti-rotation) — removes chord to AXLE_FLAT
+            dcut_h = sqrt(pow((SPROCKET_AXLE_DCUT + 0.4)/2, 2)
+                          - pow((SPROCKET_AXLE_FLAT + 0.4)/2, 2));
+            translate([e, SPROCKET_POS_Y, spr_z])
+                rotate([0, 90, 0])
+                    translate([0, (SPROCKET_AXLE_FLAT + 0.4)/2, 0])
+                        cube([(SPROCKET_AXLE_DCUT + 0.4)/2,
+                               FRAME_T + 2*e,
+                               FRAME_T + 2*e],
+                              center = false);
+
+            // ── Bearing seat recess (outboard face — inboard of track) ─
+            // Prevents Ø37.8 mm collar binding on frame face
+            translate([e - BEARING_RECESS, SPROCKET_POS_Y, spr_z])
+                rotate([0, 90, 0])
+                    cylinder(d = BEARING_OD + 1.5,
+                             h = BEARING_RECESS + e);
+
+            // ── Hub motor flange bolt holes (4× M5, 52 mm BC) ─────────
+            for (a = [45, 135, 225, 315])
+                translate([e,
+                           SPROCKET_POS_Y + HUB_FLANGE_BC/2 * sin(a),
+                           spr_z           + HUB_FLANGE_BC/2 * cos(a)])
+                    rotate([0, 90, 0])
+                        cylinder(d = HUB_BOLT_D, h = FRAME_T + 2*e);
+
+            // ── Idler tensioner slot (front) ──────────────────────────
+            // Horizontal slot in Y direction; idler block slides in it
+            slot_y_ctr = IDLER_POS_Y_NOM;
+            slot_y_min = slot_y_ctr - TENS_TRAVEL;
+            slot_y_max = slot_y_ctr + TENS_TRAVEL;
+            translate([e, slot_y_min, idl_z - TENS_SLOT_H/2])
+                cube([FRAME_T + 2*e,
+                      slot_y_max - slot_y_min,
+                      TENS_SLOT_H]);
+            // Rounded ends
+            translate([e, slot_y_min, idl_z])
+                rotate([0, 90, 0])
+                    cylinder(d = TENS_SLOT_H, h = FRAME_T + 2*e);
+            translate([e, slot_y_max, idl_z])
+                rotate([0, 90, 0])
+                    cylinder(d = TENS_SLOT_H, h = FRAME_T + 2*e);
+
+            // ── Tensioner bolt bore (M6, at front end of slot) ────────
+            // M6 bolt threads into lug at front of slot; pushes block rearward
+            translate([e,
+                       slot_y_max + 8,
+                       idl_z])
+                rotate([0, 90, 0])
+                    cylinder(d = M6_D, h = FRAME_T + 2*e);
+
+            // ── Road wheel bores (2× M8, fore/aft of centre) ──────────
+            for (ry = [ROAD_Y_1, ROAD_Y_2])
+                translate([e, ry, rw_z])
+                    rotate([0, 90, 0])
+                        cylinder(d = ROAD_AXLE_D, h = FRAME_T + 2*e);
+
+            // ── Lightening holes (between bore positions) ──────────────
+            // Row 1: between road wheel 1 and sprocket
+            translate([e,
+                       (SPROCKET_POS_Y + ROAD_Y_1) / 2,
+                       GROUND_Z + FRAME_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = 45, h = FRAME_T + 2*e);
+
+            // Row 2: between road wheel 2 and idler
+            translate([e,
+                       (IDLER_POS_Y_NOM + ROAD_Y_2) / 2,
+                       GROUND_Z + FRAME_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = 45, h = FRAME_T + 2*e);
+
+            // Row 3: between the two road wheels
+            translate([e, (ROAD_Y_1 + ROAD_Y_2) / 2, GROUND_Z + FRAME_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = 35, h = FRAME_T + 2*e);
+        }
+    }
+}
+
+// ============================================================
+// IDLER TENSIONER BLOCK  (Part C — 3D print PETG, × 2)
+// ============================================================
+// Slides in the frame's tensioner slot.
+// M8 bore holds the idler axle.
+// A flat face at the front receives the tensioner M6 bolt.
+// Lock nut (M6 nyloc) clamps block at desired track tension.
+// Print orientation: flat face on bed; no supports needed.
+// ============================================================
+module idler_block() {
+    block_h = TENS_SLOT_H - 0.6;   // height with slot clearance
+    block_l = TENS_BLOCK_L;
+    block_w = FRAME_T - 0.4;       // thickness with clearance
+
+    difference() {
+        union() {
+            // Main block body
+            cube([block_w, block_l, block_h], center = true);
+
+            // Tensioner bolt lug (extends from +Y face)
+            translate([0, block_l/2, 0])
+                cube([block_w, 10, block_h], center = true);
+        }
+
+        // M8 axle bore through full width
+        translate([0, 0, 0])
+            rotate([0, 90, 0])
+                cylinder(d = IDLER_AXLE_D, h = block_w + 2*e, center = true);
+
+        // M6 tensioner bolt bore (fore-aft, through lug)
+        translate([0, block_l/2 + 5, 0])
+            rotate([90, 0, 0])
+                cylinder(d = M6_D, h = 16, center = true);
+
+        // M6 nyloc nut pocket (rear face of lug — captured nut)
+        translate([0, block_l/2 - 1, 0])
+            rotate([90, 0, 0])
+                cylinder(d = 11.5, h = 5, $fn = 6, center = false);
+
+        // ── Lightening slot (centre, removes material not in load path)
+        cube([block_w + 2*e, block_l/2 - IDLER_AXLE_D/2 - 4, block_h - 4],
+              center = true);
+    }
+}
+
+// ============================================================
+// CSI CORNER BRACKET  (Part D — 3D print PETG, × 4)
+// ============================================================
+// Same design as saltyrover_chassis_r2.scad.
+// Mounts IMX219 / Arducam CSI camera at each deck corner,
+// angled 45° outward + CSI_TILT downward.
+// ============================================================
+module csi_corner_bracket() {
+    base_l = 42;
+    base_w = 32;
+    base_t =  5;
+
+    difference() {
+        union() {
+            cube([base_l, base_w, base_t]);
+            translate([base_l / 2, base_w / 2, base_t])
+                rotate([0, CSI_TILT, 0])
+                    translate([-CSI_PCB/2 - 3, -CSI_PCB/2 - 3, 0])
+                        cube([CSI_PCB + 6, CSI_PCB + 6, base_t]);
+        }
+        // 2× M3 base attachment holes
+        for (dx = [8, base_l - 8])
+            translate([dx, base_w / 2, -e])
+                cylinder(d = M3_D, h = base_t + 2*e);
+        // CSI M2 holes (15×15 mm pattern)
+        translate([base_l / 2, base_w / 2, base_t])
+            rotate([0, CSI_TILT, 0])
+                for (cx = [-CSI_M2_SPC/2, CSI_M2_SPC/2])
+                for (cy = [-CSI_M2_SPC/2, CSI_M2_SPC/2])
+                    translate([cx, cy, -e])
+                        cylinder(d = M2_D, h = base_t + 2*e);
+        // CSI ribbon slot
+        translate([base_l/2 - 6, base_w/2 - 1.5, -e])
+            cube([12, 3, base_t + 2*e]);
+    }
+}
+
+module csi_bracket_placed(sx, sy) {
+    cx = sx * (BODY_W/2 - 22);
+    cy = sy * (BODY_L/2 - 22);
+    rot = atan2(sy, sx) * 180 / 3.14159 - 45;
+    translate([cx, cy, DECK_T])
+        rotate([0, 0, rot])
+            translate([-21, -16, 0])
+                csi_corner_bracket();
+}
+
+// ============================================================
+// D435i FRONT BRACKET  (Part E — 3D print PETG, × 1)
+// ============================================================
+// Same design as saltyrover_chassis_r2.scad.
+// Arm extends forward from deck front edge.
+// RS_TILT degrees nose-down.  1/4-20 captured nut for D435i.
+// ============================================================
+module d435i_front_bracket() {
+    base_d  = 24;
+    base_h  =  8;
+    arm_len = RS_ARM_LEN;
+
+    nut14_af = 11.1;
+    nut14_h  =  5.6;
+    nut14_cl =  6.5;
+
+    difference() {
+        union() {
+            // Base plate (bolts to deck front face)
+            translate([-RS_BASE_W/2, 0, 0])
+                cube([RS_BASE_W, base_d, base_h]);
+            // Forward arm
+            translate([-13, base_d, 0])
+                cube([26, arm_len, base_h]);
+            // Tilted face plate
+            translate([0, base_d + arm_len, base_h/2])
+                rotate([0, RS_TILT, 0])
+                    translate([-16, 0, -base_h/2])
+                        cube([32, 14, base_h]);
+        }
+        // 2× M4 base attachment
+        for (dx = [-RS_BASE_W/2 + 10, RS_BASE_W/2 - 10])
+            translate([dx, base_d/2, -e])
+                cylinder(d = M4_D, h = base_h + 2*e);
+        // 1/4-20 UNC captured nut
+        translate([0, base_d + arm_len + 12, base_h/2])
+            rotate([0, 90, 0]) {
+                translate([0, 0, -nut14_h - 1])
+                    cylinder(d = nut14_af/cos(30), h = nut14_h + 1, $fn = 6);
+                cylinder(d = nut14_cl, h = 20);
+            }
+    }
+}
+
+module d435i_bracket_placed() {
+    translate([0, BODY_L/2 + 12, DECK_T])
+        rotate([0, 0, 180])
+            d435i_front_bracket();
+}

chassis/saltytank_skid_plate.scad

@@ -0,0 +1,214 @@
+// ============================================================
+// saltytank_skid_plate.scad — SaltyTank Underside Skid Plate
+// Issue: #121  Agent: sl-mechanical  Date: 2026-03-01
+// ============================================================
+//
+// Bolt-on underside protection panel for rough terrain.
+// Mounts to the underside of the deck plate, covering the hull
+// floor between the two track frames.
+//
+// Purpose:
+//   • Protects electronics bay wiring + deck underside from rock strikes
+//   • Sacrificial layer — replace without reworking deck plate
+//   • Provides a smooth low-drag hull bottom for terrain sliding
+//   • Four drain / inspection slots (water / mud egress)
+//
+// Material options:
+//   Primary    : 4 mm HDPE (high-density polyethylene)
+//                — lightweight, impact resistant, low friction surface
+//   Alternative: 2 mm 304 stainless steel
+//                — heavier (+0.5 kg), extremely wear resistant
+//   Prototype  : 6 mm plywood (quick, cheap; not weatherproof)
+//
+// Dimensions: BODY_W × BODY_L × SKID_T
+//   = 360 × 500 × 4 mm  (fits between track inner faces)
+//
+// Mounting:
+//   8× M4 SHCS from below; nuts captured in deck plate M4 rivet-nuts
+//   (see saltytank_chassis.scad skid plate attachment holes)
+//   Countersunk (FHCS) variant available — set COUNTERSINK = true
+//
+// Coordinate: Z=0 at skid top face (= deck plate underside)
+//   Ground contact surface at Z = -(FRAME_H) = -90 mm (in deck coords)
+//   Skid sits immediately below deck: deck_bottom → skid_top
+//
+// RENDER options:
+//   "skid_stl"    STL for 3D printing (4 mm PETG, prototype)
+//   "skid_2d"     DXF for waterjet / CNC (HDPE or steel plate)
+//   "assembly"    preview with deck ghost
+//
+// Export commands:
+//   DXF (HDPE / steel — recommended):
+//     openscad saltytank_skid_plate.scad -D 'RENDER="skid_2d"' -o saltytank_skid.dxf
+//   STL (3D print prototype):
+//     openscad saltytank_skid_plate.scad -D 'RENDER="skid_stl"' -o saltytank_skid.stl
+// ============================================================
+
+$fn  = 64;
+e    = 0.01;
+
+// ── Skid plate dimensions ────────────────────────────────────────────────────
+// Must match saltytank_chassis.scad BODY_W and BODY_L exactly.
+BODY_L    = 500.0;   // fore-aft — copy from saltytank_chassis.scad
+BODY_W    = 360.0;   // left-right
+SKID_T    =   4.0;   // skid plate thickness (4 mm HDPE)
+SKID_R    =  12.0;   // corner radius (matches deck corners, ease of cleaning)
+
+FRAME_H   =  90.0;   // side frame height (from saltytank_chassis.scad)
+
+// ── Skid reinforcement ribs (under-surface, printed variant only) ─────────────
+// Ribs add stiffness to the printed skid without adding flat-surface area.
+// Ribs are omitted on the CNC/laser version (material stiffness is sufficient).
+RIB_H      =  8.0;    // rib height below skid top face
+RIB_T      =  3.0;    // rib wall thickness
+RIB_PRINT  = false;   // set true to add ribs (print only; skip for DXF)
+
+// ── Attachment bolt holes (M4) ───────────────────────────────────────────────
+// Pattern must match saltytank_chassis.scad skid plate holes exactly.
+// 2 rows × 3 columns = 6 holes + 2 corners = 8 total
+BOLT_D     =  4.3;    // M4 clearance
+BOLT_INSET_X =  20.0; // CL inset from left/right edge
+BOLT_INSET_Y =  25.0; // CL inset from front/rear edge
+
+// Countersinking (flat-head bolts flush with skid surface — preferred for terrain)
+COUNTERSINK = true;
+CSINK_ANGLE = 82;     // M4 FHCS countersink angle (82° for DIN 7991)
+CSINK_OD    =  8.0;   // M4 FHCS head diameter
+
+// ── Drain / inspection slots ─────────────────────────────────────────────────
+DRAIN_W    =  20.0;   // slot width
+DRAIN_L    =  60.0;   // slot length
+DRAIN_R    =  10.0;   // slot corner radius
+
+// ── Road-wheel axle pass-through slots ───────────────────────────────────────
+// The road wheel axles sit OUTSIDE the skid plate (they're in the side frames),
+// so no axle holes are needed in the skid plate.
+// The skid plate is a solid panel between the track frames.
+
+// ── Fasteners ─────────────────────────────────────────────────────────────────
+M4_D = 4.3;
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
+RENDER = "assembly";
+
+if (RENDER == "skid_stl") {
+    skid_plate();
+} else if (RENDER == "skid_2d") {
+    projection(cut = true)
+        translate([0, 0, -SKID_T / 2])
+            skid_plate_flat_only();
+} else if (RENDER == "assembly") {
+    assembly();
+}
+
+// ============================================================
+// ASSEMBLY PREVIEW
+// ============================================================
+module assembly() {
+    color("Sienna",  0.85) skid_plate();
+
+    // Ghost deck plate above
+    %color("Silver", 0.25)
+        translate([0, 0, SKID_T])
+            linear_extrude(8)
+                minkowski() {
+                    square([BODY_L - 24, BODY_W - 24], center = true);
+                    circle(r = 12);
+                }
+
+    // Ghost side frames (simplified)
+    for (sx = [-1, 1])
+        %color("SteelBlue", 0.20)
+            translate([sx * (BODY_W/2 + 3), 0, SKID_T])
+                cube([6, BODY_L, FRAME_H + 8], center = true);
+}
+
+// ============================================================
+// SKID PLATE (3D print — includes optional ribs)
+// ============================================================
+module skid_plate() {
+    difference() {
+        union() {
+            // ── Base plate ─────────────────────────────────────────────
+            linear_extrude(SKID_T)
+                skid_profile_2d();
+
+            // ── Reinforcement ribs (printed version only) ──────────────
+            if (RIB_PRINT) {
+                // Longitudinal centre rib
+                translate([0, 0, SKID_T])
+                    cube([RIB_T, BODY_L - 40, RIB_H], center = true);
+                // Lateral ribs (3× fore-aft)
+                for (ry = [-BODY_L/4, 0, BODY_L/4])
+                    translate([0, ry, SKID_T])
+                        cube([BODY_W - 40, RIB_T, RIB_H], center = true);
+            }
+        }
+
+        // ── Attachment bolt holes ──────────────────────────────────────
+        for (bpos = bolt_positions())
+            translate([bpos[0], bpos[1], -e]) {
+                cylinder(d = BOLT_D, h = SKID_T + 2*e);
+                if (COUNTERSINK)
+                    // Countersink cone from top face
+                    translate([0, 0, SKID_T + e])
+                        cylinder(d1 = CSINK_OD, d2 = BOLT_D,
+                                 h = (CSINK_OD - BOLT_D) / (2 * tan(CSINK_ANGLE/2)));
+            }
+
+        // ── Drain / inspection slots (4×, one per quadrant) ───────────
+        for (sx = [-1, 1])
+        for (sy = [-1, 1]) {
+            dx = sx * (BODY_W/4 + 15);
+            dy = sy * (BODY_L/4 + 20);
+            translate([dx - DRAIN_W/2 + DRAIN_R, dy - DRAIN_L/2 + DRAIN_R, -e])
+                linear_extrude(SKID_T + 2*e)
+                    minkowski() {
+                        square([DRAIN_W - 2*DRAIN_R, DRAIN_L - 2*DRAIN_R]);
+                        circle(r = DRAIN_R);
+                    }
+        }
+    }
+}
+
+// 2D profile of skid plate (shared by skid_plate and DXF export)
+module skid_profile_2d() {
+    minkowski() {
+        square([BODY_L - 2*SKID_R, BODY_W - 2*SKID_R], center = true);
+        circle(r = SKID_R);
+    }
+}
+
+// Flat-only version for DXF projection (no ribs)
+module skid_plate_flat_only() {
+    difference() {
+        linear_extrude(SKID_T) skid_profile_2d();
+        for (bpos = bolt_positions())
+            translate([bpos[0], bpos[1], -e])
+                cylinder(d = BOLT_D, h = SKID_T + 2*e);
+        for (sx = [-1, 1])
+        for (sy = [-1, 1]) {
+            dx = sx * (BODY_W/4 + 15);
+            dy = sy * (BODY_L/4 + 20);
+            translate([dx - DRAIN_W/2 + DRAIN_R, dy - DRAIN_L/2 + DRAIN_R, -e])
+                linear_extrude(SKID_T + 2*e)
+                    minkowski() {
+                        square([DRAIN_W - 2*DRAIN_R, DRAIN_L - 2*DRAIN_R]);
+                        circle(r = DRAIN_R);
+                    }
+        }
+    }
+}
+
+// ── Bolt position list (8 positions matching saltytank_chassis.scad) ──────────
+function bolt_positions() = [
+    // From saltytank_chassis.scad skid plate holes (sx × (BODY_W/2 - 20), sy × BODY_L/3)
+    [-BODY_W/2 + BOLT_INSET_X,  -BODY_L/3],
+    [-BODY_W/2 + BOLT_INSET_X,   0],
+    [-BODY_W/2 + BOLT_INSET_X,  +BODY_L/3],
+    [+BODY_W/2 - BOLT_INSET_X,  -BODY_L/3],
+    [+BODY_W/2 - BOLT_INSET_X,   0],
+    [+BODY_W/2 - BOLT_INSET_X,  +BODY_L/3]
+];

include/config.h

@@ -120,6 +120,16 @@
 #define PINIO2_PORT         GPIOC
 #define PINIO2_PIN          GPIO_PIN_0    // pinio_config = 129 (USER2)
 
+// --- JLink: Jetson Serial Binary Protocol (USART1, Issue #120) ---
+#define JLINK_BAUD              921600  /* USART1 baud rate */
+#define JLINK_HB_TIMEOUT_MS     1000    /* Jetson heartbeat timeout (ms) */
+#define JLINK_TLM_HZ            50      /* STATUS telemetry TX rate (Hz) */
+
+// --- Firmware Version ---
+#define FW_MAJOR                1
+#define FW_MINOR                0
+#define FW_PATCH                0
+
 // --- SaltyLab Assignments ---
 // Hoverboard ESC: USART2 (PA2=TX, PA3=RX) or USART3
 // ELRS Receiver: UART4 (PA0=TX, PA1=RX) — CRSF 420000 baud

include/jlink.h

@@ -0,0 +1,127 @@
+#ifndef JLINK_H
+#define JLINK_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * JLink — Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX).
+ *
+ * Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated
+ * hardware UART at 921600 baud using DMA circular RX and IDLE interrupt.
+ *
+ * Frame format (both directions):
+ *   [STX=0x02][LEN][CMD][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
+ *
+ *   STX   : frame start sentinel (0x02)
+ *   LEN   : count of CMD + PAYLOAD bytes (1 + payload_len)
+ *   CMD   : command/telemetry type byte
+ *   PAYLOAD: 0..N bytes depending on CMD
+ *   CRC16 : CRC16-XModem over CMD+PAYLOAD (poly 0x1021, init 0), big-endian
+ *   ETX   : frame end sentinel (0x03)
+ *
+ * Jetson → STM32 commands:
+ *   0x01  HEARTBEAT  — no payload; refreshes heartbeat timer
+ *   0x02  DRIVE      — int16 speed (-1000..+1000), int16 steer (-1000..+1000)
+ *   0x03  ARM        — no payload; request arm (same interlock as CDC 'A')
+ *   0x04  DISARM     — no payload; disarm immediately
+ *   0x05  PID_SET    — float kp, float ki, float kd (12 bytes, IEEE-754 LE)
+ *   0x07  ESTOP      — no payload; engage emergency stop
+ *
+ * STM32 → Jetson telemetry:
+ *   0x80  STATUS     — jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
+ *
+ * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
+ * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
+ * RC_ASSISTED modes the Jetson speed offset and steer are injected via
+ * mode_manager_set_auto_cmd() and blended per the existing blend ramp.
+ *
+ * Heartbeat: if no valid frame arrives within JLINK_HB_TIMEOUT_MS (1000ms),
+ * jlink_is_active() returns false and the main loop clears the auto command.
+ */
+
+/* ---- Frame constants ---- */
+#define JLINK_STX               0x02u
+#define JLINK_ETX               0x03u
+
+/* ---- Command IDs (Jetson → STM32) ---- */
+#define JLINK_CMD_HEARTBEAT     0x01u
+#define JLINK_CMD_DRIVE         0x02u
+#define JLINK_CMD_ARM           0x03u
+#define JLINK_CMD_DISARM        0x04u
+#define JLINK_CMD_PID_SET       0x05u
+#define JLINK_CMD_ESTOP         0x07u
+
+/* ---- Telemetry IDs (STM32 → Jetson) ---- */
+#define JLINK_TLM_STATUS        0x80u
+
+/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
+typedef struct __attribute__((packed)) {
+    int16_t  pitch_x10;     /* pitch degrees ×10 */
+    int16_t  roll_x10;      /* roll  degrees ×10 */
+    int16_t  yaw_x10;       /* yaw   degrees ×10 (gyro-integrated) */
+    int16_t  motor_cmd;     /* ESC output -1000..+1000 */
+    uint16_t vbat_mv;       /* battery millivolts */
+    int8_t   rssi_dbm;      /* CRSF RSSI (dBm, negative) */
+    uint8_t  link_quality;  /* CRSF LQ 0-100 */
+    uint8_t  balance_state; /* 0=DISARMED, 1=ARMED, 2=TILT_FAULT */
+    uint8_t  rc_armed;      /* crsf_state.armed (1=armed) */
+    uint8_t  mode;          /* robot_mode_t: 0=RC_MANUAL,1=ASSISTED,2=AUTONOMOUS */
+    uint8_t  estop;         /* EstopSource value */
+    uint8_t  soc_pct;       /* state-of-charge 0-100, 255=unknown */
+    uint8_t  fw_major;
+    uint8_t  fw_minor;
+    uint8_t  fw_patch;
+} jlink_tlm_status_t;  /* 20 bytes */
+
+/* ---- Volatile state (read from main loop) ---- */
+typedef struct {
+    /* Drive command — updated on JLINK_CMD_DRIVE */
+    volatile int16_t  speed;        /* -1000..+1000 */
+    volatile int16_t  steer;        /* -1000..+1000 */
+
+    /* Heartbeat timer — updated on any valid frame */
+    volatile uint32_t last_rx_ms;   /* HAL_GetTick() of last valid frame; 0=none */
+
+    /* One-shot request flags — set by parser, cleared by main loop */
+    volatile uint8_t  arm_req;
+    volatile uint8_t  disarm_req;
+    volatile uint8_t  estop_req;
+
+    /* PID update — set by parser, cleared by main loop */
+    volatile uint8_t  pid_updated;
+    volatile float    pid_kp;
+    volatile float    pid_ki;
+    volatile float    pid_kd;
+} JLinkState;
+
+extern volatile JLinkState jlink_state;
+
+/* ---- API ---- */
+
+/*
+ * jlink_init() — configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
+ * DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt.
+ * Call once before safety_init().
+ */
+void jlink_init(void);
+
+/*
+ * jlink_is_active(now_ms) — returns true if a valid frame arrived within
+ * JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received.
+ */
+bool jlink_is_active(uint32_t now_ms);
+
+/*
+ * jlink_send_telemetry(status) — build and transmit a JLINK_TLM_STATUS frame
+ * over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ.
+ */
+void jlink_send_telemetry(const jlink_tlm_status_t *status);
+
+/*
+ * jlink_process() — drain DMA circular buffer and parse frames.
+ * Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending).
+ */
+void jlink_process(void);
+
+#endif /* JLINK_H */

jetson/config/rtabmap_params.yaml

@@ -76,3 +76,35 @@ rtabmap:
     # Publish 3D point cloud map
     cloud_output_voxelized: "true"
     cloud_voxel_size:       "0.05"      # match Grid/CellSize
+
+    # ── Persistence + Multi-session (Issue #123) ───────────────────────────
+    # Database path — persistent storage on NVMe.
+    # Do NOT use --delete_db_on_start; fresh_start:=true at launch clears it.
+    database_path:            "/mnt/nvme/saltybot/maps/current.db"
+
+    # On startup, load ALL previously mapped nodes into working memory.
+    # This enables relocalization: RTAB-Map matches incoming frames against
+    # the full prior map before adding new keyframes.
+    Mem/InitWMWithAllNodes:   "true"
+
+    # Append new data to the existing map across sessions (multi-session).
+    # RTAB-Map will attempt loop closure against prior-session keyframes.
+    Rtabmap/StartNewMapOnLoopClosure: "false"
+
+    # ── Memory management ─────────────────────────────────────────────────
+    # Prune near-duplicate keyframes (rehearsal): merge if similarity > 0.6.
+    # Reduces DB size without losing map coverage.
+    Mem/RehearsalSimilarity:  "0.6"
+
+    # Maximum nodes retrieved from long-term memory per iteration.
+    # Limits CPU/RAM during large-map loop closure searches.
+    Rtabmap/MaxRetrieved:     "50"
+
+    # Transfer nodes from short-term to long-term memory when STM > this limit.
+    # 0 = unlimited STM; set a limit to bound RAM use on very long sessions.
+    # At ~1 keyframe/5cm, 5000 nodes ≈ 250m of travel before LTM transfer begins.
+    Mem/STMSize:              "0"        # keep unlimited for Orin 8GB RAM
+
+    # ── Keyframe pruning ──────────────────────────────────────────────────
+    # Remove keyframes that have been rehearsed and are no longer needed.
+    Mem/RehearsedNodesKept:   "false"   # discard rehearsed nodes to save space

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

@@ -0,0 +1,30 @@
+# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
+# Binary-framed Jetson↔STM32 bridge at 921600 baud.
+
+# ── Serial port ────────────────────────────────────────────────────────────────
+# Use /dev/stm32-bridge if the udev rule is applied:
+#   SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
+#   SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
+serial_port:      /dev/ttyACM0
+baud_rate:        921600
+reconnect_delay:  2.0        # seconds between USB reconnect attempts
+
+# ── Heartbeat ─────────────────────────────────────────────────────────────────
+# HEARTBEAT frame sent every heartbeat_period seconds.
+# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
+heartbeat_period: 0.2        # 200ms → well within 500ms STM32 watchdog
+
+# ── Watchdog (Jetson-side) ────────────────────────────────────────────────────
+# If no /cmd_vel message received for watchdog_timeout seconds,
+# send SPEED_STEER(0,0) to stop the robot.
+watchdog_timeout: 0.5        # 500ms
+
+# ── Twist velocity scaling ────────────────────────────────────────────────────
+# speed = clamp(linear.x  * speed_scale, -1000, 1000)  (m/s → ESC units)
+# steer = clamp(angular.z * steer_scale, -1000, 1000)  (rad/s → ESC units)
+#
+# Default: 1 m/s → 1000 ESC units, ±2 rad/s → ±1000 steer.
+# Negative steer_scale flips ROS2 CCW+ convention to match ESC steer direction.
+# Tune speed_scale to set the physical top speed.
+speed_scale:  1000.0
+steer_scale:  -500.0

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

@@ -0,0 +1,52 @@
+"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119).
+
+Usage:
+  # Default (binary protocol, bidirectional):
+  ros2 launch saltybot_bridge stm32_cmd.launch.py
+
+  # Override serial port:
+  ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1
+
+  # Custom velocity scales:
+  ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0
+"""
+
+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() -> LaunchDescription:
+    pkg = get_package_share_directory("saltybot_bridge")
+    params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument("serial_port",      default_value="/dev/ttyACM0"),
+        DeclareLaunchArgument("baud_rate",         default_value="921600"),
+        DeclareLaunchArgument("speed_scale",       default_value="1000.0"),
+        DeclareLaunchArgument("steer_scale",       default_value="-500.0"),
+        DeclareLaunchArgument("watchdog_timeout",  default_value="0.5"),
+        DeclareLaunchArgument("heartbeat_period",  default_value="0.2"),
+
+        Node(
+            package="saltybot_bridge",
+            executable="stm32_cmd_node",
+            name="stm32_cmd_node",
+            output="screen",
+            emulate_tty=True,
+            parameters=[
+                params_file,
+                {
+                    "serial_port":      LaunchConfiguration("serial_port"),
+                    "baud_rate":        LaunchConfiguration("baud_rate"),
+                    "speed_scale":      LaunchConfiguration("speed_scale"),
+                    "steer_scale":      LaunchConfiguration("steer_scale"),
+                    "watchdog_timeout": LaunchConfiguration("watchdog_timeout"),
+                    "heartbeat_period": LaunchConfiguration("heartbeat_period"),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_bridge/package.xml

@@ -5,8 +5,11 @@
   <version>0.1.0</version>
   <description>
     STM32F722 USB CDC serial bridge for saltybot.
-    Reads JSON telemetry from the STM32 flight controller and publishes
-    sensor_msgs/Imu, std_msgs/String (balance state), and diagnostic_msgs/DiagnosticArray.
+    serial_bridge_node: JSON telemetry RX → sensor_msgs/Imu + diagnostics.
+    stm32_cmd_node (Issue #119): binary-framed protocol — STX/TYPE/LEN/CRC16/ETX,
+      commands: HEARTBEAT, SPEED_STEER, ARM, SET_MODE, PID_UPDATE;
+      telemetry: IMU, BATTERY, MOTOR_RPM, ARM_STATE, ERROR; watchdog 500ms.
+    battery_node (Issue #125): SoC tracking, threshold alerts, SQLite history.
   </description>
   <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
   <license>MIT</license>

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py

@@ -0,0 +1,483 @@
+"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge.
+
+Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary
+framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
+
+TX commands (Jetson → STM32):
+  SPEED_STEER  — 50 Hz from /cmd_vel subscription
+  HEARTBEAT    — 200 ms timer (STM32 watchdog fires at 500 ms)
+  ARM          — via /saltybot/arm service
+  SET_MODE     — via /saltybot/set_mode service
+  PID_UPDATE   — via /saltybot/pid_update topic
+
+Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning.
+
+RX telemetry (STM32 → Jetson):
+  IMU          → /saltybot/imu           (sensor_msgs/Imu)
+  BATTERY      → /saltybot/telemetry/battery (std_msgs/String JSON)
+  MOTOR_RPM    → /saltybot/telemetry/motor_rpm (std_msgs/String JSON)
+  ARM_STATE    → /saltybot/arm_state     (std_msgs/String JSON)
+  ERROR        → /saltybot/error         (std_msgs/String JSON)
+  All frames   → /diagnostics            (diagnostic_msgs/DiagnosticArray)
+
+Auto-reconnect: USB disconnect is detected when serial.read() raises; node
+continuously retries at reconnect_delay interval.
+
+This node owns /dev/ttyACM0 exclusively — do NOT run alongside
+serial_bridge_node or saltybot_cmd_node on the same port.
+
+Parameters (config/stm32_cmd_params.yaml):
+  serial_port       /dev/ttyACM0
+  baud_rate         921600
+  reconnect_delay   2.0   (seconds)
+  heartbeat_period  0.2   (seconds)
+  watchdog_timeout  0.5   (seconds — no /cmd_vel → send zero-speed)
+  speed_scale       1000.0 (linear.x m/s → ESC units)
+  steer_scale      -500.0  (angular.z rad/s → ESC units, neg to flip convention)
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+
+import serial
+
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Imu
+from std_msgs.msg import String
+from std_srvs.srv import SetBool, Trigger
+
+from .stm32_protocol import (
+    FrameParser,
+    ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
+    encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
+    encode_pid_update,
+)
+
+# ── Constants ─────────────────────────────────────────────────────────────────
+
+IMU_FRAME_ID = "imu_link"
+_ARM_LABEL   = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
+
+
+def _clamp(v: float, lo: float, hi: float) -> float:
+    return max(lo, min(hi, v))
+
+
+# ── Node ──────────────────────────────────────────────────────────────────────
+
+class Stm32CmdNode(Node):
+    """Binary-framed Jetson↔STM32 bridge node."""
+
+    def __init__(self) -> None:
+        super().__init__("stm32_cmd_node")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",      "/dev/ttyACM0")
+        self.declare_parameter("baud_rate",        921600)
+        self.declare_parameter("reconnect_delay",  2.0)
+        self.declare_parameter("heartbeat_period", 0.2)
+        self.declare_parameter("watchdog_timeout", 0.5)
+        self.declare_parameter("speed_scale",      1000.0)
+        self.declare_parameter("steer_scale",      -500.0)
+
+        port                  = self.get_parameter("serial_port").value
+        baud                  = self.get_parameter("baud_rate").value
+        self._reconnect_delay = self.get_parameter("reconnect_delay").value
+        self._hb_period       = self.get_parameter("heartbeat_period").value
+        self._wd_timeout      = self.get_parameter("watchdog_timeout").value
+        self._speed_scale     = self.get_parameter("speed_scale").value
+        self._steer_scale     = self.get_parameter("steer_scale").value
+
+        # ── QoS ───────────────────────────────────────────────────────────────
+        sensor_qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST, depth=10,
+        )
+        rel_qos = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST, depth=10,
+        )
+
+        # ── Publishers ────────────────────────────────────────────────────────
+        self._imu_pub      = self.create_publisher(Imu,    "/saltybot/imu",           sensor_qos)
+        self._arm_pub      = self.create_publisher(String, "/saltybot/arm_state",      rel_qos)
+        self._error_pub    = self.create_publisher(String, "/saltybot/error",          rel_qos)
+        self._battery_pub  = self.create_publisher(String, "/saltybot/telemetry/battery",  rel_qos)
+        self._rpm_pub      = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos)
+        self._diag_pub     = self.create_publisher(DiagnosticArray, "/diagnostics",    rel_qos)
+
+        # ── Subscribers ───────────────────────────────────────────────────────
+        self._cmd_vel_sub = self.create_subscription(
+            Twist, "/cmd_vel", self._on_cmd_vel, rel_qos,
+        )
+        self._pid_sub = self.create_subscription(
+            String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
+        )
+
+        # ── Services ──────────────────────────────────────────────────────────
+        self._arm_srv  = self.create_service(SetBool, "/saltybot/arm",      self._svc_arm)
+        self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode)
+
+        # ── Serial state ──────────────────────────────────────────────────────
+        self._port_name = port
+        self._baud      = baud
+        self._ser: serial.Serial | None = None
+        self._ser_lock  = threading.Lock()
+        self._parser    = FrameParser()
+
+        # ── TX state ──────────────────────────────────────────────────────────
+        self._last_speed    = 0
+        self._last_steer    = 0
+        self._last_cmd_t    = time.monotonic()
+        self._watchdog_sent = False     # tracks whether we already sent zero
+
+        # ── Diagnostics state ──────────────────────────────────────────────────
+        self._last_arm_state   = -1
+        self._last_battery_mv  = 0
+        self._rx_frame_count   = 0
+
+        # ── Open serial and start timers ──────────────────────────────────────
+        self._open_serial()
+
+        # Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean)
+        self._read_timer = self.create_timer(0.005, self._read_cb)
+        # Heartbeat TX
+        self._hb_timer   = self.create_timer(self._hb_period, self._heartbeat_cb)
+        # Watchdog check (fires at 2× watchdog_timeout for quick detection)
+        self._wd_timer   = self.create_timer(self._wd_timeout / 2, self._watchdog_cb)
+        # Periodic diagnostics
+        self._diag_timer = self.create_timer(1.0, self._publish_diagnostics)
+
+        self.get_logger().info(
+            f"stm32_cmd_node started — {port} @ {baud} baud | "
+            f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
+        )
+
+    # ── Serial management ─────────────────────────────────────────────────────
+
+    def _open_serial(self) -> bool:
+        with self._ser_lock:
+            try:
+                self._ser = serial.Serial(
+                    port=self._port_name,
+                    baudrate=self._baud,
+                    timeout=0.005,          # non-blocking reads
+                    write_timeout=0.1,
+                )
+                self._ser.reset_input_buffer()
+                self._parser.reset()
+                self.get_logger().info(f"Serial open: {self._port_name}")
+                return True
+            except serial.SerialException as exc:
+                self.get_logger().error(
+                    f"Cannot open {self._port_name}: {exc}",
+                    throttle_duration_sec=self._reconnect_delay,
+                )
+                self._ser = None
+                return False
+
+    def _close_serial(self) -> None:
+        with self._ser_lock:
+            if self._ser and self._ser.is_open:
+                try:
+                    self._ser.close()
+                except Exception:
+                    pass
+            self._ser = None
+
+    def _write(self, data: bytes) -> bool:
+        """Thread-safe serial write. Returns False if port is not open."""
+        with self._ser_lock:
+            if self._ser is None or not self._ser.is_open:
+                return False
+            try:
+                self._ser.write(data)
+                return True
+            except serial.SerialException as exc:
+                self.get_logger().error(f"Serial write error: {exc}")
+                self._ser = None
+                return False
+
+    # ── RX — read callback ────────────────────────────────────────────────────
+
+    def _read_cb(self) -> None:
+        """Read bytes from serial and feed them to the frame parser."""
+        raw: bytes | None = None
+        reconnect_needed  = False
+
+        with self._ser_lock:
+            if self._ser is None or not self._ser.is_open:
+                reconnect_needed = True
+            else:
+                try:
+                    n = self._ser.in_waiting
+                    if n:
+                        raw = self._ser.read(n)
+                except serial.SerialException as exc:
+                    self.get_logger().error(f"Serial read error: {exc}")
+                    self._ser = None
+                    reconnect_needed = True
+
+        if reconnect_needed:
+            self.get_logger().warn(
+                "Serial disconnected — will retry",
+                throttle_duration_sec=self._reconnect_delay,
+            )
+            time.sleep(self._reconnect_delay)
+            self._open_serial()
+            return
+
+        if not raw:
+            return
+
+        for byte in raw:
+            frame = self._parser.feed(byte)
+            if frame is not None:
+                self._rx_frame_count += 1
+                self._dispatch_frame(frame)
+
+    def _dispatch_frame(self, frame) -> None:
+        """Route a decoded frame to the appropriate publisher."""
+        now = self.get_clock().now().to_msg()
+
+        if isinstance(frame, ImuFrame):
+            self._publish_imu(frame, now)
+
+        elif isinstance(frame, BatteryFrame):
+            self._publish_battery(frame, now)
+
+        elif isinstance(frame, MotorRpmFrame):
+            self._publish_motor_rpm(frame, now)
+
+        elif isinstance(frame, ArmStateFrame):
+            self._publish_arm_state(frame, now)
+
+        elif isinstance(frame, ErrorFrame):
+            self._publish_error(frame, now)
+
+        elif isinstance(frame, tuple):
+            type_code, payload = frame
+            self.get_logger().debug(
+                f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}"
+            )
+
+    # ── Telemetry publishers ──────────────────────────────────────────────────
+
+    def _publish_imu(self, frame: ImuFrame, stamp) -> None:
+        msg = Imu()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = IMU_FRAME_ID
+
+        # orientation: unknown — signal with -1 in first covariance
+        msg.orientation_covariance[0] = -1.0
+
+        msg.angular_velocity.x = math.radians(frame.pitch_deg)
+        msg.angular_velocity.y = math.radians(frame.roll_deg)
+        msg.angular_velocity.z = math.radians(frame.yaw_deg)
+        cov = math.radians(0.3) ** 2    # ±0.3° noise estimate from STM32 BMI088
+        msg.angular_velocity_covariance[0] = cov
+        msg.angular_velocity_covariance[4] = cov
+        msg.angular_velocity_covariance[8] = cov
+
+        msg.linear_acceleration.x = frame.accel_x
+        msg.linear_acceleration.y = frame.accel_y
+        msg.linear_acceleration.z = frame.accel_z
+        acov = 0.05 ** 2    # ±0.05 m/s² noise
+        msg.linear_acceleration_covariance[0] = acov
+        msg.linear_acceleration_covariance[4] = acov
+        msg.linear_acceleration_covariance[8] = acov
+
+        self._imu_pub.publish(msg)
+
+    def _publish_battery(self, frame: BatteryFrame, stamp) -> None:
+        payload = {
+            "voltage_v":  round(frame.voltage_mv / 1000.0, 3),
+            "voltage_mv": frame.voltage_mv,
+            "current_ma": frame.current_ma,
+            "soc_pct":    frame.soc_pct,
+            "charging":   frame.current_ma < -100,
+            "ts":         f"{stamp.sec}.{stamp.nanosec:09d}",
+        }
+        self._last_battery_mv = frame.voltage_mv
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._battery_pub.publish(msg)
+
+    def _publish_motor_rpm(self, frame: MotorRpmFrame, stamp) -> None:
+        payload = {
+            "left_rpm":  frame.left_rpm,
+            "right_rpm": frame.right_rpm,
+            "ts":        f"{stamp.sec}.{stamp.nanosec:09d}",
+        }
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._rpm_pub.publish(msg)
+
+    def _publish_arm_state(self, frame: ArmStateFrame, stamp) -> None:
+        label = _ARM_LABEL.get(frame.state, f"UNKNOWN({frame.state})")
+        if frame.state != self._last_arm_state:
+            self.get_logger().info(f"Arm state → {label} (flags=0x{frame.error_flags:02X})")
+            self._last_arm_state = frame.state
+
+        payload = {
+            "state":       frame.state,
+            "state_label": label,
+            "error_flags": frame.error_flags,
+            "ts":          f"{stamp.sec}.{stamp.nanosec:09d}",
+        }
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._arm_pub.publish(msg)
+
+    def _publish_error(self, frame: ErrorFrame, stamp) -> None:
+        self.get_logger().error(
+            f"STM32 error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}"
+        )
+        payload = {
+            "error_code": frame.error_code,
+            "subcode":    frame.subcode,
+            "ts":         f"{stamp.sec}.{stamp.nanosec:09d}",
+        }
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._error_pub.publish(msg)
+
+    # ── TX — command send ─────────────────────────────────────────────────────
+
+    def _on_cmd_vel(self, msg: Twist) -> None:
+        """Convert /cmd_vel Twist to SPEED_STEER frame at up to 50 Hz."""
+        speed = int(_clamp(msg.linear.x  * self._speed_scale, -1000.0, 1000.0))
+        steer = int(_clamp(msg.angular.z * self._steer_scale, -1000.0, 1000.0))
+        self._last_speed    = speed
+        self._last_steer    = steer
+        self._last_cmd_t    = time.monotonic()
+        self._watchdog_sent = False
+
+        frame = encode_speed_steer(speed, steer)
+        if not self._write(frame):
+            self.get_logger().warn(
+                "SPEED_STEER dropped — serial not open",
+                throttle_duration_sec=2.0,
+            )
+
+    def _heartbeat_cb(self) -> None:
+        """Send HEARTBEAT every heartbeat_period (default 200ms)."""
+        self._write(encode_heartbeat())
+
+    def _watchdog_cb(self) -> None:
+        """Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
+        if time.monotonic() - self._last_cmd_t >= self._wd_timeout:
+            if not self._watchdog_sent:
+                self.get_logger().warn(
+                    f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed"
+                )
+                self._watchdog_sent = True
+            self._last_speed = 0
+            self._last_steer = 0
+            self._write(encode_speed_steer(0, 0))
+
+    def _on_pid_update(self, msg: String) -> None:
+        """Parse JSON /saltybot/pid_update and send PID_UPDATE frame."""
+        try:
+            data = json.loads(msg.data)
+            kp = float(data["kp"])
+            ki = float(data["ki"])
+            kd = float(data["kd"])
+        except (ValueError, KeyError, json.JSONDecodeError) as exc:
+            self.get_logger().error(f"Bad PID update JSON: {exc}")
+            return
+        frame = encode_pid_update(kp, ki, kd)
+        if self._write(frame):
+            self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}")
+        else:
+            self.get_logger().warn("PID_UPDATE dropped — serial not open")
+
+    # ── Services ──────────────────────────────────────────────────────────────
+
+    def _svc_arm(self, request: SetBool.Request, response: SetBool.Response):
+        """SetBool(True) = arm, SetBool(False) = disarm."""
+        arm = request.data
+        frame = encode_arm(arm)
+        ok = self._write(frame)
+        response.success = ok
+        response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open"
+        self.get_logger().info(
+            f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}"
+        )
+        return response
+
+    def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response):
+        """SetBool: data maps to mode byte (True=1, False=0)."""
+        mode = 1 if request.data else 0
+        frame = encode_set_mode(mode)
+        ok = self._write(frame)
+        response.success = ok
+        response.message = f"mode={mode}" if ok else "serial not open"
+        return response
+
+    # ── Diagnostics ───────────────────────────────────────────────────────────
+
+    def _publish_diagnostics(self) -> None:
+        diag = DiagnosticArray()
+        diag.header.stamp = self.get_clock().now().to_msg()
+
+        status = DiagnosticStatus()
+        status.name        = "saltybot/stm32_cmd_node"
+        status.hardware_id = "stm32f722"
+
+        port_ok = self._ser is not None and self._ser.is_open
+        if port_ok:
+            status.level   = DiagnosticStatus.OK
+            status.message = "Serial OK"
+        else:
+            status.level   = DiagnosticStatus.ERROR
+            status.message = f"Serial disconnected: {self._port_name}"
+
+        wd_age = time.monotonic() - self._last_cmd_t
+        status.values = [
+            KeyValue(key="serial_port",    value=self._port_name),
+            KeyValue(key="port_open",      value=str(port_ok)),
+            KeyValue(key="rx_frames",      value=str(self._rx_frame_count)),
+            KeyValue(key="rx_errors",      value=str(self._parser.frames_error)),
+            KeyValue(key="last_speed",     value=str(self._last_speed)),
+            KeyValue(key="last_steer",     value=str(self._last_steer)),
+            KeyValue(key="cmd_vel_age_s",  value=f"{wd_age:.2f}"),
+            KeyValue(key="battery_mv",     value=str(self._last_battery_mv)),
+            KeyValue(key="arm_state",      value=_ARM_LABEL.get(self._last_arm_state, "?")),
+        ]
+        diag.status.append(status)
+        self._diag_pub.publish(diag)
+
+    # ── Lifecycle ─────────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        # Send zero-speed + disarm on shutdown
+        self._write(encode_speed_steer(0, 0))
+        self._write(encode_arm(False))
+        self._close_serial()
+        super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = Stm32CmdNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py

@@ -0,0 +1,332 @@
+"""stm32_protocol.py — Binary frame codec for Jetson↔STM32 communication.
+
+Issue #119: defines the binary serial protocol between the Jetson Nano and the
+STM32F722 flight controller over USB CDC @ 921600 baud.
+
+Frame layout (all multi-byte fields are big-endian):
+  ┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐
+  │  STX │ TYPE │  LEN │     PAYLOAD      │  CRC16    │  ETX │
+  │ 0x02 │  1B  │  1B  │    LEN bytes     │   2B BE   │ 0x03 │
+  └──────┴──────┴──────┴──────────────────┴───────────┴──────┘
+
+CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves).
+CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR.
+
+Command types (Jetson → STM32):
+  0x01  HEARTBEAT   — no payload              (len=0)
+  0x02  SPEED_STEER — int16 speed + int16 steer (len=4)  range: -1000..+1000
+  0x03  ARM         — uint8 (0=disarm, 1=arm)  (len=1)
+  0x04  SET_MODE    — uint8 mode               (len=1)
+  0x05  PID_UPDATE  — float32 kp + ki + kd     (len=12)
+
+Telemetry types (STM32 → Jetson):
+  0x10  IMU         — int16×6: pitch,roll,yaw (×100 deg), ax,ay,az (×100 m/s²) (len=12)
+  0x11  BATTERY     — uint16 voltage_mv + int16 current_ma + uint8 soc_pct (len=5)
+  0x12  MOTOR_RPM   — int16 left_rpm + int16 right_rpm     (len=4)
+  0x13  ARM_STATE   — uint8 state + uint8 error_flags       (len=2)
+  0x14  ERROR       — uint8 error_code + uint8 subcode      (len=2)
+
+Usage:
+  # Encoding (Jetson → STM32)
+  frame = encode_speed_steer(300, -150)
+  ser.write(frame)
+
+  # Decoding (STM32 → Jetson), one byte at a time
+  parser = FrameParser()
+  for byte in incoming_bytes:
+      result = parser.feed(byte)
+      if result is not None:
+          handle_frame(result)
+"""
+
+from __future__ import annotations
+
+import struct
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Optional
+
+# ── Frame constants ───────────────────────────────────────────────────────────
+
+STX = 0x02
+ETX = 0x03
+MAX_PAYLOAD_LEN = 64       # hard limit; any frame larger is corrupt
+
+
+# ── Command / telemetry type codes ────────────────────────────────────────────
+
+class CmdType(IntEnum):
+    HEARTBEAT   = 0x01
+    SPEED_STEER = 0x02
+    ARM         = 0x03
+    SET_MODE    = 0x04
+    PID_UPDATE  = 0x05
+
+
+class TelType(IntEnum):
+    IMU       = 0x10
+    BATTERY   = 0x11
+    MOTOR_RPM = 0x12
+    ARM_STATE = 0x13
+    ERROR     = 0x14
+
+
+# ── Parsed telemetry objects ──────────────────────────────────────────────────
+
+@dataclass
+class ImuFrame:
+    pitch_deg: float      # degrees (positive = forward tilt)
+    roll_deg:  float
+    yaw_deg:   float
+    accel_x:   float      # m/s²
+    accel_y:   float
+    accel_z:   float
+
+
+@dataclass
+class BatteryFrame:
+    voltage_mv:  int      # millivolts (e.g. 11100 = 11.1 V)
+    current_ma:  int      # milliamps (negative = charging)
+    soc_pct:     int      # state of charge 0–100 (from STM32 fuel gauge or lookup)
+
+
+@dataclass
+class MotorRpmFrame:
+    left_rpm:   int
+    right_rpm:  int
+
+
+@dataclass
+class ArmStateFrame:
+    state:       int      # 0=DISARMED 1=ARMED 2=TILT_FAULT
+    error_flags: int      # bitmask
+
+
+@dataclass
+class ErrorFrame:
+    error_code: int
+    subcode:    int
+
+
+# Union type for decoded results
+TelemetryFrame = ImuFrame | BatteryFrame | MotorRpmFrame | ArmStateFrame | ErrorFrame
+
+
+# ── CRC16 CCITT ───────────────────────────────────────────────────────────────
+
+def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
+    """CRC16-CCITT: polynomial 0x1021, init 0xFFFF, no final XOR."""
+    crc = init
+    for byte in data:
+        crc ^= byte << 8
+        for _ in range(8):
+            if crc & 0x8000:
+                crc = (crc << 1) ^ 0x1021
+            else:
+                crc <<= 1
+        crc &= 0xFFFF
+    return crc
+
+
+# ── Frame encoder ─────────────────────────────────────────────────────────────
+
+def _build_frame(cmd_type: int, payload: bytes) -> bytes:
+    """Assemble a complete binary frame with CRC16."""
+    assert len(payload) <= MAX_PAYLOAD_LEN, "Payload too large"
+    length = len(payload)
+    header = bytes([cmd_type, length])
+    crc = _crc16_ccitt(header + payload)
+    return bytes([STX, cmd_type, length]) + payload + struct.pack(">H", crc) + bytes([ETX])
+
+
+def encode_heartbeat() -> bytes:
+    """HEARTBEAT frame — no payload."""
+    return _build_frame(CmdType.HEARTBEAT, b"")
+
+
+def encode_speed_steer(speed: int, steer: int) -> bytes:
+    """SPEED_STEER frame — int16 speed + int16 steer, both in -1000..+1000."""
+    speed = max(-1000, min(1000, int(speed)))
+    steer = max(-1000, min(1000, int(steer)))
+    return _build_frame(CmdType.SPEED_STEER, struct.pack(">hh", speed, steer))
+
+
+def encode_arm(arm: bool) -> bytes:
+    """ARM frame — 0=disarm, 1=arm."""
+    return _build_frame(CmdType.ARM, struct.pack("B", 1 if arm else 0))
+
+
+def encode_set_mode(mode: int) -> bytes:
+    """SET_MODE frame — mode byte."""
+    return _build_frame(CmdType.SET_MODE, struct.pack("B", mode & 0xFF))
+
+
+def encode_pid_update(kp: float, ki: float, kd: float) -> bytes:
+    """PID_UPDATE frame — three float32 values."""
+    return _build_frame(CmdType.PID_UPDATE, struct.pack(">fff", kp, ki, kd))
+
+
+# ── Frame decoder (state-machine parser) ─────────────────────────────────────
+
+class ParserState(IntEnum):
+    WAIT_STX  = 0
+    WAIT_TYPE = 1
+    WAIT_LEN  = 2
+    PAYLOAD   = 3
+    CRC_HI    = 4
+    CRC_LO    = 5
+    WAIT_ETX  = 6
+
+
+class ParseError(Exception):
+    pass
+
+
+class FrameParser:
+    """Byte-by-byte streaming parser for STM32 telemetry frames.
+
+    Feed individual bytes via feed(); returns a decoded TelemetryFrame (or raw
+    bytes tuple) when a complete valid frame is received.
+
+    Thread-safety: single-threaded — wrap in a lock if shared across threads.
+
+    Usage::
+        parser = FrameParser()
+        for b in incoming:
+            result = parser.feed(b)
+            if result is not None:
+                process(result)
+    """
+
+    def __init__(self) -> None:
+        self._state    = ParserState.WAIT_STX
+        self._type     = 0
+        self._length   = 0
+        self._payload  = bytearray()
+        self._crc_rcvd = 0
+        self.frames_ok    = 0
+        self.frames_error = 0
+
+    def reset(self) -> None:
+        """Reset parser to initial state (call after error or port reconnect)."""
+        self._state   = ParserState.WAIT_STX
+        self._payload = bytearray()
+
+    def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]:
+        """Process one byte. Returns decoded frame on success, None otherwise.
+
+        On CRC error, increments frames_error and resets. The return value on
+        success is a dataclass (ImuFrame, BatteryFrame, etc.) or a
+        (type_code, raw_payload) tuple for unknown type codes.
+        """
+        s = self._state
+
+        if s == ParserState.WAIT_STX:
+            if byte == STX:
+                self._state = ParserState.WAIT_TYPE
+            return None
+
+        if s == ParserState.WAIT_TYPE:
+            self._type  = byte
+            self._state = ParserState.WAIT_LEN
+            return None
+
+        if s == ParserState.WAIT_LEN:
+            self._length  = byte
+            self._payload = bytearray()
+            if self._length > MAX_PAYLOAD_LEN:
+                # Corrupt frame — too big; reset
+                self.frames_error += 1
+                self.reset()
+                return None
+            if self._length == 0:
+                self._state = ParserState.CRC_HI
+            else:
+                self._state = ParserState.PAYLOAD
+            return None
+
+        if s == ParserState.PAYLOAD:
+            self._payload.append(byte)
+            if len(self._payload) == self._length:
+                self._state = ParserState.CRC_HI
+            return None
+
+        if s == ParserState.CRC_HI:
+            self._crc_rcvd = byte << 8
+            self._state    = ParserState.CRC_LO
+            return None
+
+        if s == ParserState.CRC_LO:
+            self._crc_rcvd |= byte
+            self._state     = ParserState.WAIT_ETX
+            return None
+
+        if s == ParserState.WAIT_ETX:
+            self.reset()   # always reset so we look for next STX
+            if byte != ETX:
+                self.frames_error += 1
+                return None
+
+            # Verify CRC
+            crc_data = bytes([self._type, self._length]) + self._payload
+            expected = _crc16_ccitt(crc_data)
+            if expected != self._crc_rcvd:
+                self.frames_error += 1
+                return None
+
+            # Decode
+            self.frames_ok += 1
+            return _decode_telemetry(self._type, bytes(self._payload))
+
+        # Should never reach here
+        self.reset()
+        return None
+
+
+# ── Telemetry decoder ─────────────────────────────────────────────────────────
+
+def _decode_telemetry(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]:
+    """Decode a validated telemetry payload into a typed dataclass."""
+    try:
+        if type_code == TelType.IMU:
+            if len(payload) < 12:
+                return None
+            p, r, y, ax, ay, az = struct.unpack_from(">hhhhhh", payload)
+            return ImuFrame(
+                pitch_deg=p  / 100.0,
+                roll_deg=r   / 100.0,
+                yaw_deg=y    / 100.0,
+                accel_x=ax   / 100.0,
+                accel_y=ay   / 100.0,
+                accel_z=az   / 100.0,
+            )
+
+        if type_code == TelType.BATTERY:
+            if len(payload) < 5:
+                return None
+            v_mv, i_ma, soc = struct.unpack_from(">HhB", payload)
+            return BatteryFrame(voltage_mv=v_mv, current_ma=i_ma, soc_pct=soc)
+
+        if type_code == TelType.MOTOR_RPM:
+            if len(payload) < 4:
+                return None
+            left, right = struct.unpack_from(">hh", payload)
+            return MotorRpmFrame(left_rpm=left, right_rpm=right)
+
+        if type_code == TelType.ARM_STATE:
+            if len(payload) < 2:
+                return None
+            state, flags = struct.unpack_from("BB", payload)
+            return ArmStateFrame(state=state, error_flags=flags)
+
+        if type_code == TelType.ERROR:
+            if len(payload) < 2:
+                return None
+            code, sub = struct.unpack_from("BB", payload)
+            return ErrorFrame(error_code=code, subcode=sub)
+
+    except struct.error:
+        return None
+
+    # Unknown telemetry type — return raw
+    return (type_code, payload)

jetson/ros2_ws/src/saltybot_bridge/setup.py

@@ -13,12 +13,14 @@ setup(
             "launch/bridge.launch.py",
             "launch/cmd_vel_bridge.launch.py",
             "launch/remote_estop.launch.py",
+            "launch/stm32_cmd.launch.py",
             "launch/battery.launch.py",
         ]),
         (f"share/{package_name}/config", [
             "config/bridge_params.yaml",
             "config/cmd_vel_bridge_params.yaml",
             "config/estop_params.yaml",
+            "config/stm32_cmd_params.yaml",
             "config/battery_params.yaml",
         ]),
     ],
@@ -38,7 +40,9 @@ setup(
             # Nav2 cmd_vel bridge: velocity limits + ramp + deadman + mode gate
             "cmd_vel_bridge_node = saltybot_bridge.cmd_vel_bridge_node:main",
             "remote_estop_node  = saltybot_bridge.remote_estop_node:main",
-            # Battery management: SoC, alerts, speed limits, SQLite history
+            # Binary-framed STM32 command node (Issue #119)
+            "stm32_cmd_node    = saltybot_bridge.stm32_cmd_node:main",
+            # Battery management node (Issue #125)
             "battery_node      = saltybot_bridge.battery_node:main",
         ],
     },

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

@@ -0,0 +1,341 @@
+"""test_stm32_cmd_node.py — Unit tests for Stm32CmdNode with mock serial port.
+
+Tests:
+  - Serial open/close lifecycle
+  - Twist→SPEED_STEER frame encoding and clamping
+  - Heartbeat cadence (mock timer)
+  - Watchdog fires after 500ms silence and sends zero-speed
+  - ARM service request → ARM frame on serial
+  - PID_UPDATE topic → PID_UPDATE frame on serial
+  - Telemetry receive path: mock serial → publisher callbacks
+  - Auto-reconnect on serial disconnect
+  - Zero-speed sent on node shutdown
+  - CRC errors counted correctly
+
+Run with: pytest test/test_stm32_cmd_node.py -v
+No ROS2 runtime required — uses mock Node infrastructure.
+"""
+
+from __future__ import annotations
+
+import io
+import struct
+import threading
+import time
+import sys
+import os
+from unittest.mock import MagicMock, patch, call
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+
+from saltybot_bridge.stm32_protocol import (
+    STX, ETX, CmdType, TelType,
+    encode_speed_steer, encode_heartbeat, encode_arm, encode_pid_update,
+    _build_frame, _crc16_ccitt,
+    ImuFrame, BatteryFrame, ArmStateFrame, MotorRpmFrame, ErrorFrame,
+)
+
+
+# ── Mock serial ───────────────────────────────────────────────────────────────
+
+class MockSerial:
+    """In-memory serial port mock for testing TX/RX paths without hardware."""
+
+    def __init__(self, rx_data: bytes = b""):
+        self._tx_buf  = io.BytesIO()
+        self._rx_buf  = io.BytesIO(rx_data)
+        self.is_open  = True
+        self._lock    = threading.Lock()
+        self.write_calls  = 0
+        self.raise_on_write: Exception | None = None
+
+    def write(self, data: bytes) -> int:
+        if self.raise_on_write:
+            raise self.raise_on_write
+        with self._lock:
+            self._tx_buf.write(data)
+            self.write_calls += 1
+        return len(data)
+
+    @property
+    def in_waiting(self) -> int:
+        pos = self._rx_buf.tell()
+        end = self._rx_buf.seek(0, 2)
+        self._rx_buf.seek(pos)
+        return end - pos
+
+    def read(self, n: int) -> bytes:
+        return self._rx_buf.read(n)
+
+    def reset_input_buffer(self) -> None:
+        pass
+
+    def close(self) -> None:
+        self.is_open = False
+
+    def get_written(self) -> bytes:
+        with self._lock:
+            return self._tx_buf.getvalue()
+
+    def clear_written(self) -> None:
+        with self._lock:
+            self._tx_buf = io.BytesIO()
+
+
+# ── Frame builder helpers ─────────────────────────────────────────────────────
+
+def _imu_frame_bytes(pitch=100, roll=0, yaw=0, ax=0, ay=0, az=981) -> bytes:
+    payload = struct.pack(">hhhhhh", pitch, roll, yaw, ax, ay, az)
+    return _build_frame(TelType.IMU, payload)
+
+
+def _battery_frame_bytes(v_mv=11100, i_ma=500, soc=80) -> bytes:
+    payload = struct.pack(">HhB", v_mv, i_ma, soc)
+    return _build_frame(TelType.BATTERY, payload)
+
+
+def _arm_state_frame_bytes(state=1, flags=0) -> bytes:
+    payload = struct.pack("BB", state, flags)
+    return _build_frame(TelType.ARM_STATE, payload)
+
+
+def _motor_rpm_frame_bytes(left=1200, right=1200) -> bytes:
+    payload = struct.pack(">hh", left, right)
+    return _build_frame(TelType.MOTOR_RPM, payload)
+
+
+def _error_frame_bytes(code=0x01, sub=0x00) -> bytes:
+    payload = struct.pack("BB", code, sub)
+    return _build_frame(TelType.ERROR, payload)
+
+
+# ═══════════════════════════════════════════════════════════════════
+# Protocol logic tests (no ROS2)
+# ═══════════════════════════════════════════════════════════════════
+
+class TestCommandEncoding:
+    """Verify the TX path logic without spinning up ROS2."""
+
+    def test_zero_twist_sends_zero_frame(self):
+        frame = encode_speed_steer(0, 0)
+        speed, steer = struct.unpack(">hh", frame[3:7])
+        assert speed == 0
+        assert steer == 0
+
+    def test_forward_1ms_scales_to_1000(self):
+        """linear.x = 1.0 m/s with speed_scale=1000 → speed=1000."""
+        speed_scale = 1000.0
+        speed = int(max(-1000, min(1000, 1.0 * speed_scale)))
+        frame = encode_speed_steer(speed, 0)
+        s, _ = struct.unpack(">hh", frame[3:7])
+        assert s == 1000
+
+    def test_left_turn_scales_correctly(self):
+        """angular.z = 1.0 with steer_scale=-500 → steer=-500."""
+        steer_scale = -500.0
+        steer = int(max(-1000, min(1000, 1.0 * steer_scale)))
+        frame = encode_speed_steer(0, steer)
+        _, st = struct.unpack(">hh", frame[3:7])
+        assert st == -500
+
+    def test_heartbeat_frame_is_minimal(self):
+        frame = encode_heartbeat()
+        assert frame[1] == CmdType.HEARTBEAT
+        assert frame[2] == 0    # zero payload
+
+    def test_arm_frame_payload(self):
+        frame_arm   = encode_arm(True)
+        frame_disarm = encode_arm(False)
+        assert frame_arm[3]   == 1
+        assert frame_disarm[3] == 0
+
+    def test_pid_frame_all_zeros(self):
+        frame = encode_pid_update(0.0, 0.0, 0.0)
+        kp, ki, kd = struct.unpack(">fff", frame[3:15])
+        assert kp == pytest.approx(0.0)
+        assert ki == pytest.approx(0.0)
+        assert kd == pytest.approx(0.0)
+
+    def test_pid_frame_typical_values(self):
+        frame = encode_pid_update(3.0, 0.02, 0.12)
+        kp, ki, kd = struct.unpack(">fff", frame[3:15])
+        assert kp == pytest.approx(3.0, rel=1e-5)
+        assert ki == pytest.approx(0.02, rel=1e-5)
+        assert kd == pytest.approx(0.12, rel=1e-5)
+
+
+class TestMockSerialTX:
+    """Test TX path using MockSerial — verifies bytes sent over the wire."""
+
+    def _write_and_get(self, frames: list[bytes]) -> bytes:
+        """Write multiple frames to a MockSerial and return all written bytes."""
+        ms = MockSerial()
+        for f in frames:
+            ms.write(f)
+        return ms.get_written()
+
+    def test_speed_steer_bytes_on_wire(self):
+        frame = encode_speed_steer(300, -150)
+        ms = MockSerial()
+        ms.write(frame)
+        written = ms.get_written()
+        assert written[0] == STX
+        assert written[1] == CmdType.SPEED_STEER
+        assert written[-1] == ETX
+
+    def test_heartbeat_bytes_on_wire(self):
+        frame = encode_heartbeat()
+        ms = MockSerial()
+        ms.write(frame)
+        written = ms.get_written()
+        assert written[0] == STX
+        assert written[1] == CmdType.HEARTBEAT
+
+    def test_write_error_detected(self):
+        import serial
+        ms = MockSerial()
+        ms.raise_on_write = serial.SerialException("test error")
+        with pytest.raises(serial.SerialException):
+            ms.write(b"\x00")
+
+    def test_multiple_frames_written_sequentially(self):
+        ms = MockSerial()
+        ms.write(encode_heartbeat())
+        ms.write(encode_speed_steer(100, 0))
+        ms.write(encode_arm(True))
+        written = ms.get_written()
+        # All three frames should be in the buffer
+        assert written.count(STX) == 3
+        assert written.count(ETX) == 3
+
+    def test_write_call_count_tracked(self):
+        ms = MockSerial()
+        ms.write(encode_heartbeat())
+        ms.write(encode_heartbeat())
+        assert ms.write_calls == 2
+
+
+class TestMockSerialRX:
+    """Test RX parsing path using MockSerial with pre-loaded telemetry data."""
+
+    from saltybot_bridge.stm32_protocol import FrameParser
+
+    def test_rx_imu_frame(self):
+        from saltybot_bridge.stm32_protocol import FrameParser, ImuFrame
+        raw = _imu_frame_bytes(pitch=500, roll=-200, yaw=100, ax=0, ay=0, az=981)
+        ms  = MockSerial(rx_data=raw)
+        parser = FrameParser()
+        results = []
+        while ms.in_waiting:
+            chunk = ms.read(ms.in_waiting)
+            for b in chunk:
+                r = parser.feed(b)
+                if r is not None:
+                    results.append(r)
+        assert len(results) == 1
+        f = results[0]
+        assert isinstance(f, ImuFrame)
+        assert f.pitch_deg == pytest.approx(5.0)
+        assert f.roll_deg  == pytest.approx(-2.0)
+        assert f.accel_z   == pytest.approx(9.81)
+
+    def test_rx_battery_frame(self):
+        from saltybot_bridge.stm32_protocol import FrameParser, BatteryFrame
+        raw = _battery_frame_bytes(v_mv=10500, i_ma=1200, soc=45)
+        ms  = MockSerial(rx_data=raw)
+        parser = FrameParser()
+        results = []
+        while ms.in_waiting:
+            for b in ms.read(ms.in_waiting):
+                r = parser.feed(b)
+                if r is not None:
+                    results.append(r)
+        f = results[0]
+        assert isinstance(f, BatteryFrame)
+        assert f.voltage_mv == 10500
+        assert f.soc_pct    == 45
+
+    def test_rx_multiple_frames_in_one_read(self):
+        from saltybot_bridge.stm32_protocol import FrameParser
+        raw = (_imu_frame_bytes() + _arm_state_frame_bytes() + _battery_frame_bytes())
+        ms  = MockSerial(rx_data=raw)
+        parser = FrameParser()
+        results = []
+        data = ms.read(len(raw))
+        for b in data:
+            r = parser.feed(b)
+            if r is not None:
+                results.append(r)
+        assert len(results) == 3
+        assert parser.frames_error == 0
+
+    def test_rx_bad_crc_counted_as_error(self):
+        from saltybot_bridge.stm32_protocol import FrameParser
+        raw = bytearray(_arm_state_frame_bytes(state=1))
+        raw[-3] ^= 0xFF    # corrupt CRC
+        ms = MockSerial(rx_data=bytes(raw))
+        parser = FrameParser()
+        for b in ms.read(len(raw)):
+            parser.feed(b)
+        assert parser.frames_ok    == 0
+        assert parser.frames_error == 1
+
+    def test_rx_resync_after_corrupt_byte(self):
+        from saltybot_bridge.stm32_protocol import FrameParser, ArmStateFrame
+        garbage = b"\xDE\xAD\x00\x00"
+        valid   = _arm_state_frame_bytes(state=1)
+        ms = MockSerial(rx_data=garbage + valid)
+        parser = FrameParser()
+        results = []
+        while ms.in_waiting:
+            for b in ms.read(ms.in_waiting):
+                r = parser.feed(b)
+                if r is not None:
+                    results.append(r)
+        assert len(results) == 1
+        assert isinstance(results[0], ArmStateFrame)
+
+
+# ═══════════════════════════════════════════════════════════════════
+# Watchdog logic tests
+# ═══════════════════════════════════════════════════════════════════
+
+class TestWatchdog:
+    """Test the watchdog logic in isolation without a full ROS2 node."""
+
+    def test_watchdog_fires_after_timeout(self):
+        """Simulate watchdog: record time, wait >500ms, verify zero-speed sent."""
+        ms = MockSerial()
+        last_cmd_t = time.monotonic() - 0.6    # 600ms ago
+        wd_timeout = 0.5
+
+        # Watchdog check logic (mirrors _watchdog_cb)
+        if time.monotonic() - last_cmd_t >= wd_timeout:
+            ms.write(encode_speed_steer(0, 0))
+
+        written = ms.get_written()
+        speed, steer = struct.unpack(">hh", written[3:7])
+        assert speed == 0
+        assert steer == 0
+
+    def test_watchdog_does_not_fire_before_timeout(self):
+        """Watchdog must not fire if /cmd_vel was recent."""
+        ms = MockSerial()
+        last_cmd_t = time.monotonic() - 0.2    # 200ms ago — within timeout
+        wd_timeout = 0.5
+
+        if time.monotonic() - last_cmd_t >= wd_timeout:
+            ms.write(encode_speed_steer(0, 0))
+
+        assert ms.get_written() == b""
+
+    def test_zero_speed_frame_structure(self):
+        """Zero-speed frame must have correct structure."""
+        frame = encode_speed_steer(0, 0)
+        assert frame[0] == STX
+        assert frame[1] == CmdType.SPEED_STEER
+        assert frame[-1] == ETX
+        speed, steer = struct.unpack(">hh", frame[3:7])
+        assert speed == 0
+        assert steer == 0

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

@@ -0,0 +1,483 @@
+"""test_stm32_protocol.py — Unit tests for binary STM32 frame codec.
+
+Tests:
+  - CRC16-CCITT correctness
+  - All encoder functions produce correctly structured frames
+  - FrameParser decodes valid frames for all telemetry types
+  - FrameParser rejects frames with bad CRC
+  - FrameParser rejects frames with bad ETX
+  - FrameParser resyncs after corrupt data (finds next STX)
+  - FrameParser handles streaming byte-by-byte input
+  - FrameParser handles oversized payload gracefully
+  - Speed/steer clamping in encode_speed_steer
+  - Round-trip encode → decode for all known telemetry types
+
+Run with: pytest test/test_stm32_protocol.py -v
+"""
+
+from __future__ import annotations
+
+import struct
+import pytest
+import sys
+import os
+
+# ── Path setup (no ROS2 install needed) ──────────────────────────────────────
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+
+from saltybot_bridge.stm32_protocol import (
+    STX, ETX,
+    CmdType, TelType,
+    ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
+    FrameParser,
+    _crc16_ccitt, _build_frame, _decode_telemetry,
+    encode_heartbeat, encode_speed_steer, encode_arm,
+    encode_set_mode, encode_pid_update,
+)
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _parse_all(raw: bytes):
+    """Feed all bytes into a fresh FrameParser, return list of decoded frames."""
+    parser = FrameParser()
+    results = []
+    for b in raw:
+        r = parser.feed(b)
+        if r is not None:
+            results.append(r)
+    return results, parser
+
+
+def _build_telem_frame(type_code: int, payload: bytes) -> bytes:
+    """Build a raw telemetry frame (same framing as command encoder)."""
+    return _build_frame(type_code, payload)
+
+
+# ═══════════════════════════════════════════════════════════════════
+# CRC16 tests
+# ═══════════════════════════════════════════════════════════════════
+
+class TestCrc16:
+    def test_known_vector_empty(self):
+        """CRC of empty data must equal 0xFFFF (init value)."""
+        assert _crc16_ccitt(b"") == 0xFFFF
+
+    def test_known_vector_123456789(self):
+        """Classic CCITT-16 test vector: '123456789' → 0x29B1."""
+        assert _crc16_ccitt(b"123456789") == 0x29B1
+
+    def test_crc_changes_on_bit_flip(self):
+        data  = b"\x02\x04\x42"
+        flipped = b"\x02\x04\x43"
+        assert _crc16_ccitt(data) != _crc16_ccitt(flipped)
+
+    def test_crc_all_zeros(self):
+        crc = _crc16_ccitt(bytes(16))
+        assert isinstance(crc, int)
+        assert 0 <= crc <= 0xFFFF
+
+    def test_crc_single_byte(self):
+        c1 = _crc16_ccitt(b"\x01")
+        c2 = _crc16_ccitt(b"\x02")
+        assert c1 != c2
+
+
+# ═══════════════════════════════════════════════════════════════════
+# Encoder tests
+# ═══════════════════════════════════════════════════════════════════
+
+class TestEncoders:
+    def test_heartbeat_structure(self):
+        frame = encode_heartbeat()
+        # STX TYPE LEN CRC_HI CRC_LO ETX  (no payload → 6 bytes)
+        assert len(frame) == 6
+        assert frame[0] == STX
+        assert frame[1] == CmdType.HEARTBEAT
+        assert frame[2] == 0          # LEN=0
+        assert frame[-1] == ETX
+
+    def test_heartbeat_crc_valid(self):
+        frame = encode_heartbeat()
+        crc_expected = _crc16_ccitt(bytes([CmdType.HEARTBEAT, 0]))
+        crc_actual   = struct.unpack(">H", frame[3:5])[0]
+        assert crc_actual == crc_expected
+
+    def test_speed_steer_structure(self):
+        frame = encode_speed_steer(300, -150)
+        assert frame[0] == STX
+        assert frame[1] == CmdType.SPEED_STEER
+        assert frame[2] == 4           # LEN=4 (two int16)
+        assert frame[-1] == ETX
+        assert len(frame) == 10        # 3 header + 4 payload + 2 CRC + 1 ETX
+
+    def test_speed_steer_payload_values(self):
+        frame = encode_speed_steer(500, -200)
+        speed, steer = struct.unpack(">hh", frame[3:7])
+        assert speed == 500
+        assert steer == -200
+
+    def test_speed_steer_clamped_max(self):
+        frame = encode_speed_steer(9999, 9999)
+        speed, steer = struct.unpack(">hh", frame[3:7])
+        assert speed == 1000
+        assert steer == 1000
+
+    def test_speed_steer_clamped_min(self):
+        frame = encode_speed_steer(-9999, -9999)
+        speed, steer = struct.unpack(">hh", frame[3:7])
+        assert speed == -1000
+        assert steer == -1000
+
+    def test_speed_steer_zero(self):
+        frame = encode_speed_steer(0, 0)
+        speed, steer = struct.unpack(">hh", frame[3:7])
+        assert speed == 0
+        assert steer == 0
+
+    def test_arm_true(self):
+        frame = encode_arm(True)
+        assert frame[1] == CmdType.ARM
+        assert frame[2] == 1           # LEN=1
+        assert frame[3] == 1           # payload = 1 (arm)
+
+    def test_arm_false(self):
+        frame = encode_arm(False)
+        assert frame[3] == 0           # payload = 0 (disarm)
+
+    def test_set_mode(self):
+        frame = encode_set_mode(3)
+        assert frame[1] == CmdType.SET_MODE
+        assert frame[3] == 3
+
+    def test_set_mode_masked(self):
+        frame = encode_set_mode(0x1FF)
+        assert frame[3] == 0xFF        # masked to uint8
+
+    def test_pid_update_structure(self):
+        frame = encode_pid_update(1.5, 0.01, 0.1)
+        assert frame[1] == CmdType.PID_UPDATE
+        assert frame[2] == 12          # LEN=12 (three float32)
+        kp, ki, kd = struct.unpack(">fff", frame[3:15])
+        assert kp == pytest.approx(1.5)
+        assert ki == pytest.approx(0.01)
+        assert kd == pytest.approx(0.1)
+
+    def test_pid_update_zero(self):
+        frame = encode_pid_update(0.0, 0.0, 0.0)
+        kp, ki, kd = struct.unpack(">fff", frame[3:15])
+        assert kp == pytest.approx(0.0)
+
+    def test_all_frames_end_with_etx(self):
+        frames = [
+            encode_heartbeat(),
+            encode_speed_steer(100, -100),
+            encode_arm(True),
+            encode_set_mode(1),
+            encode_pid_update(2.0, 0.1, 0.05),
+        ]
+        for f in frames:
+            assert f[-1] == ETX, f"Frame for type 0x{f[1]:02X} doesn't end with ETX"
+
+    def test_all_frames_start_with_stx(self):
+        for f in [encode_heartbeat(), encode_speed_steer(0, 0)]:
+            assert f[0] == STX
+
+
+# ═══════════════════════════════════════════════════════════════════
+# FrameParser tests
+# ═══════════════════════════════════════════════════════════════════
+
+class TestFrameParser:
+
+    # ── IMU telemetry ─────────────────────────────────────────────────────────
+
+    def test_decode_imu_frame(self):
+        payload = struct.pack(">hhhhhh",
+            1250, -300, 900,   # pitch×100, roll×100, yaw×100
+            981,  0,   -50,   # ax×100, ay×100, az×100
+        )
+        raw = _build_telem_frame(TelType.IMU, payload)
+        results, parser = _parse_all(raw)
+        assert len(results) == 1
+        f = results[0]
+        assert isinstance(f, ImuFrame)
+        assert f.pitch_deg == pytest.approx(12.5)
+        assert f.roll_deg  == pytest.approx(-3.0)
+        assert f.yaw_deg   == pytest.approx(9.0)
+        assert f.accel_x   == pytest.approx(9.81)
+        assert f.accel_y   == pytest.approx(0.0)
+        assert f.accel_z   == pytest.approx(-0.5)
+
+    def test_decode_imu_zero(self):
+        payload = struct.pack(">hhhhhh", 0, 0, 0, 0, 0, 0)
+        raw = _build_telem_frame(TelType.IMU, payload)
+        results, _ = _parse_all(raw)
+        f = results[0]
+        assert f.pitch_deg == pytest.approx(0.0)
+        assert f.accel_z   == pytest.approx(0.0)
+
+    # ── Battery telemetry ─────────────────────────────────────────────────────
+
+    def test_decode_battery_frame(self):
+        payload = struct.pack(">HhB", 11100, -500, 72)  # 11.1V, -500mA (charging), 72%
+        raw = _build_telem_frame(TelType.BATTERY, payload)
+        results, _ = _parse_all(raw)
+        assert len(results) == 1
+        f = results[0]
+        assert isinstance(f, BatteryFrame)
+        assert f.voltage_mv == 11100
+        assert f.current_ma == -500
+        assert f.soc_pct    == 72
+
+    def test_decode_battery_discharging(self):
+        payload = struct.pack(">HhB", 10200, 2000, 45)  # 10.2V, 2A draw, 45%
+        raw = _build_telem_frame(TelType.BATTERY, payload)
+        results, _ = _parse_all(raw)
+        f = results[0]
+        assert f.current_ma == 2000     # positive = discharging
+
+    # ── Motor RPM telemetry ───────────────────────────────────────────────────
+
+    def test_decode_motor_rpm(self):
+        payload = struct.pack(">hh", 1500, -1500)   # left=1500, right=-1500 (spinning in place)
+        raw = _build_telem_frame(TelType.MOTOR_RPM, payload)
+        results, _ = _parse_all(raw)
+        f = results[0]
+        assert isinstance(f, MotorRpmFrame)
+        assert f.left_rpm  == 1500
+        assert f.right_rpm == -1500
+
+    def test_decode_motor_rpm_zero(self):
+        payload = struct.pack(">hh", 0, 0)
+        raw = _build_telem_frame(TelType.MOTOR_RPM, payload)
+        results, _ = _parse_all(raw)
+        assert results[0].left_rpm == 0
+
+    # ── Arm state telemetry ───────────────────────────────────────────────────
+
+    def test_decode_arm_state_armed(self):
+        payload = struct.pack("BB", 1, 0)   # ARMED, no error flags
+        raw = _build_telem_frame(TelType.ARM_STATE, payload)
+        results, _ = _parse_all(raw)
+        f = results[0]
+        assert isinstance(f, ArmStateFrame)
+        assert f.state       == 1
+        assert f.error_flags == 0
+
+    def test_decode_arm_state_tilt_fault(self):
+        payload = struct.pack("BB", 2, 0x04)   # TILT_FAULT, bit 2 set
+        raw = _build_telem_frame(TelType.ARM_STATE, payload)
+        results, _ = _parse_all(raw)
+        f = results[0]
+        assert f.state       == 2
+        assert f.error_flags == 0x04
+
+    # ── Error telemetry ───────────────────────────────────────────────────────
+
+    def test_decode_error_frame(self):
+        payload = struct.pack("BB", 0x0A, 0x01)   # error 0x0A, sub 0x01
+        raw = _build_telem_frame(TelType.ERROR, payload)
+        results, _ = _parse_all(raw)
+        f = results[0]
+        assert isinstance(f, ErrorFrame)
+        assert f.error_code == 0x0A
+        assert f.subcode    == 0x01
+
+    # ── CRC errors ────────────────────────────────────────────────────────────
+
+    def test_bad_crc_rejected(self):
+        raw = bytearray(_build_telem_frame(TelType.BATTERY,
+                         struct.pack(">HhB", 11000, 0, 80)))
+        raw[-3] ^= 0xFF    # flip CRC_HI byte
+        results, parser = _parse_all(bytes(raw))
+        assert results == []
+        assert parser.frames_error == 1
+        assert parser.frames_ok    == 0
+
+    def test_bad_crc_both_bytes(self):
+        raw = bytearray(_build_telem_frame(TelType.MOTOR_RPM,
+                         struct.pack(">hh", 100, 100)))
+        raw[-3] ^= 0xAA
+        raw[-2] ^= 0x55
+        results, parser = _parse_all(bytes(raw))
+        assert results == []
+        assert parser.frames_error >= 1
+
+    # ── ETX errors ────────────────────────────────────────────────────────────
+
+    def test_bad_etx_rejected(self):
+        raw = bytearray(_build_telem_frame(TelType.ARM_STATE,
+                         struct.pack("BB", 1, 0)))
+        raw[-1] = 0xFF     # corrupt ETX
+        results, parser = _parse_all(bytes(raw))
+        assert results == []
+        assert parser.frames_error == 1
+
+    # ── Resync after corrupt data ─────────────────────────────────────────────
+
+    def test_resync_after_garbage(self):
+        """Parser must resync and decode a valid frame after leading garbage."""
+        garbage = b"\xDE\xAD\xBE\xEF\xFF\x00\x42"
+        valid   = _build_telem_frame(TelType.ARM_STATE, struct.pack("BB", 1, 0))
+        results, parser = _parse_all(garbage + valid)
+        assert len(results) == 1
+        assert isinstance(results[0], ArmStateFrame)
+
+    def test_two_consecutive_frames(self):
+        """Parser must handle two back-to-back valid frames."""
+        f1 = _build_telem_frame(TelType.ARM_STATE, struct.pack("BB", 1, 0))
+        f2 = _build_telem_frame(TelType.MOTOR_RPM, struct.pack(">hh", 500, 500))
+        results, parser = _parse_all(f1 + f2)
+        assert len(results) == 2
+        assert isinstance(results[0], ArmStateFrame)
+        assert isinstance(results[1], MotorRpmFrame)
+        assert parser.frames_error == 0
+
+    def test_three_frames_with_garbage_between(self):
+        """Corrupt bytes between valid frames must not discard subsequent valid frames."""
+        f1 = _build_telem_frame(TelType.ARM_STATE, struct.pack("BB", 1, 0))
+        f2 = _build_telem_frame(TelType.BATTERY,   struct.pack(">HhB", 11000, 0, 80))
+        junk = b"\xDE\xAD"
+        results, parser = _parse_all(f1 + junk + f2)
+        assert len(results) == 2
+
+    def test_unknown_type_returns_raw_tuple(self):
+        """Unknown telemetry type must be returned as (type_code, payload) tuple."""
+        raw = _build_telem_frame(0x99, b"\xAB\xCD")
+        results, _ = _parse_all(raw)
+        assert len(results) == 1
+        type_code, payload = results[0]
+        assert type_code == 0x99
+        assert payload   == b"\xAB\xCD"
+
+    def test_oversized_payload_rejected(self):
+        """A frame claiming LEN > MAX_PAYLOAD_LEN must be silently skipped."""
+        # Craft a raw frame with LEN=200 (> 64 limit)
+        raw = bytes([STX, TelType.IMU, 200])
+        results, parser = _parse_all(raw)
+        assert results == []
+        assert parser.frames_error == 1
+
+    def test_empty_payload_frame(self):
+        """A frame with LEN=0 (heartbeat style) must decode correctly."""
+        raw = _build_frame(CmdType.HEARTBEAT, b"")
+        results, parser = _parse_all(raw)
+        assert len(results) == 1
+        type_code, payload = results[0]
+        assert type_code == CmdType.HEARTBEAT
+        assert payload   == b""
+        assert parser.frames_error == 0
+
+    # ── Streaming byte-by-byte ────────────────────────────────────────────────
+
+    def test_stream_one_byte_at_a_time(self):
+        """Every byte fed individually must produce the same result as bulk."""
+        payload = struct.pack(">HhB", 12000, -100, 90)
+        raw = _build_telem_frame(TelType.BATTERY, payload)
+        parser = FrameParser()
+        results = []
+        for b in raw:
+            r = parser.feed(b)
+            if r is not None:
+                results.append(r)
+        assert len(results) == 1
+        f = results[0]
+        assert isinstance(f, BatteryFrame)
+        assert f.soc_pct == 90
+
+    # ── Parser reset ──────────────────────────────────────────────────────────
+
+    def test_parser_reset_clears_state(self):
+        """After reset(), parser must accept a new valid frame."""
+        payload = struct.pack("BB", 0, 0)
+        raw = _build_telem_frame(TelType.ARM_STATE, payload)
+        parser = FrameParser()
+        # Feed partial frame
+        for b in raw[:3]:
+            parser.feed(b)
+        # Reset mid-frame
+        parser.reset()
+        # Feed full frame
+        results = []
+        for b in raw:
+            r = parser.feed(b)
+            if r is not None:
+                results.append(r)
+        assert len(results) == 1
+
+    def test_frames_ok_counter(self):
+        f1 = _build_telem_frame(TelType.ARM_STATE, struct.pack("BB", 1, 0))
+        f2 = _build_telem_frame(TelType.ARM_STATE, struct.pack("BB", 0, 0))
+        _, parser = _parse_all(f1 + f2)
+        assert parser.frames_ok == 2
+
+    def test_frames_error_counter(self):
+        raw = bytearray(_build_telem_frame(TelType.ARM_STATE, struct.pack("BB", 1, 0)))
+        raw[-3] ^= 0xFF   # corrupt CRC
+        _, parser = _parse_all(bytes(raw))
+        assert parser.frames_error == 1
+
+
+# ═══════════════════════════════════════════════════════════════════
+# Decode edge cases
+# ═══════════════════════════════════════════════════════════════════
+
+class TestDecodeEdgeCases:
+    def test_imu_short_payload_returns_none(self):
+        result = _decode_telemetry(TelType.IMU, b"\x00\x01")  # only 2 bytes, need 12
+        assert result is None
+
+    def test_battery_short_payload_returns_none(self):
+        result = _decode_telemetry(TelType.BATTERY, b"\x00")
+        assert result is None
+
+    def test_motor_rpm_short_payload_returns_none(self):
+        result = _decode_telemetry(TelType.MOTOR_RPM, b"\x00\x01")
+        assert result is None
+
+    def test_arm_state_short_payload_returns_none(self):
+        result = _decode_telemetry(TelType.ARM_STATE, b"\x01")
+        assert result is None
+
+    def test_error_short_payload_returns_none(self):
+        result = _decode_telemetry(TelType.ERROR, b"\x01")
+        assert result is None
+
+
+# ═══════════════════════════════════════════════════════════════════
+# Round-trip tests (encode command → re-parse frame body)
+# ═══════════════════════════════════════════════════════════════════
+
+class TestRoundTrip:
+    """Verify encoder output satisfies parser expectations (same framing)."""
+
+    def _parse_cmd_as_telem(self, frame: bytes):
+        """Feed a command frame through FrameParser (type codes overlap logic)."""
+        results, parser = _parse_all(frame)
+        return results, parser
+
+    def test_heartbeat_roundtrip(self):
+        frame = encode_heartbeat()
+        results, parser = self._parse_cmd_as_telem(frame)
+        assert len(results) == 1
+        assert parser.frames_error == 0
+
+    def test_speed_steer_roundtrip(self):
+        frame = encode_speed_steer(750, -300)
+        results, _ = self._parse_cmd_as_telem(frame)
+        assert len(results) == 1
+        type_code, payload = results[0]
+        assert type_code == CmdType.SPEED_STEER
+        speed, steer = struct.unpack(">hh", payload)
+        assert speed == 750
+        assert steer == -300
+
+    def test_pid_roundtrip(self):
+        frame = encode_pid_update(2.5, 0.05, 0.08)
+        results, _ = self._parse_cmd_as_telem(frame)
+        type_code, payload = results[0]
+        kp, ki, kd = struct.unpack(">fff", payload)
+        assert kp == pytest.approx(2.5, rel=1e-5)
+        assert ki == pytest.approx(0.05, rel=1e-5)
+        assert kd == pytest.approx(0.08, rel=1e-5)

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

@@ -10,7 +10,8 @@ RTAB-Map chosen over slam_toolbox for Orin because:
 
 Stack:
   sensors.launch.py             (RPLIDAR + RealSense D435i + static TF)
-  rtabmap_ros/rtabmap     (RTAB-Map node with RGB-D + scan input)
+  rtabmap_ros/rtabmap           (RTAB-Map SLAM node)
+  saltybot_mapping/map_manager  (auto-save every 5 min + export services)
 
 RTAB-Map input topics:
   /camera/color/image_raw          30 Hz  (RGB from D435i)
@@ -23,17 +24,35 @@ RTAB-Map output topics:
   /rtabmap/cloud_map       PointCloud2     (3D — visualization)
   /rtabmap/odom            Odometry        (visual-inertial)
 
-Config: /config/rtabmap_params.yaml (mounted from jetson/config/)
+Persistence (Issue #123):
+  database_path = /mnt/nvme/saltybot/maps/current.db  (default)
+  - Normal boot: loads existing map, relocates, continues mapping (multi-session)
+  - fresh_start:=true  → deletes current.db before launch → clean new map
+  - database_path:=<path>  → load a specific named map (from map_manager)
+
+Map management commands:
+  ros2 service call /mapping/maps/list        saltybot_mapping/srv/ListMaps
+  ros2 service call /mapping/maps/save_as     saltybot_mapping/srv/SaveMap    '{map_name: home, overwrite: false}'
+  ros2 service call /mapping/maps/load        saltybot_mapping/srv/LoadMap    '{map_name: home}'
+  ros2 service call /mapping/maps/delete      saltybot_mapping/srv/DeleteMap  '{map_name: old_map}'
+  ros2 service call /mapping/export/occupancy saltybot_mapping/srv/ExportOccupancy '{output_dir: /tmp, map_name: home}'
+  ros2 service call /mapping/export/pointcloud saltybot_mapping/srv/ExportPointCloud '{output_dir: /tmp, map_name: home, format: ply}'
 
 Verify:
   ros2 topic hz /rtabmap/map          # ~1Hz map updates
   ros2 topic hz /rtabmap/cloud_map    # ~1Hz 3D cloud updates
   ros2 topic hz /rtabmap/odom         # ~10Hz odometry
+  ros2 topic echo /mapping/status     # map_manager JSON status (1Hz)
 """
 
 import os
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+)
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -41,25 +60,56 @@ from ament_index_python.packages import get_package_share_directory
 
 
 RTABMAP_PARAMS_FILE = '/config/rtabmap_params.yaml'
+DEFAULT_DB_PATH     = '/mnt/nvme/saltybot/maps/current.db'
 
 
 def generate_launch_description():
+    bringup_share = get_package_share_directory('saltybot_bringup')
+
+    # ── Launch arguments ──────────────────────────────────────────────────
     use_sim_time_arg = DeclareLaunchArgument(
         'use_sim_time',
         default_value='false',
         description='Use simulation clock (set true for rosbag playback)',
     )
+    fresh_start_arg = DeclareLaunchArgument(
+        'fresh_start',
+        default_value='false',
+        description=(
+            'true = delete current.db before launch (fresh map). '
+            'false = reload existing map and relocalize (default).'
+        ),
+    )
+    database_path_arg = DeclareLaunchArgument(
+        'database_path',
+        default_value=DEFAULT_DB_PATH,
+        description=(
+            'RTAB-Map database path. Override to load a specific saved map, '
+            'e.g. /mnt/nvme/saltybot/maps/home_garden.db'
+        ),
+    )
 
-    bringup_share = get_package_share_directory('saltybot_bringup')
+    # ── Optional: wipe DB when fresh_start:=true ──────────────────────────
+    delete_db_action = ExecuteProcess(
+        cmd=['bash', '-c',
+             f'DB="{DEFAULT_DB_PATH}"; '
+             '[ -f "$DB" ] && rm "$DB" '
+             '&& echo "[slam_rtabmap] Deleted $DB — starting fresh map" '
+             '|| echo "[slam_rtabmap] No existing DB at $DB"'],
+        output='screen',
+        condition=IfCondition(LaunchConfiguration('fresh_start')),
+    )
 
+    # ── Sensors ───────────────────────────────────────────────────────────
     sensors_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(bringup_share, 'launch', 'sensors.launch.py')
         ),
     )
 
-    # RTAB-Map node (rtabmap_ros package)
-    # RGB-D + LIDAR mode: subscribe_scan=true, subscribe_rgbd=true
+    # ── RTAB-Map node ─────────────────────────────────────────────────────
+    # --delete_db_on_start intentionally removed: persistence is the default.
+    # Use fresh_start:=true launch arg to start a new map instead.
     rtabmap_node = Node(
         package='rtabmap_ros',
         executable='rtabmap',
@@ -69,46 +119,61 @@ def generate_launch_description():
             RTABMAP_PARAMS_FILE,
             {
                 'use_sim_time':  LaunchConfiguration('use_sim_time'),
-                # Frame IDs — must match sensors.launch.py static TF
                 'frame_id':      'base_link',
                 'odom_frame_id': 'odom',
                 'map_frame_id':  'map',
+                # Override database_path from YAML with launch arg
+                'database_path': LaunchConfiguration('database_path'),
             },
         ],
         remappings=[
-            # RGB-D inputs from RealSense
             ('rgb/image',       '/camera/color/image_raw'),
             ('rgb/camera_info', '/camera/color/camera_info'),
             ('depth/image',     '/camera/depth/image_rect_raw'),
-            # 2D LIDAR from RPLIDAR A1M8
             ('scan',            '/scan'),
         ],
-        arguments=['--delete_db_on_start'],   # fresh map each launch
     )
 
-    # RTAB-Map visualization node (optional — comment out to save CPU)
-    rtabmap_viz_node = Node(
-        package='rtabmap_ros',
-        executable='rtabmapviz',
-        name='rtabmapviz',
+    # ── Map manager node (persistence + auto-save + export) ───────────────
+    map_manager_node = Node(
+        package='saltybot_mapping',
+        executable='map_manager',
+        name='map_manager',
         output='screen',
         parameters=[{
-            'use_sim_time': LaunchConfiguration('use_sim_time'),
-            'frame_id':     'base_link',
+            'maps_dir':          '/mnt/nvme/saltybot/maps',
+            'exports_dir':       '/mnt/nvme/saltybot/maps/exports',
+            'autosave_interval': 300.0,   # every 5 minutes
+            'keep_autosaves_n':  5,
+            'max_db_size_mb':    2048,
         }],
-        remappings=[
-            ('rgb/image',       '/camera/color/image_raw'),
-            ('rgb/camera_info', '/camera/color/camera_info'),
-            ('depth/image',     '/camera/depth/image_rect_raw'),
-            ('scan',            '/scan'),
-        ],
-        # Only launch viz if DISPLAY is available
-        condition=None,    # always launch; comment this node out if headless
     )
 
+    # ── Optional: RTAB-Map visualiser (comment in for debug sessions) ─────
+    # rtabmap_viz_node = Node(
+    #     package='rtabmap_ros',
+    #     executable='rtabmapviz',
+    #     name='rtabmapviz',
+    #     output='screen',
+    #     parameters=[{
+    #         'use_sim_time': LaunchConfiguration('use_sim_time'),
+    #         'frame_id':     'base_link',
+    #     }],
+    #     remappings=[
+    #         ('rgb/image',       '/camera/color/image_raw'),
+    #         ('rgb/camera_info', '/camera/color/camera_info'),
+    #         ('depth/image',     '/camera/depth/image_rect_raw'),
+    #         ('scan',            '/scan'),
+    #     ],
+    # )
+
     return LaunchDescription([
         use_sim_time_arg,
+        fresh_start_arg,
+        database_path_arg,
+        delete_db_action,     # no-op unless fresh_start:=true
         sensors_launch,
         rtabmap_node,
-        # rtabmap_viz_node,   # uncomment for rviz-style RTAB-Map visualization
+        map_manager_node,
+        # rtabmap_viz_node,
     ])

jetson/ros2_ws/src/saltybot_mapping/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_mapping)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/ListMaps.srv"
+  "srv/SaveMap.srv"
+  "srv/LoadMap.srv"
+  "srv/DeleteMap.srv"
+  "srv/ExportOccupancy.srv"
+  "srv/ExportPointCloud.srv"
+  DEPENDENCIES std_msgs builtin_interfaces
+)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  scripts/export_map.py
+  scripts/backup_map.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_mapping/config/map_manager_params.yaml

@@ -0,0 +1,12 @@
+map_manager:
+  ros__parameters:
+    # Directory containing RTAB-Map .db files (must match database_path in rtabmap_params.yaml)
+    maps_dir: '/mnt/nvme/saltybot/maps'
+    # Directory for exported PGM / PLY / PCD files
+    exports_dir: '/mnt/nvme/saltybot/maps/exports'
+    # Auto-save interval in seconds (default: 300 = 5 minutes)
+    autosave_interval: 300.0
+    # Number of autosave_*.db files to keep; older ones pruned automatically
+    keep_autosaves_n: 5
+    # Log a warning if current.db grows beyond this size (MB)
+    max_db_size_mb: 2048

jetson/ros2_ws/src/saltybot_mapping/launch/map_manager.launch.py

@@ -0,0 +1,88 @@
+"""
+map_manager.launch.py — Launch the map manager node alongside RTAB-Map.
+
+Usage:
+    ros2 launch saltybot_mapping map_manager.launch.py
+    ros2 launch saltybot_mapping map_manager.launch.py autosave_interval:=120.0
+    ros2 launch saltybot_mapping map_manager.launch.py maps_dir:=/mnt/nvme/saltybot/maps
+
+Map management commands (once running):
+    # List saved maps
+    ros2 service call /mapping/maps/list saltybot_mapping/srv/ListMaps
+
+    # Save current map as 'home_garden'
+    ros2 service call /mapping/maps/save_as saltybot_mapping/srv/SaveMap \\
+        '{map_name: home_garden, overwrite: false}'
+
+    # Load a saved map (restart rtabmap after)
+    ros2 service call /mapping/maps/load saltybot_mapping/srv/LoadMap \\
+        '{map_name: home_garden}'
+
+    # Export 2D occupancy grid
+    ros2 service call /mapping/export/occupancy saltybot_mapping/srv/ExportOccupancy \\
+        '{output_dir: /tmp, map_name: home}'
+
+    # Export 3D point cloud as PLY
+    ros2 service call /mapping/export/pointcloud saltybot_mapping/srv/ExportPointCloud \\
+        '{output_dir: /tmp, map_name: home, format: ply}'
+
+    # Monitor map status
+    ros2 topic echo /mapping/status
+"""
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('saltybot_mapping')
+    default_config = os.path.join(pkg_dir, 'config', 'map_manager_params.yaml')
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'maps_dir',
+            default_value='/mnt/nvme/saltybot/maps',
+            description='Directory for RTAB-Map .db files',
+        ),
+        DeclareLaunchArgument(
+            'exports_dir',
+            default_value='/mnt/nvme/saltybot/maps/exports',
+            description='Directory for exported PGM/PLY/PCD files',
+        ),
+        DeclareLaunchArgument(
+            'autosave_interval',
+            default_value='300.0',
+            description='Auto-save interval in seconds (default 5 min)',
+        ),
+        DeclareLaunchArgument(
+            'keep_autosaves_n',
+            default_value='5',
+            description='Number of autosave files to retain (older pruned)',
+        ),
+        DeclareLaunchArgument(
+            'max_db_size_mb',
+            default_value='2048',
+            description='Warn when current.db exceeds this size (MB)',
+        ),
+
+        Node(
+            package='saltybot_mapping',
+            executable='map_manager',
+            name='map_manager',
+            output='screen',
+            parameters=[
+                default_config,
+                {
+                    'maps_dir':          LaunchConfiguration('maps_dir'),
+                    'exports_dir':       LaunchConfiguration('exports_dir'),
+                    'autosave_interval': LaunchConfiguration('autosave_interval'),
+                    'keep_autosaves_n':  LaunchConfiguration('keep_autosaves_n'),
+                    'max_db_size_mb':    LaunchConfiguration('max_db_size_mb'),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_mapping/package.xml

@@ -0,0 +1,34 @@
+<?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_mapping</name>
+  <version>0.1.0</version>
+  <description>RTAB-Map persistence, multi-session mapping, map management and export for saltybot</description>
+  <maintainer email="seb@vayrette.com">seb</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>builtin_interfaces</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_mapping/saltybot_mapping/map_database.py

@@ -0,0 +1,137 @@
+"""map_database.py — Filesystem map database management for saltybot.
+
+Manages the /mnt/nvme/saltybot/maps/ directory containing RTAB-Map .db files.
+Each map is a single SQLite-backed RTAB-Map database file.
+
+Directory layout:
+    /mnt/nvme/saltybot/maps/
+        current.db          ← active map (symlink or copy)
+        home_2026-03-02.db  ← named saved maps
+        garden_session2.db
+        exports/
+            home_2026-03-02_occ.pgm
+            home_2026-03-02_occ.yaml
+            home_2026-03-02_cloud.ply
+"""
+
+import os
+import shutil
+import time
+from datetime import datetime
+from pathlib import Path
+
+
+class MapDatabase:
+    """Manage saved RTAB-Map .db files on disk."""
+
+    DB_SUFFIX = '.db'
+
+    def __init__(self, maps_dir: str):
+        self._maps_dir = Path(maps_dir)
+        self._maps_dir.mkdir(parents=True, exist_ok=True)
+        (self._maps_dir / 'exports').mkdir(exist_ok=True)
+
+    @property
+    def maps_dir(self) -> Path:
+        return self._maps_dir
+
+    @property
+    def exports_dir(self) -> Path:
+        return self._maps_dir / 'exports'
+
+    def current_db_path(self) -> str:
+        """Path to the active RTAB-Map database (passed to rtabmap as database_path)."""
+        return str(self._maps_dir / 'current.db')
+
+    def list_maps(self) -> list[dict]:
+        """Return list of saved maps as dicts: {name, path, size_bytes, modified_time}."""
+        maps = []
+        for p in sorted(self._maps_dir.glob(f'*{self.DB_SUFFIX}')):
+            stat = p.stat()
+            maps.append({
+                'name': p.stem,
+                'path': str(p),
+                'size_bytes': stat.st_size,
+                'modified_time': datetime.fromtimestamp(stat.st_mtime).isoformat(),
+            })
+        return maps
+
+    def save_as(self, map_name: str, overwrite: bool = False) -> tuple[bool, str, str]:
+        """Copy current.db to <map_name>.db.
+
+        Returns:
+            (success, message, saved_path)
+        """
+        src = self._maps_dir / 'current.db'
+        if not src.exists():
+            return False, 'current.db does not exist — no active map to save', ''
+
+        dst = self._maps_dir / f'{map_name}{self.DB_SUFFIX}'
+        if dst.exists() and not overwrite:
+            return False, f'{map_name}.db already exists (set overwrite=true to replace)', ''
+
+        shutil.copy2(str(src), str(dst))
+        return True, f'Saved map as {map_name}.db ({dst.stat().st_size // 1024} KB)', str(dst)
+
+    def load_map(self, map_name: str) -> tuple[bool, str, str]:
+        """Copy <map_name>.db over current.db.
+
+        NOTE: rtabmap node must be restarted after this call to pick up the new DB.
+        Returns:
+            (success, message, loaded_path)
+        """
+        src = self._maps_dir / f'{map_name}{self.DB_SUFFIX}'
+        if not src.exists():
+            return False, f'Map not found: {map_name}.db', ''
+
+        dst = self._maps_dir / 'current.db'
+        # Backup current before overwriting
+        if dst.exists():
+            backup = self._maps_dir / f'current_backup_{int(time.time())}.db'
+            shutil.copy2(str(dst), str(backup))
+
+        shutil.copy2(str(src), str(dst))
+        size_kb = dst.stat().st_size // 1024
+        return (
+            True,
+            f'Loaded {map_name}.db → current.db ({size_kb} KB). '
+            f'Restart rtabmap node to activate.',
+            str(dst),
+        )
+
+    def delete_map(self, map_name: str) -> tuple[bool, str]:
+        """Delete <map_name>.db.
+
+        Cannot delete current.db through this method (use fresh_start arg instead).
+        Returns:
+            (success, message)
+        """
+        if map_name == 'current':
+            return False, 'Cannot delete current.db via this service. Use fresh_start:=true at launch instead.'
+
+        target = self._maps_dir / f'{map_name}{self.DB_SUFFIX}'
+        if not target.exists():
+            return False, f'Map not found: {map_name}.db'
+
+        target.unlink()
+        return True, f'Deleted {map_name}.db'
+
+    def auto_timestamped_name(self) -> str:
+        """Generate a timestamped map name for auto-saves."""
+        return f'autosave_{datetime.now().strftime("%Y%m%d_%H%M%S")}'
+
+    def prune_autosaves(self, keep_n: int = 5) -> list[str]:
+        """Keep only the N most recent autosave_*.db files, delete older ones.
+
+        Returns list of deleted file names.
+        """
+        autosaves = sorted(
+            self._maps_dir.glob('autosave_*.db'),
+            key=lambda p: p.stat().st_mtime,
+        )
+        to_delete = autosaves[:-keep_n] if len(autosaves) > keep_n else []
+        deleted = []
+        for p in to_delete:
+            p.unlink()
+            deleted.append(p.name)
+        return deleted

jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_exporter.py

@@ -0,0 +1,191 @@
+"""map_exporter.py — Export RTAB-Map data to standard formats.
+
+2D occupancy grid → PGM image + ROS map_server YAML
+3D point cloud   → PLY or PCD file
+
+Both exporters work by subscribing to RTAB-Map output topics and
+capturing the latest message on demand.
+"""
+
+import struct
+import time
+import numpy as np
+from datetime import datetime
+from pathlib import Path
+
+
+# ── 2D Occupancy Grid export ────────────────────────────────────────────────
+
+def occupancy_grid_to_pgm(
+    grid_data: list[int],
+    width: int,
+    height: int,
+) -> bytes:
+    """Convert ROS OccupancyGrid data to PGM (Portable GrayMap) bytes.
+
+    OccupancyGrid encoding:
+        -1  → unknown      → 205 (mid-grey in ROS convention)
+         0  → free         → 254 (white)
+        100 → occupied     → 0   (black)
+        1-99 → partial     → interpolated
+
+    Returns raw PGM P5 binary bytes.
+    """
+    pixels = np.zeros(len(grid_data), dtype=np.uint8)
+    arr = np.array(grid_data, dtype=np.int8)
+
+    unknown_mask  = arr == -1
+    free_mask     = arr == 0
+    occupied_mask = arr == 100
+    partial_mask  = ~unknown_mask & ~free_mask & ~occupied_mask
+
+    pixels[unknown_mask]  = 205
+    pixels[free_mask]     = 254
+    pixels[occupied_mask] = 0
+    pixels[partial_mask]  = (254 - arr[partial_mask].astype(np.int32) * 254 // 100).astype(np.uint8)
+
+    # PGM P5 (binary) format — rows stored bottom-up in ROS, flip vertically
+    image = pixels.reshape(height, width)
+    image = np.flipud(image)  # ROS maps have origin at bottom-left
+
+    header = f'P5\n{width} {height}\n255\n'.encode()
+    return header + image.tobytes()
+
+
+def write_occupancy_export(
+    grid_msg,            # nav_msgs/OccupancyGrid ROS message object
+    output_dir: str,
+    map_name: str = '',
+) -> tuple[str, str]:
+    """Write PGM + map_server YAML from an OccupancyGrid message.
+
+    Returns:
+        (pgm_path, yaml_path)
+    """
+    if not map_name:
+        map_name = f'map_{datetime.now().strftime("%Y%m%d_%H%M%S")}'
+
+    out = Path(output_dir)
+    out.mkdir(parents=True, exist_ok=True)
+    pgm_path  = out / f'{map_name}.pgm'
+    yaml_path = out / f'{map_name}.yaml'
+
+    info = grid_msg.info
+    pgm_bytes = occupancy_grid_to_pgm(
+        list(grid_msg.data), info.width, info.height)
+
+    pgm_path.write_bytes(pgm_bytes)
+
+    # map_server YAML format
+    yaml_content = (
+        f'image: {pgm_path.name}\n'
+        f'resolution: {info.resolution:.4f}\n'
+        f'origin: [{info.origin.position.x:.4f}, '
+        f'{info.origin.position.y:.4f}, '
+        f'{info.origin.position.z:.4f}]\n'
+        f'negate: 0\n'
+        f'occupied_thresh: 0.65\n'
+        f'free_thresh: 0.196\n'
+        f'mode: trinary\n'
+    )
+    yaml_path.write_text(yaml_content)
+
+    return str(pgm_path), str(yaml_path)
+
+
+# ── 3D Point Cloud export ────────────────────────────────────────────────────
+
+def pointcloud2_to_xyz(cloud_msg) -> np.ndarray:
+    """Extract XYZ points from a sensor_msgs/PointCloud2 message.
+
+    Returns:
+        np.ndarray of shape (N, 3) float32
+    """
+    # Parse field offsets
+    fields = {f.name: f for f in cloud_msg.fields}
+    x_off = fields['x'].offset
+    y_off = fields['y'].offset
+    z_off = fields['z'].offset
+    point_step = cloud_msg.point_step
+    data = bytes(cloud_msg.data)
+    n_points = cloud_msg.width * cloud_msg.height
+
+    xyz = np.zeros((n_points, 3), dtype=np.float32)
+    for i in range(n_points):
+        base = i * point_step
+        xyz[i, 0] = struct.unpack_from('<f', data, base + x_off)[0]
+        xyz[i, 1] = struct.unpack_from('<f', data, base + y_off)[0]
+        xyz[i, 2] = struct.unpack_from('<f', data, base + z_off)[0]
+
+    # Filter NaN/Inf
+    valid = np.isfinite(xyz).all(axis=1)
+    return xyz[valid]
+
+
+def write_ply(xyz: np.ndarray, path: str) -> None:
+    """Write XYZ point cloud as ASCII PLY file."""
+    n = len(xyz)
+    header = (
+        'ply\n'
+        'format ascii 1.0\n'
+        f'element vertex {n}\n'
+        'property float x\n'
+        'property float y\n'
+        'property float z\n'
+        'end_header\n'
+    )
+    with open(path, 'w') as f:
+        f.write(header)
+        for p in xyz:
+            f.write(f'{p[0]:.4f} {p[1]:.4f} {p[2]:.4f}\n')
+
+
+def write_pcd(xyz: np.ndarray, path: str) -> None:
+    """Write XYZ point cloud as ASCII PCD file (PCL-compatible)."""
+    n = len(xyz)
+    header = (
+        '# .PCD v0.7 - Point Cloud Data file format\n'
+        'VERSION 0.7\n'
+        'FIELDS x y z\n'
+        'SIZE 4 4 4\n'
+        'TYPE F F F\n'
+        'COUNT 1 1 1\n'
+        f'WIDTH {n}\n'
+        'HEIGHT 1\n'
+        'VIEWPOINT 0 0 0 1 0 0 0\n'
+        f'POINTS {n}\n'
+        'DATA ascii\n'
+    )
+    with open(path, 'w') as f:
+        f.write(header)
+        for p in xyz:
+            f.write(f'{p[0]:.4f} {p[1]:.4f} {p[2]:.4f}\n')
+
+
+def write_pointcloud_export(
+    cloud_msg,          # sensor_msgs/PointCloud2 ROS message object
+    output_dir: str,
+    map_name: str = '',
+    fmt: str = 'ply',
+) -> str:
+    """Write point cloud to PLY or PCD.
+
+    Returns:
+        file_path
+    """
+    if fmt not in ('ply', 'pcd'):
+        fmt = 'ply'
+    if not map_name:
+        map_name = f'cloud_{datetime.now().strftime("%Y%m%d_%H%M%S")}'
+
+    out = Path(output_dir)
+    out.mkdir(parents=True, exist_ok=True)
+    file_path = out / f'{map_name}.{fmt}'
+
+    xyz = pointcloud2_to_xyz(cloud_msg)
+    if fmt == 'ply':
+        write_ply(xyz, str(file_path))
+    else:
+        write_pcd(xyz, str(file_path))
+
+    return str(file_path)

jetson/ros2_ws/src/saltybot_mapping/saltybot_mapping/map_manager_node.py

@@ -0,0 +1,280 @@
+"""
+map_manager_node.py — RTAB-Map persistence and map management node.
+
+Responsibilities:
+  1. Auto-save current.db every N minutes (configurable, default 5 min)
+  2. Prune old autosaves (keep_autosaves_n, default 5)
+  3. Map management services: list / save_as / load / delete
+  4. Export services: 2D occupancy (PGM+YAML) / 3D point cloud (PLY/PCD)
+  5. Publish /mapping/status (map size, last save time, keyframe count)
+
+Map lifecycle with RTAB-Map:
+  - RTAB-Map writes continuously to current.db (configured via database_path param)
+  - On startup: if current.db exists and fresh_start=false → RTAB-Map relocates
+  - This node periodically copies current.db → autosave_YYYYMMDD_HHMMSS.db
+  - load_map copies a named .db → current.db then signals for restart
+
+ROS2 services:
+  /mapping/maps/list         (saltybot_mapping/ListMaps)
+  /mapping/maps/save_as      (saltybot_mapping/SaveMap)
+  /mapping/maps/load         (saltybot_mapping/LoadMap)
+  /mapping/maps/delete       (saltybot_mapping/DeleteMap)
+  /mapping/export/occupancy  (saltybot_mapping/ExportOccupancy)
+  /mapping/export/pointcloud (saltybot_mapping/ExportPointCloud)
+
+ROS2 topics published:
+  /mapping/status            (std_msgs/String, JSON, 1 Hz)
+
+ROS2 topics subscribed:
+  /rtabmap/map               (nav_msgs/OccupancyGrid, latched) — for export
+  /rtabmap/cloud_map         (sensor_msgs/PointCloud2, latched) — for export
+  /rtabmap/info              (rtabmap_msgs/Info) — for keyframe count
+"""
+
+import json
+import time
+import os
+from pathlib import Path
+from datetime import datetime
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+from std_msgs.msg import String
+from nav_msgs.msg import OccupancyGrid
+from sensor_msgs.msg import PointCloud2
+
+from saltybot_mapping.srv import (
+    ListMaps, SaveMap, LoadMap, DeleteMap,
+    ExportOccupancy, ExportPointCloud,
+)
+from .map_database import MapDatabase
+from .map_exporter import write_occupancy_export, write_pointcloud_export
+
+
+class MapManagerNode(Node):
+
+    def __init__(self):
+        super().__init__('map_manager')
+
+        # ── Parameters ──────────────────────────────────────────────────────
+        self.declare_parameter('maps_dir',           '/mnt/nvme/saltybot/maps')
+        self.declare_parameter('exports_dir',        '/mnt/nvme/saltybot/maps/exports')
+        self.declare_parameter('autosave_interval',  300.0)  # seconds (5 min)
+        self.declare_parameter('keep_autosaves_n',   5)
+        self.declare_parameter('max_db_size_mb',     2048)   # warn if current.db > 2GB
+
+        maps_dir       = self.get_parameter('maps_dir').value
+        self._exports_dir = self.get_parameter('exports_dir').value
+        self._autosave_interval = self.get_parameter('autosave_interval').value
+        self._keep_autosaves  = self.get_parameter('keep_autosaves_n').value
+        self._max_db_mb       = self.get_parameter('max_db_size_mb').value
+
+        self._db = MapDatabase(maps_dir)
+        self._last_autosave: float = time.monotonic()
+        self._last_occ_msg: OccupancyGrid | None = None
+        self._last_cloud_msg: PointCloud2 | None = None
+        self._keyframe_count: int = 0
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        transient_local = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+        best_effort = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+
+        self.create_subscription(
+            OccupancyGrid, '/rtabmap/map',
+            self._on_occupancy_grid, transient_local)
+        self.create_subscription(
+            PointCloud2, '/rtabmap/cloud_map',
+            self._on_cloud_map, best_effort)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._pub_status = self.create_publisher(String, '/mapping/status', 10)
+
+        # ── Services ─────────────────────────────────────────────────────────
+        self.create_service(ListMaps,        '/mapping/maps/list',         self._handle_list)
+        self.create_service(SaveMap,         '/mapping/maps/save_as',      self._handle_save_as)
+        self.create_service(LoadMap,         '/mapping/maps/load',         self._handle_load)
+        self.create_service(DeleteMap,       '/mapping/maps/delete',       self._handle_delete)
+        self.create_service(ExportOccupancy, '/mapping/export/occupancy',  self._handle_export_occ)
+        self.create_service(ExportPointCloud,'/mapping/export/pointcloud', self._handle_export_cloud)
+
+        # ── Timers ───────────────────────────────────────────────────────────
+        self.create_timer(1.0, self._status_tick)
+        self.create_timer(self._autosave_interval, self._autosave_tick)
+
+        self.get_logger().info(
+            f'map_manager ready. maps_dir={maps_dir}, '
+            f'autosave every {self._autosave_interval:.0f}s'
+        )
+
+    # ── Subscription callbacks ─────────────────────────────────────────────
+
+    def _on_occupancy_grid(self, msg: OccupancyGrid) -> None:
+        self._last_occ_msg = msg
+
+    def _on_cloud_map(self, msg: PointCloud2) -> None:
+        self._last_cloud_msg = msg
+
+    # ── Status tick ───────────────────────────────────────────────────────
+
+    def _status_tick(self) -> None:
+        current_db = Path(self._db.current_db_path())
+        db_size_mb = (current_db.stat().st_size / 1024 / 1024
+                      if current_db.exists() else 0.0)
+
+        if db_size_mb > self._max_db_mb:
+            self.get_logger().warn(
+                f'current.db size {db_size_mb:.0f} MB exceeds limit '
+                f'{self._max_db_mb} MB — consider pruning keyframes.',
+                throttle_duration_sec=60.0,
+            )
+
+        status = {
+            'db_size_mb':       round(db_size_mb, 1),
+            'maps_count':       len(self._db.list_maps()),
+            'has_occupancy':    self._last_occ_msg is not None,
+            'has_cloud':        self._last_cloud_msg is not None,
+            'next_autosave_s':  max(0, round(
+                self._autosave_interval - (time.monotonic() - self._last_autosave))),
+            'timestamp':        datetime.now().isoformat(),
+        }
+        msg = String()
+        msg.data = json.dumps(status)
+        self._pub_status.publish(msg)
+
+    # ── Autosave tick ─────────────────────────────────────────────────────
+
+    def _autosave_tick(self) -> None:
+        self._last_autosave = time.monotonic()
+        name = self._db.auto_timestamped_name()
+        ok, msg, path = self._db.save_as(name, overwrite=False)
+        if ok:
+            deleted = self._db.prune_autosaves(self._keep_autosaves)
+            self.get_logger().info(
+                f'Autosave: {path} '
+                f'(pruned {len(deleted)} old autosave(s))'
+            )
+        else:
+            self.get_logger().warn(f'Autosave skipped: {msg}')
+
+    # ── Service handlers ──────────────────────────────────────────────────
+
+    def _handle_list(self, request: ListMaps.Request,
+                     response: ListMaps.Response) -> ListMaps.Response:
+        maps = self._db.list_maps()
+        response.map_names     = [m['name']          for m in maps]
+        response.map_paths     = [m['path']          for m in maps]
+        response.sizes_bytes   = [m['size_bytes']    for m in maps]
+        response.modified_times= [m['modified_time'] for m in maps]
+        response.count         = len(maps)
+        return response
+
+    def _handle_save_as(self, request: SaveMap.Request,
+                        response: SaveMap.Response) -> SaveMap.Response:
+        if not request.map_name:
+            request.map_name = self._db.auto_timestamped_name()
+        ok, msg, path = self._db.save_as(request.map_name, request.overwrite)
+        response.success    = ok
+        response.message    = msg
+        response.saved_path = path
+        if ok:
+            self.get_logger().info(f'Map saved: {path}')
+        else:
+            self.get_logger().warn(f'Save failed: {msg}')
+        return response
+
+    def _handle_load(self, request: LoadMap.Request,
+                     response: LoadMap.Response) -> LoadMap.Response:
+        ok, msg, path = self._db.load_map(request.map_name)
+        response.success     = ok
+        response.message     = msg
+        response.loaded_path = path
+        if ok:
+            self.get_logger().info(f'Map loaded: {path}. Restart rtabmap to activate.')
+        else:
+            self.get_logger().warn(f'Load failed: {msg}')
+        return response
+
+    def _handle_delete(self, request: DeleteMap.Request,
+                       response: DeleteMap.Response) -> DeleteMap.Response:
+        ok, msg = self._db.delete_map(request.map_name)
+        response.success = ok
+        response.message = msg
+        if ok:
+            self.get_logger().info(f'Map deleted: {request.map_name}')
+        else:
+            self.get_logger().warn(f'Delete failed: {msg}')
+        return response
+
+    def _handle_export_occ(self, request: ExportOccupancy.Request,
+                           response: ExportOccupancy.Response) -> ExportOccupancy.Response:
+        if self._last_occ_msg is None:
+            response.success = False
+            response.message = 'No occupancy grid received yet from /rtabmap/map'
+            return response
+
+        out_dir = request.output_dir or self._exports_dir
+        map_name = request.map_name or ''
+        try:
+            pgm, yaml = write_occupancy_export(
+                self._last_occ_msg, out_dir, map_name)
+            response.success  = True
+            response.pgm_path = pgm
+            response.yaml_path= yaml
+            response.message  = f'Exported {pgm} ({Path(pgm).stat().st_size // 1024} KB)'
+            self.get_logger().info(response.message)
+        except Exception as e:
+            response.success = False
+            response.message = f'Export failed: {e}'
+            self.get_logger().error(response.message)
+        return response
+
+    def _handle_export_cloud(self, request: ExportPointCloud.Request,
+                             response: ExportPointCloud.Response) -> ExportPointCloud.Response:
+        if self._last_cloud_msg is None:
+            response.success = False
+            response.message = 'No point cloud received yet from /rtabmap/cloud_map'
+            return response
+
+        out_dir = request.output_dir or self._exports_dir
+        fmt = request.format if request.format in ('ply', 'pcd') else 'ply'
+        map_name = request.map_name or ''
+        try:
+            path = write_pointcloud_export(
+                self._last_cloud_msg, out_dir, map_name, fmt)
+            response.success   = True
+            response.file_path = path
+            response.message   = f'Exported {path} ({Path(path).stat().st_size // 1024} KB)'
+            self.get_logger().info(response.message)
+        except Exception as e:
+            response.success = False
+            response.message = f'Export failed: {e}'
+            self.get_logger().error(response.message)
+        return response
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MapManagerNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_mapping/scripts/backup_map.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+"""
+backup_map.py — Manually trigger map save-as via the map_manager service.
+
+Usage:
+    ros2 run saltybot_mapping backup_map -- --name garden_session1
+    ros2 run saltybot_mapping backup_map -- --name garden_session1 --overwrite
+    ros2 run saltybot_mapping backup_map --  # auto-timestamped name
+"""
+
+import argparse
+import sys
+import rclpy
+from rclpy.node import Node
+from saltybot_mapping.srv import SaveMap, ListMaps
+
+
+class BackupCLI(Node):
+    def __init__(self):
+        super().__init__('backup_map_cli')
+        self._save_client = self.create_client(SaveMap,   '/mapping/maps/save_as')
+        self._list_client = self.create_client(ListMaps,  '/mapping/maps/list')
+
+    def save(self, name: str, overwrite: bool) -> bool:
+        if not self._save_client.wait_for_service(timeout_sec=5.0):
+            print('[ERROR] /mapping/maps/save_as service not available')
+            return False
+        req = SaveMap.Request()
+        req.map_name  = name
+        req.overwrite = overwrite
+        fut = self._save_client.call_async(req)
+        rclpy.spin_until_future_complete(self, fut, timeout_sec=30.0)
+        resp = fut.result()
+        if resp and resp.success:
+            print(f'[OK] {resp.message}')
+            return True
+        print(f'[FAIL] {resp.message if resp else "No response"}')
+        return False
+
+    def list_maps(self) -> None:
+        if not self._list_client.wait_for_service(timeout_sec=5.0):
+            print('[ERROR] /mapping/maps/list service not available')
+            return
+        fut = self._list_client.call_async(ListMaps.Request())
+        rclpy.spin_until_future_complete(self, fut, timeout_sec=10.0)
+        resp = fut.result()
+        if resp:
+            print(f'Saved maps ({resp.count}):')
+            for name, path, size, mtime in zip(
+                    resp.map_names, resp.map_paths,
+                    resp.sizes_bytes, resp.modified_times):
+                print(f'  {name:<30} {size // 1024:>6} KB  {mtime}')
+
+
+def main():
+    parser = argparse.ArgumentParser(description='Backup current RTAB-Map database')
+    parser.add_argument('--name',      default='', help='Map name (default: auto timestamp)')
+    parser.add_argument('--overwrite', action='store_true', help='Overwrite existing map with same name')
+    parser.add_argument('--list',      action='store_true', help='List saved maps and exit')
+    args, ros_args = parser.parse_known_args()
+
+    rclpy.init(args=ros_args)
+    node = BackupCLI()
+    if args.list:
+        node.list_maps()
+        ok = True
+    else:
+        ok = node.save(args.name, args.overwrite)
+    node.destroy_node()
+    rclpy.shutdown()
+    sys.exit(0 if ok else 1)
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_mapping/scripts/export_map.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+"""
+export_map.py — CLI tool to export RTAB-Map data to standard formats.
+
+Calls the running map_manager_node services to trigger export.
+
+Usage:
+    # Export 2D occupancy grid (PGM + YAML)
+    ros2 run saltybot_mapping export_map -- --occupancy --name home --output /tmp
+
+    # Export 3D point cloud (PLY)
+    ros2 run saltybot_mapping export_map -- --pointcloud --name home --format ply --output /tmp
+
+    # Both
+    ros2 run saltybot_mapping export_map -- --occupancy --pointcloud --name home
+"""
+
+import argparse
+import sys
+import rclpy
+from rclpy.node import Node
+from saltybot_mapping.srv import ExportOccupancy, ExportPointCloud
+
+
+class ExportCLI(Node):
+    def __init__(self):
+        super().__init__('export_map_cli')
+        self._occ_client   = self.create_client(ExportOccupancy,  '/mapping/export/occupancy')
+        self._cloud_client = self.create_client(ExportPointCloud, '/mapping/export/pointcloud')
+
+    def export_occupancy(self, output_dir: str, map_name: str) -> bool:
+        if not self._occ_client.wait_for_service(timeout_sec=5.0):
+            print('[ERROR] /mapping/export/occupancy service not available')
+            return False
+        req = ExportOccupancy.Request()
+        req.output_dir = output_dir
+        req.map_name   = map_name
+        fut = self._occ_client.call_async(req)
+        rclpy.spin_until_future_complete(self, fut, timeout_sec=30.0)
+        resp = fut.result()
+        if resp and resp.success:
+            print(f'[OK] Occupancy: {resp.pgm_path}')
+            print(f'[OK]     YAML: {resp.yaml_path}')
+            return True
+        print(f'[FAIL] {resp.message if resp else "No response"}')
+        return False
+
+    def export_pointcloud(self, output_dir: str, map_name: str, fmt: str) -> bool:
+        if not self._cloud_client.wait_for_service(timeout_sec=5.0):
+            print('[ERROR] /mapping/export/pointcloud service not available')
+            return False
+        req = ExportPointCloud.Request()
+        req.output_dir = output_dir
+        req.map_name   = map_name
+        req.format     = fmt
+        fut = self._cloud_client.call_async(req)
+        rclpy.spin_until_future_complete(self, fut, timeout_sec=60.0)
+        resp = fut.result()
+        if resp and resp.success:
+            print(f'[OK] Point cloud: {resp.file_path}')
+            return True
+        print(f'[FAIL] {resp.message if resp else "No response"}')
+        return False
+
+
+def main():
+    parser = argparse.ArgumentParser(description='Export RTAB-Map data')
+    parser.add_argument('--occupancy',   action='store_true', help='Export 2D occupancy (PGM+YAML)')
+    parser.add_argument('--pointcloud',  action='store_true', help='Export 3D point cloud')
+    parser.add_argument('--name',        default='',          help='Base filename (default: auto timestamp)')
+    parser.add_argument('--output',      default='',          help='Output directory (default: maps/exports)')
+    parser.add_argument('--format',      default='ply',       choices=['ply', 'pcd'], help='Point cloud format')
+    args, ros_args = parser.parse_known_args()
+
+    if not args.occupancy and not args.pointcloud:
+        parser.error('Specify --occupancy and/or --pointcloud')
+
+    rclpy.init(args=ros_args)
+    node = ExportCLI()
+    ok = True
+    if args.occupancy:
+        ok &= node.export_occupancy(args.output, args.name)
+    if args.pointcloud:
+        ok &= node.export_pointcloud(args.output, args.name, args.format)
+    node.destroy_node()
+    rclpy.shutdown()
+    sys.exit(0 if ok else 1)
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_mapping/srv/DeleteMap.srv

@@ -0,0 +1,4 @@
+string map_name   # name to delete (without .db extension)
+---
+bool success
+string message

jetson/ros2_ws/src/saltybot_mapping/srv/ExportOccupancy.srv

@@ -0,0 +1,7 @@
+string output_dir   # directory for PGM+YAML output; empty = default export dir
+string map_name     # base filename; empty = auto (map_YYYYMMDD_HHMMSS)
+---
+bool success
+string pgm_path
+string yaml_path
+string message

jetson/ros2_ws/src/saltybot_mapping/srv/ExportPointCloud.srv

@@ -0,0 +1,7 @@
+string output_dir   # directory for output; empty = default export dir
+string map_name     # base filename; empty = auto
+string format       # "ply" or "pcd" (default "ply")
+---
+bool success
+string file_path
+string message

jetson/ros2_ws/src/saltybot_mapping/srv/ListMaps.srv

@@ -0,0 +1,6 @@
+---
+string[] map_names
+string[] map_paths
+int64[] sizes_bytes
+string[] modified_times
+int32 count

jetson/ros2_ws/src/saltybot_mapping/srv/LoadMap.srv

@@ -0,0 +1,5 @@
+string map_name   # name to load (without .db extension)
+---
+bool success
+string message
+string loaded_path

jetson/ros2_ws/src/saltybot_mapping/srv/SaveMap.srv

@@ -0,0 +1,6 @@
+string map_name   # name to save as (without .db extension)
+bool overwrite    # overwrite if exists
+---
+bool success
+string message
+string saved_path

jetson/ros2_ws/src/saltybot_tank_driver/config/tank_params.yaml

@@ -0,0 +1,36 @@
+tank_driver:
+  ros__parameters:
+    # Serial interface
+    serial_port: /dev/ttyUSB_tank
+    baud_rate: 115200
+
+    # Drive mode: "tracked" | "4wd" | "2wd"
+    # tracked — tank kinematics with track-slip compensation (default)
+    # 4wd     — skid-steer, 4 ESC channels, no slip compensation
+    # 2wd     — standard diff-drive, 2 ESC channels, no slip compensation
+    drive_mode: tracked
+
+    # Physical parameters
+    track_width: 0.30         # left↔right track centre distance (m)
+    wheelbase: 0.32           # front↔rear sprocket distance (m) — for reference
+
+    # Speed limits
+    max_speed_ms: 1.5         # m/s at full ESC deflection
+    max_linear_vel: 1.0       # clamp on cmd_vel linear.x (m/s)
+    max_angular_vel: 2.5      # clamp on cmd_vel angular.z (rad/s)
+
+    # Track slip compensation (tracked mode only)
+    # 0.0 = no slip (rigid wheel / high-traction surface)
+    # 0.1 = light slip (rubber tracks on concrete)
+    # 0.3 = moderate slip (rubber tracks on grass/gravel)  ← default
+    # 0.5 = heavy slip (metal tracks on mud)
+    slip_factor: 0.3
+
+    # ESC PWM settings
+    pwm_neutral_us: 1500      # neutral/brake pulse width (µs)
+    pwm_range_us: 500         # half-range from neutral → [1000..2000] µs
+
+    # Timing
+    watchdog_timeout_s: 0.3   # deadman: shorter than rover for tracked safety
+    heartbeat_period_s: 0.2
+    control_rate: 50.0        # Hz

jetson/ros2_ws/src/saltybot_tank_driver/launch/tank_driver.launch.py

@@ -0,0 +1,65 @@
+"""
+tank_driver.launch.py — Launch the SaltyTank ESC motor driver node.
+
+Usage:
+    ros2 launch saltybot_tank_driver tank_driver.launch.py
+    ros2 launch saltybot_tank_driver tank_driver.launch.py serial_port:=/dev/ttyACM0
+    ros2 launch saltybot_tank_driver tank_driver.launch.py drive_mode:=4wd
+    ros2 launch saltybot_tank_driver tank_driver.launch.py slip_factor:=0.1
+
+Emergency stop:
+    ros2 topic pub /saltybot/e_stop std_msgs/Bool '{data: true}' --once
+    ros2 topic pub /saltybot/e_stop std_msgs/Bool '{data: false}' --once
+"""
+
+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_tank_driver")
+    params_file = os.path.join(pkg_share, "config", "tank_params.yaml")
+
+    serial_port_arg = DeclareLaunchArgument(
+        "serial_port",
+        default_value="/dev/ttyUSB_tank",
+        description="Serial port for ESC controller",
+    )
+    drive_mode_arg = DeclareLaunchArgument(
+        "drive_mode",
+        default_value="tracked",
+        description="Drive mode: tracked | 4wd | 2wd",
+    )
+    slip_factor_arg = DeclareLaunchArgument(
+        "slip_factor",
+        default_value="0.3",
+        description="Track slip coefficient [0.0, 0.99]",
+    )
+
+    tank_driver = Node(
+        package="saltybot_tank_driver",
+        executable="tank_driver_node",
+        name="tank_driver",
+        parameters=[
+            params_file,
+            {
+                "serial_port": LaunchConfiguration("serial_port"),
+                "drive_mode":  LaunchConfiguration("drive_mode"),
+                "slip_factor": LaunchConfiguration("slip_factor"),
+            },
+        ],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    return LaunchDescription([
+        serial_port_arg,
+        drive_mode_arg,
+        slip_factor_arg,
+        tank_driver,
+    ])

jetson/ros2_ws/src/saltybot_tank_driver/package.xml

@@ -0,0 +1,23 @@
+<?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_tank_driver</name>
+  <version>0.1.0</version>
+  <description>SaltyTank tracked-vehicle ESC driver with slip compensation (Issue #122)</description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_tank_driver/saltybot_tank_driver/kinematics.py

@@ -0,0 +1,203 @@
+"""
+kinematics.py — Tank / tracked-vehicle kinematics for SaltyTank (Issue #122).
+
+Drive modes
+───────────
+  "2wd"     — standard differential drive, 2 ESC channels (CH1=left, CH3=right).
+              Mirrored to 4 channels by the caller (CH1=CH2=left, CH3=CH4=right).
+
+  "4wd"     — skid-steer 4-channel.  Left pair (CH1+CH2) share one speed;
+              right pair (CH3+CH4) share one speed.  No slip compensation.
+
+  "tracked" — tank kinematics with track-slip compensation.
+              Commanded angular is scaled up by 1/(1-slip_factor) so the
+              vehicle achieves the desired turn rate despite track slip.
+              Odometry angular is scaled down by (1-slip_factor) to reflect
+              the fact that wheel-speed difference overestimates actual yaw rate.
+
+Slip compensation model
+───────────────────────
+  slip_factor ∈ [0.0, 0.99]
+
+  0.0  → rigid wheel / no slip (identical to standard diff-drive)
+  0.1  → light slip (rubber tracks on concrete)
+  0.3  → moderate slip (rubber tracks on grass / gravel)  ← default
+  0.5  → heavy slip (metal tracks on mud)
+
+  Command path  : angular_cmd = angular_z / (1 - slip_factor)
+  Odometry path : angular_odom = (v_right - v_left) / B × (1 - slip_factor)
+
+  The two corrections are complementary:  a vehicle commanded at ω rad/s and
+  estimated by odometry will yield consistent trajectories regardless of the
+  chosen slip_factor value (as long as it is consistent between command and odom).
+
+Zero-radius turns
+─────────────────
+  Setting linear_x=0 and angular_z≠0 produces equal-and-opposite track speeds,
+  which is a valid zero-radius pivot for tracked vehicles.  The slip factor still
+  applies; set slip_factor=0 for a surface with known high traction.
+
+Pure module — no ROS2 dependency; fully unit-testable.
+"""
+
+from __future__ import annotations
+
+import math
+
+
+# ── Constants ────────────────────────────────────────────────────────────────
+
+SLIP_MIN = 0.0
+SLIP_MAX = 0.99       # must stay < 1.0 to avoid division-by-zero
+DRIVE_MODES = ("2wd", "4wd", "tracked")
+
+
+# ── Low-level helpers ────────────────────────────────────────────────────────
+
+def _clamp_slip(slip_factor: float) -> float:
+    return max(SLIP_MIN, min(SLIP_MAX, slip_factor))
+
+
+# ── Track-speed computation ──────────────────────────────────────────────────
+
+def tank_speeds(
+    linear_x: float,
+    angular_z: float,
+    track_width: float,
+    slip_factor: float = 0.0,
+) -> tuple[float, float]:
+    """
+    Unicycle → tank (tracked) differential drive with optional slip compensation.
+
+    Parameters
+    ----------
+    linear_x    : forward velocity (m/s)
+    angular_z   : desired yaw rate (rad/s, +ve = CCW / left turn)
+    track_width : centre-to-centre track separation (m)
+    slip_factor : track slip coefficient ∈ [0.0, 0.99]; 0 = no slip
+
+    Returns
+    -------
+    (v_left, v_right) in m/s
+    """
+    slip = _clamp_slip(slip_factor)
+    # Scale angular command up to achieve desired turn rate despite slip
+    angular_cmd = angular_z / (1.0 - slip) if slip > 0.0 else angular_z
+    half = angular_cmd * track_width / 2.0
+    return linear_x - half, linear_x + half
+
+
+def skid_steer_speeds(
+    linear_x: float,
+    angular_z: float,
+    track_width: float,
+) -> tuple[float, float]:
+    """
+    Standard unicycle → differential drive (no slip compensation).
+    Used for 2WD and 4WD modes.
+    """
+    half = angular_z * track_width / 2.0
+    return linear_x - half, linear_x + half
+
+
+# ── m/s → ESC PWM ────────────────────────────────────────────────────────────
+
+def speed_to_pwm(
+    speed_ms: float,
+    max_speed_ms: float,
+    pwm_neutral_us: int,
+    pwm_range_us: int,
+) -> int:
+    """
+    Convert wheel/track speed (m/s) to ESC PWM pulse width (µs).
+
+    Linear mapping:
+        pwm = neutral + (speed / max_speed) * range
+
+    Clamped to [neutral - range, neutral + range].
+    """
+    if max_speed_ms <= 0.0:
+        return pwm_neutral_us
+    normalised = max(-1.0, min(1.0, speed_ms / max_speed_ms))
+    pwm = pwm_neutral_us + normalised * pwm_range_us
+    lo  = pwm_neutral_us - pwm_range_us
+    hi  = pwm_neutral_us + pwm_range_us
+    return int(round(max(lo, min(hi, pwm))))
+
+
+# ── Variant-aware speed computation ─────────────────────────────────────────
+
+def compute_track_speeds(
+    linear_x: float,
+    angular_z: float,
+    track_width: float,
+    max_speed_ms: float,
+    drive_mode: str = "tracked",
+    slip_factor: float = 0.3,
+) -> tuple[float, float]:
+    """
+    Compute and clip left/right track/wheel speeds for the selected mode.
+
+    Returns
+    -------
+    (v_left, v_right) clipped to ±max_speed_ms.
+
+    All modes produce a 2-element tuple; the caller expands to 4 channels
+    for 4WD mode by mirroring (vl, vl, vr, vr).
+    """
+    def clip(v: float) -> float:
+        return max(-max_speed_ms, min(max_speed_ms, v))
+
+    if drive_mode == "tracked":
+        vl, vr = tank_speeds(linear_x, angular_z, track_width, slip_factor)
+    else:
+        # "2wd" or "4wd" — standard diff-drive, no slip compensation
+        vl, vr = skid_steer_speeds(linear_x, angular_z, track_width)
+
+    return clip(vl), clip(vr)
+
+
+def expand_to_4ch(v_left: float, v_right: float) -> tuple[float, float, float, float]:
+    """
+    Expand left/right speeds to 4-channel ESC layout.
+
+    Channel assignment:
+        CH1 = left-front   CH2 = left-rear
+        CH3 = right-front  CH4 = right-rear
+    """
+    return v_left, v_left, v_right, v_right
+
+
+# ── Odometry helper ──────────────────────────────────────────────────────────
+
+def odometry_from_track_speeds(
+    v_left: float,
+    v_right: float,
+    track_width: float,
+    slip_factor: float = 0.0,
+) -> tuple[float, float]:
+    """
+    Estimate (linear_x, angular_z) from left/right track speeds.
+
+    Applies slip compensation to the angular estimate:
+        angular_z_odom = (v_right - v_left) / B × (1 - slip_factor)
+
+    This is the inverse of tank_speeds for consistent dead-reckoning.
+
+    Parameters
+    ----------
+    v_left, v_right : measured track speeds (m/s)
+    track_width     : centre-to-centre track separation (m)
+    slip_factor     : same value used in the command path
+
+    Returns
+    -------
+    (linear_x, angular_z) in m/s and rad/s
+    """
+    linear_x = (v_left + v_right) / 2.0
+    if track_width > 0.0:
+        slip = _clamp_slip(slip_factor)
+        angular_z = (v_right - v_left) / track_width * (1.0 - slip)
+    else:
+        angular_z = 0.0
+    return linear_x, angular_z

jetson/ros2_ws/src/saltybot_tank_driver/saltybot_tank_driver/tank_driver_node.py

@@ -0,0 +1,383 @@
+"""
+tank_driver_node.py — SaltyTank tracked-vehicle ESC driver (Issue #122).
+
+Hardware
+────────
+  SaltyTank: tracked robot with left/right independent brushless ESCs.
+  ESCs controlled via PWM (servo-style 1000–2000 µs pulses).
+  Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge.
+
+  ESC channel assignments (configurable):
+    CH1 = left-front  (or left-track in 2WD/tracked mode)
+    CH2 = left-rear   (mirrored in 2WD/tracked mode)
+    CH3 = right-front (or right-track in 2WD/tracked mode)
+    CH4 = right-rear  (mirrored in 2WD/tracked mode)
+
+Drive modes
+───────────
+  Selectable at runtime via `drive_mode` parameter (default "tracked").
+
+  "2wd"     — standard differential drive; CH1+CH2 = left, CH3+CH4 = right.
+              No slip compensation.
+
+  "4wd"     — skid-steer; all four ESCs commanded.  Left pair same speed,
+              right pair same speed.  No slip compensation.
+
+  "tracked" — tank kinematics with slip compensation.  Angular command is
+              pre-scaled to achieve the desired yaw rate despite track slip.
+              Odometry angular is de-scaled consistently.
+
+  Switch live: ros2 param set /tank_driver drive_mode tracked
+
+Serial protocol
+────────────────
+  Command frame (ASCII, newline-terminated):
+    P<ch1_us>,<ch2_us>,<ch3_us>,<ch4_us>\\n
+    Values: 1000–2000 µs (1500 = neutral/brake)
+
+  Emergency stop frame:
+    E\\n  — forces all channels to neutral immediately
+
+  Heartbeat:
+    H\\n  every heartbeat_period_s (ESC controller zeros PWM after timeout)
+
+Emergency stop
+──────────────
+  Two stop paths:
+    1. Software watchdog  — /cmd_vel silence > watchdog_timeout_s → neutral
+    2. E-stop topic       — /saltybot/e_stop (std_msgs/Bool, True = stop)
+
+  Both paths send P1500,1500,1500,1500\\n and latch until a new /cmd_vel arrives
+  (e-stop topic) or new commands come in (watchdog).
+
+Subscribes
+──────────
+  /cmd_vel             (geometry_msgs/Twist) — Nav2-compatible velocity command
+                         linear.x  = forward (m/s)
+                         angular.z = yaw rate (rad/s, +CCW)
+  /saltybot/e_stop     (std_msgs/Bool)       — True = emergency stop
+
+Publishes
+─────────
+  /saltybot/tank_pwm   (std_msgs/String JSON) — current PWM values
+  /saltybot/tank_odom  (nav_msgs/Odometry)    — dead-reckoning odometry
+
+Parameters
+──────────
+  serial_port        /dev/ttyUSB_tank   (or /dev/ttyACM0, etc.)
+  baud_rate          115200
+  drive_mode         "tracked"          — "2wd" | "4wd" | "tracked"
+  track_width        0.30               — left↔right track centre distance (m)
+  wheelbase          0.32               — front↔rear axle distance (m)
+  max_speed_ms       1.5                — m/s at full ESC deflection
+  max_linear_vel     1.0                — clamp on cmd_vel linear.x (m/s)
+  max_angular_vel    2.5                — clamp on cmd_vel angular.z (rad/s)
+  slip_factor        0.3                — track slip coefficient [0.0, 0.99]
+  pwm_neutral_us     1500
+  pwm_range_us       500
+  watchdog_timeout_s 0.3                — tighter than rover (0.5 s) for safety
+  heartbeat_period_s 0.2
+  control_rate       50.0               — Hz
+
+Usage
+─────
+  ros2 launch saltybot_tank_driver tank_driver.launch.py
+  ros2 param set /tank_driver drive_mode 4wd
+  ros2 param set /tank_driver slip_factor 0.1
+  ros2 topic pub /saltybot/e_stop std_msgs/Bool '{data: true}' --once
+"""
+
+import json
+import math
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Bool, String
+
+from saltybot_tank_driver.kinematics import (
+    compute_track_speeds,
+    expand_to_4ch,
+    odometry_from_track_speeds,
+    speed_to_pwm,
+    DRIVE_MODES,
+)
+
+try:
+    import serial
+    _SERIAL_OK = True
+except ImportError:
+    _SERIAL_OK = False
+
+
+class TankDriverNode(Node):
+
+    def __init__(self):
+        super().__init__("tank_driver")
+
+        # ── Parameters ───────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",        "/dev/ttyUSB_tank")
+        self.declare_parameter("baud_rate",          115200)
+        self.declare_parameter("drive_mode",         "tracked")
+        self.declare_parameter("track_width",        0.30)
+        self.declare_parameter("wheelbase",          0.32)
+        self.declare_parameter("max_speed_ms",       1.5)
+        self.declare_parameter("max_linear_vel",     1.0)
+        self.declare_parameter("max_angular_vel",    2.5)
+        self.declare_parameter("slip_factor",        0.3)
+        self.declare_parameter("pwm_neutral_us",     1500)
+        self.declare_parameter("pwm_range_us",       500)
+        self.declare_parameter("watchdog_timeout_s", 0.3)
+        self.declare_parameter("heartbeat_period_s", 0.2)
+        self.declare_parameter("control_rate",       50.0)
+
+        self._p = self._load_params()
+
+        # ── State ────────────────────────────────────────────────────────────
+        self._target_linear_x:  float = 0.0
+        self._target_angular_z: float = 0.0
+        self._last_cmd_vel_t:   float = 0.0
+        self._last_pwm:         tuple = (1500, 1500, 1500, 1500)
+        self._e_stop:           bool  = False
+
+        self._odom_x:   float = 0.0
+        self._odom_y:   float = 0.0
+        self._odom_yaw: float = 0.0
+        self._last_odom_t: float = time.monotonic()
+
+        # ── Serial ───────────────────────────────────────────────────────────
+        self._ser = None
+        self._ser_lock = threading.Lock()
+        if _SERIAL_OK:
+            self._open_serial()
+        else:
+            self.get_logger().warn(
+                "pyserial not installed — running in simulation mode (no serial I/O)")
+
+        # ── QoS ──────────────────────────────────────────────────────────────
+        reliable = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        self.create_subscription(Twist, "/cmd_vel",        self._cmd_vel_cb, reliable)
+        self.create_subscription(Bool,  "/saltybot/e_stop", self._e_stop_cb,  reliable)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._pwm_pub  = self.create_publisher(String,   "/saltybot/tank_pwm",  reliable)
+        self._odom_pub = self.create_publisher(Odometry, "/saltybot/tank_odom", reliable)
+
+        # ── Timers ───────────────────────────────────────────────────────────
+        rate = self._p["control_rate"]
+        self._ctrl_timer = self.create_timer(1.0 / rate,           self._control_cb)
+        self._hb_timer   = self.create_timer(self._p["hb_period"], self._heartbeat_cb)
+
+        self.get_logger().info(
+            f"TankDriverNode ready  "
+            f"mode={self._p['drive_mode']}  "
+            f"track={self._p['track_width']}m  "
+            f"slip={self._p['slip_factor']}  "
+            f"max_speed={self._p['max_speed_ms']}m/s  "
+            f"port={self._p['serial_port']}"
+        )
+
+    # ── Parameter load ────────────────────────────────────────────────────────
+
+    def _load_params(self) -> dict:
+        mode = self.get_parameter("drive_mode").value
+        if mode not in DRIVE_MODES:
+            self.get_logger().warn(
+                f"Unknown drive_mode '{mode}', defaulting to 'tracked'")
+            mode = "tracked"
+        return {
+            "serial_port":    self.get_parameter("serial_port").value,
+            "baud_rate":      self.get_parameter("baud_rate").value,
+            "drive_mode":     mode,
+            "track_width":    self.get_parameter("track_width").value,
+            "wheelbase":      self.get_parameter("wheelbase").value,
+            "max_speed_ms":   self.get_parameter("max_speed_ms").value,
+            "max_linear_vel": self.get_parameter("max_linear_vel").value,
+            "max_angular_vel":self.get_parameter("max_angular_vel").value,
+            "slip_factor":    float(self.get_parameter("slip_factor").value),
+            "pwm_neutral":    int(self.get_parameter("pwm_neutral_us").value),
+            "pwm_range":      int(self.get_parameter("pwm_range_us").value),
+            "watchdog_timeout": self.get_parameter("watchdog_timeout_s").value,
+            "hb_period":      self.get_parameter("heartbeat_period_s").value,
+            "control_rate":   self.get_parameter("control_rate").value,
+        }
+
+    # ── Serial helpers ────────────────────────────────────────────────────────
+
+    def _open_serial(self) -> None:
+        with self._ser_lock:
+            try:
+                self._ser = serial.Serial(
+                    self._p["serial_port"], self._p["baud_rate"], timeout=0.05)
+                self.get_logger().info(f"Serial open: {self._p['serial_port']}")
+            except Exception as exc:
+                self.get_logger().error(f"Cannot open serial: {exc}")
+                self._ser = None
+
+    def _write(self, data: bytes) -> bool:
+        with self._ser_lock:
+            if self._ser is None or not self._ser.is_open:
+                return False
+            try:
+                self._ser.write(data)
+                return True
+            except Exception as exc:
+                self.get_logger().error(f"Serial write: {exc}",
+                                        throttle_duration_sec=2.0)
+                self._ser = None
+                return False
+
+    def _send_neutral(self) -> None:
+        self._write(b"P1500,1500,1500,1500\n")
+        self._last_pwm = (1500, 1500, 1500, 1500)
+
+    # ── cmd_vel callback ──────────────────────────────────────────────────────
+
+    def _cmd_vel_cb(self, msg: Twist) -> None:
+        p = self._p
+        self._target_linear_x  = max(-p["max_linear_vel"],
+                                      min(p["max_linear_vel"],  msg.linear.x))
+        self._target_angular_z = max(-p["max_angular_vel"],
+                                      min(p["max_angular_vel"], msg.angular.z))
+        self._last_cmd_vel_t = time.monotonic()
+        # A new cmd_vel clears a latched e-stop
+        if self._e_stop:
+            self._e_stop = False
+            self.get_logger().info("E-stop cleared by new cmd_vel")
+
+    # ── E-stop callback ───────────────────────────────────────────────────────
+
+    def _e_stop_cb(self, msg: Bool) -> None:
+        if msg.data and not self._e_stop:
+            self._e_stop = True
+            self._send_neutral()
+            self.get_logger().warn("Emergency stop engaged")
+        elif not msg.data and self._e_stop:
+            self._e_stop = False
+            self.get_logger().info("E-stop released via topic")
+
+    # ── 50 Hz control loop ────────────────────────────────────────────────────
+
+    def _control_cb(self) -> None:
+        self._p = self._load_params()
+        p   = self._p
+        now = time.monotonic()
+
+        # E-stop or watchdog
+        cmd_age = (now - self._last_cmd_vel_t) if self._last_cmd_vel_t > 0.0 else 1e9
+        if self._e_stop or cmd_age > p["watchdog_timeout"]:
+            lin_x, ang_z = 0.0, 0.0
+        else:
+            lin_x, ang_z = self._target_linear_x, self._target_angular_z
+
+        # Kinematics → left/right track speeds
+        vl, vr = compute_track_speeds(
+            lin_x, ang_z,
+            track_width=p["track_width"],
+            max_speed_ms=p["max_speed_ms"],
+            drive_mode=p["drive_mode"],
+            slip_factor=p["slip_factor"],
+        )
+
+        # Expand to 4 channels
+        speeds4 = expand_to_4ch(vl, vr)   # (vl, vl, vr, vr)
+
+        # Convert to PWM
+        def to_pwm(v):
+            return speed_to_pwm(v, p["max_speed_ms"], p["pwm_neutral"], p["pwm_range"])
+
+        pwm = tuple(to_pwm(v) for v in speeds4)
+        self._last_pwm = pwm
+
+        # Send P command
+        frame = f"P{pwm[0]},{pwm[1]},{pwm[2]},{pwm[3]}\n".encode("ascii")
+        self._write(frame)
+
+        # Publish observability
+        pwm_msg = String()
+        pwm_msg.data = json.dumps({
+            "ch1_us": pwm[0], "ch2_us": pwm[1],
+            "ch3_us": pwm[2], "ch4_us": pwm[3],
+            "drive_mode": p["drive_mode"],
+            "e_stop": self._e_stop,
+        })
+        self._pwm_pub.publish(pwm_msg)
+
+        # Odometry integration (dead reckoning with slip compensation)
+        dt = now - self._last_odom_t
+        self._last_odom_t = now
+        self._integrate_odometry(vl, vr, p["track_width"], p["slip_factor"], dt)
+        self._publish_odom()
+
+    # ── Heartbeat ─────────────────────────────────────────────────────────────
+
+    def _heartbeat_cb(self) -> None:
+        self._write(b"H\n")
+
+    # ── Odometry ─────────────────────────────────────────────────────────────
+
+    def _integrate_odometry(
+        self,
+        v_left: float,
+        v_right: float,
+        track_width: float,
+        slip_factor: float,
+        dt: float,
+    ) -> None:
+        """2D dead-reckoning with track-slip compensation."""
+        lin, ang = odometry_from_track_speeds(v_left, v_right, track_width, slip_factor)
+        self._odom_yaw += ang * dt
+        self._odom_x   += lin * math.cos(self._odom_yaw) * dt
+        self._odom_y   += lin * math.sin(self._odom_yaw) * dt
+
+    def _publish_odom(self) -> None:
+        yaw = self._odom_yaw
+        msg = Odometry()
+        msg.header.stamp    = self.get_clock().now().to_msg()
+        msg.header.frame_id = "odom"
+        msg.child_frame_id  = "base_link"
+
+        msg.pose.pose.position.x = self._odom_x
+        msg.pose.pose.position.y = self._odom_y
+        msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
+        msg.pose.pose.orientation.w = math.cos(yaw / 2.0)
+
+        msg.twist.twist.linear.x  = self._target_linear_x
+        msg.twist.twist.angular.z = self._target_angular_z
+
+        self._odom_pub.publish(msg)
+
+    # ── Lifecycle ─────────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        self._send_neutral()      # safe-state on shutdown
+        super().destroy_node()
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TankDriverNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_tank_driver/setup.cfg

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

jetson/ros2_ws/src/saltybot_tank_driver/setup.py

@@ -0,0 +1,32 @@
+from setuptools import setup, find_packages
+import os
+from glob import glob
+
+package_name = "saltybot_tank_driver"
+
+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="SaltyTank tracked-vehicle ESC driver with slip compensation",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            f"tank_driver_node = {package_name}.tank_driver_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_tank_driver/test/test_tank_kinematics.py

@@ -0,0 +1,347 @@
+"""
+test_tank_kinematics.py — Unit tests for kinematics.py (Issue #122).
+
+No ROS2 runtime required — pure Python.
+"""
+
+import math
+import pytest
+
+from saltybot_tank_driver.kinematics import (
+    tank_speeds,
+    skid_steer_speeds,
+    speed_to_pwm,
+    compute_track_speeds,
+    expand_to_4ch,
+    odometry_from_track_speeds,
+    _clamp_slip,
+    DRIVE_MODES,
+    SLIP_MIN,
+    SLIP_MAX,
+)
+
+
+# ─── _clamp_slip ─────────────────────────────────────────────────────────────
+
+class TestClampSlip:
+
+    def test_zero_passes_through(self):
+        assert _clamp_slip(0.0) == 0.0
+
+    def test_typical_value(self):
+        assert _clamp_slip(0.3) == pytest.approx(0.3)
+
+    def test_below_min_clamped(self):
+        assert _clamp_slip(-0.1) == SLIP_MIN
+
+    def test_above_max_clamped(self):
+        assert _clamp_slip(1.0) == SLIP_MAX
+
+    def test_at_max_boundary(self):
+        assert _clamp_slip(0.99) == pytest.approx(0.99)
+
+    def test_large_value_clamped(self):
+        assert _clamp_slip(5.0) == SLIP_MAX
+
+
+# ─── tank_speeds ─────────────────────────────────────────────────────────────
+
+class TestTankSpeeds:
+
+    def test_straight_no_slip(self):
+        vl, vr = tank_speeds(1.0, 0.0, 0.30, slip_factor=0.0)
+        assert vl == pytest.approx(1.0)
+        assert vr == pytest.approx(1.0)
+
+    def test_point_turn_no_slip(self):
+        """Pure CCW rotation with no slip."""
+        vl, vr = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.0)
+        assert vl == pytest.approx(-0.15)
+        assert vr == pytest.approx(0.15)
+
+    def test_point_turn_cw_no_slip(self):
+        vl, vr = tank_speeds(0.0, -1.0, 0.30, slip_factor=0.0)
+        assert vl == pytest.approx(0.15)
+        assert vr == pytest.approx(-0.15)
+
+    def test_zero_inputs(self):
+        vl, vr = tank_speeds(0.0, 0.0, 0.30, slip_factor=0.0)
+        assert vl == pytest.approx(0.0)
+        assert vr == pytest.approx(0.0)
+
+    def test_slip_increases_angular_command(self):
+        """With slip>0, angular component is amplified so output diff is larger."""
+        vl0, vr0 = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.0)
+        vl3, vr3 = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.3)
+        # More slip → larger differential to achieve same ω
+        assert abs(vr3 - vl3) > abs(vr0 - vl0)
+
+    def test_slip_zero_equals_skid_steer(self):
+        """slip_factor=0 must be identical to skid_steer_speeds."""
+        vl_t, vr_t = tank_speeds(0.7, 0.4, 0.30, slip_factor=0.0)
+        vl_s, vr_s = skid_steer_speeds(0.7, 0.4, 0.30)
+        assert vl_t == pytest.approx(vl_s)
+        assert vr_t == pytest.approx(vr_s)
+
+    def test_known_slip_value(self):
+        """slip=0.5 → angular scaled by 1/(1-0.5) = 2."""
+        vl, vr = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.5)
+        expected_half = (1.0 / (1.0 - 0.5)) * 0.30 / 2.0   # = 0.30
+        assert vl == pytest.approx(-expected_half)
+        assert vr == pytest.approx(expected_half)
+
+    def test_negative_slip_clamped_to_zero(self):
+        """Negative slip_factor is clamped to 0."""
+        vl_neg, vr_neg = tank_speeds(0.0, 1.0, 0.30, slip_factor=-0.5)
+        vl_0,   vr_0   = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.0)
+        assert vl_neg == pytest.approx(vl_0)
+        assert vr_neg == pytest.approx(vr_0)
+
+    def test_symmetry_linear(self):
+        """Reversing linear_x reverses both speeds equally."""
+        vl_f, vr_f = tank_speeds(1.0, 0.5, 0.30, slip_factor=0.2)
+        vl_r, vr_r = tank_speeds(-1.0, 0.5, 0.30, slip_factor=0.2)
+        assert vl_r == pytest.approx(vl_f - 2.0)
+        assert vr_r == pytest.approx(vr_f - 2.0)
+
+    def test_zero_radius_turn(self):
+        """lin=0, ang≠0 → equal and opposite speeds (zero-radius pivot)."""
+        vl, vr = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.0)
+        assert vl == pytest.approx(-vr)
+
+    def test_zero_track_width_no_crash(self):
+        vl, vr = tank_speeds(1.0, 2.0, 0.0, slip_factor=0.0)
+        assert vl == pytest.approx(1.0)
+        assert vr == pytest.approx(1.0)
+
+
+# ─── skid_steer_speeds ───────────────────────────────────────────────────────
+
+class TestSkidSteerSpeeds:
+
+    def test_straight(self):
+        vl, vr = skid_steer_speeds(1.0, 0.0, 0.30)
+        assert vl == pytest.approx(1.0)
+        assert vr == pytest.approx(1.0)
+
+    def test_point_turn(self):
+        vl, vr = skid_steer_speeds(0.0, 1.0, 0.30)
+        assert vl == pytest.approx(-0.15)
+        assert vr == pytest.approx(0.15)
+
+    def test_curve(self):
+        vl, vr = skid_steer_speeds(1.0, 1.0, 0.30)
+        assert vl == pytest.approx(1.0 - 0.15)
+        assert vr == pytest.approx(1.0 + 0.15)
+
+
+# ─── speed_to_pwm ────────────────────────────────────────────────────────────
+
+class TestSpeedToPwm:
+
+    def test_zero_speed_neutral(self):
+        assert speed_to_pwm(0.0, 1.5, 1500, 500) == 1500
+
+    def test_full_forward(self):
+        assert speed_to_pwm(1.5, 1.5, 1500, 500) == 2000
+
+    def test_full_reverse(self):
+        assert speed_to_pwm(-1.5, 1.5, 1500, 500) == 1000
+
+    def test_half_forward(self):
+        assert speed_to_pwm(0.75, 1.5, 1500, 500) == 1750
+
+    def test_clamp_over(self):
+        assert speed_to_pwm(10.0, 1.5, 1500, 500) == 2000
+
+    def test_clamp_under(self):
+        assert speed_to_pwm(-10.0, 1.5, 1500, 500) == 1000
+
+    def test_zero_max_returns_neutral(self):
+        assert speed_to_pwm(1.0, 0.0, 1500, 500) == 1500
+
+    def test_returns_int(self):
+        assert isinstance(speed_to_pwm(0.5, 1.5, 1500, 500), int)
+
+    def test_rounding(self):
+        # 1.0/1.5 = 0.6667 → 1500 + 0.6667*500 = 1833.33 → 1833
+        pwm = speed_to_pwm(1.0, 1.5, 1500, 500)
+        assert pwm == 1833
+
+
+# ─── compute_track_speeds ────────────────────────────────────────────────────
+
+class TestComputeTrackSpeeds:
+
+    def test_returns_two_values(self):
+        result = compute_track_speeds(1.0, 0.0, 0.30, 1.5)
+        assert len(result) == 2
+
+    def test_straight_tracked(self):
+        vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode="tracked", slip_factor=0.3)
+        assert vl == pytest.approx(1.0)
+        assert vr == pytest.approx(1.0)
+
+    def test_straight_2wd(self):
+        vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode="2wd")
+        assert vl == pytest.approx(1.0)
+        assert vr == pytest.approx(1.0)
+
+    def test_straight_4wd(self):
+        vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode="4wd")
+        assert vl == pytest.approx(1.0)
+        assert vr == pytest.approx(1.0)
+
+    def test_clipping_tracked(self):
+        vl, vr = compute_track_speeds(1.5, 5.0, 0.30, 1.5, drive_mode="tracked", slip_factor=0.3)
+        assert -1.5 <= vl <= 1.5
+        assert -1.5 <= vr <= 1.5
+
+    def test_clipping_2wd(self):
+        vl, vr = compute_track_speeds(1.5, 5.0, 0.30, 1.5, drive_mode="2wd")
+        assert -1.5 <= vl <= 1.5
+        assert -1.5 <= vr <= 1.5
+
+    def test_tracked_differs_from_2wd_on_turn(self):
+        """With slip>0, tracked mode should produce a different differential."""
+        vl_t, vr_t = compute_track_speeds(0.0, 1.0, 0.30, 1.5,
+                                           drive_mode="tracked", slip_factor=0.3)
+        vl_2, vr_2 = compute_track_speeds(0.0, 1.0, 0.30, 1.5,
+                                           drive_mode="2wd", slip_factor=0.3)
+        assert abs(vr_t - vl_t) > abs(vr_2 - vl_2)
+
+    def test_unknown_mode_falls_back(self):
+        """Unknown drive_mode: falls through to skid_steer (no slip comp)."""
+        vl, vr = compute_track_speeds(1.0, 0.5, 0.30, 1.5, drive_mode="bogus")
+        vl_ref, vr_ref = compute_track_speeds(1.0, 0.5, 0.30, 1.5, drive_mode="2wd")
+        assert vl == pytest.approx(vl_ref)
+        assert vr == pytest.approx(vr_ref)
+
+    def test_zero_pivot_tracked(self):
+        vl, vr = compute_track_speeds(0.0, 1.0, 0.30, 1.5, drive_mode="tracked", slip_factor=0.0)
+        assert vl == pytest.approx(-vr)
+
+    def test_reverse_straight(self):
+        vl, vr = compute_track_speeds(-1.0, 0.0, 0.30, 1.5, drive_mode="tracked")
+        assert vl == pytest.approx(-1.0)
+        assert vr == pytest.approx(-1.0)
+
+
+# ─── expand_to_4ch ───────────────────────────────────────────────────────────
+
+class TestExpandTo4ch:
+
+    def test_returns_four_values(self):
+        result = expand_to_4ch(1.0, -1.0)
+        assert len(result) == 4
+
+    def test_left_pair(self):
+        vfl, vlr, vrf, vrr = expand_to_4ch(0.5, 1.0)
+        assert vfl == pytest.approx(0.5)
+        assert vlr == pytest.approx(0.5)
+
+    def test_right_pair(self):
+        vfl, vlr, vrf, vrr = expand_to_4ch(0.5, 1.0)
+        assert vrf == pytest.approx(1.0)
+        assert vrr == pytest.approx(1.0)
+
+    def test_zero_inputs(self):
+        assert expand_to_4ch(0.0, 0.0) == (0.0, 0.0, 0.0, 0.0)
+
+    def test_negative(self):
+        vfl, vlr, vrf, vrr = expand_to_4ch(-0.5, -0.5)
+        assert all(v == pytest.approx(-0.5) for v in (vfl, vlr, vrf, vrr))
+
+
+# ─── odometry_from_track_speeds ──────────────────────────────────────────────
+
+class TestOdometryFromTrackSpeeds:
+
+    def test_straight_no_slip(self):
+        lin, ang = odometry_from_track_speeds(1.0, 1.0, 0.30, slip_factor=0.0)
+        assert lin == pytest.approx(1.0)
+        assert ang == pytest.approx(0.0)
+
+    def test_point_turn_ccw_no_slip(self):
+        vl, vr = -0.15, 0.15
+        lin, ang = odometry_from_track_speeds(vl, vr, 0.30, slip_factor=0.0)
+        assert lin == pytest.approx(0.0)
+        assert ang == pytest.approx(1.0)
+
+    def test_slip_reduces_angular_estimate(self):
+        """With slip, measured angular is scaled down."""
+        lin0, ang0 = odometry_from_track_speeds(-0.15, 0.15, 0.30, slip_factor=0.0)
+        lin3, ang3 = odometry_from_track_speeds(-0.15, 0.15, 0.30, slip_factor=0.3)
+        assert abs(ang3) < abs(ang0)
+
+    def test_known_slip_value(self):
+        """slip=0.5 → angular halved."""
+        lin, ang = odometry_from_track_speeds(-0.15, 0.15, 0.30, slip_factor=0.5)
+        # (0.15 - -0.15) / 0.30 * (1 - 0.5) = 1.0 * 0.5
+        assert ang == pytest.approx(0.5)
+
+    def test_zero_track_width_no_crash(self):
+        lin, ang = odometry_from_track_speeds(1.0, 1.0, 0.0, slip_factor=0.0)
+        assert lin == pytest.approx(1.0)
+        assert ang == pytest.approx(0.0)
+
+    def test_zero_inputs(self):
+        lin, ang = odometry_from_track_speeds(0.0, 0.0, 0.30, slip_factor=0.0)
+        assert lin == pytest.approx(0.0)
+        assert ang == pytest.approx(0.0)
+
+    def test_reverse_straight(self):
+        lin, ang = odometry_from_track_speeds(-1.0, -1.0, 0.30, slip_factor=0.0)
+        assert lin == pytest.approx(-1.0)
+        assert ang == pytest.approx(0.0)
+
+
+# ─── Integration: command ↔ odometry round-trip ──────────────────────────────
+
+class TestRoundTrip:
+
+    @pytest.mark.parametrize("slip", [0.0, 0.1, 0.3, 0.5])
+    @pytest.mark.parametrize("lin,ang", [
+        (1.0,  0.0),
+        (0.0,  1.0),
+        (0.5,  0.5),
+        (-0.5, 0.3),
+    ])
+    def test_roundtrip_tracked(self, lin, ang, slip):
+        """
+        Commanding (lin, ang) then estimating back via odometry should recover
+        (lin, ang) for all slip values, provided the speeds are unclipped.
+        """
+        track = 0.30
+        max_speed = 5.0   # generous to avoid clipping in test
+        vl, vr = compute_track_speeds(lin, ang, track, max_speed,
+                                       drive_mode="tracked", slip_factor=slip)
+        lin_out, ang_out = odometry_from_track_speeds(vl, vr, track, slip_factor=slip)
+        assert lin_out == pytest.approx(lin, abs=1e-9)
+        assert ang_out == pytest.approx(ang, abs=1e-9)
+
+    def test_zero_pivot_pwm_symmetric(self):
+        """Zero-radius pivot: left and right PWMs are symmetric around neutral."""
+        vl, vr = compute_track_speeds(0.0, 1.0, 0.30, 1.5,
+                                       drive_mode="tracked", slip_factor=0.0)
+        pwm_l = speed_to_pwm(vl, 1.5, 1500, 500)
+        pwm_r = speed_to_pwm(vr, 1.5, 1500, 500)
+        assert pwm_l + pwm_r == 3000   # symmetric: 1500 - x + 1500 + x
+
+    def test_all_modes_forward_equal(self):
+        """Straight forward: all drive modes produce the same speeds."""
+        for mode in DRIVE_MODES:
+            vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode=mode)
+            assert vl == pytest.approx(1.0)
+            assert vr == pytest.approx(1.0)
+
+    def test_4ch_expansion_pwm(self):
+        """4-channel expansion: all channels driven during straight motion."""
+        vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode="4wd")
+        pwms = [speed_to_pwm(v, 1.5, 1500, 500) for v in expand_to_4ch(vl, vr)]
+        assert all(p > 1500 for p in pwms)
+
+    def test_pwm_neutral_when_stopped(self):
+        vl, vr = compute_track_speeds(0.0, 0.0, 0.30, 1.5, drive_mode="tracked")
+        for v in (vl, vr):
+            assert speed_to_pwm(v, 1.5, 1500, 500) == 1500

src/jlink.c

@@ -0,0 +1,279 @@
+#include "jlink.h"
+#include "config.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+/* ---- DMA circular RX buffer ---- */
+#define JLINK_RX_BUF_LEN    128u   /* must be power-of-2 */
+static uint8_t  s_rx_buf[JLINK_RX_BUF_LEN];
+static uint32_t s_rx_tail = 0;     /* consumer index (byte already processed) */
+
+/* ---- HAL handles ---- */
+static UART_HandleTypeDef s_uart;
+static DMA_HandleTypeDef  s_dma_rx;
+
+/* ---- Volatile state ---- */
+volatile JLinkState jlink_state;
+
+/* ---- CRC16-XModem (poly 0x1021, init 0x0000) ---- */
+static uint16_t crc16_xmodem(const uint8_t *data, uint16_t len)
+{
+    uint16_t crc = 0x0000u;
+    for (uint16_t i = 0; i < len; i++) {
+        crc ^= (uint16_t)data[i] << 8;
+        for (uint8_t b = 0; b < 8; b++) {
+            if (crc & 0x8000u)
+                crc = (crc << 1) ^ 0x1021u;
+            else
+                crc <<= 1;
+        }
+    }
+    return crc;
+}
+
+/* ---- jlink_init() ---- */
+void jlink_init(void)
+{
+    /* GPIO: PB6=TX AF7 (USART1_TX), PB7=RX AF7 (USART1_RX) */
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    GPIO_InitTypeDef gpio = {0};
+    gpio.Pin       = GPIO_PIN_6 | GPIO_PIN_7;
+    gpio.Mode      = GPIO_MODE_AF_PP;
+    gpio.Pull      = GPIO_PULLUP;
+    gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
+    gpio.Alternate = GPIO_AF7_USART1;
+    HAL_GPIO_Init(GPIOB, &gpio);
+
+    /* DMA2 Stream2 Channel4 — USART1_RX circular */
+    __HAL_RCC_DMA2_CLK_ENABLE();
+    s_dma_rx.Instance                 = DMA2_Stream2;
+    s_dma_rx.Init.Channel             = DMA_CHANNEL_4;
+    s_dma_rx.Init.Direction           = DMA_PERIPH_TO_MEMORY;
+    s_dma_rx.Init.PeriphInc           = DMA_PINC_DISABLE;
+    s_dma_rx.Init.MemInc              = DMA_MINC_ENABLE;
+    s_dma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    s_dma_rx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
+    s_dma_rx.Init.Mode                = DMA_CIRCULAR;
+    s_dma_rx.Init.Priority            = DMA_PRIORITY_MEDIUM;
+    s_dma_rx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
+    HAL_DMA_Init(&s_dma_rx);
+    __HAL_LINKDMA(&s_uart, hdmarx, s_dma_rx);
+
+    /* USART1 at JLINK_BAUD (921600) */
+    __HAL_RCC_USART1_CLK_ENABLE();
+    s_uart.Instance          = USART1;
+    s_uart.Init.BaudRate     = JLINK_BAUD;
+    s_uart.Init.WordLength   = UART_WORDLENGTH_8B;
+    s_uart.Init.StopBits     = UART_STOPBITS_1;
+    s_uart.Init.Parity       = UART_PARITY_NONE;
+    s_uart.Init.Mode         = UART_MODE_TX_RX;
+    s_uart.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
+    s_uart.Init.OverSampling = UART_OVERSAMPLING_16;
+    HAL_UART_Init(&s_uart);
+
+    /* Enable USART1 IDLE interrupt for circular buffer draining */
+    __HAL_UART_ENABLE_IT(&s_uart, UART_IT_IDLE);
+    HAL_NVIC_SetPriority(USART1_IRQn, 6, 0);
+    HAL_NVIC_EnableIRQ(USART1_IRQn);
+
+    /* DMA2_Stream2 IRQ (for error handling) */
+    HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 7, 0);
+    HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
+
+    /* Start circular DMA RX — never stops */
+    HAL_UART_Receive_DMA(&s_uart, s_rx_buf, JLINK_RX_BUF_LEN);
+
+    memset((void *)&jlink_state, 0, sizeof(jlink_state));
+    s_rx_tail = 0;
+}
+
+/* ---- IRQ handlers ---- */
+void USART1_IRQHandler(void)
+{
+    /* Clear IDLE flag by reading SR then DR */
+    if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) {
+        __HAL_UART_CLEAR_IDLEFLAG(&s_uart);
+        /* jlink_process() drains the buffer from main loop — no work here */
+    }
+    HAL_UART_IRQHandler(&s_uart);
+}
+
+void DMA2_Stream2_IRQHandler(void)
+{
+    HAL_DMA_IRQHandler(&s_dma_rx);
+}
+
+/* ---- jlink_is_active() ---- */
+bool jlink_is_active(uint32_t now_ms)
+{
+    if (jlink_state.last_rx_ms == 0u) return false;
+    return (now_ms - jlink_state.last_rx_ms) < JLINK_HB_TIMEOUT_MS;
+}
+
+/* ---- Frame dispatch ---- */
+static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
+{
+    /* Update heartbeat timestamp on every valid frame */
+    jlink_state.last_rx_ms = HAL_GetTick();
+
+    switch (cmd) {
+
+    case JLINK_CMD_HEARTBEAT:
+        /* Heartbeat only — no payload action needed */
+        break;
+
+    case JLINK_CMD_DRIVE:
+        if (plen == 4u) {
+            int16_t spd, str;
+            memcpy(&spd, payload,     2);
+            memcpy(&str, payload + 2, 2);
+            /* Clamp to ±1000 */
+            if (spd >  1000) spd =  1000;
+            if (spd < -1000) spd = -1000;
+            if (str >  1000) str =  1000;
+            if (str < -1000) str = -1000;
+            jlink_state.speed = spd;
+            jlink_state.steer = str;
+        }
+        break;
+
+    case JLINK_CMD_ARM:
+        jlink_state.arm_req = 1u;
+        break;
+
+    case JLINK_CMD_DISARM:
+        jlink_state.disarm_req = 1u;
+        break;
+
+    case JLINK_CMD_PID_SET:
+        if (plen == 12u) {
+            float kp, ki, kd;
+            memcpy(&kp, payload,     4);
+            memcpy(&ki, payload + 4, 4);
+            memcpy(&kd, payload + 8, 4);
+            /* Sanity bounds — same as USB CDC PID handler in main.c */
+            if (kp >= 0.0f && kp <= 500.0f) jlink_state.pid_kp = kp;
+            if (ki >= 0.0f && ki <= 50.0f)  jlink_state.pid_ki = ki;
+            if (kd >= 0.0f && kd <= 50.0f)  jlink_state.pid_kd = kd;
+            jlink_state.pid_updated = 1u;
+        }
+        break;
+
+    case JLINK_CMD_ESTOP:
+        jlink_state.estop_req = 1u;
+        break;
+
+    default:
+        break;
+    }
+}
+
+/* ---- jlink_process() — call from main loop every tick ---- */
+/*
+ * Parser state machine.
+ * Frame: [STX][LEN][CMD][PAYLOAD 0..LEN-1][CRC_hi][CRC_lo][ETX]
+ * LEN = count of CMD + PAYLOAD bytes (1..253).
+ * CRC16-XModem over CMD+PAYLOAD.
+ * Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255,
+ * but we cap at 64 for safety).
+ */
+#define JLINK_MAX_PAYLOAD   64u
+
+typedef enum {
+    PS_WAIT_STX = 0,
+    PS_WAIT_LEN,
+    PS_WAIT_DATA,   /* receiving CMD + PAYLOAD (len bytes total) */
+    PS_WAIT_CRC_HI,
+    PS_WAIT_CRC_LO,
+    PS_WAIT_ETX,
+} ParseState;
+
+void jlink_process(void)
+{
+    static ParseState s_state   = PS_WAIT_STX;
+    static uint8_t    s_len     = 0;    /* expected CMD+PAYLOAD length */
+    static uint8_t    s_count   = 0;    /* bytes received so far in PS_WAIT_DATA */
+    static uint8_t    s_frame[JLINK_MAX_PAYLOAD + 1u]; /* [0]=CMD, [1..]=PAYLOAD */
+    static uint8_t    s_crc_hi  = 0;
+
+    /* Compute how many bytes the DMA has written since last drain */
+    uint32_t head = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx);
+    uint32_t bytes = (head - s_rx_tail) & (JLINK_RX_BUF_LEN - 1u);
+
+    for (uint32_t i = 0; i < bytes; i++) {
+        uint8_t b = s_rx_buf[s_rx_tail];
+        s_rx_tail = (s_rx_tail + 1u) & (JLINK_RX_BUF_LEN - 1u);
+
+        switch (s_state) {
+        case PS_WAIT_STX:
+            if (b == JLINK_STX) s_state = PS_WAIT_LEN;
+            break;
+
+        case PS_WAIT_LEN:
+            if (b == 0u || b > JLINK_MAX_PAYLOAD + 1u) {
+                /* Invalid length — resync */
+                s_state = PS_WAIT_STX;
+            } else {
+                s_len   = b;
+                s_count = 0;
+                s_state = PS_WAIT_DATA;
+            }
+            break;
+
+        case PS_WAIT_DATA:
+            s_frame[s_count++] = b;
+            if (s_count == s_len) s_state = PS_WAIT_CRC_HI;
+            break;
+
+        case PS_WAIT_CRC_HI:
+            s_crc_hi = b;
+            s_state  = PS_WAIT_CRC_LO;
+            break;
+
+        case PS_WAIT_CRC_LO: {
+            uint16_t rx_crc  = ((uint16_t)s_crc_hi << 8) | b;
+            uint16_t calc_crc = crc16_xmodem(s_frame, s_len);
+            if (rx_crc == calc_crc)
+                s_state = PS_WAIT_ETX;
+            else
+                s_state = PS_WAIT_STX;   /* CRC mismatch — drop */
+            break;
+        }
+
+        case PS_WAIT_ETX:
+            if (b == JLINK_ETX) {
+                /* Valid frame: s_frame[0]=CMD, s_frame[1..s_len-1]=PAYLOAD */
+                dispatch(s_frame + 1, s_frame[0], s_len - 1u);
+            }
+            /* Either way, go back to idle (resync on bad ETX) */
+            s_state = PS_WAIT_STX;
+            break;
+        }
+    }
+}
+
+/* ---- jlink_send_telemetry() ---- */
+void jlink_send_telemetry(const jlink_tlm_status_t *status)
+{
+    /*
+     * Frame: [STX][LEN][0x80][20 bytes STATUS][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 (CMD) + 20 (payload) = 21
+     * Total frame length = 1+1+1+20+2+1 = 26 bytes
+     * At 921600 baud (10 bits/byte): 26×10/921600 ≈ 0.28ms — safe to block.
+     */
+    static uint8_t frame[26];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_status_t);  /* 20 */
+    const uint8_t  len  = 1u + plen;                            /* 21 */
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_STATUS;
+    memcpy(&frame[3], status, plen);
+
+    uint16_t crc = crc16_xmodem(&frame[2], len);  /* over CMD + PAYLOAD */
+    frame[3 + plen]     = (uint8_t)(crc >> 8);
+    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
+    frame[3 + plen + 2] = JLINK_ETX;
+
+    HAL_UART_Transmit(&s_uart, frame, sizeof(frame), 5u);
+}

src/main.c

@@ -16,6 +16,7 @@
 #include "bmp280.h"
 #include "mag.h"
 #include "jetson_cmd.h"
+#include "jlink.h"
 #include "battery.h"
 #include <math.h>
 #include <string.h>
@@ -139,6 +140,9 @@ int main(void) {
     /* Init CRSF/ELRS receiver on UART4 (PA0/PA1) with DMA */
     crsf_init();
 
+    /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
+    jlink_init();
+
     /* Init mode manager (RC/autonomous blend; CH6 mode switch) */
     mode_manager_t mode;
     mode_manager_init(&mode);
@@ -172,6 +176,7 @@ int main(void) {
     uint32_t balance_tick = 0;
     uint32_t esc_tick = 0;
     uint32_t crsf_telem_tick = 0;  /* CRSF uplink telemetry TX timer */
+    uint32_t jlink_tlm_tick = 0;   /* Jetson binary telemetry TX timer */
     const float dt = 1.0f / PID_LOOP_HZ;  /* 1ms at 1kHz */
 
     while (1) {
@@ -199,6 +204,37 @@ int main(void) {
                 safety_remote_estop_clear();
         }
 
+        /* Drain JLink DMA circular buffer and parse frames */
+        jlink_process();
+
+        /* Handle JLink one-shot flags from Jetson binary protocol */
+        if (jlink_state.estop_req) {
+            jlink_state.estop_req = 0u;
+            safety_remote_estop(ESTOP_REMOTE);
+            safety_arm_cancel();
+            balance_disarm(&bal);
+            motor_driver_estop(&motors);
+        }
+        if (jlink_state.arm_req) {
+            jlink_state.arm_req = 0u;
+            if (!safety_remote_estop_active() &&
+                    mpu6000_is_calibrated() &&
+                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
+                safety_arm_start(now);
+            }
+        }
+        if (jlink_state.disarm_req) {
+            jlink_state.disarm_req = 0u;
+            safety_arm_cancel();
+            balance_disarm(&bal);
+        }
+        if (jlink_state.pid_updated) {
+            jlink_state.pid_updated = 0u;
+            bal.kp = jlink_state.pid_kp;
+            bal.ki = jlink_state.pid_ki;
+            bal.kd = jlink_state.pid_kd;
+        }
+
         /* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off.
          * Applies regardless of active mode (CH5 always has kill authority). */
         if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
@@ -279,11 +315,15 @@ int main(void) {
             jetson_cmd_process();
         }
 
-        /* Balance PID at 1kHz — apply Jetson speed offset when active */
+        /* Balance PID at 1kHz — apply Jetson speed offset when active.
+         * jlink takes priority; fall back to legacy CDC jetson_cmd if alive. */
         if (imu_ret == 0 && now - balance_tick >= 1) {
             balance_tick = now;
             float base_sp = bal.setpoint;
-            if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset();
+            if (jlink_is_active(now))
+                bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
+            else if (jetson_cmd_is_active(now))
+                bal.setpoint += jetson_cmd_sp_offset();
             balance_update(&bal, &imu, dt);
             bal.setpoint = base_sp;
         }
@@ -297,8 +337,11 @@ int main(void) {
         }
 
         /* Feed autonomous steer from Jetson into mode manager.
+         * jlink takes priority over legacy CDC jetson_cmd.
          * mode_manager_get_steer() blends it with RC steer per active mode. */
-        if (jetson_cmd_is_active(now))
+        if (jlink_is_active(now))
+            mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0);
+        else if (jetson_cmd_is_active(now))
             mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);
 
         /* Send to hoverboard ESC at 50Hz (every 20ms) */
@@ -329,6 +372,30 @@ int main(void) {
             crsf_send_flight_mode(bal.state == BALANCE_ARMED);
         }
 
+        /* JLink STATUS telemetry at JLINK_TLM_HZ (50Hz) back to Jetson */
+        if (now - jlink_tlm_tick >= (1000u / JLINK_TLM_HZ)) {
+            jlink_tlm_tick = now;
+            jlink_tlm_status_t tlm;
+            tlm.pitch_x10    = (int16_t)(bal.pitch_deg * 10.0f);
+            tlm.roll_x10     = (int16_t)(imu.roll * 10.0f);
+            tlm.yaw_x10      = (int16_t)(imu.yaw  * 10.0f);
+            tlm.motor_cmd    = bal.motor_cmd;
+            uint32_t vbat    = battery_read_mv();
+            tlm.vbat_mv      = (vbat > 65535u) ? 65535u : (uint16_t)vbat;
+            tlm.rssi_dbm     = crsf_state.rssi_dbm;
+            tlm.link_quality = crsf_state.link_quality;
+            tlm.balance_state = (uint8_t)bal.state;
+            tlm.rc_armed     = crsf_state.armed ? 1u : 0u;
+            tlm.mode         = (uint8_t)mode_manager_active(&mode);
+            EstopSource _es  = safety_get_estop();
+            tlm.estop        = (uint8_t)_es;
+            tlm.soc_pct      = battery_estimate_pct(vbat);
+            tlm.fw_major     = FW_MAJOR;
+            tlm.fw_minor     = FW_MINOR;
+            tlm.fw_patch     = FW_PATCH;
+            jlink_send_telemetry(&tlm);
+        }
+
         /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
         if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
             send_tick = now;
@@ -391,8 +458,9 @@ int main(void) {
                 if (_rs >= ESTOP_REMOTE) es = (int)_rs;
                 else if (bal.state == BALANCE_TILT_FAULT) es = ESTOP_TILT;
                 else es = ESTOP_CLEAR;
-                n = snprintf(p, rem, ",\"ja\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n",
+                n = snprintf(p, rem, ",\"ja\":%d,\"jl\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n",
                     jetson_cmd_is_active(now) ? 1 : 0,
+                    jlink_is_active(now) ? 1 : 0,
                     (unsigned)tx_count,
                     (unsigned)cdc_rx_count, es);
                 len = (int)(p + n - buf);

test/test_jlink_frames.py

@@ -0,0 +1,416 @@
+"""
+test_jlink_frames.py — Issue #120: JLink binary protocol unit tests.
+
+Tests CRC16-XModem, frame building, frame parsing (state machine),
+command payload encoding, and telemetry frame layout.
+
+No HAL or STM32 hardware required — pure Python logic.
+"""
+
+import struct
+import pytest
+
+
+# ---------------------------------------------------------------------------
+# CRC16-XModem (poly 0x1021, init 0x0000) — mirrors crc16_xmodem() in jlink.c
+# ---------------------------------------------------------------------------
+
+def crc16_xmodem(data: bytes) -> int:
+    crc = 0x0000
+    for byte in data:
+        crc ^= byte << 8
+        for _ in range(8):
+            if crc & 0x8000:
+                crc = (crc << 1) ^ 0x1021
+            else:
+                crc <<= 1
+        crc &= 0xFFFF
+    return crc
+
+
+# ---------------------------------------------------------------------------
+# Frame builder — mirrors jlink_send_telemetry() structure
+# ---------------------------------------------------------------------------
+
+JLINK_STX = 0x02
+JLINK_ETX = 0x03
+
+JLINK_CMD_HEARTBEAT = 0x01
+JLINK_CMD_DRIVE     = 0x02
+JLINK_CMD_ARM       = 0x03
+JLINK_CMD_DISARM    = 0x04
+JLINK_CMD_PID_SET   = 0x05
+JLINK_CMD_ESTOP     = 0x07
+JLINK_TLM_STATUS    = 0x80
+
+
+def build_frame(cmd: int, payload: bytes = b"") -> bytes:
+    """Build a JLink frame: [STX][LEN][CMD][PAYLOAD][CRC_hi][CRC_lo][ETX]"""
+    body = bytes([cmd]) + payload
+    length = len(body)
+    crc = crc16_xmodem(body)
+    return bytes([JLINK_STX, length, cmd]) + payload + bytes([crc >> 8, crc & 0xFF, JLINK_ETX])
+
+
+def parse_frames(data: bytes) -> list:
+    """
+    Parse all valid JLink frames from a byte stream.
+    Returns list of (cmd, payload) tuples for each valid frame.
+    """
+    WAIT_STX, WAIT_LEN, WAIT_DATA, WAIT_CRC_HI, WAIT_CRC_LO, WAIT_ETX = range(6)
+    state = WAIT_STX
+    results = []
+    length = 0
+    buf = []
+    crc_hi = 0
+
+    for b in data:
+        if state == WAIT_STX:
+            if b == JLINK_STX:
+                state = WAIT_LEN
+        elif state == WAIT_LEN:
+            if b == 0 or b > 65:
+                state = WAIT_STX
+            else:
+                length = b
+                buf = []
+                state = WAIT_DATA
+        elif state == WAIT_DATA:
+            buf.append(b)
+            if len(buf) == length:
+                state = WAIT_CRC_HI
+        elif state == WAIT_CRC_HI:
+            crc_hi = b
+            state = WAIT_CRC_LO
+        elif state == WAIT_CRC_LO:
+            rx_crc = (crc_hi << 8) | b
+            calc_crc = crc16_xmodem(bytes(buf))
+            if rx_crc == calc_crc:
+                state = WAIT_ETX
+            else:
+                state = WAIT_STX
+        elif state == WAIT_ETX:
+            if b == JLINK_ETX:
+                cmd = buf[0]
+                payload = bytes(buf[1:])
+                results.append((cmd, payload))
+            state = WAIT_STX
+
+    return results
+
+
+# ---------------------------------------------------------------------------
+# CRC16 Tests
+# ---------------------------------------------------------------------------
+
+class TestCRC16XModem:
+
+    def test_empty(self):
+        assert crc16_xmodem(b"") == 0x0000
+
+    def test_known_vector_1(self):
+        # CRC16/XMODEM of b"123456789" = 0x31C3
+        assert crc16_xmodem(b"123456789") == 0x31C3
+
+    def test_single_zero(self):
+        assert crc16_xmodem(b"\x00") == 0x0000
+
+    def test_single_ff(self):
+        assert crc16_xmodem(b"\xFF") == 0x1EF0
+
+    def test_heartbeat_cmd_byte(self):
+        # CRC of just CMD=0x01
+        crc = crc16_xmodem(bytes([JLINK_CMD_HEARTBEAT]))
+        assert isinstance(crc, int)
+        assert 0 <= crc <= 0xFFFF
+
+    def test_drive_payload(self):
+        payload = struct.pack("<hh", 500, -300)  # speed=500, steer=-300
+        body = bytes([JLINK_CMD_DRIVE]) + payload
+        crc = crc16_xmodem(body)
+        # Verify round-trip: same data same CRC
+        assert crc == crc16_xmodem(body)
+
+    def test_different_payloads_different_crc(self):
+        a = crc16_xmodem(b"\x02\x01\x02")
+        b = crc16_xmodem(b"\x02\x01\x03")
+        assert a != b
+
+    def test_zero_bytes(self):
+        assert crc16_xmodem(b"\x00\x00\x00\x00") != crc16_xmodem(b"\x00\x00\x00\x01")
+
+
+# ---------------------------------------------------------------------------
+# Frame Building Tests
+# ---------------------------------------------------------------------------
+
+class TestFrameBuilding:
+
+    def test_heartbeat_frame_length(self):
+        frame = build_frame(JLINK_CMD_HEARTBEAT)
+        # [STX][LEN=1][CMD=0x01][CRC_hi][CRC_lo][ETX] = 6 bytes
+        assert len(frame) == 6
+
+    def test_heartbeat_frame_markers(self):
+        frame = build_frame(JLINK_CMD_HEARTBEAT)
+        assert frame[0] == JLINK_STX
+        assert frame[-1] == JLINK_ETX
+
+    def test_heartbeat_frame_len_field(self):
+        frame = build_frame(JLINK_CMD_HEARTBEAT)
+        assert frame[1] == 1  # LEN = 1 (CMD only, no payload)
+
+    def test_heartbeat_frame_cmd(self):
+        frame = build_frame(JLINK_CMD_HEARTBEAT)
+        assert frame[2] == JLINK_CMD_HEARTBEAT
+
+    def test_drive_frame_length(self):
+        payload = struct.pack("<hh", 0, 0)
+        frame = build_frame(JLINK_CMD_DRIVE, payload)
+        # [STX][LEN=5][CMD][s16][s16][CRC_hi][CRC_lo][ETX] = 1+1+1+4+2+1 = 10 bytes
+        assert len(frame) == 10
+
+    def test_drive_frame_len_field(self):
+        payload = struct.pack("<hh", 0, 0)
+        frame = build_frame(JLINK_CMD_DRIVE, payload)
+        assert frame[1] == 5  # 1 (CMD) + 4 (payload)
+
+    def test_arm_frame(self):
+        frame = build_frame(JLINK_CMD_ARM)
+        assert len(frame) == 6
+        assert frame[2] == JLINK_CMD_ARM
+
+    def test_disarm_frame(self):
+        frame = build_frame(JLINK_CMD_DISARM)
+        assert len(frame) == 6
+        assert frame[2] == JLINK_CMD_DISARM
+
+    def test_estop_frame(self):
+        frame = build_frame(JLINK_CMD_ESTOP)
+        assert len(frame) == 6
+        assert frame[2] == JLINK_CMD_ESTOP
+
+    def test_pid_set_frame_length(self):
+        payload = struct.pack("<fff", 35.0, 1.0, 1.0)
+        frame = build_frame(JLINK_CMD_PID_SET, payload)
+        # [STX][LEN=13][CMD][12 bytes floats][CRC_hi][CRC_lo][ETX] = 18 bytes
+        assert len(frame) == 18
+
+    def test_pid_set_frame_len_field(self):
+        payload = struct.pack("<fff", 35.0, 1.0, 1.0)
+        frame = build_frame(JLINK_CMD_PID_SET, payload)
+        assert frame[1] == 13  # 1 (CMD) + 12 (three floats)
+
+    def test_crc_coverage(self):
+        """CRC must differ if payload differs by one bit."""
+        payload_a = struct.pack("<hh", 500, 100)
+        payload_b = struct.pack("<hh", 500, 101)
+        frame_a = build_frame(JLINK_CMD_DRIVE, payload_a)
+        frame_b = build_frame(JLINK_CMD_DRIVE, payload_b)
+        # Extract CRC bytes (second to last two bytes before ETX)
+        crc_a = (frame_a[-3] << 8) | frame_a[-2]
+        crc_b = (frame_b[-3] << 8) | frame_b[-2]
+        assert crc_a != crc_b
+
+
+# ---------------------------------------------------------------------------
+# Frame Parsing Tests
+# ---------------------------------------------------------------------------
+
+class TestFrameParsing:
+
+    def test_parse_single_heartbeat(self):
+        frame = build_frame(JLINK_CMD_HEARTBEAT)
+        results = parse_frames(frame)
+        assert len(results) == 1
+        cmd, payload = results[0]
+        assert cmd == JLINK_CMD_HEARTBEAT
+        assert payload == b""
+
+    def test_parse_drive_cmd(self):
+        payload = struct.pack("<hh", 750, -200)
+        frame = build_frame(JLINK_CMD_DRIVE, payload)
+        results = parse_frames(frame)
+        assert len(results) == 1
+        cmd, data = results[0]
+        assert cmd == JLINK_CMD_DRIVE
+        speed, steer = struct.unpack("<hh", data)
+        assert speed == 750
+        assert steer == -200
+
+    def test_parse_multiple_frames(self):
+        stream = (
+            build_frame(JLINK_CMD_HEARTBEAT) +
+            build_frame(JLINK_CMD_ARM) +
+            build_frame(JLINK_CMD_DRIVE, struct.pack("<hh", 100, 50))
+        )
+        results = parse_frames(stream)
+        assert len(results) == 3
+        assert results[0][0] == JLINK_CMD_HEARTBEAT
+        assert results[1][0] == JLINK_CMD_ARM
+        assert results[2][0] == JLINK_CMD_DRIVE
+
+    def test_bad_crc_dropped(self):
+        frame = bytearray(build_frame(JLINK_CMD_HEARTBEAT))
+        frame[-2] ^= 0xFF  # Corrupt CRC low byte
+        results = parse_frames(bytes(frame))
+        assert len(results) == 0
+
+    def test_bad_etx_dropped(self):
+        frame = bytearray(build_frame(JLINK_CMD_HEARTBEAT))
+        frame[-1] = 0xFF  # Wrong ETX
+        results = parse_frames(bytes(frame))
+        assert len(results) == 0
+
+    def test_noise_before_frame(self):
+        noise = bytes([0xAA, 0xBB, 0xCC, 0xDD])
+        frame = build_frame(JLINK_CMD_ARM)
+        results = parse_frames(noise + frame)
+        assert len(results) == 1
+        assert results[0][0] == JLINK_CMD_ARM
+
+    def test_two_frames_concatenated(self):
+        f1 = build_frame(JLINK_CMD_DISARM)
+        f2 = build_frame(JLINK_CMD_HEARTBEAT)
+        results = parse_frames(f1 + f2)
+        assert len(results) == 2
+
+    def test_pid_set_parsed_correctly(self):
+        kp, ki, kd = 42.5, 0.5, 2.0
+        payload = struct.pack("<fff", kp, ki, kd)
+        frame = build_frame(JLINK_CMD_PID_SET, payload)
+        results = parse_frames(frame)
+        assert len(results) == 1
+        cmd, data = results[0]
+        assert cmd == JLINK_CMD_PID_SET
+        got_kp, got_ki, got_kd = struct.unpack("<fff", data)
+        assert abs(got_kp - kp) < 1e-5
+        assert abs(got_ki - ki) < 1e-5
+        assert abs(got_kd - kd) < 1e-5
+
+    def test_drive_boundary_values(self):
+        for spd, str_ in [(1000, 1000), (-1000, -1000), (0, 0)]:
+            frame = build_frame(JLINK_CMD_DRIVE, struct.pack("<hh", spd, str_))
+            results = parse_frames(frame)
+            assert len(results) == 1
+            speed, steer = struct.unpack("<hh", results[0][1])
+            assert speed == spd
+            assert steer == str_
+
+
+# ---------------------------------------------------------------------------
+# Telemetry STATUS Frame Tests
+# ---------------------------------------------------------------------------
+
+# STATUS payload struct: <hhhhH b B B B B B B B B B
+# pitch_x10, roll_x10, yaw_x10, motor_cmd, vbat_mv,
+# rssi_dbm, link_quality, balance_state, rc_armed,
+# mode, estop, soc_pct, fw_major, fw_minor, fw_patch
+STATUS_FMT = "<hhhhHbBBBBBBBBB"  # 20 bytes
+STATUS_SIZE = struct.calcsize(STATUS_FMT)
+
+
+def make_status(pitch=0, roll=0, yaw=0, motor_cmd=0, vbat_mv=12600,
+                rssi_dbm=-85, link_quality=95, balance_state=1,
+                rc_armed=1, mode=2, estop=0, soc_pct=80,
+                fw_major=1, fw_minor=0, fw_patch=0) -> bytes:
+    return struct.pack(STATUS_FMT,
+                       pitch, roll, yaw, motor_cmd, vbat_mv,
+                       rssi_dbm, link_quality, balance_state, rc_armed,
+                       mode, estop, soc_pct, fw_major, fw_minor, fw_patch)
+
+
+class TestStatusTelemetry:
+
+    def test_status_payload_size(self):
+        assert STATUS_SIZE == 20
+
+    def test_status_frame_total_length(self):
+        payload = make_status()
+        frame = build_frame(JLINK_TLM_STATUS, payload)
+        # [STX][LEN=21][0x80][20 bytes][CRC_hi][CRC_lo][ETX] = 26 bytes
+        assert len(frame) == 26
+
+    def test_status_len_field(self):
+        payload = make_status()
+        frame = build_frame(JLINK_TLM_STATUS, payload)
+        assert frame[1] == 21  # 1 (CMD) + 20 (STATUS payload)
+
+    def test_status_cmd_byte(self):
+        payload = make_status()
+        frame = build_frame(JLINK_TLM_STATUS, payload)
+        assert frame[2] == JLINK_TLM_STATUS
+
+    def test_status_parseable(self):
+        payload = make_status(pitch=123, roll=-45, yaw=678, motor_cmd=500,
+                               vbat_mv=11800, rssi_dbm=-90, link_quality=80,
+                               balance_state=1, rc_armed=1, mode=2, estop=0,
+                               soc_pct=60, fw_major=1, fw_minor=0, fw_patch=0)
+        frame = build_frame(JLINK_TLM_STATUS, payload)
+        results = parse_frames(frame)
+        assert len(results) == 1
+        cmd, data = results[0]
+        assert cmd == JLINK_TLM_STATUS
+        vals = struct.unpack(STATUS_FMT, data)
+        pitch, roll, yaw, motor_cmd, vbat_mv, rssi_dbm, lq, bs, ra, mode, es, soc, fwma, fwmi, fwpa = vals
+        assert pitch == 123
+        assert roll == -45
+        assert yaw == 678
+        assert motor_cmd == 500
+        assert vbat_mv == 11800
+        assert rssi_dbm == -90
+        assert lq == 80
+        assert bs == 1
+        assert ra == 1
+        assert mode == 2
+        assert es == 0
+        assert soc == 60
+        assert fwma == 1
+        assert fwmi == 0
+        assert fwpa == 0
+
+    def test_pitch_encoding(self):
+        """pitch_deg * 10 fits in int16 for ±3276 degrees."""
+        for deg in [-20.0, 0.0, 5.5, 15.3, -10.1]:
+            encoded = int(deg * 10)
+            payload = make_status(pitch=encoded)
+            frame = build_frame(JLINK_TLM_STATUS, payload)
+            results = parse_frames(frame)
+            assert len(results) == 1
+            vals = struct.unpack(STATUS_FMT, results[0][1])
+            assert vals[0] == encoded
+
+    def test_vbat_range_3s(self):
+        """3S battery 9900–12600 mV fits in uint16."""
+        for mv in [9900, 11100, 12600]:
+            payload = make_status(vbat_mv=mv)
+            frame = build_frame(JLINK_TLM_STATUS, payload)
+            results = parse_frames(frame)
+            vals = struct.unpack(STATUS_FMT, results[0][1])
+            assert vals[4] == mv
+
+    def test_vbat_range_4s(self):
+        """4S battery 13200–16800 mV fits in uint16."""
+        for mv in [13200, 14800, 16800]:
+            payload = make_status(vbat_mv=mv)
+            frame = build_frame(JLINK_TLM_STATUS, payload)
+            results = parse_frames(frame)
+            vals = struct.unpack(STATUS_FMT, results[0][1])
+            assert vals[4] == mv
+
+    def test_fw_version_fields(self):
+        payload = make_status(fw_major=2, fw_minor=3, fw_patch=7)
+        frame = build_frame(JLINK_TLM_STATUS, payload)
+        results = parse_frames(frame)
+        vals = struct.unpack(STATUS_FMT, results[0][1])
+        assert vals[-3] == 2
+        assert vals[-2] == 3
+        assert vals[-1] == 7
+
+    def test_soc_pct_unknown(self):
+        """soc_pct=255 encodes as 0xFF (unknown)."""
+        payload = make_status(soc_pct=255)
+        frame = build_frame(JLINK_TLM_STATUS, payload)
+        results = parse_frames(frame)
+        vals = struct.unpack(STATUS_FMT, results[0][1])
+        assert vals[11] == 255

ui/social-bot/package-lock.json

@@ -10,7 +10,8 @@
       "dependencies": {
         "react": "^18.3.1",
         "react-dom": "^18.3.1",
-        "roslib": "^1.4.1"
+        "roslib": "^1.4.1",
+        "three": "^0.170.0"
       },
       "devDependencies": {
         "@vitejs/plugin-react": "^4.3.1",
@@ -2695,6 +2696,12 @@
         "node": ">=0.8"
       }
     },
+    "node_modules/three": {
+      "version": "0.170.0",
+      "resolved": "https://registry.npmjs.org/three/-/three-0.170.0.tgz",
+      "integrity": "sha512-FQK+LEpYc0fBD+J8g6oSEyyNzjp+Q7Ks1C568WWaoMRLW+TkNNWmenWeGgJjV105Gd+p/2ql1ZcjYvNiPZBhuQ==",
+      "license": "MIT"
+    },
     "node_modules/tinyglobby": {
       "version": "0.2.15",
       "resolved": "https://registry.npmjs.org/tinyglobby/-/tinyglobby-0.2.15.tgz",

ui/social-bot/package.json

@@ -12,7 +12,8 @@
   "dependencies": {
     "react": "^18.3.1",
     "react-dom": "^18.3.1",
-    "roslib": "^1.4.1"
+    "roslib": "^1.4.1",
+    "three": "^0.170.0"
   },
   "devDependencies": {
     "@vitejs/plugin-react": "^4.3.1",

ui/social-bot/src/App.jsx

@@ -1,26 +1,55 @@
 /**
- * App.jsx — Saltybot Social Dashboard root component.
+ * App.jsx — Saltybot Social + Telemetry Dashboard root component.
  *
- * Layout:
- *   [TopBar: connection config + pipeline state badge]
- *   [TabNav: Status | Faces | Conversation | Personality | Navigation]
- *   [TabContent]
+ * Social tabs (issue #107):
+ *   Status | Faces | Conversation | Personality | Navigation
+ *
+ * Telemetry tabs (issue #126):
+ *   IMU | Battery | Motors | Map | Control | Health
  */
 
 import { useState, useCallback } from 'react';
 import { useRosbridge } from './hooks/useRosbridge.js';
+
+// Social panels
 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: '◫' },
+// Telemetry panels
+import { ImuPanel }     from './components/ImuPanel.jsx';
+import { BatteryPanel } from './components/BatteryPanel.jsx';
+import { MotorPanel }   from './components/MotorPanel.jsx';
+import { MapViewer }    from './components/MapViewer.jsx';
+import { ControlMode }  from './components/ControlMode.jsx';
+import { SystemHealth } from './components/SystemHealth.jsx';
+
+const TAB_GROUPS = [
+  {
+    label: 'SOCIAL',
+    color: 'text-cyan-600',
+    tabs: [
+      { id: 'status',       label: 'Status',      },
+      { id: 'faces',        label: 'Faces',       },
+      { id: 'conversation', label: 'Convo',       },
+      { id: 'personality',  label: 'Personality', },
+      { id: 'navigation',   label: 'Nav Mode',    },
+    ],
+  },
+  {
+    label: 'TELEMETRY',
+    color: 'text-amber-600',
+    tabs: [
+      { id: 'imu',     label: 'IMU',     },
+      { id: 'battery', label: 'Battery', },
+      { id: 'motors',  label: 'Motors',  },
+      { id: 'map',     label: 'Map',     },
+      { id: 'control', label: 'Control', },
+      { id: 'health',  label: 'Health',  },
+    ],
+  },
 ];
 
 const DEFAULT_WS_URL = 'ws://localhost:9090';
@@ -29,42 +58,47 @@ function ConnectionBar({ url, setUrl, connected, error }) {
   const [editing, setEditing] = useState(false);
   const [draft, setDraft] = useState(url);
 
-  const handleApply = () => {
-    setUrl(draft);
-    setEditing(false);
-  };
+  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); }}
+            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>
+          <button
+            onClick={handleApply}
+            className="px-2 py-0.5 rounded bg-cyan-950 border border-cyan-700 text-cyan-400 hover:bg-cyan-900 text-xs"
+          >
+            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"
+          className="text-gray-500 hover:text-cyan-400 transition-colors truncate max-w-52"
           title={url}
         >
           {connected ? (
-            <span className="text-green-400">rosbridge: {url}</span>
+            <span className="text-green-400">{url}</span>
           ) : error ? (
-            <span className="text-red-400" title={error}>⚠ {url}</span>
+            <span className="text-red-400">⚠ {url}</span>
           ) : (
-            <span className="text-gray-500">{url} (connecting…)</span>
+            <span className="text-gray-500">{url}</span>
           )}
         </button>
       )}
@@ -77,80 +111,67 @@ export default function App() {
   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]
-  );
+  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>
+          <span className="text-cyan-800 text-xs hidden sm:inline">DASHBOARD</span>
         </div>
-
-        <ConnectionBar
-          url={wsUrl}
-          setUrl={setWsUrl}
-          connected={connected}
-          error={error}
-        />
+        <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) => (
+      {/* ── Tab Navigation ── */}
+      <nav className="bg-[#070712] border-b border-cyan-950 shrink-0 overflow-x-auto">
+        <div className="flex min-w-max">
+          {TAB_GROUPS.map((group, gi) => (
+            <div key={group.label} className="flex items-stretch">
+              {gi > 0 && (
+                <div className="flex items-center px-1">
+                  <div className="w-px h-4 bg-cyan-950" />
+                </div>
+              )}
+              <div className="flex items-center px-1.5 shrink-0">
+                <span className={`text-xs font-bold tracking-widest ${group.color} opacity-60`}>
+                  {group.label}
+                </span>
+              </div>
+              {group.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 ${
+                  className={`px-3 py-2.5 text-xs font-bold tracking-wider 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'
+                      : 'border-transparent text-gray-600 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>
+          ))}
+        </div>
       </nav>
 
       {/* ── Content ── */}
       <main className="flex-1 overflow-y-auto p-4">
-        {activeTab === 'status' && (
-          <StatusPanel subscribe={subscribe} />
-        )}
+        {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} />}
 
-        {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}
-          />
-        )}
+        {activeTab === 'imu'     && <ImuPanel    subscribe={subscribe} />}
+        {activeTab === 'battery' && <BatteryPanel subscribe={subscribe} />}
+        {activeTab === 'motors'  && <MotorPanel   subscribe={subscribe} />}
+        {activeTab === 'map'     && <MapViewer    subscribe={subscribe} />}
+        {activeTab === 'control' && <ControlMode  subscribe={subscribe} />}
+        {activeTab === 'health'  && <SystemHealth subscribe={subscribe} />}
       </main>
 
       {/* ── Footer ── */}

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

@@ -0,0 +1,234 @@
+/**
+ * BatteryPanel.jsx — Battery state monitoring.
+ *
+ * Topics:
+ *   /saltybot/balance_state  (std_msgs/String JSON)
+ *     Fields: motor_cmd, state, mode (no direct voltage yet)
+ *   /diagnostics             (diagnostic_msgs/DiagnosticArray)
+ *     KeyValues: battery_voltage_v, battery_current_a, battery_soc_pct
+ *
+ * NOTE: Dedicated /saltybot/battery (sensor_msgs/BatteryState) can be added
+ *       to cmd_vel_bridge_node once firmware sends voltage/current over USB.
+ *       The panel will pick it up automatically from /diagnostics KeyValues.
+ *
+ * 4S LiPo range: 12.0 V (empty) → 16.8 V (full)
+ */
+
+import { useEffect, useRef, useState } from 'react';
+
+const LIPO_4S_MIN = 12.0;
+const LIPO_4S_MAX = 16.8;
+const HISTORY_MAX = 120; // 2 min at 1 Hz
+
+function socFromVoltage(v) {
+  if (v <= 0) return null;
+  return Math.max(0, Math.min(100, ((v - LIPO_4S_MIN) / (LIPO_4S_MAX - LIPO_4S_MIN)) * 100));
+}
+
+function voltColor(v) {
+  const soc = socFromVoltage(v);
+  if (soc === null) return '#6b7280';
+  if (soc > 50) return '#22c55e';
+  if (soc > 20) return '#f59e0b';
+  return '#ef4444';
+}
+
+function SparklineCanvas({ data, width = 280, height = 60 }) {
+  const ref = useRef(null);
+
+  useEffect(() => {
+    const canvas = ref.current;
+    if (!canvas || data.length < 2) return;
+    const ctx = canvas.getContext('2d');
+    const W = canvas.width, H = canvas.height;
+    ctx.clearRect(0, 0, W, H);
+
+    // Background
+    ctx.fillStyle = '#020208';
+    ctx.fillRect(0, 0, W, H);
+
+    // Grid lines at 25% / 50% / 75%
+    [25, 50, 75].forEach(pct => {
+      const y = H - (pct / 100) * H;
+      ctx.strokeStyle = 'rgba(0,255,255,0.05)';
+      ctx.lineWidth = 0.5;
+      ctx.beginPath(); ctx.moveTo(0, y); ctx.lineTo(W, y); ctx.stroke();
+    });
+
+    // Voltage range for y-axis
+    const minV = Math.min(LIPO_4S_MIN, ...data);
+    const maxV = Math.max(LIPO_4S_MAX, ...data);
+    const rangeV = maxV - minV || 1;
+
+    // Line
+    ctx.strokeStyle = '#06b6d4';
+    ctx.lineWidth = 1.5;
+    ctx.beginPath();
+    data.forEach((v, i) => {
+      const x = (i / (data.length - 1)) * W;
+      const y = H - ((v - minV) / rangeV) * H * 0.9 - H * 0.05;
+      i === 0 ? ctx.moveTo(x, y) : ctx.lineTo(x, y);
+    });
+    ctx.stroke();
+
+    // Fill under
+    ctx.lineTo(W, H); ctx.lineTo(0, H); ctx.closePath();
+    ctx.fillStyle = 'rgba(6,182,212,0.08)';
+    ctx.fill();
+
+    // Latest value label
+    const last = data[data.length - 1];
+    ctx.fillStyle = '#06b6d4';
+    ctx.font = 'bold 10px monospace';
+    ctx.textAlign = 'right';
+    ctx.fillText(`${last.toFixed(2)}V`, W - 3, 12);
+  }, [data]);
+
+  return (
+    <canvas
+      ref={ref}
+      width={width}
+      height={height}
+      className="w-full rounded border border-gray-900 block"
+    />
+  );
+}
+
+export function BatteryPanel({ subscribe }) {
+  const [voltage, setVoltage]   = useState(0);
+  const [current, setCurrent]   = useState(null);
+  const [soc, setSoc]           = useState(null);
+  const [history, setHistory]   = useState([]);
+  const [lastTs, setLastTs]     = useState(null);
+  const lastHistoryPush = useRef(0);
+
+  // /diagnostics — look for battery KeyValues
+  useEffect(() => {
+    const unsub = subscribe(
+      '/diagnostics',
+      'diagnostic_msgs/DiagnosticArray',
+      (msg) => {
+        for (const status of msg.status ?? []) {
+          const kv = {};
+          for (const pair of status.values ?? []) {
+            kv[pair.key] = pair.value;
+          }
+          if (kv.battery_voltage_v !== undefined) {
+            const v = parseFloat(kv.battery_voltage_v);
+            setVoltage(v);
+            setSoc(socFromVoltage(v));
+            setLastTs(Date.now());
+            // Throttle history to ~1 Hz
+            const now = Date.now();
+            if (now - lastHistoryPush.current >= 1000) {
+              lastHistoryPush.current = now;
+              setHistory(h => [...h, v].slice(-HISTORY_MAX));
+            }
+          }
+          if (kv.battery_current_a !== undefined) {
+            setCurrent(parseFloat(kv.battery_current_a));
+          }
+          if (kv.battery_soc_pct !== undefined) {
+            setSoc(parseFloat(kv.battery_soc_pct));
+          }
+        }
+      }
+    );
+    return unsub;
+  }, [subscribe]);
+
+  // Also listen to balance_state for rough motor-current proxy
+  useEffect(() => {
+    const unsub = subscribe('/saltybot/balance_state', 'std_msgs/String', (msg) => {
+      try {
+        const d = JSON.parse(msg.data);
+        // motor_cmd ∈ [-1000..1000] → rough current proxy
+        if (d.motor_cmd !== undefined && current === null) {
+          setCurrent(Math.abs(d.motor_cmd) / 1000 * 20); // rough max 20A
+        }
+      } catch { /* ignore */ }
+    });
+    return unsub;
+  }, [subscribe, current]);
+
+  const socPct   = soc ?? socFromVoltage(voltage) ?? 0;
+  const col      = voltColor(voltage);
+  const stale    = lastTs && Date.now() - lastTs > 10000;
+  const runtime  = (voltage > 0 && current && current > 0.1)
+    ? ((socPct / 100) * 16000 / current / 60).toFixed(0) // rough Wh/V/A estimate
+    : null;
+
+  return (
+    <div className="space-y-4">
+      {/* Main gauges */}
+      <div className="grid grid-cols-2 sm:grid-cols-4 gap-3">
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 col-span-2 sm:col-span-1">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest mb-1">VOLTAGE</div>
+          <div className="text-3xl font-bold" style={{ color: col }}>
+            {voltage > 0 ? voltage.toFixed(2) : '—'}
+          </div>
+          <div className="text-gray-600 text-xs">V {stale ? '(stale)' : ''}</div>
+        </div>
+
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest mb-1">SOC</div>
+          <div className="text-3xl font-bold" style={{ color: col }}>
+            {socPct > 0 ? Math.round(socPct) : '—'}
+          </div>
+          <div className="text-gray-600 text-xs">%</div>
+        </div>
+
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest mb-1">CURRENT</div>
+          <div className="text-3xl font-bold text-orange-400">
+            {current != null ? current.toFixed(1) : '—'}
+          </div>
+          <div className="text-gray-600 text-xs">A {current === null ? '' : '(est.)'}</div>
+        </div>
+
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest mb-1">EST. RUN</div>
+          <div className="text-3xl font-bold text-purple-400">
+            {runtime ?? '—'}
+          </div>
+          <div className="text-gray-600 text-xs">min</div>
+        </div>
+      </div>
+
+      {/* SoC bar */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="flex justify-between text-xs mb-2">
+          <span className="text-cyan-700 font-bold tracking-widest">STATE OF CHARGE</span>
+          <span style={{ color: col }}>{Math.round(socPct)}%</span>
+        </div>
+        <div className="w-full h-4 bg-gray-900 rounded overflow-hidden border border-gray-800">
+          <div
+            className="h-full transition-all duration-1000 rounded"
+            style={{ width: `${socPct}%`, background: col }}
+          />
+        </div>
+        <div className="flex justify-between text-xs mt-1 text-gray-700">
+          <span>{LIPO_4S_MIN}V empty</span>
+          <span>{LIPO_4S_MAX}V full</span>
+        </div>
+      </div>
+
+      {/* Voltage history sparkline */}
+      <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">
+          VOLTAGE HISTORY (2 min)
+        </div>
+        {history.length >= 2 ? (
+          <SparklineCanvas data={history} height={60} />
+        ) : (
+          <div className="text-gray-600 text-xs text-center py-4 border border-dashed border-gray-800 rounded">
+            Waiting for /diagnostics data…
+            <div className="mt-1 text-gray-700 text-xs">
+              Requires battery_voltage_v KeyValue in DiagnosticArray
+            </div>
+          </div>
+        )}
+      </div>
+    </div>
+  );
+}

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

@@ -0,0 +1,198 @@
+/**
+ * ControlMode.jsx — RC / Autonomous control mode display.
+ *
+ * Topics:
+ *   /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/balance_state (std_msgs/String JSON) — robot state, mode label
+ */
+
+import { useEffect, useState } from 'react';
+
+const MODE_CONFIG = {
+  RC: {
+    label: 'RC MANUAL',
+    color: 'text-blue-300',
+    bg: 'bg-blue-950',
+    border: 'border-blue-600',
+    description: 'Pilot has full control via CRSF/ELRS RC link',
+  },
+  RAMP_TO_AUTO: {
+    label: 'RAMP → AUTO',
+    color: 'text-amber-300',
+    bg: 'bg-amber-950',
+    border: 'border-amber-600',
+    description: 'Transitioning from RC to autonomous (500 ms blend)',
+  },
+  AUTO: {
+    label: 'AUTONOMOUS',
+    color: 'text-green-300',
+    bg: 'bg-green-950',
+    border: 'border-green-600',
+    description: 'Jetson AI/Nav2 in full control',
+  },
+  RAMP_TO_RC: {
+    label: 'RAMP → RC',
+    color: 'text-orange-300',
+    bg: 'bg-orange-950',
+    border: 'border-orange-600',
+    description: 'Returning control to pilot (500 ms blend)',
+  },
+};
+
+function SafetyFlag({ label, ok, invert }) {
+  const isOk = invert ? !ok : ok;
+  return (
+    <div className={`flex items-center gap-2 rounded px-3 py-2 border ${
+      isOk ? 'bg-green-950 border-green-800' : 'bg-red-950 border-red-800'
+    }`}>
+      <div className={`w-2 h-2 rounded-full ${isOk ? 'bg-green-400' : 'bg-red-400 animate-pulse'}`} />
+      <span className="text-xs">
+        <span className={isOk ? 'text-green-300' : 'text-red-300'}>{label}</span>
+      </span>
+      <span className={`ml-auto text-xs font-bold ${isOk ? 'text-green-400' : 'text-red-400'}`}>
+        {isOk ? 'OK' : 'FAULT'}
+      </span>
+    </div>
+  );
+}
+
+export function ControlMode({ subscribe }) {
+  const [ctrlMode, setCtrlMode] = useState(null);
+  const [balState, setBalState] = useState(null);
+  const [lastTs, setLastTs]     = useState(null);
+
+  useEffect(() => {
+    const unsub = subscribe('/saltybot/control_mode', 'std_msgs/String', (msg) => {
+      try {
+        setCtrlMode(JSON.parse(msg.data));
+        setLastTs(Date.now());
+      } catch { /* ignore */ }
+    });
+    return unsub;
+  }, [subscribe]);
+
+  useEffect(() => {
+    const unsub = subscribe('/saltybot/balance_state', 'std_msgs/String', (msg) => {
+      try { setBalState(JSON.parse(msg.data)); }
+      catch { /* ignore */ }
+    });
+    return unsub;
+  }, [subscribe]);
+
+  const mode     = ctrlMode?.mode ?? 'RC';
+  const cfg      = MODE_CONFIG[mode] ?? MODE_CONFIG.RC;
+  const alpha    = ctrlMode?.blend_alpha ?? 0;
+  const stale    = lastTs && Date.now() - lastTs > 5000;
+
+  return (
+    <div className="space-y-4">
+      {/* Main mode badge */}
+      <div className={`${cfg.bg} border ${cfg.border} rounded-xl p-5`}>
+        <div className="flex items-start justify-between gap-4">
+          <div>
+            <div className="text-gray-500 text-xs font-bold tracking-widest mb-1">CONTROL MODE</div>
+            <div className={`text-3xl font-bold tracking-widest ${cfg.color}`}>{cfg.label}</div>
+            <div className="text-gray-500 text-xs mt-1">{cfg.description}</div>
+          </div>
+          <div className="shrink-0 text-right">
+            <div className="text-gray-600 text-xs">Blend α</div>
+            <div className={`text-2xl font-bold ${alpha > 0.5 ? 'text-green-400' : 'text-blue-400'}`}>
+              {alpha.toFixed(2)}
+            </div>
+            {stale && <div className="text-red-500 text-xs mt-1">STALE</div>}
+          </div>
+        </div>
+
+        {/* Blend alpha bar */}
+        {(mode === 'RAMP_TO_AUTO' || mode === 'RAMP_TO_RC' || alpha > 0) && (
+          <div className="mt-3">
+            <div className="flex justify-between text-xs text-gray-500 mb-1">
+              <span>RC</span>
+              <span>AUTO</span>
+            </div>
+            <div className="w-full h-3 bg-gray-950 rounded overflow-hidden border border-gray-800">
+              <div
+                className="h-full transition-all duration-200 rounded"
+                style={{
+                  width: `${alpha * 100}%`,
+                  background: `linear-gradient(to right, #2563eb, #16a34a)`,
+                }}
+              />
+            </div>
+          </div>
+        )}
+      </div>
+
+      {/* Safety flags */}
+      <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">SAFETY INTERLOCKS</div>
+        <div className="space-y-2">
+          <SafetyFlag label="SLAM Fix"      ok={ctrlMode?.slam_ok ?? false} />
+          <SafetyFlag label="RC Link"       ok={ctrlMode?.rc_link_ok ?? false} />
+          <SafetyFlag label="Stick Override" ok={ctrlMode?.override_active ?? false} invert />
+        </div>
+        {!ctrlMode && (
+          <div className="text-gray-600 text-xs mt-2">
+            Waiting for /saltybot/control_mode…
+          </div>
+        )}
+      </div>
+
+      {/* Balance state detail */}
+      {balState && (
+        <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">BALANCE STATE</div>
+          <div className="grid grid-cols-2 gap-2 text-xs">
+            <div>
+              <span className="text-gray-600">STM32 State: </span>
+              <span className={`font-bold ${
+                balState.state === 'ARMED' ? 'text-green-400' :
+                balState.state === 'TILT FAULT' ? 'text-red-400' : 'text-gray-400'
+              }`}>{balState.state}</span>
+            </div>
+            <div>
+              <span className="text-gray-600">STM32 Mode: </span>
+              <span className="text-cyan-400">{balState.mode}</span>
+            </div>
+            <div>
+              <span className="text-gray-600">Pitch: </span>
+              <span className="text-amber-400">{balState.pitch_deg}°</span>
+            </div>
+            <div>
+              <span className="text-gray-600">Motor CMD: </span>
+              <span className="text-orange-400">{balState.motor_cmd}</span>
+            </div>
+          </div>
+        </div>
+      )}
+
+      {/* Mode transition guide */}
+      <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">STATE MACHINE</div>
+        <div className="flex items-center gap-1 text-xs flex-wrap">
+          {['RC', 'RAMP_TO_AUTO', 'AUTO', 'RAMP_TO_RC'].map((m, i) => (
+            <div key={m} className="flex items-center gap-1">
+              <span className={`px-2 py-0.5 rounded border ${
+                mode === m
+                  ? `${MODE_CONFIG[m].bg} ${MODE_CONFIG[m].border} ${MODE_CONFIG[m].color} font-bold`
+                  : 'border-gray-800 text-gray-600'
+              }`}>{m}</span>
+              {i < 3 && <span className="text-gray-700">→</span>}
+            </div>
+          ))}
+        </div>
+        <div className="mt-2 text-gray-700 text-xs space-y-0.5">
+          <div>AUX2 switch → RC⇄AUTO | Stick &gt; 10% while AUTO → instant RC</div>
+          <div>SLAM fix lost while AUTO → RAMP_TO_RC | RC link lost → instant RC</div>
+        </div>
+      </div>
+    </div>
+  );
+}

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

@@ -0,0 +1,379 @@
+/**
+ * ImuPanel.jsx — IMU attitude visualization.
+ *
+ * Topics:
+ *   /saltybot/imu           (sensor_msgs/Imu)         — quaternion orientation
+ *   /saltybot/balance_state (std_msgs/String JSON)    — pitch/roll/yaw deg,
+ *                                                        motor_cmd, state, mode
+ *
+ * Displays:
+ *   - Artificial horizon canvas (pitch / roll)
+ *   - Compass tape (yaw)
+ *   - Three.js 3D robot orientation cube
+ *   - Numeric readouts + angular velocity
+ */
+
+import { useEffect, useRef, useState, useCallback } from 'react';
+import * as THREE from 'three';
+
+// ── 2D canvas helpers ──────────────────────────────────────────────────────────
+
+function drawHorizon(ctx, W, H, pitch, roll) {
+  const cx = W / 2, cy = H / 2;
+  const rollRad = roll * Math.PI / 180;
+  const pitchPx = pitch * (H / 60);
+
+  ctx.clearRect(0, 0, W, H);
+  ctx.fillStyle = '#051a30';
+  ctx.fillRect(0, 0, W, H);
+
+  ctx.save();
+  ctx.translate(cx, cy);
+  ctx.rotate(-rollRad);
+
+  // Ground
+  ctx.fillStyle = '#1a0f00';
+  ctx.fillRect(-W, pitchPx, W * 2, H * 2);
+
+  // Horizon
+  ctx.strokeStyle = '#00ffff';
+  ctx.lineWidth = 1.5;
+  ctx.beginPath();
+  ctx.moveTo(-W, pitchPx);
+  ctx.lineTo(W, pitchPx);
+  ctx.stroke();
+
+  // Pitch ladder
+  for (let d = -30; d <= 30; d += 10) {
+    if (d === 0) continue;
+    const y = pitchPx + d * (H / 60);
+    const lw = Math.abs(d) % 20 === 0 ? 22 : 14;
+    ctx.strokeStyle = 'rgba(0,210,210,0.4)';
+    ctx.lineWidth = 0.7;
+    ctx.beginPath();
+    ctx.moveTo(-lw, y); ctx.lineTo(lw, y);
+    ctx.stroke();
+    ctx.fillStyle = 'rgba(0,210,210,0.5)';
+    ctx.font = '7px monospace';
+    ctx.textAlign = 'left';
+    ctx.fillText((-d).toString(), lw + 2, y + 3);
+  }
+  ctx.restore();
+
+  // Reticle
+  ctx.strokeStyle = '#f97316';
+  ctx.lineWidth = 1.5;
+  ctx.beginPath();
+  ctx.moveTo(cx - 28, cy); ctx.lineTo(cx - 8, cy);
+  ctx.moveTo(cx + 8, cy);  ctx.lineTo(cx + 28, cy);
+  ctx.moveTo(cx, cy - 4);  ctx.lineTo(cx, cy + 4);
+  ctx.stroke();
+}
+
+function drawCompass(ctx, W, H, yaw) {
+  const cx = W / 2;
+  ctx.clearRect(0, 0, W, H);
+  ctx.fillStyle = '#050510';
+  ctx.fillRect(0, 0, W, H);
+
+  const degPerPx = W / 70;
+  const cardinals = { 0:'N', 45:'NE', 90:'E', 135:'SE', 180:'S', 225:'SW', 270:'W', 315:'NW' };
+
+  for (let i = -35; i <= 35; i++) {
+    const deg = ((Math.round(yaw) + i) % 360 + 360) % 360;
+    const x = cx + i * degPerPx;
+    const isMaj = deg % 45 === 0;
+    const isMed = deg % 15 === 0;
+    if (!isMed && !isMaj) continue;
+
+    ctx.strokeStyle = isMaj ? '#00cccc' : 'rgba(0,200,200,0.3)';
+    ctx.lineWidth = isMaj ? 1.5 : 0.5;
+    const tH = isMaj ? 16 : 7;
+    ctx.beginPath();
+    ctx.moveTo(x, 0); ctx.lineTo(x, tH);
+    ctx.stroke();
+
+    if (isMaj && cardinals[deg] !== undefined) {
+      ctx.fillStyle = deg === 0 ? '#ff4444' : '#00cccc';
+      ctx.font = 'bold 9px monospace';
+      ctx.textAlign = 'center';
+      ctx.fillText(cardinals[deg], x, 28);
+    }
+  }
+
+  const hdg = ((Math.round(yaw) % 360) + 360) % 360;
+  ctx.fillStyle = '#00ffff';
+  ctx.font = 'bold 11px monospace';
+  ctx.textAlign = 'center';
+  ctx.fillText(hdg + '°', cx, H - 4);
+
+  // Pointer
+  ctx.strokeStyle = '#f97316';
+  ctx.lineWidth = 2;
+  ctx.beginPath();
+  ctx.moveTo(cx, 0); ctx.lineTo(cx, 10);
+  ctx.stroke();
+}
+
+// ── Three.js hook ──────────────────────────────────────────────────────────────
+
+function useThreeOrientation(containerRef) {
+  const sceneRef = useRef(null);
+
+  useEffect(() => {
+    const el = containerRef.current;
+    if (!el) return;
+
+    const scene    = new THREE.Scene();
+    const camera   = new THREE.PerspectiveCamera(45, el.clientWidth / el.clientHeight, 0.1, 100);
+    camera.position.set(2.5, 2, 3);
+    camera.lookAt(0, 0, 0);
+
+    const renderer = new THREE.WebGLRenderer({ antialias: true, alpha: true });
+    renderer.setClearColor(0x070712, 1);
+    renderer.setPixelRatio(window.devicePixelRatio);
+    renderer.setSize(el.clientWidth, el.clientHeight);
+    el.appendChild(renderer.domElement);
+
+    // Lighting
+    scene.add(new THREE.AmbientLight(0x404060, 3));
+    const dir = new THREE.DirectionalLight(0xffffff, 4);
+    dir.position.set(5, 8, 6);
+    scene.add(dir);
+
+    // Robot body group
+    const group = new THREE.Group();
+
+    // Body
+    const body = new THREE.Mesh(
+      new THREE.BoxGeometry(0.8, 1.4, 0.4),
+      new THREE.MeshPhongMaterial({ color: 0x1a2a4a, specular: 0x334466 })
+    );
+    body.position.y = 0.5;
+    group.add(body);
+
+    // Wheels
+    const wheelGeo = new THREE.CylinderGeometry(0.35, 0.35, 0.12, 18);
+    const wheelMat = new THREE.MeshPhongMaterial({ color: 0x1a1a1a, specular: 0x333333 });
+    [-0.5, 0.5].forEach(x => {
+      const pivot = new THREE.Group();
+      pivot.position.set(x, -0.2, 0);
+      pivot.rotation.z = Math.PI / 2;
+      pivot.add(new THREE.Mesh(wheelGeo, wheelMat));
+      group.add(pivot);
+
+      const rim = new THREE.Mesh(
+        new THREE.TorusGeometry(0.3, 0.02, 8, 16),
+        new THREE.MeshPhongMaterial({ color: 0x0055cc })
+      );
+      rim.rotation.z = Math.PI / 2;
+      rim.position.set(x * 1.07, -0.2, 0);
+      group.add(rim);
+    });
+
+    // Forward indicator arrow (red tip at +Z front)
+    const arrow = new THREE.Mesh(
+      new THREE.ConeGeometry(0.08, 0.25, 8),
+      new THREE.MeshBasicMaterial({ color: 0xff3030 })
+    );
+    arrow.rotation.x = -Math.PI / 2;
+    arrow.position.set(0, 0.5, 0.35);
+    group.add(arrow);
+
+    // Sensor head
+    const head = new THREE.Mesh(
+      new THREE.BoxGeometry(0.32, 0.18, 0.32),
+      new THREE.MeshPhongMaterial({ color: 0x111122 })
+    );
+    head.position.set(0, 1.35, 0);
+    group.add(head);
+
+    // Axis helper
+    scene.add(new THREE.AxesHelper(1.4));
+    scene.add(group);
+
+    const q = new THREE.Quaternion();
+    let curQ = new THREE.Quaternion();
+
+    sceneRef.current = { group, q, curQ };
+
+    let animId;
+    const animate = () => {
+      animId = requestAnimationFrame(animate);
+      curQ.slerp(q, 0.12);
+      group.quaternion.copy(curQ);
+      renderer.render(scene, camera);
+    };
+    animate();
+
+    const ro = new ResizeObserver(() => {
+      camera.aspect = el.clientWidth / el.clientHeight;
+      camera.updateProjectionMatrix();
+      renderer.setSize(el.clientWidth, el.clientHeight);
+    });
+    ro.observe(el);
+
+    return () => {
+      cancelAnimationFrame(animId);
+      ro.disconnect();
+      renderer.dispose();
+      el.removeChild(renderer.domElement);
+      sceneRef.current = null;
+    };
+  }, [containerRef]);
+
+  const updateOrientation = useCallback((qx, qy, qz, qw) => {
+    if (!sceneRef.current) return;
+    sceneRef.current.q.set(qx, qy, qz, qw);
+  }, []);
+
+  return updateOrientation;
+}
+
+// ── Component ──────────────────────────────────────────────────────────────────
+
+function Readout({ label, value, unit, warn }) {
+  return (
+    <div className="bg-gray-950 rounded p-2 text-center">
+      <div className="text-gray-600 text-xs">{label}</div>
+      <div className={`text-lg font-bold ${warn ? 'text-amber-400' : 'text-cyan-400'}`}>{value}</div>
+      <div className="text-gray-700 text-xs">{unit}</div>
+    </div>
+  );
+}
+
+export function ImuPanel({ subscribe }) {
+  const horizonRef = useRef(null);
+  const compassRef = useRef(null);
+  const threeRef   = useRef(null);
+
+  const [attitude, setAttitude] = useState({ pitch: 0, roll: 0, yaw: 0 });
+  const [angVel, setAngVel]     = useState({ x: 0, y: 0, z: 0 });
+  const [balState, setBalState] = useState(null);
+  const [pktHz, setPktHz]       = useState(0);
+
+  const threeUpdate = useThreeOrientation(threeRef);
+  const pktRef = useRef({ count: 0, last: Date.now() });
+
+  // Draw canvases on attitude change
+  useEffect(() => {
+    const hc = horizonRef.current;
+    const cc = compassRef.current;
+    if (hc) {
+      const ctx = hc.getContext('2d');
+      drawHorizon(ctx, hc.width, hc.height, attitude.pitch, attitude.roll);
+    }
+    if (cc) {
+      const ctx = cc.getContext('2d');
+      drawCompass(ctx, cc.width, cc.height, attitude.yaw);
+    }
+  }, [attitude]);
+
+  // Subscribe to Imu
+  useEffect(() => {
+    const unsub = subscribe('/saltybot/imu', 'sensor_msgs/Imu', (msg) => {
+      const o = msg.orientation;
+      if (o) {
+        // Convert quaternion to Euler (simple ZYX)
+        const { x, y, z, w } = o;
+        const pitchRad = Math.asin(2 * (w * y - z * x));
+        const rollRad  = Math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
+        const yawRad   = Math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
+        setAttitude({
+          pitch: pitchRad * 180 / Math.PI,
+          roll:  rollRad  * 180 / Math.PI,
+          yaw:   yawRad   * 180 / Math.PI,
+        });
+        threeUpdate(x, y, z, w);
+      }
+      const av = msg.angular_velocity;
+      if (av) {
+        setAngVel({
+          x: av.x * 180 / Math.PI,
+          y: av.y * 180 / Math.PI,
+          z: av.z * 180 / Math.PI,
+        });
+      }
+      // Hz counter
+      pktRef.current.count++;
+      const now = Date.now();
+      if (now - pktRef.current.last >= 1000) {
+        setPktHz(pktRef.current.count);
+        pktRef.current.count = 0;
+        pktRef.current.last = now;
+      }
+    });
+    return unsub;
+  }, [subscribe, threeUpdate]);
+
+  // Subscribe to balance_state for motor/state info
+  useEffect(() => {
+    const unsub = subscribe('/saltybot/balance_state', 'std_msgs/String', (msg) => {
+      try {
+        setBalState(JSON.parse(msg.data));
+      } catch { /* ignore */ }
+    });
+    return unsub;
+  }, [subscribe]);
+
+  const pitchWarn = Math.abs(attitude.pitch) > 20;
+  const rollWarn  = Math.abs(attitude.roll) > 20;
+
+  return (
+    <div className="space-y-4">
+      {/* Numeric readouts */}
+      <div className="grid grid-cols-3 gap-2">
+        <Readout label="PITCH" value={attitude.pitch.toFixed(1)} unit="°" warn={pitchWarn} />
+        <Readout label="ROLL"  value={attitude.roll.toFixed(1)}  unit="°" warn={rollWarn} />
+        <Readout label="YAW"   value={((attitude.yaw + 360) % 360).toFixed(1)} unit="°" />
+      </div>
+
+      {/* Angular velocity */}
+      <div className="grid grid-cols-3 gap-2">
+        <Readout label="ω PITCH" value={angVel.x.toFixed(1)} unit="°/s" />
+        <Readout label="ω ROLL"  value={angVel.y.toFixed(1)} unit="°/s" />
+        <Readout label="ω YAW"   value={angVel.z.toFixed(1)} unit="°/s" />
+      </div>
+
+      {/* 2D gauges */}
+      <div className="grid grid-cols-1 sm:grid-cols-2 gap-4">
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest mb-2">ARTIFICIAL HORIZON</div>
+          <canvas
+            ref={horizonRef}
+            width={280}
+            height={120}
+            className="w-full rounded border border-gray-900 block"
+          />
+        </div>
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest mb-2">COMPASS</div>
+          <canvas
+            ref={compassRef}
+            width={280}
+            height={56}
+            className="w-full rounded border border-gray-900 block"
+          />
+          {/* Balance state info */}
+          {balState && (
+            <div className="mt-2 grid grid-cols-2 gap-1 text-xs">
+              <div><span className="text-gray-600">State: </span><span className="text-cyan-400">{balState.state}</span></div>
+              <div><span className="text-gray-600">Mode: </span><span className="text-amber-400">{balState.mode}</span></div>
+              <div><span className="text-gray-600">Motor: </span><span className="text-orange-400">{balState.motor_cmd}</span></div>
+              <div><span className="text-gray-600">Error: </span><span className="text-red-400">{balState.pid_error_deg}°</span></div>
+            </div>
+          )}
+        </div>
+      </div>
+
+      {/* Three.js 3D orientation */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3">
+        <div className="flex items-center justify-between mb-2">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest">3D ORIENTATION</div>
+          <div className="text-gray-600 text-xs">{pktHz} Hz</div>
+        </div>
+        <div ref={threeRef} className="w-full rounded border border-gray-900" style={{ height: '200px' }} />
+      </div>
+    </div>
+  );
+}

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

@@ -0,0 +1,298 @@
+/**
+ * MapViewer.jsx — 2D occupancy grid + robot pose + Nav2 path overlay.
+ *
+ * Topics:
+ *   /map           (nav_msgs/OccupancyGrid)  — SLAM/static map
+ *   /odom          (nav_msgs/Odometry)       — robot position & heading
+ *   /outdoor/route (nav_msgs/Path)           — Nav2 / OSM route path
+ *
+ * NOTE: OccupancyGrid data can be large (384×384 = 150K cells).
+ *       We decode on a worker-free canvas; map refreshes at topic rate
+ *       (typically 0.1–1 Hz from SLAM), odom at ~10 Hz.
+ */
+
+import { useEffect, useRef, useState, useCallback } from 'react';
+
+const CELL_COLORS = {
+  unknown: '#1a1a2e',
+  free:    '#0a1020',
+  occ:     '#00ffff33',
+  occFull: '#00b8d9',
+};
+
+function quatToYaw(o) {
+  return Math.atan2(2 * (o.w * o.z + o.x * o.y), 1 - 2 * (o.y * o.y + o.z * o.z));
+}
+
+export function MapViewer({ subscribe }) {
+  const canvasRef = useRef(null);
+  const mapRef    = useRef(null);  // last OccupancyGrid info
+  const odomRef   = useRef(null);  // last robot pose {x,y,yaw}
+  const pathRef   = useRef([]);    // [{x,y}] path points in map coords
+  const [mapInfo, setMapInfo]   = useState(null);
+  const [odomPose, setOdomPose] = useState(null);
+  const [zoom, setZoom]         = useState(1);
+  const [pan, setPan]           = useState({ x: 0, y: 0 });
+  const dragging = useRef(null);
+
+  // ── Render ────────────────────────────────────────────────────────────────
+  const render = useCallback(() => {
+    const canvas = canvasRef.current;
+    const map    = mapRef.current;
+    if (!canvas || !map) return;
+
+    const ctx = canvas.getContext('2d');
+    const W = canvas.width, H = canvas.height;
+
+    ctx.clearRect(0, 0, W, H);
+    ctx.fillStyle = CELL_COLORS.unknown;
+    ctx.fillRect(0, 0, W, H);
+
+    const { data, info } = map;
+    const mW = info.width, mH = info.height;
+    const res = info.resolution; // m/cell
+    const cellPx = zoom;         // 1 map cell = zoom pixels
+
+    // Canvas centre
+    const cx = W / 2 + pan.x;
+    const cy = H / 2 + pan.y;
+
+    // Map origin (bottom-left in world) → we flip y for canvas
+    const ox = info.origin.position.x;
+    const oy = info.origin.position.y;
+
+    // Draw map cells in chunks
+    const img = ctx.createImageData(W, H);
+    const imgData = img.data;
+
+    for (let r = 0; r < mH; r++) {
+      for (let c = 0; c < mW; c++) {
+        const val = data[r * mW + c];
+        // World coords of this cell centre
+        const wx = ox + (c + 0.5) * res;
+        const wy = oy + (r + 0.5) * res;
+        // Canvas coords (flip y)
+        const px = Math.round(cx + wx * cellPx / res);
+        const py = Math.round(cy - wy * cellPx / res);
+
+        if (px < 0 || px >= W || py < 0 || py >= H) continue;
+
+        let ro, go, bo, ao;
+        if (val < 0) { ro=26; go=26; bo=46; ao=255; }         // unknown
+        else if (val === 0) { ro=10; go=16; bo=32; ao=255; }  // free
+        else if (val < 60)  { ro=0;  go=100; bo=120; ao=120; } // low occ
+        else                { ro=0;  go=184; bo=217; ao=220; }  // occupied
+
+        const i = (py * W + px) * 4;
+        imgData[i]   = ro; imgData[i+1] = go;
+        imgData[i+2] = bo; imgData[i+3] = ao;
+      }
+    }
+    ctx.putImageData(img, 0, 0);
+
+    // ── Nav2 path ─────────────────────────────────────────────────────────
+    const path = pathRef.current;
+    if (path.length >= 2) {
+      ctx.strokeStyle = '#f59e0b';
+      ctx.lineWidth = 2;
+      ctx.setLineDash([4, 4]);
+      ctx.beginPath();
+      path.forEach(({ x, y }, i) => {
+        const px = cx + x * cellPx / res;
+        const py = cy - y * cellPx / res;
+        i === 0 ? ctx.moveTo(px, py) : ctx.lineTo(px, py);
+      });
+      ctx.stroke();
+      ctx.setLineDash([]);
+    }
+
+    // ── Robot ────────────────────────────────────────────────────────────
+    const odom = odomRef.current;
+    if (odom) {
+      const rx = cx + odom.x * cellPx / res;
+      const ry = cy - odom.y * cellPx / res;
+
+      // Heading arrow
+      const arrowLen = Math.max(12, cellPx * 1.5);
+      ctx.strokeStyle = '#f97316';
+      ctx.lineWidth = 2;
+      ctx.beginPath();
+      ctx.moveTo(rx, ry);
+      ctx.lineTo(rx + Math.cos(odom.yaw) * arrowLen, ry - Math.sin(odom.yaw) * arrowLen);
+      ctx.stroke();
+
+      // Robot circle
+      ctx.fillStyle = '#f97316';
+      ctx.shadowBlur = 8;
+      ctx.shadowColor = '#f97316';
+      ctx.beginPath();
+      ctx.arc(rx, ry, 6, 0, Math.PI * 2);
+      ctx.fill();
+      ctx.shadowBlur = 0;
+
+      // Heading dot
+      ctx.fillStyle = '#ffffff';
+      ctx.beginPath();
+      ctx.arc(rx + Math.cos(odom.yaw) * arrowLen, ry - Math.sin(odom.yaw) * arrowLen, 3, 0, Math.PI * 2);
+      ctx.fill();
+    }
+
+    // ── Scale bar ─────────────────────────────────────────────────────────
+    if (mapInfo) {
+      const scaleM = 2; // 2-metre scale bar
+      const scalePx = scaleM * cellPx / res;
+      const bx = 12, by = H - 12;
+      ctx.strokeStyle = '#06b6d4';
+      ctx.lineWidth = 2;
+      ctx.beginPath();
+      ctx.moveTo(bx, by); ctx.lineTo(bx + scalePx, by);
+      ctx.moveTo(bx, by - 4); ctx.lineTo(bx, by + 4);
+      ctx.moveTo(bx + scalePx, by - 4); ctx.lineTo(bx + scalePx, by + 4);
+      ctx.stroke();
+      ctx.fillStyle = '#06b6d4';
+      ctx.font = '9px monospace';
+      ctx.textAlign = 'left';
+      ctx.fillText(`${scaleM}m`, bx, by - 6);
+    }
+  }, [zoom, pan, mapInfo]);
+
+  // ── Subscribe /map ────────────────────────────────────────────────────────
+  useEffect(() => {
+    const unsub = subscribe('/map', 'nav_msgs/OccupancyGrid', (msg) => {
+      mapRef.current = msg;
+      setMapInfo(msg.info);
+      render();
+    });
+    return unsub;
+  }, [subscribe, render]);
+
+  // ── Subscribe /odom ───────────────────────────────────────────────────────
+  useEffect(() => {
+    const unsub = subscribe('/odom', 'nav_msgs/Odometry', (msg) => {
+      const p = msg.pose.pose.position;
+      const o = msg.pose.pose.orientation;
+      const pose = { x: p.x, y: p.y, yaw: quatToYaw(o) };
+      odomRef.current = pose;
+      setOdomPose(pose);
+      render();
+    });
+    return unsub;
+  }, [subscribe, render]);
+
+  // ── Subscribe /outdoor/route (Nav2 / OSM path) ────────────────────────────
+  useEffect(() => {
+    const unsub = subscribe('/outdoor/route', 'nav_msgs/Path', (msg) => {
+      pathRef.current = (msg.poses ?? []).map(p => ({
+        x: p.pose.position.x,
+        y: p.pose.position.y,
+      }));
+      render();
+    });
+    return unsub;
+  }, [subscribe, render]);
+
+  // Re-render when zoom/pan changes
+  useEffect(() => { render(); }, [zoom, pan, render]);
+
+  // ── Mouse pan ─────────────────────────────────────────────────────────────
+  const onMouseDown = (e) => { dragging.current = { x: e.clientX - pan.x, y: e.clientY - pan.y }; };
+  const onMouseMove = (e) => {
+    if (!dragging.current) return;
+    setPan({ x: e.clientX - dragging.current.x, y: e.clientY - dragging.current.y });
+  };
+  const onMouseUp = () => { dragging.current = null; };
+
+  // Touch pan
+  const touchRef = useRef(null);
+  const onTouchStart = (e) => {
+    const t = e.touches[0];
+    touchRef.current = { x: t.clientX - pan.x, y: t.clientY - pan.y };
+  };
+  const onTouchMove = (e) => {
+    if (!touchRef.current) return;
+    const t = e.touches[0];
+    setPan({ x: t.clientX - touchRef.current.x, y: t.clientY - touchRef.current.y });
+  };
+
+  return (
+    <div className="space-y-3">
+      {/* Toolbar */}
+      <div className="flex items-center gap-2 flex-wrap">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest">MAP VIEWER</div>
+        <div className="flex items-center gap-1 ml-auto">
+          <button onClick={() => setZoom(z => Math.min(z * 1.5, 20))}
+            className="px-2 py-1 rounded border border-gray-700 text-gray-300 hover:border-cyan-700 text-sm">+</button>
+          <span className="text-gray-500 text-xs w-10 text-center">{zoom.toFixed(1)}x</span>
+          <button onClick={() => setZoom(z => Math.max(z / 1.5, 0.2))}
+            className="px-2 py-1 rounded border border-gray-700 text-gray-300 hover:border-cyan-700 text-sm">−</button>
+          <button onClick={() => { setZoom(1); setPan({ x: 0, y: 0 }); }}
+            className="px-2 py-1 rounded border border-gray-700 text-gray-400 hover:text-gray-200 text-xs ml-2">Reset</button>
+        </div>
+      </div>
+
+      {/* Canvas */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 overflow-hidden">
+        <canvas
+          ref={canvasRef}
+          width={600}
+          height={400}
+          className="w-full cursor-grab active:cursor-grabbing block"
+          onMouseDown={onMouseDown}
+          onMouseMove={onMouseMove}
+          onMouseUp={onMouseUp}
+          onMouseLeave={onMouseUp}
+          onTouchStart={onTouchStart}
+          onTouchMove={onTouchMove}
+          onTouchEnd={() => { touchRef.current = null; }}
+        />
+      </div>
+
+      {/* Info bar */}
+      <div className="grid grid-cols-2 sm:grid-cols-4 gap-2 text-xs">
+        <div className="bg-gray-950 rounded border border-gray-800 p-2">
+          <div className="text-gray-600">MAP SIZE</div>
+          <div className="text-cyan-400 font-bold">
+            {mapInfo ? `${mapInfo.width}×${mapInfo.height}` : '—'}
+          </div>
+        </div>
+        <div className="bg-gray-950 rounded border border-gray-800 p-2">
+          <div className="text-gray-600">RESOLUTION</div>
+          <div className="text-cyan-400 font-bold">
+            {mapInfo ? `${mapInfo.resolution.toFixed(2)}m/cell` : '—'}
+          </div>
+        </div>
+        <div className="bg-gray-950 rounded border border-gray-800 p-2">
+          <div className="text-gray-600">ROBOT POS</div>
+          <div className="text-orange-400 font-bold">
+            {odomPose ? `${odomPose.x.toFixed(2)}, ${odomPose.y.toFixed(2)}` : '—'}
+          </div>
+        </div>
+        <div className="bg-gray-950 rounded border border-gray-800 p-2">
+          <div className="text-gray-600">HEADING</div>
+          <div className="text-orange-400 font-bold">
+            {odomPose ? `${(odomPose.yaw * 180 / Math.PI).toFixed(1)}°` : '—'}
+          </div>
+        </div>
+      </div>
+
+      {/* Legend */}
+      <div className="flex gap-4 text-xs text-gray-600">
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-sm" style={{ background: '#1a1a2e' }} />Unknown
+        </div>
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-sm" style={{ background: '#0a1020' }} />Free
+        </div>
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-sm" style={{ background: '#00b8d9' }} />Occupied
+        </div>
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-sm" style={{ background: '#f59e0b' }} />Path
+        </div>
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-full" style={{ background: '#f97316' }} />Robot
+        </div>
+      </div>
+    </div>
+  );
+}

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

@@ -0,0 +1,210 @@
+/**
+ * MotorPanel.jsx — Motor telemetry display.
+ *
+ * Balance bot:  /saltybot/balance_state (std_msgs/String JSON)
+ *   motor_cmd [-1000..1000], bridge_speed, bridge_steer
+ *
+ * Rover mode:   /saltybot/rover_pwm (std_msgs/String JSON)
+ *   ch1_us, ch2_us, ch3_us, ch4_us [1000..2000]
+ *
+ * Temperatures: /diagnostics (diagnostic_msgs/DiagnosticArray)
+ *   motor_temp_l_c, motor_temp_r_c
+ *
+ * Displays PWM-bar for each motor, RPM proxy, temperature.
+ */
+
+import { useEffect, useState } from 'react';
+
+const PWM_MIN = 1000;
+const PWM_MID = 1500;
+const PWM_MAX = 2000;
+
+/** Map [-1..1] normalised value to bar width and color. */
+function dutyBar(norm) {
+  const pct = Math.abs(norm) * 50;
+  const color = norm > 0 ? '#f97316' : '#3b82f6';
+  const left  = norm >= 0 ? '50%' : `${50 - pct}%`;
+  return { pct, color, left };
+}
+
+function MotorGauge({ label, norm, pwmUs, tempC }) {
+  const { pct, color, left } = dutyBar(norm);
+  const tempWarn = tempC != null && tempC > 70;
+
+  return (
+    <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+      <div className="flex items-center justify-between mb-2">
+        <span className="text-cyan-700 text-xs font-bold tracking-widest">{label}</span>
+        {tempC != null && (
+          <span className={`text-xs font-bold ${tempWarn ? 'text-red-400' : 'text-gray-400'}`}>
+            {tempC.toFixed(0)}°C
+          </span>
+        )}
+      </div>
+
+      {/* PWM value */}
+      <div className="text-2xl font-bold text-orange-400 mb-2">
+        {pwmUs != null ? `${pwmUs} µs` : `${Math.round(norm * 1000)}`}
+      </div>
+
+      {/* Bidirectional duty bar */}
+      <div className="relative h-3 bg-gray-900 rounded overflow-hidden border border-gray-800 mb-1">
+        <div className="absolute inset-y-0 left-1/2 w-px bg-gray-700" />
+        <div
+          className="absolute inset-y-0 transition-all duration-100 rounded"
+          style={{ left, width: `${pct}%`, background: color }}
+        />
+      </div>
+      <div className="flex justify-between text-xs text-gray-700">
+        <span>REV</span>
+        <span>{(norm * 100).toFixed(0)}%</span>
+        <span>FWD</span>
+      </div>
+    </div>
+  );
+}
+
+export function MotorPanel({ subscribe }) {
+  const [balState, setBalState] = useState(null);
+  const [roverPwm, setRoverPwm] = useState(null);
+  const [temps, setTemps]       = useState({ l: null, r: null });
+
+  // Balance state (2-wheel robot)
+  useEffect(() => {
+    const unsub = subscribe('/saltybot/balance_state', 'std_msgs/String', (msg) => {
+      try { setBalState(JSON.parse(msg.data)); }
+      catch { /* ignore */ }
+    });
+    return unsub;
+  }, [subscribe]);
+
+  // Rover PWM (4-wheel rover)
+  useEffect(() => {
+    const unsub = subscribe('/saltybot/rover_pwm', 'std_msgs/String', (msg) => {
+      try { setRoverPwm(JSON.parse(msg.data)); }
+      catch { /* ignore */ }
+    });
+    return unsub;
+  }, [subscribe]);
+
+  // Motor temperatures from diagnostics
+  useEffect(() => {
+    const unsub = subscribe('/diagnostics', 'diagnostic_msgs/DiagnosticArray', (msg) => {
+      for (const status of msg.status ?? []) {
+        const kv = {};
+        for (const pair of status.values ?? []) kv[pair.key] = pair.value;
+        if (kv.motor_temp_l_c !== undefined || kv.motor_temp_r_c !== undefined) {
+          setTemps({
+            l: kv.motor_temp_l_c != null ? parseFloat(kv.motor_temp_l_c) : null,
+            r: kv.motor_temp_r_c != null ? parseFloat(kv.motor_temp_r_c) : null,
+          });
+          break;
+        }
+      }
+    });
+    return unsub;
+  }, [subscribe]);
+
+  // Determine display mode: rover if rover_pwm active, else balance
+  const isRover = roverPwm != null;
+
+  let leftNorm = 0, rightNorm = 0;
+  let leftPwm = null, rightPwm = null;
+
+  if (isRover) {
+    // ch1=left-front, ch2=right-front, ch3=left-rear, ch4=right-rear
+    leftPwm  = Math.round((roverPwm.ch1_us + (roverPwm.ch3_us ?? roverPwm.ch1_us)) / 2);
+    rightPwm = Math.round((roverPwm.ch2_us + (roverPwm.ch4_us ?? roverPwm.ch2_us)) / 2);
+    leftNorm  = (leftPwm  - PWM_MID) / (PWM_MAX - PWM_MID);
+    rightNorm = (rightPwm - PWM_MID) / (PWM_MAX - PWM_MID);
+  } else if (balState) {
+    const cmd = (balState.motor_cmd ?? 0) / 1000;
+    const steer = (balState.bridge_steer ?? 0) / 1000;
+    leftNorm  = cmd + steer;
+    rightNorm = cmd - steer;
+  }
+
+  // Clip to ±1
+  leftNorm  = Math.max(-1, Math.min(1, leftNorm));
+  rightNorm = Math.max(-1, Math.min(1, rightNorm));
+
+  return (
+    <div className="space-y-4">
+      {/* Mode indicator */}
+      <div className="flex items-center gap-2">
+        <div className={`px-2 py-0.5 rounded text-xs font-bold border ${
+          isRover
+            ? 'bg-purple-950 border-purple-700 text-purple-300'
+            : 'bg-blue-950 border-blue-700 text-blue-300'
+        }`}>
+          {isRover ? 'ROVER (4WD)' : 'BALANCE (2WD)'}
+        </div>
+        {!balState && !roverPwm && (
+          <span className="text-gray-600 text-xs">No motor data</span>
+        )}
+      </div>
+
+      {/* Motor gauges */}
+      <div className="grid grid-cols-1 sm:grid-cols-2 gap-4">
+        <MotorGauge
+          label="LEFT MOTOR"
+          norm={leftNorm}
+          pwmUs={isRover ? leftPwm : null}
+          tempC={temps.l}
+        />
+        <MotorGauge
+          label="RIGHT MOTOR"
+          norm={rightNorm}
+          pwmUs={isRover ? rightPwm : null}
+          tempC={temps.r}
+        />
+      </div>
+
+      {/* Balance-specific details */}
+      {balState && !isRover && (
+        <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">PID DETAILS</div>
+          <div className="grid grid-cols-2 sm:grid-cols-4 gap-2 text-xs">
+            <div className="bg-gray-900 rounded p-2 text-center">
+              <div className="text-gray-600">CMD</div>
+              <div className="text-orange-400 font-bold">{balState.motor_cmd}</div>
+            </div>
+            <div className="bg-gray-900 rounded p-2 text-center">
+              <div className="text-gray-600">ERROR</div>
+              <div className="text-red-400 font-bold">{balState.pid_error_deg}°</div>
+            </div>
+            <div className="bg-gray-900 rounded p-2 text-center">
+              <div className="text-gray-600">INTEGRAL</div>
+              <div className="text-purple-400 font-bold">{balState.integral}</div>
+            </div>
+            <div className="bg-gray-900 rounded p-2 text-center">
+              <div className="text-gray-600">FRAMES</div>
+              <div className="text-gray-400 font-bold">{balState.frames}</div>
+            </div>
+          </div>
+        </div>
+      )}
+
+      {/* Rover PWM details */}
+      {roverPwm && (
+        <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">CHANNEL PWM (µs)</div>
+          <div className="grid grid-cols-4 gap-2 text-xs">
+            {[1,2,3,4].map(ch => {
+              const us = roverPwm[`ch${ch}_us`] ?? PWM_MID;
+              const norm = (us - PWM_MID) / (PWM_MAX - PWM_MID);
+              return (
+                <div key={ch} className="bg-gray-900 rounded p-2 text-center">
+                  <div className="text-gray-600">CH{ch}</div>
+                  <div className="font-bold" style={{ color: norm > 0.05 ? '#f97316' : norm < -0.05 ? '#3b82f6' : '#6b7280' }}>
+                    {us}
+                  </div>
+                </div>
+              );
+            })}
+          </div>
+        </div>
+      )}
+    </div>
+  );
+}

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

@@ -0,0 +1,196 @@
+/**
+ * SystemHealth.jsx — System resource monitoring and ROS2 node status.
+ *
+ * Topics:
+ *   /diagnostics  (diagnostic_msgs/DiagnosticArray)
+ *     Each DiagnosticStatus has: name, level (0=OK,1=WARN,2=ERROR,3=STALE)
+ *                                message, hardware_id, values: [{key,value}]
+ *     Expected KeyValue sources (from tegrastats bridge / custom nodes):
+ *       cpu_temp_c, gpu_temp_c, ram_used_mb, ram_total_mb,
+ *       disk_used_gb, disk_total_gb, gpu_used_mb, gpu_total_mb
+ */
+
+import { useEffect, useState } from 'react';
+
+const LEVEL_CONFIG = {
+  0: { label: 'OK',    color: 'text-green-400', bg: 'bg-green-950', border: 'border-green-800', dot: 'bg-green-400' },
+  1: { label: 'WARN',  color: 'text-amber-400', bg: 'bg-amber-950', border: 'border-amber-800', dot: 'bg-amber-400 animate-pulse' },
+  2: { label: 'ERROR', color: 'text-red-400',   bg: 'bg-red-950',   border: 'border-red-800',   dot: 'bg-red-400 animate-pulse' },
+  3: { label: 'STALE', color: 'text-gray-500',  bg: 'bg-gray-900',  border: 'border-gray-700',  dot: 'bg-gray-500' },
+};
+
+function ResourceBar({ label, used, total, unit, warnPct = 80 }) {
+  if (total == null || total === 0) return null;
+  const pct = Math.round((used / total) * 100);
+  const warn = pct >= warnPct;
+  const crit = pct >= 95;
+  const color = crit ? '#ef4444' : warn ? '#f59e0b' : '#06b6d4';
+
+  return (
+    <div className="space-y-1">
+      <div className="flex justify-between text-xs">
+        <span className="text-gray-500">{label}</span>
+        <span className={warn ? 'text-amber-400' : 'text-gray-400'}>
+          {typeof used === 'number' ? used.toFixed(1) : used}/{typeof total === 'number' ? total.toFixed(1) : total} {unit}
+          <span className="text-gray-600 ml-1">({pct}%)</span>
+        </span>
+      </div>
+      <div className="w-full h-2.5 bg-gray-900 rounded overflow-hidden border border-gray-800">
+        <div
+          className="h-full transition-all duration-500 rounded"
+          style={{ width: `${pct}%`, background: color }}
+        />
+      </div>
+    </div>
+  );
+}
+
+function TempGauge({ label, tempC }) {
+  if (tempC == null) return null;
+  const warn = tempC > 75;
+  const crit = tempC > 90;
+  const pct  = Math.min(100, (tempC / 100) * 100);
+  const color = crit ? '#ef4444' : warn ? '#f59e0b' : '#22c55e';
+
+  return (
+    <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 text-center">
+      <div className="text-gray-600 text-xs">{label}</div>
+      <div className="text-2xl font-bold mt-1" style={{ color }}>
+        {tempC.toFixed(0)}°C
+      </div>
+      <div className="w-full h-1.5 bg-gray-900 rounded overflow-hidden mt-1.5">
+        <div className="h-full rounded" style={{ width: `${pct}%`, background: color }} />
+      </div>
+    </div>
+  );
+}
+
+function NodeRow({ status }) {
+  const cfg = LEVEL_CONFIG[status.level] ?? LEVEL_CONFIG[3];
+  return (
+    <div className={`flex items-center gap-2 rounded px-3 py-1.5 border text-xs ${cfg.bg} ${cfg.border}`}>
+      <div className={`w-1.5 h-1.5 rounded-full shrink-0 ${cfg.dot}`} />
+      <span className="text-gray-300 font-bold truncate flex-1" title={status.name}>{status.name}</span>
+      <span className={`${cfg.color} shrink-0`}>{status.message || cfg.label}</span>
+    </div>
+  );
+}
+
+export function SystemHealth({ subscribe }) {
+  const [resources, setResources] = useState({
+    cpuTemp: null, gpuTemp: null,
+    ramUsed: null, ramTotal: null,
+    diskUsed: null, diskTotal: null,
+    gpuUsed: null, gpuTotal: null,
+  });
+  const [nodes, setNodes] = useState([]);
+  const [lastTs, setLastTs] = useState(null);
+
+  useEffect(() => {
+    const unsub = subscribe('/diagnostics', 'diagnostic_msgs/DiagnosticArray', (msg) => {
+      setLastTs(Date.now());
+
+      // Parse each DiagnosticStatus
+      const nodeList = [];
+
+      for (const status of msg.status ?? []) {
+        const kv = {};
+        for (const pair of status.values ?? []) kv[pair.key] = pair.value;
+
+        // Resource metrics
+        setResources(prev => {
+          const next = { ...prev };
+          if (kv.cpu_temp_c   !== undefined) next.cpuTemp  = parseFloat(kv.cpu_temp_c);
+          if (kv.gpu_temp_c   !== undefined) next.gpuTemp  = parseFloat(kv.gpu_temp_c);
+          if (kv.ram_used_mb  !== undefined) next.ramUsed  = parseFloat(kv.ram_used_mb) / 1024;
+          if (kv.ram_total_mb !== undefined) next.ramTotal = parseFloat(kv.ram_total_mb) / 1024;
+          if (kv.disk_used_gb !== undefined) next.diskUsed  = parseFloat(kv.disk_used_gb);
+          if (kv.disk_total_gb!== undefined) next.diskTotal = parseFloat(kv.disk_total_gb);
+          if (kv.gpu_used_mb  !== undefined) next.gpuUsed  = parseFloat(kv.gpu_used_mb);
+          if (kv.gpu_total_mb !== undefined) next.gpuTotal = parseFloat(kv.gpu_total_mb);
+          return next;
+        });
+
+        // Collect all statuses as node rows
+        nodeList.push({ name: status.name, level: status.level, message: status.message });
+      }
+
+      if (nodeList.length > 0) {
+        setNodes(nodeList);
+      }
+    });
+    return unsub;
+  }, [subscribe]);
+
+  const stale = lastTs && Date.now() - lastTs > 10000;
+  const errorCount = nodes.filter(n => n.level === 2).length;
+  const warnCount  = nodes.filter(n => n.level === 1).length;
+
+  return (
+    <div className="space-y-4">
+      {/* Summary banner */}
+      {nodes.length > 0 && (
+        <div className={`flex items-center gap-3 rounded-lg border p-3 ${
+          errorCount > 0 ? 'bg-red-950 border-red-800' :
+          warnCount  > 0 ? 'bg-amber-950 border-amber-800' :
+          'bg-green-950 border-green-800'
+        }`}>
+          <div className={`w-3 h-3 rounded-full ${
+            errorCount > 0 ? 'bg-red-400 animate-pulse' :
+            warnCount  > 0 ? 'bg-amber-400 animate-pulse' : 'bg-green-400'
+          }`} />
+          <div className="text-sm font-bold">
+            {errorCount > 0 ? `${errorCount} ERROR${errorCount > 1 ? 'S' : ''}` :
+             warnCount  > 0 ? `${warnCount} WARNING${warnCount > 1 ? 'S' : ''}` :
+             'All systems nominal'}
+          </div>
+          <div className="ml-auto text-xs text-gray-500">{nodes.length} nodes</div>
+          {stale && <div className="text-red-400 text-xs">STALE</div>}
+        </div>
+      )}
+
+      {/* Temperatures */}
+      {(resources.cpuTemp != null || resources.gpuTemp != null) && (
+        <div className="grid grid-cols-2 gap-3">
+          <TempGauge label="CPU TEMP" tempC={resources.cpuTemp} />
+          <TempGauge label="GPU TEMP" tempC={resources.gpuTemp} />
+        </div>
+      )}
+
+      {/* Resource bars */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4 space-y-3">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest">SYSTEM RESOURCES</div>
+        <ResourceBar label="RAM"        used={resources.ramUsed}  total={resources.ramTotal}  unit="GB" />
+        <ResourceBar label="GPU MEM"    used={resources.gpuUsed}  total={resources.gpuTotal}  unit="MB" />
+        <ResourceBar label="DISK"       used={resources.diskUsed} total={resources.diskTotal} unit="GB" warnPct={85} />
+        {resources.ramUsed == null && resources.gpuUsed == null && resources.diskUsed == null && (
+          <div className="text-gray-600 text-xs">
+            Waiting for resource metrics from /diagnostics…
+            <div className="mt-1 text-gray-700">Expected keys: cpu_temp_c, gpu_temp_c, ram_used_mb, disk_used_gb</div>
+          </div>
+        )}
+      </div>
+
+      {/* Node list */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4">
+        <div className="flex items-center justify-between mb-3">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest">ROS2 NODE HEALTH</div>
+          <div className="text-gray-600 text-xs">{nodes.length} statuses</div>
+        </div>
+        {nodes.length === 0 ? (
+          <div className="text-gray-600 text-xs text-center py-4 border border-dashed border-gray-800 rounded">
+            Waiting for /diagnostics…
+          </div>
+        ) : (
+          <div className="space-y-1.5 max-h-80 overflow-y-auto">
+            {/* Sort: errors first, then warns, then OK */}
+            {[...nodes]
+              .sort((a, b) => (b.level ?? 0) - (a.level ?? 0))
+              .map((n, i) => <NodeRow key={i} status={n} />)
+            }
+          </div>
+        )}
+      </div>
+    </div>
+  );
+}

ui/social-bot/vite.config.js

@@ -11,4 +11,16 @@ export default defineConfig({
     port: 8080,
     host: true,
   },
+  build: {
+    chunkSizeWarningLimit: 800,
+    rollupOptions: {
+      output: {
+        manualChunks: {
+          'vendor-three':  ['three'],
+          'vendor-roslib': ['roslib'],
+          'vendor-react':  ['react', 'react-dom'],
+        },
+      },
+    },
+  },
 });