Merge remote-tracking branch 'origin/sl-firmware/issue-531-pid-autotune'

Implement Ziegler-Nichols relay feedback auto-tuning with flash persistence:

Firmware (STM32F722):
- pid_flash.c/h: erase+write Kp/Ki/Kd to flash sector 7 (0x0807FFC0),
  magic-validated; load on boot to restore saved tune
- jlink.h: add JLINK_CMD_PID_SAVE (0x0A) and JLINK_TLM_PID_RESULT (0x83)
  with jlink_tlm_pid_result_t struct and pid_save_req flag in JLinkState
- jlink.c: dispatch JLINK_CMD_PID_SAVE -> pid_save_req; add
  jlink_send_pid_result() to confirm flash write outcome over USART1
- main.c: load saved PID from flash after balance_init(); handle
  pid_save_req in main loop (disarmed-only, erase stalls CPU ~1s)

Jetson ROS2 (saltybot_pid_autotune):
- pid_autotune_node.py: add Ki to Ziegler-Nichols formula (ZN PID:
  Kp=0.6Ku, Ki=1.2Ku/Tu, Kd=0.075KuTu); add JLink serial client that
  sends JLINK_CMD_PID_SET + JLINK_CMD_PID_SAVE after tuning completes
- autotune_config.yaml: add jlink_serial_port and jlink_baud_rate params

Trigger: ros2 service call /saltybot/autotune_pid std_srvs/srv/Trigger

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

chassis/phone_mount_bracket.scad

