feat: Battery voltage telemetry and LVC (Issue #613)

- Add include/lvc.h + src/lvc.c: 3-stage low voltage cutoff state machine
  WARNING  21.0V: MELODY_LOW_BATTERY buzzer, full motor power
  CRITICAL 19.8V: double-beep every 10s, 50% motor power scaling
  CUTOFF   18.6V: MELODY_ERROR one-shot, motors disabled + latched
  200mV hysteresis on recovery; CUTOFF latched until reboot
- Add JLINK_TLM_LVC (0x8B, 4 bytes): voltage_mv, percent, protection_state
  jlink_send_lvc_tlm() frame encoder in jlink.c
- Wire into main.c:
  lvc_init() at startup; lvc_tick() each 1kHz loop tick
  lvc_is_cutoff() triggers safety_arm_cancel + balance_disarm + motor_driver_estop
  lvc_get_power_scale() applied to ESC speed command (100/50/0%)
  1Hz JLINK_TLM_LVC telemetry with fuel-gauge percent field
- Add LVC thresholds to config.h (LVC_WARNING/CRITICAL/CUTOFF/HYSTERESIS_MV)

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

chassis/jetson_orin_mount.scad

@@ -0,0 +1,386 @@
+// ============================================================
+// Jetson Orin Nano Carrier Board Mount — Issue #612
+// Agent   : sl-mechanical
+// Date    : 2026-03-15
+// Part catalogue:
+//   1. tnut_base     — 2020 T-slot rail interface plate, M5 T-nut captive pockets
+//   2. standoff_post — M2.5 captive-nut standoff post (×4), 10 mm airflow gap
+//   3. side_brace    — lateral stiffening brace with port-access cutouts (×2)
+//   4. duct_shroud   — optional top heatsink duct / fan-exhaust channel
+//   5. cable_clip    — snap-on cable management clip for brace edge
+//
+// BOM:
+//   4 × M5×10 BHCS + M5 T-nuts          (tnut_base to rail, 2 per rail)
+//   4 × M2.5×20 SHCS                    (board to standoff posts)
+//   4 × M2.5 hex nuts                   (captured in standoff posts)
+//   4 × M3×8  SHCS + washers            (side_brace to tnut_base)
+//   2 × M3×16 SHCS                      (duct_shroud to side_brace tops)
+//
+// Jetson Orin Nano carrier board (Seeed reComputer / official dev kit):
+//   Board dims   : 100 × 80 mm
+//   Mounting hole pattern : 86 × 58 mm (centre-to-centre), M2.5, Ø3.5 pad
+//   PCB thickness: 1.6 mm
+//   Connector side: -Y (USB-A, USB-C, HDMI, DP, GbE, SD on one long edge)
+//   Fan header & PWM header: +X short edge
+//   M.2 / NVMe: bottom face
+//
+// Print settings (PETG):
+//   tnut_base / standoff_post / side_brace / duct_shroud : 5 perimeters, 40 % gyroid, no supports
+//   cable_clip                                            : 3 perimeters, 30 % gyroid, no supports
+//
+// Export commands:
+//   openscad -D 'RENDER="tnut_base"'     -o tnut_base.stl     jetson_orin_mount.scad
+//   openscad -D 'RENDER="standoff_post"' -o standoff_post.stl jetson_orin_mount.scad
+//   openscad -D 'RENDER="side_brace"'    -o side_brace.stl    jetson_orin_mount.scad
+//   openscad -D 'RENDER="duct_shroud"'   -o duct_shroud.stl   jetson_orin_mount.scad
+//   openscad -D 'RENDER="cable_clip"'    -o cable_clip.stl    jetson_orin_mount.scad
+//   openscad -D 'RENDER="assembly"'      -o assembly.png      jetson_orin_mount.scad
+// ============================================================
+
+// ── Render selector ─────────────────────────────────────────
+RENDER = "assembly"; // tnut_base | standoff_post | side_brace | duct_shroud | cable_clip | assembly
+
+// ── Global constants ────────────────────────────────────────
+$fn = 64;
+EPS = 0.01;
+
+// 2020 rail
+RAIL_W      = 20.0;
+SLOT_NECK_H =  3.2;
+TNUT_W      =  9.8;
+TNUT_H      =  5.5;
+TNUT_L      = 12.0;
+M5_D        =  5.2;
+M5_HEAD_D   =  9.5;
+M5_HEAD_H   =  4.0;
+
+// Jetson Orin Nano carrier board
+BOARD_L     = 100.0;  // board X
+BOARD_W     =  80.0;  // board Y
+BOARD_T     =   1.6;  // PCB thickness
+MH_SX       =  86.0;  // mounting hole span X (centre-to-centre)
+MH_SY       =  58.0;  // mounting hole span Y
+M25_D       =   2.7;  // M2.5 clearance bore
+M25_NUT_W   =   5.0;  // M2.5 hex nut across-flats
+M25_NUT_H   =   2.0;  // M2.5 hex nut height
+M25_HEAD_D  =   5.0;  // M2.5 SHCS head diameter
+M25_HEAD_H  =   2.5;
+
+// Base plate
+BASE_L      = 120.0;  // length along X (covers board + overhang for braces)
+BASE_W      =  50.0;  // width along Y (rail mount footprint)
+BASE_T      =   6.0;  // plate thickness
+BOLT_PITCH  =  40.0;  // M5 rail bolt pitch (per rail, 2 rails at Y=0 & Y=BASE_W)
+M3_D        =   3.2;
+M3_HEAD_D   =   6.0;
+M3_HEAD_H   =   3.0;
+
+// Standoff posts
+POST_H      =  12.0;  // airflow gap + PCB seating (>= 10 mm clearance spec)
+POST_OD     =   8.0;  // outer diameter
+POST_BASE_D =  11.0;  // flange diameter
+POST_BASE_H =   3.0;  // flange height
+NUT_TRAP_H  = M25_NUT_H + 0.3;
+NUT_TRAP_W  = M25_NUT_W + 0.4;
+
+// Side braces
+BRACE_T     =   5.0;  // brace thickness (X)
+BRACE_H     = POST_H + POST_BASE_H + BOARD_T + 4.0;  // full height
+BRACE_W     = BASE_W;  // same width as base
+
+// Port-access cutouts (connector side -Y)
+USB_CUT_W   =  60.0;  // wide cutout for USB-A stack + HDMI + DP
+USB_CUT_H   =  22.0;
+GBE_CUT_W   =  20.0;  // GbE jack
+GBE_CUT_H   =  18.0;
+
+// Duct shroud
+DUCT_T      =   3.0;  // wall thickness
+DUCT_FLANGE =   6.0;  // side tab width for M3 attachment
+FAN_W       =  40.0;  // standard 40 mm blower clearance cutout
+FAN_H       =  10.0;  // duct outlet height
+
+// Cable clip
+CLIP_OD     =  12.0;
+CLIP_ID     =   7.0;
+CLIP_GAP    =   7.5;
+CLIP_W      =  10.0;
+SNAP_T      =   1.8;
+
+// ── Utilities ───────────────────────────────────────────────
+module chamfer_cube(size, ch=1.0) {
+    hull() {
+        translate([ch, ch, 0])          cube([size[0]-2*ch, size[1]-2*ch, EPS]);
+        translate([0, 0, ch])           cube(size - [0, 0, ch]);
+    }
+}
+
+module hex_pocket(af, depth) {
+    cylinder(d=af/cos(30), h=depth, $fn=6);
+}
+
+// ── Part 1: tnut_base ───────────────────────────────────────
+module tnut_base() {
+    difference() {
+        union() {
+            chamfer_cube([BASE_L, BASE_W, BASE_T], ch=1.5);
+
+            // Raised mounting bosses for M3 brace attachment (4 corners)
+            for (x = [8, BASE_L-8])
+                for (y = [8, BASE_W-8])
+                    translate([x, y, BASE_T])
+                        cylinder(d=10, h=2.5);
+        }
+
+        // T-nut pockets and M5 bolts — front rail (y = BASE_W/4)
+        for (x = [BASE_L/2 - BOLT_PITCH/2, BASE_L/2 + BOLT_PITCH/2]) {
+            translate([x, BASE_W/4, -EPS]) {
+                cylinder(d=M5_D, h=BASE_T + 2*EPS);
+                cylinder(d=M5_HEAD_D, h=M5_HEAD_H + EPS);
+            }
+            translate([x - TNUT_L/2, BASE_W/4 - TNUT_W/2, BASE_T - TNUT_H])
+                cube([TNUT_L, TNUT_W, TNUT_H + EPS]);
+        }
+
+        // T-nut pockets and M5 bolts — rear rail (y = 3*BASE_W/4)
+        for (x = [BASE_L/2 - BOLT_PITCH/2, BASE_L/2 + BOLT_PITCH/2]) {
+            translate([x, 3*BASE_W/4, -EPS]) {
+                cylinder(d=M5_D, h=BASE_T + 2*EPS);
+                cylinder(d=M5_HEAD_D, h=M5_HEAD_H + EPS);
+            }
+            translate([x - TNUT_L/2, 3*BASE_W/4 - TNUT_W/2, BASE_T - TNUT_H])
+                cube([TNUT_L, TNUT_W, TNUT_H + EPS]);
+        }
+
+        // M3 boss bolt holes (corner braces)
+        for (x = [8, BASE_L-8])
+            for (y = [8, BASE_W-8])
+                translate([x, y, -EPS])
+                    cylinder(d=M3_D, h=BASE_T + 2.5 + 2*EPS);
+
+        // M3 boss counterbores (head from bottom)
+        for (x = [8, BASE_L-8])
+            for (y = [8, BASE_W-8])
+                translate([x, y, -EPS])
+                    cylinder(d=M3_HEAD_D, h=M3_HEAD_H + EPS);
+
+        // Standoff post seating holes (board hole pattern, centred on plate)
+        bx0 = BASE_L/2 - MH_SX/2;
+        by0 = BASE_W/2 - MH_SY/2;
+        for (dx = [0, MH_SX])
+            for (dy = [0, MH_SY])
+                translate([bx0+dx, by0+dy, -EPS])
+                    cylinder(d=POST_BASE_D + 0.4, h=BASE_T + 2*EPS);
+
+        // Weight relief grid (2 pockets)
+        translate([20, 12, -EPS]) cube([30, BASE_W-24, BASE_T/2]);
+        translate([BASE_L-50, 12, -EPS]) cube([30, BASE_W-24, BASE_T/2]);
+
+        // Cable pass-through slot
+        translate([BASE_L/2 - 8, BASE_W/2 - 3, -EPS])
+            cube([16, 6, BASE_T + 2*EPS]);
+    }
+}
+
+// ── Part 2: standoff_post ───────────────────────────────────
+module standoff_post() {
+    difference() {
+        union() {
+            // Flange
+            cylinder(d=POST_BASE_D, h=POST_BASE_H);
+            // Post body
+            translate([0, 0, POST_BASE_H])
+                cylinder(d=POST_OD, h=POST_H);
+        }
+
+        // M2.5 through bore
+        translate([0, 0, -EPS])
+            cylinder(d=M25_D, h=POST_BASE_H + POST_H + 2*EPS);
+
+        // Captured hex nut trap (from top)
+        translate([0, 0, POST_BASE_H + POST_H - NUT_TRAP_H])
+            hex_pocket(NUT_TRAP_W, NUT_TRAP_H + EPS);
+
+        // Anti-rotation flat on nut pocket
+        translate([-M25_NUT_W/2 - 0.2, -POST_OD/2 - EPS,
+                   POST_BASE_H + POST_H - NUT_TRAP_H])
+            cube([M25_NUT_W + 0.4, 2.0, NUT_TRAP_H + EPS]);
+    }
+}
+
+// ── Part 3: side_brace ──────────────────────────────────────
+// Printed as +X face. Mirror for -X side.
+module side_brace() {
+    difference() {
+        union() {
+            chamfer_cube([BRACE_T, BRACE_W, BRACE_H], ch=1.0);
+
+            // Top lip to retain board edge
+            translate([0, 0, BRACE_H])
+                cube([BRACE_T + 8.0, BRACE_W, 2.5]);
+        }
+
+        // M3 bolt holes at base (attach to tnut_base bosses)
+        for (y = [8, BRACE_W-8])
+            translate([-EPS, y, 4])
+                rotate([0, 90, 0])
+                    cylinder(d=M3_D, h=BRACE_T + 2*EPS);
+
+        // M3 counterbore from outer face
+        for (y = [8, BRACE_W-8])
+            translate([-EPS, y, 4])
+                rotate([0, 90, 0])
+                    cylinder(d=M3_HEAD_D, h=M3_HEAD_H + EPS);
+
+        // Port-access cutout — USB/HDMI/DP cluster (centred on brace face)
+        translate([-EPS, BRACE_W/2 - USB_CUT_W/2, POST_BASE_H + 2.0])
+            cube([BRACE_T + 2*EPS, USB_CUT_W, USB_CUT_H]);
+
+        // GbE cutout (offset toward +Y)
+        translate([-EPS, BRACE_W/2 + USB_CUT_W/2 - GBE_CUT_W - 2, POST_BASE_H + 2.0])
+            cube([BRACE_T + 2*EPS, GBE_CUT_W, GBE_CUT_H]);
+
+        // M3 duct attachment holes (top edge)
+        for (y = [BRACE_W/4, 3*BRACE_W/4])
+            translate([BRACE_T/2, y, BRACE_H - 2])
+                cylinder(d=M3_D, h=10);
+
+        // Ventilation slots (3 tall slots for airflow)
+        for (i = [0:2])
+            translate([-EPS,
+                       (BRACE_W - 3*8 - 2*4) / 2 + i*(8+4),
+                       POST_BASE_H + USB_CUT_H + 6])
+                cube([BRACE_T + 2*EPS, 8, BRACE_H - POST_BASE_H - USB_CUT_H - 10]);
+    }
+}
+
+// ── Part 4: duct_shroud ─────────────────────────────────────
+// Top cap that channels fan exhaust away from board; optional print.
+module duct_shroud() {
+    duct_l = BASE_L - 2*BRACE_T - 1.0;  // span between inner brace faces
+    duct_w = BRACE_W;
+
+    difference() {
+        union() {
+            // Top plate
+            cube([duct_l, duct_w, DUCT_T]);
+
+            // Front wall (fan inlet side)
+            translate([0, 0, -FAN_H])
+                cube([DUCT_T, duct_w, FAN_H + DUCT_T]);
+
+            // Rear wall (exhaust side — open centre)
+            translate([duct_l - DUCT_T, 0, -FAN_H])
+                cube([DUCT_T, duct_w, FAN_H + DUCT_T]);
+
+            // Side flanges for M3 attachment
+            translate([-DUCT_FLANGE, 0, -FAN_H])
+                cube([DUCT_FLANGE, duct_w, FAN_H + DUCT_T]);
+            translate([duct_l, 0, -FAN_H])
+                cube([DUCT_FLANGE, duct_w, FAN_H + DUCT_T]);
+        }
+
+        // Fan cutout on top plate (centred)
+        translate([duct_l/2 - FAN_W/2, duct_w/2 - FAN_W/2, -EPS])
+            cube([FAN_W, FAN_W, DUCT_T + 2*EPS]);
+
+        // Fan screw holes (40 mm fan, Ø3.2 at 32 mm BC)
+        for (dx = [-16, 16])
+            for (dy = [-16, 16])
+                translate([duct_l/2 + dx, duct_w/2 + dy, -EPS])
+                    cylinder(d=M3_D, h=DUCT_T + 2*EPS);
+
+        // Exhaust slot on rear wall (full width minus corners)
+        translate([duct_l - DUCT_T - EPS, 4, -FAN_H + 2])
+            cube([DUCT_T + 2*EPS, duct_w - 8, FAN_H - 2]);
+
+        // M3 flange attachment holes
+        for (y = [duct_w/4, 3*duct_w/4]) {
+            translate([-DUCT_FLANGE - EPS, y, -FAN_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d=M3_D, h=DUCT_FLANGE + 2*EPS);
+            translate([duct_l + DUCT_T - EPS, y, -FAN_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d=M3_D, h=DUCT_FLANGE + 2*EPS);
+        }
+    }
+}
+
+// ── Part 5: cable_clip ──────────────────────────────────────
+module cable_clip() {
+    difference() {
+        union() {
+            // Snap-wrap body
+            difference() {
+                cylinder(d=CLIP_OD + 2*SNAP_T, h=CLIP_W);
+                translate([0, 0, -EPS])
+                    cylinder(d=CLIP_ID, h=CLIP_W + 2*EPS);
+                // Front gap
+                translate([-CLIP_GAP/2, 0, -EPS])
+                    cube([CLIP_GAP, CLIP_OD, CLIP_W + 2*EPS]);
+            }
+
+            // Mounting tab for brace edge
+            translate([CLIP_OD/2 + SNAP_T - EPS, -SNAP_T, 0])
+                cube([8, SNAP_T*2, CLIP_W]);
+        }
+
+        // Tab screw hole
+        translate([CLIP_OD/2 + SNAP_T + 4, 0, CLIP_W/2])
+            rotate([90, 0, 0])
+                cylinder(d=M3_D, h=SNAP_T*2 + 2*EPS, center=true);
+    }
+}
+
+// ── Assembly ────────────────────────────────────────────────
+module assembly() {
+    // Base plate
+    color("SteelBlue")
+        tnut_base();
+
+    // Standoff posts (board hole pattern)
+    bx0 = BASE_L/2 - MH_SX/2;
+    by0 = BASE_W/2 - MH_SY/2;
+    for (dx = [0, MH_SX])
+        for (dy = [0, MH_SY])
+            color("DodgerBlue")
+                translate([bx0+dx, by0+dy, BASE_T])
+                    standoff_post();
+
+    // Side braces (left and right)
+    color("CornflowerBlue")
+        translate([0, 0, BASE_T])
+            side_brace();
+    color("CornflowerBlue")
+        translate([BASE_L, BRACE_W, BASE_T])
+            mirror([1, 0, 0])
+                mirror([0, 1, 0])
+                    side_brace();
+
+    // Board silhouette (translucent, for clearance visualisation)
+    color("ForestGreen", 0.25)
+        translate([BASE_L/2 - BOARD_L/2, BASE_W/2 - BOARD_W/2,
+                   BASE_T + POST_BASE_H + POST_H])
+            cube([BOARD_L, BOARD_W, BOARD_T]);
+
+    // Duct shroud (above board)
+    color("LightSteelBlue", 0.7)
+        translate([BRACE_T + 0.5, 0,
+                   BASE_T + POST_BASE_H + POST_H + BOARD_T + 2.0])
+            duct_shroud();
+
+    // Cable clips (on brace edge, 2×)
+    for (y = [BRACE_W/3, 2*BRACE_W/3])
+        color("SlateGray")
+            translate([BASE_L + 2, y, BASE_T + BRACE_H/2 - CLIP_W/2])
+                rotate([0, 90, 0])
+                    cable_clip();
+}
+
+// ── Dispatch ────────────────────────────────────────────────
+if      (RENDER == "tnut_base")     tnut_base();
+else if (RENDER == "standoff_post") standoff_post();
+else if (RENDER == "side_brace")    side_brace();
+else if (RENDER == "duct_shroud")   duct_shroud();
+else if (RENDER == "cable_clip")    cable_clip();
+else                                assembly();

include/jlink.h

@@ -50,6 +50,7 @@
  *   0x87  FAULT_LOG     - jlink_tlm_fault_log_t (20 bytes), sent on boot + FAULT_LOG_GET (Issue #565)
  *   0x88  SLOPE         - jlink_tlm_slope_t (4 bytes), sent at SLOPE_TLM_HZ (Issue #600)
  *   0x89  CAN_STATS     - jlink_tlm_can_stats_t (16 bytes), sent at CAN_TLM_HZ + CAN_STATS_GET (Issue #597)
+ *   0x8A  STEERING      - jlink_tlm_steering_t (8 bytes), sent at STEER_TLM_HZ (Issue #616)
  *   0x8B  LVC           - jlink_tlm_lvc_t (4 bytes), sent at LVC_TLM_HZ (Issue #613)
  *
  * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
@@ -94,6 +95,7 @@
 #define JLINK_TLM_FAULT_LOG     0x87u  /* jlink_tlm_fault_log_t (20 bytes, Issue #565) */
 #define JLINK_TLM_SLOPE         0x88u  /* jlink_tlm_slope_t (4 bytes, Issue #600) */
 #define JLINK_TLM_CAN_STATS     0x89u  /* jlink_tlm_can_stats_t (16 bytes, Issue #597) */
+#define JLINK_TLM_STEERING      0x8Au  /* jlink_tlm_steering_t (8 bytes, Issue #616) */
 #define JLINK_TLM_LVC           0x8Bu  /* jlink_tlm_lvc_t (4 bytes, Issue #613) */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
@@ -206,6 +208,16 @@ typedef struct __attribute__((packed)) {
     int16_t  vel1_rpm;     /* node 1 current velocity (RPM)               */
 } jlink_tlm_can_stats_t;  /* 16 bytes */
 
+/* ---- Telemetry STEERING payload (8 bytes, packed) Issue #616 ---- */
+/* Published at STEER_TLM_HZ (10 Hz); reports yaw-rate PID state. */
+typedef struct __attribute__((packed)) {
+    int16_t  target_x10;  /* target yaw rate, deg/s × 10 (0.1 deg/s resolution) */
+    int16_t  actual_x10;  /* measured yaw rate, deg/s × 10                       */
+    int16_t  output;      /* differential motor output (-STEER_OUTPUT_MAX..+MAX)  */
+    uint8_t  enabled;     /* 1 = PID active                                       */
+    uint8_t  _pad;        /* reserved                                             */
+} jlink_tlm_steering_t;  /* 8 bytes */
+
 /* ---- Telemetry LVC payload (4 bytes, packed) Issue #613 ---- */
 /* Sent at LVC_TLM_HZ (1 Hz); reports battery voltage and LVC protection state. */
 typedef struct __attribute__((packed)) {
@@ -332,6 +344,13 @@ void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm);
  */
 void jlink_send_can_stats(const jlink_tlm_can_stats_t *tlm);
 
+/*
+ * jlink_send_steering_tlm(tlm) - transmit JLINK_TLM_STEERING (0x8A) frame
+ * (14 bytes total) to Jetson.  Issue #616.
+ * Rate-limiting is handled by steering_pid_send_tlm(); call from there only.
+ */
+void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm);
+
 /*
  * jlink_send_lvc_tlm(tlm) - transmit JLINK_TLM_LVC (0x8B) frame
  * (10 bytes total) at LVC_TLM_HZ (1 Hz).  Issue #613.

include/mpu6000.h

@@ -9,6 +9,7 @@ typedef struct {
     float pitch_rate;   // degrees/sec (raw gyro pitch axis)
     float roll;         // degrees, filtered (complementary filter)
     float yaw;          // degrees, gyro-integrated (drifts — no magnetometer)
+    float yaw_rate;     // degrees/sec (raw gyro Z / board_gz, Issue #616)
     float accel_x;      // g
     float accel_z;      // g
 } IMUData;

include/steering_pid.h

@@ -0,0 +1,134 @@
+#ifndef STEERING_PID_H
+#define STEERING_PID_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * steering_pid — closed-loop yaw-rate controller for differential drive
+ * (Issue #616).
+ *
+ * OVERVIEW:
+ *   Converts a desired yaw rate (from Jetson Twist.angular.z) into a
+ *   differential wheel speed offset.  The balance PID remains the primary
+ *   controller; the steering PID generates a small differential term that
+ *   is added to the balance command inside motor_driver:
+ *
+ *     left_speed  = balance_cmd - steer_out
+ *     right_speed = balance_cmd + steer_out
+ *
+ *   This is the standard differential-drive mixing already performed by
+ *   the ESC backend (hoverboard/VESC).
+ *
+ * INPUT SIGNALS:
+ *   target_omega_dps : desired yaw rate in deg/s (+ = clockwise from above)
+ *                      Derived from JLINK_CMD_DRIVE steer field:
+ *                        target_omega_dps = steer * STEER_OMEGA_SCALE
+ *                      (steer is int16 -1000..+1000 from Jetson)
+ *   actual_omega_dps : measured yaw rate from IMU gyro Z (deg/s)
+ *                      = IMUData.yaw_rate
+ *
+ * PID ALGORITHM:
+ *   error     = target_omega - actual_omega
+ *   integral  = clamp(integral + error*dt, ±STEER_INTEGRAL_MAX)
+ *   raw_out   = Kp*error + Ki*integral + Kd*(error - prev_error)/dt
+ *   raw_out   = clamp(raw_out, ±STEER_OUTPUT_MAX)
+ *   output    = rate_limit(raw_out, STEER_RAMP_RATE_PER_MS * dt_ms)
+ *
+ * ANTI-WINDUP:
+ *   Integral is clamped to ±STEER_INTEGRAL_MAX counts before the Ki
+ *   multiply, bounding the integrator contribution independently of Kp/Kd.
+ *
+ * RATE LIMITER:
+ *   Output changes at most STEER_RAMP_RATE_PER_MS counts per millisecond.
+ *   Prevents a sudden step in steering demand from disturbing the balance
+ *   PID (which has no knowledge of the steering channel).
+ *
+ * OMEGA SCALING:
+ *   STEER_OMEGA_SCALE = 0.1 deg/s per steer unit.
+ *   Range: steer -1000..+1000 → omega -100..+100 deg/s.
+ *   100 deg/s ≈ 1.75 rad/s — covers aggressive turns without exceeding
+ *   the hoverboard ESC differential authority (STEER_OUTPUT_MAX = 400).
+ *
+ * DISABLING:
+ *   steering_pid_set_enabled(s, false) zeroes target_omega and integral,
+ *   then freezes output at 0.  Use when Jetson is not active or in
+ *   RC_MANUAL mode to avoid fighting the RC steer channel.
+ *
+ * TELEMETRY:
+ *   JLINK_TLM_STEERING (0x8A) published at STEER_TLM_HZ (10 Hz):
+ *     jlink_tlm_steering_t { int16 target_x10, int16 actual_x10,
+ *                            int16 output, uint8 enabled, uint8 _pad }
+ *   8 bytes total.
+ */
+
+/* ---- Configuration ---- */
+#define STEER_KP               2.0f   /* proportional gain (counts / (deg/s))  */
+#define STEER_KI               0.5f   /* integral gain (counts / (deg))         */
+#define STEER_KD               0.05f  /* derivative gain (counts / (deg/s²))   */
+#define STEER_INTEGRAL_MAX    200.0f  /* integrator clamp (motor counts)        */
+#define STEER_OUTPUT_MAX       400    /* peak differential output (counts)      */
+#define STEER_RAMP_RATE_PER_MS  10    /* max output change per ms (counts/ms)   */
+#define STEER_OMEGA_SCALE      0.1f   /* steer units → deg/s (0.1 deg/s/unit)  */
+#define STEER_TLM_HZ           10u    /* JLINK_TLM_STEERING publish rate (Hz)  */
+
+/* ---- State ---- */
+typedef struct {
+    float    target_omega_dps;  /* setpoint: desired yaw rate (deg/s)       */
+    float    actual_omega_dps;  /* feedback: measured yaw rate (deg/s)      */
+    float    integral;          /* PID integrator (motor counts·s)          */
+    float    prev_error;        /* error at last update (deg/s)             */
+    int16_t  output;            /* rate-limited differential output          */
+    bool     enabled;           /* false = output held at 0                 */
+    uint32_t last_tlm_ms;       /* rate-limit for TLM                       */
+} steering_pid_t;
+
+/* ---- API ---- */
+
+/*
+ * steering_pid_init(s) — zero state, enable controller.
+ * Call once during system init.
+ */
+void steering_pid_init(steering_pid_t *s);
+
+/*
+ * steering_pid_reset(s) — zero integrator, setpoint and output.
+ * Preserves enabled flag.  Call on disarm.
+ */
+void steering_pid_reset(steering_pid_t *s);
+
+/*
+ * steering_pid_set_target(s, omega_dps) — update setpoint.
+ *   omega_dps : desired yaw rate in deg/s.
+ *   Converts from JLINK_CMD_DRIVE steer field: omega = steer * STEER_OMEGA_SCALE.
+ *   No-op if disabled (output remains 0).
+ */
+void steering_pid_set_target(steering_pid_t *s, float omega_dps);
+
+/*
+ * steering_pid_update(s, actual_omega_dps, dt) — advance PID one step.
+ *   actual_omega_dps : IMU gyro Z rate (deg/s) — use IMUData.yaw_rate.
+ *   dt               : loop interval (seconds).
+ * Returns differential output (-STEER_OUTPUT_MAX..+STEER_OUTPUT_MAX).
+ * Returns 0 if disabled or dt <= 0.
+ */
+int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt);
+
+/*
+ * steering_pid_get_output(s) — last computed differential output.
+ */
+int16_t steering_pid_get_output(const steering_pid_t *s);
+
+/*
+ * steering_pid_set_enabled(s, en) — enable or disable the controller.
+ * Disabling resets integrator and zeroes output.
+ */
+void steering_pid_set_enabled(steering_pid_t *s, bool en);
+
+/*
+ * steering_pid_send_tlm(s, now_ms) — transmit JLINK_TLM_STEERING (0x8A)
+ * frame to Jetson.  Rate-limited to STEER_TLM_HZ; safe to call every tick.
+ */
+void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms);
+
+#endif /* STEERING_PID_H */

jetson/ros2_ws/src/saltybot_obstacle_detect/config/obstacle_detect_params.yaml

@@ -0,0 +1,35 @@
+obstacle_detect:
+  ros__parameters:
+    # ── Depth processing ──────────────────────────────────────────────────────
+    downsample_factor:    8       # stride-based downsample before processing
+                                  # 640x480 / 8 = 80x60 = 4800 pts (fast RANSAC)
+
+    # ── RANSAC ground plane fitting ───────────────────────────────────────────
+    ransac_n_iter:        50      # RANSAC iterations
+    ransac_inlier_m:      0.06    # inlier distance threshold (m)
+
+    # ── Obstacle height filter ────────────────────────────────────────────────
+    min_obstacle_h_m:     0.05    # min height above ground to count as obstacle (m)
+                                  #   — avoids false positives from ground noise
+    max_obstacle_h_m:     0.80    # max height above ground to count as obstacle (m)
+                                  #   — ignores ceiling / upper structure (D435i ~0.5m mount)
+
+    # ── Clustering ────────────────────────────────────────────────────────────
+    cluster_cell_m:       0.30    # 2D grid cell size for BFS clustering (m)
+    cluster_min_pts:      5       # minimum points for a cluster to be reported
+    max_obstacle_dist_m:  5.0     # discard obstacle points beyond this forward distance
+
+    # ── Alert / e-stop thresholds ─────────────────────────────────────────────
+    danger_dist_m:        0.40    # closest obstacle z < this → DANGER alert
+    warn_dist_m:          1.20    # closest obstacle z < this → WARN alert
+
+    # ── Safety zone integration ───────────────────────────────────────────────
+    # When enabled, DANGER obstacles publish zero Twist to depth_estop_topic.
+    # Wire this into the cmd_vel safety chain so safety_zone can latch e-stop.
+    # Typical chain:  depth_estop_topic=/cmd_vel_input  →  safety_zone  →  /cmd_vel
+    depth_estop_enabled:  false
+    depth_estop_topic:    /cmd_vel_input
+
+    # ── Frame / topic configuration ───────────────────────────────────────────
+    camera_frame:         camera_link    # frame_id for MarkerArray and PointCloud2
+    marker_lifetime_s:    0.20           # Marker.lifetime — 0.2 s = 6 frames at 30 Hz

jetson/ros2_ws/src/saltybot_obstacle_detect/launch/obstacle_detect.launch.py

@@ -0,0 +1,29 @@
+"""obstacle_detect.launch.py — RealSense depth obstacle detection (Issue #611)."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('saltybot_obstacle_detect')
+    default_params = os.path.join(pkg_share, 'config', 'obstacle_detect_params.yaml')
+
+    params_arg = DeclareLaunchArgument(
+        'params_file',
+        default_value=default_params,
+        description='Path to obstacle_detect_params.yaml',
+    )
+
+    obstacle_detect_node = Node(
+        package='saltybot_obstacle_detect',
+        executable='obstacle_detect',
+        name='obstacle_detect',
+        output='screen',
+        parameters=[LaunchConfiguration('params_file')],
+    )
+
+    return LaunchDescription([params_arg, obstacle_detect_node])

jetson/ros2_ws/src/saltybot_obstacle_detect/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_obstacle_detect</name>
+  <version>0.1.0</version>
+  <description>
+    RealSense D435i depth-based obstacle detection node (Issue #611).
+    RANSAC ground-plane fitting, 2D grid-BFS obstacle clustering, publishes
+    MarkerArray centroids (/saltybot/obstacles), PointCloud2 of non-ground
+    points (/saltybot/obstacles/cloud), and JSON alert (/saltybot/obstacles/alert)
+    with safety_zone e-stop integration via configurable Twist output.
+  </description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>cv_bridge</depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-opencv</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_obstacle_detect/saltybot_obstacle_detect/ground_plane.py

@@ -0,0 +1,192 @@
+"""
+ground_plane.py — RANSAC ground plane fitting for D435i depth images (Issue #611).
+
+No ROS2 dependencies — pure Python + numpy, fully unit-testable.
+
+Algorithm
+─────────
+RANSAC over 3D point cloud in camera optical frame:
+  +X = right, +Y = down, +Z = forward (depth)
+
+1. Sample 3 random non-collinear points.
+2. Fit plane: normal n, offset d  (n·p = d, ‖n‖=1).
+3. Count inliers: |n·p - d| < inlier_thresh_m.
+4. Repeat n_iter times; keep plane with most inliers.
+5. Optionally refine: refit normal via SVD on inlier subset.
+
+Normal orientation:
+  After RANSAC the normal is flipped so it points in the −Y direction
+  (upward in world / toward the camera). This makes "height above ground"
+  unambiguous: h = d − n·p > 0 means the point is above the plane.
+
+  Equivalently: normal.y < 0 after orientation.
+
+Height above ground:
+  h(p) = d − n·p
+
+  h > 0 → above plane (obstacle candidate)
+  h ≈ 0 → on the ground
+  h < 0 → below ground (artefact / noise)
+
+Obstacle filter:
+  min_obstacle_h < h < max_obstacle_h   (configurable, metres)
+
+  Typical D435i camera mount ~0.5 m above ground:
+    min_obstacle_h = 0.05 m  (ignore ground noise)
+    max_obstacle_h = 0.80 m  (ignore ceiling / upper structure)
+"""
+
+from __future__ import annotations
+
+import math
+from typing import Optional, Tuple
+
+import numpy as np
+
+
+PlaneParams = Tuple[np.ndarray, float]   # (normal_3, d_scalar)
+
+
+# ── RANSAC ─────────────────────────────────────────────────────────────────────
+
+def fit_ground_plane(
+    points:          np.ndarray,
+    n_iter:          int   = 50,
+    inlier_thresh_m: float = 0.06,
+    min_inlier_frac: float = 0.30,
+    refine:          bool  = True,
+) -> Optional[PlaneParams]:
+    """
+    RANSAC ground-plane estimation from an (N, 3) point array.
+
+    Parameters
+    ----------
+    points          : (N, 3) float32/64 point cloud in camera optical frame
+    n_iter          : RANSAC iterations
+    inlier_thresh_m : inlier distance threshold (m)
+    min_inlier_frac : minimum fraction of points that must be inliers
+    refine          : if True, refit plane via SVD on best inlier set
+
+    Returns
+    -------
+    (normal, d) if a plane was found, else None.
+    Normal is oriented so that normal.y < 0  (points upward in world).
+    """
+    n = len(points)
+    if n < 10:
+        return None
+
+    rng          = np.random.default_rng(seed=42)
+    best_n_in    = -1
+    best_params: Optional[PlaneParams] = None
+    pts          = points.astype(np.float64)
+
+    for _ in range(n_iter):
+        idx = rng.integers(0, n, size=3)
+        p0, p1, p2 = pts[idx[0]], pts[idx[1]], pts[idx[2]]
+
+        edge1 = p1 - p0
+        edge2 = p2 - p0
+        normal = np.cross(edge1, edge2)
+        norm_len = float(np.linalg.norm(normal))
+        if norm_len < 1e-8:
+            continue
+
+        normal /= norm_len
+        d = float(np.dot(normal, p0))
+
+        dists   = np.abs(pts @ normal - d)
+        inliers = dists < inlier_thresh_m
+        n_in    = int(inliers.sum())
+
+        if n_in > best_n_in:
+            best_n_in = n_in
+            best_params = (normal.copy(), d)
+
+    if best_params is None:
+        return None
+    if best_n_in < int(n * min_inlier_frac):
+        return None
+
+    normal, d = best_params
+
+    # ── SVD refinement ──────────────────────────────────────────────────────
+    if refine:
+        dists   = np.abs(pts @ normal - d)
+        in_mask = dists < inlier_thresh_m
+        if in_mask.sum() >= 4:
+            in_pts = pts[in_mask]
+            centroid = in_pts.mean(axis=0)
+            _, _, Vt = np.linalg.svd(in_pts - centroid)
+            normal = Vt[-1]               # smallest singular value = plane normal
+            normal /= float(np.linalg.norm(normal))
+            d = float(np.dot(normal, centroid))
+
+    # Orient normal upward: ensure n.y < 0 (−Y = up in camera frame)
+    if normal[1] > 0:
+        normal = -normal
+        d      = -d
+
+    return normal, d
+
+
+# ── Point classification ───────────────────────────────────────────────────────
+
+def height_above_plane(
+    points: np.ndarray,
+    plane:  PlaneParams,
+) -> np.ndarray:
+    """
+    Signed height of each point above the ground plane.
+
+    h > 0  → above ground (potential obstacle)
+    h ≈ 0  → on ground
+    h < 0  → subsurface artefact / sensor noise
+
+    Parameters
+    ----------
+    points : (N, 3) point cloud
+    plane  : (normal, d) from fit_ground_plane()
+
+    Returns
+    -------
+    heights : (N,) float64
+    """
+    normal, d = plane
+    # h = d - n·p  (positive when point is "above" the upward-facing normal)
+    return d - (points.astype(np.float64) @ normal)
+
+
+def obstacle_mask(
+    points:             np.ndarray,
+    plane:              PlaneParams,
+    min_height_m:       float = 0.05,
+    max_height_m:       float = 0.80,
+) -> np.ndarray:
+    """
+    Boolean mask: True where a point is an obstacle (above ground, in height window).
+
+    Parameters
+    ----------
+    points       : (N, 3) point cloud
+    plane        : ground plane params
+    min_height_m : minimum height above ground to count as obstacle
+    max_height_m : maximum height above ground (ceiling / upper structure cut-off)
+
+    Returns
+    -------
+    mask : (N,) bool
+    """
+    h = height_above_plane(points, plane)
+    return (h > min_height_m) & (h < max_height_m)
+
+
+def ground_mask(
+    points:         np.ndarray,
+    plane:          PlaneParams,
+    inlier_thresh:  float = 0.06,
+) -> np.ndarray:
+    """Boolean mask: True for ground-plane inlier points."""
+    normal, d = plane
+    dists = np.abs(points.astype(np.float64) @ normal - d)
+    return dists < inlier_thresh

jetson/ros2_ws/src/saltybot_obstacle_detect/saltybot_obstacle_detect/obstacle_clusterer.py

@@ -0,0 +1,170 @@
+"""
+obstacle_clusterer.py — 2D grid-based obstacle clustering (Issue #611).
+
+No ROS2 dependencies — pure Python + numpy, fully unit-testable.
+
+Algorithm
+─────────
+After RANSAC ground removal, obstacle points (non-ground) are clustered
+using a 2D occupancy grid on the horizontal (X–Z) plane in camera frame.
+
+Steps:
+  1. Project obstacle 3D points onto the (X, Z) bird's-eye plane.
+  2. Discretise into grid cells of side `cell_m`.
+  3. 4-connected BFS flood-fill to find connected components.
+  4. For each component with ≥ min_pts 3D points:
+       centroid = mean of the component's 3D points
+       radius   = max distance from centroid (bounding-sphere radius)
+
+Camera frame convention:
+  +X = right, +Y = down (not used for 2D projection), +Z = forward (depth)
+
+Output per cluster
+──────────────────
+  ObstacleCluster (dataclass):
+    centroid  : (3,) float array  — (cx, cy, cz) in camera frame
+    radius_m  : float             — bounding-sphere radius (m)
+    height_m  : float             — mean height above ground plane (m)
+    n_pts     : int               — number of 3D points in cluster
+"""
+
+from __future__ import annotations
+
+from collections import deque
+from dataclasses import dataclass, field
+from typing import List, Tuple
+
+import numpy as np
+
+
+@dataclass
+class ObstacleCluster:
+    centroid: np.ndarray          # (3,) float64 in camera optical frame
+    radius_m: float
+    height_m: float
+    n_pts:    int
+
+    @property
+    def distance_m(self) -> float:
+        """Forward distance (Z in camera frame)."""
+        return float(self.centroid[2])
+
+    @property
+    def lateral_m(self) -> float:
+        """Lateral offset (X in camera frame; positive = right)."""
+        return float(self.centroid[0])
+
+
+def cluster_obstacles(
+    points:    np.ndarray,
+    heights:   np.ndarray,
+    cell_m:    float = 0.30,
+    min_pts:   int   = 5,
+    x_range:   Tuple[float, float] = (-4.0, 4.0),
+    z_range:   Tuple[float, float] = (0.15, 6.0),
+) -> List[ObstacleCluster]:
+    """
+    Cluster obstacle points into connected groups.
+
+    Parameters
+    ----------
+    points   : (N, 3) float array — obstacle points in camera frame
+    heights  : (N,)  float array — height above ground for each point
+    cell_m   : grid cell side length (m)
+    min_pts  : minimum points for a cluster to be reported
+    x_range  : (min_x, max_x) bounds for the bird's-eye grid (m)
+    z_range  : (min_z, max_z) forward-distance bounds (m)
+
+    Returns
+    -------
+    clusters : list of ObstacleCluster, sorted by ascending distance (z)
+    """
+    if len(points) == 0:
+        return []
+
+    pts = points.astype(np.float64)
+
+    # ── Discard out-of-range points ─────────────────────────────────────────
+    x = pts[:, 0]
+    z = pts[:, 2]
+    valid = (
+        (x >= x_range[0]) & (x <= x_range[1]) &
+        (z >= z_range[0]) & (z <= z_range[1])
+    )
+    pts     = pts[valid]
+    heights = heights.astype(np.float64)[valid]
+
+    if len(pts) == 0:
+        return []
+
+    # ── Build 2D occupancy grid (X, Z) ──────────────────────────────────────
+    x_min, x_max = x_range
+    z_min, z_max = z_range
+    n_x = max(1, int(math.ceil((x_max - x_min) / cell_m)))
+    n_z = max(1, int(math.ceil((z_max - z_min) / cell_m)))
+
+    xi = np.clip(((pts[:, 0] - x_min) / cell_m).astype(np.int32), 0, n_x - 1)
+    zi = np.clip(((pts[:, 2] - z_min) / cell_m).astype(np.int32), 0, n_z - 1)
+
+    # Map each grid cell to list of 3D-point indices
+    cell_pts: dict[Tuple[int, int], list] = {}
+    for idx, (gx, gz) in enumerate(zip(xi.tolist(), zi.tolist())):
+        key = (gx, gz)
+        if key not in cell_pts:
+            cell_pts[key] = []
+        cell_pts[key].append(idx)
+
+    occupied = set(cell_pts.keys())
+
+    # ── BFS connected-component labelling ────────────────────────────────────
+    visited  = set()
+    clusters = []
+
+    for start in occupied:
+        if start in visited:
+            continue
+
+        # BFS flood fill
+        component_cells = []
+        queue = deque([start])
+        visited.add(start)
+
+        while queue:
+            cx, cz = queue.popleft()
+            component_cells.append((cx, cz))
+
+            for dx, dz in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
+                nb = (cx + dx, cz + dz)
+                if nb in occupied and nb not in visited:
+                    visited.add(nb)
+                    queue.append(nb)
+
+        # Collect 3D points in this component
+        pt_indices = []
+        for cell in component_cells:
+            pt_indices.extend(cell_pts[cell])
+
+        if len(pt_indices) < min_pts:
+            continue
+
+        cluster_pts    = pts[pt_indices]
+        cluster_h      = heights[pt_indices]
+        centroid       = cluster_pts.mean(axis=0)
+        dists          = np.linalg.norm(cluster_pts - centroid, axis=1)
+        radius_m       = float(dists.max())
+        mean_height    = float(cluster_h.mean())
+
+        clusters.append(ObstacleCluster(
+            centroid = centroid,
+            radius_m = radius_m,
+            height_m = mean_height,
+            n_pts    = len(pt_indices),
+        ))
+
+    # Sort by forward distance
+    clusters.sort(key=lambda c: c.distance_m)
+    return clusters
+
+
+# ── Local import for math ─────────────────────────────────────────────────────
+import math

jetson/ros2_ws/src/saltybot_obstacle_detect/saltybot_obstacle_detect/obstacle_detect_node.py

@@ -0,0 +1,512 @@
+"""
+obstacle_detect_node.py — RealSense depth obstacle detection node (Issue #611).
+
+Pipeline per frame (30 Hz)
+──────────────────────────
+1. Receive D435i aligned depth image (float32, metres) + camera_info.
+2. Downsample by `downsample_factor` for efficiency.
+3. Back-project non-zero pixels to 3D (camera optical frame, +Z forward).
+4. RANSAC ground-plane fitting on the 3D cloud.
+5. Classify points: ground (inlier) vs obstacle (above ground, in height window).
+6. Cluster obstacle points using 2D grid BFS on the (X, Z) plane.
+7. Publish:
+     /saltybot/obstacles          visualization_msgs/MarkerArray  (centroids)
+     /saltybot/obstacles/cloud    sensor_msgs/PointCloud2         (non-ground pts)
+     /saltybot/obstacles/alert    std_msgs/String                 (JSON alert)
+8. When `depth_estop_enabled` and DANGER detected: publish zero Twist to
+   `depth_estop_topic` (default /cmd_vel_input) for safety_zone integration.
+
+Obstacle alert levels (JSON on /saltybot/obstacles/alert)
+──────────────────────────────────────────────────────────
+  DANGER  — closest obstacle z < danger_dist_m  (requires immediate stop)
+  WARN    — closest obstacle z < warn_dist_m    (caution)
+  CLEAR   — no obstacles in range
+
+Safety zone integration
+───────────────────────
+When `depth_estop_enabled: true`, DANGER obstacles trigger a zero Twist
+published to `depth_estop_topic` (default: /cmd_vel_input).
+This feeds into the safety_zone's cmd_vel chain and triggers its e-stop,
+giving independent depth-based stopping complementary to the LIDAR zones.
+
+Subscribes
+──────────
+  /camera/depth/image_rect_raw    sensor_msgs/Image      30 Hz float32 (m)
+  /camera/depth/camera_info       sensor_msgs/CameraInfo (once)
+
+Publishes
+─────────
+  /saltybot/obstacles             visualization_msgs/MarkerArray
+  /saltybot/obstacles/cloud       sensor_msgs/PointCloud2
+  /saltybot/obstacles/alert       std_msgs/String   (JSON)
+  [depth_estop_topic]             geometry_msgs/Twist  (zero, when DANGER)
+
+Parameters (see config/obstacle_detect_params.yaml)
+────────────────────────────────────────────────────
+  downsample_factor    int    8       factor to reduce image before processing
+  ransac_n_iter        int    50      RANSAC iterations for ground plane
+  ransac_inlier_m      float  0.06    inlier threshold (m)
+  min_obstacle_h_m     float  0.05    min height above ground to be obstacle
+  max_obstacle_h_m     float  0.80    max height above ground to be obstacle
+  cluster_cell_m       float  0.30    clustering grid cell size (m)
+  cluster_min_pts      int    5       minimum points per cluster
+  max_obstacle_dist_m  float  5.0     discard obstacles beyond this distance
+  danger_dist_m        float  0.40    obstacle z < this → DANGER
+  warn_dist_m          float  1.20    obstacle z < this → WARN
+  depth_estop_enabled  bool   false   publish zero-vel when DANGER
+  depth_estop_topic    str    /cmd_vel_input
+  camera_frame         str    camera_link
+  marker_lifetime_s    float  0.2     MarkerArray marker lifetime (s)
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import struct
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+
+import numpy as np
+from cv_bridge import CvBridge
+
+from geometry_msgs.msg import Point, Twist, Vector3
+from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField
+from std_msgs.msg import Header, String, ColorRGBA
+from visualization_msgs.msg import Marker, MarkerArray
+
+from .ground_plane import fit_ground_plane, height_above_plane, obstacle_mask
+from .obstacle_clusterer import ObstacleCluster, cluster_obstacles
+
+
+# ── QoS ───────────────────────────────────────────────────────────────────────
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+_LATCHED_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+)
+
+# Alert levels
+CLEAR  = 'CLEAR'
+WARN   = 'WARN'
+DANGER = 'DANGER'
+
+# Marker colours per alert level
+_COLOUR = {
+    CLEAR:  (0.0, 1.0, 0.0, 0.8),   # green
+    WARN:   (1.0, 0.8, 0.0, 0.9),   # yellow
+    DANGER: (1.0, 0.1, 0.1, 1.0),   # red
+}
+
+# D435i depth scale (image is already float32 in metres when using 32FC1)
+_DEPTH_ENCODING = '32FC1'
+
+
+class ObstacleDetectNode(Node):
+    """RealSense depth-based obstacle detection — see module docstring."""
+
+    def __init__(self) -> None:
+        super().__init__('obstacle_detect')
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter('downsample_factor',   8)
+        self.declare_parameter('ransac_n_iter',       50)
+        self.declare_parameter('ransac_inlier_m',     0.06)
+        self.declare_parameter('min_obstacle_h_m',    0.05)
+        self.declare_parameter('max_obstacle_h_m',    0.80)
+        self.declare_parameter('cluster_cell_m',      0.30)
+        self.declare_parameter('cluster_min_pts',     5)
+        self.declare_parameter('max_obstacle_dist_m', 5.0)
+        self.declare_parameter('danger_dist_m',       0.40)
+        self.declare_parameter('warn_dist_m',         1.20)
+        self.declare_parameter('depth_estop_enabled', False)
+        self.declare_parameter('depth_estop_topic',   '/cmd_vel_input')
+        self.declare_parameter('camera_frame',        'camera_link')
+        self.declare_parameter('marker_lifetime_s',   0.2)
+
+        self._ds          = self.get_parameter('downsample_factor').value
+        self._n_iter      = self.get_parameter('ransac_n_iter').value
+        self._inlier_m    = self.get_parameter('ransac_inlier_m').value
+        self._min_h       = self.get_parameter('min_obstacle_h_m').value
+        self._max_h       = self.get_parameter('max_obstacle_h_m').value
+        self._cell_m      = self.get_parameter('cluster_cell_m').value
+        self._min_pts     = self.get_parameter('cluster_min_pts').value
+        self._max_dist    = self.get_parameter('max_obstacle_dist_m').value
+        self._danger_m    = self.get_parameter('danger_dist_m').value
+        self._warn_m      = self.get_parameter('warn_dist_m').value
+        self._estop_en    = self.get_parameter('depth_estop_enabled').value
+        self._cam_frame   = self.get_parameter('camera_frame').value
+        self._marker_life = self.get_parameter('marker_lifetime_s').value
+
+        estop_topic = self.get_parameter('depth_estop_topic').value
+
+        # ── Internal state ────────────────────────────────────────────────────
+        self._bridge   = CvBridge()
+        self._fx: float | None = None
+        self._fy: float | None = None
+        self._cx: float | None = None
+        self._cy: float | None = None
+
+        # Ground plane stability: blend current with previous (exponential)
+        self._plane_normal: np.ndarray | None = None
+        self._plane_d:      float | None       = None
+        self._plane_alpha   = 0.3   # blend weight for new plane estimate
+
+        # ── Publishers ─────────────────────────────────────────────────────────
+        self._marker_pub = self.create_publisher(
+            MarkerArray, '/saltybot/obstacles', 10
+        )
+        self._cloud_pub = self.create_publisher(
+            PointCloud2, '/saltybot/obstacles/cloud', 10
+        )
+        self._alert_pub = self.create_publisher(
+            String, '/saltybot/obstacles/alert', 10
+        )
+        if self._estop_en:
+            self._estop_pub = self.create_publisher(
+                Twist, estop_topic, 10
+            )
+        else:
+            self._estop_pub = None
+
+        # ── Subscriptions ──────────────────────────────────────────────────────
+        self.create_subscription(
+            CameraInfo,
+            '/camera/depth/camera_info',
+            self._on_camera_info,
+            _LATCHED_QOS,
+        )
+        self.create_subscription(
+            Image,
+            '/camera/depth/image_rect_raw',
+            self._on_depth,
+            _SENSOR_QOS,
+        )
+
+        self.get_logger().info(
+            f'obstacle_detect ready — '
+            f'ds={self._ds} ransac={self._n_iter}it '
+            f'danger={self._danger_m}m warn={self._warn_m}m '
+            f'estop={"ON → " + estop_topic if self._estop_en else "OFF"}'
+        )
+
+    # ── Camera info ───────────────────────────────────────────────────────────
+
+    def _on_camera_info(self, msg: CameraInfo) -> None:
+        if self._fx is not None:
+            return
+        self._fx = float(msg.k[0])
+        self._fy = float(msg.k[4])
+        self._cx = float(msg.k[2])
+        self._cy = float(msg.k[5])
+        self.get_logger().info(
+            f'camera_info: fx={self._fx:.1f} fy={self._fy:.1f} '
+            f'cx={self._cx:.1f} cy={self._cy:.1f}'
+        )
+
+    # ── Main depth callback ───────────────────────────────────────────────────
+
+    def _on_depth(self, msg: Image) -> None:
+        if self._fx is None:
+            return  # wait for camera_info
+
+        # ── Decode depth ──────────────────────────────────────────────────────
+        try:
+            depth = self._bridge.imgmsg_to_cv2(msg, desired_encoding=_DEPTH_ENCODING)
+        except Exception as exc:
+            self.get_logger().warning(f'depth decode error: {exc}', throttle_duration_sec=5.0)
+            return
+
+        h_img, w_img = depth.shape
+        stamp  = msg.header.stamp
+
+        # ── Downsample ────────────────────────────────────────────────────────
+        ds = self._ds
+        depth_ds = depth[::ds, ::ds]   # stride-based downsample
+        h_ds, w_ds = depth_ds.shape
+
+        # Back-projection intrinsics at downsampled resolution
+        fx = self._fx / ds
+        fy = self._fy / ds
+        cx = self._cx / ds
+        cy = self._cy / ds
+
+        # ── Back-project to 3D ────────────────────────────────────────────────
+        points = _backproject(depth_ds, fx, fy, cx, cy, max_dist=self._max_dist)
+
+        if len(points) < 30:
+            self._publish_empty(stamp)
+            return
+
+        # ── RANSAC ground plane ───────────────────────────────────────────────
+        result = fit_ground_plane(
+            points,
+            n_iter          = self._n_iter,
+            inlier_thresh_m = self._inlier_m,
+        )
+
+        if result is None:
+            self._publish_empty(stamp)
+            return
+
+        new_normal, new_d = result
+
+        # Blend with previous plane for temporal stability
+        if self._plane_normal is None:
+            self._plane_normal = new_normal
+            self._plane_d      = new_d
+        else:
+            alpha = self._plane_alpha
+            blended = (1.0 - alpha) * self._plane_normal + alpha * new_normal
+            norm_len = float(np.linalg.norm(blended))
+            if norm_len > 1e-8:
+                self._plane_normal = blended / norm_len
+                self._plane_d      = (1.0 - alpha) * self._plane_d + alpha * new_d
+
+        plane = (self._plane_normal, self._plane_d)
+
+        # ── Obstacle mask ─────────────────────────────────────────────────────
+        obs_mask = obstacle_mask(points, plane, self._min_h, self._max_h)
+        obs_pts  = points[obs_mask]
+
+        heights_all = height_above_plane(points, plane)
+        obs_heights = heights_all[obs_mask]
+
+        # ── Cluster obstacles ─────────────────────────────────────────────────
+        clusters = cluster_obstacles(
+            obs_pts, obs_heights,
+            cell_m  = self._cell_m,
+            min_pts = self._min_pts,
+            z_range = (0.15, self._max_dist),
+        )
+
+        # ── Alert level ───────────────────────────────────────────────────────
+        alert_level  = CLEAR
+        closest_dist = float('inf')
+
+        for cluster in clusters:
+            d = cluster.distance_m
+            if d < closest_dist:
+                closest_dist = d
+            if d < self._danger_m:
+                alert_level = DANGER
+                break
+            if d < self._warn_m and alert_level != DANGER:
+                alert_level = WARN
+
+        if closest_dist == float('inf'):
+            closest_dist = -1.0
+
+        # ── Publish ───────────────────────────────────────────────────────────
+        self._publish_markers(clusters, stamp)
+        self._publish_cloud(obs_pts, stamp)
+        self._publish_alert(alert_level, clusters, closest_dist, stamp)
+
+        # Depth e-stop: feed zero vel to safety zone chain
+        if self._estop_en and self._estop_pub is not None and alert_level == DANGER:
+            self._estop_pub.publish(Twist())
+
+    # ── Publishers ────────────────────────────────────────────────────────────
+
+    def _publish_markers(self, clusters: list, stamp) -> None:
+        """Publish obstacle centroids as sphere MarkerArray."""
+        ma      = MarkerArray()
+        lifetime = _ros_duration(self._marker_life)
+
+        # Delete-all marker first for clean slate
+        del_marker         = Marker()
+        del_marker.action  = Marker.DELETEALL
+        del_marker.header.stamp    = stamp
+        del_marker.header.frame_id = self._cam_frame
+        ma.markers.append(del_marker)
+
+        for idx, cluster in enumerate(clusters):
+            level  = self._cluster_alert_level(cluster)
+            r, g, b, a = _COLOUR[level]
+            cx, cy, cz = cluster.centroid
+
+            m               = Marker()
+            m.header.stamp    = stamp
+            m.header.frame_id = self._cam_frame
+            m.ns              = 'obstacles'
+            m.id              = idx + 1
+            m.type            = Marker.SPHERE
+            m.action          = Marker.ADD
+            m.pose.position   = Point(x=float(cx), y=float(cy), z=float(cz))
+            m.pose.orientation.w = 1.0
+            rad = max(0.05, cluster.radius_m)
+            m.scale           = Vector3(x=rad * 2.0, y=rad * 2.0, z=rad * 2.0)
+            m.color           = ColorRGBA(r=r, g=g, b=b, a=a)
+            m.lifetime        = lifetime
+            ma.markers.append(m)
+
+            # Text label above sphere
+            lbl               = Marker()
+            lbl.header          = m.header
+            lbl.ns              = 'obstacle_labels'
+            lbl.id              = idx + 1
+            lbl.type            = Marker.TEXT_VIEW_FACING
+            lbl.action          = Marker.ADD
+            lbl.pose.position   = Point(x=float(cx), y=float(cy) - 0.15, z=float(cz))
+            lbl.pose.orientation.w = 1.0
+            lbl.scale.z         = 0.12
+            lbl.color           = ColorRGBA(r=1.0, g=1.0, b=1.0, a=0.9)
+            lbl.text            = f'{cluster.distance_m:.2f}m'
+            lbl.lifetime        = lifetime
+            ma.markers.append(lbl)
+
+        self._marker_pub.publish(ma)
+
+    def _publish_cloud(self, points: np.ndarray, stamp) -> None:
+        """Publish non-ground obstacle points as PointCloud2 (xyz float32)."""
+        if len(points) == 0:
+            # Publish empty cloud
+            cloud              = PointCloud2()
+            cloud.header.stamp    = stamp
+            cloud.header.frame_id = self._cam_frame
+            self._cloud_pub.publish(cloud)
+            return
+
+        pts_f32 = points.astype(np.float32)
+
+        fields = [
+            PointField(name='x', offset=0,  datatype=PointField.FLOAT32, count=1),
+            PointField(name='y', offset=4,  datatype=PointField.FLOAT32, count=1),
+            PointField(name='z', offset=8,  datatype=PointField.FLOAT32, count=1),
+        ]
+        cloud                 = PointCloud2()
+        cloud.header.stamp    = stamp
+        cloud.header.frame_id = self._cam_frame
+        cloud.height          = 1
+        cloud.width           = len(pts_f32)
+        cloud.fields          = fields
+        cloud.is_bigendian    = False
+        cloud.point_step      = 12   # 3 × float32
+        cloud.row_step        = cloud.point_step * cloud.width
+        cloud.data            = pts_f32.tobytes()
+        cloud.is_dense        = True
+        self._cloud_pub.publish(cloud)
+
+    def _publish_alert(
+        self,
+        level:       str,
+        clusters:    list,
+        closest_m:   float,
+        stamp,
+    ) -> None:
+        obstacles_json = [
+            {
+                'x':        round(float(c.centroid[0]), 3),
+                'y':        round(float(c.centroid[1]), 3),
+                'z':        round(float(c.centroid[2]), 3),
+                'radius_m': round(c.radius_m, 3),
+                'height_m': round(c.height_m, 3),
+                'n_pts':    c.n_pts,
+                'level':    self._cluster_alert_level(c),
+            }
+            for c in clusters
+        ]
+        alert = {
+            'alert_level':      level,
+            'closest_m':        round(closest_m, 3),
+            'obstacle_count':   len(clusters),
+            'obstacles':        obstacles_json,
+        }
+        msg      = String()
+        msg.data = json.dumps(alert)
+        self._alert_pub.publish(msg)
+
+    def _publish_empty(self, stamp) -> None:
+        """Publish empty state (no obstacles detected / ground plane failed)."""
+        self._publish_markers([], stamp)
+        self._publish_cloud(np.zeros((0, 3), np.float32), stamp)
+        alert = {
+            'alert_level':    CLEAR,
+            'closest_m':      -1.0,
+            'obstacle_count': 0,
+            'obstacles':      [],
+        }
+        msg      = String()
+        msg.data = json.dumps(alert)
+        self._alert_pub.publish(msg)
+
+    # ── Helpers ───────────────────────────────────────────────────────────────
+
+    def _cluster_alert_level(self, cluster: ObstacleCluster) -> str:
+        d = cluster.distance_m
+        if d < self._danger_m:
+            return DANGER
+        if d < self._warn_m:
+            return WARN
+        return CLEAR
+
+
+# ── Pure-function helpers ──────────────────────────────────────────────────────
+
+def _backproject(
+    depth:    np.ndarray,
+    fx: float, fy: float,
+    cx: float, cy: float,
+    min_dist: float = 0.15,
+    max_dist: float = 6.0,
+) -> np.ndarray:
+    """
+    Back-project a float32 depth image (metres) to a (N, 3) 3D point array.
+
+    Camera optical frame: +X right, +Y down, +Z forward.
+    Only returns pixels where depth is in [min_dist, max_dist].
+    """
+    h, w = depth.shape
+
+    # Build pixel-grid meshgrid
+    u_arr = np.arange(w, dtype=np.float32)
+    v_arr = np.arange(h, dtype=np.float32)
+    uu, vv = np.meshgrid(u_arr, v_arr)
+
+    z = depth.astype(np.float32).ravel()
+    u = uu.ravel()
+    v = vv.ravel()
+
+    valid = (z > min_dist) & (z < max_dist) & np.isfinite(z)
+    z = z[valid]
+    u = u[valid]
+    v = v[valid]
+
+    x = (u - cx) * z / fx
+    y = (v - cy) * z / fy
+
+    return np.stack([x, y, z], axis=1).astype(np.float64)
+
+
+def _ros_duration(seconds: float):
+    """Build a rclpy Duration-compatible value for Marker.lifetime."""
+    from rclpy.duration import Duration
+    sec  = int(seconds)
+    nsec = int((seconds - sec) * 1e9)
+    return Duration(seconds=sec, nanoseconds=nsec).to_msg()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = ObstacleDetectNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_obstacle_detect/setup.cfg

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

jetson/ros2_ws/src/saltybot_obstacle_detect/setup.py

@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'saltybot_obstacle_detect'
+
+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='RealSense depth obstacle detection — RANSAC ground + clustering (Issue #611)',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'obstacle_detect = saltybot_obstacle_detect.obstacle_detect_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_obstacle_detect/test/test_ground_plane.py

@@ -0,0 +1,129 @@
+"""Unit tests for ground_plane.py — RANSAC fitting and point classification."""
+
+import math
+import numpy as np
+import pytest
+
+from saltybot_obstacle_detect.ground_plane import (
+    fit_ground_plane,
+    height_above_plane,
+    obstacle_mask,
+    ground_mask,
+)
+
+
+def make_flat_ground(n=500, y_ground=1.0, noise=0.02, rng=None):
+    """Flat ground plane at y = y_ground (camera frame: Y down)."""
+    if rng is None:
+        rng = np.random.default_rng(0)
+    x = rng.uniform(-2.0, 2.0, n).astype(np.float64)
+    z = rng.uniform(0.5, 4.0, n).astype(np.float64)
+    y = np.full(n, y_ground) + rng.normal(0, noise, n)
+    return np.stack([x, y, z], axis=1)
+
+
+def make_obstacle_above_ground(cx=0.0, y_ground=1.0, height=0.3, n=50, rng=None):
+    """Points forming a box above the ground plane."""
+    if rng is None:
+        rng = np.random.default_rng(1)
+    x = rng.uniform(cx - 0.15, cx + 0.15, n)
+    z = rng.uniform(1.5, 2.0, n)
+    y = np.full(n, y_ground - height)   # Y_down: smaller y = higher in world
+    return np.stack([x, y, z], axis=1)
+
+
+# ── fit_ground_plane ──────────────────────────────────────────────────────────
+
+def test_fit_flat_ground_returns_result():
+    pts   = make_flat_ground(n=400)
+    plane = fit_ground_plane(pts, n_iter=50)
+    assert plane is not None
+
+
+def test_fit_ground_normal_points_upward():
+    """Normal should have negative Y component (upward in camera frame)."""
+    pts   = make_flat_ground(n=400)
+    plane = fit_ground_plane(pts, n_iter=50)
+    assert plane is not None
+    normal, d = plane
+    assert normal[1] < 0.0, f"Expected normal.y < 0, got {normal[1]:.3f}"
+
+
+def test_fit_ground_normal_unit_vector():
+    pts   = make_flat_ground(n=300)
+    plane = fit_ground_plane(pts, n_iter=50)
+    assert plane is not None
+    normal, _ = plane
+    assert abs(np.linalg.norm(normal) - 1.0) < 1e-6
+
+
+def test_fit_too_few_points_returns_none():
+    pts   = make_flat_ground(n=5)
+    plane = fit_ground_plane(pts, n_iter=10)
+    assert plane is None
+
+
+def test_fit_empty_returns_none():
+    plane = fit_ground_plane(np.zeros((0, 3)))
+    assert plane is None
+
+
+def test_fit_tilted_ground():
+    """Tilted ground plane (robot on slope) should still be found."""
+    rng = np.random.default_rng(42)
+    n   = 400
+    x   = rng.uniform(-2.0, 2.0, n)
+    z   = rng.uniform(0.5, 4.0, n)
+    # Tilt: y = 0.5 + 0.1*z (plane tilted 5.7° forward)
+    y   = 0.5 + 0.1 * z + rng.normal(0, 0.02, n)
+    pts = np.stack([x, y, z], axis=1)
+    plane = fit_ground_plane(pts, n_iter=80)
+    assert plane is not None
+
+
+# ── height_above_plane ────────────────────────────────────────────────────────
+
+def test_ground_pts_near_zero_height():
+    ground_pts = make_flat_ground(n=200, y_ground=1.0, noise=0.005)
+    plane      = fit_ground_plane(ground_pts, n_iter=60)
+    assert plane is not None
+    heights    = height_above_plane(ground_pts, plane)
+    assert np.abs(heights).mean() < 0.10   # mean residual < 10 cm
+
+
+def test_obstacle_pts_positive_height():
+    rng        = np.random.default_rng(7)
+    ground_pts = make_flat_ground(n=300, y_ground=1.0, rng=rng)
+    obs_pts    = make_obstacle_above_ground(y_ground=1.0, height=0.3, rng=rng)
+    all_pts    = np.vstack([ground_pts, obs_pts])
+    plane      = fit_ground_plane(all_pts, n_iter=60)
+    assert plane is not None
+    heights    = height_above_plane(obs_pts, plane)
+    assert heights.mean() > 0.1, f"Expected obstacles above ground, got mean h={heights.mean():.3f}"
+
+
+# ── obstacle_mask ─────────────────────────────────────────────────────────────
+
+def test_obstacle_mask_selects_correct_points():
+    rng        = np.random.default_rng(3)
+    ground_pts = make_flat_ground(n=300, y_ground=1.0, rng=rng)
+    obs_pts    = make_obstacle_above_ground(y_ground=1.0, height=0.4, n=60, rng=rng)
+    all_pts    = np.vstack([ground_pts, obs_pts])
+    plane      = fit_ground_plane(all_pts, n_iter=60)
+    assert plane is not None
+    mask = obstacle_mask(all_pts, plane, min_height_m=0.05, max_height_m=0.80)
+    # Most of the obs_pts should be selected
+    n_ground_selected = mask[:300].sum()
+    n_obs_selected    = mask[300:].sum()
+    assert n_obs_selected > 40, f"Expected most obs selected, got {n_obs_selected}/60"
+    assert n_ground_selected < 30, f"Expected few ground points selected, got {n_ground_selected}/300"
+
+
+# ── ground_mask ───────────────────────────────────────────────────────────────
+
+def test_ground_mask_covers_inliers():
+    pts   = make_flat_ground(n=300, noise=0.01)
+    plane = fit_ground_plane(pts, n_iter=60)
+    assert plane is not None
+    gmask = ground_mask(pts, plane, inlier_thresh=0.08)
+    assert gmask.mean() > 0.7   # at least 70% of flat ground should be ground inliers

jetson/ros2_ws/src/saltybot_obstacle_detect/test/test_obstacle_clusterer.py

@@ -0,0 +1,105 @@
+"""Unit tests for obstacle_clusterer.py — 2D grid BFS clustering."""
+
+import numpy as np
+import pytest
+
+from saltybot_obstacle_detect.obstacle_clusterer import cluster_obstacles, ObstacleCluster
+
+
+def _make_cluster_pts(cx, cz, n=30, spread=0.15, rng=None):
+    """Generate points around a centroid in camera frame (x=lateral, z=forward)."""
+    if rng is None:
+        rng = np.random.default_rng(0)
+    x = rng.uniform(cx - spread, cx + spread, n)
+    y = rng.uniform(-0.3, 0.0, n)   # above ground (height ~0.3m)
+    z = rng.uniform(cz - spread, cz + spread, n)
+    return np.stack([x, y, z], axis=1)
+
+
+def _make_heights(pts, base_h=0.3, noise=0.05, rng=None):
+    if rng is None:
+        rng = np.random.default_rng(1)
+    return np.full(len(pts), base_h) + rng.normal(0, noise, len(pts))
+
+
+# ── Basic clustering ──────────────────────────────────────────────────────────
+
+def test_single_cluster_detected():
+    pts     = _make_cluster_pts(cx=0.0, cz=2.0, n=40)
+    heights = _make_heights(pts)
+    clusters = cluster_obstacles(pts, heights, cell_m=0.30, min_pts=5)
+    assert len(clusters) == 1
+
+
+def test_two_clusters_separated():
+    rng  = np.random.default_rng(42)
+    pts1 = _make_cluster_pts(cx=-1.0, cz=2.0, n=40, rng=rng)
+    pts2 = _make_cluster_pts(cx=+1.5, cz=3.5, n=40, rng=rng)
+    pts  = np.vstack([pts1, pts2])
+    h    = _make_heights(pts, rng=rng)
+    clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
+    assert len(clusters) == 2
+
+
+def test_clusters_sorted_by_distance():
+    rng  = np.random.default_rng(7)
+    pts_near = _make_cluster_pts(cx=0.0, cz=1.5, n=40, rng=rng)
+    pts_far  = _make_cluster_pts(cx=0.0, cz=4.0, n=40, rng=rng)
+    pts = np.vstack([pts_far, pts_near])   # far first
+    h   = _make_heights(pts, rng=rng)
+    clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
+    assert len(clusters) == 2
+    assert clusters[0].distance_m < clusters[1].distance_m
+
+
+def test_empty_input():
+    clusters = cluster_obstacles(
+        np.zeros((0, 3), np.float64),
+        np.zeros(0, np.float64),
+    )
+    assert clusters == []
+
+
+def test_min_pts_filters_small_cluster():
+    rng      = np.random.default_rng(9)
+    pts_big  = _make_cluster_pts(cx=0.0, cz=2.0, n=50, rng=rng)
+    pts_tiny = _make_cluster_pts(cx=2.0, cz=2.0, n=2, rng=rng)
+    pts      = np.vstack([pts_big, pts_tiny])
+    h        = _make_heights(pts, rng=rng)
+    clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5)
+    # Only the big cluster should pass
+    assert len(clusters) == 1
+    assert clusters[0].n_pts >= 40
+
+
+def test_centroid_near_true_center():
+    rng      = np.random.default_rng(5)
+    true_cx, true_cz = 0.0, 2.5
+    pts      = _make_cluster_pts(cx=true_cx, cz=true_cz, n=100, spread=0.1, rng=rng)
+    h        = _make_heights(pts, rng=rng)
+    clusters = cluster_obstacles(pts, h, cell_m=0.20, min_pts=5)
+    assert len(clusters) >= 1
+    c = clusters[0]
+    assert abs(c.centroid[0] - true_cx) < 0.3
+    assert abs(c.centroid[2] - true_cz) < 0.3
+
+
+def test_out_of_range_points_ignored():
+    rng     = np.random.default_rng(3)
+    pts_ok  = _make_cluster_pts(cx=0.0, cz=2.0, n=30, rng=rng)
+    # Points beyond max_obstacle_dist_m=5.0
+    pts_far = _make_cluster_pts(cx=0.0, cz=7.0, n=30, rng=rng)
+    pts     = np.vstack([pts_ok, pts_far])
+    h       = _make_heights(pts, rng=rng)
+    clusters = cluster_obstacles(pts, h, cell_m=0.30, min_pts=5, z_range=(0.15, 5.0))
+    # Only the near cluster should survive
+    assert all(c.distance_m <= 5.0 for c in clusters)
+
+
+# ── ObstacleCluster accessors ─────────────────────────────────────────────────
+
+def test_cluster_distance_and_lateral():
+    centroid = np.array([1.5, -0.3, 3.0])
+    c = ObstacleCluster(centroid=centroid, radius_m=0.2, height_m=0.3, n_pts=30)
+    assert abs(c.distance_m - 3.0) < 1e-9
+    assert abs(c.lateral_m  - 1.5) < 1e-9

jetson/ros2_ws/src/saltybot_uwb_espnow_relay/config/espnow_relay_params.yaml

@@ -0,0 +1,22 @@
+espnow_relay:
+  ros__parameters:
+    # USB serial port for the ESP32 ESP-NOW receiver.
+    # Add a udev rule to create a stable symlink:
+    #   SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001",
+    #   ATTRS{serial}=="<RECEIVER_SERIAL>", SYMLINK+="espnow-relay"
+    serial_port: /dev/espnow-relay
+    baudrate: 115200
+
+    # Serial read timeout in seconds (tune for latency vs CPU usage)
+    read_timeout_s: 0.1
+
+    # Range validity window in metres
+    min_range_m: 0.1
+    max_range_m: 120.0
+
+    # How long (seconds) to hold /saltybot/estop True after last ESTOP packet.
+    # Tag sends 3× clear packets on button release; latch handles packet loss.
+    estop_latch_s: 2.0
+
+    # Whether to publish MSG_HEARTBEAT frames on /uwb/espnow/heartbeat
+    publish_heartbeat: true

jetson/ros2_ws/src/saltybot_uwb_espnow_relay/launch/espnow_relay.launch.py

@@ -0,0 +1,43 @@
+"""
+espnow_relay.launch.py — Launch ESP-NOW to ROS2 serial relay.
+
+Usage:
+    ros2 launch saltybot_uwb_espnow_relay espnow_relay.launch.py
+    ros2 launch saltybot_uwb_espnow_relay espnow_relay.launch.py \\
+        serial_port:=/dev/ttyUSB3
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg_dir = get_package_share_directory("saltybot_uwb_espnow_relay")
+    params_file = os.path.join(pkg_dir, "config", "espnow_relay_params.yaml")
+
+    serial_port_arg = DeclareLaunchArgument(
+        "serial_port",
+        default_value="/dev/espnow-relay",
+        description="USB serial port for ESP-NOW receiver ESP32",
+    )
+
+    relay_node = Node(
+        package="saltybot_uwb_espnow_relay",
+        executable="espnow_relay",
+        name="espnow_relay",
+        parameters=[
+            params_file,
+            {"serial_port": LaunchConfiguration("serial_port")},
+        ],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    return LaunchDescription([
+        serial_port_arg,
+        relay_node,
+    ])

jetson/ros2_ws/src/saltybot_uwb_espnow_relay/package.xml

@@ -0,0 +1,28 @@
+<?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_espnow_relay</name>
+  <version>0.1.0</version>
+  <description>
+    ESP-NOW to ROS2 serial relay for SaltyBot UWB tags (Issue #618).
+    Reads 20-byte EspNowPacket frames from an ESP32 receiver over USB serial.
+    Publishes RANGE as UwbRange, ESTOP as std_msgs/Bool, HEARTBEAT as EspNowHeartbeat.
+  </description>
+  <maintainer email="sl-uwb@saltylab.local">sl-uwb</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>saltybot_uwb_msgs</depend>
+
+  <exec_depend>python3-serial</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_espnow_relay/saltybot_uwb_espnow_relay/packet.py

@@ -0,0 +1,174 @@
+"""
+packet.py — ESP-NOW packet parser for SaltyBot UWB tag relay (Issue #618)
+
+The tag firmware broadcasts 20-byte ESP-NOW packets over the air.
+An ESP32 receiver (acting as a WiFi sniffer or paired ESP-NOW node)
+forwards each received packet verbatim over USB serial as a SLIP-framed
+or length-prefixed binary blob.
+
+Serial framing (receiver firmware convention)
+─────────────────────────────────────────────
+  STX  0x02        (1 byte, start of frame)
+  LEN  0x14 (=20)  (1 byte, payload length — always 20 for this protocol)
+  DATA [20 bytes]  (raw EspNowPacket)
+  CRC  0x00        (1 byte, XOR of DATA bytes, simple integrity check)
+
+  → Total wire bytes per frame: 23
+
+Packet layout (EspNowPacket, 20 bytes, little-endian)
+──────────────────────────────────────────────────────
+  [0-1]  magic       0x5B 0x01
+  [2]    tag_id      uint8
+  [3]    msg_type    0x10=RANGE, 0x20=ESTOP, 0x30=HEARTBEAT
+  [4]    anchor_id   uint8
+  [5-8]  range_mm    int32 LE
+  [9-12] rssi_dbm    float32 LE
+  [13-16] timestamp_ms uint32 LE  (ESP32 millis())
+  [17]   battery_pct uint8  (0-100, or 0xFF = unknown)
+  [18]   flags       uint8  bit0 = estop_active
+  [19]   seq_num     uint8  (rolling)
+"""
+
+from __future__ import annotations
+
+import struct
+from dataclasses import dataclass
+from typing import Optional
+
+MAGIC_0 = 0x5B
+MAGIC_1 = 0x01
+
+MSG_RANGE     = 0x10
+MSG_ESTOP     = 0x20
+MSG_HEARTBEAT = 0x30
+
+PACKET_LEN = 20
+
+_FRAME_STX = 0x02
+_FRAME_LEN_BYTE = PACKET_LEN   # always 20
+
+FLAG_ESTOP_ACTIVE = 0x01
+
+_FMT = "<2BBI f I BBB x"  # x = 1-byte pad (matches struct alignment)
+# Breakdown:
+#   2B  = magic[2]
+#   B   = tag_id
+#   B   = msg_type
+#   B   = anchor_id  ← wait, need to account for actual layout carefully
+
+# Use explicit offset-based unpacking to be safe:
+#   [0]   magic[0]      B
+#   [1]   magic[1]      B
+#   [2]   tag_id        B
+#   [3]   msg_type      B
+#   [4]   anchor_id     B
+#   [5-8] range_mm      i  (signed 32-bit LE)
+#   [9-12]rssi_dbm      f  (float32 LE)
+#   [13-16]timestamp_ms I  (uint32 LE)
+#   [17]  battery_pct   B
+#   [18]  flags         B
+#   [19]  seq_num       B
+
+_STRUCT = struct.Struct("<BBBBBifIBBB")  # 1+1+1+1+1+4+4+4+1+1+1 = 20 ✓
+assert _STRUCT.size == 20, f"struct size mismatch: {_STRUCT.size}"
+
+
+@dataclass
+class EspNowPacket:
+    tag_id:       int
+    msg_type:     int        # MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT
+    anchor_id:    int
+    range_mm:     int        # signed, valid only for MSG_RANGE
+    rssi_dbm:     float      # valid for MSG_RANGE
+    timestamp_ms: int        # ESP32 millis() at send time
+    battery_pct:  int        # 0-100 or 255 (unknown)
+    flags:        int        # bit0 = estop_active
+    seq_num:      int
+
+    @property
+    def estop_active(self) -> bool:
+        return bool(self.flags & FLAG_ESTOP_ACTIVE)
+
+    @staticmethod
+    def from_bytes(data: bytes) -> "EspNowPacket":
+        if len(data) != PACKET_LEN:
+            raise ValueError(f"Expected {PACKET_LEN} bytes, got {len(data)}")
+        m0, m1, tag_id, msg_type, anchor_id, range_mm, rssi_dbm, \
+            timestamp_ms, battery_pct, flags, seq_num = _STRUCT.unpack(data)
+        if m0 != MAGIC_0 or m1 != MAGIC_1:
+            raise ValueError(
+                f"Bad magic: 0x{m0:02X} 0x{m1:02X} (expected 0x5B 0x01)"
+            )
+        return EspNowPacket(
+            tag_id=tag_id,
+            msg_type=msg_type,
+            anchor_id=anchor_id,
+            range_mm=range_mm,
+            rssi_dbm=rssi_dbm,
+            timestamp_ms=timestamp_ms,
+            battery_pct=battery_pct,
+            flags=flags,
+            seq_num=seq_num,
+        )
+
+
+class FrameReader:
+    """
+    Stateful framing decoder for the serial stream from the ESP32 receiver.
+
+    Framing: STX(0x02) + LEN(0x14) + DATA(20 bytes) + XOR-CRC(1 byte)
+    Yields EspNowPacket objects for each valid, CRC-passing frame.
+    Invalid bytes before STX are silently discarded (sync recovery).
+    """
+
+    _STATE_HUNT  = 0
+    _STATE_LEN   = 1
+    _STATE_DATA  = 2
+    _STATE_CRC   = 3
+
+    def __init__(self) -> None:
+        self._state = self._STATE_HUNT
+        self._buf: bytearray = bytearray()
+
+    def feed(self, data: bytes) -> list[EspNowPacket]:
+        """Feed raw bytes; return list of parsed packets (may be empty)."""
+        packets: list[EspNowPacket] = []
+        for byte in data:
+            pkt = self._step(byte)
+            if pkt is not None:
+                packets.append(pkt)
+        return packets
+
+    def _step(self, byte: int) -> Optional[EspNowPacket]:
+        if self._state == self._STATE_HUNT:
+            if byte == _FRAME_STX:
+                self._state = self._STATE_LEN
+            return None
+
+        if self._state == self._STATE_LEN:
+            if byte == _FRAME_LEN_BYTE:
+                self._buf = bytearray()
+                self._state = self._STATE_DATA
+            else:
+                self._state = self._STATE_HUNT   # unexpected length — re-hunt
+            return None
+
+        if self._state == self._STATE_DATA:
+            self._buf.append(byte)
+            if len(self._buf) == PACKET_LEN:
+                self._state = self._STATE_CRC
+            return None
+
+        if self._state == self._STATE_CRC:
+            self._state = self._STATE_HUNT
+            crc = 0
+            for b in self._buf:
+                crc ^= b
+            if crc != byte:
+                return None   # CRC fail — drop frame
+            try:
+                return EspNowPacket.from_bytes(bytes(self._buf))
+            except ValueError:
+                return None
+
+        return None  # unreachable

jetson/ros2_ws/src/saltybot_uwb_espnow_relay/saltybot_uwb_espnow_relay/relay_node.py

@@ -0,0 +1,244 @@
+"""
+relay_node.py — ESP-NOW → ROS2 serial relay (Issue #618)
+
+Hardware
+────────
+  An ESP32 (e.g. a spare UWB-Pro board in WiFi-only mode) acts as an
+  ESP-NOW broadcast receiver.  It receives 20-byte EspNowPacket frames
+  from nearby UWB tags and forwards each one verbatim over USB serial
+  with a lightweight framing wrapper:
+
+      STX(0x02) LEN(0x14) DATA[20] XOR-CRC(1)   = 23 bytes per frame
+
+Subscriptions
+─────────────
+  (none — passive relay)
+
+Publishes
+─────────
+  /uwb/espnow/ranges     saltybot_uwb_msgs/UwbRange       — MSG_RANGE frames
+  /uwb/espnow/estop      std_msgs/Bool                    — MSG_ESTOP frames
+                           True  = e-stop active
+                           False = e-stop cleared
+  /uwb/espnow/heartbeat  saltybot_uwb_espnow_relay/msg/   — MSG_HEARTBEAT
+                         EspNowHeartbeat (see below)
+  /saltybot/estop        std_msgs/Bool                    — mirrors /uwb/espnow/estop
+                           (shared e-stop bus, latched True until explicit clear)
+
+Parameters
+──────────
+  serial_port          /dev/espnow-relay  (udev symlink for receiver ESP32)
+  baudrate             115200
+  read_timeout_s       0.1               (serial read timeout)
+  min_range_m          0.1               (discard implausibly short ranges)
+  max_range_m          120.0             (DW3000 rated max)
+  estop_latch_s        2.0               (hold /saltybot/estop True for N s
+                                          after last ESTOP packet before clearing)
+  publish_heartbeat    true
+
+udev rule for receiver ESP32 (add to /etc/udev/rules.d/99-uwb-anchors.rules):
+  SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001",
+  ATTRS{serial}=="<RECEIVER_SERIAL>", SYMLINK+="espnow-relay"
+"""
+
+from __future__ import annotations
+
+import threading
+import time
+
+import serial
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from std_msgs.msg import Bool, Header
+from saltybot_uwb_msgs.msg import UwbRange, EspNowHeartbeat
+from saltybot_uwb_espnow_relay.packet import (
+    FrameReader, MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT,
+)
+
+
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+
+class EspNowRelayNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__("espnow_relay")
+
+        # ── Parameters ────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",       "/dev/espnow-relay")
+        self.declare_parameter("baudrate",          115200)
+        self.declare_parameter("read_timeout_s",    0.1)
+        self.declare_parameter("min_range_m",       0.1)
+        self.declare_parameter("max_range_m",       120.0)
+        self.declare_parameter("estop_latch_s",     2.0)
+        self.declare_parameter("publish_heartbeat", True)
+
+        self._port_path    = self.get_parameter("serial_port").value
+        self._baudrate     = self.get_parameter("baudrate").value
+        self._read_timeout = self.get_parameter("read_timeout_s").value
+        self._min_range    = self.get_parameter("min_range_m").value
+        self._max_range    = self.get_parameter("max_range_m").value
+        self._estop_latch  = self.get_parameter("estop_latch_s").value
+        self._pub_hb       = self.get_parameter("publish_heartbeat").value
+
+        # ── Publishers ────────────────────────────────────────────────────
+        self._range_pub = self.create_publisher(
+            UwbRange, "/uwb/espnow/ranges", _SENSOR_QOS
+        )
+        self._estop_espnow_pub = self.create_publisher(
+            Bool, "/uwb/espnow/estop", _SENSOR_QOS
+        )
+        self._estop_shared_pub = self.create_publisher(
+            Bool, "/saltybot/estop", _SENSOR_QOS
+        )
+        if self._pub_hb:
+            self._hb_pub = self.create_publisher(
+                EspNowHeartbeat, "/uwb/espnow/heartbeat", _SENSOR_QOS
+            )
+        else:
+            self._hb_pub = None
+
+        # ── E-stop latch ──────────────────────────────────────────────────
+        self._estop_last_t: float = 0.0
+        self._estop_active: bool = False
+        self._estop_lock = threading.Lock()
+        self.create_timer(0.1, self._estop_latch_check)
+
+        # ── Serial reader thread ──────────────────────────────────────────
+        self._reader   = FrameReader()
+        self._running  = True
+        self._ser: serial.Serial | None = None
+        self._serial_thread = threading.Thread(
+            target=self._serial_loop, daemon=True, name="espnow-serial"
+        )
+        self._serial_thread.start()
+
+        self.get_logger().info(
+            f"ESP-NOW relay ready — port={self._port_path} "
+            f"baud={self._baudrate} "
+            f"range=[{self._min_range:.1f}, {self._max_range:.1f}] m"
+        )
+
+    def destroy_node(self) -> None:
+        self._running = False
+        if self._ser and self._ser.is_open:
+            self._ser.close()
+        super().destroy_node()
+
+    # ── Serial read loop ───────────────────────────────────────────────────
+
+    def _serial_loop(self) -> None:
+        while self._running:
+            try:
+                self._ser = serial.Serial(
+                    self._port_path,
+                    baudrate=self._baudrate,
+                    timeout=self._read_timeout,
+                )
+                self.get_logger().info(f"Serial opened: {self._port_path}")
+                self._read_loop()
+            except serial.SerialException as exc:
+                if self._running:
+                    self.get_logger().warn(
+                        f"Serial error: {exc} — retrying in 2 s",
+                        throttle_duration_sec=10.0,
+                    )
+                    time.sleep(2.0)
+            except Exception as exc:
+                if self._running:
+                    self.get_logger().error(f"Unexpected serial error: {exc}")
+                    time.sleep(2.0)
+            finally:
+                if self._ser and self._ser.is_open:
+                    self._ser.close()
+
+    def _read_loop(self) -> None:
+        while self._running and self._ser and self._ser.is_open:
+            raw = self._ser.read(64)
+            if not raw:
+                continue
+            packets = self._reader.feed(raw)
+            for pkt in packets:
+                self._dispatch(pkt)
+
+    # ── Packet dispatch ────────────────────────────────────────────────────
+
+    def _dispatch(self, pkt) -> None:
+        now = self.get_clock().now().to_msg()
+        hdr = Header()
+        hdr.stamp = now
+        hdr.frame_id = "espnow"
+
+        if pkt.msg_type == MSG_RANGE:
+            range_m = pkt.range_mm / 1000.0
+            if not (self._min_range <= range_m <= self._max_range):
+                return
+            msg = UwbRange()
+            msg.header   = hdr
+            msg.anchor_id = pkt.anchor_id
+            msg.range_m  = float(range_m)
+            msg.raw_mm   = max(0, pkt.range_mm)
+            msg.rssi     = float(pkt.rssi_dbm)
+            msg.tag_id   = str(pkt.tag_id)
+            self._range_pub.publish(msg)
+
+        elif pkt.msg_type == MSG_ESTOP:
+            active = pkt.estop_active
+            with self._estop_lock:
+                if active:
+                    self._estop_last_t = time.monotonic()
+                    self._estop_active = True
+                else:
+                    # Explicit clear from tag (3× clear packets on release)
+                    self._estop_active = False
+            b = Bool()
+            b.data = active
+            self._estop_espnow_pub.publish(b)
+            b2 = Bool()
+            b2.data = self._estop_active
+            self._estop_shared_pub.publish(b2)
+
+        elif pkt.msg_type == MSG_HEARTBEAT and self._hb_pub:
+            msg = EspNowHeartbeat()
+            msg.header       = hdr
+            msg.tag_id       = pkt.tag_id
+            msg.battery_pct  = pkt.battery_pct
+            msg.seq_num      = pkt.seq_num
+            msg.timestamp_ms = pkt.timestamp_ms
+            self._hb_pub.publish(msg)
+
+    # ── E-stop latch timer ─────────────────────────────────────────────────
+
+    def _estop_latch_check(self) -> None:
+        """Clear /saltybot/estop if no ESTOP packet received within latch window."""
+        with self._estop_lock:
+            if self._estop_active:
+                age = time.monotonic() - self._estop_last_t
+                if age > self._estop_latch:
+                    self._estop_active = False
+                    b = Bool()
+                    b.data = False
+                    self._estop_shared_pub.publish(b)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = EspNowRelayNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_uwb_espnow_relay/setup.cfg

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

jetson/ros2_ws/src/saltybot_uwb_espnow_relay/setup.py

@@ -0,0 +1,29 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = "saltybot_uwb_espnow_relay"
+
+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="ESP-NOW to ROS2 serial relay for SaltyBot UWB tags (Issue #618)",
+    license="Apache-2.0",
+    entry_points={
+        "console_scripts": [
+            "espnow_relay = saltybot_uwb_espnow_relay.relay_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_uwb_espnow_relay/test/test_packet.py

@@ -0,0 +1,175 @@
+"""
+Unit tests for saltybot_uwb_espnow_relay.packet (Issue #618).
+No ROS2 or hardware required.
+"""
+
+import struct
+import sys
+import os
+
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+from saltybot_uwb_espnow_relay.packet import (
+    EspNowPacket, FrameReader,
+    MAGIC_0, MAGIC_1, MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT,
+    PACKET_LEN, _STRUCT,
+)
+
+
+# ── Helpers ────────────────────────────────────────────────────────────────
+
+def _make_raw(
+    tag_id=1, msg_type=MSG_RANGE, anchor_id=0,
+    range_mm=1500, rssi=-75.0, timestamp_ms=12345,
+    battery_pct=80, flags=0, seq_num=42
+) -> bytes:
+    """Build a valid 20-byte raw EspNowPacket."""
+    return _STRUCT.pack(
+        MAGIC_0, MAGIC_1,
+        tag_id, msg_type, anchor_id,
+        range_mm, rssi, timestamp_ms,
+        battery_pct, flags, seq_num,
+    )
+
+
+def _frame(raw: bytes) -> bytes:
+    """Wrap raw packet in STX+LEN+DATA+CRC framing."""
+    crc = 0
+    for b in raw:
+        crc ^= b
+    return bytes([0x02, len(raw)]) + raw + bytes([crc])
+
+
+# ── EspNowPacket.from_bytes ────────────────────────────────────────────────
+
+class TestFromBytes:
+    def test_range_packet_parsed(self):
+        raw = _make_raw(tag_id=1, msg_type=MSG_RANGE, anchor_id=0,
+                        range_mm=2345, rssi=-68.5)
+        pkt = EspNowPacket.from_bytes(raw)
+        assert pkt.tag_id == 1
+        assert pkt.msg_type == MSG_RANGE
+        assert pkt.anchor_id == 0
+        assert pkt.range_mm == 2345
+        assert abs(pkt.rssi_dbm - (-68.5)) < 0.01
+
+    def test_estop_active_flag(self):
+        raw = _make_raw(msg_type=MSG_ESTOP, flags=0x01)
+        pkt = EspNowPacket.from_bytes(raw)
+        assert pkt.msg_type == MSG_ESTOP
+        assert pkt.estop_active is True
+
+    def test_estop_cleared_flag(self):
+        raw = _make_raw(msg_type=MSG_ESTOP, flags=0x00)
+        pkt = EspNowPacket.from_bytes(raw)
+        assert pkt.estop_active is False
+
+    def test_heartbeat_parsed(self):
+        raw = _make_raw(msg_type=MSG_HEARTBEAT, battery_pct=55, seq_num=7)
+        pkt = EspNowPacket.from_bytes(raw)
+        assert pkt.msg_type == MSG_HEARTBEAT
+        assert pkt.battery_pct == 55
+        assert pkt.seq_num == 7
+
+    def test_bad_magic_raises(self):
+        raw = bytearray(_make_raw())
+        raw[0] = 0xAA
+        with pytest.raises(ValueError, match="magic"):
+            EspNowPacket.from_bytes(bytes(raw))
+
+    def test_wrong_length_raises(self):
+        with pytest.raises(ValueError):
+            EspNowPacket.from_bytes(b"\x00" * 10)
+
+    def test_negative_range_mm(self):
+        raw = _make_raw(range_mm=-50)
+        pkt = EspNowPacket.from_bytes(raw)
+        assert pkt.range_mm == -50
+
+    def test_battery_unknown(self):
+        raw = _make_raw(battery_pct=0xFF)
+        pkt = EspNowPacket.from_bytes(raw)
+        assert pkt.battery_pct == 255
+
+
+# ── FrameReader ────────────────────────────────────────────────────────────
+
+class TestFrameReader:
+    def _make_frame(self, **kwargs) -> bytes:
+        return _frame(_make_raw(**kwargs))
+
+    def test_single_frame_decoded(self):
+        r = FrameReader()
+        frame = self._make_frame(range_mm=3000, anchor_id=1)
+        pkts = r.feed(frame)
+        assert len(pkts) == 1
+        assert pkts[0].range_mm == 3000
+        assert pkts[0].anchor_id == 1
+
+    def test_two_consecutive_frames(self):
+        r = FrameReader()
+        f1 = self._make_frame(seq_num=1)
+        f2 = self._make_frame(seq_num=2)
+        pkts = r.feed(f1 + f2)
+        assert len(pkts) == 2
+        assert pkts[0].seq_num == 1
+        assert pkts[1].seq_num == 2
+
+    def test_garbage_before_stx_skipped(self):
+        r = FrameReader()
+        junk = bytes([0xDE, 0xAD, 0xBE, 0xEF, 0x99])
+        frame = self._make_frame()
+        pkts = r.feed(junk + frame)
+        assert len(pkts) == 1
+
+    def test_bad_crc_dropped(self):
+        r = FrameReader()
+        raw = _make_raw()
+        crc = 0
+        for b in raw:
+            crc ^= b
+        frame = bytes([0x02, 0x14]) + raw + bytes([crc ^ 0xFF])  # corrupt CRC
+        pkts = r.feed(frame)
+        assert len(pkts) == 0
+
+    def test_wrong_len_byte_dropped(self):
+        r = FrameReader()
+        raw = _make_raw()
+        frame = bytes([0x02, 0x10]) + raw  # wrong LEN
+        pkts = r.feed(frame)
+        assert len(pkts) == 0
+
+    def test_split_feed_reassembles(self):
+        """Feed the frame in two chunks — should still parse."""
+        r = FrameReader()
+        frame = self._make_frame(seq_num=99)
+        half = len(frame) // 2
+        pkts1 = r.feed(frame[:half])
+        pkts2 = r.feed(frame[half:])
+        assert len(pkts1) == 0
+        assert len(pkts2) == 1
+        assert pkts2[0].seq_num == 99
+
+    def test_byte_by_byte_feed(self):
+        """Feed one byte at a time."""
+        r = FrameReader()
+        frame = self._make_frame(tag_id=3)
+        all_pkts = []
+        for b in frame:
+            all_pkts.extend(r.feed(bytes([b])))
+        assert len(all_pkts) == 1
+        assert all_pkts[0].tag_id == 3
+
+    def test_recovery_after_truncated_frame(self):
+        """Truncated frame followed by valid frame — valid one should parse."""
+        r = FrameReader()
+        truncated = bytes([0x02, 0x14]) + b"\x00" * 10   # no CRC
+        good = self._make_frame(seq_num=77)
+        pkts = r.feed(truncated + good)
+        # Might or might not get 77 depending on CRC collision, but must not crash
+        assert isinstance(pkts, list)
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_uwb_msgs/CMakeLists.txt

@@ -9,6 +9,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/UwbRange.msg"
   "msg/UwbRangeArray.msg"
   "msg/UwbBearing.msg"
+  "msg/EspNowHeartbeat.msg"
   DEPENDENCIES std_msgs
 )
 

jetson/ros2_ws/src/saltybot_uwb_msgs/msg/EspNowHeartbeat.msg

@@ -0,0 +1,10 @@
+# EspNowHeartbeat.msg — heartbeat status from ESP-NOW UWB tag (Issue #618)
+#
+# Published by the ESP-NOW relay node on each MSG_HEARTBEAT (0x30) frame.
+
+std_msgs/Header header
+
+uint8   tag_id        # tag identifier
+uint8   battery_pct  # 0-100, or 255 = unknown
+uint8   seq_num      # rolling sequence number (detect loss)
+uint32  timestamp_ms # ESP32 millis() at time of transmission

phone/waypoint_logger.py

@@ -0,0 +1,705 @@
+#!/usr/bin/env python3
+"""
+waypoint_logger.py — GPS waypoint logger and route planner for SaltyBot (Issue #617)
+
+Interactive CLI for recording, managing, and publishing GPS waypoints from
+an Android/Termux phone.  Uses termux-location for GPS fixes (same provider
+as sensor_dashboard.py) and paho-mqtt to publish routes to the broker.
+
+Features
+────────
+  • Record GPS waypoint at current position (with name and optional tags)
+  • List all saved waypoints with distance/bearing from previous
+  • Delete a waypoint by index
+  • Clear all waypoints in the active route
+  • Publish the route as a Nav2-compatible waypoint list to MQTT
+  • Subscribe to saltybot/phone/gps for live GPS if sensor_dashboard.py
+    is already running (avoids double GPS call)
+  • Load/save route to JSON file (persistent between sessions)
+
+MQTT topics
+───────────
+  Publish:   saltybot/phone/route
+  Subscribe: saltybot/phone/gps  (live GPS from sensor_dashboard.py)
+
+Route JSON published to MQTT
+─────────────────────────────
+  {
+    "route_name": "patrol_loop",
+    "ts": 1710000000.0,
+    "waypoints": [
+      {
+        "index": 0,
+        "name": "start",
+        "tags": ["patrol", "home"],
+        "lat": 45.123, "lon": -73.456, "alt_m": 12.3,
+        "accuracy_m": 2.1,
+        "recorded_at": 1710000000.0
+      }, ...
+    ]
+  }
+
+Nav2 PoseStamped list format (also included under "nav2_poses" key):
+  Each entry has frame_id, position {x,y,z}, orientation {x,y,z,w}
+  using a flat-earth approximation relative to the first waypoint.
+
+Usage
+─────
+  python3 phone/waypoint_logger.py [OPTIONS]
+
+  --broker   HOST    MQTT broker IP        (default: 192.168.1.100)
+  --port     PORT    MQTT port             (default: 1883)
+  --file     PATH    Route JSON file       (default: ~/saltybot_route.json)
+  --route    NAME    Route name            (default: route)
+  --provider PROV    GPS provider: gps|network|passive (default: gps)
+  --live-gps         Subscribe to sensor_dashboard GPS instead of polling
+  --no-mqtt          Disable MQTT (log-only mode)
+  --debug            Verbose logging
+
+Dependencies (Termux)
+──────────────────────
+  pkg install termux-api python
+  pip install paho-mqtt
+"""
+
+import argparse
+import json
+import logging
+import math
+import os
+import subprocess
+import sys
+import threading
+import time
+from dataclasses import dataclass, field, asdict
+from datetime import datetime, timezone
+from typing import Optional
+
+try:
+    import paho.mqtt.client as mqtt
+    MQTT_AVAILABLE = True
+except ImportError:
+    MQTT_AVAILABLE = False
+
+# ── Constants ─────────────────────────────────────────────────────────────────
+
+TOPIC_ROUTE    = "saltybot/phone/route"
+TOPIC_GPS_LIVE = "saltybot/phone/gps"
+
+EARTH_RADIUS_M = 6_371_000.0   # mean Earth radius in metres
+
+DEFAULT_FILE   = os.path.expanduser("~/saltybot_route.json")
+
+# ── Data model ────────────────────────────────────────────────────────────────
+
+@dataclass
+class Waypoint:
+    index:       int
+    name:        str
+    lat:         float
+    lon:         float
+    alt_m:       float        = 0.0
+    accuracy_m:  float        = -1.0
+    tags:        list[str]    = field(default_factory=list)
+    recorded_at: float        = field(default_factory=time.time)
+
+    def to_dict(self) -> dict:
+        return asdict(self)
+
+    @staticmethod
+    def from_dict(d: dict) -> "Waypoint":
+        return Waypoint(
+            index       = int(d.get("index", 0)),
+            name        = str(d.get("name", "")),
+            lat         = float(d["lat"]),
+            lon         = float(d["lon"]),
+            alt_m       = float(d.get("alt_m", 0.0)),
+            accuracy_m  = float(d.get("accuracy_m", -1.0)),
+            tags        = list(d.get("tags", [])),
+            recorded_at = float(d.get("recorded_at", 0.0)),
+        )
+
+
+# ── Geo maths ─────────────────────────────────────────────────────────────────
+
+def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+    """Great-circle distance between two WGS84 points (metres)."""
+    φ1, φ2 = math.radians(lat1), math.radians(lat2)
+    Δφ = math.radians(lat2 - lat1)
+    Δλ = math.radians(lon2 - lon1)
+    a = math.sin(Δφ / 2) ** 2 + math.cos(φ1) * math.cos(φ2) * math.sin(Δλ / 2) ** 2
+    return 2 * EARTH_RADIUS_M * math.asin(math.sqrt(a))
+
+
+def bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+    """Initial bearing from point-1 to point-2 (degrees, 0=N, 90=E)."""
+    φ1, φ2 = math.radians(lat1), math.radians(lat2)
+    Δλ = math.radians(lon2 - lon1)
+    x = math.sin(Δλ) * math.cos(φ2)
+    y = math.cos(φ1) * math.sin(φ2) - math.sin(φ1) * math.cos(φ2) * math.cos(Δλ)
+    return (math.degrees(math.atan2(x, y)) + 360) % 360
+
+
+def _compass(deg: float) -> str:
+    """Convert bearing degrees to 8-point compass label."""
+    dirs = ["N", "NE", "E", "SE", "S", "SW", "W", "NW"]
+    return dirs[round(deg / 45) % 8]
+
+
+def flat_earth_xy(origin_lat: float, origin_lon: float,
+                  lat: float, lon: float) -> tuple[float, float]:
+    """
+    Simple flat-earth approximation — metres east/north from origin.
+    Accurate within ~1% for distances < 100 km.
+    """
+    lat_rad = math.radians((origin_lat + lat) / 2.0)
+    x = math.radians(lon - origin_lon) * EARTH_RADIUS_M * math.cos(lat_rad)
+    y = math.radians(lat - origin_lat) * EARTH_RADIUS_M
+    return x, y
+
+
+# ── GPS acquisition ───────────────────────────────────────────────────────────
+
+def get_gps_fix(provider: str = "gps", timeout: float = 12.0) -> Optional[dict]:
+    """
+    Acquire a GPS fix via termux-location.
+    Falls back to 'network' provider if 'gps' times out.
+    Returns a dict with lat, lon, alt_m, accuracy_m or None on failure.
+    """
+    def _try(prov: str, t: float) -> Optional[dict]:
+        try:
+            r = subprocess.run(
+                ["termux-location", "-p", prov, "-r", "once"],
+                capture_output=True, text=True, timeout=t,
+            )
+            if r.returncode != 0 or not r.stdout.strip():
+                return None
+            raw = json.loads(r.stdout)
+            lat = float(raw.get("latitude",  0.0))
+            lon = float(raw.get("longitude", 0.0))
+            if lat == 0.0 and lon == 0.0:
+                return None
+            return {
+                "lat":        lat,
+                "lon":        lon,
+                "alt_m":      float(raw.get("altitude",  0.0)),
+                "accuracy_m": float(raw.get("accuracy",  -1.0)),
+                "provider":   prov,
+            }
+        except (subprocess.TimeoutExpired, json.JSONDecodeError,
+                FileNotFoundError, KeyError):
+            return None
+
+    fix = _try(provider, timeout)
+    if fix is None and provider == "gps":
+        logging.debug("GPS timeout — retrying with network provider")
+        fix = _try("network", 6.0)
+    return fix
+
+
+# ── Live GPS subscriber (optional, uses sensor_dashboard.py stream) ──────────
+
+class LiveGPS:
+    """
+    Subscribe to saltybot/phone/gps on MQTT and cache the latest fix.
+    Used when sensor_dashboard.py is already running to avoid double GPS calls.
+    """
+
+    def __init__(self, broker: str, port: int):
+        self._lock   = threading.Lock()
+        self._latest: Optional[dict] = None
+
+        if not MQTT_AVAILABLE:
+            return
+
+        self._client = mqtt.Client(client_id="wp-live-gps", clean_session=True)
+        self._client.on_connect = lambda c, u, f, rc: c.subscribe(TOPIC_GPS_LIVE)
+        self._client.on_message = self._on_msg
+        try:
+            self._client.connect(broker, port, keepalive=30)
+            self._client.loop_start()
+        except Exception as e:
+            logging.warning("LiveGPS connect failed: %s", e)
+
+    def _on_msg(self, client, userdata, message) -> None:
+        try:
+            data = json.loads(message.payload)
+            with self._lock:
+                self._latest = data
+        except json.JSONDecodeError:
+            pass
+
+    def get(self, max_age_s: float = 3.0) -> Optional[dict]:
+        with self._lock:
+            d = self._latest
+        if d is None:
+            return None
+        if time.time() - d.get("ts", 0.0) > max_age_s:
+            return None
+        return {
+            "lat":        d.get("lat",       0.0),
+            "lon":        d.get("lon",       0.0),
+            "alt_m":      d.get("alt_m",     0.0),
+            "accuracy_m": d.get("accuracy_m", -1.0),
+            "provider":   d.get("provider",  "live"),
+        }
+
+    def stop(self) -> None:
+        if MQTT_AVAILABLE and hasattr(self, "_client"):
+            self._client.loop_stop()
+            self._client.disconnect()
+
+
+# ── Route ─────────────────────────────────────────────────────────────────────
+
+class Route:
+    """Ordered list of waypoints with persistence and Nav2 export."""
+
+    def __init__(self, name: str, path: str):
+        self.name       = name
+        self._path      = path
+        self._waypoints: list[Waypoint] = []
+        self._load()
+
+    # ── Persistence ───────────────────────────────────────────────────────────
+
+    def _load(self) -> None:
+        if not os.path.exists(self._path):
+            return
+        try:
+            with open(self._path, "r") as f:
+                data = json.load(f)
+            self.name       = data.get("route_name", self.name)
+            self._waypoints = [Waypoint.from_dict(w) for w in data.get("waypoints", [])]
+            # Re-index to keep indices contiguous after deletes
+            for i, wp in enumerate(self._waypoints):
+                wp.index = i
+            logging.info("Loaded %d waypoints from %s", len(self._waypoints), self._path)
+        except (json.JSONDecodeError, KeyError, ValueError) as e:
+            logging.warning("Could not load route file: %s", e)
+
+    def save(self) -> None:
+        with open(self._path, "w") as f:
+            json.dump(self.to_dict(), f, indent=2)
+        logging.debug("Saved route to %s", self._path)
+
+    # ── Waypoint operations ───────────────────────────────────────────────────
+
+    def add(self, name: str, lat: float, lon: float, alt_m: float,
+            accuracy_m: float, tags: list[str]) -> Waypoint:
+        wp = Waypoint(
+            index      = len(self._waypoints),
+            name       = name,
+            lat        = lat,
+            lon        = lon,
+            alt_m      = alt_m,
+            accuracy_m = accuracy_m,
+            tags       = tags,
+        )
+        self._waypoints.append(wp)
+        self.save()
+        return wp
+
+    def delete(self, index: int) -> bool:
+        if not (0 <= index < len(self._waypoints)):
+            return False
+        self._waypoints.pop(index)
+        for i, wp in enumerate(self._waypoints):
+            wp.index = i
+        self.save()
+        return True
+
+    def clear(self) -> int:
+        n = len(self._waypoints)
+        self._waypoints.clear()
+        self.save()
+        return n
+
+    def __len__(self) -> int:
+        return len(self._waypoints)
+
+    def __iter__(self):
+        return iter(self._waypoints)
+
+    def get(self, index: int) -> Optional[Waypoint]:
+        if 0 <= index < len(self._waypoints):
+            return self._waypoints[index]
+        return None
+
+    # ── Serialisation ─────────────────────────────────────────────────────────
+
+    def to_dict(self) -> dict:
+        return {
+            "route_name": self.name,
+            "ts":         time.time(),
+            "waypoints":  [wp.to_dict() for wp in self._waypoints],
+            "nav2_poses": self._nav2_poses(),
+        }
+
+    def _nav2_poses(self) -> list[dict]:
+        """
+        Flat-earth Nav2 PoseStamped list (x east, y north) relative to
+        the first waypoint.  Yaw faces the next waypoint; last faces prev.
+        """
+        if not self._waypoints:
+            return []
+
+        origin = self._waypoints[0]
+        poses  = []
+
+        for i, wp in enumerate(self._waypoints):
+            x, y = flat_earth_xy(origin.lat, origin.lon, wp.lat, wp.lon)
+
+            # Compute yaw: face toward next waypoint (or prev for last)
+            if i + 1 < len(self._waypoints):
+                nxt = self._waypoints[i + 1]
+                nx, ny = flat_earth_xy(origin.lat, origin.lon, nxt.lat, nxt.lon)
+                yaw = math.atan2(ny - y, nx - x)
+            elif i > 0:
+                prv = self._waypoints[i - 1]
+                px, py = flat_earth_xy(origin.lat, origin.lon, prv.lat, prv.lon)
+                yaw = math.atan2(y - py, x - px)
+            else:
+                yaw = 0.0
+
+            poses.append({
+                "frame_id": "map",
+                "position":    {"x": round(x, 3), "y": round(y, 3), "z": round(wp.alt_m, 3)},
+                "orientation": {
+                    "x": 0.0, "y": 0.0,
+                    "z": round(math.sin(yaw / 2), 6),
+                    "w": round(math.cos(yaw / 2), 6),
+                },
+                "waypoint_name": wp.name,
+            })
+
+        return poses
+
+
+# ── MQTT publisher ────────────────────────────────────────────────────────────
+
+class MQTTClient:
+    """Simple synchronous paho wrapper for route publishing."""
+
+    def __init__(self, broker: str, port: int):
+        self._broker = broker
+        self._port   = port
+        self._client = None
+        self._connected = False
+
+        if not MQTT_AVAILABLE:
+            return
+        self._client = mqtt.Client(client_id="saltybot-waypoint-logger", clean_session=True)
+        self._client.on_connect    = self._on_connect
+        self._client.on_disconnect = self._on_disconnect
+        self._client.reconnect_delay_set(min_delay=3, max_delay=30)
+        try:
+            self._client.connect_async(broker, port, keepalive=60)
+            self._client.loop_start()
+        except Exception as e:
+            logging.warning("MQTT connect failed: %s", e)
+
+    def _on_connect(self, c, u, f, rc) -> None:
+        self._connected = (rc == 0)
+        if rc == 0:
+            logging.debug("MQTT connected")
+
+    def _on_disconnect(self, c, u, rc) -> None:
+        self._connected = False
+
+    def publish(self, topic: str, payload: dict, qos: int = 1) -> bool:
+        if self._client is None:
+            return False
+        # Wait briefly for connection
+        deadline = time.monotonic() + 5.0
+        while not self._connected and time.monotonic() < deadline:
+            time.sleep(0.1)
+        if not self._connected:
+            return False
+        info = self._client.publish(
+            topic, json.dumps(payload, separators=(",", ":")), qos=qos, retain=True
+        )
+        return info.rc == 0
+
+    def stop(self) -> None:
+        if self._client:
+            self._client.loop_stop()
+            self._client.disconnect()
+
+
+# ── CLI helpers ───────────────────────────────────────────────────────────────
+
+def _fmt_ts(epoch: float) -> str:
+    return datetime.fromtimestamp(epoch, tz=timezone.utc).strftime("%Y-%m-%d %H:%M:%S UTC")
+
+
+def _fmt_dist(metres: float) -> str:
+    if metres < 1000:
+        return f"{metres:.1f} m"
+    return f"{metres / 1000:.3f} km"
+
+
+def _print_waypoints(route: Route) -> None:
+    if not len(route):
+        print("  (no waypoints)")
+        return
+    print(f"\n  {'#':>3}  {'Name':<18}  {'Lat':>10}  {'Lon':>11}  {'Alt':>7}  {'Acc':>7}  {'Tags'}")
+    print("  " + "─" * 75)
+    prev = None
+    for wp in route:
+        dist_str = ""
+        brg_str  = ""
+        if prev is not None:
+            d = haversine_m(prev.lat, prev.lon, wp.lat, wp.lon)
+            b = bearing_deg(prev.lat, prev.lon, wp.lat, wp.lon)
+            dist_str = f"  ↑ {_fmt_dist(d)} {_compass(b)}"
+        tags = ",".join(wp.tags) if wp.tags else "—"
+        print(f"  {wp.index:>3}  {wp.name:<18}  {wp.lat:>10.6f}  {wp.lon:>11.6f}"
+              f"  {wp.alt_m:>6.1f}m  "
+              f"{wp.accuracy_m:>5.1f}m  {tags}{dist_str}")
+        prev = wp
+    print()
+
+
+def _acquire_fix(args, live_gps: Optional[LiveGPS]) -> Optional[dict]:
+    """Get a GPS fix — from live MQTT stream if available, else termux-location."""
+    if live_gps is not None:
+        fix = live_gps.get(max_age_s=3.0)
+        if fix is not None:
+            return fix
+        print("  (live GPS stale — falling back to termux-location)")
+    print("  Acquiring GPS fix...", end="", flush=True)
+    fix = get_gps_fix(provider=args.provider)
+    if fix:
+        print(f" {fix['lat']:.6f}, {fix['lon']:.6f}  ±{fix['accuracy_m']:.1f}m")
+    else:
+        print(" FAILED")
+    return fix
+
+
+# ── Interactive menu ──────────────────────────────────────────────────────────
+
+def _menu_record(route: Route, args, live_gps: Optional[LiveGPS]) -> None:
+    fix = _acquire_fix(args, live_gps)
+    if fix is None:
+        print("  Could not get GPS fix.")
+        return
+
+    default_name = f"wp{len(route)}"
+    name = input(f"  Waypoint name [{default_name}]: ").strip() or default_name
+    tags_raw = input("  Tags (comma-separated, or Enter to skip): ").strip()
+    tags = [t.strip() for t in tags_raw.split(",") if t.strip()] if tags_raw else []
+
+    wp = route.add(name, fix["lat"], fix["lon"], fix["alt_m"], fix["accuracy_m"], tags)
+    print(f"  ✓ Recorded #{wp.index}: {wp.name}  ({wp.lat:.6f}, {wp.lon:.6f})")
+
+
+def _menu_list(route: Route) -> None:
+    print(f"\n  Route: '{route.name}'  ({len(route)} waypoints)")
+    _print_waypoints(route)
+
+    if len(route) >= 2:
+        wps = list(route)
+        total = sum(
+            haversine_m(wps[i].lat, wps[i].lon, wps[i+1].lat, wps[i+1].lon)
+            for i in range(len(wps) - 1)
+        )
+        print(f"  Total route distance: {_fmt_dist(total)}")
+
+
+def _menu_delete(route: Route) -> None:
+    if not len(route):
+        print("  No waypoints to delete.")
+        return
+    _print_waypoints(route)
+    raw = input("  Enter index to delete (or Enter to cancel): ").strip()
+    if not raw:
+        return
+    try:
+        idx = int(raw)
+    except ValueError:
+        print("  Invalid index.")
+        return
+    wp = route.get(idx)
+    if wp is None:
+        print(f"  No waypoint at index {idx}.")
+        return
+    confirm = input(f"  Delete '{wp.name}'? [y/N]: ").strip().lower()
+    if confirm == "y":
+        route.delete(idx)
+        print(f"  ✓ Deleted '{wp.name}'.")
+    else:
+        print("  Cancelled.")
+
+
+def _menu_publish(route: Route, mqtt_client: Optional[MQTTClient]) -> None:
+    if not len(route):
+        print("  No waypoints to publish.")
+        return
+
+    payload = route.to_dict()
+    print(f"\n  Publishing {len(route)} waypoints to {TOPIC_ROUTE}...")
+
+    if mqtt_client is None:
+        print("  (MQTT disabled — printing payload)")
+        print(json.dumps(payload, indent=2))
+        return
+
+    ok = mqtt_client.publish(TOPIC_ROUTE, payload, qos=1)
+    if ok:
+        print(f"  ✓ Published to {TOPIC_ROUTE}  (retained=true)")
+    else:
+        print("  ✗ MQTT publish failed — check broker connection.")
+
+    # Also print Nav2 summary
+    poses = payload.get("nav2_poses", [])
+    if poses:
+        print(f"\n  Nav2 poses ({len(poses)}):")
+        for p in poses:
+            pos = p["position"]
+            print(f"    {p['waypoint_name']:<18}  x={pos['x']:>8.2f}m  y={pos['y']:>8.2f}m")
+
+
+def _menu_clear(route: Route) -> None:
+    if not len(route):
+        print("  Route is already empty.")
+        return
+    confirm = input(f"  Clear all {len(route)} waypoints? [y/N]: ").strip().lower()
+    if confirm == "y":
+        n = route.clear()
+        print(f"  ✓ Cleared {n} waypoints.")
+    else:
+        print("  Cancelled.")
+
+
+def _menu_rename(route: Route) -> None:
+    new_name = input(f"  New route name [{route.name}]: ").strip()
+    if new_name:
+        route.name = new_name
+        route.save()
+        print(f"  ✓ Route renamed to '{route.name}'.")
+
+
+def _menu_info(route: Route) -> None:
+    print(f"\n  Route file: {route._path}")
+    print(f"  Route name: {route.name}")
+    print(f"  Waypoints:  {len(route)}")
+    if len(route):
+        wps = list(route)
+        print(f"  First wp:   {wps[0].name}  ({_fmt_ts(wps[0].recorded_at)})")
+        print(f"  Last wp:    {wps[-1].name}  ({_fmt_ts(wps[-1].recorded_at)})")
+        if len(route) >= 2:
+            total = sum(
+                haversine_m(wps[i].lat, wps[i].lon, wps[i+1].lat, wps[i+1].lon)
+                for i in range(len(wps) - 1)
+            )
+            print(f"  Total dist: {_fmt_dist(total)}")
+    print()
+
+
+# ── Main loop ─────────────────────────────────────────────────────────────────
+
+MENU = """
+  ┌─ SaltyBot Waypoint Logger ──────────────────────────┐
+  │  r  Record waypoint at current GPS position         │
+  │  l  List all waypoints                              │
+  │  d  Delete a waypoint                               │
+  │  p  Publish route to MQTT                           │
+  │  c  Clear all waypoints                             │
+  │  n  Rename route                                    │
+  │  i  Route info                                      │
+  │  q  Quit                                            │
+  └─────────────────────────────────────────────────────┘
+"""
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="SaltyBot GPS waypoint logger (Issue #617)"
+    )
+    parser.add_argument("--broker",   default="192.168.1.100",
+                        help="MQTT broker IP (default: 192.168.1.100)")
+    parser.add_argument("--port",     type=int, default=1883,
+                        help="MQTT port (default: 1883)")
+    parser.add_argument("--file",     default=DEFAULT_FILE,
+                        help=f"Route JSON file (default: {DEFAULT_FILE})")
+    parser.add_argument("--route",    default="route",
+                        help="Route name (default: route)")
+    parser.add_argument("--provider", default="gps",
+                        choices=["gps", "network", "passive"],
+                        help="GPS provider (default: gps)")
+    parser.add_argument("--live-gps", action="store_true",
+                        help="Subscribe to saltybot/phone/gps for live GPS")
+    parser.add_argument("--no-mqtt",  action="store_true",
+                        help="Disable MQTT (log-only mode)")
+    parser.add_argument("--debug",    action="store_true",
+                        help="Verbose logging")
+    args = parser.parse_args()
+
+    logging.basicConfig(
+        level=logging.DEBUG if args.debug else logging.WARNING,
+        format="%(asctime)s [%(levelname)s] %(message)s",
+    )
+
+    # Load or create route
+    route = Route(args.route, args.file)
+
+    # MQTT client
+    mqtt_client: Optional[MQTTClient] = None
+    if not args.no_mqtt:
+        if MQTT_AVAILABLE:
+            mqtt_client = MQTTClient(args.broker, args.port)
+        else:
+            print("Warning: paho-mqtt not installed — MQTT disabled. Run: pip install paho-mqtt")
+
+    # Optional live GPS subscription
+    live_gps: Optional[LiveGPS] = None
+    if args.live_gps and MQTT_AVAILABLE:
+        live_gps = LiveGPS(args.broker, args.port)
+        print(f"  Subscribed to live GPS on {TOPIC_GPS_LIVE}")
+
+    print(MENU)
+    print(f"  Route: '{route.name}'  |  {len(route)} waypoints loaded  |  file: {args.file}")
+    print()
+
+    try:
+        while True:
+            try:
+                choice = input("  > ").strip().lower()
+            except EOFError:
+                break
+
+            if choice in ("q", "quit", "exit"):
+                break
+            elif choice in ("r", "record"):
+                _menu_record(route, args, live_gps)
+            elif choice in ("l", "list"):
+                _menu_list(route)
+            elif choice in ("d", "delete"):
+                _menu_delete(route)
+            elif choice in ("p", "publish"):
+                _menu_publish(route, mqtt_client)
+            elif choice in ("c", "clear"):
+                _menu_clear(route)
+            elif choice in ("n", "rename"):
+                _menu_rename(route)
+            elif choice in ("i", "info"):
+                _menu_info(route)
+            elif choice == "":
+                pass
+            else:
+                print("  Unknown command. Type r/l/d/p/c/n/i/q.")
+
+    except KeyboardInterrupt:
+        print("\n  Interrupted.")
+    finally:
+        if live_gps:
+            live_gps.stop()
+        if mqtt_client:
+            mqtt_client.stop()
+
+    print("  Bye.")
+
+
+if __name__ == "__main__":
+    main()

src/mpu6000.c

@@ -151,6 +151,7 @@ void mpu6000_read(IMUData *data) {
     data->pitch_rate = gyro_pitch_rate;
     data->roll       = s_roll;
     data->yaw        = s_yaw;
+    data->yaw_rate   = gyro_yaw_rate;  /* board_gz: raw bias-corrected gyro Z (Issue #616) */
     data->accel_x    = ax;
     data->accel_z    = az;
 }

src/steering_pid.c

@@ -0,0 +1,140 @@
+/*
+ * steering_pid.c — closed-loop yaw-rate PID for differential drive (Issue #616).
+ *
+ * Converts Jetson Twist.angular.z (encoded as steer * STEER_OMEGA_SCALE deg/s)
+ * into a differential wheel speed offset using IMU gyro Z as feedback.
+ *
+ * Anti-windup:   integral clamped to ±STEER_INTEGRAL_MAX before Ki multiply.
+ * Rate limiter:  output changes at most STEER_RAMP_RATE_PER_MS per ms so that
+ *                a step in steering demand cannot disturb the balance PID.
+ */
+
+#include "steering_pid.h"
+#include "jlink.h"
+
+/* ---- steering_pid_init() ---- */
+void steering_pid_init(steering_pid_t *s)
+{
+    s->target_omega_dps = 0.0f;
+    s->actual_omega_dps = 0.0f;
+    s->integral         = 0.0f;
+    s->prev_error       = 0.0f;
+    s->output           = 0;
+    s->enabled          = true;
+    /* Initialize so first send_tlm call fires immediately */
+    s->last_tlm_ms = (uint32_t)(-(uint32_t)(1000u / (STEER_TLM_HZ > 0u ? STEER_TLM_HZ : 1u)));
+}
+
+/* ---- steering_pid_reset() ---- */
+void steering_pid_reset(steering_pid_t *s)
+{
+    s->target_omega_dps = 0.0f;
+    s->integral         = 0.0f;
+    s->prev_error       = 0.0f;
+    s->output           = 0;
+}
+
+/* ---- steering_pid_set_target() ---- */
+void steering_pid_set_target(steering_pid_t *s, float omega_dps)
+{
+    if (!s->enabled) return;
+    s->target_omega_dps = omega_dps;
+}
+
+/* ---- steering_pid_update() ---- */
+int16_t steering_pid_update(steering_pid_t *s, float actual_omega_dps, float dt)
+{
+    if (!s->enabled || dt <= 0.0f) {
+        s->output = 0;
+        return 0;
+    }
+
+    s->actual_omega_dps = actual_omega_dps;
+
+    /* PID error */
+    float error = s->target_omega_dps - actual_omega_dps;
+
+    /* Proportional */
+    float p_term = STEER_KP * error;
+
+    /* Integral with anti-windup clamp */
+    s->integral += error * dt;
+    if (s->integral >  STEER_INTEGRAL_MAX) s->integral =  STEER_INTEGRAL_MAX;
+    if (s->integral < -STEER_INTEGRAL_MAX) s->integral = -STEER_INTEGRAL_MAX;
+    float i_term = STEER_KI * s->integral;
+
+    /* Derivative on error (avoids setpoint kick for smooth yaw changes) */
+    float d_term = 0.0f;
+    if (dt > 0.0f) {
+        d_term = STEER_KD * (error - s->prev_error) / dt;
+    }
+    s->prev_error = error;
+
+    /* Sum and clamp raw output */
+    float raw = p_term + i_term + d_term;
+    if (raw >  (float)STEER_OUTPUT_MAX) raw =  (float)STEER_OUTPUT_MAX;
+    if (raw < -(float)STEER_OUTPUT_MAX) raw = -(float)STEER_OUTPUT_MAX;
+
+    /* Rate limiter: bound change per step by STEER_RAMP_RATE_PER_MS * dt */
+    float max_step = (float)STEER_RAMP_RATE_PER_MS * (dt * 1000.0f);
+    float delta = raw - (float)s->output;
+    if (delta >  max_step) delta =  max_step;
+    if (delta < -max_step) delta = -max_step;
+
+    float limited = (float)s->output + delta;
+    /* Final clamp after rate limit */
+    if (limited >  (float)STEER_OUTPUT_MAX) limited =  (float)STEER_OUTPUT_MAX;
+    if (limited < -(float)STEER_OUTPUT_MAX) limited = -(float)STEER_OUTPUT_MAX;
+
+    s->output = (int16_t)limited;
+    return s->output;
+}
+
+/* ---- steering_pid_get_output() ---- */
+int16_t steering_pid_get_output(const steering_pid_t *s)
+{
+    return s->output;
+}
+
+/* ---- steering_pid_set_enabled() ---- */
+void steering_pid_set_enabled(steering_pid_t *s, bool en)
+{
+    if (!en && s->enabled) {
+        /* Disabling: zero out state */
+        s->target_omega_dps = 0.0f;
+        s->integral         = 0.0f;
+        s->prev_error       = 0.0f;
+        s->output           = 0;
+    }
+    s->enabled = en;
+}
+
+/* ---- steering_pid_send_tlm() ---- */
+void steering_pid_send_tlm(const steering_pid_t *s, uint32_t now_ms)
+{
+    if (STEER_TLM_HZ == 0u) return;
+
+    uint32_t interval_ms = 1000u / STEER_TLM_HZ;
+    if ((now_ms - s->last_tlm_ms) < interval_ms) return;
+    /* Cast away const for timestamp update — only mutable field */
+    ((steering_pid_t *)s)->last_tlm_ms = now_ms;
+
+    jlink_tlm_steering_t tlm;
+
+    /* Scale to ×10 for 0.1 deg/s resolution, clamped to int16 range */
+    float t = s->target_omega_dps * 10.0f;
+    if (t >  32767.0f) t =  32767.0f;
+    if (t < -32768.0f) t = -32768.0f;
+    tlm.target_x10 = (int16_t)t;
+
+    float a = s->actual_omega_dps * 10.0f;
+    if (a >  32767.0f) a =  32767.0f;
+    if (a < -32768.0f) a = -32768.0f;
+    tlm.actual_x10 = (int16_t)a;
+
+    tlm.output  = s->output;
+    tlm.enabled = s->enabled ? 1u : 0u;
+    tlm._pad    = 0u;
+
+    jlink_send_steering_tlm(&tlm);
+}

test/test_steering_pid.c

@@ -0,0 +1,618 @@
+/*
+ * test_steering_pid.c — unit tests for steering_pid (Issue #616).
+ *
+ * Host build (no HAL):
+ *   gcc -I include -I src -DTEST_HOST -lm -o /tmp/test_steering test/test_steering_pid.c
+ *   /tmp/test_steering
+ *
+ * Stubs out jlink.h to avoid the full HAL dependency chain.
+ */
+
+/* ---- Minimal stubs ---- */
+#define JLINK_H  /* prevent real jlink.h from loading */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* Minimal jlink_tlm_steering_t and send stub */
+typedef struct __attribute__((packed)) {
+    int16_t  target_x10;
+    int16_t  actual_x10;
+    int16_t  output;
+    uint8_t  enabled;
+    uint8_t  _pad;
+} jlink_tlm_steering_t;
+
+static jlink_tlm_steering_t g_last_tlm;
+static int g_tlm_count = 0;
+
+void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm)
+{
+    g_last_tlm = *tlm;
+    g_tlm_count++;
+}
+
+/* ---- Include the module under test ---- */
+#include "steering_pid.h"
+#include "steering_pid.c"   /* single-TU build; steering_pid.c found via -I src */
+
+/* ---- Test framework ---- */
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+
+static int s_pass = 0;
+static int s_fail = 0;
+
+#define EXPECT(cond, msg) do { \
+    if (cond) { s_pass++; } \
+    else { s_fail++; printf("FAIL [%s:%d]: %s\n", __func__, __LINE__, msg); } \
+} while(0)
+
+#define EXPECT_EQ(a, b, msg)  EXPECT((a) == (b), msg)
+#define EXPECT_NEAR(a, b, eps, msg) EXPECT(fabsf((a) - (b)) <= (eps), msg)
+#define EXPECT_TRUE(x, msg)   EXPECT((x), msg)
+#define EXPECT_FALSE(x, msg)  EXPECT(!(x), msg)
+
+/* ---- Tests ---- */
+
+static void test_init(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "init: target_omega 0");
+    EXPECT_NEAR(s.actual_omega_dps, 0.0f, 1e-6f, "init: actual_omega 0");
+    EXPECT_NEAR(s.integral,         0.0f, 1e-6f, "init: integral 0");
+    EXPECT_NEAR(s.prev_error,       0.0f, 1e-6f, "init: prev_error 0");
+    EXPECT_EQ  (s.output,           0,           "init: output 0");
+    EXPECT_TRUE(s.enabled,                        "init: enabled true");
+}
+
+static void test_reset(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = 30.0f;
+    s.integral  = 100.0f;
+    s.prev_error = 5.0f;
+    s.output     = 200;
+
+    steering_pid_reset(&s);
+
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "reset: target 0");
+    EXPECT_NEAR(s.integral,         0.0f, 1e-6f, "reset: integral 0");
+    EXPECT_NEAR(s.prev_error,       0.0f, 1e-6f, "reset: prev_error 0");
+    EXPECT_EQ  (s.output,           0,           "reset: output 0");
+    EXPECT_TRUE(s.enabled,                        "reset: preserves enabled");
+}
+
+static void test_disabled_returns_zero(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, false);
+    steering_pid_set_target(&s, 50.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
+
+    EXPECT_EQ(out, 0, "disabled: update returns 0");
+    EXPECT_EQ(steering_pid_get_output(&s), 0, "disabled: get_output 0");
+    /* Target should not have been set when disabled */
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "disabled: target not set");
+}
+
+static void test_zero_dt_returns_zero(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 30.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, 0.0f);
+    EXPECT_EQ(out, 0, "zero_dt: returns 0");
+}
+
+static void test_negative_dt_returns_zero(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 30.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, -0.01f);
+    EXPECT_EQ(out, 0, "neg_dt: returns 0");
+}
+
+static void test_proportional_only(void)
+{
+    /*
+     * With Ki=0 and Kd=0, output = Kp * error.
+     * Set STEER_KP=2.0, error=50 deg/s → raw = 100 counts.
+     * Rate limiter allows max_step = STEER_RAMP_RATE_PER_MS * dt_ms = 10 * 1 = 10 per step.
+     * After 1 step (dt=0.001s): output capped at 10 by rate limiter.
+     */
+    steering_pid_t s;
+    memset(&s, 0, sizeof(s));
+    steering_pid_init(&s);
+    s.integral   = 0.0f;
+    s.prev_error = 0.0f;
+    s.output     = 0;
+
+    steering_pid_set_target(&s, 50.0f);
+    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
+
+    /* Rate limiter: max_step = 10*0.001*1000 = 10 → output = 10 */
+    EXPECT_EQ(out, 10, "prop_only: rate-limited step");
+}
+
+static void test_converges_to_setpoint(void)
+{
+    /*
+     * With no disturbance, run many steps until output reaches the setpoint.
+     * With target=20 deg/s, Kp=2, after many steps the P term = 2*20 = 40,
+     * plus I accumulation. The rate limiter will eventually allow the output
+     * to grow. Run for 1s at 1kHz (1000 steps, dt=0.001s each).
+     * Expected: eventually output > 0 (PID working), integral accumulates.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 20.0f);
+
+    int16_t out = 0;
+    for (int i = 0; i < 1000; i++) {
+        out = steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    EXPECT_TRUE(out > 0, "convergence: output positive after 1s");
+    EXPECT_TRUE(s.integral > 0.0f, "convergence: integral positive");
+}
+
+static void test_output_clamped_to_max(void)
+{
+    /*
+     * Very large error should produce output clamped to STEER_OUTPUT_MAX.
+     * Run many ticks so rate limiter is satisfied.
+     * STEER_OUTPUT_MAX = 400; 400 ticks at 10 counts/ms should saturate.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);  /* 500 deg/s — way beyond robot capability */
+
+    int16_t out = 0;
+    for (int i = 0; i < 500; i++) {
+        out = steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    EXPECT_EQ(out, STEER_OUTPUT_MAX, "clamp: output saturates at STEER_OUTPUT_MAX");
+}
+
+static void test_output_clamped_negative(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, -500.0f);
+
+    int16_t out = 0;
+    for (int i = 0; i < 500; i++) {
+        out = steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    EXPECT_EQ(out, -STEER_OUTPUT_MAX, "clamp_neg: output saturates at -STEER_OUTPUT_MAX");
+}
+
+static void test_anti_windup(void)
+{
+    /*
+     * Run with a persistent error for a long time.
+     * Integral should be clamped to ±STEER_INTEGRAL_MAX.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 50.0f);
+
+    for (int i = 0; i < 10000; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);  /* actual stays 0, error = 50 */
+    }
+    EXPECT_NEAR(s.integral, STEER_INTEGRAL_MAX, 0.01f, "anti-windup: integral clamped to max");
+}
+
+static void test_anti_windup_negative(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, -50.0f);
+
+    for (int i = 0; i < 10000; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    EXPECT_NEAR(s.integral, -STEER_INTEGRAL_MAX, 0.01f, "anti-windup-neg: integral clamped");
+}
+
+static void test_rate_limiter_single_step(void)
+{
+    /*
+     * At dt=0.001s: max_step = STEER_RAMP_RATE_PER_MS * 1 = 10.
+     * Even if raw PID output = 400, first step can only reach 10.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
+    EXPECT_EQ(out, STEER_RAMP_RATE_PER_MS * 1, "rate_lim: first step bounded");
+}
+
+static void test_rate_limiter_larger_dt(void)
+{
+    /*
+     * At dt=0.01s (100Hz): max_step = 10 * 10 = 100.
+     * First step limited to 100.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);
+
+    int16_t out = steering_pid_update(&s, 0.0f, 0.01f);
+    EXPECT_EQ(out, STEER_RAMP_RATE_PER_MS * 10, "rate_lim_100hz: first step bounded");
+}
+
+static void test_rate_limiter_decreasing(void)
+{
+    /*
+     * After saturating to STEER_OUTPUT_MAX, set target to 0.
+     * Output should decrease at most STEER_RAMP_RATE_PER_MS per ms.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);
+    for (int i = 0; i < 500; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    /* Now output = STEER_OUTPUT_MAX = 400 */
+    int16_t before = s.output;
+    steering_pid_reset(&s);  /* reset integral and target */
+    /* output is also zeroed by reset — which is fine: the test is
+       really about rate limiting when driving toward 0 after a step */
+    (void)before;
+
+    /* Re-saturate, then drive to 0 with matching actual */
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 500.0f);
+    for (int i = 0; i < 500; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+    steering_pid_set_target(&s, 0.0f);
+    int16_t pre  = s.output;
+    int16_t post = steering_pid_update(&s, 0.0f, 0.001f);
+
+    /* Should decrease by no more than STEER_RAMP_RATE_PER_MS per tick */
+    int16_t delta = pre - post;
+    EXPECT_TRUE(delta <= STEER_RAMP_RATE_PER_MS, "rate_lim_dec: decrease bounded per tick");
+}
+
+static void test_zero_error_zero_output(void)
+{
+    /*
+     * When target == actual from the first tick, error = 0 and output stays 0.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 30.0f);
+    int16_t out = steering_pid_update(&s, 30.0f, 0.001f);
+    EXPECT_EQ(out, 0, "zero_error: no output when tracking exactly");
+}
+
+static void test_enable_disable_clears_state(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 50.0f);
+    for (int i = 0; i < 100; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+
+    /* Disable */
+    steering_pid_set_enabled(&s, false);
+
+    EXPECT_FALSE(s.enabled,                       "disable: enabled false");
+    EXPECT_NEAR (s.target_omega_dps, 0.0f, 1e-6f, "disable: target zeroed");
+    EXPECT_NEAR (s.integral,         0.0f, 1e-6f, "disable: integral zeroed");
+    EXPECT_EQ   (s.output,           0,           "disable: output zeroed");
+
+    /* Calls while disabled return 0 */
+    int16_t out = steering_pid_update(&s, 50.0f, 0.001f);
+    EXPECT_EQ(out, 0, "disable: update returns 0");
+}
+
+static void test_re_enable(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, false);
+    steering_pid_set_enabled(&s, true);
+
+    EXPECT_TRUE(s.enabled, "re-enable: enabled true");
+
+    /* After re-enable, new target and update should work */
+    steering_pid_set_target(&s, 20.0f);
+    EXPECT_NEAR(s.target_omega_dps, 20.0f, 1e-6f, "re-enable: set_target works");
+}
+
+static void test_set_target_ignored_when_disabled(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, false);
+    steering_pid_set_target(&s, 99.0f);
+
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "disabled_target: target not set");
+}
+
+static void test_actual_omega_updated(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_update(&s, 42.5f, 0.001f);
+    EXPECT_NEAR(s.actual_omega_dps, 42.5f, 1e-4f, "actual_omega: stored after update");
+}
+
+static void test_get_output_matches_update(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 20.0f);
+    int16_t from_update = steering_pid_update(&s, 0.0f, 0.001f);
+    EXPECT_EQ(from_update, steering_pid_get_output(&s), "get_output: matches update return");
+}
+
+static void test_omega_scale_convention(void)
+{
+    /*
+     * Verify that setting target via STEER_OMEGA_SCALE:
+     *   omega_dps = steer * STEER_OMEGA_SCALE
+     *   steer=100 → omega=10 deg/s
+     */
+    float omega = 100.0f * STEER_OMEGA_SCALE;
+    EXPECT_NEAR(omega, 10.0f, 1e-4f, "omega_scale: 100 units = 10 deg/s");
+
+    omega = -1000.0f * STEER_OMEGA_SCALE;
+    EXPECT_NEAR(omega, -100.0f, 1e-4f, "omega_scale: -1000 units = -100 deg/s");
+}
+
+static void test_tlm_first_call_fires(void)
+{
+    /*
+     * After init, last_tlm_ms is set so that the first send_tlm fires immediately
+     * at now_ms = 0.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_tlm_count, 1, "tlm_first: fires at ms=0 after init");
+}
+
+static void test_tlm_rate_limited(void)
+{
+    /*
+     * STEER_TLM_HZ = 10 → interval = 100ms.
+     * Two calls at ms=0 and ms=50 should only send once.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);    /* fires (first call) */
+    steering_pid_send_tlm(&s, 50);   /* blocked (< 100ms) */
+    EXPECT_EQ(g_tlm_count, 1, "tlm_rate: blocked within interval");
+
+    steering_pid_send_tlm(&s, 100);  /* fires (interval elapsed) */
+    EXPECT_EQ(g_tlm_count, 2, "tlm_rate: fires after interval");
+}
+
+static void test_tlm_payload_target(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = 25.5f;  /* → target_x10 = 255 */
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.target_x10, 255, "tlm_payload: target_x10 correct");
+}
+
+static void test_tlm_payload_actual(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 10.0f);
+    steering_pid_update(&s, 12.3f, 0.001f);
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);
+    /* actual = 12.3 deg/s → actual_x10 = 123 */
+    EXPECT_EQ(g_last_tlm.actual_x10, 123, "tlm_payload: actual_x10 correct");
+}
+
+static void test_tlm_payload_output(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.output = 150;
+    g_tlm_count = 0;
+
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.output, 150, "tlm_payload: output correct");
+}
+
+static void test_tlm_payload_enabled_flag(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, true);
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.enabled, 1u, "tlm_payload: enabled=1 when on");
+
+    steering_pid_init(&s);
+    steering_pid_set_enabled(&s, false);
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.enabled, 0u, "tlm_payload: enabled=0 when off");
+}
+
+static void test_tlm_payload_pad_zero(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm._pad, 0u, "tlm_payload: _pad = 0");
+}
+
+static void test_tlm_target_x10_negative(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = -30.7f;  /* → target_x10 = -307 */
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.target_x10, -307, "tlm_payload: negative target_x10");
+}
+
+static void test_tlm_int16_clamp_positive(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = 4000.0f;  /* × 10 = 40000 > INT16_MAX */
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.target_x10, 32767, "tlm_clamp: target_x10 clamped to INT16_MAX");
+}
+
+static void test_tlm_int16_clamp_negative(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    s.target_omega_dps = -4000.0f;  /* × 10 = -40000 < INT16_MIN */
+    g_tlm_count = 0;
+    steering_pid_send_tlm(&s, 0);
+    EXPECT_EQ(g_last_tlm.target_x10, (int16_t)-32768, "tlm_clamp: target_x10 clamped to INT16_MIN");
+}
+
+static void test_balance_not_disturbed_by_small_step(void)
+{
+    /*
+     * A balance bot is most sensitive to sudden changes in steer_cmd.
+     * Verify that a large target step at t=0 only increments output by
+     * STEER_RAMP_RATE_PER_MS per millisecond — not a step change.
+     * This confirms the rate limiter protects the balance PID.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 100.0f);   /* large yaw demand */
+
+    /* 10ms at 1kHz */
+    for (int i = 0; i < 10; i++) {
+        int16_t pre  = s.output;
+        int16_t post = steering_pid_update(&s, 0.0f, 0.001f);
+        int16_t step = post - pre;
+        EXPECT_TRUE(step <= STEER_RAMP_RATE_PER_MS, "balance_protect: step bounded each tick");
+        EXPECT_TRUE(step >= -STEER_RAMP_RATE_PER_MS, "balance_protect: neg step bounded each tick");
+    }
+}
+
+static void test_derivative_on_error_change(void)
+{
+    /*
+     * When error changes suddenly (e.g. actual_omega jumps), the derivative
+     * term should add a braking contribution.
+     * With STEER_KD=0.05 and a 10 deg/s²-equivalent error rate change:
+     *   d_term = 0.05 * (error1 - error0) / dt
+     * We just verify output changes when error changes, indicating D is active.
+     */
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 20.0f);
+
+    /* Step 1: actual = 0, error = 20 */
+    int16_t out1 = steering_pid_update(&s, 0.0f, 0.001f);
+
+    /* Step 2: actual jumps to 20, error = 0. D will oppose. */
+    int16_t out2 = steering_pid_update(&s, 20.0f, 0.001f);
+
+    /* out2 should be less than out1 (derivative braking as error drops) */
+    EXPECT_TRUE(out2 <= out1, "derivative: braking when error shrinks");
+}
+
+static void test_reset_then_restart(void)
+{
+    steering_pid_t s;
+    steering_pid_init(&s);
+    steering_pid_set_target(&s, 50.0f);
+    for (int i = 0; i < 100; i++) {
+        steering_pid_update(&s, 0.0f, 0.001f);
+    }
+
+    steering_pid_reset(&s);
+    EXPECT_EQ  (s.output,            0,    "reset_restart: output 0");
+    EXPECT_NEAR(s.integral,  0.0f, 1e-6f,  "reset_restart: integral 0");
+    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "reset_restart: target 0");
+
+    /* After reset, new target should work cleanly */
+    steering_pid_set_target(&s, 10.0f);
+    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
+    EXPECT_TRUE(out > 0, "reset_restart: positive output after fresh start");
+}
+
+static void test_tlm_hz_constant(void)
+{
+    EXPECT_TRUE(STEER_TLM_HZ == 10u, "tlm_hz: STEER_TLM_HZ is 10");
+}
+
+static void test_output_max_constant(void)
+{
+    EXPECT_TRUE(STEER_OUTPUT_MAX == 400, "output_max: STEER_OUTPUT_MAX is 400");
+}
+
+/* ---- main ---- */
+int main(void)
+{
+    printf("=== test_steering_pid ===\n\n");
+
+    test_init();
+    test_reset();
+    test_disabled_returns_zero();
+    test_zero_dt_returns_zero();
+    test_negative_dt_returns_zero();
+    test_proportional_only();
+    test_converges_to_setpoint();
+    test_output_clamped_to_max();
+    test_output_clamped_negative();
+    test_anti_windup();
+    test_anti_windup_negative();
+    test_rate_limiter_single_step();
+    test_rate_limiter_larger_dt();
+    test_rate_limiter_decreasing();
+    test_zero_error_zero_output();
+    test_enable_disable_clears_state();
+    test_re_enable();
+    test_set_target_ignored_when_disabled();
+    test_actual_omega_updated();
+    test_get_output_matches_update();
+    test_omega_scale_convention();
+    test_tlm_first_call_fires();
+    test_tlm_rate_limited();
+    test_tlm_payload_target();
+    test_tlm_payload_actual();
+    test_tlm_payload_output();
+    test_tlm_payload_enabled_flag();
+    test_tlm_payload_pad_zero();
+    test_tlm_target_x10_negative();
+    test_tlm_int16_clamp_positive();
+    test_tlm_int16_clamp_negative();
+    test_balance_not_disturbed_by_small_step();
+    test_derivative_on_error_change();
+    test_reset_then_restart();
+    test_tlm_hz_constant();
+    test_output_max_constant();
+
+    printf("\nResults: %d passed, %d failed\n", s_pass, s_fail);
+    return s_fail ? 1 : 0;
+}

ui/settings_panel.css

@@ -0,0 +1,343 @@
+/* settings_panel.css — Saltybot Settings (Issue #614) */
+
+*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
+
+:root {
+  --bg0:   #050510;
+  --bg1:   #070712;
+  --bg2:   #0a0a1a;
+  --bd:    #0c2a3a;
+  --bd2:   #1e3a5f;
+  --dim:   #374151;
+  --mid:   #6b7280;
+  --base:  #9ca3af;
+  --hi:    #d1d5db;
+  --cyan:  #06b6d4;
+  --green: #22c55e;
+  --amber: #f59e0b;
+  --red:   #ef4444;
+}
+
+body {
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: var(--bg0);
+  color: var(--base);
+  height: 100dvh;
+  display: flex;
+  flex-direction: column;
+  overflow: hidden;
+}
+
+/* ── Header ── */
+#header {
+  display: flex;
+  align-items: center;
+  padding: 5px 12px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  gap: 8px;
+  flex-wrap: wrap;
+}
+.logo { color: #f97316; font-weight: bold; letter-spacing: .15em; font-size: 13px; flex-shrink: 0; }
+
+#conn-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--dim); flex-shrink: 0; transition: background .3s;
+}
+#conn-dot.connected { background: var(--green); }
+#conn-dot.error     { background: var(--red); animation: blink 1s infinite; }
+
+@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.3} }
+@keyframes fadeout { 0%{opacity:1} 70%{opacity:1} 100%{opacity:0} }
+
+.save-ind {
+  font-size: 10px; font-weight: bold; letter-spacing: .1em;
+  color: var(--green); padding: 2px 8px;
+  border: 1px solid var(--green); border-radius: 3px;
+  animation: fadeout 2s forwards;
+}
+.save-ind.hidden { display: none; }
+
+#ws-input {
+  background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
+  color: #67e8f9; padding: 2px 7px; font-family: monospace; font-size: 11px; width: 185px;
+}
+#ws-input:focus { outline: none; border-color: var(--cyan); }
+
+.hbtn {
+  padding: 2px 8px; border-radius: 4px; border: 1px solid var(--bd2);
+  background: var(--bg2); color: #67e8f9; font-family: monospace;
+  font-size: 10px; font-weight: bold; cursor: pointer; transition: background .15s;
+  white-space: nowrap;
+}
+.hbtn:hover { background: #0e4f69; }
+.apply-btn { border-color: #14532d; color: #86efac; }
+.apply-btn:hover { background: #052010; }
+.del-btn { border-color: #7f1d1d; color: #fca5a5; }
+.del-btn:hover { background: #1c0505; }
+
+/* ── Tab bar ── */
+#tab-bar {
+  display: flex;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  padding: 0 12px;
+  gap: 2px;
+}
+.tab-btn {
+  padding: 6px 14px;
+  background: none;
+  border: none;
+  border-bottom: 2px solid transparent;
+  color: var(--mid);
+  font-family: 'Courier New', monospace;
+  font-size: 11px;
+  font-weight: bold;
+  letter-spacing: .1em;
+  cursor: pointer;
+  transition: color .15s, border-color .15s;
+}
+.tab-btn:hover { color: var(--base); }
+.tab-btn.active { color: var(--cyan); border-bottom-color: var(--cyan); }
+
+/* ── Main ── */
+#main {
+  flex: 1;
+  overflow-y: auto;
+  padding: 12px;
+  min-height: 0;
+}
+
+/* ── Tab panels ── */
+.tab-panel { display: none; }
+.tab-panel.active { display: block; }
+
+.panel-col {
+  display: flex;
+  flex-direction: column;
+  gap: 12px;
+  max-width: 860px;
+  margin: 0 auto;
+}
+
+/* ── Section card ── */
+.section-card {
+  background: var(--bg2);
+  border: 1px solid var(--bd2);
+  border-radius: 8px;
+  padding: 12px;
+}
+.sec-header {
+  display: flex;
+  justify-content: space-between;
+  align-items: flex-start;
+  margin-bottom: 10px;
+  gap: 8px;
+  flex-wrap: wrap;
+}
+.sec-title {
+  font-size: 11px; font-weight: bold; letter-spacing: .1em;
+  color: #67e8f9; text-transform: uppercase;
+}
+.sec-node {
+  font-size: 9px; color: var(--mid); margin-top: 2px;
+}
+.sec-actions { display: flex; gap: 6px; flex-shrink: 0; }
+
+/* ── Parameter fields ── */
+.param-row {
+  display: grid;
+  grid-template-columns: 180px 1fr 80px 50px;
+  align-items: center;
+  gap: 8px;
+  padding: 5px 0;
+  border-bottom: 1px solid var(--bd);
+}
+.param-row:last-child { border-bottom: none; }
+.param-label { font-size: 10px; color: var(--mid); }
+.param-unit  { font-size: 9px;  color: var(--dim); text-align: right; }
+
+/* Slider */
+.param-slider {
+  -webkit-appearance: none;
+  width: 100%; height: 4px; border-radius: 2px;
+  background: var(--bd2); outline: none; cursor: pointer;
+}
+.param-slider::-webkit-slider-thumb {
+  -webkit-appearance: none;
+  width: 12px; height: 12px; border-radius: 50%;
+  background: var(--cyan); cursor: pointer;
+}
+.param-slider::-moz-range-thumb {
+  width: 12px; height: 12px; border-radius: 50%;
+  background: var(--cyan); cursor: pointer; border: none;
+}
+.param-slider.changed::-webkit-slider-thumb { background: var(--amber); }
+.param-slider.changed::-moz-range-thumb     { background: var(--amber); }
+
+/* Number input */
+.param-input {
+  background: var(--bg0); border: 1px solid var(--bd2);
+  border-radius: 3px; color: var(--hi); padding: 2px 5px;
+  font-family: monospace; font-size: 10px; text-align: right;
+  width: 72px;
+}
+.param-input:focus { outline: none; border-color: var(--cyan); }
+.param-input.changed { border-color: var(--amber); color: var(--amber); }
+
+/* Toggle switch */
+.toggle-row {
+  display: grid;
+  grid-template-columns: 180px 1fr auto;
+  align-items: center;
+  gap: 8px;
+  padding: 6px 0;
+  border-bottom: 1px solid var(--bd);
+}
+.toggle-row:last-child { border-bottom: none; }
+.toggle-desc { font-size: 9px; color: var(--dim); }
+
+.toggle-switch {
+  position: relative;
+  width: 36px; height: 18px; flex-shrink: 0;
+}
+.toggle-switch input { opacity: 0; width: 0; height: 0; }
+.toggle-track {
+  position: absolute; inset: 0;
+  background: var(--dim); border-radius: 9px; cursor: pointer;
+  transition: background .2s;
+}
+.toggle-track::after {
+  content: '';
+  position: absolute;
+  left: 2px; top: 2px;
+  width: 14px; height: 14px;
+  border-radius: 50%;
+  background: #fff;
+  transition: transform .2s;
+}
+.toggle-switch input:checked + .toggle-track { background: var(--cyan); }
+.toggle-switch input:checked + .toggle-track::after { transform: translateX(18px); }
+
+/* Status / feedback line */
+.sec-status {
+  margin-top: 6px;
+  font-size: 9px;
+  min-height: 14px;
+  transition: color .3s;
+}
+.sec-status.ok     { color: var(--green); }
+.sec-status.err    { color: var(--red); }
+.sec-status.loading { color: var(--amber); }
+
+/* ── System tab ── */
+.diag-placeholder {
+  color: var(--dim); font-size: 10px; padding: 12px 0; text-align: center;
+}
+#diag-grid {
+  display: grid;
+  grid-template-columns: repeat(auto-fill, minmax(180px, 1fr));
+  gap: 8px;
+}
+.diag-card {
+  background: var(--bg0);
+  border: 1px solid var(--bd);
+  border-radius: 5px;
+  padding: 8px;
+}
+.diag-card-title {
+  font-size: 9px; font-weight: bold; letter-spacing: .08em;
+  color: #0891b2; margin-bottom: 5px; text-transform: uppercase;
+}
+.diag-kv {
+  display: flex; justify-content: space-between;
+  padding: 2px 0; font-size: 9px;
+  border-bottom: 1px solid var(--bd);
+}
+.diag-kv:last-child { border-bottom: none; }
+.diag-k { color: var(--mid); }
+.diag-v { color: var(--hi); font-family: monospace; }
+.diag-v.ok    { color: var(--green); }
+.diag-v.warn  { color: var(--amber); }
+.diag-v.err   { color: var(--red); }
+
+#net-grid {
+  display: grid;
+  grid-template-columns: repeat(auto-fill, minmax(180px, 1fr));
+  gap: 8px;
+}
+
+/* Signal bars */
+.sig-bars {
+  display: inline-flex; align-items: flex-end; gap: 2px; height: 14px;
+}
+.sig-bar {
+  width: 4px; border-radius: 1px; background: var(--dim);
+}
+.sig-bar.lit { background: var(--cyan); }
+
+/* Node list */
+#node-list-wrap {
+  display: flex; flex-wrap: wrap; gap: 4px;
+  max-height: 160px; overflow-y: auto;
+}
+.node-chip {
+  font-size: 9px; padding: 1px 6px; border-radius: 2px;
+  border: 1px solid var(--bd); background: var(--bg0);
+  color: var(--mid);
+}
+.node-chip.active-node { border-color: var(--bd2); color: var(--base); }
+
+/* ── Preset bar ── */
+#preset-bar {
+  display: flex;
+  align-items: center;
+  gap: 8px;
+  padding: 5px 12px;
+  background: var(--bg1);
+  border-top: 1px solid var(--bd);
+  flex-shrink: 0;
+  flex-wrap: wrap;
+}
+.plbl { font-size: 10px; color: var(--mid); letter-spacing: .08em; }
+
+#preset-select {
+  background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
+  color: #67e8f9; padding: 2px 6px; font-family: monospace; font-size: 10px;
+  cursor: pointer;
+}
+#preset-select:focus { outline: none; }
+
+#preset-name {
+  background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
+  color: var(--hi); padding: 2px 7px; font-family: monospace; font-size: 11px; width: 140px;
+}
+#preset-name:focus { outline: none; border-color: var(--cyan); }
+
+/* ── Footer ── */
+#footer {
+  background: var(--bg1); border-top: 1px solid var(--bd);
+  padding: 3px 12px;
+  display: flex; align-items: center; justify-content: space-between;
+  flex-shrink: 0; font-size: 10px; color: var(--dim);
+}
+
+/* ── Responsive ── */
+@media (max-width: 640px) {
+  .param-row {
+    grid-template-columns: 140px 1fr 70px 44px;
+    gap: 5px;
+  }
+  #diag-grid { grid-template-columns: 1fr 1fr; }
+}
+@media (max-width: 420px) {
+  .param-row {
+    grid-template-columns: 1fr 1fr;
+    grid-template-rows: auto auto;
+  }
+  .param-label { grid-column: 1 / 3; }
+  .param-unit  { text-align: left; }
+}

ui/settings_panel.html

@@ -0,0 +1,313 @@
+<!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 — Settings</title>
+<link rel="stylesheet" href="settings_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 — SETTINGS</div>
+  <div id="conn-dot"></div>
+  <input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
+  <button id="btn-connect" class="hbtn">CONNECT</button>
+  <span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
+  <div style="flex:1"></div>
+  <div id="save-indicator" class="save-ind hidden">✓ SAVED</div>
+</div>
+
+<!-- ── Tab bar ── -->
+<div id="tab-bar">
+  <button class="tab-btn active" data-tab="pid">PID</button>
+  <button class="tab-btn" data-tab="speed">SPEED</button>
+  <button class="tab-btn" data-tab="safety">SAFETY</button>
+  <button class="tab-btn" data-tab="sensors">SENSORS</button>
+  <button class="tab-btn" data-tab="system">SYSTEM</button>
+</div>
+
+<!-- ── Main ── -->
+<div id="main">
+
+  <!-- ═══ PID TAB ═══ -->
+  <div class="tab-panel active" id="tab-pid">
+    <div class="panel-col">
+
+      <!-- Balance PID -->
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Balance Controller PID</div>
+            <div class="sec-node">/balance_controller</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('balance_pid')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('balance_pid')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="balance_pid-fields"></div>
+        <div class="sec-status" id="balance_pid-status"></div>
+      </div>
+
+      <!-- Adaptive PID -->
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Adaptive PID — Empty Load</div>
+            <div class="sec-node">/adaptive_pid</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('adaptive_pid_empty')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('adaptive_pid_empty')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="adaptive_pid_empty-fields"></div>
+        <div class="sec-status" id="adaptive_pid_empty-status"></div>
+      </div>
+
+      <!-- Adaptive PID bounds -->
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Adaptive PID — Bounds</div>
+            <div class="sec-node">/adaptive_pid</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('adaptive_pid_bounds')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('adaptive_pid_bounds')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="adaptive_pid_bounds-fields"></div>
+        <div class="sec-status" id="adaptive_pid_bounds-status"></div>
+      </div>
+
+    </div>
+  </div>
+
+  <!-- ═══ SPEED TAB ═══ -->
+  <div class="tab-panel" id="tab-speed">
+    <div class="panel-col">
+
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Tank Driver Limits</div>
+            <div class="sec-node">/tank_driver</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('tank_limits')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('tank_limits')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="tank_limits-fields"></div>
+        <div class="sec-status" id="tank_limits-status"></div>
+      </div>
+
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Smooth Velocity Controller</div>
+            <div class="sec-node">/smooth_velocity_controller</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('smooth_vel')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('smooth_vel')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="smooth_vel-fields"></div>
+        <div class="sec-status" id="smooth_vel-status"></div>
+      </div>
+
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Battery Speed Limiter</div>
+            <div class="sec-node">/battery_speed_limiter</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('batt_speed')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('batt_speed')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="batt_speed-fields"></div>
+        <div class="sec-status" id="batt_speed-status"></div>
+      </div>
+
+    </div>
+  </div>
+
+  <!-- ═══ SAFETY TAB ═══ -->
+  <div class="tab-panel" id="tab-safety">
+    <div class="panel-col">
+
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Safety Zone</div>
+            <div class="sec-node">/safety_zone</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('safety_zone')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('safety_zone')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="safety_zone-fields"></div>
+        <div class="sec-status" id="safety_zone-status"></div>
+      </div>
+
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Power Supervisor</div>
+            <div class="sec-node">/power_supervisor_node</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('power_sup')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('power_sup')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="power_sup-fields"></div>
+        <div class="sec-status" id="power_sup-status"></div>
+      </div>
+
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">LIDAR Avoidance</div>
+            <div class="sec-node">/lidar_avoidance</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('lidar_avoid')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('lidar_avoid')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="lidar_avoid-fields"></div>
+        <div class="sec-status" id="lidar_avoid-status"></div>
+      </div>
+
+    </div>
+  </div>
+
+  <!-- ═══ SENSORS TAB ═══ -->
+  <div class="tab-panel" id="tab-sensors">
+    <div class="panel-col">
+
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Sensor Enable / Disable</div>
+            <div class="sec-node">various nodes</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('sensor_toggles')">↓ LOAD ALL</button>
+            <button class="hbtn apply-btn" onclick="applySection('sensor_toggles')">↑ APPLY ALL</button>
+          </div>
+        </div>
+        <div id="sensor_toggles-fields"></div>
+        <div class="sec-status" id="sensor_toggles-status"></div>
+      </div>
+
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">IMU / Odometry Fusion</div>
+            <div class="sec-node">/uwb_imu_fusion</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadSection('imu_fusion')">↓ LOAD</button>
+            <button class="hbtn apply-btn" onclick="applySection('imu_fusion')">↑ APPLY</button>
+          </div>
+        </div>
+        <div id="imu_fusion-fields"></div>
+        <div class="sec-status" id="imu_fusion-status"></div>
+      </div>
+
+    </div>
+  </div>
+
+  <!-- ═══ SYSTEM TAB ═══ -->
+  <div class="tab-panel" id="tab-system">
+    <div class="panel-col">
+
+      <!-- Live diagnostics -->
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">System Diagnostics</div>
+            <div class="sec-node">/diagnostics · auto-refresh 2 s</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" id="btn-refresh-diag" onclick="refreshDiag()">⟳ REFRESH</button>
+          </div>
+        </div>
+        <div id="diag-grid">
+          <div class="diag-placeholder">Connect to rosbridge to view diagnostics.</div>
+        </div>
+      </div>
+
+      <!-- WiFi / network -->
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Network</div>
+            <div class="sec-node">/diagnostics (wifi keys)</div>
+          </div>
+        </div>
+        <div id="net-grid">
+          <div class="diag-placeholder">Waiting for network data…</div>
+        </div>
+      </div>
+
+      <!-- Node list -->
+      <div class="section-card">
+        <div class="sec-header">
+          <div>
+            <div class="sec-title">Active ROS2 Nodes</div>
+            <div class="sec-node">rosbridge /rosapi/nodes</div>
+          </div>
+          <div class="sec-actions">
+            <button class="hbtn" onclick="loadNodeList()">⟳ REFRESH</button>
+          </div>
+        </div>
+        <div id="node-list-wrap">
+          <div class="diag-placeholder">Click refresh to list active nodes.</div>
+        </div>
+      </div>
+
+    </div>
+  </div>
+
+</div><!-- /#main -->
+
+<!-- ── Preset bar ── -->
+<div id="preset-bar">
+  <span class="plbl">PRESETS:</span>
+  <select id="preset-select">
+    <option value="">— select —</option>
+  </select>
+  <button class="hbtn" onclick="loadPreset()">↓ LOAD</button>
+  <input id="preset-name" type="text" placeholder="Preset name…" />
+  <button class="hbtn" onclick="savePreset()">↑ SAVE</button>
+  <button class="hbtn del-btn" onclick="deletePreset()">✕ DELETE</button>
+  <div style="flex:1"></div>
+  <button class="hbtn" onclick="resetAllToDefaults()">⟲ DEFAULTS</button>
+</div>
+
+<!-- ── Footer ── -->
+<div id="footer">
+  <span>ROS2 param services · rcl_interfaces/srv/GetParameters · SetParameters</span>
+  <span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
+  <span>settings panel — issue #614</span>
+</div>
+
+<script src="settings_panel.js"></script>
+<script>
+  document.getElementById('ws-input').addEventListener('input', (e) => {
+    document.getElementById('footer-ws').textContent = e.target.value;
+  });
+</script>
+</body>
+</html>

ui/settings_panel.js

@@ -0,0 +1,716 @@
+/* settings_panel.js — Saltybot Settings Panel (Issue #614) */
+'use strict';
+
+// ── ROS2 parameter type constants ──────────────────────────────────────────
+const P_BOOL    = 1;
+const P_INT     = 2;
+const P_DOUBLE  = 3;
+
+// ── Section / parameter definitions ───────────────────────────────────────
+// Each section: { id, node, params: [{name, label, type, min, max, step, unit, def}] }
+const SECTIONS = {
+
+  balance_pid: {
+    node: 'balance_controller',
+    params: [
+      { name: 'pid_p', label: 'Proportional (Kp)', type: P_DOUBLE, min: 0, max: 5,    step: 0.01,  unit: '',      def: 0.5  },
+      { name: 'pid_i', label: 'Integral (Ki)',      type: P_DOUBLE, min: 0, max: 2,    step: 0.005, unit: '',      def: 0.1  },
+      { name: 'pid_d', label: 'Derivative (Kd)',    type: P_DOUBLE, min: 0, max: 1,    step: 0.005, unit: '',      def: 0.05 },
+      { name: 'i_clamp',label:'I clamp',            type: P_DOUBLE, min: 0, max: 50,   step: 0.5,   unit: '',      def: 10.0 },
+      { name: 'frequency',label:'Control rate',     type: P_INT,    min: 10, max: 200, step: 10,    unit: 'Hz',    def: 50   },
+    ],
+  },
+
+  adaptive_pid_empty: {
+    node: 'adaptive_pid',
+    params: [
+      { name: 'kp_empty', label: 'Kp (empty load)',  type: P_DOUBLE, min: 0, max: 50,  step: 0.5,  unit: '', def: 15.0 },
+      { name: 'ki_empty', label: 'Ki (empty load)',  type: P_DOUBLE, min: 0, max: 5,   step: 0.05, unit: '', def: 0.5  },
+      { name: 'kd_empty', label: 'Kd (empty load)',  type: P_DOUBLE, min: 0, max: 10,  step: 0.1,  unit: '', def: 1.5  },
+      { name: 'kp_light', label: 'Kp (light load)',  type: P_DOUBLE, min: 0, max: 50,  step: 0.5,  unit: '', def: 18.0 },
+      { name: 'ki_light', label: 'Ki (light load)',  type: P_DOUBLE, min: 0, max: 5,   step: 0.05, unit: '', def: 0.6  },
+      { name: 'kd_light', label: 'Kd (light load)',  type: P_DOUBLE, min: 0, max: 10,  step: 0.1,  unit: '', def: 2.0  },
+      { name: 'kp_heavy', label: 'Kp (heavy load)',  type: P_DOUBLE, min: 0, max: 50,  step: 0.5,  unit: '', def: 22.0 },
+      { name: 'ki_heavy', label: 'Ki (heavy load)',  type: P_DOUBLE, min: 0, max: 5,   step: 0.05, unit: '', def: 0.8  },
+      { name: 'kd_heavy', label: 'Kd (heavy load)',  type: P_DOUBLE, min: 0, max: 10,  step: 0.1,  unit: '', def: 2.5  },
+    ],
+  },
+
+  adaptive_pid_bounds: {
+    node: 'adaptive_pid',
+    params: [
+      { name: 'kp_min',      label: 'Kp min',       type: P_DOUBLE, min: 0, max: 20,  step: 0.5,  unit: '', def: 5.0  },
+      { name: 'kp_max',      label: 'Kp max',       type: P_DOUBLE, min: 0, max: 80,  step: 1,    unit: '', def: 40.0 },
+      { name: 'ki_min',      label: 'Ki min',       type: P_DOUBLE, min: 0, max: 5,   step: 0.05, unit: '', def: 0.0  },
+      { name: 'ki_max',      label: 'Ki max',       type: P_DOUBLE, min: 0, max: 10,  step: 0.1,  unit: '', def: 5.0  },
+      { name: 'kd_min',      label: 'Kd min',       type: P_DOUBLE, min: 0, max: 5,   step: 0.05, unit: '', def: 0.0  },
+      { name: 'kd_max',      label: 'Kd max',       type: P_DOUBLE, min: 0, max: 20,  step: 0.2,  unit: '', def: 10.0 },
+      { name: 'output_min',  label: 'Output min',   type: P_DOUBLE, min: -100, max: 0, step: 1,   unit: '', def: -50.0},
+      { name: 'output_max',  label: 'Output max',   type: P_DOUBLE, min: 0, max: 100, step: 1,    unit: '', def: 50.0 },
+    ],
+  },
+
+  tank_limits: {
+    node: 'tank_driver',
+    params: [
+      { name: 'max_linear_vel',  label: 'Max linear vel',  type: P_DOUBLE, min: 0.1, max: 3.0, step: 0.05, unit: 'm/s',   def: 1.0 },
+      { name: 'max_angular_vel', label: 'Max angular vel', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.1,  unit: 'rad/s', def: 2.5 },
+      { name: 'max_speed_ms',    label: 'Max drive speed', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.1,  unit: 'm/s',   def: 1.5 },
+      { name: 'slip_factor',     label: 'Track slip factor',type: P_DOUBLE,min: 0,   max: 0.5, step: 0.01, unit: '',      def: 0.3 },
+      { name: 'watchdog_timeout_s',label:'Watchdog timeout',type:P_DOUBLE, min: 0.1, max: 2.0, step: 0.05, unit: 's',     def: 0.3 },
+    ],
+  },
+
+  smooth_vel: {
+    node: 'smooth_velocity_controller',
+    params: [
+      { name: 'max_linear_accel',  label: 'Max linear accel',  type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.05, unit: 'm/s²',   def: 0.5 },
+      { name: 'max_linear_decel',  label: 'Max linear decel',  type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.05, unit: 'm/s²',   def: 0.8 },
+      { name: 'max_angular_accel', label: 'Max angular accel', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.1,  unit: 'rad/s²', def: 1.0 },
+      { name: 'max_angular_decel', label: 'Max angular decel', type: P_DOUBLE, min: 0.1, max: 5.0, step: 0.1,  unit: 'rad/s²', def: 1.0 },
+    ],
+  },
+
+  batt_speed: {
+    node: 'battery_speed_limiter',
+    params: [
+      { name: 'speed_factor_full',     label: 'Speed factor (full)',     type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 1.0 },
+      { name: 'speed_factor_reduced',  label: 'Speed factor (reduced)',  type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.7 },
+      { name: 'speed_factor_critical', label: 'Speed factor (critical)', type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.4 },
+    ],
+  },
+
+  safety_zone: {
+    node: 'safety_zone',
+    params: [
+      { name: 'danger_range_m',       label: 'Danger range',        type: P_DOUBLE, min: 0.05, max: 1.0,  step: 0.01, unit: 'm',   def: 0.30 },
+      { name: 'warn_range_m',         label: 'Warn range',          type: P_DOUBLE, min: 0.2,  max: 5.0,  step: 0.05, unit: 'm',   def: 1.00 },
+      { name: 'forward_arc_deg',      label: 'Forward arc (±)',     type: P_DOUBLE, min: 10,   max: 180,  step: 5,    unit: '°',   def: 60.0 },
+      { name: 'estop_debounce_frames',label: 'E-stop debounce',     type: P_INT,    min: 1,    max: 20,   step: 1,    unit: 'frames', def: 2  },
+      { name: 'min_valid_range_m',    label: 'Min valid range',     type: P_DOUBLE, min: 0.01, max: 0.5,  step: 0.01, unit: 'm',   def: 0.05 },
+      { name: 'publish_rate',         label: 'Publish rate',        type: P_DOUBLE, min: 1,    max: 50,   step: 1,    unit: 'Hz',  def: 10.0 },
+    ],
+  },
+
+  power_sup: {
+    node: 'power_supervisor_node',
+    params: [
+      { name: 'warning_pct',     label: 'Warning threshold',    type: P_DOUBLE, min: 5,  max: 60, step: 1, unit: '%', def: 30.0 },
+      { name: 'dock_search_pct', label: 'Dock-search threshold',type: P_DOUBLE, min: 5,  max: 50, step: 1, unit: '%', def: 20.0 },
+      { name: 'critical_pct',    label: 'Critical threshold',   type: P_DOUBLE, min: 2,  max: 30, step: 1, unit: '%', def: 10.0 },
+      { name: 'emergency_pct',   label: 'Emergency threshold',  type: P_DOUBLE, min: 1,  max: 15, step: 1, unit: '%', def: 5.0  },
+      { name: 'warn_speed_factor',   label: 'Speed factor (warn)',type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.6 },
+      { name: 'critical_speed_factor',label:'Speed factor (crit)',type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.3 },
+    ],
+  },
+
+  lidar_avoid: {
+    node: 'lidar_avoidance',
+    params: [
+      { name: 'emergency_stop_distance', label: 'E-stop distance',    type: P_DOUBLE, min: 0.1, max: 3.0, step: 0.05, unit: 'm',   def: 0.5  },
+      { name: 'min_safety_zone',         label: 'Min safety zone',    type: P_DOUBLE, min: 0.1, max: 2.0, step: 0.05, unit: 'm',   def: 0.6  },
+      { name: 'safety_zone_at_max_speed',label: 'Zone at max speed',  type: P_DOUBLE, min: 0.5, max: 10,  step: 0.1,  unit: 'm',   def: 3.0  },
+      { name: 'max_speed_reference',     label: 'Max speed reference',type: P_DOUBLE, min: 0.5, max: 20,  step: 0.1,  unit: 'm/s', def: 5.56 },
+    ],
+  },
+
+  sensor_toggles: {
+    node: 'safety_zone',  // placeholder — booleans often live on their own node
+    params: [
+      { name: 'estop_all_arcs',   label: 'E-stop all arcs',      type: P_BOOL, unit: '', def: false, desc: 'Any sector triggers e-stop' },
+      { name: 'lidar_enabled',    label: 'LIDAR enabled',         type: P_BOOL, unit: '', def: true,  desc: '/scan input active' },
+      { name: 'uwb_enabled',      label: 'UWB positioning',       type: P_BOOL, unit: '', def: true,  desc: 'UWB anchors active' },
+    ],
+  },
+
+  imu_fusion: {
+    node: 'uwb_imu_fusion',
+    params: [
+      { name: 'uwb_weight',   label: 'UWB weight',    type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.7 },
+      { name: 'imu_weight',   label: 'IMU weight',    type: P_DOUBLE, min: 0, max: 1, step: 0.05, unit: '', def: 0.3 },
+      { name: 'publish_rate', label: 'Publish rate',  type: P_DOUBLE, min: 1, max: 200, step: 1,   unit: 'Hz', def: 50.0 },
+    ],
+  },
+
+};
+
+// ── Runtime state: current values per section ──────────────────────────────
+const values = {};   // values[sectionId][paramName] = currentValue
+const dirty  = {};   // dirty[sectionId][paramName]  = true if changed vs loaded
+
+// Initialise values from defaults
+Object.keys(SECTIONS).forEach(sid => {
+  values[sid] = {};
+  dirty[sid]  = {};
+  SECTIONS[sid].params.forEach(p => {
+    values[sid][p.name] = p.def;
+    dirty[sid][p.name]  = false;
+  });
+});
+
+// ── ROS ────────────────────────────────────────────────────────────────────
+let ros = null;
+
+function getService(nodeName, type) {
+  return new ROSLIB.Service({ ros, name: `/${nodeName}/${type}`, serviceType: `rcl_interfaces/srv/${type === 'get_parameters' ? 'GetParameters' : 'SetParameters'}` });
+}
+
+function extractValue(rosVal) {
+  switch (rosVal.type) {
+    case P_BOOL:   return rosVal.bool_value;
+    case P_INT:    return rosVal.integer_value;
+    case P_DOUBLE: return rosVal.double_value;
+    default: return undefined;
+  }
+}
+
+function makeRosValue(type, value) {
+  const v = { type };
+  if (type === P_BOOL)   v.bool_value    = !!value;
+  if (type === P_INT)    v.integer_value = Math.round(value);
+  if (type === P_DOUBLE) v.double_value  = parseFloat(value);
+  return v;
+}
+
+// ── Section load / apply ───────────────────────────────────────────────────
+window.loadSection = function(sid) {
+  if (!ros) { setStatus(sid, 'err', 'Not connected'); return; }
+  const sec = SECTIONS[sid];
+  const svc = getService(sec.node, 'get_parameters');
+  const names = sec.params.map(p => p.name);
+
+  setStatus(sid, 'loading', `Loading from /${sec.node}…`);
+
+  svc.callService({ names }, (resp) => {
+    if (!resp || !resp.values) {
+      setStatus(sid, 'err', 'No response'); return;
+    }
+    sec.params.forEach((p, i) => {
+      const v = extractValue(resp.values[i]);
+      if (v !== undefined) {
+        values[sid][p.name] = v;
+        dirty[sid][p.name]  = false;
+        updateFieldUI(sid, p.name, v, false);
+      }
+    });
+    setStatus(sid, 'ok', `Loaded ${resp.values.length} params`);
+  }, (err) => {
+    setStatus(sid, 'err', `Error: ${err}`);
+  });
+};
+
+window.applySection = function(sid) {
+  if (!ros) { setStatus(sid, 'err', 'Not connected'); return; }
+  const sec = SECTIONS[sid];
+  const svc = getService(sec.node, 'set_parameters');
+
+  const parameters = sec.params.map(p => ({
+    name:  p.name,
+    value: makeRosValue(p.type, values[sid][p.name]),
+  }));
+
+  setStatus(sid, 'loading', `Applying to /${sec.node}…`);
+
+  svc.callService({ parameters }, (resp) => {
+    if (!resp || !resp.results) {
+      setStatus(sid, 'err', 'No response'); return;
+    }
+    const failures = resp.results.filter(r => !r.successful);
+    if (failures.length === 0) {
+      sec.params.forEach(p => { dirty[sid][p.name] = false; });
+      refreshFieldDirty(sid);
+      setStatus(sid, 'ok', `Applied ${parameters.length} params ✓`);
+    } else {
+      const reasons = failures.map(r => r.reason).join('; ');
+      setStatus(sid, 'err', `${failures.length} failed: ${reasons}`);
+    }
+  }, (err) => {
+    setStatus(sid, 'err', `Error: ${err}`);
+  });
+};
+
+// ── UI builder ─────────────────────────────────────────────────────────────
+function buildFields() {
+  Object.keys(SECTIONS).forEach(sid => {
+    const sec = SECTIONS[sid];
+    const container = document.getElementById(`${sid}-fields`);
+    if (!container) return;
+
+    sec.params.forEach(p => {
+      if (p.type === P_BOOL) {
+        // Toggle row
+        const row = document.createElement('div');
+        row.className = 'toggle-row';
+        row.innerHTML = `
+          <span class="param-label">${p.label}</span>
+          <span class="toggle-desc">${p.desc || ''}</span>
+          <label class="toggle-switch">
+            <input type="checkbox" id="tgl-${sid}-${p.name}"
+              ${p.def ? 'checked' : ''}>
+            <div class="toggle-track"></div>
+          </label>`;
+        container.appendChild(row);
+
+        const cb = row.querySelector(`#tgl-${sid}-${p.name}`);
+        cb.addEventListener('change', () => {
+          values[sid][p.name] = cb.checked;
+          dirty[sid][p.name]  = true;
+        });
+      } else {
+        // Slider + number input row
+        const row = document.createElement('div');
+        row.className = 'param-row';
+        const sliderMin = p.min !== undefined ? p.min : 0;
+        const sliderMax = p.max !== undefined ? p.max : 100;
+        const sliderStep = p.step || 0.01;
+        const defVal = p.def !== undefined ? p.def : 0;
+
+        row.innerHTML = `
+          <span class="param-label">${p.label}</span>
+          <input type="range" class="param-slider"
+            id="sld-${sid}-${p.name}"
+            min="${sliderMin}" max="${sliderMax}" step="${sliderStep}"
+            value="${defVal}">
+          <input type="number" class="param-input"
+            id="inp-${sid}-${p.name}"
+            min="${sliderMin}" max="${sliderMax}" step="${sliderStep}"
+            value="${defVal}">
+          <span class="param-unit">${p.unit || ''}</span>`;
+        container.appendChild(row);
+
+        const slider = row.querySelector(`#sld-${sid}-${p.name}`);
+        const input  = row.querySelector(`#inp-${sid}-${p.name}`);
+
+        slider.addEventListener('input', () => {
+          const v = parseFloat(slider.value);
+          input.value = v;
+          values[sid][p.name] = v;
+          dirty[sid][p.name]  = true;
+          input.classList.add('changed');
+          slider.classList.add('changed');
+        });
+
+        input.addEventListener('change', () => {
+          let v = parseFloat(input.value);
+          v = Math.max(sliderMin, Math.min(sliderMax, v));
+          input.value  = v;
+          slider.value = v;
+          values[sid][p.name] = v;
+          dirty[sid][p.name]  = true;
+          input.classList.add('changed');
+          slider.classList.add('changed');
+        });
+      }
+    });
+  });
+}
+
+function updateFieldUI(sid, paramName, value, markDirty) {
+  const sec = SECTIONS[sid];
+  const p   = sec.params.find(x => x.name === paramName);
+  if (!p) return;
+
+  if (p.type === P_BOOL) {
+    const cb = document.getElementById(`tgl-${sid}-${paramName}`);
+    if (cb) cb.checked = !!value;
+  } else {
+    const sld = document.getElementById(`sld-${sid}-${paramName}`);
+    const inp = document.getElementById(`inp-${sid}-${paramName}`);
+    if (sld) sld.value = value;
+    if (inp) inp.value = value;
+    if (!markDirty) {
+      if (sld) sld.classList.remove('changed');
+      if (inp) inp.classList.remove('changed');
+    }
+  }
+}
+
+function refreshFieldDirty(sid) {
+  const sec = SECTIONS[sid];
+  sec.params.forEach(p => {
+    if (p.type !== P_BOOL) {
+      const sld = document.getElementById(`sld-${sid}-${p.name}`);
+      const inp = document.getElementById(`inp-${sid}-${p.name}`);
+      if (!dirty[sid][p.name]) {
+        if (sld) sld.classList.remove('changed');
+        if (inp) inp.classList.remove('changed');
+      }
+    }
+  });
+}
+
+function setStatus(sid, cls, msg) {
+  const el = document.getElementById(`${sid}-status`);
+  if (!el) return;
+  el.className = `sec-status ${cls}`;
+  el.textContent = msg;
+  if (cls === 'ok') setTimeout(() => { el.textContent = ''; el.className = 'sec-status'; }, 4000);
+}
+
+// ── Tabs ───────────────────────────────────────────────────────────────────
+document.querySelectorAll('.tab-btn').forEach(btn => {
+  btn.addEventListener('click', () => {
+    document.querySelectorAll('.tab-btn').forEach(b => b.classList.remove('active'));
+    document.querySelectorAll('.tab-panel').forEach(p => p.classList.remove('active'));
+    btn.classList.add('active');
+    document.getElementById(`tab-${btn.dataset.tab}`).classList.add('active');
+    if (btn.dataset.tab === 'system') startDiagRefresh();
+  });
+});
+
+// ── Presets ────────────────────────────────────────────────────────────────
+const PRESET_KEY = 'saltybot_settings_presets';
+
+function getPresets() {
+  try { return JSON.parse(localStorage.getItem(PRESET_KEY) || '{}'); } catch(_) { return {}; }
+}
+
+function savePresetsToStorage(presets) {
+  localStorage.setItem(PRESET_KEY, JSON.stringify(presets));
+}
+
+function refreshPresetSelect() {
+  const sel = document.getElementById('preset-select');
+  const cur = sel.value;
+  sel.innerHTML = '<option value="">— select —</option>';
+  const presets = getPresets();
+  Object.keys(presets).sort().forEach(name => {
+    const opt = document.createElement('option');
+    opt.value = name;
+    opt.textContent = name;
+    sel.appendChild(opt);
+  });
+  if (cur) sel.value = cur;
+}
+
+function snapshotAllValues() {
+  const snap = {};
+  Object.keys(values).forEach(sid => {
+    snap[sid] = Object.assign({}, values[sid]);
+  });
+  return snap;
+}
+
+window.savePreset = function() {
+  const nameInput = document.getElementById('preset-name');
+  const name = nameInput.value.trim();
+  if (!name) { alert('Enter a preset name'); return; }
+  const presets = getPresets();
+  presets[name] = snapshotAllValues();
+  savePresetsToStorage(presets);
+  nameInput.value = '';
+  refreshPresetSelect();
+  document.getElementById('preset-select').value = name;
+  flashSaved();
+};
+
+window.loadPreset = function() {
+  const name = document.getElementById('preset-select').value;
+  if (!name) return;
+  const presets = getPresets();
+  const snap = presets[name];
+  if (!snap) return;
+  Object.keys(snap).forEach(sid => {
+    if (!values[sid]) return;
+    Object.keys(snap[sid]).forEach(paramName => {
+      values[sid][paramName] = snap[sid][paramName];
+      dirty[sid][paramName]  = true;
+      updateFieldUI(sid, paramName, snap[sid][paramName], true);
+    });
+  });
+  flashSaved();
+};
+
+window.deletePreset = function() {
+  const name = document.getElementById('preset-select').value;
+  if (!name) return;
+  if (!confirm(`Delete preset "${name}"?`)) return;
+  const presets = getPresets();
+  delete presets[name];
+  savePresetsToStorage(presets);
+  refreshPresetSelect();
+};
+
+window.resetAllToDefaults = function() {
+  if (!confirm('Reset all fields to built-in defaults?')) return;
+  Object.keys(SECTIONS).forEach(sid => {
+    SECTIONS[sid].params.forEach(p => {
+      values[sid][p.name] = p.def;
+      dirty[sid][p.name]  = false;
+      updateFieldUI(sid, p.name, p.def, false);
+    });
+  });
+};
+
+function flashSaved() {
+  const el = document.getElementById('save-indicator');
+  el.classList.remove('hidden');
+  el.style.animation = 'none';
+  void el.offsetHeight;
+  el.style.animation = 'fadeout 2s forwards';
+  setTimeout(() => el.classList.add('hidden'), 2100);
+}
+
+// ── System tab: diagnostics ────────────────────────────────────────────────
+let diagTopic = null;
+let diagRefreshTimer = null;
+let diagState = {
+  cpuTemp: null, gpuTemp: null, boardTemp: null,
+  motorTempL: null, motorTempR: null,
+  ramUsed: null, ramTotal: null,
+  gpuUsed: null, gpuTotal: null,
+  diskUsed: null, diskTotal: null,
+  rssi: null, latency: null, mqttConnected: null,
+  nodes: [],
+  lastUpdate: null,
+};
+
+function startDiagRefresh() {
+  if (!ros || diagTopic) return;
+  diagTopic = new ROSLIB.Topic({
+    ros, name: '/diagnostics', messageType: 'diagnostic_msgs/DiagnosticArray',
+    throttle_rate: 2000,
+  });
+  diagTopic.subscribe(onDiagnostics);
+}
+
+function stopDiagRefresh() {
+  if (diagTopic) { diagTopic.unsubscribe(); diagTopic = null; }
+}
+
+function onDiagnostics(msg) {
+  const kv = {};
+  (msg.status || []).forEach(status => {
+    (status.values || []).forEach(pair => {
+      kv[pair.key] = pair.value;
+    });
+    diagState.nodes.push({ name: status.name, level: status.level, msg: status.message });
+    if (diagState.nodes.length > 40) diagState.nodes.splice(0, diagState.nodes.length - 40);
+  });
+
+  // Extract known keys
+  if (kv.cpu_temp_c)        diagState.cpuTemp    = parseFloat(kv.cpu_temp_c);
+  if (kv.gpu_temp_c)        diagState.gpuTemp    = parseFloat(kv.gpu_temp_c);
+  if (kv.board_temp_c)      diagState.boardTemp  = parseFloat(kv.board_temp_c);
+  if (kv.motor_temp_l_c)    diagState.motorTempL = parseFloat(kv.motor_temp_l_c);
+  if (kv.motor_temp_r_c)    diagState.motorTempR = parseFloat(kv.motor_temp_r_c);
+  if (kv.ram_used_mb)       diagState.ramUsed    = parseFloat(kv.ram_used_mb);
+  if (kv.ram_total_mb)      diagState.ramTotal   = parseFloat(kv.ram_total_mb);
+  if (kv.gpu_used_mb)       diagState.gpuUsed    = parseFloat(kv.gpu_used_mb);
+  if (kv.gpu_total_mb)      diagState.gpuTotal   = parseFloat(kv.gpu_total_mb);
+  if (kv.disk_used_gb)      diagState.diskUsed   = parseFloat(kv.disk_used_gb);
+  if (kv.disk_total_gb)     diagState.diskTotal  = parseFloat(kv.disk_total_gb);
+  if (kv.wifi_rssi_dbm)     diagState.rssi       = parseFloat(kv.wifi_rssi_dbm);
+  if (kv.wifi_latency_ms)   diagState.latency    = parseFloat(kv.wifi_latency_ms);
+  if (kv.mqtt_connected !== undefined) diagState.mqttConnected = kv.mqtt_connected === 'true';
+
+  diagState.lastUpdate = new Date();
+  renderDiag();
+  renderNet();
+}
+
+window.refreshDiag = function() {
+  startDiagRefresh();
+  renderDiag();
+};
+
+function tempColor(t) {
+  if (t === null) return '';
+  if (t > 80) return 'err';
+  if (t > 65) return 'warn';
+  return 'ok';
+}
+
+function pct(used, total) {
+  if (!total) return '—';
+  return ((used / total) * 100).toFixed(0) + '%';
+}
+
+function renderDiag() {
+  const g = document.getElementById('diag-grid');
+  if (!g) return;
+  const d = diagState;
+  if (d.lastUpdate === null) {
+    g.innerHTML = '<div class="diag-placeholder">Waiting for /diagnostics…</div>';
+    return;
+  }
+
+  const cards = [
+    {
+      title: 'Temperature',
+      rows: [
+        { k: 'CPU', v: d.cpuTemp !== null ? d.cpuTemp.toFixed(1) + ' °C' : '—', cls: tempColor(d.cpuTemp) },
+        { k: 'GPU', v: d.gpuTemp !== null ? d.gpuTemp.toFixed(1) + ' °C' : '—', cls: tempColor(d.gpuTemp) },
+        { k: 'Board',v:d.boardTemp !== null ? d.boardTemp.toFixed(1)+' °C':'—', cls:tempColor(d.boardTemp)},
+        { k: 'Motor L', v: d.motorTempL !== null ? d.motorTempL.toFixed(1)+' °C':'—', cls:tempColor(d.motorTempL)},
+        { k: 'Motor R', v: d.motorTempR !== null ? d.motorTempR.toFixed(1)+' °C':'—', cls:tempColor(d.motorTempR)},
+      ],
+    },
+    {
+      title: 'Memory',
+      rows: [
+        { k: 'RAM used',  v: d.ramUsed  !== null ? d.ramUsed.toFixed(0)+' MB':  '—', cls:'' },
+        { k: 'RAM total', v: d.ramTotal !== null ? d.ramTotal.toFixed(0)+' MB': '—', cls:'' },
+        { k: 'RAM %',     v: pct(d.ramUsed, d.ramTotal), cls: d.ramUsed/d.ramTotal > 0.85 ? 'warn' : 'ok' },
+        { k: 'GPU mem',   v: d.gpuUsed  !== null ? d.gpuUsed.toFixed(0)+' MB':  '—', cls:'' },
+        { k: 'GPU %',     v: pct(d.gpuUsed, d.gpuTotal), cls:'' },
+      ],
+    },
+    {
+      title: 'Storage',
+      rows: [
+        { k: 'Disk used',  v: d.diskUsed  !== null ? d.diskUsed.toFixed(1)+' GB':  '—', cls:'' },
+        { k: 'Disk total', v: d.diskTotal !== null ? d.diskTotal.toFixed(1)+' GB': '—', cls:'' },
+        { k: 'Disk %',     v: pct(d.diskUsed, d.diskTotal), cls: d.diskUsed/d.diskTotal > 0.9 ? 'err' : d.diskUsed/d.diskTotal > 0.75 ? 'warn' : 'ok' },
+      ],
+    },
+    {
+      title: 'Updated',
+      rows: [
+        { k: 'Last msg', v: d.lastUpdate ? d.lastUpdate.toLocaleTimeString() : '—', cls:'' },
+      ],
+    },
+  ];
+
+  g.innerHTML = cards.map(c => `
+    <div class="diag-card">
+      <div class="diag-card-title">${c.title}</div>
+      ${c.rows.map(r => `<div class="diag-kv"><span class="diag-k">${r.k}</span><span class="diag-v ${r.cls||''}">${r.v}</span></div>`).join('')}
+    </div>`).join('');
+}
+
+function rssiColor(rssi) {
+  if (rssi === null) return '';
+  if (rssi > -50) return 'ok';
+  if (rssi > -70) return 'warn';
+  return 'err';
+}
+
+function rssiBarCount(rssi) {
+  if (rssi === null) return 0;
+  if (rssi > -50) return 5;
+  if (rssi > -60) return 4;
+  if (rssi > -70) return 3;
+  if (rssi > -80) return 2;
+  return 1;
+}
+
+function latencyColor(ms) {
+  if (ms === null) return '';
+  if (ms < 50)  return 'ok';
+  if (ms < 150) return 'warn';
+  return 'err';
+}
+
+function renderNet() {
+  const g = document.getElementById('net-grid');
+  if (!g) return;
+  const d = diagState;
+  const bars = rssiBarCount(d.rssi);
+  const heights = [4, 6, 9, 12, 15];
+
+  const barsHtml = heights.map((h, i) =>
+    `<div class="sig-bar ${i < bars ? 'lit' : ''}" style="height:${h}px"></div>`
+  ).join('');
+
+  g.innerHTML = `
+    <div class="diag-card">
+      <div class="diag-card-title">WiFi</div>
+      <div class="diag-kv">
+        <span class="diag-k">RSSI</span>
+        <span class="diag-v ${rssiColor(d.rssi)}">${d.rssi !== null ? d.rssi + ' dBm' : '—'}</span>
+      </div>
+      <div class="diag-kv">
+        <span class="diag-k">Signal</span>
+        <span class="diag-v"><div class="sig-bars">${barsHtml}</div></span>
+      </div>
+      <div class="diag-kv">
+        <span class="diag-k">Latency</span>
+        <span class="diag-v ${latencyColor(d.latency)}">${d.latency !== null ? d.latency.toFixed(0) + ' ms' : '—'}</span>
+      </div>
+    </div>
+    <div class="diag-card">
+      <div class="diag-card-title">Services</div>
+      <div class="diag-kv">
+        <span class="diag-k">MQTT</span>
+        <span class="diag-v ${d.mqttConnected === null ? '' : d.mqttConnected ? 'ok' : 'err'}">
+          ${d.mqttConnected === null ? '—' : d.mqttConnected ? 'Connected' : 'Disconnected'}
+        </span>
+      </div>
+      <div class="diag-kv">
+        <span class="diag-k">rosbridge</span>
+        <span class="diag-v ${ros ? 'ok' : 'err'}">${ros ? 'Connected' : 'Disconnected'}</span>
+      </div>
+    </div>`;
+}
+
+// ── Node list ──────────────────────────────────────────────────────────────
+window.loadNodeList = function() {
+  if (!ros) return;
+  const svc = new ROSLIB.Service({
+    ros, name: '/rosapi/nodes', serviceType: 'rosapi/Nodes',
+  });
+  svc.callService({}, (resp) => {
+    const wrap = document.getElementById('node-list-wrap');
+    if (!resp || !resp.nodes) { wrap.innerHTML = '<div class="diag-placeholder">No response</div>'; return; }
+    const sorted = [...resp.nodes].sort();
+    wrap.innerHTML = sorted.map(n =>
+      `<span class="node-chip active-node">${n}</span>`
+    ).join('');
+  }, () => {
+    document.getElementById('node-list-wrap').innerHTML =
+      '<div class="diag-placeholder">Service unavailable — ensure rosapi is running</div>';
+  });
+};
+
+// ── Connection ─────────────────────────────────────────────────────────────
+const $connDot   = document.getElementById('conn-dot');
+const $connLabel = document.getElementById('conn-label');
+
+function connect() {
+  const url = document.getElementById('ws-input').value.trim() || 'ws://localhost:9090';
+  if (ros) { stopDiagRefresh(); try { ros.close(); } catch(_){} }
+
+  $connLabel.textContent = 'Connecting…';
+  $connLabel.style.color = '#d97706';
+  $connDot.className = '';
+
+  ros = new ROSLIB.Ros({ url });
+
+  ros.on('connection', () => {
+    $connDot.className = 'connected';
+    $connLabel.style.color = '#22c55e';
+    $connLabel.textContent = 'Connected';
+    localStorage.setItem('settings_ws_url', url);
+    // Auto-start diagnostics if system tab visible
+    const sysPanel = document.getElementById('tab-system');
+    if (sysPanel && sysPanel.classList.contains('active')) startDiagRefresh();
+  });
+
+  ros.on('error', () => {
+    $connDot.className = 'error';
+    $connLabel.style.color = '#ef4444';
+    $connLabel.textContent = 'Error';
+  });
+
+  ros.on('close', () => {
+    $connDot.className = '';
+    $connLabel.style.color = '#6b7280';
+    $connLabel.textContent = 'Disconnected';
+    stopDiagRefresh();
+    ros = null;
+  });
+}
+
+document.getElementById('btn-connect').addEventListener('click', connect);
+
+// ── Init ───────────────────────────────────────────────────────────────────
+buildFields();
+refreshPresetSelect();
+
+const savedUrl = localStorage.getItem('settings_ws_url');
+if (savedUrl) {
+  document.getElementById('ws-input').value = savedUrl;
+  document.getElementById('footer-ws').textContent = savedUrl;
+  connect();
+}