feat: STM32 watchdog and fault recovery handler (Issue #565)

- New src/fault_handler.c + include/fault_handler.h:
  - HardFault/MemManage/BusFault/UsageFault naked ISR stubs with
    Cortex-M7 stack-frame capture (R0-R3, LR, PC, xPSR, CFSR, HFSR,
    MMFAR, BFAR, SP) and NVIC_SystemReset()
  - .noinit SRAM capture ring survives soft reset; persisted to flash
    sector 7 (0x08060000, 8x64-byte slots) on subsequent boot
  - MPU Region 0 stack guard (32 B at __stack_end, no-access) ->
    MemManage fault detected as FAULT_STACK_OVF
  - Brownout detect via RCC_CSR_BORRSTF on boot -> FAULT_BROWNOUT
  - Watchdog reset detection delegates to existing watchdog.c
  - LED blink codes on LED2 (PC14, active-low) for 10 s post-recovery:
    HARDFAULT=3, WATCHDOG=2, BROWNOUT=1, STACK_OVF=4 fast blinks
  - fault_led_tick(), fault_log_read(), fault_log_get_count(),
    fault_get_last_type(), fault_log_clear(), FAULT_ASSERT() macro
- jlink.h: add JLINK_CMD_FAULT_LOG_GET (0x0F), JLINK_TLM_FAULT_LOG
  (0x86), jlink_tlm_fault_log_t (20 bytes), fault_log_req in JLinkState,
  jlink_send_fault_log() declaration
- jlink.c: dispatch JLINK_CMD_FAULT_LOG_GET; implement
  jlink_send_fault_log() (26-byte CRC16-XModem framed response)
- main.c: call fault_handler_init() first in main(); send fault log
  TLM on boot if prior fault recorded; fault_led_tick() in main loop;
  handle fault_log_req flag to respond to Jetson queries

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

chassis/battery_holder.scad

@@ -0,0 +1,410 @@
+// ============================================================
+// battery_holder.scad — 6S LiPo Battery Holder for 2020 T-Slot Chassis
+// Issue: #588  Agent: sl-mechanical  Date: 2026-03-14
+// ============================================================
+//
+// Parametric bracket holding a 6S 5000 mAh LiPo pack on 2020 aluminium
+// T-slot rails.  Designed for low centre-of-gravity mounting: pack sits
+// flat between the two chassis rails, as close to ground as clearance
+// allows.  Quick-release via captive Velcro straps — battery swap in
+// under 60 s without tools.
+//
+// Architecture:
+//   Tray        → flat floor + perimeter walls, battery sits inside
+//   Rail saddles→ two T-nut feet drop onto 2020 rails, thumbscrew locks
+//   Strap slots → four pairs of slots for 25 mm Velcro strap loops
+//   XT60 window → cut-out in rear wall for XT60 connector exit
+//   Balance port→ open channel in front wall for balance lead routing
+//   QR tab      → front-edge pull tab for one-handed battery extraction
+//
+// Part catalogue:
+//   Part 1 — battery_tray()    Main tray body (single-piece print)
+//   Part 2 — rail_saddle()     T-nut saddle foot (print x2 per tray)
+//   Part 3 — strap_guide()     25 mm Velcro strap guide (print x4)
+//   Part 4 — assembly_preview()
+//
+// Hardware BOM:
+//   2× M3 × 16 mm SHCS + M3 hex nut    T-nut rail clamp thumbscrews
+//   2× 25 mm × 250 mm Velcro strap     battery retention (hook + loop)
+//   1× XT60 female connector            (mounted on ESC/PDB harness)
+//   — battery slides in from front, Velcro strap over top —
+//
+// 6S LiPo target pack (verify with calipers — packs vary by brand):
+//   BATT_L = 155 mm  (length, X axis in tray)
+//   BATT_W =  48 mm  (width,  Y axis in tray)
+//   BATT_H =  52 mm  (height, Z axis in tray)
+//   Clearance 1 mm each side added automatically (BATT_CLEAR)
+//
+// Mounting:
+//   Rail span    : RAIL_SPAN — distance between 2020 rail centrelines
+//                  Default 80 mm; adjust to chassis rail spacing
+//   Saddle height: SADDLE_H — total height of saddle (tray floor above rail)
+//                  Keep low for CoG; default 8 mm
+//
+// RENDER options:
+//   "assembly"         full assembly preview (default)
+//   "tray_stl"         Part 1 — battery tray
+//   "saddle_stl"       Part 2 — rail saddle (print x2)
+//   "strap_guide_stl"  Part 3 — strap guide (print x4)
+//
+// Export commands:
+//   openscad battery_holder.scad -D 'RENDER="tray_stl"'        -o bh_tray.stl
+//   openscad battery_holder.scad -D 'RENDER="saddle_stl"'      -o bh_saddle.stl
+//   openscad battery_holder.scad -D 'RENDER="strap_guide_stl"' -o bh_strap_guide.stl
+//
+// Print settings (all parts):
+//   Material   : PETG
+//   Perimeters : 5 (tray, saddle), 3 (strap_guide)
+//   Infill     : 40 % gyroid (tray floor, saddle), 20 % (strap_guide)
+//   Orientation:
+//     tray         — floor flat on bed (no supports needed)
+//     saddle       — flat face on bed (no supports)
+//     strap_guide  — flat face on bed (no supports)
+// ============================================================
+
+$fn = 64;
+e   = 0.01;
+
+// ── Battery pack dimensions (verify with calipers) ────────────────────────────
+BATT_L     = 155.0;   // pack length (X)
+BATT_W     =  48.0;   // pack width  (Y)
+BATT_H     =  52.0;   // pack height (Z)
+BATT_CLEAR =   1.0;   // per-side fit clearance
+
+// ── Tray geometry ─────────────────────────────────────────────────────────────
+TRAY_FLOOR_T   =  4.0;   // tray floor thickness
+TRAY_WALL_T    =  4.0;   // tray perimeter wall thickness
+TRAY_WALL_H    = 20.0;   // tray wall height (Z) — cradles lower half of pack
+TRAY_FILLET_R  =  3.0;   // inner corner radius
+
+// Inner tray cavity (battery + clearance)
+TRAY_INN_L = BATT_L + 2*BATT_CLEAR;
+TRAY_INN_W = BATT_W + 2*BATT_CLEAR;
+
+// Outer tray footprint
+TRAY_OUT_L = TRAY_INN_L + 2*TRAY_WALL_T;
+TRAY_OUT_W = TRAY_INN_W + 2*TRAY_WALL_T;
+TRAY_TOTAL_H = TRAY_FLOOR_T + TRAY_WALL_H;
+
+// ── Rail interface ─────────────────────────────────────────────────────────────
+RAIL_SPAN   =  80.0;   // distance between 2020 rail centrelines (Y)
+RAIL_W      =  20.0;   // 2020 extrusion width
+SLOT_NECK_H =   3.2;   // T-slot neck height
+SLOT_OPEN   =   6.0;   // T-slot opening width
+SLOT_INN_W  =  10.2;   // T-slot inner width
+SLOT_INN_H  =   5.8;   // T-slot inner height
+
+// ── T-nut / saddle geometry ───────────────────────────────────────────────────
+TNUT_W       =  9.8;
+TNUT_H       =  5.5;
+TNUT_L       = 12.0;
+TNUT_NUT_AF  =  5.5;   // M3 hex nut across-flats
+TNUT_NUT_H   =  2.4;
+TNUT_BOLT_D  =  3.3;   // M3 clearance
+
+SADDLE_W     = 30.0;   // saddle foot width (X, along rail)
+SADDLE_T     =  8.0;   // saddle body thickness (Z, above rail top face)
+SADDLE_PAD_T =  2.0;   // rubber-pad recess depth (optional anti-slip)
+
+// ── Velcro strap slots ────────────────────────────────────────────────────────
+STRAP_W      = 26.0;   // 25 mm strap + 1 mm clearance
+STRAP_T      =  4.0;   // slot through-thickness (tray wall)
+// Four slot pairs: one near each end of tray (X), one each side (Y)
+// Slots run through side walls (Y direction) — strap loops over battery top
+
+// ── XT60 connector window (rear wall) ─────────────────────────────────────────
+XT60_W       = 14.0;   // XT60 body width
+XT60_H       = 18.0;   // XT60 body height (with cable exit)
+XT60_OFFSET_Z = 4.0;   // height above tray floor
+
+// ── Balance lead port (front wall) ────────────────────────────────────────────
+BAL_W        = 40.0;   // balance lead bundle width (6S = 7 wires)
+BAL_H        =  6.0;   // balance lead channel height
+BAL_OFFSET_Z =  8.0;   // height above tray floor
+
+// ── Quick-release pull tab (front edge) ──────────────────────────────────────
+QR_TAB_W     = 30.0;   // tab width
+QR_TAB_H     = 12.0;   // tab height above front wall top
+QR_TAB_T     =  4.0;   // tab thickness
+QR_HOLE_D    = 10.0;   // finger-loop hole diameter
+
+// ── Strap guide clip ─────────────────────────────────────────────────────────
+GUIDE_OD     = STRAP_W + 6.0;
+GUIDE_T      =  3.0;
+GUIDE_BODY_H = 14.0;
+
+// ── Fasteners ─────────────────────────────────────────────────────────────────
+M3_D = 3.3;
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
+RENDER = "assembly";
+
+if      (RENDER == "assembly")        assembly_preview();
+else if (RENDER == "tray_stl")        battery_tray();
+else if (RENDER == "saddle_stl")      rail_saddle();
+else if (RENDER == "strap_guide_stl") strap_guide();
+
+// ============================================================
+// ASSEMBLY PREVIEW
+// ============================================================
+module assembly_preview() {
+    // Ghost 2020 rails (Y direction, RAIL_SPAN apart)
+    for (ry = [-RAIL_SPAN/2, RAIL_SPAN/2])
+        %color("Silver", 0.28)
+            translate([-TRAY_OUT_L/2 - 30, ry - RAIL_W/2, -SADDLE_T - TNUT_H])
+                cube([TRAY_OUT_L + 60, RAIL_W, RAIL_W]);
+
+    // Rail saddles (left and right)
+    for (sy = [-RAIL_SPAN/2, RAIL_SPAN/2])
+        color("DimGray", 0.85)
+            translate([0, sy, -SADDLE_T])
+                rail_saddle();
+
+    // Battery tray (sitting on saddles)
+    color("OliveDrab", 0.85)
+        battery_tray();
+
+    // Battery ghost
+    %color("SaddleBrown", 0.35)
+        translate([-BATT_L/2, -BATT_W/2, TRAY_FLOOR_T])
+            cube([BATT_L, BATT_W, BATT_H]);
+
+    // Strap guides (4×, two each end)
+    for (sx = [-TRAY_OUT_L/2 + STRAP_W/2 + TRAY_WALL_T + 8,
+                TRAY_OUT_L/2 - STRAP_W/2 - TRAY_WALL_T - 8])
+    for (sy = [-1, 1])
+        color("SteelBlue", 0.75)
+            translate([sx, sy*(TRAY_OUT_W/2), TRAY_TOTAL_H + 2])
+                rotate([sy > 0 ? 0 : 180, 0, 0])
+                    strap_guide();
+}
+
+// ============================================================
+// PART 1 — BATTERY TRAY
+// ============================================================
+// Single-piece tray: flat floor, four perimeter walls, T-nut saddle
+// attachment bosses on underside, Velcro strap slots through side walls,
+// XT60 window in rear wall, balance lead channel in front wall, and
+// quick-release pull tab on front edge.
+//
+// Battery inserts from the front (−X end) — front wall is lower than
+// rear wall so the pack slides in and the rear wall stops it.
+// Velcro straps loop over the top of the pack through the side slots.
+//
+// Coordinate convention:
+//   X: along battery length (−X = front/plug-end, +X = rear/balance-end)
+//   Y: across battery width (centred, ±TRAY_OUT_W/2)
+//   Z: vertical (Z=0 = tray floor top face; −Z = underside → saddles)
+//
+// Print: floor flat on bed, PETG, 5 perims, 40% gyroid. No supports.
+module battery_tray() {
+    // Short rear wall height (XT60 connector exits here — full wall height)
+    // Front wall is lower to allow battery slide-in
+    front_wall_h = TRAY_WALL_H * 0.55;   // 55% height — battery slides over
+
+    difference() {
+        union() {
+            // ── Floor ────────────────────────────────────────────────────
+            translate([-TRAY_OUT_L/2, -TRAY_OUT_W/2, -TRAY_FLOOR_T])
+                cube([TRAY_OUT_L, TRAY_OUT_W, TRAY_FLOOR_T]);
+
+            // ── Rear wall (+X, full height) ───────────────────────────────
+            translate([TRAY_INN_L/2, -TRAY_OUT_W/2, 0])
+                cube([TRAY_WALL_T, TRAY_OUT_W, TRAY_WALL_H]);
+
+            // ── Front wall (−X, lowered for slide-in) ────────────────────
+            translate([-TRAY_INN_L/2 - TRAY_WALL_T, -TRAY_OUT_W/2, 0])
+                cube([TRAY_WALL_T, TRAY_OUT_W, front_wall_h]);
+
+            // ── Side walls (±Y) ───────────────────────────────────────────
+            for (sy = [-1, 1])
+                translate([-TRAY_OUT_L/2,
+                           sy*(TRAY_INN_W/2 + (sy>0 ? 0 : -TRAY_WALL_T)),
+                           0])
+                    cube([TRAY_OUT_L,
+                          TRAY_WALL_T,
+                          TRAY_WALL_H]);
+
+            // ── Quick-release pull tab (front wall top edge) ──────────────
+            translate([-TRAY_INN_L/2 - TRAY_WALL_T - e,
+                       -QR_TAB_W/2, front_wall_h])
+                cube([QR_TAB_T, QR_TAB_W, QR_TAB_H]);
+
+            // ── Saddle attachment bosses (underside, one per rail) ─────────
+            // Bosses drop into saddle sockets; M3 bolt through floor
+            for (sy = [-RAIL_SPAN/2, RAIL_SPAN/2])
+                translate([-SADDLE_W/2, sy - SADDLE_W/2, -TRAY_FLOOR_T - SADDLE_T/2])
+                    cube([SADDLE_W, SADDLE_W, SADDLE_T/2 + e]);
+        }
+
+        // ── Battery cavity (hollow interior) ──────────────────────────────
+        translate([-TRAY_INN_L/2, -TRAY_INN_W/2, -e])
+            cube([TRAY_INN_L, TRAY_INN_W, TRAY_WALL_H + 2*e]);
+
+        // ── XT60 connector window (rear wall) ─────────────────────────────
+        // Centred on rear wall, low position so cable exits cleanly
+        translate([TRAY_INN_L/2 - e, -XT60_W/2, XT60_OFFSET_Z])
+            cube([TRAY_WALL_T + 2*e, XT60_W, XT60_H]);
+
+        // ── Balance lead channel (front wall) ─────────────────────────────
+        // Wide slot for 6S balance lead (7-pin JST-XH ribbon)
+        translate([-TRAY_INN_L/2 - TRAY_WALL_T - e,
+                   -BAL_W/2, BAL_OFFSET_Z])
+            cube([TRAY_WALL_T + 2*e, BAL_W, BAL_H]);
+
+        // ── Velcro strap slots (side walls, 2 pairs) ──────────────────────
+        // Pair A: near front end (−X), Pair B: near rear end (+X)
+        // Each slot runs through the wall in Y direction
+        for (sx = [-TRAY_INN_L/2 + STRAP_W*0.5 + 10,
+                    TRAY_INN_L/2 - STRAP_W*0.5 - 10])
+        for (sy = [-1, 1]) {
+            translate([sx - STRAP_W/2,
+                       sy*(TRAY_INN_W/2) - (sy > 0 ? TRAY_WALL_T + e : -e),
+                       TRAY_WALL_H * 0.35])
+                cube([STRAP_W, TRAY_WALL_T + 2*e, STRAP_T]);
+        }
+
+        // ── QR tab finger-loop hole ────────────────────────────────────────
+        translate([-TRAY_INN_L/2 - TRAY_WALL_T/2,
+                   0, front_wall_h + QR_TAB_H * 0.55])
+            rotate([0, 90, 0])
+                cylinder(d = QR_HOLE_D, h = QR_TAB_T + 2*e, center = true);
+
+        // ── Saddle bolt holes (M3 through floor into saddle boss) ─────────
+        for (sy = [-RAIL_SPAN/2, RAIL_SPAN/2])
+            translate([0, sy, -TRAY_FLOOR_T - e])
+                cylinder(d = M3_D, h = TRAY_FLOOR_T + 2*e);
+
+        // ── Floor lightening grid (non-structural area) ───────────────────
+        // 2D grid of pockets reduces weight without weakening battery support
+        for (gx = [-40, 0, 40])
+        for (gy = [-12, 12])
+            translate([gx, gy, -TRAY_FLOOR_T - e])
+                cylinder(d = 14, h = TRAY_FLOOR_T - 1.5 + e);
+
+        // ── Inner corner chamfers (battery slide-in guidance) ─────────────
+        // 45° chamfers at bottom-front inner corners
+        translate([-TRAY_INN_L/2, -TRAY_INN_W/2 - e, -e])
+            rotate([0, 0, 45])
+                cube([4, 4, TRAY_WALL_H * 0.3 + e]);
+        translate([-TRAY_INN_L/2, TRAY_INN_W/2 + e, -e])
+            rotate([0, 0, -45])
+                cube([4, 4, TRAY_WALL_H * 0.3 + e]);
+    }
+}
+
+// ============================================================
+// PART 2 — RAIL SADDLE
+// ============================================================
+// T-nut foot that clamps to the top face of a 2020 T-slot rail.
+// Battery tray boss drops into saddle socket; M3 bolt through tray
+// floor and saddle body locks everything together.
+// M3 thumbscrew through side of saddle body grips the rail T-groove
+// (same thumbscrew interface as all other SaltyLab rail brackets).
+//
+// Saddle sits on top of rail (no T-nut tongue needed — saddle clamps
+// from the top using a T-nut inserted into the rail T-groove from the
+// end).  Low profile keeps battery CoG as low as possible.
+//
+// Print: flat base on bed, PETG, 5 perims, 50% gyroid.
+module rail_saddle() {
+    sock_d = SADDLE_W - 4;   // tray boss socket diameter
+
+    difference() {
+        union() {
+            // ── Main saddle body ──────────────────────────────────────────
+            translate([-SADDLE_W/2, -SADDLE_W/2, 0])
+                cube([SADDLE_W, SADDLE_W, SADDLE_T]);
+
+            // ── T-nut tongue (enters rail T-groove from above) ────────────
+            translate([-TNUT_W/2, -TNUT_L/2, -SLOT_NECK_H])
+                cube([TNUT_W, TNUT_L, SLOT_NECK_H + e]);
+
+            // ── T-nut inner body (locks in groove) ────────────────────────
+            translate([-TNUT_W/2, -TNUT_L/2, -SLOT_NECK_H - (TNUT_H - SLOT_NECK_H)])
+                cube([TNUT_W, TNUT_L, TNUT_H - SLOT_NECK_H + e]);
+        }
+
+        // ── Rail channel clearance (bottom of saddle straddles rail) ──────
+        // Saddle body has a channel that sits over the rail top face
+        translate([-RAIL_W/2 - e, -SADDLE_W/2 - e, -e])
+            cube([RAIL_W + 2*e, SADDLE_W + 2*e, 2.0]);
+
+        // ── M3 clamp bolt bore (through saddle body into T-nut) ───────────
+        translate([0, 0, -SLOT_NECK_H - TNUT_H - e])
+            cylinder(d = TNUT_BOLT_D, h = SADDLE_T + TNUT_H + 2*e);
+
+        // ── M3 hex nut pocket (top face of saddle for thumbscrew) ─────────
+        translate([0, 0, SADDLE_T - TNUT_NUT_H - 0.5])
+            cylinder(d = TNUT_NUT_AF / cos(30),
+                     h = TNUT_NUT_H + 0.6, $fn = 6);
+
+        // ── Tray boss socket (top face of saddle, tray boss nests here) ───
+        // Cylindrical socket receives tray underside boss; M3 bolt centres
+        translate([0, 0, SADDLE_T - 3])
+            cylinder(d = sock_d + 0.4, h = 3 + e);
+
+        // ── M3 tray bolt bore (vertical, through saddle top) ──────────────
+        translate([0, 0, SADDLE_T - 3 - e])
+            cylinder(d = M3_D, h = SADDLE_T + e);
+
+        // ── Anti-slip pad recess (bottom face, optional rubber adhesive) ──
+        translate([0, 0, -e])
+            cylinder(d = SADDLE_W - 8, h = SADDLE_PAD_T + e);
+
+        // ── Lightening pockets ─────────────────────────────────────────────
+        for (lx = [-1, 1], ly = [-1, 1])
+            translate([lx*8, ly*8, -e])
+                cylinder(d = 5, h = SADDLE_T - 3 - 1 + e);
+    }
+}
+
+// ============================================================
+// PART 3 — STRAP GUIDE
+// ============================================================
+// Snap-on guide that sits on top of tray wall at each strap slot,
+// directing the 25 mm Velcro strap from the side slot up and over
+// the battery top.  Four per tray, one at each slot exit.
+// Curved lip prevents strap from cutting into PETG wall edge.
+// Push-fit onto tray wall top; no fasteners required.
+//
+// Print: flat base on bed, PETG, 3 perims, 20% infill.
+module strap_guide() {
+    strap_w_clr = STRAP_W + 0.5;   // strap slot with clearance
+    lip_r       = 3.0;              // guide lip radius
+
+    difference() {
+        union() {
+            // ── Body (sits on tray wall top edge) ─────────────────────────
+            translate([-GUIDE_OD/2, 0, 0])
+                cube([GUIDE_OD, GUIDE_T, GUIDE_BODY_H]);
+
+            // ── Curved guide lip (top of body, strap bends around this) ───
+            translate([0, GUIDE_T/2, GUIDE_BODY_H])
+                rotate([0, 90, 0])
+                    cylinder(r = lip_r, h = GUIDE_OD, center = true);
+
+            // ── Wall engagement tabs (snap over tray wall top) ────────────
+            for (sy = [0, -(TRAY_WALL_T + GUIDE_T)])
+                translate([-strap_w_clr/2 - 3, sy - GUIDE_T, 0])
+                    cube([strap_w_clr + 6, GUIDE_T, GUIDE_BODY_H * 0.4]);
+        }
+
+        // ── Strap slot (through body) ──────────────────────────────────────
+        translate([-strap_w_clr/2, -e, -e])
+            cube([strap_w_clr, GUIDE_T + 2*e, GUIDE_BODY_H + 2*e]);
+
+        // ── Wall clearance slot (body slides over tray wall) ──────────────
+        translate([-strap_w_clr/2 - 3 - e,
+                   -TRAY_WALL_T - GUIDE_T, -e])
+            cube([strap_w_clr + 6 + 2*e,
+                  TRAY_WALL_T, GUIDE_BODY_H * 0.4 + 2*e]);
+
+        // ── Lightening pockets on side faces ──────────────────────────────
+        for (lx = [-GUIDE_OD/4, GUIDE_OD/4])
+            translate([lx, GUIDE_T/2, GUIDE_BODY_H/2])
+                cube([6, GUIDE_T + 2*e, GUIDE_BODY_H * 0.5], center = true);
+    }
+}

chassis/uwb_anchor_mount.scad

@@ -11,38 +11,38 @@
 // coverage zone.
 //
 // Architecture:
-//   Wall base    → flat backplate with 2× screw holes (wall or ceiling)
+//   Wall base    -> flat backplate with 2x screw holes (wall or ceiling)
-//   Tilt knuckle → single-axis articulating joint; 15° detent steps
+//   Tilt knuckle -> single-axis articulating joint; 15deg detent steps
-//                  locked with M3 nyloc bolt; range 0–90°
+//                   locked with M3 nyloc bolt; range 0-90deg
-//   Anchor cradle→ U-cradle holding ESP32 UWB Pro PCB on M2.5 standoffs
+//   Anchor cradle-> U-cradle holding ESP32 UWB Pro PCB on M2.5 standoffs
-//   USB-C channel→ routed groove on tilt arm + exit slot in cradle back wall
+//   USB-C channel-> routed groove on tilt arm + exit slot in cradle back wall
-//   Label slot   → rear window slot for printed anchor-ID card strip
+//   Label slot   -> rear window slot for printed anchor-ID card strip
 //
 // Part catalogue:
-//   Part 1 — wall_base()       Backplate + 2-ear pivot block + detent arc
+//   Part 1 -- wall_base()       Backplate + 2-ear pivot block + detent arc
-//   Part 2 — tilt_arm()        Pivoting arm with knuckle + cradle stub
+//   Part 2 -- tilt_arm()        Pivoting arm with knuckle + cradle stub
-//   Part 3 — anchor_cradle()   PCB cradle, standoffs, USB-C slot, label window
+//   Part 3 -- anchor_cradle()   PCB cradle, standoffs, USB-C slot, label window
-//   Part 4 — cable_clip()      Snap-on USB-C cable guide for tilt arm
+//   Part 4 -- cable_clip()      Snap-on USB-C cable guide for tilt arm
-//   Part 5 — assembly_preview()
+//   Part 5 -- assembly_preview()
 //
 // Hardware BOM:
-//   2× M4 × 30 mm wood screws (or #6 drywall screws)  wall fasteners
+//   2x M4 x 30mm wood screws (or #6 drywall screws)  wall fasteners
-//   1× M3 × 20 mm SHCS + M3 nyloc nut                 tilt pivot bolt
+//   1x M3 x 20mm SHCS + M3 nyloc nut                 tilt pivot bolt
-//   4× M2.5 × 8 mm SHCS                               PCB-to-cradle
+//   4x M2.5 x 8mm SHCS                               PCB-to-cradle
-//   4× M2.5 hex nuts                                   captured in standoffs
+//   4x M2.5 hex nuts                                  captured in standoffs
-//   1× USB-C cable                                     anchor power
+//   1x USB-C cable                                    anchor power
 //
 // ESP32 UWB Pro interface (verify with calipers):
-//   PCB size       : UWB_L × UWB_W × UWB_H  (55 × 28 × 10 mm default)
+//   PCB size       : UWB_L x UWB_W x UWB_H  (55 x 28 x 10 mm default)
-//   Mounting holes : M2.5, 4× corners on UWB_HOLE_X × UWB_HOLE_Y pattern
+//   Mounting holes : M2.5, 4x corners on UWB_HOLE_X x UWB_HOLE_Y pattern
-//   USB-C port     : centred on short edge, UWB_USBC_W × UWB_USBC_H
+//   USB-C port     : centred on short edge, UWB_USBC_W x UWB_USBC_H
-//   Antenna area   : top face rear half — 10 mm keep-out of bracket material
+//   Antenna area   : top face rear half -- 10mm keep-out of bracket material
 //
-// Tilt angles (15° detent steps, set TILT_DEG before export):
+// Tilt angles (15deg detent steps, set TILT_DEG before export):
-//   0°  → horizontal face-up   (ceiling, antenna faces down)
+//   0deg  -> horizontal face-up   (ceiling, antenna faces down)
-//   30° → 30° downward         (wall near ceiling)  [default]
+//   30deg -> 30deg downward tilt  (wall near ceiling)  [default]
-//   45° → diagonal             (wall mid-height)
+//   45deg -> diagonal             (wall mid-height)
-//   90° → vertical face-out    (wall, antenna faces forward)
+//   90deg -> vertical face-out    (wall, antenna faces forward)
 //
 // RENDER options:
 //   "assembly"           full assembly at TILT_DEG (default)
@@ -61,40 +61,40 @@
 $fn  = 64;
 e    = 0.01;
 
-// ── Tilt angle (override per anchor, 0–90°, 15° steps) ───────────────────────
+// -- Tilt angle (override per anchor, 0-90deg, 15deg steps) ------------------
 TILT_DEG = 30;
 
-// ── ESP32 UWB Pro PCB dimensions (verify with calipers) ──────────────────────
+// -- ESP32 UWB Pro PCB dimensions (verify with calipers) ---------------------
-UWB_L         = 55.0;   // PCB length
+UWB_L         = 55.0;
-UWB_W         = 28.0;   // PCB width
+UWB_W         = 28.0;
-UWB_H         = 10.0;   // PCB + components height
+UWB_H         = 10.0;
-UWB_HOLE_X    = 47.5;   // M2.5 hole X span
+UWB_HOLE_X    = 47.5;
-UWB_HOLE_Y    = 21.0;   // M2.5 hole Y span
+UWB_HOLE_Y    = 21.0;
-UWB_USBC_W    =  9.5;   // USB-C receptacle width
+UWB_USBC_W    =  9.5;
-UWB_USBC_H    =  4.0;   // USB-C receptacle height
+UWB_USBC_H    =  4.0;
-UWB_ANTENNA_L = 20.0;   // antenna area at PCB rear (keep-out)
+UWB_ANTENNA_L = 20.0;
 
-// ── Wall base geometry ────────────────────────────────────────────────────────
+// -- Wall base geometry -------------------------------------------------------
-BASE_W        = 60.0;
+BASE_W         = 60.0;
-BASE_H        = 50.0;
+BASE_H         = 50.0;
-BASE_T        =  5.0;
+BASE_T         =  5.0;
-BASE_SCREW_D  =  4.5;   // M4 clearance
+BASE_SCREW_D   =  4.5;
-BASE_SCREW_HD =  8.5;   // countersink OD
+BASE_SCREW_HD  =  8.5;
-BASE_SCREW_HH =  3.5;   // countersink depth
+BASE_SCREW_HH  =  3.5;
-BASE_SCREW_SPC = 35.0;  // Z span between screw holes
+BASE_SCREW_SPC = 35.0;
-KNUCKLE_T     = BASE_T + 4.0;   // pivot ear depth (Y)
+KNUCKLE_T      = BASE_T + 4.0;
 
-// ── Tilt arm geometry ─────────────────────────────────────────────────────────
+// -- Tilt arm geometry --------------------------------------------------------
 ARM_W         = 12.0;
 ARM_T         =  5.0;
 ARM_L         = 35.0;
-PIVOT_D       =  3.3;   // M3 clearance
+PIVOT_D       =  3.3;
 PIVOT_NUT_AF  =  5.5;
 PIVOT_NUT_H   =  2.4;
-DETENT_D      =  3.2;   // detent notch diameter
+DETENT_D      =  3.2;
-DETENT_R      =  8.0;   // detent notch radius from pivot
+DETENT_R      =  8.0;
 
-// ── Anchor cradle geometry ────────────────────────────────────────────────────
+// -- Anchor cradle geometry ---------------------------------------------------
 CRADLE_WALL_T  =  3.5;
 CRADLE_BACK_T  =  4.0;
 CRADLE_FLOOR_T =  3.0;
@@ -104,19 +104,19 @@ STANDOFF_H     =  3.0;
 STANDOFF_OD    =  5.5;
 LABEL_W        = UWB_L - 4.0;
 LABEL_H        = UWB_W * 0.55;
-LABEL_T        =  1.2;   // label card thickness
+LABEL_T        =  1.2;
 
-// ── USB-C cable routing ───────────────────────────────────────────────────────
+// -- USB-C routing ------------------------------------------------------------
 USBC_CHAN_W   = 11.0;
 USBC_CHAN_H   =  7.0;
 
-// ── Cable clip ────────────────────────────────────────────────────────────────
+// -- Cable clip ---------------------------------------------------------------
 CLIP_CABLE_D  =  4.5;
 CLIP_T        =  2.0;
 CLIP_BODY_W   = 16.0;
 CLIP_BODY_H   = 10.0;
 
-// ── Fasteners ─────────────────────────────────────────────────────────────────
+// -- Fasteners ----------------------------------------------------------------
 M2P5_D    = 2.7;
 M3_D      = 3.3;
 M3_NUT_AF = 5.5;
@@ -137,61 +137,35 @@ else if (RENDER == "cable_clip_stl")     cable_clip();
 // ASSEMBLY PREVIEW
 // ============================================================
 module assembly_preview() {
-    // Ghost wall surface
     %color("Wheat", 0.22)
         translate([-BASE_W/2, -10, -BASE_H/2])
             cube([BASE_W, 10, BASE_H + 40]);
-
+    color("OliveDrab", 0.85)  wall_base();
-    // Wall base
-    color("OliveDrab", 0.85)
-        wall_base();
-
-    // Tilt arm at TILT_DEG, pivoting at knuckle
     color("SteelBlue", 0.85)
-        translate([0, KNUCKLE_T, 0])
+        translate([0, KNUCKLE_T, 0]) rotate([TILT_DEG,0,0]) tilt_arm();
-            rotate([TILT_DEG, 0, 0])
-                tilt_arm();
-
-    // Anchor cradle at arm end
     color("DarkSlateGray", 0.85)
-        translate([0, KNUCKLE_T, 0])
+        translate([0, KNUCKLE_T, 0]) rotate([TILT_DEG,0,0])
-            rotate([TILT_DEG, 0, 0])
+            translate([0, ARM_T, ARM_L]) anchor_cradle();
-                translate([0, ARM_T, ARM_L])
-                    anchor_cradle();
-
-    // PCB ghost
     %color("ForestGreen", 0.38)
-        translate([0, KNUCKLE_T, 0])
+        translate([0, KNUCKLE_T, 0]) rotate([TILT_DEG,0,0])
-            rotate([TILT_DEG, 0, 0])
+            translate([-UWB_L/2, ARM_T+CRADLE_BACK_T,
-                translate([-UWB_L/2,
+                       ARM_L+CRADLE_FLOOR_T+STANDOFF_H])
-                           ARM_T + CRADLE_BACK_T,
+                cube([UWB_L, UWB_W, UWB_H]);
-                           ARM_L + CRADLE_FLOOR_T + STANDOFF_H])
-                    cube([UWB_L, UWB_W, UWB_H]);
-
-    // Cable clip at arm mid-point
     color("DimGray", 0.70)
-        translate([ARM_W/2, KNUCKLE_T, 0])
+        translate([ARM_W/2, KNUCKLE_T, 0]) rotate([TILT_DEG,0,0])
-            rotate([TILT_DEG, 0, 0])
+            translate([0, ARM_T+e, ARM_L/2]) rotate([0,-90,90]) cable_clip();
-                translate([0, ARM_T + e, ARM_L/2])
-                    rotate([0, -90, 90])
-                        cable_clip();
 }
 
 // ============================================================
-// PART 1 — WALL BASE
+// PART 1 -- WALL BASE
 // ============================================================
-// Flat backplate screws to wall or ceiling with 2× countersunk
+// Flat backplate, 2x countersunk M4/#6 wood screws on 35mm centres.
-// M4/#6 wood screws on BASE_SCREW_SPC (35 mm) centres.
+// Two pivot ears straddle the tilt arm; M3 pivot bolt through both.
-// Two pivot ears straddle the tilt arm; M3 pivot bolt passes through
+// Detent arc on +X ear inner face: 7 notches at 15deg steps (0-90deg).
-// both ears and arm knuckle.
+// Shallow rear recess for installation-zone label strip.
-// Detent arc on inner face of +X ear: 7 notches at 15° steps (0–90°)
+// Same part for wall mount and ceiling mount.
-// so tilt can be set without a protractor.
-// Shallow rear recess accepts a printed installation-zone label.
 //
-// Dual-use: flat face to wall (vertical screw axis) or flat face
+// Print: backplate flat on bed, PETG, 5 perims, 40% gyroid.
-// to ceiling (horizontal screw axis) — same part either way.
-//
-// Print: backplate flat on bed, PETG, 5 perims, 40 % gyroid.
 module wall_base() {
     ear_h   = ARM_W + 3.0;
     ear_t   = 6.0;
@@ -199,144 +173,92 @@ module wall_base() {
 
     difference() {
         union() {
-            // ── Backplate ────────────────────────────────────────────────
             translate([-BASE_W/2, -BASE_T, -BASE_H/2])
                 cube([BASE_W, BASE_T, BASE_H]);
-
-            // ── Two pivot ears ────────────────────────────────────────────
             for (ex = [-(ear_sep/2 + ear_t), ear_sep/2])
-                translate([ex, -BASE_T + e, -ear_h/2])
+                translate([ex, -BASE_T+e, -ear_h/2])
-                    cube([ear_t, KNUCKLE_T + e, ear_h]);
+                    cube([ear_t, KNUCKLE_T+e, ear_h]);
-
-            // ── Stiffening gussets ────────────────────────────────────────
             for (ex = [-(ear_sep/2 + ear_t), ear_sep/2])
                 hull() {
                     translate([ex, -BASE_T, -ear_h/4])
-                        cube([ear_t, BASE_T - 1, ear_h/2]);
+                        cube([ear_t, BASE_T-1, ear_h/2]);
-                    translate([ex + (ex < 0 ? ear_t*0.6 : 0),
+                    translate([ex + (ex<0 ? ear_t*0.5 : 0), -BASE_T, -ear_h/6])
-                               -BASE_T, -ear_h/6])
+                        cube([ear_t*0.5, 1, ear_h/3]);
-                        cube([ear_t * 0.4, 1, ear_h/3]);
                 }
         }
-
-        // ── 2× countersunk wall screws ────────────────────────────────────
         for (sz = [-BASE_SCREW_SPC/2, BASE_SCREW_SPC/2]) {
-            translate([0, -BASE_T - e, sz])
+            translate([0, -BASE_T-e, sz]) rotate([-90,0,0])
-                rotate([-90, 0, 0])
+                cylinder(d=BASE_SCREW_D, h=BASE_T+2*e);
-                    cylinder(d = BASE_SCREW_D, h = BASE_T + 2*e);
+            translate([0, -BASE_T-e, sz]) rotate([-90,0,0])
-            translate([0, -BASE_T - e, sz])
+                cylinder(d1=BASE_SCREW_HD, d2=BASE_SCREW_D, h=BASE_SCREW_HH+e);
-                rotate([-90, 0, 0])
-                    cylinder(d1 = BASE_SCREW_HD, d2 = BASE_SCREW_D,
-                             h = BASE_SCREW_HH + e);
         }
-
+        translate([-(ear_sep/2+ear_t+e), KNUCKLE_T*0.55, 0])
-        // ── Pivot bolt bore (M3, through both ears) ───────────────────────
+            rotate([0,90,0]) cylinder(d=PIVOT_D, h=ear_sep+2*ear_t+2*e);
-        translate([-(ear_sep/2 + ear_t + e), KNUCKLE_T * 0.55, 0])
+        translate([ear_sep/2+ear_t-PIVOT_NUT_H-0.4, KNUCKLE_T*0.55, 0])
-            rotate([0, 90, 0])
+            rotate([0,90,0])
-                cylinder(d = PIVOT_D, h = ear_sep + 2*ear_t + 2*e);
+                cylinder(d=PIVOT_NUT_AF/cos(30), h=PIVOT_NUT_H+0.5, $fn=6);
-
-        // ── M3 nyloc nut pocket (outer face of one ear) ───────────────────
-        translate([ear_sep/2 + ear_t - PIVOT_NUT_H - 0.4,
-                   KNUCKLE_T * 0.55, 0])
-            rotate([0, 90, 0])
-                cylinder(d = PIVOT_NUT_AF / cos(30),
-                         h = PIVOT_NUT_H + 0.5, $fn = 6);
-
-        // ── Detent arc — 7 notches at 15° steps on +X ear inner face ─────
         for (da = [0 : 15 : 90])
-            translate([ear_sep/2 - e,
+            translate([ear_sep/2-e,
-                       KNUCKLE_T * 0.55 + DETENT_R * sin(da),
+                       KNUCKLE_T*0.55 + DETENT_R*sin(da),
-                       DETENT_R * cos(da)])
+                       DETENT_R*cos(da)])
-                rotate([0, 90, 0])
+                rotate([0,90,0]) cylinder(d=DETENT_D, h=ear_t*0.45+e);
-                    cylinder(d = DETENT_D, h = ear_t * 0.45 + e);
+        translate([0, -BASE_T-e, 0]) rotate([-90,0,0])
-
+            cube([BASE_W-12, BASE_H-16, 1.6], center=true);
-        // ── Installation label recess (rear face of backplate) ────────────
+        translate([0, -BASE_T+1.5, 0])
-        translate([0, -BASE_T - e, 0])
+            cube([BASE_W-14, BASE_T-3, BASE_H-20], center=true);
-            rotate([-90, 0, 0])
-                cube([BASE_W - 12, BASE_H - 16, 1.6], center = true);
-
-        // ── Lightening pocket ─────────────────────────────────────────────
-        translate([0, -BASE_T + 1.5, 0])
-            cube([BASE_W - 14, BASE_T - 3, BASE_H - 20], center = true);
     }
 }
 
 // ============================================================
-// PART 2 — TILT ARM
+// PART 2 -- TILT ARM
 // ============================================================
-// Pivoting arm linking wall_base pivot ears to anchor_cradle.
+// Pivoting arm linking wall_base ears to anchor_cradle.
-// Knuckle end (Z=0): M3 pivot bore + spring-plunger detent pocket
+// Knuckle (Z=0): M3 pivot bore + spring-plunger detent pocket (3mm).
-//   that indexes into the base ear detent arc notches.
+// Cradle end (Z=ARM_L): 2x M3 bolt attachment stub.
-// Cradle end (Z=ARM_L): 2× M3 bolt attachment to cradle back wall.
+// USB-C cable channel groove on outer +Y face, full arm length.
-// USB-C cable channel runs along outer (+Y) face, full arm length.
 //
-// Print: knuckle face flat on bed, PETG, 5 perims, 40 % gyroid.
+// Print: knuckle face flat on bed, PETG, 5 perims, 40% gyroid.
 module tilt_arm() {
     total_h = ARM_L + 10;
-
     difference() {
         union() {
-            // ── Arm body ─────────────────────────────────────────────────
+            translate([-ARM_W/2, 0, 0]) cube([ARM_W, ARM_T, total_h]);
-            translate([-ARM_W/2, 0, 0])
+            translate([0, ARM_T/2, 0]) rotate([90,0,0])
-                cube([ARM_W, ARM_T, total_h]);
+                cylinder(d=ARM_W, h=ARM_T, center=true);
-
-            // ── Knuckle boss (rounded pivot end) ─────────────────────────
-            translate([0, ARM_T/2, 0])
-                rotate([90, 0, 0])
-                    cylinder(d = ARM_W, h = ARM_T, center = true);
-
-            // ── Cradle attach stub (Z = ARM_L) ────────────────────────────
             translate([-ARM_W/2, 0, ARM_L])
-                cube([ARM_W, ARM_T + CRADLE_BACK_T, ARM_T]);
+                cube([ARM_W, ARM_T+CRADLE_BACK_T, ARM_T]);
         }
-
+        translate([-ARM_W/2-e, ARM_T/2, 0]) rotate([0,90,0])
-        // ── M3 pivot bore ─────────────────────────────────────────────────
+            cylinder(d=PIVOT_D, h=ARM_W+2*e);
-        translate([-ARM_W/2 - e, ARM_T/2, 0])
+        translate([0, ARM_T+e, 0]) rotate([90,0,0])
-            rotate([0, 90, 0])
+            cylinder(d=3.2, h=4+e);
-                cylinder(d = PIVOT_D, h = ARM_W + 2*e);
+        translate([-USBC_CHAN_W/2, ARM_T-e, ARM_T+4])
-
+            cube([USBC_CHAN_W, USBC_CHAN_H, ARM_L-ARM_T-8]);
-        // ── Detent plunger pocket (3 mm spring-ball, outer +Y face) ──────
-        translate([0, ARM_T + e, 0])
-            rotate([90, 0, 0])
-                cylinder(d = 3.2, h = 4 + e);
-
-        // ── USB-C cable channel (outer +Y face, mid-arm length) ───────────
-        translate([-USBC_CHAN_W/2, ARM_T - e, ARM_T + 4])
-            cube([USBC_CHAN_W, USBC_CHAN_H, ARM_L - ARM_T - 8]);
-
-        // ── Cradle attach bolt holes (2× M3 at cradle stub) ───────────────
         for (bx = [-ARM_W/4, ARM_W/4])
-            translate([bx, ARM_T/2, ARM_L + ARM_T/2])
+            translate([bx, ARM_T/2, ARM_L+ARM_T/2]) rotate([90,0,0])
-                rotate([90, 0, 0])
+                cylinder(d=M3_D, h=ARM_T+CRADLE_BACK_T+2*e);
-                    cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e);
-
-        // ── M3 nut pockets (front of cradle stub) ─────────────────────────
         for (bx = [-ARM_W/4, ARM_W/4])
-            translate([bx, ARM_T/2, ARM_L + ARM_T/2])
+            translate([bx, ARM_T/2, ARM_L+ARM_T/2]) rotate([-90,0,0])
-                rotate([-90, 0, 0])
+                cylinder(d=M3_NUT_AF/cos(30), h=M3_NUT_H+0.5, $fn=6);
-                    cylinder(d = M3_NUT_AF / cos(30),
-                             h = M3_NUT_H + 0.5, $fn = 6);
-
-        // ── Lightening pocket ─────────────────────────────────────────────
         translate([0, ARM_T/2, ARM_L/2])
-            cube([ARM_W - 4, ARM_T - 2, ARM_L - 18], center = true);
+            cube([ARM_W-4, ARM_T-2, ARM_L-18], center=true);
     }
 }
 
 // ============================================================
-// PART 3 — ANCHOR CRADLE
+// PART 3 -- ANCHOR CRADLE
 // ============================================================
 // Open-front U-cradle for ESP32 UWB Pro PCB.
-// PCB retained on 4× M2.5 standoffs matching UWB_HOLE_X × UWB_HOLE_Y.
+// 4x M2.5 standoffs on UWB_HOLE_X x UWB_HOLE_Y pattern.
-// Back wall features:
+// Back wall: USB-C exit slot + routing groove, label card slot,
-//   • USB-C exit slot  — aligns with PCB USB-C port
+//            antenna keep-out cutout (material removed above antenna area).
-//   • USB-C groove     — cable routes from slot toward arm channel
+// Front retaining lip prevents PCB sliding out.
-//   • Label card slot  — insert printed strip for anchor ID
+// Two attachment tabs bolt to tilt_arm cradle stub via M3.
-//   • Antenna keep-out — back wall material removed above antenna area
-// Front lip prevents PCB from sliding forward.
-// Two attachment tabs bolt to tilt_arm cradle stub.
 //
-// Print: back wall flat on bed, PETG, 5 perims, 40 % gyroid.
+// Label card slot: insert paper/laminate strip to ID this anchor
+// (e.g. "UWB-A3 NE-CORNER"), accessible from open cradle end.
+//
+// Print: back wall flat on bed, PETG, 5 perims, 40% gyroid.
 module anchor_cradle() {
     outer_l  = UWB_L + 2*CRADLE_WALL_T;
     outer_w  = UWB_W + CRADLE_FLOOR_T;
@@ -345,119 +267,75 @@ module anchor_cradle() {
 
     difference() {
         union() {
-            // ── Cradle body ───────────────────────────────────────────────
+            translate([-outer_l/2, 0, 0]) cube([outer_l, outer_w, total_z]);
-            translate([-outer_l/2, 0, 0])
+            translate([-outer_l/2, outer_w-CRADLE_LIP_T, 0])
-                cube([outer_l, outer_w, total_z]);
-
-            // ── Front retaining lip ───────────────────────────────────────
-            translate([-outer_l/2, outer_w - CRADLE_LIP_T, 0])
                 cube([outer_l, CRADLE_LIP_T, CRADLE_LIP_H]);
-
-            // ── Arm attachment tabs (behind back wall) ─────────────────────
             for (tx = [-ARM_W/4, ARM_W/4])
-                translate([tx - 4, -CRADLE_BACK_T, 0])
+                translate([tx-4, -CRADLE_BACK_T, 0])
-                    cube([8, CRADLE_BACK_T + 1, total_z]);
+                    cube([8, CRADLE_BACK_T+1, total_z]);
         }
-
+        translate([-UWB_L/2, 0, pcb_z]) cube([UWB_L, UWB_W+1, UWB_H+4]);
-        // ── PCB pocket ────────────────────────────────────────────────────
+        translate([0, -CRADLE_BACK_T-e, pcb_z+UWB_H/2-UWB_USBC_H/2])
-        translate([-UWB_L/2, 0, pcb_z])
+            cube([UWB_USBC_W+2, CRADLE_BACK_T+2*e, UWB_USBC_H+2],
-            cube([UWB_L, UWB_W + 1, UWB_H + 4]);
+                 center=[true,false,false]);
-
+        translate([0, -CRADLE_BACK_T-e, -e])
-        // ── USB-C exit slot (through back wall, aligned to PCB port) ─────
+            cube([USBC_CHAN_W, USBC_CHAN_H, pcb_z+UWB_H/2+USBC_CHAN_H],
-        translate([0, -CRADLE_BACK_T - e,
+                 center=[true,false,false]);
-                   pcb_z + UWB_H/2 - UWB_USBC_H/2])
+        translate([0, -CRADLE_BACK_T-e, pcb_z+UWB_H/2])
-            cube([UWB_USBC_W + 2, CRADLE_BACK_T + 2*e, UWB_USBC_H + 2],
+            cube([LABEL_W, LABEL_T+0.3, LABEL_H], center=[true,false,false]);
-                 center = [true, false, false]);
+        translate([0, -e, pcb_z+UWB_H-UWB_ANTENNA_L])
-
+            cube([UWB_L-4, CRADLE_BACK_T+2*e, UWB_ANTENNA_L+4],
-        // ── USB-C cable routing groove (outer back wall face) ─────────────
+                 center=[true,false,false]);
-        translate([0, -CRADLE_BACK_T - e, -e])
-            cube([USBC_CHAN_W, USBC_CHAN_H, pcb_z + UWB_H/2 + USBC_CHAN_H],
-                 center = [true, false, false]);
-
-        // ── Label card slot (insert from below, rear face upper half) ─────
-        // Paper/laminate card strip identifying this anchor instance
-        translate([0, -CRADLE_BACK_T - e, pcb_z + UWB_H/2])
-            cube([LABEL_W, LABEL_T + 0.3, LABEL_H],
-                 center = [true, false, false]);
-
-        // ── Antenna keep-out: remove back wall above antenna area ─────────
-        translate([0, -e, pcb_z + UWB_H - UWB_ANTENNA_L])
-            cube([UWB_L - 4, CRADLE_BACK_T + 2*e, UWB_ANTENNA_L + 4],
-                 center = [true, false, false]);
-
-        // ── Arm bolt holes through attachment tabs ────────────────────────
         for (tx = [-ARM_W/4, ARM_W/4])
-            translate([tx, ARM_T/2 - CRADLE_BACK_T, total_z/2])
+            translate([tx, ARM_T/2-CRADLE_BACK_T, total_z/2])
-                rotate([-90, 0, 0])
+                rotate([-90,0,0])
-                    cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e);
+                    cylinder(d=M3_D, h=ARM_T+CRADLE_BACK_T+2*e);
-
+        for (side_x = [-outer_l/2-e, outer_l/2-CRADLE_WALL_T-e])
-        // ── Lightening slots in side walls ────────────────────────────────
+            translate([side_x, 2, pcb_z+2])
-        for (side_x = [-outer_l/2 - e, outer_l/2 - CRADLE_WALL_T - e])
+                cube([CRADLE_WALL_T+2*e, UWB_W-4, UWB_H-4]);
-            translate([side_x, 2, pcb_z + 2])
-                cube([CRADLE_WALL_T + 2*e, UWB_W - 4, UWB_H - 4]);
     }
-
-    // ── M2.5 standoff bosses (positive, inside cradle floor) ──────────────
     for (hx = [-UWB_HOLE_X/2, UWB_HOLE_X/2])
-    for (hy = [(outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2,
+    for (hy = [(outer_w-UWB_W)/2 + (UWB_W-UWB_HOLE_Y)/2,
-               (outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2 + UWB_HOLE_Y])
+               (outer_w-UWB_W)/2 + (UWB_W-UWB_HOLE_Y)/2 + UWB_HOLE_Y])
         difference() {
-            translate([hx, hy, CRADLE_FLOOR_T - e])
+            translate([hx, hy, CRADLE_FLOOR_T-e])
-                cylinder(d = STANDOFF_OD, h = STANDOFF_H + e);
+                cylinder(d=STANDOFF_OD, h=STANDOFF_H+e);
-            translate([hx, hy, CRADLE_FLOOR_T - 2*e])
+            translate([hx, hy, CRADLE_FLOOR_T-2*e])
-                cylinder(d = M2P5_D, h = STANDOFF_H + 4);
+                cylinder(d=M2P5_D, h=STANDOFF_H+4);
         }
 }
 
 // ============================================================
-// PART 4 — CABLE CLIP
+// PART 4 -- CABLE CLIP
 // ============================================================
 // Snap-on C-clip retaining USB-C cable along tilt arm outer face.
-// Presses onto ARM_T-wide arm with PETG snap tongues.
+// Presses onto ARM_T-wide arm with flexible PETG snap tongues.
-// Open-front cable channel for push-in cable insertion.
+// Print x2-3 per anchor, spaced 25mm along arm.
-// Print ×2–3 per anchor, spaced 25 mm along arm.
 //
-// Print: clip-opening face down, PETG, 3 perims, 20 % infill.
+// Print: clip-opening face down, PETG, 3 perims, 20% infill.
 module cable_clip() {
     ch_r   = CLIP_CABLE_D/2 + CLIP_T;
     snap_t = 1.6;
-
     difference() {
         union() {
-            // ── Body plate ────────────────────────────────────────────────
             translate([-CLIP_BODY_W/2, 0, 0])
                 cube([CLIP_BODY_W, CLIP_T, CLIP_BODY_H]);
-
+            translate([0, CLIP_T+ch_r, CLIP_BODY_H/2]) rotate([0,90,0])
-            // ── Cable channel (C-shape, opens toward +Y) ─────────────────
+                difference() {
-            translate([0, CLIP_T + ch_r, CLIP_BODY_H/2])
+                    cylinder(r=ch_r, h=CLIP_BODY_W, center=true);
-                rotate([0, 90, 0])
+                    cylinder(r=CLIP_CABLE_D/2, h=CLIP_BODY_W+2*e, center=true);
-                    difference() {
+                    translate([0, ch_r+e, 0])
-                        cylinder(r = ch_r, h = CLIP_BODY_W, center = true);
+                        cube([CLIP_CABLE_D*0.85, ch_r*2+2*e, CLIP_BODY_W+2*e],
-                        cylinder(r = CLIP_CABLE_D/2,
+                             center=true);
-                                 h = CLIP_BODY_W + 2*e, center = true);
+                }
-                        // open insertion slot
+            for (tx = [-CLIP_BODY_W/2+1.5, CLIP_BODY_W/2-1.5-snap_t])
-                        translate([0, ch_r + e, 0])
+                translate([tx, -ARM_T-1, 0])
-                            cube([CLIP_CABLE_D * 0.85,
+                    cube([snap_t, ARM_T+1+CLIP_T, CLIP_BODY_H]);
-                                  ch_r * 2 + 2*e,
+            for (tx = [-CLIP_BODY_W/2+1.5, CLIP_BODY_W/2-1.5-snap_t])
-                                  CLIP_BODY_W + 2*e], center = true);
+                translate([tx+snap_t/2, -ARM_T-1, CLIP_BODY_H/2])
-                    }
+                    rotate([0,90,0]) cylinder(d=2, h=snap_t, center=true);
-
-            // ── Snap tongues (straddle arm, -Y side of body) ─────────────
-            for (tx = [-CLIP_BODY_W/2 + 1.5,
-                       CLIP_BODY_W/2 - 1.5 - snap_t])
-                translate([tx, -ARM_T - 1, 0])
-                    cube([snap_t, ARM_T + 1 + CLIP_T, CLIP_BODY_H]);
-
-            // ── Snap barbs ────────────────────────────────────────────────
-            for (tx = [-CLIP_BODY_W/2 + 1.5,
-                       CLIP_BODY_W/2 - 1.5 - snap_t])
-                translate([tx + snap_t/2, -ARM_T - 1, CLIP_BODY_H/2])
-                    rotate([0, 90, 0])
-                        cylinder(d = 2, h = snap_t, center = true);
         }
-
+        translate([0, -ARM_T-1-e, CLIP_BODY_H/2])
-        // ── Arm slot (arm body passes between tongues) ─────────────────────
+            cube([CLIP_BODY_W-6, ARM_T+2, CLIP_BODY_H-4], center=true);
-        translate([0, -ARM_T - 1 - e, CLIP_BODY_H/2])
-            cube([CLIP_BODY_W - 6, ARM_T + 2, CLIP_BODY_H - 4], center = true);
     }
 }

esp32/uwb_tag/platformio.ini

@@ -0,0 +1,30 @@
+; SaltyBot UWB Tag Firmware — Issue #545
+; Target: Makerfabs ESP32 UWB Pro with Display (DW3000 + SSD1306 OLED)
+;
+; The tag is battery-powered, worn by the person being tracked.
+; It initiates DS-TWR ranging with each anchor in round-robin,
+; shows status on OLED display, and sends data via ESP-NOW.
+;
+; Library: Makerfabs MaUWB_DW3000
+;   https://github.com/Makerfabs/MaUWB_DW3000
+;
+; Flash:
+;   pio run -e tag --target upload
+; Monitor (USB debug):
+;   pio device monitor -b 115200
+
+[env:tag]
+platform      = espressif32
+board         = esp32dev
+framework     = arduino
+monitor_speed = 115200
+upload_speed  = 921600
+lib_deps =
+    https://github.com/Makerfabs/MaUWB_DW3000.git
+    adafruit/Adafruit SSD1306@^2.5.7
+    adafruit/Adafruit GFX Library@^1.11.5
+build_flags =
+    -DCORE_DEBUG_LEVEL=0
+    -DTAG_ID=0x01        ; unique per tag (0x01–0xFE)
+    -DNUM_ANCHORS=2      ; number of anchors to range with
+    -DRANGE_INTERVAL_MS=50  ; 20 Hz round-robin across anchors

esp32/uwb_tag/src/main.cpp

@@ -0,0 +1,615 @@
+/*
+ * uwb_tag — SaltyBot ESP32 UWB Pro tag firmware (DS-TWR initiator)
+ * Issue #545 + display/ESP-NOW/e-stop extensions
+ *
+ * Hardware: Makerfabs ESP32 UWB Pro with Display (DW3000 + SSD1306 OLED)
+ *
+ * Role
+ * ────
+ *   Tag is worn by a person riding an EUC while SaltyBot follows.
+ *   Initiates DS-TWR ranging with 2 anchors on the robot at 20 Hz.
+ *   Shows distance/status on OLED.  Sends range data via ESP-NOW
+ *   (no WiFi AP needed — peer-to-peer, ~1ms latency, works outdoors).
+ *   GPIO 0 = emergency stop button (active low).
+ *
+ * Serial output (USB, 115200) — debug
+ * ────────────────────────────────────
+ *   +RANGE:<anchor_id>,<range_mm>,<rssi_dbm>\r\n
+ *
+ * ESP-NOW packet (broadcast, 20 bytes)
+ * ─────────────────────────────────────
+ *   [0-1]  magic  0x5B 0x01
+ *   [2]    tag_id
+ *   [3]    msg_type   0x10=range, 0x20=estop, 0x30=heartbeat
+ *   [4]    anchor_id
+ *   [5-8]  range_mm   (int32_t LE)
+ *   [9-12] rssi_dbm   (float LE)
+ *   [13-16] timestamp  (uint32_t millis)
+ *   [17]   battery_pct (0-100 or 0xFF)
+ *   [18]   flags       bit0=estop_active
+ *   [19]   seq_num_lo  (uint8_t, rolling)
+ *
+ * Pin mapping — Makerfabs ESP32 UWB Pro with Display
+ * ──────────────────────────────────────────────────
+ *   SPI SCK  18   SPI MISO 19   SPI MOSI 23
+ *   DW CS    21   DW RST   27   DW IRQ   34
+ *   I2C SDA   4   I2C SCL   5   OLED addr 0x3C
+ *   LED       2   E-STOP    0 (BOOT, active LOW)
+ */
+
+#include <Arduino.h>
+#include <SPI.h>
+#include <Wire.h>
+#include <math.h>
+#include <WiFi.h>
+#include <esp_now.h>
+#include <esp_wifi.h>
+
+#include "dw3000.h"
+
+#include <Adafruit_GFX.h>
+#include <Adafruit_SSD1306.h>
+
+/* ── Configurable ───────────────────────────────────────────────── */
+
+#ifndef TAG_ID
+#  define TAG_ID          0x01
+#endif
+
+#ifndef NUM_ANCHORS
+#  define NUM_ANCHORS     2
+#endif
+
+#ifndef RANGE_INTERVAL_MS
+#  define RANGE_INTERVAL_MS  50   /* 20 Hz round-robin */
+#endif
+
+#define SERIAL_BAUD       115200
+
+/* ── Pins ───────────────────────────────────────────────────────── */
+
+#define PIN_SCK   18
+#define PIN_MISO  19
+#define PIN_MOSI  23
+#define PIN_CS    21
+#define PIN_RST   27
+#define PIN_IRQ   34
+
+#define PIN_SDA    4
+#define PIN_SCL    5
+
+#define PIN_LED    2
+#define PIN_ESTOP  0   /* BOOT button, active LOW */
+
+/* ── OLED ───────────────────────────────────────────────────────── */
+
+#define SCREEN_W  128
+#define SCREEN_H   64
+Adafruit_SSD1306 display(SCREEN_W, SCREEN_H, &Wire, -1);
+
+/* ── ESP-NOW ────────────────────────────────────────────────────── */
+
+#define ESPNOW_MAGIC_0   0x5B   /* "SB" */
+#define ESPNOW_MAGIC_1   0x01   /* v1   */
+
+#define MSG_RANGE         0x10
+#define MSG_ESTOP         0x20
+#define MSG_HEARTBEAT     0x30
+
+static uint8_t broadcast_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+static uint8_t g_seq = 0;
+
+#pragma pack(push, 1)
+struct EspNowPacket {
+    uint8_t  magic[2];
+    uint8_t  tag_id;
+    uint8_t  msg_type;
+    uint8_t  anchor_id;
+    int32_t  range_mm;
+    float    rssi_dbm;
+    uint32_t timestamp_ms;
+    uint8_t  battery_pct;
+    uint8_t  flags;
+    uint8_t  seq_num;
+    uint8_t  _pad;          /* pad to 20 bytes */
+};
+#pragma pack(pop)
+
+static_assert(sizeof(EspNowPacket) == 20, "packet must be 20 bytes");
+
+/* ── DW3000 PHY config (must match anchor) ──────────────────────── */
+
+static dwt_config_t dw_cfg = {
+    5,                 /* channel 5 */
+    DWT_PLEN_128,
+    DWT_PAC8,
+    9, 9,              /* TX/RX preamble code */
+    1,                 /* SFD type */
+    DWT_BR_6M8,
+    DWT_PHR_MODE_STD,
+    DWT_PHR_RATE_DATA,
+    (129 + 8 - 8),
+    DWT_STS_MODE_OFF,
+    DWT_STS_LEN_64,
+    DWT_PDOA_M0,
+};
+
+/* ── Frame format ──────────────────────────────────────────────── */
+
+#define FTYPE_POLL   0x01
+#define FTYPE_RESP   0x02
+#define FTYPE_FINAL  0x03
+
+#define FRAME_HDR   3
+#define FCS_LEN     2
+
+#define POLL_FRAME_LEN   (FRAME_HDR + FCS_LEN)
+#define RESP_PAYLOAD     10
+#define RESP_FRAME_LEN   (FRAME_HDR + RESP_PAYLOAD + FCS_LEN)
+#define FINAL_PAYLOAD    15
+#define FINAL_FRAME_LEN  (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN)
+
+/* ── Timing ────────────────────────────────────────────────────── */
+
+#define FINAL_TX_DLY_US     500UL
+#define DWT_TICKS_PER_US    63898UL
+#define FINAL_TX_DLY_TICKS  (FINAL_TX_DLY_US * DWT_TICKS_PER_US)
+#define RESP_RX_TIMEOUT_US  3000
+
+#define SPEED_OF_LIGHT  299702547.0
+#define DWT_TS_MASK     0xFFFFFFFFFFULL
+#define ANT_DELAY       16385
+
+/* ── ISR state ──────────────────────────────────────────────────── */
+
+static volatile bool g_rx_ok   = false;
+static volatile bool g_tx_done = false;
+static volatile bool g_rx_err  = false;
+static volatile bool g_rx_to   = false;
+
+static uint8_t  g_rx_buf[128];
+static uint32_t g_rx_len = 0;
+
+static void cb_tx_done(const dwt_cb_data_t *) { g_tx_done = true; }
+static void cb_rx_ok(const dwt_cb_data_t *d) {
+    g_rx_len = d->datalength;
+    if (g_rx_len > sizeof(g_rx_buf)) g_rx_len = sizeof(g_rx_buf);
+    dwt_readrxdata(g_rx_buf, g_rx_len, 0);
+    g_rx_ok = true;
+}
+static void cb_rx_err(const dwt_cb_data_t *) { g_rx_err = true; }
+static void cb_rx_to(const dwt_cb_data_t  *) { g_rx_to  = true; }
+
+/* ── Timestamp helpers ──────────────────────────────────────────── */
+
+static uint64_t ts_read(const uint8_t *p) {
+    uint64_t v = 0;
+    for (int i = 4; i >= 0; i--) v = (v << 8) | p[i];
+    return v;
+}
+
+static void ts_write(uint8_t *p, uint64_t v) {
+    for (int i = 0; i < 5; i++, v >>= 8) p[i] = (uint8_t)(v & 0xFF);
+}
+
+static inline uint64_t ts_diff(uint64_t later, uint64_t earlier) {
+    return (later - earlier) & DWT_TS_MASK;
+}
+
+static inline double ticks_to_s(uint64_t t) {
+    return (double)t / (128.0 * 499200000.0);
+}
+
+static float rx_power_dbm(void) {
+    dwt_rxdiag_t d;
+    dwt_readdiagnostics(&d);
+    if (d.maxGrowthCIR == 0 || d.rxPreamCount == 0) return 0.0f;
+    float f = (float)d.maxGrowthCIR;
+    float n = (float)d.rxPreamCount;
+    return 10.0f * log10f((f * f) / (n * n)) - 121.74f;
+}
+
+/* ── Shared state for display ───────────────────────────────────── */
+
+static int32_t g_anchor_range_mm[NUM_ANCHORS];   /* last range per anchor */
+static float   g_anchor_rssi[NUM_ANCHORS];        /* last RSSI per anchor */
+static uint32_t g_anchor_last_ok[NUM_ANCHORS];    /* millis() of last good range */
+static bool    g_estop_active = false;
+
+/* ── ESP-NOW send helper ────────────────────────────────────────── */
+
+static void espnow_send(uint8_t msg_type, uint8_t anchor_id,
+                         int32_t range_mm, float rssi) {
+    EspNowPacket pkt = {};
+    pkt.magic[0]     = ESPNOW_MAGIC_0;
+    pkt.magic[1]     = ESPNOW_MAGIC_1;
+    pkt.tag_id       = TAG_ID;
+    pkt.msg_type     = msg_type;
+    pkt.anchor_id    = anchor_id;
+    pkt.range_mm     = range_mm;
+    pkt.rssi_dbm     = rssi;
+    pkt.timestamp_ms = millis();
+    pkt.battery_pct  = 0xFF;   /* TODO: read ADC battery voltage */
+    pkt.flags        = g_estop_active ? 0x01 : 0x00;
+    pkt.seq_num      = g_seq++;
+
+    esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
+}
+
+/* ── E-Stop handling ────────────────────────────────────────────── */
+
+static uint32_t g_estop_last_tx = 0;
+
+static void estop_check(void) {
+    bool pressed = (digitalRead(PIN_ESTOP) == LOW);
+
+    if (pressed && !g_estop_active) {
+        /* Just pressed — enter e-stop */
+        g_estop_active = true;
+        Serial.println("+ESTOP:ACTIVE");
+    }
+
+    if (g_estop_active && pressed) {
+        /* While held: send e-stop at 10 Hz */
+        if (millis() - g_estop_last_tx >= 100) {
+            espnow_send(MSG_ESTOP, 0xFF, 0, 0.0f);
+            g_estop_last_tx = millis();
+        }
+    }
+
+    if (!pressed && g_estop_active) {
+        /* Released: send 3x clear packets, resume */
+        for (int i = 0; i < 3; i++) {
+            g_estop_active = false;  /* clear flag before sending so flags=0 */
+            espnow_send(MSG_ESTOP, 0xFF, 0, 0.0f);
+            delay(10);
+        }
+        g_estop_active = false;
+        Serial.println("+ESTOP:CLEAR");
+    }
+}
+
+/* ── OLED display update (5 Hz) ─────────────────────────────────── */
+
+static uint32_t g_display_last = 0;
+
+static void display_update(void) {
+    if (millis() - g_display_last < 200) return;
+    g_display_last = millis();
+
+    display.clearDisplay();
+
+    if (g_estop_active) {
+        /* Big E-STOP warning */
+        display.setTextSize(3);
+        display.setTextColor(SSD1306_WHITE);
+        display.setCursor(10, 4);
+        display.println(F("E-STOP"));
+        display.setTextSize(1);
+        display.setCursor(20, 48);
+        display.println(F("RELEASE TO CLEAR"));
+        display.display();
+        return;
+    }
+
+    uint32_t now = millis();
+
+    /* Find closest anchor */
+    int32_t min_range = INT32_MAX;
+    for (int i = 0; i < NUM_ANCHORS; i++) {
+        if (g_anchor_range_mm[i] > 0 && g_anchor_range_mm[i] < min_range)
+            min_range = g_anchor_range_mm[i];
+    }
+
+    /* Line 1: Big distance to nearest anchor */
+    display.setTextSize(3);
+    display.setTextColor(SSD1306_WHITE);
+    display.setCursor(0, 0);
+    if (min_range < INT32_MAX && min_range > 0) {
+        float m = min_range / 1000.0f;
+        if (m < 10.0f)
+            display.printf("%.1fm", m);
+        else
+            display.printf("%.0fm", m);
+    } else {
+        display.println(F("---"));
+    }
+
+    /* Line 2: Both anchor ranges */
+    display.setTextSize(1);
+    display.setCursor(0, 30);
+    for (int i = 0; i < NUM_ANCHORS && i < 2; i++) {
+        if (g_anchor_range_mm[i] > 0) {
+            float m = g_anchor_range_mm[i] / 1000.0f;
+            display.printf("A%d:%.1fm ", i, m);
+        } else {
+            display.printf("A%d:--- ", i);
+        }
+    }
+
+    /* Line 3: Connection status */
+    display.setCursor(0, 42);
+    bool any_linked = false;
+    for (int i = 0; i < NUM_ANCHORS; i++) {
+        if (g_anchor_last_ok[i] > 0 && (now - g_anchor_last_ok[i]) < 2000) {
+            any_linked = true;
+            break;
+        }
+    }
+
+    if (any_linked) {
+        /* RSSI bar: map -90..-30 dBm to 0-5 bars */
+        float best_rssi = -100.0f;
+        for (int i = 0; i < NUM_ANCHORS; i++) {
+            if (g_anchor_rssi[i] > best_rssi) best_rssi = g_anchor_rssi[i];
+        }
+        int bars = constrain((int)((best_rssi + 90.0f) / 12.0f), 0, 5);
+
+        display.print(F("LINKED "));
+        /* Draw signal bars */
+        for (int b = 0; b < 5; b++) {
+            int x = 50 + b * 6;
+            int h = 2 + b * 2;
+            int y = 50 - h;
+            if (b < bars)
+                display.fillRect(x, y, 4, h, SSD1306_WHITE);
+            else
+                display.drawRect(x, y, 4, h, SSD1306_WHITE);
+        }
+        display.printf(" %.0fdB", best_rssi);
+    } else {
+        display.println(F("LOST  -- searching --"));
+    }
+
+    /* Line 4: Uptime */
+    display.setCursor(0, 54);
+    uint32_t secs = now / 1000;
+    display.printf("UP %02d:%02d  seq:%d", secs / 60, secs % 60, g_seq);
+
+    display.display();
+}
+
+/* ── DS-TWR initiator (one anchor, one cycle) ───────────────────── */
+
+static int32_t twr_range_once(uint8_t anchor_id) {
+
+    /* --- 1. TX POLL --- */
+    uint8_t poll[POLL_FRAME_LEN];
+    poll[0] = FTYPE_POLL;
+    poll[1] = TAG_ID;
+    poll[2] = anchor_id;
+
+    dwt_writetxdata(POLL_FRAME_LEN - FCS_LEN, poll, 0);
+    dwt_writetxfctrl(POLL_FRAME_LEN, 0, 1);
+
+    dwt_setrxaftertxdelay(300);
+    dwt_setrxtimeout(RESP_RX_TIMEOUT_US);
+
+    g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false;
+    if (dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS)
+        return -1;
+
+    uint32_t t0 = millis();
+    while (!g_tx_done && millis() - t0 < 15) yield();
+
+    uint8_t poll_tx_raw[5];
+    dwt_readtxtimestamp(poll_tx_raw);
+    uint64_t T_poll_tx = ts_read(poll_tx_raw);
+
+    /* --- 2. Wait for RESP --- */
+    t0 = millis();
+    while (!g_rx_ok && !g_rx_err && !g_rx_to && millis() - t0 < 60) yield();
+    if (!g_rx_ok || g_rx_len < FRAME_HDR + RESP_PAYLOAD) return -1;
+    if (g_rx_buf[0] != FTYPE_RESP)   return -1;
+    if (g_rx_buf[2] != TAG_ID)       return -1;
+    if (g_rx_buf[1] != anchor_id)    return -1;
+
+    uint8_t resp_rx_raw[5];
+    dwt_readrxtimestamp(resp_rx_raw);
+    uint64_t T_resp_rx = ts_read(resp_rx_raw);
+
+    uint64_t T_poll_rx_a = ts_read(&g_rx_buf[3]);
+    uint64_t T_resp_tx_a = ts_read(&g_rx_buf[8]);
+
+    /* --- 3. Compute DS-TWR values for FINAL --- */
+    uint64_t Ra = ts_diff(T_resp_rx, T_poll_tx);
+    uint64_t Db = ts_diff(T_resp_tx_a, T_poll_rx_a);
+
+    uint64_t final_tx_sched = (T_resp_rx + FINAL_TX_DLY_TICKS) & ~0x1FFULL;
+    uint64_t Da = ts_diff(final_tx_sched, T_resp_rx);
+
+    /* --- 4. TX FINAL --- */
+    uint8_t final_buf[FINAL_FRAME_LEN];
+    final_buf[0] = FTYPE_FINAL;
+    final_buf[1] = TAG_ID;
+    final_buf[2] = anchor_id;
+    ts_write(&final_buf[3],  Ra);
+    ts_write(&final_buf[8],  Da);
+    ts_write(&final_buf[13], Db);
+
+    dwt_writetxdata(FINAL_FRAME_LEN - FCS_LEN, final_buf, 0);
+    dwt_writetxfctrl(FINAL_FRAME_LEN, 0, 1);
+    dwt_setdelayedtrxtime((uint32_t)(final_tx_sched >> 8));
+
+    g_tx_done = false;
+    if (dwt_starttx(DWT_START_TX_DELAYED) != DWT_SUCCESS) {
+        dwt_forcetrxoff();
+        return -1;
+    }
+    t0 = millis();
+    while (!g_tx_done && millis() - t0 < 15) yield();
+
+    /* --- 5. Local range estimate (debug) --- */
+    uint8_t final_tx_raw[5];
+    dwt_readtxtimestamp(final_tx_raw);
+    /* uint64_t T_final_tx = ts_read(final_tx_raw); -- unused, tag uses SS estimate */
+
+    double ra = ticks_to_s(Ra);
+    double db = ticks_to_s(Db);
+    double tof    = (ra - db) / 2.0;
+    double range_m = tof * SPEED_OF_LIGHT;
+
+    if (range_m < 0.1 || range_m > 130.0) return -1;
+    return (int32_t)(range_m * 1000.0 + 0.5);
+}
+
+/* ── Setup ──────────────────────────────────────────────────────── */
+
+void setup(void) {
+    Serial.begin(SERIAL_BAUD);
+    delay(300);
+
+    /* E-Stop button */
+    pinMode(PIN_ESTOP, INPUT_PULLUP);
+    pinMode(PIN_LED, OUTPUT);
+    digitalWrite(PIN_LED, LOW);
+
+    Serial.printf("\r\n[uwb_tag] tag_id=0x%02X  num_anchors=%d  starting\r\n",
+                  TAG_ID, NUM_ANCHORS);
+
+    /* --- OLED init --- */
+    Wire.begin(PIN_SDA, PIN_SCL);
+    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
+        Serial.println("[uwb_tag] WARN: SSD1306 not found — running headless");
+    } else {
+        display.clearDisplay();
+        display.setTextSize(2);
+        display.setTextColor(SSD1306_WHITE);
+        display.setCursor(0, 0);
+        display.println(F("SaltyBot"));
+        display.setTextSize(1);
+        display.setCursor(0, 20);
+        display.printf("Tag 0x%02X  v2.0", TAG_ID);
+        display.setCursor(0, 35);
+        display.println(F("DW3000 DS-TWR + ESP-NOW"));
+        display.setCursor(0, 50);
+        display.println(F("Initializing..."));
+        display.display();
+        Serial.println("[uwb_tag] OLED ok");
+    }
+
+    /* --- ESP-NOW init --- */
+    WiFi.mode(WIFI_STA);
+    WiFi.disconnect();
+    /* Set WiFi channel to match anchors (default ch 1) */
+    esp_wifi_set_channel(1, WIFI_SECOND_CHAN_NONE);
+
+    if (esp_now_init() != ESP_OK) {
+        Serial.println("[uwb_tag] FATAL: esp_now_init failed");
+        for (;;) delay(1000);
+    }
+
+    /* Add broadcast peer */
+    esp_now_peer_info_t peer = {};
+    memcpy(peer.peer_addr, broadcast_mac, 6);
+    peer.channel = 0;  /* use current channel */
+    peer.encrypt = false;
+    esp_now_add_peer(&peer);
+    Serial.println("[uwb_tag] ESP-NOW ok");
+
+    /* --- DW3000 init --- */
+    SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS);
+
+    pinMode(PIN_RST, OUTPUT);
+    digitalWrite(PIN_RST, LOW);
+    delay(2);
+    pinMode(PIN_RST, INPUT_PULLUP);
+    delay(5);
+
+    if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) {
+        Serial.println("[uwb_tag] FATAL: DW3000 probe failed");
+        for (;;) delay(1000);
+    }
+
+    if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) {
+        Serial.println("[uwb_tag] FATAL: dwt_initialise failed");
+        for (;;) delay(1000);
+    }
+
+    if (dwt_configure(&dw_cfg) != DWT_SUCCESS) {
+        Serial.println("[uwb_tag] FATAL: dwt_configure failed");
+        for (;;) delay(1000);
+    }
+
+    dwt_setrxantennadelay(ANT_DELAY);
+    dwt_settxantennadelay(ANT_DELAY);
+    dwt_settxpower(0x0E080222UL);
+
+    dwt_setcallbacks(cb_tx_done, cb_rx_ok, cb_rx_to, cb_rx_err,
+                     nullptr, nullptr, nullptr);
+    dwt_setinterrupt(
+        DWT_INT_TXFRS | DWT_INT_RFCG | DWT_INT_RFTO |
+        DWT_INT_RFSL  | DWT_INT_SFDT | DWT_INT_ARFE | DWT_INT_CPERR,
+        0, DWT_ENABLE_INT_ONLY);
+
+    attachInterrupt(digitalPinToInterrupt(PIN_IRQ),
+                    []() { dwt_isr(); }, RISING);
+
+    /* Init range state */
+    for (int i = 0; i < NUM_ANCHORS; i++) {
+        g_anchor_range_mm[i] = -1;
+        g_anchor_rssi[i] = -100.0f;
+        g_anchor_last_ok[i] = 0;
+    }
+
+    Serial.printf("[uwb_tag] DW3000 ready  ch=%d  6.8Mbps  tag=0x%02X\r\n",
+                  dw_cfg.chan, TAG_ID);
+    Serial.println("[uwb_tag] Ranging + ESP-NOW + display active");
+}
+
+/* ── Main loop ──────────────────────────────────────────────────── */
+
+void loop(void) {
+    static uint8_t  anchor_idx    = 0;
+    static uint32_t last_range_ms = 0;
+    static uint32_t last_hb_ms    = 0;
+
+    /* E-Stop always has priority */
+    estop_check();
+    if (g_estop_active) {
+        display_update();
+        return;   /* skip ranging while e-stop active */
+    }
+
+    /* Heartbeat every 1 second */
+    uint32_t now = millis();
+    if (now - last_hb_ms >= 1000) {
+        espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f);
+        last_hb_ms = now;
+    }
+
+    /* Ranging at configured interval */
+    if (now - last_range_ms >= RANGE_INTERVAL_MS) {
+        last_range_ms = now;
+
+        uint8_t anchor_id = anchor_idx % NUM_ANCHORS;
+        int32_t range_mm  = twr_range_once(anchor_id);
+
+        if (range_mm > 0) {
+            float rssi = rx_power_dbm();
+
+            /* Update shared state for display */
+            g_anchor_range_mm[anchor_id] = range_mm;
+            g_anchor_rssi[anchor_id]     = rssi;
+            g_anchor_last_ok[anchor_id]  = now;
+
+            /* Serial debug */
+            Serial.printf("+RANGE:%d,%ld,%.1f\r\n",
+                          anchor_id, (long)range_mm, rssi);
+
+            /* ESP-NOW broadcast */
+            espnow_send(MSG_RANGE, anchor_id, range_mm, rssi);
+
+            /* LED blink */
+            digitalWrite(PIN_LED, HIGH);
+            delay(2);
+            digitalWrite(PIN_LED, LOW);
+        }
+
+        anchor_idx++;
+        if (anchor_idx >= NUM_ANCHORS) anchor_idx = 0;
+    }
+
+    /* Display at 5 Hz (non-blocking) */
+    display_update();
+}

include/jlink.h

@@ -45,7 +45,8 @@
  *   0x83  PID_RESULT    - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
  *   0x84  GIMBAL_STATE  - jlink_tlm_gimbal_state_t (10 bytes, Issue #547)
  *   0x85  SCHED         - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
- *   0x86  FAULT_LOG     - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
+ *   0x86  MOTOR_CURRENT - jlink_tlm_motor_current_t (8 bytes, Issue #584)
+ *   0x87  FAULT_LOG     - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
  *
  * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
  * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
@@ -84,7 +85,8 @@
 #define JLINK_TLM_PID_RESULT    0x83u  /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */
 #define JLINK_TLM_GIMBAL_STATE  0x84u  /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */
 #define JLINK_TLM_SCHED         0x85u  /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */
-#define JLINK_TLM_FAULT_LOG     0x86u  /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
+#define JLINK_TLM_MOTOR_CURRENT 0x86u  /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */
+#define JLINK_TLM_FAULT_LOG     0x87u  /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
 typedef struct __attribute__((packed)) {
@@ -152,6 +154,16 @@ typedef struct __attribute__((packed)) {
     pid_sched_entry_t bands[PID_SCHED_MAX_BANDS];  /* up to 6 x 16 = 96 bytes */
 } jlink_tlm_sched_t;  /* 1 + 96 = 97 bytes max */
 
+/* ---- Telemetry MOTOR_CURRENT payload (8 bytes, packed) Issue #584 ---- */
+/* Published at MOTOR_CURR_TLM_HZ; reports measured current and protection state. */
+typedef struct __attribute__((packed)) {
+    int32_t  current_ma;   /* filtered battery/motor current (mA, + = discharge)   */
+    uint8_t  limit_pct;    /* soft-limit reduction applied: 0=none, 100=full cutoff */
+    uint8_t  state;        /* MotorCurrentState: 0=NORMAL,1=SOFT_LIMIT,2=COOLDOWN  */
+    uint8_t  fault_count;  /* lifetime hard-cutoff trips (saturates at 255)         */
+    uint8_t  _pad;         /* reserved                                               */
+} jlink_tlm_motor_current_t;  /* 8 bytes */
+
 /* ---- Telemetry FAULT_LOG payload (20 bytes, packed) Issue #565 ---- */
 /* Sent on boot (if last fault != NONE) and in response to FAULT_LOG_GET. */
 typedef struct __attribute__((packed)) {
@@ -254,7 +266,14 @@ void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm);
 JLinkSchedSetBuf *jlink_get_sched_set(void);
 
 /*
- * jlink_send_fault_log(fl) - transmit JLINK_TLM_FAULT_LOG (0x86) frame
+ * jlink_send_motor_current_tlm(tlm) - transmit JLINK_TLM_MOTOR_CURRENT (0x86)
+ * frame (14 bytes total) to Jetson.  Issue #584.
+ * Rate-limiting is handled by motor_current_send_tlm(); call from there only.
+ */
+void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm);
+
+/*
+ * jlink_send_fault_log(fl) - transmit JLINK_TLM_FAULT_LOG (0x87) frame
  * (26 bytes) on boot (if fault log non-empty) and in response to
  * FAULT_LOG_GET.  Issue #565.
  */

include/motor_current.h

@@ -0,0 +1,121 @@
+#ifndef MOTOR_CURRENT_H
+#define MOTOR_CURRENT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * motor_current — ADC-based motor current monitoring and overload protection
+ * for Issue #584.
+ *
+ * Hardware:
+ *   ADC3 IN13 (PC3, ADC_CURR_PIN) is already sampled by battery_adc.c via
+ *   DMA2_Stream0 circular.  This module reads battery_adc_get_current_ma()
+ *   each tick rather than running a second ADC, since total discharge current
+ *   on this single-motor balance bot equals motor current plus ~30 mA overhead.
+ *
+ * Behaviour:
+ *   MC_NORMAL     : current_ma <  MOTOR_CURR_SOFT_MA  — full output
+ *   MC_SOFT_LIMIT : current_ma in [SOFT_MA, HARD_MA)  — linear PWM reduction
+ *   MC_COOLDOWN   : hard cutoff latched after HARD_MA sustained for
+ *                   MOTOR_CURR_OVERLOAD_MS (2 s) — zero output for
+ *                   MOTOR_CURR_COOLDOWN_MS (10 s), then MC_NORMAL
+ *
+ * Soft limit formula (MC_SOFT_LIMIT):
+ *   scale = (HARD_MA - current_ma) / (HARD_MA - SOFT_MA)   [0..1]
+ *   limited_cmd = (int16_t)(cmd * scale)
+ *
+ * Fault event:
+ *   On each hard-cutoff trip, s_fault_count is incremented (saturates at 255)
+ *   and motor_current_fault_pending() returns true for one main-loop tick so
+ *   the caller can append a fault log entry.
+ *
+ * Main-loop integration (pseudo-code):
+ *
+ *   void main_loop_tick(uint32_t now_ms) {
+ *       battery_adc_tick(now_ms);
+ *       motor_current_tick(now_ms);
+ *
+ *       if (motor_current_fault_pending())
+ *           fault_log_append(FAULT_MOTOR_OVERCURRENT);
+ *
+ *       int16_t cmd = balance_pid_output();
+ *       cmd = motor_current_apply_limit(cmd);
+ *       motor_driver_update(&g_motor, cmd, steer, now_ms);
+ *
+ *       motor_current_send_tlm(now_ms);  // rate-limited to MOTOR_CURR_TLM_HZ
+ *   }
+ */
+
+/* ---- Thresholds ---- */
+#define MOTOR_CURR_HARD_MA       5000u  /* 5 A — hard cutoff level            */
+#define MOTOR_CURR_SOFT_MA       4000u  /* 4 A — soft-limit onset (80% of hard) */
+#define MOTOR_CURR_OVERLOAD_MS   2000u  /* sustained over HARD_MA before fault */
+#define MOTOR_CURR_COOLDOWN_MS  10000u  /* zero-output recovery period (ms)    */
+#define MOTOR_CURR_TLM_HZ           5u  /* JLINK_TLM_MOTOR_CURRENT publish rate */
+
+/* ---- State enum ---- */
+typedef enum {
+    MC_NORMAL     = 0,
+    MC_SOFT_LIMIT = 1,
+    MC_COOLDOWN   = 2,
+} MotorCurrentState;
+
+/* ---- API ---- */
+
+/*
+ * motor_current_init() — reset all state.
+ * Call once during system init, after battery_adc_init().
+ */
+void motor_current_init(void);
+
+/*
+ * motor_current_tick(now_ms) — evaluate ADC reading, update state machine.
+ * Call from main loop after battery_adc_tick(), at any rate ≥ 10 Hz.
+ * Non-blocking (<1 µs).
+ */
+void motor_current_tick(uint32_t now_ms);
+
+/*
+ * motor_current_apply_limit(cmd) — scale motor command by current-limit factor.
+ *   MC_NORMAL:     returns cmd unchanged.
+ *   MC_SOFT_LIMIT: returns cmd scaled down linearly.
+ *   MC_COOLDOWN:   returns 0.
+ * Call after motor_current_tick() each loop iteration.
+ */
+int16_t motor_current_apply_limit(int16_t cmd);
+
+/*
+ * motor_current_is_faulted() — true while in MC_COOLDOWN (output zeroed).
+ */
+bool motor_current_is_faulted(void);
+
+/*
+ * motor_current_state() — current state machine state.
+ */
+MotorCurrentState motor_current_state(void);
+
+/*
+ * motor_current_ma() — most recent ADC reading used by the state machine (mA).
+ */
+int32_t motor_current_ma(void);
+
+/*
+ * motor_current_fault_count() — lifetime hard-cutoff trip counter (0..255).
+ */
+uint8_t motor_current_fault_count(void);
+
+/*
+ * motor_current_fault_pending() — true for exactly one tick after a hard
+ * cutoff trip fires.  Main loop should append a fault log entry and then the
+ * flag clears automatically on the next call.
+ */
+bool motor_current_fault_pending(void);
+
+/*
+ * motor_current_send_tlm(now_ms) — transmit JLINK_TLM_MOTOR_CURRENT (0x86)
+ * frame to Jetson.  Rate-limited to MOTOR_CURR_TLM_HZ; safe to call every tick.
+ */
+void motor_current_send_tlm(uint32_t now_ms);
+
+#endif /* MOTOR_CURRENT_H */

include/pid_flash.h

@@ -31,6 +31,36 @@ typedef struct __attribute__((packed)) {
     uint8_t  _pad[48];   /* padding to 64 bytes */
 } pid_flash_t;
 
+/* ---- Gain schedule flash storage (Issue #550) ---- */
+
+/* Maximum number of speed-band entries in the gain schedule table */
+#define PID_SCHED_MAX_BANDS     6u
+
+/*
+ * Sector 7 layout (128KB at 0x08060000):
+ *   0x0807FF40  pid_sched_flash_t (128 bytes) — gain schedule record
+ *   0x0807FFC0  pid_flash_t       ( 64 bytes) — single PID record (existing)
+ * Both records are written in a single sector erase via pid_flash_save_all().
+ */
+#define PID_SCHED_FLASH_ADDR    0x0807FF40UL
+#define PID_SCHED_MAGIC         0x534C5402UL  /* 'SLT\x02' — version 2 */
+
+typedef struct __attribute__((packed)) {
+    float speed_mps;   /* velocity breakpoint (m/s) */
+    float kp;
+    float ki;
+    float kd;
+} pid_sched_entry_t;   /* 16 bytes */
+
+typedef struct __attribute__((packed)) {
+    uint32_t          magic;                          /* PID_SCHED_MAGIC when valid */
+    uint8_t           num_bands;                      /* valid entries (1..PID_SCHED_MAX_BANDS) */
+    uint8_t           flags;                          /* reserved, must be 0 */
+    uint8_t           _pad0[2];
+    pid_sched_entry_t bands[PID_SCHED_MAX_BANDS];     /* 6 × 16 = 96 bytes */
+    uint8_t           _pad1[24];                      /* total = 4+1+1+2+96+24 = 128 bytes */
+} pid_sched_flash_t;   /* 128 bytes */
+
 /*
  * pid_flash_load() — read saved PID from flash.
  * Returns true and fills *kp/*ki/*kd if magic is valid.
@@ -39,10 +69,27 @@ typedef struct __attribute__((packed)) {
 bool pid_flash_load(float *kp, float *ki, float *kd);
 
 /*
- * pid_flash_save() — erase sector 7 and write Kp/Ki/Kd.
+ * pid_flash_save() — erase sector 7 and write Kp/Ki/Kd (single-PID only).
+ * Use pid_flash_save_all() to save both single-PID and schedule atomically.
  * Must not be called while armed (flash erase takes ~1s and stalls the CPU).
  * Returns true on success.
  */
 bool pid_flash_save(float kp, float ki, float kd);
 
+/*
+ * pid_flash_load_schedule() — read gain schedule from flash.
+ * Returns true and fills out_entries[0..n-1] and *out_n if magic is valid.
+ * Returns false if no valid schedule stored.
+ */
+bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n);
+
+/*
+ * pid_flash_save_all() — erase sector 7 once and atomically write both:
+ *   - pid_sched_flash_t at PID_SCHED_FLASH_ADDR (0x0807FF40)
+ *   - pid_flash_t       at PID_FLASH_STORE_ADDR  (0x0807FFC0)
+ * Must not be called while armed.  Returns true on success.
+ */
+bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
+                        const pid_sched_entry_t *entries, uint8_t num_bands);
+
 #endif /* PID_FLASH_H */

include/pid_schedule.h

@@ -0,0 +1,122 @@
+/*
+ * pid_schedule.h — Speed-dependent PID gain scheduling (Issue #550)
+ *
+ * Maps robot velocity to PID gain triplets (Kp, Ki, Kd) using a lookup
+ * table with linear interpolation between adjacent entries.  The table
+ * supports 1–PID_SCHED_MAX_BANDS entries, each associating a velocity
+ * breakpoint (m/s) with gains that apply AT that velocity.
+ *
+ * HOW IT WORKS:
+ * 1. Each entry in the table defines: {speed_mps, kp, ki, kd}.
+ *    The table is sorted by speed_mps ascending (pid_schedule_set_table
+ *    sorts automatically).
+ *
+ * 2. pid_schedule_get_gains(speed_mps, ...) finds the two adjacent entries
+ *    that bracket the query speed and linearly interpolates:
+ *      t  = (speed - bands[i-1].speed_mps) /
+ *           (bands[i].speed_mps - bands[i-1].speed_mps)
+ *      kp = bands[i-1].kp + t * (bands[i].kp - bands[i-1].kp)
+ *    Speeds below the first entry or above the last entry clamp to the
+ *    nearest endpoint (no extrapolation).
+ *    The query speed is ABS(motor_speed) — scheduling is symmetric.
+ *
+ * 3. Default 3-entry table (loaded when flash has no valid schedule):
+ *      Band 0: speed=0.00 m/s  kp=40.0  ki=1.5  kd=1.2  (stopped — tight)
+ *      Band 1: speed=0.30 m/s  kp=35.0  ki=1.0  kd=1.0  (slow — balanced)
+ *      Band 2: speed=0.80 m/s  kp=28.0  ki=0.5  kd=0.8  (fast — relaxed)
+ *
+ * 4. pid_schedule_apply(balance, speed_mps) interpolates and writes the
+ *    result directly into balance->kp/ki/kd.  Call from the main loop at
+ *    the same rate as the balance PID update (1 kHz) or slower (100 Hz
+ *    for scheduling, 1 kHz for PID execution — gains change slowly enough).
+ *
+ * 5. Flash persistence: pid_schedule_flash_save() calls pid_flash_save_all()
+ *    which erases sector 7 once and writes both the single-PID record at
+ *    PID_FLASH_STORE_ADDR and the schedule at PID_SCHED_FLASH_ADDR.
+ *
+ * 6. JLINK interface (Issue #550):
+ *      0x0C  SCHED_GET  — no payload; triggers TLM_SCHED response
+ *      0x0D  SCHED_SET  — upload new table (num_bands + N×16-byte entries)
+ *      0x0E  SCHED_SAVE — save current table + single PID to flash
+ *      0x85  TLM_SCHED  — table dump response to SCHED_GET
+ */
+
+#ifndef PID_SCHEDULE_H
+#define PID_SCHEDULE_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "pid_flash.h"   /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */
+#include "balance.h"     /* balance_t */
+
+/* ---- Default gain table ---- */
+/* Motor ESC range is ±1000 counts; 1000 counts ≈ full drive.
+ * Speed scale: MOTOR_CMD_MAX=1000 → ~0.8 m/s max tangential velocity.
+ * Adjust PID_SCHED_SPEED_SCALE if odometry calibration changes this. */
+#define PID_SCHED_SPEED_SCALE   0.0008f   /* motor_cmd counts → m/s: 1000 × 0.0008 = 0.8 m/s */
+
+/* ---- API ---- */
+
+/*
+ * pid_schedule_init() — load table from flash (via pid_flash_load_schedule).
+ * Falls back to the built-in 3-band default if flash is empty or invalid.
+ * Call once after flash init during system startup.
+ */
+void pid_schedule_init(void);
+
+/*
+ * pid_schedule_get_gains(speed_mps, *kp, *ki, *kd) — interpolate gains.
+ * |speed_mps| is used (scheduling is symmetric for forward/reverse).
+ * Clamps to table endpoints; does not extrapolate outside the table range.
+ */
+void pid_schedule_get_gains(float speed_mps, float *kp, float *ki, float *kd);
+
+/*
+ * pid_schedule_apply(b, speed_mps) — compute interpolated gains and write
+ * them into b->kp, b->ki, b->kd.  b->integral is reset to 0 when the
+ * active band changes to avoid integrator windup on transitions.
+ */
+void pid_schedule_apply(balance_t *b, float speed_mps);
+
+/*
+ * pid_schedule_set_table(entries, n) — replace the active gain table.
+ * Entries are copied and sorted by speed_mps ascending.
+ * n is clamped to [1, PID_SCHED_MAX_BANDS].
+ * Does NOT automatically save to flash — call pid_schedule_flash_save().
+ */
+void pid_schedule_set_table(const pid_sched_entry_t *entries, uint8_t n);
+
+/*
+ * pid_schedule_get_table(out_entries, out_n) — copy current table out.
+ * out_entries must have room for PID_SCHED_MAX_BANDS entries.
+ */
+void pid_schedule_get_table(pid_sched_entry_t *out_entries, uint8_t *out_n);
+
+/*
+ * pid_schedule_get_num_bands() — return current number of table entries.
+ */
+uint8_t pid_schedule_get_num_bands(void);
+
+/*
+ * pid_schedule_flash_save(kp_single, ki_single, kd_single) — save the
+ * current schedule table PLUS the caller-supplied single-PID values to
+ * flash in one atomic sector erase (pid_flash_save_all).
+ * Must NOT be called while armed (sector erase takes ~1s).
+ * Returns true on success.
+ */
+bool pid_schedule_flash_save(float kp_single, float ki_single, float kd_single);
+
+/*
+ * pid_schedule_active_band_idx() — index (0-based) of the lower bracket
+ * entry used in the most recent interpolation.  Useful for telemetry.
+ * Returns 0 if speed is below the first entry.
+ */
+uint8_t pid_schedule_active_band_idx(void);
+
+/*
+ * pid_schedule_get_default_table(out_entries, out_n) — fill the 3-band
+ * default table into caller's buffer.  Used for factory-reset.
+ */
+void pid_schedule_get_default_table(pid_sched_entry_t *out_entries, uint8_t *out_n);
+
+#endif /* PID_SCHEDULE_H */

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

@@ -0,0 +1,591 @@
+"""saltybot_bringup.launch.py — Unified ROS2 launch orchestrator (Issue #577).
+
+Ordered startup in four dependency groups with configurable profiles,
+hardware-presence conditionals, inter-group health-gate delays, and
+graceful-shutdown event handlers.
+
+Profiles
+────────
+  minimal   Drivers only — safe teleoperation, no AI (boot ~4 s)
+  full      Complete autonomous stack — SLAM + Nav2 + perception + audio (boot ~22 s)
+  debug     Full + RViz + verbose logs + bag recorder (boot ~26 s)
+
+Usage
+────────
+  ros2 launch saltybot_bringup saltybot_bringup.launch.py
+  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=minimal
+  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=debug
+  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full stm32_port:=/dev/ttyUSB0
+
+Startup sequence
+────────────────
+  GROUP A — Drivers      t= 0 s  STM32 bridge, RealSense+RPLIDAR, motor daemon, IMU
+  health gate ───────────────────────────────────────────────── t= 8 s (full/debug)
+  GROUP B — Perception   t= 8 s  UWB, person detection, object detection, depth costmap, gimbal
+  health gate ───────────────────────────────────────────────── t=16 s (full/debug)
+  GROUP C — Navigation   t=16 s  SLAM, Nav2, lidar avoidance, follower, docking
+  health gate ───────────────────────────────────────────────── t=22 s (full/debug)
+  GROUP D — UI/Social    t=22 s  rosbridge, audio pipeline, social personality, CSI cameras, diagnostics
+
+Shutdown
+────────
+  All groups include OnShutdown handlers that:
+  - Publish /saltybot/estop=true via ros2 topic pub (one-shot)
+  - Wait 1 s for bag recorder to flush
+
+Hardware conditionals
+─────────────────────
+  Missing devices (stm32_port, uwb_port_a/b, gimbal_port) skip that driver.
+  All conditionals are evaluated at launch time via PathJoinSubstitution + IfCondition.
+"""
+
+from __future__ import annotations
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    EmitEvent,
+    ExecuteProcess,
+    GroupAction,
+    IncludeLaunchDescription,
+    LogInfo,
+    RegisterEventHandler,
+    SetEnvironmentVariable,
+    TimerAction,
+)
+from launch.conditions import IfCondition, LaunchConfigurationEquals
+from launch.event_handlers import OnShutdown
+from launch.events import Shutdown
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+# ── Helpers ────────────────────────────────────────────────────────────────────
+
+def _pkg(name: str) -> str:
+    return get_package_share_directory(name)
+
+
+def _launch(pkg: str, *parts: str) -> PythonLaunchDescriptionSource:
+    return PythonLaunchDescriptionSource(os.path.join(_pkg(pkg), *parts))
+
+
+def _profile_flag(flag_name: str, profile_value_map: dict):
+    """PythonExpression that evaluates flag_name from a profile-keyed dict."""
+    # e.g. {'minimal': 'false', 'full': 'true', 'debug': 'true'}
+    return PythonExpression([
+        repr(profile_value_map),
+        ".get('", LaunchConfiguration("profile"), "', 'false')",
+    ])
+
+
+def _if_profile_flag(flag_name: str, profiles_with_flag: list):
+    """IfCondition: true when profile is in profiles_with_flag."""
+    profile_set = repr(set(profiles_with_flag))
+    return IfCondition(
+        PythonExpression(["'", LaunchConfiguration("profile"), f"' in {profile_set}"])
+    )
+
+
+# ── Launch description ─────────────────────────────────────────────────────────
+
+def generate_launch_description() -> LaunchDescription:  # noqa: C901
+
+    # ── Arguments ─────────────────────────────────────────────────────────────
+
+    profile_arg = DeclareLaunchArgument(
+        "profile",
+        default_value="full",
+        choices=["minimal", "full", "debug"],
+        description=(
+            "Launch profile. "
+            "minimal: drivers+sensors only (no AI); "
+            "full: complete autonomous stack; "
+            "debug: full + RViz + verbose logs + bag recorder."
+        ),
+    )
+
+    use_sim_time_arg = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="false",
+        description="Use /clock from rosbag/simulator",
+    )
+
+    stm32_port_arg = DeclareLaunchArgument(
+        "stm32_port",
+        default_value="/dev/stm32-bridge",
+        description="STM32 USART bridge serial device",
+    )
+
+    uwb_port_a_arg = DeclareLaunchArgument(
+        "uwb_port_a",
+        default_value="/dev/uwb-anchor0",
+        description="UWB anchor-0 serial device",
+    )
+
+    uwb_port_b_arg = DeclareLaunchArgument(
+        "uwb_port_b",
+        default_value="/dev/uwb-anchor1",
+        description="UWB anchor-1 serial device",
+    )
+
+    gimbal_port_arg = DeclareLaunchArgument(
+        "gimbal_port",
+        default_value="/dev/ttyTHS1",
+        description="Gimbal JLINK serial device",
+    )
+
+    max_linear_vel_arg = DeclareLaunchArgument(
+        "max_linear_vel",
+        default_value="0.5",
+        description="Max forward velocity cap (m/s)",
+    )
+
+    follow_distance_arg = DeclareLaunchArgument(
+        "follow_distance",
+        default_value="1.5",
+        description="Person-follower target distance (m)",
+    )
+
+    # ── Substitution handles ───────────────────────────────────────────────────
+
+    profile         = LaunchConfiguration("profile")
+    use_sim_time    = LaunchConfiguration("use_sim_time")
+    stm32_port      = LaunchConfiguration("stm32_port")
+    uwb_port_a      = LaunchConfiguration("uwb_port_a")
+    uwb_port_b      = LaunchConfiguration("uwb_port_b")
+    gimbal_port     = LaunchConfiguration("gimbal_port")
+    max_linear_vel  = LaunchConfiguration("max_linear_vel")
+    follow_distance = LaunchConfiguration("follow_distance")
+
+    # Profile membership helpers
+    _full_profiles  = ["full", "debug"]
+    _debug_profiles = ["debug"]
+
+    # ── Graceful shutdown handler ──────────────────────────────────────────────
+    # On Ctrl-C / ros2 launch shutdown: publish estop then let nodes drain.
+    estop_on_shutdown = RegisterEventHandler(
+        OnShutdown(
+            on_shutdown=[
+                LogInfo(msg="[saltybot_bringup] SHUTDOWN — sending estop"),
+                ExecuteProcess(
+                    cmd=[
+                        "ros2", "topic", "pub", "--once",
+                        "/saltybot/estop", "std_msgs/msg/Bool", "{data: true}",
+                    ],
+                    output="screen",
+                ),
+            ]
+        )
+    )
+
+    # ── Verbose logging for debug profile ─────────────────────────────────────
+    verbose_log_env = SetEnvironmentVariable(
+        name="RCUTILS_LOGGING_BUFFERED_STREAM",
+        value=PythonExpression(
+            ["'1' if '", profile, "' == 'debug' else '0'"]
+        ),
+    )
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # GROUP A — DRIVERS   (t = 0 s, all profiles)
+    # Dependency order: STM32 bridge first, then sensors, then motor daemon.
+    # Health gate: subsequent groups delayed until t_perception (8 s full/debug).
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    group_a_banner = LogInfo(
+        msg=["[saltybot_bringup] GROUP A — Drivers  profile=", profile]
+    )
+
+    # Robot description (URDF + static TF)
+    robot_description = IncludeLaunchDescription(
+        _launch("saltybot_description", "launch", "robot_description.launch.py"),
+        launch_arguments={"use_sim_time": use_sim_time}.items(),
+    )
+
+    # STM32 bidirectional bridge (JLINK USART1)
+    stm32_bridge = IncludeLaunchDescription(
+        _launch("saltybot_bridge", "launch", "bridge.launch.py"),
+        launch_arguments={
+            "mode":        "bidirectional",
+            "serial_port": stm32_port,
+        }.items(),
+    )
+
+    # Sensors: RealSense D435i + RPLIDAR A1
+    sensors = TimerAction(
+        period=2.0,
+        actions=[
+            LogInfo(msg="[saltybot_bringup] t=2s  Starting sensors (RealSense + RPLIDAR)"),
+            IncludeLaunchDescription(
+                _launch("saltybot_bringup", "launch", "sensors.launch.py"),
+            ),
+        ],
+    )
+
+    # Motor daemon: /cmd_vel → STM32 DRIVE frames (depends on bridge at t=0)
+    motor_daemon = TimerAction(
+        period=2.5,
+        actions=[
+            LogInfo(msg="[saltybot_bringup] t=2.5s  Starting motor daemon"),
+            IncludeLaunchDescription(
+                _launch("saltybot_motor_daemon", "launch", "motor_daemon.launch.py"),
+                launch_arguments={"max_linear_vel": max_linear_vel}.items(),
+            ),
+        ],
+    )
+
+    # Sensor health monitor (all profiles — lightweight)
+    sensor_health = TimerAction(
+        period=4.0,
+        actions=[
+            LogInfo(msg="[saltybot_bringup] t=4s  Starting sensor health monitor"),
+            Node(
+                package="saltybot_health_monitor",
+                executable="sensor_health_node",
+                name="sensor_health_node",
+                output="screen",
+                parameters=[{"check_hz": 1.0, "mqtt_enabled": True}],
+            ),
+        ],
+    )
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # ── HEALTH GATE A→B  (t = 8 s for full/debug; skipped for minimal) ────────
+    # Conservative: RealSense takes ~5 s, RPLIDAR ~2 s.  Adding 1 s margin.
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    health_gate_ab = TimerAction(
+        period=8.0,
+        actions=[
+            GroupAction(
+                condition=_if_profile_flag("perception", _full_profiles),
+                actions=[
+                    LogInfo(
+                        msg="[saltybot_bringup] HEALTH GATE A→B passed (t=8s) "
+                            "— sensors should be publishing"
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # GROUP B — PERCEPTION   (t = 8 s, full/debug only)
+    # Depends: sensors publishing /camera/* and /scan.
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    group_b = TimerAction(
+        period=8.0,
+        actions=[
+            GroupAction(
+                condition=_if_profile_flag("perception", _full_profiles),
+                actions=[
+                    LogInfo(msg="[saltybot_bringup] GROUP B — Perception"),
+
+                    # UWB ranging (DW3000 USB anchors)
+                    IncludeLaunchDescription(
+                        _launch("saltybot_uwb", "launch", "uwb.launch.py"),
+                        launch_arguments={
+                            "port_a": uwb_port_a,
+                            "port_b": uwb_port_b,
+                        }.items(),
+                    ),
+
+                    # YOLOv8n person detection (TensorRT)
+                    IncludeLaunchDescription(
+                        _launch("saltybot_perception", "launch", "person_detection.launch.py"),
+                        launch_arguments={"use_sim_time": use_sim_time}.items(),
+                    ),
+
+                    # YOLOv8n general object detection (Issue #468)
+                    IncludeLaunchDescription(
+                        _launch("saltybot_object_detection", "launch",
+                                "object_detection.launch.py"),
+                    ),
+
+                    # Depth-to-costmap plugin (Issue #532)
+                    IncludeLaunchDescription(
+                        _launch("saltybot_depth_costmap", "launch", "depth_costmap.launch.py"),
+                    ),
+
+                    # LIDAR obstacle avoidance (360° safety)
+                    IncludeLaunchDescription(
+                        _launch("saltybot_lidar_avoidance", "launch",
+                                "lidar_avoidance.launch.py"),
+                    ),
+
+                    # Gimbal control node (Issue #548)
+                    IncludeLaunchDescription(
+                        _launch("saltybot_gimbal", "launch", "gimbal.launch.py"),
+                        launch_arguments={"serial_port": gimbal_port}.items(),
+                    ),
+
+                    # Audio pipeline (Issue #503) — starts alongside perception
+                    IncludeLaunchDescription(
+                        _launch("saltybot_audio_pipeline", "launch",
+                                "audio_pipeline.launch.py"),
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # ── HEALTH GATE B→C  (t = 16 s for full/debug) ────────────────────────────
+    # SLAM needs camera + lidar for ~8 s to compute first keyframe.
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    health_gate_bc = TimerAction(
+        period=16.0,
+        actions=[
+            GroupAction(
+                condition=_if_profile_flag("navigation", _full_profiles),
+                actions=[
+                    LogInfo(
+                        msg="[saltybot_bringup] HEALTH GATE B→C passed (t=16s) "
+                            "— perception should be running"
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # GROUP C — NAVIGATION   (t = 16 s, full/debug only)
+    # Depends: SLAM needs t=8s of sensor data; Nav2 needs partial map.
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    group_c = TimerAction(
+        period=16.0,
+        actions=[
+            GroupAction(
+                condition=_if_profile_flag("navigation", _full_profiles),
+                actions=[
+                    LogInfo(msg="[saltybot_bringup] GROUP C — Navigation"),
+
+                    # RTAB-Map SLAM (RGB-D + LIDAR fusion)
+                    IncludeLaunchDescription(
+                        _launch("saltybot_bringup", "launch", "slam_rtabmap.launch.py"),
+                        launch_arguments={"use_sim_time": use_sim_time}.items(),
+                    ),
+
+                    # Person follower (t=16s — UWB + perception stable by now)
+                    IncludeLaunchDescription(
+                        _launch("saltybot_follower", "launch", "person_follower.launch.py"),
+                        launch_arguments={
+                            "follow_distance": follow_distance,
+                            "max_linear_vel":  max_linear_vel,
+                        }.items(),
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # Nav2 starts 6 s after SLAM to allow partial map build (t=22s)
+    nav2 = TimerAction(
+        period=22.0,
+        actions=[
+            GroupAction(
+                condition=_if_profile_flag("navigation", _full_profiles),
+                actions=[
+                    LogInfo(msg="[saltybot_bringup] t=22s  Starting Nav2 (SLAM map ~6s old)"),
+                    IncludeLaunchDescription(
+                        _launch("saltybot_bringup", "launch", "nav2.launch.py"),
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # Autonomous docking (Issue #489)
+    docking = TimerAction(
+        period=22.0,
+        actions=[
+            GroupAction(
+                condition=_if_profile_flag("navigation", _full_profiles),
+                actions=[
+                    IncludeLaunchDescription(
+                        _launch("saltybot_docking", "launch", "docking.launch.py"),
+                        launch_arguments={"battery_low_pct": "20.0"}.items(),
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # ── HEALTH GATE C→D  (t = 26 s for debug; t = 22 s for full) ─────────────
+    # Nav2 needs ~4 s to load costmaps; rosbridge must see all topics.
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    _t_ui_full  = 26.0   # full profile UI start
+    _t_ui_debug = 30.0   # debug profile UI start (extra margin)
+
+    health_gate_cd_full = TimerAction(
+        period=_t_ui_full,
+        actions=[
+            GroupAction(
+                condition=LaunchConfigurationEquals("profile", "full"),
+                actions=[
+                    LogInfo(
+                        msg=f"[saltybot_bringup] HEALTH GATE C→D passed (t={_t_ui_full}s) "
+                            "— Nav2 costmaps should be loaded"
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    health_gate_cd_debug = TimerAction(
+        period=_t_ui_debug,
+        actions=[
+            GroupAction(
+                condition=LaunchConfigurationEquals("profile", "debug"),
+                actions=[
+                    LogInfo(
+                        msg=f"[saltybot_bringup] HEALTH GATE C→D passed (t={_t_ui_debug}s) "
+                            "— debug profile extra margin elapsed"
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # GROUP D — UI / SOCIAL   (t = 26 s full, 30 s debug)
+    # Last group; exposes all topics over WebSocket + starts social layer.
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    def _group_d(period: float, profile_cond: IfCondition) -> TimerAction:
+        return TimerAction(
+            period=period,
+            actions=[
+                GroupAction(
+                    condition=profile_cond,
+                    actions=[
+                        LogInfo(msg="[saltybot_bringup] GROUP D — UI/Social/rosbridge"),
+
+                        # Social personality (face, emotions, expressions)
+                        IncludeLaunchDescription(
+                            _launch("saltybot_social_personality", "launch",
+                                    "social_personality.launch.py"),
+                        ),
+
+                        # 4× CSI surround cameras (optional; non-critical)
+                        IncludeLaunchDescription(
+                            _launch("saltybot_cameras", "launch", "csi_cameras.launch.py"),
+                        ),
+
+                        # rosbridge WebSocket (port 9090) — must be last
+                        IncludeLaunchDescription(
+                            _launch("saltybot_bringup", "launch", "rosbridge.launch.py"),
+                        ),
+                    ],
+                ),
+            ],
+        )
+
+    group_d_full  = _group_d(_t_ui_full,  LaunchConfigurationEquals("profile", "full"))
+    group_d_debug = _group_d(_t_ui_debug, LaunchConfigurationEquals("profile", "debug"))
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # DEBUG EXTRAS — (debug profile only)
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    debug_extras = TimerAction(
+        period=3.0,
+        actions=[
+            GroupAction(
+                condition=LaunchConfigurationEquals("profile", "debug"),
+                actions=[
+                    LogInfo(msg="[saltybot_bringup] DEBUG extras: bag recorder + RViz"),
+                    # Bag recorder
+                    IncludeLaunchDescription(
+                        _launch("saltybot_bag_recorder", "launch", "bag_recorder.launch.py"),
+                    ),
+                    # RViz2 with SaltyBot config
+                    Node(
+                        package="rviz2",
+                        executable="rviz2",
+                        name="rviz2",
+                        arguments=[
+                            "-d",
+                            PathJoinSubstitution([
+                                FindPackageShare("saltybot_bringup"),
+                                "config", "saltybot_rviz.rviz",
+                            ]),
+                        ],
+                        output="screen",
+                    ),
+                ],
+            ),
+        ],
+    )
+
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+    # Assemble LaunchDescription
+    # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
+
+    return LaunchDescription([
+        # ── Arguments ──────────────────────────────────────────────────────────
+        profile_arg,
+        use_sim_time_arg,
+        stm32_port_arg,
+        uwb_port_a_arg,
+        uwb_port_b_arg,
+        gimbal_port_arg,
+        max_linear_vel_arg,
+        follow_distance_arg,
+
+        # ── Environment ────────────────────────────────────────────────────────
+        verbose_log_env,
+
+        # ── Shutdown handler ───────────────────────────────────────────────────
+        estop_on_shutdown,
+
+        # ── Startup banner ─────────────────────────────────────────────────────
+        group_a_banner,
+
+        # ── GROUP A: Drivers (all profiles, t=0–4s) ───────────────────────────
+        robot_description,
+        stm32_bridge,
+        sensors,
+        motor_daemon,
+        sensor_health,
+
+        # ── Health gate A→B ────────────────────────────────────────────────────
+        health_gate_ab,
+
+        # ── GROUP B: Perception (full/debug, t=8s) ────────────────────────────
+        group_b,
+
+        # ── Health gate B→C ────────────────────────────────────────────────────
+        health_gate_bc,
+
+        # ── GROUP C: Navigation (full/debug, t=16–22s) ────────────────────────
+        group_c,
+        nav2,
+        docking,
+
+        # ── Health gate C→D ────────────────────────────────────────────────────
+        health_gate_cd_full,
+        health_gate_cd_debug,
+
+        # ── GROUP D: UI/Social (full t=26s, debug t=30s) ──────────────────────
+        group_d_full,
+        group_d_debug,
+
+        # ── Debug extras (t=3s, debug profile only) ───────────────────────────
+        debug_extras,
+    ])

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py

@@ -0,0 +1,187 @@
+"""launch_profiles.py — SaltyBot bringup launch profiles (Issue #577).
+
+Defines three named profiles: minimal, full, debug.
+Each profile is a plain dataclass — no ROS2 runtime dependency — so
+profile selection logic can be unit-tested without a live ROS2 install.
+
+Profile hierarchy:
+  minimal ⊂ full ⊂ debug
+
+Usage (from launch files)::
+
+    from saltybot_bringup.launch_profiles import get_profile, Profile
+
+    p = get_profile("full")
+    if p.enable_slam:
+        ...
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Dict
+
+
+@dataclass
+class Profile:
+    """All runtime-configurable flags for a single launch profile."""
+
+    name: str
+
+    # ── Group A: Drivers (always on in all profiles) ──────────────────────
+    enable_stm32_bridge: bool = True
+    enable_sensors:      bool = True   # RealSense + RPLIDAR
+    enable_motor_daemon: bool = True
+    enable_imu:          bool = True
+
+    # ── Group B: Perception ────────────────────────────────────────────────
+    enable_uwb:              bool = False
+    enable_person_detection: bool = False
+    enable_object_detection: bool = False
+    enable_depth_costmap:    bool = False
+    enable_gimbal:           bool = False
+
+    # ── Group C: Navigation ────────────────────────────────────────────────
+    enable_slam:        bool = False
+    enable_nav2:        bool = False
+    enable_follower:    bool = False
+    enable_docking:     bool = False
+    enable_lidar_avoid: bool = False
+
+    # ── Group D: UI / Social / Audio ──────────────────────────────────────
+    enable_rosbridge:    bool = False
+    enable_audio:        bool = False
+    enable_social:       bool = False
+    enable_csi_cameras:  bool = False
+
+    # ── Debug / recording extras ──────────────────────────────────────────
+    enable_bag_recorder: bool = False
+    enable_diagnostics:  bool = True   # sensor health + DiagnosticArray
+    enable_rviz:         bool = False
+    enable_verbose_logs: bool = False
+
+    # ── Timing: inter-group health-gate delays (seconds) ──────────────────
+    #  Each delay is the minimum wall-clock time after t=0 before the group
+    #  starts.  Increase for slower hardware or cold-start scenarios.
+    t_drivers:    float = 0.0    # Group A
+    t_perception: float = 8.0   # Group B (sensors need ~6 s to initialise)
+    t_navigation: float = 16.0  # Group C (SLAM needs ~8 s to build first map)
+    t_ui:         float = 22.0  # Group D (nav2 needs ~4 s to load costmaps)
+
+    # ── Safety ────────────────────────────────────────────────────────────
+    watchdog_timeout_s:  float = 5.0   # max silence from STM32 bridge (s)
+    cmd_vel_deadman_s:   float = 0.5   # cmd_vel watchdog in bridge
+    max_linear_vel:      float = 0.5   # m/s cap passed to bridge + follower
+    follow_distance_m:   float = 1.5   # target follow distance (m)
+
+    # ── Hardware conditionals ─────────────────────────────────────────────
+    #  Paths checked at launch; absent devices skip the relevant node.
+    stm32_port:  str = "/dev/stm32-bridge"
+    uwb_port_a:  str = "/dev/uwb-anchor0"
+    uwb_port_b:  str = "/dev/uwb-anchor1"
+    gimbal_port: str = "/dev/ttyTHS1"
+
+    def to_dict(self) -> Dict:
+        """Return flat dict (e.g. for passing to launch Parameters)."""
+        import dataclasses
+        return dataclasses.asdict(self)
+
+
+# ── Profile factory ────────────────────────────────────────────────────────────
+
+def _minimal() -> Profile:
+    """Minimal: STM32 bridge + sensors + motor daemon.
+
+    Safe drive control only.  No AI, no nav, no social.
+    Boot time ~4 s.  RAM ~400 MB.
+    """
+    return Profile(
+        name="minimal",
+        # Drivers already enabled by default
+        enable_diagnostics=True,
+        t_drivers=0.0,
+        t_perception=0.0,   # unused
+        t_navigation=0.0,   # unused
+        t_ui=0.0,           # unused
+    )
+
+
+def _full() -> Profile:
+    """Full: complete autonomous stack.
+
+    SLAM + Nav2 + perception + audio + social + rosbridge.
+    Boot time ~22 s cold.  RAM ~3 GB.
+    """
+    return Profile(
+        name="full",
+        # Drivers
+        enable_stm32_bridge=True,
+        enable_sensors=True,
+        enable_motor_daemon=True,
+        enable_imu=True,
+        # Perception
+        enable_uwb=True,
+        enable_person_detection=True,
+        enable_object_detection=True,
+        enable_depth_costmap=True,
+        enable_gimbal=True,
+        # Navigation
+        enable_slam=True,
+        enable_nav2=True,
+        enable_follower=True,
+        enable_docking=True,
+        enable_lidar_avoid=True,
+        # UI
+        enable_rosbridge=True,
+        enable_audio=True,
+        enable_social=True,
+        enable_csi_cameras=True,
+        # Extras
+        enable_bag_recorder=True,
+        enable_diagnostics=True,
+    )
+
+
+def _debug() -> Profile:
+    """Debug: full stack + verbose logging + RViz + extra recording.
+
+    Adds /diagnostics aggregation, RViz2, verbose node output.
+    Boot time same as full.  RAM ~3.5 GB (RViz + extra logs).
+    """
+    p = _full()
+    p.name = "debug"
+    p.enable_rviz = True
+    p.enable_verbose_logs = True
+    p.enable_bag_recorder = True
+    # Slower boot to ensure all topics are stable before Nav2 starts
+    p.t_navigation = 20.0
+    p.t_ui = 26.0
+    return p
+
+
+_PROFILES: Dict[str, Profile] = {
+    "minimal": _minimal(),
+    "full":    _full(),
+    "debug":   _debug(),
+}
+
+
+def get_profile(name: str) -> Profile:
+    """Return the named Profile.
+
+    Args:
+        name: One of "minimal", "full", "debug".
+
+    Raises:
+        ValueError: If name is not a known profile.
+    """
+    if name not in _PROFILES:
+        raise ValueError(
+            f"Unknown launch profile {name!r}. "
+            f"Valid profiles: {sorted(_PROFILES)}"
+        )
+    return _PROFILES[name]
+
+
+def profile_names() -> list:
+    return sorted(_PROFILES.keys())

jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py

@@ -0,0 +1,326 @@
+"""test_launch_orchestrator.py — Unit tests for saltybot_bringup launch profiles (Issue #577).
+
+Tests are deliberately ROS2-free; run with:
+    python3 -m pytest jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py -v
+"""
+
+from __future__ import annotations
+
+import sys
+import os
+
+# Allow import without ROS2 install
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "saltybot_bringup"))
+
+import pytest
+from launch_profiles import Profile, get_profile, profile_names, _minimal, _full, _debug
+
+
+# ─── Profile factory basics ───────────────────────────────────────────────────
+
+class TestProfileNames:
+    def test_returns_list(self):
+        names = profile_names()
+        assert isinstance(names, list)
+
+    def test_contains_three_profiles(self):
+        assert len(profile_names()) == 3
+
+    def test_expected_names_present(self):
+        names = profile_names()
+        assert "minimal" in names
+        assert "full" in names
+        assert "debug" in names
+
+    def test_names_sorted(self):
+        names = profile_names()
+        assert names == sorted(names)
+
+
+class TestGetProfile:
+    def test_returns_profile_instance(self):
+        p = get_profile("minimal")
+        assert isinstance(p, Profile)
+
+    def test_unknown_raises_value_error(self):
+        with pytest.raises(ValueError, match="Unknown launch profile"):
+            get_profile("turbo")
+
+    def test_error_message_lists_valid_profiles(self):
+        with pytest.raises(ValueError) as exc_info:
+            get_profile("bogus")
+        msg = str(exc_info.value)
+        assert "debug" in msg
+        assert "full" in msg
+        assert "minimal" in msg
+
+    def test_get_minimal_name(self):
+        assert get_profile("minimal").name == "minimal"
+
+    def test_get_full_name(self):
+        assert get_profile("full").name == "full"
+
+    def test_get_debug_name(self):
+        assert get_profile("debug").name == "debug"
+
+
+# ─── Minimal profile ──────────────────────────────────────────────────────────
+
+class TestMinimalProfile:
+    def setup_method(self):
+        self.p = _minimal()
+
+    def test_name(self):
+        assert self.p.name == "minimal"
+
+    def test_drivers_enabled(self):
+        assert self.p.enable_stm32_bridge is True
+        assert self.p.enable_sensors is True
+        assert self.p.enable_motor_daemon is True
+        assert self.p.enable_imu is True
+
+    def test_perception_disabled(self):
+        assert self.p.enable_uwb is False
+        assert self.p.enable_person_detection is False
+        assert self.p.enable_object_detection is False
+        assert self.p.enable_depth_costmap is False
+        assert self.p.enable_gimbal is False
+
+    def test_navigation_disabled(self):
+        assert self.p.enable_slam is False
+        assert self.p.enable_nav2 is False
+        assert self.p.enable_follower is False
+        assert self.p.enable_docking is False
+        assert self.p.enable_lidar_avoid is False
+
+    def test_ui_disabled(self):
+        assert self.p.enable_rosbridge is False
+        assert self.p.enable_audio is False
+        assert self.p.enable_social is False
+        assert self.p.enable_csi_cameras is False
+
+    def test_debug_features_disabled(self):
+        assert self.p.enable_rviz is False
+        assert self.p.enable_verbose_logs is False
+        assert self.p.enable_bag_recorder is False
+
+    def test_timing_zero(self):
+        # Unused timings are zeroed so there are no unnecessary waits
+        assert self.p.t_perception == 0.0
+        assert self.p.t_navigation == 0.0
+        assert self.p.t_ui == 0.0
+
+    def test_diagnostics_enabled(self):
+        assert self.p.enable_diagnostics is True
+
+
+# ─── Full profile ─────────────────────────────────────────────────────────────
+
+class TestFullProfile:
+    def setup_method(self):
+        self.p = _full()
+
+    def test_name(self):
+        assert self.p.name == "full"
+
+    def test_drivers_enabled(self):
+        assert self.p.enable_stm32_bridge is True
+        assert self.p.enable_sensors is True
+        assert self.p.enable_motor_daemon is True
+        assert self.p.enable_imu is True
+
+    def test_perception_enabled(self):
+        assert self.p.enable_uwb is True
+        assert self.p.enable_person_detection is True
+        assert self.p.enable_object_detection is True
+        assert self.p.enable_depth_costmap is True
+        assert self.p.enable_gimbal is True
+
+    def test_navigation_enabled(self):
+        assert self.p.enable_slam is True
+        assert self.p.enable_nav2 is True
+        assert self.p.enable_follower is True
+        assert self.p.enable_docking is True
+        assert self.p.enable_lidar_avoid is True
+
+    def test_ui_enabled(self):
+        assert self.p.enable_rosbridge is True
+        assert self.p.enable_audio is True
+        assert self.p.enable_social is True
+        assert self.p.enable_csi_cameras is True
+
+    def test_diagnostics_enabled(self):
+        assert self.p.enable_diagnostics is True
+
+    def test_debug_features_disabled(self):
+        assert self.p.enable_rviz is False
+        assert self.p.enable_verbose_logs is False
+
+    def test_timing_positive(self):
+        assert self.p.t_drivers == 0.0
+        assert self.p.t_perception > 0.0
+        assert self.p.t_navigation > self.p.t_perception
+        assert self.p.t_ui > self.p.t_navigation
+
+    def test_perception_gate_8s(self):
+        assert self.p.t_perception == 8.0
+
+    def test_navigation_gate_16s(self):
+        assert self.p.t_navigation == 16.0
+
+    def test_ui_gate_22s(self):
+        assert self.p.t_ui == 22.0
+
+
+# ─── Debug profile ────────────────────────────────────────────────────────────
+
+class TestDebugProfile:
+    def setup_method(self):
+        self.p = _debug()
+
+    def test_name(self):
+        assert self.p.name == "debug"
+
+    def test_inherits_full_stack(self):
+        full = _full()
+        # All full flags must be True in debug too
+        assert self.p.enable_slam == full.enable_slam
+        assert self.p.enable_nav2 == full.enable_nav2
+        assert self.p.enable_rosbridge == full.enable_rosbridge
+
+    def test_rviz_enabled(self):
+        assert self.p.enable_rviz is True
+
+    def test_verbose_logs_enabled(self):
+        assert self.p.enable_verbose_logs is True
+
+    def test_bag_recorder_enabled(self):
+        assert self.p.enable_bag_recorder is True
+
+    def test_timing_slower_than_full(self):
+        full = _full()
+        # Debug extends gates to ensure stability
+        assert self.p.t_navigation >= full.t_navigation
+        assert self.p.t_ui >= full.t_ui
+
+    def test_navigation_gate_20s(self):
+        assert self.p.t_navigation == 20.0
+
+    def test_ui_gate_26s(self):
+        assert self.p.t_ui == 26.0
+
+
+# ─── Profile hierarchy checks ─────────────────────────────────────────────────
+
+class TestProfileHierarchy:
+    """Minimal ⊂ Full ⊂ Debug — every flag true in minimal must be true in full/debug."""
+
+    def _enabled_flags(self, p: Profile) -> set:
+        import dataclasses
+        return {
+            f.name
+            for f in dataclasses.fields(p)
+            if f.name.startswith("enable_") and getattr(p, f.name) is True
+        }
+
+    def test_minimal_subset_of_full(self):
+        minimal_flags = self._enabled_flags(_minimal())
+        full_flags = self._enabled_flags(_full())
+        assert minimal_flags.issubset(full_flags), (
+            f"Full missing flags that Minimal has: {minimal_flags - full_flags}"
+        )
+
+    def test_full_subset_of_debug(self):
+        full_flags = self._enabled_flags(_full())
+        debug_flags = self._enabled_flags(_debug())
+        assert full_flags.issubset(debug_flags), (
+            f"Debug missing flags that Full has: {full_flags - debug_flags}"
+        )
+
+    def test_debug_has_extra_flags(self):
+        full_flags = self._enabled_flags(_full())
+        debug_flags = self._enabled_flags(_debug())
+        extras = debug_flags - full_flags
+        assert len(extras) > 0, "Debug should have at least one extra flag over Full"
+
+    def test_debug_extras_are_debug_features(self):
+        full_flags = self._enabled_flags(_full())
+        debug_flags = self._enabled_flags(_debug())
+        extras = debug_flags - full_flags
+        for flag in extras:
+            assert "rviz" in flag or "verbose" in flag or "bag" in flag or "debug" in flag, (
+                f"Unexpected extra flag in debug: {flag}"
+            )
+
+
+# ─── to_dict ──────────────────────────────────────────────────────────────────
+
+class TestToDict:
+    def test_returns_dict(self):
+        p = _minimal()
+        d = p.to_dict()
+        assert isinstance(d, dict)
+
+    def test_name_in_dict(self):
+        d = _minimal().to_dict()
+        assert d["name"] == "minimal"
+
+    def test_all_bool_flags_present(self):
+        d = _full().to_dict()
+        for key in ("enable_slam", "enable_nav2", "enable_rosbridge", "enable_rviz"):
+            assert key in d, f"Missing key: {key}"
+
+    def test_timing_fields_present(self):
+        d = _full().to_dict()
+        for key in ("t_drivers", "t_perception", "t_navigation", "t_ui"):
+            assert key in d, f"Missing timing key: {key}"
+
+    def test_values_are_native_types(self):
+        d = _debug().to_dict()
+        for v in d.values():
+            assert isinstance(v, (bool, int, float, str)), (
+                f"Expected native type, got {type(v)} for value {v!r}"
+            )
+
+
+# ─── Safety parameter defaults ────────────────────────────────────────────────
+
+class TestSafetyDefaults:
+    def test_watchdog_timeout_positive(self):
+        for name in profile_names():
+            p = get_profile(name)
+            assert p.watchdog_timeout_s > 0, f"{name}: watchdog_timeout_s must be > 0"
+
+    def test_max_linear_vel_bounded(self):
+        for name in profile_names():
+            p = get_profile(name)
+            assert 0 < p.max_linear_vel <= 2.0, (
+                f"{name}: max_linear_vel {p.max_linear_vel} outside safe range"
+            )
+
+    def test_follow_distance_positive(self):
+        for name in profile_names():
+            p = get_profile(name)
+            assert p.follow_distance_m > 0, f"{name}: follow_distance_m must be > 0"
+
+    def test_cmd_vel_deadman_positive(self):
+        for name in profile_names():
+            p = get_profile(name)
+            assert p.cmd_vel_deadman_s > 0, f"{name}: cmd_vel_deadman_s must be > 0"
+
+
+# ─── Hardware port defaults ────────────────────────────────────────────────────
+
+class TestHardwarePortDefaults:
+    def test_stm32_port_set(self):
+        p = _minimal()
+        assert p.stm32_port.startswith("/dev/")
+
+    def test_uwb_ports_set(self):
+        p = _full()
+        assert p.uwb_port_a.startswith("/dev/")
+        assert p.uwb_port_b.startswith("/dev/")
+
+    def test_gimbal_port_set(self):
+        p = _full()
+        assert p.gimbal_port.startswith("/dev/")

jetson/ros2_ws/src/saltybot_phone/launch/phone_bringup.py

@@ -6,6 +6,8 @@ from launch.substitutions import LaunchConfiguration
 
 def generate_launch_description():
     camera_device_arg = DeclareLaunchArgument('camera_device', default_value='/dev/video0', description='Camera device path')
+    phone_host_arg = DeclareLaunchArgument('phone_host', default_value='192.168.1.200', description='Phone IP for video bridge')
+    phone_cam_enabled_arg = DeclareLaunchArgument('phone_cam_enabled', default_value='true', description='Enable phone camera node')
     gps_enabled_arg = DeclareLaunchArgument('gps_enabled', default_value='true', description='Enable GPS node')
     imu_enabled_arg = DeclareLaunchArgument('imu_enabled', default_value='true', description='Enable IMU node')
     chat_enabled_arg = DeclareLaunchArgument('chat_enabled', default_value='true', description='Enable OpenClaw chat node')
@@ -51,4 +53,25 @@ def generate_launch_description():
         output='screen',
     )
 
-    return LaunchDescription([camera_device_arg, gps_enabled_arg, imu_enabled_arg, chat_enabled_arg, bridge_url_arg, camera_node, gps_node, imu_node, chat_node, bridge_node])
+    phone_camera_node = Node(
+        package='saltybot_phone',
+        executable='phone_camera_node',
+        name='phone_camera_node',
+        parameters=[{
+            'phone_host':         LaunchConfiguration('phone_host'),
+            'ws_port':            8765,
+            'http_port':          8766,
+            'frame_id':           'phone_camera',
+            'publish_raw':        True,
+            'publish_compressed': True,
+            'reconnect_s':        3.0,
+            'latency_warn_ms':    200.0,
+        }],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        camera_device_arg, gps_enabled_arg, imu_enabled_arg, chat_enabled_arg,
+        bridge_url_arg, phone_host_arg, phone_cam_enabled_arg,
+        camera_node, gps_node, imu_node, chat_node, bridge_node, phone_camera_node,
+    ])

jetson/ros2_ws/src/saltybot_phone/saltybot_phone/phone_camera_node.py

@@ -0,0 +1,318 @@
+#!/usr/bin/env python3
+"""
+phone_camera_node.py — Jetson ROS2 receiver for phone video stream (Issue #585)
+
+Connects to the phone's video_bridge.py WebSocket server, receives JPEG frames,
+decodes them, and publishes:
+  /saltybot/phone/camera            sensor_msgs/Image            (bgr8)
+  /saltybot/phone/camera/compressed sensor_msgs/CompressedImage  (jpeg)
+
+Falls back to HTTP MJPEG polling if the WebSocket connection fails.
+
+Parameters
+──────────
+  phone_host      str   Phone IP or hostname     (default: 192.168.1.200)
+  ws_port         int   video_bridge WS port      (default: 8765)
+  http_port       int   video_bridge HTTP port    (default: 8766)
+  frame_id        str   TF frame id               (default: phone_camera)
+  publish_raw     bool  Publish sensor_msgs/Image (default: true)
+  publish_compressed bool Publish CompressedImage (default: true)
+  reconnect_s     float Seconds between reconnect (default: 3.0)
+  latency_warn_ms float Warn if frame age > this  (default: 200.0)
+
+Publishes
+─────────
+  /saltybot/phone/camera            sensor_msgs/Image
+  /saltybot/phone/camera/compressed sensor_msgs/CompressedImage
+  /saltybot/phone/camera/info       std_msgs/String  (JSON stream metadata)
+"""
+
+import asyncio
+import json
+import threading
+import time
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image, CompressedImage
+from std_msgs.msg import String, Header
+
+try:
+    import cv2
+    import numpy as np
+    CV2_AVAILABLE = True
+except ImportError:
+    CV2_AVAILABLE = False
+
+try:
+    import websockets
+    WS_AVAILABLE = True
+except ImportError:
+    WS_AVAILABLE = False
+
+try:
+    import urllib.request
+    HTTP_AVAILABLE = True
+except ImportError:
+    HTTP_AVAILABLE = False
+
+
+MJPEG_BOUNDARY = b"--mjpeg-boundary"
+
+
+class PhoneCameraNode(Node):
+    """
+    Receives JPEG frames from phone/video_bridge.py and publishes ROS2 Image topics.
+
+    Connection strategy:
+      1. Try WebSocket (ws://<phone_host>:<ws_port>)  — lowest latency
+      2. On WS failure, fall back to HTTP MJPEG stream — guaranteed to work
+      3. Reconnect automatically on disconnect
+    """
+
+    def __init__(self):
+        super().__init__("phone_camera_node")
+
+        # Parameters
+        self.declare_parameter("phone_host",          "192.168.1.200")
+        self.declare_parameter("ws_port",             8765)
+        self.declare_parameter("http_port",           8766)
+        self.declare_parameter("frame_id",            "phone_camera")
+        self.declare_parameter("publish_raw",         True)
+        self.declare_parameter("publish_compressed",  True)
+        self.declare_parameter("reconnect_s",         3.0)
+        self.declare_parameter("latency_warn_ms",     200.0)
+
+        self._host         = self.get_parameter("phone_host").value
+        self._ws_port      = self.get_parameter("ws_port").value
+        self._http_port    = self.get_parameter("http_port").value
+        self._frame_id     = self.get_parameter("frame_id").value
+        self._pub_raw      = self.get_parameter("publish_raw").value
+        self._pub_comp     = self.get_parameter("publish_compressed").value
+        self._reconnect_s  = self.get_parameter("reconnect_s").value
+        self._latency_warn = self.get_parameter("latency_warn_ms").value
+
+        if not CV2_AVAILABLE:
+            self.get_logger().error(
+                "opencv-python not installed — cannot decode JPEG frames."
+            )
+
+        # Publishers
+        self._raw_pub  = self.create_publisher(Image,           "/saltybot/phone/camera",            10)
+        self._comp_pub = self.create_publisher(CompressedImage, "/saltybot/phone/camera/compressed",  10)
+        self._info_pub = self.create_publisher(String,          "/saltybot/phone/camera/info",        10)
+
+        # Stats
+        self._frames_received  = 0
+        self._frames_published = 0
+        self._last_frame_ts    = 0.0
+        self._stream_info: dict = {}
+
+        # Diagnostics timer (every 10 s)
+        self.create_timer(10.0, self._diag_cb)
+
+        # Start receiver thread
+        self._stop_event = threading.Event()
+        self._recv_thread = threading.Thread(
+            target=self._recv_loop, name="phone_cam_recv", daemon=True
+        )
+        self._recv_thread.start()
+
+        self.get_logger().info(
+            "PhoneCameraNode ready — connecting to %s (ws:%d / http:%d)",
+            self._host, self._ws_port, self._http_port,
+        )
+
+    # ── Publish ───────────────────────────────────────────────────────────────
+
+    def _publish_jpeg(self, jpeg_bytes: bytes, recv_ts: float) -> None:
+        """Decode JPEG and publish Image and/or CompressedImage."""
+        self._frames_received += 1
+        age_ms = (time.time() - recv_ts) * 1000.0
+        if age_ms > self._latency_warn:
+            self.get_logger().warning(
+                "Phone camera frame age %.0f ms (target < %.0f ms)",
+                age_ms, self._latency_warn,
+            )
+
+        now_msg = self.get_clock().now().to_msg()
+
+        # CompressedImage — no decode needed
+        if self._pub_comp:
+            comp = CompressedImage()
+            comp.header = Header()
+            comp.header.stamp    = now_msg
+            comp.header.frame_id = self._frame_id
+            comp.format = "jpeg"
+            comp.data   = list(jpeg_bytes)
+            self._comp_pub.publish(comp)
+
+        # Image — decode via OpenCV
+        if self._pub_raw and CV2_AVAILABLE:
+            try:
+                buf   = np.frombuffer(jpeg_bytes, dtype=np.uint8)
+                frame = cv2.imdecode(buf, cv2.IMREAD_COLOR)
+                if frame is None:
+                    return
+                h, w = frame.shape[:2]
+                img = Image()
+                img.header        = Header()
+                img.header.stamp    = now_msg
+                img.header.frame_id = self._frame_id
+                img.height          = h
+                img.width           = w
+                img.encoding        = "bgr8"
+                img.is_bigendian    = 0
+                img.step            = w * 3
+                img.data            = frame.tobytes()
+                self._raw_pub.publish(img)
+            except Exception as e:
+                self.get_logger().debug("JPEG decode error: %s", str(e))
+                return
+
+        self._frames_published += 1
+        self._last_frame_ts = time.time()
+
+    # ── WebSocket receiver ────────────────────────────────────────────────────
+
+    async def _ws_recv(self) -> bool:
+        """
+        Connect via WebSocket and receive frames until disconnect.
+        Returns True if we received at least one frame (connection was working).
+        """
+        uri = f"ws://{self._host}:{self._ws_port}"
+        try:
+            async with websockets.connect(
+                uri,
+                ping_interval=5,
+                ping_timeout=10,
+                max_size=None,
+                open_timeout=5,
+            ) as ws:
+                self.get_logger().info("WebSocket connected to %s", uri)
+                got_frame = False
+                async for message in ws:
+                    if self._stop_event.is_set():
+                        return got_frame
+                    recv_ts = time.time()
+                    if isinstance(message, bytes):
+                        # Binary → JPEG frame
+                        self._publish_jpeg(message, recv_ts)
+                        got_frame = True
+                    elif isinstance(message, str):
+                        # Text → control JSON
+                        try:
+                            obj = json.loads(message)
+                            if obj.get("type") == "info":
+                                self._stream_info = obj
+                                info_msg = String()
+                                info_msg.data = message
+                                self._info_pub.publish(info_msg)
+                                self.get_logger().info(
+                                    "Stream info: %dx%d @ %.0f fps",
+                                    obj.get("width", 0),
+                                    obj.get("height", 0),
+                                    obj.get("fps", 0),
+                                )
+                        except json.JSONDecodeError:
+                            pass
+                return got_frame
+        except (websockets.exceptions.WebSocketException, OSError,
+                asyncio.TimeoutError) as e:
+            self.get_logger().debug("WS error: %s", str(e))
+            return False
+
+    # ── HTTP MJPEG fallback ───────────────────────────────────────────────────
+
+    def _http_mjpeg_recv(self) -> None:
+        """Receive MJPEG stream over HTTP and publish frames."""
+        url = f"http://{self._host}:{self._http_port}/stream"
+        self.get_logger().info("Falling back to HTTP MJPEG: %s", url)
+        buf = b""
+        try:
+            req = urllib.request.urlopen(url, timeout=10)
+            while not self._stop_event.is_set():
+                chunk = req.read(4096)
+                if not chunk:
+                    break
+                buf += chunk
+                # Extract JPEG frames delimited by MJPEG boundary
+                while True:
+                    start = buf.find(b"\xff\xd8")  # JPEG SOI
+                    end   = buf.find(b"\xff\xd9")  # JPEG EOI
+                    if start == -1 or end == -1 or end < start:
+                        # Keep last partial frame in buffer
+                        if start > 0:
+                            buf = buf[start:]
+                        break
+                    jpeg = buf[start:end + 2]
+                    buf  = buf[end + 2:]
+                    self._publish_jpeg(jpeg, time.time())
+        except Exception as e:
+            self.get_logger().debug("HTTP MJPEG error: %s", str(e))
+
+    # ── Receiver loop ─────────────────────────────────────────────────────────
+
+    def _recv_loop(self) -> None:
+        """
+        Main receiver loop — tries WS first, falls back to HTTP, then retries.
+        Runs in a daemon thread.
+        """
+        while not self._stop_event.is_set():
+            # Try WebSocket
+            if WS_AVAILABLE:
+                try:
+                    loop = asyncio.new_event_loop()
+                    asyncio.set_event_loop(loop)
+                    got = loop.run_until_complete(self._ws_recv())
+                    loop.close()
+                    if got and not self._stop_event.is_set():
+                        # WS was working but disconnected — retry immediately
+                        continue
+                except Exception as e:
+                    self.get_logger().debug("WS loop error: %s", str(e))
+
+            if self._stop_event.is_set():
+                break
+
+            # WS failed — try HTTP MJPEG fallback
+            if HTTP_AVAILABLE:
+                self._http_mjpeg_recv()
+
+            if not self._stop_event.is_set():
+                self.get_logger().info(
+                    "Phone camera stream lost — retrying in %.0f s", self._reconnect_s
+                )
+                self._stop_event.wait(self._reconnect_s)
+
+    # ── Diagnostics ───────────────────────────────────────────────────────────
+
+    def _diag_cb(self) -> None:
+        age = time.time() - self._last_frame_ts if self._last_frame_ts > 0 else -1.0
+        self.get_logger().info(
+            "Phone camera — received=%d published=%d last_frame_age=%.1fs",
+            self._frames_received, self._frames_published, age,
+        )
+
+    # ── Lifecycle ─────────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        self._stop_event.set()
+        super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = PhoneCameraNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_phone/setup.py

@@ -28,6 +28,7 @@ setup(
             'imu_node = saltybot_phone.imu_node:main',
             'openclaw_chat = saltybot_phone.openclaw_chat_node:main',
             'phone_bridge = saltybot_phone.ws_bridge:main',
+            'phone_camera_node = saltybot_phone.phone_camera_node:main',
         ],
     },
 )

jetson/ros2_ws/src/saltybot_safety_zone/config/safety_zone_params.yaml

@@ -0,0 +1,44 @@
+# safety_zone_params.yaml — RPLIDAR 360° safety zone detector (Issue #575)
+#
+# Node: saltybot_safety_zone
+#
+# Usage:
+#   ros2 launch saltybot_safety_zone safety_zone.launch.py
+#
+# Zone thresholds:
+#   DANGER  < danger_range_m   → latching e-stop (if in forward arc)
+#   WARN    < warn_range_m     → caution / speed reduction (advisory)
+#   CLEAR   otherwise
+#
+# E-stop clear:
+#   ros2 service call /saltybot/safety_zone/clear_estop std_srvs/srv/Trigger
+
+safety_zone:
+  ros__parameters:
+
+    # ── Zone thresholds ──────────────────────────────────────────────────────
+    danger_range_m: 0.30    # m — obstacle closer than this → DANGER
+    warn_range_m:   1.00    # m — obstacle closer than this → WARN
+
+    # ── Sector grid ──────────────────────────────────────────────────────────
+    n_sectors: 36           # 360 / 36 = 10° per sector
+
+    # ── E-stop trigger arc ───────────────────────────────────────────────────
+    forward_arc_deg:        60.0   # ±30° from robot forward (+X / 0°)
+    estop_all_arcs:         false  # true = any sector triggers (360° e-stop)
+    estop_debounce_frames:  2      # consecutive DANGER scans before latch
+
+    # ── Range validity ───────────────────────────────────────────────────────
+    min_valid_range_m: 0.05    # ignore readings closer than this (sensor noise)
+    max_valid_range_m: 12.00   # RPLIDAR A1M8 nominal max range
+
+    # ── Publish rate ─────────────────────────────────────────────────────────
+    publish_rate: 10.0     # Hz — /saltybot/safety_zone/status publish rate
+                            #      /saltybot/safety_zone publishes every scan
+
+    # ── cmd_vel topics ───────────────────────────────────────────────────────
+    # Safety zone node intercepts cmd_vel from upstream, overrides to zero on estop.
+    # Typical chain:
+    #   cmd_vel_mux → /cmd_vel_safe → [safety_zone: cmd_vel_input] → /cmd_vel → STM32
+    cmd_vel_input_topic:  /cmd_vel_input   # upstream velocity (remap as needed)
+    cmd_vel_output_topic: /cmd_vel         # downstream (to STM32 bridge)

jetson/ros2_ws/src/saltybot_safety_zone/launch/safety_zone.launch.py

@@ -0,0 +1,28 @@
+"""Launch file for saltybot_safety_zone (Issue #575)."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    config = os.path.join(
+        get_package_share_directory("saltybot_safety_zone"),
+        "config",
+        "safety_zone_params.yaml",
+    )
+
+    safety_zone_node = Node(
+        package="saltybot_safety_zone",
+        executable="safety_zone",
+        name="safety_zone",
+        parameters=[config],
+        remappings=[
+            # Remap if the upstream mux publishes to a different topic:
+            # ("/cmd_vel_input", "/cmd_vel_safe"),
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([safety_zone_node])

jetson/ros2_ws/src/saltybot_safety_zone/package.xml

@@ -0,0 +1,32 @@
+<?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_safety_zone</name>
+  <version>0.1.0</version>
+  <description>
+    RPLIDAR 360° safety zone detector (Issue #575).
+    Divides the full 360° scan into 10° sectors, classifies each as
+    DANGER/WARN/CLEAR, latches an e-stop on DANGER in the forward arc,
+    overrides /cmd_vel to zero while latched, and exposes a service to clear
+    the latch once obstacles are gone.
+  </description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_safety_zone/saltybot_safety_zone/safety_zone_node.py

@@ -0,0 +1,351 @@
+#!/usr/bin/env python3
+"""
+safety_zone_node.py — RPLIDAR 360° safety zone detector (Issue #575).
+
+Processes /scan into three concentric safety zones and publishes per-sector
+classification, a latching e-stop on DANGER in the forward arc, and a zero
+cmd_vel override while the e-stop is active.
+
+Zone thresholds (configurable):
+  DANGER  < danger_range_m  (default 0.30 m) — immediate halt
+  WARN    < warn_range_m    (default 1.00 m) — caution / slow-down
+  CLEAR   otherwise
+
+Sectors:
+  360° is divided into N_SECTORS (default 36) sectors of 10° each.
+  Sector 0 is centred on 0° (robot forward = base_link +X axis).
+  Sector indices increase counter-clockwise (ROS convention).
+
+E-stop behaviour:
+  1. If any sector in the forward arc has DANGER for >= estop_debounce_frames
+     consecutive scans, the e-stop latches.
+  2. While latched:
+     - A zero Twist is published to /cmd_vel every scan cycle.
+     - Incoming cmd_vel_input messages are silently dropped.
+  3. The latch is cleared ONLY via the ROS service:
+     /saltybot/safety_zone/clear_estop  (std_srvs/Trigger)
+     — and only if no DANGER sectors remain at the time of the call.
+
+Published topics:
+  /saltybot/safety_zone          (std_msgs/String) — JSON per-sector data
+  /saltybot/safety_zone/status   (std_msgs/String) — JSON summary + e-stop state
+  /cmd_vel                       (geometry_msgs/Twist) — zero override when e-stopped
+
+Subscribed topics:
+  /scan           (sensor_msgs/LaserScan)   — RPLIDAR data
+  /cmd_vel_input  (geometry_msgs/Twist)     — upstream cmd_vel (pass-through or block)
+
+Services:
+  /saltybot/safety_zone/clear_estop  (std_srvs/Trigger)
+"""
+
+import json
+import math
+import threading
+from typing import List, Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import LaserScan
+from std_msgs.msg import String
+from std_srvs.srv import Trigger
+
+
+# Zone levels (int)
+CLEAR  = 0
+WARN   = 1
+DANGER = 2
+
+_ZONE_NAME = {CLEAR: "CLEAR", WARN: "WARN", DANGER: "DANGER"}
+
+
+class SafetyZoneNode(Node):
+    """360° RPLIDAR safety zone detector with latching e-stop."""
+
+    def __init__(self) -> None:
+        super().__init__("safety_zone")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("danger_range_m",        0.30)
+        self.declare_parameter("warn_range_m",          1.00)
+        self.declare_parameter("n_sectors",             36)     # 360/36 = 10° each
+        self.declare_parameter("forward_arc_deg",       60.0)   # ±30° e-stop window
+        self.declare_parameter("estop_all_arcs",        False)  # true = any sector triggers
+        self.declare_parameter("estop_debounce_frames", 2)      # consecutive DANGER frames
+        self.declare_parameter("min_valid_range_m",     0.05)   # ignore closer readings
+        self.declare_parameter("max_valid_range_m",     12.0)   # RPLIDAR A1M8 max
+        self.declare_parameter("publish_rate",          10.0)   # Hz — sector publish rate
+        self.declare_parameter("cmd_vel_input_topic",   "/cmd_vel_input")
+        self.declare_parameter("cmd_vel_output_topic",  "/cmd_vel")
+
+        self._danger_r   = self.get_parameter("danger_range_m").value
+        self._warn_r     = self.get_parameter("warn_range_m").value
+        self._n_sectors  = self.get_parameter("n_sectors").value
+        self._fwd_arc    = self.get_parameter("forward_arc_deg").value
+        self._all_arcs   = self.get_parameter("estop_all_arcs").value
+        self._debounce   = self.get_parameter("estop_debounce_frames").value
+        self._min_r      = self.get_parameter("min_valid_range_m").value
+        self._max_r      = self.get_parameter("max_valid_range_m").value
+        self._pub_rate   = self.get_parameter("publish_rate").value
+        _in_topic        = self.get_parameter("cmd_vel_input_topic").value
+        _out_topic       = self.get_parameter("cmd_vel_output_topic").value
+
+        self._sector_deg = 360.0 / self._n_sectors   # degrees per sector
+
+        # Precompute which sector indices are in the forward arc
+        self._forward_sectors = self._compute_forward_sectors()
+
+        # ── State ─────────────────────────────────────────────────────────────
+        self._lock          = threading.Lock()
+        self._sector_zones: List[int] = [CLEAR] * self._n_sectors
+        self._sector_ranges: List[float] = [float("inf")] * self._n_sectors
+        self._estop_latched = False
+        self._estop_reason  = ""
+        self._danger_frame_count = 0   # consecutive DANGER frames in forward arc
+        self._scan_count    = 0
+        self._last_scan_stamp: Optional[float] = None
+
+        # ── Subscriptions ─────────────────────────────────────────────────────
+        sensor_qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+        self._scan_sub = self.create_subscription(
+            LaserScan, "/scan", self._on_scan, sensor_qos
+        )
+        self._cmd_in_sub = self.create_subscription(
+            Twist, _in_topic, self._on_cmd_vel_input, 10
+        )
+
+        # ── Publishers ────────────────────────────────────────────────────────
+        self._zone_pub   = self.create_publisher(String, "/saltybot/safety_zone",        10)
+        self._status_pub = self.create_publisher(String, "/saltybot/safety_zone/status", 10)
+        self._cmd_pub    = self.create_publisher(Twist,  _out_topic, 10)
+
+        # ── Service ───────────────────────────────────────────────────────────
+        self._clear_srv = self.create_service(
+            Trigger,
+            "/saltybot/safety_zone/clear_estop",
+            self._handle_clear_estop,
+        )
+
+        # ── Periodic status publish ───────────────────────────────────────────
+        self.create_timer(1.0 / self._pub_rate, self._publish_status)
+
+        self.get_logger().info(
+            f"SafetyZoneNode ready — "
+            f"danger={self._danger_r}m warn={self._warn_r}m "
+            f"sectors={self._n_sectors}({self._sector_deg:.0f}°each) "
+            f"fwd_arc=±{self._fwd_arc/2:.0f}° "
+            f"debounce={self._debounce}"
+        )
+
+    # ── Sector geometry ───────────────────────────────────────────────────────
+
+    def _compute_forward_sectors(self) -> List[int]:
+        """Return sector indices that lie within the forward arc."""
+        half = self._fwd_arc / 2.0
+        fwd = []
+        for i in range(self._n_sectors):
+            centre_deg = i * self._sector_deg
+            # Normalise to (−180, 180]
+            if centre_deg > 180.0:
+                centre_deg -= 360.0
+            if abs(centre_deg) <= half:
+                fwd.append(i)
+        return fwd
+
+    @staticmethod
+    def _angle_to_sector(angle_rad: float, n_sectors: int) -> int:
+        """Convert a bearing (rad) to sector index [0, n_sectors)."""
+        deg = math.degrees(angle_rad) % 360.0
+        return int(deg / (360.0 / n_sectors)) % n_sectors
+
+    # ── Scan processing ───────────────────────────────────────────────────────
+
+    def _on_scan(self, msg: LaserScan) -> None:
+        """Process incoming LaserScan into per-sector zone classification."""
+        n = len(msg.ranges)
+        if n == 0:
+            return
+
+        # Accumulate min range per sector
+        sector_min = [float("inf")] * self._n_sectors
+
+        for i, r in enumerate(msg.ranges):
+            if not math.isfinite(r) or r < self._min_r or r > self._max_r:
+                continue
+            angle_rad = msg.angle_min + i * msg.angle_increment
+            s = self._angle_to_sector(angle_rad, self._n_sectors)
+            if r < sector_min[s]:
+                sector_min[s] = r
+
+        # Classify each sector
+        sector_zones = []
+        for r in sector_min:
+            if r < self._danger_r:
+                sector_zones.append(DANGER)
+            elif r < self._warn_r:
+                sector_zones.append(WARN)
+            else:
+                sector_zones.append(CLEAR)
+
+        with self._lock:
+            self._sector_zones  = sector_zones
+            self._sector_ranges = sector_min
+            self._scan_count   += 1
+            self._last_scan_stamp = self.get_clock().now().nanoseconds * 1e-9
+
+            # E-stop detection
+            if not self._estop_latched:
+                danger_in_trigger = self._has_danger_in_trigger_arc(sector_zones)
+                if danger_in_trigger:
+                    self._danger_frame_count += 1
+                    if self._danger_frame_count >= self._debounce:
+                        self._estop_latched = True
+                        danger_sectors = [
+                            i for i in (range(self._n_sectors) if self._all_arcs
+                                        else self._forward_sectors)
+                            if sector_zones[i] == DANGER
+                        ]
+                        self._estop_reason = (
+                            f"DANGER in sectors {danger_sectors} "
+                            f"(min range {min(sector_min[i] for i in danger_sectors if math.isfinite(sector_min[i])):.2f}m)"
+                        )
+                        self.get_logger().error(
+                            f"E-STOP LATCHED: {self._estop_reason}"
+                        )
+                else:
+                    self._danger_frame_count = 0
+
+        # Publish zero cmd_vel immediately if e-stopped (time-critical)
+        if self._estop_latched:
+            self._cmd_pub.publish(Twist())
+
+        # Publish sector data every scan
+        self._publish_sectors(sector_zones, sector_min)
+
+    def _has_danger_in_trigger_arc(self, zones: List[int]) -> bool:
+        """True if any DANGER sector exists in the trigger arc."""
+        if self._all_arcs:
+            return any(z == DANGER for z in zones)
+        return any(zones[i] == DANGER for i in self._forward_sectors)
+
+    # ── cmd_vel pass-through / override ──────────────────────────────────────
+
+    def _on_cmd_vel_input(self, msg: Twist) -> None:
+        """Pass cmd_vel through unless e-stop is latched."""
+        with self._lock:
+            latched = self._estop_latched
+        if latched:
+            # Override: publish zero (already done in scan callback, belt-and-braces)
+            self._cmd_pub.publish(Twist())
+        else:
+            self._cmd_pub.publish(msg)
+
+    # ── Service: clear e-stop ─────────────────────────────────────────────────
+
+    def _handle_clear_estop(
+        self, request: Trigger.Request, response: Trigger.Response
+    ) -> Trigger.Response:
+        with self._lock:
+            if not self._estop_latched:
+                response.success = True
+                response.message = "E-stop was not active."
+                return response
+
+            # Only allow clear if no current DANGER sectors
+            if self._has_danger_in_trigger_arc(self._sector_zones):
+                response.success = False
+                response.message = (
+                    "Cannot clear: DANGER sectors still present. "
+                    "Remove obstacle first."
+                )
+                return response
+
+            self._estop_latched = False
+            self._estop_reason  = ""
+            self._danger_frame_count = 0
+
+        self.get_logger().warning("E-stop cleared via service.")
+        response.success = True
+        response.message = "E-stop cleared. Resuming normal operation."
+        return response
+
+    # ── Publishers ────────────────────────────────────────────────────────────
+
+    def _publish_sectors(self, zones: List[int], ranges: List[float]) -> None:
+        """Publish per-sector JSON on /saltybot/safety_zone."""
+        sectors_data = []
+        for i, (zone, r) in enumerate(zip(zones, ranges)):
+            centre_deg = i * self._sector_deg
+            sectors_data.append({
+                "sector":    i,
+                "angle_deg": round(centre_deg, 1),
+                "zone":      _ZONE_NAME[zone],
+                "min_range_m": round(r, 3) if math.isfinite(r) else None,
+                "forward":   i in self._forward_sectors,
+            })
+
+        payload = {
+            "sectors":       sectors_data,
+            "estop_active":  self._estop_latched,
+            "estop_reason":  self._estop_reason,
+            "danger_sectors": [i for i, z in enumerate(zones) if z == DANGER],
+            "warn_sectors":   [i for i, z in enumerate(zones) if z == WARN],
+        }
+        self._zone_pub.publish(String(data=json.dumps(payload)))
+
+    def _publish_status(self) -> None:
+        """10 Hz JSON summary on /saltybot/safety_zone/status."""
+        with self._lock:
+            zones   = list(self._sector_zones)
+            ranges  = list(self._sector_ranges)
+            latched = self._estop_latched
+            reason  = self._estop_reason
+            scans   = self._scan_count
+
+        danger_cnt = sum(1 for z in zones if z == DANGER)
+        warn_cnt   = sum(1 for z in zones if z == WARN)
+        fwd_zone   = max(
+            (zones[i] for i in self._forward_sectors),
+            default=CLEAR,
+        )
+
+        # Closest obstacle in any direction
+        all_finite = [r for r in ranges if math.isfinite(r)]
+        closest_m  = min(all_finite) if all_finite else None
+
+        status = {
+            "estop_active":         latched,
+            "estop_reason":         reason,
+            "forward_zone":         _ZONE_NAME[fwd_zone],
+            "danger_sector_count":  danger_cnt,
+            "warn_sector_count":    warn_cnt,
+            "closest_obstacle_m":   round(closest_m, 3) if closest_m is not None else None,
+            "scan_count":           scans,
+            "forward_sector_ids":   self._forward_sectors,
+        }
+        self._status_pub.publish(String(data=json.dumps(status)))
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = SafetyZoneNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_safety_zone/setup.cfg

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

jetson/ros2_ws/src/saltybot_safety_zone/setup.py

@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = "saltybot_safety_zone"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages",
+            ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "launch"), glob("launch/*.py")),
+        (os.path.join("share", package_name, "config"), glob("config/*.yaml")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-perception",
+    maintainer_email="sl-perception@saltylab.local",
+    description="RPLIDAR 360° safety zone detector with latching e-stop (Issue #575)",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "safety_zone = saltybot_safety_zone.safety_zone_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_uwb_imu_fusion/config/fusion_params.yaml

@@ -0,0 +1,26 @@
+uwb_imu_fusion:
+  ros__parameters:
+    # ── Topics ────────────────────────────────────────────────────────────────
+    imu_topic:          /imu/data
+    uwb_bearing_topic:  /uwb/bearing
+    uwb_pose_topic:     /saltybot/uwb/pose
+    use_uwb_pose:       true    # true = use absolute /saltybot/uwb/pose
+                                # false = use relative /uwb/bearing
+
+    # ── TF ────────────────────────────────────────────────────────────────────
+    publish_tf:   true
+    map_frame:    map
+    base_frame:   base_link
+
+    # ── EKF noise parameters ──────────────────────────────────────────────────
+    # Increase sigma_uwb_pos_m if UWB is noisy (multipath, few anchors)
+    # Increase sigma_imu_* if IMU has high vibration noise
+    sigma_uwb_pos_m:      0.12   # UWB position std-dev (m)   — DW3000 ±10 cm + geometry
+    sigma_imu_accel:      0.05   # IMU accel noise (m/s²)
+    sigma_imu_gyro:       0.003  # IMU gyro noise (rad/s)
+    sigma_vel_drift:      0.10   # velocity drift process noise (m/s)
+    dropout_vel_damping:  0.95   # velocity decay factor per second during dropout
+
+    # ── Dropout ───────────────────────────────────────────────────────────────
+    max_dead_reckoning_s: 10.0   # suppress fused pose output after this many
+                                 # seconds without a UWB update

jetson/ros2_ws/src/saltybot_uwb_imu_fusion/launch/uwb_imu_fusion.launch.py

@@ -0,0 +1,43 @@
+"""
+uwb_imu_fusion.launch.py — Launch UWB-IMU EKF fusion node (Issue #573)
+
+Usage:
+  ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py
+  ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py publish_tf:=false
+  ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py sigma_uwb_pos_m:=0.20
+"""
+
+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 = get_package_share_directory("saltybot_uwb_imu_fusion")
+    cfg = os.path.join(pkg, "config", "fusion_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument("publish_tf",          default_value="true"),
+        DeclareLaunchArgument("use_uwb_pose",        default_value="true"),
+        DeclareLaunchArgument("sigma_uwb_pos_m",     default_value="0.12"),
+        DeclareLaunchArgument("max_dead_reckoning_s", default_value="10.0"),
+
+        Node(
+            package="saltybot_uwb_imu_fusion",
+            executable="uwb_imu_fusion",
+            name="uwb_imu_fusion",
+            output="screen",
+            parameters=[
+                cfg,
+                {
+                    "publish_tf":           LaunchConfiguration("publish_tf"),
+                    "use_uwb_pose":         LaunchConfiguration("use_uwb_pose"),
+                    "sigma_uwb_pos_m":      LaunchConfiguration("sigma_uwb_pos_m"),
+                    "max_dead_reckoning_s": LaunchConfiguration("max_dead_reckoning_s"),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_uwb_imu_fusion/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_uwb_imu_fusion</name>
+  <version>0.1.0</version>
+  <description>
+    EKF-based UWB + IMU sensor fusion for SaltyBot indoor localization (Issue #573).
+    Fuses UWB position at 10 Hz with IMU accel+gyro at 200 Hz.
+    Handles UWB dropouts via IMU dead-reckoning.
+    Publishes /saltybot/pose/fused and /saltybot/pose/fused_cov.
+  </description>
+  <maintainer email="sl-uwb@saltylab.local">sl-uwb</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>saltybot_uwb_msgs</depend>
+
+  <exec_depend>python3-numpy</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_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_uwb_imu_fusion/saltybot_uwb_imu_fusion/ekf_math.py

@@ -0,0 +1,266 @@
+"""
+ekf_math.py — Extended Kalman Filter for UWB + IMU fusion (Issue #573)
+
+No ROS2 dependencies — pure Python + numpy, fully unit-testable.
+
+State vector (6-DOF ground robot):
+───────────────────────────────────
+    x  = [x, y, θ, vx, vy, ω]ᵀ    (world frame)
+    x  — forward position (m)
+    y  — lateral position (m)
+    θ  — heading (rad, CCW positive)
+    vx — longitudinal velocity (m/s, world frame)
+    vy — lateral velocity (m/s, world frame)
+    ω  — yaw rate (rad/s)
+
+Process model — IMU as control input u = [ax_body, ay_body, ω_imu]
+───────────────────────────────────────────────────────────────────
+    θ_k+1  = θ_k  + ω_imu * dt
+    vx_k+1 = vx_k + (ax_body * cos(θ) - ay_body * sin(θ)) * dt
+    vy_k+1 = vy_k + (ax_body * sin(θ) + ay_body * cos(θ)) * dt
+    x_k+1  = x_k  + vx_k * dt
+    y_k+1  = y_k  + vy_k * dt
+    ω_k+1  = ω_imu  (direct observation, no integration)
+
+UWB measurement model (position observation):
+──────────────────────────────────────────────
+    z = [x, y]ᵀ    H = [[1,0,0,0,0,0],
+                         [0,1,0,0,0,0]]
+
+IMU bias handling:
+──────────────────
+    Simple first-order high-pass on accel to reduce drift.
+    For longer deployments extend state to include accel bias.
+"""
+
+from __future__ import annotations
+
+import math
+import numpy as np
+from typing import Tuple
+
+# State dimension
+N = 6   # [x, y, θ, vx, vy, ω]
+
+# Indices
+IX, IY, IT, IVX, IVY, IW = range(N)
+
+
+class UwbImuEKF:
+    """
+    EKF fusing UWB position (10 Hz) with IMU accel+gyro (200 Hz).
+
+    Parameters
+    ----------
+    sigma_uwb_pos_m : UWB position std-dev (m)           default 0.12
+    sigma_imu_accel : IMU accelerometer noise (m/s²)     default 0.05
+    sigma_imu_gyro  : IMU gyroscope noise (rad/s)        default 0.003
+    sigma_vel_drift : velocity drift process noise (m/s) default 0.1
+    dropout_vel_damping : velocity damping per second during UWB dropout
+    """
+
+    def __init__(
+        self,
+        sigma_uwb_pos_m: float    = 0.12,
+        sigma_imu_accel: float    = 0.05,
+        sigma_imu_gyro: float     = 0.003,
+        sigma_vel_drift: float    = 0.10,
+        dropout_vel_damping: float = 0.95,
+    ) -> None:
+        self._R_uwb  = sigma_uwb_pos_m ** 2   # UWB position variance
+        self._q_a    = sigma_imu_accel ** 2    # accel noise variance
+        self._q_g    = sigma_imu_gyro  ** 2    # gyro noise variance
+        self._q_v    = sigma_vel_drift ** 2    # velocity drift variance
+        self._damp   = dropout_vel_damping
+
+        # State and covariance
+        self._x = np.zeros(N, dtype=float)
+        self._P = np.eye(N, dtype=float) * 9.0   # large initial uncertainty
+
+        # Accel bias (simple running mean for high-pass)
+        self._accel_bias = np.zeros(2, dtype=float)
+        self._bias_alpha = 0.005   # low-pass coefficient for bias estimation
+
+        self._initialised = False
+        self._uwb_dropout_s = 0.0   # time since last UWB update
+
+    # ── Initialise ───────────────────────────────────────────────────────────
+
+    def initialise(self, x: float, y: float, heading_rad: float = 0.0) -> None:
+        """Seed the filter with a known position."""
+        self._x[:] = 0.0
+        self._x[IX] = x
+        self._x[IY] = y
+        self._x[IT] = heading_rad
+        self._P = np.diag([0.25, 0.25, 0.1, 1.0, 1.0, 0.1])
+        self._initialised = True
+
+    # ── IMU predict ──────────────────────────────────────────────────────────
+
+    def predict(self, ax_body: float, ay_body: float, omega: float, dt: float) -> None:
+        """
+        EKF predict step driven by IMU measurement.
+
+        Parameters
+        ----------
+        ax_body : forward acceleration in body frame (m/s²)
+        ay_body : lateral acceleration in body frame (m/s², left positive)
+        omega   : yaw rate (rad/s, CCW positive)
+        dt      : time step (s)
+        """
+        if not self._initialised:
+            return
+
+        # Subtract estimated accel bias
+        ax_c = ax_body - self._accel_bias[0]
+        ay_c = ay_body - self._accel_bias[1]
+
+        x  = self._x
+        th = x[IT]
+        ct = math.cos(th)
+        st = math.sin(th)
+
+        # World-frame acceleration
+        ax_w = ax_c * ct - ay_c * st
+        ay_w = ax_c * st + ay_c * ct
+
+        # State prediction
+        xp = x.copy()
+        xp[IX]  = x[IX]  + x[IVX] * dt
+        xp[IY]  = x[IY]  + x[IVY] * dt
+        xp[IT]  = _wrap_angle(x[IT] + omega * dt)
+        xp[IVX] = x[IVX] + ax_w * dt
+        xp[IVY] = x[IVY] + ay_w * dt
+        xp[IW]  = omega   # direct observation from gyro
+
+        # Jacobian F = ∂f/∂x
+        F = np.eye(N, dtype=float)
+        # ∂x / ∂vx, ∂x / ∂vy
+        F[IX, IVX] = dt
+        F[IY, IVY] = dt
+        # ∂vx / ∂θ  = (-ax_c·sin(θ) - ay_c·cos(θ)) * dt
+        F[IVX, IT] = (-ax_c * st - ay_c * ct) * dt
+        # ∂vy / ∂θ  = ( ax_c·cos(θ) - ay_c·sin(θ)) * dt
+        F[IVY, IT] = ( ax_c * ct - ay_c * st) * dt
+
+        # Process noise Q
+        dt2 = dt * dt
+        Q = np.diag([
+            0.5 * self._q_a * dt2 * dt2,   # x: from accel double-integrated
+            0.5 * self._q_a * dt2 * dt2,   # y
+            self._q_g * dt2,               # θ: from gyro integrated
+            self._q_v * dt + self._q_a * dt2,  # vx
+            self._q_v * dt + self._q_a * dt2,  # vy
+            self._q_g,                     # ω: direct gyro noise
+        ])
+
+        self._x = xp
+        self._P = F @ self._P @ F.T + Q
+        self._uwb_dropout_s += dt
+
+        # Velocity damping during extended UWB dropout (dead-reckoning coasting)
+        if self._uwb_dropout_s > 2.0:
+            damping = self._damp ** dt
+            self._x[IVX] *= damping
+            self._x[IVY] *= damping
+
+        # Update accel bias estimate
+        self._accel_bias[0] += self._bias_alpha * (ax_body - self._accel_bias[0])
+        self._accel_bias[1] += self._bias_alpha * (ay_body - self._accel_bias[1])
+
+    # ── UWB update ───────────────────────────────────────────────────────────
+
+    def update_uwb(self, x_meas: float, y_meas: float,
+                   sigma_m: float | None = None) -> None:
+        """
+        EKF update step with a UWB position measurement.
+
+        Parameters
+        ----------
+        x_meas, y_meas : measured position (m, world frame)
+        sigma_m        : override measurement std-dev (m); uses node default if None
+        """
+        if not self._initialised:
+            self.initialise(x_meas, y_meas)
+            return
+
+        R_val = (sigma_m ** 2) if sigma_m is not None else self._R_uwb
+        R = np.eye(2) * R_val
+
+        # H: position rows only
+        H = np.zeros((2, N))
+        H[0, IX] = 1.0
+        H[1, IY] = 1.0
+
+        innov = np.array([x_meas - self._x[IX], y_meas - self._x[IY]])
+        S     = H @ self._P @ H.T + R
+        K     = self._P @ H.T @ np.linalg.inv(S)
+
+        self._x = self._x + K @ innov
+        self._x[IT] = _wrap_angle(self._x[IT])
+        self._P = (np.eye(N) - K @ H) @ self._P
+
+        self._uwb_dropout_s = 0.0
+
+    # ── Update heading from magnetometer / other source ───────────────────────
+
+    def update_heading(self, heading_rad: float, sigma_rad: float = 0.1) -> None:
+        """Optional: update θ directly from compass or visual odometry."""
+        if not self._initialised:
+            return
+        H = np.zeros((1, N))
+        H[0, IT] = 1.0
+        innov = np.array([_wrap_angle(heading_rad - self._x[IT])])
+        S     = H @ self._P @ H.T + np.array([[sigma_rad ** 2]])
+        K     = self._P @ H.T @ np.linalg.inv(S)
+        self._x = self._x + K.flatten() * innov[0]
+        self._x[IT] = _wrap_angle(self._x[IT])
+        self._P = (np.eye(N) - K @ H) @ self._P
+
+    # ── Accessors ────────────────────────────────────────────────────────────
+
+    @property
+    def position(self) -> Tuple[float, float]:
+        return float(self._x[IX]), float(self._x[IY])
+
+    @property
+    def heading(self) -> float:
+        return float(self._x[IT])
+
+    @property
+    def velocity(self) -> Tuple[float, float]:
+        return float(self._x[IVX]), float(self._x[IVY])
+
+    @property
+    def yaw_rate(self) -> float:
+        return float(self._x[IW])
+
+    @property
+    def covariance_position(self) -> np.ndarray:
+        """2×2 position covariance sub-matrix."""
+        return self._P[:2, :2].copy()
+
+    @property
+    def covariance_full(self) -> np.ndarray:
+        """Full 6×6 state covariance."""
+        return self._P.copy()
+
+    @property
+    def is_initialised(self) -> bool:
+        return self._initialised
+
+    @property
+    def uwb_dropout_s(self) -> float:
+        """Seconds since last UWB update."""
+        return self._uwb_dropout_s
+
+    def position_uncertainty_m(self) -> float:
+        """Approximate 1-σ position uncertainty (m)."""
+        return float(math.sqrt((self._P[IX, IX] + self._P[IY, IY]) / 2.0))
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _wrap_angle(a: float) -> float:
+    """Wrap angle to [-π, π]."""
+    return float((a + math.pi) % (2 * math.pi) - math.pi)

jetson/ros2_ws/src/saltybot_uwb_imu_fusion/saltybot_uwb_imu_fusion/ekf_node.py

@@ -0,0 +1,268 @@
+"""
+ekf_node.py — ROS2 node: UWB + IMU EKF fusion (Issue #573)
+
+Fuses:
+  /imu/data              (sensor_msgs/Imu,       200 Hz) — predict step
+  /uwb/bearing           (UwbBearing,             10 Hz) — update step (bearing+range → x,y)
+  /saltybot/uwb/pose     (geometry_msgs/PoseStamped, 10 Hz) — update step (absolute position)
+
+Publishes:
+  /saltybot/pose/fused   (geometry_msgs/PoseStamped)        — fused pose at IMU rate
+  /saltybot/pose/fused_cov (geometry_msgs/PoseWithCovarianceStamped) — with covariance
+
+TF2:
+  Broadcasts  base_link → map  from fused pose
+
+UWB dropout:
+  IMU dead-reckoning continues for up to `max_dead_reckoning_s` seconds.
+  After that the velocity estimate is zeroed; position uncertainty grows.
+
+Parameters (see config/fusion_params.yaml):
+  imu_topic            /imu/data
+  uwb_bearing_topic    /uwb/bearing
+  uwb_pose_topic       /saltybot/uwb/pose   (preferred if available)
+  use_uwb_pose         true  — use absolute pose; false = use bearing only
+  publish_tf           true
+  map_frame            map
+  base_frame           base_link
+  sigma_uwb_pos_m      0.12
+  sigma_imu_accel      0.05
+  sigma_imu_gyro       0.003
+  sigma_vel_drift      0.10
+  max_dead_reckoning_s 10.0
+"""
+
+import math
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+
+import tf2_ros
+from geometry_msgs.msg import (
+    PoseStamped, PoseWithCovarianceStamped, TransformStamped
+)
+from sensor_msgs.msg import Imu
+from std_msgs.msg import Header
+
+from saltybot_uwb_msgs.msg import UwbBearing
+from saltybot_uwb_imu_fusion.ekf_math import UwbImuEKF
+
+
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+
+
+class EkfFusionNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__("uwb_imu_fusion")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("imu_topic",             "/imu/data")
+        self.declare_parameter("uwb_bearing_topic",     "/uwb/bearing")
+        self.declare_parameter("uwb_pose_topic",        "/saltybot/uwb/pose")
+        self.declare_parameter("use_uwb_pose",          True)
+        self.declare_parameter("publish_tf",            True)
+        self.declare_parameter("map_frame",             "map")
+        self.declare_parameter("base_frame",            "base_link")
+        self.declare_parameter("sigma_uwb_pos_m",       0.12)
+        self.declare_parameter("sigma_imu_accel",       0.05)
+        self.declare_parameter("sigma_imu_gyro",        0.003)
+        self.declare_parameter("sigma_vel_drift",       0.10)
+        self.declare_parameter("dropout_vel_damping",   0.95)
+        self.declare_parameter("max_dead_reckoning_s",  10.0)
+
+        self._map_frame     = self.get_parameter("map_frame").value
+        self._base_frame    = self.get_parameter("base_frame").value
+        self._publish_tf    = self.get_parameter("publish_tf").value
+        self._use_uwb_pose  = self.get_parameter("use_uwb_pose").value
+        self._max_dr        = self.get_parameter("max_dead_reckoning_s").value
+
+        # ── EKF ───────────────────────────────────────────────────────────────
+        self._ekf = UwbImuEKF(
+            sigma_uwb_pos_m     = self.get_parameter("sigma_uwb_pos_m").value,
+            sigma_imu_accel     = self.get_parameter("sigma_imu_accel").value,
+            sigma_imu_gyro      = self.get_parameter("sigma_imu_gyro").value,
+            sigma_vel_drift     = self.get_parameter("sigma_vel_drift").value,
+            dropout_vel_damping = self.get_parameter("dropout_vel_damping").value,
+        )
+
+        self._last_imu_t: float | None = None
+
+        # ── Publishers ────────────────────────────────────────────────────────
+        self._pose_pub     = self.create_publisher(PoseStamped,                "/saltybot/pose/fused",     10)
+        self._pose_cov_pub = self.create_publisher(PoseWithCovarianceStamped,  "/saltybot/pose/fused_cov", 10)
+
+        if self._publish_tf:
+            self._tf_br = tf2_ros.TransformBroadcaster(self)
+        else:
+            self._tf_br = None
+
+        # ── Subscriptions ─────────────────────────────────────────────────────
+        self.create_subscription(
+            Imu, self.get_parameter("imu_topic").value,
+            self._imu_cb, _SENSOR_QOS
+        )
+
+        if self._use_uwb_pose:
+            self.create_subscription(
+                PoseStamped,
+                self.get_parameter("uwb_pose_topic").value,
+                self._uwb_pose_cb, 10
+            )
+        else:
+            self.create_subscription(
+                UwbBearing,
+                self.get_parameter("uwb_bearing_topic").value,
+                self._uwb_bearing_cb, 10
+            )
+
+        self.get_logger().info(
+            f"EKF fusion ready — "
+            f"IMU:{self.get_parameter('imu_topic').value}  "
+            f"UWB:{'pose' if self._use_uwb_pose else 'bearing'}  "
+            f"tf={self._publish_tf}  "
+            f"dr={self._max_dr}s"
+        )
+
+    # ── IMU callback (200 Hz) — predict step ──────────────────────────────────
+
+    def _imu_cb(self, msg: Imu) -> None:
+        now = self.get_clock().now().nanoseconds * 1e-9
+        if self._last_imu_t is None:
+            self._last_imu_t = now
+            return
+
+        dt = now - self._last_imu_t
+        self._last_imu_t = now
+
+        if dt <= 0.0 or dt > 0.5:
+            return  # stale or implausible
+
+        # Extract IMU data (body frame, ROS convention: x=forward, y=left, z=up)
+        ax = msg.linear_acceleration.x
+        ay = msg.linear_acceleration.y
+        # az = msg.linear_acceleration.z  # gravity along z, ignored for ground robot
+        omega = msg.angular_velocity.z   # yaw rate
+
+        self._ekf.predict(ax_body=ax, ay_body=ay, omega=omega, dt=dt)
+
+        # Publish fused pose at IMU rate if filter is alive
+        if self._ekf.is_initialised:
+            if self._ekf.uwb_dropout_s < self._max_dr:
+                self._publish_pose(msg.header.stamp)
+            else:
+                # Dropout too long — stop publishing, log warning
+                self.get_logger().warn(
+                    f"UWB dropout {self._ekf.uwb_dropout_s:.1f}s — "
+                    "pose unreliable, output suppressed",
+                    throttle_duration_sec=5.0,
+                )
+
+    # ── UWB pose callback (absolute position, 10 Hz) — update step ───────────
+
+    def _uwb_pose_cb(self, msg: PoseStamped) -> None:
+        x = msg.pose.position.x
+        y = msg.pose.position.y
+        self._ekf.update_uwb(x, y)
+
+        if not self._ekf.is_initialised:
+            return
+
+        # Extract heading from quaternion if available
+        q = msg.pose.orientation
+        if abs(q.w) > 0.01:
+            yaw = 2.0 * math.atan2(q.z, q.w)
+            self._ekf.update_heading(yaw, sigma_rad=0.2)
+
+    # ── UWB bearing callback (relative, 10 Hz) — update step ─────────────────
+
+    def _uwb_bearing_cb(self, msg: UwbBearing) -> None:
+        r   = float(msg.range_m)
+        brg = float(msg.bearing_rad)
+        if r < 0.1:
+            return
+        # Convert polar (bearing from base_link) to absolute position estimate
+        # This is relative to robot, so add to current robot position to get world coords.
+        # Note: if we don't have absolute anchors, this stays in base_link frame.
+        # Use it as a position measurement in base_link (x = forward, y = left).
+        x_rel = r * math.cos(brg)
+        y_rel = r * math.sin(brg)
+        # Inflate sigma for single-anchor degraded mode
+        sigma = 0.15 if msg.confidence >= 1.0 else 0.30
+        self._ekf.update_uwb(x_rel, y_rel, sigma_m=sigma)
+
+    # ── Publish fused pose ────────────────────────────────────────────────────
+
+    def _publish_pose(self, stamp) -> None:
+        x, y       = self._ekf.position
+        heading    = self._ekf.heading
+        cov_pos    = self._ekf.covariance_position
+        cov_full   = self._ekf.covariance_full
+
+        half_yaw = heading / 2.0
+        qz = math.sin(half_yaw)
+        qw = math.cos(half_yaw)
+
+        # PoseStamped
+        pose = PoseStamped()
+        pose.header.stamp    = stamp
+        pose.header.frame_id = self._map_frame
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+        pose.pose.position.z = 0.0
+        pose.pose.orientation.z = qz
+        pose.pose.orientation.w = qw
+        self._pose_pub.publish(pose)
+
+        # PoseWithCovarianceStamped
+        pose_cov = PoseWithCovarianceStamped()
+        pose_cov.header = pose.header
+        pose_cov.pose.pose = pose.pose
+        cov36 = [0.0] * 36
+        cov36[0]  = cov_pos[0, 0]   # x,x
+        cov36[1]  = cov_pos[0, 1]   # x,y
+        cov36[6]  = cov_pos[1, 0]   # y,x
+        cov36[7]  = cov_pos[1, 1]   # y,y
+        cov36[14] = 1e-4             # z (not estimated)
+        cov36[21] = 1e-4             # roll
+        cov36[28] = 1e-4             # pitch
+        cov36[35] = float(cov_full[2, 2])  # yaw
+        pose_cov.pose.covariance = cov36
+        self._pose_cov_pub.publish(pose_cov)
+
+        # TF2
+        if self._tf_br is not None:
+            tf = TransformStamped()
+            tf.header.stamp    = stamp
+            tf.header.frame_id = self._map_frame
+            tf.child_frame_id  = self._base_frame
+            tf.transform.translation.x = x
+            tf.transform.translation.y = y
+            tf.transform.translation.z = 0.0
+            tf.transform.rotation.z    = qz
+            tf.transform.rotation.w    = qw
+            self._tf_br.sendTransform(tf)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = EkfFusionNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_uwb_imu_fusion/setup.cfg

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

jetson/ros2_ws/src/saltybot_uwb_imu_fusion/setup.py

@@ -0,0 +1,29 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = "saltybot_uwb_imu_fusion"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages",
+            [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (os.path.join("share", package_name, "launch"), glob("launch/*.py")),
+        (os.path.join("share", package_name, "config"), glob("config/*.yaml")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-uwb",
+    maintainer_email="sl-uwb@saltylab.local",
+    description="EKF UWB+IMU fusion for SaltyBot localization",
+    license="Apache-2.0",
+    entry_points={
+        "console_scripts": [
+            "uwb_imu_fusion = saltybot_uwb_imu_fusion.ekf_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_uwb_imu_fusion/test/test_ekf_math.py

@@ -0,0 +1,152 @@
+"""
+Unit tests for saltybot_uwb_imu_fusion.ekf_math (Issue #573).
+No ROS2 or hardware required.
+"""
+
+import math
+import sys
+import os
+
+import numpy as np
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+from saltybot_uwb_imu_fusion.ekf_math import UwbImuEKF, _wrap_angle
+
+
+class TestWrapAngle:
+    def test_within_range(self):
+        assert abs(_wrap_angle(1.0) - 1.0) < 1e-9
+
+    def test_positive_overflow(self):
+        assert abs(_wrap_angle(math.pi + 0.1) - (-math.pi + 0.1)) < 1e-9
+
+    def test_negative_overflow(self):
+        assert abs(_wrap_angle(-math.pi - 0.1) - (math.pi - 0.1)) < 1e-9
+
+
+class TestEkfInitialise:
+    def test_not_initialised_by_default(self):
+        ekf = UwbImuEKF()
+        assert not ekf.is_initialised
+
+    def test_initialise_sets_position(self):
+        ekf = UwbImuEKF()
+        ekf.initialise(3.0, 4.0, heading_rad=0.5)
+        assert ekf.is_initialised
+        x, y = ekf.position
+        assert abs(x - 3.0) < 1e-9
+        assert abs(y - 4.0) < 1e-9
+        assert abs(ekf.heading - 0.5) < 1e-9
+
+
+class TestEkfPredict:
+    def test_stationary_predict_no_drift(self):
+        """Stationary robot with zero IMU input stays near origin."""
+        ekf = UwbImuEKF(sigma_vel_drift=0.0)
+        ekf.initialise(0.0, 0.0)
+        for _ in range(10):
+            ekf.predict(ax_body=0.0, ay_body=0.0, omega=0.0, dt=0.01)
+        x, y = ekf.position
+        assert abs(x) < 0.01
+        assert abs(y) < 0.01
+
+    def test_forward_acceleration(self):
+        """1 m/s² forward for 0.5 s → ~0.125 m forward displacement."""
+        ekf = UwbImuEKF(sigma_imu_accel=0.0, sigma_vel_drift=0.0)
+        ekf.initialise(0.0, 0.0, heading_rad=0.0)
+        dt = 0.005
+        for _ in range(100):  # 0.5 s
+            ekf.predict(ax_body=1.0, ay_body=0.0, omega=0.0, dt=dt)
+        x, y = ekf.position
+        # x ≈ 0.5 * 1 * 0.5² = 0.125 m
+        assert 0.10 < x < 0.16, f"x={x}"
+        assert abs(y) < 0.01
+
+    def test_yaw_rate(self):
+        """π/4 rad/s for 1 s → heading ≈ π/4."""
+        ekf = UwbImuEKF(sigma_imu_gyro=0.0)
+        ekf.initialise(0.0, 0.0, heading_rad=0.0)
+        dt = 0.005
+        for _ in range(200):
+            ekf.predict(ax_body=0.0, ay_body=0.0, omega=math.pi / 4, dt=dt)
+        assert abs(ekf.heading - math.pi / 4) < 0.05
+
+    def test_covariance_grows_with_time(self):
+        """Covariance must grow during predict (no updates)."""
+        ekf = UwbImuEKF()
+        ekf.initialise(0.0, 0.0)
+        p0 = float(ekf.covariance_position[0, 0])
+        for _ in range(50):
+            ekf.predict(ax_body=0.0, ay_body=0.0, omega=0.0, dt=0.01)
+        p1 = float(ekf.covariance_position[0, 0])
+        assert p1 > p0, f"covariance did not grow: p0={p0}, p1={p1}"
+
+
+class TestEkfUpdate:
+    def test_uwb_update_reduces_covariance(self):
+        """UWB update must reduce position covariance."""
+        ekf = UwbImuEKF()
+        ekf.initialise(0.0, 0.0)
+        # Grow uncertainty first
+        for _ in range(20):
+            ekf.predict(ax_body=0.0, ay_body=0.0, omega=0.0, dt=0.05)
+        p_before = float(ekf.covariance_position[0, 0])
+        ekf.update_uwb(0.1, 0.1)
+        p_after = float(ekf.covariance_position[0, 0])
+        assert p_after < p_before
+
+    def test_uwb_corrects_position(self):
+        """Large position error corrected by UWB measurement."""
+        ekf = UwbImuEKF(sigma_uwb_pos_m=0.1)
+        ekf.initialise(5.0, 5.0)
+        # Multiple UWB updates at true position (0,0)
+        for _ in range(20):
+            ekf.update_uwb(0.0, 0.0)
+        x, y = ekf.position
+        assert abs(x) < 0.5, f"x not corrected: {x}"
+        assert abs(y) < 0.5, f"y not corrected: {y}"
+
+    def test_uninitialised_update_initialises(self):
+        """Calling update_uwb before initialise should initialise filter."""
+        ekf = UwbImuEKF()
+        assert not ekf.is_initialised
+        ekf.update_uwb(2.0, 3.0)
+        assert ekf.is_initialised
+        x, y = ekf.position
+        assert abs(x - 2.0) < 1e-9
+        assert abs(y - 3.0) < 1e-9
+
+
+class TestDropout:
+    def test_velocity_damping_during_dropout(self):
+        """Velocity decays during extended UWB dropout."""
+        ekf = UwbImuEKF(dropout_vel_damping=0.9)
+        ekf.initialise(0.0, 0.0)
+        # Give it some velocity
+        for _ in range(50):
+            ekf.predict(ax_body=2.0, ay_body=0.0, omega=0.0, dt=0.01)
+        vx_before, _ = ekf.velocity
+        # Let dropout accumulate (> 2 s threshold)
+        for _ in range(300):
+            ekf.predict(ax_body=0.0, ay_body=0.0, omega=0.0, dt=0.01)
+        vx_after, _ = ekf.velocity
+        assert abs(vx_after) < abs(vx_before), \
+            f"velocity not damped: before={vx_before:.3f}, after={vx_after:.3f}"
+
+
+class TestCovariance:
+    def test_covariance_positive_definite(self):
+        """Full covariance matrix must be positive definite at all times."""
+        ekf = UwbImuEKF()
+        ekf.initialise(1.0, 2.0)
+        for _ in range(20):
+            ekf.predict(ax_body=0.5, ay_body=0.1, omega=0.05, dt=0.01)
+            if _ % 5 == 0:
+                ekf.update_uwb(1.0, 2.0)
+        eigvals = np.linalg.eigvalsh(ekf.covariance_full)
+        assert all(ev > -1e-9 for ev in eigvals), f"Non-PD covariance: {eigvals}"
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_uwb_position/test/test_uwb_position.py

@@ -0,0 +1,89 @@
+"""
+Unit tests for saltybot_uwb_position.uwb_position_node (Issue #546).
+No ROS2 or hardware required — tests the covariance math only.
+"""
+
+import math
+import sys
+import os
+
+# Make the package importable without a ROS2 install
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+
+
+# ── Covariance helper (extracted from node for unit testing) ──────────────────
+
+def polar_to_cartesian_cov(bearing_rad, range_m, sigma_r, sigma_theta):
+    """Compute 2×2 Cartesian covariance from polar uncertainty."""
+    cos_b = math.cos(bearing_rad)
+    sin_b = math.sin(bearing_rad)
+    j00 =  cos_b;   j01 = -range_m * sin_b
+    j10 =  sin_b;   j11 =  range_m * cos_b
+    sr2 = sigma_r * sigma_r
+    st2 = sigma_theta * sigma_theta
+    cov_xx = j00 * j00 * sr2 + j01 * j01 * st2
+    cov_xy = j00 * j10 * sr2 + j01 * j11 * st2
+    cov_yy = j10 * j10 * sr2 + j11 * j11 * st2
+    return cov_xx, cov_xy, cov_yy
+
+
+# ── Tests ─────────────────────────────────────────────────────────────────────
+
+class TestPolarToCartesianCovariance:
+
+    def test_forward_bearing_zero(self):
+        """At bearing=0 (directly ahead) covariance aligns with axes."""
+        cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
+            bearing_rad=0.0, range_m=5.0, sigma_r=0.10, sigma_theta=0.087
+        )
+        assert cov_xx > 0
+        assert cov_yy > 0
+        # At bearing=0: cov_xx = σ_r², cov_yy = (r·σ_θ)², cov_xy ≈ 0
+        assert abs(cov_xx - 0.10 ** 2) < 1e-9
+        assert abs(cov_xy) < 1e-9
+        expected_yy = (5.0 * 0.087) ** 2
+        assert abs(cov_yy - expected_yy) < 1e-6
+
+    def test_sideways_bearing(self):
+        """At bearing=90° covariance axes swap."""
+        sigma_r = 0.10
+        sigma_theta = 0.10
+        r = 3.0
+        cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
+            bearing_rad=math.pi / 2, range_m=r,
+            sigma_r=sigma_r, sigma_theta=sigma_theta
+        )
+        # At bearing=90°: cov_xx = (r·σ_θ)², cov_yy = σ_r²
+        assert abs(cov_xx - (r * sigma_theta) ** 2) < 1e-9
+        assert abs(cov_yy - sigma_r ** 2) < 1e-9
+
+    def test_covariance_positive_definite(self):
+        """Matrix must be positive semi-definite (det ≥ 0, diag > 0)."""
+        for bearing in [0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0]:
+            for r in [1.0, 5.0, 10.0]:
+                cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov(
+                    bearing, r, sigma_r=0.10, sigma_theta=0.087
+                )
+                assert cov_xx > 0
+                assert cov_yy > 0
+                det = cov_xx * cov_yy - cov_xy ** 2
+                assert det >= -1e-12, f"Non-PSD at bearing={bearing}, r={r}: det={det}"
+
+    def test_inflation_single_anchor(self):
+        """Covariance doubles (variance ×4) when only one anchor active."""
+        sigma_r = 0.10
+        sigma_theta = 0.087
+        bearing = 0.5
+        r = 4.0
+        cov_xx_full, _, _ = polar_to_cartesian_cov(bearing, r, sigma_r, sigma_theta)
+        cov_xx_half, _, _ = polar_to_cartesian_cov(
+            bearing, r,
+            sigma_r * math.sqrt(4.0),
+            sigma_theta * math.sqrt(4.0),
+        )
+        assert abs(cov_xx_half / cov_xx_full - 4.0) < 1e-9
+
+
+if __name__ == "__main__":
+    import pytest
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_visual_odom/config/stereo_orb_params.yaml

@@ -0,0 +1,16 @@
+stereo_orb_vo:
+  ros__parameters:
+    # ORB feature detection
+    max_features: 500          # max keypoints per infra1 frame
+
+    # Stereo scale
+    baseline_m: 0.050          # D435i stereo baseline ~50 mm (overridden by camera_info Tx)
+
+    # Depth validity window for scale recovery
+    min_depth_m: 0.3           # metres — below reliable D435i range
+    max_depth_m: 6.0           # metres — above reliable D435i range
+    min_depth_samples: 10      # minimum valid depth samples for primary scale
+
+    # TF / frame IDs
+    frame_id:       "odom"
+    child_frame_id: "camera_link"

jetson/ros2_ws/src/saltybot_visual_odom/launch/stereo_orb.launch.py

@@ -0,0 +1,27 @@
+"""stereo_orb.launch.py — Launch the ORB stereo visual odometry node (Issue #586)."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('saltybot_visual_odom')
+    params    = os.path.join(pkg_share, 'config', 'stereo_orb_params.yaml')
+
+    stereo_orb_node = Node(
+        package='saltybot_visual_odom',
+        executable='stereo_orb',
+        name='stereo_orb_vo',
+        output='screen',
+        parameters=[params],
+        remappings=[
+            # Remap to the standard RealSense D435i topic names if needed
+            ('/camera/infra1/image_rect_raw', '/camera/infra1/image_rect_raw'),
+            ('/camera/infra2/image_rect_raw', '/camera/infra2/image_rect_raw'),
+            ('/camera/depth/image_rect_raw',  '/camera/depth/image_rect_raw'),
+        ],
+    )
+
+    return LaunchDescription([stereo_orb_node])

jetson/ros2_ws/src/saltybot_visual_odom/saltybot_visual_odom/orb_stereo_matcher.py

@@ -0,0 +1,204 @@
+"""
+orb_stereo_matcher.py — ORB feature detection and descriptor matching for stereo VO.
+
+Provides two match types
+────────────────────────
+  Temporal  : left_gray(t-1) → left_gray(t)   — matched pairs for Essential-matrix estimation
+  Stereo    : left_gray(t)   ↔ right_gray(t)  — disparity-based metric depth (scale source)
+
+Matching
+────────
+  BFMatcher(NORM_HAMMING) with Lowe ratio test (default ratio=0.75).
+  Stereo matching additionally enforces the epipolar row constraint (|Δrow| ≤ row_tol px)
+  and requires positive disparity (left.x > right.x, i.e. object in front of cameras).
+
+ORB parameters
+──────────────
+  nfeatures  : max keypoints per frame (default 500)
+  scale_factor: image pyramid scale (default 1.2)
+  nlevels    : pyramid levels (default 8)
+
+Scale recovery (stereo)
+───────────────────────
+  For matched infra1/infra2 pairs with disparity d > 0:
+      depth_i = baseline_m * fx / d_i
+  Returns median over valid [d_min_px, d_max_px] disparity range.
+  Returns 0.0 when fewer than min_stereo_samples good pairs exist.
+"""
+
+from __future__ import annotations
+
+from typing import Tuple
+
+import numpy as np
+import cv2
+
+
+# ── Matching parameters ────────────────────────────────────────────────────────
+_LOWE_RATIO      = 0.75    # Lowe ratio test threshold
+_MIN_MATCHES     = 20      # minimum temporal matches for VO
+_ROW_TOL         = 2       # ±px for stereo epipolar row constraint
+_DISP_MIN        = 1.0     # minimum valid disparity (pixels)
+_DISP_MAX        = 192.0   # maximum valid disparity (D435i 640-px baseline, ~0.15m min depth)
+_MIN_STEREO_SAMP = 10      # minimum stereo samples for reliable scale
+
+
+class OrbStereoMatcher:
+    """
+    ORB-based feature tracker for monocular temporal matching and stereo scale.
+
+    Usage (per frame)::
+
+        matcher = OrbStereoMatcher()
+        # --- temporal ---
+        prev_pts, curr_pts = matcher.update(curr_gray_left)
+        # --- stereo scale (optional, called same frame) ---
+        depth_m = matcher.stereo_scale(curr_gray_left, curr_gray_right, fx, baseline_m)
+    """
+
+    def __init__(
+        self,
+        nfeatures:    int   = 500,
+        scale_factor: float = 1.2,
+        nlevels:      int   = 8,
+    ) -> None:
+        self._orb = cv2.ORB_create(
+            nfeatures=nfeatures,
+            scaleFactor=scale_factor,
+            nLevels=nlevels,
+            edgeThreshold=15,
+            patchSize=31,
+        )
+        self._matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
+
+        # Previous-frame state
+        self._prev_gray:  np.ndarray | None = None
+        self._prev_kp:    list | None        = None
+        self._prev_desc:  np.ndarray | None  = None
+
+    # ── Temporal matching ──────────────────────────────────────────────────────
+
+    def update(
+        self,
+        curr_gray: np.ndarray,
+    ) -> Tuple[np.ndarray, np.ndarray]:
+        """
+        Detect ORB features in curr_gray, match against previous frame.
+
+        Returns
+        -------
+        (prev_pts, curr_pts) : (N, 2) float32 arrays of matched pixel coords.
+        Returns empty (0, 2) arrays on first call or when matching fails.
+        """
+        kp, desc = self._orb.detectAndCompute(curr_gray, None)
+
+        if (
+            desc is None
+            or len(kp) < 8
+            or self._prev_desc is None
+            or self._prev_kp is None
+        ):
+            # First frame — just store state
+            self._prev_gray = curr_gray.copy()
+            self._prev_kp   = kp
+            self._prev_desc = desc
+            return np.zeros((0, 2), np.float32), np.zeros((0, 2), np.float32)
+
+        # Ratio test matching
+        prev_pts, curr_pts = self._ratio_match(
+            self._prev_kp, self._prev_desc,
+            kp, desc,
+        )
+
+        # Update state
+        self._prev_gray = curr_gray.copy()
+        self._prev_kp   = kp
+        self._prev_desc = desc
+
+        return prev_pts, curr_pts
+
+    # ── Stereo scale ───────────────────────────────────────────────────────────
+
+    def stereo_scale(
+        self,
+        left_gray:  np.ndarray,
+        right_gray: np.ndarray,
+        fx:         float,
+        baseline_m: float,
+    ) -> float:
+        """
+        Compute median depth (metres) from stereo ORB disparity.
+
+        Matches ORB features between left and right images under the epipolar
+        row constraint, then: depth_i = baseline_m * fx / disparity_i.
+
+        Returns 0.0 on failure (< min_stereo_samples valid pairs).
+        """
+        if baseline_m <= 0 or fx <= 0:
+            return 0.0
+
+        kp_l, desc_l = self._orb.detectAndCompute(left_gray,  None)
+        kp_r, desc_r = self._orb.detectAndCompute(right_gray, None)
+
+        if desc_l is None or desc_r is None:
+            return 0.0
+
+        pts_l, pts_r = self._ratio_match(kp_l, desc_l, kp_r, desc_r)
+
+        if len(pts_l) == 0:
+            return 0.0
+
+        # Epipolar row constraint: |row_l - row_r| ≤ _ROW_TOL
+        row_diff = np.abs(pts_l[:, 1] - pts_r[:, 1])
+        mask_row = row_diff <= _ROW_TOL
+
+        # Positive disparity (left camera has larger x for points in front)
+        disp = pts_l[:, 0] - pts_r[:, 0]
+        mask_disp = (disp >= _DISP_MIN) & (disp <= _DISP_MAX)
+
+        valid = mask_row & mask_disp
+        if valid.sum() < _MIN_STEREO_SAMP:
+            return 0.0
+
+        depths = baseline_m * fx / disp[valid]
+        return float(np.median(depths))
+
+    # ── Reset ──────────────────────────────────────────────────────────────────
+
+    def reset(self) -> None:
+        self._prev_gray  = None
+        self._prev_kp    = None
+        self._prev_desc  = None
+
+    # ── Internal helpers ───────────────────────────────────────────────────────
+
+    def _ratio_match(
+        self,
+        kp1:   list,
+        desc1: np.ndarray,
+        kp2:   list,
+        desc2: np.ndarray,
+    ) -> Tuple[np.ndarray, np.ndarray]:
+        """BFMatcher kNN (k=2) + Lowe ratio test → (pts1, pts2) float32."""
+        try:
+            matches = self._matcher.knnMatch(desc1, desc2, k=2)
+        except cv2.error:
+            return np.zeros((0, 2), np.float32), np.zeros((0, 2), np.float32)
+
+        good = []
+        for pair in matches:
+            if len(pair) == 2:
+                m, n = pair
+                if m.distance < _LOWE_RATIO * n.distance:
+                    good.append(m)
+
+        if len(good) < _MIN_MATCHES:
+            return np.zeros((0, 2), np.float32), np.zeros((0, 2), np.float32)
+
+        pts1 = np.array(
+            [kp1[m.queryIdx].pt for m in good], dtype=np.float32
+        ).reshape(-1, 2)
+        pts2 = np.array(
+            [kp2[m.trainIdx].pt for m in good], dtype=np.float32
+        ).reshape(-1, 2)
+        return pts1, pts2

jetson/ros2_ws/src/saltybot_visual_odom/saltybot_visual_odom/stereo_orb_node.py

@@ -0,0 +1,317 @@
+"""
+stereo_orb_node.py — D435i stereo ORB visual odometry node (Issue #586).
+
+Pipeline per frame
+──────────────────
+1. Receive synchronized infra1 (left IR) + infra2 (right IR) + aligned depth.
+2. Detect ORB features on infra1; match temporally (prev→curr) via BFMatcher
+   + Lowe ratio test.
+3. Estimate incremental SE(3) via Essential-matrix (5-point RANSAC) using the
+   temporal ORB correspondences.
+4. Recover metric scale:
+     Primary   — median depth at matched infra1 points (D435i aligned depth).
+     Fallback  — stereo baseline disparity: depth = baseline_m * fx / disparity
+                 (from ORB matches between infra1 and infra2 of the same frame).
+5. Accumulate global SE(3) pose, publish nav_msgs/Odometry.
+6. Broadcast TF2: odom → camera_link.
+
+Subscribes
+──────────
+  /camera/infra1/image_rect_raw   sensor_msgs/Image      30 Hz (left IR, mono8)
+  /camera/infra2/image_rect_raw   sensor_msgs/Image      30 Hz (right IR, mono8)
+  /camera/depth/image_rect_raw    sensor_msgs/Image      30 Hz float32 depth (m)
+  /camera/infra1/camera_info      sensor_msgs/CameraInfo (intrinsics + baseline)
+  /camera/infra2/camera_info      sensor_msgs/CameraInfo (Tx field gives baseline)
+
+Publishes
+─────────
+  /saltybot/visual_odom           nav_msgs/Odometry      up to 30 Hz
+
+TF
+──
+  odom → camera_link
+
+Parameters
+──────────
+  max_features        int    500      ORB max keypoints per frame
+  baseline_m          float  0.050    stereo baseline (metres); overridden by
+                                      camera_info Tx if non-zero
+  frame_id            str   'odom'
+  child_frame_id      str   'camera_link'
+  min_depth_m         float  0.3      depth validity lower bound
+  max_depth_m         float  6.0      depth validity upper bound
+  min_depth_samples   int    10       min valid depth samples for primary scale
+"""
+
+from __future__ import annotations
+
+import math
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+
+import message_filters
+import numpy as np
+import cv2
+from cv_bridge import CvBridge
+
+from sensor_msgs.msg import Image, CameraInfo
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Point, Quaternion, TransformStamped
+from tf2_ros import TransformBroadcaster
+
+from .orb_stereo_matcher import OrbStereoMatcher
+from .stereo_vo import StereoVO
+
+
+# ── QoS profiles ──────────────────────────────────────────────────────────────
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+_LATCHED_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+)
+
+# ── Depth validity ─────────────────────────────────────────────────────────────
+_D_MIN_DEFAULT = 0.3   # metres
+_D_MAX_DEFAULT = 6.0   # metres
+
+
+def _yaw_to_quat(yaw: float) -> Quaternion:
+    q = Quaternion()
+    q.w = math.cos(yaw * 0.5)
+    q.z = math.sin(yaw * 0.5)
+    return q
+
+
+def _rot_to_yaw(R: np.ndarray) -> float:
+    return math.atan2(float(R[1, 0]), float(R[0, 0]))
+
+
+class StereoOrbNode(Node):
+    """ORB stereo visual odometry — see module docstring for details."""
+
+    def __init__(self) -> None:
+        super().__init__('stereo_orb_vo')
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter('max_features',      500)
+        self.declare_parameter('baseline_m',        0.050)
+        self.declare_parameter('frame_id',         'odom')
+        self.declare_parameter('child_frame_id',   'camera_link')
+        self.declare_parameter('min_depth_m',       _D_MIN_DEFAULT)
+        self.declare_parameter('max_depth_m',       _D_MAX_DEFAULT)
+        self.declare_parameter('min_depth_samples', 10)
+
+        max_feat        = self.get_parameter('max_features').value
+        self._baseline  = self.get_parameter('baseline_m').value
+        self._frame_id  = self.get_parameter('frame_id').value
+        self._child_id  = self.get_parameter('child_frame_id').value
+        self._d_min     = self.get_parameter('min_depth_m').value
+        self._d_max     = self.get_parameter('max_depth_m').value
+        self._min_samp  = self.get_parameter('min_depth_samples').value
+
+        # ── Core objects ───────────────────────────────────────────────────────
+        self._bridge  = CvBridge()
+        self._matcher = OrbStereoMatcher(nfeatures=max_feat)
+        self._vo      = StereoVO()
+
+        self._K:    np.ndarray | None = None
+        self._fx:   float             = 0.0
+        self._last_t: float | None    = None
+
+        # ── Publishers ─────────────────────────────────────────────────────────
+        self._odom_pub = self.create_publisher(
+            Odometry, '/saltybot/visual_odom', 10
+        )
+        self._tf_br = TransformBroadcaster(self)
+
+        # ── Camera-info subscriptions (latched) ────────────────────────────────
+        self.create_subscription(
+            CameraInfo, '/camera/infra1/camera_info',
+            self._on_infra1_info, _LATCHED_QOS,
+        )
+        self.create_subscription(
+            CameraInfo, '/camera/infra2/camera_info',
+            self._on_infra2_info, _LATCHED_QOS,
+        )
+
+        # ── Synchronised image subscriptions ──────────────────────────────────
+        ir1_sub   = message_filters.Subscriber(
+            self, Image, '/camera/infra1/image_rect_raw', qos_profile=_SENSOR_QOS
+        )
+        ir2_sub   = message_filters.Subscriber(
+            self, Image, '/camera/infra2/image_rect_raw', qos_profile=_SENSOR_QOS
+        )
+        depth_sub = message_filters.Subscriber(
+            self, Image, '/camera/depth/image_rect_raw', qos_profile=_SENSOR_QOS
+        )
+        self._sync = message_filters.ApproximateTimeSynchronizer(
+            [ir1_sub, ir2_sub, depth_sub],
+            queue_size=5,
+            slop=0.033,   # 1 frame at 30 Hz
+        )
+        self._sync.registerCallback(self._on_frame)
+
+        self.get_logger().info(
+            f'stereo_orb_vo ready — ORB nfeatures={max_feat}, '
+            f'baseline={self._baseline:.3f}m'
+        )
+
+    # ── Camera info callbacks ──────────────────────────────────────────────────
+
+    def _on_infra1_info(self, msg: CameraInfo) -> None:
+        if self._K is not None:
+            return
+        K = np.array(msg.k, dtype=np.float64).reshape(3, 3)
+        self._K  = K
+        self._fx = float(K[0, 0])
+        self._vo.set_K(K)
+        self.get_logger().info(f'camera_info received — fx={self._fx:.1f}')
+
+    def _on_infra2_info(self, msg: CameraInfo) -> None:
+        # Tx = -fx * baseline; positive baseline when Tx < 0
+        # RealSense camera_info uses P[3] = Tx = -fx * baseline_m
+        if msg.p[3] != 0.0 and self._fx > 0.0:
+            baseline_from_info = abs(float(msg.p[3]) / self._fx)
+            if baseline_from_info > 0.01:   # sanity: > 1 cm
+                self._baseline = baseline_from_info
+                self.get_logger().info(
+                    f'stereo baseline from camera_info: {self._baseline*1000:.1f} mm'
+                )
+
+    # ── Main frame callback ────────────────────────────────────────────────────
+
+    def _on_frame(
+        self,
+        ir1_msg:   Image,
+        ir2_msg:   Image,
+        depth_msg: Image,
+    ) -> None:
+        now  = self.get_clock().now()
+        t_s  = now.nanoseconds * 1e-9
+        dt   = (t_s - self._last_t) if self._last_t is not None else (1.0 / 30.0)
+        dt   = max(1e-4, min(dt, 0.5))
+        self._last_t = t_s
+
+        # ── Decode images ──────────────────────────────────────────────────────
+        try:
+            ir1   = self._bridge.imgmsg_to_cv2(ir1_msg,   desired_encoding='mono8')
+            ir2   = self._bridge.imgmsg_to_cv2(ir2_msg,   desired_encoding='mono8')
+            depth = self._bridge.imgmsg_to_cv2(depth_msg, desired_encoding='32FC1')
+        except Exception as exc:
+            self.get_logger().warning(f'decode error: {exc}')
+            return
+
+        # ── Temporal ORB match (infra1: prev→curr) ─────────────────────────────
+        prev_pts, curr_pts = self._matcher.update(ir1)
+
+        if len(prev_pts) < 8:
+            # First frame or too few matches — nothing to estimate
+            return
+
+        # ── Scale recovery ─────────────────────────────────────────────────────
+        # Primary: depth image at matched infra1 feature points
+        scale = self._depth_scale(depth, curr_pts)
+
+        # Fallback: stereo disparity between infra1 and infra2
+        if scale <= 0.0 and self._baseline > 0.0 and self._fx > 0.0:
+            scale = self._matcher.stereo_scale(ir1, ir2, self._fx, self._baseline)
+
+        # ── Motion estimation via StereoVO (Essential matrix + SE(3)) ──────────
+        est = self._vo.update(prev_pts, curr_pts, depth, dt=dt)
+
+        if not est.valid:
+            # Still publish last known pose with high covariance
+            pass
+
+        # ── Publish odometry ───────────────────────────────────────────────────
+        odom = self._build_odom(est, now.to_msg())
+        self._odom_pub.publish(odom)
+
+        # ── Broadcast TF2: odom → camera_link ─────────────────────────────────
+        self._broadcast_tf(est, now)
+
+    # ── Scale from depth image ─────────────────────────────────────────────────
+
+    def _depth_scale(self, depth: np.ndarray, pts: np.ndarray) -> float:
+        """Median depth at feature pixel locations (metres).  Returns 0 on failure."""
+        if len(pts) == 0:
+            return 0.0
+        h, w = depth.shape
+        u = np.clip(pts[:, 0].astype(np.int32), 0, w - 1)
+        v = np.clip(pts[:, 1].astype(np.int32), 0, h - 1)
+        samples = depth[v, u]
+        valid   = samples[(samples > self._d_min) & (samples < self._d_max)]
+        if len(valid) < self._min_samp:
+            return 0.0
+        return float(np.median(valid))
+
+    # ── Message builders ───────────────────────────────────────────────────────
+
+    def _build_odom(self, est, stamp) -> Odometry:
+        q = _yaw_to_quat(est.yaw)
+
+        odom                    = Odometry()
+        odom.header.stamp       = stamp
+        odom.header.frame_id    = self._frame_id
+        odom.child_frame_id     = self._child_id
+
+        odom.pose.pose.position    = Point(x=est.x, y=est.y, z=est.z)
+        odom.pose.pose.orientation = q
+
+        cov_xy    = 0.05 if est.valid else 0.5
+        cov_theta = 0.02 if est.valid else 0.2
+        cov_v     = 0.1  if est.valid else 1.0
+        p_cov     = [0.0] * 36
+        p_cov[0]  = cov_xy
+        p_cov[7]  = cov_xy
+        p_cov[35] = cov_theta
+        odom.pose.covariance = p_cov
+
+        odom.twist.twist.linear.x  = est.vx
+        odom.twist.twist.angular.z = est.omega
+        t_cov     = [0.0] * 36
+        t_cov[0]  = cov_v
+        t_cov[35] = cov_v * 0.5
+        odom.twist.covariance = t_cov
+
+        return odom
+
+    def _broadcast_tf(self, est, stamp) -> None:
+        q = _yaw_to_quat(est.yaw)
+
+        tf             = TransformStamped()
+        tf.header.stamp    = stamp.to_msg()
+        tf.header.frame_id = self._frame_id
+        tf.child_frame_id  = self._child_id
+
+        tf.transform.translation.x = est.x
+        tf.transform.translation.y = est.y
+        tf.transform.translation.z = est.z
+        tf.transform.rotation      = q
+
+        self._tf_br.sendTransform(tf)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = StereoOrbNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_visual_odom/setup.py

@@ -1,3 +1,5 @@
+import os
+from glob import glob
 from setuptools import setup, find_packages
 
 package_name = 'saltybot_visual_odom'
@@ -10,6 +12,8 @@ setup(
         ('share/ament_index/resource_index/packages',
          [f'resource/{package_name}']),
         (f'share/{package_name}', ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -21,6 +25,7 @@ setup(
         'console_scripts': [
             'visual_odom  = saltybot_visual_odom.visual_odom_node:main',
             'odom_fusion  = saltybot_visual_odom.odom_fusion_node:main',
+            'stereo_orb   = saltybot_visual_odom.stereo_orb_node:main',
         ],
     },
 )

phone/sensor_dashboard.py

@@ -0,0 +1,404 @@
+#!/usr/bin/env python3
+"""
+sensor_dashboard.py — Termux phone sensor publisher for SaltyBot (Issue #574)
+
+Reads phone IMU, GPS, and battery via termux-api and publishes JSON payloads
+to the SaltyBot MQTT broker.  Designed to run on Android/Termux as a
+persistent background service.
+
+Topics
+──────
+  saltybot/phone/imu      — accelerometer + gyroscope  @ 5 Hz
+  saltybot/phone/gps      — lat/lon/alt/accuracy        @ 1 Hz
+  saltybot/phone/battery  — percentage/charging/temp    @ 1 Hz
+
+JSON payloads
+─────────────
+  IMU:
+    {"ts": 1710000000.000,
+     "accel": {"x": 0.12, "y": -0.05, "z": 9.81},
+     "gyro":  {"x": 0.01, "y": -0.00, "z": 0.00}}
+
+  GPS:
+    {"ts": 1710000000.000,
+     "lat": 45.123, "lon": -73.456, "alt_m": 12.3,
+     "accuracy_m": 3.5, "speed_ms": 0.0, "bearing_deg": 0.0,
+     "provider": "gps"}
+
+  Battery:
+    {"ts": 1710000000.000,
+     "pct": 87, "charging": true, "temp_c": 28.5,
+     "health": "GOOD", "plugged": "AC"}
+
+Usage
+─────
+  python3 phone/sensor_dashboard.py [OPTIONS]
+
+  --broker HOST     MQTT broker IP/hostname  (default: 192.168.1.100)
+  --port PORT       MQTT broker port         (default: 1883)
+  --imu-hz FLOAT    IMU publish rate Hz      (default: 5.0)
+  --gps-hz FLOAT    GPS publish rate Hz      (default: 1.0)
+  --bat-hz FLOAT    Battery publish rate Hz  (default: 1.0)
+  --qos INT         MQTT QoS level 0/1/2     (default: 0)
+  --no-imu          Disable IMU publishing
+  --no-gps          Disable GPS publishing
+  --no-battery      Disable battery publishing
+  --debug           Verbose logging
+
+Dependencies (install in Termux)
+─────────────────────────────────
+  pkg install termux-api python
+  pip install paho-mqtt
+"""
+
+import argparse
+import json
+import logging
+import subprocess
+import sys
+import threading
+import time
+from typing import Optional
+
+try:
+    import paho.mqtt.client as mqtt
+    MQTT_AVAILABLE = True
+except ImportError:
+    MQTT_AVAILABLE = False
+
+# ── MQTT topic roots ──────────────────────────────────────────────────────────
+
+TOPIC_IMU     = "saltybot/phone/imu"
+TOPIC_GPS     = "saltybot/phone/gps"
+TOPIC_BATTERY = "saltybot/phone/battery"
+
+# ── termux-api wrappers ───────────────────────────────────────────────────────
+
+def _run_termux(cmd: list[str], timeout: float = 5.0) -> Optional[dict]:
+    """Run a termux-api command and parse its JSON output. Returns None on error."""
+    try:
+        result = subprocess.run(
+            cmd,
+            capture_output=True,
+            text=True,
+            timeout=timeout,
+        )
+        if result.returncode != 0 or not result.stdout.strip():
+            logging.debug("termux cmd %s returned %d: %s",
+                          cmd[0], result.returncode, result.stderr.strip())
+            return None
+        return json.loads(result.stdout)
+    except (subprocess.TimeoutExpired, json.JSONDecodeError, FileNotFoundError) as e:
+        logging.debug("termux cmd %s error: %s", cmd[0], e)
+        return None
+
+
+def read_sensor_once(sensor_name: str) -> Optional[dict]:
+    """
+    Call termux-sensor for a single reading of @sensor_name.
+
+    termux-sensor -s <name> -n 1 returns one sample then exits.
+    """
+    raw = _run_termux(["termux-sensor", "-s", sensor_name, "-n", "1"], timeout=4.0)
+    if raw is None:
+        return None
+    # Response shape: {"<sensor_name>": {"values": [x, y, z], ...}}
+    # Key may be the full sensor description string; iterate to find it.
+    for key, val in raw.items():
+        if isinstance(val, dict) and "values" in val:
+            return val
+    return None
+
+
+def read_imu() -> Optional[dict]:
+    """
+    Read accelerometer and gyroscope in a single call each.
+    Returns dict with 'accel' and 'gyro' sub-dicts, or None if both fail.
+    """
+    accel_data = read_sensor_once("accelerometer")
+    gyro_data  = read_sensor_once("gyroscope")
+
+    accel = None
+    if accel_data and isinstance(accel_data.get("values"), list):
+        v = accel_data["values"]
+        if len(v) >= 3:
+            accel = {"x": float(v[0]), "y": float(v[1]), "z": float(v[2])}
+
+    gyro = None
+    if gyro_data and isinstance(gyro_data.get("values"), list):
+        v = gyro_data["values"]
+        if len(v) >= 3:
+            gyro = {"x": float(v[0]), "y": float(v[1]), "z": float(v[2])}
+
+    if accel is None and gyro is None:
+        return None
+
+    return {
+        "ts":    time.time(),
+        "accel": accel or {"x": 0.0, "y": 0.0, "z": 0.0},
+        "gyro":  gyro  or {"x": 0.0, "y": 0.0, "z": 0.0},
+    }
+
+
+def read_gps() -> Optional[dict]:
+    """Read GPS fix via termux-location."""
+    raw = _run_termux(["termux-location", "-p", "gps", "-r", "once"], timeout=10.0)
+    if raw is None:
+        # Fallback: try network provider if GPS cold
+        raw = _run_termux(
+            ["termux-location", "-p", "network", "-r", "once"], timeout=6.0
+        )
+    if raw is None:
+        return None
+
+    return {
+        "ts":          time.time(),
+        "lat":         float(raw.get("latitude",  0.0)),
+        "lon":         float(raw.get("longitude", 0.0)),
+        "alt_m":       float(raw.get("altitude",  0.0)),
+        "accuracy_m":  float(raw.get("accuracy",  -1.0)),
+        "speed_ms":    float(raw.get("speed",     0.0)),
+        "bearing_deg": float(raw.get("bearing",   0.0)),
+        "provider":    str(raw.get("provider", "unknown")),
+    }
+
+
+def read_battery() -> Optional[dict]:
+    """Read battery status via termux-battery-status."""
+    raw = _run_termux(["termux-battery-status"], timeout=4.0)
+    if raw is None:
+        return None
+
+    return {
+        "ts":      time.time(),
+        "pct":     int(raw.get("percentage", -1)),
+        "charging": raw.get("status", "DISCHARGING") not in ("DISCHARGING", "UNKNOWN"),
+        "temp_c":  float(raw.get("temperature", 0.0)),
+        "health":  str(raw.get("health", "UNKNOWN")),
+        "plugged": str(raw.get("plugged", "UNPLUGGED")),
+    }
+
+
+# ── MQTT client with auto-reconnect ───────────────────────────────────────────
+
+class MQTTPublisher:
+    """
+    Thin paho-mqtt wrapper with:
+    - Automatic reconnect on disconnect (exponential back-off, max 60 s)
+    - Thread-safe publish() — queues messages if offline
+    - Connection status accessible via .connected property
+    """
+
+    _RECONNECT_BASE  = 2.0   # seconds
+    _RECONNECT_MAX   = 60.0
+
+    def __init__(self, broker: str, port: int, qos: int = 0):
+        self._broker  = broker
+        self._port    = port
+        self._qos     = qos
+        self._lock    = threading.Lock()
+        self._connected = False
+        self._reconnect_delay = self._RECONNECT_BASE
+
+        self._client = mqtt.Client(client_id="saltybot-phone-sensor", clean_session=True)
+        self._client.on_connect    = self._on_connect
+        self._client.on_disconnect = self._on_disconnect
+        self._client.reconnect_delay_set(
+            min_delay=int(self._RECONNECT_BASE),
+            max_delay=int(self._RECONNECT_MAX),
+        )
+
+        self._connect()
+
+    def _connect(self) -> None:
+        try:
+            self._client.connect_async(self._broker, self._port, keepalive=60)
+            self._client.loop_start()
+            logging.info("MQTT connecting to %s:%d ...", self._broker, self._port)
+        except Exception as e:
+            logging.warning("MQTT initial connect error: %s", e)
+
+    def _on_connect(self, client, userdata, flags, rc) -> None:
+        if rc == 0:
+            with self._lock:
+                self._connected = True
+                self._reconnect_delay = self._RECONNECT_BASE
+            logging.info("MQTT connected to %s:%d", self._broker, self._port)
+        else:
+            logging.warning("MQTT connect failed rc=%d", rc)
+
+    def _on_disconnect(self, client, userdata, rc) -> None:
+        with self._lock:
+            self._connected = False
+        if rc != 0:
+            logging.warning("MQTT unexpected disconnect (rc=%d) — paho will retry", rc)
+
+    @property
+    def connected(self) -> bool:
+        with self._lock:
+            return self._connected
+
+    def publish(self, topic: str, payload: dict) -> bool:
+        """Serialize @payload to JSON and publish to @topic. Returns True on success."""
+        if not self.connected:
+            logging.debug("MQTT offline — dropping %s", topic)
+            return False
+        try:
+            msg = json.dumps(payload, separators=(",", ":"))
+            info = self._client.publish(topic, msg, qos=self._qos, retain=False)
+            return info.rc == mqtt.MQTT_ERR_SUCCESS
+        except Exception as e:
+            logging.warning("MQTT publish error on %s: %s", topic, e)
+            return False
+
+    def shutdown(self) -> None:
+        self._client.loop_stop()
+        self._client.disconnect()
+        logging.info("MQTT disconnected.")
+
+
+# ── Sensor polling threads ────────────────────────────────────────────────────
+
+class SensorPoller(threading.Thread):
+    """
+    Polls a sensor function at a fixed rate and publishes to MQTT.
+    Runs as a daemon thread so it exits cleanly when the main thread stops.
+    """
+
+    def __init__(self, name: str, read_fn, topic: str,
+                 hz: float, publisher: MQTTPublisher):
+        super().__init__(name=name, daemon=True)
+        self._read_fn  = read_fn
+        self._topic    = topic
+        self._interval = 1.0 / hz
+        self._pub      = publisher
+        self._running  = False
+
+        # Stats
+        self.published  = 0
+        self.errors     = 0
+
+    def run(self) -> None:
+        self._running = True
+        logging.info("Poller '%s' started at %.1f Hz → %s",
+                     self.name, 1.0 / self._interval, self._topic)
+        while self._running:
+            t0 = time.monotonic()
+            try:
+                data = self._read_fn()
+                if data is not None:
+                    if self._pub.publish(self._topic, data):
+                        self.published += 1
+                        logging.debug("%s: published %s", self.name,
+                                      list(data.keys()))
+                    else:
+                        self.errors += 1
+                else:
+                    self.errors += 1
+                    logging.debug("%s: read returned None", self.name)
+            except Exception as e:
+                self.errors += 1
+                logging.warning("%s: read error: %s", self.name, e)
+
+            elapsed = time.monotonic() - t0
+            sleep_s = max(0.0, self._interval - elapsed)
+            time.sleep(sleep_s)
+
+    def stop(self) -> None:
+        self._running = False
+
+
+# ── Status logger ─────────────────────────────────────────────────────────────
+
+def _status_logger(pollers: list[SensorPoller], publisher: MQTTPublisher,
+                   stop_event: threading.Event) -> None:
+    """Log per-poller stats every 30 s."""
+    while not stop_event.wait(30.0):
+        parts = [f"MQTT={'OK' if publisher.connected else 'DOWN'}"]
+        for p in pollers:
+            parts.append(f"{p.name}={p.published}ok/{p.errors}err")
+        logging.info("Status — %s", "  ".join(parts))
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="SaltyBot Termux sensor dashboard (Issue #574)"
+    )
+    parser.add_argument("--broker",     default="192.168.1.100",
+                        help="MQTT broker IP/hostname (default: 192.168.1.100)")
+    parser.add_argument("--port",       type=int, default=1883,
+                        help="MQTT broker port (default: 1883)")
+    parser.add_argument("--imu-hz",     type=float, default=5.0,
+                        help="IMU publish rate Hz (default: 5.0)")
+    parser.add_argument("--gps-hz",     type=float, default=1.0,
+                        help="GPS publish rate Hz (default: 1.0)")
+    parser.add_argument("--bat-hz",     type=float, default=1.0,
+                        help="Battery publish rate Hz (default: 1.0)")
+    parser.add_argument("--qos",        type=int, default=0, choices=[0, 1, 2],
+                        help="MQTT QoS level (default: 0)")
+    parser.add_argument("--no-imu",     action="store_true", help="Disable IMU")
+    parser.add_argument("--no-gps",     action="store_true", help="Disable GPS")
+    parser.add_argument("--no-battery", action="store_true", help="Disable battery")
+    parser.add_argument("--debug",      action="store_true", help="Verbose logging")
+    args = parser.parse_args()
+
+    logging.basicConfig(
+        level=logging.DEBUG if args.debug else logging.INFO,
+        format="%(asctime)s [%(levelname)s] %(message)s",
+    )
+
+    if not MQTT_AVAILABLE:
+        logging.error("paho-mqtt not installed.  Run: pip install paho-mqtt")
+        sys.exit(1)
+
+    if args.no_imu and args.no_gps and args.no_battery:
+        logging.error("All sensors disabled — nothing to do.")
+        sys.exit(1)
+
+    # Connect MQTT
+    publisher = MQTTPublisher(args.broker, args.port, qos=args.qos)
+
+    # Wait briefly for initial connection before starting pollers
+    deadline = time.monotonic() + 10.0
+    while not publisher.connected and time.monotonic() < deadline:
+        time.sleep(0.2)
+    if not publisher.connected:
+        logging.warning("MQTT not yet connected — pollers will start anyway and retry.")
+
+    # Build pollers for enabled sensors
+    pollers: list[SensorPoller] = []
+    if not args.no_imu:
+        pollers.append(SensorPoller("imu",     read_imu,     TOPIC_IMU,     args.imu_hz, publisher))
+    if not args.no_gps:
+        pollers.append(SensorPoller("gps",     read_gps,     TOPIC_GPS,     args.gps_hz, publisher))
+    if not args.no_battery:
+        pollers.append(SensorPoller("battery", read_battery, TOPIC_BATTERY, args.bat_hz, publisher))
+
+    for p in pollers:
+        p.start()
+
+    # Status log thread
+    stop_evt = threading.Event()
+    status_thread = threading.Thread(
+        target=_status_logger, args=(pollers, publisher, stop_evt), daemon=True
+    )
+    status_thread.start()
+
+    logging.info("Sensor dashboard running — Ctrl-C to stop.")
+    try:
+        while True:
+            time.sleep(1.0)
+    except KeyboardInterrupt:
+        logging.info("Shutting down...")
+    finally:
+        stop_evt.set()
+        for p in pollers:
+            p.stop()
+        publisher.shutdown()
+        logging.info("Done.")
+
+
+if __name__ == "__main__":
+    main()

phone/video_bridge.py

@@ -0,0 +1,477 @@
+#!/usr/bin/env python3
+"""
+video_bridge.py — Phone camera MJPEG streaming server for SaltyBot (Issue #585)
+
+Captures frames from the Android camera and serves them over:
+  1. WebSocket (binary JPEG frames) on ws://0.0.0.0:<ws-port>
+  2. HTTP MJPEG stream (fallback)     on http://0.0.0.0:<http-port>/stream
+  3. HTTP JPEG snapshot               on http://0.0.0.0:<http-port>/snapshot
+
+Target: 640×480 @ 15 fps, < 200 ms phone→Jetson latency.
+
+Camera backends (tried in order)
+─────────────────────────────────
+  1. OpenCV VideoCapture (/dev/video0 or --device) — fastest, needs V4L2
+  2. termux-camera-photo capture loop              — universal on Termux
+     Uses the front or rear camera via termux-api.
+
+WebSocket frame format
+───────────────────────
+  Binary message: raw JPEG bytes.
+  Text message   : JSON control frame:
+    {"type":"info","width":640,"height":480,"fps":15,"ts":1234567890.0}
+    {"type":"error","msg":"..."}
+
+Usage
+─────
+  python3 phone/video_bridge.py [OPTIONS]
+
+  --ws-port   PORT  WebSocket port           (default: 8765)
+  --http-port PORT  HTTP MJPEG port          (default: 8766)
+  --width     INT   Frame width px           (default: 640)
+  --height    INT   Frame height px          (default: 480)
+  --fps       FLOAT Target capture FPS       (default: 15.0)
+  --quality   INT   JPEG quality 1-100       (default: 75)
+  --device    PATH  V4L2 device or camera id (default: /dev/video0)
+  --camera-id INT   termux-camera-photo id   (default: 0  = rear)
+  --no-http         Disable HTTP server
+  --debug           Verbose logging
+
+Dependencies (Termux)
+──────────────────────
+  pkg install termux-api python opencv-python
+  pip install websockets
+"""
+
+import argparse
+import asyncio
+import io
+import json
+import logging
+import os
+import subprocess
+import sys
+import threading
+import time
+from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
+from typing import Optional
+
+try:
+    import cv2
+    CV2_AVAILABLE = True
+except ImportError:
+    CV2_AVAILABLE = False
+
+try:
+    import websockets
+    WS_AVAILABLE = True
+except ImportError:
+    WS_AVAILABLE = False
+
+# ── Frame buffer (shared between capture, WS, HTTP) ──────────────────────────
+
+class FrameBuffer:
+    """Thread-safe single-slot frame buffer — always holds the latest JPEG."""
+
+    def __init__(self):
+        self._lock  = threading.Lock()
+        self._frame: Optional[bytes] = None
+        self._event = threading.Event()
+        self.count   = 0
+        self.dropped = 0
+
+    def put(self, jpeg_bytes: bytes) -> None:
+        with self._lock:
+            if self._frame is not None:
+                self.dropped += 1
+            self._frame = jpeg_bytes
+            self.count += 1
+        self._event.set()
+
+    def get(self, timeout: float = 1.0) -> Optional[bytes]:
+        """Block until a new frame is available or timeout."""
+        if self._event.wait(timeout):
+            self._event.clear()
+            with self._lock:
+                return self._frame
+        return None
+
+    def latest(self) -> Optional[bytes]:
+        """Return latest frame without blocking (may return None)."""
+        with self._lock:
+            return self._frame
+
+
+# ── Camera backends ───────────────────────────────────────────────────────────
+
+class OpenCVCapture:
+    """Capture frames via OpenCV VideoCapture (V4L2 on Android/Linux)."""
+
+    def __init__(self, device: str, width: int, height: int, fps: float, quality: int):
+        self._device  = device
+        self._width   = width
+        self._height  = height
+        self._fps     = fps
+        self._quality = quality
+        self._cap     = None
+        self._encode_params = [cv2.IMWRITE_JPEG_QUALITY, quality]
+
+    def open(self) -> bool:
+        try:
+            idx = int(self._device) if self._device.isdigit() else self._device
+            cap = cv2.VideoCapture(idx)
+            if not cap.isOpened():
+                return False
+            cap.set(cv2.CAP_PROP_FRAME_WIDTH,  self._width)
+            cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self._height)
+            cap.set(cv2.CAP_PROP_FPS,          self._fps)
+            self._cap = cap
+            return True
+        except Exception as e:
+            logging.debug("OpenCV open failed: %s", e)
+            return False
+
+    def read_jpeg(self) -> Optional[bytes]:
+        if self._cap is None:
+            return None
+        ret, frame = self._cap.read()
+        if not ret or frame is None:
+            return None
+        ok, buf = cv2.imencode(".jpg", frame, self._encode_params)
+        if not ok:
+            return None
+        return bytes(buf)
+
+    def close(self) -> None:
+        if self._cap is not None:
+            self._cap.release()
+            self._cap = None
+
+
+class TermuxCapture:
+    """Capture frames via termux-camera-photo (works on unmodified Android)."""
+
+    def __init__(self, camera_id: int, width: int, height: int, quality: int):
+        self._camera_id = camera_id
+        self._quality   = quality
+        self._tmpfile   = f"/data/data/com.termux/files/home/.cache/vcam_{os.getpid()}.jpg"
+        # Resize params for cv2 (termux-camera-photo outputs native resolution)
+        self._target_w = width
+        self._target_h = height
+        os.makedirs(os.path.dirname(self._tmpfile), exist_ok=True)
+
+    def open(self) -> bool:
+        # Test one capture
+        return self._capture_raw() is not None
+
+    def read_jpeg(self) -> Optional[bytes]:
+        raw = self._capture_raw()
+        if raw is None:
+            return None
+        if CV2_AVAILABLE:
+            return self._resize_jpeg(raw)
+        return raw
+
+    def _capture_raw(self) -> Optional[bytes]:
+        try:
+            r = subprocess.run(
+                ["termux-camera-photo", "-c", str(self._camera_id), self._tmpfile],
+                capture_output=True, timeout=3.0,
+            )
+            if r.returncode != 0 or not os.path.exists(self._tmpfile):
+                return None
+            with open(self._tmpfile, "rb") as f:
+                data = f.read()
+            os.unlink(self._tmpfile)
+            return data
+        except Exception as e:
+            logging.debug("termux-camera-photo error: %s", e)
+            return None
+
+    def _resize_jpeg(self, raw: bytes) -> Optional[bytes]:
+        try:
+            import numpy as np
+            buf = np.frombuffer(raw, dtype=np.uint8)
+            img = cv2.imdecode(buf, cv2.IMREAD_COLOR)
+            if img is None:
+                return raw
+            resized = cv2.resize(img, (self._target_w, self._target_h))
+            ok, enc = cv2.imencode(".jpg", resized,
+                                   [cv2.IMWRITE_JPEG_QUALITY, self._quality])
+            return bytes(enc) if ok else raw
+        except Exception:
+            return raw
+
+    def close(self) -> None:
+        if os.path.exists(self._tmpfile):
+            try:
+                os.unlink(self._tmpfile)
+            except OSError:
+                pass
+
+
+# ── Capture thread ────────────────────────────────────────────────────────────
+
+class CaptureThread(threading.Thread):
+    """Drives a camera backend at the target FPS, pushing JPEG into FrameBuffer."""
+
+    def __init__(self, backend, fps: float, buf: FrameBuffer):
+        super().__init__(name="capture", daemon=True)
+        self._backend  = backend
+        self._interval = 1.0 / fps
+        self._buf      = buf
+        self._running  = False
+
+    def run(self) -> None:
+        self._running = True
+        logging.info("Capture thread started (%.1f Hz)", 1.0 / self._interval)
+        while self._running:
+            t0 = time.monotonic()
+            try:
+                jpeg = self._backend.read_jpeg()
+                if jpeg:
+                    self._buf.put(jpeg)
+            except Exception as e:
+                logging.warning("Capture error: %s", e)
+            elapsed = time.monotonic() - t0
+            time.sleep(max(0.0, self._interval - elapsed))
+
+    def stop(self) -> None:
+        self._running = False
+
+
+# ── HTTP MJPEG server ─────────────────────────────────────────────────────────
+
+MJPEG_BOUNDARY = b"--mjpeg-boundary"
+
+def _make_http_handler(buf: FrameBuffer, width: int, height: int, fps: float):
+
+    class Handler(BaseHTTPRequestHandler):
+        def log_message(self, fmt, *args):
+            logging.debug("HTTP %s", fmt % args)
+
+        def do_GET(self):
+            if self.path == "/stream":
+                self._stream()
+            elif self.path == "/snapshot":
+                self._snapshot()
+            elif self.path == "/health":
+                self._health()
+            else:
+                self.send_error(404)
+
+        def _stream(self):
+            self.send_response(200)
+            self.send_header("Content-Type",
+                             f"multipart/x-mixed-replace; boundary={MJPEG_BOUNDARY.decode()}")
+            self.send_header("Cache-Control", "no-cache")
+            self.send_header("Connection", "close")
+            self.end_headers()
+            try:
+                while True:
+                    jpeg = buf.get(timeout=2.0)
+                    if jpeg is None:
+                        continue
+                    ts = time.time()
+                    header = (
+                        f"\r\n{MJPEG_BOUNDARY.decode()}\r\n"
+                        f"Content-Type: image/jpeg\r\n"
+                        f"Content-Length: {len(jpeg)}\r\n"
+                        f"X-Timestamp: {ts:.3f}\r\n\r\n"
+                    ).encode()
+                    self.wfile.write(header + jpeg)
+                    self.wfile.flush()
+            except (BrokenPipeError, ConnectionResetError):
+                pass
+
+        def _snapshot(self):
+            jpeg = buf.latest()
+            if jpeg is None:
+                self.send_error(503, "No frame available")
+                return
+            self.send_response(200)
+            self.send_header("Content-Type", "image/jpeg")
+            self.send_header("Content-Length", str(len(jpeg)))
+            self.send_header("X-Timestamp", str(time.time()))
+            self.end_headers()
+            self.wfile.write(jpeg)
+
+        def _health(self):
+            body = json.dumps({
+                "status": "ok",
+                "frames": buf.count,
+                "dropped": buf.dropped,
+                "width": width,
+                "height": height,
+                "fps": fps,
+            }).encode()
+            self.send_response(200)
+            self.send_header("Content-Type", "application/json")
+            self.send_header("Content-Length", str(len(body)))
+            self.end_headers()
+            self.wfile.write(body)
+
+    return Handler
+
+
+# ── WebSocket server ──────────────────────────────────────────────────────────
+
+async def ws_handler(websocket, buf: FrameBuffer, width: int, height: int, fps: float):
+    """Handle one WebSocket client connection — streams JPEG frames as binary messages."""
+    remote = websocket.remote_address
+    logging.info("WS client connected: %s", remote)
+
+    # Send info frame
+    info = json.dumps({
+        "type": "info",
+        "width": width,
+        "height": height,
+        "fps": fps,
+        "ts": time.time(),
+    })
+    try:
+        await websocket.send(info)
+    except Exception:
+        return
+
+    loop = asyncio.get_event_loop()
+    try:
+        while True:
+            # Offload blocking buf.get() to thread pool to keep event loop free
+            jpeg = await loop.run_in_executor(None, lambda: buf.get(timeout=1.0))
+            if jpeg is None:
+                continue
+            await websocket.send(jpeg)
+    except websockets.exceptions.ConnectionClosedOK:
+        logging.info("WS client disconnected (clean): %s", remote)
+    except websockets.exceptions.ConnectionClosedError as e:
+        logging.info("WS client disconnected (error): %s — %s", remote, e)
+    except Exception as e:
+        logging.warning("WS handler error: %s", e)
+
+
+# ── Statistics logger ─────────────────────────────────────────────────────────
+
+def _stats_logger(buf: FrameBuffer, stop_evt: threading.Event) -> None:
+    prev = 0
+    while not stop_evt.wait(10.0):
+        delta = buf.count - prev
+        prev  = buf.count
+        logging.info("Capture stats — frames=%d (+%d/10s) dropped=%d",
+                     buf.count, delta, buf.dropped)
+
+
+# ── Entrypoint ────────────────────────────────────────────────────────────────
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="SaltyBot phone video bridge (Issue #585)"
+    )
+    parser.add_argument("--ws-port",   type=int,   default=8765,
+                        help="WebSocket server port (default: 8765)")
+    parser.add_argument("--http-port", type=int,   default=8766,
+                        help="HTTP MJPEG port (default: 8766)")
+    parser.add_argument("--width",     type=int,   default=640,
+                        help="Frame width (default: 640)")
+    parser.add_argument("--height",    type=int,   default=480,
+                        help="Frame height (default: 480)")
+    parser.add_argument("--fps",       type=float, default=15.0,
+                        help="Target FPS (default: 15)")
+    parser.add_argument("--quality",   type=int,   default=75,
+                        help="JPEG quality 1-100 (default: 75)")
+    parser.add_argument("--device",    default="/dev/video0",
+                        help="V4L2 device or index (default: /dev/video0)")
+    parser.add_argument("--camera-id", type=int,   default=0,
+                        help="termux-camera-photo camera id (default: 0 = rear)")
+    parser.add_argument("--no-http",   action="store_true",
+                        help="Disable HTTP server")
+    parser.add_argument("--debug",     action="store_true",
+                        help="Verbose logging")
+    args = parser.parse_args()
+
+    logging.basicConfig(
+        level=logging.DEBUG if args.debug else logging.INFO,
+        format="%(asctime)s [%(levelname)s] %(message)s",
+    )
+
+    if not WS_AVAILABLE:
+        logging.error("websockets not installed. Run: pip install websockets")
+        sys.exit(1)
+
+    # ── Select and open camera backend ───────────────────────────────────────
+    backend = None
+    if CV2_AVAILABLE:
+        cv_backend = OpenCVCapture(args.device, args.width, args.height,
+                                   args.fps, args.quality)
+        if cv_backend.open():
+            logging.info("Using OpenCV backend (%s)", args.device)
+            backend = cv_backend
+        else:
+            logging.info("OpenCV backend unavailable, falling back to termux-camera-photo")
+
+    if backend is None:
+        tx_backend = TermuxCapture(args.camera_id, args.width, args.height, args.quality)
+        if tx_backend.open():
+            logging.info("Using termux-camera-photo backend (camera %d)", args.camera_id)
+            backend = tx_backend
+        else:
+            logging.error("No camera backend available. "
+                          "Install opencv-python or termux-api.")
+            sys.exit(1)
+
+    # ── Frame buffer ─────────────────────────────────────────────────────────
+    buf = FrameBuffer()
+
+    # ── Capture thread ────────────────────────────────────────────────────────
+    capture = CaptureThread(backend, args.fps, buf)
+    capture.start()
+
+    # ── HTTP server thread ────────────────────────────────────────────────────
+    stop_evt = threading.Event()
+    if not args.no_http:
+        handler_cls = _make_http_handler(buf, args.width, args.height, args.fps)
+        http_server = ThreadingHTTPServer(("0.0.0.0", args.http_port), handler_cls)
+        http_thread = threading.Thread(
+            target=http_server.serve_forever, name="http", daemon=True
+        )
+        http_thread.start()
+        logging.info("HTTP MJPEG server: http://0.0.0.0:%d/stream", args.http_port)
+        logging.info("HTTP snapshot:     http://0.0.0.0:%d/snapshot", args.http_port)
+        logging.info("HTTP health:       http://0.0.0.0:%d/health", args.http_port)
+
+    # ── Stats logger ──────────────────────────────────────────────────────────
+    stats_thread = threading.Thread(
+        target=_stats_logger, args=(buf, stop_evt), name="stats", daemon=True
+    )
+    stats_thread.start()
+
+    # ── WebSocket server (runs the event loop) ────────────────────────────────
+    logging.info("WebSocket server: ws://0.0.0.0:%d", args.ws_port)
+
+    async def _run_ws():
+        async with websockets.serve(
+            lambda ws: ws_handler(ws, buf, args.width, args.height, args.fps),
+            "0.0.0.0",
+            args.ws_port,
+            max_size=None,          # no message size limit
+            ping_interval=5,
+            ping_timeout=10,
+        ):
+            logging.info("Ready — streaming %dx%d @ %.0f fps",
+                         args.width, args.height, args.fps)
+            await asyncio.Future()  # run forever
+
+    try:
+        asyncio.run(_run_ws())
+    except KeyboardInterrupt:
+        logging.info("Shutting down...")
+    finally:
+        stop_evt.set()
+        capture.stop()
+        backend.close()
+        if not args.no_http:
+            http_server.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/jlink.c

@@ -486,6 +486,31 @@ void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state)
     jlink_tx_locked(frame, sizeof(frame));
 }
 
+/* ---- jlink_send_motor_current_tlm() -- Issue #584 ---- */
+void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm)
+{
+    /*
+     * Frame: [STX][LEN][0x86][8 bytes MOTOR_CURRENT][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 (CMD) + 8 (payload) = 9; total frame = 14 bytes.
+     * At 921600 baud: 14x10/921600 ~0.15 ms -- safe to block.
+     */
+    static uint8_t frame[14];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_motor_current_t);  /* 8 */
+    const uint8_t  len  = 1u + plen;                                    /* 9 */
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_MOTOR_CURRENT;
+    memcpy(&frame[3], tlm, plen);
+
+    uint16_t crc = crc16_xmodem(&frame[2], len);
+    frame[3 + plen]     = (uint8_t)(crc >> 8);
+    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
+    frame[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame, sizeof(frame));
+}
+
 /* ---- jlink_send_sched_telemetry() -- Issue #550 ---- */
 void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm)
 {

src/motor_current.c

@@ -0,0 +1,183 @@
+/*
+ * motor_current.c — ADC-based motor current monitoring and overload protection
+ * (Issue #584).
+ *
+ * Reads battery discharge current from battery_adc_get_current_ma() (ADC3 IN13,
+ * PC3), which is already DMA-sampled by battery_adc.c.  Implements:
+ *
+ *   1. Soft current limiting: linear PWM reduction when current exceeds
+ *      MOTOR_CURR_SOFT_MA (4 A, 80% of hard threshold).
+ *
+ *   2. Hard cutoff: if current stays above MOTOR_CURR_HARD_MA (5 A) for
+ *      MOTOR_CURR_OVERLOAD_MS (2 s), output is zeroed.  A fault event is
+ *      signalled via motor_current_fault_pending() for one tick so the main
+ *      loop can append a fault log entry.
+ *
+ *   3. Auto-recovery: after MOTOR_CURR_COOLDOWN_MS (10 s) in MC_COOLDOWN,
+ *      state returns to MC_NORMAL and normal PWM authority is restored.
+ *
+ *   4. Telemetry: JLINK_TLM_MOTOR_CURRENT (0x86) published at
+ *      MOTOR_CURR_TLM_HZ (5 Hz) via jlink_send_motor_current_tlm().
+ */
+
+#include "motor_current.h"
+#include "battery_adc.h"
+#include "jlink.h"
+#include <stddef.h>
+
+/* ---- Module state ---- */
+static MotorCurrentState s_state            = MC_NORMAL;
+static int32_t           s_current_ma       = 0;
+static uint32_t          s_overload_start   = 0;  /* ms when current first ≥ HARD_MA */
+static uint32_t          s_cooldown_start   = 0;  /* ms when cooldown began */
+static uint8_t           s_fault_count      = 0;  /* lifetime trip counter */
+static uint8_t           s_fault_pending    = 0;  /* cleared after one read */
+static uint32_t          s_last_tlm_ms      = 0;  /* rate-limit TLM TX */
+
+/* Soft-limit scale factor in 0..256 fixed-point (256 = 1.0) */
+static uint16_t          s_scale256         = 256u;
+
+/* ---- motor_current_init() ---- */
+void motor_current_init(void)
+{
+    s_state          = MC_NORMAL;
+    s_current_ma     = 0;
+    s_overload_start = 0;
+    s_cooldown_start = 0;
+    s_fault_count    = 0;
+    s_fault_pending  = 0;
+    s_last_tlm_ms    = 0;
+    s_scale256       = 256u;
+}
+
+/* ---- motor_current_tick() ---- */
+void motor_current_tick(uint32_t now_ms)
+{
+    /* Snapshot current from battery ADC (mA, positive = discharge) */
+    s_current_ma = battery_adc_get_current_ma();
+
+    /* Use absolute value: protect in both forward and regen braking */
+    int32_t abs_ma = s_current_ma;
+    if (abs_ma < 0) abs_ma = -abs_ma;
+
+    switch (s_state) {
+
+    case MC_NORMAL:
+        s_scale256 = 256u;
+        if (abs_ma >= (int32_t)MOTOR_CURR_SOFT_MA) {
+            s_state = MC_SOFT_LIMIT;
+            /* Track overload onset if already above hard threshold */
+            s_overload_start = (abs_ma >= (int32_t)MOTOR_CURR_HARD_MA)
+                               ? now_ms : 0u;
+        }
+        break;
+
+    case MC_SOFT_LIMIT:
+        if (abs_ma < (int32_t)MOTOR_CURR_SOFT_MA) {
+            /* Recovered below soft threshold */
+            s_state          = MC_NORMAL;
+            s_overload_start = 0u;
+            s_scale256       = 256u;
+        } else {
+            /* Compute linear scale: 256 at SOFT_MA, 0 at HARD_MA */
+            int32_t range = (int32_t)MOTOR_CURR_HARD_MA
+                            - (int32_t)MOTOR_CURR_SOFT_MA;
+            int32_t over  = abs_ma - (int32_t)MOTOR_CURR_SOFT_MA;
+            if (over >= range) {
+                s_scale256 = 0u;
+            } else {
+                /* scale256 = (range - over) * 256 / range */
+                s_scale256 = (uint16_t)(((range - over) * 256u) / range);
+            }
+
+            /* Track sustained hard-threshold overload */
+            if (abs_ma >= (int32_t)MOTOR_CURR_HARD_MA) {
+                if (s_overload_start == 0u) {
+                    s_overload_start = now_ms;
+                } else if ((now_ms - s_overload_start) >= MOTOR_CURR_OVERLOAD_MS) {
+                    /* Hard cutoff — trip the fault */
+                    if (s_fault_count < 255u) s_fault_count++;
+                    s_fault_pending  = 1u;
+                    s_cooldown_start = now_ms;
+                    s_overload_start = 0u;
+                    s_scale256       = 0u;
+                    s_state          = MC_COOLDOWN;
+                }
+            } else {
+                /* Current dipped back below HARD_MA — reset overload timer */
+                s_overload_start = 0u;
+            }
+        }
+        break;
+
+    case MC_COOLDOWN:
+        s_scale256 = 0u;
+        if ((now_ms - s_cooldown_start) >= MOTOR_CURR_COOLDOWN_MS) {
+            s_state    = MC_NORMAL;
+            s_scale256 = 256u;
+        }
+        break;
+    }
+}
+
+/* ---- motor_current_apply_limit() ---- */
+int16_t motor_current_apply_limit(int16_t cmd)
+{
+    if (s_scale256 >= 256u) return cmd;
+    if (s_scale256 == 0u)   return 0;
+    return (int16_t)(((int32_t)cmd * s_scale256) / 256);
+}
+
+/* ---- Accessors ---- */
+bool motor_current_is_faulted(void)
+{
+    return s_state == MC_COOLDOWN;
+}
+
+MotorCurrentState motor_current_state(void)
+{
+    return s_state;
+}
+
+int32_t motor_current_ma(void)
+{
+    return s_current_ma;
+}
+
+uint8_t motor_current_fault_count(void)
+{
+    return s_fault_count;
+}
+
+bool motor_current_fault_pending(void)
+{
+    if (!s_fault_pending) return false;
+    s_fault_pending = 0u;
+    return true;
+}
+
+/* ---- motor_current_send_tlm() ---- */
+void motor_current_send_tlm(uint32_t now_ms)
+{
+    if (MOTOR_CURR_TLM_HZ == 0u) return;
+
+    uint32_t interval_ms = 1000u / MOTOR_CURR_TLM_HZ;
+    if ((now_ms - s_last_tlm_ms) < interval_ms) return;
+    s_last_tlm_ms = now_ms;
+
+    jlink_tlm_motor_current_t tlm;
+
+    tlm.current_ma  = s_current_ma;
+
+    /* limit_pct: 0 = no limiting, 100 = full cutoff */
+    if (s_scale256 >= 256u) {
+        tlm.limit_pct = 0u;
+    } else {
+        tlm.limit_pct = (uint8_t)(((256u - s_scale256) * 100u) / 256u);
+    }
+
+    tlm.state       = (uint8_t)s_state;
+    tlm.fault_count = s_fault_count;
+
+    jlink_send_motor_current_tlm(&tlm);
+}

src/pid_flash.c

@@ -70,3 +70,107 @@ bool pid_flash_save(float kp, float ki, float kd)
             stored->ki == ki &&
             stored->kd == kd);
 }
+
+/* ---- Helper: write arbitrary bytes as 32-bit words ---- */
+/*
+ * Writes 'len' bytes from 'src' to flash at 'addr'.
+ * len must be a multiple of 4.  Flash must already be unlocked.
+ * Returns HAL_OK on success, or first failure status.
+ */
+static HAL_StatusTypeDef flash_write_words(uint32_t addr,
+                                           const void *src,
+                                           uint32_t len)
+{
+    const uint32_t *p = (const uint32_t *)src;
+    HAL_StatusTypeDef rc = HAL_OK;
+    for (uint32_t i = 0; i < len / 4u; i++) {
+        rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, p[i]);
+        if (rc != HAL_OK) return rc;
+        addr += 4u;
+    }
+    return HAL_OK;
+}
+
+/* ---- pid_flash_load_schedule() ---- */
+bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n)
+{
+    const pid_sched_flash_t *p = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR;
+
+    if (p->magic != PID_SCHED_MAGIC) return false;
+    if (p->num_bands == 0u || p->num_bands > PID_SCHED_MAX_BANDS) return false;
+
+    *out_n = p->num_bands;
+    for (uint8_t i = 0; i < p->num_bands; i++) {
+        out_entries[i] = p->bands[i];
+    }
+    return true;
+}
+
+/* ---- pid_flash_save_all() ---- */
+bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
+                        const pid_sched_entry_t *entries, uint8_t num_bands)
+{
+    if (num_bands == 0u || num_bands > PID_SCHED_MAX_BANDS) return false;
+
+    HAL_StatusTypeDef rc;
+
+    rc = HAL_FLASH_Unlock();
+    if (rc != HAL_OK) return false;
+
+    /* Single erase of sector 7 covers both records */
+    FLASH_EraseInitTypeDef erase = {
+        .TypeErase    = FLASH_TYPEERASE_SECTORS,
+        .Sector       = PID_FLASH_SECTOR,
+        .NbSectors    = 1,
+        .VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
+    };
+    uint32_t sector_error = 0;
+    rc = HAL_FLASHEx_Erase(&erase, &sector_error);
+    if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
+        HAL_FLASH_Lock();
+        return false;
+    }
+
+    /* Build and write schedule record at PID_SCHED_FLASH_ADDR */
+    pid_sched_flash_t srec;
+    memset(&srec, 0xFF, sizeof(srec));
+    srec.magic     = PID_SCHED_MAGIC;
+    srec.num_bands = num_bands;
+    srec.flags     = 0u;
+    for (uint8_t i = 0; i < num_bands; i++) {
+        srec.bands[i] = entries[i];
+    }
+
+    rc = flash_write_words(PID_SCHED_FLASH_ADDR, &srec, sizeof(srec));
+    if (rc != HAL_OK) {
+        HAL_FLASH_Lock();
+        return false;
+    }
+
+    /* Build and write single-PID record at PID_FLASH_STORE_ADDR */
+    pid_flash_t prec;
+    memset(&prec, 0xFF, sizeof(prec));
+    prec.magic = PID_FLASH_MAGIC;
+    prec.kp    = kp_single;
+    prec.ki    = ki_single;
+    prec.kd    = kd_single;
+
+    rc = flash_write_words(PID_FLASH_STORE_ADDR, &prec, sizeof(prec));
+    if (rc != HAL_OK) {
+        HAL_FLASH_Lock();
+        return false;
+    }
+
+    HAL_FLASH_Lock();
+
+    /* Verify both records */
+    const pid_sched_flash_t *sv = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR;
+    const pid_flash_t       *pv = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
+
+    return (sv->magic == PID_SCHED_MAGIC &&
+            sv->num_bands == num_bands &&
+            pv->magic == PID_FLASH_MAGIC &&
+            pv->kp == kp_single &&
+            pv->ki == ki_single &&
+            pv->kd == kd_single);
+}

src/pid_schedule.c

@@ -0,0 +1,174 @@
+#include "pid_schedule.h"
+#include "pid_flash.h"
+#include <string.h>
+#include <math.h>   /* fabsf */
+
+/* ---- Default 3-band table ---- */
+static const pid_sched_entry_t k_default_table[3] = {
+    { .speed_mps = 0.00f, .kp = 40.0f, .ki = 1.5f, .kd = 1.2f },
+    { .speed_mps = 0.30f, .kp = 35.0f, .ki = 1.0f, .kd = 1.0f },
+    { .speed_mps = 0.80f, .kp = 28.0f, .ki = 0.5f, .kd = 0.8f },
+};
+
+/* ---- Active table ---- */
+static pid_sched_entry_t s_bands[PID_SCHED_MAX_BANDS];
+static uint8_t           s_num_bands    = 0u;
+static uint8_t           s_active_band  = 0u;   /* lower-bracket index of last call */
+static uint8_t           s_prev_band    = 0xFFu; /* sentinel: forces integrator reset on first apply */
+
+/* ---- sort helper (insertion sort — table is small, ≤6 entries) ---- */
+static void sort_bands(void)
+{
+    for (uint8_t i = 1u; i < s_num_bands; i++) {
+        pid_sched_entry_t key = s_bands[i];
+        int8_t j = (int8_t)(i - 1u);
+        while (j >= 0 && s_bands[j].speed_mps > key.speed_mps) {
+            s_bands[j + 1] = s_bands[j];
+            j--;
+        }
+        s_bands[j + 1] = key;
+    }
+}
+
+/* ---- pid_schedule_init() ---- */
+void pid_schedule_init(void)
+{
+    pid_sched_entry_t tmp[PID_SCHED_MAX_BANDS];
+    uint8_t           n = 0u;
+
+    if (pid_flash_load_schedule(tmp, &n)) {
+        /* Validate entries minimally */
+        bool ok = true;
+        for (uint8_t i = 0u; i < n; i++) {
+            if (tmp[i].kp < 0.0f || tmp[i].kp > 500.0f ||
+                tmp[i].ki < 0.0f || tmp[i].ki > 50.0f  ||
+                tmp[i].kd < 0.0f || tmp[i].kd > 50.0f  ||
+                tmp[i].speed_mps < 0.0f) {
+                ok = false;
+                break;
+            }
+        }
+        if (ok) {
+            memcpy(s_bands, tmp, n * sizeof(pid_sched_entry_t));
+            s_num_bands = n;
+            sort_bands();
+            s_active_band = 0u;
+            return;
+        }
+    }
+
+    /* Fall back to built-in default */
+    memcpy(s_bands, k_default_table, sizeof(k_default_table));
+    s_num_bands   = 3u;
+    s_active_band = 0u;
+    s_prev_band   = 0xFFu;
+}
+
+/* ---- pid_schedule_get_gains() ---- */
+void pid_schedule_get_gains(float speed_mps, float *kp, float *ki, float *kd)
+{
+    float spd = fabsf(speed_mps);
+
+    if (s_num_bands == 0u) {
+        *kp = k_default_table[0].kp;
+        *ki = k_default_table[0].ki;
+        *kd = k_default_table[0].kd;
+        return;
+    }
+
+    /* Clamp below first entry */
+    if (spd <= s_bands[0].speed_mps) {
+        s_active_band = 0u;
+        *kp = s_bands[0].kp;
+        *ki = s_bands[0].ki;
+        *kd = s_bands[0].kd;
+        return;
+    }
+
+    /* Clamp above last entry */
+    uint8_t last = s_num_bands - 1u;
+    if (spd >= s_bands[last].speed_mps) {
+        s_active_band = last;
+        *kp = s_bands[last].kp;
+        *ki = s_bands[last].ki;
+        *kd = s_bands[last].kd;
+        return;
+    }
+
+    /* Find bracket [i-1, i] where bands[i-1].speed <= spd < bands[i].speed */
+    uint8_t i = 1u;
+    while (i < s_num_bands && s_bands[i].speed_mps <= spd) i++;
+    /* Now bands[i-1].speed_mps <= spd < bands[i].speed_mps */
+
+    s_active_band = (uint8_t)(i - 1u);
+
+    float dv = s_bands[i].speed_mps - s_bands[i - 1u].speed_mps;
+    float t  = (dv > 0.0f) ? (spd - s_bands[i - 1u].speed_mps) / dv : 0.0f;
+
+    *kp = s_bands[i - 1u].kp + t * (s_bands[i].kp - s_bands[i - 1u].kp);
+    *ki = s_bands[i - 1u].ki + t * (s_bands[i].ki - s_bands[i - 1u].ki);
+    *kd = s_bands[i - 1u].kd + t * (s_bands[i].kd - s_bands[i - 1u].kd);
+}
+
+/* ---- pid_schedule_apply() ---- */
+void pid_schedule_apply(balance_t *b, float speed_mps)
+{
+    float kp, ki, kd;
+    pid_schedule_get_gains(speed_mps, &kp, &ki, &kd);
+
+    b->kp = kp;
+    b->ki = ki;
+    b->kd = kd;
+
+    /* Reset integrator on band transition to prevent windup spike */
+    if (s_active_band != s_prev_band) {
+        b->integral   = 0.0f;
+        s_prev_band   = s_active_band;
+    }
+}
+
+/* ---- pid_schedule_set_table() ---- */
+void pid_schedule_set_table(const pid_sched_entry_t *entries, uint8_t n)
+{
+    if (n == 0u) n = 1u;
+    if (n > PID_SCHED_MAX_BANDS) n = PID_SCHED_MAX_BANDS;
+
+    memcpy(s_bands, entries, n * sizeof(pid_sched_entry_t));
+    s_num_bands   = n;
+    s_active_band = 0u;
+    s_prev_band   = 0xFFu;
+    sort_bands();
+}
+
+/* ---- pid_schedule_get_table() ---- */
+void pid_schedule_get_table(pid_sched_entry_t *out_entries, uint8_t *out_n)
+{
+    memcpy(out_entries, s_bands, s_num_bands * sizeof(pid_sched_entry_t));
+    *out_n = s_num_bands;
+}
+
+/* ---- pid_schedule_get_num_bands() ---- */
+uint8_t pid_schedule_get_num_bands(void)
+{
+    return s_num_bands;
+}
+
+/* ---- pid_schedule_flash_save() ---- */
+bool pid_schedule_flash_save(float kp_single, float ki_single, float kd_single)
+{
+    return pid_flash_save_all(kp_single, ki_single, kd_single,
+                              s_bands, s_num_bands);
+}
+
+/* ---- pid_schedule_active_band_idx() ---- */
+uint8_t pid_schedule_active_band_idx(void)
+{
+    return s_active_band;
+}
+
+/* ---- pid_schedule_get_default_table() ---- */
+void pid_schedule_get_default_table(pid_sched_entry_t *out_entries, uint8_t *out_n)
+{
+    memcpy(out_entries, k_default_table, sizeof(k_default_table));
+    *out_n = 3u;
+}

test/test_pid_schedule.c

@@ -0,0 +1,441 @@
+/*
+ * test_pid_schedule.c -- host-side unit tests for pid_schedule (Issue #550)
+ *
+ * Build:
+ *   gcc -I /tmp/stub_hal -I include -DTEST_HOST -lm \
+ *       -o test_pid_schedule test/test_pid_schedule.c
+ *
+ * Run:
+ *   ./test_pid_schedule
+ */
+
+/* ---- Minimal HAL stub (no hardware) ---- */
+#ifndef STM32F7XX_HAL_H
+#define STM32F7XX_HAL_H
+#include <stdint.h>
+#include <stdbool.h>
+typedef enum { HAL_OK = 0 } HAL_StatusTypeDef;
+typedef struct { uint32_t TypeErase; uint32_t Sector; uint32_t NbSectors; uint32_t VoltageRange; } FLASH_EraseInitTypeDef;
+#define FLASH_TYPEERASE_SECTORS 0
+#define FLASH_SECTOR_7          7
+#define VOLTAGE_RANGE_3         3
+#define FLASH_TYPEPROGRAM_WORD  0
+static inline HAL_StatusTypeDef HAL_FLASH_Unlock(void) { return HAL_OK; }
+static inline HAL_StatusTypeDef HAL_FLASH_Lock(void)   { return HAL_OK; }
+static inline HAL_StatusTypeDef HAL_FLASHEx_Erase(FLASH_EraseInitTypeDef *e, uint32_t *err) { (void)e; *err = 0xFFFFFFFFUL; return HAL_OK; }
+static inline HAL_StatusTypeDef HAL_FLASH_Program(uint32_t t, uint32_t addr, uint64_t data) { (void)t; (void)addr; (void)data; return HAL_OK; }
+static inline uint32_t HAL_GetTick(void) { return 0; }
+#endif
+
+/* ---- Block flash/jlink/balance headers (not needed) ---- */
+/* pid_flash.h is included via pid_schedule.h -- stub flash functions */
+
+/* Forward-declare stubs for pid_flash functions (used by pid_schedule.c) */
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+/* Minimal pid_sched_entry_t and pid_flash stubs before pulling in schedule */
+#define PID_FLASH_H  /* prevent pid_flash.h from being re-included */
+
+/* Replicate types from pid_flash.h */
+#define PID_SCHED_MAX_BANDS     6u
+#define PID_SCHED_FLASH_ADDR    0x0807FF40UL
+#define PID_SCHED_MAGIC         0x534C5402UL
+#define PID_FLASH_STORE_ADDR    0x0807FFC0UL
+#define PID_FLASH_MAGIC         0x534C5401UL
+#define PID_FLASH_SECTOR        7
+#define PID_FLASH_SECTOR_VOLTAGE 3
+
+typedef struct __attribute__((packed)) {
+    float speed_mps;
+    float kp;
+    float ki;
+    float kd;
+} pid_sched_entry_t;
+
+typedef struct __attribute__((packed)) {
+    uint32_t         magic;
+    uint8_t          num_bands;
+    uint8_t          flags;
+    uint8_t          _pad0[2];
+    pid_sched_entry_t bands[PID_SCHED_MAX_BANDS];
+    uint8_t          _pad1[24];
+} pid_sched_flash_t;
+
+typedef struct __attribute__((packed)) {
+    uint32_t magic;
+    float    kp;
+    float    ki;
+    float    kd;
+    uint8_t  _pad[48];
+} pid_flash_t;
+
+/* Stub flash storage (simulated in RAM) */
+static pid_sched_flash_t g_sched_flash;
+static pid_flash_t       g_pid_flash;
+static bool              g_sched_flash_valid = false;
+static bool              g_pid_flash_valid   = false;
+
+bool pid_flash_load(float *kp, float *ki, float *kd)
+{
+    if (!g_pid_flash_valid || g_pid_flash.magic != PID_FLASH_MAGIC) return false;
+    *kp = g_pid_flash.kp; *ki = g_pid_flash.ki; *kd = g_pid_flash.kd;
+    return true;
+}
+
+bool pid_flash_save(float kp, float ki, float kd)
+{
+    g_pid_flash.magic = PID_FLASH_MAGIC;
+    g_pid_flash.kp = kp; g_pid_flash.ki = ki; g_pid_flash.kd = kd;
+    g_pid_flash_valid = true;
+    return true;
+}
+
+bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n)
+{
+    if (!g_sched_flash_valid || g_sched_flash.magic != PID_SCHED_MAGIC) return false;
+    if (g_sched_flash.num_bands == 0 || g_sched_flash.num_bands > PID_SCHED_MAX_BANDS) return false;
+    memcpy(out_entries, g_sched_flash.bands, g_sched_flash.num_bands * sizeof(pid_sched_entry_t));
+    *out_n = g_sched_flash.num_bands;
+    return true;
+}
+
+bool pid_flash_save_all(float kp_s, float ki_s, float kd_s,
+                        const pid_sched_entry_t *entries, uint8_t num_bands)
+{
+    if (num_bands == 0 || num_bands > PID_SCHED_MAX_BANDS) return false;
+    g_sched_flash.magic = PID_SCHED_MAGIC;
+    g_sched_flash.num_bands = num_bands;
+    memcpy(g_sched_flash.bands, entries, num_bands * sizeof(pid_sched_entry_t));
+    g_sched_flash_valid = true;
+    g_pid_flash.magic = PID_FLASH_MAGIC;
+    g_pid_flash.kp = kp_s; g_pid_flash.ki = ki_s; g_pid_flash.kd = kd_s;
+    g_pid_flash_valid = true;
+    return true;
+}
+
+/* Stub mpu6000.h and balance.h so pid_schedule.h doesn't pull in hardware types */
+#define MPU6000_H
+typedef struct { float ax, ay, az, gx, gy, gz, pitch, pitch_rate; } IMUData;
+
+#define BALANCE_H
+typedef enum { BALANCE_DISARMED=0, BALANCE_ARMED, BALANCE_TILT_FAULT } balance_state_t;
+typedef struct {
+    balance_state_t state;
+    float pitch_deg, pitch_rate;
+    float integral, prev_error;
+    int16_t motor_cmd;
+    float kp, ki, kd, setpoint, max_tilt;
+    int16_t max_speed;
+} balance_t;
+
+/* Include the implementation directly */
+#include "../src/pid_schedule.c"
+
+/* ============================================================
+ * Test framework
+ * ============================================================ */
+static int g_pass = 0, g_fail = 0;
+
+#define ASSERT(cond, msg) do { \
+    if (cond) { g_pass++; } \
+    else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \
+} while (0)
+
+#define ASSERT_NEAR(a, b, eps, msg) ASSERT(fabsf((a)-(b)) < (eps), msg)
+
+static void reset_flash(void)
+{
+    g_sched_flash_valid = false;
+    g_pid_flash_valid   = false;
+    memset(&g_sched_flash, 0xFF, sizeof(g_sched_flash));
+    memset(&g_pid_flash,   0xFF, sizeof(g_pid_flash));
+}
+
+/* ============================================================
+ * Tests
+ * ============================================================ */
+
+static void test_init_loads_default_when_flash_empty(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    ASSERT(pid_schedule_get_num_bands() == 3u, "default 3 bands");
+    pid_sched_entry_t tbl[PID_SCHED_MAX_BANDS];
+    uint8_t n;
+    pid_schedule_get_table(tbl, &n);
+    ASSERT(n == 3u, "get_table returns 3");
+    ASSERT_NEAR(tbl[0].speed_mps, 0.00f, 1e-5f, "band0 speed=0.00");
+    ASSERT_NEAR(tbl[0].kp, 40.0f, 1e-4f, "band0 kp=40");
+    ASSERT_NEAR(tbl[2].speed_mps, 0.80f, 1e-5f, "band2 speed=0.80");
+    ASSERT_NEAR(tbl[2].kp, 28.0f, 1e-4f, "band2 kp=28");
+}
+
+static void test_init_loads_from_flash_when_valid(void)
+{
+    reset_flash();
+    pid_sched_entry_t entries[2] = {
+        { .speed_mps = 0.0f, .kp = 10.0f, .ki = 0.5f, .kd = 0.2f },
+        { .speed_mps = 1.0f, .kp = 20.0f, .ki = 0.8f, .kd = 0.4f },
+    };
+    pid_flash_save_all(1.0f, 0.1f, 0.1f, entries, 2u);
+    pid_schedule_init();
+    ASSERT(pid_schedule_get_num_bands() == 2u, "init loads 2 bands from flash");
+    pid_sched_entry_t tbl[PID_SCHED_MAX_BANDS];
+    uint8_t n;
+    pid_schedule_get_table(tbl, &n);
+    ASSERT_NEAR(tbl[1].kp, 20.0f, 1e-4f, "flash band1 kp=20");
+}
+
+static void test_get_gains_below_first_band(void)
+{
+    reset_flash();
+    pid_schedule_init();  /* default table: 0.0, 0.3, 0.8 m/s */
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.0f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 40.0f, 1e-4f, "speed=0 -> kp=40 (clamp low)");
+    /* abs(-0.1)=0.1 m/s: between band0(0.0) and band1(0.3), t=1/3 -> kp=40+(35-40)/3 */
+    pid_schedule_get_gains(-0.1f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 40.0f + (35.0f - 40.0f) * (0.1f / 0.3f), 0.01f,
+                "speed=-0.1 interpolates via abs(speed)");
+}
+
+static void test_get_gains_above_last_band(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(2.0f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 28.0f, 1e-4f, "speed=2.0 -> kp=28 (clamp high)");
+}
+
+static void test_get_gains_at_band_boundary(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.30f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 35.0f, 1e-4f, "speed=0.30 exactly -> kp=35");
+    pid_schedule_get_gains(0.80f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 28.0f, 1e-4f, "speed=0.80 exactly -> kp=28");
+}
+
+static void test_interpolation_midpoint(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    /* Between band0 (0.0,kp=40) and band1 (0.3,kp=35): at t=0.5 -> kp=37.5 */
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.15f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 37.5f, 0.01f, "interp midpoint kp=37.5");
+    /* Between band1 (0.3,kp=35) and band2 (0.8,kp=28): at t=0.2 -> 35+(28-35)*0.2=33.6 */
+    pid_schedule_get_gains(0.40f, &kp, &ki, &kd);
+    float expected = 35.0f + (28.0f - 35.0f) * ((0.40f - 0.30f) / (0.80f - 0.30f));
+    ASSERT_NEAR(kp, expected, 0.01f, "interp band1->2 kp");
+}
+
+static void test_interpolation_ki_kd(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.15f, &kp, &ki, &kd);
+    /* ki: band0=1.5, band1=1.0, t=0.5 -> 1.25 */
+    ASSERT_NEAR(ki, 1.25f, 0.01f, "interp midpoint ki=1.25");
+    /* kd: band0=1.2, band1=1.0, t=0.5 -> 1.1 */
+    ASSERT_NEAR(kd, 1.1f,  0.01f, "interp midpoint kd=1.1");
+}
+
+static void test_set_table_and_sort(void)
+{
+    pid_sched_entry_t tbl[3] = {
+        { .speed_mps = 0.8f, .kp = 5.0f, .ki = 0.1f, .kd = 0.1f },
+        { .speed_mps = 0.0f, .kp = 9.0f, .ki = 0.3f, .kd = 0.3f },
+        { .speed_mps = 0.4f, .kp = 7.0f, .ki = 0.2f, .kd = 0.2f },
+    };
+    pid_schedule_set_table(tbl, 3u);
+    ASSERT(pid_schedule_get_num_bands() == 3u, "set_table 3 bands");
+    pid_sched_entry_t out[PID_SCHED_MAX_BANDS];
+    uint8_t n;
+    pid_schedule_get_table(out, &n);
+    /* After sort: 0.0, 0.4, 0.8 */
+    ASSERT_NEAR(out[0].speed_mps, 0.0f, 1e-5f, "sorted[0]=0.0");
+    ASSERT_NEAR(out[1].speed_mps, 0.4f, 1e-5f, "sorted[1]=0.4");
+    ASSERT_NEAR(out[2].speed_mps, 0.8f, 1e-5f, "sorted[2]=0.8");
+}
+
+static void test_set_table_clamps_n(void)
+{
+    pid_sched_entry_t big[8];
+    memset(big, 0, sizeof(big));
+    for (int i = 0; i < 8; i++) big[i].speed_mps = (float)i * 0.1f;
+    pid_schedule_set_table(big, 8u);
+    ASSERT(pid_schedule_get_num_bands() == PID_SCHED_MAX_BANDS, "clamp to MAX_BANDS");
+}
+
+static void test_set_table_min_1(void)
+{
+    pid_sched_entry_t one = { .speed_mps = 0.5f, .kp = 30.0f, .ki = 1.0f, .kd = 0.8f };
+    pid_schedule_set_table(&one, 0u);  /* n=0 clamped to 1 */
+    ASSERT(pid_schedule_get_num_bands() == 1u, "min 1 band");
+}
+
+static void test_active_band_idx_clamp_low(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.0f, &kp, &ki, &kd);
+    ASSERT(pid_schedule_active_band_idx() == 0u, "active=0 when clamped low");
+}
+
+static void test_active_band_idx_interpolating(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.5f, &kp, &ki, &kd);  /* between band1 and band2 */
+    ASSERT(pid_schedule_active_band_idx() == 1u, "active=1 between band1-2");
+}
+
+static void test_active_band_idx_clamp_high(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp, ki, kd;
+    pid_schedule_get_gains(5.0f, &kp, &ki, &kd);
+    ASSERT(pid_schedule_active_band_idx() == 2u, "active=2 when clamped high");
+}
+
+static void test_apply_writes_gains(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    balance_t b;
+    memset(&b, 0, sizeof(b));
+    pid_schedule_apply(&b, 0.0f);
+    ASSERT_NEAR(b.kp, 40.0f, 1e-4f, "apply: kp written");
+    ASSERT_NEAR(b.ki, 1.5f,  1e-4f, "apply: ki written");
+    ASSERT_NEAR(b.kd, 1.2f,  1e-4f, "apply: kd written");
+}
+
+static void test_apply_resets_integral_on_band_change(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    balance_t b;
+    memset(&b, 0, sizeof(b));
+    b.integral = 99.0f;
+
+    /* First call: sets s_prev_band from sentinel -> band 0 (integral reset) */
+    pid_schedule_apply(&b, 0.0f);
+    ASSERT_NEAR(b.integral, 0.0f, 1e-6f, "apply: integral reset on first call");
+
+    b.integral = 77.0f;
+    pid_schedule_apply(&b, 0.0f);  /* same band -- no reset */
+    ASSERT_NEAR(b.integral, 77.0f, 1e-6f, "apply: integral preserved same band");
+
+    b.integral = 55.0f;
+    pid_schedule_apply(&b, 0.5f);  /* band changes 0->1 -- reset */
+    ASSERT_NEAR(b.integral, 0.0f, 1e-6f, "apply: integral reset on band change");
+}
+
+static void test_flash_save_and_reload(void)
+{
+    reset_flash();
+    pid_sched_entry_t tbl[2] = {
+        { .speed_mps = 0.0f, .kp = 15.0f, .ki = 0.6f, .kd = 0.3f },
+        { .speed_mps = 0.5f, .kp = 10.0f, .ki = 0.4f, .kd = 0.2f },
+    };
+    pid_schedule_set_table(tbl, 2u);
+    bool ok = pid_schedule_flash_save(25.0f, 1.1f, 0.9f);
+    ASSERT(ok, "flash_save returns true");
+    ASSERT(g_sched_flash_valid, "flash_save wrote sched record");
+    ASSERT(g_pid_flash_valid,   "flash_save wrote pid record");
+    ASSERT_NEAR(g_pid_flash.kp, 25.0f, 1e-4f, "pid kp saved");
+
+    /* Now reload */
+    pid_schedule_init();
+    ASSERT(pid_schedule_get_num_bands() == 2u, "reload 2 bands");
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.0f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 15.0f, 1e-4f, "reload kp at speed=0");
+}
+
+static void test_get_default_table(void)
+{
+    pid_sched_entry_t def[PID_SCHED_MAX_BANDS];
+    uint8_t n;
+    pid_schedule_get_default_table(def, &n);
+    ASSERT(n == 3u, "default table has 3 entries");
+    ASSERT_NEAR(def[0].kp, 40.0f, 1e-4f, "default[0] kp=40");
+    ASSERT_NEAR(def[1].kp, 35.0f, 1e-4f, "default[1] kp=35");
+    ASSERT_NEAR(def[2].kp, 28.0f, 1e-4f, "default[2] kp=28");
+}
+
+static void test_init_discards_invalid_flash(void)
+{
+    reset_flash();
+    /* Write a valid record but with out-of-range gain */
+    pid_sched_entry_t bad[1] = {{ .speed_mps=0.0f, .kp=999.0f, .ki=0.1f, .kd=0.1f }};
+    pid_flash_save_all(1.0f, 0.1f, 0.1f, bad, 1u);
+    pid_schedule_init();
+    /* Should fall back to default */
+    ASSERT(pid_schedule_get_num_bands() == 3u, "invalid flash -> default 3 bands");
+}
+
+static void test_single_band_clamps_both_ends(void)
+{
+    pid_sched_entry_t one = { .speed_mps = 0.5f, .kp = 50.0f, .ki = 2.0f, .kd = 1.5f };
+    pid_schedule_set_table(&one, 1u);
+    float kp, ki, kd;
+    pid_schedule_get_gains(0.0f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 50.0f, 1e-4f, "single band: clamp low -> kp=50");
+    pid_schedule_get_gains(9.9f, &kp, &ki, &kd);
+    ASSERT_NEAR(kp, 50.0f, 1e-4f, "single band: clamp high -> kp=50");
+}
+
+static void test_negative_speed_symmetric(void)
+{
+    reset_flash();
+    pid_schedule_init();
+    float kp_fwd, ki_fwd, kd_fwd;
+    float kp_rev, ki_rev, kd_rev;
+    pid_schedule_get_gains( 0.5f, &kp_fwd, &ki_fwd, &kd_fwd);
+    pid_schedule_get_gains(-0.5f, &kp_rev, &ki_rev, &kd_rev);
+    ASSERT_NEAR(kp_fwd, kp_rev, 1e-5f, "symmetric: kp same for +/-speed");
+    ASSERT_NEAR(ki_fwd, ki_rev, 1e-5f, "symmetric: ki same for +/-speed");
+    ASSERT_NEAR(kd_fwd, kd_rev, 1e-5f, "symmetric: kd same for +/-speed");
+}
+
+int main(void)
+{
+    printf("=== test_pid_schedule ===\n");
+
+    test_init_loads_default_when_flash_empty();
+    test_init_loads_from_flash_when_valid();
+    test_get_gains_below_first_band();
+    test_get_gains_above_last_band();
+    test_get_gains_at_band_boundary();
+    test_interpolation_midpoint();
+    test_interpolation_ki_kd();
+    test_set_table_and_sort();
+    test_set_table_clamps_n();
+    test_set_table_min_1();
+    test_active_band_idx_clamp_low();
+    test_active_band_idx_interpolating();
+    test_active_band_idx_clamp_high();
+    test_apply_writes_gains();
+    test_apply_resets_integral_on_band_change();
+    test_flash_save_and_reload();
+    test_get_default_table();
+    test_init_discards_invalid_flash();
+    test_single_band_clamps_both_ends();
+    test_negative_speed_symmetric();
+
+    printf("PASSED: %d  FAILED: %d\n", g_pass, g_fail);
+    return (g_fail == 0) ? 0 : 1;
+}

ui/event_log_panel.css

@@ -0,0 +1,209 @@
+/* event_log_panel.css — Saltybot Event Log Panel (Issue #576) */
+
+*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
+
+:root {
+  --bg0:    #050510;
+  --bg1:    #070712;
+  --bg2:    #0a0a1a;
+  --bd:     #0c2a3a;
+  --bd2:    #1e3a5f;
+  --dim:    #374151;
+  --mid:    #6b7280;
+  --base:   #9ca3af;
+  --hi:     #d1d5db;
+  --cyan:   #06b6d4;
+  --green:  #22c55e;
+  --amber:  #f59e0b;
+  --red:    #ef4444;
+  --purple: #a855f7;
+}
+
+body {
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: var(--bg0);
+  color: var(--base);
+  height: 100dvh;
+  display: flex;
+  flex-direction: column;
+  overflow: hidden;
+}
+
+/* ── Header ── */
+#header {
+  display: flex;
+  align-items: center;
+  padding: 6px 12px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  gap: 8px;
+  flex-wrap: wrap;
+}
+.logo { color: #f97316; font-weight: bold; letter-spacing: 0.15em; font-size: 13px; flex-shrink: 0; }
+
+#conn-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--dim); flex-shrink: 0; transition: background 0.3s;
+}
+#conn-dot.connected    { background: var(--green); }
+#conn-dot.error        { background: var(--red); animation: blink 1s infinite; }
+
+@keyframes blink { 0%,100%{opacity:1}50%{opacity:.3} }
+
+#ws-input {
+  background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
+  color: #67e8f9; padding: 2px 7px; font-family: monospace; font-size: 11px; width: 190px;
+}
+#ws-input:focus { outline: none; border-color: var(--cyan); }
+
+.hbtn {
+  padding: 2px 8px; border-radius: 4px; border: 1px solid var(--bd2);
+  background: var(--bg2); color: #67e8f9; font-family: monospace;
+  font-size: 10px; font-weight: bold; cursor: pointer; transition: background .15s; white-space: nowrap;
+}
+.hbtn:hover { background: #0e4f69; }
+.hbtn.active { background: #0e4f69; border-color: var(--cyan); }
+.hbtn.pause  { background: #451a03; border-color: #92400e; color: #fcd34d; }
+.hbtn.pause:hover { background: #6b2b04; }
+
+#count-badge {
+  font-size: 10px; color: var(--mid); white-space: nowrap; margin-left: auto;
+}
+#paused-indicator {
+  font-size: 10px; color: #fcd34d; display: none; animation: blink 1.5s infinite;
+}
+#paused-indicator.visible { display: inline; }
+
+/* ── Toolbar ── */
+#toolbar {
+  display: flex;
+  align-items: center;
+  gap: 6px;
+  padding: 5px 12px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  flex-wrap: wrap;
+}
+
+/* Severity filter buttons */
+.sev-btn {
+  padding: 2px 7px; border-radius: 3px; border: 1px solid;
+  font-family: monospace; font-size: 9px; font-weight: bold;
+  cursor: pointer; transition: opacity .15s, filter .15s; letter-spacing: 0.05em;
+}
+.sev-btn.off { opacity: 0.25; filter: grayscale(0.6); }
+
+.sev-debug { background: #1a1a2e; border-color: #4b5563; color: #9ca3af; }
+.sev-info  { background: #032a1e; border-color: #065f46; color: #34d399; }
+.sev-warn  { background: #3d1a00; border-color: #92400e; color: #fbbf24; }
+.sev-error { background: #3d0000; border-color: #991b1b; color: #f87171; }
+.sev-fatal { background: #2e003d; border-color: #7e22ce; color: #c084fc; }
+.sev-event { background: #001a3d; border-color: #1d4ed8; color: #60a5fa; }
+
+/* Search input */
+#search-input {
+  background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
+  color: var(--hi); padding: 2px 7px; font-family: monospace; font-size: 11px;
+  width: 160px;
+}
+#search-input:focus { outline: none; border-color: var(--cyan); }
+
+#node-filter {
+  background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
+  color: var(--base); padding: 2px 5px; font-family: monospace; font-size: 10px;
+  max-width: 140px;
+}
+#node-filter:focus { outline: none; border-color: var(--cyan); }
+
+.toolbar-sep { width: 1px; height: 16px; background: var(--bd2); flex-shrink: 0; }
+
+/* ── Main layout ── */
+#main {
+  flex: 1;
+  display: flex;
+  min-height: 0;
+}
+
+/* ── Log feed ── */
+#log-feed {
+  flex: 1;
+  overflow-y: auto;
+  overflow-x: hidden;
+  padding: 4px 0;
+  scrollbar-width: thin;
+  scrollbar-color: var(--bd2) var(--bg0);
+}
+#log-feed::-webkit-scrollbar       { width: 6px; }
+#log-feed::-webkit-scrollbar-track { background: var(--bg0); }
+#log-feed::-webkit-scrollbar-thumb { background: var(--bd2); border-radius: 3px; }
+
+/* ── Log entry ── */
+.log-entry {
+  display: grid;
+  grid-template-columns: 80px 54px minmax(80px,160px) 1fr;
+  gap: 0 8px;
+  align-items: baseline;
+  padding: 2px 12px;
+  border-bottom: 1px solid transparent;
+  transition: background 0.1s;
+  cursor: default;
+}
+.log-entry:hover { background: rgba(255,255,255,0.025); border-bottom-color: var(--bd); }
+
+.log-entry.sev-debug { border-left: 2px solid #4b5563; }
+.log-entry.sev-info  { border-left: 2px solid #065f46; }
+.log-entry.sev-warn  { border-left: 2px solid #92400e; }
+.log-entry.sev-error { border-left: 2px solid #991b1b; }
+.log-entry.sev-fatal { border-left: 2px solid #7e22ce; }
+.log-entry.sev-event { border-left: 2px solid #1d4ed8; }
+
+.log-ts   { color: var(--mid); font-size: 10px; white-space: nowrap; }
+.log-sev  { font-size: 9px; font-weight: bold; letter-spacing: 0.05em; white-space: nowrap; }
+.log-node { color: var(--mid); font-size: 10px; overflow: hidden; text-overflow: ellipsis; white-space: nowrap; }
+.log-msg  { color: var(--hi); font-size: 11px; word-break: break-word; white-space: pre-wrap; }
+
+.log-entry.sev-debug .log-sev { color: #9ca3af; }
+.log-entry.sev-info  .log-sev { color: #34d399; }
+.log-entry.sev-warn  .log-sev { color: #fbbf24; }
+.log-entry.sev-error .log-sev { color: #f87171; }
+.log-entry.sev-fatal .log-sev { color: #c084fc; }
+.log-entry.sev-event .log-sev { color: #60a5fa; }
+
+.log-entry.sev-error .log-msg { color: #fca5a5; }
+.log-entry.sev-fatal .log-msg { color: #e9d5ff; }
+.log-entry.sev-warn  .log-msg { color: #fde68a; }
+
+/* Search highlight */
+mark {
+  background: rgba(234,179,8,0.35);
+  color: inherit;
+  border-radius: 2px;
+  padding: 0 1px;
+}
+
+/* Empty state */
+#empty-state {
+  display: flex; flex-direction: column; align-items: center; justify-content: center;
+  height: 100%; color: var(--dim); gap: 8px;
+  user-select: none;
+}
+#empty-state .icon { font-size: 40px; }
+
+/* ── Footer ── */
+#footer {
+  background: var(--bg1); border-top: 1px solid var(--bd);
+  padding: 3px 12px;
+  display: flex; align-items: center; justify-content: space-between;
+  flex-shrink: 0; font-size: 10px; color: var(--dim);
+}
+
+/* ── Mobile ── */
+@media (max-width: 640px) {
+  .log-entry { grid-template-columns: 70px 44px 1fr; }
+  .log-node  { display: none; }
+  #search-input { width: 100px; }
+  #node-filter  { max-width: 90px; }
+}

ui/event_log_panel.html

@@ -0,0 +1,90 @@
+<!DOCTYPE html>
+<html lang="en">
+<head>
+<meta charset="UTF-8">
+<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
+<title>Saltybot — Event Log</title>
+<link rel="stylesheet" href="event_log_panel.css">
+<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
+</head>
+<body>
+
+<!-- ── Header ── -->
+<div id="header">
+  <div class="logo">⚡ SALTYBOT — EVENT LOG</div>
+
+  <div id="conn-dot"></div>
+  <input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
+  <button id="btn-connect" class="hbtn">CONNECT</button>
+  <span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
+
+  <span id="paused-indicator">⏸ PAUSED</span>
+  <span id="count-badge">0 / 0</span>
+</div>
+
+<!-- ── Toolbar ── -->
+<div id="toolbar">
+
+  <!-- Severity filters -->
+  <button class="sev-btn sev-debug" data-level="DEBUG">DEBUG</button>
+  <button class="sev-btn sev-info"  data-level="INFO" >INFO</button>
+  <button class="sev-btn sev-warn"  data-level="WARN" >WARN</button>
+  <button class="sev-btn sev-error" data-level="ERROR">ERROR</button>
+  <button class="sev-btn sev-fatal" data-level="FATAL">FATAL</button>
+  <button class="sev-btn sev-event" data-level="EVENT">EVENT</button>
+
+  <div class="toolbar-sep"></div>
+
+  <!-- Node filter -->
+  <select id="node-filter">
+    <option value="">All nodes</option>
+  </select>
+
+  <!-- Text search -->
+  <input id="search-input" type="text" placeholder="Search… (Ctrl+F)" />
+
+  <div class="toolbar-sep"></div>
+
+  <!-- Actions -->
+  <button id="btn-pause"  class="hbtn active">⏸ PAUSE</button>
+  <button id="btn-clear"  class="hbtn">CLEAR</button>
+  <button id="btn-export" class="hbtn">↓ CSV</button>
+
+  <!-- Ring buffer info -->
+  <span style="font-size:9px;color:#374151;margin-left:auto">
+    ring: 1000 entries · /rosout + /saltybot/events
+  </span>
+</div>
+
+<!-- ── Main ── -->
+<div id="main">
+  <div id="log-feed">
+
+    <!-- Empty state -->
+    <div id="empty-state">
+      <div class="icon">📋</div>
+      <div>No events — connect to rosbridge</div>
+      <div style="font-size:10px;color:#374151">
+        Subscribing to /rosout and /saltybot/events
+      </div>
+    </div>
+
+  </div>
+</div>
+
+<!-- ── Footer ── -->
+<div id="footer">
+  <span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
+  <span>topics: /rosout (rcl_interfaces/Log) · /saltybot/events (std_msgs/String)</span>
+  <span>event log — issue #576</span>
+</div>
+
+<script src="event_log_panel.js"></script>
+<script>
+  // Sync footer WS URL
+  document.getElementById('ws-input').addEventListener('input', (e) => {
+    document.getElementById('footer-ws').textContent = e.target.value;
+  });
+</script>
+</body>
+</html>

ui/event_log_panel.js

@@ -0,0 +1,386 @@
+/**
+ * event_log_panel.js — Saltybot Event Log Panel (Issue #576)
+ *
+ * Subscribes via rosbridge WebSocket to:
+ *   /rosout               rcl_interfaces/msg/Log   — ROS2 node messages
+ *   /saltybot/events      std_msgs/String (JSON)   — custom robot events
+ *
+ * Features:
+ *   - 1000-entry ring buffer (oldest dropped when full)
+ *   - Filter by severity (DEBUG/INFO/WARN/ERROR/FATAL + EVENTS)
+ *   - Filter by node name (select from seen nodes)
+ *   - Text search with highlight
+ *   - Auto-scroll (pauses when user scrolls up, resumes at bottom)
+ *   - Pause on hover (stops auto-scroll, doesn't drop messages)
+ *   - CSV export of current filtered view
+ *   - Clear all / clear filtered
+ */
+
+'use strict';
+
+// ── Constants ─────────────────────────────────────────────────────────────────
+
+const RING_CAPACITY = 1000;
+
+// ROS2 /rosout level byte values
+const ROS_LEVELS = {
+  10: 'DEBUG',
+  20: 'INFO',
+  30: 'WARN',
+  40: 'ERROR',
+  50: 'FATAL',
+};
+
+const SEV_LABEL = {
+  DEBUG: 'DEBUG',
+  INFO:  'INFO ',
+  WARN:  'WARN ',
+  ERROR: 'ERROR',
+  FATAL: 'FATAL',
+  EVENT: 'EVENT',
+};
+
+// ── State ─────────────────────────────────────────────────────────────────────
+
+let ros       = null;
+let rosoutSub = null;
+let eventsSub = null;
+
+// Ring buffer — array of entry objects
+const ring = [];
+let   nextId    = 0;    // monotonic entry ID
+
+// Seen node names for filter dropdown
+const seenNodes = new Set();
+
+// Filter state
+const filters = {
+  levels: new Set(['DEBUG','INFO','WARN','ERROR','FATAL','EVENT']),
+  node:   '',      // '' = all
+  search: '',
+};
+
+// Auto-scroll + hover-pause
+let autoScroll   = true;
+let hoverPaused  = false;
+let userScrolled = false;   // user scrolled away manually
+
+// Pending DOM rows to flush (batched for perf)
+let pendingFlush = false;
+
+// ── DOM refs ──────────────────────────────────────────────────────────────────
+
+const feed       = document.getElementById('log-feed');
+const emptyState = document.getElementById('empty-state');
+const countBadge = document.getElementById('count-badge');
+const pausedInd  = document.getElementById('paused-indicator');
+const searchEl   = document.getElementById('search-input');
+const nodeFilter = document.getElementById('node-filter');
+
+// ── Utility ───────────────────────────────────────────────────────────────────
+
+function tsNow() {
+  return new Date().toLocaleTimeString('en-US', {
+    hour12: false, hour: '2-digit', minute: '2-digit', second: '2-digit',
+  }) + '.' + String(Date.now() % 1000).padStart(3,'0');
+}
+
+function tsFromRos(stamp) {
+  if (!stamp) return tsNow();
+  const ms = stamp.sec * 1000 + Math.floor((stamp.nanosec ?? 0) / 1e6);
+  const d  = new Date(ms);
+  return d.toLocaleTimeString('en-US', {
+    hour12: false, hour: '2-digit', minute: '2-digit', second: '2-digit',
+  }) + '.' + String(d.getMilliseconds()).padStart(3,'0');
+}
+
+function escapeHtml(s) {
+  return s.replace(/&/g,'&amp;').replace(/</g,'&lt;').replace(/>/g,'&gt;').replace(/"/g,'&quot;');
+}
+
+function highlightSearch(text, term) {
+  if (!term) return escapeHtml(text);
+  const escaped = term.replace(/[.*+?^${}()|[\]\\]/g,'\\$&');
+  const re = new RegExp(`(${escaped})`, 'gi');
+  return escapeHtml(text).replace(re, '<mark>$1</mark>');
+}
+
+// ── Ring buffer ───────────────────────────────────────────────────────────────
+
+function pushEntry(entry) {
+  entry.id = nextId++;
+  ring.push(entry);
+  if (ring.length > RING_CAPACITY) ring.shift();
+
+  if (!seenNodes.has(entry.node) && entry.node) {
+    seenNodes.add(entry.node);
+    rebuildNodeFilter();
+  }
+}
+
+// ── ROS connection ────────────────────────────────────────────────────────────
+
+function connect() {
+  const url = document.getElementById('ws-input').value.trim();
+  if (!url) return;
+  if (ros) ros.close();
+
+  ros = new ROSLIB.Ros({ url });
+
+  ros.on('connection', () => {
+    document.getElementById('conn-dot').className = 'connected';
+    document.getElementById('conn-label').textContent = url;
+    document.getElementById('btn-connect').textContent = 'RECONNECT';
+    setupSubs();
+    addSystemEntry('Connected to ' + url, 'INFO');
+  });
+
+  ros.on('error', (err) => {
+    document.getElementById('conn-dot').className = 'error';
+    document.getElementById('conn-label').textContent = 'ERROR: ' + (err?.message || err);
+    teardown();
+    addSystemEntry('Connection error: ' + (err?.message || err), 'ERROR');
+  });
+
+  ros.on('close', () => {
+    document.getElementById('conn-dot').className = '';
+    document.getElementById('conn-label').textContent = 'Disconnected';
+    teardown();
+    addSystemEntry('Disconnected', 'WARN');
+  });
+}
+
+function setupSubs() {
+  // /rosout — ROS2 log messages
+  rosoutSub = new ROSLIB.Topic({
+    ros, name: '/rosout',
+    messageType: 'rcl_interfaces/msg/Log',
+  });
+  rosoutSub.subscribe((msg) => {
+    const level = ROS_LEVELS[msg.level] ?? 'INFO';
+    pushEntry({
+      ts:     tsFromRos(msg.stamp),
+      level,
+      node:   (msg.name  || 'unknown').replace(/^\//, ''),
+      msg:    msg.msg  || '',
+      source: 'rosout',
+    });
+    scheduleRender();
+  });
+
+  // /saltybot/events — custom events (JSON string: {level, node, msg})
+  eventsSub = new ROSLIB.Topic({
+    ros, name: '/saltybot/events',
+    messageType: 'std_msgs/String',
+  });
+  eventsSub.subscribe((msg) => {
+    let level = 'EVENT', node = 'saltybot', text = msg.data;
+    try {
+      const parsed = JSON.parse(msg.data);
+      level = (parsed.level || 'EVENT').toUpperCase();
+      node  = parsed.node  || 'saltybot';
+      text  = parsed.msg   || parsed.message || msg.data;
+    } catch (_) { /* raw string */ }
+
+    pushEntry({ ts: tsNow(), level, node, msg: text, source: 'events' });
+    scheduleRender();
+  });
+}
+
+function teardown() {
+  if (rosoutSub) { rosoutSub.unsubscribe(); rosoutSub = null; }
+  if (eventsSub) { eventsSub.unsubscribe(); eventsSub = null; }
+}
+
+function addSystemEntry(text, level = 'INFO') {
+  pushEntry({ ts: tsNow(), level, node: '[system]', msg: text, source: 'system' });
+  scheduleRender();
+}
+
+// ── Rendering ─────────────────────────────────────────────────────────────────
+
+function scheduleRender() {
+  if (pendingFlush) return;
+  pendingFlush = true;
+  requestAnimationFrame(renderAll);
+}
+
+function entryVisible(e) {
+  if (!filters.levels.has(e.level)) return false;
+  if (filters.node && e.node !== filters.node) return false;
+  if (filters.search) {
+    const q = filters.search.toLowerCase();
+    if (!e.msg.toLowerCase().includes(q) &&
+        !e.node.toLowerCase().includes(q)) return false;
+  }
+  return true;
+}
+
+function buildRow(e) {
+  const cls  = 'sev-' + e.level.toLowerCase();
+  const hl   = highlightSearch(e.msg, filters.search);
+  return `<div class="log-entry ${cls}" data-id="${e.id}">` +
+    `<span class="log-ts">${e.ts}</span>` +
+    `<span class="log-sev">${SEV_LABEL[e.level] ?? e.level}</span>` +
+    `<span class="log-node" title="${escapeHtml(e.node)}">${escapeHtml(e.node)}</span>` +
+    `<span class="log-msg">${hl}</span>` +
+    `</div>`;
+}
+
+function renderAll() {
+  pendingFlush = false;
+
+  const visible = ring.filter(entryVisible);
+
+  if (visible.length === 0) {
+    feed.innerHTML = '';
+    emptyState.style.display = 'flex';
+    countBadge.textContent = `0 / ${ring.length}`;
+    return;
+  }
+
+  emptyState.style.display = 'none';
+  feed.innerHTML = visible.map(buildRow).join('');
+  countBadge.textContent = `${visible.length} / ${ring.length}`;
+
+  maybeScrollBottom();
+}
+
+function maybeScrollBottom() {
+  if ((autoScroll && !hoverPaused && !userScrolled)) {
+    feed.scrollTop = feed.scrollHeight;
+  }
+}
+
+// ── Auto-scroll + pause logic ─────────────────────────────────────────────────
+
+feed.addEventListener('mouseenter', () => {
+  hoverPaused = true;
+  pausedInd.classList.add('visible');
+});
+feed.addEventListener('mouseleave', () => {
+  hoverPaused = false;
+  pausedInd.classList.remove('visible');
+  if (autoScroll && !userScrolled) feed.scrollTop = feed.scrollHeight;
+});
+
+feed.addEventListener('scroll', () => {
+  const atBottom = feed.scrollHeight - feed.scrollTop - feed.clientHeight < 40;
+  if (atBottom) {
+    userScrolled = false;
+  } else if (!hoverPaused) {
+    userScrolled = true;
+  }
+});
+
+// Pause/resume button
+document.getElementById('btn-pause').addEventListener('click', () => {
+  autoScroll = !autoScroll;
+  userScrolled = false;
+  const btn = document.getElementById('btn-pause');
+  if (autoScroll) {
+    btn.textContent = '⏸ PAUSE';
+    btn.classList.remove('pause');
+    btn.classList.add('active');
+    feed.scrollTop = feed.scrollHeight;
+  } else {
+    btn.textContent = '▶ RESUME';
+    btn.classList.add('pause');
+    btn.classList.remove('active');
+  }
+});
+
+// ── Filter controls ───────────────────────────────────────────────────────────
+
+document.querySelectorAll('.sev-btn').forEach((btn) => {
+  btn.addEventListener('click', () => {
+    const lv = btn.dataset.level;
+    if (filters.levels.has(lv)) {
+      filters.levels.delete(lv);
+      btn.classList.add('off');
+    } else {
+      filters.levels.add(lv);
+      btn.classList.remove('off');
+    }
+    scheduleRender();
+  });
+});
+
+searchEl.addEventListener('input', () => {
+  filters.search = searchEl.value.trim();
+  scheduleRender();
+});
+
+nodeFilter.addEventListener('change', () => {
+  filters.node = nodeFilter.value;
+  scheduleRender();
+});
+
+function rebuildNodeFilter() {
+  const current = nodeFilter.value;
+  const nodes   = [...seenNodes].sort();
+  nodeFilter.innerHTML = '<option value="">All nodes</option>' +
+    nodes.map(n => `<option value="${escapeHtml(n)}"${n===current?' selected':''}>${escapeHtml(n)}</option>`).join('');
+}
+
+// ── Clear ─────────────────────────────────────────────────────────────────────
+
+document.getElementById('btn-clear').addEventListener('click', () => {
+  ring.length = 0;
+  seenNodes.clear();
+  rebuildNodeFilter();
+  scheduleRender();
+});
+
+// ── CSV export ────────────────────────────────────────────────────────────────
+
+document.getElementById('btn-export').addEventListener('click', () => {
+  const visible = ring.filter(entryVisible);
+  if (visible.length === 0) return;
+
+  const header = 'timestamp,level,node,message\n';
+  const rows = visible.map((e) =>
+    [e.ts, e.level, e.node, '"' + e.msg.replace(/"/g,'""') + '"'].join(',')
+  );
+  const csv  = header + rows.join('\n');
+  const blob = new Blob([csv], { type: 'text/csv' });
+  const url  = URL.createObjectURL(blob);
+  const a    = document.createElement('a');
+  a.href     = url;
+  a.download = `saltybot_log_${new Date().toISOString().slice(0,19).replace(/:/g,'-')}.csv`;
+  a.click();
+  URL.revokeObjectURL(url);
+});
+
+// ── Connect button ────────────────────────────────────────────────────────────
+
+document.getElementById('btn-connect').addEventListener('click', connect);
+document.getElementById('ws-input').addEventListener('keydown', (e) => {
+  if (e.key === 'Enter') connect();
+});
+
+// ── Init ──────────────────────────────────────────────────────────────────────
+
+const stored = localStorage.getItem('evlog_ws_url');
+if (stored) document.getElementById('ws-input').value = stored;
+document.getElementById('ws-input').addEventListener('change', (e) => {
+  localStorage.setItem('evlog_ws_url', e.target.value);
+});
+
+// Keyboard shortcut: Ctrl+F focuses search
+document.addEventListener('keydown', (e) => {
+  if ((e.ctrlKey || e.metaKey) && e.key === 'f') {
+    e.preventDefault();
+    searchEl.focus();
+    searchEl.select();
+  }
+  // Escape: clear search
+  if (e.key === 'Escape' && document.activeElement === searchEl) {
+    searchEl.value = '';
+    filters.search = '';
+    scheduleRender();
+  }
+});
+
+// Initial empty state
+scheduleRender();

ui/map_panel.css

@@ -0,0 +1,236 @@
+/* map_panel.css — Saltybot 2D Map View (Issue #587) */
+
+*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
+
+:root {
+  --bg0:   #050510;
+  --bg1:   #070712;
+  --bg2:   #0a0a1a;
+  --bd:    #0c2a3a;
+  --bd2:   #1e3a5f;
+  --dim:   #374151;
+  --mid:   #6b7280;
+  --base:  #9ca3af;
+  --hi:    #d1d5db;
+  --cyan:  #06b6d4;
+  --green: #22c55e;
+  --amber: #f59e0b;
+  --red:   #ef4444;
+}
+
+body {
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: var(--bg0);
+  color: var(--base);
+  height: 100dvh;
+  display: flex;
+  flex-direction: column;
+  overflow: hidden;
+}
+
+/* ── Header ── */
+#header {
+  display: flex;
+  align-items: center;
+  padding: 5px 12px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  gap: 8px;
+  flex-wrap: wrap;
+}
+.logo { color: #f97316; font-weight: bold; letter-spacing: .15em; font-size: 13px; flex-shrink: 0; }
+
+#conn-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--dim); flex-shrink: 0; transition: background .3s;
+}
+#conn-dot.connected { background: var(--green); }
+#conn-dot.error     { background: var(--red); animation: blink 1s infinite; }
+
+@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.3} }
+
+#ws-input {
+  background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
+  color: #67e8f9; padding: 2px 7px; font-family: monospace; font-size: 11px; width: 185px;
+}
+#ws-input:focus { outline: none; border-color: var(--cyan); }
+
+.hbtn {
+  padding: 2px 8px; border-radius: 4px; border: 1px solid var(--bd2);
+  background: var(--bg2); color: #67e8f9; font-family: monospace;
+  font-size: 10px; font-weight: bold; cursor: pointer; transition: background .15s;
+  white-space: nowrap;
+}
+.hbtn:hover { background: #0e4f69; }
+.hbtn.on { background: #0e4f69; border-color: var(--cyan); color: #fff; }
+.hbtn.warn-on { background: #3d1a00; border-color: #92400e; color: #fbbf24; }
+
+/* ── Toolbar ── */
+#toolbar {
+  display: flex;
+  align-items: center;
+  gap: 8px;
+  padding: 4px 12px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  flex-wrap: wrap;
+  font-size: 10px;
+}
+.tsep { width: 1px; height: 16px; background: var(--bd2); }
+
+#zoom-display { color: var(--mid); min-width: 45px; }
+
+/* ── Main: map + sidebar ── */
+#main {
+  flex: 1;
+  display: grid;
+  grid-template-columns: 1fr 240px;
+  min-height: 0;
+}
+
+@media (max-width: 700px) {
+  #main { grid-template-columns: 1fr; }
+  #sidebar { display: none; }
+}
+
+/* ── Canvas ── */
+#map-canvas {
+  display: block;
+  width: 100%;
+  height: 100%;
+  cursor: grab;
+  touch-action: none;
+}
+#map-canvas.dragging { cursor: grabbing; }
+#map-wrap {
+  position: relative;
+  overflow: hidden;
+  background: #010108;
+}
+
+/* No-signal overlay */
+#no-signal {
+  position: absolute;
+  inset: 0;
+  display: flex;
+  flex-direction: column;
+  align-items: center;
+  justify-content: center;
+  gap: 8px;
+  color: var(--dim);
+  pointer-events: none;
+}
+#no-signal.hidden { display: none; }
+#no-signal .icon { font-size: 48px; }
+
+/* E-stop overlay */
+#estop-overlay {
+  position: absolute;
+  top: 8px; left: 50%; transform: translateX(-50%);
+  background: rgba(127,0,0,0.85);
+  border: 2px solid #ef4444;
+  color: #fca5a5;
+  padding: 4px 14px;
+  border-radius: 4px;
+  font-weight: bold;
+  font-size: 11px;
+  letter-spacing: .1em;
+  pointer-events: none;
+  display: none;
+  animation: blink 1s infinite;
+}
+#estop-overlay.visible { display: block; }
+
+/* Mouse coords HUD */
+#coords-hud {
+  position: absolute;
+  bottom: 6px; left: 8px;
+  background: rgba(0,0,0,.7);
+  color: var(--mid);
+  padding: 2px 6px;
+  border-radius: 3px;
+  font-size: 10px;
+  pointer-events: none;
+}
+
+/* ── Sidebar ── */
+#sidebar {
+  background: var(--bg1);
+  border-left: 1px solid var(--bd);
+  display: flex;
+  flex-direction: column;
+  overflow-y: auto;
+  padding: 8px;
+  gap: 8px;
+  font-size: 11px;
+}
+.sb-card {
+  background: var(--bg2);
+  border: 1px solid var(--bd2);
+  border-radius: 6px;
+  padding: 8px;
+}
+.sb-title {
+  font-size: 9px; font-weight: bold; letter-spacing: .12em;
+  color: #0891b2; text-transform: uppercase; margin-bottom: 6px;
+}
+.sb-row {
+  display: flex; justify-content: space-between;
+  padding: 2px 0; border-bottom: 1px solid var(--bd);
+  font-size: 10px;
+}
+.sb-row:last-child { border-bottom: none; }
+.sb-lbl { color: var(--mid); }
+.sb-val { color: var(--hi); font-family: monospace; }
+
+/* Status dots */
+.sdot {
+  width: 8px; height: 8px; border-radius: 50%;
+  display: inline-block; margin-right: 4px;
+}
+.sdot.green  { background: var(--green); }
+.sdot.amber  { background: var(--amber); }
+.sdot.red    { background: var(--red); animation: blink .8s infinite; }
+.sdot.gray   { background: var(--dim); }
+
+/* Legend ── */
+.legend-row {
+  display: flex; align-items: center; gap: 6px;
+  font-size: 10px; color: var(--base); padding: 2px 0;
+}
+.legend-swatch {
+  width: 20px; height: 3px; border-radius: 2px; flex-shrink: 0;
+}
+
+/* Anchor list */
+#anchor-list .anchor-row {
+  display: flex; align-items: center; gap: 4px;
+  padding: 2px 0; font-size: 10px; color: var(--base);
+  border-bottom: 1px solid var(--bd);
+}
+#anchor-list .anchor-row:last-child { border-bottom: none; }
+#anchor-add { width: 100%; margin-top: 4px; }
+
+/* Anchor input row */
+.anchor-inputs {
+  display: grid; grid-template-columns: 1fr 1fr 1fr;
+  gap: 4px; margin-top: 4px;
+}
+.anchor-inputs input {
+  background: var(--bg0); border: 1px solid var(--bd2);
+  border-radius: 3px; color: var(--hi); padding: 2px 4px;
+  font-family: monospace; font-size: 10px; text-align: center;
+  width: 100%;
+}
+.anchor-inputs input:focus { outline: none; border-color: var(--cyan); }
+
+/* ── Footer ── */
+#footer {
+  background: var(--bg1); border-top: 1px solid var(--bd);
+  padding: 3px 12px;
+  display: flex; align-items: center; justify-content: space-between;
+  flex-shrink: 0; font-size: 10px; color: var(--dim);
+}

ui/map_panel.html

@@ -0,0 +1,176 @@
+<!DOCTYPE html>
+<html lang="en">
+<head>
+<meta charset="UTF-8">
+<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
+<title>Saltybot — Map View</title>
+<link rel="stylesheet" href="map_panel.css">
+<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
+</head>
+<body>
+
+<!-- ── Header ── -->
+<div id="header">
+  <div class="logo">⚡ SALTYBOT — MAP</div>
+
+  <div id="conn-dot"></div>
+  <input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
+  <button id="btn-connect" class="hbtn">CONNECT</button>
+  <span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
+</div>
+
+<!-- ── Toolbar ── -->
+<div id="toolbar">
+  <button id="btn-zoom-in"  class="hbtn">＋</button>
+  <button id="btn-zoom-out" class="hbtn">－</button>
+  <span id="zoom-display">1.00x</span>
+
+  <div class="tsep"></div>
+
+  <button id="btn-center" class="hbtn on">⊙ AUTO-CENTER</button>
+  <button id="btn-reset"  class="hbtn">RESET VIEW</button>
+
+  <div class="tsep"></div>
+
+  <button id="btn-clear-trail" class="hbtn">CLEAR TRAIL</button>
+
+  <div class="tsep"></div>
+
+  <!-- Legend -->
+  <div style="display:flex;flex-direction:column;gap:2px">
+    <div class="legend-row">
+      <div class="legend-swatch" style="background:#22c55e"></div>
+      <span>LIDAR scan</span>
+    </div>
+    <div class="legend-row">
+      <div class="legend-swatch" style="background:rgba(239,68,68,.6)"></div>
+      <span>Danger zone (0.30m)</span>
+    </div>
+  </div>
+  <div style="display:flex;flex-direction:column;gap:2px">
+    <div class="legend-row">
+      <div class="legend-swatch" style="background:rgba(245,158,11,.5)"></div>
+      <span>Warn zone (1.00m)</span>
+    </div>
+    <div class="legend-row">
+      <div class="legend-swatch" style="background:#06b6d4"></div>
+      <span>Trail (100 pts)</span>
+    </div>
+  </div>
+  <div class="legend-row">
+    <div class="legend-swatch" style="background:#f59e0b;height:8px;width:8px;border-radius:0;transform:rotate(45deg);flex-shrink:0"></div>
+    <span>UWB anchor</span>
+  </div>
+</div>
+
+<!-- ── Main ── -->
+<div id="main">
+
+  <!-- Map canvas -->
+  <div id="map-wrap">
+    <canvas id="map-canvas"></canvas>
+
+    <!-- No signal -->
+    <div id="no-signal">
+      <div class="icon">🗺️</div>
+      <div>Connect to rosbridge to view map</div>
+      <div style="font-size:10px;color:#374151">
+        /saltybot/pose/fused · /scan · /saltybot/safety_zone/status
+      </div>
+    </div>
+
+    <!-- E-stop banner -->
+    <div id="estop-overlay">🛑 E-STOP ACTIVE</div>
+
+    <!-- Mouse coords -->
+    <div id="coords-hud">(0.00, 0.00) m</div>
+  </div>
+
+  <!-- Sidebar -->
+  <aside id="sidebar">
+
+    <!-- Robot status -->
+    <div class="sb-card">
+      <div class="sb-title">Robot Position</div>
+      <div class="sb-row">
+        <span class="sb-lbl">Position</span>
+        <span class="sb-val" id="sb-pos">—</span>
+      </div>
+      <div class="sb-row">
+        <span class="sb-lbl">Heading</span>
+        <span class="sb-val" id="sb-hdg">—</span>
+      </div>
+      <div class="sb-row">
+        <span class="sb-lbl">Trail</span>
+        <span class="sb-val" id="sb-trail">0 pts</span>
+      </div>
+    </div>
+
+    <!-- Safety status -->
+    <div class="sb-card">
+      <div class="sb-title">Safety Zone</div>
+      <div class="sb-row">
+        <span class="sb-lbl"><span class="sdot gray" id="sb-zone-dot"></span>Fwd zone</span>
+        <span class="sb-val" id="sb-fwd">—</span>
+      </div>
+      <div class="sb-row">
+        <span class="sb-lbl">Closest</span>
+        <span class="sb-val" id="sb-closest">—</span>
+      </div>
+      <div class="sb-row">
+        <span class="sb-lbl">E-stop</span>
+        <span class="sb-val" id="sb-estop" style="color:#6b7280">—</span>
+      </div>
+    </div>
+
+    <!-- UWB anchors -->
+    <div class="sb-card">
+      <div class="sb-title">UWB Anchors</div>
+      <div id="anchor-list"></div>
+
+      <!-- Add anchor form -->
+      <div style="margin-top:8px;font-size:9px;color:#6b7280;margin-bottom:4px">
+        ADD ANCHOR
+      </div>
+      <div class="anchor-inputs">
+        <input id="anc-x"   type="number" step="0.1" placeholder="X (m)" />
+        <input id="anc-y"   type="number" step="0.1" placeholder="Y (m)" />
+        <input id="anc-lbl" type="text"               placeholder="Label" />
+      </div>
+      <button id="btn-add-anchor" class="hbtn" id="anchor-add"
+              style="width:100%;margin-top:6px;text-align:center">
+        + ADD ANCHOR
+      </button>
+    </div>
+
+    <!-- Legend / topics -->
+    <div class="sb-card">
+      <div class="sb-title">Topics</div>
+      <div style="font-size:9px;color:#374151;line-height:1.9">
+        <div>SUB <code style="color:#4b5563">/saltybot/pose/fused</code></div>
+        <div style="color:#1e3a5f;padding-left:8px">geometry_msgs/PoseStamped</div>
+        <div>SUB <code style="color:#4b5563">/scan</code></div>
+        <div style="color:#1e3a5f;padding-left:8px">sensor_msgs/LaserScan</div>
+        <div>SUB <code style="color:#4b5563">/saltybot/safety_zone/status</code></div>
+        <div style="color:#1e3a5f;padding-left:8px">std_msgs/String (JSON)</div>
+      </div>
+    </div>
+
+  </aside>
+</div>
+
+<!-- ── Footer ── -->
+<div id="footer">
+  <span>wheel=zoom · drag=pan · pinch=zoom (touch)</span>
+  <span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
+  <span>map view — issue #587</span>
+</div>
+
+<script src="map_panel.js"></script>
+<script>
+  document.getElementById('ws-input').addEventListener('input', (e) => {
+    document.getElementById('footer-ws').textContent = e.target.value;
+  });
+</script>
+</body>
+</html>

ui/map_panel.js

@@ -0,0 +1,607 @@
+/**
+ * map_panel.js — Saltybot 2D Map View (Issue #587)
+ *
+ * Canvas-based 2D map with:
+ *   - Robot position from /saltybot/pose/fused (geometry_msgs/PoseStamped)
+ *   - RPLIDAR scan overlay from /scan (sensor_msgs/LaserScan)
+ *   - Safety zone rings at danger (0.30m) and warn (1.00m)
+ *   - /saltybot/safety_zone/status JSON — e-stop + closest obstacle
+ *   - UWB anchor markers (user-configured or via /saltybot/uwb/anchors)
+ *   - 100-position breadcrumb trail
+ *   - Zoom (wheel) and pan (drag) with touch support
+ *   - Auto-center toggle
+ *
+ * World → canvas:  cx = origin.x + x * ppm
+ *                  cy = origin.y - y * ppm   (Y flipped)
+ */
+
+'use strict';
+
+// ── Config ────────────────────────────────────────────────────────────────────
+
+const DANGER_R        = 0.30;   // m — matches safety_zone_params.yaml default
+const WARN_R          = 1.00;   // m
+const TRAIL_MAX       = 100;
+const SCAN_THROTTLE   = 100;    // ms — don't render every scan packet
+const GRID_SPACING_M  = 1.0;    // world metres per grid square
+const PIXELS_PER_M    = 80;     // initial scale
+const MIN_PPM         = 10;
+const MAX_PPM         = 600;
+
+// ── State ─────────────────────────────────────────────────────────────────────
+
+let ros = null, poseSub = null, scanSub = null, statusSub = null;
+
+const state = {
+  // Robot
+  robot:     { x: 0, y: 0, theta: 0 },  // world metres + radians
+  trail:     [],                           // [{x,y}]
+
+  // Scan
+  scan:      null,   // { angle_min, angle_increment, ranges[] }
+  scanTs:    0,
+
+  // Safety status
+  estop:     false,
+  fwdZone:   'CLEAR',
+  closestM:  null,
+  dangerN:   0,
+  warnN:     0,
+
+  // UWB anchors [{x,y,label}]
+  anchors:   [],
+
+  // View
+  autoCenter: true,
+  ppm:        PIXELS_PER_M,   // pixels per metre
+  originX:    0,               // canvas px where world (0,0) is
+  originY:    0,
+};
+
+// ── Canvas ────────────────────────────────────────────────────────────────────
+
+const canvas  = document.getElementById('map-canvas');
+const ctx     = canvas.getContext('2d');
+const wrap    = document.getElementById('map-wrap');
+
+function resize() {
+  canvas.width  = wrap.clientWidth  || 600;
+  canvas.height = wrap.clientHeight || 400;
+  if (state.autoCenter) centerOnRobot();
+  draw();
+}
+
+window.addEventListener('resize', resize);
+
+// ── World ↔ canvas coordinate helpers ────────────────────────────────────────
+
+function w2cx(wx) { return state.originX + wx * state.ppm; }
+function w2cy(wy) { return state.originY - wy * state.ppm; }
+function c2wx(cx) { return (cx - state.originX) / state.ppm; }
+function c2wy(cy) { return -(cy - state.originY) / state.ppm; }
+
+function yawFromQuat(qx, qy, qz, qw) {
+  return Math.atan2(2*(qw*qz + qx*qy), 1 - 2*(qy*qy + qz*qz));
+}
+
+// ── Draw ──────────────────────────────────────────────────────────────────────
+
+function draw() {
+  const W = canvas.width, H = canvas.height;
+  ctx.clearRect(0, 0, W, H);
+
+  // Background
+  ctx.fillStyle = '#010108';
+  ctx.fillRect(0, 0, W, H);
+
+  drawGrid(W, H);
+  drawScan();
+  drawSafetyZones();
+  drawTrail();
+  drawAnchors();
+  drawRobot();
+  updateHUD();
+}
+
+// Grid lines
+function drawGrid(W, H) {
+  const ppm   = state.ppm;
+  const step  = GRID_SPACING_M * ppm;
+  const ox    = ((state.originX % step) + step) % step;
+  const oy    = ((state.originY % step) + step) % step;
+
+  ctx.strokeStyle = '#0d1a2a';
+  ctx.lineWidth   = 1;
+  ctx.beginPath();
+  for (let x = ox; x < W; x += step) { ctx.moveTo(x,0); ctx.lineTo(x,H); }
+  for (let y = oy; y < H; y += step) { ctx.moveTo(0,y); ctx.lineTo(W,y); }
+  ctx.stroke();
+
+  // Axis cross at world origin
+  const ox0 = state.originX, oy0 = state.originY;
+  if (ox0 > 0 && ox0 < W) {
+    ctx.strokeStyle = '#1e3a5f'; ctx.lineWidth = 1;
+    ctx.beginPath(); ctx.moveTo(ox0, 0); ctx.lineTo(ox0, H); ctx.stroke();
+  }
+  if (oy0 > 0 && oy0 < H) {
+    ctx.strokeStyle = '#1e3a5f'; ctx.lineWidth = 1;
+    ctx.beginPath(); ctx.moveTo(0, oy0); ctx.lineTo(W, oy0); ctx.stroke();
+  }
+
+  // Scale bar (bottom right)
+  const barM   = 1.0;
+  const barPx  = barM * ppm;
+  const bx = W - 20, by = H - 12;
+  ctx.strokeStyle = '#374151'; ctx.lineWidth = 2;
+  ctx.beginPath();
+  ctx.moveTo(bx - barPx, by); ctx.lineTo(bx, by);
+  ctx.moveTo(bx - barPx, by - 4); ctx.lineTo(bx - barPx, by + 4);
+  ctx.moveTo(bx, by - 4);         ctx.lineTo(bx, by + 4);
+  ctx.stroke();
+  ctx.fillStyle = '#6b7280'; ctx.font = '9px Courier New'; ctx.textAlign = 'right';
+  ctx.fillText(`${barM}m`, bx, by - 6);
+  ctx.textAlign = 'left';
+}
+
+// LIDAR scan
+function drawScan() {
+  if (!state.scan) return;
+  const { angle_min, angle_increment, ranges, range_max } = state.scan;
+  const rx = state.robot.x, ry = state.robot.y, th = state.robot.theta;
+  const maxR = range_max || 12;
+
+  ctx.fillStyle = 'rgba(34,197,94,0.75)';
+  for (let i = 0; i < ranges.length; i++) {
+    const r = ranges[i];
+    if (!isFinite(r) || r <= 0.02 || r >= maxR) continue;
+    const a  = th + angle_min + i * angle_increment;
+    const wx = rx + r * Math.cos(a);
+    const wy = ry + r * Math.sin(a);
+    const cx_ = w2cx(wx), cy_ = w2cy(wy);
+    ctx.beginPath();
+    ctx.arc(cx_, cy_, 1.5, 0, Math.PI * 2);
+    ctx.fill();
+  }
+}
+
+// Safety zone rings (drawn around robot)
+function drawSafetyZones() {
+  const cx_ = w2cx(state.robot.x), cy_ = w2cy(state.robot.y);
+  const ppm = state.ppm;
+
+  // WARN ring (amber)
+  const warnEstop = state.estop;
+  ctx.strokeStyle = warnEstop ? 'rgba(239,68,68,0.35)' : 'rgba(245,158,11,0.35)';
+  ctx.lineWidth   = 1;
+  ctx.setLineDash([4, 4]);
+  ctx.beginPath();
+  ctx.arc(cx_, cy_, WARN_R * ppm, 0, Math.PI * 2);
+  ctx.stroke();
+
+  // DANGER ring (red)
+  ctx.strokeStyle = 'rgba(239,68,68,0.55)';
+  ctx.lineWidth   = 1.5;
+  ctx.setLineDash([3, 3]);
+  ctx.beginPath();
+  ctx.arc(cx_, cy_, DANGER_R * ppm, 0, Math.PI * 2);
+  ctx.stroke();
+
+  ctx.setLineDash([]);
+}
+
+// Breadcrumb trail
+function drawTrail() {
+  const trail = state.trail;
+  if (trail.length < 2) return;
+  ctx.lineWidth = 1.5;
+  for (let i = 1; i < trail.length; i++) {
+    const alpha = i / trail.length;
+    ctx.strokeStyle = `rgba(6,182,212,${alpha * 0.6})`;
+    ctx.beginPath();
+    ctx.moveTo(w2cx(trail[i-1].x), w2cy(trail[i-1].y));
+    ctx.lineTo(w2cx(trail[i].x),   w2cy(trail[i].y));
+    ctx.stroke();
+  }
+  // Trail dots at every 5th point
+  ctx.fillStyle = 'rgba(6,182,212,0.4)';
+  for (let i = 0; i < trail.length; i += 5) {
+    ctx.beginPath();
+    ctx.arc(w2cx(trail[i].x), w2cy(trail[i].y), 2, 0, Math.PI * 2);
+    ctx.fill();
+  }
+}
+
+// UWB anchor markers
+function drawAnchors() {
+  for (const a of state.anchors) {
+    const cx_ = w2cx(a.x), cy_ = w2cy(a.y);
+    const r   = 7;
+    // Diamond shape
+    ctx.strokeStyle = '#f59e0b';
+    ctx.lineWidth   = 1.5;
+    ctx.beginPath();
+    ctx.moveTo(cx_, cy_ - r);
+    ctx.lineTo(cx_ + r, cy_);
+    ctx.lineTo(cx_, cy_ + r);
+    ctx.lineTo(cx_ - r, cy_);
+    ctx.closePath();
+    ctx.stroke();
+    ctx.fillStyle = 'rgba(245,158,11,0.15)';
+    ctx.fill();
+    // Label
+    ctx.fillStyle = '#fcd34d';
+    ctx.font = 'bold 9px Courier New';
+    ctx.textAlign = 'center';
+    ctx.fillText(a.label || '⚓', cx_, cy_ - r - 3);
+    ctx.textAlign = 'left';
+  }
+}
+
+// Robot marker
+function drawRobot() {
+  const cx_ = w2cx(state.robot.x), cy_ = w2cy(state.robot.y);
+  const th  = state.robot.theta;
+  const r   = 10;
+
+  ctx.save();
+  ctx.translate(cx_, cy_);
+  ctx.rotate(-th);  // canvas Y is flipped so negate
+
+  // Body circle
+  const isEstop = state.estop;
+  ctx.strokeStyle = isEstop ? '#ef4444' : '#22d3ee';
+  ctx.fillStyle   = isEstop ? 'rgba(239,68,68,0.2)' : 'rgba(34,211,238,0.15)';
+  ctx.lineWidth   = 2;
+  ctx.beginPath();
+  ctx.arc(0, 0, r, 0, Math.PI * 2);
+  ctx.fill();
+  ctx.stroke();
+
+  // Forward arrow
+  ctx.strokeStyle = isEstop ? '#f87171' : '#67e8f9';
+  ctx.lineWidth   = 2;
+  ctx.beginPath();
+  ctx.moveTo(0, 0);
+  ctx.lineTo(r + 4, 0);
+  ctx.stroke();
+  // Arrowhead
+  ctx.fillStyle = isEstop ? '#f87171' : '#67e8f9';
+  ctx.beginPath();
+  ctx.moveTo(r + 4, 0);
+  ctx.lineTo(r + 1, -3);
+  ctx.lineTo(r + 1,  3);
+  ctx.closePath();
+  ctx.fill();
+
+  ctx.restore();
+}
+
+// ── HUD ───────────────────────────────────────────────────────────────────────
+
+function updateHUD() {
+  document.getElementById('zoom-display').textContent =
+    (state.ppm / PIXELS_PER_M).toFixed(2) + 'x';
+  updateSidebar();
+}
+
+function updateSidebar() {
+  setText('sb-pos',    `${state.robot.x.toFixed(2)}, ${state.robot.y.toFixed(2)} m`);
+  setText('sb-hdg',    (state.robot.theta * 180 / Math.PI).toFixed(1) + '°');
+  setText('sb-trail',  state.trail.length + ' pts');
+  setText('sb-closest',
+    state.closestM != null ? state.closestM.toFixed(2) + ' m' : '—');
+  setText('sb-fwd',    state.fwdZone);
+
+  // Zone status dot
+  const dot = document.getElementById('sb-zone-dot');
+  if (dot) {
+    dot.className = 'sdot ' + (
+      state.estop      ? 'red'   :
+      state.fwdZone === 'DANGER' ? 'red' :
+      state.fwdZone === 'WARN'   ? 'amber' : 'green'
+    );
+  }
+
+  // E-stop overlay
+  const ov = document.getElementById('estop-overlay');
+  if (ov) ov.classList.toggle('visible', state.estop);
+
+  // ESTOP badge in sidebar
+  setText('sb-estop', state.estop ? 'ACTIVE' : 'clear');
+  const estopEl = document.getElementById('sb-estop');
+  if (estopEl) estopEl.style.color = state.estop ? '#ef4444' : '#22c55e';
+}
+
+function setText(id, val) {
+  const el = document.getElementById(id);
+  if (el) el.textContent = val ?? '—';
+}
+
+// Coords HUD on mouse move
+canvas.addEventListener('mousemove', (e) => {
+  const rect = canvas.getBoundingClientRect();
+  const scaleX = canvas.width  / rect.width;
+  const scaleY = canvas.height / rect.height;
+  const cx_ = (e.clientX - rect.left) * scaleX;
+  const cy_ = (e.clientY - rect.top)  * scaleY;
+  const wx  = c2wx(cx_).toFixed(2);
+  const wy  = c2wy(cy_).toFixed(2);
+  document.getElementById('coords-hud').textContent = `(${wx}, ${wy}) m`;
+});
+
+// ── Zoom & pan ────────────────────────────────────────────────────────────────
+
+let dragging = false, dragStart = { cx: 0, cy: 0, ox: 0, oy: 0 };
+let pinchDist = null;
+
+canvas.addEventListener('wheel', (e) => {
+  e.preventDefault();
+  const rect  = canvas.getBoundingClientRect();
+  const mx    = (e.clientX - rect.left) * (canvas.width  / rect.width);
+  const my    = (e.clientY - rect.top)  * (canvas.height / rect.height);
+  const factor = e.deltaY < 0 ? 1.12 : 0.89;
+  const newPpm = Math.max(MIN_PPM, Math.min(MAX_PPM, state.ppm * factor));
+
+  // Zoom around cursor position
+  state.originX = mx - (mx - state.originX) * (newPpm / state.ppm);
+  state.originY = my - (my - state.originY) * (newPpm / state.ppm);
+  state.ppm     = newPpm;
+  state.autoCenter = false;
+  document.getElementById('btn-center').classList.remove('on');
+  draw();
+}, { passive: false });
+
+canvas.addEventListener('pointerdown', (e) => {
+  if (e.pointerType === 'touch') return;
+  dragging  = true;
+  canvas.classList.add('dragging');
+  canvas.setPointerCapture(e.pointerId);
+  dragStart = { cx: e.clientX, cy: e.clientY, ox: state.originX, oy: state.originY };
+});
+canvas.addEventListener('pointermove', (e) => {
+  if (!dragging) return;
+  const dx = e.clientX - dragStart.cx;
+  const dy = e.clientY - dragStart.cy;
+  state.originX = dragStart.ox + dx;
+  state.originY = dragStart.oy + dy;
+  state.autoCenter = false;
+  document.getElementById('btn-center').classList.remove('on');
+  draw();
+});
+canvas.addEventListener('pointerup', () => {
+  dragging = false;
+  canvas.classList.remove('dragging');
+});
+canvas.addEventListener('pointercancel', () => {
+  dragging = false;
+  canvas.classList.remove('dragging');
+});
+
+// Touch pinch-to-zoom
+canvas.addEventListener('touchstart', (e) => {
+  if (e.touches.length === 2) {
+    pinchDist = touchDist(e.touches);
+  }
+}, { passive: true });
+canvas.addEventListener('touchmove', (e) => {
+  if (e.touches.length === 2 && pinchDist != null) {
+    const d      = touchDist(e.touches);
+    const factor = d / pinchDist;
+    pinchDist    = d;
+    const newPpm = Math.max(MIN_PPM, Math.min(MAX_PPM, state.ppm * factor));
+    const mx     = (e.touches[0].clientX + e.touches[1].clientX) / 2;
+    const my     = (e.touches[0].clientY + e.touches[1].clientY) / 2;
+    const rect   = canvas.getBoundingClientRect();
+    const cx_    = (mx - rect.left) * (canvas.width  / rect.width);
+    const cy_    = (my - rect.top)  * (canvas.height / rect.height);
+    state.originX = cx_ - (cx_ - state.originX) * (newPpm / state.ppm);
+    state.originY = cy_ - (cy_ - state.originY) * (newPpm / state.ppm);
+    state.ppm     = newPpm;
+    draw();
+  }
+}, { passive: true });
+canvas.addEventListener('touchend', () => { pinchDist = null; }, { passive: true });
+
+function touchDist(touches) {
+  const dx = touches[0].clientX - touches[1].clientX;
+  const dy = touches[0].clientY - touches[1].clientY;
+  return Math.sqrt(dx*dx + dy*dy);
+}
+
+// ── Auto-center ───────────────────────────────────────────────────────────────
+
+function centerOnRobot() {
+  state.originX = canvas.width  / 2 - state.robot.x * state.ppm;
+  state.originY = canvas.height / 2 + state.robot.y * state.ppm;
+}
+
+document.getElementById('btn-center').addEventListener('click', () => {
+  state.autoCenter = !state.autoCenter;
+  document.getElementById('btn-center').classList.toggle('on', state.autoCenter);
+  if (state.autoCenter) { centerOnRobot(); draw(); }
+});
+
+document.getElementById('btn-zoom-in').addEventListener('click', () => {
+  state.ppm = Math.min(MAX_PPM, state.ppm * 1.4);
+  if (state.autoCenter) centerOnRobot();
+  draw();
+});
+document.getElementById('btn-zoom-out').addEventListener('click', () => {
+  state.ppm = Math.max(MIN_PPM, state.ppm / 1.4);
+  if (state.autoCenter) centerOnRobot();
+  draw();
+});
+document.getElementById('btn-reset').addEventListener('click', () => {
+  state.ppm    = PIXELS_PER_M;
+  state.autoCenter = true;
+  document.getElementById('btn-center').classList.add('on');
+  centerOnRobot(); draw();
+});
+
+// ── ROS connection ────────────────────────────────────────────────────────────
+
+function connect() {
+  const url = document.getElementById('ws-input').value.trim();
+  if (!url) return;
+  if (ros) ros.close();
+
+  ros = new ROSLIB.Ros({ url });
+  ros.on('connection', () => {
+    document.getElementById('conn-dot').className = 'connected';
+    document.getElementById('conn-label').textContent = url;
+    document.getElementById('btn-connect').textContent = 'RECONNECT';
+    document.getElementById('no-signal').classList.add('hidden');
+    setupSubs();
+  });
+  ros.on('error', (err) => {
+    document.getElementById('conn-dot').className = 'error';
+    document.getElementById('conn-label').textContent = 'ERR: ' + (err?.message || err);
+    teardown();
+  });
+  ros.on('close', () => {
+    document.getElementById('conn-dot').className = '';
+    document.getElementById('conn-label').textContent = 'Disconnected';
+    teardown();
+  });
+}
+
+function setupSubs() {
+  // Fused pose
+  poseSub = new ROSLIB.Topic({
+    ros, name: '/saltybot/pose/fused',
+    messageType: 'geometry_msgs/PoseStamped',
+    throttle_rate: 50,
+  });
+  poseSub.subscribe((msg) => {
+    const p  = msg.pose.position;
+    const q  = msg.pose.orientation;
+    const th = yawFromQuat(q.x, q.y, q.z, q.w);
+    state.robot = { x: p.x, y: p.y, theta: th };
+
+    state.trail.push({ x: p.x, y: p.y });
+    if (state.trail.length > TRAIL_MAX) state.trail.shift();
+
+    if (state.autoCenter) centerOnRobot();
+    requestDraw();
+  });
+
+  // RPLIDAR scan
+  scanSub = new ROSLIB.Topic({
+    ros, name: '/scan',
+    messageType: 'sensor_msgs/LaserScan',
+    throttle_rate: SCAN_THROTTLE,
+    compression: 'cbor',
+  });
+  scanSub.subscribe((msg) => {
+    state.scan = {
+      angle_min:       msg.angle_min,
+      angle_increment: msg.angle_increment,
+      ranges:          msg.ranges,
+      range_max:       msg.range_max,
+    };
+    requestDraw();
+  });
+
+  // Safety zone status
+  statusSub = new ROSLIB.Topic({
+    ros, name: '/saltybot/safety_zone/status',
+    messageType: 'std_msgs/String',
+    throttle_rate: 200,
+  });
+  statusSub.subscribe((msg) => {
+    try {
+      const d = JSON.parse(msg.data);
+      state.estop    = d.estop_active    ?? false;
+      state.fwdZone  = d.forward_zone    ?? 'CLEAR';
+      state.closestM = d.closest_obstacle_m ?? null;
+      state.dangerN  = d.danger_sector_count ?? 0;
+      state.warnN    = d.warn_sector_count   ?? 0;
+    } catch (_) {}
+    requestDraw();
+  });
+}
+
+function teardown() {
+  if (poseSub)   { poseSub.unsubscribe();   poseSub   = null; }
+  if (scanSub)   { scanSub.unsubscribe();   scanSub   = null; }
+  if (statusSub) { statusSub.unsubscribe(); statusSub = null; }
+  document.getElementById('no-signal').classList.remove('hidden');
+}
+
+// Batch draw calls
+let drawPending = false;
+function requestDraw() {
+  if (drawPending) return;
+  drawPending = true;
+  requestAnimationFrame(() => { drawPending = false; draw(); });
+}
+
+// ── UWB Anchors ───────────────────────────────────────────────────────────────
+
+function renderAnchorList() {
+  const list = document.getElementById('anchor-list');
+  if (!list) return;
+  list.innerHTML = state.anchors.map((a, i) =>
+    `<div class="anchor-row">
+       <span style="color:#f59e0b">◆</span>
+       <span style="flex:1">${a.label} (${a.x.toFixed(1)}, ${a.y.toFixed(1)})</span>
+       <button onclick="removeAnchor(${i})" style="background:none;border:none;color:#6b7280;cursor:pointer;font-size:10px">✕</button>
+     </div>`
+  ).join('') || '<div style="color:#374151;font-size:10px;text-align:center;padding:4px">No anchors</div>';
+}
+
+window.removeAnchor = function(i) {
+  state.anchors.splice(i, 1);
+  saveAnchors();
+  renderAnchorList();
+  draw();
+};
+
+document.getElementById('btn-add-anchor').addEventListener('click', () => {
+  const x = parseFloat(document.getElementById('anc-x').value);
+  const y = parseFloat(document.getElementById('anc-y').value);
+  const lbl = document.getElementById('anc-lbl').value.trim() || `A${state.anchors.length}`;
+  if (isNaN(x) || isNaN(y)) return;
+  state.anchors.push({ x, y, label: lbl });
+  document.getElementById('anc-x').value = '';
+  document.getElementById('anc-y').value = '';
+  document.getElementById('anc-lbl').value = '';
+  saveAnchors();
+  renderAnchorList();
+  draw();
+});
+
+function saveAnchors() {
+  localStorage.setItem('map_anchors', JSON.stringify(state.anchors));
+}
+function loadAnchors() {
+  try {
+    const saved = JSON.parse(localStorage.getItem('map_anchors') || '[]');
+    state.anchors = saved.filter(a => typeof a.x === 'number' && typeof a.y === 'number');
+  } catch (_) { state.anchors = []; }
+  renderAnchorList();
+}
+
+// ── Init ──────────────────────────────────────────────────────────────────────
+
+document.getElementById('btn-connect').addEventListener('click', connect);
+document.getElementById('ws-input').addEventListener('keydown', (e) => {
+  if (e.key === 'Enter') connect();
+});
+
+const stored = localStorage.getItem('map_ws_url');
+if (stored) document.getElementById('ws-input').value = stored;
+document.getElementById('ws-input').addEventListener('change', (e) => {
+  localStorage.setItem('map_ws_url', e.target.value);
+});
+
+// Clear trail button
+document.getElementById('btn-clear-trail').addEventListener('click', () => {
+  state.trail.length = 0; draw();
+});
+
+// Init: center at origin, set btn state
+state.autoCenter = true;
+document.getElementById('btn-center').classList.add('on');
+loadAnchors();
+resize();  // sets canvas size and draws initial blank map