@@ -0,0 +1,504 @@
+// ============================================================
+// phone_mount_bracket.scad — Spring-Loaded Phone Mount for T-Slot Rail
+// Issue: #535  Agent: sl-mechanical  Date: 2026-03-07
+// ============================================================
+//
+// Parametric spring-loaded phone mount that clamps to the 2020 aluminium
+// T-slot sensor rail.  Adjustable phone width 60–85 mm.  Quick-release
+// cam lever for tool-free phone swap.  Vibration-damping flexure ribs
+// on grip pads absorb motor/terrain vibration (PETG compliance).
+//
+// Design overview:
+//   - Fixed jaw + sliding jaw on a 40 mm guide rail (M4 rod)
+//   - Coil spring (Ø8 × 30 mm) compressed between jaw and end-stop —
+//     spring pre-load keeps phone clamped at any width in range
+//   - Cam lever (printed PETG) rotates 90° to release / lock spring
+//   - Anti-vibration flexure ribs on both grip pad faces
+//   - Landscape or portrait orientation: bracket rotates on T-nut base
+//
+// Parts (STL exports):
+//   Part 1 — tnut_base()          Rail attachment base (universal)
+//   Part 2 — fixed_jaw()          Fixed bottom jaw + guide rail bosses
+//   Part 3 — sliding_jaw()        Spring-loaded sliding jaw
+//   Part 4 — cam_lever()          Quick-release cam lever
+//   Part 5 — grip_pad()           Flexure grip pad (print ×2, TPU optional)
+//   Part 6 — assembly_preview()   Full assembly
+//
+// Hardware BOM (per mount):
+//   1× M4 × 60 mm SHCS          guide rod + spring bolt
+//   1× M4 hex nut                end-stop on sliding jaw
+//   1× Ø8 × 30 mm coil spring   ~0.5 N/mm rate (spring clamping)
+//   2× M3 × 16 mm SHCS          T-nut base thumbscrew + arm bolts
+//   1× M3 hex nut                thumbscrew nut in T-nut
+//   4× M2 × 8 mm SHCS           grip pad retention bolts (optional)
+//
+// Dimensions:
+//   Phone width range : PHONE_W_MIN–PHONE_W_MAX (60–85 mm) parametric
+//   Phone thickness   : up to PHONE_THICK_MAX (12 mm) — open-front jaw
+//   Phone height held : GRIP_SPAN (22 mm each jaw) — portrait/landscape
+//   Overall bracket H : ~110 mm  W: ~90 mm  D: ~55 mm
+//
+// Print settings:
+//   Material  : PETG (tnut_base, fixed_jaw, sliding_jaw, cam_lever)
+//               TPU 95A optional for grip_pad (or PETG for rigidity)
+//   Perimeters: 5 (structural parts), 3 (grip_pad)
+//   Infill    : 40 % gyroid (jaws), 20 % (grip_pad)
+//   Supports  : none needed (designed for FDM orientation)
+//   Layer ht  : 0.2 mm
+//
+// Export commands:
+//   openscad phone_mount_bracket.scad -D 'RENDER="tnut_base_stl"'    -o pm_tnut_base.stl
+//   openscad phone_mount_bracket.scad -D 'RENDER="fixed_jaw_stl"'    -o pm_fixed_jaw.stl
+//   openscad phone_mount_bracket.scad -D 'RENDER="sliding_jaw_stl"'  -o pm_sliding_jaw.stl
+//   openscad phone_mount_bracket.scad -D 'RENDER="cam_lever_stl"'    -o pm_cam_lever.stl
+//   openscad phone_mount_bracket.scad -D 'RENDER="grip_pad_stl"'     -o pm_grip_pad.stl
+// ============================================================
+
+$fn = 64;
+e   = 0.01;   // epsilon for boolean clearance
+
+// ── Phone parameters (adjust to target device) ───────────────────────────────
+PHONE_W_MIN    = 60.0;   // narrowest phone width supported (mm)
+PHONE_W_MAX    = 85.0;   // widest phone width supported (mm)
+PHONE_THICK_MAX = 12.0;  // max phone body thickness incl. case (mm)
+
+// ── Rail geometry (must match 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 constants ───────────────────────────────────────────────────────────
+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
+
+// ── Base plate geometry ───────────────────────────────────────────────────────
+BASE_FACE_W  = 30.0;
+BASE_FACE_H  = 25.0;
+BASE_FACE_T  = SLOT_NECK_H + 1.5;
+
+// ── Jaw geometry ─────────────────────────────────────────────────────────────
+JAW_BODY_W   = 88.0;    // jaw outer width (> PHONE_W_MAX for rim)
+JAW_BODY_H   = 28.0;    // jaw height (Z) — phone grip span
+JAW_BODY_T   = 14.0;    // jaw depth (Y) — phone cradled this deep
+JAW_WALL_T   =  4.0;    // jaw side wall thickness
+JAW_LIP_T    =  3.0;    // front retaining lip thickness
+JAW_LIP_H    =  5.0;    // front lip height (retains phone)
+PHONE_POCKET_D = PHONE_THICK_MAX + 0.5;  // pocket depth for phone
+
+// ── Guide rod / spring system ─────────────────────────────────────────────────
+GUIDE_ROD_D  =  4.3;    // M4 clearance bore in sliding jaw
+GUIDE_BOSS_D = 10.0;    // boss OD around guide bore
+GUIDE_BOSS_T =  6.0;    // boss length
+SPRING_OD    =  8.5;    // coil spring OD pocket (spring is Ø8)
+SPRING_L     = 32.0;    // spring pocket length (spring compressed ~22 mm)
+SPRING_SEAT_T =  3.0;   // spring seat wall at end-stop boss
+JAW_TRAVEL   = PHONE_W_MAX - PHONE_W_MIN + 4.0;   // max jaw travel (mm)
+ARM_SPAN     = PHONE_W_MAX + 2 * JAW_WALL_T + 8;  // fixed jaw total width
+
+// ── Cam lever geometry ────────────────────────────────────────────────────────
+CAM_R_MIN    =  5.0;    // cam small radius (engaged / clamped)
+CAM_R_MAX    =  9.0;    // cam large radius (released, spring compressed)
+CAM_THICK    =  8.0;    // cam disc thickness
+CAM_HANDLE_L = 45.0;    // lever arm length
+CAM_HANDLE_W =  8.0;    // lever handle width
+CAM_HANDLE_T =  5.0;    // lever handle thickness
+CAM_BORE_D   =  4.3;    // M4 pivot bore
+CAM_DETENT_D =  3.0;    // detent ball pocket (3 mm bearing)
+
+// ── Grip pad geometry (vibration dampening flexure ribs) ─────────────────────
+PAD_W        = JAW_BODY_W - 2*JAW_WALL_T - 2;  // pad width
+PAD_H        = JAW_BODY_H - 2;                  // pad height
+PAD_T        =  2.5;    // pad body thickness
+RIB_H        =  1.5;    // flexure rib height above pad face
+RIB_W        =  1.2;    // rib width
+RIB_PITCH    =  5.0;    // rib pitch (centre-to-centre)
+RIB_COUNT    = floor(PAD_W / RIB_PITCH) - 1;
+
+// ── Arm geometry (base to jaw body) ──────────────────────────────────────────
+ARM_REACH    = 38.0;    // distance from rail face to jaw centreline (+Y)
+ARM_T        =  4.0;    // arm thickness
+ARM_H        = BASE_FACE_H;
+
+// ── Fasteners ─────────────────────────────────────────────────────────────────
+M2_D = 2.4;
+M3_D = 3.3;
+M4_D = 4.3;
+M4_NUT_AF = 7.0;   // M4 hex nut across-flats
+M4_NUT_H  = 3.2;   // M4 hex nut height
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
+RENDER = "assembly";
+
+if      (RENDER == "assembly")        assembly_preview();
+else if (RENDER == "tnut_base_stl")   tnut_base();
+else if (RENDER == "fixed_jaw_stl")   fixed_jaw();
+else if (RENDER == "sliding_jaw_stl") sliding_jaw();
+else if (RENDER == "cam_lever_stl")   cam_lever();
+else if (RENDER == "grip_pad_stl")    grip_pad();
+
+// ============================================================
+// ASSEMBLY PREVIEW
+// ============================================================
+module assembly_preview() {
+    // Ghost rail section (20 × 20 × 200)
+    %color("Silver", 0.30)
+        linear_extrude(200)
+            square([RAIL_W, RAIL_W], center = true);
+
+    // T-nut base at Z=80 on rail
+    color("OliveDrab", 0.85)
+        translate([0, 0, 80])
+            tnut_base();
+
+    // Fixed jaw assembly (centred, extending +Y from base)
+    color("DarkSlateGray", 0.85)
+        translate([0, SLOT_NECK_H + BASE_FACE_T + ARM_REACH, 80])
+            fixed_jaw();
+
+    // Sliding jaw — shown at mid-travel (phone ~72 mm wide)
+    color("SteelBlue", 0.85)
+        translate([PHONE_W_MIN + (PHONE_W_MAX - PHONE_W_MIN)/2,
+                   SLOT_NECK_H + BASE_FACE_T + ARM_REACH, 80])
+            sliding_jaw();
+
+    // Grip pads on both jaws
+    color("DimGray", 0.85) {
+        translate([0, SLOT_NECK_H + BASE_FACE_T + ARM_REACH, 80])
+            translate([JAW_WALL_T, JAW_BODY_T, JAW_BODY_H/2])
+                rotate([90, 0, 0])
+                    grip_pad();
+        translate([PHONE_W_MIN + (PHONE_W_MAX - PHONE_W_MIN)/2,
+                   SLOT_NECK_H + BASE_FACE_T + ARM_REACH, 80])
+            translate([-JAW_WALL_T - PAD_T, JAW_BODY_T, JAW_BODY_H/2])
+                rotate([90, 0, 180])
+                    grip_pad();
+    }
+
+    // Cam lever — shown in locked (clamped) position
+    color("OrangeRed", 0.85)
+        translate([ARM_SPAN/2 + 6,
+                   SLOT_NECK_H + BASE_FACE_T + ARM_REACH + GUIDE_BOSS_D/2,
+                   80 + JAW_BODY_H/2])
+            rotate([0, 0, 0])
+                cam_lever();
+}
+
+// ============================================================
+// PART 1 — T-NUT BASE
+// ============================================================
+// Standard 2020 T-slot rail attachment base.
+// Identical interface to sensor_rail_brackets.scad universal_tnut_base().
+// Arm extends in +Y; rail clamp bolt in -Y face.
+//
+// Print flat (face plate down), PETG, 5 perims, 60 % infill.
+module tnut_base() {
+    difference() {
+        union() {
+            // Face plate (flush against rail outer face)
+            translate([-BASE_FACE_W/2, -BASE_FACE_T, 0])
+                cube([BASE_FACE_W, BASE_FACE_T, BASE_FACE_H]);
+
+            // 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]);
+
+            // T-nut body (wider, inside 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]);
+
+            // Arm stub (face plate → jaw)
+            translate([-BASE_FACE_W/2, -BASE_FACE_T, 0])
+                cube([BASE_FACE_W, BASE_FACE_T + ARM_REACH, ARM_T]);
+        }
+
+        // M3 rail clamp bolt bore (centre of T-nut, through 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);
+
+        // 2× M3 bolt holes for arm-to-jaw bolting
+        for (bx = [-10, 10])
+            translate([bx, ARM_REACH - BASE_FACE_T - e, ARM_T/2])
+                rotate([-90, 0, 0])
+                    cylinder(d = M3_D, h = 8 + 2*e);
+
+        // Lightening slot in arm
+        translate([0, -BASE_FACE_T/2 + ARM_REACH/2, ARM_T/2])
+            cube([BASE_FACE_W - 12, ARM_REACH - 16, ARM_T + 2*e],
+                 center = true);
+    }
+}
+
+// ============================================================
+// PART 2 — FIXED JAW
+// ============================================================
+// Fixed lower jaw of the clamping system.  Phone sits in the pocket
+// formed by the fixed jaw (bottom) and sliding jaw (top).
+// Two guide bosses on the right wall carry the M4 guide rod + spring.
+// The cam lever pivot boss is on the outer right face.
+//
+// Coordinate origin: centre-bottom of jaw body.
+// Phone entry face: +Y (open front), phone pocket opens toward +Y.
+// Fixed jaw left edge is at X = -JAW_BODY_W/2.
+//
+// Print jaw-pocket-face down, PETG, 5 perims, 40 % infill.
+module fixed_jaw() {
+    difference() {
+        union() {
+            // ── Main jaw body ────────────────────────────────────────────
+            translate([-JAW_BODY_W/2, -JAW_BODY_T/2, 0])
+                cube([JAW_BODY_W, JAW_BODY_T, JAW_BODY_H]);
+
+            // ── Front retaining lip (keeps phone from falling forward) ───
+            translate([-JAW_BODY_W/2, JAW_BODY_T/2 - JAW_LIP_T, 0])
+                cube([JAW_BODY_W, JAW_LIP_T, JAW_LIP_H]);
+
+            // ── Guide boss right (outer, carries spring + end-stop) ──────
+            translate([JAW_BODY_W/2, 0, JAW_BODY_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = GUIDE_BOSS_D, h = GUIDE_BOSS_T);
+
+            // ── Cam lever pivot boss (right face, above guide boss) ──────
+            translate([JAW_BODY_W/2, 0, JAW_BODY_H/2 + GUIDE_BOSS_D + 4])
+                rotate([0, 90, 0])
+                    cylinder(d = CAM_THICK + 4, h = 6);
+
+            // ── Arm attachment bosses (left side, connect to tnut_base) ──
+            for (bx = [-10, 10])
+                translate([bx, -JAW_BODY_T/2 - 8, ARM_T/2])
+                    cylinder(d = 8, h = 8);
+        }
+
+        // ── Phone pocket (open-top U channel centred in jaw) ────────────
+        // Pocket opens toward +Y (front), phone drops in from above.
+        translate([0, -JAW_BODY_T/2 - e,
+                   JAW_LIP_H])
+            cube([JAW_BODY_W - 2*JAW_WALL_T,
+                  PHONE_POCKET_D + JAW_WALL_T,
+                  JAW_BODY_H - JAW_LIP_H + e],
+                 center = [true, false, false]);
+
+        // ── Guide rod bore (M4 clearance, through both guide bosses) ────
+        translate([-JAW_BODY_W/2 - e, 0, JAW_BODY_H/2])
+            rotate([0, 90, 0])
+                cylinder(d = GUIDE_ROD_D,
+                         h = JAW_BODY_W + GUIDE_BOSS_T + 2*e);
+
+        // ── Spring pocket (coaxial with guide rod, in right boss) ────────
+        translate([JAW_BODY_W/2 + e, 0, JAW_BODY_H/2])
+            rotate([0, -90, 0])
+                cylinder(d = SPRING_OD, h = SPRING_L);
+
+        // ── M4 hex nut pocket in spring-seat wall (end-stop nut) ────────
+        translate([JAW_BODY_W/2 + GUIDE_BOSS_T + e, 0, JAW_BODY_H/2])
+            rotate([0, -90, 0])
+                cylinder(d = M4_NUT_AF / cos(30), h = M4_NUT_H + 0.5,
+                         $fn = 6);
+
+        // ── Cam pivot bore (M4 pivot, through pivot boss) ────────────────
+        translate([JAW_BODY_W/2 - e, 0, JAW_BODY_H/2 + GUIDE_BOSS_D + 4])
+            rotate([0, 90, 0])
+                cylinder(d = CAM_BORE_D, h = 6 + 2*e);
+
+        // ── Arm attachment bolt holes (M3, to tnut_base arm stubs) ──────
+        for (bx = [-10, 10])
+            translate([bx, -JAW_BODY_T/2 - 8 - e, ARM_T/2])
+                rotate([-90, 0, 0])
+                    cylinder(d = M3_D, h = 12 + 2*e);
+
+        // ── Grip pad seats (recessed Ø1.5 mm, 2 mm deep, optional) ──────
+        for (pz = [JAW_BODY_H * 0.3, JAW_BODY_H * 0.7])
+        for (px = [-PAD_W/4, PAD_W/4])
+            translate([px, -JAW_BODY_T/2 + PHONE_POCKET_D + 1, pz])
+                rotate([-90, 0, 0])
+                    cylinder(d = M2_D, h = 10);
+
+        // ── Lightening pockets (non-structural core removal) ─────────────
+        translate([0, 0, JAW_BODY_H/2])
+            cube([JAW_BODY_W - 2*JAW_WALL_T - 4,
+                  JAW_BODY_T - 2*JAW_WALL_T,
+                  JAW_BODY_H - JAW_LIP_H - 4],
+                 center = true);
+    }
+}
+
+// ============================================================
+// PART 3 — SLIDING JAW
+// ============================================================
+// Upper clamping jaw.  Slides along the M4 guide rod.
+// Spring pushes this jaw toward the phone (inward).
+// M4 hex nut on the guide rod limits maximum travel (full open).
+// Cam lever pushes on this jaw face to compress spring (release).
+//
+// Coordinate origin same convention as fixed_jaw() for assembly.
+// Jaw slides in +X direction (away from fixed jaw left wall).
+//
+// Print jaw-pocket-face down, PETG, 5 perims, 40 % infill.
+module sliding_jaw() {
+    difference() {
+        union() {
+            // ── Main jaw body ────────────────────────────────────────────
+            translate([-JAW_WALL_T, -JAW_BODY_T/2, 0])
+                cube([JAW_BODY_W/2 + JAW_WALL_T, JAW_BODY_T, JAW_BODY_H]);
+
+            // ── Front retaining lip ──────────────────────────────────────
+            translate([-JAW_WALL_T, JAW_BODY_T/2 - JAW_LIP_T, 0])
+                cube([JAW_BODY_W/2 + JAW_WALL_T, JAW_LIP_T, JAW_LIP_H]);
+
+            // ── Guide boss (carries guide rod, spring butts against face) ─
+            translate([-JAW_WALL_T - GUIDE_BOSS_T, 0, JAW_BODY_H/2])
+                rotate([0, 90, 0])
+                    cylinder(d = GUIDE_BOSS_D, h = GUIDE_BOSS_T);
+
+            // ── Cam follower ear (contacts cam lever) ────────────────────
+            translate([-JAW_WALL_T - 2, 0,
+                       JAW_BODY_H/2 + GUIDE_BOSS_D + 4])
+                cube([4, CAM_THICK + 2, CAM_THICK + 2], center = true);
+        }
+
+        // ── Phone pocket (inner face, contacts phone side) ───────────────
+        translate([-JAW_WALL_T - e, -JAW_BODY_T/2 - e, JAW_LIP_H])
+            cube([JAW_BODY_W/2 - JAW_WALL_T + e,
+                  PHONE_POCKET_D + JAW_WALL_T + 2*e,
+                  JAW_BODY_H - JAW_LIP_H + e]);
+
+        // ── Guide rod bore (M4 clearance through boss + jaw wall) ────────
+        translate([-JAW_WALL_T - GUIDE_BOSS_T - e, 0, JAW_BODY_H/2])
+            rotate([0, 90, 0])
+                cylinder(d = GUIDE_ROD_D,
+                         h = GUIDE_BOSS_T + JAW_WALL_T + 2*e);
+
+        // ── M4 nut pocket (end-stop nut, rear of guide boss) ────────────
+        translate([-JAW_WALL_T - GUIDE_BOSS_T - e, 0, JAW_BODY_H/2])
+            rotate([0, 90, 0])
+                cylinder(d = M4_NUT_AF / cos(30), h = M4_NUT_H + 1,
+                         $fn = 6);
+
+        // ── Cam follower bore (M4 pivot passes through ear) ─────────────
+        translate([-JAW_WALL_T - 2 - e, 0,
+                   JAW_BODY_H/2 + GUIDE_BOSS_D + 4])
+            rotate([0, 90, 0])
+                cylinder(d = CAM_BORE_D, h = 6 + 2*e);
+
+        // ── Grip pad seats ───────────────────────────────────────────────
+        for (pz = [JAW_BODY_H * 0.3, JAW_BODY_H * 0.7])
+        for (px = [JAW_BODY_W/8])
+            translate([px, -JAW_BODY_T/2 + PHONE_POCKET_D + 1, pz])
+                rotate([-90, 0, 0])
+                    cylinder(d = M2_D, h = 10);
+    }
+}
+
+// ============================================================
+// PART 4 — CAM LEVER (QUICK-RELEASE)
+// ============================================================
+// Eccentric cam disc + integral handle lever.
+// Rotates 90° on M4 pivot pin between CLAMPED and RELEASED states:
+//   CLAMPED  : cam small radius (CAM_R_MIN) toward jaw → spring pushes jaw
+//   RELEASED : cam large radius (CAM_R_MAX) toward jaw → compresses spring
+//               by (CAM_R_MAX - CAM_R_MIN) = 4 mm, opening jaw
+//
+// Detent ball pocket (Ø3 mm) snaps into rail-dimple for each position.
+// Handle points rearward (-Y) in clamped state for low profile.
+//
+// Print standing on cam edge (cam disc vertical), PETG, 5 perims, 40%.
+module cam_lever() {
+    cam_offset = (CAM_R_MAX - CAM_R_MIN) / 2;   // 2 mm eccentricity
+    union() {
+        difference() {
+            union() {
+                // ── Eccentric cam disc ───────────────────────────────────
+                // Offset so pivot bore is eccentric to disc profile
+                translate([cam_offset, 0, 0])
+                    cylinder(r = CAM_R_MAX, h = CAM_THICK, center = true);
+
+                // ── Lever handle arm ─────────────────────────────────────
+                hull() {
+                    translate([cam_offset, 0, 0])
+                        cylinder(r = CAM_R_MAX, h = CAM_THICK, center = true);
+                    translate([cam_offset + CAM_HANDLE_L, 0, 0])
+                        cylinder(r = CAM_HANDLE_W/2,
+                                 h = CAM_HANDLE_T, center = true);
+                }
+            }
+
+            // ── M4 pivot bore (through cam centre) ───────────────────────
+            cylinder(d = CAM_BORE_D, h = CAM_THICK + 2*e, center = true);
+
+            // ── Detent pockets (2× Ø3 mm, at 0° and 90°) ────────────────
+            // Pocket at 0° → clamped detent
+            translate([CAM_R_MAX - 2, 0, CAM_THICK/2 - 1.5])
+                cylinder(d = CAM_DETENT_D + 0.2, h = 2);
+            // Pocket at 90° → released detent
+            translate([0, CAM_R_MAX - 2, CAM_THICK/2 - 1.5])
+                cylinder(d = CAM_DETENT_D + 0.2, h = 2);
+
+            // ── Lightening recesses on cam disc face ─────────────────────
+            for (a = [0, 60, 120, 180, 240, 300])
+                translate([cam_offset + (CAM_R_MAX - 4) * cos(a),
+                           (CAM_R_MAX - 4) * sin(a), 0])
+                    cylinder(d = 4, h = CAM_THICK + 2*e, center = true);
+
+            // ── Handle grip grooves ──────────────────────────────────────
+            for (i = [0:4])
+                translate([cam_offset + 20 + i * 5, 0, 0])
+                    rotate([90, 0, 0])
+                        cylinder(d = 2.5, h = CAM_HANDLE_W + 2*e,
+                                 center = true);
+        }
+    }
+}
+
+// ============================================================
+// PART 5 — GRIP PAD (VIBRATION DAMPENING)
+// ============================================================
+// Flat pad with transverse flexure ribs that press against phone side.
+// The rib profile (thin PETG fins) provides compliance in Z (vertical)
+// absorbing vibration transmitted through the bracket.
+// Optional: print in TPU 95A for superior damping.
+// M2 bolts or adhesive-backed foam tape attach pad to jaw pocket face.
+//
+// Pad face (+Y) contacts phone.  Mounting face (-Y) bonds to jaw.
+// Ribs run parallel to Z axis (vertical).
+//
+// Print flat (mounting face down), PETG or TPU 95A, 3 perims, 20%.
+module grip_pad() {
+    union() {
+        // ── Base plate ───────────────────────────────────────────────────
+        translate([-PAD_W/2, -PAD_T, -PAD_H/2])
+            cube([PAD_W, PAD_T, PAD_H]);
+
+        // ── Flexure ribs (transverse, dampening in Z) ────────────────────
+        // RIB_COUNT ribs spaced RIB_PITCH apart, centred on pad
+        for (i = [0 : RIB_COUNT - 1]) {
+            rx = -PAD_W/2 + RIB_PITCH/2 + i * RIB_PITCH;
+            if (abs(rx) <= PAD_W/2 - RIB_W/2)   // stay within pad
+                translate([rx, 0, 0])
+                    cube([RIB_W, RIB_H, PAD_H - 4], center = true);
+        }
+
+        // ── Corner retention nubs (M2 boss for optional bolt-through) ────
+        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 = M2_D, h = PAD_T + 2*e, center = true);
+                }
+    }
+}

