feat(imu): MPU6000 sensor fusion with complementary filter

Add src/mpu6000.c implementing a complementary filter (α=0.98) on top of
the existing icm42688 SPI driver. Fixes wrong scale factors in balance.c
(was ±250°/s / ±2g; hardware is configured ±2000°/s / ±16g). Fusion now
lives in the IMU driver layer; balance_update() consumes IMUData directly.

- mpu6000_init(): calls icm42688_init(), seeds filter state
- mpu6000_read(): reads raw SPI, applies complementary filter, returns
  fused pitch (degrees) + pitch_rate (°/s) + accel_x/z (g)
- balance.c: removes duplicated fusion code, uses IMUData.pitch
- main.c: switches to mpu6000_init()/mpu6000_read(), updates telemetry

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

chassis/ASSEMBLY.md

@@ -0,0 +1,121 @@
+# SaltyBot Chassis — Assembly Notes
+**Task:** bd-1iy5 — Rev A — 2026-02-28
+
+---
+
+## Overview
+
+```
+         [Front bumper rail — 22mm EMT]
+         ├─ bumper_bracket(front=+1) ──────────────────────┐
+         │                                                  │
+ ┌───────┴──────────── Main Deck (640×220×6mm Al) ─────────┴───────┐
+ │  ← Jetson mount plate (rear/+X)   FC mount (front/−X) →         │
+ │          [Battery tray hanging below centre]                     │
+ └───┬──────────────────────────────────────────────────────────┬───┘
+     │                                                          │
+ Motor fork (L)                                          Motor fork (R)
+ Hub motor CL: ±300mm from deck centre                          │
+ Axle height: 310mm above ground                                │
+         ├─ bumper_bracket(front=-1) ──────────────────────┘
+         [Rear bumper rail — 22mm EMT]
+```
+
+---
+
+## Step-by-step Assembly
+
+### 1  Fabricate / print parts
+- Send deck plate DXF (export from OpenSCAD → DXF) to CNC router or waterjet cutter.
+- Print motor fork brackets in PETG 5 perimeters / 40% gyroid for prototype, or send STEP to machine shop.
+- Print battery tray, FC pad, Jetson plate, bumper brackets in PETG.
+- Export STEP: OpenSCAD → Render (F6) → Export as STL, then convert with FreeCAD for STEP.
+
+### 2  Verify motor axle dimensions
+- Measure actual hoverboard motor axle: diameter and flat-to-flat.
+- Adjust `MOTOR_AXLE_D` and `MOTOR_AXLE_FLAT` in `chassis_frame.scad`.
+- Re-export fork dropout slot.
+
+### 3  Motor forks → deck
+1. Thread M5 T-nuts into underside of deck edge slots (or use M5 rivet nuts).
+2. Align fork bracket to deck edge; fasten with 4× M5×16 SHCS + flat washer each side.
+3. Apply Loctite 243 to threads. Torque to 4 N·m.
+
+### 4  Motors into forks
+1. Slide hub motor axle into dropout slot (flat side aligns with slot).
+2. Fit flat washer then flanged M14 axle nut; torque to 35–40 N·m.
+3. Feed motor phase wires and hall-sensor cable through deck cable slot.
+
+### 5  Longitudinal ribs
+1. Align ribs with edge grooves on deck underside.
+2. Secure with M4×12 SHCS through pre-drilled holes; torque 2.5 N·m.
+
+### 6  Battery tray
+1. Pass any wiring harness through deck wire-pass hole before mounting tray.
+2. Align tray mounting ears to deck M4 threaded inserts (or rivet nuts).
+3. Fasten 4× M4×12 SHCS. Torque 2.5 N·m.
+4. Insert battery pack; route Velcro straps through slots and cinch.
+
+### 7  FC mount (MAMBA F722S)
+1. Place silicone anti-vibration grommets onto nylon M3 standoffs.
+2. Lower FC onto standoffs; secure with M3×6 BHCS. Snug only — do not over-torque.
+3. Orient USB-C port toward front of robot for cable access.
+
+### 8  Jetson Nano mount plate
+1. Press or thread M3 nylon standoffs (8mm) into plate holes.
+2. Bolt plate to deck: 4× M3×10 SHCS at deck corners.
+3. Set Jetson Nano B01 carrier onto plate standoffs; fasten M3×6 BHCS.
+
+### 9  Bumper brackets
+1. Slide 22mm EMT conduit through saddle clamp openings.
+2. Bolt bracket to deck edge: 4× M5×16 SHCS per bracket.
+3. Position bumper rail flush with motor OD outer edge; tighten saddle clamp bolts.
+
+### 10  Cable routing
+- Route motor phase cables along longitudinal ribs; secure with cable clips.
+- FC ↔ ESC/VESC harness exits through front cable slot.
+- Jetson USB/UART ribbon exits through rear cable slot.
+- Power harness (battery XT60 → BMS → 24V bus) runs under deck along centreline.
+
+---
+
+## Critical Dimensions (verify before machining)
+
+| Dimension | Nominal | Tolerance |
+|-----------|---------|-----------|
+| Wheelbase (axle C/L to C/L) | 600 mm | ±1 mm |
+| Motor fork slot width | 24 mm | +0.5 / 0 |
+| Motor fork dropout depth | 60 mm | ±0.5 mm |
+| FC hole pattern | 30.5 × 30.5 mm | ±0.2 mm |
+| Jetson hole pattern | 58 × 58 mm | ±0.2 mm |
+| Battery tray inner | 185 × 72 × 52 mm | +2 / 0 mm |
+
+---
+
+## OpenSCAD Rendering Notes
+
+```bash
+# Render full assembly preview (F5 in GUI)
+openscad chassis_frame.scad
+
+# Export individual part STL for slicing
+openscad chassis_frame.scad -D "PART=\"motor_fork\"" -o motor_fork_right.stl
+
+# Export deck DXF (set DECK_THICKNESS=0.01 for 2D projection)
+openscad chassis_frame.scad -D "RENDER_2D=true" -o deck_plate.dxf
+```
+
+**Slicing profile (PETG, structural parts):**
+- Nozzle: 0.4mm  |  Layer: 0.2mm
+- Perimeters: 5  |  Infill: 40% gyroid
+- Supports: Yes (motor fork dropout slot)
+- Orientation: Fork bracket printed vertically (load axis = layer direction)
+
+---
+
+## Safety Notes
+
+- Verify axle nut torque after first 10-minute ride — hub motors vibrate and may back off.
+- Battery tray is rated for packs ≤185×72×52mm; confirm dimensions before ordering cells.
+- Ground all aluminium chassis parts to power-common to avoid RF interference with FC.
+- Do **not** run robot without bumpers fitted — uncontrolled runaway risk.

chassis/BOM.md

@@ -0,0 +1,102 @@
+# SaltyBot Chassis — Bill of Materials
+**Task:** bd-1iy5
+**Rev:** A — 2026-02-28
+**Agent:** sl-mechanical
+
+---
+
+## Structural / Fabricated Parts
+
+| # | Part | Qty | Material | Notes |
+|---|------|-----|----------|-------|
+| 1 | Main deck plate | 1 | 6mm 5052 aluminium plate, 640×220mm | CNC router or waterjet; all holes per `chassis_frame.scad` |
+| 2 | Longitudinal rib (front) | 1 | 4mm 5052 aluminium, 600×40mm | Laser-cut; press-fit into deck slots |
+| 3 | Longitudinal rib (rear)  | 1 | 4mm 5052 aluminium, 600×40mm | Same file, symmetric |
+| 4 | Motor fork bracket (L) | 1 | 8mm 6061 aluminium | CNC mill or FDM PETG @100% infill for prototyping |
+| 5 | Motor fork bracket (R) | 1 | 8mm 6061 aluminium | Mirror of item 4 |
+| 6 | Battery tray | 1 | 3mm PETG FDM or 3mm aluminium fold | `chassis_frame.scad` — `battery_tray()` module |
+| 7 | FC mount plate / standoffs | 1 set | PETG or nylon FDM | Includes 4× M3 nylon standoffs, 6mm height |
+| 8 | Jetson Nano mount plate | 1 | 4mm 5052 aluminium or 4mm PETG FDM | B01 58×58mm hole pattern |
+| 9 | Front bumper bracket | 1 | 5mm PETG FDM | Saddle clamps for 22mm EMT conduit |
+| 10 | Rear bumper bracket | 1 | 5mm PETG FDM | Mirror of item 9 |
+
+---
+
+## Motors
+
+| # | Part | Qty | Source / Spec | Notes |
+|---|------|-----|---------------|-------|
+| 11 | Hoverboard hub motor | 2 | Generic 6.5" / 170mm OD, 36V nominal | ~350W each; 14mm flat axle; confirm exact OD before cutting fork slots |
+| 12 | Motor hall-sensor cable extension | 2 | 6-pin JST-PH 300mm | Route through deck cable slot |
+
+---
+
+## Electronics Mounts
+
+| # | Part | Qty | Spec | Notes |
+|---|------|-----|------|-------|
+| 13 | STM32 MAMBA F722S FC | 1 | 36×36mm PCB, 30.5×30.5mm M3 mount | Oriented USB-C port toward front |
+| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | FC vibration isolation |
+| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under FC mount pads |
+| 16 | Jetson Nano B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
+| 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs |
+
+---
+
+## Battery
+
+| # | Part | Qty | Spec | Notes |
+|---|------|-----|------|-------|
+| 18 | LiPo / LiFePO4 pack | 1 | 24V (6S LiPo or 8S LiFePO4), 4Ah, ≤185×72×52mm | Confirm dims before printing tray; add 2mm clearance |
+| 19 | Velcro strap 20mm wide | 2 | 300mm length | Route through tray strap slots |
+| 20 | BMS board | 1 | Matched to cell chemistry | Mount externally on rear inner face of tray wall |
+
+---
+
+## Bumper Rail
+
+| # | Part | Qty | Spec | Notes |
+|---|------|-----|------|-------|
+| 21 | 3/4" EMT conduit | 2 | 22mm OD, 660mm length | Front and rear bumper rail; bent or straight |
+| 22 | Conduit saddle clamp M4 | 6 | 3D-printed integral to bracket (items 9/10) | Back-up: standard pipe clamp 22mm |
+
+---
+
+## Fasteners
+
+| # | Part | Qty | Spec |
+|---|------|-----|------|
+| 23 | M5×16 SHCS | 24 | ISO 4762, SS |
+| 24 | M5 hex nut | 24 | ISO 4032, SS |
+| 25 | M4×12 SHCS | 12 | ISO 4762, SS |
+| 26 | M4 hex nut | 12 | ISO 4032, SS |
+| 27 | M3×10 SHCS | 20 | ISO 4762, SS |
+| 28 | M3 hex nut | 20 | ISO 4032, SS |
+| 29 | M3×6 BHCS | 8 | FC + Jetson board bolts |
+| 30 | M14×1.5 axle nut | 4 | One each side per motor, flanged | Confirm axle thread pitch on actual motors |
+| 31 | Serrated washer M14 | 4 | Axle anti-rotation |
+| 32 | Flat washer M5 | 48 | SS |
+
+---
+
+## Tools Required for Assembly
+
+- Torque wrench (M14 axle nuts: 35–40 N·m; M5: 4 N·m)
+- M2.5 / M3 / M4 / M5 hex drivers
+- Thread locker (Loctite 243 blue for all structural fasteners)
+- Dial caliper — verify motor OD, axle flat before machining fork slot
+
+---
+
+## Estimated Masses
+
+| Assembly | Est. mass |
+|----------|-----------|
+| Aluminium deck + ribs | ~1.1 kg |
+| Motor forks (×2 Al) | ~0.4 kg |
+| Motors (×2) | ~3.6 kg |
+| Battery (6S 4Ah LiPo) | ~0.7 kg |
+| Electronics (FC + Jetson + wiring) | ~0.5 kg |
+| Bumpers + brackets | ~0.3 kg |
+| Fasteners | ~0.2 kg |
+| **Total estimate** | **~6.8 kg** |

