feat: PID gain scheduling for speed-dependent balance (Issue #550)

Implements a speed-dependent PID gain scheduler that interpolates Kp/Ki/Kd
across a configurable table of velocity breakpoints, replacing the fixed
single-gain PID used previously.

Changes:
- include/pid_flash.h: add pid_sched_entry_t (16-byte entry), pid_sched_flash_t
  (128-byte record at 0x0807FF40), pid_flash_load_schedule(), pid_flash_save_all()
  (atomic single-sector erase for both schedule and single-PID records)
- src/pid_flash.c: implement load_schedule and save_all; single erase covers
  both records at 0x0807FF40 (schedule) and 0x0807FFC0 (single PID)
- include/pid_schedule.h: API header -- init, get_gains, apply, set/get table,
  flash_save, active_band_idx, get_default_table
- src/pid_schedule.c: linear interpolation between sorted speed-band entries;
  integrator reset on band transition; default 3-band table (0/0.3/0.8 m/s)
- include/jlink.h: add SCHED_GET (0x0C), SCHED_SET (0x0D), SCHED_SAVE (0x0E)
  commands; TLM_SCHED (0x85); jlink_tlm_sched_t; JLinkSchedSetBuf;
  sched_get_req, sched_save_req fields in JLinkState; include pid_flash.h
- src/jlink.c: dispatch SCHED_GET/SET/SAVE; implement jlink_send_sched_telemetry,
  jlink_get_sched_set; add JLinkSchedSetBuf static buffer
- test/test_pid_schedule.c: 48 unit tests -- all passing (gcc host build)

Flash layout (sector 7):
  0x0807FF40  pid_sched_flash_t (128 bytes) -- schedule
  0x0807FFC0  pid_flash_t       ( 64 bytes) -- single PID (existing)

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

chassis/gimbal_camera_mount.scad

@@ -0,0 +1,599 @@
+// ============================================================
+// gimbal_camera_mount.scad — Pan/Tilt Gimbal Mount for RealSense D435i
+// Issue: #552  Agent: sl-mechanical  Date: 2026-03-14
+// ============================================================
+//
+// Parametric gimbal bracket system mounting an Intel RealSense D435i
+// (or similar box camera) on a 2-axis pan/tilt gimbal driven by
+// ST3215 serial bus servos (25T spline, Feetech/Waveshare).
+//
+// Architecture:
+//   Pan axis  — base T-nut clamps to 2020 rail; pan servo rotates yoke
+//   Tilt axis — tilt servo horn plate bolts to ST3215 horn; camera cradle
+//               rocks on tilt axis
+//   Camera    — D435i captured via 1/4-20 UNC hex nut in cradle floor
+//   Damping   — PETG flexure ribs on camera contact faces (or TPU pads)
+//   Wiring    — USB-C cable routed through channel in cradle arm
+//
+// Part catalogue:
+//   Part 1 — tnut_rail_base()       2020 rail T-nut base + pan servo seat
+//   Part 2 — pan_yoke()             U-yoke connecting pan servo to tilt axis
+//   Part 3 — tilt_horn_plate()      Plate bolting to ST3215 tilt servo horn
+//   Part 4 — camera_cradle()        D435i cradle with 1/4-20 captured nut
+//   Part 5 — vibe_pad()             PETG flexure vibration-damping pad (×2)
+//   Part 6 — assembly_preview()     Full assembly preview
+//
+// Hardware BOM (per gimbal):
+//   2× ST3215 serial bus servo (pan + tilt)
+//   2× servo horn (25T spline, ≥Ø36 mm, 4× M3 bolt holes on Ø24 mm BC)
+//   2× M3 × 8 mm SHCS          horn-to-plate bolts (×4 each horn = 8 total)
+//   1× M3 × 16 mm SHCS + nut   T-nut rail clamp thumbscrew
+//   1× 1/4-20 UNC × 8 mm SHCS  camera retention bolt (or existing tripod screw)
+//   1× 1/4-20 UNC hex nut       captured in cradle floor
+//   4× M3 × 12 mm SHCS          yoke-to-tilt-plate pivot axle bolts
+//   2× M3 × 25 mm SHCS          pan yoke attachment to servo body
+//   (optional) 2× vibe_pad printed in TPU 95A
+//
+// ST3215 servo interface (caliper-verified Feetech ST3215):
+//   Body footprint : 40.0 × 20.0 mm (W × D), 36.5 mm tall
+//   Shaft centre H : 28.5 mm from mounting face
+//   Shaft spline   : 25T, centre Ø5.8 mm, D-cut
+//   Mount holes    : 4× M3 on 32 × 10 mm rectangular pattern (18 mm offset)
+//   Horn bolt circle: Ø24 mm, 4× M3
+//   Horn OD        : ~36 mm
+//
+// D435i camera interface (caliper-verified):
+//   Body           : 90 × 25 × 25 mm (W × D × H)
+//   Tripod thread  : 1/4-20 UNC, centred bottom face, 9 mm from front
+//   USB-C connector: right rear, 8 × 5 mm opening, 4 mm from edge
+//
+// Parametric camera size (override to adapt to other cameras):
+//   CAM_W, CAM_D, CAM_H  — body envelope
+//   CAM_MOUNT_X           — tripod hole X offset from camera centre
+//   CAM_MOUNT_Y           — tripod hole Y offset from front face
+//
+// Coordinate convention:
+//   Camera looks in +Y direction (forward)
+//   Pan axis is Z (vertical); tilt axis is X (lateral)
+//   Rail runs along Z; T-nut base at Z=0
+//   All parts at assembly origin; translate for assembly_preview
+//
+// RENDER options:
+//   "assembly"            full assembly preview (default)
+//   "tnut_rail_base_stl"  Part 1
+//   "pan_yoke_stl"        Part 2
+//   "tilt_horn_plate_stl" Part 3
+//   "camera_cradle_stl"   Part 4
+//   "vibe_pad_stl"        Part 5
+//
+// Export commands:
+//   openscad gimbal_camera_mount.scad -D 'RENDER="tnut_rail_base_stl"'  -o gcm_tnut_base.stl
+//   openscad gimbal_camera_mount.scad -D 'RENDER="pan_yoke_stl"'        -o gcm_pan_yoke.stl
+//   openscad gimbal_camera_mount.scad -D 'RENDER="tilt_horn_plate_stl"' -o gcm_tilt_horn_plate.stl
+//   openscad gimbal_camera_mount.scad -D 'RENDER="camera_cradle_stl"'   -o gcm_camera_cradle.stl
+//   openscad gimbal_camera_mount.scad -D 'RENDER="vibe_pad_stl"'        -o gcm_vibe_pad.stl
+// ============================================================
+
+$fn = 64;
+e   = 0.01;   // epsilon for boolean clearance
+
+// ── Parametric camera envelope ────────────────────────────────────────────────
+// Override these for cameras other than D435i
+CAM_W          = 90.0;   // camera body width (X)
+CAM_D          = 25.0;   // camera body depth (Y)
+CAM_H          = 25.0;   // camera body height (Z)
+CAM_MOUNT_X    =  0.0;   // tripod hole X offset from camera body centre
+CAM_MOUNT_Y    =  9.0;   // tripod hole from front face (Y)  [D435i: 9 mm]
+CAM_USBC_X     = CAM_W/2 - 4;   // USB-C connector X (right side)
+CAM_USBC_Z     = CAM_H/2;       // USB-C connector Z (mid-height rear)
+CAM_USBC_W     =  9.0;   // USB-C opening width (X)
+CAM_USBC_H     =  5.0;   // USB-C opening height (Z)
+
+// ── Rail geometry (matches sensor_rail.scad / sensor_rail_brackets.scad) ─────
+RAIL_W         = 20.0;
+SLOT_OPEN      =  6.0;
+SLOT_INNER_W   = 10.2;
+SLOT_INNER_H   =  5.8;
+SLOT_NECK_H    =  3.2;
+
+// ── T-nut geometry (matches sensor_rail_brackets.scad) ───────────────────────
+TNUT_W         =  9.8;
+TNUT_H         =  5.5;
+TNUT_L         = 12.0;
+TNUT_M3_NUT_AF =  5.5;
+TNUT_M3_NUT_H  =  2.5;
+TNUT_BOLT_D    =  3.3;   // M3 clearance
+
+// ── T-nut base plate geometry ─────────────────────────────────────────────────
+BASE_W         = 44.0;   // wide enough for pan servo body (40 mm)
+BASE_H         = 40.0;   // height along rail (Z)
+BASE_T         = SLOT_NECK_H + 2.0;   // plate depth (Y), rail-face side
+
+// ── ST3215 servo geometry ─────────────────────────────────────────────────────
+SERVO_W        = 40.0;   // servo body width (X)
+SERVO_D        = 20.0;   // servo body depth (Y)
+SERVO_H        = 36.5;   // servo body height (Z)
+SERVO_SHAFT_Z  = 28.5;   // shaft centre height from mounting face
+SERVO_HOLE_X   = 16.0;   // mount hole half-span X (32 mm span)
+SERVO_HOLE_Y   =  5.0;   // mount hole half-span Y (10 mm span)
+SERVO_M3_D     =  3.3;   // M3 clearance
+
+// ── Servo horn geometry ───────────────────────────────────────────────────────
+HORN_OD        = 36.0;   // horn outer diameter
+HORN_SPLINE_D  =  5.9;   // 25T spline bore clearance (5.8 + 0.1)
+HORN_BC_D      = 24.0;   // bolt circle diameter (4× M3)
+HORN_BOLT_D    =  3.3;   // M3 clearance through horn plate
+HORN_PLATE_T   =  5.0;   // tilt horn plate thickness
+
+// ── Yoke geometry ─────────────────────────────────────────────────────────────
+YOKE_WALL_T    =  5.0;   // yoke arm wall thickness
+YOKE_ARM_H     = 50.0;   // yoke arm height (Z) — clears servo body + camera
+YOKE_INNER_W   = CAM_W + 8.0;  // yoke inner span (camera + pad clearance)
+YOKE_BASE_T    =  8.0;   // yoke base plate thickness
+
+// ── Tilt pivot ────────────────────────────────────────────────────────────────
+PIVOT_D        =  4.3;   // M4 pivot axle bore
+PIVOT_BOSS_D   = 10.0;   // boss OD around pivot bore
+PIVOT_BOSS_L   =  6.0;   // boss protrusion from yoke wall
+
+// ── Camera cradle geometry ────────────────────────────────────────────────────
+CRADLE_WALL_T  =  4.0;   // cradle side wall thickness
+CRADLE_FLOOR_T =  5.0;   // cradle floor thickness (holds 1/4-20 nut)
+CRADLE_LIP_T   =  3.0;   // front retaining lip thickness
+CRADLE_LIP_H   =  8.0;   // front lip height
+CABLE_CH_W     = 12.0;   // USB-C cable channel width
+CABLE_CH_H     =  8.0;   // USB-C cable channel height
+
+// ── 1/4-20 UNC tripod thread ──────────────────────────────────────────────────
+QTR20_D        =  6.6;   // 1/4-20 clearance bore
+QTR20_NUT_AF   = 11.1;   // 1/4-20 hex nut across-flats (standard)
+QTR20_NUT_H    =  5.5;   // 1/4-20 hex nut height
+
+// ── Vibration-damping pad geometry ────────────────────────────────────────────
+PAD_W          = CAM_W - 2*CRADLE_WALL_T - 2;
+PAD_H          = CAM_H + 4;
+PAD_T          =  2.5;   // pad body thickness
+RIB_H          =  1.5;   // flexure rib height
+RIB_W          =  1.2;   // rib width
+RIB_PITCH      =  5.0;   // rib pitch
+
+// ── Fastener sizes ────────────────────────────────────────────────────────────
+M3_D = 3.3;
+M4_D = 4.3;
+M3_NUT_AF = 5.5;
+M3_NUT_H  = 2.4;
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
+RENDER = "assembly";
+
+if      (RENDER == "assembly")            assembly_preview();
+else if (RENDER == "tnut_rail_base_stl")  tnut_rail_base();
+else if (RENDER == "pan_yoke_stl")        pan_yoke();
+else if (RENDER == "tilt_horn_plate_stl") tilt_horn_plate();
+else if (RENDER == "camera_cradle_stl")   camera_cradle();
+else if (RENDER == "vibe_pad_stl")        vibe_pad();
+
+// ============================================================
+// ASSEMBLY PREVIEW
+// ============================================================
+module assembly_preview() {
+    asm_rail_z = 0;
+    // Rail section ghost (200 mm)
+    %color("Silver", 0.25)
+        translate([-RAIL_W/2, -RAIL_W/2, asm_rail_z])
+            cube([RAIL_W, RAIL_W, 200]);
+
+    // T-nut rail base
+    color("OliveDrab", 0.85)
+        translate([0, 0, asm_rail_z + 80])
+            tnut_rail_base();
+
+    // Pan servo ghost (sitting in base seat)
+    %color("DimGray", 0.4)
+        translate([-SERVO_W/2, BASE_T, asm_rail_z + 80 + (BASE_H - SERVO_H)/2])
+            cube([SERVO_W, SERVO_D, SERVO_H]);
+
+    // Pan yoke rising from servo shaft
+    color("SteelBlue", 0.85)
+        translate([0, BASE_T + SERVO_D, asm_rail_z + 80 + BASE_H/2])
+            pan_yoke();
+
+    // Tilt horn plate (tilt axis — left yoke wall)
+    color("DarkOrange", 0.85)
+        translate([-YOKE_INNER_W/2 - YOKE_WALL_T - HORN_PLATE_T,
+                   BASE_T + SERVO_D + YOKE_BASE_T,
+                   asm_rail_z + 80 + BASE_H/2 + YOKE_ARM_H/2])
+            rotate([0, 90, 0])
+                tilt_horn_plate();
+
+    // Camera cradle (centred in yoke)
+    color("DarkSlateGray", 0.85)
+        translate([0, BASE_T + SERVO_D + YOKE_BASE_T + CRADLE_FLOOR_T,
+                   asm_rail_z + 80 + BASE_H/2 + YOKE_ARM_H/2 - CAM_H/2])
+            camera_cradle();
+
+    // D435i ghost
+    %color("Black", 0.4)
+        translate([-CAM_W/2,
+                   BASE_T + SERVO_D + YOKE_BASE_T + CRADLE_FLOOR_T + PAD_T,
+                   asm_rail_z + 80 + Base_H_mid() - CAM_H/2])
+            cube([CAM_W, CAM_D, CAM_H]);
+
+    // Vibe pads (front + rear camera face)
+    color("DimGray", 0.80) {
+        translate([-CAM_W/2 + CRADLE_WALL_T + 1,
+                   Base_T + SERVO_D + YOKE_BASE_T + CRADLE_FLOOR_T,
+                   asm_rail_z + 80 + Base_H_mid() - PAD_H/2])
+            rotate([90, 0, 0])
+                vibe_pad();
+    }
+}
+
+// helper (avoids recomputing same expression)
+function Base_T() = BASE_T;
+function Base_H_mid() = BASE_H/2 + YOKE_ARM_H/2;
+
+// ============================================================
+// PART 1 — T-NUT RAIL BASE  (pan servo seat + rail clamp)
+// ============================================================
+// Mounts to 2020 rail via standard T-nut tongue.
+// Front face (+Y side) provides flat seat for pan ST3215 servo body.
+// Servo body recessed 1 mm into seat for positive lateral registration.
+// Pan servo shaft axis = Z (vertical) → pan rotation about Z.
+//
+// Print: PETG, 5 perims, 50 % gyroid. Orient face-plate down (flat).
+module tnut_rail_base() {
+    difference() {
+        union() {
+            // ── Face plate (against rail outer face, -Y side) ────────────
+            translate([-BASE_W/2, -BASE_T, 0])
+                cube([BASE_W, BASE_T, BASE_H]);
+
+            // ── T-nut neck (enters rail slot, +Y side of face plate) ─────
+            translate([-TNUT_W/2, 0, (BASE_H - TNUT_L)/2])
+                cube([TNUT_W, SLOT_NECK_H + e, TNUT_L]);
+
+            // ── T-nut inner body (wider, locks inside T-groove) ──────────
+            translate([-TNUT_W/2, SLOT_NECK_H - e, (BASE_H - TNUT_L)/2])
+                cube([TNUT_W, TNUT_H - SLOT_NECK_H + e, TNUT_L]);
+
+            // ── Pan servo seat boss (front face, +Y side) ────────────────
+            // Proud pad that servo body sits on; 1 mm registration recess
+            translate([-BASE_W/2, -BASE_T, 0])
+                cube([BASE_W, BASE_T + 6, BASE_H]);
+        }
+
+        // ── Rail clamp bolt bore (M3 through face plate) ─────────────────
+        translate([0, -BASE_T - e, BASE_H/2])
+            rotate([-90, 0, 0])
+                cylinder(d = TNUT_BOLT_D, h = BASE_T + TNUT_H + 2*e);
+
+        // ── M3 hex nut pocket (inside T-nut body) ────────────────────────
+        translate([0, SLOT_NECK_H + 0.3, BASE_H/2])
+            rotate([-90, 0, 0])
+                cylinder(d = TNUT_M3_NUT_AF / cos(30),
+                         h = TNUT_M3_NUT_H + 0.3, $fn = 6);
+
+        // ── Servo body recess (1 mm registration pocket in seat face) ────
+        translate([-SERVO_W/2 - 0.3, -BASE_T + 6 - 1.0,
+                   (BASE_H - SERVO_H)/2 - 0.3])
+            cube([SERVO_W + 0.6, 1.2, SERVO_H + 0.6]);
+
+        // ── Pan servo mount holes (4× M3 in rectangular pattern) ─────────
+        for (sx = [-SERVO_HOLE_X, SERVO_HOLE_X])
+        for (sy = [-SERVO_HOLE_Y, SERVO_HOLE_Y])
+            translate([sx, -BASE_T + 6 + e, BASE_H/2 + sy])
+                rotate([90, 0, 0])
+                    cylinder(d = SERVO_M3_D, h = BASE_T + 2*e);
+
+        // ── Pan servo shaft bore (passes shaft through base if needed) ────
+        // Centre of shaft at Z = BASE_H/2, no bore needed (shaft exits top)
+
+        // ── Lightening pockets ────────────────────────────────────────────
+        translate([0, -BASE_T/2 + 3, BASE_H/2])
+            cube([BASE_W - 14, BASE_T - 4, BASE_H - 14], center = true);
+    }
+}
+
+// ============================================================
+// PART 2 — PAN YOKE
+// ============================================================
+// U-shaped yoke that attaches to pan servo horn (below) and carries
+// the tilt axis (above).  Two vertical arms straddle the camera cradle.
+// Tilt servo sits on top of one arm; tilt pivot boss on the other.
+//
+// Yoke base bolts to pan servo horn (4× M3 on HORN_BC_D bolt circle).
+// Pan servo horn spline bore passes through yoke base centre.
+// Tilt axis: M4 pivot axle through boss on each arm (X-axis rotation).
+//
+// Print: upright (yoke in final orientation), PETG, 5 perims, 40% gyroid.
+module pan_yoke() {
+    arm_z_total = YOKE_ARM_H + YOKE_BASE_T;
+    inner_w     = YOKE_INNER_W;
+
+    difference() {
+        union() {
+            // ── Yoke base plate (bolts to pan servo horn) ─────────────────
+            translate([-inner_w/2 - YOKE_WALL_T, 0, 0])
+                cube([inner_w + 2*YOKE_WALL_T, YOKE_BASE_T, YOKE_BASE_T]);
+
+            // ── Left arm ──────────────────────────────────────────────────
+            translate([-inner_w/2 - YOKE_WALL_T, 0, 0])
+                cube([YOKE_WALL_T, YOKE_BASE_T, arm_z_total]);
+
+            // ── Right arm (tilt servo side) ───────────────────────────────
+            translate([inner_w/2, 0, 0])
+                cube([YOKE_WALL_T, YOKE_BASE_T, arm_z_total]);
+
+            // ── Tilt pivot bosses (both arms, X-axis) ─────────────────────
+            // Left pivot boss (plain pivot — M4 bolt)
+            translate([-inner_w/2 - YOKE_WALL_T - PIVOT_BOSS_L,
+                       YOKE_BASE_T/2,
+                       YOKE_BASE_T + YOKE_ARM_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = PIVOT_BOSS_D, h = PIVOT_BOSS_L + YOKE_WALL_T);
+
+            // Right pivot boss (tilt servo horn seat)
+            translate([inner_w/2,
+                       YOKE_BASE_T/2,
+                       YOKE_BASE_T + YOKE_ARM_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = PIVOT_BOSS_D + 4, h = PIVOT_BOSS_L + YOKE_WALL_T);
+
+            // ── Tilt servo body seat on right arm top ─────────────────────
+            translate([inner_w/2, 0, arm_z_total - SERVO_H - 4])
+                cube([YOKE_WALL_T + SERVO_D + 2, YOKE_BASE_T, SERVO_H + 4]);
+        }
+
+        // ── Pan horn spline bore (centre of yoke base) ────────────────────
+        translate([0, YOKE_BASE_T/2, YOKE_BASE_T/2])
+            rotate([90, 0, 0])
+                cylinder(d = HORN_SPLINE_D, h = YOKE_BASE_T + 2*e,
+                         center = true);
+
+        // ── Pan horn bolt holes (4× M3 on HORN_BC_D) ─────────────────────
+        for (a = [45, 135, 225, 315])
+            translate([HORN_BC_D/2 * cos(a),
+                       YOKE_BASE_T/2,
+                       HORN_BC_D/2 * sin(a) + YOKE_BASE_T/2])
+                rotate([90, 0, 0])
+                    cylinder(d = HORN_BOLT_D, h = YOKE_BASE_T + 2*e,
+                             center = true);
+
+        // ── Left tilt pivot bore (M4 clearance) ───────────────────────────
+        translate([-inner_w/2 - YOKE_WALL_T - PIVOT_BOSS_L - e,
+                   YOKE_BASE_T/2,
+                   YOKE_BASE_T + YOKE_ARM_H/2])
+            rotate([0, 90, 0])
+                cylinder(d = PIVOT_D, h = PIVOT_BOSS_L + YOKE_WALL_T + 2*e);
+
+        // ── Right tilt pivot bore (larger — tilt horn plate seats here) ───
+        translate([inner_w/2 - e,
+                   YOKE_BASE_T/2,
+                   YOKE_BASE_T + YOKE_ARM_H/2])
+            rotate([0, 90, 0])
+                cylinder(d = HORN_SPLINE_D,
+                         h = PIVOT_BOSS_L + YOKE_WALL_T + 2*e);
+
+        // ── Tilt servo mount holes in right arm seat ──────────────────────
+        for (sz = [-SERVO_HOLE_Y, SERVO_HOLE_Y])
+            translate([inner_w/2 + YOKE_WALL_T + SERVO_D/2,
+                       YOKE_BASE_T/2,
+                       arm_z_total - SERVO_H/2 + sz])
+                rotate([90, 0, 0])
+                    cylinder(d = SERVO_M3_D, h = YOKE_BASE_T + 2*e,
+                             center = true);
+
+        // ── M3 nut pockets (tilt servo mount, rear of arm seat) ──────────
+        for (sz = [-SERVO_HOLE_Y, SERVO_HOLE_Y])
+            translate([inner_w/2 + YOKE_WALL_T + SERVO_D/2,
+                       YOKE_BASE_T - M3_NUT_H - 0.5,
+                       arm_z_total - SERVO_H/2 + sz])
+                rotate([90, 0, 0])
+                    cylinder(d = M3_NUT_AF / cos(30), h = M3_NUT_H + 0.5,
+                             $fn = 6);
+
+        // ── Lightening slots in yoke arms ─────────────────────────────────
+        translate([-inner_w/2 - YOKE_WALL_T/2,
+                   YOKE_BASE_T/2,
+                   YOKE_BASE_T + YOKE_ARM_H/2 - 10])
+            cube([YOKE_WALL_T - 2, YOKE_BASE_T - 2, YOKE_ARM_H - 24],
+                 center = true);
+        translate([inner_w/2 + YOKE_WALL_T/2,
+                   YOKE_BASE_T/2,
+                   YOKE_BASE_T + YOKE_ARM_H/2 - 10])
+            cube([YOKE_WALL_T - 2, YOKE_BASE_T - 2, YOKE_ARM_H - 30],
+                 center = true);
+    }
+}
+
+// ============================================================
+// PART 3 — TILT HORN PLATE
+// ============================================================
+// Disc plate bolting to tilt ST3215 servo horn on the right yoke arm.
+// Servo horn spline centres into disc bore (captured, no free rotation).
+// Camera cradle attaches to opposite face via 2× M3 bolts.
+//
+// Tilt range: ±45° limited by yoke arm geometry.
+// Plate thickness HORN_PLATE_T provides stiffness for cantilevered cradle.
+//
+// Print: flat (disc face down), PETG, 5 perims, 50 % infill.
+module tilt_horn_plate() {
+    plate_od = HORN_OD + 8;   // plate OD (4 mm rim outside horn BC)
+
+    difference() {
+        union() {
+            // ── Main disc ─────────────────────────────────────────────────
+            cylinder(d = plate_od, h = HORN_PLATE_T);
+
+            // ── Cradle attachment arm (extends to camera cradle) ──────────
+            // Rectangular boss on top of disc toward camera
+            translate([-CAM_W/2, HORN_PLATE_T - e, -CAM_H/2])
+                cube([CAM_W, HORN_PLATE_T + 4, CAM_H]);
+        }
+
+        // ── Servo horn spline bore (centre) ───────────────────────────────
+        translate([0, 0, -e])
+            cylinder(d = HORN_SPLINE_D, h = HORN_PLATE_T + 2*e);
+
+        // ── Horn bolt holes (4× M3 on HORN_BC_D) ─────────────────────────
+        for (a = [45, 135, 225, 315])
+            translate([HORN_BC_D/2 * cos(a),
+                       HORN_BC_D/2 * sin(a), -e])
+                cylinder(d = HORN_BOLT_D, h = HORN_PLATE_T + 2*e);
+
+        // ── Pivot axle bore (M4, coaxial with horn centre) ────────────────
+        translate([0, 0, -e])
+            cylinder(d = PIVOT_D, h = HORN_PLATE_T + 2*e);
+
+        // ── Cradle attachment bolts (2× M3 in arm boss) ──────────────────
+        for (cz = [-CAM_H/2 + 6, CAM_H/2 - 6])
+            translate([0, HORN_PLATE_T + 2, cz])
+                rotate([90, 0, 0])
+                    cylinder(d = M3_D, h = HORN_PLATE_T + 6 + 2*e);
+
+        // ── M3 hex nut pockets (rear of disc face) ────────────────────────
+        for (cz = [-CAM_H/2 + 6, CAM_H/2 - 6])
+            translate([0, M3_NUT_H + 0.5, cz])
+                rotate([90, 0, 0])
+                    cylinder(d = M3_NUT_AF / cos(30),
+                             h = M3_NUT_H + 0.5, $fn = 6);
+
+        // ── Weight-relief arcs (between horn bolt holes) ──────────────────
+        for (a = [0, 90, 180, 270])
+            translate([(plate_od/2 - 5) * cos(a),
+                       (plate_od/2 - 5) * sin(a), -e])
+                cylinder(d = 6, h = HORN_PLATE_T + 2*e);
+    }
+}
+
+// ============================================================
+// PART 4 — CAMERA CRADLE
+// ============================================================
+// Open-front U-cradle holding D435i via captured 1/4-20 hex nut.
+// Front lip retains camera from sliding forward (+Y).
+// Vibration-damping pads seat in recessed pockets on inner faces.
+// USB-C cable routing channel exits cradle right rear wall.
+//
+// 1/4-20 captured nut in cradle floor — tighten with standard
+// tripod screw or M6→1/4-20 adapter from camera bottom.
+//
+// Print: cradle-floor-down (flat), PETG, 5 perims, 40 % gyroid.
+// No supports needed (overhangs < 45°).
+module camera_cradle() {
+    outer_w = CAM_W + 2*CRADLE_WALL_T;
+    outer_h = CAM_H + CRADLE_FLOOR_T;
+
+    difference() {
+        union() {
+            // ── Cradle body ───────────────────────────────────────────────
+            translate([-outer_w/2, 0, 0])
+                cube([outer_w, CAM_D + CRADLE_WALL_T, outer_h]);
+
+            // ── Front retaining lip ───────────────────────────────────────
+            translate([-outer_w/2, CAM_D + CRADLE_WALL_T - CRADLE_LIP_T, 0])
+                cube([outer_w, CRADLE_LIP_T, CRADLE_LIP_H]);
+
+            // ── Cable channel boss (right rear, exits +X side) ────────────
+            translate([CAM_W/2 + CRADLE_WALL_T - e,
+                       0,
+                       CRADLE_FLOOR_T + CAM_H/2 - CABLE_CH_H/2])
+                cube([CABLE_CH_W + CRADLE_WALL_T, CAM_D * 0.6, CABLE_CH_H]);
+
+            // ── Tilt horn attachment tabs (left + right, bolt to horn plate)─
+            for (sx = [-outer_w/2 - 4, outer_w/2])
+                translate([sx, CAM_D/2, CRADLE_FLOOR_T + CAM_H/2 - 6])
+                    cube([4, 12, 12]);
+        }
+
+        // ── Camera pocket (hollow interior) ──────────────────────────────
+        translate([-CAM_W/2, 0, CRADLE_FLOOR_T])
+            cube([CAM_W, CAM_D + CRADLE_WALL_T + e, CAM_H + e]);
+
+        // ── 1/4-20 UNC clearance bore (camera tripod thread, bottom) ─────
+        translate([CAM_MOUNT_X, CAM_MOUNT_Y, -e])
+            cylinder(d = QTR20_D, h = CRADLE_FLOOR_T + 2*e);
+
+        // ── 1/4-20 hex nut pocket (captured in cradle floor) ─────────────
+        translate([CAM_MOUNT_X, CAM_MOUNT_Y, CRADLE_FLOOR_T - QTR20_NUT_H - 0.5])
+            cylinder(d = QTR20_NUT_AF / cos(30),
+                     h = QTR20_NUT_H + 0.6, $fn = 6);
+
+        // ── USB-C cable channel (exit through right rear wall) ────────────
+        translate([CAM_W/2 - e,
+                   0,
+                   CRADLE_FLOOR_T + CAM_H/2 - CABLE_CH_H/2])
+            cube([CABLE_CH_W + CRADLE_WALL_T + 2*e,
+                  CAM_D * 0.6 + e, CABLE_CH_H]);
+
+        // ── Vibe pad recesses on inner camera-contact faces ───────────────
+        // Rear wall recess (camera front face → +Y side of rear wall)
+        translate([-CAM_W/2 + CRADLE_WALL_T, CRADLE_WALL_T, CRADLE_FLOOR_T])
+            cube([CAM_W, PAD_T, CAM_H]);
+
+        // ── Tilt horn bolt holes in attachment tabs ───────────────────────
+        for (sx = [-outer_w/2 - 4 - e, outer_w/2 - e])
+            translate([sx, CAM_D/2 + 6, CRADLE_FLOOR_T + CAM_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = M3_D, h = 6 + 2*e);
+
+        // ── M3 nut pockets in attachment tabs ─────────────────────────────
+        translate([outer_w/2 + 4 - M3_NUT_H - 0.4,
+                   CAM_D/2 + 6,
+                   CRADLE_FLOOR_T + CAM_H/2])
+            rotate([0, 90, 0])
+                cylinder(d = M3_NUT_AF / cos(30),
+                         h = M3_NUT_H + 0.4, $fn = 6);
+        translate([-outer_w/2 - 4 - e,
+                   CAM_D/2 + 6,
+                   CRADLE_FLOOR_T + CAM_H/2])
+            rotate([0, 90, 0])
+                cylinder(d = M3_NUT_AF / cos(30),
+                         h = M3_NUT_H + 0.4, $fn = 6);
+
+        // ── Lightening pockets in cradle walls ────────────────────────────
+        for (face_x = [-CAM_W/2 - CRADLE_WALL_T - e, CAM_W/2 - e])
+            translate([face_x, CAM_D * 0.2, CRADLE_FLOOR_T + 3])
+                cube([CRADLE_WALL_T + 2*e, CAM_D * 0.55, CAM_H - 6]);
+    }
+}
+
+// ============================================================
+// PART 5 — VIBRATION-DAMPING PAD
+// ============================================================
+// Flat pad with transverse PETG flexure ribs pressing against camera body.
+// Rib geometry (thin fins ~1.5 mm tall) deflects under camera vibration,
+// attenuating high-frequency input from motor/drive-train.
+// For superior damping: print in TPU 95A (no infill changes needed).
+// Pads seat in recessed pockets in camera cradle inner wall.
+// Optional M2 bolt-through at corners or adhesive-back foam tape.
+//
+// Print: pad-back-face-down, PETG or TPU 95A, 3 perims, 20 % infill.
+module vibe_pad() {
+    rib_count = floor((PAD_W - RIB_W) / RIB_PITCH);
+
+    union() {
+        // ── Base plate ────────────────────────────────────────────────────
+        translate([-PAD_W/2, -PAD_T, -PAD_H/2])
+            cube([PAD_W, PAD_T, PAD_H]);
+
+        // ── Flexure ribs (parallel to Z, spaced RIB_PITCH apart) ─────────
+        for (i = [0 : rib_count - 1]) {
+            rx = -PAD_W/2 + RIB_PITCH/2 + i * RIB_PITCH + RIB_W/2;
+            if (rx <= PAD_W/2 - RIB_W/2)
+                translate([rx, 0, 0])
+                    cube([RIB_W, RIB_H, PAD_H - 6], center = true);
+        }
+
+        // ── Corner nubs (M2 bolt-through retention, optional) ─────────────
+        for (px = [-PAD_W/2 + 5, PAD_W/2 - 5])
+        for (pz = [-PAD_H/2 + 5, PAD_H/2 - 5])
+            translate([px, -PAD_T/2, pz])
+                difference() {
+                    cylinder(d = 5, h = PAD_T, center = true);
+                    cylinder(d = 2.4, h = PAD_T + 2*e, center = true);
+                }
+    }
+}

chassis/rplidar_mount.scad

@@ -1,76 +1,502 @@
 // ============================================================
-// rplidar_mount.scad — RPLIDAR A1M8 Anti-Vibration Ring  Rev A
-// Agent: sl-mechanical   2026-02-28
+// rplidar_mount.scad — RPLIDAR A1 Elevated Bracket for 2020 T-Slot Rail
+// Issue: #561  Agent: sl-mechanical  Date: 2026-03-14
+// (supersedes Rev A anti-vibration ring — ring integrated as Part 4)
 // ============================================================
-// Flat ring sits between platform and RPLIDAR A1M8.
-// Anti-vibration isolation via 4× M3 silicone grommets
-// (same type as FC vibration mounts — Ø6 mm silicone, M3).
 //
-// Bolt stack (bottom → top):
-//   M3×30 SHCS → platform (8 mm) → grommet (8 mm) →
-//   ring (4 mm) → RPLIDAR bottom (threaded M3, ~6 mm engagement)
+// Complete elevated mount system for RPLIDAR A1 on 2020 aluminium T-slot
+// rail.  Scanner raised ELEV_H mm above rail attachment point so the
+// 360° laser scan plane clears the rover/tank chassis body.
 //
-// RENDER options:
-//   "ring"       print-ready flat ring (default)
-//   "assembly"   ring in position on platform stub
+// Architecture:
+//   T-nut base  → clamps to 2020 rail (standard thumbscrew interface)
+//   Column      → parametric-height hollow mast; USB cable routed inside
+//   Platform    → disc receives RPLIDAR via 4× M3 on Ø40 mm bolt circle
+//   Vibe ring   → anti-vibration isolation ring with silicone grommet seats
+//   Cable guide → snap-on clips along column for USB cable management
+//
+// Part catalogue:
+//   Part 1 — tnut_base()      2020 T-nut rail base + column stub socket
+//   Part 2 — column()         Hollow elevation mast (parametric ELEV_H)
+//   Part 3 — scan_platform()  RPLIDAR mounting disc + motor connector slot
+//   Part 4 — vibe_ring()      Anti-vibration isolation ring (grommet seats)
+//   Part 5 — cable_guide()    Snap-on cable management clip for column
+//   Part 6 — assembly_preview()
+//
+// Hardware BOM (per mount):
+//   1× M3 × 16 mm SHCS + M3 hex nut   rail clamp thumbscrew
+//   4× M3 × 30 mm SHCS                RPLIDAR → vibe_ring → platform
+//   4× M3 silicone grommets (Ø6 mm)   anti-vibration isolators
+//   4× M3 hex nuts                    captured in platform underside
+//   2× M4 × 12 mm SHCS                column → base socket bolts
+//   2× M4 hex nuts                    captured in base socket
+//   1× USB-A cable (RPLIDAR → Jetson) routed through column bore
+//
+// RPLIDAR A1 interface (caliper-verified Slamtec RPLIDAR A1):
+//   Body diameter     : Ø70 mm
+//   Bolt circle       : Ø40 mm, 4× M3, at 45°/135°/225°/315°
+//   USB connector     : micro-USB, right-rear quadrant, exits at 0° (front)
+//   Motor connector   : JST 2-pin, rear centreline
+//   Scan plane height : 19 mm above bolt mounting face
+//   Min clearance     : Ø80 mm cylinder around body for 360° scan
+//
+// Parametric constants (override for variants):
+//   ELEV_H     — scan elevation above rail face (default 120 mm)
+//   COL_OD     — column outer diameter (default 25 mm)
+//   RAIL choice — RAIL_W = 20 for 2020, = 40 for 4040 extrusion
+//
+// Print settings:
+//   Material   : PETG (all parts); vibe_ring optionally in TPU 95A
+//   Perimeters : 5 (tnut_base, column, platform), 3 (vibe_ring, cable_guide)
+//   Infill     : 40 % gyroid (structural), 20 % (vibe_ring, guide)
+//   Orientation:
+//     tnut_base    — face-plate flat on bed (no supports)
+//     column       — standing upright (no supports; hollow bore bridgeable)
+//     scan_platform — disc face down (no supports)
+//     vibe_ring    — flat on bed (no supports)
+//     cable_guide  — clip-open face down (no supports)
+//
+// Export commands:
+//   openscad rplidar_mount.scad -D 'RENDER="tnut_base_stl"'    -o rpm_tnut_base.stl
+//   openscad rplidar_mount.scad -D 'RENDER="column_stl"'       -o rpm_column.stl
+//   openscad rplidar_mount.scad -D 'RENDER="platform_stl"'     -o rpm_platform.stl
+//   openscad rplidar_mount.scad -D 'RENDER="vibe_ring_stl"'    -o rpm_vibe_ring.stl
+//   openscad rplidar_mount.scad -D 'RENDER="cable_guide_stl"'  -o rpm_cable_guide.stl
 // ============================================================
 
-RENDER = "ring";
-
-// ── RPLIDAR A1M8 ─────────────────────────────────────────────
-RPL_BODY_D  = 70.0;          // body diameter
-RPL_BC      = 58.0;          // M3 mounting bolt circle
-
-// ── Mount ring ───────────────────────────────────────────────
-RING_OD     = 82.0;          // outer diameter (RPL_BODY_D + 12 mm)
-RING_ID     = 30.0;          // inner cutout (cable / airflow)
-RING_H      =  4.0;          // ring thickness
-
-BOLT_D      =  3.3;          // M3 clearance through-hole
-GROMMET_D   =  7.0;          // silicone grommet OD (seat recess on bottom)
-GROMMET_H   =  1.0;          // seating recess depth
-
 $fn = 64;
 e   = 0.01;
 
-// ─────────────────────────────────────────────────────────────
-module rplidar_ring() {
+// ── Parametric elevation ──────────────────────────────────────────────────────
+ELEV_H     = 120.0;   // scan plane elevation above rail face (mm)
+                      // increase for taller chassis; min ~60 mm recommended
+
+// ── RPLIDAR A1 interface constants ───────────────────────────────────────────
+RPL_BODY_D = 70.0;    // scanner body outer diameter
+RPL_BC_D   = 40.0;    // mounting bolt circle diameter (4× M3 at 45° offsets)
+RPL_BOLT_D =  3.3;    // M3 clearance bore
+RPL_SCAN_Z = 19.0;    // scan plane height above mount face
+RPL_CLEAR_D = 82.0;   // minimum radial clearance diameter for 360° scan
+
+// ── Rail geometry (matches sensor_rail.scad) ─────────────────────────────────
+RAIL_W       = 20.0;
+SLOT_OPEN    =  6.0;
+SLOT_INNER_W = 10.2;
+SLOT_INNER_H =  5.8;
+SLOT_NECK_H  =  3.2;
+
+// ── T-nut geometry (matches sensor_rail_brackets.scad) ───────────────────────
+TNUT_W         =  9.8;
+TNUT_H         =  5.5;
+TNUT_L         = 12.0;
+TNUT_M3_NUT_AF =  5.5;
+TNUT_M3_NUT_H  =  2.5;
+TNUT_BOLT_D    =  3.3;
+
+// ── Base plate geometry ───────────────────────────────────────────────────────
+BASE_FACE_W = 38.0;   // wider than rail, provides column socket footprint
+BASE_FACE_H = 38.0;   // height along rail Z
+BASE_FACE_T = SLOT_NECK_H + 2.0;   // plate depth (Y)
+
+// ── Column geometry ───────────────────────────────────────────────────────────
+COL_OD      = 25.0;   // column outer diameter
+COL_ID      = 17.0;   // column inner bore (cable routing + weight saving)
+COL_SOCKET_D = COL_OD + 6.0;   // socket boss OD (column inserts into base)
+COL_SOCKET_L = 14.0;  // socket depth in base (14 mm engagement)
+COL_BOLT_BC  = COL_OD + 4.0;   // M4 column-lock bolt span (centre-to-centre)
+COL_SLOT_W   =  5.0;  // cable exit slot width in column base
+COL_SLOT_H   =  8.0;  // cable exit slot height
+
+// ── Platform geometry ─────────────────────────────────────────────────────────
+PLAT_OD      = RPL_CLEAR_D + 4.0;   // platform disc OD (covers scan clear zone)
+PLAT_T       =  5.0;   // platform disc thickness
+PLAT_SOCKET_D = COL_OD + 0.3;       // column-top socket ID (slip fit)
+PLAT_SOCKET_L =  12.0; // socket depth on platform underside
+PLAT_RIM_T   =  3.5;   // rim wall thickness around RPLIDAR body
+
+// ── Anti-vibration ring geometry ─────────────────────────────────────────────
+RING_OD      = RPL_BODY_D + 12.0;   // 82 mm (body + 6 mm rim)
+RING_ID      = 28.0;   // central bore (connector/cable access)
+RING_H       =  4.0;   // ring thickness
+GROMMET_D    =  7.0;   // silicone grommet OD pocket
+GROMMET_RECESS =  1.5; // grommet seating recess depth (bottom face)
+
+// ── Cable guide clip geometry ─────────────────────────────────────────────────
+GUIDE_CABLE_D =  6.0;  // max cable OD (USB-A cable)
+GUIDE_T       =  2.0;  // clip wall thickness
+GUIDE_BODY_W  = 20.0;  // clip body width
+GUIDE_BODY_H  = 12.0;  // clip body height
+
+// ── Fastener sizes ────────────────────────────────────────────────────────────
+M3_D = 3.3;
+M4_D = 4.3;
+M3_NUT_AF = 5.5;
+M3_NUT_H  = 2.4;
+M4_NUT_AF = 7.0;
+M4_NUT_H  = 3.2;
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
+RENDER = "assembly";
+
+if      (RENDER == "assembly")       assembly_preview();
+else if (RENDER == "tnut_base_stl")  tnut_base();
+else if (RENDER == "column_stl")     column();
+else if (RENDER == "platform_stl")   scan_platform();
+else if (RENDER == "vibe_ring_stl")  vibe_ring();
+else if (RENDER == "cable_guide_stl") cable_guide();
+
+// ============================================================
+// ASSEMBLY PREVIEW
+// ============================================================
+module assembly_preview() {
+    // Ghost 2020 rail section (250 mm)
+    %color("Silver", 0.28)
+        translate([-RAIL_W/2, -RAIL_W/2, 0])
+            cube([RAIL_W, RAIL_W, 250]);
+
+    // T-nut base at Z=60 on rail
+    color("OliveDrab", 0.85)
+        translate([0, 0, 60])
+            tnut_base();
+
+    // Column rising from base
+    color("SteelBlue", 0.85)
+        translate([0, BASE_FACE_T + COL_OD/2, 60 + BASE_FACE_H/2])
+            column();
+
+    // Vibe ring on top of platform
+    color("Teal", 0.85)
+        translate([0, BASE_FACE_T + COL_OD/2,
+                   60 + BASE_FACE_H/2 + ELEV_H + PLAT_T])
+            vibe_ring();
+
+    // Scan platform at column top
+    color("DarkSlateGray", 0.85)
+        translate([0, BASE_FACE_T + COL_OD/2,
+                   60 + BASE_FACE_H/2 + ELEV_H])
+            scan_platform();
+
+    // RPLIDAR body ghost
+    %color("Black", 0.35)
+        translate([0, BASE_FACE_T + COL_OD/2,
+                   60 + BASE_FACE_H/2 + ELEV_H + PLAT_T + RING_H + 1])
+            cylinder(d = RPL_BODY_D, h = 30);
+
+    // Cable guides at 30 mm intervals along column
+    for (gz = [20, 50, 80])
+        color("DimGray", 0.75)
+            translate([COL_OD/2,
+                       BASE_FACE_T + COL_OD/2,
+                       60 + BASE_FACE_H/2 + gz])
+                rotate([0, -90, 0])
+                    cable_guide();
+}
+
+// ============================================================
+// PART 1 — T-NUT RAIL BASE
+// ============================================================
+// Standard 2020 rail T-nut attachment, matching interface used across
+// all SaltyLab sensor brackets (sensor_rail_brackets.scad convention).
+// Column socket boss on front face (+Y) receives column bottom.
+// Column locked with 2× M4 cross-bolts through socket boss.
+//
+// Cable exit slot at base of socket directs RPLIDAR USB cable
+// downward and rearward toward Jetson USB port.
+//
+// Print: face-plate flat on bed, PETG, 5 perims, 50 % gyroid.
+module tnut_base() {
     difference() {
-        cylinder(d = RING_OD, h = RING_H);
+        union() {
+            // ── Face plate (flush against rail outer face, -Y) ───────────
+            translate([-BASE_FACE_W/2, -BASE_FACE_T, 0])
+                cube([BASE_FACE_W, BASE_FACE_T, BASE_FACE_H]);
 
-        // Central cutout
-        translate([0, 0, -e])
-            cylinder(d = RING_ID, h = RING_H + 2*e);
+            // ── T-nut neck (enters rail slot) ────────────────────────────
+            translate([-TNUT_W/2, 0, (BASE_FACE_H - TNUT_L)/2])
+                cube([TNUT_W, SLOT_NECK_H + e, TNUT_L]);
 
-        // 4× M3 clearance holes on bolt circle
-        for (a = [45, 135, 225, 315]) {
-            translate([RPL_BC/2 * cos(a), RPL_BC/2 * sin(a), -e])
-                cylinder(d = BOLT_D, h = RING_H + 2*e);
+            // ── T-nut body (wider, locks in T-groove) ────────────────────
+            translate([-TNUT_W/2, SLOT_NECK_H - e, (BASE_FACE_H - TNUT_L)/2])
+                cube([TNUT_W, TNUT_H - SLOT_NECK_H + e, TNUT_L]);
+
+            // ── Column socket boss (front face, centred) ─────────────────
+            translate([0, -BASE_FACE_T, BASE_FACE_H/2])
+                rotate([-90, 0, 0])
+                    cylinder(d = COL_SOCKET_D, h = BASE_FACE_T + COL_SOCKET_L);
         }
 
-        // Grommet seating recesses — bottom face
-        for (a = [45, 135, 225, 315]) {
-            translate([RPL_BC/2 * cos(a), RPL_BC/2 * sin(a), -e])
-                cylinder(d = GROMMET_D, h = GROMMET_H + e);
+        // ── Rail clamp bolt bore (M3, centre of face plate) ──────────────
+        translate([0, -BASE_FACE_T - e, BASE_FACE_H/2])
+            rotate([-90, 0, 0])
+                cylinder(d = TNUT_BOLT_D, h = BASE_FACE_T + TNUT_H + 2*e);
+
+        // ── M3 hex nut pocket (inside T-nut body) ────────────────────────
+        translate([0, SLOT_NECK_H + 0.3, BASE_FACE_H/2])
+            rotate([-90, 0, 0])
+                cylinder(d = TNUT_M3_NUT_AF / cos(30),
+                         h = TNUT_M3_NUT_H + 0.3, $fn = 6);
+
+        // ── Column socket bore (column inserts from +Y side) ─────────────
+        translate([0, -BASE_FACE_T, BASE_FACE_H/2])
+            rotate([-90, 0, 0])
+                cylinder(d = COL_OD + 0.3, h = BASE_FACE_T + COL_SOCKET_L + e);
+
+        // ── Column lock bolt bores (2× M4, horizontal through socket boss) ─
+        // One bolt from +X, one from -X, on COL_SOCKET_L/2 depth
+        for (lx = [-1, 1])
+            translate([lx * (COL_SOCKET_D/2 + e), COL_SOCKET_L/2, BASE_FACE_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = M4_D, h = COL_SOCKET_D + 2*e,
+                             center = true);
+
+        // ── M4 nut pockets (one side of socket boss for each bolt) ────────
+        for (lx = [-1, 1])
+            translate([lx * (COL_SOCKET_D/2 - M4_NUT_H - 1),
+                       COL_SOCKET_L/2,
+                       BASE_FACE_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = M4_NUT_AF / cos(30),
+                             h = M4_NUT_H + 0.5, $fn = 6);
+
+        // ── Cable exit slot (bottom of socket, cable exits downward) ──────
+        translate([0, COL_SOCKET_L * 0.6, BASE_FACE_H/2 - COL_SOCKET_D/2])
+            cube([COL_SLOT_W, COL_SOCKET_D + e, COL_SLOT_H], center = [true, false, false]);
+
+        // ── Lightening pockets in face plate ─────────────────────────────
+        translate([0, -BASE_FACE_T/2, BASE_FACE_H/2])
+            cube([BASE_FACE_W - 12, BASE_FACE_T - 2, BASE_FACE_H - 16],
+                 center = true);
+    }
+}
+
+// ============================================================
+// PART 2 — ELEVATION COLUMN
+// ============================================================
+// Hollow cylindrical mast (ELEV_H tall) raising the RPLIDAR scan
+// plane above the chassis body for unobstructed 360° coverage.
+// Inner bore routes USB cable from scanner to base exit slot.
+// Bottom peg inserts into tnut_base socket; top peg inserts into
+// scan_platform socket.  Both ends are plain Ø(COL_OD) cylinders,
+// interference-free slip fit into Ø(COL_OD+0.3) sockets.
+//
+// Three longitudinal ribs on outer surface add torsional stiffness
+// without added diameter.  Cable slot on one rib for cable retention.
+//
+// Print: standing upright, PETG, 5 perims, 20 % gyroid (hollow).
+module column() {
+    rib_w = 3.0;
+    rib_h = 2.0;   // rib protrusion from column OD
+
+    difference() {
+        union() {
+            // ── Hollow cylinder ───────────────────────────────────────────
+            cylinder(d = COL_OD, h = ELEV_H + COL_SOCKET_L);
+
+            // ── Three stiffening ribs (120° apart) ────────────────────────
+            for (ra = [0, 120, 240])
+                rotate([0, 0, ra])
+                    translate([COL_OD/2 - e, -rib_w/2, 0])
+                        cube([rib_h + e, rib_w, ELEV_H + COL_SOCKET_L]);
+        }
+
+        // ── Central cable bore (full length) ─────────────────────────────
+        translate([0, 0, -e])
+            cylinder(d = COL_ID, h = ELEV_H + COL_SOCKET_L + 2*e);
+
+        // ── Cable entry slot at column base (aligns with base exit slot) ──
+        translate([-COL_SLOT_W/2, COL_OD/2 - e, -e])
+            cube([COL_SLOT_W, COL_ID/2 + rib_h + 2, COL_SLOT_H + 2]);
+
+        // ── Cable exit slot at column top (USB exits to scanner) ──────────
+        translate([-COL_SLOT_W/2, COL_OD/2 - e,
+                   ELEV_H + COL_SOCKET_L - COL_SLOT_H - 2])
+            cube([COL_SLOT_W, COL_ID/2 + rib_h + 2, COL_SLOT_H + 2]);
+
+        // ── Column lock flat (prevents rotation in socket) ────────────────
+        // Two opposed flats at column base & top socket peg
+        for (peg_z = [0, ELEV_H]) {
+            translate([-COL_OD/2 - e, COL_OD/2 - 2.0, peg_z])
+                cube([COL_OD + 2*e, 2.5, COL_SOCKET_L]);
         }
     }
 }
 
-// ─────────────────────────────────────────────────────────────
-// Render selector
-// ─────────────────────────────────────────────────────────────
-if (RENDER == "ring") {
-    rplidar_ring();
+// ============================================================
+// PART 3 — SCAN PLATFORM
+// ============================================================
+// Disc that RPLIDAR A1 mounts to.  Matches RPLIDAR A1 bolt pattern:
+// 4× M3 on Ø40 mm bolt circle at 45°/135°/225°/315°.
+// M3 hex nuts captured in underside pockets (blind, tool-free install).
+// Column-top socket on underside receives column top peg (Ø25 slip fit).
+// Motor connector slot on rear edge for JST cable exit.
+// Vibe ring sits on top face between platform and RPLIDAR (separate part).
+//
+// Scan plane (19 mm above mount face) clears platform top by design;
+// minimum platform OD = RPL_CLEAR_D (82 mm) leaves scan plane open.
+//
+// Print: disc face down, PETG, 5 perims, 40 % gyroid.
+module scan_platform() {
+    difference() {
+        union() {
+            // ── Platform disc ─────────────────────────────────────────────
+            cylinder(d = PLAT_OD, h = PLAT_T);
 
-} else if (RENDER == "assembly") {
-    // Platform stub
-    color("Silver", 0.5)
-        difference() {
-            cylinder(d = 90, h = 8);
-            translate([0, 0, -e]) cylinder(d = 25.4, h = 8 + 2*e);
+            // ── Column socket boss (underside, -Z) ────────────────────────
+            translate([0, 0, -PLAT_SOCKET_L])
+                cylinder(d = COL_SOCKET_D, h = PLAT_SOCKET_L + e);
         }
-    // Ring floating 8 mm above (grommet gap)
-    color("SkyBlue", 0.9)
-        translate([0, 0, 8 + 8])
-            rplidar_ring();
+
+        // ── Column socket bore (column top peg inserts from below) ────────
+        translate([0, 0, -PLAT_SOCKET_L - e])
+            cylinder(d = PLAT_SOCKET_D, h = PLAT_SOCKET_L + e + 1);
+
+        // ── Column lock bores (2× M4 through socket boss) ─────────────────
+        for (lx = [-1, 1])
+            translate([lx * (COL_SOCKET_D/2 + e), 0, -PLAT_SOCKET_L/2])
+                rotate([0, 90, 0])
+                    cylinder(d = M4_D, h = COL_SOCKET_D + 2*e, center = true);
+
+        // ── M4 nut pockets (one side socket boss) ─────────────────────────
+        translate([COL_SOCKET_D/2 - M4_NUT_H - 1, 0, -PLAT_SOCKET_L/2])
+            rotate([0, 90, 0])
+                cylinder(d = M4_NUT_AF / cos(30), h = M4_NUT_H + 0.5,
+                         $fn = 6);
+
+        // ── 4× RPLIDAR mounting bolt holes (M3, Ø40 mm BC at 45°) ────────
+        for (a = [45, 135, 225, 315])
+            translate([RPL_BC_D/2 * cos(a),
+                       RPL_BC_D/2 * sin(a), -e])
+                cylinder(d = RPL_BOLT_D, h = PLAT_T + 2*e);
+
+        // ── M3 hex nut pockets on underside (captured, tool-free) ─────────
+        for (a = [45, 135, 225, 315])
+            translate([RPL_BC_D/2 * cos(a),
+                       RPL_BC_D/2 * sin(a), -e])
+                cylinder(d = M3_NUT_AF / cos(30),
+                         h = M3_NUT_H + 0.5, $fn = 6);
+
+        // ── Motor connector slot (JST rear centreline, 10×6 mm) ──────────
+        translate([0, PLAT_OD/2 - 8, -e])
+            cube([10, 10, PLAT_T + 2*e], center = [true, false, false]);
+
+        // ── USB connector slot (micro-USB, right-rear, 12×6 mm) ──────────
+        translate([PLAT_OD/4, PLAT_OD/2 - 8, -e])
+            cube([12, 10, PLAT_T + 2*e], center = [true, false, false]);
+
+        // ── Lightening pockets (between bolt holes) ────────────────────────
+        for (a = [0, 90, 180, 270])
+            translate([(RPL_BC_D/2 + 10) * cos(a),
+                       (RPL_BC_D/2 + 10) * sin(a), -e])
+                cylinder(d = 8, h = PLAT_T + 2*e);
+
+        // ── Central cable bore (USB from scanner routes down column) ──────
+        translate([0, 0, -e])
+            cylinder(d = COL_ID - 2, h = PLAT_T + 2*e);
+    }
+}
+
+// ============================================================
+// PART 4 — VIBRATION ISOLATION RING
+// ============================================================
+// Flat ring sits between scan_platform top face and RPLIDAR bottom.
+// Anti-vibration isolation via 4× M3 silicone FC-style grommets
+// (Ø6 mm silicone, M3 bore — same type used on flight controllers).
+//
+// Bolt stack (bottom → top):
+//   M3 × 30 SHCS → platform (countersunk) → grommet (Ø7 seat) →
+//   ring (4 mm) → RPLIDAR threaded boss (~6 mm engagement)
+//
+// Grommet seats are recessed 1.5 mm into ring bottom face so grommets
+// are captured and self-locating.  Ring top face is flat for RPLIDAR.
+//
+// Print: flat on bed, PETG or TPU 95A, 3 perims, 20 % infill.
+// TPU 95A provides additional compliance in axial direction.
+module vibe_ring() {
+    difference() {
+        // ── Ring body ────────────────────────────────────────────────────
+        cylinder(d = RING_OD, h = RING_H);
+
+        // ── Central bore (cable / connector access) ───────────────────────
+        translate([0, 0, -e])
+            cylinder(d = RING_ID, h = RING_H + 2*e);
+
+        // ── 4× M3 clearance bores on Ø40 mm bolt circle ───────────────────
+        for (a = [45, 135, 225, 315])
+            translate([RPL_BC_D/2 * cos(a),
+                       RPL_BC_D/2 * sin(a), -e])
+                cylinder(d = RPL_BOLT_D, h = RING_H + 2*e);
+
+        // ── Grommet seating recesses (bottom face, Ø7 mm × 1.5 mm deep) ──
+        for (a = [45, 135, 225, 315])
+            translate([RPL_BC_D/2 * cos(a),
+                       RPL_BC_D/2 * sin(a), -e])
+                cylinder(d = GROMMET_D, h = GROMMET_RECESS + e);
+
+        // ── Motor connector notch (rear centreline, passes through ring) ──
+        translate([0, RING_OD/2 - 6, -e])
+            cube([10, 8, RING_H + 2*e], center = [true, false, false]);
+
+        // ── Lightening arcs ───────────────────────────────────────────────
+        for (a = [0, 90, 180, 270])
+            translate([(RPL_BC_D/2 + 9) * cos(a),
+                       (RPL_BC_D/2 + 9) * sin(a), -e])
+                cylinder(d = 7, h = RING_H + 2*e);
+    }
+}
+
+// ============================================================
+// PART 5 — CABLE GUIDE CLIP
+// ============================================================
+// Snap-on C-clip that presses onto column ribs to retain USB cable
+// along column exterior.  Cable sits in a semicircular channel;
+// snap tongue grips the rib.  No fasteners — push-fit on rib.
+// Print multiples: one every ~30 mm along column for clean routing.
+//
+// Print: clip-opening face down, PETG, 3 perims, 20 % infill.
+// Orientation matters — clip opening (-Y face) must face down for bridging.
+module cable_guide() {
+    snap_t   = 1.8;    // snap tongue thickness (springy PETG)
+    snap_oc  = GUIDE_CABLE_D + 2*GUIDE_T;   // channel outer cylinder OD
+    body_h   = GUIDE_BODY_H;
+
+    difference() {
+        union() {
+            // ── Clip body (flat plate on column face) ─────────────────────
+            translate([-GUIDE_BODY_W/2, 0, 0])
+                cube([GUIDE_BODY_W, GUIDE_T, body_h]);
+
+            // ── Cable channel (C-shape, opens toward +Y) ──────────────────
+            translate([0, GUIDE_T + snap_oc/2, body_h/2])
+                rotate([0, 90, 0])
+                    difference() {
+                        cylinder(d = snap_oc, h = GUIDE_BODY_W,
+                                 center = true);
+                        cylinder(d = GUIDE_CABLE_D, h = GUIDE_BODY_W + 2*e,
+                                 center = true);
+                        // Open front slot for cable insertion
+                        translate([0, snap_oc/2 + e, 0])
+                            cube([GUIDE_CABLE_D * 0.85,
+                                  snap_oc + 2*e,
+                                  GUIDE_BODY_W + 2*e], center = true);
+                    }
+
+            // ── Snap-fit tongue (grips column rib, -Y side of body) ───────
+            // Two flexible tabs that straddle column rib
+            for (tx = [-GUIDE_BODY_W/2 + 2, GUIDE_BODY_W/2 - 2 - snap_t])
+                translate([tx, -4, 0])
+                    cube([snap_t, 4 + GUIDE_T, body_h]);
+
+            // Snap barbs (slight overhang engages rib back edge)
+            for (tx = [-GUIDE_BODY_W/2 + 2, GUIDE_BODY_W/2 - 2 - snap_t])
+                translate([tx + snap_t/2, -4, body_h/2])
+                    rotate([0, 90, 0])
+                        cylinder(d = 2, h = snap_t, center = true);
+        }
+
+        // ── Rib slot (column rib passes through clip body) ─────────────────
+        translate([0, -2, body_h/2])
+            cube([3.5, GUIDE_T + 4 + e, body_h - 4], center = true);
+    }
 }

chassis/uwb_anchor_mount.scad

@@ -1,275 +1,463 @@
 // ============================================================
-// uwb_anchor_mount.scad — Stem-Mounted UWB Anchor  Rev A
-// Agent: sl-mechanical   2026-03-01
-// Closes issues #57, #62
+// uwb_anchor_mount.scad — Wall/Ceiling UWB Anchor Mount Bracket
+// Issue: #564  Agent: sl-mechanical  Date: 2026-03-14
+// (supersedes Rev A stem-collar mount — see git history)
 // ============================================================
-// Clamp-on bracket for 2× MaUWB ESP32-S3 anchor modules on
-// SaltyBot 25 mm OD vertical stem.
-// Anchors spaced ANCHOR_SPACING = 250 mm apart.
 //
-// Features:
-//   • Split D-collar with M4 clamping bolts + M4 set screw
-//   • Anti-rotation flat tab that keys against a small pin
-//     OR printed key tab that registers on the stem flat (if stem
-//     has a ground flat) — see ANTI_ROT_MODE parameter
-//   • Module bracket: faces outward, tilted 10° from vertical
-//     so antenna clears stem and faces horizon
-//   • USB cable channel (power from Orin via USB-A) on collar
-//   • Tool-free capture: M4 thumbscrews (slot-head, hand-tighten)
-//   • UWB antenna area: NO material within 10 mm of PCB top face
+// Parametric wall or ceiling mount bracket for ESP32 UWB Pro anchor.
+// Designed for fixed-infrastructure deployment: anchors screw into
+// wall or ceiling drywall/timber with standard M4 or #6 wood screws,
+// at a user-defined tilt angle so the UWB antenna faces the desired
+// coverage zone.
 //
-// Components per mount:
-//   2× collar_half        print in PLA/PETG, flat-face-down
-//   1× module_bracket     print in PLA/PETG, flat-face-down
+// Architecture:
+//   Wall base    → flat backplate with 2× screw holes (wall or ceiling)
+//   Tilt knuckle → single-axis articulating joint; 15° detent steps
+//                  locked with M3 nyloc bolt; range 0–90°
+//   Anchor cradle→ U-cradle holding ESP32 UWB Pro PCB on M2.5 standoffs
+//   USB-C channel→ routed groove on tilt arm + exit slot in cradle back wall
+//   Label slot   → rear window slot for printed anchor-ID card strip
+//
+// Part catalogue:
+//   Part 1 — wall_base()       Backplate + 2-ear pivot block + detent arc
+//   Part 2 — tilt_arm()        Pivoting arm with knuckle + cradle stub
+//   Part 3 — anchor_cradle()   PCB cradle, standoffs, USB-C slot, label window
+//   Part 4 — cable_clip()      Snap-on USB-C cable guide for tilt arm
+//   Part 5 — assembly_preview()
+//
+// Hardware BOM:
+//   2× M4 × 30 mm wood screws (or #6 drywall screws)  wall fasteners
+//   1× M3 × 20 mm SHCS + M3 nyloc nut                 tilt pivot bolt
+//   4× M2.5 × 8 mm SHCS                               PCB-to-cradle
+//   4× M2.5 hex nuts                                   captured in standoffs
+//   1× USB-C cable                                     anchor power
+//
+// ESP32 UWB Pro interface (verify with calipers):
+//   PCB size       : UWB_L × UWB_W × UWB_H  (55 × 28 × 10 mm default)
+//   Mounting holes : M2.5, 4× corners on UWB_HOLE_X × UWB_HOLE_Y pattern
+//   USB-C port     : centred on short edge, UWB_USBC_W × UWB_USBC_H
+//   Antenna area   : top face rear half — 10 mm keep-out of bracket material
+//
+// Tilt angles (15° detent steps, set TILT_DEG before export):
+//   0°  → horizontal face-up   (ceiling, antenna faces down)
+//   30° → 30° downward         (wall near ceiling)  [default]
+//   45° → diagonal             (wall mid-height)
+//   90° → vertical face-out    (wall, antenna faces forward)
 //
 // RENDER options:
-//   "assembly"       single mount assembled (default)
-//   "collar_front"   front collar half for slicing (×2 per mount × 2 mounts = 4)
-//   "collar_rear"    rear collar half
-//   "bracket"        module bracket (×2 mounts)
-//   "pair"           both mounts on 350 mm stem section
+//   "assembly"           full assembly at TILT_DEG (default)
+//   "wall_base_stl"      Part 1
+//   "tilt_arm_stl"       Part 2
+//   "anchor_cradle_stl"  Part 3
+//   "cable_clip_stl"     Part 4
+//
+// Export commands:
+//   openscad uwb_anchor_mount.scad -D 'RENDER="wall_base_stl"'     -o uwb_wall_base.stl
+//   openscad uwb_anchor_mount.scad -D 'RENDER="tilt_arm_stl"'      -o uwb_tilt_arm.stl
+//   openscad uwb_anchor_mount.scad -D 'RENDER="anchor_cradle_stl"' -o uwb_anchor_cradle.stl
+//   openscad uwb_anchor_mount.scad -D 'RENDER="cable_clip_stl"'    -o uwb_cable_clip.stl
 // ============================================================
 
+$fn  = 64;
+e    = 0.01;
+
+// ── Tilt angle (override per anchor, 0–90°, 15° steps) ───────────────────────
+TILT_DEG = 30;
+
+// ── ESP32 UWB Pro PCB dimensions (verify with calipers) ──────────────────────
+UWB_L         = 55.0;   // PCB length
+UWB_W         = 28.0;   // PCB width
+UWB_H         = 10.0;   // PCB + components height
+UWB_HOLE_X    = 47.5;   // M2.5 hole X span
+UWB_HOLE_Y    = 21.0;   // M2.5 hole Y span
+UWB_USBC_W    =  9.5;   // USB-C receptacle width
+UWB_USBC_H    =  4.0;   // USB-C receptacle height
+UWB_ANTENNA_L = 20.0;   // antenna area at PCB rear (keep-out)
+
+// ── Wall base geometry ────────────────────────────────────────────────────────
+BASE_W        = 60.0;
+BASE_H        = 50.0;
+BASE_T        =  5.0;
+BASE_SCREW_D  =  4.5;   // M4 clearance
+BASE_SCREW_HD =  8.5;   // countersink OD
+BASE_SCREW_HH =  3.5;   // countersink depth
+BASE_SCREW_SPC = 35.0;  // Z span between screw holes
+KNUCKLE_T     = BASE_T + 4.0;   // pivot ear depth (Y)
+
+// ── Tilt arm geometry ─────────────────────────────────────────────────────────
+ARM_W         = 12.0;
+ARM_T         =  5.0;
+ARM_L         = 35.0;
+PIVOT_D       =  3.3;   // M3 clearance
+PIVOT_NUT_AF  =  5.5;
+PIVOT_NUT_H   =  2.4;
+DETENT_D      =  3.2;   // detent notch diameter
+DETENT_R      =  8.0;   // detent notch radius from pivot
+
+// ── Anchor cradle geometry ────────────────────────────────────────────────────
+CRADLE_WALL_T  =  3.5;
+CRADLE_BACK_T  =  4.0;
+CRADLE_FLOOR_T =  3.0;
+CRADLE_LIP_H   =  4.0;
+CRADLE_LIP_T   =  2.5;
+STANDOFF_H     =  3.0;
+STANDOFF_OD    =  5.5;
+LABEL_W        = UWB_L - 4.0;
+LABEL_H        = UWB_W * 0.55;
+LABEL_T        =  1.2;   // label card thickness
+
+// ── USB-C cable routing ───────────────────────────────────────────────────────
+USBC_CHAN_W   = 11.0;
+USBC_CHAN_H   =  7.0;
+
+// ── Cable clip ────────────────────────────────────────────────────────────────
+CLIP_CABLE_D  =  4.5;
+CLIP_T        =  2.0;
+CLIP_BODY_W   = 16.0;
+CLIP_BODY_H   = 10.0;
+
+// ── Fasteners ─────────────────────────────────────────────────────────────────
+M2P5_D    = 2.7;
+M3_D      = 3.3;
+M3_NUT_AF = 5.5;
+M3_NUT_H  = 2.4;
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
 RENDER = "assembly";
 
-// ── ⚠ Verify with calipers ───────────────────────────────────
-MAWB_L      = 50.0;   // PCB length
-MAWB_W      = 25.0;   // PCB width
-MAWB_H      = 10.0;   // PCB + components
-MAWB_HOLE_X = 43.0;   // M2 mounting hole X span
-MAWB_HOLE_Y = 20.0;   // M2 mounting hole Y span
-M2_D        =  2.2;   // M2 clearance
+if      (RENDER == "assembly")           assembly_preview();
+else if (RENDER == "wall_base_stl")      wall_base();
+else if (RENDER == "tilt_arm_stl")       tilt_arm();
+else if (RENDER == "anchor_cradle_stl")  anchor_cradle();
+else if (RENDER == "cable_clip_stl")     cable_clip();
 
-// ── Stem ─────────────────────────────────────────────────────
-STEM_OD     = 25.0;
-STEM_BORE   = 25.4;   // +0.4 clearance
-WALL        =  2.0;   // wall thickness (used in thumbscrew recess)
+// ============================================================
+// ASSEMBLY PREVIEW
+// ============================================================
+module assembly_preview() {
+    // Ghost wall surface
+    %color("Wheat", 0.22)
+        translate([-BASE_W/2, -10, -BASE_H/2])
+            cube([BASE_W, 10, BASE_H + 40]);
 
-// ── Collar ───────────────────────────────────────────────────
-COL_OD      = 52.0;
-COL_H       = 30.0;   // taller than sensor-head collar for rigidity
-COL_BOLT_X  = 19.0;   // M4 bolt CL from stem axis
-COL_BOLT_D  =  4.5;   // M4 clearance
-THUMB_HEAD_D=  8.0;   // M4 thumbscrew head OD (slot for access)
-COL_NUT_W   =  7.0;   // M4 hex nut A/F
-COL_NUT_H   =  3.4;
+    // Wall base
+    color("OliveDrab", 0.85)
+        wall_base();
 
-// Anti-rotation flat tab: a 3 mm wall tab that protrudes radially
-// and bears against the bracket arm, preventing axial rotation
-// without needing a stem flat.
-ANTI_ROT_T  =  3.0;   // tab thickness (radial)
-ANTI_ROT_W  =  8.0;   // tab width (tangential)
-ANTI_ROT_Z  =  4.0;   // distance from collar base
+    // Tilt arm at TILT_DEG, pivoting at knuckle
+    color("SteelBlue", 0.85)
+        translate([0, KNUCKLE_T, 0])
+            rotate([TILT_DEG, 0, 0])
+                tilt_arm();
 
-// USB cable channel: groove on collar outer surface, runs Z direction
-// Cable routes from anchor module down to base
-USB_CHAN_W  =  9.0;   // channel width (fits USB-A cable Ø6 mm)
-USB_CHAN_D  =  5.0;   // channel depth
+    // Anchor cradle at arm end
+    color("DarkSlateGray", 0.85)
+        translate([0, KNUCKLE_T, 0])
+            rotate([TILT_DEG, 0, 0])
+                translate([0, ARM_T, ARM_L])
+                    anchor_cradle();
 
-// ── Module bracket ───────────────────────────────────────────
-ARM_L       = 20.0;   // arm length from collar OD to bracket face
-ARM_W       = MAWB_W + 6.0; // bracket width (Y, includes side walls)
-ARM_H       =  6.0;   // arm thickness (Z)
-BRKT_TILT   = 10.0;   // tilt outward from vertical (antenna faces horizon)
+    // PCB ghost
+    %color("ForestGreen", 0.38)
+        translate([0, KNUCKLE_T, 0])
+            rotate([TILT_DEG, 0, 0])
+                translate([-UWB_L/2,
+                           ARM_T + CRADLE_BACK_T,
+                           ARM_L + CRADLE_FLOOR_T + STANDOFF_H])
+                    cube([UWB_L, UWB_W, UWB_H]);
 
-BRKT_BACK_T =  3.0;   // bracket back wall (module sits against this)
-BRKT_SIDE_T =  2.0;   // bracket side walls
+    // Cable clip at arm mid-point
+    color("DimGray", 0.70)
+        translate([ARM_W/2, KNUCKLE_T, 0])
+            rotate([TILT_DEG, 0, 0])
+                translate([0, ARM_T + e, ARM_L/2])
+                    rotate([0, -90, 90])
+                        cable_clip();
+}
 
-M2_STNDFF   =  3.0;   // M2 standoff height
-M2_STNDFF_OD=  4.5;
-
-// USB port access notch in bracket side wall (8×5 mm)
-USB_NOTCH_W = 10.0;
-USB_NOTCH_H =  7.0;
-
-// ── Spacing ───────────────────────────────────────────────────
-ANCHOR_SPACING = 250.0;  // centre-to-centre Z separation
-
-$fn = 64;
-e   = 0.01;
-
-// ─────────────────────────────────────────────────────────────
-// collar_half(side)
-//   split at Y=0 plane.  Bracket arm on front (+Y) half.
-//   Print flat-face-down.
-// ─────────────────────────────────────────────────────────────
-module collar_half(side = "front") {
-    y_front = (side == "front");
+// ============================================================
+// PART 1 — WALL BASE
+// ============================================================
+// Flat backplate screws to wall or ceiling with 2× countersunk
+// M4/#6 wood screws on BASE_SCREW_SPC (35 mm) centres.
+// Two pivot ears straddle the tilt arm; M3 pivot bolt passes through
+// both ears and arm knuckle.
+// Detent arc on inner face of +X ear: 7 notches at 15° steps (0–90°)
+// so tilt can be set without a protractor.
+// Shallow rear recess accepts a printed installation-zone label.
+//
+// Dual-use: flat face to wall (vertical screw axis) or flat face
+// to ceiling (horizontal screw axis) — same part either way.
+//
+// Print: backplate flat on bed, PETG, 5 perims, 40 % gyroid.
+module wall_base() {
+    ear_h   = ARM_W + 3.0;
+    ear_t   = 6.0;
+    ear_sep = ARM_W + 1.0;
 
     difference() {
         union() {
-            // D-shaped body
-            intersection() {
-                cylinder(d=COL_OD, h=COL_H);
-                translate([-COL_OD/2, y_front ? 0 : -COL_OD/2, 0])
-                    cube([COL_OD, COL_OD/2, COL_H]);
-            }
+            // ── Backplate ────────────────────────────────────────────────
+            translate([-BASE_W/2, -BASE_T, -BASE_H/2])
+                cube([BASE_W, BASE_T, BASE_H]);
 
-            // Anti-rotation tab (front half only, at +X side)
-            if (y_front) {
-                translate([COL_OD/2, -ANTI_ROT_W/2, ANTI_ROT_Z])
-                    cube([ANTI_ROT_T, ANTI_ROT_W,
-                          COL_H - ANTI_ROT_Z - 4]);
-            }
+            // ── Two pivot ears ────────────────────────────────────────────
+            for (ex = [-(ear_sep/2 + ear_t), ear_sep/2])
+                translate([ex, -BASE_T + e, -ear_h/2])
+                    cube([ear_t, KNUCKLE_T + e, ear_h]);
 
-            // Bracket arm attachment boss (front half only, top centre)
-            if (y_front) {
-                translate([-ARM_W/2, COL_OD/2, COL_H * 0.3])
-                    cube([ARM_W, ARM_L, COL_H * 0.4]);
-            }
+            // ── Stiffening gussets ────────────────────────────────────────
+            for (ex = [-(ear_sep/2 + ear_t), ear_sep/2])
+                hull() {
+                    translate([ex, -BASE_T, -ear_h/4])
+                        cube([ear_t, BASE_T - 1, ear_h/2]);
+                    translate([ex + (ex < 0 ? ear_t*0.6 : 0),
+                               -BASE_T, -ear_h/6])
+                        cube([ear_t * 0.4, 1, ear_h/3]);
+                }
         }
 
-        // ── Stem bore ─────────────────────────────────────────
-        translate([0,0,-e])
-            cylinder(d=STEM_BORE, h=COL_H + 2*e);
-
-        // ── M4 clamping bolt holes (Y direction) ──────────────
-        for (bx=[-COL_BOLT_X, COL_BOLT_X]) {
-            translate([bx, y_front ? COL_OD/2 : 0, COL_H/2])
-                rotate([90,0,0])
-                    cylinder(d=COL_BOLT_D, h=COL_OD/2 + e);
-            // Thumbscrew head recess on outer face (front only — access side)
-            if (y_front) {
-                translate([bx, COL_OD/2 - WALL, COL_H/2])
-                    rotate([90,0,0])
-                        cylinder(d=THUMB_HEAD_D, h=8 + e);
-            }
+        // ── 2× countersunk wall screws ────────────────────────────────────
+        for (sz = [-BASE_SCREW_SPC/2, BASE_SCREW_SPC/2]) {
+            translate([0, -BASE_T - e, sz])
+                rotate([-90, 0, 0])
+                    cylinder(d = BASE_SCREW_D, h = BASE_T + 2*e);
+            translate([0, -BASE_T - e, sz])
+                rotate([-90, 0, 0])
+                    cylinder(d1 = BASE_SCREW_HD, d2 = BASE_SCREW_D,
+                             h = BASE_SCREW_HH + e);
         }
 
-        // ── M4 hex nut pockets (rear half) ────────────────────
-        if (!y_front) {
-            for (bx=[-COL_BOLT_X, COL_BOLT_X]) {
-                translate([bx, -(COL_OD/4 + e), COL_H/2])
-                    rotate([90,0,0])
-                        cylinder(d=COL_NUT_W/cos(30), h=COL_NUT_H + e,
-                                 $fn=6);
-            }
-        }
+        // ── Pivot bolt bore (M3, through both ears) ───────────────────────
+        translate([-(ear_sep/2 + ear_t + e), KNUCKLE_T * 0.55, 0])
+            rotate([0, 90, 0])
+                cylinder(d = PIVOT_D, h = ear_sep + 2*ear_t + 2*e);
 
-        // ── Set screw (height lock, front half) ───────────────
-        if (y_front) {
-            translate([0, COL_OD/2, COL_H * 0.8])
-                rotate([90,0,0])
-                    cylinder(d=COL_BOLT_D,
-                             h=COL_OD/2 - STEM_BORE/2 + e);
-        }
+        // ── M3 nyloc nut pocket (outer face of one ear) ───────────────────
+        translate([ear_sep/2 + ear_t - PIVOT_NUT_H - 0.4,
+                   KNUCKLE_T * 0.55, 0])
+            rotate([0, 90, 0])
+                cylinder(d = PIVOT_NUT_AF / cos(30),
+                         h = PIVOT_NUT_H + 0.5, $fn = 6);
 
-        // ── USB cable routing channel (rear half, −X side) ────
-        if (!y_front) {
-            translate([-COL_OD/2, -USB_CHAN_W/2, -e])
-                cube([USB_CHAN_D, USB_CHAN_W, COL_H + 2*e]);
-        }
+        // ── Detent arc — 7 notches at 15° steps on +X ear inner face ─────
+        for (da = [0 : 15 : 90])
+            translate([ear_sep/2 - e,
+                       KNUCKLE_T * 0.55 + DETENT_R * sin(da),
+                       DETENT_R * cos(da)])
+                rotate([0, 90, 0])
+                    cylinder(d = DETENT_D, h = ear_t * 0.45 + e);
 
-        // ── M4 hole through arm boss (Z direction, for bracket bolt) ─
-        if (y_front) {
-            for (dx=[-ARM_W/4, ARM_W/4])
-                translate([dx, COL_OD/2 + ARM_L/2, COL_H * 0.35])
-                    cylinder(d=COL_BOLT_D, h=COL_H * 0.35 + e);
-        }
+        // ── Installation label recess (rear face of backplate) ────────────
+        translate([0, -BASE_T - e, 0])
+            rotate([-90, 0, 0])
+                cube([BASE_W - 12, BASE_H - 16, 1.6], center = true);
+
+        // ── Lightening pocket ─────────────────────────────────────────────
+        translate([0, -BASE_T + 1.5, 0])
+            cube([BASE_W - 14, BASE_T - 3, BASE_H - 20], center = true);
     }
 }
 
-// ─────────────────────────────────────────────────────────────
-// module_bracket()
-//   Bolts to collar arm boss. Holds MaUWB PCB facing outward.
-//   Tilted BRKT_TILT° from vertical — antenna clears stem.
-//   Print flat-face-down (back wall on bed).
-// ─────────────────────────────────────────────────────────────
-module module_bracket() {
-    bk = BRKT_BACK_T;
-    sd = BRKT_SIDE_T;
+// ============================================================
+// PART 2 — TILT ARM
+// ============================================================
+// Pivoting arm linking wall_base pivot ears to anchor_cradle.
+// Knuckle end (Z=0): M3 pivot bore + spring-plunger detent pocket
+//   that indexes into the base ear detent arc notches.
+// Cradle end (Z=ARM_L): 2× M3 bolt attachment to cradle back wall.
+// USB-C cable channel runs along outer (+Y) face, full arm length.
+//
+// Print: knuckle face flat on bed, PETG, 5 perims, 40 % gyroid.
+module tilt_arm() {
+    total_h = ARM_L + 10;
 
     difference() {
         union() {
-            // ── Back wall (mounts to collar arm boss) ─────────
-            cube([ARM_W, bk, MAWB_H + M2_STNDFF + 6]);
+            // ── Arm body ─────────────────────────────────────────────────
+            translate([-ARM_W/2, 0, 0])
+                cube([ARM_W, ARM_T, total_h]);
 
-            // ── Side walls ────────────────────────────────────
-            for (sx=[0, ARM_W - sd])
-                translate([sx, bk, 0])
-                    cube([sd, MAWB_L + 2, MAWB_H + M2_STNDFF + 6]);
+            // ── Knuckle boss (rounded pivot end) ─────────────────────────
+            translate([0, ARM_T/2, 0])
+                rotate([90, 0, 0])
+                    cylinder(d = ARM_W, h = ARM_T, center = true);
 
-            // ── M2 standoff posts (PCB mounts to these) ───────
-            for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y])
-                translate([(ARM_W - MAWB_HOLE_X)/2 + hx,
-                           bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy,
-                           0])
-                    cylinder(d=M2_STNDFF_OD, h=M2_STNDFF);
+            // ── Cradle attach stub (Z = ARM_L) ────────────────────────────
+            translate([-ARM_W/2, 0, ARM_L])
+                cube([ARM_W, ARM_T + CRADLE_BACK_T, ARM_T]);
         }
 
-        // ── M2 bores through standoffs ────────────────────────
-        for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y])
-            translate([(ARM_W - MAWB_HOLE_X)/2 + hx,
-                       bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy,
-                       -e])
-                cylinder(d=M2_D, h=M2_STNDFF + e);
+        // ── M3 pivot bore ─────────────────────────────────────────────────
+        translate([-ARM_W/2 - e, ARM_T/2, 0])
+            rotate([0, 90, 0])
+                cylinder(d = PIVOT_D, h = ARM_W + 2*e);
 
-        // ── Antenna clearance cutout in back wall ─────────────
-        // Open slot near top of back wall so antenna is unobstructed
-        translate([sd, -e, M2_STNDFF + 2])
-            cube([ARM_W - 2*sd, bk + 2*e, MAWB_H]);
+        // ── Detent plunger pocket (3 mm spring-ball, outer +Y face) ──────
+        translate([0, ARM_T + e, 0])
+            rotate([90, 0, 0])
+                cylinder(d = 3.2, h = 4 + e);
 
-        // ── USB port access notch on one side wall ────────────
-        translate([-e, bk + 2, M2_STNDFF - 1])
-            cube([sd + 2*e, USB_NOTCH_W, USB_NOTCH_H]);
+        // ── USB-C cable channel (outer +Y face, mid-arm length) ───────────
+        translate([-USBC_CHAN_W/2, ARM_T - e, ARM_T + 4])
+            cube([USBC_CHAN_W, USBC_CHAN_H, ARM_L - ARM_T - 8]);
 
-        // ── Mounting holes to collar arm boss (×2) ────────────
-        for (dx=[-ARM_W/4, ARM_W/4])
-            translate([ARM_W/2 + dx, bk + ARM_L/2, -e])
-                cylinder(d=COL_BOLT_D, h=6 + e);
+        // ── Cradle attach bolt holes (2× M3 at cradle stub) ───────────────
+        for (bx = [-ARM_W/4, ARM_W/4])
+            translate([bx, ARM_T/2, ARM_L + ARM_T/2])
+                rotate([90, 0, 0])
+                    cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e);
+
+        // ── M3 nut pockets (front of cradle stub) ─────────────────────────
+        for (bx = [-ARM_W/4, ARM_W/4])
+            translate([bx, ARM_T/2, ARM_L + ARM_T/2])
+                rotate([-90, 0, 0])
+                    cylinder(d = M3_NUT_AF / cos(30),
+                             h = M3_NUT_H + 0.5, $fn = 6);
+
+        // ── Lightening pocket ─────────────────────────────────────────────
+        translate([0, ARM_T/2, ARM_L/2])
+            cube([ARM_W - 4, ARM_T - 2, ARM_L - 18], center = true);
     }
 }
 
-// ─────────────────────────────────────────────────────────────
-// single_anchor_assembly()
-// ─────────────────────────────────────────────────────────────
-module single_anchor_assembly(show_phantom=false) {
-    // Collar
-    color("SteelBlue", 0.9)   collar_half("front");
-    color("CornflowerBlue", 0.9) mirror([0,1,0]) collar_half("rear");
+// ============================================================
+// PART 3 — ANCHOR CRADLE
+// ============================================================
+// Open-front U-cradle for ESP32 UWB Pro PCB.
+// PCB retained on 4× M2.5 standoffs matching UWB_HOLE_X × UWB_HOLE_Y.
+// Back wall features:
+//   • USB-C exit slot  — aligns with PCB USB-C port
+//   • USB-C groove     — cable routes from slot toward arm channel
+//   • Label card slot  — insert printed strip for anchor ID
+//   • Antenna keep-out — back wall material removed above antenna area
+// Front lip prevents PCB from sliding forward.
+// Two attachment tabs bolt to tilt_arm cradle stub.
+//
+// Print: back wall flat on bed, PETG, 5 perims, 40 % gyroid.
+module anchor_cradle() {
+    outer_l  = UWB_L + 2*CRADLE_WALL_T;
+    outer_w  = UWB_W + CRADLE_FLOOR_T;
+    pcb_z    = CRADLE_FLOOR_T + STANDOFF_H;
+    total_z  = pcb_z + UWB_H + 2;
 
-    // Bracket tilted BRKT_TILT° outward from top of arm boss
-    color("LightSteelBlue", 0.85)
-        translate([0, COL_OD/2 + ARM_L, COL_H * 0.3])
-            rotate([BRKT_TILT, 0, 0])
-                translate([-ARM_W/2, 0, 0])
-                    module_bracket();
+    difference() {
+        union() {
+            // ── Cradle body ───────────────────────────────────────────────
+            translate([-outer_l/2, 0, 0])
+                cube([outer_l, outer_w, total_z]);
 
-    // Phantom UWB PCB
-    if (show_phantom)
-        color("ForestGreen", 0.4)
-            translate([-MAWB_L/2,
-                       COL_OD/2 + ARM_L + BRKT_BACK_T,
-                       COL_H * 0.3 + M2_STNDFF])
-                cube([MAWB_L, MAWB_W, MAWB_H]);
+            // ── Front retaining lip ───────────────────────────────────────
+            translate([-outer_l/2, outer_w - CRADLE_LIP_T, 0])
+                cube([outer_l, CRADLE_LIP_T, CRADLE_LIP_H]);
+
+            // ── Arm attachment tabs (behind back wall) ─────────────────────
+            for (tx = [-ARM_W/4, ARM_W/4])
+                translate([tx - 4, -CRADLE_BACK_T, 0])
+                    cube([8, CRADLE_BACK_T + 1, total_z]);
+        }
+
+        // ── PCB pocket ────────────────────────────────────────────────────
+        translate([-UWB_L/2, 0, pcb_z])
+            cube([UWB_L, UWB_W + 1, UWB_H + 4]);
+
+        // ── USB-C exit slot (through back wall, aligned to PCB port) ─────
+        translate([0, -CRADLE_BACK_T - e,
+                   pcb_z + UWB_H/2 - UWB_USBC_H/2])
+            cube([UWB_USBC_W + 2, CRADLE_BACK_T + 2*e, UWB_USBC_H + 2],
+                 center = [true, false, false]);
+
+        // ── USB-C cable routing groove (outer back wall face) ─────────────
+        translate([0, -CRADLE_BACK_T - e, -e])
+            cube([USBC_CHAN_W, USBC_CHAN_H, pcb_z + UWB_H/2 + USBC_CHAN_H],
+                 center = [true, false, false]);
+
+        // ── Label card slot (insert from below, rear face upper half) ─────
+        // Paper/laminate card strip identifying this anchor instance
+        translate([0, -CRADLE_BACK_T - e, pcb_z + UWB_H/2])
+            cube([LABEL_W, LABEL_T + 0.3, LABEL_H],
+                 center = [true, false, false]);
+
+        // ── Antenna keep-out: remove back wall above antenna area ─────────
+        translate([0, -e, pcb_z + UWB_H - UWB_ANTENNA_L])
+            cube([UWB_L - 4, CRADLE_BACK_T + 2*e, UWB_ANTENNA_L + 4],
+                 center = [true, false, false]);
+
+        // ── Arm bolt holes through attachment tabs ────────────────────────
+        for (tx = [-ARM_W/4, ARM_W/4])
+            translate([tx, ARM_T/2 - CRADLE_BACK_T, total_z/2])
+                rotate([-90, 0, 0])
+                    cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e);
+
+        // ── Lightening slots in side walls ────────────────────────────────
+        for (side_x = [-outer_l/2 - e, outer_l/2 - CRADLE_WALL_T - e])
+            translate([side_x, 2, pcb_z + 2])
+                cube([CRADLE_WALL_T + 2*e, UWB_W - 4, UWB_H - 4]);
+    }
+
+    // ── M2.5 standoff bosses (positive, inside cradle floor) ──────────────
+    for (hx = [-UWB_HOLE_X/2, UWB_HOLE_X/2])
+    for (hy = [(outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2,
+               (outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2 + UWB_HOLE_Y])
+        difference() {
+            translate([hx, hy, CRADLE_FLOOR_T - e])
+                cylinder(d = STANDOFF_OD, h = STANDOFF_H + e);
+            translate([hx, hy, CRADLE_FLOOR_T - 2*e])
+                cylinder(d = M2P5_D, h = STANDOFF_H + 4);
+        }
 }
 
-// ─────────────────────────────────────────────────────────────
-// Render selector
-// ─────────────────────────────────────────────────────────────
-if (RENDER == "assembly") {
-    single_anchor_assembly(show_phantom=true);
+// ============================================================
+// PART 4 — CABLE CLIP
+// ============================================================
+// Snap-on C-clip retaining USB-C cable along tilt arm outer face.
+// Presses onto ARM_T-wide arm with PETG snap tongues.
+// Open-front cable channel for push-in cable insertion.
+// Print ×2–3 per anchor, spaced 25 mm along arm.
+//
+// Print: clip-opening face down, PETG, 3 perims, 20 % infill.
+module cable_clip() {
+    ch_r   = CLIP_CABLE_D/2 + CLIP_T;
+    snap_t = 1.6;
 
-} else if (RENDER == "collar_front") {
-    collar_half("front");
+    difference() {
+        union() {
+            // ── Body plate ────────────────────────────────────────────────
+            translate([-CLIP_BODY_W/2, 0, 0])
+                cube([CLIP_BODY_W, CLIP_T, CLIP_BODY_H]);
 
-} else if (RENDER == "collar_rear") {
-    collar_half("rear");
+            // ── Cable channel (C-shape, opens toward +Y) ─────────────────
+            translate([0, CLIP_T + ch_r, CLIP_BODY_H/2])
+                rotate([0, 90, 0])
+                    difference() {
+                        cylinder(r = ch_r, h = CLIP_BODY_W, center = true);
+                        cylinder(r = CLIP_CABLE_D/2,
+                                 h = CLIP_BODY_W + 2*e, center = true);
+                        // open insertion slot
+                        translate([0, ch_r + e, 0])
+                            cube([CLIP_CABLE_D * 0.85,
+                                  ch_r * 2 + 2*e,
+                                  CLIP_BODY_W + 2*e], center = true);
+                    }
 
-} else if (RENDER == "bracket") {
-    module_bracket();
+            // ── Snap tongues (straddle arm, -Y side of body) ─────────────
+            for (tx = [-CLIP_BODY_W/2 + 1.5,
+                       CLIP_BODY_W/2 - 1.5 - snap_t])
+                translate([tx, -ARM_T - 1, 0])
+                    cube([snap_t, ARM_T + 1 + CLIP_T, CLIP_BODY_H]);
 
-} else if (RENDER == "pair") {
-    // Both anchors at 250 mm spacing on a stem stub
-    color("Silver", 0.2)
-        translate([0, 0, -50])
-            cylinder(d=STEM_OD, h=ANCHOR_SPACING + COL_H + 100);
+            // ── Snap barbs ────────────────────────────────────────────────
+            for (tx = [-CLIP_BODY_W/2 + 1.5,
+                       CLIP_BODY_W/2 - 1.5 - snap_t])
+                translate([tx + snap_t/2, -ARM_T - 1, CLIP_BODY_H/2])
+                    rotate([0, 90, 0])
+                        cylinder(d = 2, h = snap_t, center = true);
+        }
 
-    // Lower anchor (Z = 0)
-    single_anchor_assembly(show_phantom=true);
-
-    // Upper anchor (Z = ANCHOR_SPACING)
-    translate([0, 0, ANCHOR_SPACING])
-        single_anchor_assembly(show_phantom=true);
+        // ── Arm slot (arm body passes between tongues) ─────────────────────
+        translate([0, -ARM_T - 1 - e, CLIP_BODY_H/2])
+            cube([CLIP_BODY_W - 6, ARM_T + 2, CLIP_BODY_H - 4], center = true);
+    }
 }
-

esp32/uwb_anchor/platformio.ini

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

esp32/uwb_anchor/src/main.cpp

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

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

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

include/config.h

@@ -244,4 +244,17 @@
 #define AUDIO_BUF_HALF          441u          // DMA half-buffer: 20ms at 22050 Hz
 #define AUDIO_VOLUME_DEFAULT    80u           // default volume 0-100
 
+// --- Gimbal Servo Bus (ST3215, USART3 half-duplex, Issue #547) ---
+// Half-duplex single-wire on PB10 (USART3_TX, AF7) at 1 Mbps.
+// USART3 is available: not assigned to any active subsystem.
+#define SERVO_BUS_UART          USART3
+#define SERVO_BUS_PORT          GPIOB
+#define SERVO_BUS_PIN           GPIO_PIN_10   // USART3_TX, AF7
+#define SERVO_BUS_BAUD          1000000u      // 1 Mbps (ST3215 default)
+#define GIMBAL_PAN_ID           1u            // ST3215 servo ID for pan
+#define GIMBAL_TILT_ID          2u            // ST3215 servo ID for tilt
+#define GIMBAL_TLM_HZ           50u           // position feedback rate (Hz)
+#define GIMBAL_PAN_LIMIT_DEG    180.0f        // pan  soft limit (deg each side)
+#define GIMBAL_TILT_LIMIT_DEG   90.0f         // tilt soft limit (deg each side)
+
 #endif // CONFIG_H

include/gimbal.h

@@ -0,0 +1,72 @@
+#ifndef GIMBAL_H
+#define GIMBAL_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * gimbal.h — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
+ *
+ * Manages dual ST3215 serial bus servos:
+ *   Pan  servo: ID GIMBAL_PAN_ID  (config.h, default 1)
+ *   Tilt servo: ID GIMBAL_TILT_ID (config.h, default 2)
+ *
+ * Position units: degrees x10 (int16), matching JLink protocol convention.
+ *   e.g. 900 = 90.0°, -450 = -45.0°
+ *
+ * Limits:
+ *   Pan:  -1800..+1800 (x10 deg) = -180..+180 deg
+ *   Tilt:  -900..+900  (x10 deg) = -90..+90 deg
+ *
+ * The gimbal_tick() function polls servo feedback at GIMBAL_TLM_HZ (50 Hz).
+ * Alternates reading pan position on even ticks, tilt on odd ticks — each
+ * servo polled at 25 Hz to keep bus utilization low.
+ */
+
+typedef struct {
+    /* Command state */
+    int16_t  cmd_pan_x10;    /* Commanded pan  (deg x10) */
+    int16_t  cmd_tilt_x10;   /* Commanded tilt (deg x10) */
+    uint16_t cmd_speed;      /* Servo bus speed (0=max, 1-4095) */
+    bool     torque_enabled; /* True when torques are enabled */
+
+    /* Feedback state (updated at ~25 Hz per axis) */
+    int16_t  fb_pan_x10;     /* Measured pan  (deg x10) */
+    int16_t  fb_tilt_x10;    /* Measured tilt (deg x10) */
+    uint16_t fb_pan_speed;   /* Raw speed register, pan servo */
+    uint16_t fb_tilt_speed;  /* Raw speed register, tilt servo */
+
+    /* Diagnostics */
+    uint32_t rx_ok;          /* Successful position reads */
+    uint32_t rx_err;         /* Failed position reads */
+
+    uint32_t _last_tick_ms;  /* Internal: last tick timestamp */
+    uint8_t  _poll_phase;    /* Internal: alternates 0=pan 1=tilt */
+} gimbal_t;
+
+/*
+ * gimbal_init(g) — enable torque on both servos, center them.
+ * servo_bus_init() must be called first.
+ */
+void gimbal_init(gimbal_t *g);
+
+/*
+ * gimbal_set_pos(g, pan_x10, tilt_x10, speed) — command a new pan/tilt
+ * position.  pan_x10 and tilt_x10 are degrees×10, clamped to servo limits.
+ * speed: 0=max servo speed, 1-4095 = scaled.
+ */
+void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
+                    uint16_t speed);
+
+/*
+ * gimbal_torque(g, enable) — enable or disable torque on both servos.
+ */
+void gimbal_torque(gimbal_t *g, bool enable);
+
+/*
+ * gimbal_tick(g, now_ms) — poll servo feedback at GIMBAL_TLM_HZ.
+ * Call every 1 ms from the main loop; function self-throttles.
+ */
+void gimbal_tick(gimbal_t *g, uint32_t now_ms);
+
+#endif /* GIMBAL_H */

include/jlink.h

@@ -32,16 +32,18 @@
  *   0x08  AUDIO      - int16 PCM samples (up to 126 samples)
  *   0x09  SLEEP      - no payload; request STOP-mode sleep
  *   0x0A  PID_SAVE   - no payload; save current Kp/Ki/Kd to flash (Issue #531)
+ *   0x0B  GIMBAL_POS - int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547)
  *   0x0C  SCHED_GET  - no payload; reply with TLM_SCHED (Issue #550)
  *   0x0D  SCHED_SET  - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550)
  *   0x0E  SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550)
  *
  * STM32 to Jetson telemetry:
- *   0x80  STATUS     - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
- *   0x81  POWER      - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
- *   0x82  BATTERY    - jlink_tlm_battery_t (10 bytes, Issue #533)
- *   0x83  PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
- *   0x85  SCHED      - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
+ *   0x80  STATUS        - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
+ *   0x81  POWER         - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ
+ *   0x82  BATTERY       - jlink_tlm_battery_t (10 bytes, Issue #533)
+ *   0x83  PID_RESULT    - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
+ *   0x84  GIMBAL_STATE  - jlink_tlm_gimbal_state_t (10 bytes, Issue #547)
+ *   0x85  SCHED         - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
  *
  * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
  * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
@@ -67,6 +69,7 @@
 #define JLINK_CMD_AUDIO         0x08u  /* PCM audio chunk: int16 samples, up to 126 */
 #define JLINK_CMD_SLEEP         0x09u  /* no payload; request STOP-mode sleep */
 #define JLINK_CMD_PID_SAVE      0x0Au  /* no payload; save Kp/Ki/Kd to flash (Issue #531) */
+#define JLINK_CMD_GIMBAL_POS    0x0Bu  /* int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) */
 #define JLINK_CMD_SCHED_GET     0x0Cu  /* no payload; reply TLM_SCHED (Issue #550) */
 #define JLINK_CMD_SCHED_SET     0x0Du  /* uint8 num_bands + N*16-byte entries (Issue #550) */
 #define JLINK_CMD_SCHED_SAVE    0x0Eu  /* float kp,ki,kd; save sched+single to flash (Issue #550) */
@@ -76,6 +79,7 @@
 #define JLINK_TLM_POWER         0x81u  /* jlink_tlm_power_t (11 bytes) */
 #define JLINK_TLM_BATTERY       0x82u  /* jlink_tlm_battery_t (10 bytes, Issue #533) */
 #define JLINK_TLM_PID_RESULT    0x83u  /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */
+#define JLINK_TLM_GIMBAL_STATE  0x84u  /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */
 #define JLINK_TLM_SCHED         0x85u  /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
@@ -126,6 +130,17 @@ typedef struct __attribute__((packed)) {
     uint8_t saved_ok;  /* 1 = flash write verified, 0 = write failed */
 } jlink_tlm_pid_result_t;  /* 13 bytes */
 
+/* ---- Telemetry GIMBAL_STATE payload (10 bytes, packed) Issue #547 ---- */
+/* Sent at GIMBAL_TLM_HZ (50 Hz); reports measured pan/tilt and speed. */
+typedef struct __attribute__((packed)) {
+    int16_t  pan_x10;        /* Measured pan  angle, deg x10 */
+    int16_t  tilt_x10;       /* Measured tilt angle, deg x10 */
+    uint16_t pan_speed_raw;  /* Present speed register, pan  servo */
+    uint16_t tilt_speed_raw; /* Present speed register, tilt servo */
+    uint8_t  torque_en;      /* 1 = torque enabled */
+    uint8_t  rx_err_pct;     /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */
+} jlink_tlm_gimbal_state_t;  /* 10 bytes */
+
 /* ---- Telemetry SCHED payload (1 + N*16 bytes, packed) Issue #550 ---- */
 /* Sent in response to JLINK_CMD_SCHED_GET; N = num_bands (1..PID_SCHED_MAX_BANDS). */
 typedef struct __attribute__((packed)) {
@@ -160,6 +175,12 @@ typedef struct {
     /* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */
     volatile uint8_t  pid_save_req;
 
+    /* Gimbal position command - set by JLINK_CMD_GIMBAL_POS (Issue #547) */
+    volatile uint8_t  gimbal_updated;  /* set by parser, cleared by main loop */
+    volatile int16_t  gimbal_pan_x10;  /* pan  angle deg x10 */
+    volatile int16_t  gimbal_tilt_x10; /* tilt angle deg x10 */
+    volatile uint16_t gimbal_speed;    /* servo speed 0-4095 (0=max) */
+
     /* PID schedule commands (Issue #550) - set by parser, cleared by main loop */
     volatile uint8_t  sched_get_req;   /* SCHED_GET: main loop calls jlink_send_sched_telemetry() */
     volatile uint8_t  sched_save_req;  /* SCHED_SAVE: main loop calls pid_schedule_flash_save() */
@@ -191,6 +212,12 @@ void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
 void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt);
 void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
 
+/*
+ * jlink_send_gimbal_state(state) - transmit JLINK_TLM_GIMBAL_STATE (0x84)
+ * frame (16 bytes) at GIMBAL_TLM_HZ (50 Hz). Issue #547.
+ */
+void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state);
+
 /*
  * jlink_send_sched_telemetry(tlm) - transmit JLINK_TLM_SCHED (0x85) in
  * response to SCHED_GET.  tlm->num_bands determines actual frame size.

include/pid_flash.h

@@ -5,7 +5,7 @@
 #include <stdbool.h>
 
 /*
- * pid_flash -- persistent PID storage for Issue #531 (auto-tune).
+ * pid_flash — persistent PID storage for Issue #531 (auto-tune).
  *
  * Stores Kp, Ki, Kd in the last 64 bytes of STM32F722 flash sector 7
  * (0x0807FFC0). Magic word validates presence of saved params.
@@ -14,22 +14,14 @@
  * Flash writes require an erase of the full sector (128KB) before re-writing.
  * The store address is the very last 64-byte block so future expansion can
  * grow toward lower addresses within sector 7 without conflict.
- *
- * Issue #550 adds pid_sched_flash_t at 0x0807FF40 (128 bytes below the PID
- * record).  Both are written atomically by pid_flash_save_all() in one erase.
- *
- * Sector 7 layout:
- *   0x0807FF40  pid_sched_flash_t  (128 bytes) -- schedule (Issue #550)
- *   0x0807FFC0  pid_flash_t        ( 64 bytes) -- single PID (Issue #531)
- *   0x08080000  sector boundary
  */
 
-#define PID_FLASH_SECTOR         FLASH_SECTOR_7
+#define PID_FLASH_SECTOR        FLASH_SECTOR_7
 #define PID_FLASH_SECTOR_VOLTAGE VOLTAGE_RANGE_3   /* 2.7V-3.6V, 32-bit parallelism */
 
-/* Sector 7: 128KB at 0x08060000; store single-PID in last 64 bytes */
+/* Sector 7: 128KB at 0x08060000; store in last 64 bytes */
 #define PID_FLASH_STORE_ADDR    0x0807FFC0UL
-#define PID_FLASH_MAGIC         0x534C5401UL   /* 'SLT\x01' -- version 1 */
+#define PID_FLASH_MAGIC         0x534C5401UL   /* 'SLT\x01' — version 1 */
 
 typedef struct __attribute__((packed)) {
     uint32_t magic;       /* PID_FLASH_MAGIC when valid */
@@ -40,62 +32,17 @@ typedef struct __attribute__((packed)) {
 } pid_flash_t;
 
 /*
- * pid_flash_load() -- read saved PID from flash.
+ * pid_flash_load() — read saved PID from flash.
  * Returns true and fills *kp/*ki/*kd if magic is valid.
  * Returns false if no valid params stored (caller keeps defaults).
  */
 bool pid_flash_load(float *kp, float *ki, float *kd);
 
 /*
- * pid_flash_save() -- erase sector 7 and write Kp/Ki/Kd.
+ * pid_flash_save() — erase sector 7 and write Kp/Ki/Kd.
  * Must not be called while armed (flash erase takes ~1s and stalls the CPU).
  * Returns true on success.
  */
 bool pid_flash_save(float kp, float ki, float kd);
 
-/* ==================================================================
- * PID Gain Schedule flash storage (Issue #550)
- * ================================================================== */
-
-#define PID_SCHED_MAX_BANDS     6u
-#define PID_SCHED_FLASH_ADDR    0x0807FF40UL
-#define PID_SCHED_MAGIC         0x534C5402UL   /* 'SLT\x02' -- version 2 */
-
-/* One speed band: 16 bytes, IEEE-754 LE floats */
-typedef struct __attribute__((packed)) {
-    float speed_mps;   /* breakpoint velocity (m/s, absolute) */
-    float kp;
-    float ki;
-    float kd;
-} pid_sched_entry_t;   /* 16 bytes */
-
-/* Flash record: 128 bytes */
-typedef struct __attribute__((packed)) {
-    uint32_t          magic;                       /*  4 bytes */
-    uint8_t           num_bands;                   /*  1 byte  */
-    uint8_t           flags;                       /*  1 byte (reserved) */
-    uint8_t           _pad0[2];                    /*  2 bytes */
-    pid_sched_entry_t bands[PID_SCHED_MAX_BANDS];  /* 96 bytes */
-    uint8_t           _pad1[24];                   /* 24 bytes */
-} pid_sched_flash_t;   /* 128 bytes total */
-
-/*
- * pid_flash_load_schedule() -- read gain schedule from flash.
- * Returns true and fills out_entries/out_n if magic valid and num_bands in
- * [1, PID_SCHED_MAX_BANDS].  Returns false otherwise.
- * out_entries must have room for PID_SCHED_MAX_BANDS entries.
- */
-bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n);
-
-/*
- * pid_flash_save_all() -- erase sector 7 ONCE and write both records:
- *   - pid_sched_flash_t  at PID_SCHED_FLASH_ADDR
- *   - pid_flash_t        at PID_FLASH_STORE_ADDR
- * Atomic: one sector erase covers both.
- * Must not be called while armed (erase takes ~1s).
- * Returns true on success.
- */
-bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
-                        const pid_sched_entry_t *entries, uint8_t num_bands);
-
 #endif /* PID_FLASH_H */

include/servo_bus.h

@@ -0,0 +1,97 @@
+#ifndef SERVO_BUS_H
+#define SERVO_BUS_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * servo_bus.h — Feetech/ST3215 serial bus servo driver (Issue #547)
+ *
+ * Half-duplex single-wire UART protocol at 1 Mbps.
+ * Hardware: USART3 half-duplex on PB10 (USART3_TX, AF7).
+ * No separate RX pin — the TX line is bidirectional with HDSEL.
+ *
+ * Packet format (host to servo):
+ *   [0xFF][0xFF][ID][LEN][INSTR][PARAMS...][CKSUM]
+ *   CKSUM = ~(ID + LEN + INSTR + PARAMS) & 0xFF
+ *
+ * Response (servo to host):
+ *   [0xFF][0xFF][ID][LEN][ERROR][DATA...][CKSUM]
+ *
+ * Key ST3215 registers:
+ *   0x28  Torque Enable       (1=on, 0=off)
+ *   0x2A  Goal Position L     (0-4095)
+ *   0x2B  Goal Position H
+ *   0x2E  Moving Speed L      (0=max, 1-4095)
+ *   0x2F  Moving Speed H
+ *   0x38  Present Position L  (0-4095)
+ *   0x39  Present Position H
+ *   0x3A  Present Speed L     (sign+magnitude: bit15=dir)
+ *   0x3B  Present Speed H
+ *
+ * Position encoding: 0-4095 maps to 0-360 degrees.
+ * Center (180 deg) = 2048 raw.
+ */
+
+/* ST3215 register addresses */
+#define SB_REG_TORQUE_EN    0x28u
+#define SB_REG_GOAL_POS_L   0x2Au
+#define SB_REG_MOVING_SPD_L 0x2Eu
+#define SB_REG_PRES_POS_L   0x38u
+#define SB_REG_PRES_SPD_L   0x3Au
+
+/* ST3215 instructions */
+#define SB_INSTR_PING       0x01u
+#define SB_INSTR_READ       0x02u
+#define SB_INSTR_WRITE      0x03u
+
+/* Position encoding */
+#define SB_POS_CENTER       2048u   /* 180 deg */
+#define SB_POS_MAX          4095u   /* 360 deg */
+#define SB_SPEED_MAX        4095u   /* counts/sec (0 = max speed) */
+
+/* Timeout for servo response (ms) */
+#define SB_RX_TIMEOUT_MS    5u
+
+/*
+ * servo_bus_init() - configure USART3 in half-duplex mode (PB10, AF7) at
+ * SB_BAUD (1 Mbps, 8N1). Call once at startup before gimbal_init().
+ */
+void servo_bus_init(void);
+
+/*
+ * servo_bus_write_pos(id, raw_pos, speed) - write goal position and moving
+ * speed in a single WRITE DATA packet. raw_pos: 0-4095. speed: 0=max, 1-4095.
+ * Returns true on successful TX (no response expected on write).
+ */
+bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed);
+
+/*
+ * servo_bus_write_torque(id, enable) - enable or disable servo torque.
+ */
+bool servo_bus_write_torque(uint8_t id, bool enable);
+
+/*
+ * servo_bus_read_pos(id, raw_pos) - read present position.
+ * Returns true on success; raw_pos is 0-4095.
+ */
+bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos);
+
+/*
+ * servo_bus_read_speed(id, speed) - read present speed.
+ * speed bit 15 is direction. Returns magnitude in lower 15 bits.
+ */
+bool servo_bus_read_speed(uint8_t id, uint16_t *speed);
+
+/*
+ * servo_bus_deg_to_raw(deg) - convert degree (-180..+180) to raw position.
+ * Center (0 deg) = 2048. Clamps to 0-4095.
+ */
+uint16_t servo_bus_deg_to_raw(float deg);
+
+/*
+ * servo_bus_raw_to_deg(raw) - convert raw position (0-4095) to degree (-180..+180).
+ */
+float servo_bus_raw_to_deg(uint16_t raw);
+
+#endif /* SERVO_BUS_H */

jetson/ros2_ws/src/saltybot_gimbal/config/gimbal_params.yaml

@@ -0,0 +1,29 @@
+gimbal_node:
+  ros__parameters:
+    # Serial port connecting to STM32 over JLINK protocol
+    serial_port: "/dev/ttyTHS1"
+    baud_rate: 921600
+
+    # Soft angle limits (degrees, ± from center)
+    pan_limit_deg: 150.0
+    tilt_limit_deg: 45.0
+
+    # Home position (degrees from center)
+    home_pan_deg: 0.0
+    home_tilt_deg: 0.0
+
+    # Motion profile
+    max_speed_deg_s: 90.0      # maximum pan/tilt speed (°/s)
+    accel_deg_s2: 180.0        # trapezoidal acceleration (°/s²)
+
+    # Update rates
+    update_rate_hz: 20.0       # motion tick + JLINK TX rate
+    state_publish_hz: 10.0     # /saltybot/gimbal/state publish rate
+
+    # Serial reconnect
+    reconnect_delay_s: 2.0
+
+    # D435i camera parameters (for look_at 3D→pan/tilt projection)
+    camera_focal_length_px: 600.0
+    image_width_px: 848
+    image_height_px: 480

jetson/ros2_ws/src/saltybot_gimbal/launch/gimbal.launch.py

@@ -0,0 +1,50 @@
+"""gimbal.launch.py — Launch saltybot_gimbal node (Issue #548)."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg = get_package_share_directory("saltybot_gimbal")
+
+    serial_port_arg = DeclareLaunchArgument(
+        "serial_port",
+        default_value="/dev/ttyTHS1",
+        description="JLINK serial port to STM32",
+    )
+    pan_limit_arg = DeclareLaunchArgument(
+        "pan_limit_deg",
+        default_value="150.0",
+        description="Pan soft limit ± degrees",
+    )
+    tilt_limit_arg = DeclareLaunchArgument(
+        "tilt_limit_deg",
+        default_value="45.0",
+        description="Tilt soft limit ± degrees",
+    )
+
+    gimbal_node = Node(
+        package="saltybot_gimbal",
+        executable="gimbal_node",
+        name="gimbal_node",
+        output="screen",
+        parameters=[
+            os.path.join(pkg, "config", "gimbal_params.yaml"),
+            {
+                "serial_port":    LaunchConfiguration("serial_port"),
+                "pan_limit_deg":  LaunchConfiguration("pan_limit_deg"),
+                "tilt_limit_deg": LaunchConfiguration("tilt_limit_deg"),
+            },
+        ],
+    )
+
+    return LaunchDescription([
+        serial_port_arg,
+        pan_limit_arg,
+        tilt_limit_arg,
+        gimbal_node,
+    ])

jetson/ros2_ws/src/saltybot_gimbal/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>saltybot_gimbal</name>
+  <version>1.0.0</version>
+  <description>
+    ROS2 gimbal control node: pan/tilt camera head via JLINK serial to STM32.
+    Smooth trapezoidal motion profiles, configurable limits, look_at 3D projection.
+    Issue #548.
+  </description>
+  <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+
+  <test_depend>pytest</test_depend>
+</package>

jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/gimbal_node.py

@@ -0,0 +1,497 @@
+#!/usr/bin/env python3
+"""gimbal_node.py — ROS2 gimbal control node for SaltyBot pan/tilt camera head (Issue #548).
+
+Controls pan/tilt gimbal via JLINK binary protocol over serial to STM32.
+Implements smooth trapezoidal motion profiles with configurable axis limits.
+
+Subscribed topics:
+  /saltybot/gimbal/cmd       (geometry_msgs/Vector3)  x=pan_deg, y=tilt_deg, z=speed_deg_s
+  /saltybot/gimbal/look_at_target (geometry_msgs/PointStamped) 3D look-at target (camera frame)
+
+Published topics:
+  /saltybot/gimbal/state     (std_msgs/String)    JSON: pan_deg, tilt_deg, moving, fault, …
+  /saltybot/gimbal/cmd_echo  (geometry_msgs/Vector3)  current target cmd (monitoring)
+
+Services:
+  /saltybot/gimbal/home      (std_srvs/Trigger)   command gimbal to home position
+  /saltybot/gimbal/look_at   (std_srvs/Trigger)   execute look-at to last look_at_target
+
+Parameters (config/gimbal_params.yaml):
+  serial_port          /dev/ttyTHS1   JLINK serial device
+  baud_rate            921600
+  pan_limit_deg        150.0          soft limit ± degrees
+  tilt_limit_deg        45.0          soft limit ± degrees
+  home_pan_deg           0.0          home pan position
+  home_tilt_deg          0.0          home tilt position
+  max_speed_deg_s       90.0          default motion speed
+  accel_deg_s2         180.0          trapezoidal acceleration
+  update_rate_hz        20.0          motion profile update rate
+  state_publish_hz      10.0          /saltybot/gimbal/state publish rate
+  reconnect_delay_s      2.0          serial reconnect interval
+  camera_focal_length_px 600.0        D435i focal length (for look_at projection)
+  image_width_px         848
+  image_height_px        480
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import threading
+import time
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+import serial
+
+from geometry_msgs.msg import PointStamped, Vector3
+from std_msgs.msg import String
+from std_srvs.srv import Trigger
+
+from .jlink_gimbal import (
+    encode_gimbal_pos,
+    encode_gimbal_home,
+    decode_gimbal_state,
+    TLM_GIMBAL_STATE,
+    GimbalStateFrame,
+    STX, ETX,
+    _crc16_ccitt,
+)
+
+
+def _clamp(v: float, lo: float, hi: float) -> float:
+    return max(lo, min(hi, v))
+
+
+class MotionAxis:
+    """Single-axis trapezoidal velocity profile.
+
+    Tracks current position and velocity; each tick() call advances the
+    axis toward the target using bounded acceleration and velocity.
+    """
+
+    def __init__(self, initial_pos: float, max_speed: float, accel: float) -> None:
+        self.pos = initial_pos
+        self.vel = 0.0
+        self.target = initial_pos
+        self.max_speed = max_speed
+        self.accel = accel
+
+    def set_target(self, target: float, speed: Optional[float] = None) -> None:
+        self.target = target
+        if speed is not None:
+            self.max_speed = max(1.0, speed)
+
+    def tick(self, dt: float) -> float:
+        """Advance one timestep. Returns new position."""
+        error = self.target - self.pos
+        if abs(error) < 0.01:
+            self.pos = self.target
+            self.vel = 0.0
+            return self.pos
+
+        # Deceleration distance at current speed: v²/(2a)
+        decel_dist = (self.vel ** 2) / (2.0 * self.accel) if self.accel > 0 else 0.0
+        direction = 1.0 if error > 0 else -1.0
+
+        if abs(error) <= decel_dist + 0.01:
+            # Deceleration phase
+            desired_vel = direction * math.sqrt(max(0.0, 2.0 * self.accel * abs(error)))
+        else:
+            # Acceleration/cruise phase
+            desired_vel = direction * self.max_speed
+
+        # Apply acceleration limit
+        dv = desired_vel - self.vel
+        dv = _clamp(dv, -self.accel * dt, self.accel * dt)
+        self.vel += dv
+        self.vel = _clamp(self.vel, -self.max_speed, self.max_speed)
+
+        self.pos += self.vel * dt
+        # Don't overshoot
+        if direction > 0 and self.pos > self.target:
+            self.pos = self.target
+            self.vel = 0.0
+        elif direction < 0 and self.pos < self.target:
+            self.pos = self.target
+            self.vel = 0.0
+
+        return self.pos
+
+    @property
+    def is_moving(self) -> bool:
+        return abs(self.target - self.pos) > 0.05 or abs(self.vel) > 0.05
+
+
+class JLinkGimbalSerial:
+    """Serial connection manager for JLINK gimbal frames.
+
+    Handles connect/disconnect/reconnect and provides send() and
+    read_state() methods. Thread-safe via internal lock.
+    """
+
+    _PARSER_WAIT_STX  = 0
+    _PARSER_WAIT_CMD  = 1
+    _PARSER_WAIT_LEN  = 2
+    _PARSER_PAYLOAD   = 3
+    _PARSER_CRC_HI    = 4
+    _PARSER_CRC_LO    = 5
+    _PARSER_WAIT_ETX  = 6
+
+    def __init__(self, port: str, baud: int) -> None:
+        self._port = port
+        self._baud = baud
+        self._ser: Optional[serial.Serial] = None
+        self._lock = threading.Lock()
+        self._parser_state = self._PARSER_WAIT_STX
+        self._parser_cmd = 0
+        self._parser_len = 0
+        self._parser_payload = bytearray()
+        self._parser_crc_rcvd = 0
+
+    def connect(self) -> bool:
+        try:
+            self._ser = serial.Serial(
+                port=self._port,
+                baudrate=self._baud,
+                timeout=0.05,
+                bytesize=8,
+                parity=serial.PARITY_NONE,
+                stopbits=1,
+            )
+            self._reset_parser()
+            return True
+        except Exception as e:
+            print(f"[gimbal] serial connect failed ({self._port}): {e}")
+            self._ser = None
+            return False
+
+    def disconnect(self) -> None:
+        with self._lock:
+            if self._ser:
+                self._ser.close()
+                self._ser = None
+
+    def is_connected(self) -> bool:
+        return self._ser is not None and self._ser.is_open
+
+    def send(self, frame: bytes) -> bool:
+        with self._lock:
+            if not self.is_connected():
+                return False
+            try:
+                self._ser.write(frame)
+                return True
+            except Exception as e:
+                print(f"[gimbal] serial write error: {e}")
+                self._ser = None
+                return False
+
+    def read_pending(self) -> Optional[GimbalStateFrame]:
+        """Drain RX buffer; return GimbalStateFrame if a complete telemetry
+        frame was received, or None if no complete frame is available."""
+        with self._lock:
+            if not self.is_connected():
+                return None
+            try:
+                raw = self._ser.read(self._ser.in_waiting or 1)
+            except Exception:
+                self._ser = None
+                return None
+
+        result = None
+        for b in raw:
+            frame = self._feed(b)
+            if frame is not None:
+                result = frame  # keep last complete frame
+        return result
+
+    def _reset_parser(self) -> None:
+        self._parser_state = self._PARSER_WAIT_STX
+        self._parser_payload = bytearray()
+
+    def _feed(self, byte: int) -> Optional[GimbalStateFrame]:
+        s = self._parser_state
+
+        if s == self._PARSER_WAIT_STX:
+            if byte == STX:
+                self._parser_state = self._PARSER_WAIT_CMD
+            return None
+
+        if s == self._PARSER_WAIT_CMD:
+            self._parser_cmd = byte
+            self._parser_state = self._PARSER_WAIT_LEN
+            return None
+
+        if s == self._PARSER_WAIT_LEN:
+            self._parser_len = byte
+            self._parser_payload = bytearray()
+            if byte > 64:
+                self._reset_parser()
+                return None
+            self._parser_state = self._PARSER_PAYLOAD if byte > 0 else self._PARSER_CRC_HI
+            return None
+
+        if s == self._PARSER_PAYLOAD:
+            self._parser_payload.append(byte)
+            if len(self._parser_payload) == self._parser_len:
+                self._parser_state = self._PARSER_CRC_HI
+            return None
+
+        if s == self._PARSER_CRC_HI:
+            self._parser_crc_rcvd = byte << 8
+            self._parser_state = self._PARSER_CRC_LO
+            return None
+
+        if s == self._PARSER_CRC_LO:
+            self._parser_crc_rcvd |= byte
+            self._parser_state = self._PARSER_WAIT_ETX
+            return None
+
+        if s == self._PARSER_WAIT_ETX:
+            self._reset_parser()
+            if byte != ETX:
+                return None
+            crc_data = bytes([self._parser_cmd, self._parser_len]) + self._parser_payload
+            if _crc16_ccitt(crc_data) != self._parser_crc_rcvd:
+                return None
+            if self._parser_cmd == TLM_GIMBAL_STATE:
+                return decode_gimbal_state(bytes(self._parser_payload))
+            return None
+
+        self._reset_parser()
+        return None
+
+
+class GimbalNode(Node):
+    """ROS2 gimbal control node: smooth motion profiles + JLINK serial bridge."""
+
+    def __init__(self) -> None:
+        super().__init__("gimbal_node")
+
+        # ── Parameters ─────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",           "/dev/ttyTHS1")
+        self.declare_parameter("baud_rate",             921600)
+        self.declare_parameter("pan_limit_deg",         150.0)
+        self.declare_parameter("tilt_limit_deg",         45.0)
+        self.declare_parameter("home_pan_deg",            0.0)
+        self.declare_parameter("home_tilt_deg",           0.0)
+        self.declare_parameter("max_speed_deg_s",        90.0)
+        self.declare_parameter("accel_deg_s2",          180.0)
+        self.declare_parameter("update_rate_hz",         20.0)
+        self.declare_parameter("state_publish_hz",       10.0)
+        self.declare_parameter("reconnect_delay_s",       2.0)
+        self.declare_parameter("camera_focal_length_px", 600.0)
+        self.declare_parameter("image_width_px",          848)
+        self.declare_parameter("image_height_px",         480)
+
+        self._port           = self.get_parameter("serial_port").value
+        self._baud           = self.get_parameter("baud_rate").value
+        self._pan_limit      = self.get_parameter("pan_limit_deg").value
+        self._tilt_limit     = self.get_parameter("tilt_limit_deg").value
+        self._home_pan       = self.get_parameter("home_pan_deg").value
+        self._home_tilt      = self.get_parameter("home_tilt_deg").value
+        self._max_speed      = self.get_parameter("max_speed_deg_s").value
+        self._accel          = self.get_parameter("accel_deg_s2").value
+        update_hz            = self.get_parameter("update_rate_hz").value
+        state_hz             = self.get_parameter("state_publish_hz").value
+        self._reconnect_del  = self.get_parameter("reconnect_delay_s").value
+        self._focal_px       = self.get_parameter("camera_focal_length_px").value
+        self._img_w          = self.get_parameter("image_width_px").value
+        self._img_h          = self.get_parameter("image_height_px").value
+
+        # ── State ──────────────────────────────────────────────────────────
+        self._pan_axis  = MotionAxis(self._home_pan,  self._max_speed, self._accel)
+        self._tilt_axis = MotionAxis(self._home_tilt, self._max_speed, self._accel)
+
+        self._hw_state:  Optional[GimbalStateFrame] = None
+        self._look_at_target: Optional[PointStamped] = None
+        self._last_cmd_time = time.monotonic()
+        self._reconnect_ts  = 0.0
+        self._state_lock    = threading.Lock()
+
+        # ── Serial ─────────────────────────────────────────────────────────
+        self._serial = JLinkGimbalSerial(self._port, self._baud)
+        if not self._serial.connect():
+            self.get_logger().warn(
+                f"[gimbal] serial not available on {self._port} — will retry"
+            )
+
+        # ── Subscribers ────────────────────────────────────────────────────
+        qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=5,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+        )
+        self.create_subscription(Vector3, "/saltybot/gimbal/cmd",
+                                 self._on_cmd, qos)
+        self.create_subscription(PointStamped, "/saltybot/gimbal/look_at_target",
+                                 self._on_look_at_target, 10)
+
+        # ── Publishers ─────────────────────────────────────────────────────
+        self._pub_state    = self.create_publisher(String,  "/saltybot/gimbal/state",    10)
+        self._pub_cmd_echo = self.create_publisher(Vector3, "/saltybot/gimbal/cmd_echo", 10)
+
+        # ── Services ───────────────────────────────────────────────────────
+        self.create_service(Trigger, "/saltybot/gimbal/home",    self._svc_home)
+        self.create_service(Trigger, "/saltybot/gimbal/look_at", self._svc_look_at)
+
+        # ── Timers ─────────────────────────────────────────────────────────
+        self.create_timer(1.0 / update_hz, self._motion_tick)
+        self.create_timer(1.0 / state_hz,  self._publish_state)
+
+        self.get_logger().info(
+            f"[gimbal] node ready — port={self._port} "
+            f"pan=±{self._pan_limit}° tilt=±{self._tilt_limit}° "
+            f"speed={self._max_speed}°/s accel={self._accel}°/s²"
+        )
+
+    # ── Subscriber callbacks ───────────────────────────────────────────────
+
+    def _on_cmd(self, msg: Vector3) -> None:
+        """Handle /saltybot/gimbal/cmd (x=pan, y=tilt, z=speed_deg_s)."""
+        pan   = _clamp(msg.x, -self._pan_limit,  self._pan_limit)
+        tilt  = _clamp(msg.y, -self._tilt_limit, self._tilt_limit)
+        speed = msg.z if msg.z > 0.0 else self._max_speed
+        speed = _clamp(speed, 1.0, self._max_speed)
+        with self._state_lock:
+            self._pan_axis.set_target(pan,  speed)
+            self._tilt_axis.set_target(tilt, speed)
+        self._last_cmd_time = time.monotonic()
+
+    def _on_look_at_target(self, msg: PointStamped) -> None:
+        """Cache look-at 3D target for use by /look_at service."""
+        self._look_at_target = msg
+
+    # ── Service callbacks ──────────────────────────────────────────────────
+
+    def _svc_home(self, _req: Trigger.Request, resp: Trigger.Response) -> Trigger.Response:
+        """Return gimbal to home position."""
+        with self._state_lock:
+            self._pan_axis.set_target(self._home_pan,  self._max_speed)
+            self._tilt_axis.set_target(self._home_tilt, self._max_speed)
+        self._serial.send(encode_gimbal_home())
+        resp.success = True
+        resp.message = f"Homing to pan={self._home_pan}° tilt={self._home_tilt}°"
+        self.get_logger().info("[gimbal] home command")
+        return resp
+
+    def _svc_look_at(self, _req: Trigger.Request, resp: Trigger.Response) -> Trigger.Response:
+        """Convert last look_at_target point to pan/tilt and command motion."""
+        tgt = self._look_at_target
+        if tgt is None:
+            resp.success = False
+            resp.message = "No look_at_target received on /saltybot/gimbal/look_at_target"
+            return resp
+
+        # Project 3D point (camera frame) → pan/tilt angles.
+        # Assumes point is in camera_depth_optical_frame (Z=forward, X=right, Y=down).
+        x, y, z = tgt.point.x, tgt.point.y, tgt.point.z
+        if z <= 0.0:
+            resp.success = False
+            resp.message = f"Invalid look_at_target: z={z:.3f} (must be > 0)"
+            return resp
+
+        pan_deg  =  math.degrees(math.atan2(x, z))
+        tilt_deg = -math.degrees(math.atan2(y, z))  # negative: down in image = tilt up
+
+        pan_deg  = _clamp(pan_deg,  -self._pan_limit,  self._pan_limit)
+        tilt_deg = _clamp(tilt_deg, -self._tilt_limit, self._tilt_limit)
+
+        with self._state_lock:
+            self._pan_axis.set_target(pan_deg,  self._max_speed)
+            self._tilt_axis.set_target(tilt_deg, self._max_speed)
+
+        resp.success = True
+        resp.message = (
+            f"Looking at ({x:.2f}, {y:.2f}, {z:.2f}) → "
+            f"pan={pan_deg:.1f}° tilt={tilt_deg:.1f}°"
+        )
+        self.get_logger().info(f"[gimbal] look_at: {resp.message}")
+        return resp
+
+    # ── Motion tick ────────────────────────────────────────────────────────
+
+    def _motion_tick(self) -> None:
+        """Advance motion profiles and send JLINK frame at update_rate_hz."""
+        # Reconnect if needed
+        if not self._serial.is_connected():
+            now = time.monotonic()
+            if now - self._reconnect_ts >= self._reconnect_del:
+                self._reconnect_ts = now
+                if self._serial.connect():
+                    self.get_logger().info(f"[gimbal] serial reconnected on {self._port}")
+
+        dt = 1.0 / self.get_parameter("update_rate_hz").value
+
+        with self._state_lock:
+            pan  = self._pan_axis.tick(dt)
+            tilt = self._tilt_axis.tick(dt)
+            moving = self._pan_axis.is_moving or self._tilt_axis.is_moving
+            speed = max(self._pan_axis.max_speed, self._tilt_axis.max_speed)
+
+        # Send JLINK frame
+        frame = encode_gimbal_pos(pan, tilt, speed)
+        self._serial.send(frame)
+
+        # Read telemetry
+        hw = self._serial.read_pending()
+        if hw is not None:
+            with self._state_lock:
+                self._hw_state = hw
+
+        # Echo command
+        echo = Vector3(x=pan, y=tilt, z=speed)
+        self._pub_cmd_echo.publish(echo)
+
+    # ── State publisher ────────────────────────────────────────────────────
+
+    def _publish_state(self) -> None:
+        """Publish gimbal state as JSON string."""
+        with self._state_lock:
+            pan    = self._pan_axis.pos
+            tilt   = self._tilt_axis.pos
+            pan_t  = self._pan_axis.target
+            tilt_t = self._tilt_axis.target
+            moving = self._pan_axis.is_moving or self._tilt_axis.is_moving
+            hw     = self._hw_state
+
+        state: dict = {
+            "pan_deg":       round(pan,  2),
+            "tilt_deg":      round(tilt, 2),
+            "pan_target_deg":  round(pan_t,  2),
+            "tilt_target_deg": round(tilt_t, 2),
+            "moving":        moving,
+            "serial_ok":     self._serial.is_connected(),
+        }
+        if hw is not None:
+            state["hw_pan_deg"]        = round(hw.pan_deg, 1)
+            state["hw_tilt_deg"]       = round(hw.tilt_deg, 1)
+            state["hw_pan_speed_raw"]  = hw.pan_speed_raw
+            state["hw_tilt_speed_raw"] = hw.tilt_speed_raw
+            state["hw_torque_en"]      = hw.torque_en
+            state["hw_rx_err_pct"]     = hw.rx_err_pct
+
+        self._pub_state.publish(String(data=json.dumps(state)))
+
+    def destroy_node(self) -> None:
+        self._serial.disconnect()
+        super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = GimbalNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_gimbal/saltybot_gimbal/jlink_gimbal.py

@@ -0,0 +1,141 @@
+"""jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548).
+
+Matches the JLINK protocol defined in include/jlink.h (Issue #547 STM32 side).
+
+Command type (Jetson → STM32):
+  0x0B  GIMBAL_POS  — int16 pan_x10 + int16 tilt_x10 + uint16 speed   (6 bytes)
+                       pan_x10  = pan_deg  * 10  (±1500 for ±150°)
+                       tilt_x10 = tilt_deg * 10  (±450  for ±45°)
+                       speed    = servo speed register 0–4095 (0 = max)
+
+Telemetry type (STM32 → Jetson):
+  0x84  GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 +
+                        uint16 pan_speed_raw + uint16 tilt_speed_raw +
+                        uint8 torque_en + uint8 rx_err_pct             (10 bytes)
+
+Frame format (shared with stm32_protocol.py):
+  [STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
+  CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD bytes.
+"""
+
+from __future__ import annotations
+
+import struct
+from dataclasses import dataclass
+from typing import Optional
+
+# ── Frame constants ────────────────────────────────────────────────────────────
+
+STX = 0x02
+ETX = 0x03
+
+# ── Command / telemetry type codes ─────────────────────────────────────────────
+
+CMD_GIMBAL_POS   = 0x0B   # Jetson → STM32: set pan/tilt target
+TLM_GIMBAL_STATE = 0x84   # STM32 → Jetson: measured state
+
+# Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed.
+# Map deg/s to this register: speed_reg = max(0, 4095 - int(deg_s * 4095 / 360))
+_MAX_SPEED_DEGS = 360.0   # degrees/sec at speed_reg=0
+
+
+# ── Parsed telemetry ───────────────────────────────────────────────────────────
+
+@dataclass
+class GimbalStateFrame:
+    pan_deg:        float   # measured pan position (degrees, + = right)
+    tilt_deg:       float   # measured tilt position (degrees, + = up)
+    pan_speed_raw:  int     # pan servo present-speed register value
+    tilt_speed_raw: int     # tilt servo present-speed register value
+    torque_en:      bool    # True = servo torque is on
+    rx_err_pct:     int     # SCS bus error rate 0–100 %
+
+
+# ── CRC16-CCITT ────────────────────────────────────────────────────────────────
+
+def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
+    """CRC16-CCITT: poly=0x1021, init=0xFFFF, no final XOR."""
+    crc = init
+    for byte in data:
+        crc ^= byte << 8
+        for _ in range(8):
+            crc = ((crc << 1) ^ 0x1021) if (crc & 0x8000) else (crc << 1)
+        crc &= 0xFFFF
+    return crc
+
+
+# ── Frame builder ──────────────────────────────────────────────────────────────
+
+def _build_frame(cmd: int, payload: bytes) -> bytes:
+    length = len(payload)
+    header = bytes([cmd, length])
+    crc = _crc16_ccitt(header + payload)
+    return bytes([STX, cmd, length]) + payload + struct.pack(">H", crc) + bytes([ETX])
+
+
+# ── Speed conversion ───────────────────────────────────────────────────────────
+
+def degs_to_speed_reg(deg_s: float) -> int:
+    """Convert degrees/second to servo speed register value.
+
+    speed_reg=0 is maximum speed; speed_reg=4095 is the slowest non-zero.
+    Linear mapping: 0 deg/s → 0 (max), 360 deg/s → 0, clamped.
+    """
+    if deg_s <= 0.0:
+        return 0  # max speed
+    ratio = min(1.0, deg_s / _MAX_SPEED_DEGS)
+    return max(0, min(4095, int((1.0 - ratio) * 4095)))
+
+
+def speed_reg_to_degs(reg: int) -> float:
+    """Convert speed register to approximate degrees/second."""
+    if reg == 0:
+        return _MAX_SPEED_DEGS
+    return (1.0 - reg / 4095.0) * _MAX_SPEED_DEGS
+
+
+# ── Encoder ────────────────────────────────────────────────────────────────────
+
+def encode_gimbal_pos(pan_deg: float, tilt_deg: float, speed_deg_s: float) -> bytes:
+    """Build GIMBAL_POS frame.
+
+    Args:
+        pan_deg:     Pan angle (degrees, clamped to ±327°).
+        tilt_deg:    Tilt angle (degrees, clamped to ±327°).
+        speed_deg_s: Motion speed (degrees/second; 0 = max).
+
+    Returns:
+        Complete JLINK binary frame (10 bytes total).
+    """
+    pan_x10  = max(-32767, min(32767, int(round(pan_deg  * 10))))
+    tilt_x10 = max(-32767, min(32767, int(round(tilt_deg * 10))))
+    speed    = degs_to_speed_reg(speed_deg_s)
+    return _build_frame(CMD_GIMBAL_POS, struct.pack(">hhH", pan_x10, tilt_x10, speed))
+
+
+def encode_gimbal_home() -> bytes:
+    """Build GIMBAL_POS frame targeting (0°, 0°) at default speed."""
+    return encode_gimbal_pos(0.0, 0.0, 0.0)  # speed=0 → max speed for homing
+
+
+# ── Decoder ────────────────────────────────────────────────────────────────────
+
+def decode_gimbal_state(payload: bytes) -> Optional[GimbalStateFrame]:
+    """Decode GIMBAL_STATE telemetry payload (10 bytes).
+
+    Returns:
+        GimbalStateFrame on success, None if payload is too short.
+    """
+    if len(payload) < 10:
+        return None
+    pan_x10, tilt_x10, pan_spd, tilt_spd, torque_en, rx_err_pct = (
+        struct.unpack_from(">hhHHBB", payload)
+    )
+    return GimbalStateFrame(
+        pan_deg=pan_x10 / 10.0,
+        tilt_deg=tilt_x10 / 10.0,
+        pan_speed_raw=pan_spd,
+        tilt_speed_raw=tilt_spd,
+        torque_en=bool(torque_en),
+        rx_err_pct=int(rx_err_pct),
+    )

jetson/ros2_ws/src/saltybot_gimbal/setup.cfg

@@ -0,0 +1,2 @@
+[develop]
+script_dir=$base/lib/saltybot_gimbal/scripts

jetson/ros2_ws/src/saltybot_gimbal/setup.py

@@ -0,0 +1,23 @@
+from setuptools import setup
+
+package_name = 'saltybot_gimbal'
+
+setup(
+    name=package_name,
+    version='1.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/gimbal.launch.py']),
+        ('share/' + package_name + '/config', ['config/gimbal_params.yaml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    author='Salty Lab',
+    entry_points={
+        'console_scripts': [
+            'gimbal_node = saltybot_gimbal.gimbal_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_gimbal/test/test_gimbal.py

@@ -0,0 +1,422 @@
+"""test_gimbal.py — Unit tests for saltybot_gimbal (Issue #548).
+
+Runs without hardware: all serial I/O is mocked.
+"""
+
+import math
+import struct
+import sys
+import types
+import unittest
+from unittest.mock import MagicMock
+
+# ── Stub ROS2 / serial so tests run offline ────────────────────────────────────
+
+def _make_rclpy_stub():
+    rclpy = types.ModuleType("rclpy")
+    rclpy.init = lambda **_: None
+    rclpy.spin = lambda _: None
+    rclpy.shutdown = lambda: None
+
+    node_mod = types.ModuleType("rclpy.node")
+    class _Node:
+        def __init__(self, *a, **kw): pass
+        def declare_parameter(self, *a, **kw): pass
+        def get_parameter(self, name):
+            defaults = {
+                "serial_port": "/dev/null", "baud_rate": 921600,
+                "pan_limit_deg": 150.0, "tilt_limit_deg": 45.0,
+                "home_pan_deg": 0.0, "home_tilt_deg": 0.0,
+                "max_speed_deg_s": 90.0, "accel_deg_s2": 180.0,
+                "update_rate_hz": 20.0, "state_publish_hz": 10.0,
+                "reconnect_delay_s": 2.0,
+                "camera_focal_length_px": 600.0,
+                "image_width_px": 848, "image_height_px": 480,
+            }
+            m = MagicMock()
+            m.value = defaults.get(name, 0)
+            return m
+        def get_logger(self): return MagicMock()
+        def create_subscription(self, *a, **kw): pass
+        def create_publisher(self, *a, **kw): return MagicMock()
+        def create_service(self, *a, **kw): pass
+        def create_timer(self, *a, **kw): pass
+        def destroy_node(self): pass
+    node_mod.Node = _Node
+    rclpy.node = node_mod
+
+    qos_mod = types.ModuleType("rclpy.qos")
+    qos_mod.QoSProfile = MagicMock(return_value=None)
+    qos_mod.HistoryPolicy = MagicMock()
+    qos_mod.ReliabilityPolicy = MagicMock()
+    rclpy.qos = qos_mod
+
+    return rclpy
+
+
+def _install_stubs():
+    rclpy_stub = _make_rclpy_stub()
+    sys.modules.setdefault("rclpy", rclpy_stub)
+    sys.modules.setdefault("rclpy.node", rclpy_stub.node)
+    sys.modules.setdefault("rclpy.qos", rclpy_stub.qos)
+
+    geo = types.ModuleType("geometry_msgs")
+    geo_msg = types.ModuleType("geometry_msgs.msg")
+    class _Vector3:
+        def __init__(self, x=0.0, y=0.0, z=0.0): self.x=x; self.y=y; self.z=z
+    class _PointStamped:
+        def __init__(self): self.point = _Vector3()
+    geo_msg.Vector3 = _Vector3
+    geo_msg.PointStamped = _PointStamped
+    geo.msg = geo_msg
+    sys.modules.setdefault("geometry_msgs", geo)
+    sys.modules.setdefault("geometry_msgs.msg", geo_msg)
+
+    std_msgs = types.ModuleType("std_msgs")
+    std_msgs_msg = types.ModuleType("std_msgs.msg")
+    class _String:
+        def __init__(self, data=""): self.data = data
+    std_msgs_msg.String = _String
+    std_msgs.msg = std_msgs_msg
+    sys.modules.setdefault("std_msgs", std_msgs)
+    sys.modules.setdefault("std_msgs.msg", std_msgs_msg)
+
+    std_srvs = types.ModuleType("std_srvs")
+    std_srvs_srv = types.ModuleType("std_srvs.srv")
+    class _Trigger:
+        class Request: pass
+        class Response:
+            success = False
+            message = ""
+    std_srvs_srv.Trigger = _Trigger
+    std_srvs.srv = std_srvs_srv
+    sys.modules.setdefault("std_srvs", std_srvs)
+    sys.modules.setdefault("std_srvs.srv", std_srvs_srv)
+    sys.modules.setdefault("serial", MagicMock())
+
+
+_install_stubs()
+
+from saltybot_gimbal.jlink_gimbal import (  # noqa: E402
+    encode_gimbal_pos, encode_gimbal_home, decode_gimbal_state,
+    GimbalStateFrame, CMD_GIMBAL_POS, TLM_GIMBAL_STATE,
+    STX, ETX, _crc16_ccitt, _build_frame,
+    degs_to_speed_reg, speed_reg_to_degs,
+)
+from saltybot_gimbal.gimbal_node import MotionAxis, _clamp  # noqa: E402
+
+
+# ── Helper ─────────────────────────────────────────────────────────────────────
+
+def _parse_frame(raw: bytes):
+    """Parse a JLINK frame; return (cmd, payload) or raise on error."""
+    assert raw[0] == STX, f"STX expected, got 0x{raw[0]:02x}"
+    cmd = raw[1]
+    length = raw[2]
+    payload = raw[3:3 + length]
+    crc_hi = raw[3 + length]
+    crc_lo = raw[4 + length]
+    crc_rcvd = (crc_hi << 8) | crc_lo
+    assert raw[-1] == ETX, f"ETX expected, got 0x{raw[-1]:02x}"
+    crc_data = bytes([cmd, length]) + payload
+    crc_calc = _crc16_ccitt(crc_data)
+    assert crc_rcvd == crc_calc, (
+        f"CRC mismatch: got {crc_rcvd:#06x}, expected {crc_calc:#06x}"
+    )
+    return cmd, payload
+
+
+# ── CRC16 Tests ────────────────────────────────────────────────────────────────
+
+class TestCRC16(unittest.TestCase):
+
+    def test_empty_data(self):
+        self.assertEqual(_crc16_ccitt(b""), 0xFFFF)
+
+    def test_known_value_123456789(self):
+        # Standard CCITT-FFFF check value for "123456789"
+        self.assertEqual(_crc16_ccitt(b"123456789"), 0x29B1)
+
+    def test_single_byte_is_int(self):
+        v = _crc16_ccitt(b"\x0B")
+        self.assertIsInstance(v, int)
+        self.assertLessEqual(v, 0xFFFF)
+
+    def test_different_data_different_crc(self):
+        self.assertNotEqual(_crc16_ccitt(b"\x00\x01"), _crc16_ccitt(b"\x00\x02"))
+
+
+# ── Speed Conversion Tests ─────────────────────────────────────────────────────
+
+class TestSpeedConversion(unittest.TestCase):
+
+    def test_zero_speed_is_max(self):
+        self.assertEqual(degs_to_speed_reg(0.0), 0)
+
+    def test_negative_speed_is_max(self):
+        self.assertEqual(degs_to_speed_reg(-10.0), 0)
+
+    def test_360_degs_maps_to_zero(self):
+        # 360°/s → ratio=1 → reg=0 (max)
+        self.assertEqual(degs_to_speed_reg(360.0), 0)
+
+    def test_mid_speed(self):
+        reg = degs_to_speed_reg(90.0)
+        self.assertGreater(reg, 0)
+        self.assertLess(reg, 4095)
+
+    def test_round_trip_approx(self):
+        for deg_s in (30.0, 60.0, 120.0, 200.0):
+            reg = degs_to_speed_reg(deg_s)
+            back = speed_reg_to_degs(reg)
+            self.assertAlmostEqual(back, deg_s, delta=5.0)
+
+    def test_reg_zero_is_max_speed(self):
+        self.assertAlmostEqual(speed_reg_to_degs(0), 360.0)
+
+
+# ── Encode Tests ───────────────────────────────────────────────────────────────
+
+class TestEncodeGimbalPos(unittest.TestCase):
+
+    def test_zero_pose(self):
+        raw = encode_gimbal_pos(0.0, 0.0, 90.0)
+        cmd, payload = _parse_frame(raw)
+        self.assertEqual(cmd, CMD_GIMBAL_POS)  # 0x0B
+        pan_x10, tilt_x10, speed = struct.unpack(">hhH", payload)
+        self.assertEqual(pan_x10, 0)
+        self.assertEqual(tilt_x10, 0)
+
+    def test_positive_pan_tilt(self):
+        raw = encode_gimbal_pos(30.0, 15.0, 45.0)
+        _, payload = _parse_frame(raw)
+        pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
+        self.assertEqual(pan_x10, 300)   # 30.0 * 10
+        self.assertEqual(tilt_x10, 150)  # 15.0 * 10
+
+    def test_negative_angles(self):
+        raw = encode_gimbal_pos(-90.0, -20.0, 60.0)
+        _, payload = _parse_frame(raw)
+        pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
+        self.assertEqual(pan_x10, -900)
+        self.assertEqual(tilt_x10, -200)
+
+    def test_pan_limit_150_deg(self):
+        raw = encode_gimbal_pos(150.0, 0.0, 30.0)
+        _, payload = _parse_frame(raw)
+        pan_x10, _, _ = struct.unpack(">hhH", payload)
+        self.assertEqual(pan_x10, 1500)
+
+    def test_tilt_limit_45_deg(self):
+        raw = encode_gimbal_pos(0.0, 45.0, 30.0)
+        _, payload = _parse_frame(raw)
+        _, tilt_x10, _ = struct.unpack(">hhH", payload)
+        self.assertEqual(tilt_x10, 450)
+
+    def test_extreme_clamped(self):
+        raw = encode_gimbal_pos(9999.0, -9999.0, 0.0)
+        _, payload = _parse_frame(raw)
+        pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
+        self.assertEqual(pan_x10, 32767)
+        self.assertEqual(tilt_x10, -32767)
+
+    def test_frame_byte_layout(self):
+        raw = encode_gimbal_pos(10.0, -5.0, 30.0)
+        self.assertEqual(raw[0], STX)
+        self.assertEqual(raw[-1], ETX)
+        self.assertEqual(raw[1], CMD_GIMBAL_POS)  # 0x0B
+        self.assertEqual(raw[2], 6)               # 6-byte payload
+
+    def test_crc_valid(self):
+        # _parse_frame raises if CRC is bad
+        _parse_frame(encode_gimbal_pos(45.0, -10.0, 120.0))
+
+
+class TestEncodeGimbalHome(unittest.TestCase):
+
+    def test_homes_to_zero(self):
+        raw = encode_gimbal_home()
+        cmd, payload = _parse_frame(raw)
+        self.assertEqual(cmd, CMD_GIMBAL_POS)
+        pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
+        self.assertEqual(pan_x10, 0)
+        self.assertEqual(tilt_x10, 0)
+
+    def test_crc_valid(self):
+        _parse_frame(encode_gimbal_home())
+
+
+# ── Decode Tests ───────────────────────────────────────────────────────────────
+
+class TestDecodeGimbalState(unittest.TestCase):
+
+    def _make_payload(self, pan_x10, tilt_x10, pan_spd, tilt_spd,
+                      torque_en, rx_err_pct) -> bytes:
+        return struct.pack(">hhHHBB",
+                           pan_x10, tilt_x10,
+                           pan_spd, tilt_spd,
+                           torque_en, rx_err_pct)
+
+    def test_zero_state(self):
+        state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 0))
+        self.assertIsNotNone(state)
+        self.assertAlmostEqual(state.pan_deg, 0.0)
+        self.assertAlmostEqual(state.tilt_deg, 0.0)
+        self.assertFalse(state.torque_en)
+        self.assertEqual(state.rx_err_pct, 0)
+
+    def test_angles_decode(self):
+        state = decode_gimbal_state(self._make_payload(500, -250, 100, 200, 1, 5))
+        self.assertAlmostEqual(state.pan_deg,  50.0)
+        self.assertAlmostEqual(state.tilt_deg, -25.0)
+
+    def test_torque_en(self):
+        state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 1, 0))
+        self.assertTrue(state.torque_en)
+
+    def test_torque_off(self):
+        state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 0))
+        self.assertFalse(state.torque_en)
+
+    def test_speed_raw(self):
+        state = decode_gimbal_state(self._make_payload(0, 0, 1234, 5678, 0, 0))
+        self.assertEqual(state.pan_speed_raw, 1234)
+        self.assertEqual(state.tilt_speed_raw, 5678)
+
+    def test_rx_err_pct(self):
+        state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 42))
+        self.assertEqual(state.rx_err_pct, 42)
+
+    def test_short_payload_returns_none(self):
+        self.assertIsNone(decode_gimbal_state(b"\x00\x01\x02"))
+
+    def test_empty_payload_returns_none(self):
+        self.assertIsNone(decode_gimbal_state(b""))
+
+    def test_exactly_10_bytes_ok(self):
+        payload = self._make_payload(100, -50, 512, 512, 1, 3)
+        state = decode_gimbal_state(payload)
+        self.assertIsNotNone(state)
+
+
+# ── MotionAxis Tests ───────────────────────────────────────────────────────────
+
+class TestMotionAxis(unittest.TestCase):
+
+    def test_starts_at_initial_pos(self):
+        ax = MotionAxis(10.0, 90.0, 180.0)
+        self.assertAlmostEqual(ax.pos, 10.0)
+
+    def test_not_moving_at_rest(self):
+        ax = MotionAxis(0.0, 90.0, 180.0)
+        self.assertFalse(ax.is_moving)
+
+    def test_moves_toward_target(self):
+        ax = MotionAxis(0.0, 90.0, 180.0)
+        ax.set_target(90.0)
+        for _ in range(5):
+            ax.tick(0.05)
+        self.assertGreater(ax.pos, 0.0)
+
+    def test_reaches_target(self):
+        ax = MotionAxis(0.0, 90.0, 180.0)
+        ax.set_target(10.0)
+        for _ in range(200):
+            ax.tick(0.05)
+        self.assertAlmostEqual(ax.pos, 10.0, places=1)
+        self.assertFalse(ax.is_moving)
+
+    def test_negative_target(self):
+        ax = MotionAxis(0.0, 90.0, 180.0)
+        ax.set_target(-20.0)
+        for _ in range(200):
+            ax.tick(0.05)
+        self.assertAlmostEqual(ax.pos, -20.0, places=1)
+
+    def test_faster_speed_reaches_sooner(self):
+        ax_fast = MotionAxis(0.0, 180.0, 360.0)
+        ax_slow = MotionAxis(0.0,  30.0, 360.0)
+        ax_fast.set_target(30.0)
+        ax_slow.set_target(30.0)
+        for _ in range(5):
+            ax_fast.tick(0.05)
+            ax_slow.tick(0.05)
+        self.assertGreater(ax_fast.pos, ax_slow.pos)
+
+    def test_is_moving_while_en_route(self):
+        ax = MotionAxis(0.0, 90.0, 180.0)
+        ax.set_target(60.0)
+        ax.tick(0.05)
+        self.assertTrue(ax.is_moving)
+
+    def test_no_overshoot(self):
+        ax = MotionAxis(0.0, 90.0, 180.0)
+        ax.set_target(5.0)
+        for _ in range(500):
+            ax.tick(0.05)
+        self.assertLessEqual(ax.pos, 5.01)
+
+    def test_set_target_updates_speed(self):
+        ax = MotionAxis(0.0, 90.0, 180.0)
+        ax.set_target(10.0, speed=45.0)
+        self.assertAlmostEqual(ax.max_speed, 45.0)
+
+
+# ── Clamp Tests ────────────────────────────────────────────────────────────────
+
+class TestClamp(unittest.TestCase):
+
+    def test_within_range(self):
+        self.assertEqual(_clamp(5.0, 0.0, 10.0), 5.0)
+
+    def test_below_min(self):
+        self.assertEqual(_clamp(-5.0, 0.0, 10.0), 0.0)
+
+    def test_above_max(self):
+        self.assertEqual(_clamp(15.0, 0.0, 10.0), 10.0)
+
+    def test_at_lower_boundary(self):
+        self.assertEqual(_clamp(0.0, 0.0, 10.0), 0.0)
+
+    def test_at_upper_boundary(self):
+        self.assertEqual(_clamp(10.0, 0.0, 10.0), 10.0)
+
+
+# ── Look-at Projection Tests ───────────────────────────────────────────────────
+
+class TestLookAtProjection(unittest.TestCase):
+    """Test the look-at 3D → pan/tilt math (mirrors gimbal_node logic)."""
+
+    def _project(self, x, y, z):
+        pan_deg  =  math.degrees(math.atan2(x, z))
+        tilt_deg = -math.degrees(math.atan2(y, z))
+        return pan_deg, tilt_deg
+
+    def test_straight_ahead(self):
+        pan, tilt = self._project(0.0, 0.0, 1.0)
+        self.assertAlmostEqual(pan,  0.0, places=5)
+        self.assertAlmostEqual(tilt, 0.0, places=5)
+
+    def test_right_target(self):
+        pan, tilt = self._project(1.0, 0.0, 1.0)
+        self.assertAlmostEqual(pan,  45.0, places=4)
+        self.assertAlmostEqual(tilt,  0.0, places=4)
+
+    def test_up_target(self):
+        # camera frame: negative y = above centre → tilt up (positive)
+        pan, tilt = self._project(0.0, -1.0, 1.0)
+        self.assertAlmostEqual(pan,   0.0, places=4)
+        self.assertAlmostEqual(tilt, 45.0, places=4)
+
+    def test_left_down_target(self):
+        pan, tilt = self._project(-1.0, 1.0, 2.0)
+        self.assertLess(pan,  0.0)   # left → negative pan
+        self.assertLess(tilt, 0.0)   # below → negative tilt
+
+    def test_90_degree_right(self):
+        pan, _ = self._project(1.0, 0.0, 0.0)
+        self.assertAlmostEqual(pan, 90.0, places=4)
+
+
+if __name__ == "__main__":
+    unittest.main()

jetson/ros2_ws/src/saltybot_head_tracking/config/head_tracking_params.yaml

@@ -0,0 +1,36 @@
+head_tracking:
+  ros__parameters:
+    # ── Camera (Intel RealSense D435i) ──────────────────────────────────────
+    image_width:  848       # pixels
+    image_height: 480       # pixels
+    fov_h_deg:    87.0      # horizontal field of view (degrees)
+    fov_v_deg:    58.0      # vertical field of view (degrees)
+
+    # ── PID gains — pan axis ────────────────────────────────────────────────
+    # Output: deg/step.  Start conservative; increase kp for faster tracking.
+    pan_kp:  0.08           # proportional gain (deg per deg-of-error per step)
+    pan_ki:  0.002          # integral gain
+    pan_kd:  0.015          # derivative gain
+
+    # ── PID gains — tilt axis ───────────────────────────────────────────────
+    tilt_kp: 0.08
+    tilt_ki: 0.002
+    tilt_kd: 0.015
+
+    # ── Servo limits (degrees from centre) ──────────────────────────────────
+    pan_min_deg:  -60.0     # max left
+    pan_max_deg:   60.0     # max right
+    tilt_min_deg: -30.0     # max down
+    tilt_max_deg:  30.0     # max up
+
+    # ── Tracking behaviour ──────────────────────────────────────────────────
+    hold_duration_s:    3.0   # hold last position this long after target loss
+    center_speed_deg_s: 20.0  # degrees/sec to return to centre after hold
+    dead_zone_px:       10.0  # pixel error smaller than this is treated as zero
+    control_hz:         20.0  # PID loop rate
+
+    # ── Target selection ────────────────────────────────────────────────────
+    # score = distance_weight * (1/(1+dist_m)) + confidence_weight * confidence
+    # Highest score wins.  If distance_m == 0 (unknown), confidence only.
+    distance_weight:    0.6
+    confidence_weight:  0.4

jetson/ros2_ws/src/saltybot_head_tracking/launch/head_tracking.launch.py

@@ -0,0 +1,35 @@
+"""Launch file for saltybot_head_tracking (Issue #549)."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    config = os.path.join(
+        get_package_share_directory("saltybot_head_tracking"),
+        "config",
+        "head_tracking_params.yaml",
+    )
+
+    head_tracking_node = Node(
+        package="saltybot_head_tracking",
+        executable="head_tracking",
+        name="head_tracking",
+        parameters=[config],
+        remappings=[
+            # Remap if object detection publishes on a different topic
+            # ("/saltybot/objects", "/saltybot/objects"),
+        ],
+        output="screen",
+    )
+
+    # Note: pair with saltybot_pan_tilt node.
+    # pan_tilt_node subscribes to /saltybot/target_track (Point, pixel coords).
+    # head_tracking publishes to /saltybot/gimbal/cmd (Point, angle degrees).
+    # To bridge, remap pan_tilt's subscription:
+    #   remappings=[("/saltybot/target_track", "/saltybot/gimbal/cmd")]
+    # in the pan_tilt launch — or update pan_tilt to accept angle setpoints.
+
+    return LaunchDescription([head_tracking_node])

jetson/ros2_ws/src/saltybot_head_tracking/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_head_tracking</name>
+  <version>0.1.0</version>
+  <description>
+    Person-following head tracking (Issue #549).
+    Subscribes to YOLOv8n person detections, selects the best target
+    (closest / highest confidence), runs dual-axis PID controllers, and
+    publishes pan/tilt angle setpoints to /saltybot/gimbal/cmd.
+    Lost-target behaviour: hold 3 s then centre.
+  </description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>saltybot_object_detection_msgs</depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_head_tracking/saltybot_head_tracking/head_tracking_node.py

@@ -0,0 +1,422 @@
+#!/usr/bin/env python3
+"""Person-following head tracking node (Issue #549).
+
+Subscribes to person detection bounding boxes, selects the best target
+(closest / highest confidence), runs independent PID controllers for pan
+and tilt axes, and publishes angle setpoints to /saltybot/gimbal/cmd.
+
+Subscribed topics:
+  /saltybot/objects  (saltybot_object_detection_msgs/DetectedObjectArray)
+    — YOLOv8n detections; this node filters for class_id==0 (person).
+
+Published topics:
+  /saltybot/gimbal/cmd  (geometry_msgs/Point)
+    — x = desired pan  angle (deg, +left / −right from centre)
+    — y = desired tilt angle (deg, +up   / −down from centre)
+    — z = tracking confidence (0.0 = no target / centering)
+
+  /saltybot/head_tracking/state  (std_msgs/String)
+    — JSON status string: state, target_id, pan_deg, tilt_deg, error_pan_px,
+      error_tilt_px, confidence, distance_m
+
+State machine:
+  IDLE       → waiting for first detection
+  TRACKING   → person visible, PID active
+  LOST       → person gone; hold last angle for hold_duration_s seconds
+  CENTERING  → returning to (0°, 0°) after hold expires
+"""
+
+import json
+import math
+from dataclasses import dataclass, field
+from enum import Enum, auto
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from geometry_msgs.msg import Point
+from std_msgs.msg import String
+
+from saltybot_object_detection_msgs.msg import DetectedObject, DetectedObjectArray
+
+
+# ── PID controller ────────────────────────────────────────────────────────────
+
+class PIDController:
+    """Simple discrete-time PID with anti-windup integral clamp."""
+
+    def __init__(
+        self,
+        kp: float,
+        ki: float,
+        kd: float,
+        min_output: float,
+        max_output: float,
+        integral_clamp: float = 30.0,
+    ) -> None:
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.min_output = min_output
+        self.max_output = max_output
+        self.integral_clamp = integral_clamp
+
+        self._integral: float = 0.0
+        self._prev_error: float = 0.0
+
+    def update(self, error: float, dt: float) -> float:
+        """Compute PID output for the given error and timestep."""
+        if dt <= 0.0:
+            return 0.0
+
+        self._integral += error * dt
+        # Anti-windup: clamp integral term
+        self._integral = max(
+            -self.integral_clamp, min(self.integral_clamp, self._integral)
+        )
+
+        derivative = (error - self._prev_error) / dt
+        self._prev_error = error
+
+        output = (
+            self.kp * error
+            + self.ki * self._integral
+            + self.kd * derivative
+        )
+        return max(self.min_output, min(self.max_output, output))
+
+    def reset(self) -> None:
+        self._integral = 0.0
+        self._prev_error = 0.0
+
+
+# ── Target dataclass ──────────────────────────────────────────────────────────
+
+@dataclass
+class Target:
+    """Best-candidate person in the current detection frame."""
+    track_id: int = -1
+    center_x: float = 0.0   # bbox centre, pixels
+    center_y: float = 0.0
+    confidence: float = 0.0
+    distance_m: float = 0.0  # 0 = unknown
+    bbox_area: float = 0.0
+    stamp: float = 0.0       # rclpy.clock time (seconds)
+
+
+# ── State machine ─────────────────────────────────────────────────────────────
+
+class TrackState(Enum):
+    IDLE      = auto()
+    TRACKING  = auto()
+    LOST      = auto()
+    CENTERING = auto()
+
+
+# ── Node ──────────────────────────────────────────────────────────────────────
+
+class HeadTrackingNode(Node):
+    """Person-following head tracking with PID control."""
+
+    _PERSON_CLASS_ID = 0  # COCO class 0 = "person"
+
+    def __init__(self) -> None:
+        super().__init__("head_tracking")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("image_width",    848)     # D435i default
+        self.declare_parameter("image_height",   480)
+        self.declare_parameter("fov_h_deg",      87.0)    # D435i horizontal FOV
+        self.declare_parameter("fov_v_deg",      58.0)    # D435i vertical FOV
+
+        # PID gains — pan axis
+        self.declare_parameter("pan_kp",         0.08)    # deg / px
+        self.declare_parameter("pan_ki",         0.002)
+        self.declare_parameter("pan_kd",         0.015)
+
+        # PID gains — tilt axis
+        self.declare_parameter("tilt_kp",        0.08)
+        self.declare_parameter("tilt_ki",        0.002)
+        self.declare_parameter("tilt_kd",        0.015)
+
+        # Servo limits
+        self.declare_parameter("pan_min_deg",   -60.0)
+        self.declare_parameter("pan_max_deg",    60.0)
+        self.declare_parameter("tilt_min_deg",  -30.0)
+        self.declare_parameter("tilt_max_deg",   30.0)
+
+        # Tracking behaviour
+        self.declare_parameter("hold_duration_s",     3.0)   # hold after loss
+        self.declare_parameter("center_speed_deg_s",  20.0)  # centering speed
+        self.declare_parameter("dead_zone_px",        10.0)  # ignore small errors
+        self.declare_parameter("control_hz",          20.0)  # PID loop rate
+
+        # Target selection weights
+        self.declare_parameter("distance_weight",     0.6)   # vs confidence
+        self.declare_parameter("confidence_weight",   0.4)
+
+        self._img_w   = self.get_parameter("image_width").value
+        self._img_h   = self.get_parameter("image_height").value
+        self._fov_h   = self.get_parameter("fov_h_deg").value
+        self._fov_v   = self.get_parameter("fov_v_deg").value
+
+        pan_kp  = self.get_parameter("pan_kp").value
+        pan_ki  = self.get_parameter("pan_ki").value
+        pan_kd  = self.get_parameter("pan_kd").value
+        tilt_kp = self.get_parameter("tilt_kp").value
+        tilt_ki = self.get_parameter("tilt_ki").value
+        tilt_kd = self.get_parameter("tilt_kd").value
+
+        pan_min  = self.get_parameter("pan_min_deg").value
+        pan_max  = self.get_parameter("pan_max_deg").value
+        tilt_min = self.get_parameter("tilt_min_deg").value
+        tilt_max = self.get_parameter("tilt_max_deg").value
+
+        self._hold_duration  = self.get_parameter("hold_duration_s").value
+        self._center_speed   = self.get_parameter("center_speed_deg_s").value
+        self._dead_zone      = self.get_parameter("dead_zone_px").value
+        self._control_hz     = self.get_parameter("control_hz").value
+        self._dist_weight    = self.get_parameter("distance_weight").value
+        self._conf_weight    = self.get_parameter("confidence_weight").value
+
+        # ── PID controllers ───────────────────────────────────────────────────
+        self._pid_pan  = PIDController(pan_kp,  pan_ki,  pan_kd,  pan_min,  pan_max)
+        self._pid_tilt = PIDController(tilt_kp, tilt_ki, tilt_kd, tilt_min, tilt_max)
+
+        # ── State ─────────────────────────────────────────────────────────────
+        self._state: TrackState = TrackState.IDLE
+        self._target: Optional[Target] = None
+        self._last_seen: float = 0.0      # wall-clock time of last detection
+
+        # Desired angle setpoint (absolute, degrees from centre)
+        self._pan_deg:  float = 0.0
+        self._tilt_deg: float = 0.0
+
+        # ── Subscriptions ─────────────────────────────────────────────────────
+        sensor_qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+        self._sub_objects = self.create_subscription(
+            DetectedObjectArray,
+            "/saltybot/objects",
+            self._on_detections,
+            sensor_qos,
+        )
+
+        # ── Publishers ────────────────────────────────────────────────────────
+        self._pub_cmd = self.create_publisher(
+            Point, "/saltybot/gimbal/cmd", 10
+        )
+        self._pub_state = self.create_publisher(
+            String, "/saltybot/head_tracking/state", 10
+        )
+
+        # ── Control loop timer ────────────────────────────────────────────────
+        dt = 1.0 / self._control_hz
+        self._prev_time: float = self.get_clock().now().nanoseconds * 1e-9
+        self.create_timer(dt, self._control_loop)
+
+        self.get_logger().info(
+            f"HeadTrackingNode ready — "
+            f"image={self._img_w}×{self._img_h} "
+            f"FOV={self._fov_h:.0f}°×{self._fov_v:.0f}° "
+            f"hold={self._hold_duration:.1f}s "
+            f"rate={self._control_hz:.0f}Hz"
+        )
+
+    # ── Detection callback ────────────────────────────────────────────────────
+
+    def _on_detections(self, msg: DetectedObjectArray) -> None:
+        """Process incoming object detections and select best person target."""
+        persons = [
+            obj for obj in msg.objects
+            if obj.class_id == self._PERSON_CLASS_ID
+        ]
+        if not persons:
+            return
+
+        best = self._select_target(persons)
+        if best is None:
+            return
+
+        now = self.get_clock().now().nanoseconds * 1e-9
+        self._target = best
+        self._target.stamp = now
+        self._last_seen = now
+
+        if self._state in (TrackState.IDLE, TrackState.LOST, TrackState.CENTERING):
+            self._state = TrackState.TRACKING
+            # Don't reset PIDs on re-acquisition — avoids jerk
+            self.get_logger().info(
+                f"Target acquired: id={best.track_id} "
+                f"conf={best.confidence:.2f} dist={best.distance_m:.1f}m"
+            )
+
+    def _select_target(self, persons: list) -> Optional[Target]:
+        """Select highest-priority person: closest first, then confidence.
+
+        Scoring:
+          score = dist_weight * dist_score + conf_weight * confidence
+          dist_score = 1 / (1 + distance_m)  — higher = closer
+          If distance_m == 0 (unknown), use confidence only.
+        """
+        best_score = -1.0
+        best_obj: Optional[DetectedObject] = None
+
+        for obj in persons:
+            dist_m = obj.distance_m
+            conf   = obj.confidence
+
+            if dist_m > 0.0:
+                dist_score = 1.0 / (1.0 + dist_m)
+                score = self._dist_weight * dist_score + self._conf_weight * conf
+            else:
+                score = conf  # distance unknown — use confidence only
+
+            if score > best_score:
+                best_score = score
+                best_obj = obj
+
+        if best_obj is None:
+            return None
+
+        # Extract bbox centre in pixel coordinates
+        cx = best_obj.bbox.center.position.x
+        cy = best_obj.bbox.center.position.y
+        area = best_obj.bbox.size_x * best_obj.bbox.size_y
+
+        return Target(
+            track_id=0,           # DetectedObject has no track_id; use 0
+            center_x=cx,
+            center_y=cy,
+            confidence=best_obj.confidence,
+            distance_m=best_obj.distance_m,
+            bbox_area=area,
+        )
+
+    # ── Control loop ──────────────────────────────────────────────────────────
+
+    def _control_loop(self) -> None:
+        """20 Hz PID update and gimbal command publish."""
+        now   = self.get_clock().now().nanoseconds * 1e-9
+        dt    = now - self._prev_time
+        self._prev_time = now
+
+        if dt <= 0.0:
+            return
+
+        time_since_seen = now - self._last_seen
+
+        # ── State transitions ──────────────────────────────────────────────
+        if self._state == TrackState.TRACKING:
+            if time_since_seen > (2.0 / self._control_hz):
+                # No new detection in the last ~2 control cycles → lost
+                self._state = TrackState.LOST
+                self._last_seen = now  # reset hold timer
+                self.get_logger().info("Target lost — holding position")
+
+        elif self._state == TrackState.LOST:
+            if time_since_seen >= self._hold_duration:
+                self._state = TrackState.CENTERING
+                self._pid_pan.reset()
+                self._pid_tilt.reset()
+                self.get_logger().info("Hold expired — centering")
+
+        # ── PID update ────────────────────────────────────────────────────
+        error_pan_px  = 0.0
+        error_tilt_px = 0.0
+        confidence    = 0.0
+
+        if self._state == TrackState.TRACKING and self._target is not None:
+            cx = self._target.center_x
+            cy = self._target.center_y
+            confidence = self._target.confidence
+
+            # Pixel error: positive = target right of centre (→ pan right = negative pan)
+            error_pan_px  = cx - self._img_w / 2.0
+            error_tilt_px = cy - self._img_h / 2.0
+
+            # Dead-zone: don't chase tiny errors
+            if abs(error_pan_px)  < self._dead_zone:
+                error_pan_px  = 0.0
+            if abs(error_tilt_px) < self._dead_zone:
+                error_tilt_px = 0.0
+
+            # Convert pixel error → angle error (degrees)
+            # px_per_deg = image_dim / fov_deg
+            err_pan_deg  = error_pan_px  / (self._img_w / self._fov_h)
+            err_tilt_deg = error_tilt_px / (self._img_h / self._fov_v)
+
+            # PID output: delta angle to apply this timestep
+            # Pan:  positive error (target right) → decrease pan (turn right)
+            # Tilt: positive error (target below)  → decrease tilt (look down)
+            pan_delta  = self._pid_pan.update(-err_pan_deg,  dt)
+            tilt_delta = self._pid_tilt.update(-err_tilt_deg, dt)
+
+            pan_min  = self.get_parameter("pan_min_deg").value
+            pan_max  = self.get_parameter("pan_max_deg").value
+            tilt_min = self.get_parameter("tilt_min_deg").value
+            tilt_max = self.get_parameter("tilt_max_deg").value
+
+            self._pan_deg  = max(pan_min,  min(pan_max,  self._pan_deg  + pan_delta))
+            self._tilt_deg = max(tilt_min, min(tilt_max, self._tilt_deg + tilt_delta))
+
+        elif self._state == TrackState.CENTERING:
+            # Drive angles back toward zero at bounded speed
+            center_step = self._center_speed * dt
+
+            if abs(self._pan_deg) < center_step:
+                self._pan_deg = 0.0
+            else:
+                self._pan_deg -= math.copysign(center_step, self._pan_deg)
+
+            if abs(self._tilt_deg) < center_step:
+                self._tilt_deg = 0.0
+            else:
+                self._tilt_deg -= math.copysign(center_step, self._tilt_deg)
+
+            if self._pan_deg == 0.0 and self._tilt_deg == 0.0:
+                self._state = TrackState.IDLE
+                self.get_logger().info("Centered — IDLE")
+
+        # ── Publish gimbal command ─────────────────────────────────────────
+        cmd = Point()
+        cmd.x = self._pan_deg
+        cmd.y = self._tilt_deg
+        cmd.z = confidence
+        self._pub_cmd.publish(cmd)
+
+        # ── Publish state ─────────────────────────────────────────────────
+        state_data = {
+            "state":        self._state.name,
+            "pan_deg":      round(self._pan_deg,  2),
+            "tilt_deg":     round(self._tilt_deg, 2),
+            "error_pan_px":  round(error_pan_px,  1),
+            "error_tilt_px": round(error_tilt_px, 1),
+            "confidence":   round(confidence, 3),
+            "distance_m":   round(self._target.distance_m, 2) if self._target else 0.0,
+            "time_since_seen_s": round(now - self._last_seen, 2),
+        }
+        self._pub_state.publish(String(data=json.dumps(state_data)))
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = HeadTrackingNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_head_tracking/setup.cfg

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

jetson/ros2_ws/src/saltybot_head_tracking/setup.py

@@ -0,0 +1,32 @@
+from setuptools import setup, find_packages
+
+package_name = 'saltybot_head_tracking'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', [
+            'launch/head_tracking.launch.py',
+        ]),
+        ('share/' + package_name + '/config', [
+            'config/head_tracking_params.yaml',
+        ]),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='sl-perception',
+    maintainer_email='sl-perception@saltylab.local',
+    description='Person-following head tracking with PID control (Issue #549)',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'head_tracking = saltybot_head_tracking.head_tracking_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_health_monitor/config/sensor_health_params.yaml

@@ -0,0 +1,66 @@
+sensor_health_node:
+  ros__parameters:
+    # Publish rate for DiagnosticArray and JSON summary
+    check_hz: 1.0
+
+    # MQTT broker for saltybot/health topic
+    mqtt_broker: "localhost"
+    mqtt_port: 1883
+    mqtt_topic: "saltybot/health"
+    mqtt_enabled: true
+
+# Per-sensor thresholds and configuration
+# Each entry: topic, name, warn_s (WARN threshold), error_s (ERROR threshold), critical
+#
+# critical=true: system cannot operate without this sensor
+# warn_s: topic age (s) that triggers WARN level
+# error_s: topic age (s) that triggers ERROR level
+
+sensors:
+  - topic: "/camera/color/image_raw"
+    name: "camera_color"
+    warn_s: 1.0
+    error_s: 3.0
+    critical: true
+
+  - topic: "/camera/depth/image_rect_raw"
+    name: "camera_depth"
+    warn_s: 1.0
+    error_s: 3.0
+    critical: true
+
+  - topic: "/camera/color/camera_info"
+    name: "camera_info"
+    warn_s: 2.0
+    error_s: 5.0
+    critical: false
+
+  - topic: "/scan"
+    name: "lidar"
+    warn_s: 1.0
+    error_s: 3.0
+    critical: true
+
+  - topic: "/imu/data"
+    name: "imu"
+    warn_s: 0.5
+    error_s: 2.0
+    critical: true
+
+  - topic: "/saltybot/uwb/range"
+    name: "uwb"
+    warn_s: 2.0
+    error_s: 5.0
+    critical: false
+
+  - topic: "/saltybot/battery"
+    name: "battery"
+    warn_s: 3.0
+    error_s: 8.0
+    critical: false
+
+  - topic: "/saltybot/motor_daemon/status"
+    name: "motor_daemon"
+    warn_s: 2.0
+    error_s: 5.0
+    critical: true

jetson/ros2_ws/src/saltybot_health_monitor/launch/sensor_health.launch.py

@@ -0,0 +1,50 @@
+"""sensor_health.launch.py — Launch sensor health monitor node (Issue #566)."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg = get_package_share_directory("saltybot_health_monitor")
+
+    check_hz_arg = DeclareLaunchArgument(
+        "check_hz",
+        default_value="1.0",
+        description="Health check publish rate (Hz)",
+    )
+    mqtt_broker_arg = DeclareLaunchArgument(
+        "mqtt_broker",
+        default_value="localhost",
+        description="MQTT broker hostname",
+    )
+    mqtt_enabled_arg = DeclareLaunchArgument(
+        "mqtt_enabled",
+        default_value="true",
+        description="Enable MQTT publishing to saltybot/health",
+    )
+
+    sensor_health_node = Node(
+        package="saltybot_health_monitor",
+        executable="sensor_health_node",
+        name="sensor_health_node",
+        output="screen",
+        parameters=[
+            os.path.join(pkg, "config", "sensor_health_params.yaml"),
+            {
+                "check_hz":    LaunchConfiguration("check_hz"),
+                "mqtt_broker": LaunchConfiguration("mqtt_broker"),
+                "mqtt_enabled": LaunchConfiguration("mqtt_enabled"),
+            },
+        ],
+    )
+
+    return LaunchDescription([
+        check_hz_arg,
+        mqtt_broker_arg,
+        mqtt_enabled_arg,
+        sensor_health_node,
+    ])

jetson/ros2_ws/src/saltybot_health_monitor/package.xml

@@ -1,27 +1,24 @@
 <?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_health_monitor</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>
-    ROS2 system health monitor for SaltyBot. Central node that monitors heartbeats
-    from all critical nodes, detects when nodes go down (>5s silent), triggers
-    auto-restart, publishes /saltybot/system_health JSON, and alerts face display
-    on critical failures.
+    ROS2 health monitor for SaltyBot. Tracks node heartbeats and sensor topic
+    staleness. Publishes DiagnosticArray on /saltybot/diagnostics and MQTT on
+    saltybot/health. Issue #566.
   </description>
-  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
-  <license>MIT</license>
+  <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
+  <license>Apache-2.0</license>
 
   <depend>rclpy</depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>diagnostic_msgs</depend>
 
   <buildtool_depend>ament_python</buildtool_depend>
 
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
+  <test_depend>pytest</test_depend>
 
   <export>
     <build_type>ament_python</build_type>

jetson/ros2_ws/src/saltybot_health_monitor/saltybot_health_monitor/sensor_health_node.py

@@ -0,0 +1,385 @@
+#!/usr/bin/env python3
+"""sensor_health_node.py — ROS2 sensor topic health monitor (Issue #566).
+
+Monitors all sensor topics for staleness. Each sensor is checked against
+configurable WARN and ERROR timeouts. Results are published as a ROS2
+DiagnosticArray and as an MQTT JSON summary.
+
+Monitored topics (configurable via sensor_health_params.yaml):
+  /camera/color/image_raw             — RealSense colour stream
+  /camera/depth/image_rect_raw        — RealSense depth stream
+  /camera/color/camera_info           — RealSense camera info
+  /scan                               — LiDAR 2-D scan
+  /imu/data                           — IMU (BNO055 via JLink)
+  /saltybot/uwb/range                 — UWB ranging
+  /saltybot/battery                   — Battery telemetry (JSON string)
+  /saltybot/motor_daemon/status       — Motor daemon status
+
+Published topics:
+  /saltybot/diagnostics   (diagnostic_msgs/DiagnosticArray)  — full per-sensor status
+  /saltybot/sensor_health (std_msgs/String)                   — JSON summary
+
+MQTT:
+  Topic: saltybot/health
+  Payload: JSON {timestamp, overall, sensors:{name: {status, age_s, hz}}}
+
+Parameters (config/sensor_health_params.yaml):
+  check_hz             1.0     Health check publish rate
+  mqtt_broker          localhost
+  mqtt_port            1883
+  mqtt_topic           saltybot/health
+  mqtt_enabled         true
+  sensors              list of {topic, name, warn_s, error_s, critical}
+"""
+
+from __future__ import annotations
+
+import json
+import time
+import threading
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
+)
+
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+from sensor_msgs.msg import CameraInfo, Image, Imu, LaserScan
+from std_msgs.msg import String
+
+
+# ── Diagnostic level aliases ───────────────────────────────────────────────────
+
+OK    = DiagnosticStatus.OK     # 0
+WARN  = DiagnosticStatus.WARN   # 1
+ERROR = DiagnosticStatus.ERROR  # 2
+
+_LEVEL_NAMES = {OK: "OK", WARN: "WARN", ERROR: "ERROR"}
+
+# ── Default sensor configuration ──────────────────────────────────────────────
+
+DEFAULT_SENSORS: List[dict] = [
+    {"topic": "/camera/color/image_raw",        "name": "camera_color",     "warn_s": 1.0, "error_s": 3.0, "critical": True},
+    {"topic": "/camera/depth/image_rect_raw",   "name": "camera_depth",     "warn_s": 1.0, "error_s": 3.0, "critical": True},
+    {"topic": "/camera/color/camera_info",      "name": "camera_info",      "warn_s": 2.0, "error_s": 5.0, "critical": False},
+    {"topic": "/scan",                          "name": "lidar",            "warn_s": 1.0, "error_s": 3.0, "critical": True},
+    {"topic": "/imu/data",                      "name": "imu",              "warn_s": 0.5, "error_s": 2.0, "critical": True},
+    {"topic": "/saltybot/uwb/range",            "name": "uwb",              "warn_s": 2.0, "error_s": 5.0, "critical": False},
+    {"topic": "/saltybot/battery",              "name": "battery",          "warn_s": 3.0, "error_s": 8.0, "critical": False},
+    {"topic": "/saltybot/motor_daemon/status",  "name": "motor_daemon",     "warn_s": 2.0, "error_s": 5.0, "critical": True},
+]
+
+
+# ── SensorWatcher ─────────────────────────────────────────────────────────────
+
+class SensorWatcher:
+    """Tracks message receipt timestamps and rate for a single topic.
+
+    Thread-safe: callback may fire from any executor thread.
+    """
+
+    def __init__(self, topic: str, name: str,
+                 warn_s: float, error_s: float, critical: bool) -> None:
+        self.topic    = topic
+        self.name     = name
+        self.warn_s   = warn_s
+        self.error_s  = error_s
+        self.critical = critical
+
+        self._last_rx:  float = 0.0   # monotonic; 0 = never received
+        self._count:    int   = 0
+        self._rate_hz:  float = 0.0
+        self._rate_ts:  float = time.monotonic()
+        self._rate_cnt: int   = 0
+        self._lock = threading.Lock()
+
+    # ── Callback (called from subscription) ────────────────────────────────
+
+    def on_message(self, _msg) -> None:
+        """Record receipt of any message on this topic."""
+        now = time.monotonic()
+        with self._lock:
+            self._last_rx = now
+            self._count  += 1
+            self._rate_cnt += 1
+            # Update rate estimate every ~2 s
+            elapsed = now - self._rate_ts
+            if elapsed >= 2.0:
+                self._rate_hz  = self._rate_cnt / elapsed
+                self._rate_cnt = 0
+                self._rate_ts  = now
+
+    # ── Status query ───────────────────────────────────────────────────────
+
+    def age_s(self, now: Optional[float] = None) -> float:
+        """Seconds since last message (∞ if never received)."""
+        if now is None:
+            now = time.monotonic()
+        with self._lock:
+            if self._last_rx == 0.0:
+                return float("inf")
+            return now - self._last_rx
+
+    def rate_hz(self) -> float:
+        with self._lock:
+            return self._rate_hz
+
+    def msg_count(self) -> int:
+        with self._lock:
+            return self._count
+
+    def level(self, now: Optional[float] = None) -> int:
+        age = self.age_s(now)
+        if age >= self.error_s:
+            return ERROR
+        if age >= self.warn_s:
+            return WARN
+        return OK
+
+    def diagnostic_status(self, now: Optional[float] = None) -> DiagnosticStatus:
+        if now is None:
+            now = time.monotonic()
+        age   = self.age_s(now)
+        lvl   = self.level(now)
+        hz    = self.rate_hz()
+        cnt   = self.msg_count()
+
+        if age == float("inf"):
+            msg = f"ERROR: no data received on {self.topic}"
+        elif lvl == ERROR:
+            msg = f"ERROR: stale {age:.1f}s (threshold {self.error_s:.1f}s)"
+        elif lvl == WARN:
+            msg = f"WARN: stale {age:.1f}s (threshold {self.warn_s:.1f}s)"
+        else:
+            msg = f"OK ({hz:.1f} Hz)"
+
+        age_str = "inf" if age == float("inf") else f"{age:.2f}"
+        status = DiagnosticStatus()
+        status.level   = lvl
+        status.name    = self.name
+        status.message = msg
+        status.hardware_id = self.topic
+        status.values  = [
+            KeyValue(key="topic",    value=self.topic),
+            KeyValue(key="age_s",    value=age_str),
+            KeyValue(key="rate_hz",  value=f"{hz:.2f}"),
+            KeyValue(key="count",    value=str(cnt)),
+            KeyValue(key="warn_s",   value=str(self.warn_s)),
+            KeyValue(key="error_s",  value=str(self.error_s)),
+            KeyValue(key="critical", value=str(self.critical)),
+        ]
+        return status
+
+    def summary_dict(self, now: Optional[float] = None) -> dict:
+        if now is None:
+            now = time.monotonic()
+        age = self.age_s(now)
+        return {
+            "status":    _LEVEL_NAMES[self.level(now)],
+            "age_s":     round(age, 2) if age != float("inf") else None,
+            "rate_hz":   round(self.rate_hz(), 2),
+            "count":     self.msg_count(),
+            "critical":  self.critical,
+        }
+
+
+# ── SensorHealthNode ───────────────────────────────────────────────────────────
+
+class SensorHealthNode(Node):
+    """Monitor all sensor topics; publish DiagnosticArray + MQTT JSON."""
+
+    def __init__(self) -> None:
+        super().__init__("sensor_health_node")
+
+        # ── Parameters ─────────────────────────────────────────────────────
+        self.declare_parameter("check_hz",    1.0)
+        self.declare_parameter("mqtt_broker", "localhost")
+        self.declare_parameter("mqtt_port",   1883)
+        self.declare_parameter("mqtt_topic",  "saltybot/health")
+        self.declare_parameter("mqtt_enabled", True)
+
+        check_hz         = self.get_parameter("check_hz").value
+        self._mqtt_broker = self.get_parameter("mqtt_broker").value
+        self._mqtt_port   = int(self.get_parameter("mqtt_port").value)
+        self._mqtt_topic  = self.get_parameter("mqtt_topic").value
+        mqtt_enabled      = self.get_parameter("mqtt_enabled").value
+
+        # ── Build sensor watchers ───────────────────────────────────────────
+        self._watchers: Dict[str, SensorWatcher] = {}
+        for cfg in DEFAULT_SENSORS:
+            w = SensorWatcher(
+                topic    = cfg["topic"],
+                name     = cfg["name"],
+                warn_s   = float(cfg.get("warn_s",   1.0)),
+                error_s  = float(cfg.get("error_s",  3.0)),
+                critical = bool(cfg.get("critical",  False)),
+            )
+            self._watchers[cfg["name"]] = w
+
+        # ── Subscriptions — one per sensor type ────────────────────────────
+        # Best-effort QoS for sensor data (sensors may use BEST_EFFORT publishers)
+        be_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST, depth=1,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+        rel_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST, depth=5,
+            reliability=ReliabilityPolicy.RELIABLE,
+        )
+
+        self._subscribe(Image,      "/camera/color/image_raw",       "camera_color",  be_qos)
+        self._subscribe(Image,      "/camera/depth/image_rect_raw",  "camera_depth",  be_qos)
+        self._subscribe(CameraInfo, "/camera/color/camera_info",     "camera_info",   be_qos)
+        self._subscribe(LaserScan,  "/scan",                         "lidar",         be_qos)
+        self._subscribe(Imu,        "/imu/data",                     "imu",           be_qos)
+        self._subscribe(String,     "/saltybot/uwb/range",           "uwb",           rel_qos)
+        self._subscribe(String,     "/saltybot/battery",             "battery",       rel_qos)
+        self._subscribe(String,     "/saltybot/motor_daemon/status", "motor_daemon",  rel_qos)
+
+        # ── Publishers ─────────────────────────────────────────────────────
+        self._pub_diag   = self.create_publisher(
+            DiagnosticArray, "/saltybot/diagnostics",   10)
+        self._pub_health = self.create_publisher(
+            String,          "/saltybot/sensor_health", 10)
+
+        # ── MQTT ───────────────────────────────────────────────────────────
+        self._mqtt_client = None
+        if mqtt_enabled:
+            self._connect_mqtt()
+
+        # ── Timer ──────────────────────────────────────────────────────────
+        self.create_timer(1.0 / max(0.1, check_hz), self._publish_diagnostics)
+
+        sensor_list = ", ".join(self._watchers.keys())
+        self.get_logger().info(
+            f"[sensor_health] monitoring {len(self._watchers)} sensors: {sensor_list}"
+        )
+
+    # ── Subscription helper ────────────────────────────────────────────────
+
+    def _subscribe(self, msg_type, topic: str, name: str, qos) -> None:
+        if name not in self._watchers:
+            return
+        watcher = self._watchers[name]
+        self.create_subscription(msg_type, topic, watcher.on_message, qos)
+
+    # ── MQTT ───────────────────────────────────────────────────────────────
+
+    def _connect_mqtt(self) -> None:
+        try:
+            import paho.mqtt.client as mqtt  # type: ignore
+            self._mqtt_client = mqtt.Client(client_id="saltybot_sensor_health")
+            self._mqtt_client.on_connect = self._on_mqtt_connect
+            self._mqtt_client.connect_async(self._mqtt_broker, self._mqtt_port, 60)
+            self._mqtt_client.loop_start()
+        except ImportError:
+            self.get_logger().warn(
+                "[sensor_health] paho-mqtt not installed — MQTT publishing disabled"
+            )
+        except Exception as e:
+            self.get_logger().warn(f"[sensor_health] MQTT connect failed: {e}")
+
+    def _on_mqtt_connect(self, client, userdata, flags, rc) -> None:
+        if rc == 0:
+            self.get_logger().info(
+                f"[sensor_health] MQTT connected to {self._mqtt_broker}:{self._mqtt_port}"
+            )
+        else:
+            self.get_logger().warn(f"[sensor_health] MQTT connect rc={rc}")
+
+    def _publish_mqtt(self, payload: str) -> None:
+        if self._mqtt_client is None:
+            return
+        try:
+            self._mqtt_client.publish(self._mqtt_topic, payload, qos=0, retain=True)
+        except Exception as e:
+            self.get_logger().warn(f"[sensor_health] MQTT publish error: {e}")
+
+    # ── Diagnostic publisher ───────────────────────────────────────────────
+
+    def _publish_diagnostics(self) -> None:
+        now    = time.monotonic()
+        wall   = time.time()
+
+        # Build DiagnosticArray
+        diag_array = DiagnosticArray()
+        diag_array.header.stamp = self.get_clock().now().to_msg()
+
+        sensor_summaries: dict = {}
+        worst_level = OK
+
+        for name, watcher in self._watchers.items():
+            ds = watcher.diagnostic_status(now)
+            diag_array.status.append(ds)
+
+            if ds.level > worst_level:
+                worst_level = ds.level
+
+            sensor_summaries[name] = watcher.summary_dict(now)
+
+        # Overall system status
+        n_error   = sum(1 for w in self._watchers.values() if w.level(now) == ERROR)
+        n_warn    = sum(1 for w in self._watchers.values() if w.level(now) == WARN)
+        crit_err  = [n for n, w in self._watchers.items()
+                     if w.critical and w.level(now) == ERROR]
+
+        overall = "OK"
+        if crit_err:
+            overall = "ERROR"
+        elif n_error > 0:
+            overall = "ERROR"
+        elif n_warn > 0:
+            overall = "WARN"
+
+        # Publish DiagnosticArray
+        self._pub_diag.publish(diag_array)
+
+        # JSON summary
+        summary = {
+            "timestamp":    wall,
+            "overall":      overall,
+            "n_ok":         len(self._watchers) - n_error - n_warn,
+            "n_warn":       n_warn,
+            "n_error":      n_error,
+            "critical_err": crit_err,
+            "sensors":      sensor_summaries,
+        }
+        payload = json.dumps(summary)
+        self._pub_health.publish(String(data=payload))
+        self._publish_mqtt(payload)
+
+        # Log on transitions or errors
+        if worst_level >= ERROR:
+            self.get_logger().warn(
+                f"[sensor_health] {overall}: {n_error} error(s), {n_warn} warn(s)"
+                + (f" — critical: {crit_err}" if crit_err else "")
+            )
+
+    def destroy_node(self) -> None:
+        if self._mqtt_client is not None:
+            try:
+                self._mqtt_client.loop_stop()
+                self._mqtt_client.disconnect()
+            except Exception:
+                pass
+        super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = SensorHealthNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_health_monitor/setup.py

@@ -4,27 +4,34 @@ package_name = "saltybot_health_monitor"
 
 setup(
     name=package_name,
-    version="0.1.0",
+    version="0.2.0",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
         (f"share/{package_name}", ["package.xml"]),
-        (f"share/{package_name}/launch", ["launch/health_monitor.launch.py"]),
-        (f"share/{package_name}/config", ["config/health_config.yaml"]),
+        (f"share/{package_name}/launch", [
+            "launch/health_monitor.launch.py",
+            "launch/sensor_health.launch.py",
+        ]),
+        (f"share/{package_name}/config", [
+            "config/health_config.yaml",
+            "config/sensor_health_params.yaml",
+        ]),
     ],
-    install_requires=["setuptools", "pyyaml"],
+    install_requires=["setuptools", "pyyaml", "paho-mqtt"],
     zip_safe=True,
-    maintainer="sl-controls",
-    maintainer_email="sl-controls@saltylab.local",
+    maintainer="sl-jetson",
+    maintainer_email="sl-jetson@saltylab.local",
     description=(
-        "System health monitor: tracks node heartbeats, detects down nodes, "
-        "triggers auto-restart, publishes system health status"
+        "System health monitor: node heartbeats + sensor topic staleness "
+        "detection with DiagnosticArray and MQTT (Issue #566)"
     ),
-    license="MIT",
+    license="Apache-2.0",
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
             "health_monitor_node = saltybot_health_monitor.health_monitor_node:main",
+            "sensor_health_node  = saltybot_health_monitor.sensor_health_node:main",
         ],
     },
 )

jetson/ros2_ws/src/saltybot_health_monitor/test/test_sensor_health.py

@@ -0,0 +1,344 @@
+"""test_sensor_health.py — Unit tests for sensor_health_node (Issue #566).
+
+Runs entirely offline: no ROS2 runtime, no hardware, no MQTT broker required.
+"""
+
+import json
+import sys
+import time
+import types
+import unittest
+from unittest.mock import MagicMock, patch
+
+# ── Stub ROS2 and sensor_msgs so tests run offline ────────────────────────────
+
+def _install_stubs():
+    # rclpy
+    rclpy = types.ModuleType("rclpy")
+    rclpy.init = lambda **_: None
+    rclpy.spin = lambda _: None
+    rclpy.shutdown = lambda: None
+
+    node_mod = types.ModuleType("rclpy.node")
+    class _Node:
+        def __init__(self, *a, **kw): pass
+        def declare_parameter(self, *a, **kw): pass
+        def get_parameter(self, name):
+            defaults = {
+                "check_hz": 1.0, "mqtt_broker": "localhost",
+                "mqtt_port": 1883, "mqtt_topic": "saltybot/health",
+                "mqtt_enabled": False,
+            }
+            m = MagicMock(); m.value = defaults.get(name, False)
+            return m
+        def get_logger(self): return MagicMock()
+        def get_clock(self): return MagicMock()
+        def create_subscription(self, *a, **kw): pass
+        def create_publisher(self, *a, **kw): return MagicMock()
+        def create_timer(self, *a, **kw): pass
+        def destroy_node(self): pass
+    node_mod.Node = _Node
+    rclpy.node = node_mod
+
+    qos_mod = types.ModuleType("rclpy.qos")
+    for attr in ("QoSProfile", "HistoryPolicy", "ReliabilityPolicy", "DurabilityPolicy"):
+        setattr(qos_mod, attr, MagicMock())
+    rclpy.qos = qos_mod
+
+    sys.modules.setdefault("rclpy",      rclpy)
+    sys.modules.setdefault("rclpy.node", rclpy.node)
+    sys.modules.setdefault("rclpy.qos",  rclpy.qos)
+
+    # diagnostic_msgs
+    diag = types.ModuleType("diagnostic_msgs")
+    diag_msg = types.ModuleType("diagnostic_msgs.msg")
+
+    class _DiagStatus:
+        OK    = 0
+        WARN  = 1
+        ERROR = 2
+        def __init__(self):
+            self.level = 0; self.name = ""; self.message = ""
+            self.hardware_id = ""; self.values = []
+
+    class _DiagArray:
+        def __init__(self):
+            self.header = MagicMock(); self.status = []
+
+    class _KeyValue:
+        def __init__(self, key="", value=""):
+            self.key = key; self.value = value
+
+    diag_msg.DiagnosticStatus = _DiagStatus
+    diag_msg.DiagnosticArray  = _DiagArray
+    diag_msg.KeyValue         = _KeyValue
+    diag.msg = diag_msg
+    sys.modules.setdefault("diagnostic_msgs",     diag)
+    sys.modules.setdefault("diagnostic_msgs.msg", diag_msg)
+
+    # sensor_msgs
+    sens = types.ModuleType("sensor_msgs")
+    sens_msg = types.ModuleType("sensor_msgs.msg")
+    for cls_name in ("Image", "CameraInfo", "Imu", "LaserScan"):
+        setattr(sens_msg, cls_name, MagicMock)
+    sens.msg = sens_msg
+    sys.modules.setdefault("sensor_msgs",     sens)
+    sys.modules.setdefault("sensor_msgs.msg", sens_msg)
+
+    # std_msgs
+    std = types.ModuleType("std_msgs")
+    std_msg = types.ModuleType("std_msgs.msg")
+    class _String:
+        def __init__(self, data=""): self.data = data
+    std_msg.String = _String
+    std.msg = std_msg
+    sys.modules.setdefault("std_msgs",     std)
+    sys.modules.setdefault("std_msgs.msg", std_msg)
+
+
+_install_stubs()
+
+from saltybot_health_monitor.sensor_health_node import (  # noqa: E402
+    SensorWatcher, OK, WARN, ERROR, _LEVEL_NAMES,
+)
+
+
+# ── SensorWatcher: initial state ──────────────────────────────────────────────
+
+class TestSensorWatcherInitial(unittest.TestCase):
+
+    def setUp(self):
+        self.w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=True)
+
+    def test_initial_age_is_inf(self):
+        self.assertEqual(self.w.age_s(), float("inf"))
+
+    def test_initial_level_is_error(self):
+        # Never received → age=inf ≥ error_s → ERROR
+        self.assertEqual(self.w.level(), ERROR)
+
+    def test_initial_count_zero(self):
+        self.assertEqual(self.w.msg_count(), 0)
+
+    def test_initial_rate_zero(self):
+        self.assertAlmostEqual(self.w.rate_hz(), 0.0)
+
+    def test_name_stored(self):
+        self.assertEqual(self.w.name, "lidar")
+
+    def test_topic_stored(self):
+        self.assertEqual(self.w.topic, "/scan")
+
+    def test_critical_stored(self):
+        self.assertTrue(self.w.critical)
+
+
+# ── SensorWatcher: after receiving messages ───────────────────────────────────
+
+class TestSensorWatcherAfterMessage(unittest.TestCase):
+
+    def setUp(self):
+        self.w = SensorWatcher("/imu/data", "imu", warn_s=0.5, error_s=2.0, critical=True)
+        self.w.on_message(None)  # simulate message receipt
+
+    def test_age_is_small_after_message(self):
+        self.assertLess(self.w.age_s(), 0.1)
+
+    def test_level_ok_immediately_after(self):
+        self.assertEqual(self.w.level(), OK)
+
+    def test_count_increments(self):
+        self.w.on_message(None)
+        self.assertEqual(self.w.msg_count(), 2)
+
+    def test_multiple_messages(self):
+        for _ in range(10):
+            self.w.on_message(None)
+        self.assertEqual(self.w.msg_count(), 11)
+
+
+# ── SensorWatcher: staleness thresholds ──────────────────────────────────────
+
+class TestSensorWatcherStaleness(unittest.TestCase):
+
+    def _make_stale(self, seconds_ago: float) -> SensorWatcher:
+        """Return a watcher whose last_rx was `seconds_ago` seconds in the past."""
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        # Backdate the last_rx directly
+        with w._lock:
+            w._last_rx -= seconds_ago
+        return w
+
+    def test_ok_when_fresh(self):
+        w = self._make_stale(0.5)
+        self.assertEqual(w.level(), OK)
+
+    def test_warn_at_warn_threshold(self):
+        w = self._make_stale(1.1)
+        self.assertEqual(w.level(), WARN)
+
+    def test_error_at_error_threshold(self):
+        w = self._make_stale(3.1)
+        self.assertEqual(w.level(), ERROR)
+
+    def test_age_matches_backdated_time(self):
+        w = self._make_stale(2.0)
+        self.assertAlmostEqual(w.age_s(), 2.0, delta=0.1)
+
+    def test_warn_level_between_thresholds(self):
+        w = self._make_stale(2.0)   # 1.0 < 2.0 < 3.0
+        self.assertEqual(w.level(), WARN)
+
+
+# ── SensorWatcher: diagnostic_status output ──────────────────────────────────
+
+class TestSensorWatcherDiagStatus(unittest.TestCase):
+
+    def test_never_received_is_error(self):
+        w = SensorWatcher("/camera/color/image_raw", "camera_color",
+                          warn_s=1.0, error_s=3.0, critical=True)
+        ds = w.diagnostic_status()
+        self.assertEqual(ds.level, ERROR)
+        self.assertIn("no data", ds.message)
+
+    def test_ok_status_message(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        ds = w.diagnostic_status()
+        self.assertEqual(ds.level, OK)
+        self.assertIn("OK", ds.message)
+
+    def test_warn_status_message(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        with w._lock:
+            w._last_rx -= 1.5
+        ds = w.diagnostic_status()
+        self.assertEqual(ds.level, WARN)
+        self.assertIn("WARN", ds.message)
+
+    def test_hardware_id_is_topic(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        ds = w.diagnostic_status()
+        self.assertEqual(ds.hardware_id, "/scan")
+
+    def test_kv_keys_present(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        ds = w.diagnostic_status()
+        kv_keys = {kv.key for kv in ds.values}
+        for expected in ("topic", "age_s", "rate_hz", "count", "warn_s", "error_s"):
+            self.assertIn(expected, kv_keys)
+
+    def test_age_inf_displayed_as_inf(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        ds = w.diagnostic_status()
+        kv = {kv.key: kv.value for kv in ds.values}
+        self.assertEqual(kv["age_s"], "inf")
+
+
+# ── SensorWatcher: summary_dict output ───────────────────────────────────────
+
+class TestSensorWatcherSummaryDict(unittest.TestCase):
+
+    def test_never_received_age_is_none(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        d = w.summary_dict()
+        self.assertIsNone(d["age_s"])
+        self.assertEqual(d["status"], "ERROR")
+
+    def test_ok_status_string(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        d = w.summary_dict()
+        self.assertEqual(d["status"], "OK")
+
+    def test_warn_status_string(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        with w._lock:
+            w._last_rx -= 1.5
+        d = w.summary_dict()
+        self.assertEqual(d["status"], "WARN")
+
+    def test_error_status_string(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        with w._lock:
+            w._last_rx -= 5.0
+        d = w.summary_dict()
+        self.assertEqual(d["status"], "ERROR")
+
+    def test_age_rounded(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        d = w.summary_dict()
+        self.assertIsInstance(d["age_s"], float)
+
+    def test_critical_flag(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=True)
+        self.assertTrue(w.summary_dict()["critical"])
+
+    def test_non_critical_flag(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        self.assertFalse(w.summary_dict()["critical"])
+
+    def test_count_in_summary(self):
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        for _ in range(5):
+            w.on_message(None)
+        self.assertEqual(w.summary_dict()["count"], 5)
+
+
+# ── Level name mapping ─────────────────────────────────────────────────────────
+
+class TestLevelNames(unittest.TestCase):
+
+    def test_ok_name(self):
+        self.assertEqual(_LEVEL_NAMES[OK], "OK")
+
+    def test_warn_name(self):
+        self.assertEqual(_LEVEL_NAMES[WARN], "WARN")
+
+    def test_error_name(self):
+        self.assertEqual(_LEVEL_NAMES[ERROR], "ERROR")
+
+
+# ── Thread safety ─────────────────────────────────────────────────────────────
+
+class TestSensorWatcherThreadSafety(unittest.TestCase):
+
+    def test_concurrent_messages(self):
+        import threading
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        N = 500
+        threads = [threading.Thread(target=w.on_message, args=(None,)) for _ in range(N)]
+        for t in threads:
+            t.start()
+        for t in threads:
+            t.join()
+        self.assertEqual(w.msg_count(), N)
+
+    def test_concurrent_reads(self):
+        import threading
+        w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False)
+        w.on_message(None)
+        errors = []
+        def read_loop():
+            for _ in range(100):
+                try:
+                    w.level()
+                    w.age_s()
+                    w.rate_hz()
+                except Exception as e:
+                    errors.append(e)
+        threads = [threading.Thread(target=read_loop) for _ in range(5)]
+        for t in threads: t.start()
+        for t in threads: t.join()
+        self.assertEqual(errors, [])
+
+
+if __name__ == "__main__":
+    unittest.main()

jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml

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

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

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

jetson/ros2_ws/src/saltybot_uwb_position/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_uwb_position</name>
+  <version>0.1.0</version>
+  <description>
+    ROS2 UWB position node for Jetson (Issue #546).
+    Reads JSON range data from an ESP32 DW3000 UWB tag over USB serial,
+    trilaterates from 3+ fixed infrastructure anchors, publishes
+    PoseStamped on /saltybot/uwb/pose, per-anchor UwbRange on
+    /saltybot/uwb/range/&lt;id&gt;, JSON diagnostics on /saltybot/uwb/status,
+    and broadcasts the uwb_link→map TF2 transform.
+  </description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>saltybot_uwb_msgs</depend>
+
+  <!-- numpy is a system dep on Jetson (python3-numpy) -->
+  <exec_depend>python3-numpy</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py

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

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

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

jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg

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

jetson/ros2_ws/src/saltybot_uwb_position/setup.py

@@ -0,0 +1,32 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = "saltybot_uwb_position"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages",
+            ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "launch"),
+            glob("launch/*.py")),
+        (os.path.join("share", package_name, "config"),
+            glob("config/*.yaml")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-perception",
+    maintainer_email="sl-perception@saltylab.local",
+    description="ROS2 UWB position node — JSON serial bridge with trilateration (Issue #546)",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "uwb_position = saltybot_uwb_position.uwb_position_node:main",
+        ],
+    },
+)

phone/voice_commander.py

@@ -0,0 +1,448 @@
+#!/usr/bin/env python3
+"""
+voice_commander.py — Phone-based voice command interface for SaltyBot (Issue #553)
+
+Runs on Android/Termux. Listens for the wake word 'Hey Salty', transcribes speech
+via OpenAI Whisper (local), parses robot commands, and publishes to ROS2 topic
+/saltybot/voice/cmd via WebSocket bridge to Jetson Orin. Confirms commands via
+termux-tts-speak.
+
+Supported commands:
+  go forward / go back / go left / go right
+  stop / halt
+  follow me
+  go home
+  look at me
+
+Usage:
+  python3 phone/voice_commander.py [OPTIONS]
+
+Options:
+  --host HOST         Jetson IP or hostname (default: 192.168.1.100)
+  --port PORT         rosbridge WebSocket port (default: 9090)
+  --model MODEL       Whisper model size: tiny/base/small (default: base)
+  --threshold FLOAT   Wake word match threshold 0.0-1.0 (default: 0.6)
+  --record-sec FLOAT  Seconds to record after wake word (default: 3.0)
+  --no-tts            Disable TTS confirmation
+  --debug             Verbose logging
+"""
+
+import argparse
+import json
+import logging
+import os
+import subprocess
+import sys
+import tempfile
+import threading
+import time
+from dataclasses import dataclass, field
+from enum import Enum
+from pathlib import Path
+from typing import Optional
+
+# ── Optional ROS2 ────────────────────────────────────────────────────────────
+try:
+    import rclpy
+    from rclpy.node import Node
+    from std_msgs.msg import String
+    ROS2_AVAILABLE = True
+except ImportError:
+    ROS2_AVAILABLE = False
+
+# ── Whisper (local STT) ───────────────────────────────────────────────────────
+try:
+    import whisper
+    WHISPER_AVAILABLE = True
+except ImportError:
+    WHISPER_AVAILABLE = False
+
+# ── WebSocket client ──────────────────────────────────────────────────────────
+try:
+    import websocket  # websocket-client
+    WS_AVAILABLE = True
+except ImportError:
+    WS_AVAILABLE = False
+
+# ── Constants ────────────────────────────────────────────────────────────────
+WAKE_WORD = "hey salty"
+VOICE_CMD_TOPIC = "/saltybot/voice/cmd"
+SAMPLE_RATE = 16000          # Hz required by Whisper
+WAKE_RECORD_SEC = 1.5        # short clip to check for wake word
+CMD_RECORD_SEC = 3.0         # command clip length after wake word
+RECONNECT_DELAY = 5.0        # seconds between WS reconnects
+CHUNK_BYTES = 4096
+
+
+class Command(Enum):
+    GO_FORWARD  = "go_forward"
+    GO_BACK     = "go_back"
+    GO_LEFT     = "go_left"
+    GO_RIGHT    = "go_right"
+    STOP        = "stop"
+    FOLLOW_ME   = "follow_me"
+    GO_HOME     = "go_home"
+    LOOK_AT_ME  = "look_at_me"
+    UNKNOWN     = "unknown"
+
+
+# ── Command parsing ───────────────────────────────────────────────────────────
+
+# Each entry: (list_of_trigger_phrases, Command)
+COMMAND_TABLE = [
+    (["go forward", "move forward", "forward", "ahead", "go straight"],    Command.GO_FORWARD),
+    (["go back", "go backward", "move back", "reverse", "back up"],        Command.GO_BACK),
+    (["go left",  "turn left",  "move left",  "left"],                     Command.GO_LEFT),
+    (["go right", "turn right", "move right", "right"],                    Command.GO_RIGHT),
+    (["stop", "halt", "freeze", "stay", "stand by"],                       Command.STOP),
+    (["follow me", "come here", "come with me", "follow"],                 Command.FOLLOW_ME),
+    (["go home", "return home", "return to base", "dock"],                 Command.GO_HOME),
+    (["look at me", "face me", "look here", "turn to me"],                 Command.LOOK_AT_ME),
+]
+
+TTS_CONFIRMATIONS = {
+    Command.GO_FORWARD: "Going forward",
+    Command.GO_BACK:    "Going back",
+    Command.GO_LEFT:    "Turning left",
+    Command.GO_RIGHT:   "Turning right",
+    Command.STOP:       "Stopping",
+    Command.FOLLOW_ME:  "Following you",
+    Command.GO_HOME:    "Heading home",
+    Command.LOOK_AT_ME: "Looking at you",
+    Command.UNKNOWN:    "Sorry, I didn't understand that",
+}
+
+
+def parse_command(text: str) -> Command:
+    """Match transcribed text against command table. Returns best match or UNKNOWN."""
+    text = text.lower().strip()
+    for phrases, cmd in COMMAND_TABLE:
+        for phrase in phrases:
+            if phrase in text:
+                return cmd
+    return Command.UNKNOWN
+
+
+def contains_wake_word(text: str, threshold: float = 0.6) -> bool:
+    """Check if transcribed text contains the wake word (fuzzy match)."""
+    text = text.lower().strip()
+    if WAKE_WORD in text:
+        return True
+    # Simple token overlap fallback
+    wake_tokens = set(WAKE_WORD.split())
+    text_tokens = set(text.split())
+    overlap = len(wake_tokens & text_tokens) / len(wake_tokens)
+    return overlap >= threshold
+
+
+# ── Audio capture via termux-microphone-record ────────────────────────────────
+
+def record_audio(duration_sec: float, output_path: str) -> bool:
+    """
+    Record audio using termux-microphone-record.
+    Saves a 16 kHz mono WAV to output_path.
+    Returns True on success.
+    """
+    # termux-microphone-record writes to a file; we start, wait, then stop.
+    try:
+        subprocess.run(
+            [
+                "termux-microphone-record",
+                "-l", str(int(duration_sec)),   # duration in seconds
+                "-r", str(SAMPLE_RATE),          # sample rate
+                "-c", "1",                        # mono
+                "-e", "aac",                      # encoding (aac is reliable on Android)
+                "-f", output_path,
+            ],
+            check=True,
+            timeout=duration_sec + 5,
+            capture_output=True,
+        )
+        return Path(output_path).exists()
+    except (subprocess.CalledProcessError, subprocess.TimeoutExpired, FileNotFoundError) as e:
+        logging.debug("termux-microphone-record error: %s", e)
+        return False
+
+
+def tts_speak(text: str) -> None:
+    """Speak text via termux-tts-speak (non-blocking)."""
+    try:
+        subprocess.Popen(
+            ["termux-tts-speak", text],
+            stdout=subprocess.DEVNULL,
+            stderr=subprocess.DEVNULL,
+        )
+    except FileNotFoundError:
+        logging.debug("termux-tts-speak not available")
+
+
+# ── Whisper STT ───────────────────────────────────────────────────────────────
+
+class WhisperSTT:
+    """Thin wrapper around local Whisper model."""
+
+    def __init__(self, model_size: str = "base"):
+        if not WHISPER_AVAILABLE:
+            raise RuntimeError(
+                "openai-whisper not installed. Run: pip install openai-whisper"
+            )
+        logging.info("Loading Whisper model '%s'...", model_size)
+        self.model = whisper.load_model(model_size)
+        logging.info("Whisper model loaded.")
+
+    def transcribe(self, audio_path: str) -> str:
+        """Transcribe audio file, return lowercase text."""
+        try:
+            result = self.model.transcribe(audio_path, language="en", fp16=False)
+            text = result.get("text", "").strip()
+            logging.debug("Whisper transcription: '%s'", text)
+            return text
+        except Exception as e:
+            logging.warning("Whisper transcription failed: %s", e)
+            return ""
+
+
+# ── Publisher backends ────────────────────────────────────────────────────────
+
+class ROS2Publisher:
+    """Publish voice commands as std_msgs/String on /saltybot/voice/cmd."""
+
+    def __init__(self):
+        rclpy.init()
+        self._node = Node("voice_commander")
+        self._pub = self._node.create_publisher(String, VOICE_CMD_TOPIC, 10)
+        self._spin_thread = threading.Thread(
+            target=lambda: rclpy.spin(self._node), daemon=True
+        )
+        self._spin_thread.start()
+
+    def publish(self, cmd: Command, raw_text: str) -> None:
+        payload = json.dumps({"command": cmd.value, "raw": raw_text, "ts": time.time()})
+        msg = String()
+        msg.data = payload
+        self._pub.publish(msg)
+        logging.info("ROS2 published: %s", payload)
+
+    def shutdown(self) -> None:
+        self._node.destroy_node()
+        rclpy.shutdown()
+
+
+class WebSocketPublisher:
+    """Publish voice commands over rosbridge WebSocket protocol."""
+
+    def __init__(self, host: str, port: int):
+        self.uri = f"ws://{host}:{port}"
+        self._ws: Optional[websocket.WebSocket] = None
+        self._lock = threading.Lock()
+        self._connect()
+
+    def _connect(self) -> None:
+        try:
+            ws = websocket.WebSocket()
+            ws.connect(self.uri, timeout=5)
+            with self._lock:
+                self._ws = ws
+            logging.info("WebSocket connected to %s", self.uri)
+        except Exception as e:
+            logging.warning("WebSocket connect failed (%s): %s", self.uri, e)
+            self._ws = None
+
+    def _ensure_connected(self) -> bool:
+        if self._ws is not None:
+            return True
+        self._connect()
+        return self._ws is not None
+
+    def publish(self, cmd: Command, raw_text: str) -> None:
+        payload = json.dumps({"command": cmd.value, "raw": raw_text, "ts": time.time()})
+        # rosbridge advertise + publish message
+        advertise_msg = json.dumps({
+            "op": "advertise",
+            "topic": VOICE_CMD_TOPIC,
+            "type": "std_msgs/String",
+        })
+        publish_msg = json.dumps({
+            "op": "publish",
+            "topic": VOICE_CMD_TOPIC,
+            "msg": {"data": payload},
+        })
+        with self._lock:
+            if not self._ensure_connected():
+                logging.error("Cannot publish — WebSocket not connected.")
+                return
+            try:
+                self._ws.send(advertise_msg)
+                self._ws.send(publish_msg)
+                logging.info("WS published: %s", payload)
+            except Exception as e:
+                logging.warning("WebSocket send failed: %s", e)
+                self._ws = None
+
+    def shutdown(self) -> None:
+        with self._lock:
+            if self._ws:
+                try:
+                    self._ws.close()
+                except Exception:
+                    pass
+                self._ws = None
+
+
+# ── Main listener loop ────────────────────────────────────────────────────────
+
+@dataclass
+class VoiceCommanderConfig:
+    host: str = "192.168.1.100"
+    port: int = 9090
+    model: str = "base"
+    wake_threshold: float = 0.6
+    record_sec: float = CMD_RECORD_SEC
+    no_tts: bool = False
+    debug: bool = False
+
+
+class VoiceCommander:
+    """
+    Main voice command loop.
+
+    State machine:
+      IDLE → record short clip → check for wake word
+      WAKE_DETECTED → record command clip → transcribe → parse → publish → confirm
+    """
+
+    def __init__(self, config: VoiceCommanderConfig):
+        self.cfg = config
+        self._running = False
+
+        # STT
+        self._stt = WhisperSTT(model_size=config.model)
+
+        # Publisher
+        if ROS2_AVAILABLE:
+            logging.info("Using ROS2 publisher backend.")
+            self._pub: ROS2Publisher | WebSocketPublisher = ROS2Publisher()
+        elif WS_AVAILABLE:
+            logging.info("Using WebSocket publisher backend (%s:%d).", config.host, config.port)
+            self._pub = WebSocketPublisher(config.host, config.port)
+        else:
+            raise RuntimeError(
+                "No publisher backend available. "
+                "Install rclpy (ROS2) or websocket-client: pip install websocket-client"
+            )
+
+    # ── lifecycle ──────────────────────────────────────────────────────────────
+
+    def start(self) -> None:
+        self._running = True
+        logging.info("Voice commander started. Listening for '%s'...", WAKE_WORD)
+        if not self.cfg.no_tts:
+            tts_speak("Hey Salty is ready")
+        try:
+            self._listen_loop()
+        except KeyboardInterrupt:
+            logging.info("Interrupted.")
+        finally:
+            self.stop()
+
+    def stop(self) -> None:
+        self._running = False
+        self._pub.shutdown()
+        logging.info("Voice commander stopped.")
+
+    # ── main loop ─────────────────────────────────────────────────────────────
+
+    def _listen_loop(self) -> None:
+        """Continuously poll for wake word then capture command."""
+        with tempfile.TemporaryDirectory() as tmpdir:
+            wake_audio = os.path.join(tmpdir, "wake.aac")
+            cmd_audio  = os.path.join(tmpdir, "cmd.aac")
+
+            while self._running:
+                # 1. Record short clip for wake word detection
+                logging.debug("Recording %.1fs for wake word...", WAKE_RECORD_SEC)
+                if not record_audio(WAKE_RECORD_SEC, wake_audio):
+                    logging.debug("Wake clip recording failed, retrying.")
+                    time.sleep(0.5)
+                    continue
+
+                wake_text = self._stt.transcribe(wake_audio)
+                if not wake_text:
+                    continue
+
+                if not contains_wake_word(wake_text, self.cfg.wake_threshold):
+                    logging.debug("No wake word in: '%s'", wake_text)
+                    continue
+
+                # 2. Wake word detected — acknowledge and record command
+                logging.info("Wake word detected! Recording command...")
+                if not self.cfg.no_tts:
+                    tts_speak("Yes?")
+
+                if not record_audio(self.cfg.record_sec, cmd_audio):
+                    logging.warning("Command clip recording failed.")
+                    continue
+
+                cmd_text = self._stt.transcribe(cmd_audio)
+                if not cmd_text:
+                    logging.info("No speech detected after wake word.")
+                    continue
+
+                # 3. Parse and dispatch
+                cmd = parse_command(cmd_text)
+                logging.info("Parsed command: %s (from '%s')", cmd.value, cmd_text)
+
+                self._pub.publish(cmd, cmd_text)
+
+                # 4. TTS confirmation
+                if not self.cfg.no_tts:
+                    tts_speak(TTS_CONFIRMATIONS[cmd])
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="SaltyBot phone voice command interface (Issue #553)"
+    )
+    parser.add_argument("--host",      default="192.168.1.100",
+                        help="Jetson IP/hostname (default: 192.168.1.100)")
+    parser.add_argument("--port",      type=int, default=9090,
+                        help="rosbridge WebSocket port (default: 9090)")
+    parser.add_argument("--model",     default="base",
+                        choices=["tiny", "base", "small"],
+                        help="Whisper model size (default: base)")
+    parser.add_argument("--threshold", type=float, default=0.6,
+                        help="Wake word match threshold 0.0-1.0 (default: 0.6)")
+    parser.add_argument("--record-sec", type=float, default=CMD_RECORD_SEC,
+                        help=f"Seconds to record command (default: {CMD_RECORD_SEC})")
+    parser.add_argument("--no-tts",    action="store_true",
+                        help="Disable TTS confirmation")
+    parser.add_argument("--debug",     action="store_true",
+                        help="Verbose logging")
+    args = parser.parse_args()
+
+    logging.basicConfig(
+        level=logging.DEBUG if args.debug else logging.INFO,
+        format="%(asctime)s [%(levelname)s] %(message)s",
+    )
+
+    if not WHISPER_AVAILABLE:
+        logging.error("openai-whisper not installed. Run: pip install openai-whisper")
+        sys.exit(1)
+
+    cfg = VoiceCommanderConfig(
+        host=args.host,
+        port=args.port,
+        model=args.model,
+        wake_threshold=args.threshold,
+        record_sec=args.record_sec,
+        no_tts=args.no_tts,
+        debug=args.debug,
+    )
+
+    VoiceCommander(cfg).start()
+
+
+if __name__ == "__main__":
+    main()

src/gimbal.c

@@ -0,0 +1,127 @@
+#include "gimbal.h"
+#include "servo_bus.h"
+#include "config.h"
+#include <stddef.h>
+
+/*
+ * gimbal.c — Pan/tilt gimbal controller for ST3215 bus servos (Issue #547)
+ *
+ * Tick rate: called every 1 ms from main loop; self-throttles to GIMBAL_TLM_HZ.
+ * Feedback polling alternates: pan on even ticks, tilt on odd ticks.
+ * This gives ~25 Hz per axis, keeping per-read latency under 2 ms total.
+ *
+ * Safety limits:
+ *   Pan:  -GIMBAL_PAN_LIMIT_DEG  .. +GIMBAL_PAN_LIMIT_DEG  (±180 deg)
+ *   Tilt: -GIMBAL_TILT_LIMIT_DEG .. +GIMBAL_TILT_LIMIT_DEG  (± 90 deg)
+ */
+
+#define TICK_PERIOD_MS          (1000u / GIMBAL_TLM_HZ)  /* 20 ms at 50 Hz */
+
+/* Clamp int16 to [lo, hi] */
+static int16_t _clamp16(int16_t v, int16_t lo, int16_t hi)
+{
+    if (v < lo) return lo;
+    if (v > hi) return hi;
+    return v;
+}
+
+/* ---- gimbal_init() ---- */
+
+void gimbal_init(gimbal_t *g)
+{
+    g->cmd_pan_x10  = 0;
+    g->cmd_tilt_x10 = 0;
+    g->cmd_speed    = 0;           /* 0 = max speed */
+    g->fb_pan_x10   = 0;
+    g->fb_tilt_x10  = 0;
+    g->fb_pan_speed = 0;
+    g->fb_tilt_speed = 0;
+    g->rx_ok        = 0;
+    g->rx_err       = 0;
+    g->_last_tick_ms = 0;
+    g->_poll_phase  = 0;
+
+    /* Enable torque and center both servos */
+    servo_bus_write_torque(GIMBAL_PAN_ID,  true);
+    servo_bus_write_torque(GIMBAL_TILT_ID, true);
+    g->torque_enabled = true;
+
+    uint16_t center = servo_bus_deg_to_raw(0.0f);
+    servo_bus_write_pos(GIMBAL_PAN_ID,  center, 0);
+    servo_bus_write_pos(GIMBAL_TILT_ID, center, 0);
+}
+
+/* ---- gimbal_set_pos() ---- */
+
+void gimbal_set_pos(gimbal_t *g, int16_t pan_x10, int16_t tilt_x10,
+                    uint16_t speed)
+{
+    /* Clamp to hardware limits */
+    pan_x10  = _clamp16(pan_x10,
+                        -(int16_t)(GIMBAL_PAN_LIMIT_DEG  * 10),
+                        (int16_t)(GIMBAL_PAN_LIMIT_DEG   * 10));
+    tilt_x10 = _clamp16(tilt_x10,
+                        -(int16_t)(GIMBAL_TILT_LIMIT_DEG * 10),
+                        (int16_t)(GIMBAL_TILT_LIMIT_DEG  * 10));
+    if (speed > SB_SPEED_MAX) speed = SB_SPEED_MAX;
+
+    g->cmd_pan_x10  = pan_x10;
+    g->cmd_tilt_x10 = tilt_x10;
+    g->cmd_speed    = speed;
+
+    float pan_deg  = (float)pan_x10  / 10.0f;
+    float tilt_deg = (float)tilt_x10 / 10.0f;
+
+    servo_bus_write_pos(GIMBAL_PAN_ID,
+                        servo_bus_deg_to_raw(pan_deg),  speed);
+    servo_bus_write_pos(GIMBAL_TILT_ID,
+                        servo_bus_deg_to_raw(tilt_deg), speed);
+}
+
+/* ---- gimbal_torque() ---- */
+
+void gimbal_torque(gimbal_t *g, bool enable)
+{
+    servo_bus_write_torque(GIMBAL_PAN_ID,  enable);
+    servo_bus_write_torque(GIMBAL_TILT_ID, enable);
+    g->torque_enabled = enable;
+}
+
+/* ---- gimbal_tick() ---- */
+
+void gimbal_tick(gimbal_t *g, uint32_t now_ms)
+{
+    if ((now_ms - g->_last_tick_ms) < TICK_PERIOD_MS) return;
+    g->_last_tick_ms = now_ms;
+
+    uint16_t raw = 0;
+
+    if (g->_poll_phase == 0u) {
+        /* Poll pan position */
+        if (servo_bus_read_pos(GIMBAL_PAN_ID, &raw)) {
+            g->fb_pan_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
+            g->rx_ok++;
+        } else {
+            g->rx_err++;
+        }
+
+        /* Also refresh pan speed */
+        uint16_t spd = 0;
+        (void)servo_bus_read_speed(GIMBAL_PAN_ID, &spd);
+        g->fb_pan_speed = spd;
+    } else {
+        /* Poll tilt position */
+        if (servo_bus_read_pos(GIMBAL_TILT_ID, &raw)) {
+            g->fb_tilt_x10 = (int16_t)(servo_bus_raw_to_deg(raw) * 10.0f);
+            g->rx_ok++;
+        } else {
+            g->rx_err++;
+        }
+
+        uint16_t spd = 0;
+        (void)servo_bus_read_speed(GIMBAL_TILT_ID, &spd);
+        g->fb_tilt_speed = spd;
+    }
+
+    g->_poll_phase ^= 1u;  /* toggle 0 / 1 */
+}

src/jlink.c

@@ -38,6 +38,14 @@ static void jlink_tx_locked(uint8_t *buf, uint16_t len)
 /* ---- Volatile state ---- */
 volatile JLinkState jlink_state;
 
+/* ---- SCHED_SET static receive buffer (Issue #550) ---- */
+static JLinkSchedSetBuf s_sched_set_buf;
+
+JLinkSchedSetBuf *jlink_get_sched_set(void)
+{
+    return &s_sched_set_buf;
+}
+
 /* ---- CRC16-XModem (poly 0x1021, init 0x0000) ---- */
 static uint16_t crc16_xmodem(const uint8_t *data, uint16_t len)
 {
@@ -67,7 +75,7 @@ void jlink_init(void)
     gpio.Alternate = GPIO_AF7_USART1;
     HAL_GPIO_Init(GPIOB, &gpio);
 
-    /* DMA2 Stream2 Channel4 — USART1_RX circular */
+    /* DMA2 Stream2 Channel4 -- USART1_RX circular */
     __HAL_RCC_DMA2_CLK_ENABLE();
     s_dma_rx.Instance                 = DMA2_Stream2;
     s_dma_rx.Init.Channel             = DMA_CHANNEL_4;
@@ -103,10 +111,11 @@ void jlink_init(void)
     HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 7, 0);
     HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
 
-    /* Start circular DMA RX — never stops */
+    /* Start circular DMA RX -- never stops */
     HAL_UART_Receive_DMA(&s_uart, s_rx_buf, JLINK_RX_BUF_LEN);
 
     memset((void *)&jlink_state, 0, sizeof(jlink_state));
+    memset(&s_sched_set_buf, 0, sizeof(s_sched_set_buf));
     s_rx_tail = 0;
 }
 
@@ -116,7 +125,7 @@ void USART1_IRQHandler(void)
     /* Clear IDLE flag by reading SR then DR */
     if (__HAL_UART_GET_FLAG(&s_uart, UART_FLAG_IDLE)) {
         __HAL_UART_CLEAR_IDLEFLAG(&s_uart);
-        /* jlink_process() drains the buffer from main loop — no work here */
+        /* jlink_process() drains the buffer from main loop -- no work here */
     }
     HAL_UART_IRQHandler(&s_uart);
 }
@@ -142,7 +151,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
     switch (cmd) {
 
     case JLINK_CMD_HEARTBEAT:
-        /* Heartbeat only — no payload action needed */
+        /* Heartbeat only -- no payload action needed */
         break;
 
     case JLINK_CMD_DRIVE:
@@ -150,7 +159,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
             int16_t spd, str;
             memcpy(&spd, payload,     2);
             memcpy(&str, payload + 2, 2);
-            /* Clamp to ±1000 */
+            /* Clamp to +/-1000 */
             if (spd >  1000) spd =  1000;
             if (spd < -1000) spd = -1000;
             if (str >  1000) str =  1000;
@@ -174,7 +183,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
             memcpy(&kp, payload,     4);
             memcpy(&ki, payload + 4, 4);
             memcpy(&kd, payload + 8, 4);
-            /* Sanity bounds — same as USB CDC PID handler in main.c */
+            /* Sanity bounds -- same as USB CDC PID handler in main.c */
             if (kp >= 0.0f && kp <= 500.0f) jlink_state.pid_kp = kp;
             if (ki >= 0.0f && ki <= 50.0f)  jlink_state.pid_ki = ki;
             if (kd >= 0.0f && kd <= 50.0f)  jlink_state.pid_kd = kd;
@@ -208,12 +217,62 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
         jlink_state.pid_save_req = 1u;
         break;
 
+    case JLINK_CMD_GIMBAL_POS:
+        /* Payload: int16 pan_x10, int16 tilt_x10, uint16 speed (6 bytes) Issue #547 */
+        if (plen == 6u) {
+            int16_t  pan, tilt;
+            uint16_t spd;
+            memcpy(&pan,  payload,     2);
+            memcpy(&tilt, payload + 2, 2);
+            memcpy(&spd,  payload + 4, 2);
+            jlink_state.gimbal_pan_x10  = pan;
+            jlink_state.gimbal_tilt_x10 = tilt;
+            jlink_state.gimbal_speed    = spd;
+            jlink_state.gimbal_updated  = 1u;
+        }
+        break;
+
+    case JLINK_CMD_SCHED_GET:
+        /* Payload-less; main loop calls jlink_send_sched_telemetry() (Issue #550) */
+        jlink_state.sched_get_req = 1u;
+        break;
+
+    case JLINK_CMD_SCHED_SET:
+        /* Payload: uint8 num_bands + num_bands * sizeof(pid_sched_entry_t) bytes */
+        if (plen >= 1u) {
+            uint8_t nb = payload[0];
+            if (nb == 0u) nb = 1u;
+            if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS;
+            uint8_t expected = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t);
+            if (plen >= expected) {
+                s_sched_set_buf.num_bands = nb;
+                memcpy(s_sched_set_buf.bands, payload + 1,
+                       nb * sizeof(pid_sched_entry_t));
+                s_sched_set_buf.ready = 1u;
+            }
+        }
+        break;
+
+    case JLINK_CMD_SCHED_SAVE:
+        /* Payload: float kp, float ki, float kd (12 bytes) for single-PID record */
+        if (plen == 12u) {
+            float kp, ki, kd;
+            memcpy(&kp, payload,     4);
+            memcpy(&ki, payload + 4, 4);
+            memcpy(&kd, payload + 8, 4);
+            if (kp >= 0.0f && kp <= 500.0f) jlink_state.sched_save_kp = kp;
+            if (ki >= 0.0f && ki <= 50.0f)  jlink_state.sched_save_ki = ki;
+            if (kd >= 0.0f && kd <= 50.0f)  jlink_state.sched_save_kd = kd;
+            jlink_state.sched_save_req = 1u;
+        }
+        break;
+
     default:
         break;
     }
 }
 
-/* ---- jlink_process() — call from main loop every tick ---- */
+/* ---- jlink_process() -- call from main loop every tick ---- */
 /*
  * Parser state machine.
  * Frame: [STX][LEN][CMD][PAYLOAD 0..LEN-1][CRC_hi][CRC_lo][ETX]
@@ -222,7 +281,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
  * Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255,
  * but we cap at 64 for safety).
  */
-#define JLINK_MAX_PAYLOAD   252u  /* enlarged for AUDIO chunks (126 × int16) */
+#define JLINK_MAX_PAYLOAD   252u  /* enlarged for AUDIO chunks (126 x int16) */
 
 typedef enum {
     PS_WAIT_STX = 0,
@@ -242,7 +301,7 @@ void jlink_process(void)
     static uint8_t    s_crc_hi  = 0;
 
     /* Compute how many bytes the DMA has written since last drain */
-    uint32_t head = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx);
+    uint32_t head  = JLINK_RX_BUF_LEN - __HAL_DMA_GET_COUNTER(&s_dma_rx);
     uint32_t bytes = (head - s_rx_tail) & (JLINK_RX_BUF_LEN - 1u);
 
     for (uint32_t i = 0; i < bytes; i++) {
@@ -256,7 +315,7 @@ void jlink_process(void)
 
         case PS_WAIT_LEN:
             if (b == 0u || b > JLINK_MAX_PAYLOAD + 1u) {
-                /* Invalid length — resync */
+                /* Invalid length -- resync */
                 s_state = PS_WAIT_STX;
             } else {
                 s_len   = b;
@@ -276,12 +335,12 @@ void jlink_process(void)
             break;
 
         case PS_WAIT_CRC_LO: {
-            uint16_t rx_crc  = ((uint16_t)s_crc_hi << 8) | b;
+            uint16_t rx_crc   = ((uint16_t)s_crc_hi << 8) | b;
             uint16_t calc_crc = crc16_xmodem(s_frame, s_len);
             if (rx_crc == calc_crc)
                 s_state = PS_WAIT_ETX;
             else
-                s_state = PS_WAIT_STX;   /* CRC mismatch — drop */
+                s_state = PS_WAIT_STX;   /* CRC mismatch -- drop */
             break;
         }
 
@@ -304,7 +363,7 @@ void jlink_send_telemetry(const jlink_tlm_status_t *status)
      * Frame: [STX][LEN][0x80][20 bytes STATUS][CRC_hi][CRC_lo][ETX]
      * LEN = 1 (CMD) + 20 (payload) = 21
      * Total frame length = 1+1+1+20+2+1 = 26 bytes
-     * At 921600 baud (10 bits/byte): 26×10/921600 ≈ 0.28ms — safe to block.
+     * At 921600 baud (10 bits/byte): 26x10/921600 ~0.28ms -- safe to block.
      */
     static uint8_t frame[26];
     const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_status_t);  /* 20 */
@@ -329,7 +388,7 @@ void jlink_send_power_telemetry(const jlink_tlm_power_t *power)
     /*
      * Frame: [STX][LEN][0x81][11 bytes POWER][CRC_hi][CRC_lo][ETX]
      * LEN = 1 (CMD) + 11 (payload) = 12; total = 17 bytes
-     * At 921600 baud: 17×10/921600 ≈ 0.18 ms — safe to block.
+     * At 921600 baud: 17x10/921600 ~0.18 ms -- safe to block.
      */
     static uint8_t frame[17];
     const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_power_t);  /* 11 */
@@ -373,13 +432,13 @@ void jlink_send_pid_result(const jlink_tlm_pid_result_t *result)
     jlink_tx_locked(frame_pid, sizeof(frame_pid));
 }
 
-/* ---- jlink_send_battery_telemetry() — Issue #533 ---- */
+/* ---- jlink_send_battery_telemetry() -- Issue #533 ---- */
 void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt)
 {
     /*
      * Frame: [STX][LEN][0x82][10 bytes BATTERY][CRC_hi][CRC_lo][ETX]
      * LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes
-     * At 921600 baud: 16×10/921600 ≈ 0.17 ms — safe to block.
+     * At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block.
      */
     static uint8_t frame[16];
     const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_battery_t);  /* 10 */
@@ -397,3 +456,58 @@ void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt)
 
     jlink_tx_locked(frame, sizeof(frame));
 }