include/jlink.h

@@ -20,17 +20,20 @@
  *   CRC16 : CRC16-XModem over CMD+PAYLOAD (poly 0x1021, init 0), big-endian
  *   ETX   : frame end sentinel (0x03)
  *
- * Jetson → STM32 commands:
+ * Jetson to STM32 commands:
- *   0x01  HEARTBEAT  — no payload; refreshes heartbeat timer
+ *   0x01  HEARTBEAT  - no payload; refreshes heartbeat timer
- *   0x02  DRIVE      — int16 speed (-1000..+1000), int16 steer (-1000..+1000)
+ *   0x02  DRIVE      - int16 speed (-1000..+1000), int16 steer (-1000..+1000)
- *   0x03  ARM        — no payload; request arm (same interlock as CDC 'A')
+ *   0x03  ARM        - no payload; request arm (same interlock as CDC 'A')
- *   0x04  DISARM     — no payload; disarm immediately
+ *   0x04  DISARM     - no payload; disarm immediately
- *   0x05  PID_SET    — float kp, float ki, float kd (12 bytes, IEEE-754 LE)
+ *   0x05  PID_SET    - float kp, float ki, float kd (12 bytes, IEEE-754 LE)
- *   0x06  DFU_ENTER  — no payload; request OTA DFU reboot (denied while armed)
+ *   0x06  DFU_ENTER  - no payload; request OTA DFU reboot (denied while armed)
- *   0x07  ESTOP      — no payload; engage emergency stop
+ *   0x07  ESTOP      - no payload; engage emergency stop
+ *   0x0A  PID_SAVE   - no payload; save current Kp/Ki/Kd to flash (Issue #531)
  *
- * STM32 → Jetson telemetry:
+ * STM32 to Jetson telemetry:
- *   0x80  STATUS     — jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ
+ *   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
+ *   0x83  PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
  *
  * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
  * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
@@ -45,7 +48,7 @@
 #define JLINK_STX               0x02u
 #define JLINK_ETX               0x03u
 
-/* ---- Command IDs (Jetson → STM32) ---- */
+/* ---- Command IDs (Jetson to STM32) ---- */
 #define JLINK_CMD_HEARTBEAT     0x01u
 #define JLINK_CMD_DRIVE         0x02u
 #define JLINK_CMD_ARM           0x03u
@@ -55,16 +58,18 @@
 #define JLINK_CMD_ESTOP         0x07u
 #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) */
 
-/* ---- Telemetry IDs (STM32 → Jetson) ---- */
+/* ---- Telemetry IDs (STM32 to Jetson) ---- */
 #define JLINK_TLM_STATUS        0x80u
 #define JLINK_TLM_POWER         0x81u  /* jlink_tlm_power_t (11 bytes) */
+#define JLINK_TLM_PID_RESULT    0x83u  /* jlink_tlm_pid_result_t (13 bytes) Issue #531 */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
 typedef struct __attribute__((packed)) {
-    int16_t  pitch_x10;     /* pitch degrees ×10 */
+    int16_t  pitch_x10;     /* pitch degrees x10 */
-    int16_t  roll_x10;      /* roll  degrees ×10 */
+    int16_t  roll_x10;      /* roll  degrees x10 */
-    int16_t  yaw_x10;       /* yaw   degrees ×10 (gyro-integrated) */
+    int16_t  yaw_x10;       /* yaw   degrees x10 (gyro-integrated) */
     int16_t  motor_cmd;     /* ESC output -1000..+1000 */
     uint16_t vbat_mv;       /* battery millivolts */
     int8_t   rssi_dbm;      /* CRSF RSSI (dBm, negative) */
@@ -88,30 +93,41 @@ typedef struct __attribute__((packed)) {
     uint32_t idle_ms;       /* ms since last cmd_vel activity */
 } jlink_tlm_power_t;  /* 11 bytes */
 
+/* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */
+/* Sent after JLINK_CMD_PID_SAVE is processed; confirms gains written to flash. */
+typedef struct __attribute__((packed)) {
+    float   kp;        /* Kp saved */
+    float   ki;        /* Ki saved */
+    float   kd;        /* Kd saved */
+    uint8_t saved_ok;  /* 1 = flash write verified, 0 = write failed */
+} jlink_tlm_pid_result_t;  /* 13 bytes */
+
 /* ---- Volatile state (read from main loop) ---- */
 typedef struct {
-    /* Drive command — updated on JLINK_CMD_DRIVE */
+    /* Drive command - updated on JLINK_CMD_DRIVE */
     volatile int16_t  speed;        /* -1000..+1000 */
     volatile int16_t  steer;        /* -1000..+1000 */
 
-    /* Heartbeat timer — updated on any valid frame */
+    /* Heartbeat timer - updated on any valid frame */
     volatile uint32_t last_rx_ms;   /* HAL_GetTick() of last valid frame; 0=none */
 
-    /* One-shot request flags — set by parser, cleared by main loop */
+    /* One-shot request flags - set by parser, cleared by main loop */
     volatile uint8_t  arm_req;
     volatile uint8_t  disarm_req;
     volatile uint8_t  estop_req;
 
-    /* PID update — set by parser, cleared by main loop */
+    /* PID update - set by parser, cleared by main loop */
     volatile uint8_t  pid_updated;
     volatile float    pid_kp;
     volatile float    pid_ki;
     volatile float    pid_kd;
 
-    /* DFU reboot request — set by parser, cleared by main loop */
+    /* DFU reboot request - set by parser, cleared by main loop */
     volatile uint8_t  dfu_req;
-    /* Sleep request — set by JLINK_CMD_SLEEP, cleared by main loop */
+    /* Sleep request - set by JLINK_CMD_SLEEP, cleared by main loop */
     volatile uint8_t  sleep_req;
+    /* PID save request - set by JLINK_CMD_PID_SAVE, cleared by main loop (Issue #531) */
+    volatile uint8_t  pid_save_req;
 } JLinkState;
 
 extern volatile JLinkState jlink_state;
@@ -119,34 +135,40 @@ extern volatile JLinkState jlink_state;
 /* ---- API ---- */
 
 /*
- * jlink_init() — configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
+ * jlink_init() - configure USART1 (PB6=TX, PB7=RX) at 921600 baud with
  * DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt.
  * Call once before safety_init().
  */
 void jlink_init(void);
 
 /*
- * jlink_is_active(now_ms) — returns true if a valid frame arrived within
+ * jlink_is_active(now_ms) - returns true if a valid frame arrived within
  * JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received.
  */
 bool jlink_is_active(uint32_t now_ms);
 
 /*
- * jlink_send_telemetry(status) — build and transmit a JLINK_TLM_STATUS frame
+ * jlink_send_telemetry(status) - build and transmit a JLINK_TLM_STATUS frame
  * over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ.
  */
 void jlink_send_telemetry(const jlink_tlm_status_t *status);
 
 /*
- * jlink_process() — drain DMA circular buffer and parse frames.
+ * jlink_process() - drain DMA circular buffer and parse frames.
  * Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending).
  */
 void jlink_process(void);
 
 /*
- * jlink_send_power_telemetry(power) — build and transmit a JLINK_TLM_POWER
+ * jlink_send_power_telemetry(power) - build and transmit a JLINK_TLM_POWER
  * frame (17 bytes) at PM_TLM_HZ. Call from main loop when not in STOP mode.
  */
 void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
 
+/*
+ * jlink_send_pid_result(result) - build and transmit a JLINK_TLM_PID_RESULT
+ * frame (19 bytes) to confirm PID flash save outcome (Issue #531).
+ */
+void jlink_send_pid_result(const jlink_tlm_pid_result_t *result);
+
 #endif /* JLINK_H */

include/pid_flash.h

@@ -0,0 +1,48 @@
+#ifndef PID_FLASH_H
+#define PID_FLASH_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * 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.
+ * Sector 7 is 128KB starting at 0x08060000; firmware never exceeds sector 6.
+ *
+ * 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.
+ */
+
+#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 in last 64 bytes */
+#define PID_FLASH_STORE_ADDR    0x0807FFC0UL
+#define PID_FLASH_MAGIC         0x534C5401UL   /* 'SLT\x01' — version 1 */
+
+typedef struct __attribute__((packed)) {
+    uint32_t magic;       /* PID_FLASH_MAGIC when valid */
+    float    kp;
+    float    ki;
+    float    kd;
+    uint8_t  _pad[48];   /* padding to 64 bytes */
+} pid_flash_t;
+
+/*
+ * 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.
+ * 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);
+
+#endif /* PID_FLASH_H */

jetson/ros2_ws/src/saltybot_bringup/config/nav2_params.yaml

@@ -10,12 +10,14 @@
 #   → No AMCL, no map_server needed.
 #
 # Sensors (Issue #478 — Costmap configuration):
-#   /scan                    — RPLIDAR A1M8 360° LaserScan (obstacle)
+#   /scan                          — RPLIDAR A1M8 360° LaserScan (obstacle)
-#   /depth_scan              — RealSense D435i depth_to_laserscan (obstacle)
+#   /depth_scan                    — RealSense D435i depth_to_laserscan (obstacle)
-#   /camera/depth/color/points — RealSense D435i PointCloud2 (voxel)
+#   /camera/depth/color/points     — RealSense D435i PointCloud2 (voxel)
+#   /camera/depth/image_rect_raw   — RealSense D435i depth image (Issue #532 DepthCostmapLayer)
 #
 # Inflation:
 #   inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
+#   DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer)
 #
 # Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
 
@@ -256,7 +258,21 @@ local_costmap:
       resolution: 0.05
       robot_radius: 0.15
       footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"  # 0.4m x 0.4m
-      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      plugins: ["obstacle_layer", "voxel_layer", "depth_costmap_layer", "inflation_layer"]
+
+      # Issue #532: RealSense D435i depth-to-costmap plugin
+      # Point cloud projection with height filter + in-layer inflation
+      depth_costmap_layer:
+        plugin: "saltybot_depth_costmap::DepthCostmapLayer"
+        enabled: true
+        depth_topic: /camera/depth/image_rect_raw
+        camera_info_topic: /camera/depth/camera_info
+        min_height: 0.05          # m — ignore floor reflections
+        max_height: 0.80          # m — ignore ceiling/lights
+        obstacle_range: 3.5       # m — max range to mark obstacles
+        clearing_range: 4.0       # m — max range for clearing
+        inflation_radius: 0.10    # m — in-layer inflation (before inflation_layer)
+        downsample_factor: 4      # process 1 of 4 rows/cols (~19k pts @ 640x480)
 
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
@@ -327,7 +343,20 @@ global_costmap:
       footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]"  # 0.4m x 0.4m
       resolution: 0.05
       track_unknown_space: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_layer", "depth_costmap_layer", "inflation_layer"]
+
+      # Issue #532: RealSense D435i depth-to-costmap plugin (global costmap)
+      depth_costmap_layer:
+        plugin: "saltybot_depth_costmap::DepthCostmapLayer"
+        enabled: true
+        depth_topic: /camera/depth/image_rect_raw
+        camera_info_topic: /camera/depth/camera_info
+        min_height: 0.05
+        max_height: 0.80
+        obstacle_range: 3.5
+        clearing_range: 4.0
+        inflation_radius: 0.10
+        downsample_factor: 4
 
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"

jetson/ros2_ws/src/saltybot_depth_costmap/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_depth_costmap)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# ── Dependencies ──────────────────────────────────────────────────────────────
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(pluginlib REQUIRED)
+
+# ── Shared library (the plugin) ───────────────────────────────────────────────
+add_library(${PROJECT_NAME} SHARED
+  src/depth_costmap_layer.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  nav2_costmap_2d
+  sensor_msgs
+  geometry_msgs
+  tf2_ros
+  tf2
+  pluginlib
+)
+
+# Export plugin XML so pluginlib can discover it
+pluginlib_export_plugin_description_file(nav2_costmap_2d plugin.xml)
+
+# ── Install ───────────────────────────────────────────────────────────────────
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(FILES plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# ── Tests ─────────────────────────────────────────────────────────────────────
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(
+  rclcpp
+  nav2_costmap_2d
+  sensor_msgs
+  geometry_msgs
+  tf2_ros
+  tf2
+  pluginlib
+)
+
+ament_package()

jetson/ros2_ws/src/saltybot_depth_costmap/include/saltybot_depth_costmap/depth_costmap_layer.hpp

@@ -0,0 +1,113 @@
+#pragma once
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "nav2_costmap_2d/layer.hpp"
+#include "nav2_costmap_2d/layered_costmap.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+namespace saltybot_depth_costmap
+{
+
+struct ObstaclePoint
+{
+  double wx;  // world x (m)
+  double wy;  // world y (m)
+};
+
+/**
+ * DepthCostmapLayer — Nav2 costmap2d plugin (Issue #532)
+ *
+ * Converts RealSense D435i depth images to a Nav2 costmap layer.
+ *
+ * Pipeline:
+ *   1. Subscribe to /camera/depth/image_rect_raw (16UC1, mm) + camera_info
+ *   2. Back-project depth pixels to 3D using camera intrinsics
+ *   3. Transform points to costmap global_frame via TF2
+ *   4. Apply height filter (min_height .. max_height above ground)
+ *   5. Mark obstacle cells as LETHAL_OBSTACLE
+ *   6. Inflate nearby cells as INSCRIBED_INFLATED_OBSTACLE (in-layer inflation)
+ *
+ * Nav2 yaml usage:
+ *   local_costmap:
+ *     plugins: ["obstacle_layer", "voxel_layer", "depth_costmap_layer", "inflation_layer"]
+ *     depth_costmap_layer:
+ *       plugin: "saltybot_depth_costmap::DepthCostmapLayer"
+ *       enabled: true
+ *       depth_topic: /camera/depth/image_rect_raw
+ *       camera_info_topic: /camera/depth/camera_info
+ *       min_height: 0.05          # m — floor clearance
+ *       max_height: 0.80          # m — ceiling cutoff
+ *       obstacle_range: 3.5       # m — max marking range
+ *       clearing_range: 4.0       # m — max clearing range
+ *       inflation_radius: 0.10    # m — in-layer inflation radius
+ *       downsample_factor: 4      # process 1 of N rows/cols
+ */
+class DepthCostmapLayer : public nav2_costmap_2d::Layer
+{
+public:
+  DepthCostmapLayer();
+  ~DepthCostmapLayer() override = default;
+
+  void onInitialize() override;
+
+  void updateBounds(
+    double robot_x, double robot_y, double robot_yaw,
+    double * min_x, double * min_y,
+    double * max_x, double * max_y) override;
+
+  void updateCosts(
+    nav2_costmap_2d::Costmap2D & master_grid,
+    int min_i, int min_j, int max_i, int max_j) override;
+
+  void reset() override;
+  bool isClearable() override { return true; }
+
+private:
+  void depthCallback(const sensor_msgs::msg::Image::SharedPtr msg);
+  void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
+  void processDepthImage(const sensor_msgs::msg::Image::SharedPtr & img);
+
+  // ROS interfaces
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr info_sub_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  // Camera intrinsics (from camera_info)
+  double fx_{0.0}, fy_{0.0}, cx_{0.0}, cy_{0.0};
+  bool have_camera_info_{false};
+  std::string camera_frame_;
+
+  // Plugin parameters
+  std::string depth_topic_;
+  std::string camera_info_topic_;
+  double min_height_{0.05};        // m — filter floor
+  double max_height_{0.80};        // m — filter ceiling
+  double obstacle_range_{3.5};     // m — max marking range (from camera)
+  double clearing_range_{4.0};     // m — max clearing range
+  double inflation_radius_{0.10};  // m — in-layer inflation
+  int    downsample_factor_{4};    // skip N-1 of every N pixels
+
+  // Latest processed obstacle points in world frame
+  std::vector<ObstaclePoint> obstacle_points_;
+  std::mutex points_mutex_;
+  bool need_update_{false};
+
+  // Dirty bounds for updateBounds()
+  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
+
+  // For throttled logging outside onInitialize
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
+};
+
+}  // namespace saltybot_depth_costmap

jetson/ros2_ws/src/saltybot_depth_costmap/package.xml

@@ -0,0 +1,30 @@
+<?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_depth_costmap</name>
+  <version>0.1.0</version>
+  <description>
+    Nav2 costmap2d plugin: DepthCostmapLayer (Issue #532).
+    Converts RealSense D435i depth images to a Nav2 costmap layer with
+    configurable height filtering and in-layer obstacle inflation.
+  </description>
+  <maintainer email="seb@vayrette.com">seb</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_depth_costmap/plugin.xml

@@ -0,0 +1,15 @@
+<library path="saltybot_depth_costmap">
+  <class
+    name="saltybot_depth_costmap::DepthCostmapLayer"
+    type="saltybot_depth_costmap::DepthCostmapLayer"
+    base_class_type="nav2_costmap_2d::Layer">
+    <description>
+      Nav2 costmap layer that converts RealSense D435i depth images into
+      obstacle markings. Back-projects depth pixels to 3D, transforms to the
+      costmap global frame via TF2, applies a configurable height filter
+      (min_height .. max_height), marks lethal obstacles, and inflates them
+      by inflation_radius within the layer. Integrates with existing Nav2
+      costmap config (Issue #478).
+    </description>
+  </class>
+</library>

jetson/ros2_ws/src/saltybot_depth_costmap/src/depth_costmap_layer.cpp

@@ -0,0 +1,306 @@
+#include "saltybot_depth_costmap/depth_costmap_layer.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+#include <string>
+#include <vector>
+
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "tf2/exceptions.h"
+
+PLUGINLIB_EXPORT_CLASS(
+  saltybot_depth_costmap::DepthCostmapLayer,
+  nav2_costmap_2d::Layer)
+
+namespace saltybot_depth_costmap
+{
+
+using nav2_costmap_2d::FREE_SPACE;
+using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
+using nav2_costmap_2d::LETHAL_OBSTACLE;
+using nav2_costmap_2d::NO_INFORMATION;
+
+DepthCostmapLayer::DepthCostmapLayer()
+: last_min_x_(-1e9), last_min_y_(-1e9),
+  last_max_x_(1e9), last_max_y_(1e9),
+  logger_(rclcpp::get_logger("DepthCostmapLayer"))
+{}
+
+// ── onInitialize ──────────────────────────────────────────────────────────────
+
+void DepthCostmapLayer::onInitialize()
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error("DepthCostmapLayer: node handle expired");
+  }
+
+  logger_ = node->get_logger();
+  clock_  = node->get_clock();
+
+  declareParameter("enabled",            rclcpp::ParameterValue(true));
+  declareParameter("depth_topic",        rclcpp::ParameterValue(
+    std::string("/camera/depth/image_rect_raw")));
+  declareParameter("camera_info_topic",  rclcpp::ParameterValue(
+    std::string("/camera/depth/camera_info")));
+  declareParameter("min_height",         rclcpp::ParameterValue(0.05));
+  declareParameter("max_height",         rclcpp::ParameterValue(0.80));
+  declareParameter("obstacle_range",     rclcpp::ParameterValue(3.5));
+  declareParameter("clearing_range",     rclcpp::ParameterValue(4.0));
+  declareParameter("inflation_radius",   rclcpp::ParameterValue(0.10));
+  declareParameter("downsample_factor",  rclcpp::ParameterValue(4));
+
+  enabled_           = node->get_parameter(name_ + ".enabled").as_bool();
+  depth_topic_       = node->get_parameter(name_ + ".depth_topic").as_string();
+  camera_info_topic_ = node->get_parameter(name_ + ".camera_info_topic").as_string();
+  min_height_        = node->get_parameter(name_ + ".min_height").as_double();
+  max_height_        = node->get_parameter(name_ + ".max_height").as_double();
+  obstacle_range_    = node->get_parameter(name_ + ".obstacle_range").as_double();
+  clearing_range_    = node->get_parameter(name_ + ".clearing_range").as_double();
+  inflation_radius_  = node->get_parameter(name_ + ".inflation_radius").as_double();
+  downsample_factor_ = node->get_parameter(name_ + ".downsample_factor").as_int();
+
+  if (downsample_factor_ < 1) downsample_factor_ = 1;
+
+  RCLCPP_INFO(
+    logger_,
+    "DepthCostmapLayer '%s': depth=%s height=[%.2f, %.2f]m range=%.1fm inflate=%.2fm ds=%d",
+    name_.c_str(), depth_topic_.c_str(),
+    min_height_, max_height_, obstacle_range_, inflation_radius_, downsample_factor_);
+
+  // TF2
+  tf_buffer_   = std::make_shared<tf2_ros::Buffer>(clock_);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  // Subscriptions
+  depth_sub_ = node->create_subscription<sensor_msgs::msg::Image>(
+    depth_topic_, rclcpp::SensorDataQoS(),
+    std::bind(&DepthCostmapLayer::depthCallback, this, std::placeholders::_1));
+
+  info_sub_ = node->create_subscription<sensor_msgs::msg::CameraInfo>(
+    camera_info_topic_, rclcpp::QoS(1).reliable(),
+    std::bind(&DepthCostmapLayer::cameraInfoCallback, this, std::placeholders::_1));
+
+  current_ = true;
+}
+
+// ── cameraInfoCallback ────────────────────────────────────────────────────────
+
+void DepthCostmapLayer::cameraInfoCallback(
+  const sensor_msgs::msg::CameraInfo::SharedPtr msg)
+{
+  if (have_camera_info_) return;  // Intrinsics are stable — only need once
+
+  // Pinhole intrinsics from K matrix (row-major 3x3)
+  fx_ = msg->k[0];   // K[0,0]
+  fy_ = msg->k[4];   // K[1,1]
+  cx_ = msg->k[2];   // K[0,2]
+  cy_ = msg->k[5];   // K[1,2]
+  camera_frame_ = msg->header.frame_id;
+
+  if (fx_ <= 0.0 || fy_ <= 0.0) {
+    RCLCPP_WARN(logger_, "DepthCostmapLayer: invalid camera intrinsics (fx=%.1f fy=%.1f)", fx_, fy_);
+    return;
+  }
+
+  have_camera_info_ = true;
+  RCLCPP_INFO(
+    logger_,
+    "DepthCostmapLayer: camera_info received — frame=%s fx=%.1f fy=%.1f cx=%.1f cy=%.1f",
+    camera_frame_.c_str(), fx_, fy_, cx_, cy_);
+}
+
+// ── depthCallback / processDepthImage ────────────────────────────────────────
+
+void DepthCostmapLayer::depthCallback(
+  const sensor_msgs::msg::Image::SharedPtr msg)
+{
+  if (!enabled_ || !have_camera_info_) return;
+  processDepthImage(msg);
+}
+
+void DepthCostmapLayer::processDepthImage(
+  const sensor_msgs::msg::Image::SharedPtr & img)
+{
+  // Resolve encoding
+  bool is_16u = (img->encoding == "16UC1" || img->encoding == "mono16");
+  bool is_32f = (img->encoding == "32FC1");
+  if (!is_16u && !is_32f) {
+    RCLCPP_WARN_ONCE(logger_, "DepthCostmapLayer: unsupported encoding '%s'", img->encoding.c_str());
+    return;
+  }
+
+  // Get TF from camera → costmap global frame (latest available)
+  const std::string global_frame = layered_costmap_->getGlobalFrameID();
+  geometry_msgs::msg::TransformStamped tf_stamped;
+  try {
+    tf_stamped = tf_buffer_->lookupTransform(
+      global_frame, camera_frame_,
+      rclcpp::Time(0),
+      rclcpp::Duration::from_seconds(0.1));
+  } catch (const tf2::TransformException & ex) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000,
+      "DepthCostmapLayer: TF %s→%s failed: %s",
+      camera_frame_.c_str(), global_frame.c_str(), ex.what());
+    return;
+  }
+
+  // Build rotation matrix from quaternion (camera frame → world frame)
+  // p_world = R * p_cam + t
+  const auto & t = tf_stamped.transform.translation;
+  const auto & q = tf_stamped.transform.rotation;
+  double qw = q.w, qx = q.x, qy = q.y, qz = q.z;
+
+  double R[3][3] = {
+    {1.0 - 2.0*(qy*qy + qz*qz),       2.0*(qx*qy - qz*qw),  2.0*(qx*qz + qy*qw)},
+    {      2.0*(qx*qy + qz*qw), 1.0 - 2.0*(qx*qx + qz*qz),  2.0*(qy*qz - qx*qw)},
+    {      2.0*(qx*qz - qy*qw),       2.0*(qy*qz + qx*qw), 1.0 - 2.0*(qx*qx + qy*qy)}
+  };
+
+  int rows = static_cast<int>(img->height);
+  int cols = static_cast<int>(img->width);
+  int step = static_cast<int>(img->step);
+
+  const double obstacle_range_sq = obstacle_range_ * obstacle_range_;
+
+  std::vector<ObstaclePoint> new_points;
+  new_points.reserve(512);
+
+  for (int v = 0; v < rows; v += downsample_factor_) {
+    const uint8_t * row_ptr = img->data.data() + v * step;
+
+    for (int u = 0; u < cols; u += downsample_factor_) {
+      double depth_m = 0.0;
+
+      if (is_16u) {
+        uint16_t raw;
+        std::memcpy(&raw, row_ptr + u * 2, sizeof(raw));
+        if (raw == 0) continue;
+        depth_m = raw * 0.001;  // mm → m
+      } else {
+        float raw;
+        std::memcpy(&raw, row_ptr + u * 4, sizeof(raw));
+        if (!std::isfinite(raw) || raw <= 0.0f) continue;
+        depth_m = static_cast<double>(raw);
+      }
+
+      if (depth_m > clearing_range_) continue;
+
+      // Back-project: depth optical frame (Z forward, X right, Y down)
+      double xc = (u - cx_) * depth_m / fx_;
+      double yc = (v - cy_) * depth_m / fy_;
+      double zc = depth_m;
+
+      // Distance check in camera frame (cheaper than world-frame dist)
+      double dist2 = xc*xc + yc*yc + zc*zc;
+      if (dist2 > obstacle_range_sq) continue;
+
+      // Transform to world frame
+      double xw = R[0][0]*xc + R[0][1]*yc + R[0][2]*zc + t.x;
+      double yw = R[1][0]*xc + R[1][1]*yc + R[1][2]*zc + t.y;
+      double zw = R[2][0]*xc + R[2][1]*yc + R[2][2]*zc + t.z;
+
+      // Height filter (zw = height above ground in world frame)
+      if (zw < min_height_ || zw > max_height_) continue;
+
+      new_points.push_back({xw, yw});
+    }
+  }
+
+  {
+    std::lock_guard<std::mutex> lock(points_mutex_);
+    obstacle_points_ = std::move(new_points);
+    need_update_ = true;
+  }
+}
+
+// ── updateBounds ──────────────────────────────────────────────────────────────
+
+void DepthCostmapLayer::updateBounds(
+  double robot_x, double robot_y, double /*robot_yaw*/,
+  double * min_x, double * min_y,
+  double * max_x, double * max_y)
+{
+  if (!enabled_) return;
+
+  // Mark region around robot that depth camera can reach
+  double margin = obstacle_range_ + inflation_radius_;
+  last_min_x_ = robot_x - margin;
+  last_min_y_ = robot_y - margin;
+  last_max_x_ = robot_x + margin;
+  last_max_y_ = robot_y + margin;
+
+  *min_x = std::min(*min_x, last_min_x_);
+  *min_y = std::min(*min_y, last_min_y_);
+  *max_x = std::max(*max_x, last_max_x_);
+  *max_y = std::max(*max_y, last_max_y_);
+}
+
+// ── updateCosts ───────────────────────────────────────────────────────────────
+
+void DepthCostmapLayer::updateCosts(
+  nav2_costmap_2d::Costmap2D & master,
+  int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_) return;
+
+  std::lock_guard<std::mutex> lock(points_mutex_);
+  if (!need_update_ || obstacle_points_.empty()) return;
+
+  double res = master.getResolution();
+  // Number of cells to inflate in each direction
+  int inflate_cells = (inflation_radius_ > 0.0)
+    ? static_cast<int>(std::ceil(inflation_radius_ / res))
+    : 0;
+
+  for (const auto & pt : obstacle_points_) {
+    unsigned int mx, my;
+    if (!master.worldToMap(pt.wx, pt.wy, mx, my)) continue;
+
+    int ci = static_cast<int>(mx);
+    int cj = static_cast<int>(my);
+
+    // Mark the obstacle cell as lethal
+    if (ci >= min_i && ci < max_i && cj >= min_j && cj < max_j) {
+      master.setCost(mx, my, LETHAL_OBSTACLE);
+    }
+
+    // In-layer inflation: mark neighbouring cells within inflation_radius
+    // as INSCRIBED_INFLATED_OBSTACLE (if not already lethal)
+    for (int di = -inflate_cells; di <= inflate_cells; ++di) {
+      for (int dj = -inflate_cells; dj <= inflate_cells; ++dj) {
+        if (di == 0 && dj == 0) continue;
+
+        double dist = std::hypot(static_cast<double>(di), static_cast<double>(dj)) * res;
+        if (dist > inflation_radius_) continue;
+
+        int ni = ci + di;
+        int nj = cj + dj;
+        if (ni < min_i || ni >= max_i || nj < min_j || nj >= max_j) continue;
+
+        auto uni = static_cast<unsigned int>(ni);
+        auto unj = static_cast<unsigned int>(nj);
+        unsigned char existing = master.getCost(uni, unj);
+        if (existing < INSCRIBED_INFLATED_OBSTACLE) {
+          master.setCost(uni, unj, INSCRIBED_INFLATED_OBSTACLE);
+        }
+      }
+    }
+  }
+
+  need_update_ = false;
+}
+
+// ── reset ─────────────────────────────────────────────────────────────────────
+
+void DepthCostmapLayer::reset()
+{
+  std::lock_guard<std::mutex> lock(points_mutex_);
+  obstacle_points_.clear();
+  need_update_ = false;
+  current_ = true;
+}
+
+}  // namespace saltybot_depth_costmap

jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml

@@ -16,3 +16,8 @@ pid_autotune:
 
     # Time to allow system to settle before starting relay (seconds)
     settle_time: 2.0
+
+    # JLink serial port for sending computed gains to FC (Issue #531)
+    # USART1 on FC maps to this ttyTHS device on Jetson Orin
+    jlink_serial_port: "/dev/ttyTHS0"
+    jlink_baud_rate: 921600

jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py

@@ -2,7 +2,9 @@
 """PID auto-tuner for SaltyBot using Astrom-Hagglund relay feedback method.
 
 Service-triggered relay oscillation measures ultimate gain (Ku) and ultimate period
-(Tu), then computes Ziegler-Nichols PD gains. Aborts on >25deg tilt for safety.
+(Tu), then computes Ziegler-Nichols PID gains (Kp, Ki, Kd). After tuning the computed
+gains are written to the FC via JLink binary protocol (JLINK_CMD_PID_SET) and
+immediately saved to flash (JLINK_CMD_PID_SAVE). Aborts on >25deg tilt for safety.
 
 Service:
   /saltybot/autotune_pid (std_srvs/Trigger) - Start auto-tuning process
@@ -18,18 +20,53 @@ Subscribed topics:
 
 import json
 import math
+import struct
 import time
 from enum import Enum
 from typing import Optional, Tuple, List
 
 import rclpy
 from rclpy.node import Node
-from rclpy.service import Service
 from geometry_msgs.msg import Twist
 from sensor_msgs.msg import Imu
 from std_msgs.msg import Float32, String
 from std_srvs.srv import Trigger
 
+try:
+    import serial
+    _SERIAL_AVAILABLE = True
+except ImportError:
+    _SERIAL_AVAILABLE = False
+
+
+# ── JLink binary protocol constants (must match jlink.h) ─────────────────────
+JLINK_STX = 0x02
+JLINK_ETX = 0x03
+JLINK_CMD_PID_SET  = 0x05
+JLINK_CMD_PID_SAVE = 0x0A
+
+
+def _crc16_xmodem(data: bytes) -> int:
+    """CRC16-XModem (poly 0x1021, init 0x0000) — matches FC implementation."""
+    crc = 0x0000
+    for byte in data:
+        crc ^= byte << 8
+        for _ in range(8):
+            if crc & 0x8000:
+                crc = (crc << 1) ^ 0x1021
+            else:
+                crc <<= 1
+        crc &= 0xFFFF
+    return crc
+
+
+def _build_jlink_frame(cmd: int, payload: bytes) -> bytes:
+    """Build a JLink binary frame: [STX][LEN][CMD][PAYLOAD][CRC_hi][CRC_lo][ETX]."""
+    data = bytes([cmd]) + payload
+    length = len(data)
+    crc = _crc16_xmodem(data)
+    return bytes([JLINK_STX, length]) + data + bytes([crc >> 8, crc & 0xFF, JLINK_ETX])
+
 
 class TuneState(Enum):
     """State machine for auto-tuning process."""
@@ -47,24 +84,28 @@ class PIDAutotuneNode(Node):
         super().__init__("pid_autotune")
 
         # Parameters
-        self.declare_parameter("relay_magnitude", 0.3)  # Relay on-off magnitude
+        self.declare_parameter("relay_magnitude", 0.3)
-        self.declare_parameter("tilt_safety_limit", 25.0)  # degrees
+        self.declare_parameter("tilt_safety_limit", 25.0)
-        self.declare_parameter("max_tune_duration", 120.0)  # seconds
+        self.declare_parameter("max_tune_duration", 120.0)
-        self.declare_parameter("oscillation_count", 5)  # Number of cycles to measure
+        self.declare_parameter("oscillation_count", 5)
-        self.declare_parameter("settle_time", 2.0)  # Settle before starting relay
+        self.declare_parameter("settle_time", 2.0)
+        self.declare_parameter("jlink_serial_port", "/dev/ttyTHS0")
+        self.declare_parameter("jlink_baud_rate", 921600)
 
-        self.relay_magnitude = self.get_parameter("relay_magnitude").value
+        self.relay_magnitude   = self.get_parameter("relay_magnitude").value
         self.tilt_safety_limit = self.get_parameter("tilt_safety_limit").value
         self.max_tune_duration = self.get_parameter("max_tune_duration").value
         self.oscillation_count = self.get_parameter("oscillation_count").value
-        self.settle_time = self.get_parameter("settle_time").value
+        self.settle_time       = self.get_parameter("settle_time").value
+        self._jlink_port       = self.get_parameter("jlink_serial_port").value
+        self._jlink_baud       = self.get_parameter("jlink_baud_rate").value
 
-        # State tracking
+        # State
         self.tune_state = TuneState.IDLE
         self.last_imu_tilt = 0.0
         self.last_feedback = 0.0
-        self.feedback_history: List[Tuple[float, float]] = []  # (time, feedback)
+        self.feedback_history: List[Tuple[float, float]] = []
-        self.peaks: List[Tuple[float, float]] = []  # (time, peak_value)
+        self.peaks: List[Tuple[float, float]] = []
         self.start_time = None
 
         # Subscriptions
@@ -74,7 +115,9 @@ class PIDAutotuneNode(Node):
         )
 
         # Publications
-        self.pub_cmd_vel = self.create_publisher(Twist, "/saltybot/autotune_cmd_vel", 10)
+        self.pub_cmd_vel = self.create_publisher(
+            Twist, "/saltybot/autotune_cmd_vel", 10
+        )
         self.pub_info = self.create_publisher(String, "/saltybot/autotune_info", 10)
 
         # Service
@@ -86,31 +129,29 @@ class PIDAutotuneNode(Node):
             "PID auto-tuner initialized. Service: /saltybot/autotune_pid\n"
             f"Relay magnitude: {self.relay_magnitude}, "
             f"Tilt safety limit: {self.tilt_safety_limit}°, "
-            f"Max duration: {self.max_tune_duration}s"
+            f"Max duration: {self.max_tune_duration}s, "
+            f"JLink port: {self._jlink_port}"
         )
 
+    # ── IMU / feedback ────────────────────────────────────────────────────────
+
     def _on_imu(self, msg: Imu) -> None:
-        """Extract tilt angle from IMU."""
-        # Approximate tilt from acceleration (pitch angle)
-        # pitch = atan2(ax, sqrt(ay^2 + az^2))
         accel = msg.linear_acceleration
         self.last_imu_tilt = math.degrees(
             math.atan2(accel.x, math.sqrt(accel.y**2 + accel.z**2))
         )
 
     def _on_feedback(self, msg: Float32) -> None:
-        """Record balance feedback for oscillation measurement."""
         now = self.get_clock().now().nanoseconds / 1e9
         self.last_feedback = msg.data
-
+        if self.tune_state == TuneState.MEASURING:
-        # During tuning, record history
-        if self.tune_state in (TuneState.MEASURING,):
             self.feedback_history.append((now, msg.data))
 
+    # ── Service handler ───────────────────────────────────────────────────────
+
     def _handle_autotune_request(
         self, request: Trigger.Request, response: Trigger.Response
     ) -> Trigger.Response:
-        """Handle service request to start auto-tuning."""
         if self.tune_state != TuneState.IDLE:
             response.success = False
             response.message = f"Already tuning (state: {self.tune_state.name})"
@@ -122,15 +163,15 @@ class PIDAutotuneNode(Node):
         self.peaks = []
         self.start_time = self.get_clock().now().nanoseconds / 1e9
 
-        # Start tuning process
         self._run_autotuning()
 
         response.success = self.tune_state == TuneState.COMPLETE
         response.message = f"Tuning {self.tune_state.name}"
         return response
 
+    # ── Tuning sequence ───────────────────────────────────────────────────────
+
     def _run_autotuning(self) -> None:
-        """Execute the complete auto-tuning sequence."""
         # Phase 1: Settle
         self.get_logger().info("Phase 1: Settling...")
         if not self._settle():
@@ -150,7 +191,7 @@ class PIDAutotuneNode(Node):
             )
             return
 
-        # Phase 3: Analysis
+        # Phase 3: Analyse
         self.get_logger().info("Phase 3: Analyzing measurements...")
         ku, tu = self._analyze_oscillation()
         if ku is None or tu is None:
@@ -160,10 +201,12 @@ class PIDAutotuneNode(Node):
             )
             return
 
-        # Phase 4: Compute gains
+        # Phase 4: Compute ZN gains
-        kp, kd = self._compute_ziegler_nichols(ku, tu)
+        kp, ki, kd = self._compute_ziegler_nichols(ku, tu)
+
+        # Phase 5: Push gains to FC and save to flash
+        saved = self._send_pid_to_fc(kp, ki, kd)
 
-        # Done
         self.tune_state = TuneState.COMPLETE
         self._publish_info(
             {
@@ -171,35 +214,33 @@ class PIDAutotuneNode(Node):
                 "ku": ku,
                 "tu": tu,
                 "kp": kp,
+                "ki": ki,
                 "kd": kd,
+                "flash_saved": saved,
             }
         )
         self.get_logger().info(
-            f"Auto-tune complete: Ku={ku:.4f}, Tu={tu:.2f}s, Kp={kp:.4f}, Kd={kd:.4f}"
+            f"Auto-tune complete: Ku={ku:.4f}, Tu={tu:.2f}s | "
+            f"Kp={kp:.4f}, Ki={ki:.4f}, Kd={kd:.4f} | flash_saved={saved}"
         )
 
     def _settle(self) -> bool:
-        """Allow system to settle before starting relay."""
         settle_start = self.get_clock().now().nanoseconds / 1e9
-        relay_off = Twist()  # Zero command
+        relay_off = Twist()
 
         while True:
             now = self.get_clock().now().nanoseconds / 1e9
             elapsed = now - settle_start
 
-            # Safety check
             if abs(self.last_imu_tilt) > self.tilt_safety_limit:
                 self.get_logger().error(
-                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°"
+                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > "
+                    f"{self.tilt_safety_limit}°"
                 )
                 return False
-
-            # Timeout check
             if elapsed > self.max_tune_duration:
                 self.get_logger().error("Settling timeout")
                 return False
-
-            # Continue settling
             if elapsed > self.settle_time:
                 return True
 
@@ -207,57 +248,47 @@ class PIDAutotuneNode(Node):
             time.sleep(0.01)
 
     def _relay_oscillation(self) -> bool:
-        """Apply relay control to induce oscillation."""
         relay_on = Twist()
         relay_on.linear.x = self.relay_magnitude
-
         relay_off = Twist()
 
         oscillation_start = self.get_clock().now().nanoseconds / 1e9
-        relay_state = True  # True = relay ON, False = relay OFF
+        relay_state = True
-        last_switch = oscillation_start
         cycles_measured = 0
-        last_peak_value = None
 
         while cycles_measured < self.oscillation_count:
             now = self.get_clock().now().nanoseconds / 1e9
             elapsed = now - oscillation_start
 
-            # Safety checks
             if abs(self.last_imu_tilt) > self.tilt_safety_limit:
                 self.get_logger().error(
-                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°"
+                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > "
+                    f"{self.tilt_safety_limit}°"
                 )
                 return False
-
             if elapsed > self.max_tune_duration:
                 self.get_logger().error("Relay oscillation timeout")
                 return False
 
-            # Switch relay at zero crossings
+            # Switch relay at zero crossings of balance feedback
             if relay_state and self.last_feedback < 0:
                 relay_state = False
                 self.peaks.append((now, self.last_feedback))
                 cycles_measured += 1
-                last_switch = now
                 self.get_logger().debug(
-                    f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}"
+                    f"Cycle {cycles_measured}: peak={self.last_feedback:.4f} "
+                    f"t={elapsed:.2f}s"
                 )
-
             elif not relay_state and self.last_feedback > 0:
                 relay_state = True
                 self.peaks.append((now, self.last_feedback))
                 cycles_measured += 1
-                last_switch = now
                 self.get_logger().debug(
-                    f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}"
+                    f"Cycle {cycles_measured}: peak={self.last_feedback:.4f} "
+                    f"t={elapsed:.2f}s"
                 )
 
-            # Publish relay command
+            self.pub_cmd_vel.publish(relay_on if relay_state else relay_off)
-            cmd = relay_on if relay_state else relay_off
-            self.pub_cmd_vel.publish(cmd)
-
-            # Report progress
             self._publish_info(
                 {
                     "status": "measuring",
@@ -266,25 +297,24 @@ class PIDAutotuneNode(Node):
                     "tilt": self.last_imu_tilt,
                 }
             )
-
             time.sleep(0.01)
 
-        # Stop relay
         self.pub_cmd_vel.publish(relay_off)
         return len(self.peaks) >= self.oscillation_count
 
-    def _analyze_oscillation(self) -> Tuple[Optional[float], Optional[float]]:
+    def _analyze_oscillation(
-        """Calculate Ku and Tu from measured peaks."""
+        self,
+    ) -> Tuple[Optional[float], Optional[float]]:
         if len(self.peaks) < 3:
             self.get_logger().error(f"Insufficient peaks: {len(self.peaks)}")
             return None, None
 
-        # Ku = 4*relay_magnitude / (pi * |peak|)
+        # Ku = 4·d / (π·|a|)  where d = relay magnitude, a = average peak amplitude
         peak_values = [abs(p[1]) for p in self.peaks]
         avg_peak = sum(peak_values) / len(peak_values)
         ku = (4.0 * self.relay_magnitude) / (math.pi * avg_peak)
 
-        # Tu = 2 * (time between peaks) - average period
+        # Tu = 2 × mean half-period (time between consecutive zero crossings)
         peak_times = [p[0] for p in self.peaks]
         time_diffs = [
             peak_times[i + 1] - peak_times[i] for i in range(len(peak_times) - 1)
@@ -297,26 +327,73 @@ class PIDAutotuneNode(Node):
         return ku, tu
 
     @staticmethod
-    def _compute_ziegler_nichols(ku: float, tu: float) -> Tuple[float, float]:
+    def _compute_ziegler_nichols(
-        """Compute Ziegler-Nichols PD gains.
+        ku: float, tu: float
+    ) -> Tuple[float, float, float]:
+        """Ziegler-Nichols PID gains from relay feedback critical params.
 
-        For PD controller:
+        Standard ZN PID table:
-          Kp = 0.6 * Ku
+          Kp = 0.60 · Ku
-          Kd = 0.075 * Ku * Tu (approximately Kp*Tu/8)
+          Ki = 1.20 · Ku / Tu     (= 2·Kp / Tu)
+          Kd = 0.075 · Ku · Tu    (= Kp·Tu / 8)
         """