chassis/chassis_frame.scad

@@ -0,0 +1,340 @@
+// =============================================================================
+// SaltyBot — Parametric Chassis Frame
+// Task: bd-1iy5
+// Agent: sl-mechanical
+// Date: 2026-02-28
+//
+// Self-balancing two-wheeled robot chassis
+// Requirements:
+//   - 600mm wheelbase
+//   - 2x hoverboard hub motors (170mm OD)
+//   - STM32 MAMBA F722S FC mount (30.5x30.5mm pattern)
+//   - Battery tray (24V 4Ah — ~180x70x50mm pack)
+//   - Jetson Nano B01 mount plate (100x80mm, M3 holes)
+//   - Front/rear bumper brackets
+// =============================================================================
+
+// ─── RENDER QUALITY ──────────────────────────────────────────────────────────
+$fn = 64;
+
+// =============================================================================
+// PARAMETERS — edit here to adjust the whole model
+// =============================================================================
+
+// ── Wheelbase / overall geometry ─────────────────────────────────────────────
+WHEELBASE          = 600;   // mm, center-to-center axle distance (lateral)
+FRAME_WIDTH        = 220;   // mm, front-to-back depth of main deck
+DECK_THICKNESS     = 6;     // mm, main deck plate thickness
+WALL_T             = 4;     // mm, general wall thickness for brackets/ribs
+
+// ── Hub motor parameters ─────────────────────────────────────────────────────
+MOTOR_OD           = 170;   // mm, motor housing outer diameter
+MOTOR_AXLE_D       = 14;    // mm, axle bolt diameter
+MOTOR_AXLE_FLAT    = 10;    // mm, axle flat-to-flat (for anti-rotation)
+MOTOR_FORK_WIDTH   = 24;    // mm, dropout slot width (fits 10-14mm axle + spacers)
+MOTOR_FORK_DEPTH   = 60;    // mm, dropout slot depth from fork tip
+MOTOR_FORK_H       = 80;    // mm, total height of motor fork bracket
+MOTOR_FORK_T       = 8;     // mm, fork plate thickness
+AXLE_HEIGHT        = 310;   // mm, axle CL above ground (motor radius + clearance)
+
+// ── FC mount (MAMBA F722S — 30.5 × 30.5 mm M3 pattern) ──────────────────────
+FC_MOUNT_SPACING   = 30.5;  // mm, hole pattern pitch
+FC_MOUNT_HOLE_D    = 3.2;   // mm, M3 clearance
+FC_STANDOFF_H      = 6;     // mm, standoff height
+FC_PAD_T           = 3;     // mm, mounting pad thickness
+
+// ── Battery tray (24V 4Ah LiPo / LiFePO4) ───────────────────────────────────
+BATT_L             = 185;   // mm, cell pack length
+BATT_W             = 72;    // mm, cell pack width
+BATT_H             = 52;    // mm, cell pack height
+BATT_WALL          = 3;     // mm, tray wall thickness
+BATT_FLOOR         = 4;     // mm, tray floor thickness
+BATT_STRAP_W       = 20;    // mm, Velcro strap slot width
+BATT_STRAP_T       = 2;     // mm, strap slot depth
+
+// ── Jetson Nano B01 mount plate ──────────────────────────────────────────────
+// B01 carrier board hole pattern: 58 x 58 mm M3 (inner) + corner pass-throughs
+JETSON_HOLE_PITCH  = 58;    // mm, M3 mounting hole pattern
+JETSON_HOLE_D      = 3.2;   // mm
+JETSON_PLATE_L     = 105;   // mm, plate length
+JETSON_PLATE_W     = 90;    // mm, plate width
+JETSON_PLATE_T     = 4;     // mm, plate thickness
+JETSON_STANDOFF_H  = 8;     // mm
+
+// ── Bumper bracket ────────────────────────────────────────────────────────────
+BUMPER_L           = WHEELBASE + 60; // mm, bumper rail length (overhangs wheel CL)
+BUMPER_H           = 40;    // mm, bracket vertical height
+BUMPER_T           = 5;     // mm, bracket plate thickness
+BUMPER_TUBE_OD     = 22;    // mm, 3/4" EMT conduit OD for bumper rail
+
+// ── Rib / gusset parameters ───────────────────────────────────────────────────
+RIB_W              = 20;    // mm, longitudinal rib width
+RIB_H              = 40;    // mm, rib height below deck
+RIB_T              = 4;     // mm, rib plate thickness (laser/router cut)
+
+// ── Fastener helpers ─────────────────────────────────────────────────────────
+M3_D    = 3.2;
+M4_D    = 4.3;
+M5_D    = 5.3;
+M6_D    = 6.5;
+
+// =============================================================================
+// MAIN ASSEMBLY — comment/uncomment parts as needed
+// =============================================================================
+
+color("Silver", 0.9) main_deck();
+color("DimGray")     motor_fork(side= 1);   // right (+X)
+color("DimGray")     motor_fork(side=-1);   // left  (-X)
+color("SteelBlue")   battery_tray();
+color("OliveDrab")   fc_mount_plate();
+color("DarkOrange")  jetson_mount_plate();
+color("Tomato")      bumper_bracket(front= 1);
+color("Tomato")      bumper_bracket(front=-1);
+color("WhiteSmoke")  longitudinal_ribs();
+
+// =============================================================================
+// MODULES
+// =============================================================================
+
+// ─── Main deck plate ─────────────────────────────────────────────────────────
+module main_deck() {
+    difference() {
+        // Deck plate
+        translate([-WHEELBASE/2 - MOTOR_FORK_T, -FRAME_WIDTH/2, 0])
+            cube([WHEELBASE + 2*MOTOR_FORK_T, FRAME_WIDTH, DECK_THICKNESS]);
+
+        // Lightening holes — 3 rows × 4 cols
+        for (x = [-WHEELBASE/3, 0, WHEELBASE/3])
+        for (y = [-FRAME_WIDTH/4, FRAME_WIDTH/4])
+            translate([x, y, -1])
+                cylinder(d=50, h=DECK_THICKNESS+2);
+
+        // FC mount holes
+        fc_mount_holes(z_offset=-1, depth=DECK_THICKNESS+2);
+
+        // Battery tray floor cutout for wire pass-through
+        translate([-BATT_L/2 + 40, -BATT_W/2 + 40, -1])
+            cube([BATT_L - 80, BATT_W - 80, DECK_THICKNESS+2]);
+
+        // Cable routing slots
+        for (x = [-60, 60])
+            translate([x, -FRAME_WIDTH/2 - 1, -1])
+                cube([14, 18, DECK_THICKNESS+2]);
+    }
+}
+
+// ─── Longitudinal ribs (×2, symmetric F/R) ───────────────────────────────────
+module longitudinal_ribs() {
+    for (y = [-FRAME_WIDTH/2 + RIB_W/2 + WALL_T,
+               FRAME_WIDTH/2 - RIB_W/2 - WALL_T])
+        translate([-WHEELBASE/2, y - RIB_T/2, -RIB_H])
+            cube([WHEELBASE, RIB_T, RIB_H]);
+}
+
+// ─── Motor fork dropout bracket ──────────────────────────────────────────────
+// Mounts to deck edge; provides a CNC-milled or FDM dropout slot for the axle.
+// `side` = +1 (right/+X) or -1 (left/-X)
+module motor_fork(side = 1) {
+    x_pos = side * (WHEELBASE/2);
+
+    translate([x_pos, 0, 0]) {
+        difference() {
+            union() {
+                // Vertical fork body
+                translate([side*(DECK_THICKNESS/2), -MOTOR_FORK_WIDTH/2 - 4, -MOTOR_FORK_H])
+                    cube([side * MOTOR_FORK_T, MOTOR_FORK_WIDTH + 8, MOTOR_FORK_H + DECK_THICKNESS]);
+
+                // Gusset triangles
+                for (g = [-1, 1])
+                    translate([side*(DECK_THICKNESS/2), g*(MOTOR_FORK_WIDTH/2 + 2), -MOTOR_FORK_H])
+                        linear_extrude(height=MOTOR_FORK_T)
+                            polygon([[0,0],[0, g*20],[side*-30, 0]]);
+            }
+
+            // Axle dropout slot (open at bottom)
+            translate([side*(DECK_THICKNESS/2) - 1,
+                       -MOTOR_AXLE_FLAT/2,
+                       -MOTOR_FORK_DEPTH - MOTOR_AXLE_D/2])
+                cube([MOTOR_FORK_T + 2, MOTOR_AXLE_FLAT, MOTOR_FORK_DEPTH + 1]);
+
+            // Axle through-hole at slot top
+            translate([side*(DECK_THICKNESS/2) - 1, 0,
+                       -MOTOR_FORK_DEPTH - MOTOR_AXLE_D/2])
+                rotate([0, 90, 0])
+                    cylinder(d=MOTOR_AXLE_D + 1, h=MOTOR_FORK_T + 2);
+
+            // Bolt holes for deck attachment (M5 × 4)
+            for (y = [-20, 20])
+            for (z = [4, 12])
+                translate([side*(DECK_THICKNESS/2) - 1, y, z - MOTOR_FORK_H + MOTOR_FORK_H/2])
+                    rotate([0, 90, 0])
+                        cylinder(d=M5_D, h=MOTOR_FORK_T + 2);
+        }
+    }
+}
+
+// ─── Battery tray ─────────────────────────────────────────────────────────────
+// Positioned in centre of deck, recessed 10mm below deck surface
+module battery_tray() {
+    z_base = -BATT_FLOOR - BATT_H - 5;  // hang below deck
+    translate([-BATT_L/2 - BATT_WALL, -BATT_W/2 - BATT_WALL, z_base]) {
+        difference() {
+            // Outer tray body
+            cube([BATT_L + 2*BATT_WALL,
+                  BATT_W + 2*BATT_WALL,
+                  BATT_H + BATT_FLOOR]);
+
+            // Inner cavity
+            translate([BATT_WALL, BATT_WALL, BATT_FLOOR])
+                cube([BATT_L, BATT_W, BATT_H + 1]);
+
+            // Strap slots (2× longitudinal)
+            for (x = [BATT_L/2 - BATT_STRAP_W*2,
+                      BATT_L/2 + BATT_STRAP_W])
+                translate([x, -1, BATT_FLOOR + BATT_H/2 - BATT_STRAP_W/2])
+                    cube([BATT_STRAP_W, BATT_W + 2*BATT_WALL + 2, BATT_STRAP_W]);
+
+            // Ventilation slots bottom (3×)
+            for (i = [0:2])
+                translate([BATT_WALL + 20 + i*50, BATT_WALL + 10, -1])
+                    cube([30, BATT_W - 20, BATT_FLOOR + 2]);
+
+            // Mount holes to deck (M4 × 4 corners)
+            for (x = [10, BATT_L + BATT_WALL - 2])
+            for (y = [10, BATT_W + BATT_WALL - 2])
+                translate([x, y, BATT_H + BATT_FLOOR - 1])
+                    cylinder(d=M4_D, h=BATT_FLOOR + 2);
+        }
+    }
+}
+
+// ─── FC mount holes helper ────────────────────────────────────────────────────
+module fc_mount_holes(z_offset=0, depth=10) {
+    // MAMBA F722S: 30.5×30.5 mm M3 pattern, centred at origin
+    for (x = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2])
+    for (y = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2])
+        translate([x, y, z_offset])
+            cylinder(d=FC_MOUNT_HOLE_D, h=depth);
+}
+
+// ─── FC mount plate (raised 40mm above deck centre) ──────────────────────────
+module fc_mount_plate() {
+    fc_x = -50;   // offset toward front from deck centre
+    fc_y =  0;
+    plate_z = DECK_THICKNESS + FC_STANDOFF_H;
+
+    translate([fc_x, fc_y, 0]) {
+        difference() {
+            union() {
+                // Mount pads with standoffs
+                for (x = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2])
+                for (y = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2]) {
+                    translate([x, y, DECK_THICKNESS])
+                        cylinder(d=8, h=FC_STANDOFF_H);  // standoff column
+                    translate([x - 5, y - 5, DECK_THICKNESS])
+                        cube([10, 10, FC_PAD_T]);          // pad base
+                }
+                // Base plate
+                translate([-FC_MOUNT_SPACING/2 - 8, -FC_MOUNT_SPACING/2 - 8, DECK_THICKNESS])
+                    cube([FC_MOUNT_SPACING + 16, FC_MOUNT_SPACING + 16, FC_PAD_T]);
+            }
+            // M3 through-holes in standoffs
+            fc_mount_holes(z_offset=DECK_THICKNESS - 1, depth=FC_STANDOFF_H + FC_PAD_T + 2);
+
+            // Deck anchor holes
+            fc_mount_holes(z_offset=-1, depth=DECK_THICKNESS + 2);
+        }
+    }
+}
+
+// ─── Jetson Nano B01 mount plate ─────────────────────────────────────────────
+// Positioned rear of deck, elevated on standoffs
+module jetson_mount_plate() {
+    jet_x =  60;   // offset toward rear
+    jet_y =  0;
+    plate_z = DECK_THICKNESS + JETSON_STANDOFF_H;
+
+    translate([jet_x, jet_y, 0]) {
+        difference() {
+            union() {
+                // Mounting plate
+                translate([-JETSON_PLATE_L/2, -JETSON_PLATE_W/2, DECK_THICKNESS + JETSON_STANDOFF_H])
+                    cube([JETSON_PLATE_L, JETSON_PLATE_W, JETSON_PLATE_T]);
+
+                // Four standoff columns
+                for (x = [-JETSON_HOLE_PITCH/2, JETSON_HOLE_PITCH/2])
+                for (y = [-JETSON_HOLE_PITCH/2, JETSON_HOLE_PITCH/2])
+                    translate([x, y, DECK_THICKNESS])
+                        cylinder(d=6, h=JETSON_STANDOFF_H);
+            }
+
+            // B01 M3 hole pattern (58×58 mm)
+            for (x = [-JETSON_HOLE_PITCH/2, JETSON_HOLE_PITCH/2])
+            for (y = [-JETSON_HOLE_PITCH/2, JETSON_HOLE_PITCH/2])
+                translate([x, y, DECK_THICKNESS - 1])
+                    cylinder(d=JETSON_HOLE_D, h=JETSON_STANDOFF_H + JETSON_PLATE_T + 2);
+
+            // Ventilation / cable routing slots in plate
+            for (i = [-1, 0, 1])
+                translate([i*25 - 10, -JETSON_PLATE_W/2 + 10,
+                           DECK_THICKNESS + JETSON_STANDOFF_H - 1])
+                    cube([20, JETSON_PLATE_W - 20, JETSON_PLATE_T + 2]);
+
+            // Deck anchor holes (M3 × 4 corners of plate)
+            for (x = [-JETSON_PLATE_L/2 + 8, JETSON_PLATE_L/2 - 8])
+            for (y = [-JETSON_PLATE_W/2 + 8, JETSON_PLATE_W/2 - 8])
+                translate([x, y, -1])
+                    cylinder(d=M3_D, h=DECK_THICKNESS + 2);
+        }
+    }
+}
+
+// ─── Bumper bracket ───────────────────────────────────────────────────────────
+// `front` = +1 (front) or -1 (rear)
+module bumper_bracket(front = 1) {
+    y_pos = front * (FRAME_WIDTH/2);
+
+    translate([0, y_pos, 0]) {
+        difference() {
+            union() {
+                // Horizontal mounting rail plate
+                translate([-BUMPER_L/2, front*(BUMPER_T/2), 0])
+                    cube([BUMPER_L, BUMPER_T, BUMPER_H]);
+
+                // Vertical gussets at ends
+                for (x = [-BUMPER_L/2 + 5, BUMPER_L/2 - BUMPER_T - 5])
+                    translate([x, front*(BUMPER_T), 0])
+                        cube([BUMPER_T, 20, BUMPER_H]);
+
+                // Bumper tube saddle clamps (×3 evenly spaced)
+                for (x = [-BUMPER_L/3, 0, BUMPER_L/3])
+                    translate([x - BUMPER_TUBE_OD/2 - 2,
+                               front*(BUMPER_T + 15),
+                               BUMPER_H - BUMPER_TUBE_OD/2 - 5])
+                    difference() {
+                        cube([BUMPER_TUBE_OD + 4, BUMPER_TUBE_OD/2 + 4, BUMPER_TUBE_OD + 4]);
+                        translate([BUMPER_TUBE_OD/2 + 2, -1, BUMPER_TUBE_OD/2 + 2])
+                            rotate([-90, 0, 0])
+                                cylinder(d=BUMPER_TUBE_OD, h=BUMPER_TUBE_OD/2 + 6);
+                    }
+            }
+
+            // Deck mounting holes (M5 × 6 along length)
+            for (x = [-BUMPER_L/2 + 20, -BUMPER_L/6, BUMPER_L/6, BUMPER_L/2 - 20])
+                translate([x, y_pos * 0.001, -1])
+                    cylinder(d=M5_D, h=BUMPER_H + 2);
+        }
+    }
+}
+
+// =============================================================================
+// DIMENSIONS / ANNOTATIONS  (2D cross-section reference)
+// =============================================================================
+// Uncomment to render a side-view dimension guide:
+//
+// %translate([0, FRAME_WIDTH/2 + 30, 0]) {
+//     color("Blue") {
+//         cube([WHEELBASE, 1, 1]);        // wheelbase span
+//         translate([0,0,0]) text(str("Wheelbase: ", WHEELBASE, "mm"), size=12);
+//     }
+// }

