feat: UWB anchor mount bracket (Issue #564)

EKF fusing UWB position (10Hz) with IMU accel+gyro (200Hz) for
SaltyBot indoor localization with UWB dropout resilience.

Package: saltybot_uwb_imu_fusion

- ekf_math.py: 6-state EKF [x,y,θ,vx,vy,ω], IMU predict + UWB update
  - IMU as process input: body-frame accel rotated to world via heading
  - Jacobian F for nonlinear rotation effect
  - Process noise Q from continuous white-noise model
  - UWB 2D position update, heading update from quaternion
  - Accel bias estimation (low-pass)
  - Velocity damping during UWB dropout (>2s threshold)
- ekf_node.py: ROS2 node subscribing to /imu/data (200Hz) + /saltybot/uwb/pose
  or /uwb/bearing (10Hz)
  - Publishes /saltybot/pose/fused (PoseStamped)
  - Publishes /saltybot/pose/fused_cov (PoseWithCovarianceStamped)
  - Broadcasts base_link → map TF2 at IMU rate
  - Suppresses output after max_dead_reckoning_s without UWB
- 14/14 unit tests passing (predict, update, dropout, PD covariance)
- Launch: ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py

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

chassis/uwb_anchor_mount.scad

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

@@ -0,0 +1,30 @@
+; SaltyBot UWB Anchor Firmware — Issue #544
+; Target: Makerfabs ESP32 UWB Pro (DW3000 chip)
+;
+; Library: Makerfabs MaUWB_DW3000
+;   https://github.com/Makerfabs/MaUWB_DW3000
+;
+; Flash:
+;   pio run -e anchor0 --target upload   (port-side anchor)
+;   pio run -e anchor1 --target upload   (starboard anchor)
+; Monitor:
+;   pio device monitor -e anchor0 -b 115200
+
+[common]
+platform     = espressif32
+board        = esp32dev
+framework    = arduino
+monitor_speed = 115200
+upload_speed  = 921600
+lib_deps =
+    https://github.com/Makerfabs/MaUWB_DW3000.git
+build_flags =
+    -DCORE_DEBUG_LEVEL=0
+
+[env:anchor0]
+extends     = common
+build_flags = ${common.build_flags} -DANCHOR_ID=0
+
+[env:anchor1]
+extends     = common
+build_flags = ${common.build_flags} -DANCHOR_ID=1

esp32/uwb_anchor/src/main.cpp

@@ -0,0 +1,413 @@
+/*
+ * uwb_anchor — SaltyBot ESP32 UWB Pro anchor firmware (TWR responder)
+ * Issue #544
+ *
+ * Hardware: Makerfabs ESP32 UWB Pro (DW3000 chip)
+ *
+ * Role
+ * ────
+ *   Anchor sits on SaltyBot body, USB-connected to Jetson Orin.
+ *   Two anchors per robot (anchor-0 port side, anchor-1 starboard).
+ *   Person-worn tags initiate ranging; anchors respond.
+ *
+ * Protocol: Double-Sided TWR (DS-TWR)
+ * ────────────────────────────────────
+ *   Tag → Anchor   POLL   (msg_type 0x01)
+ *   Anchor → Tag   RESP   (msg_type 0x02, payload: T_poll_rx, T_resp_tx)
+ *   Tag → Anchor   FINAL  (msg_type 0x03, payload: Ra, Da, Db timestamps)
+ *   Anchor computes range via DS-TWR formula, emits +RANGE on Serial.
+ *
+ * Serial output (115200 8N1, USB-CDC to Jetson)
+ * ──────────────────────────────────────────────
+ *   +RANGE:<anchor_id>,<range_mm>,<rssi_dbm>\r\n   (on each successful range)
+ *
+ * AT commands (host → anchor)
+ * ───────────────────────────
+ *   AT+RANGE?                  → returns last buffered +RANGE line
+ *   AT+RANGE_ADDR=<hex_addr>   → pair with specific tag (filter others)
+ *   AT+RANGE_ADDR=             → clear pairing (accept all tags)
+ *   AT+ID?                     → returns +ID:<anchor_id>
+ *
+ * Pin mapping — Makerfabs ESP32 UWB Pro
+ * ──────────────────────────────────────
+ *   SPI SCK   18    SPI MISO  19    SPI MOSI  23
+ *   DW CS     21    DW RST    27    DW IRQ    34
+ *
+ * Build
+ * ──────
+ *   pio run -e anchor0 --target upload   (port side)
+ *   pio run -e anchor1 --target upload   (starboard)
+ */
+
+#include <Arduino.h>
+#include <SPI.h>
+#include <math.h>
+#include "dw3000.h"   // Makerfabs MaUWB_DW3000 library
+
+/* ── Configurable ───────────────────────────────────────────────── */
+
+#ifndef ANCHOR_ID
+#  define ANCHOR_ID  0          /* 0 = port, 1 = starboard */
+#endif
+
+#define SERIAL_BAUD  115200
+
+/* ── Pin map (Makerfabs ESP32 UWB Pro) ─────────────────────────── */
+
+#define PIN_SCK   18
+#define PIN_MISO  19
+#define PIN_MOSI  23
+#define PIN_CS    21
+#define PIN_RST   27
+#define PIN_IRQ   34
+
+/* ── DW3000 channel / PHY config ───────────────────────────────── */
+
+static dwt_config_t dw_cfg = {
+    5,                 /* channel 5 (6.5 GHz, best penetration) */
+    DWT_PLEN_128,      /* preamble length                       */
+    DWT_PAC8,          /* PAC size                              */
+    9,                 /* TX preamble code                      */
+    9,                 /* RX preamble code                      */
+    1,                 /* SFD type (IEEE 802.15.4z)             */
+    DWT_BR_6M8,        /* data rate 6.8 Mbps                    */
+    DWT_PHR_MODE_STD,  /* standard PHR                          */
+    DWT_PHR_RATE_DATA,
+    (129 + 8 - 8),     /* SFD timeout                           */
+    DWT_STS_MODE_OFF,  /* STS off — standard TWR                */
+    DWT_STS_LEN_64,
+    DWT_PDOA_M0,       /* no PDoA                               */
+};
+
+/* ── Frame format ──────────────────────────────────────────────── */
+
+/* Byte layout for all frames:
+ *   [0]     frame_type  (FTYPE_*)
+ *   [1]     src_id      (tag 8-bit addr, or ANCHOR_ID)
+ *   [2]     dst_id
+ *   [3..]   payload
+ *   (FCS appended automatically by DW3000 — 2 bytes)
+ */
+
+#define FTYPE_POLL   0x01
+#define FTYPE_RESP   0x02
+#define FTYPE_FINAL  0x03
+
+#define FRAME_HDR   3
+#define FCS_LEN     2
+
+/* RESP payload: T_poll_rx(5 B) + T_resp_tx(5 B) */
+#define RESP_PAYLOAD   10
+#define RESP_FRAME_LEN (FRAME_HDR + RESP_PAYLOAD + FCS_LEN)
+
+/* FINAL payload: Ra(5 B) + Da(5 B) + Db(5 B) */
+#define FINAL_PAYLOAD   15
+#define FINAL_FRAME_LEN (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN)
+
+/* ── Timing ────────────────────────────────────────────────────── */
+
+/* Turnaround delay: anchor waits 500 µs after poll_rx before tx_resp.
+ * DW3000 tick = 1/(128×499.2e6) ≈ 15.65 ps  →  500 µs = ~31.95M ticks.
+ * Stored as uint32 shifted right 8 bits for dwt_setdelayedtrxtime. */
+#define RESP_TX_DLY_US      500UL
+#define DWT_TICKS_PER_US    63898UL          /* 1µs in DW3000 ticks (×8 prescaler) */
+#define RESP_TX_DLY_TICKS   (RESP_TX_DLY_US * DWT_TICKS_PER_US)
+
+/* How long anchor listens for FINAL after sending RESP */
+#define FINAL_RX_TIMEOUT_US  3000
+
+/* Speed of light (m/s) */
+#define SPEED_OF_LIGHT  299702547.0
+
+/* DW3000 40-bit timestamp mask */
+#define DWT_TS_MASK  0xFFFFFFFFFFULL
+
+/* Antenna delay (factory default; calibrate per unit for best accuracy) */
+#define ANT_DELAY  16385
+
+/* ── Interrupt flags (set in ISR, polled in main) ──────────────── */
+
+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;
+
+/* ── State ──────────────────────────────────────────────────────── */
+
+/* Last successful range (serves AT+RANGE? queries) */
+static int32_t g_last_range_mm = -1;
+static char    g_last_range_line[72] = {};
+
+/* Optional tag pairing: 0 = accept all tags */
+static uint8_t g_paired_tag_id = 0;
+
+/* ── DW3000 ISR callbacks ───────────────────────────────────────── */
+
+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);
+}
+
+/* Estimate receive power from CIR diagnostics (dBm) */
+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;
+}
+
+/* ── AT command handler ─────────────────────────────────────────── */
+
+static char g_at_buf[64];
+static int  g_at_idx = 0;
+
+static void at_dispatch(const char *cmd) {
+    if (strcmp(cmd, "AT+RANGE?") == 0) {
+        if (g_last_range_mm >= 0)
+            Serial.println(g_last_range_line);
+        else
+            Serial.println("+RANGE:NO_DATA");
+
+    } else if (strcmp(cmd, "AT+ID?") == 0) {
+        Serial.printf("+ID:%d\r\n", ANCHOR_ID);
+
+    } else if (strncmp(cmd, "AT+RANGE_ADDR=", 14) == 0) {
+        const char *v = cmd + 14;
+        if (*v == '\0') {
+            g_paired_tag_id = 0;
+            Serial.println("+OK:UNPAIRED");
+        } else {
+            g_paired_tag_id = (uint8_t)strtoul(v, nullptr, 0);
+            Serial.printf("+OK:PAIRED=0x%02X\r\n", g_paired_tag_id);
+        }
+    } else {
+        Serial.println("+ERR:UNKNOWN_CMD");
+    }
+}
+
+static void serial_poll(void) {
+    while (Serial.available()) {
+        char c = (char)Serial.read();
+        if (c == '\r') continue;
+        if (c == '\n') {
+            g_at_buf[g_at_idx] = '\0';
+            if (g_at_idx > 0) at_dispatch(g_at_buf);
+            g_at_idx = 0;
+        } else if (g_at_idx < (int)(sizeof(g_at_buf) - 1)) {
+            g_at_buf[g_at_idx++] = c;
+        }
+    }
+}
+
+/* ── DS-TWR anchor state machine ────────────────────────────────── */
+
+/*
+ * DS-TWR responder (one shot):
+ *   1. Wait for POLL from tag
+ *   2. Delayed-TX RESP (carry T_poll_rx + scheduled T_resp_tx)
+ *   3. Wait for FINAL from tag  (tag embeds Ra, Da, Db)
+ *   4. Compute: Rb = T_final_rx − T_resp_tx
+ *               tof = (Ra·Rb − Da·Db) / (Ra+Rb+Da+Db)
+ *               range_m = tof × c
+ *   5. Print +RANGE line
+ */
+static void twr_cycle(void) {
+
+    /* --- 1. Listen for POLL --- */
+    dwt_setrxtimeout(0);
+    dwt_rxenable(DWT_START_RX_IMMEDIATE);
+
+    g_rx_ok = g_rx_err = false;
+    uint32_t deadline = millis() + 2000;
+    while (!g_rx_ok && !g_rx_err) {
+        serial_poll();
+        if (millis() > deadline) {
+            /* restart RX if we've been stuck */
+            dwt_rxenable(DWT_START_RX_IMMEDIATE);
+            deadline = millis() + 2000;
+        }
+        yield();
+    }
+    if (!g_rx_ok || g_rx_len < FRAME_HDR) return;
+
+    /* validate POLL */
+    if (g_rx_buf[0] != FTYPE_POLL) return;
+    uint8_t tag_id = g_rx_buf[1];
+    if (g_paired_tag_id != 0 && tag_id != g_paired_tag_id) return;
+
+    /* --- 2. Record T_poll_rx --- */
+    uint8_t  poll_rx_raw[5];
+    dwt_readrxtimestamp(poll_rx_raw);
+    uint64_t T_poll_rx = ts_read(poll_rx_raw);
+
+    /* Compute delayed TX time: poll_rx + turnaround, aligned to 512-tick grid */
+    uint64_t resp_tx_sched = (T_poll_rx + RESP_TX_DLY_TICKS) & ~0x1FFULL;
+
+    /* Build RESP frame */
+    uint8_t resp[RESP_FRAME_LEN];
+    resp[0] = FTYPE_RESP;
+    resp[1] = ANCHOR_ID;
+    resp[2] = tag_id;
+    ts_write(&resp[3], T_poll_rx);      /* T_poll_rx  (tag uses this) */
+    ts_write(&resp[8], resp_tx_sched);  /* scheduled T_resp_tx */
+
+    dwt_writetxdata(RESP_FRAME_LEN - FCS_LEN, resp, 0);
+    dwt_writetxfctrl(RESP_FRAME_LEN, 0, 1 /*ranging*/);
+    dwt_setdelayedtrxtime((uint32_t)(resp_tx_sched >> 8));
+
+    /* Enable RX after TX to receive FINAL */
+    dwt_setrxaftertxdelay(300);
+    dwt_setrxtimeout(FINAL_RX_TIMEOUT_US);
+
+    /* Fire delayed TX */
+    g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false;
+    if (dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) {
+        dwt_forcetrxoff();
+        return; /* TX window missed — try next cycle */
+    }
+
+    /* Wait for TX done (short wait, ISR fires fast) */
+    uint32_t t0 = millis();
+    while (!g_tx_done && millis() - t0 < 15) { yield(); }
+
+    /* Read actual T_resp_tx */
+    uint8_t resp_tx_raw[5];
+    dwt_readtxtimestamp(resp_tx_raw);
+    uint64_t T_resp_tx = ts_read(resp_tx_raw);
+
+    /* --- 3. Wait for FINAL --- */
+    t0 = millis();
+    while (!g_rx_ok && !g_rx_err && !g_rx_to && millis() - t0 < 60) {
+        serial_poll();
+        yield();
+    }
+    if (!g_rx_ok || g_rx_len < FRAME_HDR + FINAL_PAYLOAD) return;
+    if (g_rx_buf[0] != FTYPE_FINAL) return;
+    if (g_rx_buf[1] != tag_id)      return;
+
+    /* Extract DS-TWR timestamps from FINAL payload */
+    uint64_t Ra = ts_read(&g_rx_buf[3]);   /* tag: T_resp_rx − T_poll_tx */
+    uint64_t Da = ts_read(&g_rx_buf[8]);   /* tag: T_final_tx − T_resp_rx */
+    /* g_rx_buf[13..17] = Db from tag (cross-check, unused here) */
+
+    /* T_final_rx */
+    uint8_t final_rx_raw[5];
+    dwt_readrxtimestamp(final_rx_raw);
+    uint64_t T_final_rx = ts_read(final_rx_raw);
+
+    /* --- 4. DS-TWR formula --- */
+    uint64_t Rb = ts_diff(T_final_rx, T_resp_tx);  /* anchor round-trip */
+    uint64_t Db = ts_diff(T_resp_tx,  T_poll_rx);  /* anchor turnaround */
+
+    double ra = ticks_to_s(Ra), rb = ticks_to_s(Rb);
+    double da = ticks_to_s(Da), db = ticks_to_s(Db);
+
+    double denom = ra + rb + da + db;
+    if (denom < 1e-15) return;
+
+    double tof    = (ra * rb - da * db) / denom;
+    double range_m = tof * SPEED_OF_LIGHT;
+
+    /* Validity window: 0.1 m – 130 m */
+    if (range_m < 0.1 || range_m > 130.0) return;
+
+    int32_t range_mm = (int32_t)(range_m * 1000.0 + 0.5);
+    float   rssi     = rx_power_dbm();
+
+    /* --- 5. Emit +RANGE --- */
+    snprintf(g_last_range_line, sizeof(g_last_range_line),
+             "+RANGE:%d,%ld,%.1f", ANCHOR_ID, (long)range_mm, rssi);
+    g_last_range_mm = range_mm;
+    Serial.println(g_last_range_line);
+}
+
+/* ── Arduino setup ──────────────────────────────────────────────── */
+
+void setup(void) {
+    Serial.begin(SERIAL_BAUD);
+    delay(300);
+
+    Serial.printf("\r\n[uwb_anchor] anchor_id=%d  starting\r\n", ANCHOR_ID);
+
+    SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS);
+
+    /* Hardware reset */
+    pinMode(PIN_RST, OUTPUT);
+    digitalWrite(PIN_RST, LOW);
+    delay(2);
+    pinMode(PIN_RST, INPUT_PULLUP);
+    delay(5);
+
+    /* DW3000 probe + init (Makerfabs MaUWB_DW3000 library) */
+    if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) {
+        Serial.println("[uwb_anchor] FATAL: DW3000 probe failed — check SPI wiring");
+        for (;;) delay(1000);
+    }
+
+    if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) {
+        Serial.println("[uwb_anchor] FATAL: dwt_initialise failed");
+        for (;;) delay(1000);
+    }
+
+    if (dwt_configure(&dw_cfg) != DWT_SUCCESS) {
+        Serial.println("[uwb_anchor] FATAL: dwt_configure failed");
+        for (;;) delay(1000);
+    }
+
+    dwt_setrxantennadelay(ANT_DELAY);
+    dwt_settxantennadelay(ANT_DELAY);
+    dwt_settxpower(0x0E080222UL);  /* max TX power for 120 m range */
+
+    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);
+
+    Serial.printf("[uwb_anchor] DW3000 ready  ch=%d  6.8Mbps  id=%d\r\n",
+                  dw_cfg.chan, ANCHOR_ID);
+    Serial.println("[uwb_anchor] Listening for tags...");
+}
+
+/* ── Arduino loop ───────────────────────────────────────────────── */
+
+void loop(void) {
+    serial_poll();
+    twr_cycle();
+}