-        kp = 0.6 * ku
+        kp = 0.60 * ku
+        ki = 1.20 * ku / tu
         kd = 0.075 * ku * tu
-        return kp, kd
+        return kp, ki, kd
+
+    # ── JLink communication ───────────────────────────────────────────────────
+
+    def _send_pid_to_fc(self, kp: float, ki: float, kd: float) -> bool:
+        """Send JLINK_CMD_PID_SET then JLINK_CMD_PID_SAVE over JLink serial.
+
+        Returns True if both frames were written without serial error.
+        """
+        if not _SERIAL_AVAILABLE:
+            self.get_logger().warn(
+                "pyserial not available — PID gains not sent to FC. "
+                "Install with: pip install pyserial"
+            )
+            return False
+
+        try:
+            payload_set = struct.pack("<fff", kp, ki, kd)  # 12 bytes, little-endian
+            frame_set  = _build_jlink_frame(JLINK_CMD_PID_SET, payload_set)
+            frame_save = _build_jlink_frame(JLINK_CMD_PID_SAVE, b"")
+
+            with serial.Serial(
+                port=self._jlink_port,
+                baudrate=self._jlink_baud,
+                timeout=1.0,
+                write_timeout=0.5,
+            ) as ser:
+                ser.write(frame_set)
+                time.sleep(0.05)   # allow FC to process PID_SET
+                ser.write(frame_save)
+                # Flash erase takes ~1s on STM32F7; wait for it
+                time.sleep(1.5)
+
+            self.get_logger().info(
+                f"Sent PID_SET+PID_SAVE to FC via {self._jlink_port}: "
+                f"Kp={kp:.4f}, Ki={ki:.4f}, Kd={kd:.4f}"
+            )
+            return True
+
+        except Exception as exc:
+            self.get_logger().error(
+                f"Failed to send PID to FC on {self._jlink_port}: {exc}"
+            )
+            return False
+
+    # ── Helpers ───────────────────────────────────────────────────────────────
 
     def _publish_info(self, data: dict) -> None:
-        """Publish tuning information as JSON."""
         info = {
             "timestamp": self.get_clock().now().nanoseconds / 1e9,
             "state": self.tune_state.name,
             **data,
         }
-        msg = String(data=json.dumps(info))
+        self.pub_info.publish(String(data=json.dumps(info)))
-        self.pub_info.publish(msg)
 
 
 def main(args=None):

src/jlink.c

@@ -203,6 +203,11 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
         jlink_state.sleep_req = 1u;
         break;
 
+    case JLINK_CMD_PID_SAVE:
+        /* Payload-less; main loop calls pid_flash_save() (Issue #531) */
+        jlink_state.pid_save_req = 1u;
+        break;
+
     default:
         break;
     }
@@ -342,3 +347,28 @@ void jlink_send_power_telemetry(const jlink_tlm_power_t *power)
 
     jlink_tx_locked(frame, sizeof(frame));
 }
+
+/* ---- jlink_send_pid_result() -- Issue #531 ---- */
+void jlink_send_pid_result(const jlink_tlm_pid_result_t *result)
+{
+    /*
+     * Frame: [STX][LEN][0x83][13 bytes PID_RESULT][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 (CMD) + 13 (payload) = 14; total frame = 19 bytes.
+     * At 921600 baud: 19x10/921600 ~0.21 ms -- safe to block.
+     */
+    static uint8_t frame_pid[19];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_pid_result_t);  /* 13 */
+    const uint8_t  flen = 1u + plen;                                 /* 14 */
+
+    frame_pid[0] = JLINK_STX;
+    frame_pid[1] = flen;
+    frame_pid[2] = JLINK_TLM_PID_RESULT;
+    memcpy(&frame_pid[3], result, plen);
+
+    uint16_t crc_pid = crc16_xmodem(&frame_pid[2], flen);
+    frame_pid[3 + plen]     = (uint8_t)(crc_pid >> 8);
+    frame_pid[3 + plen + 1] = (uint8_t)(crc_pid & 0xFFu);
+    frame_pid[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame_pid, sizeof(frame_pid));
+}