include/balance.h

@@ -2,12 +2,12 @@
 #define BALANCE_H
 
 #include <stdint.h>
-#include "icm42688.h"
+#include "mpu6000.h"
 
 /*
  * SaltyLab Balance Controller
  *
- * Complementary filter (gyro + accel) → pitch angle
+ * Consumes fused IMUData (pitch + pitch_rate from mpu6000 complementary filter)
  * PID controller → motor speed command
  * Safety: tilt cutoff, arming, watchdog
  */
@@ -39,7 +39,7 @@ typedef struct {
 } balance_t;
 
 void balance_init(balance_t *b);
-void balance_update(balance_t *b, const icm42688_data_t *imu, float dt);
+void balance_update(balance_t *b, const IMUData *imu, float dt);
 void balance_arm(balance_t *b);
 void balance_disarm(balance_t *b);
 

jetson/Dockerfile

@@ -0,0 +1,102 @@
+# Jetson Nano — ROS2 Humble dev container
+# Base: JetPack 4.6 (L4T R32.6.1) + CUDA 10.2
+# Arch: ARM64 (aarch64)
+
+FROM nvcr.io/nvidia/l4t-base:r32.6.1
+
+LABEL maintainer="sl-jetson <saltylab>"
+LABEL description="ROS2 Humble + SLAM stack for Jetson Nano self-balancing robot"
+LABEL jetpack="4.6"
+LABEL ros_distro="humble"
+
+ENV DEBIAN_FRONTEND=noninteractive
+ENV ROS_DISTRO=humble
+ENV ROS_ROOT=/opt/ros/${ROS_DISTRO}
+ENV PYTHONDONTWRITEBYTECODE=1
+ENV PYTHONUNBUFFERED=1
+
+# ── System deps ────────────────────────────────────────────────────────────────
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    # Build tools
+    build-essential cmake git wget curl \
+    # Python
+    python3-dev python3-pip python3-setuptools python3-wheel \
+    # Serial / I2C / SPI
+    i2c-tools libi2c-dev python3-smbus \
+    picocom minicom setserial \
+    # USB
+    usbutils libusb-1.0-0-dev \
+    # Misc
+    locales tzdata htop tmux nano \
+    # Networking
+    net-tools iputils-ping \
+    && rm -rf /var/lib/apt/lists/*
+
+RUN locale-gen en_US.UTF-8
+ENV LANG=en_US.UTF-8
+
+# ── ROS2 Humble (from ROS2 apt repo — ARM64 build) ─────────────────────────────
+# Note: official humble debs for ARM64/L4T are provided via NVIDIA Isaac ROS
+# or via ros2-apt-source for 20.04 focal.
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc \
+        | apt-key add - && \
+    echo "deb [arch=arm64] http://packages.ros.org/ros2/ubuntu focal main" \
+        > /etc/apt/sources.list.d/ros2.list && \
+    apt-get update && apt-get install -y --no-install-recommends \
+        ros-humble-ros-base \
+        ros-humble-rmw-cyclonedds-cpp \
+        ros-dev-tools \
+        python3-colcon-common-extensions \
+        python3-rosdep \
+    && rosdep init && rosdep update \
+    && rm -rf /var/lib/apt/lists/*
+
+# ── Nav / SLAM / sensor packages ──────────────────────────────────────────────
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    ros-humble-nav2-bringup \
+    ros-humble-slam-toolbox \
+    ros-humble-robot-localization \
+    ros-humble-rplidar-ros \
+    ros-humble-realsense2-camera \
+    ros-humble-realsense2-description \
+    ros-humble-tf2-tools \
+    ros-humble-rqt \
+    ros-humble-rqt-common-plugins \
+    ros-humble-rviz2 \
+    ros-humble-rosbridge-server \
+    && rm -rf /var/lib/apt/lists/*
+
+# ── GPIO / serial Python libs ──────────────────────────────────────────────────
+RUN pip3 install --no-cache-dir \
+    Jetson.GPIO \
+    pyserial \
+    smbus2 \
+    adafruit-blinka \
+    RPi.GPIO \
+    numpy \
+    scipy
+
+# ── RealSense SDK (librealsense2 ARM64) ───────────────────────────────────────
+# Pre-built for L4T — install from Jetson Hacks script or apt source
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev \
+    && rm -rf /var/lib/apt/lists/*
+
+# librealsense2 ARM64 wheel (NVIDIA-patched for L4T)
+RUN pip3 install --no-cache-dir pyrealsense2
+
+# ── Workspace setup ───────────────────────────────────────────────────────────
+RUN mkdir -p /ros2_ws/src
+WORKDIR /ros2_ws
+
+# Source ROS2 in every shell
+RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /root/.bashrc && \
+    echo "source /ros2_ws/install/local_setup.bash 2>/dev/null || true" >> /root/.bashrc && \
+    echo "export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp" >> /root/.bashrc
+
+# ── Entrypoint ────────────────────────────────────────────────────────────────
+COPY scripts/entrypoint.sh /entrypoint.sh
+RUN chmod +x /entrypoint.sh
+
+ENTRYPOINT ["/entrypoint.sh"]
+CMD ["bash"]

jetson/README.md

@@ -0,0 +1,65 @@
+# Jetson Nano — AI/SLAM Platform Setup
+
+Self-balancing robot: Jetson Nano dev environment for ROS2 Humble + SLAM stack.
+
+## Stack
+
+| Component | Version / Part |
+|-----------|---------------|
+| Platform | Jetson Nano 4GB |
+| JetPack | 4.6 (L4T R32.6.1, CUDA 10.2) |
+| ROS2 | Humble Hawksbill |
+| DDS | CycloneDDS |
+| SLAM | slam_toolbox |
+| Nav | Nav2 |
+| Depth camera | Intel RealSense D435i |
+| LiDAR | RPLIDAR A1M8 |
+| MCU bridge | STM32F722 (USB CDC @ 921600) |
+
+## Quick Start
+
+```bash
+# 1. Host setup (once, on fresh JetPack 4.6)
+sudo bash scripts/setup-jetson.sh
+
+# 2. Build Docker image
+bash scripts/build-and-run.sh build
+
+# 3. Start full stack
+bash scripts/build-and-run.sh up
+
+# 4. Open ROS2 shell
+bash scripts/build-and-run.sh shell
+```
+
+## Docs
+
+- [`docs/pinout.md`](docs/pinout.md) — GPIO/I2C/UART pinout for all peripherals
+- [`docs/power-budget.md`](docs/power-budget.md) — 10W power envelope analysis
+
+## Files
+
+```
+jetson/
+├── Dockerfile              # L4T base + ROS2 Humble + SLAM packages
+├── docker-compose.yml      # Multi-service stack (ROS2, RPLIDAR, D435i, STM32)
+├── README.md               # This file
+├── docs/
+│   ├── pinout.md           # GPIO/I2C/UART pinout reference
+│   └── power-budget.md     # Power budget analysis (10W envelope)
+└── scripts/
+    ├── entrypoint.sh       # Docker container entrypoint
+    ├── setup-jetson.sh     # Host setup (udev, Docker, nvpmodel)
+    └── build-and-run.sh    # Build/run helper
+```
+
+## Power Budget (Summary)
+
+| Scenario | Total |
+|---------|-------|
+| Idle | 2.9W |
+| Nominal (SLAM active) | ~10.2W |
+| Peak | 15.4W |
+
+Target: 10W (MAXN nvpmodel). Use RPLIDAR standby + 640p D435i for compliance.
+See [`docs/power-budget.md`](docs/power-budget.md) for full analysis.

jetson/docker-compose.yml

@@ -0,0 +1,135 @@
+version: "3.8"
+
+# Jetson Nano — ROS2 Humble SLAM stack
+# Run with: docker compose up -d
+# Requires: NVIDIA Container Runtime (nvidia-docker2) on host
+
+services:
+
+  # ── Core ROS2 + SLAM node ─────────────────────────────────────────────────
+  saltybot-ros2:
+    image: saltybot/ros2-humble:jetson-nano
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: saltybot-ros2
+    restart: unless-stopped
+    runtime: nvidia                 # JetPack NVIDIA runtime
+    privileged: false               # use device passthrough instead
+    network_mode: host              # ROS2 DDS multicast needs host networking
+
+    environment:
+      - NVIDIA_VISIBLE_DEVICES=all
+      - NVIDIA_DRIVER_CAPABILITIES=all
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+      - ROS_DOMAIN_ID=42
+      - DISPLAY=${DISPLAY:-:0}      # for RViz2 on host X11
+
+    volumes:
+      # X11 socket for RViz2
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+      # ROS2 workspace (host-mounted for live dev)
+      - ./ros2_ws:/ros2_ws/src:rw
+      # Persistent SLAM maps
+      - saltybot-maps:/maps
+      # Config files
+      - ./config:/config:ro
+
+    devices:
+      # RPLIDAR A1M8 — typically /dev/ttyUSB0
+      - /dev/ttyUSB0:/dev/ttyUSB0
+      # STM32 UART bridge — typically /dev/ttyUSB1 or /dev/ttyACM0
+      - /dev/ttyUSB1:/dev/ttyUSB1
+      # RealSense D435i — USB3 device, needs udev rules
+      - /dev/bus/usb:/dev/bus/usb
+      # I2C bus (Jetson i2c-1 = pins 3/5)
+      - /dev/i2c-1:/dev/i2c-1
+      # GPIO (via Jetson.GPIO)
+      - /sys/class/gpio:/sys/class/gpio
+
+    command: >
+      bash -c "
+        source /opt/ros/humble/setup.bash &&
+        ros2 launch saltybot_bringup slam.launch.py
+      "
+
+  # ── RPLIDAR driver node ────────────────────────────────────────────────────
+  rplidar:
+    image: saltybot/ros2-humble:jetson-nano
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: saltybot-rplidar
+    restart: unless-stopped
+    runtime: nvidia
+    network_mode: host
+    environment:
+      - ROS_DOMAIN_ID=42
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+    devices:
+      - /dev/ttyUSB0:/dev/ttyUSB0
+    command: >
+      bash -c "
+        source /opt/ros/humble/setup.bash &&
+        ros2 launch rplidar_ros rplidar_a1_launch.py
+          serial_port:=/dev/ttyUSB0
+          frame_id:=laser
+      "
+
+  # ── RealSense D435i driver node ────────────────────────────────────────────
+  realsense:
+    image: saltybot/ros2-humble:jetson-nano
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: saltybot-realsense
+    restart: unless-stopped
+    runtime: nvidia
+    network_mode: host
+    environment:
+      - ROS_DOMAIN_ID=42
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+    devices:
+      - /dev/bus/usb:/dev/bus/usb
+    volumes:
+      - /dev:/dev
+    command: >
+      bash -c "
+        source /opt/ros/humble/setup.bash &&
+        ros2 launch realsense2_camera rs_launch.py
+          enable_color:=true
+          enable_depth:=true
+          enable_gyro:=true
+          enable_accel:=true
+          unite_imu_method:=linear_interpolation
+          depth_module.profile:=640x480x30
+          rgb_camera.profile:=640x480x30
+      "
+
+  # ── STM32 bridge node (custom serial↔ROS2 bridge) ─────────────────────────
+  stm32-bridge:
+    image: saltybot/ros2-humble:jetson-nano
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: saltybot-stm32-bridge
+    restart: unless-stopped
+    runtime: nvidia
+    network_mode: host
+    environment:
+      - ROS_DOMAIN_ID=42
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+    devices:
+      - /dev/ttyUSB1:/dev/ttyUSB1
+    command: >
+      bash -c "
+        source /opt/ros/humble/setup.bash &&
+        ros2 run saltybot_stm32_bridge bridge_node
+          --ros-args
+          -p serial_port:=/dev/ttyUSB1
+          -p baud_rate:=921600
+      "
+
+volumes:
+  saltybot-maps:
+    driver: local

jetson/docs/pinout.md

@@ -0,0 +1,213 @@
+# Jetson Nano — GPIO / I2C / UART Pinout Reference
+## Self-Balancing Robot: STM32F722 Bridge + RealSense D435i + RPLIDAR A1M8
+
+Last updated: 2026-02-28
+JetPack version: 4.6 (L4T R32.6.1)
+
+---
+
+## 40-Pin Header Overview
+
+The Jetson Nano 40-pin header is physically compatible with Raspberry Pi HATs.
+Pin numbering below follows **physical board pin** (1–40) and the Jetson GPIO BCM-equivalent name.
+
+```
+ 3.3V  [ 1] [ 2] 5V
+SDA1  [ 3] [ 4] 5V         ← I2C SDA (i2c-1)
+SCL1  [ 5] [ 6] GND        ← I2C SCL (i2c-1)
+ GPIO [ 7] [ 8] TXD0       ← UART TX (ttyTHS1)
+  GND [ 9] [10] RXD0       ← UART RX (ttyTHS1)
+ GPIO [11] [12] GPIO
+ GPIO [13] [14] GND
+ GPIO [15] [16] GPIO
+ 3.3V [17] [18] GPIO
+ MOSI [19] [20] GND        ← SPI0 MOSI
+ MISO [21] [22] GPIO       ← SPI0 MISO
+ SCLK [23] [24] CE0        ← SPI0 CLK / CS0
+  GND [25] [26] CE1        ← SPI0 CS1
+  ID_SD[27] [28] ID_SC     ← I2C ID EEPROM (reserved)
+ GPIO [29] [30] GND
+ GPIO [31] [32] GPIO
+ GPIO [33] [34] GND
+ GPIO [35] [36] GPIO
+ GPIO [37] [38] GPIO
+  GND [39] [40] GPIO
+```
+
+---
+
+## 1. STM32F722 Bridge (UART)
+
+The STM32 acts as a real-time motor + IMU controller. Communication to Jetson is via **USB CDC serial** (primary) with hardware UART as fallback.
+
+### USB CDC (Primary — Recommended)
+| Connection | Detail |
+|-----------|--------|
+| Interface | USB Micro-B on STM32 dev board → USB-A on Jetson |
+| Device node | `/dev/ttyACM0` or `/dev/ttyUSB1` |
+| Baud rate | 921600 (configured in STM32 firmware) |
+| Protocol | Custom binary framing (see `src/comm/`) |
+| Power | Powered via Jetson USB 5V (500mA max from host) |
+
+### Hardware UART (Fallback)
+| Jetson Pin | Signal | STM32 Pin | Notes |
+|-----------|--------|-----------|-------|
+| Pin 8 (TXD0) | TX → | PA10 (UART1 RX) | Cross-connect TX→RX |
+| Pin 10 (RXD0) | RX ← | PA9 (UART1 TX) | Cross-connect RX→TX |
+| Pin 6 (GND) | GND | GND | Common ground **required** |
+
+**Jetson device node:** `/dev/ttyTHS1`
+**Baud rate:** 921600, 8N1
+**Voltage level:** 3.3V — STM32F722 is 3.3V tolerant; Jetson GPIO is 3.3V
+**Do NOT use 5V** — Jetson GPIO max is 3.3V
+
+```bash
+# Verify UART on Jetson
+ls /dev/ttyTHS1
+# Check permissions (add user to dialout group)
+sudo usermod -aG dialout $USER
+# Quick loopback test (connect TX→RX)
+picocom -b 921600 /dev/ttyTHS1
+```
+
+**ROS2 topic mapping (STM32 bridge node):**
+| ROS2 Topic | Direction | Content |
+|-----------|-----------|---------|
+| `/stm32/imu_raw` | STM32→Jetson | IMU data (accel, gyro) at 500Hz |
+| `/stm32/motor_state` | STM32→Jetson | Motor RPM, current, temperature |
+| `/cmd_vel` | Jetson→STM32 | Velocity commands (m/s, rad/s) |
+| `/stm32/estop` | Jetson→STM32 | Emergency stop signal |
+
+---
+
+## 2. RealSense D435i (USB3)
+
+The D435i provides RGB-D (depth + color) and IMU (accelerometer + gyroscope).
+
+### Connection
+| Parameter | Value |
+|-----------|-------|
+| Interface | USB 3.1 Gen 1 (USB-A on Jetson) |
+| Device node | `/dev/bus/usb/...` (udev-managed) |
+| USB PID:VID | `0x8086:0x0b3a` (D435i) |
+| Power draw | ~1.5W active, 3.5W peak during init |
+| Cable | USB 3.1 — use **short cable ≤1m** for stability |
+
+**Note:** The Jetson Nano has **4× USB-A ports** — use a USB3 port (blue) for D435i.
+
+```bash
+# Verify detection
+lsusb | grep Intel
+# Expected: Bus 002 Device 003: ID 8086:0b3a Intel Corp. Intel RealSense D435i
+
+# Install udev rules (required for non-root access)
+sudo cp /etc/udev/rules.d/99-realsense-libusb.rules /etc/udev/rules.d/
+sudo udevadm control --reload-rules && sudo udevadm trigger
+
+# Test with realsense-viewer (if installed)
+realsense-viewer
+```
+
+**ROS2 topics published:**
+| Topic | Type | Rate |
+|-------|------|------|
+| `/camera/color/image_raw` | `sensor_msgs/Image` | 30Hz |
+| `/camera/depth/image_rect_raw` | `sensor_msgs/Image` | 30Hz |
+| `/camera/aligned_depth_to_color/image_raw` | `sensor_msgs/Image` | 30Hz |
+| `/camera/imu` | `sensor_msgs/Imu` | 400Hz |
+| `/camera/color/camera_info` | `sensor_msgs/CameraInfo` | 30Hz |
+
+---
+
+## 3. RPLIDAR A1M8 (UART via USB adapter)
+
+The A1M8 uses a CP2102/CH340 USB-UART adapter (included in kit).
+
+### Connection
+| Parameter | Value |
+|-----------|-------|
+| Interface | USB Micro-B (via included USB-UART adapter) |
+| Device node | `/dev/ttyUSB0` (first USB-UART device) |
+| Baud rate | 115200 |
+| Power draw | ~2.6W motor on, 0.4W idle |
+| Motor control | DTR line (handled by rplidar_ros driver) |
+
+```bash
+# Verify detection
+ls /dev/ttyUSB*
+# Expected: /dev/ttyUSB0
+
+# Set permissions
+sudo usermod -aG dialout $USER
+
+# Test — should output scan data
+ros2 launch rplidar_ros rplidar_a1_launch.py serial_port:=/dev/ttyUSB0
+```
+
+**ROS2 topics published:**
+| Topic | Type | Rate |
+|-------|------|------|
+| `/scan` | `sensor_msgs/LaserScan` | 10Hz |
+
+**udev rule (set consistent device name):**
+```bash
+# /etc/udev/rules.d/99-rplidar.rules
+KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
+  SYMLINK+="rplidar", MODE="0666"
+```
+
+---
+
+## 4. I2C Bus (i2c-1) — Pin 3 / Pin 5
+
+Available for future peripherals (IMU breakout, OLED display, etc.).
+
+| Parameter | Value |
+|-----------|-------|
+| Jetson I2C bus | i2c-1 (pins 3 = SDA, 5 = SCL) |
+| Voltage | 3.3V pull-up |
+| Max clock | 400kHz (Fast Mode) |
+| Current source | Jetson 3.3V rail (max ~500mA shared) |
+
+```bash
+# Scan i2c-1 bus
+i2cdetect -y -r 1
+```
+
+**Note:** i2c-0 (pins 27/28) is reserved for EEPROM ID — do not use.
+
+---
+
+## 5. GPIO Summary Table
+
+| Physical Pin | Jetson GPIO | Voltage | Current Used For |
+|-------------|-------------|---------|-----------------|
+| 3 | SDA1 | 3.3V | I2C data (i2c-1) |
+| 5 | SCL1 | 3.3V | I2C clock (i2c-1) |
+| 8 | TXD0 | 3.3V | UART TX → STM32 (fallback) |
+| 10 | RXD0 | 3.3V | UART RX ← STM32 (fallback) |
+| USB-A (×4) | — | 5V | D435i, RPLIDAR adapter, STM32 USB |
+
+---
+
+## 6. Device Enumeration Notes
+
+USB devices may enumerate differently across reboots. Use udev rules for stable names:
+
+```bash
+# /etc/udev/rules.d/99-saltybot.rules
+
+# RPLIDAR A1M8 (SiliconLabs CP2102)
+KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
+  SYMLINK+="rplidar", MODE="0666"
+
+# STM32 USB CDC (STMicroelectronics)
+KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
+  SYMLINK+="stm32-bridge", MODE="0666"
+```
+
+Apply rules:
+```bash
+sudo cp docs/99-saltybot.rules /etc/udev/rules.d/
+sudo udevadm control --reload-rules && sudo udevadm trigger
+```

jetson/docs/power-budget.md

@@ -0,0 +1,188 @@
+# Jetson Nano Power Budget Analysis
+## Self-Balancing Robot — 10W Envelope
+
+Last updated: 2026-02-28
+Target: Operate within 10W SoC power envelope (MAXN 10W mode)
+
+---
+
+## Power Modes
+
+Jetson Nano supports two NVPModel power modes:
+
+| Mode | GPU | CPU cores | CPU freq | Memory freq | TDP |
+|------|-----|-----------|----------|-------------|-----|
+| **MAXN (Mode 0)** | 128 core | 4 | 1.43GHz | 1600MHz | **10W** |
+| 5W (Mode 1) | 128 core | 2 | 0.92GHz | 1600MHz | 5W |
+
+For this robot, we target **MAXN 10W mode** with careful peripheral management.
+
+```bash
+# Check current mode
+sudo nvpmodel -q
+
+# Set 10W MAXN mode
+sudo nvpmodel -m 0
+
+# Set 5W mode (thermal/battery save)
+sudo nvpmodel -m 1
+
+# Monitor power in real time
+sudo tegrastats
+```
+
+---
+
+## Component Power Budget
+
+### SoC (Jetson Nano Module)
+
+| Component | Idle (W) | Load (W) | Peak (W) | Notes |
+|-----------|----------|----------|----------|-------|
+| CPU (4× Cortex-A57) | 1.0 | 3.5 | 4.0 | ROS2 + SLAM compute |
+| GPU (128-core Maxwell) | 0.5 | 2.5 | 3.0 | Depth processing, ML inference |
+| DDR4 RAM (4GB) | 0.3 | 0.6 | 0.8 | |
+| eMMC / SD | 0.1 | 0.2 | 0.3 | |
+| **SoC Subtotal** | **1.9** | **6.8** | **8.1** | |
+
+### Peripherals (USB / GPIO)
+
+| Peripheral | Idle (W) | Active (W) | Peak (W) | Interface | Notes |
+|-----------|----------|------------|----------|-----------|-------|
+| RealSense D435i | 0.3 | 1.5 | 3.5 | USB 3.1 | Peak during boot/init |
+| RPLIDAR A1M8 | 0.4 | 2.6 | 3.0 | USB (UART adapter) | Motor spinning |
+| STM32F722 bridge | 0.3 | 0.5 | 0.8 | USB CDC | Powered from Jetson USB |
+| **Peripheral Subtotal** | **1.0** | **4.6** | **7.3** | | |
+
+### Total System (from Jetson 5V barrel jack)
+
+| Scenario | SoC (W) | Peripherals (W) | **Total (W)** | Margin |
+|----------|---------|-----------------|---------------|--------|
+| Idle | 1.9 | 1.0 | **2.9** | +7.1W |
+| Nominal (SLAM running) | 6.8 | 4.6 | **11.4** | **-1.4W ⚠️** |
+| Peak (all active, ML) | 8.1 | 7.3 | **15.4** | **-5.4W ❌** |
+
+---
+
+## Budget Compliance Strategy
+
+The nominal load of **11.4W exceeds the 10W envelope** — mitigation required:
+
+### Mitigation 1: RPLIDAR Power Gating
+The RPLIDAR motor can be stopped when not scanning. The ROS2 driver handles this via DTR line.
+
+| Mode | Savings |
+|------|---------|
+| RPLIDAR motor off | −2.2W |
+| RPLIDAR idle | 0.4W vs 2.6W |
+
+### Mitigation 2: RealSense Resolution Reduction
+Lower RGB-D resolution reduces USB bandwidth and D435i processing:
+
+| Profile | Power |
+|---------|-------|
+| 1280×720 @ 30fps | 1.5W |
+| 640×480 @ 30fps | 1.1W ← **Recommended** |
+| 424×240 @ 30fps | 0.8W |
+
+### Mitigation 3: Jetson GPU Workload Scheduling
+Avoid running depth inference and SLAM simultaneously at full throttle:
+
+```bash
+# Cap GPU frequency (reduce from max 921.6MHz)
+sudo jetson_clocks --show
+# Set conservative clocks
+echo 614400000 | sudo tee /sys/devices/17000000.gp10b/devfreq/17000000.gp10b/min_freq
+```
+
+### Mitigation 4: STM32 Self-Powered
+Power STM32 from robot's 5V bus (separate from Jetson USB rail):
+
+| Option | Jetson USB load |
+|--------|----------------|
+| STM32 powered from Jetson USB | 0.5W |
+| STM32 powered from robot 5V | **0W** (data only via USB) |
+
+---
+
+## Revised Budget with Mitigations
+
+Applying: 640×480 D435i + RPLIDAR gating + STM32 self-powered:
+
+| Component | Power (W) |
+|-----------|-----------|
+| CPU (SLAM, 4 cores) | 3.5 |
+| GPU (depth processing) | 2.0 |
+| RAM + misc SoC | 1.0 |
+| RealSense D435i (640×480) | 1.1 |
+| RPLIDAR A1M8 (active) | 2.6 |
+| STM32 bridge (self-powered) | 0.0 |
+| **Total** | **10.2W** |
+
+**Near-compliant at 10.2W.** Further savings achievable by:
+- Enabling RPLIDAR standby between scan cycles (−0.5W avg)
+- Using 5W nvpmodel during motor-heavy phases
+
+---
+
+## Input Power Requirements
+
+### Jetson Nano Power Input
+| Spec | Value |
+|------|-------|
+| Input connector | 5.5mm / 2.1mm barrel jack |
+| Input voltage | 5V DC |
+| Recommended current | ≥4A (20W supply for headroom) |
+| Absolute max | 5.25V |
+
+> **Use a 5V 4A supply minimum.** A 2A supply will brownout under load.
+
+### Robot Power Architecture (Recommended)
+
+```
+LiPo 3S (12.6V max)
+       │
+       ├─► DC-DC Buck → 5V 5A ──► Jetson Nano barrel jack
+       │   (e.g., XL4016)
+       │
+       ├─► DC-DC Buck → 5V 3A ──► STM32 + logic 5V rail
+       │
+       └─► Hoverboard ESC ──► Hub motors (48V loop)
+```
+
+This isolates the Jetson 5V supply from motor switching noise.
+
+---
+
+## Real-Time Monitoring
+
+```bash
+# Live power telemetry
+sudo tegrastats --interval 500
+
+# Key fields:
+# POM_5V_IN X/Y   — total input power (current W / average W)
+# POM_5V_GPU X/Y  — GPU power
+# POM_5V_CPU X/Y  — CPU power
+
+# Log to file
+sudo tegrastats --interval 1000 --logfile /tmp/power_log.txt &
+
+# Parse log
+grep "POM_5V_IN" /tmp/power_log.txt | \
+  awk '{for(i=1;i<=NF;i++) if($i=="POM_5V_IN") print $(i+1)}' | \
+  awk -F'/' '{sum+=$1; count++} END {print "Avg:", sum/count, "mW"}'
+```
+
+---
+
+## Summary
+
+| Metric | Value |
+|--------|-------|
+| Target envelope | 10W |
+| Nominal (no mitigation) | 11.4W |
+| Nominal (with mitigations) | ~10.2W |
+| Compliant scenario | RPLIDAR standby + 640p D435i |
+| Recommended PSU | 5V 4A (20W) |
+| Power mode | nvpmodel MAXN (Mode 0) |

jetson/scripts/build-and-run.sh

@@ -0,0 +1,51 @@
+#!/usr/bin/env bash
+# Quick build + run helper for the Jetson ROS2 stack
+# Usage: bash build-and-run.sh [build|up|down|logs|shell]
+
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+JETSON_DIR="$(dirname "$SCRIPT_DIR")"
+
+cd "$JETSON_DIR"
+
+ACTION="${1:-up}"
+
+case "$ACTION" in
+    build)
+        echo "[saltybot] Building ROS2 Docker image..."
+        docker compose build --no-cache
+        ;;
+    up)
+        echo "[saltybot] Starting ROS2 stack..."
+        docker compose up -d
+        docker compose logs -f
+        ;;
+    down)
+        echo "[saltybot] Stopping ROS2 stack..."
+        docker compose down
+        ;;
+    logs)
+        docker compose logs -f
+        ;;
+    shell)
+        echo "[saltybot] Opening shell in ros2 container..."
+        docker compose exec saltybot-ros2 bash
+        ;;
+    slam)
+        echo "[saltybot] Launching SLAM only..."
+        docker compose up -d rplidar realsense saltybot-ros2
+        docker compose logs -f saltybot-ros2
+        ;;
+    status)
+        docker compose ps
+        echo ""
+        echo "--- ROS2 topics (requires running container) ---"
+        docker compose exec saltybot-ros2 \
+            bash -c "source /opt/ros/humble/setup.bash && ros2 topic list" 2>/dev/null || true
+        ;;
+    *)
+        echo "Usage: $0 [build|up|down|logs|shell|slam|status]"
+        exit 1
+        ;;
+esac

jetson/scripts/entrypoint.sh

@@ -0,0 +1,18 @@
+#!/usr/bin/env bash
+# Docker entrypoint for Jetson Nano ROS2 container
+
+set -e
+
+# Source ROS2
+source /opt/ros/humble/setup.bash
+
+# Source workspace if built
+if [ -f /ros2_ws/install/local_setup.bash ]; then
+    source /ros2_ws/install/local_setup.bash
+fi
+
+# Set DDS implementation
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+
+echo "[saltybot] ROS2 Humble ready — domain ID: ${ROS_DOMAIN_ID:-42}"
+exec "$@"

jetson/scripts/setup-jetson.sh

@@ -0,0 +1,127 @@
+#!/usr/bin/env bash
+# Jetson Nano host setup script
+# Run once on fresh JetPack 4.6 installation
+# Usage: sudo bash setup-jetson.sh
+
+set -euo pipefail
+
+echo "=== Jetson Nano Host Setup — saltybot ==="
+echo "JetPack 4.6 / L4T R32.6.1 expected"
+
+# ── Verify we're on Jetson ────────────────────────────────────────────────────
+if ! uname -m | grep -q aarch64; then
+    echo "ERROR: Must run on Jetson (aarch64). Got: $(uname -m)"
+    exit 1
+fi
+
+# ── System update ─────────────────────────────────────────────────────────────
+apt-get update && apt-get upgrade -y
+
+# ── Install Docker + NVIDIA runtime ──────────────────────────────────────────
+if ! command -v docker &>/dev/null; then
+    echo "[+] Installing Docker..."
+    curl -fsSL https://get.docker.com | sh
+    usermod -aG docker "$SUDO_USER"
+fi
+
+# NVIDIA container runtime
+if ! dpkg -l | grep -q nvidia-container-runtime; then
+    echo "[+] Installing NVIDIA Container Runtime..."
+    distribution=$(. /etc/os-release; echo $ID$VERSION_ID)
+    curl -s -L https://nvidia.github.io/libnvidia-container/gpgkey | apt-key add -
+    curl -s -L "https://nvidia.github.io/libnvidia-container/$distribution/libnvidia-container.list" \
+        > /etc/apt/sources.list.d/nvidia-container-runtime.list
+    apt-get update
+    apt-get install -y nvidia-container-runtime
+fi
+
+# Configure Docker daemon for NVIDIA runtime
+cat > /etc/docker/daemon.json << 'EOF'
+{
+    "runtimes": {
+        "nvidia": {
+            "path": "nvidia-container-runtime",
+            "runtimeArgs": []
+        }
+    },
+    "default-runtime": "nvidia"
+}
+EOF
+systemctl restart docker
+
+# ── Set Jetson power mode to MAXN 10W ────────────────────────────────────────
+echo "[+] Setting MAXN 10W power mode..."
+nvpmodel -m 0
+jetson_clocks
+
+# ── Install udev rules ────────────────────────────────────────────────────────
+echo "[+] Installing udev rules..."
+cat > /etc/udev/rules.d/99-saltybot.rules << 'EOF'
+# RPLIDAR A1M8 (SiliconLabs CP2102)
+KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
+  SYMLINK+="rplidar", MODE="0666"
+
+# STM32 USB CDC (STMicroelectronics Virtual COM)
+KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
+  SYMLINK+="stm32-bridge", MODE="0666"
+
+# Intel RealSense D435i
+SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \
+  MODE="0666"
+EOF
+
+udevadm control --reload-rules
+udevadm trigger
+
+# ── Install RealSense udev rules ──────────────────────────────────────────────
+echo "[+] Installing RealSense udev rules..."
+if [ -f /etc/udev/rules.d/99-realsense-libusb.rules ]; then
+    echo "  Already installed."
+else
+    # Download from librealsense repo
+    wget -q -O /etc/udev/rules.d/99-realsense-libusb.rules \
+        https://raw.githubusercontent.com/IntelRealSense/librealsense/master/config/99-realsense-libusb.rules
+    udevadm control --reload-rules
+    udevadm trigger
+fi
+
+# ── Enable I2C + UART ─────────────────────────────────────────────────────────
+echo "[+] Enabling I2C and UART..."
+modprobe i2c-dev
+# Add user to i2c and dialout groups
+usermod -aG i2c,dialout,gpio "$SUDO_USER"
+
+# ── Configure UART (disable console on ttyTHS1) ───────────────────────────────
+# ttyTHS1 is the 40-pin header UART — disable serial console to free it
+if grep -q "console=ttyS0" /boot/extlinux/extlinux.conf; then
+    echo "[+] UART ttyTHS1 already free for application use."
+else
+    echo "[!] Warning: Check /boot/extlinux/extlinux.conf if serial console"
+    echo "    is using ttyTHS1. Disable it to use UART for STM32 bridge."
+fi
+
+# ── Docker Compose ────────────────────────────────────────────────────────────
+if ! command -v docker-compose &>/dev/null && ! docker compose version &>/dev/null 2>&1; then
+    echo "[+] Installing docker-compose..."
+    pip3 install docker-compose
+fi
+
+# ── Swap (improve stability under memory pressure) ────────────────────────────
+if [ "$(swapon --show | wc -l)" -le 1 ]; then
+    echo "[+] Creating 4GB swap file..."
+    fallocate -l 4G /swapfile
+    chmod 600 /swapfile
+    mkswap /swapfile
+    swapon /swapfile
+    echo '/swapfile none swap sw 0 0' >> /etc/fstab
+fi
+
+echo ""
+echo "=== Setup complete ==="
+echo "Please log out and back in for group membership to take effect."
+echo ""
+echo "Next steps:"
+echo "  1. cd jetson/"
+echo "  2. docker compose build"
+echo "  3. docker compose up -d"
+echo "  4. docker compose logs -f"

lib/USB_CDC/src/usbd_cdc_if.c

@@ -121,7 +121,6 @@ static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
 USBD_CDC_ItfTypeDef USBD_CDC_fops = { CDC_Init, CDC_DeInit, CDC_Control, CDC_Receive };
 
 uint8_t CDC_Transmit(uint8_t *buf, uint16_t len) {
-
     USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef *)hUsbDevice.pClassData;
     if (hcdc == NULL) return USBD_FAIL;
     if (hcdc->TxState != 0) {
@@ -130,6 +129,12 @@ uint8_t CDC_Transmit(uint8_t *buf, uint16_t len) {
         if (++busy_count > 100) { hcdc->TxState = 0; busy_count = 0; }
         return USBD_BUSY;
     }
-    USBD_CDC_SetTxBuffer(&hUsbDevice, buf, len);
+    /* Always copy into the static UserTxBuffer so the USB hardware reads
+     * from a known fixed SRAM address — never from the caller's stack.
+     * The USB TXFE IRQ fires asynchronously; a stack buffer could be
+     * overwritten by the time the FIFO is loaded. */
+    if (len > sizeof(UserTxBuffer)) len = sizeof(UserTxBuffer);
+    memcpy(UserTxBuffer, buf, len);
+    USBD_CDC_SetTxBuffer(&hUsbDevice, UserTxBuffer, len);
     return USBD_CDC_TransmitPacket(&hUsbDevice);
 }