esp32/uwb_anchor/udev/99-uwb-anchors.rules

@@ -0,0 +1,19 @@
+# SaltyBot UWB anchor USB-serial persistent symlinks
+# Install:
+#   sudo cp 99-uwb-anchors.rules /etc/udev/rules.d/
+#   sudo udevadm control --reload && sudo udevadm trigger
+#
+# Find serial numbers:
+#   udevadm info -a /dev/ttyUSB0 | grep ATTRS{serial}
+#
+# Fill ANCHOR0_SERIAL and ANCHOR1_SERIAL with the values found above.
+# Anchor 0 = port  side → /dev/uwb-anchor0
+# Anchor 1 = starboard  → /dev/uwb-anchor1
+
+SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
+    ATTRS{serial}=="ANCHOR0_SERIAL", \
+    SYMLINK+="uwb-anchor0", MODE="0666"
+
+SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
+    ATTRS{serial}=="ANCHOR1_SERIAL", \
+    SYMLINK+="uwb-anchor1", MODE="0666"

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_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/config/uwb_position_params.yaml

@@ -0,0 +1,58 @@
+# uwb_position_params.yaml — UWB position node configuration (Issue #546)
+#
+# ROS2 Python node: saltybot_uwb_position
+# Serial: ESP32 UWB tag → Jetson via USB-CDC (JSON per line)
+#
+# Usage:
+#   ros2 launch saltybot_uwb_position uwb_position.launch.py
+#
+# JSON protocol (ESP32 → Jetson):
+#   Full frame (preferred):
+#     {"ts": 123456, "ranges": [{"id": 0, "d_mm": 1500, "rssi": -65.0}, ...]}
+#   Per-anchor:
+#     {"id": 0, "d_mm": 1500, "rssi": -65.0}
+#   Both "d_mm" and "range_mm" accepted for the range field.
+
+uwb_position:
+  ros__parameters:
+
+    # ── Serial (USB-CDC from ESP32 DW3000 tag) ──────────────────────────────
+    serial_port:  /dev/ttyUSB0      # udev rule: /dev/ttyUWB_TAG recommended
+    baudrate:     115200
+
+    # ── Anchor definitions ──────────────────────────────────────────────────
+    # anchor_ids:        integer list of anchor IDs
+    # anchor_positions:  flat float list, 3 values per anchor [x, y, z] in
+    #                    the map frame (metres). Length must be 3 × len(anchor_ids).
+    #
+    # Example: 4-anchor rectangular room layout, anchors at 2 m height
+    #   Corner NW (0,0), NE (5,0), SE (5,5), SW (0,5)
+    anchor_ids:       [0, 1, 2, 3]
+    anchor_positions: [
+      0.0, 0.0, 2.0,    # anchor 0 — NW corner
+      5.0, 0.0, 2.0,    # anchor 1 — NE corner
+      5.0, 5.0, 2.0,    # anchor 2 — SE corner
+      0.0, 5.0, 2.0,    # anchor 3 — SW corner
+    ]
+
+    # ── Trilateration mode ──────────────────────────────────────────────────
+    solve_z:    false   # false = 2D (robot_z fixed), true = full 3D (needs 4+ anchors)
+    robot_z:    0.0     # m — robot floor height in map frame (used when solve_z=false)
+
+    # ── Validity & timing ───────────────────────────────────────────────────
+    publish_rate:    10.0   # Hz — /saltybot/uwb/pose + /saltybot/uwb/status
+    range_timeout_s:  1.5   # s  — discard anchor range after this age
+    min_range_m:      0.05  # m  — minimum valid range
+    max_range_m:     30.0   # m  — maximum valid range (DW3000: up to 120 m, use conservative)
+
+    # ── Outlier rejection ───────────────────────────────────────────────────
+    outlier_threshold_m: 0.40   # m  — residual above this → outlier
+    outlier_strikes_max: 5      # consecutive outlier frames before anchor is flagged
+
+    # ── Kalman filter ───────────────────────────────────────────────────────
+    kf_process_noise:    0.05   # Q — lower = smoother but slower to respond
+    kf_meas_noise:       0.10   # R — higher = trust KF prediction more than measurement
+
+    # ── TF2 frames ──────────────────────────────────────────────────────────
+    map_frame:  map        # parent frame
+    uwb_frame:  uwb_link   # child frame (robot UWB tag position)

jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py