+
+/* ---- jlink_send_gimbal_state() -- Issue #547 ---- */
+void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state)
+{
+    /*
+     * Frame: [STX][LEN][0x84][10 bytes GIMBAL_STATE][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 (CMD) + 10 (payload) = 11; total = 16 bytes
+     * At 921600 baud: 16x10/921600 ~0.17 ms -- safe to block.
+     */
+    static uint8_t frame[16];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_gimbal_state_t);  /* 10 */
+    const uint8_t  len  = 1u + plen;                                   /* 11 */
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_GIMBAL_STATE;
+    memcpy(&frame[3], state, plen);
+
+    uint16_t crc = crc16_xmodem(&frame[2], len);
+    frame[3 + plen]     = (uint8_t)(crc >> 8);
+    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
+    frame[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame, sizeof(frame));
+}
+
+/* ---- jlink_send_sched_telemetry() -- Issue #550 ---- */
+void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm)
+{
+    /*
+     * Frame: [STX][LEN][0x85][1+N*16 bytes SCHED][CRC_hi][CRC_lo][ETX]
+     * Actual payload = 1 (num_bands) + tlm->num_bands * 16 bytes.
+     * Max payload = 1 + 6*16 = 97; max frame = 103 bytes.
+     * At 921600 baud: 103x10/921600 ~1.1 ms -- safe to block.
+     */
+    uint8_t  nb   = tlm->num_bands;
+    if (nb > PID_SCHED_MAX_BANDS) nb = PID_SCHED_MAX_BANDS;
+    uint8_t  plen = 1u + nb * (uint8_t)sizeof(pid_sched_entry_t);
+    uint8_t  len  = 1u + plen;  /* CMD + payload */
+    /* frame: STX + LEN + CMD + payload + CRC_hi + CRC_lo + ETX */
+    uint8_t  frame[103];
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_SCHED;
+    frame[3] = nb;
+    memcpy(&frame[4], tlm->bands, nb * sizeof(pid_sched_entry_t));
+
+    uint16_t crc = crc16_xmodem(&frame[2], len);
+    frame[3 + plen]     = (uint8_t)(crc >> 8);
+    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
+    frame[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame, (uint16_t)(3u + plen + 3u));
+}