projects/saltybot/SLAM-SETUP-PLAN.md

@@ -0,0 +1,264 @@
+# bd-wax: Jetson Nano + RealSense D435i + RPLIDAR SLAM Setup — Technical Plan
+
+**Bead:** bd-wax
+**Phase:** 2 (lower priority than Phase 1 balance)
+**Owner:** sl-perception
+**Hardware:** Jetson Nano 4GB + Intel RealSense D435i + RPLIDAR A1M8
+**Goal:** Indoor SLAM, mapping, and autonomous navigation (person-following)
+
+---
+
+## Hardware Summary
+
+| Component | Specs |
+|-----------|-------|
+| Jetson Nano 4GB | Quad-core ARM Cortex-A57 @ 1.43GHz, 128-core Maxwell GPU, 4GB LPDDR4 |
+| RealSense D435i | Stereo depth (0.1–10m), 848×480 @ 90fps, BMI055 IMU (accel+gyro) |
+| RPLIDAR A1M8 | 360° 2D LIDAR, 12m range, 8000 samples/s, ~5.5Hz scan rate |
+
+---
+
+## 1. OS & ROS2 Environment
+
+### Recommended: Docker on JetPack 4.6 (Ubuntu 18.04 / L4T 32.x)
+
+Jetson Nano ships with JetPack 4.6 (Ubuntu 18.04). Native ROS2 Humble requires Ubuntu 22.04, so **Docker is the correct approach**:
+
+- NVIDIA provides official ROS2 containers for Jetson via [dusty-nv/jetson-containers](https://github.com/dusty-nv/jetson-containers)
+- Container: `dustynv/ros:humble-ros-base-l4t-r32.7.1` (arm64, CUDA 10.2)
+- Compose file pins container, mounts device nodes (`/dev/video*`, `/dev/ttyUSB*`), and handles GPU access
+
+**Alternative: JetPack 5.x (Ubuntu 20.04)** allows native ROS2 Foxy but requires flashing newer JetPack — only worth it if we need direct hardware access outside Docker.
+
+**Decision:** Use JetPack 4.6 + Docker (Humble). Fastest path, avoids full OS reflash, proven for Nano.
+
+---
+
+## 2. SLAM Stack Recommendation
+
+### Recommendation: RTAB-Map (primary) with ORB-SLAM3 as fallback
+
+#### Why RTAB-Map
+
+| Criterion | ORB-SLAM3 | RTAB-Map |
+|-----------|-----------|----------|
+| Sensor fusion (D435i + RPLIDAR) | D435i only | D435i + RPLIDAR natively |
+| Output map type | Sparse 3D point cloud | Dense 2D occupancy + 3D point cloud |
+| Nav2 compatibility | Needs wrapper | Direct occupancy map output |
+| ATE RMSE accuracy | 0.009m | 0.019m |
+| Nano resource usage | Lower | Higher (needs tuning) |
+| Loop closure robustness | Good | Excellent |
+
+RTAB-Map's `subscribe_scan` mode uses the RPLIDAR A1M8 as the primary 2D odometry front-end (fast, low-CPU) with the D435i providing depth for loop closure and 3D reconstruction. This hybrid approach is well-documented for Nano deployments.
+
+**Note:** Cannot simultaneously use `subscribe_scan` (2D LIDAR) and `subscribe_scan_cloud` (3D point cloud) in RTAB-Map — use 2D mode.
+
+#### ORB-SLAM3 Role
+
+Keep ORB-SLAM3 as a lightweight visual odometry alternative if RTAB-Map proves too heavy for real-time operation. Can feed its odometry output into RTAB-Map's map management node.
+
+---
+
+## 3. Software Architecture
+
+```
+┌─────────────────────────────────────────────────────┐
+│                  Jetson Nano (Docker)                │
+│                                                     │
+│  ┌───────────────┐    ┌──────────────────────────┐  │
+│  │ realsense2_   │    │     rplidar_ros2          │  │
+│  │ camera node   │    │  /scan (LaserScan)        │  │
+│  │               │    │  5.5Hz, 360°, 12m range   │  │
+│  │ /depth/image  │    └──────────┬───────────────┘  │
+│  │ /color/image  │               │                  │
+│  │ /imu          │               │                  │
+│  └──────┬────────┘               │                  │
+│         │                        │                  │
+│         └──────────┬─────────────┘                  │
+│                    ▼                                 │
+│         ┌──────────────────┐                        │
+│         │   rtabmap_ros    │                        │
+│         │                  │                        │
+│         │ subscribe_scan=true (RPLIDAR)              │
+│         │ subscribe_rgbd=true (D435i depth)          │
+│         │                  │                        │
+│         │ → /map (OccupancyGrid)                    │
+│         │ → /rtabmap/odom                           │
+│         │ → /rtabmap/cloud_map (3D)                 │
+│         └──────────┬────────┘                       │
+│                    │                                │
+│                    ▼                                │
+│         ┌──────────────────┐                        │
+│         │   nav2 stack     │  (Phase 2b)            │
+│         │  (reduced freq)  │                        │
+│         │  5-10Hz costmap  │                        │
+│         └──────────────────┘                        │
+└─────────────────────────────────────────────────────┘
+         │                        │
+    USB3 (D435i)             USB2 (RPLIDAR)
+```
+
+### ROS2 Node Graph
+
+| Node | Package | Key Topics |
+|------|---------|------------|
+| `camera/realsense2_camera_node` | `realsense2_camera` | `/camera/depth/image_rect_raw`, `/camera/color/image_raw`, `/camera/imu` |
+| `rplidar_node` | `rplidar_ros` | `/scan` |
+| `rtabmap` | `rtabmap_ros` | `/map`, `/rtabmap/odom`, `/rtabmap/cloud_map` |
+| `robot_state_publisher` | `robot_state_publisher` | `/tf` (static transforms) |
+
+---
+
+## 4. Implementation Phases
+
+### Phase 2a — SLAM Bring-up (this bead, bd-wax)
+
+**Deliverables:**
+- [ ] Docker Compose file (`docker/slam/docker-compose.yml`)
+- [ ] ROS2 Humble container with RTAB-Map + RealSense + RPLIDAR packages
+- [ ] Launch file: `launch/slam.launch.py` (RTAB-Map + both sensor nodes)
+- [ ] URDF/static TF: D435i and RPLIDAR positions on robot frame
+- [ ] `README.md` for Jetson setup instructions
+
+**Out of scope (Phase 2b):**
+- Nav2 autonomous navigation
+- Person-following
+- Integration with STM32 motor commands (needs Phase 1 balance complete first)
+
+### Phase 2b — Nav2 Integration (separate bead)
+
+- Nav2 stack with pre-built occupancy maps
+- 5–10Hz costmap updates (reduced from desktop default 20Hz)
+- Velocity command bridge: Nav2 `/cmd_vel` → STM32 via serial/ROS2 bridge
+- Person detection (YOLOv5 on Nano, or TensorRT-optimized)
+
+---
+
+## 5. Key Configuration Parameters
+
+### RTAB-Map Tuning for Jetson Nano
+
+```yaml
+# rtabmap_ros params — power/performance tradeoffs for Nano
+rtabmap:
+  Rtabmap/DetectionRate: "2"          # Process keyframes at 2Hz (not every frame)
+  Rtabmap/TimeThr: "700"              # Max processing time 700ms per iteration
+  Kp/MaxFeatures: "400"               # Reduce keypoints from default 1000
+  Vis/MaxFeatures: "400"
+  RGBD/LinearUpdate: "0.1"            # Only update map if moved 10cm
+  RGBD/AngularUpdate: "0.09"          # Or rotated ~5 degrees
+  Mem/STMSize: "30"                   # Short-term memory limit (saves RAM)
+  Grid/3D: "false"                    # Use 2D occupancy only (lighter)
+  Grid/RangeMin: "0.5"                # Ignore points closer than 0.5m
+  Grid/RangeMax: "5.0"                # Limit to 5m (A1M8 is reliable to ~8m)
+```
+
+### RealSense D435i Launch Params
+
+```yaml
+# realsense2_camera — reduce load on Nano
+depth_module.profile: "640x480x15"    # 15fps depth (not 90fps)
+rgb_camera.profile: "640x480x15"      # 15fps color
+enable_gyro: true
+enable_accel: true
+unite_imu_method: 2                   # Publish unified /camera/imu topic
+align_depth.enable: true
+```
+
+### RPLIDAR
+
+```yaml
+serial_port: "/dev/ttyUSB0"
+serial_baudrate: 115200
+scan_mode: "Standard"                 # A1M8 only supports Standard mode
+frame_id: "laser_frame"
+```
+
+---
+
+## 6. Static TF / URDF Robot Frame
+
+Robot coordinate frame (`base_link`) transforms needed:
+
+```
+base_link
+├── laser_frame      (RPLIDAR A1M8 — top center of robot)
+├── camera_link      (RealSense D435i — front, ~camera_height above base)
+│   ├── camera_depth_frame
+│   └── camera_imu_frame
+└── imu_link         (STM32 IMU — FC board location)
+```
+
+Transforms will be defined in `urdf/saltybot.urdf.xacro` with measured offsets once hardware is physically mounted.
+
+---
+
+## 7. Power Budget Concern
+
+Jetson Nano 4GB maximum draw: **10W** (5V/2A barrel jack mode)
+
+| Component | Estimated Draw |
+|-----------|---------------|
+| Jetson Nano (under SLAM load) | 7–8W |
+| RealSense D435i (USB3) | 1.5W |
+| RPLIDAR A1M8 (USB) | 0.5W |
+| **Total** | **~10W** |
+
+**Risk:** Marginal power budget. Recommend:
+1. Use 5V/4A supply (requires Jetson J48 header, not USB-C)
+2. Disable Jetson display output (`sudo systemctl disable gdm3`)
+3. Use `nvpmodel -m 1` (5W mode) during mapping-only tasks
+4. External powered USB hub for peripherals
+
+---
+
+## 8. Milestone Checklist
+
+- [ ] Flash JetPack 4.6 on Nano (or verify existing install)
+- [ ] Install Docker + NVIDIA Container Runtime on Nano
+- [ ] Pull `dustynv/ros:humble-ros-base-l4t-r32.7.1` container
+- [ ] Verify D435i recognized: `realsense-viewer` or `rs-enumerate-devices`
+- [ ] Verify RPLIDAR port: `ls /dev/ttyUSB*`
+- [ ] Build/pull `realsense2_camera` ROS2 package in container
+- [ ] Build/pull `rplidar_ros` ROS2 package in container
+- [ ] Build/pull `rtabmap_ros` ROS2 package in container
+- [ ] Test individual sensor topics with `ros2 topic echo`
+- [ ] Run SLAM launch file — verify `/map` published
+- [ ] Record rosbag for offline tuning
+- [ ] Document measured TF offsets (physical mount positions)
+- [ ] Write Phase 2b bead for Nav2 integration
+
+---
+
+## 9. Repo Structure (to be created)
+
+```
+saltylab-firmware/
+└── projects/
+    └── saltybot/
+        ├── SLAM-SETUP-PLAN.md         ← this file
+        ├── SALTYLAB.md                ← main design doc (TBD)
+        └── slam/
+            ├── docker/
+            │   ├── docker-compose.yml
+            │   └── Dockerfile.slam
+            ├── launch/
+            │   └── slam.launch.py
+            ├── config/
+            │   ├── rtabmap_params.yaml
+            │   └── realsense_params.yaml
+            └── urdf/
+                └── saltybot.urdf.xacro
+```
+
+---
+
+## 10. References
+
+- [dusty-nv/jetson-containers](https://github.com/dusty-nv/jetson-containers) — official NVIDIA ROS2 Docker containers for Jetson
+- [rtabmap_ros](https://github.com/introlab/rtabmap_ros) — RTAB-Map ROS2 wrapper
+- [realsense-ros](https://github.com/IntelRealSense/realsense-ros) — Intel RealSense ROS2 wrapper
+- [rplidar_ros](https://github.com/Slamtec/rplidar_ros) — Slamtec RPLIDAR ROS2 package
+- [reedjacobp/JetsonBot](https://github.com/reedjacobp/JetsonBot) — Nano + D435i + RPLIDAR reference implementation
+- [RTAB-Map D435 + RPLidar discussion](https://answers.ros.org/question/367019/using-2-d435-cameras-and-an-rplidar-with-rtabmap/)
+- [Comparative SLAM evaluation (2024)](https://arxiv.org/html/2401.02816v1)

src/balance.c

@@ -2,13 +2,6 @@
 #include "config.h"
 #include <math.h>
 
-/* MPU6000 raw to physical units (default ±2g, ±250°/s) */
-#define ACCEL_SCALE  (1.0f / 16384.0f)   /* LSB to g (±2g range) */
-#define GYRO_SCALE   (1.0f / 131.0f)     /* LSB to °/s (±250°/s range) */
-
-/* Complementary filter coefficient — 0.98 trusts gyro, 0.02 corrects with accel */
-#define COMP_ALPHA   0.98f
-
 void balance_init(balance_t *b) {
     b->state = BALANCE_DISARMED;
     b->pitch_deg = 0.0f;
@@ -28,30 +21,12 @@ void balance_init(balance_t *b) {
     b->max_speed = MAX_SPEED_LIMIT;
 }
 
-void balance_update(balance_t *b, const icm42688_data_t *imu, float dt) {
+void balance_update(balance_t *b, const IMUData *imu, float dt) {
     if (dt <= 0.0f || dt > 0.1f) return;  /* Sanity check dt */
 
-    /* Convert raw IMU to physical units */
-    float ax = imu->ax * ACCEL_SCALE;
-    float ay = imu->ay * ACCEL_SCALE;
-    float az = imu->az * ACCEL_SCALE;
-
-    /*
-     * Gyro axis for pitch depends on mounting orientation.
-     * MPU6000 on MAMBA F722S with CW270 alignment:
-     * Pitch rate = gx axis (adjust sign if needed during testing)
-     */
-    float gyro_pitch_rate = imu->gx * GYRO_SCALE;
-    b->pitch_rate = gyro_pitch_rate;
-
-    /* Accelerometer pitch angle (atan2 of forward/down axes)
-     * With CW270: pitch = atan2(ax, az)
-     * Adjust axes based on actual mounting during testing */
-    float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265f);
-
-    /* Complementary filter */
-    b->pitch_deg = COMP_ALPHA * (b->pitch_deg + gyro_pitch_rate * dt)
-                 + (1.0f - COMP_ALPHA) * accel_pitch;
+    /* Consume fused angle from mpu6000 complementary filter */
+    b->pitch_deg  = imu->pitch;
+    b->pitch_rate = imu->pitch_rate;
 
     /* Safety: tilt cutoff */
     if (b->state == BALANCE_ARMED) {
@@ -82,8 +57,8 @@ void balance_update(balance_t *b, const icm42688_data_t *imu, float dt) {
     if (b->integral < -PID_INTEGRAL_MAX) b->integral = -PID_INTEGRAL_MAX;
     float i_term = b->ki * b->integral;
 
-    /* Derivative (on measurement to avoid setpoint kick) */
-    float d_term = b->kd * (-gyro_pitch_rate);  /* Use gyro directly, cleaner than differencing */
+    /* Derivative on measurement (avoids setpoint kick) */
+    float d_term = b->kd * (-b->pitch_rate);
 
     /* Sum and clamp */
     float output = p_term + i_term + d_term;

src/icm42688.c

@@ -104,7 +104,8 @@ int icm42688_init(void) {
     HAL_GPIO_Init(MPU_CS_PORT, &gpio);
     cs_high();
 
-    SCB_DisableDCache();  /* Avoid SPI DCache issues on F7 */
+    /* DCache is disabled at startup in main.c before USB/peripherals init.
+     * No SCB_DisableDCache() here — calling it after USB starts corrupts USB state. */
 
     hspi1.Instance = SPI1;
     hspi1.Init.Mode = SPI_MODE_MASTER;
@@ -137,7 +138,8 @@ int icm42688_init(void) {
         imu_type = 2;
         ret = -99; /* TODO: ICM init */
     } else {
-        ret = -who;
+        /* who==0 means no SPI response — must not return 0 (false success) */
+        ret = (who != 0) ? -(int)who : -128;
     }
 
     /* Speed up SPI for reads */

src/main.c

@@ -3,8 +3,7 @@
 #include "usbd_cdc.h"
 #include "usbd_cdc_if.h"
 #include "usbd_desc.h"
-#include "icm42688.h"
-#include "bmp280.h"
+#include "mpu6000.h"
 #include "balance.h"
 #include "hoverboard.h"
 #include "config.h"
@@ -54,6 +53,7 @@ void SysTick_Handler(void) { HAL_IncTick(); }
 
 int main(void) {
     SCB_EnableICache();
+    SCB_DisableDCache();  /* Must be off before USB starts — STM32F7 DCache/USB coherency */
     checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */
     HAL_Init();
     SystemClock_Config();
@@ -67,8 +67,8 @@ int main(void) {
     status_init();
     HAL_Delay(3000);  /* Wait for USB host to enumerate */
 
-    /* Init IMU */
-    int imu_ret = icm42688_init();
+    /* Init IMU (MPU6000 via SPI1 — mpu6000.c wraps icm42688 + complementary filter) */
+    int imu_ret = mpu6000_init() ? 0 : -1;
 
     /* Init hoverboard ESC UART */
     hoverboard_init();
@@ -80,14 +80,14 @@ int main(void) {
     char buf[256];
     int len;
 
-    icm42688_data_t imu;
+    IMUData imu;
     uint32_t send_tick = 0;
     uint32_t balance_tick = 0;
     uint32_t esc_tick = 0;
     const float dt = 1.0f / PID_LOOP_HZ;  /* 1ms at 1kHz */
 
     while (1) {
-        if (imu_ret == 0) icm42688_read(&imu);
+        if (imu_ret == 0) mpu6000_read(&imu);
 
         uint32_t now = HAL_GetTick();
 
@@ -123,10 +123,9 @@ int main(void) {
             send_tick = now;
             if (imu_ret == 0) {
                 len = snprintf(buf, sizeof(buf),
-                    "{\"ax\":%d,\"ay\":%d,\"az\":%d,\"gx\":%d,\"gy\":%d,\"gz\":%d,"
-                    "\"p\":%d,\"m\":%d,\"s\":%d}\n",
-                    imu.ax, imu.ay, imu.az, imu.gx, imu.gy, imu.gz,
-                    (int)(bal.pitch_deg * 10),   /* pitch x10 for precision without %f */
+                    "{\"p\":%d,\"r\":%d,\"m\":%d,\"s\":%d}\n",
+                    (int)(imu.pitch * 10),       /* fused pitch degrees x10 */
+                    (int)(imu.pitch_rate * 10),  /* gyro pitch rate °/s x10 */
                     (int)bal.motor_cmd,
                     (int)bal.state);
             } else {

src/mpu6000.c

@@ -0,0 +1,85 @@
+/*
+ * mpu6000.c — IMU Sensor Fusion for MPU6000
+ *
+ * Wraps the icm42688 SPI driver (which auto-detects MPU6000) and applies
+ * a complementary filter to produce a stable pitch estimate.
+ *
+ * Hardware: MAMBA F722S, MPU6000 on SPI1, CW270 alignment
+ * Config:   Gyro ±2000°/s (init_mpu6000 sets FS_SEL=3)
+ *           Accel ±16g    (init_mpu6000 sets AFS_SEL=3)
+ */
+
+#include "mpu6000.h"
+#include "icm42688.h"
+#include "stm32f7xx_hal.h"
+#include <math.h>
+
+/* Scale factors matching init_mpu6000() config in icm42688.c */
+#define GYRO_SCALE    (1.0f / 16.384f)   /* LSB to °/s  — ±2000°/s range */
+#define ACCEL_SCALE   (1.0f / 2048.0f)   /* LSB to g    — ±16g range     */
+
+/*
+ * Complementary filter coefficient.
+ * 0.98 = trust gyro integration, 0.02 = accel correction for drift.
+ * Tune higher (0.99) for noisier environments.
+ */
+#define COMP_ALPHA    0.98f
+
+/* Filter state */
+static float s_pitch = 0.0f;
+static uint32_t s_last_tick = 0;
+
+bool mpu6000_init(void) {
+    int ret = icm42688_init();
+    if (ret == 0) {
+        s_pitch = 0.0f;
+        s_last_tick = HAL_GetTick();
+    }
+    return (ret == 0);
+}
+
+void mpu6000_read(IMUData *data) {
+    icm42688_data_t raw;
+    icm42688_read(&raw);
+
+    /* Compute dt from wall clock — robust to loop jitter */
+    uint32_t now = HAL_GetTick();
+    uint32_t elapsed_ms = now - s_last_tick;
+    if (elapsed_ms == 0) elapsed_ms = 1;         /* min 1ms to avoid divide-by-zero */
+    if (elapsed_ms > 100) elapsed_ms = 100;       /* clamp: don't integrate stale data */
+    float dt = elapsed_ms * 0.001f;
+    s_last_tick = now;
+
+    /* Convert raw to physical units */
+    float ax = raw.ax * ACCEL_SCALE;   /* g  */
+    float az = raw.az * ACCEL_SCALE;   /* g  */
+
+    /*
+     * Gyro pitch axis with CW270 alignment: pitch rate = gx.
+     * Sign may need inverting depending on mounting orientation —
+     * verify during bench test (positive nose-up should be positive).
+     */
+    float gyro_pitch_rate = raw.gx * GYRO_SCALE;  /* °/s */
+
+    /*
+     * Accel pitch angle (degrees).
+     * CW270 alignment: pitch = atan2(ax, az).
+     * Valid while ax² + az² ≈ 1g (i.e., low linear acceleration).
+     */
+    float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265358979f);
+
+    /*
+     * Complementary filter:
+     *   pitch = α * (pitch + ω*dt) + (1−α) * accel_pitch
+     *
+     * Gyro integration tracks fast dynamics; accel correction
+     * prevents long-term drift.
+     */
+    s_pitch = COMP_ALPHA * (s_pitch + gyro_pitch_rate * dt)
+            + (1.0f - COMP_ALPHA) * accel_pitch;
+
+    data->pitch      = s_pitch;
+    data->pitch_rate = gyro_pitch_rate;
+    data->accel_x    = ax;
+    data->accel_z    = az;
+}