src/main.c

@@ -30,6 +30,7 @@
 #include "battery.h"
 #include "coulomb_counter.h"
 #include "watchdog.h"
+#include "pid_flash.h"
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
@@ -163,6 +164,18 @@ int main(void) {
     balance_t bal;
     balance_init(&bal);
 
+    /* Load PID from flash if previously auto-tuned (Issue #531) */
+    {
+        float flash_kp, flash_ki, flash_kd;
+        if (pid_flash_load(&flash_kp, &flash_ki, &flash_kd)) {
+            bal.kp = flash_kp;
+            bal.ki = flash_ki;
+            bal.kd = flash_kd;
+            printf("[PID] Loaded from flash: Kp=%.3f Ki=%.3f Kd=%.3f\n",
+                   (double)flash_kp, (double)flash_ki, (double)flash_kd);
+        }
+    }
+
     /* Init motor driver */
     motor_driver_t motors;
     motor_driver_init(&motors);
@@ -334,6 +347,21 @@ int main(void) {
             jlink_state.sleep_req = 0u;
             power_mgmt_request_sleep();
         }
+        /* 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) {
+            jlink_state.pid_save_req = 0u;
+            bool save_ok = pid_flash_save(bal.kp, bal.ki, bal.kd);
+            jlink_tlm_pid_result_t pid_res;
+            pid_res.kp       = bal.kp;
+            pid_res.ki       = bal.ki;
+            pid_res.kd       = bal.kd;
+            pid_res.saved_ok = save_ok ? 1u : 0u;
+            jlink_send_pid_result(&pid_res);
+            printf("[PID] Flash save %s: Kp=%.3f Ki=%.3f Kd=%.3f\n",
+                   save_ok ? "OK" : "FAIL",
+                   (double)bal.kp, (double)bal.ki, (double)bal.kd);
+        }
 
         /* Power management: CRSF/JLink activity or armed state resets idle timer */
         if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) ||

src/pid_flash.c

@@ -0,0 +1,72 @@
+#include "pid_flash.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+bool pid_flash_load(float *kp, float *ki, float *kd)
+{
+    const pid_flash_t *p = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
+
+    if (p->magic != PID_FLASH_MAGIC) return false;
+
+    /* 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;
+
+    *kp = p->kp;
+    *ki = p->ki;
+    *kd = p->kd;
+    return true;
+}
+
+bool pid_flash_save(float kp, float ki, float kd)
+{
+    HAL_StatusTypeDef rc;
+
+    /* Unlock flash */
+    rc = HAL_FLASH_Unlock();
+    if (rc != HAL_OK) return false;
+
+    /* Erase sector 7 */
+    FLASH_EraseInitTypeDef erase = {
+        .TypeErase    = FLASH_TYPEERASE_SECTORS,
+        .Sector       = PID_FLASH_SECTOR,
+        .NbSectors    = 1,
+        .VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
+    };
+    uint32_t sector_error = 0;
+    rc = HAL_FLASHEx_Erase(&erase, &sector_error);
+    if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
+        HAL_FLASH_Lock();
+        return false;
+    }
+
+    /* Build record */
+    pid_flash_t rec;
+    memset(&rec, 0xFF, sizeof(rec));
+    rec.magic = PID_FLASH_MAGIC;
+    rec.kp    = kp;
+    rec.ki    = ki;
+    rec.kd    = kd;
+
+    /* 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++) {
+        rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, src[i]);
+        if (rc != HAL_OK) {
+            HAL_FLASH_Lock();
+            return false;
+        }
+        addr += 4u;
+    }
+
+    HAL_FLASH_Lock();
+
+    /* Verify readback */
+    const pid_flash_t *stored = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
+    return (stored->magic == PID_FLASH_MAGIC &&
+            stored->kp == kp &&
+            stored->ki == ki &&
+            stored->kd == kd);
+}

ui/social-bot/src/App.jsx

@@ -91,7 +91,17 @@ import { SaltyFace } from './components/SaltyFace.jsx';
 // Parameter server (issue #471)
 import { ParameterServer } from './components/ParameterServer.jsx';
 