src/main.c

@@ -31,6 +31,8 @@
 #include "coulomb_counter.h"
 #include "watchdog.h"
 #include "pid_flash.h"
+#include "servo_bus.h"
+#include "gimbal.h"
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
@@ -208,6 +210,13 @@ int main(void) {
     /* Init servo pan-tilt driver for camera head (TIM4 PWM on PB6/PB7, Issue #206) */
     servo_init();
 
+    /* Init ST3215 serial bus servo driver (USART3 half-duplex PB10, Issue #547) */
+    servo_bus_init();
+
+    /* Init pan/tilt gimbal (ST3215 servos, centers both axes on boot) */
+    gimbal_t gimbal;
+    gimbal_init(&gimbal);
+
     /* Init HC-SR04 ultrasonic distance sensor (TIM1 input capture on PA1, Issue #243) */
     ultrasonic_init();
 
@@ -270,6 +279,9 @@ int main(void) {
         /* Servo pan-tilt animation tick — updates smooth sweeps */
         servo_tick(now);
 
+        /* Gimbal ST3215 feedback polling tick (self-throttles to GIMBAL_TLM_HZ) */
+        gimbal_tick(&gimbal, now);
+
         /* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
         battery_accumulate_coulombs();
 
@@ -347,6 +359,15 @@ int main(void) {
             jlink_state.sleep_req = 0u;
             power_mgmt_request_sleep();
         }
+        /* GIMBAL_POS: forward new pan/tilt command to gimbal (Issue #547) */
+        if (jlink_state.gimbal_updated) {
+            jlink_state.gimbal_updated = 0u;
+            gimbal_set_pos(&gimbal,
+                           (int16_t)jlink_state.gimbal_pan_x10,
+                           (int16_t)jlink_state.gimbal_tilt_x10,
+                           jlink_state.gimbal_speed);
+        }
+
         /* PID_SAVE: persist current gains to flash, reply with result (Issue #531).
          * Only allowed while disarmed -- flash sector erase stalls CPU for ~1 s. */
         if (jlink_state.pid_save_req && bal.state != BALANCE_ARMED) {
@@ -558,6 +579,19 @@ int main(void) {
             tlm.fw_minor     = FW_MINOR;
             tlm.fw_patch     = FW_PATCH;
             jlink_send_telemetry(&tlm);
+
+            /* Send gimbal state at same 50 Hz cadence (Issue #547) */
+            jlink_tlm_gimbal_state_t gtlm;
+            gtlm.pan_x10        = gimbal.fb_pan_x10;
+            gtlm.tilt_x10       = gimbal.fb_tilt_x10;
+            gtlm.pan_speed_raw  = gimbal.fb_pan_speed;
+            gtlm.tilt_speed_raw = gimbal.fb_tilt_speed;
+            gtlm.torque_en      = gimbal.torque_enabled ? 1u : 0u;
+            uint32_t rx_total   = gimbal.rx_ok + gimbal.rx_err;
+            gtlm.rx_err_pct     = (rx_total > 0u)
+                                      ? (uint8_t)(gimbal.rx_err * 100u / rx_total)
+                                      : 0u;
+            jlink_send_gimbal_state(&gtlm);
         }
 
         /* JLINK_TLM_POWER telemetry at PM_TLM_HZ (1 Hz) */

src/pid_flash.c

@@ -8,7 +8,7 @@ bool pid_flash_load(float *kp, float *ki, float *kd)
 
     if (p->magic != PID_FLASH_MAGIC) return false;
 
-    /* Basic sanity bounds -- same as JLINK_CMD_PID_SET handler */
+    /* Basic sanity bounds — same as JLINK_CMD_PID_SET handler */
     if (p->kp < 0.0f || p->kp > 500.0f) return false;
     if (p->ki < 0.0f || p->ki > 50.0f)  return false;
     if (p->kd < 0.0f || p->kd > 50.0f)  return false;
@@ -49,7 +49,7 @@ bool pid_flash_save(float kp, float ki, float kd)
     rec.ki    = ki;
     rec.kd    = kd;
 
-    /* Write 64 bytes as 16 x 32-bit words */
+    /* Write 64 bytes as 16 × 32-bit words */
     const uint32_t *src  = (const uint32_t *)&rec;
     uint32_t        addr = PID_FLASH_STORE_ADDR;
     for (uint8_t i = 0; i < sizeof(rec) / 4u; i++) {
@@ -70,92 +70,3 @@ bool pid_flash_save(float kp, float ki, float kd)
             stored->ki == ki &&
             stored->kd == kd);
 }
-
-/* ---- Helper: write N bytes to flash as 32-bit words ---- */
-static bool flash_write_words(uint32_t addr, const void *src_buf, uint32_t byte_len)
-{
-    const uint32_t *w = (const uint32_t *)src_buf;
-    for (uint32_t i = 0; i < byte_len / 4u; i++) {
-        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, w[i]) != HAL_OK)
-            return false;
-        addr += 4u;
-    }
-    return true;
-}
-
-/* ---- pid_flash_load_schedule() ---- */
-bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n)
-{
-    const pid_sched_flash_t *p = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR;
-
-    if (p->magic != PID_SCHED_MAGIC) return false;
-    if (p->num_bands == 0u || p->num_bands > PID_SCHED_MAX_BANDS) return false;
-
-    memcpy(out_entries, p->bands, p->num_bands * sizeof(pid_sched_entry_t));
-    *out_n = p->num_bands;
-    return true;
-}
-
-/* ---- pid_flash_save_all() ---- */
-bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
-                        const pid_sched_entry_t *entries, uint8_t num_bands)
-{
-    HAL_StatusTypeDef rc;
-
-    if (num_bands == 0u || num_bands > PID_SCHED_MAX_BANDS) return false;
-
-    rc = HAL_FLASH_Unlock();
-    if (rc != HAL_OK) return false;
-
-    /* Erase sector 7 -- one erase covers both records */
-    FLASH_EraseInitTypeDef erase = {
-        .TypeErase    = FLASH_TYPEERASE_SECTORS,
-        .Sector       = PID_FLASH_SECTOR,
-        .NbSectors    = 1,
-        .VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
-    };
-    uint32_t sector_error = 0u;
-    rc = HAL_FLASHEx_Erase(&erase, &sector_error);
-    if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
-        HAL_FLASH_Lock();
-        return false;
-    }
-
-    /* Build and write schedule record at PID_SCHED_FLASH_ADDR */
-    pid_sched_flash_t srec;
-    memset(&srec, 0xFF, sizeof(srec));
-    srec.magic     = PID_SCHED_MAGIC;
-    srec.num_bands = num_bands;
-    srec.flags     = 0u;
-    srec._pad0[0]  = 0u;
-    srec._pad0[1]  = 0u;
-    memcpy(srec.bands, entries, num_bands * sizeof(pid_sched_entry_t));
-
-    if (!flash_write_words(PID_SCHED_FLASH_ADDR, &srec, sizeof(srec))) {
-        HAL_FLASH_Lock();
-        return false;
-    }
-
-    /* Build and write single-PID record at PID_FLASH_STORE_ADDR */
-    pid_flash_t prec;
-    memset(&prec, 0xFF, sizeof(prec));
-    prec.magic = PID_FLASH_MAGIC;
-    prec.kp    = kp_single;
-    prec.ki    = ki_single;
-    prec.kd    = kd_single;
-
-    if (!flash_write_words(PID_FLASH_STORE_ADDR, &prec, sizeof(prec))) {
-        HAL_FLASH_Lock();
-        return false;
-    }
-
-    HAL_FLASH_Lock();
-
-    /* Verify both records */
-    const pid_sched_flash_t *sv = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR;
-    const pid_flash_t       *pv = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
-
-    return (sv->magic == PID_SCHED_MAGIC && sv->num_bands == num_bands &&
-            pv->magic == PID_FLASH_MAGIC && pv->kp == kp_single &&
-            pv->ki == ki_single && pv->kd == kd_single);
-}