@@ -0,0 +1,35 @@
+"""Launch file for saltybot_uwb_position (Issue #546)."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    config_path = os.path.join(
+        get_package_share_directory("saltybot_uwb_position"),
+        "config",
+        "uwb_position_params.yaml",
+    )
+
+    port_arg = DeclareLaunchArgument(
+        "serial_port",
+        default_value="/dev/ttyUSB0",
+        description="USB serial port for ESP32 UWB tag (e.g. /dev/ttyUWB_TAG)",
+    )
+
+    uwb_node = Node(
+        package="saltybot_uwb_position",
+        executable="uwb_position",
+        name="uwb_position",
+        parameters=[
+            config_path,
+            {"serial_port": LaunchConfiguration("serial_port")},
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([port_arg, uwb_node])

jetson/ros2_ws/src/saltybot_uwb_position/package.xml

@@ -0,0 +1,36 @@
+<?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_position</name>
+  <version>0.1.0</version>
+  <description>
+    ROS2 UWB position node for Jetson (Issue #546).
+    Reads JSON range data from an ESP32 DW3000 UWB tag over USB serial,
+    trilaterates from 3+ fixed infrastructure anchors, publishes
+    PoseStamped on /saltybot/uwb/pose, per-anchor UwbRange on
+    /saltybot/uwb/range/&lt;id&gt;, JSON diagnostics on /saltybot/uwb/status,
+    and broadcasts the uwb_link→map TF2 transform.
+  </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>std_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>saltybot_uwb_msgs</depend>
+
+  <!-- numpy is a system dep on Jetson (python3-numpy) -->
+  <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_position/saltybot_uwb_position/trilateration.py

@@ -0,0 +1,254 @@
+"""
+trilateration.py — Pure-math helpers for UWB trilateration (Issue #546).
+
+No ROS2 dependencies — importable in unit tests without a ROS2 install.
+
+Algorithm: linear least-squares from N ≥ 3 anchors
+────────────────────────────────────────────────────
+Given anchors A_i = (x_i, y_i, z_i) with measured ranges r_i, the robot
+position p = (x, y, z) satisfies:
+
+    (x - x_i)² + (y - y_i)² + (z - z_i)² = r_i²
+
+Subtract the equation for anchor 0 from each subsequent equation to
+linearise:
+
+    2(x_i - x_0)x + 2(y_i - y_0)y + 2(z_i - z_0)z
+        = r_0² - r_i² + ‖a_i‖² - ‖a_0‖²
+
+This yields A·p = b where A is (N-1)×3 and b is (N-1)×1.
+Solve with numpy least-squares (lstsq).
+
+2D mode (solve_z=False):
+    z is fixed (robot_z parameter, default 0).
+    Reduce to 2 unknowns (x, y): subtract z terms from b, drop the z column.
+    Needs N ≥ 3 anchors.
+
+Outlier rejection:
+    Compute residual for each anchor: |r_meas - ‖p - a_i‖|.
+    Reject anchors with residual > threshold.  Repeat if enough remain.
+"""
+
+from __future__ import annotations
+
+import math
+from typing import List, Optional, Tuple
+
+import numpy as np
+
+
+AnchorPos = Tuple[float, float, float]   # (x, y, z) in map frame (metres)
+RangeM    = float                         # measured range (metres)
+
+
+# ── Core trilateration ────────────────────────────────────────────────────────
+
+def trilaterate(
+    anchors: List[AnchorPos],
+    ranges:  List[RangeM],
+    fixed_z: Optional[float] = None,
+) -> Tuple[float, float, float]:
+    """Least-squares trilateration from N ≥ 3 anchor ranges.
+
+    Parameters
+    ----------
+    anchors   : list of (x, y, z) anchor positions in the map frame (metres)
+    ranges    : measured distances from robot to each anchor (metres)
+    fixed_z   : if not None, treat robot z as this value and solve 2D only
+
+    Returns
+    -------
+    (x, y, z) : estimated robot position in the map frame (metres).
+                When fixed_z is given, z = fixed_z.
+
+    Raises
+    ------
+    ValueError  : fewer than 3 anchors provided
+    RuntimeError: linear system is rank-deficient (e.g., collinear anchors)
+    """
+    n = len(anchors)
+    if n < 3:
+        raise ValueError(f"Need ≥ 3 anchors for trilateration, got {n}")
+    if len(ranges) != n:
+        raise ValueError("len(anchors) != len(ranges)")
+
+    a = np.array(anchors, dtype=np.float64)   # (N, 3)
+    r = np.array(ranges,  dtype=np.float64)   # (N,)
+
+    # Reference anchor (index 0)
+    a0 = a[0]
+    r0 = r[0]
+    norm_a0_sq = float(np.dot(a0, a0))
+
+    if fixed_z is not None:
+        # 2D mode: solve for (x, y), z is known
+        z = fixed_z
+        A_rows = []
+        b_rows = []
+        for i in range(1, n):
+            ai = a[i]
+            ri = r[i]
+            norm_ai_sq = float(np.dot(ai, ai))
+            # rhs adjusted for known z
+            rhs = (r0**2 - ri**2 + norm_ai_sq - norm_a0_sq
+                   + 2.0 * (ai[2] - a0[2]) * z)
+            A_rows.append([2.0 * (ai[0] - a0[0]),
+                           2.0 * (ai[1] - a0[1])])
+            b_rows.append(rhs)
+
+        A_mat = np.array(A_rows, dtype=np.float64)
+        b_vec = np.array(b_rows, dtype=np.float64)
+        result, _, rank, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None)
+        if rank < 2:
+            raise RuntimeError("Rank-deficient 2D trilateration system — collinear anchors?")
+        return float(result[0]), float(result[1]), z
+
+    else:
+        # 3D mode: solve for (x, y, z) — needs N ≥ 4 for well-conditioned solve
+        A_rows = []
+        b_rows = []
+        for i in range(1, n):
+            ai = a[i]
+            ri = r[i]
+            norm_ai_sq = float(np.dot(ai, ai))
+            rhs = r0**2 - ri**2 + norm_ai_sq - norm_a0_sq
+            A_rows.append([2.0 * (ai[0] - a0[0]),
+                           2.0 * (ai[1] - a0[1]),
+                           2.0 * (ai[2] - a0[2])])
+            b_rows.append(rhs)
+
+        A_mat = np.array(A_rows, dtype=np.float64)
+        b_vec = np.array(b_rows, dtype=np.float64)
+        result, _, rank, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None)
+        if rank < 3:
+            raise RuntimeError("Rank-deficient 3D trilateration system — coplanar anchors?")
+        return float(result[0]), float(result[1]), float(result[2])
+
+
+# ── Outlier rejection ─────────────────────────────────────────────────────────
+
+def reject_outliers(
+    anchors:    List[AnchorPos],
+    ranges:     List[RangeM],
+    position:   Tuple[float, float, float],
+    threshold_m: float = 0.4,
+) -> List[int]:
+    """Return indices of inlier anchors (residual ≤ threshold_m).
+
+    Parameters
+    ----------
+    anchors     : anchor positions in map frame
+    ranges      : measured ranges (metres)
+    position    : current position estimate (x, y, z)
+    threshold_m : max allowable range residual to be an inlier
+
+    Returns
+    -------
+    inlier_indices : sorted list of indices whose residual is within threshold
+    """
+    px, py, pz = position
+    inliers = []
+    for i, (anchor, r_meas) in enumerate(zip(anchors, ranges)):
+        ax, ay, az = anchor
+        r_pred = math.sqrt((px - ax)**2 + (py - ay)**2 + (pz - az)**2)
+        residual = abs(r_meas - r_pred)
+        if residual <= threshold_m:
+            inliers.append(i)
+    return inliers
+
+
+def residuals(
+    anchors:  List[AnchorPos],
+    ranges:   List[RangeM],
+    position: Tuple[float, float, float],
+) -> List[float]:
+    """Compute per-anchor range residual (metres) for diagnostics."""
+    px, py, pz = position
+    res = []
+    for anchor, r_meas in zip(anchors, ranges):
+        ax, ay, az = anchor
+        r_pred = math.sqrt((px - ax)**2 + (py - ay)**2 + (pz - az)**2)
+        res.append(abs(r_meas - r_pred))
+    return res
+
+
+# ── 3D Kalman filter (constant-velocity, position-only observations) ──────────
+
+class KalmanFilter3D:
+    """Constant-velocity Kalman filter for 3D UWB position.
+
+    State: [x, y, z, vx, vy, vz]ᵀ
+    Observation: [x, y, z]  (position only)
+    """
+
+    def __init__(
+        self,
+        process_noise:    float = 0.1,
+        measurement_noise: float = 0.15,
+        dt: float = 0.1,
+    ) -> None:
+        self._q    = process_noise
+        self._r    = measurement_noise
+        self._dt   = dt
+        self._x    = np.zeros(6)
+        self._P    = np.diag([1.0, 1.0, 1.0, 0.5, 0.5, 0.5])
+        self._init = False
+
+    def _build_F(self, dt: float) -> np.ndarray:
+        F = np.eye(6)
+        F[0, 3] = dt
+        F[1, 4] = dt
+        F[2, 5] = dt
+        return F
+
+    def _build_Q(self, dt: float) -> np.ndarray:
+        q  = self._q
+        dt2 = dt * dt
+        dt3 = dt2 * dt
+        dt4 = dt3 * dt
+        Q = np.zeros((6, 6))
+        # Position-velocity cross terms (constant white-noise model)
+        for i in range(3):
+            Q[i,   i  ] = q * dt4 / 4.0
+            Q[i,   i+3] = q * dt3 / 2.0
+            Q[i+3, i  ] = q * dt3 / 2.0
+            Q[i+3, i+3] = q * dt2
+        return Q
+
+    def predict(self, dt: float | None = None) -> None:
+        if dt is not None:
+            self._dt = dt
+        F = self._build_F(self._dt)
+        Q = self._build_Q(self._dt)
+        self._x = F @ self._x
+        self._P = F @ self._P @ F.T + Q
+
+    def update(self, x_m: float, y_m: float, z_m: float) -> None:
+        if not self._init:
+            self._x[:3] = [x_m, y_m, z_m]
+            self._init = True
+            return
+
+        H = np.zeros((3, 6))
+        H[0, 0] = 1.0
+        H[1, 1] = 1.0
+        H[2, 2] = 1.0
+        R = np.eye(3) * self._r
+
+        z_vec = np.array([x_m, y_m, z_m])
+        innov = z_vec - H @ self._x
+        S     = H @ self._P @ H.T + R
+        K     = self._P @ H.T @ np.linalg.inv(S)
+        self._x = self._x + K @ innov
+        self._P = (np.eye(6) - K @ H) @ self._P
+
+    def position(self) -> Tuple[float, float, float]:
+        return float(self._x[0]), float(self._x[1]), float(self._x[2])
+
+    def velocity(self) -> Tuple[float, float, float]:
+        return float(self._x[3]), float(self._x[4]), float(self._x[5])
+
+    def reset(self) -> None:
+        self._x    = np.zeros(6)
+        self._P    = np.diag([1.0, 1.0, 1.0, 0.5, 0.5, 0.5])
+        self._init = False

jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py

@@ -0,0 +1,487 @@
+#!/usr/bin/env python3
+"""
+uwb_position_node.py — ROS2 UWB position node for Jetson (Issue #546).
+
+Reads JSON range measurements from an ESP32 UWB tag (DW3000) over USB
+serial, trilaterates position from 3+ fixed infrastructure anchors, and
+publishes the robot's absolute position in the map frame.
+
+Serial protocol (one JSON object per line, UTF-8):
+────────────────────────────────────────────────────
+  Tag → Jetson (full frame, all anchors at once):
+    {"ts": 123456, "ranges": [
+        {"id": 0, "d_mm": 1500, "rssi": -65.0},
+        {"id": 1, "d_mm": 2100, "rssi": -68.0},
+        {"id": 2, "d_mm": 1800, "rssi": -70.0}
+    ]}
+
+  Alternate (per-anchor, one line per measurement):
+    {"id": 0, "d_mm": 1500, "rssi": -65.0}
+
+  Both "d_mm" and "range_mm" are accepted.
+
+Anchor positions:
+─────────────────
+  Fixed anchors are configured in uwb_position_params.yaml as flat arrays:
+    anchor_ids:       [0, 1, 2, 3]
+    anchor_positions: [x0, y0, z0, x1, y1, z1, ...]   # metres in map frame
+
+Publishes:
+──────────
+  /saltybot/uwb/pose          (geometry_msgs/PoseStamped)    10 Hz
+  /saltybot/uwb/range/<id>    (saltybot_uwb_msgs/UwbRange)   per anchor, on arrival
+  /saltybot/uwb/status        (std_msgs/String)              10 Hz JSON diagnostics
+
+TF2:
+────
+  Broadcasts  uwb_link → map  from /saltybot/uwb/pose position.
+
+Outlier rejection:
+──────────────────
+  After initial trilateration, anchors with range residual > outlier_threshold_m
+  are dropped.  If ≥ 3 inliers remain (2D mode) the position is re-solved.
+  Anchors rejected in N_strikes_max consecutive frames are flagged in status.
+"""
+
+import json
+import math
+import threading
+import time
+from typing import Dict, List, Optional, Tuple
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from geometry_msgs.msg import (
+    PoseStamped, TransformStamped,
+    Quaternion,
+)
+from std_msgs.msg import Header, String
+from tf2_ros import TransformBroadcaster
+
+from saltybot_uwb_msgs.msg import UwbRange
+
+from saltybot_uwb_position.trilateration import (
+    trilaterate,
+    reject_outliers,
+    residuals,
+    KalmanFilter3D,
+)
+
+try:
+    import serial
+    _SERIAL_OK = True
+except ImportError:
+    _SERIAL_OK = False
+
+
+# ── Serial reader thread ──────────────────────────────────────────────────────
+
+class SerialJsonReader(threading.Thread):
+    """Background thread: reads newline-delimited JSON from a serial port."""
+
+    def __init__(self, port: str, baudrate: int, callback, logger) -> None:
+        super().__init__(daemon=True)
+        self._port     = port
+        self._baudrate = baudrate
+        self._callback = callback
+        self._logger   = logger
+        self._running  = False
+        self._ser      = None
+
+    def run(self) -> None:
+        self._running = True
+        while self._running:
+            try:
+                self._ser = serial.Serial(
+                    self._port, self._baudrate, timeout=1.0
+                )
+                self._logger.info(f"UWB serial: opened {self._port} @ {self._baudrate}")
+                self._read_loop()
+            except Exception as exc:
+                self._logger.warning(
+                    f"UWB serial error ({self._port}): {exc} — retry in 2 s"
+                )
+                if self._ser and self._ser.is_open:
+                    self._ser.close()
+                time.sleep(2.0)
+
+    def _read_loop(self) -> None:
+        while self._running:
+            try:
+                raw = self._ser.readline()
+                if not raw:
+                    continue
+                line = raw.decode("utf-8", errors="replace").strip()
+                if not line:
+                    continue
+                try:
+                    obj = json.loads(line)
+                    self._callback(obj)
+                except json.JSONDecodeError:
+                    pass  # ignore malformed lines silently
+            except Exception as exc:
+                self._logger.warning(f"UWB read error: {exc}")
+                break
+
+    def stop(self) -> None:
+        self._running = False
+        if self._ser and self._ser.is_open:
+            self._ser.close()
+
+
+# ── Node ──────────────────────────────────────────────────────────────────────
+
+class UwbPositionNode(Node):
+    """ROS2 UWB position node — trilateration from 3+ fixed anchors."""
+
+    _MIN_ANCHORS_2D = 3
+    _MIN_ANCHORS_3D = 4
+
+    def __init__(self) -> None:
+        super().__init__("uwb_position")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",          "/dev/ttyUSB0")
+        self.declare_parameter("baudrate",             115200)
+
+        # Anchor configuration: flat arrays, 3 floats per anchor
+        self.declare_parameter("anchor_ids",       [0, 1, 2])
+        self.declare_parameter("anchor_positions", [
+            0.0, 0.0, 2.0,   # anchor 0: x, y, z (metres in map frame)
+            5.0, 0.0, 2.0,   # anchor 1
+            5.0, 5.0, 2.0,   # anchor 2
+        ])
+
+        self.declare_parameter("solve_z",              False)   # 2D mode by default
+        self.declare_parameter("robot_z",              0.0)     # m — robot floor height
+        self.declare_parameter("publish_rate",         10.0)    # Hz — pose + status
+        self.declare_parameter("range_timeout_s",      1.5)     # s — stale range threshold
+        self.declare_parameter("max_range_m",          30.0)    # m — validity max
+        self.declare_parameter("min_range_m",          0.05)    # m — validity min
+        self.declare_parameter("outlier_threshold_m",  0.4)     # m — RANSAC threshold
+        self.declare_parameter("outlier_strikes_max",  5)       # frames before flagging
+        self.declare_parameter("kf_process_noise",     0.05)    # Kalman Q
+        self.declare_parameter("kf_meas_noise",        0.10)    # Kalman R
+        self.declare_parameter("map_frame",            "map")
+        self.declare_parameter("uwb_frame",            "uwb_link")
+
+        # Load params
+        self._port         = self.get_parameter("serial_port").value
+        self._baudrate     = self.get_parameter("baudrate").value
+        self._solve_z      = self.get_parameter("solve_z").value
+        self._robot_z      = self.get_parameter("robot_z").value
+        self._publish_rate = self.get_parameter("publish_rate").value
+        self._timeout      = self.get_parameter("range_timeout_s").value
+        self._max_r        = self.get_parameter("max_range_m").value
+        self._min_r        = self.get_parameter("min_range_m").value
+        self._outlier_thr  = self.get_parameter("outlier_threshold_m").value
+        self._strikes_max  = self.get_parameter("outlier_strikes_max").value
+        self._map_frame    = self.get_parameter("map_frame").value
+        self._uwb_frame    = self.get_parameter("uwb_frame").value
+
+        # Parse anchor config
+        anchor_ids_raw  = self.get_parameter("anchor_ids").value
+        anchor_pos_flat = self.get_parameter("anchor_positions").value
+        self._anchor_ids, self._anchor_positions = self._parse_anchors(
+            anchor_ids_raw, anchor_pos_flat
+        )
+        self.get_logger().info(
+            f"UWB anchors: {[f'#{i}@({p[0]:.1f},{p[1]:.1f},{p[2]:.1f})' for i,p in zip(self._anchor_ids, self._anchor_positions)]}"
+        )
+
+        # ── State ─────────────────────────────────────────────────────────────
+        self._lock   = threading.Lock()
+        # anchor_id → (range_m, rssi, timestamp_monotonic)
+        self._ranges: Dict[int, Tuple[float, float, float]] = {}
+        self._strikes: Dict[int, int] = {aid: 0 for aid in self._anchor_ids}
+        self._last_fix: Optional[Tuple[float, float, float]] = None
+        self._has_fix  = False
+        self._fix_age  = 0.0
+
+        # Kalman filter
+        self._kf = KalmanFilter3D(
+            process_noise=self.get_parameter("kf_process_noise").value,
+            measurement_noise=self.get_parameter("kf_meas_noise").value,
+            dt=1.0 / self._publish_rate,
+        )
+
+        # Per-anchor outlier publishers (pre-create for configured anchors)
+        sensor_qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=5,
+        )
+        self._range_pubs: Dict[int, object] = {}
+        for aid in self._anchor_ids:
+            self._range_pubs[aid] = self.create_publisher(
+                UwbRange, f"/saltybot/uwb/range/{aid}", sensor_qos
+            )
+
+        # Main publishers
+        self._pose_pub   = self.create_publisher(PoseStamped, "/saltybot/uwb/pose",   10)
+        self._status_pub = self.create_publisher(String,      "/saltybot/uwb/status", 10)
+
+        # TF2
+        self._tf_broadcaster = TransformBroadcaster(self)
+
+        # ── Serial reader ──────────────────────────────────────────────────────
+        if _SERIAL_OK:
+            self._reader = SerialJsonReader(
+                self._port, self._baudrate,
+                callback=self._on_serial_json,
+                logger=self.get_logger(),
+            )
+            self._reader.start()
+        else:
+            self.get_logger().warning(
+                "pyserial not installed — serial I/O disabled (simulation mode)"
+            )
+
+        # ── Publish timer ──────────────────────────────────────────────────────
+        self._prev_publish_t = self.get_clock().now().nanoseconds * 1e-9
+        self.create_timer(1.0 / self._publish_rate, self._publish_cb)
+
+        self.get_logger().info(
+            f"UwbPositionNode ready — port={self._port} "
+            f"anchors={len(self._anchor_ids)} "
+            f"mode={'3D' if self._solve_z else '2D'} "
+            f"rate={self._publish_rate:.0f}Hz"
+        )
+
+    # ── Anchor config parsing ─────────────────────────────────────────────────
+
+    def _parse_anchors(
+        self,
+        ids_raw,
+        pos_flat,
+    ):
+        ids = list(ids_raw)
+        n   = len(ids)
+        if len(pos_flat) < n * 3:
+            raise ValueError(
+                f"anchor_positions must have 3×anchor_ids entries. "
+                f"Got {len(pos_flat)} values for {n} anchors."
+            )
+        positions = []
+        for i in range(n):
+            base = i * 3
+            positions.append((
+                float(pos_flat[base]),
+                float(pos_flat[base + 1]),
+                float(pos_flat[base + 2]),
+            ))
+        return ids, positions
+
+    # ── Serial JSON callback (called from reader thread) ──────────────────────
+
+    def _on_serial_json(self, obj: dict) -> None:
+        """Parse incoming JSON and update range table."""
+        now = time.monotonic()
+
+        if "ranges" in obj:
+            # Full-frame format: {"ts": ..., "ranges": [{id, d_mm, rssi}, ...]}
+            for entry in obj["ranges"]:
+                self._ingest_range_entry(entry, now)
+        else:
+            # Per-anchor format: {"id": 0, "d_mm": 1500, "rssi": -65.0}
+            self._ingest_range_entry(obj, now)
+
+    def _ingest_range_entry(self, entry: dict, timestamp: float) -> None:
+        """Store a single anchor range measurement after validity checks."""
+        try:
+            anchor_id = int(entry.get("id", entry.get("anchor_id", -1)))
+            # Accept both "d_mm" and "range_mm"
+            raw_mm    = int(entry.get("d_mm", entry.get("range_mm", 0)))
+            rssi      = float(entry.get("rssi", 0.0))
+        except (KeyError, TypeError, ValueError):
+            return
+
+        if anchor_id not in self._anchor_ids:
+            return  # unknown / unconfigured anchor
+
+        range_m = raw_mm / 1000.0
+        if range_m < self._min_r or range_m > self._max_r:
+            return  # outside validity window
+
+        with self._lock:
+            self._ranges[anchor_id] = (range_m, rssi, timestamp)
+
+        # Publish per-anchor range topic immediately
+        self._publish_anchor_range(anchor_id, range_m, raw_mm, rssi)
+
+    # ── Per-anchor range publisher ────────────────────────────────────────────
+
+    def _publish_anchor_range(
+        self, anchor_id: int, range_m: float, raw_mm: int, rssi: float
+    ) -> None:
+        if anchor_id not in self._range_pubs:
+            return
+        msg = UwbRange()
+        msg.header.stamp    = self.get_clock().now().to_msg()
+        msg.header.frame_id = self._map_frame
+        msg.anchor_id       = anchor_id
+        msg.range_m         = float(range_m)
+        msg.raw_mm          = int(raw_mm)
+        msg.rssi            = float(rssi)
+        msg.tag_id          = ""
+        self._range_pubs[anchor_id].publish(msg)
+
+    # ── Main publish callback (10 Hz) ─────────────────────────────────────────
+
+    def _publish_cb(self) -> None:
+        now_ros  = self.get_clock().now()
+        now_mono = time.monotonic()
+
+        # Collect fresh ranges
+        with self._lock:
+            fresh = {
+                aid: entry
+                for aid, entry in self._ranges.items()
+                if (now_mono - entry[2]) <= self._timeout
+            }
+
+        # Check minimum anchor count for chosen mode
+        min_anch = self._MIN_ANCHORS_3D if self._solve_z else self._MIN_ANCHORS_2D
+        state    = "no_fix"
+        pos      = None
+        used_ids: List[int] = []
+        res_map: Dict[int, float] = {}
+
+        if len(fresh) >= min_anch:
+            active_ids = list(fresh.keys())
+            anch_pos   = [self._anchor_positions[self._anchor_ids.index(i)]
+                          for i in active_ids]
+            anch_r     = [fresh[i][0] for i in active_ids]
+
+            try:
+                fixed_z = None if self._solve_z else self._robot_z
+                raw_pos = trilaterate(anch_pos, anch_r, fixed_z=fixed_z)
+
+                # Outlier rejection
+                inlier_idx = reject_outliers(
+                    anch_pos, anch_r, raw_pos,
+                    threshold_m=self._outlier_thr,
+                )
+                res_all = residuals(anch_pos, anch_r, raw_pos)
+                for k, aid in enumerate(active_ids):
+                    res_map[aid] = round(res_all[k], 3)
+
+                # Update consecutive strike counters
+                for k, aid in enumerate(active_ids):
+                    if k in inlier_idx:
+                        self._strikes[aid] = 0
+                    else:
+                        self._strikes[aid] = self._strikes.get(aid, 0) + 1
+
+                # Re-solve with inliers if any were rejected
+                if len(inlier_idx) < len(active_ids) and len(inlier_idx) >= min_anch:
+                    inlier_anch = [anch_pos[k] for k in inlier_idx]
+                    inlier_r    = [anch_r[k]   for k in inlier_idx]
+                    raw_pos     = trilaterate(inlier_anch, inlier_r, fixed_z=fixed_z)
+
+                used_ids = [active_ids[k] for k in inlier_idx]
+                pos = raw_pos
+
+                # Kalman predict + update
+                dt = now_ros.nanoseconds * 1e-9 - self._prev_publish_t
+                self._kf.predict(dt=dt)
+                self._kf.update(pos[0], pos[1], pos[2])
+                pos = self._kf.position()
+
+                self._last_fix = pos
+                self._has_fix  = True
+                self._fix_age  = 0.0
+                state = "ok" if len(used_ids) >= min_anch else "degraded"
+
+            except (ValueError, RuntimeError) as exc:
+                self.get_logger().warning(f"Trilateration failed: {exc}")
+                state = "degraded"
+
+        elif self._has_fix:
+            # KF predict-only: coast on last known position
+            dt = now_ros.nanoseconds * 1e-9 - self._prev_publish_t
+            self._kf.predict(dt=dt)
+            pos = self._kf.position()
+            self._fix_age += 1.0 / self._publish_rate
+            state = "degraded" if self._fix_age < 5.0 else "no_fix"
+
+        self._prev_publish_t = now_ros.nanoseconds * 1e-9
+
+        if pos is None:
+            self._publish_status(state, 0, [], {})
+            return
+
+        x, y, z = pos
+
+        # ── Publish PoseStamped ────────────────────────────────────────────
+        pose_msg = PoseStamped()
+        pose_msg.header.stamp    = now_ros.to_msg()
+        pose_msg.header.frame_id = self._map_frame
+        pose_msg.pose.position.x = x
+        pose_msg.pose.position.y = y
+        pose_msg.pose.position.z = z
+        pose_msg.pose.orientation.w = 1.0  # no orientation from UWB alone
+        self._pose_pub.publish(pose_msg)
+
+        # ── TF2: uwb_link → map ───────────────────────────────────────────
+        tf_msg = TransformStamped()
+        tf_msg.header.stamp    = now_ros.to_msg()
+        tf_msg.header.frame_id = self._map_frame
+        tf_msg.child_frame_id  = self._uwb_frame
+        tf_msg.transform.translation.x = x
+        tf_msg.transform.translation.y = y
+        tf_msg.transform.translation.z = z
+        tf_msg.transform.rotation.w    = 1.0
+        self._tf_broadcaster.sendTransform(tf_msg)
+
+        # ── Diagnostics ───────────────────────────────────────────────────
+        self._publish_status(state, len(used_ids), used_ids, res_map)
+
+    # ── Status publisher ──────────────────────────────────────────────────────
+
+    def _publish_status(
+        self,
+        state:     str,
+        n_anchors: int,
+        used_ids:  List[int],
+        res_map:   Dict[int, float],
+    ) -> None:
+        # Flag anchors with too many consecutive outlier strikes
+        flagged = [
+            aid for aid, s in self._strikes.items() if s >= self._strikes_max
+        ]
+        pos = self._kf.position() if self._has_fix else None
+        status = {
+            "state":          state,
+            "active_anchors": n_anchors,
+            "used_anchor_ids": used_ids,
+            "flagged_anchors": flagged,
+            "position": {
+                "x": round(pos[0], 3),
+                "y": round(pos[1], 3),
+                "z": round(pos[2], 3),
+            } if pos else None,
+            "residuals_m": res_map,
+            "fix_age_s":   round(self._fix_age, 2),
+        }
+        self._status_pub.publish(String(data=json.dumps(status)))
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = UwbPositionNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg

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

jetson/ros2_ws/src/saltybot_uwb_position/setup.py

@@ -0,0 +1,32 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = "saltybot_uwb_position"
+
+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="ROS2 UWB position node — JSON serial bridge with trilateration (Issue #546)",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "uwb_position = saltybot_uwb_position.uwb_position_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()

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/diagnostics_panel.css

@@ -0,0 +1,230 @@
+/* diagnostics_panel.css — Saltybot System Diagnostics Dashboard (Issue #562) */
+
+*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
+
+:root {
+  --bg0:       #050510;
+  --bg1:       #070712;
+  --bg2:       #0a0a1a;
+  --border:    #0c2a3a;
+  --border2:   #1e3a5f;
+  --text-dim:  #374151;
+  --text-mid:  #6b7280;
+  --text-base: #9ca3af;
+  --text-hi:   #d1d5db;
+  --cyan:      #06b6d4;
+  --cyan-dim:  #0e4f69;
+  --green:     #22c55e;
+  --amber:     #f59e0b;
+  --red:       #ef4444;
+  --orange:    #f97316;
+}
+
+body {
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: var(--bg0);
+  color: var(--text-base);
+  min-height: 100dvh;
+  display: flex;
+  flex-direction: column;
+}
+
+/* ── Header ── */
+#header {
+  display: flex;
+  align-items: center;
+  justify-content: space-between;
+  padding: 6px 16px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--border);
+  flex-shrink: 0;
+  flex-wrap: wrap;
+  gap: 8px;
+}
+.logo { color: #f97316; font-weight: bold; letter-spacing: 0.15em; font-size: 13px; }
+
+#conn-bar { display: flex; align-items: center; gap: 6px; }
+#conn-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--text-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:0.4; } }
+
+#ws-input {
+  background: var(--bg2); border: 1px solid var(--border2); border-radius: 4px;
+  color: #67e8f9; padding: 2px 8px; font-family: monospace; font-size: 11px; width: 200px;
+}
+#ws-input:focus { outline: none; border-color: var(--cyan); }
+
+.hdr-btn {
+  padding: 3px 10px; border-radius: 4px; border: 1px solid var(--border2);
+  background: var(--bg2); color: #67e8f9; font-family: monospace;
+  font-size: 10px; font-weight: bold; cursor: pointer; transition: background 0.15s;
+}
+.hdr-btn:hover { background: var(--cyan-dim); }
+
+#refresh-info {
+  font-size: 10px; color: var(--text-mid);
+  display: flex; align-items: center; gap: 4px;
+}
+#refresh-dot {
+  width: 6px; height: 6px; border-radius: 50%;
+  background: var(--text-dim); transition: background 0.2s;
+}
+#refresh-dot.pulse { background: var(--cyan); animation: blink 0.4s; }
+
+/* ── Status bar ── */
+#status-bar {
+  display: flex; gap: 8px; align-items: center;
+  padding: 4px 16px; background: var(--bg1);
+  border-bottom: 1px solid var(--border); font-size: 10px; flex-wrap: wrap;
+}
+.sys-badge {
+  padding: 2px 8px; border-radius: 3px; font-weight: bold;
+  border: 1px solid; letter-spacing: 0.05em;
+}
+.badge-ok    { background: #052e16; border-color: #166534; color: #4ade80; }
+.badge-warn  { background: #451a03; border-color: #92400e; color: #fcd34d; }
+.badge-error { background: #450a0a; border-color: #991b1b; color: #f87171; animation: blink 1s infinite; }
+.badge-stale { background: #111827; border-color: #374151; color: #6b7280; }
+#last-update { color: var(--text-mid); margin-left: auto; }
+
+/* ── Dashboard grid ── */
+#dashboard {
+  flex: 1;
+  display: grid;
+  grid-template-columns: repeat(3, 1fr);
+  grid-template-rows: auto;
+  gap: 12px;
+  padding: 12px;
+  align-content: start;
+  overflow-y: auto;
+}
+
+@media (max-width: 1024px) { #dashboard { grid-template-columns: repeat(2, 1fr); } }
+@media (max-width: 640px)  { #dashboard { grid-template-columns: 1fr; } }
+
+/* Spanning cards */
+.span2 { grid-column: span 2; }
+.span3 { grid-column: 1 / -1; }
+
+/* ── Card ── */
+.card {
+  background: var(--bg1);
+  border: 1px solid var(--border);
+  border-radius: 8px;
+  padding: 10px 12px;
+  display: flex;
+  flex-direction: column;
+  gap: 8px;
+}
+.card.alert-warn  { border-color: #92400e; }
+.card.alert-error { border-color: #991b1b; }
+
+.card-title {
+  font-size: 9px; font-weight: bold; letter-spacing: 0.15em;
+  color: #0891b2; text-transform: uppercase;
+  display: flex; align-items: center; justify-content: space-between;
+}
+.card-title .badge { font-size: 9px; padding: 1px 6px; border-radius: 3px; }
+
+/* ── Gauge bar ── */
+.gauge-row {
+  display: flex; flex-direction: column; gap: 3px;
+}
+.gauge-label-row {
+  display: flex; justify-content: space-between; font-size: 10px;
+}
+.gauge-label { color: var(--text-mid); }
+.gauge-value { font-family: monospace; }
+.gauge-track {
+  width: 100%; height: 8px;
+  background: var(--bg2); border-radius: 4px;
+  overflow: hidden; border: 1px solid var(--border2);
+}
+.gauge-fill {
+  height: 100%; border-radius: 4px;
+  transition: width 0.5s ease, background 0.5s ease;
+}
+
+/* ── Big metric ── */
+.big-metric {
+  display: flex; align-items: baseline; gap: 4px;
+}
+.big-num { font-size: 28px; font-weight: bold; font-family: monospace; }
+.big-unit { font-size: 11px; color: var(--text-mid); }
+
+/* ── Sparkline ── */
+#batt-sparkline {
+  width: 100%; border-radius: 4px;
+  border: 1px solid var(--border2);
+  background: var(--bg2);
+  display: block;
+}
+
+/* ── Node list ── */
+.node-row {
+  display: flex; align-items: center; justify-content: space-between;
+  padding: 3px 0; border-bottom: 1px solid var(--border);
+  font-size: 10px;
+}
+.node-row:last-child { border-bottom: none; }
+.node-name { color: var(--text-base); font-family: monospace; }
+.node-status {
+  padding: 1px 6px; border-radius: 3px; font-size: 9px; font-weight: bold;
+}
+.ns-ok    { background: #052e16; color: #4ade80; }
+.ns-warn  { background: #451a03; color: #fcd34d; }
+.ns-error { background: #450a0a; color: #f87171; }
+.ns-stale { background: #111827; color: #6b7280; }
+
+/* ── Temp arc display ── */
+.temp-grid {
+  display: grid; grid-template-columns: repeat(2, 1fr); gap: 6px;
+}
+.temp-box {
+  background: var(--bg2); border: 1px solid var(--border2);
+  border-radius: 6px; padding: 6px; text-align: center;
+}
+.temp-label { font-size: 9px; color: var(--text-mid); margin-bottom: 2px; }
+.temp-value { font-size: 22px; font-weight: bold; font-family: monospace; }
+.temp-bar-track {
+  margin-top: 4px; width: 100%; height: 4px;
+  background: var(--bg0); border-radius: 2px; overflow: hidden;
+}
+.temp-bar-fill { height: 100%; border-radius: 2px; transition: width 0.5s; }
+
+/* ── Motor section ── */
+.motor-grid {
+  display: grid; grid-template-columns: repeat(2, 1fr); gap: 8px;
+}
+.motor-box {
+  background: var(--bg2); border: 1px solid var(--border2);
+  border-radius: 6px; padding: 8px;
+}
+.motor-label { font-size: 9px; color: var(--text-mid); margin-bottom: 4px; }
+
+/* ── WiFi bars ── */
+.rssi-bars { display: flex; align-items: flex-end; gap: 3px; }
+.rssi-bar { width: 6px; border-radius: 2px 2px 0 0; }
+
+/* ── MQTT indicator ── */
+.mqtt-status { display: flex; align-items: center; gap: 6px; }
+.mqtt-dot {
+  width: 10px; height: 10px; border-radius: 50%;
+  background: var(--text-dim); transition: background 0.3s;
+}
+.mqtt-dot.connected { background: var(--green); animation: none; }
+.mqtt-dot.disconnected { background: var(--red); animation: blink 1s infinite; }
+
+/* ── Footer ── */
+#footer {
+  background: var(--bg1); border-top: 1px solid var(--border);
+  padding: 4px 16px;
+  display: flex; align-items: center; justify-content: space-between;
+  flex-shrink: 0; font-size: 10px; color: var(--text-dim);
+}

ui/diagnostics_panel.html

@@ -0,0 +1,267 @@
+<!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 — System Diagnostics</title>
+<link rel="stylesheet" href="diagnostics_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 — DIAGNOSTICS</div>
+
+  <div id="conn-bar">
+    <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="hdr-btn">CONNECT</button>
+    <span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
+  </div>
+
+  <div id="refresh-info">
+    <div id="refresh-dot"></div>
+    <span>auto 2 Hz</span>
+  </div>
+</div>
+
+<!-- ── System status bar ── -->
+<div id="status-bar">
+  <span style="color:#6b7280;font-size:10px">SYSTEM</span>
+  <span class="sys-badge badge-stale" id="system-badge">STALE</span>
+  <span style="color:#4b5563">│</span>
+  <span style="color:#6b7280;font-size:10px">BATTERY</span>
+  <span class="sys-badge badge-stale" id="batt-badge">—</span>
+  <span style="color:#4b5563">│</span>
+  <span style="color:#6b7280;font-size:10px">THERMAL</span>
+  <span class="sys-badge badge-stale" id="temp-badge">—</span>
+  <span style="color:#4b5563">│</span>
+  <span style="color:#6b7280;font-size:10px">NETWORK</span>
+  <span class="sys-badge badge-stale" id="net-badge">—</span>
+  <span id="last-update">Awaiting data…</span>
+</div>
+
+<!-- ── Dashboard grid ── -->
+<div id="dashboard">
+
+  <!-- ╔═══════════════════ BATTERY ═══════════════════╗ -->
+  <div class="card span2">
+    <div class="card-title">
+      BATTERY — 4S LiPo (12.0–16.8 V)
+      <span class="badge badge-stale" id="batt-badge-2" style="font-size:9px">—</span>
+    </div>
+
+    <!-- Big readout row -->
+    <div style="display:flex;gap:16px;align-items:flex-end;flex-wrap:wrap">
+      <div>
+        <div style="font-size:9px;color:#6b7280;margin-bottom:2px">VOLTAGE</div>
+        <div class="big-metric">
+          <span class="big-num" id="batt-voltage" style="color:#22c55e">—</span>
+        </div>
+      </div>
+      <div>
+        <div style="font-size:9px;color:#6b7280;margin-bottom:2px">SOC</div>
+        <div class="big-metric">
+          <span class="big-num" id="batt-soc" style="color:#22c55e">—</span>
+        </div>
+      </div>
+      <div>
+        <div style="font-size:9px;color:#6b7280;margin-bottom:2px">CURRENT</div>
+        <div class="big-metric">
+          <span class="big-num" id="batt-current" style="color:#06b6d4;font-size:20px">—</span>
+        </div>
+      </div>
+    </div>
+
+    <!-- Gauge bars -->
+    <div class="gauge-row">
+      <div class="gauge-label-row">
+        <span class="gauge-label">Voltage</span>
+        <span class="gauge-value" id="batt-voltage-2" style="color:#6b7280">12.0–16.8 V</span>
+      </div>
+      <div class="gauge-track"><div class="gauge-fill" id="batt-volt-bar" style="width:0%"></div></div>
+    </div>
+    <div class="gauge-row">
+      <div class="gauge-label-row">
+        <span class="gauge-label">State of Charge</span>
+        <span class="gauge-value" style="color:#6b7280">0–100%</span>
+      </div>
+      <div class="gauge-track"><div class="gauge-fill" id="batt-soc-bar" style="width:0%"></div></div>
+    </div>
+
+    <!-- Sparkline history -->
+    <div>
+      <div style="font-size:9px;color:#6b7280;margin-bottom:4px">VOLTAGE HISTORY (last 2 min)</div>
+      <canvas id="batt-sparkline" height="56"></canvas>
+    </div>
+  </div>
+
+  <!-- ╔═══════════════════ TEMPERATURES ═══════════════════╗ -->
+  <div class="card">
+    <div class="card-title">TEMPERATURES</div>
+    <div class="temp-grid">
+      <div class="temp-box" id="cpu-temp-box">
+        <div class="temp-label">CPU (Jetson)</div>
+        <div class="temp-value" id="cpu-temp-val">—</div>
+        <div class="temp-bar-track"><div class="temp-bar-fill" id="cpu-temp-bar" style="width:0%"></div></div>
+      </div>
+      <div class="temp-box" id="gpu-temp-box">
+        <div class="temp-label">GPU (Jetson)</div>
+        <div class="temp-value" id="gpu-temp-val">—</div>
+        <div class="temp-bar-track"><div class="temp-bar-fill" id="gpu-temp-bar" style="width:0%"></div></div>
+      </div>
+      <div class="temp-box" id="board-temp-box">
+        <div class="temp-label">Board / STM32</div>
+        <div class="temp-value" id="board-temp-val">—</div>
+        <div class="temp-bar-track"><div class="temp-bar-fill" id="board-temp-bar" style="width:0%"></div></div>
+      </div>
+      <div class="temp-box" id="motor-temp-l-box">
+        <div class="temp-label">Motor L</div>
+        <div class="temp-value" id="motor-temp-l-val">—</div>
+        <div class="temp-bar-track"><div class="temp-bar-fill" id="motor-temp-l-bar" style="width:0%"></div></div>
+      </div>
+      <!-- spacer to keep 2-col grid balanced if motor-temp-r is alone -->
+      <div class="temp-box" id="motor-temp-r-box">
+        <div class="temp-label">Motor R</div>
+        <div class="temp-value" id="motor-temp-r-val">—</div>
+        <div class="temp-bar-track"><div class="temp-bar-fill" id="motor-temp-r-bar" style="width:0%"></div></div>
+      </div>
+    </div>
+    <div style="font-size:9px;color:#374151">
+      Zones: &lt;60°C green · 60–75°C amber · &gt;75°C red
+    </div>
+  </div>
+
+  <!-- ╔═══════════════════ MOTOR CURRENT ═══════════════════╗ -->
+  <div class="card">
+    <div class="card-title">MOTOR CURRENT &amp; CMD</div>
+    <div class="motor-grid">
+      <div class="motor-box">
+        <div class="motor-label">LEFT WHEEL</div>
+        <div style="font-size:20px;font-weight:bold;font-family:monospace" id="motor-cur-l">—</div>
+        <div class="gauge-track" style="margin-top:4px">
+          <div class="gauge-fill" id="motor-bar-l" style="width:0%"></div>
+        </div>
+        <div style="font-size:9px;color:#4b5563;margin-top:4px">
+          CMD: <span id="motor-cmd-l" style="color:#6b7280">—</span>
+        </div>
+      </div>
+      <div class="motor-box">
+        <div class="motor-label">RIGHT WHEEL</div>
+        <div style="font-size:20px;font-weight:bold;font-family:monospace" id="motor-cur-r">—</div>
+        <div class="gauge-track" style="margin-top:4px">
+          <div class="gauge-fill" id="motor-bar-r" style="width:0%"></div>
+        </div>
+        <div style="font-size:9px;color:#4b5563;margin-top:4px">
+          CMD: <span id="motor-cmd-r" style="color:#6b7280">—</span>
+        </div>
+      </div>
+    </div>
+    <div style="font-size:10px;color:#6b7280">
+      Balance state: <span id="balance-state" style="color:#9ca3af;font-family:monospace">—</span>
+    </div>
+    <div style="font-size:9px;color:#374151">Thresholds: warn 8A · crit 12A</div>
+  </div>
+
+  <!-- ╔═══════════════════ MEMORY / DISK ═══════════════════╗ -->
+  <div class="card">
+    <div class="card-title">RESOURCES</div>
+    <div class="gauge-row">
+      <div class="gauge-label-row">
+        <span class="gauge-label">RAM</span>
+        <span class="gauge-value" id="ram-val" style="color:#9ca3af">—</span>
+      </div>
+      <div class="gauge-track"><div class="gauge-fill" id="ram-bar" style="width:0%"></div></div>
+    </div>
+    <div class="gauge-row">
+      <div class="gauge-label-row">
+        <span class="gauge-label">GPU Memory</span>
+        <span class="gauge-value" id="gpu-val" style="color:#9ca3af">—</span>
+      </div>
+      <div class="gauge-track"><div class="gauge-fill" id="gpu-bar" style="width:0%"></div></div>
+    </div>
+    <div class="gauge-row">
+      <div class="gauge-label-row">
+        <span class="gauge-label">Disk</span>
+        <span class="gauge-value" id="disk-val" style="color:#9ca3af">—</span>
+      </div>
+      <div class="gauge-track"><div class="gauge-fill" id="disk-bar" style="width:0%"></div></div>
+    </div>
+    <div style="font-size:9px;color:#374151">Warn ≥80% · Critical ≥95%</div>
+  </div>
+
+  <!-- ╔═══════════════════ WIFI / LATENCY ═══════════════════╗ -->
+  <div class="card">
+    <div class="card-title">NETWORK</div>
+    <div style="display:flex;align-items:center;gap:12px;flex-wrap:wrap">
+      <div id="rssi-bars" class="rssi-bars"></div>
+      <div>
+        <div style="font-size:9px;color:#6b7280">RSSI</div>
+        <div style="font-size:20px;font-weight:bold;font-family:monospace" id="rssi-val">—</div>
+        <div style="font-size:9px" id="rssi-label" style="color:#6b7280">—</div>
+      </div>
+    </div>
+    <div class="gauge-row">
+      <div class="gauge-label-row">
+        <span class="gauge-label">Rosbridge Latency</span>
+        <span class="gauge-value" id="latency-val" style="color:#9ca3af">—</span>
+      </div>
+    </div>
+    <!-- MQTT status -->
+    <div style="margin-top:4px;padding-top:8px;border-top:1px solid #0c2a3a">
+      <div style="font-size:9px;color:#6b7280;margin-bottom:4px">MQTT BROKER</div>
+      <div class="mqtt-status">
+        <div class="mqtt-dot" id="mqtt-dot"></div>
+        <span id="mqtt-label" style="color:#6b7280">No data</span>
+      </div>
+      <div style="font-size:9px;color:#374151;margin-top:4px">
+        Via /diagnostics KeyValue: mqtt_connected
+      </div>
+    </div>
+  </div>
+
+  <!-- ╔═══════════════════ ROS2 NODE HEALTH ═══════════════════╗ -->
+  <div class="card span3">
+    <div class="card-title">
+      ROS2 NODE HEALTH — /diagnostics
+      <span style="font-size:9px;color:#4b5563" id="node-count">0 nodes</span>
+    </div>
+    <div id="node-list">
+      <div style="color:#374151;font-size:10px;text-align:center;padding:12px">
+        Waiting for /diagnostics data…
+      </div>
+    </div>
+  </div>
+
+</div>
+
+<!-- ── Footer ── -->
+<div id="footer">
+  <span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
+  <span>diagnostics dashboard — issue #562 · auto 2Hz</span>
+</div>
+
+<script src="diagnostics_panel.js"></script>
+<script>
+  // Sync footer WS URL
+  document.getElementById('ws-input').addEventListener('input', (e) => {
+    document.getElementById('footer-ws').textContent = e.target.value;
+  });
+
+  // Keep node count updated after render
+  const origRenderNodes = window.renderNodes;
+  const nodeCountEl = document.getElementById('node-count');
+  const _origOnDiag = window.onDiagnostics;
+
+  // Node count updates via MutationObserver on the node-list div
+  const nl = document.getElementById('node-list');
+  if (nl) {
+    new MutationObserver(() => {
+      const rows = nl.querySelectorAll('.node-row');
+      if (nodeCountEl) nodeCountEl.textContent = rows.length + ' node' + (rows.length !== 1 ? 's' : '');
+    }).observe(nl, { childList: true, subtree: false });
+  }
+</script>
+</body>
+</html>

ui/diagnostics_panel.js

@@ -0,0 +1,592 @@
+/**
+ * diagnostics_panel.js — Saltybot System Diagnostics Dashboard (Issue #562)
+ *
+ * Connects to rosbridge WebSocket and subscribes to:
+ *   /diagnostics           diagnostic_msgs/DiagnosticArray  — everything
+ *   /saltybot/balance_state std_msgs/String (JSON)           — motor cmd / state
+ *
+ * Diagnostic KeyValues decoded:
+ *   battery_voltage_v, battery_current_a, battery_soc_pct
+ *   cpu_temp_c, gpu_temp_c, board_temp_c
+ *   motor_temp_l_c, motor_temp_r_c
+ *   motor_current_l_a, motor_current_r_a
+ *   ram_used_mb, ram_total_mb
+ *   gpu_used_mb, gpu_total_mb
+ *   disk_used_gb, disk_total_gb
+ *   wifi_rssi_dbm, wifi_latency_ms
+ *   mqtt_connected
+ *
+ * Auto-refresh: rosbridge subscriptions push at ~2 Hz; UI polls canvas draws
+ * at the same rate and updates DOM elements.
+ */
+
+'use strict';
+
+// ── Constants ─────────────────────────────────────────────────────────────────
+
+const LIPO_MIN = 12.0;   // 4S empty
+const LIPO_MAX = 16.8;   // 4S full
+
+const BATT_HISTORY_MAX = 120;  // ~2 min at 1 Hz
+
+// Alert thresholds
+const THRESHOLDS = {
+  voltage_warn:    13.6,
+  voltage_crit:    13.0,
+  soc_warn:        25,
+  soc_crit:        10,
+  cpu_temp_warn:   75,
+  cpu_temp_crit:   90,
+  gpu_temp_warn:   75,
+  gpu_temp_crit:   90,
+  motor_temp_warn: 70,
+  motor_temp_crit: 85,
+  ram_pct_warn:    80,
+  ram_pct_crit:    95,
+  disk_pct_warn:   80,
+  disk_pct_crit:   95,
+  rssi_warn:       -70,
+  rssi_crit:       -80,
+  latency_warn:    150,
+  latency_crit:    500,
+  current_warn:    8.0,
+  current_crit:    12.0,
+};
+
+// ── State ─────────────────────────────────────────────────────────────────────
+
+let ros = null;
+let diagSub = null;
+let balanceSub = null;
+
+const state = {
+  // Battery
+  voltage:  null,
+  current:  null,
+  soc:      null,
+  battHistory: [],   // [{ts, v}]
+
+  // Temperatures
+  cpuTemp:   null,
+  gpuTemp:   null,
+  boardTemp: null,
+  motorTempL: null,
+  motorTempR: null,
+
+  // Motor
+  motorCurrentL: null,
+  motorCurrentR: null,
+  motorCmdL:     null,
+  motorCmdR:     null,
+  balanceState:  'UNKNOWN',
+
+  // Resources
+  ramUsed:  null, ramTotal:  null,
+  gpuUsed:  null, gpuTotal:  null,
+  diskUsed: null, diskTotal: null,
+
+  // Network
+  rssi:       null,
+  latency:    null,
+
+  // MQTT
+  mqttConnected: null,
+
+  // ROS nodes (DiagnosticStatus array)
+  nodes: [],
+
+  // Overall health
+  overallLevel: 3,  // 3=STALE by default
+  lastUpdate: null,
+};
+
+// ── Utility ───────────────────────────────────────────────────────────────────
+
+function numOrNull(s) {
+  const n = parseFloat(s);
+  return isNaN(n) ? null : n;
+}
+
+function pct(used, total) {
+  if (!total || total === 0) return 0;
+  return Math.min(100, Math.max(0, (used / total) * 100));
+}
+
+function socFromVoltage(v) {
+  if (v == null || v <= 0) return null;
+  return Math.max(0, Math.min(100, ((v - LIPO_MIN) / (LIPO_MAX - LIPO_MIN)) * 100));
+}
+
+function threshColor(value, warnThr, critThr, invert = false) {
+  // invert: lower is worse (e.g. voltage, SOC, RSSI)
+  if (value == null) return '#6b7280';
+  const warn = invert ? value <= warnThr : value >= warnThr;
+  const crit = invert ? value <= critThr : value >= critThr;
+  if (crit) return '#ef4444';
+  if (warn) return '#f59e0b';
+  return '#22c55e';
+}
+
+function levelClass(level) {
+  return ['ns-ok', 'ns-warn', 'ns-error', 'ns-stale'][level] ?? 'ns-stale';
+}
+
+function levelLabel(level) {
+  return ['OK', 'WARN', 'ERROR', 'STALE'][level] ?? 'STALE';
+}
+
+function setBadgeClass(el, level) {
+  el.className = 'sys-badge ' + ['badge-ok','badge-warn','badge-error','badge-stale'][level ?? 3];
+}
+
+// ── 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;
+    setupSubscriptions();
+  });
+
+  ros.on('error', (err) => {
+    document.getElementById('conn-dot').className = 'error';
+    document.getElementById('conn-label').textContent = 'ERROR: ' + (err?.message || err);
+    teardown();
+  });
+
+  ros.on('close', () => {
+    document.getElementById('conn-dot').className = '';
+    document.getElementById('conn-label').textContent = 'Disconnected';
+    teardown();
+  });
+}
+
+function setupSubscriptions() {
+  // /diagnostics — throttle 500ms → ~2 Hz
+  diagSub = new ROSLIB.Topic({
+    ros, name: '/diagnostics',
+    messageType: 'diagnostic_msgs/DiagnosticArray',
+    throttle_rate: 500,
+  });
+  diagSub.subscribe(onDiagnostics);
+
+  // /saltybot/balance_state
+  balanceSub = new ROSLIB.Topic({
+    ros, name: '/saltybot/balance_state',
+    messageType: 'std_msgs/String',
+    throttle_rate: 500,
+  });
+  balanceSub.subscribe(onBalanceState);
+}
+
+function teardown() {
+  if (diagSub)    { diagSub.unsubscribe();    diagSub    = null; }
+  if (balanceSub) { balanceSub.unsubscribe(); balanceSub = null; }
+  state.overallLevel = 3;
+  state.lastUpdate   = null;
+  render();
+}
+
+// ── Message handlers ──────────────────────────────────────────────────────────
+
+function onDiagnostics(msg) {
+  const statuses = msg.status ?? [];
+  state.nodes = [];
+  let overallLevel = 0;
+
+  for (const status of statuses) {
+    const kv = {};
+    for (const { key, value } of (status.values ?? [])) {
+      kv[key] = value;
+    }
+
+    // Battery
+    if ('battery_voltage_v'  in kv) state.voltage = numOrNull(kv.battery_voltage_v);
+    if ('battery_current_a'  in kv) state.current = numOrNull(kv.battery_current_a);
+    if ('battery_soc_pct'    in kv) state.soc     = numOrNull(kv.battery_soc_pct);
+
+    // Temperatures
+    if ('cpu_temp_c'         in kv) state.cpuTemp    = numOrNull(kv.cpu_temp_c);
+    if ('gpu_temp_c'         in kv) state.gpuTemp    = numOrNull(kv.gpu_temp_c);
+    if ('board_temp_c'       in kv) state.boardTemp  = numOrNull(kv.board_temp_c);
+    if ('motor_temp_l_c'     in kv) state.motorTempL = numOrNull(kv.motor_temp_l_c);
+    if ('motor_temp_r_c'     in kv) state.motorTempR = numOrNull(kv.motor_temp_r_c);
+
+    // Motor current
+    if ('motor_current_l_a'  in kv) state.motorCurrentL = numOrNull(kv.motor_current_l_a);
+    if ('motor_current_r_a'  in kv) state.motorCurrentR = numOrNull(kv.motor_current_r_a);
+
+    // Resources
+    if ('ram_used_mb'        in kv) state.ramUsed   = numOrNull(kv.ram_used_mb);
+    if ('ram_total_mb'       in kv) state.ramTotal  = numOrNull(kv.ram_total_mb);
+    if ('gpu_used_mb'        in kv) state.gpuUsed   = numOrNull(kv.gpu_used_mb);
+    if ('gpu_total_mb'       in kv) state.gpuTotal  = numOrNull(kv.gpu_total_mb);
+    if ('disk_used_gb'       in kv) state.diskUsed  = numOrNull(kv.disk_used_gb);
+    if ('disk_total_gb'      in kv) state.diskTotal = numOrNull(kv.disk_total_gb);
+
+    // Network
+    if ('wifi_rssi_dbm'      in kv) state.rssi     = numOrNull(kv.wifi_rssi_dbm);
+    if ('wifi_latency_ms'    in kv) state.latency  = numOrNull(kv.wifi_latency_ms);
+
+    // MQTT
+    if ('mqtt_connected'     in kv) state.mqttConnected = kv.mqtt_connected === 'true';
+
+    // Node health
+    state.nodes.push({
+      name:    status.name || status.hardware_id || 'unknown',
+      level:   status.level ?? 3,
+      message: status.message || '',
+    });
+
+    overallLevel = Math.max(overallLevel, status.level ?? 0);
+  }
+
+  state.overallLevel = overallLevel;
+  state.lastUpdate   = new Date();
+
+  // Battery history
+  if (state.voltage != null) {
+    state.battHistory.push({ ts: Date.now(), v: state.voltage });
+    if (state.battHistory.length > BATT_HISTORY_MAX) {
+      state.battHistory.shift();
+    }
+  }
+
+  // Derive SOC from voltage if not provided
+  if (state.soc == null && state.voltage != null) {
+    state.soc = socFromVoltage(state.voltage);
+  }
+
+  render();
+  flashRefreshDot();
+}
+
+function onBalanceState(msg) {
+  try {
+    const data = JSON.parse(msg.data);
+    state.balanceState  = data.state  ?? 'UNKNOWN';
+    state.motorCmdL     = data.motor_cmd ?? null;
+    state.motorCmdR     = data.motor_cmd ?? null;  // single motor_cmd field
+  } catch (_) {}
+}
+
+// ── Refresh indicator ─────────────────────────────────────────────────────────
+
+function flashRefreshDot() {
+  const dot = document.getElementById('refresh-dot');
+  dot.classList.add('pulse');
+  setTimeout(() => dot.classList.remove('pulse'), 400);
+}
+
+// ── Sparkline canvas ──────────────────────────────────────────────────────────
+
+function drawSparkline() {
+  const canvas = document.getElementById('batt-sparkline');
+  if (!canvas) return;
+  const ctx = canvas.getContext('2d');
+  const W = canvas.width = canvas.offsetWidth || 300;
+  const H = canvas.height;
+
+  ctx.clearRect(0, 0, W, H);
+  ctx.fillStyle = '#0a0a1a';
+  ctx.fillRect(0, 0, W, H);
+
+  const data = state.battHistory;
+  if (data.length < 2) {
+    ctx.fillStyle = '#374151';
+    ctx.font = '10px Courier New';
+    ctx.textAlign = 'center';
+    ctx.fillText('Awaiting battery data…', W / 2, H / 2 + 4);
+    return;
+  }
+
+  const vMin = LIPO_MIN - 0.2;
+  const vMax = LIPO_MAX + 0.2;
+
+  // Grid lines
+  ctx.strokeStyle = '#1e3a5f';
+  ctx.lineWidth = 0.5;
+  [0.25, 0.5, 0.75].forEach((f) => {
+    const y = H - f * H;
+    ctx.beginPath(); ctx.moveTo(0, y); ctx.lineTo(W, y); ctx.stroke();
+  });
+
+  // Plot
+  ctx.lineWidth = 1.5;
+  ctx.beginPath();
+  data.forEach((pt, i) => {
+    const x = (i / (data.length - 1)) * W;
+    const y = H - ((pt.v - vMin) / (vMax - vMin)) * H;
+    i === 0 ? ctx.moveTo(x, y) : ctx.lineTo(x, y);
+  });
+
+  const lastV = data[data.length - 1].v;
+  ctx.strokeStyle = threshColor(lastV, THRESHOLDS.voltage_warn, THRESHOLDS.voltage_crit, true);
+  ctx.stroke();
+
+  // Fill under
+  ctx.lineTo(W, H); ctx.lineTo(0, H); ctx.closePath();
+  ctx.fillStyle = 'rgba(6,182,212,0.06)';
+  ctx.fill();
+
+  // Labels
+  ctx.fillStyle = '#374151';
+  ctx.font = '9px Courier New';
+  ctx.textAlign = 'left';
+  ctx.fillText(`${LIPO_MAX}V`, 2, 10);
+  ctx.fillText(`${LIPO_MIN}V`, 2, H - 2);
+}
+
+// ── Gauge helpers ─────────────────────────────────────────────────────────────
+
+function setGauge(barId, pctVal, color) {
+  const el = document.getElementById(barId);
+  if (!el) return;
+  el.style.width = `${Math.max(0, Math.min(100, pctVal))}%`;
+  el.style.background = color;
+}
+
+function setText(id, text) {
+  const el = document.getElementById(id);
+  if (el) el.textContent = text ?? '—';
+}
+
+function setColor(id, color) {
+  const el = document.getElementById(id);
+  if (el) el.style.color = color;
+}
+
+function setTempBox(id, tempC, warnT, critT) {
+  const box = document.getElementById(id + '-box');
+  const val = document.getElementById(id + '-val');
+  const bar = document.getElementById(id + '-bar');
+  const color = threshColor(tempC, warnT, critT);
+
+  if (val)  { val.textContent = tempC != null ? tempC.toFixed(0) + '°C' : '—'; val.style.color = color; }
+  if (bar)  {
+    const p = tempC != null ? Math.min(100, (tempC / 100) * 100) : 0;
+    bar.style.width = p + '%';
+    bar.style.background = color;
+  }
+  if (box && tempC != null) {
+    box.style.borderColor = tempC >= critT ? '#991b1b' : tempC >= warnT ? '#92400e' : '#1e3a5f';
+  }
+}
+
+// ── RSSI bars ─────────────────────────────────────────────────────────────────
+
+function rssiToLevel(dBm) {
+  if (dBm == null) return { label: 'N/A', color: '#374151', bars: 0 };
+  if (dBm >= -50)  return { label: 'Excellent', color: '#22c55e', bars: 5 };
+  if (dBm >= -60)  return { label: 'Good',      color: '#3b82f6', bars: 4 };
+  if (dBm >= -70)  return { label: 'Fair',      color: '#f59e0b', bars: 3 };
+  if (dBm >= -80)  return { label: 'Weak',      color: '#ef4444', bars: 2 };
+  return             { label: 'Poor',      color: '#7f1d1d', bars: 1 };
+}
+
+function drawRssiBars() {
+  const container = document.getElementById('rssi-bars');
+  if (!container) return;
+  const lv = rssiToLevel(state.rssi);
+  const BAR_H = [4, 8, 12, 16, 20];
+
+  container.innerHTML = '';
+  for (let i = 0; i < 5; i++) {
+    const bar = document.createElement('div');
+    bar.className = 'rssi-bar';
+    bar.style.height = BAR_H[i] + 'px';
+    bar.style.width  = '7px';
+    bar.style.background = i < lv.bars ? lv.color : '#1f2937';
+    container.appendChild(bar);
+  }
+}
+
+// ── Node list renderer ────────────────────────────────────────────────────────
+
+function renderNodes() {
+  const container = document.getElementById('node-list');
+  if (!container) return;
+
+  if (state.nodes.length === 0) {
+    container.innerHTML = '<div style="color:#374151;font-size:10px;text-align:center;padding:8px">No /diagnostics data yet</div>';
+    return;
+  }
+
+  container.innerHTML = state.nodes.map((n) => `
+    <div class="node-row">
+      <span class="node-name">${n.name}</span>
+      <div style="display:flex;align-items:center;gap:6px">
+        ${n.message ? `<span style="color:#4b5563;font-size:9px;max-width:120px;overflow:hidden;text-overflow:ellipsis;white-space:nowrap" title="${n.message}">${n.message}</span>` : ''}
+        <span class="node-status ${levelClass(n.level)}">${levelLabel(n.level)}</span>
+      </div>
+    </div>
+  `).join('');
+}
+
+// ── Overall status bar ────────────────────────────────────────────────────────
+
+function updateStatusBar() {
+  const lvl = state.overallLevel;
+  const badge = document.getElementById('system-badge');
+  const labels = ['HEALTHY', 'DEGRADED', 'FAULT', 'STALE'];
+  if (badge) {
+    badge.textContent = labels[lvl] ?? 'STALE';
+    setBadgeClass(badge, lvl);
+  }
+
+  // Individual badges
+  updateBattBadge();
+  updateTempBadge();
+  updateNetBadge();
+
+  const upd = document.getElementById('last-update');
+  if (upd && state.lastUpdate) {
+    upd.textContent = 'Updated ' + state.lastUpdate.toLocaleTimeString();
+  }
+}
+
+function updateBattBadge() {
+  const el = document.getElementById('batt-badge');
+  if (!el) return;
+  const v = state.voltage;
+  if (v == null)             { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; }
+  if (v <= THRESHOLDS.voltage_crit) { el.textContent = 'CRITICAL'; setBadgeClass(el, 2); }
+  else if (v <= THRESHOLDS.voltage_warn) { el.textContent = 'LOW'; setBadgeClass(el, 1); }
+  else                       { el.textContent = 'OK'; setBadgeClass(el, 0); }
+}
+
+function updateTempBadge() {
+  const el = document.getElementById('temp-badge');
+  if (!el) return;
+  const temps = [state.cpuTemp, state.gpuTemp, state.motorTempL, state.motorTempR].filter(t => t != null);
+  if (temps.length === 0)    { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; }
+  const max = Math.max(...temps);
+  if (max >= THRESHOLDS.cpu_temp_crit) { el.textContent = 'CRITICAL'; setBadgeClass(el, 2); }
+  else if (max >= THRESHOLDS.cpu_temp_warn) { el.textContent = 'HOT'; setBadgeClass(el, 1); }
+  else                       { el.textContent = 'OK'; setBadgeClass(el, 0); }
+}
+
+function updateNetBadge() {
+  const el = document.getElementById('net-badge');
+  if (!el) return;
+  const r = state.rssi;
+  if (r == null)                   { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; }
+  if (r <= THRESHOLDS.rssi_crit)   { el.textContent = 'POOR'; setBadgeClass(el, 2); }
+  else if (r <= THRESHOLDS.rssi_warn) { el.textContent = 'WEAK'; setBadgeClass(el, 1); }
+  else                             { el.textContent = 'OK'; setBadgeClass(el, 0); }
+}
+
+// ── Main render ───────────────────────────────────────────────────────────────
+
+function render() {
+  // ── Battery ──
+  const v    = state.voltage;
+  const soc  = state.soc ?? socFromVoltage(v);
+  const vColor = threshColor(v, THRESHOLDS.voltage_warn, THRESHOLDS.voltage_crit, true);
+  const sColor = threshColor(soc, THRESHOLDS.soc_warn, THRESHOLDS.soc_crit, true);
+
+  setText('batt-voltage', v != null ? v.toFixed(2) + ' V' : '—');
+  setColor('batt-voltage', vColor);
+  setText('batt-soc', soc != null ? soc.toFixed(0) + ' %' : '—');
+  setColor('batt-soc', sColor);
+  setText('batt-current', state.current != null ? state.current.toFixed(2) + ' A' : '—');
+
+  setGauge('batt-soc-bar', soc ?? 0, sColor);
+  setGauge('batt-volt-bar', v != null ? ((v - LIPO_MIN) / (LIPO_MAX - LIPO_MIN)) * 100 : 0, vColor);
+
+  drawSparkline();
+
+  // ── Temperatures ──
+  setTempBox('cpu-temp', state.cpuTemp, THRESHOLDS.cpu_temp_warn, THRESHOLDS.cpu_temp_crit);
+  setTempBox('gpu-temp', state.gpuTemp, THRESHOLDS.gpu_temp_warn, THRESHOLDS.gpu_temp_crit);
+  setTempBox('board-temp', state.boardTemp, 60, 80);
+  setTempBox('motor-temp-l', state.motorTempL, THRESHOLDS.motor_temp_warn, THRESHOLDS.motor_temp_crit);
+  setTempBox('motor-temp-r', state.motorTempR, THRESHOLDS.motor_temp_warn, THRESHOLDS.motor_temp_crit);
+
+  // ── Motor current ──
+  const curL = state.motorCurrentL;
+  const curR = state.motorCurrentR;
+  const curColorL = threshColor(curL, THRESHOLDS.current_warn, THRESHOLDS.current_crit);
+  const curColorR = threshColor(curR, THRESHOLDS.current_warn, THRESHOLDS.current_crit);
+
+  setText('motor-cur-l', curL != null ? curL.toFixed(2) + ' A' : '—');
+  setColor('motor-cur-l', curColorL);
+  setText('motor-cur-r', curR != null ? curR.toFixed(2) + ' A' : '—');
+  setColor('motor-cur-r', curColorR);
+  setGauge('motor-bar-l', curL != null ? (curL / THRESHOLDS.current_crit) * 100 : 0, curColorL);
+  setGauge('motor-bar-r', curR != null ? (curR / THRESHOLDS.current_crit) * 100 : 0, curColorR);
+  setText('motor-cmd-l', state.motorCmdL != null ? state.motorCmdL.toString() : '—');
+  setText('motor-cmd-r', state.motorCmdR != null ? state.motorCmdR.toString() : '—');
+  setText('balance-state', state.balanceState);
+
+  // ── Resources ──
+  const ramPct  = pct(state.ramUsed,  state.ramTotal);
+  const gpuPct  = pct(state.gpuUsed,  state.gpuTotal);
+  const diskPct = pct(state.diskUsed, state.diskTotal);
+
+  setText('ram-val',
+    state.ramUsed != null ? `${state.ramUsed.toFixed(0)} / ${state.ramTotal?.toFixed(0) ?? '?'} MB` : '—');
+  setGauge('ram-bar', ramPct, threshColor(ramPct, THRESHOLDS.ram_pct_warn, THRESHOLDS.ram_pct_crit));
+
+  setText('gpu-val',
+    state.gpuUsed != null ? `${state.gpuUsed.toFixed(0)} / ${state.gpuTotal?.toFixed(0) ?? '?'} MB` : '—');
+  setGauge('gpu-bar', gpuPct, threshColor(gpuPct, 70, 90));
+
+  setText('disk-val',
+    state.diskUsed != null ? `${state.diskUsed.toFixed(1)} / ${state.diskTotal?.toFixed(1) ?? '?'} GB` : '—');
+  setGauge('disk-bar', diskPct, threshColor(diskPct, THRESHOLDS.disk_pct_warn, THRESHOLDS.disk_pct_crit));
+
+  // ── WiFi ──
+  const rLevel = rssiToLevel(state.rssi);
+  setText('rssi-val', state.rssi != null ? state.rssi + ' dBm' : '—');
+  setColor('rssi-val', rLevel.color);
+  setText('rssi-label', rLevel.label);
+  setColor('rssi-label', rLevel.color);
+  setText('latency-val', state.latency != null ? state.latency.toFixed(0) + ' ms' : '—');
+  setColor('latency-val', threshColor(state.latency, THRESHOLDS.latency_warn, THRESHOLDS.latency_crit));
+  drawRssiBars();
+
+  // ── MQTT ──
+  const mqttDot = document.getElementById('mqtt-dot');
+  const mqttLbl = document.getElementById('mqtt-label');
+  if (mqttDot) {
+    mqttDot.className = 'mqtt-dot ' + (state.mqttConnected ? 'connected' : 'disconnected');
+  }
+  if (mqttLbl) {
+    mqttLbl.textContent = state.mqttConnected === null ? 'No data'
+      : state.mqttConnected ? 'Broker connected'
+      : 'Broker disconnected';
+    mqttLbl.style.color = state.mqttConnected ? '#4ade80' : '#f87171';
+  }
+
+  // ── Nodes ──
+  renderNodes();
+
+  // ── Status bar ──
+  updateStatusBar();
+}
+
+// ── Init ──────────────────────────────────────────────────────────────────────
+
+document.getElementById('btn-connect').addEventListener('click', connect);
+document.getElementById('ws-input').addEventListener('keydown', (e) => {
+  if (e.key === 'Enter') connect();
+});
+
+const stored = localStorage.getItem('diag_ws_url');
+if (stored) document.getElementById('ws-input').value = stored;
+document.getElementById('ws-input').addEventListener('change', (e) => {
+  localStorage.setItem('diag_ws_url', e.target.value);
+});
+
+// Initial render (blank state)
+render();
+
+// Periodic sparkline resize + redraw on window resize
+window.addEventListener('resize', drawSparkline);

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();