+// Teleop web interface (issue #534)
+import { TeleopWebUI } from './components/TeleopWebUI.jsx';
+
 const TAB_GROUPS = [
+  {
+    label: 'TELEOP',
+    color: 'text-orange-600',
+    tabs: [
+      { id: 'teleop-webui', label: 'Drive' },
+    ],
+  },
   {
     label: 'DISPLAY',
     color: 'text-rose-600',
@@ -286,8 +296,10 @@ export default function App() {
       {/* ── Content ── */}
       <main className={`flex-1 ${
         activeTab === 'salty-face' ? '' :
-        ['eventlog', 'control', 'imu'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'
+        ['eventlog', 'control', 'imu', 'teleop-webui'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'
       } ${activeTab === 'salty-face' ? '' : 'p-4'}`}>
+        {activeTab === 'teleop-webui' && <TeleopWebUI subscribe={subscribe} publish={publishFn} />}
+
         {activeTab === 'salty-face'   && <SaltyFace      subscribe={subscribe} />}
 
         {activeTab === 'status'       && <StatusPanel     subscribe={subscribe} />}

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

@@ -0,0 +1,480 @@
+/**
+ * TeleopWebUI.jsx — Browser-based remote control with live video (Issue #534).
+ *
+ * Features:
+ *   - Live camera stream via rosbridge (sensor_msgs/CompressedImage)
+ *   - Camera selector: front / rear / left / right
+ *   - Virtual joystick (touch + mouse) for mobile-friendly control
+ *   - WASD / arrow-key keyboard fallback
+ *   - Speed presets: SLOW (20%), NORMAL (50%), FAST (100%)
+ *   - E-stop button (latching, prominent)
+ *   - Real-time linear/angular velocity display
+ *   - Mobile-responsive split layout
+ *
+ * Topics:
+ *   /camera/<name>/image_raw/compressed   sensor_msgs/CompressedImage  (subscribe)
+ *   /cmd_vel                              geometry_msgs/Twist           (publish)
+ */
+
+import { useEffect, useRef, useState, useCallback } from 'react';
+
+// ── Constants ─────────────────────────────────────────────────────────────────
+
+const MAX_LINEAR  = 0.5;   // m/s
+const MAX_ANGULAR = 1.0;   // rad/s
+const DEADZONE    = 0.10;  // 10 %
+const CMD_HZ      = 20;    // publish rate (ms)
+
+const SPEED_PRESETS = [
+  { label: 'SLOW',   value: 0.20, color: 'text-green-400',  border: 'border-green-800',  bg: 'bg-green-950'  },
+  { label: 'NORMAL', value: 0.50, color: 'text-amber-400',  border: 'border-amber-800',  bg: 'bg-amber-950'  },
+  { label: 'FAST',   value: 1.00, color: 'text-red-400',    border: 'border-red-800',    bg: 'bg-red-950'    },
+];
+
+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' },
+];
+
+// ── Utilities ─────────────────────────────────────────────────────────────────
+
+function applyDeadzone(value) {
+  const abs = Math.abs(value);
+  if (abs < DEADZONE) return 0;
+  return Math.sign(value) * ((abs - DEADZONE) / (1 - DEADZONE));
+}
+
+function clamp(v, lo, hi) {
+  return Math.max(lo, Math.min(hi, v));
+}
+
+// ── Virtual joystick ──────────────────────────────────────────────────────────
+
+function VirtualJoystick({ onMove }) {
+  const canvasRef = useRef(null);
+  const activeRef = useRef(false);
+  const stickRef  = useRef({ x: 0, y: 0 });
+
+  const draw = useCallback(() => {
+    const canvas = canvasRef.current;
+    if (!canvas) return;
+    const ctx = canvas.getContext('2d');
+    const W = canvas.width;
+    const H = canvas.height;
+    const cx = W / 2;
+    const cy = H / 2;
+    const R  = Math.min(W, H) * 0.38;
+    const kr = Math.min(W, H) * 0.14;
+
+    ctx.clearRect(0, 0, W, H);
+
+    // Base plate
+    ctx.fillStyle = 'rgba(15,23,42,0.85)';
+    ctx.beginPath();
+    ctx.arc(cx, cy, R + kr, 0, Math.PI * 2);
+    ctx.fill();
+
+    // Base ring
+    ctx.strokeStyle = '#1e3a5f';
+    ctx.lineWidth = 2;
+    ctx.beginPath();
+    ctx.arc(cx, cy, R, 0, Math.PI * 2);
+    ctx.stroke();
+
+    // Deadzone ring
+    ctx.strokeStyle = '#374151';
+    ctx.lineWidth = 1;
+    ctx.setLineDash([3, 3]);
+    ctx.beginPath();
+    ctx.arc(cx, cy, R * DEADZONE, 0, Math.PI * 2);
+    ctx.stroke();
+    ctx.setLineDash([]);
+
+    // Crosshair
+    ctx.strokeStyle = '#1e3a5f';
+    ctx.lineWidth = 1;
+    ctx.beginPath();
+    ctx.moveTo(cx - R * 0.3, cy); ctx.lineTo(cx + R * 0.3, cy);
+    ctx.moveTo(cx, cy - R * 0.3); ctx.lineTo(cx, cy + R * 0.3);
+    ctx.stroke();
+
+    // Knob
+    const kx = cx + stickRef.current.x * R;
+    const ky = cy - stickRef.current.y * R;
+    const active = activeRef.current;
+
+    const grad = ctx.createRadialGradient(kx - kr * 0.3, ky - kr * 0.3, 0, kx, ky, kr);
+    grad.addColorStop(0, active ? '#38bdf8' : '#1d4ed8');
+    grad.addColorStop(1, active ? '#0369a1' : '#1e3a8a');
+    ctx.fillStyle = grad;
+    ctx.beginPath();
+    ctx.arc(kx, ky, kr, 0, Math.PI * 2);
+    ctx.fill();
+
+    ctx.strokeStyle = active ? '#7dd3fc' : '#3b82f6';
+    ctx.lineWidth = 2;
+    ctx.beginPath();
+    ctx.arc(kx, ky, kr, 0, Math.PI * 2);
+    ctx.stroke();
+
+    // Vector line
+    const { x, y } = stickRef.current;
+    if (Math.abs(x) > 0.02 || Math.abs(y) > 0.02) {
+      ctx.strokeStyle = 'rgba(56,189,248,0.5)';
+      ctx.lineWidth = 2;
+      ctx.beginPath();
+      ctx.moveTo(cx, cy);
+      ctx.lineTo(kx, ky);
+      ctx.stroke();
+    }
+  }, []);
+
+  const getPos = (clientX, clientY) => {
+    const canvas = canvasRef.current;
+    const rect = canvas.getBoundingClientRect();
+    const R = Math.min(rect.width, rect.height) * 0.38;
+    const cx = rect.left + rect.width  / 2;
+    const cy = rect.top  + rect.height / 2;
+    let x = (clientX - cx) / R;
+    let y = (clientY - cy) / R;
+    const mag = Math.sqrt(x * x + y * y);
+    if (mag > 1) { x /= mag; y /= mag; }
+    return { x: clamp(x, -1, 1), y: clamp(-y, -1, 1) };
+  };
+
+  const onStart = useCallback((clientX, clientY) => {
+    activeRef.current = true;
+    const { x, y } = getPos(clientX, clientY);
+    stickRef.current = { x, y };
+    onMove({ x: applyDeadzone(x), y: applyDeadzone(y) });
+    draw();
+  }, [draw, onMove]);
+
+  const onContinue = useCallback((clientX, clientY) => {
+    if (!activeRef.current) return;
+    const { x, y } = getPos(clientX, clientY);
+    stickRef.current = { x, y };
+    onMove({ x: applyDeadzone(x), y: applyDeadzone(y) });
+    draw();
+  }, [draw, onMove]);
+
+  const onEnd = useCallback(() => {
+    activeRef.current = false;
+    stickRef.current = { x: 0, y: 0 };
+    onMove({ x: 0, y: 0 });
+    draw();
+  }, [draw, onMove]);
+
+  // Mount: draw once + attach pointer events
+  useEffect(() => {
+    draw();
+    const canvas = canvasRef.current;
+    if (!canvas) return;
+
+    const onPD = (e) => { e.preventDefault(); onStart(e.clientX, e.clientY); };
+    const onPM = (e) => { e.preventDefault(); onContinue(e.clientX, e.clientY); };
+    const onPU = (e) => { e.preventDefault(); onEnd(); };
+
+    const onTD = (e) => { e.preventDefault(); const t = e.touches[0]; onStart(t.clientX, t.clientY); };
+    const onTM = (e) => { e.preventDefault(); const t = e.touches[0]; onContinue(t.clientX, t.clientY); };
+    const onTE = (e) => { e.preventDefault(); onEnd(); };
+
+    canvas.addEventListener('pointerdown', onPD);
+    window.addEventListener('pointermove', onPM);
+    window.addEventListener('pointerup',   onPU);
+    canvas.addEventListener('touchstart',  onTD, { passive: false });
+    canvas.addEventListener('touchmove',   onTM, { passive: false });
+    canvas.addEventListener('touchend',    onTE, { passive: false });
+
+    return () => {
+      canvas.removeEventListener('pointerdown', onPD);
+      window.removeEventListener('pointermove', onPM);
+      window.removeEventListener('pointerup',   onPU);
+      canvas.removeEventListener('touchstart',  onTD);
+      canvas.removeEventListener('touchmove',   onTM);
+      canvas.removeEventListener('touchend',    onTE);
+    };
+  }, [onStart, onContinue, onEnd, draw]);
+
+  return (
+    <canvas
+      ref={canvasRef}
+      width={200}
+      height={200}
+      className="rounded-full cursor-grab active:cursor-grabbing select-none"
+      style={{ touchAction: 'none' }}
+    />
+  );
+}
+
+// ── Camera feed ───────────────────────────────────────────────────────────────
+
+function CameraFeed({ subscribe, topic }) {
+  const [src, setSrc]       = useState(null);
+  const [fps, setFps]       = useState(0);
+  const frameCountRef       = useRef(0);
+  const lastFpsTimeRef      = useRef(Date.now());
+
+  useEffect(() => {
+    setSrc(null);
+    frameCountRef.current  = 0;
+    lastFpsTimeRef.current = Date.now();
+
+    const unsub = subscribe(topic, 'sensor_msgs/CompressedImage', (msg) => {
+      const fmt    = msg.format?.includes('png') ? 'png' : 'jpeg';
+      const dataUrl = `data:image/${fmt};base64,${msg.data}`;
+      setSrc(dataUrl);
+
+      frameCountRef.current++;
+      const now = Date.now();
+      const dt  = now - lastFpsTimeRef.current;
+      if (dt >= 1000) {
+        setFps(Math.round((frameCountRef.current * 1000) / dt));
+        frameCountRef.current  = 0;
+        lastFpsTimeRef.current = now;
+      }
+    });
+
+    return unsub;
+  }, [subscribe, topic]);
+
+  return (
+    <div className="relative w-full h-full flex items-center justify-center bg-black rounded-lg overflow-hidden">
+      {src ? (
+        <img
+          src={src}
+          alt="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-3xl">📷</div>
+          <div className="text-xs tracking-widest">NO SIGNAL</div>
+        </div>
+      )}
+      {/* FPS badge */}
+      {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 TeleopWebUI({ subscribe, publish }) {
+  const [selectedCam, setSelectedCam] = useState('front');
+  const [speedPreset, setSpeedPreset] = useState(0.50);
+  const [isEstopped,  setIsEstopped]  = useState(false);
+  const [linearVel,   setLinearVel]   = useState(0);
+  const [angularVel,  setAngularVel]  = useState(0);
+
+  // Joystick normalized input ref [-1,1] each axis
+  const joystickRef = useRef({ x: 0, y: 0 });
+  // Keyboard pressed keys
+  const keysRef = useRef({});
+
+  const currentCam = CAMERAS.find(c => c.id === selectedCam);
+
+  // ── Keyboard input ──────────────────────────────────────────────────────────
+  useEffect(() => {
+    const down = (e) => {
+      const k = e.key.toLowerCase();
+      if (['w','a','s','d','arrowup','arrowdown','arrowleft','arrowright',' '].includes(k)) {
+        keysRef.current[k] = true;
+        e.preventDefault();
+      }
+    };
+    const up = (e) => {
+      const k = e.key.toLowerCase();
+      keysRef.current[k] = false;
+    };
+    window.addEventListener('keydown', down);
+    window.addEventListener('keyup',   up);
+    return () => {
+      window.removeEventListener('keydown', down);
+      window.removeEventListener('keyup',   up);
+    };
+  }, []);
+
+  // ── Publish loop ────────────────────────────────────────────────────────────
+  useEffect(() => {
+    const timer = setInterval(() => {
+      const k = keysRef.current;
+      const j = joystickRef.current;
+
+      // Keyboard overrides joystick if any key pressed
+      let lin = j.y;
+      let ang = -j.x;   // right stick-x → negative angular (turn left on left)
+
+      const anyKey = k['w'] || k['s'] || k['a'] || k['d'] ||
+                     k['arrowup'] || k['arrowdown'] || k['arrowleft'] || k['arrowright'];
+
+      if (anyKey) {
+        lin = 0;
+        ang = 0;
+        if (k['w'] || k['arrowup'])    lin =  1;
+        if (k['s'] || k['arrowdown'])  lin = -1;
+        if (k['a'] || k['arrowleft'])  ang =  1;
+        if (k['d'] || k['arrowright']) ang = -1;
+      }
+
+      // Space = quick stop (doesn't latch e-stop)
+      if (k[' ']) { lin = 0; ang = 0; }
+
+      const factor = isEstopped ? 0 : speedPreset;
+      const finalLin = clamp(lin * MAX_LINEAR  * factor, -MAX_LINEAR,  MAX_LINEAR);
+      const finalAng = clamp(ang * MAX_ANGULAR * factor, -MAX_ANGULAR, MAX_ANGULAR);
+
+      setLinearVel(finalLin);
+      setAngularVel(finalAng);
+
+      if (publish) {
+        publish('/cmd_vel', 'geometry_msgs/Twist', {
+          linear:  { x: finalLin, y: 0, z: 0 },
+          angular: { x: 0, y: 0, z: finalAng },
+        });
+      }
+    }, CMD_HZ);
+
+    return () => clearInterval(timer);
+  }, [isEstopped, speedPreset, publish]);
+
+  // ── E-stop ──────────────────────────────────────────────────────────────────
+  const handleEstop = () => {
+    const next = !isEstopped;
+    setIsEstopped(next);
+    if (publish) {
+      publish('/cmd_vel', 'geometry_msgs/Twist', {
+        linear:  { x: 0, y: 0, z: 0 },
+        angular: { x: 0, y: 0, z: 0 },
+      });
+    }
+  };
+
+  // Joystick callback
+  const handleJoystick = useCallback((pos) => {
+    joystickRef.current = pos;
+  }, []);
+
+  // ── Render ──────────────────────────────────────────────────────────────────
+  return (
+    <div className="flex flex-col lg:flex-row h-full gap-3">
+
+      {/* ── Left: Camera feed ── */}
+      <div className="flex flex-col gap-2 flex-1 min-h-0">
+
+        {/* Camera selector */}
+        <div className="flex gap-1 shrink-0">
+          {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 font-mono self-center">
+            {currentCam?.topic}
+          </div>
+        </div>
+
+        {/* Video feed */}
+        <div className="flex-1 min-h-[200px] lg:min-h-0">
+          <CameraFeed
+            subscribe={subscribe}
+            topic={currentCam?.topic}
+          />
+        </div>
+      </div>
+
+      {/* ── Right: Controls ── */}
+      <div className="flex flex-col gap-3 w-full lg:w-72 shrink-0">
+
+        {/* E-stop */}
+        <button
+          onClick={handleEstop}
+          className={`w-full py-4 text-sm font-black tracking-widest rounded-lg border-2 transition-all ${
+            isEstopped
+              ? 'bg-red-900 border-red-500 text-red-200 animate-pulse'
+              : 'bg-red-950 border-red-800 text-red-400 hover:bg-red-900 hover:border-red-600'
+          }`}
+        >
+          {isEstopped ? '🛑 E-STOP — TAP TO RESUME' : '⚡ E-STOP'}
+        </button>
+
+        {/* Speed 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">SPEED PRESET</div>
+          <div className="grid grid-cols-3 gap-2">
+            {SPEED_PRESETS.map((p) => (
+              <button
+                key={p.label}
+                onClick={() => setSpeedPreset(p.value)}
+                disabled={isEstopped}
+                className={`py-2 text-xs font-bold rounded border tracking-widest transition-colors ${
+                  speedPreset === p.value && !isEstopped
+                    ? `${p.bg} ${p.border} ${p.color}`
+                    : 'bg-gray-900 border-gray-800 text-gray-600 hover:text-gray-300 disabled:opacity-40 disabled:cursor-not-allowed'
+                }`}
+              >
+                {p.label}
+                <div className="text-gray-500 font-normal mt-0.5">{(p.value * 100).toFixed(0)}%</div>
+              </button>
+            ))}
+          </div>
+        </div>
+
+        {/* Velocity display */}
+        <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">LINEAR</div>
+            <div className={`text-xl font-mono ${isEstopped ? 'text-gray-700' : 'text-cyan-300'}`}>
+              {linearVel.toFixed(2)}
+            </div>
+            <div className="text-gray-700 text-xs">m/s</div>
+          </div>
+          <div>
+            <div className="text-gray-600 text-xs mb-1">ANGULAR</div>
+            <div className={`text-xl font-mono ${isEstopped ? 'text-gray-700' : 'text-amber-300'}`}>
+              {angularVel.toFixed(2)}
+            </div>
+            <div className="text-gray-700 text-xs">rad/s</div>
+          </div>
+        </div>
+
+        {/* Joystick */}
+        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 flex flex-col items-center gap-2">
+          <div className="text-xs font-bold tracking-widest text-cyan-700">JOYSTICK</div>
+          <VirtualJoystick onMove={handleJoystick} />
+          <div className="text-xs text-gray-700 text-center">
+            drag or use W A S D / arrow keys
+          </div>
+        </div>
+
+        {/* Key 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">KEYBOARD</div>
+          <div className="grid grid-cols-2 gap-x-3">
+            <div><span className="text-gray-500">W/↑</span> Forward</div>
+            <div><span className="text-gray-500">S/↓</span> Backward</div>
+            <div><span className="text-gray-500">A/←</span> Turn Left</div>
+            <div><span className="text-gray-500">D/→</span> Turn Right</div>
+            <div className="col-span-2"><span className="text-gray-500">Space</span> Quick stop</div>
+          </div>
+        </div>
+      </div>
+    </div>
+  );
+}