src/servo_bus.c

@@ -0,0 +1,189 @@
+#include "servo_bus.h"
+#include "config.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+/*
+ * servo_bus.c — Feetech/ST3215 serial bus servo driver (Issue #547)
+ *
+ * Half-duplex on USART3 PB10 (USART3_TX, AF7) at 1 Mbps.
+ * HDSEL bit (CR3.3) routes TX and RX to the TX pin.
+ * TX direction: TX active, RX gated.
+ * RX direction: TX idle, RX active — enabled after TC interrupt.
+ *
+ * All operations are blocking with short timeouts; the servo bus
+ * is polled at 50 Hz from the main loop (not from an ISR).
+ */
+
+static UART_HandleTypeDef s_uart;
+
+/* ---- Init ---- */
+
+void servo_bus_init(void)
+{
+    /* GPIO: PB10 open-drain AF7 (USART3_TX) with external pull-up on bus */
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    GPIO_InitTypeDef gpio = {0};
+    gpio.Pin       = SERVO_BUS_PIN;
+    gpio.Mode      = GPIO_MODE_AF_OD;      /* open-drain for half-duplex bus */
+    gpio.Pull      = GPIO_PULLUP;
+    gpio.Speed     = GPIO_SPEED_FREQ_VERY_HIGH;
+    gpio.Alternate = GPIO_AF7_USART3;
+    HAL_GPIO_Init(SERVO_BUS_PORT, &gpio);
+
+    /* USART3 at SERVO_BUS_BAUD (1 Mbps), 8N1 */
+    __HAL_RCC_USART3_CLK_ENABLE();
+    s_uart.Instance          = USART3;
+    s_uart.Init.BaudRate     = SERVO_BUS_BAUD;
+    s_uart.Init.WordLength   = UART_WORDLENGTH_8B;
+    s_uart.Init.StopBits     = UART_STOPBITS_1;
+    s_uart.Init.Parity       = UART_PARITY_NONE;
+    s_uart.Init.Mode         = UART_MODE_TX_RX;
+    s_uart.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
+    s_uart.Init.OverSampling = UART_OVERSAMPLING_8;  /* required for 1 Mbps @ APB1 54 MHz */
+    HAL_UART_Init(&s_uart);
+
+    /* Enable half-duplex: HDSEL bit in CR3 routes TX/RX to TX pin */
+    SET_BIT(s_uart.Instance->CR3, USART_CR3_HDSEL);
+}
+
+/* ---- Checksum ---- */
+
+static uint8_t _cksum(uint8_t id, uint8_t len, uint8_t instr,
+                      const uint8_t *params, uint8_t nparams)
+{
+    uint16_t sum = (uint16_t)id + len + instr;
+    for (uint8_t i = 0; i < nparams; i++) sum += params[i];
+    return (uint8_t)(~sum & 0xFFu);
+}
+
+/* ---- Transmit a raw packet ---- */
+
+static bool _tx(uint8_t id, uint8_t instr, const uint8_t *params, uint8_t nparams)
+{
+    uint8_t  len = nparams + 2u;  /* instr + params + cksum */
+    uint8_t  buf[32];
+    uint8_t  n = 0;
+
+    buf[n++] = 0xFFu;
+    buf[n++] = 0xFFu;
+    buf[n++] = id;
+    buf[n++] = len;
+    buf[n++] = instr;
+    for (uint8_t i = 0; i < nparams; i++) buf[n++] = params[i];
+    buf[n++] = _cksum(id, len, instr, params, nparams);
+
+    return HAL_UART_Transmit(&s_uart, buf, n, 10u) == HAL_OK;
+}
+
+/* ---- Receive response packet ---- */
+
+/*
+ * Read one byte with timeout. In half-duplex the TX line is still driving
+ * after the last stop bit; we must wait ~1 bit-time before the servo begins
+ * its response. HAL_UART_Receive handles the timing via the timeout.
+ */
+static bool _rx(uint8_t id, uint8_t *data, uint8_t ndata)
+{
+    uint8_t hdr[4];   /* 0xFF 0xFF ID LEN */
+    uint8_t tail[2];  /* ERROR CKSUM (or just CKSUM if ndata==0) */
+
+    /* Read header */
+    if (HAL_UART_Receive(&s_uart, hdr, 4u, SB_RX_TIMEOUT_MS) != HAL_OK)
+        return false;
+
+    /* Validate header */
+    if (hdr[0] != 0xFFu || hdr[1] != 0xFFu || hdr[2] != id)
+        return false;
+
+    uint8_t pkt_len = hdr[3];              /* ERROR + DATA... + CKSUM */
+    if (pkt_len < 2u || pkt_len > 20u)     /* sanity */
+        return false;
+
+    /* Read payload: ERROR + DATA bytes + CKSUM */
+    uint8_t  payload[20];
+    uint8_t  payload_len = pkt_len;        /* includes error byte and cksum */
+    if (HAL_UART_Receive(&s_uart, payload, payload_len, SB_RX_TIMEOUT_MS) != HAL_OK)
+        return false;
+
+    /* Verify checksum: ~(ID + LEN + ERROR + DATA) */
+    uint16_t sum = (uint16_t)id + pkt_len;
+    for (uint8_t i = 0; i < (uint8_t)(pkt_len - 1u); i++) sum += payload[i];
+    uint8_t expected_cksum = (uint8_t)(~sum & 0xFFu);
+    if (payload[pkt_len - 1u] != expected_cksum)
+        return false;
+
+    /* payload[0] = ERROR; payload[1..] = data */
+    if (payload[0] != 0u) return false;    /* servo reported error */
+
+    uint8_t n_data_bytes = pkt_len - 2u;   /* minus ERROR and CKSUM */
+    if (n_data_bytes < ndata) return false;
+
+    for (uint8_t i = 0; i < ndata; i++) data[i] = payload[1u + i];
+    return true;
+}
+
+/* ---- Public API ---- */
+
+bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed)
+{
+    if (raw_pos > SB_POS_MAX) raw_pos = SB_POS_MAX;
+    if (speed   > SB_SPEED_MAX) speed = SB_SPEED_MAX;
+
+    /* Write 4 bytes starting at SB_REG_GOAL_POS_L:
+     * [addr][POS_L][POS_H][SPD_L][SPD_H] */
+    uint8_t params[5] = {
+        SB_REG_GOAL_POS_L,
+        (uint8_t)(raw_pos & 0xFFu),
+        (uint8_t)(raw_pos >> 8),
+        (uint8_t)(speed   & 0xFFu),
+        (uint8_t)(speed   >> 8),
+    };
+    return _tx(id, SB_INSTR_WRITE, params, 5u);
+    /* WRITE does not generate a status packet by default */
+}
+
+bool servo_bus_write_torque(uint8_t id, bool enable)
+{
+    uint8_t params[2] = { SB_REG_TORQUE_EN, enable ? 1u : 0u };
+    return _tx(id, SB_INSTR_WRITE, params, 2u);
+}
+
+bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos)
+{
+    /* READ instruction: [addr][count] */
+    uint8_t params[2] = { SB_REG_PRES_POS_L, 2u };
+    if (!_tx(id, SB_INSTR_READ, params, 2u)) return false;
+
+    uint8_t data[2];
+    if (!_rx(id, data, 2u)) return false;
+
+    *raw_pos = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
+    return true;
+}
+
+bool servo_bus_read_speed(uint8_t id, uint16_t *speed)
+{
+    uint8_t params[2] = { SB_REG_PRES_SPD_L, 2u };
+    if (!_tx(id, SB_INSTR_READ, params, 2u)) return false;
+
+    uint8_t data[2];
+    if (!_rx(id, data, 2u)) return false;
+
+    *speed = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
+    return true;
+}
+
+uint16_t servo_bus_deg_to_raw(float deg)
+{
+    /* Center = 0 deg = 2048 raw; 1 deg = 4096/360 raw */
+    int32_t raw = (int32_t)(SB_POS_CENTER + (int32_t)(deg * (4096.0f / 360.0f)));
+    if (raw < 0)             raw = 0;
+    if (raw > (int32_t)SB_POS_MAX) raw = (int32_t)SB_POS_MAX;
+    return (uint16_t)raw;
+}
+
+float servo_bus_raw_to_deg(uint16_t raw)
+{
+    return ((float)raw - (float)SB_POS_CENTER) * (360.0f / 4096.0f);
+}

