feat(mechanical): modular payload bay system (Issue #170 )

Dovetail rail + tool-free swappable payload modules for all variants: - payload_bay_rail.scad: 50×12 mm 60° dovetail rail (DXF for CNC Al bar), spring ball detent (Ø6 mm, 50 mm pitch), continuous safety-lock groove (M4 thumbscrew), 4-pin pogo connector housing (GND/5V/12V/UART), lab/rover/tank deck adapter plates - payload_bay_modules.scad: universal _module_base() (male tongue, detent bore, 4× Ø4 mm target pads, lock bore) + 3 example modules: cargo tray (200×100 mm, Velcro slots, bungee cord slots), camera boom (120 mm mast + 80 mm arm, 2020-rail-compatible head, 3-position tilt), cup holder (Ø80 mm tapered, 8-slot flex grip). Includes copy-paste module template. - payload_bay_BOM.md: hardware list, CNC spec (dovetail dimensions, surface finish, connector pocket), load analysis (2 kg rated with Al rail + lock), module developer guide with constraints table Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
Merge pull request 'feat(jetson): night vision mode — IR emitter, headlight, ambient-light FSM, IR SLAM bridge (#168 )' (#175 ) from sl-perception/issue-168-night-vision into main
2026-03-02 10:38:07 -05:00 · 2026-03-02 10:36:55 -05:00 · 2026-03-02 10:36:36 -05:00 · 2026-03-02 10:36:21 -05:00 · 2026-03-02 10:34:35 -05:00 · 2026-03-02 10:34:20 -05:00
25 changed files with 3194 additions and 3 deletions
--- a/chassis/payload_bay_BOM.md
+++ b/chassis/payload_bay_BOM.md
@ -0,0 +1,165 @@
 # Payload Bay BOM — Issue #170
 **Agent:** sl-mechanical | **Date:** 2026-03-01
 Modular dovetail payload rail system. Tool-free slide-and-click module swapping.
 Cross-variant: SaltyLab, SaltyRover, SaltyTank (same rail profile).
 ---
 ## A. Rail Hardware
 | # | Description | Spec | Qty (per robot) | Notes |
 |---|-------------|------|-----------------|-------|
 | R1 | Aluminium bar stock | 50×12 mm, 6061-T6, 200 mm length | 1–2 | Preferred over printed rail for 2 kg load rating. CNC mill or route dovetail slot per `payload_rail.dxf` profile. |
 | R2 | M4×10 FHCS | Stainless, countersunk | 4–8 | Rail to adapter plate (or direct to deck); FHCS sits flush below rail bottom face |
 | R3 | M4 heat-set insert | M4×5.7 L, Ø5.6 OD | 4–8 | Into deck adapter plate |
 | R4 | Detent ball bearing | Ø6 mm, chrome steel (GCr15) | 2 per module | Module spring detent; standard bearing ball |
 | R5 | Detent spring | Ø5.5 mm OD, 12 mm free length, ~2 N/mm | 2 per module | Lee Spring LC 055A 06 S or equivalent; behind ball in plunger |
 | R6 | M4 thumbscrew (knurled) | M4×12, knurled head Ø14 mm | 1 per module | Safety lock; threads into M4 nut pressed into module side |
 | R7 | M4 hex nut | DIN 934, stainless | 1 per module | Captured in module body for thumbscrew |
 ## B. Power + Data Connector
 | # | Description | Spec | Qty (per rail) | Notes |
 |---|-------------|------|----------------|-------|
 | C1 | Pogo pin | P75-E2 style, Ø2 mm, 6 mm travel, rated 2 A | 4 | Rail-side spring contacts. AliExpress "P75-E2 pogo pin" or Mill-Max 0906 series. |
 | C2 | Brass target pad | Ø4 × 1.5 mm disc | 4 per module | Module-side contact pads. Machine from Ø4 mm brass rod or order PCB pads. Press-fit with Loctite 603. |
 | C3 | JST-XH 2.54 mm header | 4-pin, right-angle | 1 per rail | Rail-side connector to power harness |
 | C4 | JST-XH housing + crimps | 4-pin female | 1 per robot | Wires from robot PSU (5 V, 12 V, GND, UART) |
 | C5 | 20 AWG silicone wire | Red / black / yellow / white, 300 mm each | 4 | Rail connector to robot bus |
 | C6 | Connector housing | `payload_connector_stl` | 1 | Press-fit into rail pocket |
 ### Pin Map
 | Pin | Signal | Wire colour | Max current |
 |-----|--------|-------------|-------------|
 | 1 | GND | Black | Return |
 | 2 | +5 V | Red | 2 A |
 | 3 | +12 V | Yellow | 2 A |
 | 4 | UART (3.3 V) | White | 0.5 A |
 > **UART note**: Half-duplex (single wire). Module firmware connects to Jetson Orin NX UART2. Use RS-485 transceiver if module cable > 500 mm or multi-drop needed.
 ## C. Deck Adapters
 | Part | File | Qty | Print | Mass est. |
 |------|------|-----|-------|-----------|
 | SaltyLab adapter | `payload_bay_rail.scad` `lab_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~30 g |
 | SaltyRover adapter | `payload_bay_rail.scad` `rover_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~35 g |
 | SaltyTank adapter | `payload_bay_rail.scad` `tank_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~35 g |
 ## D. Printed Parts
 | Part | File | Qty | Print | Mass est. |
 |------|------|-----|-------|-----------|
 | Rail section (prototype) | `payload_bay_rail.scad` `rail_stl` | 1 | PETG, 5 perims, 60% infill, 0.2 mm layer | ~85 g |
 | Connector housing | `payload_bay_rail.scad` `connector_stl` | 1 | PETG, 5 perims, 100% infill | ~4 g |
 | Detent plunger | `payload_bay_rail.scad` `detent_plunger_stl` | 2 per module | PETG, 5 perims, 80% infill | ~2 g each |
 | Module base (universal) | `payload_bay_modules.scad` `base_stl` | N | PETG, 5 perims, 60% infill | ~18 g |
 | Cargo tray (200 mm) | `payload_bay_modules.scad` `cargo_tray_stl` | 1 | PETG, 4 perims, 30% infill | ~180 g |
 | Camera boom | `payload_bay_modules.scad` `camera_boom_stl` | 1 | PETG, 5 perims, 50% infill | ~95 g |
 | Cup holder | `payload_bay_modules.scad` `cup_holder_stl` | 1 | PETG, 4 perims, 25% infill | ~55 g |
 ---
 ## Dovetail Rail — CNC Specification
 For aluminium production rail (preferred over printed for 2 kg rating):
 ```
 Material: 6061-T6 aluminium
 Stock: 50 mm × 12 mm flat bar, length to suit (200 mm, 300 mm, 400 mm)
 Dovetail slot (top face, centred):
  Slot open width at top:    37.2 mm
  Slot width at bottom:      28.0 mm
  Slot depth:                 8.0 mm
  Wall angle from vertical:  30.0° (60° included angle)
  Surface finish: Ra 1.6 µm (smooth for low-friction sliding)
 Detent dimples (slot floor):
  Diameter: 4.9 mm (ball seats in)
  Depth: 1.5 mm
  Pitch: 50 mm
  First dimple: 25 mm from each end
 Safety-lock groove (both side faces, continuous):
  Groove diameter: 4.5 mm
  Depth: 1.5 mm
  Z position: RAIL_T/2 - DOVE_H/2 = 8 mm from top face
  (CNC with Ø4 mm ball-nose end mill, single pass at Z = -4 mm from top)
 Mounting holes (bottom face, countersunk):
  Diameter: 4.3 mm (M4 clearance)
  C/sink: Ø8 mm × 82° (M4 FHCS)
  Pitch: 50 mm
  First hole: 25 mm from each end
 Connector pocket (slot floor, centred in rail length):
  Width: 26 mm (X), Depth: 8.4 mm (Y), Height: 7 mm (Z into slot floor)
  Tolerance: +0.2 / 0 mm (press-fit housing)
 DXF cross-section: export payload_rail.dxf for supplier drawing.
 ```
 ---
 ## Load Analysis
 | Mode | Load | Safety factor | Method |
 |------|------|---------------|--------|
 | Static payload (detent only) | 0.5 kg | 2× | Ball detent retention force ~10 N |
 | Static payload (thumbscrew locked) | 2.0 kg | 2× | Dovetail shear area ~800 mm² Al |
 | Dynamic (robot motion, 2 m/s²) | 2.0 kg | 1.5× | Inertial force = 2 kg × 2 m/s² = 4 N; detent holds 10 N |
 | Dovetail shear (PETG printed) | 1.2 kg | 1.5× | PETG tensile ~50 MPa; recommend Al rail for rated 2 kg |
 > **⚠ For 2 kg payload: use machined aluminium rail. Printed PETG rail is prototype/light-duty only (<0.8 kg payload).**
 ---
 ## Module Developer Guide
 ### Adding a new module in 5 steps
 1. **Copy the template** at the bottom of `payload_bay_modules.scad`.
 2. **Set `MY_LEN`** — must be a multiple of 50 mm (detent pitch) for repeatable positioning.
 3. **Call `_module_base(MY_LEN, n_detents)`** as the first statement in your module.
 4. **Build payload geometry** starting at `Z = 0` (rail top face). Keep total height ≤ 200 mm for robot clearance under doorways.
 5. **Verify connector alignment** — when module is slid to its operating position, the 4 target pads on the tongue bottom must align with `CONN_Y` on the rail (default: 100 mm from rail entry end). Adjust `conn_offset` if needed.
 ### Constraints
 | Parameter | Limit |
 |-----------|-------|
 | Module length | Min 60 mm, max 400 mm |
 | Module height above rail | Max 200 mm (clearance) |
 | Payload mass | ≤ 2 kg (Al rail + thumbscrew locked) |
 | Module width | Max 120 mm (robot shoulder clearance) |
 | Connector draw | Max 2 A per power pin (5 V or 12 V) |
 ---
 ## Export Commands
 ```bash
 # Rail DXF (for CNC / waterjet machining)
 openscad payload_bay_rail.scad -D 'RENDER="rail_2d"'            -o payload_rail_profile.dxf
 # Rail STL (PETG prototype)
 openscad payload_bay_rail.scad -D 'RENDER="rail_stl"'           -o payload_rail_200mm.stl
 # Rail accessories
 openscad payload_bay_rail.scad -D 'RENDER="connector_stl"'      -o payload_connector.stl
 openscad payload_bay_rail.scad -D 'RENDER="detent_plunger_stl"' -o payload_detent_plunger.stl
 # Deck adapters
 openscad payload_bay_rail.scad -D 'RENDER="lab_adapter_stl"'    -o payload_adapter_lab.stl
 openscad payload_bay_rail.scad -D 'RENDER="rover_adapter_stl"'  -o payload_adapter_rover.stl
 openscad payload_bay_rail.scad -D 'RENDER="tank_adapter_stl"'   -o payload_adapter_tank.stl
 # Example modules
 openscad payload_bay_modules.scad -D 'RENDER="cargo_tray_stl"'  -o payload_cargo_tray.stl
 openscad payload_bay_modules.scad -D 'RENDER="camera_boom_stl"' -o payload_camera_boom.stl
 openscad payload_bay_modules.scad -D 'RENDER="cup_holder_stl"'  -o payload_cup_holder.stl
 openscad payload_bay_modules.scad -D 'RENDER="target_pad_2d"'   -o payload_target_pad.dxf
 ```
--- a/chassis/payload_bay_modules.scad
+++ b/chassis/payload_bay_modules.scad
@ -0,0 +1,462 @@
 // ============================================================
 // payload_bay_modules.scad — Payload Bay Module Template + Examples
 // Issue: #170  Agent: sl-mechanical  Date: 2026-03-01
 // ============================================================
 //
 // ── HOW TO CREATE A NEW MODULE ──────────────────────────────────────────────
 //
 //   1. Copy the "MODULE TEMPLATE" section at the bottom of this file.
 //   2. Set MODULE_L to your module's Y length (min 60 mm).
 //   3. Add your payload geometry on top of the _module_base() call.
 //   4. The _module_base() provides:
 //        • Male dovetail tongue (slides into rail slot)
 //        • Spring detent bore(s) (for Ø6 mm ball + spring plunger)
 //        • Connector target pads (4× Ø4 mm brass, matching rail pogo pins)
 //        • Safety-lock M4 thumbscrew bore (side of tongue)
 //        • Bottom-face flush with rail top (Z = 0 at rail top / module base)
 //   5. Your payload geometry sits at Z ≥ 0 (above the rail top face).
 //   6. Add a RENDER dispatch entry and export command.
 //
 // ── DOVETAIL TONGUE GEOMETRY ────────────────────────────────────────────────
 //
 //   TONGUE_BOT = DOVE_SLOT_BOT - 2*DOVE_CLEAR  = 28.0 - 0.6 = 27.4 mm
 //   TONGUE_TOP = DOVE_SLOT_TOP + 2*DOVE_CLEAR  = 37.2 + 0.6 = 37.8 mm
 //   TONGUE_H   = DOVE_H + 0.2  (slight extra depth, no binding at corners)
 //
 //   Tongue runs full module length (-Y to +Y in module coords).
 //   Module body sits on top of tongue at Z = 0.
 //
 // ── CONNECTOR PADS ──────────────────────────────────────────────────────────
 //   4× Ø4 mm brass discs press-fit into tongue bottom face.
 //   Pad positions: must align with rail connector at CONN_Y when module
 //   is slid to its intended position (any detent step).
 //   Default: pads centred in module length → module must be placed so its
 //   centre aligns with CONN_Y on rail.  Or: set MODULE_CONN_OFFSET to shift.
 //
 // ── PAYLOAD RATING ──────────────────────────────────────────────────────────
 //   2 kg rated payload when safety-lock thumbscrew is tightened.
 //   Detent-only (no thumbscrew): ~0.5 kg (impact / vibration condition).
 //
 // ── RENDERED EXAMPLES ────────────────────────────────────────────────────────
 //   Part 1 — cargo_tray()     200×100 mm cargo tray, 30 mm walls
 //   Part 2 — camera_boom()    L-arm with sensor_rail-compatible head
 //   Part 3 — cup_holder()     Ø80 mm tapered cup cradle
 //
 // RENDER options:
 //   "assembly"          all 3 modules on ghost rail
 //   "base_stl"          module base / tongue only (universal)
 //   "cargo_tray_stl"    cargo tray module
 //   "camera_boom_stl"   camera boom module
 //   "cup_holder_stl"    cup holder module
 //   "target_pad_2d"     DXF — Ø4 mm brass target pad profile
 //
 // Export:
 //   openscad payload_bay_modules.scad -D 'RENDER="base_stl"'        -o payload_base.stl
 //   openscad payload_bay_modules.scad -D 'RENDER="cargo_tray_stl"'  -o payload_cargo_tray.stl
 //   openscad payload_bay_modules.scad -D 'RENDER="camera_boom_stl"' -o payload_camera_boom.stl
 //   openscad payload_bay_modules.scad -D 'RENDER="cup_holder_stl"'  -o payload_cup_holder.stl
 // ============================================================
 $fn = 64;
 e   = 0.01;
 // ── Rail geometry constants (must match payload_bay_rail.scad) ────────────────
 RAIL_W         = 50.0;
 RAIL_T         = 12.0;
 DOVE_ANGLE     = 30.0;
 DOVE_H         =  8.0;
 DOVE_SLOT_BOT  = 28.0;
 DOVE_SLOT_TOP  = DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE);
 DOVE_CLEAR     =  0.3;
 DETENT_PITCH   = 50.0;
 DETENT_BALL_D  =  6.0;
 DETENT_SPG_OD  =  6.2;
 DETENT_SPG_L   = 16.0;
 CONN_PIN_SPC   =  5.0;
 CONN_N_PINS    =  4;
 CONN_HOUSING_D =  8.0;
 // ── Module tongue (male dovetail) geometry ────────────────────────────────────
 TONGUE_BOT     = DOVE_SLOT_BOT - 2*DOVE_CLEAR;  // 27.4 mm
 TONGUE_TOP     = DOVE_SLOT_TOP + 2*DOVE_CLEAR;  // 37.8 mm
 TONGUE_H       = DOVE_H + 0.2;                  // 8.2 mm (slight extra depth)
 // ── Connector target pad ──────────────────────────────────────────────────────
 TARGET_PAD_D   =  4.0;    // brass pad OD (slightly larger than pogo Ø2 mm)
 TARGET_PAD_T   =  1.5;    // brass pad thickness
 TARGET_PAD_RECESS = 1.3;  // press-fit recess depth (pad is 0.2 mm proud)
 // 4 pads at CONN_PIN_SPC pitch, centred in module tongue
 CONN_SPAN      = (CONN_N_PINS - 1) * CONN_PIN_SPC;  // 15 mm
 // ── Module safety lock ────────────────────────────────────────────────────────
 LOCK_BOLT_D    =  4.3;    // M4 thumbscrew bore through tongue side
 LOCK_BOLT_Z    =  TONGUE_H/2; // Z of thumbscrew CL from tongue bottom
 // Thumbscrew tightens against continuous groove in rail side.
 // Fasteners
 M3_D = 3.3;
 M4_D = 4.3;
 // ============================================================
 // RENDER DISPATCH
 // ============================================================
 RENDER = "assembly";
 if      (RENDER == "assembly")        assembly();
 else if (RENDER == "base_stl")        _module_base(80);
 else if (RENDER == "cargo_tray_stl")  cargo_tray();
 else if (RENDER == "camera_boom_stl") camera_boom();
 else if (RENDER == "cup_holder_stl")  cup_holder();
 else if (RENDER == "target_pad_2d") {
    projection(cut = true) translate([0, 0, -0.5])
        linear_extrude(1) circle(d = TARGET_PAD_D);
 }
 // ============================================================
 // ASSEMBLY PREVIEW
 // ============================================================
 module assembly() {
    // Ghost rail
    %color("Silver", 0.25)
        translate([0, 0, -RAIL_T])
            cube([RAIL_W, 600, RAIL_T], center = true);
    // Cargo tray at Y=50 (first detent position)
    color("OliveDrab", 0.85)
        translate([0, 50, 0])
            cargo_tray();
    // Cup holder at Y=300
    color("RoyalBlue", 0.85)
        translate([0, 300, 0])
            cup_holder();
    // Camera boom at Y=500
    color("DarkSlateGray", 0.85)
        translate([0, 500, 0])
            camera_boom();
 }
 // ============================================================
 // UNIVERSAL MODULE BASE  (_module_base)
 // ============================================================
 // All modules use this as their foundation.
 // Provides: male dovetail tongue, detent bore, connector pads, lock bore.
 //
 // module_len : module length in Y (≥ 60 mm)
 // n_detents  : how many ball detent bores to include (1 or 2)
 // conn_offset: Y offset of connector pads from module centre (default 0)
 //
 // After calling _module_base(), add payload geometry at Z = 0 and above.
 // The tongue occupies Z = -(TONGUE_H) to Z = 0.
 // Rail top face is at Z = 0.
 module _module_base(module_len, n_detents = 1, conn_offset = 0) {
    ml  = module_len;
    difference() {
        // ── Dovetail tongue (male) ────────────────────────────────────
        translate([0, ml/2, -TONGUE_H/2])
            linear_extrude(TONGUE_H, center = true, twist = 0,
                           convexity = 2)
                _tongue_profile_2d();
        // ── Spring detent bore(s) (up through tongue top face) ────────
        // 1 detent: at module centre; 2 detents: at ±25 mm from centre
        for (i = [0 : n_detents - 1]) {
            dy = (n_detents == 1)
                    ? ml/2
                    : ml/2 + (i - (n_detents-1)/2) * DETENT_PITCH;
            translate([0, dy, -(TONGUE_H/2) - DETENT_SPG_L/2])
                cylinder(d = DETENT_SPG_OD,
                         h = DETENT_SPG_L + TONGUE_H/2 + e);
        }
        // ── Connector target pad recesses (tongue bottom face) ────────
        for (i = [0 : CONN_N_PINS - 1]) {
            px = (i - (CONN_N_PINS-1)/2) * CONN_PIN_SPC;
            translate([px,
                       ml/2 + conn_offset,
                       -TONGUE_H + e])
                cylinder(d = TARGET_PAD_D + 0.1,
                         h = TARGET_PAD_RECESS + e);
        }
        // ── Safety-lock M4 thumbscrew bore (right side of tongue) ─────
        // Bore exits tongue right face, tip bears on rail side groove
        translate([TONGUE_TOP/2 + e, ml/2, LOCK_BOLT_Z - TONGUE_H])
            rotate([0, 90, 0])
                cylinder(d = LOCK_BOLT_D,
                         h = (TONGUE_TOP - TONGUE_BOT)/2 + 6 + e);
        // ── Lead-in chamfer on entry end of tongue ────────────────────
        // 2 mm chamfer on bottom corners of tongue at Y=0 end
        translate([0, 0, -TONGUE_H])
            rotate([45, 0, 0])
                cube([TONGUE_TOP + 2*e, 4, 4], center = true);
    }
 }
 // ── Tongue 2D cross-section (male dovetail, trapezoid wider at bottom) ────────
 // Module tongue: narrower at top (entering slot), wider at bottom (interlocking).
 // Note orientation: tongue points DOWN (-Z), so wider face is at bottom (-Z).
 module _tongue_profile_2d() {
    polygon([
        [-TONGUE_TOP/2,  0],         // top-left  (at Z = 0, flush with rail top)
        [ TONGUE_TOP/2,  0],         // top-right
        [ TONGUE_BOT/2, -TONGUE_H],  // bottom-right (interlocking face)
        [-TONGUE_BOT/2, -TONGUE_H],  // bottom-left
    ]);
 }
 // ============================================================
 // PART 1 — CARGO TRAY
 // ============================================================
 // 200×100 mm open tray for transporting small items.
 // 30 mm walls, chamfered rim, 4× drainage holes.
 // Two Velcro strip slots on tray floor for cargo retention.
 // Module length: 200 mm (4 detent positions).
 // Weight budget: tray printed ~180 g; payload up to 2 kg.
 TRAY_L       = 200.0;   // module Y length
 TRAY_W       = 100.0;   // tray interior width (X)
 TRAY_WALL    =   3.0;   // tray wall thickness
 TRAY_H       =  30.0;   // tray wall height above module base
 TRAY_FLOOR_T =   3.0;   // tray floor thickness
 module cargo_tray() {
    // Mount base on rail (2 detents at ±50 mm from module centre)
    _module_base(TRAY_L, n_detents = 2);
    difference() {
        union() {
            // ── Tray body on top of base ────────────────────────────
            translate([-TRAY_W/2 - TRAY_WALL, 0,
                       0])
                cube([TRAY_W + 2*TRAY_WALL,
                      TRAY_L,
                      TRAY_FLOOR_T + TRAY_H]);
            // ── Corner gussets (stiffening for 2 kg load) ───────────
            for (cx = [-1, 1]) for (cy = [0, 1])
                hull() {
                    translate([cx * TRAY_W/2,
                               cy * (TRAY_L - TRAY_WALL),
                               0])
                        cube([TRAY_WALL + e, TRAY_WALL + e,
                              TRAY_FLOOR_T + TRAY_H * 0.6], center = true);
                    translate([cx * (TRAY_W/2 - 10),
                               cy * (TRAY_L - TRAY_WALL),
                               0])
                        cylinder(d = TRAY_WALL * 2, h = e);
                }
        }
        // ── Tray interior cavity ─────────────────────────────────────
        translate([-TRAY_W/2, TRAY_WALL,
                   TRAY_FLOOR_T])
            cube([TRAY_W, TRAY_L - 2*TRAY_WALL,
                  TRAY_H + e]);
        // ── Drainage holes (4× Ø8 mm in floor) ──────────────────────
        for (dx = [-TRAY_W/4, TRAY_W/4])
        for (dy = [TRAY_L/4, 3*TRAY_L/4])
            translate([dx, dy, -e])
                cylinder(d = 8, h = TRAY_FLOOR_T + 2*e);
        // ── Velcro slot × 2 (25 mm wide grooves in floor) ────────────
        for (dy = [TRAY_L/3, 2*TRAY_L/3])
            translate([0, dy, TRAY_FLOOR_T - 1.5])
                cube([TRAY_W - 10, 25, 2 + e], center = true);
        // ── Rim chamfer (top inner edge, ergonomic) ───────────────────
        translate([0, TRAY_L/2, TRAY_FLOOR_T + TRAY_H + e])
            cube([TRAY_W + 2*TRAY_WALL + 2*e,
                  TRAY_L + 2*e, 4], center = true);
        // ── Side slots for bungee cord retention (3× each long side) ─
        for (sx = [-1, 1])
        for (sy = [TRAY_L/4, TRAY_L/2, 3*TRAY_L/4])
            translate([sx * (TRAY_W/2 + TRAY_WALL/2),
                       sy, TRAY_FLOOR_T + TRAY_H/2])
                cube([TRAY_WALL + 2*e, 8, 6], center = true);
    }
 }
 // ============================================================
 // PART 2 — CAMERA BOOM
 // ============================================================
 // L-shaped arm: vertical mast + horizontal boom.
 // Boom head accepts sensor_rail 2020 T-slot (RAIL_W/2 bolt pattern).
 // Sensor head can be rotated 0/90/180° and locked with M4 bolt.
 // Module length: 80 mm; arm rises 120 mm, boom extends 80 mm forward.
 BOOM_MODULE_L  =  80.0;
 BOOM_MAST_H    = 120.0;   // mast height above rail top (Z)
 BOOM_ARM_L     =  80.0;   // horizontal boom length (+Y forward)
 BOOM_ARM_W     =  20.0;   // arm cross-section width
 BOOM_ARM_T     =  20.0;   // arm cross-section height
 BOOM_HEAD_W    =  50.0;   // sensor head width (matches 2020 rail flange)
 BOOM_HEAD_H    =  20.0;   // sensor head plate height
 BOOM_HEAD_T    =   5.0;   // sensor head plate thickness
 module camera_boom() {
    _module_base(BOOM_MODULE_L, n_detents = 1);
    difference() {
        union() {
            // ── Mast (vertical column from module body) ──────────────
            translate([-BOOM_ARM_W/2, BOOM_MODULE_L/2 - BOOM_ARM_T/2, 0])
                cube([BOOM_ARM_W, BOOM_ARM_T, BOOM_MAST_H]);
            // ── Horizontal boom (from mast top, extends +Y) ──────────
            translate([-BOOM_ARM_W/2,
                       BOOM_MODULE_L/2 - BOOM_ARM_T/2,
                       BOOM_MAST_H - BOOM_ARM_W])
                cube([BOOM_ARM_W, BOOM_ARM_L, BOOM_ARM_W]);
            // ── Sensor head plate (at boom tip) ──────────────────────
            translate([-BOOM_HEAD_W/2,
                       BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L,
                       BOOM_MAST_H - BOOM_ARM_W - BOOM_HEAD_H/2 + BOOM_ARM_W/2])
                cube([BOOM_HEAD_W, BOOM_HEAD_T, BOOM_HEAD_H]);
            // ── Junction gussets (mast + horizontal boom) ────────────
            translate([-BOOM_ARM_W/2,
                       BOOM_MODULE_L/2 - BOOM_ARM_T/2,
                       BOOM_MAST_H - BOOM_ARM_W])
                rotate([45, 0, 0])
                    cube([BOOM_ARM_W, BOOM_ARM_W * 0.7, BOOM_ARM_W * 0.7]);
        }
        // ── 2020 sensor-rail bolt pattern in head plate ───────────────
        // 2× M5 slots (matches sensor_rail.scad tank_clamp slot geometry)
        for (sz = [-BOOM_HEAD_H/4, BOOM_HEAD_H/4])
            translate([0,
                       BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L + BOOM_HEAD_T + e,
                       BOOM_MAST_H - BOOM_ARM_W/2 + sz])
                rotate([90, 0, 0])
                    hull() {
                        translate([-6, 0, 0])
                            cylinder(d = 5.3, h = BOOM_HEAD_T + 2*e);
                        translate([+6, 0, 0])
                            cylinder(d = 5.3, h = BOOM_HEAD_T + 2*e);
                    }
        // ── Tilt angle slots (3 positions: 0°, ±15°) ─────────────────
        for (ta = [-15, 0, 15]) {
            translate([0,
                       BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L/2,
                       BOOM_MAST_H - BOOM_ARM_W/2])
                rotate([ta, 0, 0])
                    translate([0, BOOM_ARM_L/2, 0])
                        cylinder(d = 4.3, h = BOOM_ARM_W + 2*e,
                                 center = true);
        }
        // ── Cable tie slots in mast ───────────────────────────────────
        for (cz = [BOOM_MAST_H * 0.3, BOOM_MAST_H * 0.6])
            translate([0, BOOM_MODULE_L/2, cz])
                cube([BOOM_ARM_W + 2*e, 4, 4], center = true);
        // ── Lightening pockets in mast ────────────────────────────────
        translate([0, BOOM_MODULE_L/2, BOOM_MAST_H * 0.4])
            cube([BOOM_ARM_W - 6, BOOM_ARM_T - 6,
                  BOOM_MAST_H * 0.4], center = true);
    }
 }
 // ============================================================
 // PART 3 — CUP HOLDER
 // ============================================================
 // Tapered cup cradle for standard travel mugs / water bottles.
 // Inner diameter: 80 mm at top, 68 mm at bottom (matches Ø70 mm typical mug).
 // Flexible gripper ribs (cut slots) provide spring retention.
 // Drain hole at bottom for condensation.
 // Module length: 80 mm.
 CUP_MODULE_L  =  80.0;
 CUP_INNER_TOP = 80.0;    // inner bore OD at top
 CUP_INNER_BOT = 68.0;    // inner bore OD at bottom (taper for grip)
 CUP_OUTER_T   =  4.0;    // wall thickness
 CUP_H         = 80.0;    // cup holder height
 CUP_GRIP_SLOTS = 8;      // number of flex slots (spring grip)
 CUP_SLOT_W    =  2.5;    // flex slot width
 CUP_SLOT_H    = 40.0;    // flex slot height (from top)
 module cup_holder() {
    _module_base(CUP_MODULE_L, n_detents = 1);
    difference() {
        union() {
            // ── Outer shell (tapered cylinder) ───────────────────────
            cylinder(d1 = CUP_INNER_BOT + 2*CUP_OUTER_T,
                     d2 = CUP_INNER_TOP + 2*CUP_OUTER_T,
                     h  = CUP_H);
            // ── Base flange (connects to module body footprint) ───────
            hull() {
                cylinder(d = CUP_INNER_BOT + 2*CUP_OUTER_T + 4,
                         h = 4);
                translate([0, CUP_MODULE_L/2, 0])
                    cube([RAIL_W, CUP_MODULE_L, 4], center = true);
            }
        }
        // ── Inner bore (tapered cup cavity) ──────────────────────────
        translate([0, 0, CUP_OUTER_T])
            cylinder(d1 = CUP_INNER_BOT, d2 = CUP_INNER_TOP,
                     h  = CUP_H + e);
        // ── Base drain hole ──────────────────────────────────────────
        translate([0, 0, -e])
            cylinder(d = 12, h = CUP_OUTER_T + 2*e);
        // ── Flex grip slots (from top down) ──────────────────────────
        // Slots allow upper rim to flex inward and grip cup body
        for (i = [0 : CUP_GRIP_SLOTS - 1]) {
            angle = i * 360 / CUP_GRIP_SLOTS;
            rotate([0, 0, angle])
                translate([CUP_INNER_TOP/2 + CUP_OUTER_T/2, 0, CUP_H - CUP_SLOT_H])
                    cube([CUP_OUTER_T + 2*e, CUP_SLOT_W, CUP_SLOT_H + e],
                         center = true);
        }
        // ── Exterior branding recess (optional label area) ────────────
        translate([CUP_INNER_BOT/2 + CUP_OUTER_T/2 - 0.5, 0, CUP_H/2])
            rotate([0, 90, 0])
                cube([25, 40, 1 + e], center = true);
    }
 }
 // ============================================================
 // ══ MODULE TEMPLATE ═══════════════════════════════════════════
 // ══ Copy this block to create a new payload module ════════════
 // ============================================================
 //
 // module my_new_module() {
 //     MY_LEN = 120.0;   // module length — must be multiple of DETENT_PITCH (50 mm)
 //
 //     // Always start with the base (provides tongue, pads, detent bore)
 //     _module_base(MY_LEN, n_detents = 2);
 //
 //     // Add your payload geometry here.
 //     // Z = 0 is the rail top face / module mounting face.
 //     // Build upward from Z = 0.
 //
 //     difference() {
 //         union() {
 //             // Example: a simple platform
 //             translate([-(RAIL_W + 10)/2, 0, 0])
 //                 cube([RAIL_W + 10, MY_LEN, 10]);
 //
 //             // Add your geometry...
 //         }
 //
 //         // Add your cutouts...
 //     }
 // }
 //
 // ── Don't forget to: ──────────────────────────────────────────
 //   1. Add  else if (RENDER == "my_module_stl")  my_new_module();
 //      in the RENDER DISPATCH block above.
 //   2. Add export command in BOM / README.
 //   3. Test: verify tongue fits rail slot (should slide with 0.3 mm clearance).
 //   4. Verify connector pad positions align with CONN_Y on rail.
 // ============================================================
--- a/chassis/payload_bay_rail.scad
+++ b/chassis/payload_bay_rail.scad
@ -0,0 +1,429 @@
 // ============================================================
 // payload_bay_rail.scad — Modular Payload Bay Rail System
 // Issue: #170  Agent: sl-mechanical  Date: 2026-03-01
 // ============================================================
 //
 // Dovetail rail mounted on robot top deck.  Payload modules slide on
 // from either end and are retained by a spring-loaded ball detent plus
 // an optional M4 thumbscrew safety lock.
 //
 // Dovetail geometry (60° included angle — balanced for print + load):
 //
 //    ← RAIL_W (50 mm) →
 //    ┌──────────────────┐  ← rail top face (Z = RAIL_T)
 //    │  ╲            ╱  │
 //    │   ╲__________╱   │  ← dovetail slot (female, cut into top)
 //    │   (DOVE_SLOT)    │
 //    └──────────────────┘  ← rail bottom face (Z = 0)
 //
 // DOVE_ANGLE = 30° from vertical (= 60° included).
 // Slot width at top  = DOVE_SLOT_TOP  (open face)
 // Slot width at bottom = DOVE_SLOT_BOT (inner base of slot)
 // Slot depth = DOVE_H
 //
 // Module tongue (male dovetail) slides in with 0.3 mm clearance each side.
 //
 // Spring detent: Ø6 mm steel ball in module, spring behind, seats in
 //   Ø4.9 mm dimples drilled into rail slot bottom at DETENT_PITCH spacing.
 //   Provides tactile click-lock at each indexed position.
 //
 // Safety lock: M4 thumbscrew through module side, tightens against rail
 //   side wall.  For vibration environments or >1 kg payload.
 //
 // Power+data connector: 4-pin pogo array in rail at fixed position.
 //   Pins: GND | +5 V | +12 V | UART (half-duplex)
 //   Module has matching brass target pads (Ø4 mm).
 //   Connector position: centred in rail length, at CONN_Y from one end.
 //
 // Cross-variant adapter plates (this file):
 //   lab_rail_adapter()   — SaltyLab chassis top (Ø25 mm stem clear)
 //   rover_rail_adapter() — SaltyRover deck (M4 grid)
 //   tank_rail_adapter()  — SaltyTank deck (M4 grid)
 //
 // Coordinate convention:
 //   Rail runs along Y axis.  Cross-section in X-Z plane.
 //   Z = 0 at rail bottom face (= robot deck top face).
 //   Module slides in +Y direction.
 //
 // RENDER options:
 //   "assembly"            rail + adapter + module ghost
 //   "rail_2d"             DXF — dovetail cross-section (CNC/waterjet)
 //   "rail_stl"            STL — printable rail section (PETG prototype)
 //   "connector_stl"       STL — pogo connector housing insert
 //   "detent_plunger_stl"  STL — spring detent plunger (print ×2 per module)
 //   "lab_adapter_stl"     STL — SaltyLab deck adapter
 //   "rover_adapter_stl"   STL — SaltyRover deck adapter
 //   "tank_adapter_stl"    STL — SaltyTank deck adapter
 //
 // Export:
 //   openscad payload_bay_rail.scad -D 'RENDER="rail_2d"'            -o payload_rail.dxf
 //   openscad payload_bay_rail.scad -D 'RENDER="rail_stl"'           -o payload_rail.stl
 //   openscad payload_bay_rail.scad -D 'RENDER="connector_stl"'      -o payload_connector.stl
 //   openscad payload_bay_rail.scad -D 'RENDER="detent_plunger_stl"' -o payload_detent.stl
 //   openscad payload_bay_rail.scad -D 'RENDER="lab_adapter_stl"'    -o payload_lab_adapter.stl
 //   openscad payload_bay_rail.scad -D 'RENDER="rover_adapter_stl"'  -o payload_rover_adapter.stl
 //   openscad payload_bay_rail.scad -D 'RENDER="tank_adapter_stl"'   -o payload_tank_adapter.stl
 // ============================================================
 $fn = 64;
 e   = 0.01;
 // ── Dovetail rail cross-section ───────────────────────────────────────────────
 RAIL_W       = 50.0;    // rail total width (X)
 RAIL_T       = 12.0;    // rail total height (Z)
 RAIL_R       =  2.0;    // outer corner radius
 RAIL_LEN     = 200.0;   // default rail section length (Y)
 // Dovetail slot geometry
 DOVE_ANGLE   = 30.0;    // degrees from vertical (60° included angle)
 DOVE_H       =  8.0;    // slot depth (Z into rail from top)
 DOVE_SLOT_BOT=  28.0;   // slot width at bottom (inner)
 // Derived: slot width at top = DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE)
 DOVE_SLOT_TOP= DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE);  // ≈ 37.2 mm
 // Module tongue (male) clearance: 0.3 mm per side
 DOVE_CLEAR   =  0.3;
 // → module tongue: bot_w = DOVE_SLOT_BOT - 2*DOVE_CLEAR,  top_w = DOVE_SLOT_TOP + 2*DOVE_CLEAR
 // ── Spring ball detent ────────────────────────────────────────────────────────
 // Ø6 mm steel ball presses up through module tongue into dimples in rail slot.
 DETENT_BALL_D  =  6.0;    // ball diameter
 DETENT_HOLE_D  =  4.9;    // dimple bore in rail slot bottom (ball seats in)
 DETENT_DEPTH   =  1.5;    // dimple depth (ball sinks in this far)
 DETENT_PITCH   = 50.0;    // dimple spacing along rail (Y) — module index positions
 DETENT_SPG_OD  =  6.2;    // plunger bore OD (ball + spring housing in module)
 DETENT_SPG_L   = 16.0;    // spring pocket depth in module tongue
 // ── Safety lock (M4 thumbscrew through module side into rail side groove) ─────
 LOCK_GROOVE_D  =  4.5;    // groove in rail side wall (M4 thumbscrew tip seats in)
 LOCK_GROOVE_DEPTH = 1.5;  // groove depth into rail side
 // Lock groove runs full rail length (continuous slot) for tool-free slide + lock anywhere
 // ── 4-pin power+data connector ───────────────────────────────────────────────
 // Pogo pin array mounted in rail body at CONN_Y from entry end.
 // Pin map: 1=GND  2=+5V  3=+12V  4=UART
 // Pogo: Ø2 mm spring contact (P75-E2 style), rated 2 A (power), 0.5 A (signal)
 CONN_Y        = RAIL_LEN / 2;   // connector centred in rail section
 CONN_PIN_D    =  2.2;           // pogo bore (2 mm pin + 0.2 mm clearance)
 CONN_PIN_SPC  =  5.0;           // pin centre-to-centre spacing
 CONN_N_PINS   =  4;             // GND / +5V / +12V / UART
 CONN_HOUSING_W= CONN_N_PINS * CONN_PIN_SPC + 4;  // housing width (X)
 CONN_HOUSING_D=  8.0;           // housing depth (Y, inside rail)
 CONN_HOUSING_H= DOVE_H - 1.0;  // housing height; sits inside slot (flush with slot floor)
 // Connector pogo pins point upward into module pad targets.
 // ── Deck mounting holes ───────────────────────────────────────────────────────
 MOUNT_PITCH  = 50.0;   // M4 FHCS hole pitch along rail (countersunk from bottom)
 MOUNT_INSET  = 25.0;   // first hole Y from rail end
 MOUNT_D      =  4.3;   // M4 clearance
 CSINK_D      =  8.0;   // M4 FHCS head diameter
 // ── Cross-variant adapter plates ─────────────────────────────────────────────
 ADAPT_T      =  4.0;   // adapter plate thickness
 ADAPT_OVHG   = 10.0;   // adapter overhang past rail edge each side (flange width)
 // SaltyLab deck: stem bore at centre
 LAB_STEM_BORE =  26.0;   // clear stem Ø25 mm
 // SaltyRover deck: M4 bolt grid (spacing from rover_chassis_r2.scad)
 ROVER_BOLT_SPC = 40.0;
 // SaltyTank deck: M4 bolt grid (spacing from saltytank_chassis.scad)
 TANK_BOLT_SPC  = 40.0;
 // Fasteners
 M3_D = 3.3;
 M4_D = 4.3;
 // ============================================================
 // RENDER DISPATCH
 // ============================================================
 RENDER = "assembly";
 if      (RENDER == "assembly")           assembly();
 else if (RENDER == "rail_2d")
    projection(cut = true) translate([0, 0, -0.5])
        linear_extrude(1) rail_profile_2d();
 else if (RENDER == "rail_stl")           rail_section(RAIL_LEN);
 else if (RENDER == "connector_stl")      connector_housing();
 else if (RENDER == "detent_plunger_stl") detent_plunger();
 else if (RENDER == "lab_adapter_stl")    lab_rail_adapter();
 else if (RENDER == "rover_adapter_stl")  rover_rail_adapter();
 else if (RENDER == "tank_adapter_stl")   tank_rail_adapter();
 // ============================================================
 // ASSEMBLY PREVIEW
 // ============================================================
 module assembly() {
    // Rail section
    color("Silver", 0.85) rail_section(RAIL_LEN);
    // Rover adapter under rail
    color("SteelBlue", 0.70)
        translate([0, 0, -ADAPT_T])
            rover_rail_adapter();
    // Ghost module sliding on
    %color("OliveDrab", 0.3)
        translate([0, 60, RAIL_T])
            cube([RAIL_W + 10, 100, 40], center = true);
    // Connector position marker
    %color("Gold", 0.5)
        translate([0, CONN_Y, RAIL_T - DOVE_H])
            cube([CONN_HOUSING_W, CONN_HOUSING_D, CONN_HOUSING_H],
                 center = true);
    // Detent dimple markers
    for (dy = [MOUNT_INSET : DETENT_PITCH : RAIL_LEN - MOUNT_INSET])
        %color("Red", 0.6)
            translate([0, dy, RAIL_T - DOVE_H])
                cylinder(d = DETENT_HOLE_D, h = DETENT_DEPTH);
 }
 // ============================================================
 // RAIL CROSS-SECTION 2D  (DXF export)
 // ============================================================
 // Outer profile minus dovetail slot.
 // For CNC milling from 50×12 mm aluminium bar, or waterjet from plate.
 // Also used for PETG prototype extrusion.
 module rail_profile_2d() {
    difference() {
        // Outer rail cross-section (rounded rect)
        minkowski() {
            square([RAIL_W - 2*RAIL_R, RAIL_T - 2*RAIL_R],
                   center = true);
            circle(r = RAIL_R);
        }
        // Dovetail slot (trapezoid, open at top)
        translate([0, RAIL_T/2])
            _dovetail_slot_2d();
    }
 }
 // ── Dovetail slot 2D (trapezoid with open top) ───────────────────────────────
 module _dovetail_slot_2d() {
    // Trapezoid: wider at top (open face), narrower at bottom.
    // Points listed clockwise from top-left:
    polygon([
        [-DOVE_SLOT_TOP/2,  0],                  // top-left
        [ DOVE_SLOT_TOP/2,  0],                  // top-right
        [ DOVE_SLOT_BOT/2, -DOVE_H],             // bottom-right
        [-DOVE_SLOT_BOT/2, -DOVE_H],             // bottom-left
    ]);
 }
 // ── Dovetail slot for difference() operations (3D volume) ────────────────────
 module _dovetail_slot_3d(length) {
    translate([0, -e, RAIL_T])
        linear_extrude(DOVE_H + e)
            offset(delta = e)
                _dovetail_slot_2d();
 }
 // ============================================================
 // RAIL SECTION  (3D — printable or aluminium)
 // ============================================================
 module rail_section(len = RAIL_LEN) {
    difference() {
        // ── Extruded profile ────────────────────────────────────────
        linear_extrude(len)
            rotate([0, 0, 90])
                rail_profile_2d();
        // ── Dovetail slot ────────────────────────────────────────────
        translate([0, -e, RAIL_T])
            rotate([0, 0, 0])
                linear_extrude(len + 2*e)
                    rotate([0, 0, 90])
                        offset(delta = e)
                            _dovetail_slot_2d();
        // ── Deck mounting holes (M4 FHCS, from bottom) ───────────────
        for (my = [MOUNT_INSET : MOUNT_PITCH : len - MOUNT_INSET])
            translate([0, my, -e])
                cylinder(d = MOUNT_D, h = RAIL_T - DOVE_H + 2*e);
        // Countersinks on bottom face
        for (my = [MOUNT_INSET : MOUNT_PITCH : len - MOUNT_INSET])
            translate([0, my, -e])
                cylinder(d1 = CSINK_D, d2 = MOUNT_D,
                         h = (CSINK_D - MOUNT_D) / (2 * tan(41)));
        // ── Spring detent dimples (slot bottom, at DETENT_PITCH) ──────
        for (dy = [MOUNT_INSET : DETENT_PITCH : len - MOUNT_INSET])
            translate([0, dy, RAIL_T - DOVE_H - e])
                cylinder(d = DETENT_HOLE_D, h = DETENT_DEPTH + e);
        // ── Safety-lock groove (continuous slot, both sides of rail) ──
        // M4 thumbscrew tip seats anywhere along groove
        for (sx = [-1, 1])
            translate([sx * (RAIL_W/2 + e), -e,
                       RAIL_T - DOVE_H/2])
                rotate([0, 90, 0])
                    hull() {
                        translate([0, 0, 0])
                            cylinder(d = LOCK_GROOVE_D, h = RAIL_W + 2*e);
                    }
        // ── Connector housing pocket (at CONN_Y) ──────────────────────
        translate([0, CONN_Y, RAIL_T - DOVE_H - e])
            cube([CONN_HOUSING_W + 0.4,
                  CONN_HOUSING_D + 0.4,
                  CONN_HOUSING_H + e], center = true);
        // ── Lightening slots (rail body between mounting holes) ────────
        for (my = [MOUNT_INSET + 25 : MOUNT_PITCH : len - MOUNT_INSET - 25])
            translate([0, my, RAIL_T/4])
                cube([RAIL_W - 16, 20, RAIL_T/2 + 2*e], center = true);
    }
 }
 // ============================================================
 // CONNECTOR HOUSING  (pogo-pin insert, press-fits into rail pocket)
 // ============================================================
 // 4× spring-loaded pogo pins (P75-E2, Ø2 mm, 6 mm travel).
 // Printed housing press-fits into rail pocket; pins protrude up into module.
 // Module has 4× Ø4 mm brass target pads at matching pitch.
 //
 // Pin map (left to right, looking at rail top from +Z):
 //   Pin 1: GND  Pin 2: +5 V  Pin 3: +12 V  Pin 4: UART
 module connector_housing() {
    hw = CONN_HOUSING_W;
    hd = CONN_HOUSING_D;
    hh = CONN_HOUSING_H;
    difference() {
        // Housing body (press-fit into rail pocket)
        cube([hw, hd, hh], center = true);
        // 4× pogo pin bores (through housing, top to bottom)
        for (i = [0 : CONN_N_PINS - 1]) {
            px = (i - (CONN_N_PINS - 1) / 2) * CONN_PIN_SPC;
            translate([px, 0, -hh/2 - e])
                cylinder(d = CONN_PIN_D, h = hh + 2*e);
        }
        // Wire exit slot (bottom, routes into rail body)
        translate([0, 0, -hh/2 - e])
            cube([hw - 6, hd/2, hh/3 + e], center = true);
        // Retention barbs (prevent housing pulling out of pocket)
        for (sx = [-1, 1])
            translate([sx * (hw/2 - 1), 0, hh/4])
                rotate([0, sx * 15, 0])
                    cube([2, hd + 2*e, 2.5], center = true);
    }
    // Pin polarity label recess (on top face, GND side)
    difference() {
        translate([0, 0, 0]) cube([0, 0, 0]); // null
        translate([-(CONN_N_PINS * CONN_PIN_SPC)/2 + 1, 0, hh/2 - 0.4])
            cube([3, hd * 0.6, 0.5 + e], center = true);
    }
 }
 // ============================================================
 // DETENT PLUNGER  (lives in module tongue; print 2× per module)
 // ============================================================
 // Press-fit into Ø6.2 mm bore in module tongue.
 // Includes spring pocket; ball seated on top.
 // Spring: Ø5.5 mm OD, 12 mm free length, ~2 N/mm (light detent).
 // Ball: Ø6 mm steel (standard bearing ball, purchase).
 module detent_plunger() {
    bore_d   = DETENT_BALL_D + 0.2;   // 6.2 mm
    body_od  = DETENT_SPG_OD;
    body_len = DETENT_SPG_L;
    spg_d    = 5.8;    // spring OD
    spg_pocket = 10.0; // spring pocket depth (bottom of housing)
    difference() {
        cylinder(d = body_od, h = body_len);
        // Ball socket (top — partial sphere, retains ball)
        translate([0, 0, body_len])
            sphere(d = bore_d);
        translate([0, 0, body_len - bore_d/4])
            cylinder(d = bore_d, h = bore_d/2 + e);
        // Spring pocket (bottom)
        translate([0, 0, -e])
            cylinder(d = spg_d + 0.3, h = spg_pocket + e);
        // Retention lip (allows push-in but prevents pullout before spring seated)
        translate([0, 0, spg_pocket])
            cylinder(d1 = spg_d + 0.3, d2 = spg_d - 1,
                     h = 1.5);
    }
 }
 // ============================================================
 // CROSS-VARIANT DECK ADAPTER PLATES
 // ============================================================
 // Thin plates that bolt to the robot deck and provide M4 threaded
 // studs (or through holes) for the rail mounting holes.
 // All adapters: RAIL_LEN × (RAIL_W + 2×ADAPT_OVHG) footprint.
 module _adapter_base() {
    adapt_l = RAIL_LEN;
    adapt_w = RAIL_W + 2*ADAPT_OVHG;
    difference() {
        // Plate
        cube([adapt_w, adapt_l, ADAPT_T], center = true);
        // Rail mounting holes (M4 FHCS up through adapter into rail bottom)
        for (my = [MOUNT_INSET : MOUNT_PITCH : adapt_l - MOUNT_INSET])
            translate([0, my - adapt_l/2, -ADAPT_T/2 - e])
                cylinder(d = MOUNT_D, h = ADAPT_T + 2*e);
        // Corner lightening
        for (cx = [-1, 1]) for (cy = [-1, 1])
            translate([cx * (adapt_w/2 - 12),
                       cy * (adapt_l/2 - 20), 0])
                cylinder(d = 10, h = ADAPT_T + 2*e, center = true);
    }
 }
 // SaltyLab adapter: clears Ø25 mm stem, 4× M4 to lab chassis ring
 module lab_rail_adapter() {
    difference() {
        _adapter_base();
        // Stem bore clearance (at centre of adapter)
        cylinder(d = LAB_STEM_BORE, h = ADAPT_T + 2*e, center = true);
        // 4× M4 mounting to lab chassis top ring (Ø44 mm bolt circle)
        for (a = [45, 135, 225, 315])
            translate([22*cos(a), 22*sin(a), -ADAPT_T/2 - e])
                cylinder(d = M4_D, h = ADAPT_T + 2*e);
    }
 }
 // SaltyRover adapter: 4× M4 to rover deck bolt grid
 module rover_rail_adapter() {
    adapt_l = RAIL_LEN;
    adapt_w = RAIL_W + 2*ADAPT_OVHG;
    difference() {
        _adapter_base();
        // 2 rows × 3 cols of M4 bolts into rover deck
        for (rx = [-ROVER_BOLT_SPC/2, ROVER_BOLT_SPC/2])
        for (ry = [-adapt_l/3, 0, adapt_l/3])
            translate([rx, ry, -ADAPT_T/2 - e])
                cylinder(d = M4_D, h = ADAPT_T + 2*e);
    }
 }
 // SaltyTank adapter: M4 to tank deck; relieved for deck cable slots
 module tank_rail_adapter() {
    adapt_l = RAIL_LEN;
    adapt_w = RAIL_W + 2*ADAPT_OVHG;
    difference() {
        _adapter_base();
        // 2 rows × 3 cols of M4 bolts into tank deck
        for (rx = [-TANK_BOLT_SPC/2, TANK_BOLT_SPC/2])
        for (ry = [-adapt_l/3, 0, adapt_l/3])
            translate([rx, ry, -ADAPT_T/2 - e])
                cylinder(d = M4_D, h = ADAPT_T + 2*e);
        // Deck cable slot clearance (tank deck has centre cable channel)
        translate([0, 0, 0])
            cube([10, adapt_l - 40, ADAPT_T + 2*e], center = true);
    }
 }
--- a/include/audio.h
+++ b/include/audio.h
@ -0,0 +1,106 @@
 #ifndef AUDIO_H
 #define AUDIO_H
 #include <stdint.h>
 #include <stdbool.h>
 /*
 * audio.h — I2S audio output driver (Issue #143)
 *
 * Hardware: SPI3 repurposed as I2S3 master TX (blackbox flash not used
 * on balance bot).  Supports MAX98357A (I2S class-D amp) and PCM5102A
 * (I2S DAC + external amp) — both use standard Philips I2S.
 *
 * Pin assignment (SPI3 / I2S3, defined in config.h):
 *   PC10  I2S3_CK  (BCLK)   AF6
 *   PA15  I2S3_WS  (LRCLK)  AF6
 *   PB5   I2S3_SD  (DIN)    AF6
 *   PC5   AUDIO_MUTE (GPIO) active-high = enabled; low = muted/shutdown
 *
 * PLLI2S: N=192, R=2 → 96 MHz I2S clock → 22058 Hz (< 0.04% from 22050)
 * DMA1 Stream7 Channel0 (SPI3_TX), circular, double-buffer ping-pong.
 *
 * Mixer priority (highest to lowest):
 *   1. PCM audio chunks from Jetson (via JLINK_CMD_AUDIO, written to FIFO)
 *   2. Notification tones (queued by audio_play_tone)
 *   3. Silence
 *
 * Volume applies to all sources via integer sample scaling (0–100).
 */
 /* Maximum int16_t samples per JLINK_CMD_AUDIO frame (252-byte payload / 2) */
 #define AUDIO_CHUNK_MAX_SAMPLES  126u
 /* Pre-defined notification tones */
 typedef enum {
    AUDIO_TONE_BEEP_SHORT = 0,  /* 880 Hz, 100 ms — acknowledge / UI feedback  */
    AUDIO_TONE_BEEP_LONG  = 1,  /* 880 Hz, 500 ms — generic warning            */
    AUDIO_TONE_STARTUP    = 2,  /* C5→E5→G5 arpeggio (3 × 120 ms)             */
    AUDIO_TONE_ARM        = 3,  /* 880 Hz→1047 Hz two-beep ascending           */
    AUDIO_TONE_DISARM     = 4,  /* 880 Hz→659 Hz two-beep descending           */
    AUDIO_TONE_FAULT      = 5,  /* 200 Hz buzz, 500 ms — tilt/safety fault     */
    AUDIO_TONE_COUNT
 } AudioTone;
 /*
 * audio_init()
 *
 * Configure PLLI2S, GPIO, DMA1 Stream7, and SPI3/I2S3.
 * Pre-fills DMA buffer with silence, starts circular DMA TX, then
 * unmutes the amp.  Call once before safety_init().
 */
 void audio_init(void);
 /*
 * audio_mute(mute)
 *
 * Drive AUDIO_MUTE_PIN: false = hardware-muted (SD/XSMT low),
 * true = active (amp enabled).  Does NOT stop DMA; allows instant
 * un-mute without DMA restart clicks.
 */
 void audio_mute(bool active);
 /*
 * audio_set_volume(vol)
 *
 * Software volume 0–100.  Applied in ISR fill path via integer scaling.
 * 0 = silence, 100 = full scale (±16384 for square wave, passthrough for PCM).
 */
 void audio_set_volume(uint8_t vol);
 /*
 * audio_play_tone(tone)
 *
 * Queue a pre-defined notification tone.  The tone plays after any tones
 * already in the queue.  Returns false if the tone queue is full (depth 4).
 * Tones are pre-empted by incoming PCM audio from the Jetson.
 */
 bool audio_play_tone(AudioTone tone);
 /*
 * audio_write_pcm(samples, n)
 *
 * Write mono 16-bit 22050 Hz PCM samples into the Jetson PCM FIFO.
 * Called from jlink_process() dispatch on JLINK_CMD_AUDIO (main-loop context).
 * Returns the number of samples actually accepted (0 if FIFO is full).
 */
 uint16_t audio_write_pcm(const int16_t *samples, uint16_t n);
 /*
 * audio_tick(now_ms)
 *
 * Advance the tone sequencer state machine.  Must be called every 1 ms
 * from the main loop.  Manages step transitions and gap timing; updates
 * the volatile active-tone parameters read by the ISR fill path.
 */
 void audio_tick(uint32_t now_ms);
 /*
 * audio_is_playing()
 *
 * Returns true if the DMA is running (always true after audio_init()
 * unless the amp is hardware-muted or the I2S peripheral has an error).
 */
 bool audio_is_playing(void);
 #endif /* AUDIO_H */
--- a/include/config.h
+++ b/include/config.h
@ -189,4 +189,19 @@
 /* Full blend transition time: MANUAL→AUTO takes this many ms */
 #define MODE_BLEND_MS           500
 // --- Audio Amplifier (I2S3, Issue #143) ---
 // SPI3 repurposed as I2S3; blackbox flash unused on balance bot
 #define AUDIO_BCLK_PORT         GPIOC
 #define AUDIO_BCLK_PIN          GPIO_PIN_10   // I2S3_CK (PC10, AF6)
 #define AUDIO_LRCK_PORT         GPIOA
 #define AUDIO_LRCK_PIN          GPIO_PIN_15   // I2S3_WS (PA15, AF6)
 #define AUDIO_DOUT_PORT         GPIOB
 #define AUDIO_DOUT_PIN          GPIO_PIN_5    // I2S3_SD (PB5, AF6)
 #define AUDIO_MUTE_PORT         GPIOC
 #define AUDIO_MUTE_PIN          GPIO_PIN_5    // active-high = amp enabled
 // PLLI2S: N=192, R=2 → I2S clock=96 MHz → FS≈22058 Hz (< 0.04% error)
 #define AUDIO_SAMPLE_RATE       22050u        // nominal sample rate (Hz)
 #define AUDIO_BUF_HALF          441u          // DMA half-buffer: 20ms at 22050 Hz
 #define AUDIO_VOLUME_DEFAULT    80u           // default volume 0-100
 #endif // CONFIG_H
--- a/include/jlink.h
+++ b/include/jlink.h
@ -53,6 +53,7 @@
 #define JLINK_CMD_PID_SET       0x05u
 #define JLINK_CMD_DFU_ENTER     0x06u
 #define JLINK_CMD_ESTOP         0x07u
 #define JLINK_CMD_AUDIO         0x08u  /* PCM audio chunk: int16 samples, up to 126 */
 /* ---- Telemetry IDs (STM32 → Jetson) ---- */
 #define JLINK_TLM_STATUS        0x80u
--- a/jetson/ros2_ws/src/saltybot_night_vision/config/night_vision_params.yaml
+++ b/jetson/ros2_ws/src/saltybot_night_vision/config/night_vision_params.yaml
@ -0,0 +1,49 @@
 # saltybot_night_vision — runtime parameters
 #
 # Ambient light detection requires ONE of:
 #   /camera/color/image_raw         — IMX219 front camera (primary)
 #   /camera/infra1/image_rect_raw   — D435i IR fallback
 #
 # D435i IR emitter is controlled via the realsense2_camera parameter service.
 # Make sure the node is running as /camera/camera (default).
 #
 # GPIO headlight on physical pin 33 of the Jetson Orin Nano Super 40-pin header.
 # Requires Jetson.GPIO (jetson-gpio pip package) or /sys/class/gpio write access.
 light_monitor:
  ros__parameters:
    primary_camera_topic:  '/camera/color/image_raw'
    fallback_ir_topic:     '/camera/infra1/image_rect_raw'
    publish_hz:             2.0      # ambient light evaluation rate
    initial_mode:          'day'
    history_len:            6        # frames to median-filter intensity
 night_vision_controller:
  ros__parameters:
    headlight_pin:         33        # physical board pin (Jetson 40-pin J26)
    day_headlight:         0.0       # off
    twilight_headlight:    0.25      # 25 % — subtle fill
    night_headlight:       0.50      # 50 % — normal dark room
    ir_only_headlight:     1.0       # 100 % — pitch black
 ir_slam_bridge:
  ros__parameters:
    active_modes:   'night,ir_only'  # publish IR→RGB only in these modes
    output_encoding: 'rgb8'          # RTAB-Map expects rgb8 or bgr8
 # ── RTAB-Map integration note ──────────────────────────────────────────────────
 # When the bridge is active (/saltybot/ir_bridge/image_raw), redirect RTAB-Map's
 # RGB input by adding to slam_rtabmap.launch.py Node remappings:
 #
 #   remappings=[
 #       ('rgb/image',       '/saltybot/ir_bridge/image_raw'),
 #       ('rgb/camera_info', '/saltybot/ir_bridge/camera_info'),
 #   ]
 #
 # Or set the topic at launch:
 #   ros2 launch saltybot_bringup slam_rtabmap.launch.py \
 #       rgb_image_topic:=/saltybot/ir_bridge/image_raw
 #
 # This ensures loop-closure detection continues using the IR feature stream
 # instead of a dark / blank colour image.
--- a/jetson/ros2_ws/src/saltybot_night_vision/launch/night_vision.launch.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/launch/night_vision.launch.py
@ -0,0 +1,68 @@
 """
 night_vision.launch.py — SaltyBot night vision stack.
 Starts:
  light_monitor          — histogram ambient-light analysis → /saltybot/vision_mode
  night_vision_ctrl      — D435i emitter + IMX219 gain + GPIO headlight
  ir_slam_bridge         — IR→RGB republish for RTAB-Map (active in dark modes)
 Launch args:
  primary_camera_topic   str    '/camera/color/image_raw'
  headlight_pin          int    '33'
  publish_hz             float  '2.0'
  active_ir_modes        str    'night,ir_only'
 Verify:
  ros2 topic echo /saltybot/vision_mode
  ros2 topic echo /saltybot/ambient_light
  ros2 topic echo /saltybot/night_vision_status
  ros2 topic hz /saltybot/ir_bridge/image_raw     # should be ~30 Hz in dark
 """
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 def generate_launch_description():
    args = [
        DeclareLaunchArgument('primary_camera_topic',
                              default_value='/camera/color/image_raw'),
        DeclareLaunchArgument('headlight_pin',  default_value='33'),
        DeclareLaunchArgument('publish_hz',     default_value='2.0'),
        DeclareLaunchArgument('active_ir_modes', default_value='night,ir_only'),
    ]
    light_monitor = Node(
        package='saltybot_night_vision',
        executable='light_monitor',
        name='light_monitor',
        output='screen',
        parameters=[{
            'primary_camera_topic': LaunchConfiguration('primary_camera_topic'),
            'publish_hz':           LaunchConfiguration('publish_hz'),
        }],
    )
    nv_ctrl = Node(
        package='saltybot_night_vision',
        executable='night_vision_ctrl',
        name='night_vision_controller',
        output='screen',
        parameters=[{
            'headlight_pin': LaunchConfiguration('headlight_pin'),
        }],
    )
    ir_bridge = Node(
        package='saltybot_night_vision',
        executable='ir_slam_bridge',
        name='ir_slam_bridge',
        output='screen',
        parameters=[{
            'active_modes': LaunchConfiguration('active_ir_modes'),
        }],
    )
    return LaunchDescription(args + [light_monitor, nv_ctrl, ir_bridge])
--- a/jetson/ros2_ws/src/saltybot_night_vision/package.xml
+++ b/jetson/ros2_ws/src/saltybot_night_vision/package.xml
@ -0,0 +1,27 @@
 <?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_night_vision</name>
  <version>0.1.0</version>
  <description>
    Night vision mode for SaltyBot: ambient-light histogram detection,
    D435i IR emitter control, IMX219 auto-gain/exposure, GPIO headlight,
    and IR-only SLAM bridging.
  </description>
  <maintainer email="robot@saltylab.local">SaltyLab</maintainer>
  <license>MIT</license>
  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>cv_bridge</depend>
  <exec_depend>python3-numpy</exec_depend>
  <exec_depend>python3-opencv</exec_depend>
  <test_depend>pytest</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
 </package>
--- a/jetson/ros2_ws/src/saltybot_night_vision/resource/saltybot_night_vision
+++ b/jetson/ros2_ws/src/saltybot_night_vision/resource/saltybot_night_vision
--- a/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/init.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/init.py
--- a/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/camera_controller.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/camera_controller.py
@ -0,0 +1,158 @@
 """
 camera_controller.py — D435i parameter service client + IMX219 v4l2 control.
 D435i (realsense2_camera node):
  depth_module.emitter_enabled  int  0=off  1=on  2=auto
  depth_module.emitter_always_on bool  keep emitter on between frames
 IMX219 (CSI cameras via v4l2):
  exposure_auto     0=manual  1=auto
  exposure          microseconds (manual)
  analogue_gain     raw sensor gain (16–170 typical range)
 The IMX219 controls are exercised via subprocess `v4l2-ctl`, which is
 always available on JetPack 6.
 """
 from __future__ import annotations
 import subprocess
 import logging
 from dataclasses import dataclass
 from typing import Optional
 import rclpy
 from rclpy.node import Node
 from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType
 from rcl_interfaces.srv import SetParameters
 _LOG = logging.getLogger(__name__)
 # D435i camera node (realsense2_camera default namespace)
 _RS_NODE = '/camera/camera'
 # IMX219 v4l2 device paths for each camera position
 # Front camera is the primary source for ambient-light detection
 _IMX219_DEVICES: dict[str, str] = {
    'front': '/dev/video0',
    'right': '/dev/video2',
    'rear':  '/dev/video4',
    'left':  '/dev/video6',
 }
 # Auto-exposure values for IMX219
 _IMX219_AE_AUTO   = 1
 _IMX219_AE_MANUAL = 0
@dataclass
 class CameraProfile:
    """Parameter set for a given vision mode."""
    emitter_enabled: int         # 0=off, 1=on, 2=auto
    imx_ae_mode: int             # 0=manual, 1=auto
    imx_exposure_us: Optional[int]  # None → leave AE to control it
    imx_gain: Optional[int]         # None → leave AE to control it
 # Per-mode camera profiles
 PROFILES: dict[str, CameraProfile] = {
    'day': CameraProfile(
        emitter_enabled=2,          # RS depth auto
        imx_ae_mode=_IMX219_AE_AUTO,
        imx_exposure_us=None,
        imx_gain=None,
    ),
    'twilight': CameraProfile(
        emitter_enabled=1,          # emitter on for better depth
        imx_ae_mode=_IMX219_AE_MANUAL,
        imx_exposure_us=20_000,     # 20 ms
        imx_gain=64,
    ),
    'night': CameraProfile(
        emitter_enabled=1,
        imx_ae_mode=_IMX219_AE_MANUAL,
        imx_exposure_us=50_000,     # 50 ms (sacrifice motion sharpness)
        imx_gain=128,
    ),
    'ir_only': CameraProfile(
        emitter_enabled=1,          # emitter critical for IR stereo in dark
        imx_ae_mode=_IMX219_AE_MANUAL,
        imx_exposure_us=80_000,
        imx_gain=170,               # near-max analogue gain
    ),
 }
 class CameraController:
    """
    Non-blocking camera controller.
    set_profile() sends async ROS2 parameter requests and v4l2 commands.
    It should be called from within a ROS2 node (pass the node reference).
    """
    def __init__(self, node: Node):
        self._node = node
        self._cli: Optional[rclpy.client.Client] = None
        self._pending_mode: Optional[str] = None
        self._current_mode: Optional[str] = None
        self._init_rs_client()
    # ── Public API ────────────────────────────────────────────────────────────
    def set_profile(self, mode: str) -> None:
        """Apply the camera profile for `mode`. Fire-and-forget async."""
        if mode not in PROFILES:
            self._node.get_logger().warning(f'[camera_ctrl] Unknown mode: {mode}')
            return
        if mode == self._current_mode:
            return
        profile = PROFILES[mode]
        self._set_rs_emitter(profile.emitter_enabled)
        self._set_imx219(profile)
        self._current_mode = mode
        self._node.get_logger().info(
            f'[camera_ctrl] Applied profile={mode} '
            f'emitter={profile.emitter_enabled} '
            f'gain={profile.imx_gain} exp={profile.imx_exposure_us}'
        )
    # ── D435i (RealSense) ─────────────────────────────────────────────────────
    def _init_rs_client(self) -> None:
        self._cli = self._node.create_client(
            SetParameters, f'{_RS_NODE}/set_parameters'
        )
    def _set_rs_emitter(self, value: int) -> None:
        if not self._cli.service_is_ready():
            self._node.get_logger().debug(
                '[camera_ctrl] RS parameter service not ready — skipping emitter set'
            )
            return
        pval = ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=value)
        param = Parameter(name='depth_module.emitter_enabled', value=pval)
        req = SetParameters.Request(parameters=[param])
        self._cli.call_async(req)  # fire-and-forget
    # ── IMX219 via v4l2-ctl ──────────────────────────────────────────────────
    def _set_imx219(self, profile: CameraProfile) -> None:
        for dev in _IMX219_DEVICES.values():
            self._v4l2_set(dev, 'exposure_auto', profile.imx_ae_mode)
            if profile.imx_exposure_us is not None:
                self._v4l2_set(dev, 'exposure', profile.imx_exposure_us)
            if profile.imx_gain is not None:
                self._v4l2_set(dev, 'analogue_gain', profile.imx_gain)
    def _v4l2_set(self, dev: str, ctrl: str, value: int) -> None:
        cmd = ['v4l2-ctl', '-d', dev, f'--set-ctrl={ctrl}={value}']
        try:
            result = subprocess.run(cmd, capture_output=True, timeout=1.0)
            if result.returncode != 0:
                self._node.get_logger().debug(
                    f'[camera_ctrl] v4l2 {dev} {ctrl}={value}: '
                    + result.stderr.decode().strip()
                )
        except (subprocess.TimeoutExpired, FileNotFoundError) as exc:
            self._node.get_logger().debug(f'[camera_ctrl] v4l2-ctl unavailable: {exc}')
--- a/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/gpio_headlight.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/gpio_headlight.py
@ -0,0 +1,192 @@
 """
 gpio_headlight.py — Jetson Orin Nano Super 40-pin GPIO headlight control.
 Physical pin 33 on the J26 40-pin header is used for the headlight.
 On JetPack 6 this corresponds to sysfs GPIO chip gpio-tegra234 / offset 12.
 Brightness levels (0.0–1.0):
  0.0  → headlight off
  0.25 → dim       (e.g. indicator, twilight assist)
  0.50 → medium    (normal dark indoor)
  1.0  → full      (outdoor night, ir_only mode)
 Implementation:
  1. Tries Jetson.GPIO PWM (RPi-compatible API, available if jetson-gpio installed)
  2. Falls back to sysfs binary (on/off only) via /sys/class/gpio
  3. If sysfs export fails (permissions / virtual env), logs a warning and no-ops.
 Sysfs GPIO path on Orin Nano Super (JetPack 6):
  /sys/class/gpio/gpiochip<N>/  — use the tegra234 chip
  Or the legacy /sys/class/gpio/gpio<number>/ path after export.
 Note: udev rule or `sudo` may be needed in production.
  Add to /etc/udev/rules.d/99-jetson-gpio.rules:
    SUBSYSTEM=="gpio", ACTION=="add", RUN+="/bin/chmod 777 /sys/class/gpio"
 """
 from __future__ import annotations
 import logging
 import os
 from typing import Optional
 _LOG = logging.getLogger(__name__)
 # Physical board pin for the headlight
 _BOARD_PIN = 33
 # Brightness → duty cycle string mapping for sysfs PWM (when available)
 _LEVEL_DUTY: dict[str, float] = {
    'off':    0.0,
    'dim':    0.25,
    'medium': 0.50,
    'full':   1.0,
 }
 class GPIOHeadlight:
    """Controls the GPIO headlight with graceful fallbacks."""
    def __init__(self, pin: int = _BOARD_PIN):
        self._pin = pin
        self._brightness: float = 0.0
        self._backend: str = 'none'
        self._pwm = None
        self._gpio_path: Optional[str] = None
        self._init()
    # ── Public API ────────────────────────────────────────────────────────────
    def set_brightness(self, level: float) -> None:
        """Set headlight brightness in [0.0, 1.0]."""
        level = max(0.0, min(1.0, level))
        if level == self._brightness:
            return
        self._brightness = level
        self._apply(level)
    def set_level(self, name: str) -> None:
        """Set headlight to a named level: 'off', 'dim', 'medium', 'full'."""
        self.set_brightness(_LEVEL_DUTY.get(name, 0.0))
    def off(self) -> None:
        self.set_brightness(0.0)
    def cleanup(self) -> None:
        self.off()
        if self._pwm is not None:
            try:
                self._pwm.stop()
                import Jetson.GPIO as GPIO
                GPIO.cleanup()
            except Exception:
                pass
        elif self._gpio_path:
            self._gpio_write('0')
    # ── Init backends ─────────────────────────────────────────────────────────
    def _init(self) -> None:
        # Try Jetson.GPIO PWM first
        try:
            import Jetson.GPIO as GPIO
            GPIO.setmode(GPIO.BOARD)
            GPIO.setup(self._pin, GPIO.OUT, initial=GPIO.LOW)
            self._pwm = GPIO.PWM(self._pin, 1000)   # 1 kHz PWM
            self._pwm.start(0)
            self._backend = 'jetson_gpio_pwm'
            _LOG.info(f'[headlight] Jetson.GPIO PWM on pin {self._pin}')
            return
        except ImportError:
            _LOG.debug('[headlight] Jetson.GPIO not available')
        except Exception as exc:
            _LOG.warning(f'[headlight] Jetson.GPIO init failed: {exc}')
        # Fall back to sysfs binary
        gpio_num = self._board_pin_to_sysfs(self._pin)
        if gpio_num is not None:
            path = f'/sys/class/gpio/gpio{gpio_num}'
            if self._sysfs_export(gpio_num):
                self._gpio_path = path
                self._backend = 'sysfs'
                _LOG.info(f'[headlight] sysfs GPIO /sys/class/gpio/gpio{gpio_num}')
                return
        _LOG.warning('[headlight] No GPIO backend available — headlight is simulated')
        self._backend = 'sim'
    # ── Apply ─────────────────────────────────────────────────────────────────
    def _apply(self, level: float) -> None:
        if self._backend == 'jetson_gpio_pwm':
            try:
                self._pwm.ChangeDutyCycle(level * 100.0)
            except Exception as exc:
                _LOG.warning(f'[headlight] PWM error: {exc}')
        elif self._backend == 'sysfs':
            self._gpio_write('1' if level > 0 else '0')
        # sim: no-op
    # ── Sysfs helpers ─────────────────────────────────────────────────────────
    @staticmethod
    def _board_pin_to_sysfs(board_pin: int) -> Optional[int]:
        """
        Map physical 40-pin board pin to sysfs GPIO number on Orin Nano Super.
        Only a subset is mapped here; extend as needed.
        """
        # Partial map of Orin Nano Super 40-pin header (physical → sysfs offset)
        _ORIN_MAP: dict[int, int] = {
            7:  106,
            11: 112,
            12: 50,
            13: 108,
            15: 109,
            16: 85,
            18: 110,
            19: 130,
            21: 128,
            22: 111,
            23: 129,
            24: 131,
            26: 132,
            29: 118,
            31: 120,
            32: 122,
            33: 200,   # headlight pin
            35: 116,
            36: 54,
            37: 121,
            38: 117,
            40: 56,
        }
        return _ORIN_MAP.get(board_pin)
    def _sysfs_export(self, gpio_num: int) -> bool:
        export_path = f'/sys/class/gpio/gpio{gpio_num}'
        if not os.path.exists(export_path):
            try:
                with open('/sys/class/gpio/export', 'w') as f:
                    f.write(str(gpio_num))
            except PermissionError:
                _LOG.warning('[headlight] No permission for /sys/class/gpio/export')
                return False
            except OSError as exc:
                _LOG.warning(f'[headlight] sysfs export error: {exc}')
                return False
        try:
            with open(f'{export_path}/direction', 'w') as f:
                f.write('out')
            return True
        except OSError as exc:
            _LOG.warning(f'[headlight] sysfs direction error: {exc}')
            return False
    def _gpio_write(self, value: str) -> None:
        if not self._gpio_path:
            return
        try:
            with open(f'{self._gpio_path}/value', 'w') as f:
                f.write(value)
        except OSError as exc:
            _LOG.warning(f'[headlight] sysfs write error: {exc}')
--- a/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/ir_slam_bridge_node.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/ir_slam_bridge_node.py
@ -0,0 +1,157 @@
 """
 ir_slam_bridge_node.py — Bridge D435i IR1 stream to RGB-style topics for SLAM.
 When the vision mode is 'night' or 'ir_only', visible-light cameras are
 unusable. RTAB-Map (and other SLAM front-ends) expect a colour image. This
 node re-publishes the D435i infra1 (mono8) stream as fake RGB (rgb8) so that
 RTAB-Map's visual front-end continues to operate without reconfiguration.
 Also re-publishes the camera_info with the same frame_id so that the pipeline
 stays consistent.
 When mode is 'day' or 'twilight' the bridge is inactive (no republish) to
 avoid double-feeding RTAB-Map.
 Subscribes:
  /saltybot/vision_mode                std_msgs/String        mode gate
  /camera/infra1/image_rect_raw        sensor_msgs/Image      IR source (mono8)
  /camera/infra1/camera_info           sensor_msgs/CameraInfo IR intrinsics
 Publishes (when active):
  /saltybot/ir_bridge/image_raw        sensor_msgs/Image      grey-as-rgb8
  /saltybot/ir_bridge/camera_info      sensor_msgs/CameraInfo passthrough
 Parameters:
  active_modes    str   'night,ir_only'   (comma-separated)
  output_encoding str   'rgb8'
 """
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
 import numpy as np
 import cv2
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image, CameraInfo
 from std_msgs.msg import String
 _SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=5,
 )
 _LATCHED_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=1,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,
 )
 _RELIABLE_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
 )
 class IRSlamBridgeNode(Node):
    def __init__(self):
        super().__init__('ir_slam_bridge')
        self.declare_parameter('active_modes',    'night,ir_only')
        self.declare_parameter('output_encoding', 'rgb8')
        active_str = self.get_parameter('active_modes').value
        self._active_modes: set[str] = {m.strip() for m in active_str.split(',')}
        self._out_enc: str           = self.get_parameter('output_encoding').value
        self._bridge = CvBridge()
        self._active = False
        self._latest_info: CameraInfo | None = None
        # Mode gate
        self.create_subscription(
            String, '/saltybot/vision_mode', self._on_mode, _RELIABLE_QOS
        )
        # IR source
        self.create_subscription(
            Image, '/camera/infra1/image_rect_raw', self._on_ir_image, _SENSOR_QOS
        )
        self.create_subscription(
            CameraInfo, '/camera/infra1/camera_info', self._on_camera_info, _LATCHED_QOS
        )
        # Bridge output
        self._img_pub  = self.create_publisher(
            Image,      '/saltybot/ir_bridge/image_raw',   10
        )
        self._info_pub = self.create_publisher(
            CameraInfo, '/saltybot/ir_bridge/camera_info', 10
        )
        self.get_logger().info(
            f'ir_slam_bridge ready — active in modes: {sorted(self._active_modes)}'
        )
    # ── Subscriptions ─────────────────────────────────────────────────────────
    def _on_mode(self, msg: String) -> None:
        was_active = self._active
        self._active = msg.data in self._active_modes
        if self._active != was_active:
            state = 'ACTIVE' if self._active else 'idle'
            self.get_logger().info(
                f'[ir_bridge] mode={msg.data} → bridge {state}'
            )
    def _on_camera_info(self, msg: CameraInfo) -> None:
        self._latest_info = msg
    def _on_ir_image(self, msg: Image) -> None:
        if not self._active:
            return
        try:
            mono = self._bridge.imgmsg_to_cv2(msg, desired_encoding='mono8')
        except Exception as exc:
            self.get_logger().warning(f'[ir_bridge] decode error: {exc}')
            return
        # Convert mono8 → target encoding
        if self._out_enc == 'rgb8':
            colour = cv2.cvtColor(mono, cv2.COLOR_GRAY2RGB)
        elif self._out_enc == 'bgr8':
            colour = cv2.cvtColor(mono, cv2.COLOR_GRAY2BGR)
        else:
            colour = mono   # passthrough
        try:
            out_msg = self._bridge.cv2_to_imgmsg(colour, encoding=self._out_enc)
        except Exception as exc:
            self.get_logger().warning(f'[ir_bridge] encode error: {exc}')
            return
        out_msg.header = msg.header
        self._img_pub.publish(out_msg)
        if self._latest_info is not None:
            self._latest_info.header = msg.header
            self._info_pub.publish(self._latest_info)
 def main(args=None):
    rclpy.init(args=args)
    node = IRSlamBridgeNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()
 if __name__ == '__main__':
    main()
--- a/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/light_analyser.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/light_analyser.py
@ -0,0 +1,107 @@
 """
 light_analyser.py — Histogram-based ambient light analysis and vision mode FSM.
 Vision modes (string keys used on /saltybot/vision_mode):
  'day'      — full colour, all cameras nominal
  'twilight' — dim light, start boosting IMX219 gain
  'night'    — dark, max gain + headlight on, IR emitter off
  'ir_only'  — effectively no ambient light, switch to D435i IR stereo only
 Transitions use hysteresis to avoid rapid toggling:
  Promotion  (to darker mode): threshold - HYST
  Demotion   (to lighter mode): threshold + HYST
 """
 from __future__ import annotations
 import numpy as np
 from typing import Tuple
 # ── Thresholds (0–255 mean grey intensity) ───────────────────────────────────
 # Modes:   ir_only < NIGHT_THR <= night < TWILIGHT_THR <= twilight < DAY_THR <= day
 _TWILIGHT_THR = 110   # below → twilight
 _NIGHT_THR    = 45    # below → night
 _IR_ONLY_THR  = 15    # below → ir_only
 _HYST         = 8     # hysteresis band (half-width)
 # Ordered list of modes darkest → brightest
 _MODES = ('ir_only', 'night', 'twilight', 'day')
 # Per-mode thresholds: (promote_below, demote_above)
 #   promote_below — if median < this, enter the next darker mode
 #   demote_above  — if median > this, enter the next lighter mode
 #
 # Hysteresis: promote threshold is thr - HYST; demote is thr + HYST
 # where thr is the boundary between THIS mode and the adjacent lighter mode.
 _THRESHOLDS = {
    #          (promote_below,              demote_above)
    'day':      (_TWILIGHT_THR - _HYST,     None),
    'twilight': (_NIGHT_THR    - _HYST,     _TWILIGHT_THR + _HYST),
    'night':    (_IR_ONLY_THR  - _HYST,     _NIGHT_THR    + _HYST),
    'ir_only':  (None,                      _IR_ONLY_THR  + _HYST),
 }
 class LightAnalyser:
    """Stateful ambient-light classifier with hysteresis."""
    def __init__(
        self,
        initial_mode: str = 'day',
        history_len: int = 6,
    ):
        if initial_mode not in _MODES:
            raise ValueError(f'Unknown mode: {initial_mode}')
        self._mode = initial_mode
        self._history: list[float] = []
        self._history_len = history_len
    # ── Public API ────────────────────────────────────────────────────────────
    @property
    def mode(self) -> str:
        return self._mode
    def update(self, frame: np.ndarray) -> Tuple[str, float]:
        """
        Analyse a new image frame (any colour space; converted to grey internally).
        Returns:
            (mode, mean_intensity)  —  mode string + 0–255 grey mean
        """
        intensity = self._frame_intensity(frame)
        self._history.append(intensity)
        if len(self._history) > self._history_len:
            self._history.pop(0)
        smooth = float(np.median(self._history))
        self._mode = self._next_mode(self._mode, smooth)
        return self._mode, smooth
    # ── Internal helpers ──────────────────────────────────────────────────────
    @staticmethod
    def _frame_intensity(frame: np.ndarray) -> float:
        """Return mean pixel intensity of the frame as float in [0, 255]."""
        if frame.ndim == 3:
            # RGB / BGR → greyscale
            grey = np.mean(frame, axis=2).astype(np.float32)
        else:
            grey = frame.astype(np.float32)
        return float(grey.mean())
    @staticmethod
    def _next_mode(current: str, intensity: float) -> str:
        """Apply hysteresis FSM to decide next mode."""
        idx = _MODES.index(current)
        # Try promotion (→ darker mode) first
        lo, hi = _THRESHOLDS[current]
        if lo is not None and intensity < lo:
            return _MODES[max(0, idx - 1)]
        # Try demotion (→ lighter mode)
        if hi is not None and intensity > hi:
            return _MODES[min(len(_MODES) - 1, idx + 1)]
        return current
--- a/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/light_monitor_node.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/light_monitor_node.py
@ -0,0 +1,141 @@
 """
 light_monitor_node.py — Ambient light monitor for SaltyBot night vision.
 Uses the front IMX219 RGB stream (or D435i infra1 as fallback) to estimate
 ambient brightness via histogram analysis.  Publishes the current vision mode
 and raw intensity on a 2 Hz digest timer.
 Subscribes (any ONE active at a time):
  /camera/color/image_raw          sensor_msgs/Image   RGB from IMX219 front (primary)
  /camera/infra1/image_rect_raw    sensor_msgs/Image   IR fallback (mono8)
 Publishes:
  /saltybot/ambient_light          std_msgs/Float32    mean grey intensity 0–255
  /saltybot/vision_mode            std_msgs/String     'day'|'twilight'|'night'|'ir_only'
 Parameters:
  primary_camera_topic    str   '/camera/color/image_raw'
  fallback_ir_topic       str   '/camera/infra1/image_rect_raw'
  publish_hz              float  2.0
  initial_mode            str   'day'
  history_len             int    6
 """
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
 import numpy as np
 import cv2
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image
 from std_msgs.msg import Float32, String
 from .light_analyser import LightAnalyser
 _SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=3,
 )
 class LightMonitorNode(Node):
    def __init__(self):
        super().__init__('light_monitor')
        self.declare_parameter('primary_camera_topic',  '/camera/color/image_raw')
        self.declare_parameter('fallback_ir_topic',     '/camera/infra1/image_rect_raw')
        self.declare_parameter('publish_hz',             2.0)
        self.declare_parameter('initial_mode',           'day')
        self.declare_parameter('history_len',             6)
        primary_topic  = self.get_parameter('primary_camera_topic').value
        fallback_topic = self.get_parameter('fallback_ir_topic').value
        pub_hz         = self.get_parameter('publish_hz').value
        initial_mode   = self.get_parameter('initial_mode').value
        history_len    = self.get_parameter('history_len').value
        self._bridge   = CvBridge()
        self._analyser = LightAnalyser(initial_mode=initial_mode, history_len=history_len)
        # Latest decoded frame (thread-safe in single-threaded executor)
        self._latest_frame: np.ndarray | None = None
        self._using_fallback = False
        # Subscriptions — primary preferred; fallback used when primary silent
        self.create_subscription(Image, primary_topic,  self._on_primary,  _SENSOR_QOS)
        self.create_subscription(Image, fallback_topic, self._on_fallback,  _SENSOR_QOS)
        self._mode_pub  = self.create_publisher(String,  '/saltybot/vision_mode',   10)
        self._light_pub = self.create_publisher(Float32, '/saltybot/ambient_light', 10)
        self.create_timer(1.0 / pub_hz, self._tick)
        self.get_logger().info(
            f'light_monitor ready — primary={primary_topic} fallback={fallback_topic}'
        )
    # ── Subscriptions ─────────────────────────────────────────────────────────
    def _on_primary(self, msg: Image) -> None:
        try:
            frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
            self._latest_frame  = frame
            self._using_fallback = False
        except Exception as exc:
            self.get_logger().warning(f'[light_monitor] primary decode: {exc}')
    def _on_fallback(self, msg: Image) -> None:
        # Only use fallback if no primary frame recently arrived
        if self._latest_frame is not None and not self._using_fallback:
            return   # primary is active
        try:
            frame = self._bridge.imgmsg_to_cv2(msg, desired_encoding='mono8')
            self._latest_frame  = frame
            self._using_fallback = True
        except Exception as exc:
            self.get_logger().warning(f'[light_monitor] fallback decode: {exc}')
    # ── Publish timer ─────────────────────────────────────────────────────────
    def _tick(self) -> None:
        if self._latest_frame is None:
            return   # no data yet
        # Subsample for speed (analyse quarter-resolution)
        frame = self._latest_frame
        if frame.shape[0] > 120:
            frame = frame[::4, ::4]
        mode, intensity = self._analyser.update(frame)
        msg_mode          = String()
        msg_mode.data     = mode
        self._mode_pub.publish(msg_mode)
        msg_light         = Float32()
        msg_light.data    = float(intensity)
        self._light_pub.publish(msg_light)
        if self._using_fallback:
            self.get_logger().debug(
                f'[light_monitor] IR fallback — mode={mode} intensity={intensity:.1f}'
            )
 def main(args=None):
    rclpy.init(args=args)
    node = LightMonitorNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()
 if __name__ == '__main__':
    main()
--- a/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/night_vision_controller_node.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/saltybot_night_vision/night_vision_controller_node.py
@ -0,0 +1,137 @@
 """
 night_vision_controller_node.py — Hardware adaptation on vision mode changes.
 Subscribes to /saltybot/vision_mode and drives:
  • D435i IR emitter (via realsense2_camera parameter service)
  • IMX219 gain / exposure (via v4l2-ctl subprocess)
  • GPIO headlight brightness
 Mode → hardware mapping:
  'day'      → emitter=auto, IMX219 AE, headlight off
  'twilight' → emitter=on,   IMX219 boosted, headlight dim
  'night'    → emitter=on,   IMX219 max gain, headlight medium
  'ir_only'  → emitter=on,   IMX219 max gain, headlight full
 Publishes:
  /saltybot/visual_odom_status  std_msgs/String  (JSON — reuses VO status topic)
 Parameters:
  headlight_pin    int    33     (physical 40-pin board pin)
  day_headlight    float  0.0
  twilight_headlight float 0.25
  night_headlight  float  0.50
  ir_only_headlight float 1.0
 """
 import json
 import time
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
 from std_msgs.msg import String
 from .camera_controller import CameraController
 from .gpio_headlight import GPIOHeadlight
 _RELIABLE_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
 )
 class NightVisionControllerNode(Node):
    def __init__(self):
        super().__init__('night_vision_controller')
        self.declare_parameter('headlight_pin',       33)
        self.declare_parameter('day_headlight',        0.0)
        self.declare_parameter('twilight_headlight',   0.25)
        self.declare_parameter('night_headlight',      0.50)
        self.declare_parameter('ir_only_headlight',    1.0)
        pin = self.get_parameter('headlight_pin').value
        self._brightness_map = {
            'day':      self.get_parameter('day_headlight').value,
            'twilight': self.get_parameter('twilight_headlight').value,
            'night':    self.get_parameter('night_headlight').value,
            'ir_only':  self.get_parameter('ir_only_headlight').value,
        }
        self._cam_ctrl   = CameraController(self)
        self._headlight  = GPIOHeadlight(pin=pin)
        self._cur_mode: str | None = None
        self._mode_change_t: float = 0.0
        self.create_subscription(
            String, '/saltybot/vision_mode', self._on_mode, _RELIABLE_QOS
        )
        self._status_pub = self.create_publisher(
            String, '/saltybot/night_vision_status', 10
        )
        self.create_timer(5.0, self._status_tick)
        self.get_logger().info(
            f'night_vision_controller ready — headlight pin={pin}'
        )
    # ── Subscription ──────────────────────────────────────────────────────────
    def _on_mode(self, msg: String) -> None:
        mode = msg.data
        if mode == self._cur_mode:
            return
        prev = self._cur_mode
        self._cur_mode = mode
        self._mode_change_t = time.monotonic()
        # Apply camera profile (D435i emitter + IMX219 gain/exposure)
        self._cam_ctrl.set_profile(mode)
        # Apply headlight
        brightness = self._brightness_map.get(mode, 0.0)
        self._headlight.set_brightness(brightness)
        self.get_logger().info(
            f'[nv_ctrl] {prev} → {mode} | '
            f'headlight={brightness:.2f}'
        )
    # ── Status heartbeat ──────────────────────────────────────────────────────
    def _status_tick(self) -> None:
        status = {
            'mode':              self._cur_mode or 'unknown',
            'headlight':         round(self._headlight._brightness, 2),
            'mode_age_s':        round(time.monotonic() - self._mode_change_t, 1)
                                 if self._cur_mode else None,
        }
        msg      = String()
        msg.data = json.dumps(status)
        self._status_pub.publish(msg)
    # ── Cleanup ───────────────────────────────────────────────────────────────
    def destroy_node(self) -> None:
        self._headlight.cleanup()
        super().destroy_node()
 def main(args=None):
    rclpy.init(args=args)
    node = NightVisionControllerNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()
 if __name__ == '__main__':
    main()
--- a/jetson/ros2_ws/src/saltybot_night_vision/setup.cfg
+++ b/jetson/ros2_ws/src/saltybot_night_vision/setup.cfg
@ -0,0 +1,4 @@
 [develop]
 script_dir=$base/lib/saltybot_night_vision
 [install]
 install_scripts=$base/lib/saltybot_night_vision
--- a/jetson/ros2_ws/src/saltybot_night_vision/setup.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/setup.py
@ -0,0 +1,34 @@
 from setuptools import setup, find_packages
 import os
 from glob import glob
 package_name = 'saltybot_night_vision'
 setup(
    name=package_name,
    version='0.1.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' + package_name + '/launch',
            glob('launch/*.launch.py')),
        ('share/' + package_name + '/config',
            glob('config/*.yaml')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='SaltyLab',
    maintainer_email='robot@saltylab.local',
    description='Night vision mode: IR emitter, headlight, ambient-light FSM, IR SLAM bridge',
    license='MIT',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'light_monitor        = saltybot_night_vision.light_monitor_node:main',
            'night_vision_ctrl   = saltybot_night_vision.night_vision_controller_node:main',
            'ir_slam_bridge      = saltybot_night_vision.ir_slam_bridge_node:main',
        ],
    },
 )
--- a/jetson/ros2_ws/src/saltybot_night_vision/test/test_night_vision.py
+++ b/jetson/ros2_ws/src/saltybot_night_vision/test/test_night_vision.py
@ -0,0 +1,152 @@
 """
 test_night_vision.py — Unit tests for LightAnalyser and GPIOHeadlight.
 Runs without ROS2 / Jetson hardware (no rclpy, no GPIO).
 """
 import sys
 import os
 import math
 import numpy as np
 import pytest
 sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
 from saltybot_night_vision.light_analyser import LightAnalyser
 from saltybot_night_vision.gpio_headlight import GPIOHeadlight
 # ── LightAnalyser tests ───────────────────────────────────────────────────────
 class TestLightAnalyser:
    def _bright_frame(self, value: int = 180) -> np.ndarray:
        return np.full((480, 640), value, dtype=np.uint8)
    def _dark_frame(self, value: int = 5) -> np.ndarray:
        return np.full((480, 640), value, dtype=np.uint8)
    def test_initial_mode_default(self):
        la = LightAnalyser()
        assert la.mode == 'day'
    def test_bright_frame_stays_day(self):
        la = LightAnalyser()
        for _ in range(10):
            mode, _ = la.update(self._bright_frame(180))
        assert mode == 'day'
    def test_dark_frame_transitions_to_ir_only(self):
        la = LightAnalyser()
        for _ in range(10):
            mode, _ = la.update(self._dark_frame(3))
        assert mode == 'ir_only'
    def test_medium_intensity_twilight(self):
        la = LightAnalyser()
        frame = np.full((480, 640), 75, dtype=np.uint8)
        for _ in range(10):
            mode, _ = la.update(frame)
        assert mode == 'twilight'
    def test_night_range(self):
        la = LightAnalyser()
        frame = np.full((480, 640), 25, dtype=np.uint8)
        for _ in range(10):
            mode, _ = la.update(frame)
        assert mode == 'night'
    def test_intensity_value_correct(self):
        la = LightAnalyser()
        frame = np.full((60, 80), 128, dtype=np.uint8)
        _, intensity = la.update(frame)
        assert intensity == pytest.approx(128.0, abs=0.5)
    def test_rgb_frame_accepted(self):
        la = LightAnalyser()
        frame = np.full((480, 640, 3), 60, dtype=np.uint8)
        mode, intensity = la.update(frame)
        assert isinstance(mode, str)
        assert 0.0 <= intensity <= 255.0
    def test_hysteresis_prevents_oscillation(self):
        """Frame right at threshold should not cause mode flip on each tick."""
        la = LightAnalyser()
        # Start from day, push to just below twilight threshold
        frame = np.full((480, 640), 100, dtype=np.uint8)
        modes: list[str] = []
        for _ in range(20):
            mode, _ = la.update(frame)
            modes.append(mode)
        # Should not oscillate — all same mode
        assert len(set(modes[-10:])) == 1
    def test_recovery_to_day(self):
        la = LightAnalyser(initial_mode='ir_only')
        bright = np.full((480, 640), 200, dtype=np.uint8)
        for _ in range(20):
            mode, _ = la.update(bright)
        assert mode == 'day'
 # ── GPIOHeadlight tests (simulated — no hardware) ─────────────────────────────
 class TestGPIOHeadlightSim:
    """
    Tests that GPIOHeadlight correctly tracks brightness internally
    even when running in simulation mode (no hardware GPIO).
    """
    def test_default_brightness_zero(self):
        hl = GPIOHeadlight()
        assert hl._brightness == pytest.approx(0.0)
    def test_set_brightness_clamps_high(self):
        hl = GPIOHeadlight()
        hl.set_brightness(2.5)
        assert hl._brightness == pytest.approx(1.0)
    def test_set_brightness_clamps_low(self):
        hl = GPIOHeadlight()
        hl.set_brightness(-0.5)
        assert hl._brightness == pytest.approx(0.0)
    def test_set_level_off(self):
        hl = GPIOHeadlight()
        hl.set_brightness(0.8)
        hl.set_level('off')
        assert hl._brightness == pytest.approx(0.0)
    def test_set_level_full(self):
        hl = GPIOHeadlight()
        hl.set_level('full')
        assert hl._brightness == pytest.approx(1.0)
    def test_set_level_dim(self):
        hl = GPIOHeadlight()
        hl.set_level('dim')
        assert hl._brightness == pytest.approx(0.25)
    def test_off_method(self):
        hl = GPIOHeadlight()
        hl.set_brightness(0.75)
        hl.off()
        assert hl._brightness == pytest.approx(0.0)
    def test_cleanup_sets_zero(self):
        hl = GPIOHeadlight()
        hl.set_brightness(0.9)
        hl.cleanup()
        assert hl._brightness == pytest.approx(0.0)
    def test_no_op_on_same_brightness(self):
        """Repeated set of same value should not raise."""
        hl = GPIOHeadlight()
        hl.set_brightness(0.5)
        hl.set_brightness(0.5)   # should be no-op, no error
        assert hl._brightness == pytest.approx(0.5)
 if __name__ == '__main__':
    pytest.main([__file__, '-v'])
--- a/src/audio.c
+++ b/src/audio.c
@ -0,0 +1,352 @@
 #include "audio.h"
 #include "config.h"
 #include "stm32f7xx_hal.h"
 #include <string.h>
 /* ================================================================
 * Buffer layout
 * ================================================================
 * AUDIO_BUF_HALF samples per DMA half (config.h: 441 at 22050 Hz → 20 ms).
 * DMA runs in circular mode over 2 halves; TxHalfCplt refills [0] and
 * TxCplt refills [AUDIO_BUF_HALF].  Both callbacks simply call fill_half().
 */
 #define AUDIO_BUF_SIZE    (AUDIO_BUF_HALF * 2u)
 /* DMA buffer: must be in non-cached SRAM or flushed before DMA access.
 * Placed in SRAM1 (default .bss section on STM32F722 — below 512 KB).
 * The I-cache / D-cache is on by default; DMA1 is an AHB master that
 * bypasses cache, so we keep this buffer in DTCM-uncacheable SRAM. */
 static int16_t s_dma_buf[AUDIO_BUF_SIZE];
 /* ================================================================
 * PCM FIFO — Jetson audio (main-loop writer, ISR reader)
 * ================================================================
 * Single-producer / single-consumer lock-free ring buffer.
 * Power-of-2 size so wrap uses bitwise AND (safe across ISR boundary).
 * 4096 samples ≈ 185 ms at 22050 Hz — enough to absorb JLink jitter.
 */
 #define PCM_FIFO_SIZE   4096u
 #define PCM_FIFO_MASK   (PCM_FIFO_SIZE - 1u)
 static int16_t           s_pcm_fifo[PCM_FIFO_SIZE];
 static volatile uint16_t s_pcm_rd = 0;   /* consumer (ISR advances) */
 static volatile uint16_t s_pcm_wr = 0;   /* producer (main loop advances) */
 /* ================================================================
 * Tone sequencer
 * ================================================================
 * Each AudioTone maps to a const ToneStep array.  Steps are played
 * sequentially; a gap_ms of 0 means no silence between steps.
 * audio_tick() in the main loop advances the state machine and
 * writes the volatile active-tone params read by the ISR fill path.
 */
 typedef struct {
    uint16_t freq_hz;
    uint16_t dur_ms;
    uint16_t gap_ms;   /* silence after this step */
 } ToneStep;
 /* ---- Tone definitions ---- */
 static const ToneStep s_def_beep_short[]  = {{880,  100, 0}};
 static const ToneStep s_def_beep_long[]   = {{880,  500, 0}};
 static const ToneStep s_def_startup[]     = {{523,  120, 60},
                                              {659,  120, 60},
                                              {784,  200, 0}};
 static const ToneStep s_def_arm[]         = {{880,   80, 60},
                                              {1047, 100, 0}};
 static const ToneStep s_def_disarm[]      = {{880,   80, 60},
                                              {659,  100, 0}};
 static const ToneStep s_def_fault[]       = {{200,  500, 0}};
 typedef struct {
    const ToneStep *steps;
    uint8_t n_steps;
 } ToneDef;
 static const ToneDef s_tone_defs[AUDIO_TONE_COUNT] = {
    [AUDIO_TONE_BEEP_SHORT] = {s_def_beep_short, 1},
    [AUDIO_TONE_BEEP_LONG]  = {s_def_beep_long,  1},
    [AUDIO_TONE_STARTUP]    = {s_def_startup,     3},
    [AUDIO_TONE_ARM]        = {s_def_arm,         2},
    [AUDIO_TONE_DISARM]     = {s_def_disarm,      2},
    [AUDIO_TONE_FAULT]      = {s_def_fault,       1},
 };
 /* Active tone queue */
 #define TONE_QUEUE_DEPTH  4u
 typedef struct {
    const ToneDef *def;
    uint8_t  step;           /* current step index within def */
    bool     in_gap;         /* true while playing inter-step silence */
    uint32_t step_end_ms;    /* abs HAL_GetTick() when step/gap expires */
 } ToneSeq;
 static ToneSeq   s_tone_q[TONE_QUEUE_DEPTH];
 static uint8_t   s_tq_head = 0;   /* consumer (audio_tick reads) */
 static uint8_t   s_tq_tail = 0;   /* producer (audio_play_tone writes) */
 /* Volatile parameters written by audio_tick(), read by ISR fill_half() */
 static volatile uint16_t s_active_freq  = 0;   /* 0 = silence / gap */
 static volatile uint32_t s_active_phase = 0;   /* sample phase counter */
 /* ================================================================
 * Volume
 * ================================================================ */
 static uint8_t s_volume = AUDIO_VOLUME_DEFAULT;  /* 0–100 */
 /* ================================================================
 * HAL handles
 * ================================================================ */
 static I2S_HandleTypeDef s_i2s;
 static DMA_HandleTypeDef s_dma_tx;
 /* ================================================================
 * fill_half() — called from ISR, O(AUDIO_BUF_HALF)
 * ================================================================
 * Priority: PCM FIFO (Jetson TTS) > square-wave tone > silence.
 * Volume applied via integer scaling — no float in ISR.
 */
 static void fill_half(int16_t *buf, uint16_t n)
 {
    uint16_t rd    = s_pcm_rd;
    uint16_t wr    = s_pcm_wr;
    uint16_t avail = (uint16_t)((wr - rd) & PCM_FIFO_MASK);
    if (avail >= n) {
        /* ---- Drain Jetson PCM FIFO ---- */
        for (uint16_t i = 0; i < n; i++) {
            int32_t s = (int32_t)s_pcm_fifo[rd] * (int32_t)s_volume / 100;
            buf[i] = (s > 32767) ? (int16_t)32767 :
                     (s < -32768) ? (int16_t)-32768 : (int16_t)s;
            rd = (rd + 1u) & PCM_FIFO_MASK;
        }
        s_pcm_rd = rd;
    } else if (s_active_freq) {
        /* ---- Square wave tone generator ---- */
        uint32_t half_p = (uint32_t)AUDIO_SAMPLE_RATE / (2u * s_active_freq);
        int16_t  amp    = (int16_t)(16384u * (uint32_t)s_volume / 100u);
        uint32_t ph     = s_active_phase;
        uint32_t period = 2u * half_p;
        for (uint16_t i = 0; i < n; i++) {
            buf[i] = ((ph % period) < half_p) ? amp : (int16_t)(-amp);
            ph++;
        }
        s_active_phase = ph;
    } else {
        /* ---- Silence ---- */
        memset(buf, 0, (size_t)(n * 2u));
    }
 }
 /* ================================================================
 * DMA callbacks (ISR context)
 * ================================================================ */
 void HAL_I2S_TxHalfCpltCallback(I2S_HandleTypeDef *hi2s)
 {
    if (hi2s->Instance == SPI3)
        fill_half(s_dma_buf, AUDIO_BUF_HALF);
 }
 void HAL_I2S_TxCpltCallback(I2S_HandleTypeDef *hi2s)
 {
    if (hi2s->Instance == SPI3)
        fill_half(s_dma_buf + AUDIO_BUF_HALF, AUDIO_BUF_HALF);
 }
 /* DMA1 Stream7 IRQ (SPI3/I2S3 TX) */
 void DMA1_Stream7_IRQHandler(void)
 {
    HAL_DMA_IRQHandler(&s_dma_tx);
 }
 /* ================================================================
 * audio_init()
 * ================================================================ */
 void audio_init(void)
 {
    /* ---- PLLI2S: N=192, R=2 → 96 MHz I2S clock ----
     * With SPI3/I2S3 and I2S_DATAFORMAT_16B (16-bit, 32-bit frame slot):
     *   FS = 96 MHz / (32 × 2 × I2SDIV) where HAL picks I2SDIV = 68
     *   → 96 000 000 / (32 × 2 × 68) = 22 058 Hz  (< 0.04 % error)
     */
    RCC_PeriphCLKInitTypeDef pclk = {0};
    pclk.PeriphClockSelection = RCC_PERIPHCLK_I2S;
    pclk.I2sClockSelection    = RCC_I2SCLKSOURCE_PLLI2S;
    pclk.PLLI2S.PLLI2SN       = 192;   /* VCO = (HSE/PLLM) × 192 = 192 MHz */
    pclk.PLLI2S.PLLI2SR       = 2;     /* I2S clock = 96 MHz */
    HAL_RCCEx_PeriphCLKConfig(&pclk);
    /* ---- GPIO ---- */
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();
    GPIO_InitTypeDef gpio = {0};
    /* PA15: I2S3_WS (LRCLK), AF6 */
    gpio.Pin       = AUDIO_LRCK_PIN;
    gpio.Mode      = GPIO_MODE_AF_PP;
    gpio.Pull      = GPIO_NOPULL;
    gpio.Speed     = GPIO_SPEED_FREQ_VERY_HIGH;
    gpio.Alternate = GPIO_AF6_SPI3;
    HAL_GPIO_Init(AUDIO_LRCK_PORT, &gpio);
    /* PC10: I2S3_CK (BCLK), AF6 */
    gpio.Pin       = AUDIO_BCLK_PIN;
    HAL_GPIO_Init(AUDIO_BCLK_PORT, &gpio);
    /* PB5: I2S3_SD (DIN), AF6 */
    gpio.Pin       = AUDIO_DOUT_PIN;
    HAL_GPIO_Init(AUDIO_DOUT_PORT, &gpio);
    /* PC5: AUDIO_MUTE GPIO output — drive low (muted) initially */
    gpio.Pin       = AUDIO_MUTE_PIN;
    gpio.Mode      = GPIO_MODE_OUTPUT_PP;
    gpio.Pull      = GPIO_NOPULL;
    gpio.Speed     = GPIO_SPEED_FREQ_LOW;
    gpio.Alternate = 0;
    HAL_GPIO_Init(AUDIO_MUTE_PORT, &gpio);
    HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_RESET);
    /* ---- DMA1 Stream7 Channel0 (SPI3/I2S3 TX) ---- */
    __HAL_RCC_DMA1_CLK_ENABLE();
    s_dma_tx.Instance                 = DMA1_Stream7;
    s_dma_tx.Init.Channel             = DMA_CHANNEL_0;
    s_dma_tx.Init.Direction           = DMA_MEMORY_TO_PERIPH;
    s_dma_tx.Init.PeriphInc           = DMA_PINC_DISABLE;
    s_dma_tx.Init.MemInc              = DMA_MINC_ENABLE;
    s_dma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
    s_dma_tx.Init.MemDataAlignment    = DMA_MDATAALIGN_HALFWORD;
    s_dma_tx.Init.Mode                = DMA_CIRCULAR;
    s_dma_tx.Init.Priority            = DMA_PRIORITY_MEDIUM;
    s_dma_tx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
    HAL_DMA_Init(&s_dma_tx);
    __HAL_LINKDMA(&s_i2s, hdmatx, s_dma_tx);
    HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 7, 0);
    HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn);
    /* ---- SPI3 in I2S3 master TX mode ---- */
    __HAL_RCC_SPI3_CLK_ENABLE();
    s_i2s.Instance         = SPI3;
    s_i2s.Init.Mode        = I2S_MODE_MASTER_TX;
    s_i2s.Init.Standard    = I2S_STANDARD_PHILIPS;
    s_i2s.Init.DataFormat  = I2S_DATAFORMAT_16B;
    s_i2s.Init.MCLKOutput  = I2S_MCLKOUTPUT_DISABLE;
    s_i2s.Init.AudioFreq   = I2S_AUDIOFREQ_22K;
    s_i2s.Init.CPOL        = I2S_CPOL_LOW;
    s_i2s.Init.ClockSource = I2S_CLOCK_PLL;
    HAL_I2S_Init(&s_i2s);
    /* Pre-fill with silence and start circular DMA TX */
    memset(s_dma_buf, 0, sizeof(s_dma_buf));
    HAL_I2S_Transmit_DMA(&s_i2s, (uint16_t *)s_dma_buf, AUDIO_BUF_SIZE);
    /* Unmute amp after DMA is running — avoids start-up click */
    HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_SET);
 }
 /* ================================================================
 * Public API
 * ================================================================ */
 void audio_mute(bool active)
 {
    HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN,
                      active ? GPIO_PIN_SET : GPIO_PIN_RESET);
 }
 void audio_set_volume(uint8_t vol)
 {
    s_volume = (vol > 100u) ? 100u : vol;
 }
 bool audio_play_tone(AudioTone tone)
 {
    if (tone >= AUDIO_TONE_COUNT) return false;
    uint8_t next = (s_tq_tail + 1u) % TONE_QUEUE_DEPTH;
    if (next == s_tq_head) return false;  /* queue full */
    s_tone_q[s_tq_tail].def         = &s_tone_defs[tone];
    s_tone_q[s_tq_tail].step        = 0;
    s_tone_q[s_tq_tail].in_gap      = false;
    s_tone_q[s_tq_tail].step_end_ms = 0;  /* audio_tick() sets this on first run */
    s_tq_tail = next;
    return true;
 }
 uint16_t audio_write_pcm(const int16_t *samples, uint16_t n)
 {
    uint16_t wr    = s_pcm_wr;
    uint16_t rd    = s_pcm_rd;
    uint16_t space = (uint16_t)((rd - wr - 1u) & PCM_FIFO_MASK);
    uint16_t accept = (n < space) ? n : space;
    for (uint16_t i = 0; i < accept; i++) {
        s_pcm_fifo[wr] = samples[i];
        wr = (wr + 1u) & PCM_FIFO_MASK;
    }
    s_pcm_wr = wr;
    return accept;
 }
 void audio_tick(uint32_t now_ms)
 {
    /* Nothing to do if queue is empty */
    if (s_tq_head == s_tq_tail) {
        s_active_freq = 0;
        return;
    }
    ToneSeq *seq = &s_tone_q[s_tq_head];
    /* First call for this sequence entry: arm the first step */
    if (seq->step_end_ms == 0u) {
        const ToneStep *st = &seq->def->steps[0];
        seq->in_gap      = false;
        seq->step_end_ms = now_ms + st->dur_ms;
        s_active_freq    = st->freq_hz;
        s_active_phase   = 0;
        return;
    }
    /* Step / gap still running */
    if (now_ms < seq->step_end_ms) return;
    /* Current step or gap has expired */
    const ToneStep *st = &seq->def->steps[seq->step];
    if (!seq->in_gap && st->gap_ms) {
        /* Transition: tone → inter-step gap (silence) */
        seq->in_gap      = true;
        seq->step_end_ms = now_ms + st->gap_ms;
        s_active_freq    = 0;
        return;
    }
    /* Advance to next step */
    seq->step++;
    seq->in_gap = false;
    if (seq->step >= seq->def->n_steps) {
        /* Sequence complete — pop from queue */
        s_tq_head = (s_tq_head + 1u) % TONE_QUEUE_DEPTH;
        s_active_freq = 0;
        return;
    }
    /* Start next step */
    st               = &seq->def->steps[seq->step];
    seq->step_end_ms = now_ms + st->dur_ms;
    s_active_freq    = st->freq_hz;
    s_active_phase   = 0;
 }
 bool audio_is_playing(void)
 {
    return (s_i2s.State == HAL_I2S_STATE_BUSY_TX);
 }
--- a/src/jlink.c
+++ b/src/jlink.c
@ -1,4 +1,5 @@
 #include "jlink.h"
 #include "audio.h"
 #include "config.h"
 #include "stm32f7xx_hal.h"
 #include <string.h>
@ -168,6 +169,13 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
        jlink_state.estop_req = 1u;
        break;
    case JLINK_CMD_AUDIO:
        /* Payload: int16 PCM samples, little-endian, 1..126 samples (2..252 bytes) */
        if (plen >= 2u && (plen & 1u) == 0u) {
            audio_write_pcm((const int16_t *)payload, plen / 2u);
        }
        break;
    default:
        break;
    }
@ -182,7 +190,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
 * Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255,
 * but we cap at 64 for safety).
 */
-#define JLINK_MAX_PAYLOAD   64u
+#define JLINK_MAX_PAYLOAD   252u  /* enlarged for AUDIO chunks (126 × int16) */
 typedef enum {
    PS_WAIT_STX = 0,
--- a/src/main.c
+++ b/src/main.c
@ -18,6 +18,7 @@
 #include "jetson_cmd.h"
 #include "jlink.h"
 #include "ota.h"
 #include "audio.h"
 #include "battery.h"
 #include <math.h>
 #include <string.h>
@ -144,6 +145,10 @@ int main(void) {
    /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
    jlink_init();
    /* Init I2S3 audio amplifier (PC10/PA15/PB5, mute=PC5) */
    audio_init();
    audio_play_tone(AUDIO_TONE_STARTUP);
    /* Init mode manager (RC/autonomous blend; CH6 mode switch) */
    mode_manager_t mode;
    mode_manager_init(&mode);
@ -188,6 +193,9 @@ int main(void) {
        /* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */
        safety_refresh();
        /* Advance audio tone sequencer (non-blocking, call every tick) */
        audio_tick(now);
        /* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
        mode_manager_update(&mode, now);
@ -293,6 +301,7 @@ int main(void) {
        if (safety_arm_ready(now) && bal.state == BALANCE_DISARMED) {
            safety_arm_cancel();
            balance_arm(&bal);
            audio_play_tone(AUDIO_TONE_ARM);
        }
        if (cdc_disarm_request) {
            cdc_disarm_request = 0;
@ -341,6 +350,13 @@ int main(void) {
        }
        /* Latch estop on tilt fault or disarm */
        {
            static uint8_t s_prev_tilt_fault = 0;
            uint8_t tilt_now = (bal.state == BALANCE_TILT_FAULT) ? 1u : 0u;
            if (tilt_now && !s_prev_tilt_fault)
                audio_play_tone(AUDIO_TONE_FAULT);
            s_prev_tilt_fault = tilt_now;
        }
        if (bal.state == BALANCE_TILT_FAULT) {
            motor_driver_estop(&motors);
        } else if (bal.state == BALANCE_DISARMED && motors.estop &&
--- a/src/mpu6000.c
+++ b/src/mpu6000.c
@ -68,8 +68,10 @@ void mpu6000_calibrate(void) {
        sum_gy += raw.gy;
        sum_gz += raw.gz;
        HAL_Delay(1);
-        /* Refresh IWDG every 40ms — safe during re-cal with watchdog running */
+        /* Refresh IWDG every 40ms, starting immediately (i=0) — the gap between
-        if (i % 40 == 39) safety_refresh();
+         * safety_refresh() at the top of the main loop and entry here can be
         * ~10ms, so we must refresh on i=0 to avoid the 50ms IWDG window. */
        if (i % 40 == 0) safety_refresh();
    }
    s_bias_gx = (float)sum_gx / GYRO_CAL_SAMPLES;
--- a/test/test_audio.py
+++ b/test/test_audio.py
@ -0,0 +1,409 @@
 """
 test_audio.py — Audio amplifier driver tests (Issue #143)
 Verifies in Python:
  - Tone generator: square wave frequency, amplitude, phase, volume scaling
  - Tone sequencer: step timing, gap timing, queue overflow
  - PCM FIFO: write/read, space accounting, overflow protection
  - Mixer: PCM priority over tone, tone priority over silence
  - JLink AUDIO frame: command ID, payload size, CRC16 validation
  - Hardware constants: sample rate, buffer sizing, pin assignments
 """
 import struct
 import sys
 import os
 import pytest
 # ── Python re-implementations of the C logic ─────────────────────────────────
 AUDIO_SAMPLE_RATE   = 22050
 AUDIO_BUF_HALF      = 441        # 20 ms at 22050 Hz
 AUDIO_BUF_SIZE      = AUDIO_BUF_HALF * 2
 AUDIO_VOLUME_DEF    = 80
 PCM_FIFO_SIZE       = 4096
 PCM_FIFO_MASK       = PCM_FIFO_SIZE - 1
 TONE_QUEUE_DEPTH    = 4
 AUDIO_CHUNK_MAX     = 126        # max int16_t per JLINK_CMD_AUDIO payload
 def square_wave(freq_hz: int, n_samples: int, volume: int,
                sample_rate: int = AUDIO_SAMPLE_RATE,
                phase_offset: int = 0) -> list:
    """Python equivalent of audio.c fill_half() square-wave branch."""
    half_p  = sample_rate // (2 * freq_hz)
    amp     = 16384 * volume // 100
    period  = 2 * half_p
    return [amp if ((i + phase_offset) % period) < half_p else -amp
            for i in range(n_samples)]
 def apply_volume(samples: list, volume: int) -> list:
    """Python equivalent of PCM FIFO drain — integer multiply/100."""
    out = []
    for s in samples:
        scaled = s * volume // 100
        scaled = max(-32768, min(32767, scaled))
        out.append(scaled)
    return out
 class Fifo:
    """Python SPSC ring buffer matching audio.c PCM FIFO semantics."""
    def __init__(self, size: int = PCM_FIFO_SIZE):
        self.buf  = [0] * size
        self.mask = size - 1
        self.rd   = 0
        self.wr   = 0
    @property
    def avail(self) -> int:
        return (self.wr - self.rd) & self.mask
    @property
    def space(self) -> int:
        return (self.rd - self.wr - 1) & self.mask
    def write(self, samples: list) -> int:
        space   = self.space
        accept  = min(len(samples), space)
        for i in range(accept):
            self.buf[self.wr] = samples[i]
            self.wr = (self.wr + 1) & self.mask
        return accept
    def read(self, n: int) -> list:
        out = []
        for _ in range(min(n, self.avail)):
            out.append(self.buf[self.rd])
            self.rd = (self.rd + 1) & self.mask
        return out
 def _crc16_xmodem(data: bytes) -> int:
    crc = 0x0000
    for b in data:
        crc ^= b << 8
        for _ in range(8):
            crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
    return crc
 def build_audio_frame(samples: list) -> bytes:
    """Build JLINK_CMD_AUDIO frame for given int16_t samples."""
    STX, ETX = 0x02, 0x03
    CMD = 0x08
    payload = struct.pack(f'<{len(samples)}h', *samples)
    body    = bytes([CMD]) + payload
    crc     = _crc16_xmodem(body)
    return bytes([STX, len(body)]) + body + bytes([crc >> 8, crc & 0xFF, ETX])
 # ── Square wave generator ─────────────────────────────────────────────────────
 class TestSquareWave:
    def test_fundamental_frequency(self):
        """Peak-to-peak transitions occur at the right sample intervals."""
        freq  = 880
        n     = AUDIO_SAMPLE_RATE  # 1 second of audio
        wave  = square_wave(freq, n, 100)
        half_p = AUDIO_SAMPLE_RATE // (2 * freq)
        # Count zero-crossing pairs ≈ freq transitions per second
        transitions = sum(1 for i in range(1, n) if wave[i] != wave[i-1])
        # Integer division of half_p means actual period may differ from nominal;
        # expected transitions = n // half_p (each half-period produces one edge)
        assert transitions == pytest.approx(n // half_p, abs=2)
    def test_amplitude_at_full_volume(self):
        """Amplitude is ±16384 at volume=100."""
        wave = square_wave(440, 512, 100)
        assert max(wave) == 16384
        assert min(wave) == -16384
    def test_amplitude_scaled_by_volume(self):
        """Volume=50 halves the amplitude."""
        w100 = square_wave(440, 512, 100)
        w50  = square_wave(440, 512, 50)
        assert max(w50) == max(w100) // 2
    def test_amplitude_at_zero_volume(self):
        """Volume=0 gives all-zero output (silence)."""
        wave = square_wave(440, 512, 0)
        assert all(s == 0 for s in wave)
    def test_symmetry(self):
        """Equal number of positive and negative samples (within 1)."""
        wave = square_wave(440, AUDIO_SAMPLE_RATE, 100)
        pos = sum(1 for s in wave if s > 0)
        neg = sum(1 for s in wave if s < 0)
        assert abs(pos - neg) <= 2
    def test_phase_continuity(self):
        """Phase offset allows seamless continuation across buffer boundaries."""
        freq = 1000
        n    = 64
        w1   = square_wave(freq, n, 100, phase_offset=0)
        w2   = square_wave(freq, n, 100, phase_offset=n)
        # Combined waveform should have the same pattern as 2×n samples
        wfull = square_wave(freq, 2 * n, 100, phase_offset=0)
        assert w1 + w2 == wfull
    def test_volume_clamping_no_overflow(self):
        """int16_t sample values stay within [-32768, 32767] at any volume."""
        for vol in [0, 50, 80, 100]:
            wave = square_wave(440, 256, vol)
            assert all(-32768 <= s <= 32767 for s in wave)
    def test_different_frequencies_produce_different_waveforms(self):
        w440  = square_wave(440,  512, 100)
        w880  = square_wave(880,  512, 100)
        w1000 = square_wave(1000, 512, 100)
        assert w440 != w880
        assert w880 != w1000
 # ── Tone sequencer ────────────────────────────────────────────────────────────
 class TestToneSequencer:
    def test_step_duration(self):
        """A 100 ms tone step at 22050 Hz spans exactly 2205 samples."""
        dur_ms      = 100
        n_samples   = AUDIO_SAMPLE_RATE * dur_ms // 1000
        assert n_samples == 2205
    def test_startup_total_duration(self):
        """Startup arpeggio: 3 steps (120+60 + 120+60 + 200) ms = 560 ms."""
        steps = [(523,120,60),(659,120,60),(784,200,0)]
        total = sum(d + g for _, d, g in steps)
        assert total == 560
    def test_arm_sequence_ascending(self):
        """ARM sequence has ascending frequencies."""
        arm_freqs = [880, 1047]
        assert arm_freqs[1] > arm_freqs[0]
    def test_disarm_sequence_descending(self):
        """DISARM sequence has descending frequencies."""
        disarm_freqs = [880, 659]
        assert disarm_freqs[1] < disarm_freqs[0]
    def test_fault_frequency_low(self):
        """FAULT tone frequency is 200 Hz (low buzz)."""
        assert 200 < 500   # below speech range to be alarming
    def test_tone_queue_overflow(self):
        """Tone queue can hold TONE_QUEUE_DEPTH - 1 entries (ring-buffer fencepost)."""
        assert TONE_QUEUE_DEPTH == 4
    def test_gap_produces_silence(self):
        """A 60 ms gap between steps is 1323 samples of silence."""
        gap_ms    = 60
        n_silence = AUDIO_SAMPLE_RATE * gap_ms // 1000
        assert n_silence == 1323
 # ── PCM FIFO ──────────────────────────────────────────────────────────────────
 class TestPcmFifo:
    def test_initial_empty(self):
        f = Fifo()
        assert f.avail == 0
        assert f.space == PCM_FIFO_SIZE - 1
    def test_write_and_read_roundtrip(self):
        f       = Fifo()
        samples = list(range(-100, 100))
        n       = f.write(samples)
        assert n == len(samples)
        out = f.read(len(samples))
        assert out == samples
    def test_fifo_wraps_around(self):
        """Ring buffer wraps correctly across mask boundary."""
        f = Fifo(size=8)
        # Advance pointer to near end
        f.wr = 6; f.rd = 6
        f.write([10, 20, 30])
        out = f.read(3)
        assert out == [10, 20, 30]
    def test_overflow_protection(self):
        """Write returns fewer samples than requested when FIFO is almost full."""
        f = Fifo(size=8)
        written = f.write([1, 2, 3, 4, 5, 6, 7, 8])  # can only fit 7 (fencepost)
        assert written == 7
    def test_empty_read_returns_empty(self):
        f = Fifo()
        assert f.read(10) == []
    def test_space_decreases_after_write(self):
        f = Fifo()
        space_before = f.space
        f.write([0] * 100)
        assert f.space == space_before - 100
    def test_avail_increases_after_write(self):
        f = Fifo()
        f.write(list(range(50)))
        assert f.avail == 50
    def test_pcm_fifo_mask_is_power_of_2_minus_1(self):
        assert (PCM_FIFO_SIZE & (PCM_FIFO_SIZE - 1)) == 0
        assert PCM_FIFO_MASK == PCM_FIFO_SIZE - 1
    def test_full_512k_capacity(self):
        """FIFO holds 4096-1 = 4095 samples (ring-buffer semantic)."""
        f     = Fifo()
        chunk = [42] * 4095
        n     = f.write(chunk)
        assert n == 4095
        assert f.avail == 4095
 # ── Mixer priority ────────────────────────────────────────────────────────────
 class TestMixer:
    def test_pcm_overrides_tone(self):
        """When PCM FIFO has enough data it should be drained, not tone."""
        f = Fifo()
        f.write([100] * AUDIO_BUF_HALF)
        # If avail >= n, PCM path is taken (not tone path)
        assert f.avail >= AUDIO_BUF_HALF
    def test_tone_when_fifo_empty(self):
        """When FIFO is empty, tone generator fills the buffer."""
        f     = Fifo()
        avail = f.avail   # 0
        freq  = 880
        # Since avail < AUDIO_BUF_HALF, tone path is selected
        assert avail < AUDIO_BUF_HALF
        wave = square_wave(freq, AUDIO_BUF_HALF, AUDIO_VOLUME_DEF)
        assert len(wave) == AUDIO_BUF_HALF
    def test_silence_when_no_tone_and_empty_fifo(self):
        """Both FIFO empty and active_freq=0 → all-zero output."""
        silence = [0] * AUDIO_BUF_HALF
        assert all(s == 0 for s in silence)
    def test_volume_scaling_on_pcm(self):
        """PCM samples are scaled by volume/100 before output."""
        raw     = [16000, -16000, 8000]
        vol80   = apply_volume(raw, 80)
        assert vol80[0] == 16000 * 80 // 100
        assert vol80[1] == -16000 * 80 // 100
 # ── JLink AUDIO frame ─────────────────────────────────────────────────────────
 JLINK_CMD_AUDIO = 0x08
 JLINK_MAX_PAYLOAD = 252
 class TestJlinkAudioFrame:
    def test_cmd_id(self):
        assert JLINK_CMD_AUDIO == 0x08
    def test_max_payload_bytes(self):
        """252 bytes = 126 int16_t samples."""
        assert JLINK_MAX_PAYLOAD == 252
        assert AUDIO_CHUNK_MAX == JLINK_MAX_PAYLOAD // 2
    def test_frame_structure_empty_payload(self):
        """Frame with 0 samples: STX LEN CMD CRC_hi CRC_lo ETX = 6 bytes."""
        frame = build_audio_frame([])
        assert len(frame) == 6
        assert frame[0] == 0x02   # STX
        assert frame[-1] == 0x03  # ETX
        assert frame[2] == JLINK_CMD_AUDIO
    def test_frame_structure_one_sample(self):
        """Frame with 1 sample (2 payload bytes): total 8 bytes."""
        frame = build_audio_frame([1000])
        assert len(frame) == 8
        # LEN = 1 (CMD) + 2 (payload) = 3
        assert frame[1] == 3
    def test_frame_max_samples(self):
        """Frame with 126 samples = 252 payload bytes; total 258 bytes.
        STX(1)+LEN(1)+CMD(1)+payload(252)+CRC_hi(1)+CRC_lo(1)+ETX(1) = 258."""
        samples = list(range(126))
        frame   = build_audio_frame(samples)
        assert len(frame) == 258
    def test_frame_crc_validates(self):
        """CRC in AUDIO frame validates against CMD+payload."""
        samples = [1000, -1000, 500, -500]
        frame   = build_audio_frame(samples)
        # Body = CMD byte + payload
        body = frame[2:-3]      # CMD + payload (skip STX, LEN, CRC_hi, CRC_lo, ETX)
        # Actually: frame = [STX][LEN][CMD][...payload...][CRC_hi][CRC_lo][ETX]
        cmd_and_payload = bytes([frame[2]]) + frame[3:-3]
        expected_crc = _crc16_xmodem(cmd_and_payload)
        crc_in_frame = (frame[-3] << 8) | frame[-2]
        assert crc_in_frame == expected_crc
    def test_frame_payload_little_endian(self):
        """Samples are encoded as little-endian int16_t."""
        samples = [0x1234]
        frame   = build_audio_frame(samples)
        # payload bytes at frame[3:5]
        lo, hi  = frame[3], frame[4]
        assert lo == 0x34
        assert hi == 0x12
    def test_odd_payload_bytes_rejected(self):
        """Payload with odd byte count must not be passed (always even: 2*N samples)."""
        # Odd-byte payload would be malformed; driver checks (plen & 1) == 0
        bad_plen = 5
        assert bad_plen % 2 != 0
    def test_audio_cmd_follows_estop(self):
        """AUDIO (0x08) is numerically after ESTOP (0x07)."""
        JLINK_CMD_ESTOP = 0x07
        assert JLINK_CMD_AUDIO > JLINK_CMD_ESTOP
 # ── Hardware constants ────────────────────────────────────────────────────────
 class TestHardwareConstants:
    def test_sample_rate(self):
        assert AUDIO_SAMPLE_RATE == 22050
    def test_buf_half_is_20ms(self):
        """441 samples at 22050 Hz ≈ 20 ms."""
        ms = AUDIO_BUF_HALF * 1000 / AUDIO_SAMPLE_RATE
        assert abs(ms - 20.0) < 0.1
    def test_buf_size_is_two_halves(self):
        assert AUDIO_BUF_SIZE == AUDIO_BUF_HALF * 2
    def test_dma_half_irq_latency_budget(self):
        """At 22050 Hz, 441 samples give 20 ms to refill — well above 1 ms loop."""
        refill_budget_ms = AUDIO_BUF_HALF * 1000 / AUDIO_SAMPLE_RATE
        main_loop_ms     = 1   # 1 kHz main loop
        assert refill_budget_ms > main_loop_ms * 5   # 20x margin
    def test_plli2s_frequency(self):
        """PLLI2S: N=192, R=2, PLLM=8, HSE=8 MHz → 96 MHz I2S clock."""
        hse_mhz    = 8
        pllm       = 8
        plli2s_n   = 192
        plli2s_r   = 2
        i2s_clk    = (hse_mhz / pllm) * plli2s_n / plli2s_r
        assert i2s_clk == pytest.approx(96.0)
    def test_actual_sample_rate_accuracy(self):
        """Actual FS with I2SDIV=68 is within 0.1% of 22050 Hz."""
        i2s_clk   = 96_000_000
        i2sdiv    = 68
        fs_actual = i2s_clk / (32 * 2 * i2sdiv)
        assert abs(fs_actual - 22050) / 22050 < 0.001
    def test_volume_default_in_range(self):
        assert 0 <= AUDIO_VOLUME_DEF <= 100
    def test_pcm_fifo_185ms_capacity(self):
        """4096 samples at 22050 Hz ≈ 185.76 ms of audio (Jetson jitter buffer)."""
        ms = PCM_FIFO_SIZE * 1000 / AUDIO_SAMPLE_RATE
        assert ms == pytest.approx(185.76, abs=0.1)
Author	SHA1	Message	Date
sl-mechanical	796e343b78	feat(mechanical): modular payload bay system (Issue #170 ) Dovetail rail + tool-free swappable payload modules for all variants: - payload_bay_rail.scad: 50×12 mm 60° dovetail rail (DXF for CNC Al bar), spring ball detent (Ø6 mm, 50 mm pitch), continuous safety-lock groove (M4 thumbscrew), 4-pin pogo connector housing (GND/5V/12V/UART), lab/rover/tank deck adapter plates - payload_bay_modules.scad: universal _module_base() (male tongue, detent bore, 4× Ø4 mm target pads, lock bore) + 3 example modules: cargo tray (200×100 mm, Velcro slots, bungee cord slots), camera boom (120 mm mast + 80 mm arm, 2020-rail-compatible head, 3-position tilt), cup holder (Ø80 mm tapered, 8-slot flex grip). Includes copy-paste module template. - payload_bay_BOM.md: hardware list, CNC spec (dovetail dimensions, surface finish, connector pocket), load analysis (2 kg rated with Al rail + lock), module developer guide with constraints table Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>	2026-03-02 10:38:07 -05:00
sl-jetson	d2cada00a0	Merge pull request 'feat(jetson): night vision mode — IR emitter, headlight, ambient-light FSM, IR SLAM bridge (#168 )' (#175 ) from sl-perception/issue-168-night-vision into main	2026-03-02 10:36:55 -05:00
sl-jetson	f2b24e29d8	Merge pull request 'feat(audio): I2S3 audio amplifier driver — Issue #143 ' (#173 ) from sl-firmware/issue-143-audio-amp into main	2026-03-02 10:36:36 -05:00
sl-perception	7dcdd2088a	feat(jetson): add saltybot_night_vision package (issue #168 ) Implements ambient-light-aware night vision mode for the D435i + IMX219 stack on the Jetson Orin Nano Super: • light_analyser.py — histogram-based intensity FSM with hysteresis: day → twilight → night → ir_only • camera_controller.py — D435i IR emitter via realsense2_camera param service + IMX219 gain/exposure via v4l2-ctl • gpio_headlight.py — physical pin 33 headlight; Jetson.GPIO PWM primary, sysfs on/off fallback, sim mode • light_monitor_node.py — subscribes IMX219/IR1, publishes /saltybot/ambient_light + /saltybot/vision_mode • night_vision_controller_node.py — reacts to mode changes; drives D435i emitter, IMX219 gain, headlight • ir_slam_bridge_node.py — mono8 IR1 → rgb8 republish so RTAB-Map keeps loop-closing in darkness • launch/night_vision.launch.py + config/night_vision_params.yaml • test/test_night_vision.py — 18 unit tests (18/18 pass, no ROS2 needed) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>	2026-03-02 10:36:21 -05:00
sl-firmware	c3ada4a156	feat(audio): I2S3 audio amplifier driver — Issue #143 Add I2S3/DMA audio output driver for MAX98357A/PCM5102A class-D amps: - audio_init(): PLLI2S N=192/R=2 → 96 MHz → FS≈22058 Hz (<0.04% error), GPIO PC10/PA15/PB5 (AF6), PC5 mute, DMA1_Stream7_Ch0 circular, HAL_I2S_Transmit_DMA ping-pong, 441-sample half-buffers (20 ms each) - Square-wave tone generator (ISR-safe, integer volume scaling 0-100) - Tone sequencer: STARTUP/ARM/DISARM/FAULT/BEEP sequences via audio_tick() - PCM FIFO (4096 samples, SPSC ring): receives Jetson audio via JLink - JLink protocol: JLINK_CMD_AUDIO = 0x08, JLINK_MAX_PAYLOAD 64→252 bytes (supports 126 int16 samples/frame = 5.7 ms @22050 Hz) - main.c: audio_init(), STARTUP tone on boot, ARM/FAULT tones, audio_tick() - config.h: AUDIO_BCLK/LRCK/DOUT/MUTE pin defines + PLLI2S constants - test_audio.py: 45 tests, all passing Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>	2026-03-02 10:34:35 -05:00
sl-jetson	566cfc8811	Merge pull request 'fix: IWDG reset during gyro recal — refresh at i=0 not i=39 (P0 #42 )' (#172 ) from sl-firmware/gyro-recal-button into main	2026-03-02 10:34:20 -05:00
sl-firmware	cf0a5a3583	fix: IWDG reset during gyro recal — refresh at i=0 not i=39 (P0 #42 ) i%40==39 fired the first IWDG refresh only after 40ms of calibration. Combined with ~10ms of main loop overhead before entering calibrate(), total elapsed since last refresh could exceed the 50ms IWDG window. Change to i%40==0: first refresh fires at i=0 (<1ms after entry), subsequent refreshes every 40ms — safely within the 50ms window. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>	2026-02-28 22:04:27 -05:00