ui/diagnostics_panel.css

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

ui/diagnostics_panel.html

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

ui/diagnostics_panel.js

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

ui/gimbal_panel.css

@@ -0,0 +1,210 @@
+/* gimbal_panel.css — Saltybot Gimbal Control Panel (Issue #551) */
+
+*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
+
+body {
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: #050510;
+  color: #d1d5db;
+  height: 100dvh;
+  overflow: hidden;
+  display: flex;
+  flex-direction: column;
+}
+
+/* ── Header ── */
+#header {
+  display: flex;
+  align-items: center;
+  justify-content: space-between;
+  padding: 6px 16px;
+  background: #070712;
+  border-bottom: 1px solid #083344;
+  flex-shrink: 0;
+  flex-wrap: wrap;
+  gap: 6px;
+}
+#header .logo { color: #f97316; font-weight: bold; letter-spacing: 0.15em; font-size: 13px; }
+#conn-bar { display: flex; align-items: center; gap: 6px; }
+#conn-dot { width: 8px; height: 8px; border-radius: 50%; background: #374151; flex-shrink: 0; }
+#conn-dot.connected { background: #4ade80; }
+#conn-dot.error { background: #f87171; }
+#ws-input {
+  background: #111827; border: 1px solid #1e3a5f; border-radius: 4px;
+  color: #67e8f9; padding: 2px 8px; font-family: monospace; font-size: 11px; width: 200px;
+}
+#ws-input:focus { outline: none; border-color: #0891b2; }
+.btn {
+  padding: 3px 10px; border-radius: 4px; border: 1px solid; cursor: pointer;
+  font-family: monospace; font-size: 10px; font-weight: bold; letter-spacing: 0.05em;
+  transition: background 0.15s;
+}
+.btn-cyan  { background: #083344; border-color: #155e75; color: #67e8f9; }
+.btn-cyan:hover  { background: #0e4f69; }
+.btn-green { background: #052e16; border-color: #166534; color: #4ade80; }
+.btn-green:hover { background: #0a4a24; }
+.btn-amber { background: #451a03; border-color: #92400e; color: #fcd34d; }
+.btn-amber:hover { background: #6b2b04; }
+.btn-red   { background: #450a0a; border-color: #991b1b; color: #f87171; }
+.btn-red:hover   { background: #6b1010; }
+.btn-red.active  { background: #7f1d1d; border-color: #dc2626; color: #fca5a5; animation: pulse 1.5s infinite; }
+
+@keyframes pulse {
+  0%, 100% { opacity: 1; }
+  50%       { opacity: 0.65; }
+}
+
+/* ── Main layout ── */
+#main {
+  flex: 1;
+  display: grid;
+  grid-template-columns: 1fr 280px;
+  gap: 12px;
+  padding: 12px;
+  min-height: 0;
+}
+
+@media (max-width: 720px) {
+  #main { grid-template-columns: 1fr; grid-template-rows: 1fr auto; }
+  body { font-size: 11px; overflow-y: auto; height: auto; }
+}
+
+/* ── Camera feed ── */
+#camera-section {
+  display: flex; flex-direction: column; gap: 8px; min-height: 0;
+}
+#cam-toolbar {
+  display: flex; gap: 6px; align-items: center; flex-shrink: 0; flex-wrap: wrap;
+}
+#cam-toolbar span { color: #6b7280; font-size: 10px; margin-left: auto; }
+#camera-frame {
+  flex: 1;
+  background: #000;
+  border-radius: 8px;
+  border: 1px solid #083344;
+  display: flex; align-items: center; justify-content: center;
+  position: relative;
+  overflow: hidden;
+  min-height: 160px;
+}
+#camera-img {
+  max-width: 100%; max-height: 100%;
+  object-fit: contain;
+  display: none;
+}
+#camera-img.visible { display: block; }
+#no-signal {
+  display: flex; flex-direction: column; align-items: center; gap: 6px;
+  color: #374151; user-select: none;
+}
+#no-signal .icon { font-size: 36px; }
+#fps-badge {
+  position: absolute; top: 8px; right: 8px;
+  background: rgba(0,0,0,0.7); color: #4ade80;
+  padding: 2px 6px; border-radius: 4px; font-size: 10px;
+  display: none;
+}
+#fps-badge.visible { display: block; }
+
+/* Angle overlay on camera */
+#angle-overlay {
+  position: absolute; bottom: 8px; left: 8px;
+  background: rgba(0,0,0,0.7);
+  padding: 4px 8px; border-radius: 4px;
+  color: #67e8f9; font-size: 10px; font-family: monospace;
+  display: none;
+}
+#angle-overlay.visible { display: block; }
+
+/* ── Controls sidebar ── */
+#controls {
+  display: flex; flex-direction: column; gap: 10px; overflow-y: auto; min-height: 0;
+}
+
+.card {
+  background: #070712; border: 1px solid #083344;
+  border-radius: 8px; padding: 10px;
+}
+.card-title {
+  font-size: 9px; font-weight: bold; letter-spacing: 0.15em;
+  color: #0891b2; margin-bottom: 8px; text-transform: uppercase;
+}
+
+/* ── Pan/Tilt Joystick ── */
+#pad-wrap {
+  display: flex; flex-direction: column; align-items: center; gap: 6px;
+}
+#gimbal-pad {
+  cursor: crosshair;
+  border-radius: 6px;
+  border: 1px solid #1e3a5f;
+  touch-action: none;
+  user-select: none;
+}
+.pad-labels {
+  display: grid; grid-template-columns: 1fr auto 1fr;
+  width: 200px; font-size: 9px; color: #4b5563; text-align: center; align-items: center;
+}
+
+/* ── Angle display ── */
+#angle-display {
+  display: grid; grid-template-columns: 1fr 1fr; gap: 6px;
+}
+.angle-box {
+  background: #0a0a1a; border: 1px solid #1e3a5f; border-radius: 6px;
+  padding: 6px; display: flex; flex-direction: column; gap: 2px;
+}
+.angle-label { font-size: 9px; color: #6b7280; }
+.angle-value { font-size: 18px; color: #67e8f9; font-family: monospace; }
+.angle-unit  { font-size: 9px; color: #374151; }
+
+/* ── Presets ── */
+#presets {
+  display: grid; grid-template-columns: 1fr 1fr 1fr; gap: 6px;
+}
+#presets button { padding: 6px 4px; text-align: center; line-height: 1.3; }
+#presets button .preset-name { display: block; font-size: 9px; font-weight: bold; letter-spacing: 0.05em; }
+#presets button .preset-val  { display: block; font-size: 9px; color: #6b7280; }
+
+/* Home button */
+#btn-home {
+  width: 100%; padding: 8px;
+  background: #1c1c2e; border: 1px solid #374151; border-radius: 6px;
+  color: #9ca3af; font-family: monospace; font-size: 10px; font-weight: bold;
+  letter-spacing: 0.1em; cursor: pointer; transition: background 0.15s;
+}
+#btn-home:hover { background: #2d2d44; color: #d1d5db; }
+
+/* Tracking toggle */
+#tracking-row {
+  display: flex; align-items: center; justify-content: space-between;
+}
+.toggle-label { font-size: 10px; color: #9ca3af; }
+.toggle-switch {
+  position: relative; width: 40px; height: 20px;
+}
+.toggle-switch input { opacity: 0; width: 0; height: 0; }
+.toggle-slider {
+  position: absolute; inset: 0;
+  background: #1f2937; border-radius: 10px; cursor: pointer;
+  transition: background 0.2s;
+}
+.toggle-slider::before {
+  content: ''; position: absolute;
+  width: 14px; height: 14px; border-radius: 50%;
+  background: #6b7280; top: 3px; left: 3px;
+  transition: transform 0.2s, background 0.2s;
+}
+.toggle-switch input:checked + .toggle-slider { background: #0c4a6e; }
+.toggle-switch input:checked + .toggle-slider::before {
+  background: #38bdf8; transform: translateX(20px);
+}
+
+/* ── Footer ── */
+#footer {
+  background: #070712; border-top: 1px solid #083344;
+  padding: 4px 16px;
+  display: flex; align-items: center; justify-content: space-between;
+  flex-shrink: 0; font-size: 10px; color: #374151;
+}

ui/gimbal_panel.html

@@ -0,0 +1,183 @@
+<!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 — Gimbal Control</title>
+<link rel="stylesheet" href="gimbal_panel.css">
+<!-- roslib from CDN -->
+<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
+<style>
+/* Cam button active state (can't use CSS-only with JS-toggled class without Tailwind) */
+.cam-btn { padding: 3px 10px; border-radius: 4px; border: 1px solid #1e3a5f;
+           background: #070712; color: #4b5563; font-family: monospace;
+           font-size: 10px; font-weight: bold; cursor: pointer; transition: all 0.15s; }
+.cam-btn:hover { color: #d1d5db; }
+.cam-btn.active { background: #083344; border-color: #155e75; color: #67e8f9; }
+</style>
+</head>
+<body>
+
+<!-- ── Header ── -->
+<div id="header">
+  <div class="logo">⚡ SALTYBOT — GIMBAL</div>
+
+  <div id="conn-bar">
+    <div id="conn-dot"></div>
+    <input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
+    <button id="btn-connect" class="btn btn-cyan">CONNECT</button>
+    <span id="conn-status" style="color:#4b5563;font-size:10px;">Not connected</span>
+  </div>
+</div>
+
+<!-- ── Main ── -->
+<div id="main">
+
+  <!-- ── Left: Camera feed ── -->
+  <section id="camera-section">
+
+    <!-- Camera selector toolbar -->
+    <div id="cam-toolbar">
+      <button class="cam-btn active" data-cam="front">FRONT</button>
+      <button class="cam-btn"        data-cam="rear" >REAR</button>
+      <button class="cam-btn"        data-cam="left" >LEFT</button>
+      <button class="cam-btn"        data-cam="right">RIGHT</button>
+      <span id="cam-topic">/camera/front/image_raw/compressed</span>
+    </div>
+
+    <!-- Video frame -->
+    <div id="camera-frame">
+      <img id="camera-img" alt="gimbal camera feed" />
+      <div id="no-signal">
+        <div class="icon">📷</div>
+        <div>NO SIGNAL</div>
+      </div>
+      <div id="fps-badge">— fps</div>
+      <div id="angle-overlay">PAN 0.0°  TILT 0.0°</div>
+    </div>
+
+  </section>
+
+  <!-- ── Right: Controls ── -->
+  <aside id="controls">
+
+    <!-- Pan/Tilt pad -->
+    <div class="card">
+      <div class="card-title">PAN / TILT CONTROL</div>
+      <div id="pad-wrap">
+        <div class="pad-labels">
+          <span>← PAN</span>
+          <span style="color:#155e75">DRAG</span>
+          <span>PAN →</span>
+        </div>
+        <canvas id="gimbal-pad" width="200" height="150"></canvas>
+        <div style="font-size:9px;color:#374151;text-align:center">
+          ↑ TILT UP &nbsp;|&nbsp; ↓ TILT DOWN
+        </div>
+      </div>
+    </div>
+
+    <!-- Current angles -->
+    <div class="card">
+      <div class="card-title">CURRENT ANGLES (feedback)</div>
+      <div id="angle-display">
+        <div class="angle-box">
+          <div class="angle-label">PAN</div>
+          <div class="angle-value" id="pan-val">0.0</div>
+          <div class="angle-unit">degrees</div>
+        </div>
+        <div class="angle-box">
+          <div class="angle-label">TILT</div>
+          <div class="angle-value" id="tilt-val">0.0</div>
+          <div class="angle-unit">degrees</div>
+        </div>
+      </div>
+    </div>
+
+    <!-- Preset positions -->
+    <div class="card">
+      <div class="card-title">PRESET POSITIONS</div>
+      <div id="presets">
+        <button class="btn btn-cyan" data-preset="0">
+          <span class="preset-name">CENTER</span>
+          <span class="preset-val">0° / 0°</span>
+        </button>
+        <button class="btn btn-cyan" data-preset="1">
+          <span class="preset-name">LEFT</span>
+          <span class="preset-val">-45° / 0°</span>
+        </button>
+        <button class="btn btn-cyan" data-preset="2">
+          <span class="preset-name">RIGHT</span>
+          <span class="preset-val">+45° / 0°</span>
+        </button>
+        <button class="btn btn-cyan" data-preset="3">
+          <span class="preset-name">UP</span>
+          <span class="preset-val">0° / +45°</span>
+        </button>
+        <button class="btn btn-cyan" data-preset="4">
+          <span class="preset-name">DOWN</span>
+          <span class="preset-val">0° / -30°</span>
+        </button>
+      </div>
+      <button id="btn-home" style="margin-top:8px">⌂ HOME (0° / 0°)</button>
+    </div>
+
+    <!-- Person tracking -->
+    <div class="card">
+      <div class="card-title">PERSON TRACKING</div>
+      <div id="tracking-row">
+        <span class="toggle-label" id="tracking-label">OFF — manual control</span>
+        <label class="toggle-switch">
+          <input type="checkbox" id="tracking-toggle">
+          <span class="toggle-slider"></span>
+        </label>
+      </div>
+      <div style="font-size:9px;color:#4b5563;margin-top:6px">
+        Publishes to <code>/gimbal/tracking_enabled</code>
+      </div>
+    </div>
+
+    <!-- Topic info -->
+    <div class="card" style="font-size:9px;color:#4b5563;line-height:1.8">
+      <div class="card-title">ROS TOPICS</div>
+      <div>PUB <code style="color:#374151">/gimbal/cmd</code> → geometry_msgs/Vector3</div>
+      <div>SUB <code style="color:#374151">/gimbal/state</code> → geometry_msgs/Vector3</div>
+      <div>PUB <code style="color:#374151">/gimbal/tracking_enabled</code> → std_msgs/Bool</div>
+      <div>SUB <code style="color:#374151">/camera/*/image_raw/compressed</code></div>
+    </div>
+
+  </aside>
+</div>
+
+<!-- ── Footer ── -->
+<div id="footer">
+  <span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
+  <span>gimbal control panel — issue #551</span>
+</div>
+
+<script src="gimbal_panel.js"></script>
+<script>
+  // Sync footer ws URL
+  document.getElementById('ws-input').addEventListener('input', (e) => {
+    document.getElementById('footer-ws').textContent = e.target.value;
+  });
+
+  // Tracking label update
+  document.getElementById('tracking-toggle').addEventListener('change', (e) => {
+    document.getElementById('tracking-label').textContent =
+      e.target.checked ? 'ON — following person' : 'OFF — manual control';
+  });
+
+  // Camera topic label update
+  document.querySelectorAll('.cam-btn').forEach((btn) => {
+    btn.addEventListener('click', () => {
+      const cam = btn.dataset.cam;
+      document.getElementById('cam-topic').textContent =
+        `/camera/${cam}/image_raw/compressed`;
+      document.getElementById('footer-ws').textContent =
+        document.getElementById('ws-input').value;
+    });
+  });
+</script>
+</body>
+</html>

ui/gimbal_panel.js

@@ -0,0 +1,392 @@
+/**
+ * gimbal_panel.js — Saltybot Gimbal Control Panel (Issue #551)
+ *
+ * Connects to rosbridge via WebSocket (roslib.js).
+ * Publishes pan/tilt commands, subscribes to gimbal state + camera.
+ *
+ * ROS topics:
+ *   /gimbal/cmd              geometry_msgs/Vector3   publish  (x=pan°, y=tilt°)
+ *   /gimbal/state            geometry_msgs/Vector3   subscribe (x=pan°, y=tilt°)
+ *   /gimbal/tracking_enabled std_msgs/Bool           publish
+ *   /camera/front/image_raw/compressed  sensor_msgs/CompressedImage  subscribe
+ */
+
+'use strict';
+
+// ── Config ────────────────────────────────────────────────────────────────────
+
+const PAN_MIN   = -180;  // degrees
+const PAN_MAX   =  180;
+const TILT_MIN  =  -45;  // degrees
+const TILT_MAX  =   90;
+const CMD_TOPIC      = '/gimbal/cmd';
+const STATE_TOPIC    = '/gimbal/state';
+const TRACKING_TOPIC = '/gimbal/tracking_enabled';
+const CAM_TOPIC      = '/camera/front/image_raw/compressed';
+const CAM_TOPICS = {
+  front: '/camera/front/image_raw/compressed',
+  rear:  '/camera/rear/image_raw/compressed',
+  left:  '/camera/left/image_raw/compressed',
+  right: '/camera/right/image_raw/compressed',
+};
+
+const PRESETS = [
+  { name: 'CENTER', pan:   0, tilt:  0 },
+  { name: 'LEFT',   pan: -45, tilt:  0 },
+  { name: 'RIGHT',  pan:  45, tilt:  0 },
+  { name: 'UP',     pan:   0, tilt: 45 },
+  { name: 'DOWN',   pan:   0, tilt:-30 },
+  { name: 'HOME',   pan:   0, tilt:  0 },
+];
+
+// ── State ─────────────────────────────────────────────────────────────────────
+
+let ros           = null;
+let cmdTopic      = null;
+let trackingTopic = null;
+let stateSub      = null;
+let camSub        = null;
+
+let currentPan    = 0;
+let currentTilt   = 0;
+let targetPan     = 0;
+let targetTilt    = 0;
+let trackingOn    = false;
+let selectedCam   = 'front';
+
+// Camera FPS tracking
+let fpsCount = 0;
+let fpsTime  = Date.now();
+let fps      = 0;
+
+// Pad dragging state
+let padDragging = false;
+
+// ── DOM refs ──────────────────────────────────────────────────────────────────
+
+const connDot    = document.getElementById('conn-dot');
+const connStatus = document.getElementById('conn-status');
+const wsInput    = document.getElementById('ws-input');
+const padCanvas  = document.getElementById('gimbal-pad');
+const panVal     = document.getElementById('pan-val');
+const tiltVal    = document.getElementById('tilt-val');
+const camImg     = document.getElementById('camera-img');
+const noSignal   = document.getElementById('no-signal');
+const fpsBadge   = document.getElementById('fps-badge');
+const angleOvl   = document.getElementById('angle-overlay');
+
+// ── Connection ────────────────────────────────────────────────────────────────
+
+function connect() {
+  const url = wsInput.value.trim();
+  if (!url) return;
+  if (ros) { ros.close(); }
+
+  ros = new ROSLIB.Ros({ url });
+
+  ros.on('connection', () => {
+    connDot.className = 'connected';
+    connStatus.textContent = url;
+    document.getElementById('btn-connect').textContent = 'RECONNECT';
+    setupTopics();
+  });
+
+  ros.on('error', (err) => {
+    connDot.className = 'error';
+    connStatus.textContent = 'ERROR: ' + (err?.message || err);
+  });
+
+  ros.on('close', () => {
+    connDot.className = '';
+    connStatus.textContent = 'DISCONNECTED';
+    teardownTopics();
+  });
+}
+
+function setupTopics() {
+  // Publish: /gimbal/cmd
+  cmdTopic = new ROSLIB.Topic({
+    ros, name: CMD_TOPIC,
+    messageType: 'geometry_msgs/Vector3',
+  });
+
+  // Publish: /gimbal/tracking_enabled
+  trackingTopic = new ROSLIB.Topic({
+    ros, name: TRACKING_TOPIC,
+    messageType: 'std_msgs/Bool',
+  });
+
+  // Subscribe: /gimbal/state
+  stateSub = new ROSLIB.Topic({
+    ros, name: STATE_TOPIC,
+    messageType: 'geometry_msgs/Vector3',
+    throttle_rate: 100,
+  });
+  stateSub.subscribe((msg) => {
+    currentPan  = msg.x ?? 0;
+    currentTilt = msg.y ?? 0;
+    updateAngleDisplay();
+    drawPad();
+  });
+
+  // Subscribe: camera
+  subscribeCamera(selectedCam);
+}
+
+function teardownTopics() {
+  if (stateSub)  { stateSub.unsubscribe();  stateSub  = null; }
+  if (camSub)    { camSub.unsubscribe();    camSub    = null; }
+  cmdTopic = trackingTopic = null;
+}
+
+function subscribeCamera(camId) {
+  if (camSub) { camSub.unsubscribe(); camSub = null; }
+  const topic = CAM_TOPICS[camId] ?? CAM_TOPICS.front;
+  if (!ros) return;
+  camSub = new ROSLIB.Topic({
+    ros, name: topic,
+    messageType: 'sensor_msgs/CompressedImage',
+    throttle_rate: 50,
+  });
+  camSub.subscribe((msg) => {
+    const fmt = msg.format?.includes('png') ? 'png' : 'jpeg';
+    camImg.src = `data:image/${fmt};base64,${msg.data}`;
+    camImg.classList.add('visible');
+    noSignal.style.display = 'none';
+    fpsBadge.classList.add('visible');
+    angleOvl.classList.add('visible');
+
+    // FPS
+    fpsCount++;
+    const now = Date.now();
+    if (now - fpsTime >= 1000) {
+      fps = Math.round(fpsCount * 1000 / (now - fpsTime));
+      fpsCount = 0; fpsTime = now;
+      fpsBadge.textContent = fps + ' fps';
+    }
+  });
+}
+
+// ── Publish helpers ───────────────────────────────────────────────────────────
+
+function publishCmd(pan, tilt) {
+  if (!cmdTopic) return;
+  cmdTopic.publish(new ROSLIB.Message({ x: pan, y: tilt, z: 0.0 }));
+}
+
+function publishTracking(enabled) {
+  if (!trackingTopic) return;
+  trackingTopic.publish(new ROSLIB.Message({ data: enabled }));
+}
+
+// ── Gimbal pad drawing ────────────────────────────────────────────────────────
+
+function drawPad() {
+  const canvas = padCanvas;
+  const ctx = canvas.getContext('2d');
+  const W = canvas.width;
+  const H = canvas.height;
+
+  ctx.clearRect(0, 0, W, H);
+
+  // Background
+  ctx.fillStyle = '#0a0a1a';
+  ctx.fillRect(0, 0, W, H);
+
+  // Grid lines
+  ctx.strokeStyle = '#1e3a5f';
+  ctx.lineWidth = 1;
+  // Vertical lines
+  for (let i = 1; i < 4; i++) {
+    ctx.beginPath();
+    ctx.moveTo(W * i / 4, 0); ctx.lineTo(W * i / 4, H);
+    ctx.stroke();
+  }
+  // Horizontal lines
+  for (let i = 1; i < 4; i++) {
+    ctx.beginPath();
+    ctx.moveTo(0, H * i / 4); ctx.lineTo(W, H * i / 4);
+    ctx.stroke();
+  }
+
+  // Center cross
+  ctx.strokeStyle = '#374151';
+  ctx.lineWidth = 1;
+  ctx.beginPath();
+  ctx.moveTo(W / 2, 0); ctx.lineTo(W / 2, H);
+  ctx.moveTo(0, H / 2); ctx.lineTo(W, H / 2);
+  ctx.stroke();
+
+  // Helpers to map degrees ↔ canvas coords
+  const panToX  = (p) => ((p - PAN_MIN)  / (PAN_MAX  - PAN_MIN))  * W;
+  const tiltToY = (t) => (1 - (t - TILT_MIN) / (TILT_MAX - TILT_MIN)) * H;
+
+  // Target position (what we're commanding)
+  const tx = panToX(targetPan);
+  const ty = tiltToY(targetTilt);
+
+  // Line from center to target
+  ctx.strokeStyle = 'rgba(6,182,212,0.3)';
+  ctx.lineWidth = 1;
+  ctx.beginPath();
+  ctx.moveTo(W / 2, H / 2);
+  ctx.lineTo(tx, ty);
+  ctx.stroke();
+
+  // Target ring
+  ctx.strokeStyle = '#0891b2';
+  ctx.lineWidth = 1.5;
+  ctx.beginPath();
+  ctx.arc(tx, ty, 10, 0, Math.PI * 2);
+  ctx.stroke();
+
+  // Current position (feedback)
+  const cx_ = panToX(currentPan);
+  const cy_ = tiltToY(currentTilt);
+  ctx.fillStyle = '#22d3ee';
+  ctx.beginPath();
+  ctx.arc(cx_, cy_, 6, 0, Math.PI * 2);
+  ctx.fill();
+
+  // Labels: axis limits
+  ctx.fillStyle = '#374151';
+  ctx.font = '9px Courier New';
+  ctx.fillText(`${PAN_MIN}°`, 2, H / 2 - 3);
+  ctx.fillText(`${PAN_MAX}°`, W - 24, H / 2 - 3);
+  ctx.fillText(`${TILT_MAX}°`, W / 2 + 3, 11);
+  ctx.fillText(`${TILT_MIN}°`, W / 2 + 3, H - 3);
+
+  // Current angle text
+  ctx.fillStyle = '#0891b2';
+  ctx.font = 'bold 9px Courier New';
+  ctx.fillText(`PAN ${targetPan.toFixed(0)}°  TILT ${targetTilt.toFixed(0)}°`, 4, H - 3);
+}
+
+// ── Pad interaction ───────────────────────────────────────────────────────────
+
+function padCoordsToAngles(clientX, clientY) {
+  const rect = padCanvas.getBoundingClientRect();
+  const rx = (clientX - rect.left) / rect.width;
+  const ry = (clientY - rect.top)  / rect.height;
+  const pan  = PAN_MIN  + rx * (PAN_MAX  - PAN_MIN);
+  const tilt = TILT_MAX - ry * (TILT_MAX - TILT_MIN);
+  return {
+    pan:  Math.max(PAN_MIN,  Math.min(PAN_MAX,  pan)),
+    tilt: Math.max(TILT_MIN, Math.min(TILT_MAX, tilt)),
+  };
+}
+
+function padPointerDown(e) {
+  padDragging = true;
+  padCanvas.setPointerCapture(e.pointerId);
+  const { pan, tilt } = padCoordsToAngles(e.clientX, e.clientY);
+  setTarget(pan, tilt);
+}
+
+function padPointerMove(e) {
+  if (!padDragging) return;
+  const { pan, tilt } = padCoordsToAngles(e.clientX, e.clientY);
+  setTarget(pan, tilt);
+}
+
+function padPointerUp() {
+  padDragging = false;
+}
+
+padCanvas.addEventListener('pointerdown',   padPointerDown);
+padCanvas.addEventListener('pointermove',   padPointerMove);
+padCanvas.addEventListener('pointerup',     padPointerUp);
+padCanvas.addEventListener('pointercancel', padPointerUp);
+
+// Touch passthrough
+padCanvas.addEventListener('touchstart',  (e) => e.preventDefault(), { passive: false });
+padCanvas.addEventListener('touchmove',   (e) => e.preventDefault(), { passive: false });
+
+// ── Target update ─────────────────────────────────────────────────────────────
+
+function setTarget(pan, tilt) {
+  targetPan  = Math.round(pan  * 10) / 10;
+  targetTilt = Math.round(tilt * 10) / 10;
+  updateAngleDisplay();
+  drawPad();
+  publishCmd(targetPan, targetTilt);
+}
+
+function updateAngleDisplay() {
+  panVal.textContent  = currentPan.toFixed(1);
+  tiltVal.textContent = currentTilt.toFixed(1);
+  angleOvl.textContent = `PAN ${currentPan.toFixed(1)}°  TILT ${currentTilt.toFixed(1)}°`;
+}
+
+// ── Presets ───────────────────────────────────────────────────────────────────
+
+function applyPreset(pan, tilt) {
+  setTarget(pan, tilt);
+}
+
+function goHome() {
+  setTarget(0, 0);
+}
+
+// ── Tracking toggle ───────────────────────────────────────────────────────────
+
+document.getElementById('tracking-toggle').addEventListener('change', (e) => {
+  trackingOn = e.target.checked;
+  publishTracking(trackingOn);
+});
+
+// ── Camera selector ───────────────────────────────────────────────────────────
+
+document.querySelectorAll('.cam-btn').forEach((btn) => {
+  btn.addEventListener('click', () => {
+    selectedCam = btn.dataset.cam;
+    document.querySelectorAll('.cam-btn').forEach((b) => b.classList.remove('active'));
+    btn.classList.add('active');
+    subscribeCamera(selectedCam);
+    // Reset fps display
+    camImg.classList.remove('visible');
+    noSignal.style.display = '';
+    fpsBadge.classList.remove('visible');
+    angleOvl.classList.remove('visible');
+    fpsCount = 0; fpsTime = Date.now();
+  });
+});
+
+// ── Connect button ────────────────────────────────────────────────────────────
+
+document.getElementById('btn-connect').addEventListener('click', connect);
+wsInput.addEventListener('keydown', (e) => { if (e.key === 'Enter') connect(); });
+
+// ── Preset buttons ────────────────────────────────────────────────────────────
+
+document.querySelectorAll('[data-preset]').forEach((btn) => {
+  const idx = parseInt(btn.dataset.preset, 10);
+  const p   = PRESETS[idx];
+  btn.addEventListener('click', () => applyPreset(p.pan, p.tilt));
+});
+
+document.getElementById('btn-home').addEventListener('click', goHome);
+
+// ── Init ──────────────────────────────────────────────────────────────────────
+
+// Size canvas to fill its container initially
+function resizePad() {
+  const w = padCanvas.offsetWidth || 200;
+  padCanvas.width  = w;
+  padCanvas.height = Math.round(w * 0.75);   // 4:3 aspect
+  drawPad();
+}
+
+window.addEventListener('resize', resizePad);
+resizePad();
+
+// Auto-connect on load using stored URL or default
+const storedUrl = localStorage.getItem('gimbal_ws_url');
+if (storedUrl) wsInput.value = storedUrl;
+wsInput.addEventListener('change', () => {
+  localStorage.setItem('gimbal_ws_url', wsInput.value);
+});
+
+// Initial draw
+drawPad();
+updateAngleDisplay();

ui/social-bot/src/App.jsx

@@ -94,12 +94,16 @@ import { ParameterServer } from './components/ParameterServer.jsx';
 // Teleop web interface (issue #534)
 import { TeleopWebUI } from './components/TeleopWebUI.jsx';
 
+// Gimbal control panel (issue #551)
+import { GimbalPanel } from './components/GimbalPanel.jsx';
+
 const TAB_GROUPS = [
   {
     label: 'TELEOP',
     color: 'text-orange-600',
     tabs: [
       { id: 'teleop-webui', label: 'Drive' },
+      { id: 'gimbal',       label: 'Gimbal' },
     ],
   },
   {
@@ -296,9 +300,10 @@ export default function App() {
       {/* ── Content ── */}
       <main className={`flex-1 ${
         activeTab === 'salty-face' ? '' :
-        ['eventlog', 'control', 'imu', 'teleop-webui'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'
+        ['eventlog', 'control', 'imu', 'teleop-webui', 'gimbal'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'
       } ${activeTab === 'salty-face' ? '' : 'p-4'}`}>
         {activeTab === 'teleop-webui' && <TeleopWebUI subscribe={subscribe} publish={publishFn} />}
+        {activeTab === 'gimbal'       && <GimbalPanel subscribe={subscribe} publish={publishFn} />}
 
         {activeTab === 'salty-face'   && <SaltyFace      subscribe={subscribe} />}
 

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

@@ -0,0 +1,382 @@
+/**
+ * GimbalPanel.jsx — Gimbal control panel with live camera preview (Issue #551).
+ *
+ * Features:
+ *   - Interactive 2-D pan/tilt joystick pad (click-drag + touch)
+ *   - Live camera stream via rosbridge (sensor_msgs/CompressedImage)
+ *   - Camera selector: front / rear / left / right
+ *   - Preset positions: CENTER / LEFT / RIGHT / UP / DOWN
+ *   - Home button (0°, 0°)
+ *   - Person-tracking toggle (publishes std_msgs/Bool)
+ *   - Current angle display from /gimbal/state feedback
+ *   - Mobile-responsive two-column → single-column layout
+ *
+ * ROS topics:
+ *   /gimbal/cmd              geometry_msgs/Vector3   publish  (x=pan°, y=tilt°)
+ *   /gimbal/state            geometry_msgs/Vector3   subscribe
+ *   /gimbal/tracking_enabled std_msgs/Bool           publish
+ *   /camera/<name>/image_raw/compressed  sensor_msgs/CompressedImage  subscribe
+ */
+
+import { useEffect, useRef, useState, useCallback } from 'react';
+
+// ── Constants ─────────────────────────────────────────────────────────────────
+
+const PAN_MIN  = -180;
+const PAN_MAX  =  180;
+const TILT_MIN =  -45;
+const TILT_MAX =   90;
+
+const CAMERAS = [
+  { id: 'front', label: 'Front', topic: '/camera/front/image_raw/compressed' },
+  { id: 'rear',  label: 'Rear',  topic: '/camera/rear/image_raw/compressed'  },
+  { id: 'left',  label: 'Left',  topic: '/camera/left/image_raw/compressed'  },
+  { id: 'right', label: 'Right', topic: '/camera/right/image_raw/compressed' },
+];
+
+const PRESETS = [
+  { label: 'CENTER', pan:   0, tilt:  0 },
+  { label: 'LEFT',   pan: -45, tilt:  0 },
+  { label: 'RIGHT',  pan:  45, tilt:  0 },
+  { label: 'UP',     pan:   0, tilt: 45 },
+  { label: 'DOWN',   pan:   0, tilt:-30 },
+];
+
+// ── Pan/Tilt pad ──────────────────────────────────────────────────────────────
+
+function GimbalPad({ targetPan, targetTilt, currentPan, currentTilt, onMove }) {
+  const canvasRef = useRef(null);
+  const dragging  = useRef(false);
+
+  const draw = useCallback(() => {
+    const canvas = canvasRef.current;
+    if (!canvas) return;
+    const ctx = canvas.getContext('2d');
+    const W = canvas.width;
+    const H = canvas.height;
+
+    ctx.clearRect(0, 0, W, H);
+    ctx.fillStyle = '#0a0a1a';
+    ctx.fillRect(0, 0, W, H);
+
+    // Grid
+    ctx.strokeStyle = '#1e3a5f';
+    ctx.lineWidth = 1;
+    for (let i = 1; i < 4; i++) {
+      ctx.beginPath(); ctx.moveTo(W * i / 4, 0); ctx.lineTo(W * i / 4, H); ctx.stroke();
+      ctx.beginPath(); ctx.moveTo(0, H * i / 4); ctx.lineTo(W, H * i / 4); ctx.stroke();
+    }
+
+    // Center cross
+    ctx.strokeStyle = '#374151';
+    ctx.beginPath();
+    ctx.moveTo(W / 2, 0);    ctx.lineTo(W / 2, H);
+    ctx.moveTo(0, H / 2);    ctx.lineTo(W, H / 2);
+    ctx.stroke();
+
+    const panToX  = (p) => ((p - PAN_MIN)  / (PAN_MAX  - PAN_MIN))  * W;
+    const tiltToY = (t) => (1 - (t - TILT_MIN) / (TILT_MAX - TILT_MIN)) * H;
+
+    // Targeting line
+    const tx = panToX(targetPan);
+    const ty = tiltToY(targetTilt);
+    ctx.strokeStyle = 'rgba(6,182,212,0.25)';
+    ctx.lineWidth = 1;
+    ctx.beginPath(); ctx.moveTo(W / 2, H / 2); ctx.lineTo(tx, ty); ctx.stroke();
+
+    // Target ring
+    ctx.strokeStyle = '#0891b2';
+    ctx.lineWidth = 2;
+    ctx.beginPath(); ctx.arc(tx, ty, 10, 0, Math.PI * 2); ctx.stroke();
+    // Cross-hair inside ring
+    ctx.strokeStyle = '#0e7490';
+    ctx.lineWidth = 1;
+    ctx.beginPath();
+    ctx.moveTo(tx - 6, ty); ctx.lineTo(tx + 6, ty);
+    ctx.moveTo(tx, ty - 6); ctx.lineTo(tx, ty + 6);
+    ctx.stroke();
+
+    // Current position dot (feedback)
+    const cx_ = panToX(currentPan);
+    const cy_ = tiltToY(currentTilt);
+    ctx.fillStyle = '#22d3ee';
+    ctx.beginPath(); ctx.arc(cx_, cy_, 5, 0, Math.PI * 2); ctx.fill();
+
+    // Axis labels
+    ctx.fillStyle = '#374151';
+    ctx.font = '9px Courier New';
+    ctx.fillText(`${PAN_MIN}°`, 3, H / 2 - 3);
+    ctx.fillText(`${PAN_MAX}°`, W - 28, H / 2 - 3);
+    ctx.fillText(`+${TILT_MAX}°`, W / 2 + 3, 11);
+    ctx.fillText(`${TILT_MIN}°`, W / 2 + 3, H - 3);
+
+    // Live readout
+    ctx.fillStyle = '#0891b2';
+    ctx.font = 'bold 9px Courier New';
+    ctx.fillText(`▶ ${targetPan.toFixed(0)}° / ${targetTilt.toFixed(0)}°`, 4, H - 3);
+  }, [targetPan, targetTilt, currentPan, currentTilt]);
+
+  useEffect(() => { draw(); }, [draw]);
+
+  const toAngles = (clientX, clientY) => {
+    const rect = canvasRef.current.getBoundingClientRect();
+    const rx = (clientX - rect.left) / rect.width;
+    const ry = (clientY - rect.top)  / rect.height;
+    const pan  = PAN_MIN  + rx * (PAN_MAX  - PAN_MIN);
+    const tilt = TILT_MAX - ry * (TILT_MAX - TILT_MIN);
+    return {
+      pan:  Math.max(PAN_MIN,  Math.min(PAN_MAX,  pan)),
+      tilt: Math.max(TILT_MIN, Math.min(TILT_MAX, tilt)),
+    };
+  };
+
+  const onPD = useCallback((e) => {
+    dragging.current = true;
+    canvasRef.current.setPointerCapture(e.pointerId);
+    const { pan, tilt } = toAngles(e.clientX, e.clientY);
+    onMove(pan, tilt);
+  }, [onMove]);
+
+  const onPM = useCallback((e) => {
+    if (!dragging.current) return;
+    const { pan, tilt } = toAngles(e.clientX, e.clientY);
+    onMove(pan, tilt);
+  }, [onMove]);
+
+  const onPU = useCallback(() => { dragging.current = false; }, []);
+
+  return (
+    <canvas
+      ref={canvasRef}
+      width={240}
+      height={160}
+      className="rounded border border-cyan-950 cursor-crosshair w-full"
+      style={{ touchAction: 'none' }}
+      onPointerDown={onPD}
+      onPointerMove={onPM}
+      onPointerUp={onPU}
+      onPointerCancel={onPU}
+    />
+  );
+}
+
+// ── Camera feed ───────────────────────────────────────────────────────────────
+
+function CameraFeed({ subscribe, topic }) {
+  const [src, setSrc]  = useState(null);
+  const [fps, setFps]  = useState(0);
+  const cntRef         = useRef(0);
+  const timeRef        = useRef(Date.now());
+
+  useEffect(() => {
+    setSrc(null);
+    cntRef.current  = 0;
+    timeRef.current = Date.now();
+
+    const unsub = subscribe(topic, 'sensor_msgs/CompressedImage', (msg) => {
+      const fmt = msg.format?.includes('png') ? 'png' : 'jpeg';
+      setSrc(`data:image/${fmt};base64,${msg.data}`);
+      cntRef.current++;
+      const now = Date.now();
+      const dt  = now - timeRef.current;
+      if (dt >= 1000) {
+        setFps(Math.round(cntRef.current * 1000 / dt));
+        cntRef.current  = 0;
+        timeRef.current = now;
+      }
+    });
+    return unsub;
+  }, [subscribe, topic]);
+
+  return (
+    <div className="relative flex-1 min-h-0 bg-black rounded-lg border border-cyan-950 flex items-center justify-center overflow-hidden">
+      {src ? (
+        <img src={src} alt="gimbal camera" className="max-w-full max-h-full object-contain" draggable={false} />
+      ) : (
+        <div className="flex flex-col items-center gap-2 text-gray-700">
+          <div className="text-4xl">📷</div>
+          <div className="text-xs tracking-widest">NO SIGNAL</div>
+        </div>
+      )}
+      {src && (
+        <div className="absolute top-2 right-2 bg-black bg-opacity-60 text-green-400 text-xs font-mono px-1.5 py-0.5 rounded">
+          {fps} fps
+        </div>
+      )}
+    </div>
+  );
+}
+
+// ── Main component ────────────────────────────────────────────────────────────
+
+export function GimbalPanel({ subscribe, publish }) {
+  const [selectedCam,  setSelectedCam]  = useState('front');
+  const [targetPan,    setTargetPan]    = useState(0);
+  const [targetTilt,   setTargetTilt]   = useState(0);
+  const [currentPan,   setCurrentPan]   = useState(0);
+  const [currentTilt,  setCurrentTilt]  = useState(0);
+  const [tracking,     setTracking]     = useState(false);
+
+  const currentCam = CAMERAS.find(c => c.id === selectedCam);
+
+  // Subscribe to gimbal state feedback
+  useEffect(() => {
+    const unsub = subscribe('/gimbal/state', 'geometry_msgs/Vector3', (msg) => {
+      setCurrentPan(msg.x  ?? 0);
+      setCurrentTilt(msg.y ?? 0);
+    });
+    return unsub;
+  }, [subscribe]);
+
+  // Publish gimbal command
+  const sendCmd = useCallback((pan, tilt) => {
+    const p = Math.round(pan  * 10) / 10;
+    const t = Math.round(tilt * 10) / 10;
+    setTargetPan(p);
+    setTargetTilt(t);
+    if (publish) {
+      publish('/gimbal/cmd', 'geometry_msgs/Vector3', { x: p, y: t, z: 0.0 });
+    }
+  }, [publish]);
+
+  // Preset handler
+  const applyPreset = useCallback((pan, tilt) => {
+    sendCmd(pan, tilt);
+  }, [sendCmd]);
+
+  // Tracking toggle
+  const toggleTracking = () => {
+    const next = !tracking;
+    setTracking(next);
+    if (publish) {
+      publish('/gimbal/tracking_enabled', 'std_msgs/Bool', { data: next });
+    }
+  };
+
+  return (
+    <div className="flex flex-col lg:flex-row h-full gap-3">
+
+      {/* ── Left: Camera + pad ── */}
+      <div className="flex flex-col gap-2 flex-1 min-h-0">
+
+        {/* Camera selector */}
+        <div className="flex gap-1 shrink-0 flex-wrap">
+          {CAMERAS.map((cam) => (
+            <button
+              key={cam.id}
+              onClick={() => setSelectedCam(cam.id)}
+              className={`px-3 py-1 text-xs font-bold rounded border tracking-widest transition-colors ${
+                selectedCam === cam.id
+                  ? 'bg-cyan-950 border-cyan-700 text-cyan-300'
+                  : 'bg-gray-950 border-gray-800 text-gray-600 hover:text-gray-300'
+              }`}
+            >
+              {cam.label.toUpperCase()}
+            </button>
+          ))}
+          <div className="flex-1" />
+          <div className="text-xs text-gray-700 self-center font-mono truncate max-w-xs">
+            {currentCam?.topic}
+          </div>
+        </div>
+
+        {/* Camera feed */}
+        <CameraFeed subscribe={subscribe} topic={currentCam?.topic} />
+      </div>
+
+      {/* ── Right: Controls ── */}
+      <div className="flex flex-col gap-3 w-full lg:w-72 shrink-0">
+
+        {/* Pan/Tilt pad */}
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
+          <div className="text-xs font-bold tracking-widest text-cyan-700">PAN / TILT</div>
+          <GimbalPad
+            targetPan={targetPan}
+            targetTilt={targetTilt}
+            currentPan={currentPan}
+            currentTilt={currentTilt}
+            onMove={sendCmd}
+          />
+          <div className="text-xs text-gray-700 text-center">click or drag to aim</div>
+        </div>
+
+        {/* Angle readout */}
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 grid grid-cols-2 gap-2">
+          <div>
+            <div className="text-gray-600 text-xs mb-1">TARGET PAN</div>
+            <div className="text-xl font-mono text-cyan-300">{targetPan.toFixed(1)}°</div>
+          </div>
+          <div>
+            <div className="text-gray-600 text-xs mb-1">TARGET TILT</div>
+            <div className="text-xl font-mono text-amber-300">{targetTilt.toFixed(1)}°</div>
+          </div>
+          <div>
+            <div className="text-gray-600 text-xs mb-1">ACTUAL PAN</div>
+            <div className="text-sm font-mono text-cyan-600">{currentPan.toFixed(1)}°</div>
+          </div>
+          <div>
+            <div className="text-gray-600 text-xs mb-1">ACTUAL TILT</div>
+            <div className="text-sm font-mono text-amber-600">{currentTilt.toFixed(1)}°</div>
+          </div>
+        </div>
+
+        {/* Presets */}
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
+          <div className="text-xs font-bold tracking-widest text-cyan-700">PRESETS</div>
+          <div className="grid grid-cols-3 gap-1.5">
+            {PRESETS.map((p) => (
+              <button
+                key={p.label}
+                onClick={() => applyPreset(p.pan, p.tilt)}
+                className="py-1.5 text-xs font-bold rounded border border-cyan-950 bg-gray-900 text-gray-500 hover:bg-cyan-950 hover:text-cyan-300 hover:border-cyan-800 transition-colors"
+              >
+                <div>{p.label}</div>
+                <div className="text-gray-700 font-normal text-xs">{p.pan}°/{p.tilt}°</div>
+              </button>
+            ))}
+            <button
+              onClick={() => applyPreset(0, 0)}
+              className="py-1.5 col-span-3 text-xs font-bold rounded border border-gray-800 bg-gray-900 text-gray-600 hover:bg-gray-800 hover:text-gray-300 transition-colors"
+            >
+              ⌂ HOME (0° / 0°)
+            </button>
+          </div>
+        </div>
+
+        {/* Person tracking */}
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
+          <div className="text-xs font-bold tracking-widest text-cyan-700">PERSON TRACKING</div>
+          <div className="flex items-center justify-between">
+            <span className={`text-xs ${tracking ? 'text-cyan-400' : 'text-gray-600'}`}>
+              {tracking ? 'ON — following person' : 'OFF — manual control'}
+            </span>
+            <button
+              onClick={toggleTracking}
+              className={`relative w-10 h-5 rounded-full border transition-colors ${
+                tracking
+                  ? 'bg-cyan-950 border-cyan-700'
+                  : 'bg-gray-900 border-gray-700'
+              }`}
+            >
+              <span className={`absolute top-0.5 w-4 h-4 rounded-full transition-transform ${
+                tracking
+                  ? 'translate-x-5 bg-cyan-400'
+                  : 'translate-x-0.5 bg-gray-600'
+              }`} />
+            </button>
+          </div>
+          <div className="text-xs text-gray-700 font-mono">/gimbal/tracking_enabled</div>
+        </div>
+
+        {/* Topic reference */}
+        <div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-700 space-y-1">
+          <div className="font-bold text-gray-600 mb-1">ROS TOPICS</div>
+          <div><span className="text-gray-600">PUB</span> /gimbal/cmd (Vector3)</div>
+          <div><span className="text-gray-600">SUB</span> /gimbal/state (Vector3)</div>
+          <div><span className="text-gray-600">PUB</span> /gimbal/tracking_enabled (Bool)</div>
+        </div>
+
+      </div>
+    </div>
+  );
+}