feat: BME280 baro pressure & ambient temp driver (Issue #672)

- New baro module (include/baro.h, src/baro.c): reads BME280/BMP280
  at 1 Hz on I2C1, computes pressure altitude (ISA formula), publishes
  JLINK_TLM_BARO (0x8D) telemetry to Orin. Runs entirely on Mamba F722S
  with no Orin dependency. baro_get_alt_cm() exposes altitude to balance
  PID slope compensation.
- New JLink telemetry frame 0x8D (jlink_tlm_baro_t, 12 bytes packed):
  pressure_pa (int32), temp_x10 (int16), alt_cm (int32),
  humidity_pct_x10 (int16; -1 = BMP280/absent).
- Wire into main.c: baro_init() after bmp280_init(), baro_tick(now)
  each ms (self-rate-limits to 1 Hz).
- Unit tests (test/test_baro.c): 31 tests, all pass. Build:
    gcc -I include -I test/stubs -DTEST_HOST -lm -o /tmp/test_baro test/test_baro.c

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

chassis/canable_mount.scad

@@ -0,0 +1,265 @@
+// ============================================================
+// canable_mount.scad — CANable 2.0 USB-CAN Adapter Cradle
+// Issue #654 / sl-mechanical  2026-03-16
+// ============================================================
+// Snap-fit cradle for CANable 2.0 PCB (~60 × 18 × 10 mm).
+// Attaches to 2020 aluminium T-slot rail via 2× M5 T-nuts.
+//
+// Port access:
+//   USB-C port       — X− end wall cutout (connector protrudes through)
+//   CAN terminal     — X+ end wall cutout (CANH / CANL / GND wire exit)
+//   LED status window— slot in Y+ side wall, PCB top-face LEDs visible
+//
+// Retention: snap-fit cantilever lips on both side walls (PETG flex).
+// Cable strain relief: zip-tie boss pair on X+ shelf (CAN wires).
+//
+// ⚠  VERIFY WITH CALIPERS BEFORE PRINTING:
+//    PCB_L, PCB_W          board outline
+//    USBC_W, USBC_H        USB-C shell at X− edge
+//    TERM_W, TERM_H        3-pos terminal block at X+ edge
+//    LED_X_CTR, LED_WIN_W  LED window position on Y+ wall
+//
+// Print settings (PETG):
+//   3 perimeters, 40 % gyroid infill, no supports, 0.2 mm layer
+//   Print orientation: open face UP (as modelled)
+//
+// BOM:
+//   2 × M5×10 BHCS  +  2 × M5 slide-in T-nut  (2020 rail)
+//
+// Export commands:
+//   openscad -D 'RENDER="mount"'    -o canable_mount.stl    canable_mount.scad
+//   openscad -D 'RENDER="assembly"' -o canable_assembly.png canable_mount.scad
+// ============================================================
+
+RENDER = "assembly"; // mount | assembly
+
+$fn = 48;
+EPS = 0.01;
+
+// ── ⚠ Verify before printing ─────────────────────────────────
+// CANable 2.0 PCB
+PCB_L    = 60.0;   // board length  (X: USB-C end → terminal end)
+PCB_W    = 18.0;   // board width   (Y)
+PCB_T    =  1.6;   // board thickness
+COMP_H   =  8.5;   // tallest component above board (USB-C shell ≈ 3.5 mm;
+                   //   terminal block ≈ 8.5 mm)
+
+// USB-C connector (at X− end face of PCB)
+USBC_W   =  9.5;   // connector outer width
+USBC_H   =  3.8;   // connector outer height above board surface
+USBC_Z0  =  0.0;   // connector bottom offset above board surface
+
+// CAN screw-terminal block (at X+ end face, 3-pos 5.0 mm pitch)
+TERM_W   = 16.0;   // terminal block span (3 × 5 mm + housing)
+TERM_H   =  9.0;   // terminal block height above board surface
+TERM_Z0  =  0.5;   // terminal bottom offset above board surface
+
+// Status LED window (LEDs near USB-C end on PCB top face)
+//   Rectangular slot cut in Y+ side wall — LEDs visible from the side
+LED_X_CTR  = 11.0;  // LED zone centre measured from PCB X− edge
+LED_WIN_W  = 14.0;  // window width (X)
+LED_WIN_H  =  5.5;  // window height (Z) — opens top portion of side wall
+
+// ── Cradle geometry ──────────────────────────────────────────
+WALL_T   = 2.5;    // side/end wall thickness
+FLOOR_T  = 4.0;    // floor plate thickness (accommodates M5 BHCS head pocket)
+CL_SIDE  = 0.30;   // Y clearance per side (total 0.6 mm play)
+CL_END   = 0.40;   // X clearance per end
+
+// Interior cavity
+INN_W    = PCB_W + 2*CL_SIDE;          // Y span
+INN_L    = PCB_L + 2*CL_END;           // X span
+INN_H    = PCB_T + COMP_H + 1.2;       // Z height (board + tallest comp + margin)
+
+// Outer body
+OTR_W    = INN_W  + 2*WALL_T;          // Y
+OTR_L    = INN_L  + 2*WALL_T;          // X
+OTR_H    = FLOOR_T + INN_H;            // Z
+
+// PCB reference origin within body (lower-left corner of board)
+PCB_X0   = WALL_T + CL_END;            // board X start inside body
+PCB_Y0   = WALL_T + CL_SIDE;           // board Y start inside body
+PCB_Z0   = FLOOR_T;                    // board bottom sits on floor
+
+// ── Snap-fit lips ─────────────────────────────────────────────
+// Cantilever ledge on inner face of each side wall, at PCB-top Z.
+// Tapered (chamfered) entry guides PCB in from above.
+SNAP_IN  = 0.8;    // how far inward ledge protrudes over PCB edge
+SNAP_T   = 1.2;    // snap-arm thickness (thin for PETG flex)
+SNAP_H   = 4.0;    // cantilever arm height (root at OTR_H, tip near PCB_Z0+PCB_T)
+SNAP_L   = 18.0;   // arm length along X (centred on PCB, shorter = more flex)
+// Snap on Y− wall protrudes in +Y direction; Y+ wall protrudes in −Y direction
+
+// ── M5 T-nut mount (2020 rail) ────────────────────────────────
+M5_D       =  5.3;   // M5 bolt clearance bore
+M5_HEAD_D  =  9.5;   // M5 BHCS head pocket diameter (from bottom face)
+M5_HEAD_H  =  3.0;   // BHCS head pocket depth
+M5_SPAC    = 20.0;   // bolt spacing along X (centred on cradle)
+// Standard M5 slide-in T-nuts used — no T-nut pocket moulded in.
+
+// ── Cable strain relief ───────────────────────────────────────
+// Two zip-tie anchor bosses on a shelf inside the X+ end, straddling
+// the CAN terminal wires.
+SR_BOSS_OD =  7.0;   // boss outer diameter
+SR_BOSS_H  =  5.5;   // boss height above floor
+SR_SLOT_W  =  3.5;   // zip-tie slot width
+SR_SLOT_T  =  2.2;   // zip-tie slot through-height
+// Boss Y positions (straddle terminal block)
+SR_Y1      = WALL_T + INN_W * 0.25;
+SR_Y2      = WALL_T + INN_W * 0.75;
+SR_X       = OTR_L - WALL_T - SR_BOSS_OD/2 - 2.5;  // just inside X+ end wall
+
+// ─────────────────────────────────────────────────────────────
+module canable_mount() {
+    difference() {
+        // ── Outer solid body ──────────────────────────────────
+        union() {
+            cube([OTR_L, OTR_W, OTR_H]);
+
+            // ── Snap cantilever arms on Y− wall (protrude inward +Y) ──
+            // Arms hang down from top of Y− wall inner face.
+            // Root is flush with inner face (Y = WALL_T); tip at PCB level.
+            translate([OTR_L/2 - SNAP_L/2, WALL_T - SNAP_T, OTR_H - SNAP_H])
+                cube([SNAP_L, SNAP_T, SNAP_H]);
+
+            // ── Snap cantilever arms on Y+ wall (protrude inward −Y) ──
+            translate([OTR_L/2 - SNAP_L/2, OTR_W - WALL_T, OTR_H - SNAP_H])
+                cube([SNAP_L, SNAP_T, SNAP_H]);
+
+            // ── Cable strain relief bosses (X+ end, inside) ────
+            for (sy = [SR_Y1, SR_Y2])
+                translate([SR_X, sy, 0])
+                    cylinder(d=SR_BOSS_OD, h=SR_BOSS_H);
+        }
+
+        // ── Interior cavity ───────────────────────────────────
+        translate([WALL_T, WALL_T, FLOOR_T])
+            cube([INN_L, INN_W, INN_H + EPS]);
+
+        // ── USB-C cutout — X− end wall ────────────────────────
+        // Centred on PCB width; opened from board surface upward
+        translate([-EPS,
+                   PCB_Y0 + PCB_W/2 - (USBC_W + 1.5)/2,
+                   PCB_Z0 + USBC_Z0 - 0.5])
+            cube([WALL_T + 2*EPS, USBC_W + 1.5, USBC_H + 2.5]);
+
+        // ── CAN terminal cutout — X+ end wall ─────────────────
+        // Full terminal width + 2 mm margin for screwdriver access;
+        // height clears terminal block + wire bend radius
+        translate([OTR_L - WALL_T - EPS,
+                   PCB_Y0 + PCB_W/2 - (TERM_W + 2.0)/2,
+                   PCB_Z0 + TERM_Z0 - 0.5])
+            cube([WALL_T + 2*EPS, TERM_W + 2.0, TERM_H + 5.0]);
+
+        // ── LED status window — Y+ side wall ─────────────────
+        // Rectangular slot; LEDs at top-face of PCB are visible through it
+        translate([PCB_X0 + LED_X_CTR - LED_WIN_W/2,
+                   OTR_W - WALL_T - EPS,
+                   OTR_H - LED_WIN_H])
+            cube([LED_WIN_W, WALL_T + 2*EPS, LED_WIN_H + EPS]);
+
+        // ── M5 BHCS head pockets (from bottom face of floor) ──
+        for (mx = [OTR_L/2 - M5_SPAC/2, OTR_L/2 + M5_SPAC/2])
+            translate([mx, OTR_W/2, -EPS]) {
+                // Clearance bore through full floor
+                cylinder(d=M5_D, h=FLOOR_T + 2*EPS);
+                // BHCS head pocket from bottom face
+                cylinder(d=M5_HEAD_D, h=M5_HEAD_H + EPS);
+            }
+
+        // ── Snap-arm ledge slot — Y− arm (hollow out to thin arm) ──
+        // Arm is SNAP_T thick; cut away material behind arm
+        translate([OTR_L/2 - SNAP_L/2 - EPS, EPS, OTR_H - SNAP_H])
+            cube([SNAP_L + 2*EPS, WALL_T - SNAP_T - EPS, SNAP_H + EPS]);
+
+        // ── Snap-arm ledge slot — Y+ arm ──────────────────────
+        translate([OTR_L/2 - SNAP_L/2 - EPS, OTR_W - WALL_T + SNAP_T, OTR_H - SNAP_H])
+            cube([SNAP_L + 2*EPS, WALL_T - SNAP_T - EPS, SNAP_H + EPS]);
+
+        // ── Snap-arm inward ledge notch (entry chamfer removed) ─
+        // Chamfer top of snap arm so PCB slides in easily
+        // Y− arm: chamfer on upper-inner edge → 45° wedge on +Y/+Z corner
+        translate([OTR_L/2 - SNAP_L/2 - EPS,
+                   WALL_T - SNAP_T - EPS,
+                   OTR_H - SNAP_IN])
+            rotate([0, 0, 0])
+            rotate([45, 0, 0])
+                cube([SNAP_L + 2*EPS, SNAP_IN * 1.5, SNAP_IN * 1.5]);
+
+        // Y+ arm: chamfer on upper-inner edge
+        translate([OTR_L/2 - SNAP_L/2 - EPS,
+                   OTR_W - WALL_T + SNAP_T - SNAP_IN * 1.5 + EPS,
+                   OTR_H - SNAP_IN])
+            rotate([45, 0, 0])
+                cube([SNAP_L + 2*EPS, SNAP_IN * 1.5, SNAP_IN * 1.5]);
+
+        // ── Snap ledge cutout on Y− arm inner tip ─────────────
+        // Creates inward nub: remove top portion of arm inner tip
+        // leaving bottom SNAP_IN height as the retaining ledge
+        translate([OTR_L/2 - SNAP_L/2 - EPS,
+                   WALL_T - SNAP_T - EPS,
+                   PCB_Z0 + PCB_T + SNAP_IN])
+            cube([SNAP_L + 2*EPS, SNAP_T + 2*EPS,
+                  OTR_H - (PCB_Z0 + PCB_T + SNAP_IN) + EPS]);
+
+        // ── Snap ledge cutout on Y+ arm inner tip ─────────────
+        translate([OTR_L/2 - SNAP_L/2 - EPS,
+                   OTR_W - WALL_T - EPS,
+                   PCB_Z0 + PCB_T + SNAP_IN])
+            cube([SNAP_L + 2*EPS, SNAP_T + 2*EPS,
+                  OTR_H - (PCB_Z0 + PCB_T + SNAP_IN) + EPS]);
+
+        // ── Zip-tie slots through strain relief bosses ─────────
+        for (sy = [SR_Y1, SR_Y2])
+            translate([SR_X, sy,
+                       SR_BOSS_H/2 - SR_SLOT_T/2])
+                rotate([0, 90, 0])
+                    cube([SR_SLOT_T, SR_SLOT_W,
+                          SR_BOSS_OD + 2*EPS],
+                         center=true);
+
+        // ── Weight relief pocket in floor (underside) ─────────
+        translate([WALL_T + 8, WALL_T + 3, -EPS])
+            cube([OTR_L - 2*WALL_T - 16, OTR_W - 2*WALL_T - 6,
+                  FLOOR_T - 1.5 + EPS]);
+    }
+}
+
+// ── Assembly preview ─────────────────────────────────────────
+if (RENDER == "assembly") {
+    color("DimGray", 0.93) canable_mount();
+
+    // Phantom PCB
+    color("MidnightBlue", 0.35)
+        translate([PCB_X0, PCB_Y0, PCB_Z0])
+            cube([PCB_L, PCB_W, PCB_T]);
+
+    // Phantom component block (top of PCB)
+    color("DarkSlateGray", 0.25)
+        translate([PCB_X0, PCB_Y0, PCB_Z0 + PCB_T])
+            cube([PCB_L, PCB_W, COMP_H]);
+
+    // USB-C port highlight
+    color("Gold", 0.8)
+        translate([-1,
+                   PCB_Y0 + PCB_W/2 - USBC_W/2,
+                   PCB_Z0 + USBC_Z0])
+            cube([WALL_T + 2, USBC_W, USBC_H]);
+
+    // Terminal block highlight
+    color("Tomato", 0.7)
+        translate([OTR_L - WALL_T - 1,
+                   PCB_Y0 + PCB_W/2 - TERM_W/2,
+                   PCB_Z0 + TERM_Z0])
+            cube([WALL_T + 2, TERM_W, TERM_H]);
+
+    // LED zone highlight
+    color("LimeGreen", 0.9)
+        translate([PCB_X0 + LED_X_CTR - LED_WIN_W/2,
+                   OTR_W - WALL_T - 0.5,
+                   OTR_H - LED_WIN_H + 1])
+            cube([LED_WIN_W, 1, LED_WIN_H - 2]);
+
+} else {
+    canable_mount();
+}

include/balance.h

@@ -15,8 +15,9 @@
 
 typedef enum {
     BALANCE_DISARMED   = 0,  /* Motors off, waiting for arm command */
-    BALANCE_ARMED,          /* Active balancing */
-    BALANCE_TILT_FAULT,     /* Tilt exceeded limit, motors killed */
+    BALANCE_ARMED      = 1,  /* Active balancing */
+    BALANCE_TILT_FAULT = 2,  /* Tilt exceeded limit, motors killed */
+    BALANCE_PARKED     = 3,  /* PID frozen, motors off — quick re-arm via button (Issue #682) */
 } balance_state_t;
 
 typedef struct {
@@ -46,5 +47,7 @@ void balance_init(balance_t *b);
 void balance_update(balance_t *b, const IMUData *imu, float dt);
 void balance_arm(balance_t *b);
 void balance_disarm(balance_t *b);
+void balance_park(balance_t *b);    /* ARMED -> PARKED: freeze PID, zero motors (Issue #682) */
+void balance_unpark(balance_t *b);  /* PARKED -> ARMED if pitch < 20 deg (Issue #682) */
 
 #endif

include/can_driver.h

@@ -5,7 +5,7 @@
 #include <stdbool.h>
 
 /* CAN bus driver for BLDC motor controllers (Issue #597)
- * CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps
+ * CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) at 500 kbps   (Issue #676 remap)
  * APB1 = 54 MHz: PSC=6, BS1=13tq, BS2=4tq, SJW=1tq → 18 tq/bit = 500 kbps
  */
 
@@ -78,4 +78,24 @@ void can_driver_get_stats(can_stats_t *out);
 /* Drain RX FIFO0; call every main-loop tick */
 void can_driver_process(void);
 
+/* ---- Extended / standard frame support (Issue #674) ---- */
+
+/* Callback for extended-ID (29-bit) frames arriving in FIFO1 (VESC STATUS) */
+typedef void (*can_ext_frame_cb_t)(uint32_t ext_id, const uint8_t *data, uint8_t len);
+
+/* Callback for standard-ID (11-bit) frames arriving in FIFO0 (Orin commands) */
+typedef void (*can_std_frame_cb_t)(uint16_t std_id, const uint8_t *data, uint8_t len);
+
+/* Register callback for 29-bit extended frames (register before can_driver_init) */
+void can_driver_set_ext_cb(can_ext_frame_cb_t cb);
+
+/* Register callback for 11-bit standard frames (register before can_driver_init) */
+void can_driver_set_std_cb(can_std_frame_cb_t cb);
+
+/* Transmit a 29-bit extended-ID data frame (VESC RPM/current commands) */
+void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len);
+
+/* Transmit an 11-bit standard-ID data frame (Orin telemetry broadcast) */
+void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len);
+
 #endif /* CAN_DRIVER_H */

include/config.h

@@ -257,8 +257,9 @@
 #define GIMBAL_PAN_LIMIT_DEG    180.0f        // pan  soft limit (deg each side)
 #define GIMBAL_TILT_LIMIT_DEG   90.0f         // tilt soft limit (deg each side)
 
-// --- CAN Bus Driver (Issue #597) ---
-// CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) — repurposes SPI2/OSD pins (unused on balance bot)
+// --- CAN Bus Driver (Issue #597, remapped Issue #676) ---
+// CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) — SCL/SDA pads on Mamba F722S MK2
+// I2C1 freed: BME280 moved to I2C2 (PB10/PB11); PB8/PB9 repurposed for CAN1
 #define CAN_RPM_SCALE           10             // motor_cmd to RPM: 1 cmd count = 10 RPM
 #define CAN_TLM_HZ              1u             // JLINK_TLM_CAN_STATS transmit rate (Hz)
 
@@ -295,4 +296,13 @@
 #define ENC_RIGHT_CH2_PIN       GPIO_PIN_7    // TIM3_CH2, AF2
 #define ENC_RIGHT_AF            GPIO_AF2_TIM3
 
+// --- Hardware Button (Issue #682) ---
+// Active-low push button on PC2 (internal pull-up)
+#define BTN_PORT            GPIOC
+#define BTN_PIN             GPIO_PIN_2
+#define BTN_DEBOUNCE_MS     20u     // ms debounce window
+#define BTN_LONG_MIN_MS     1500u   // ms threshold: LONG press
+#define BTN_COMMIT_MS       500u    // ms quiet after lone SHORT -> PARK event
+#define BTN_SEQ_TIMEOUT_MS  3000u   // ms: sequence window; expired buffer abandoned
+
 #endif // CONFIG_H

include/hw_button.h

@@ -0,0 +1,61 @@
+#ifndef HW_BUTTON_H
+#define HW_BUTTON_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * hw_button — hardware button debounce + gesture detection (Issue #682).
+ *
+ * Debounce FSM:
+ *   IDLE       → (raw press detected)                   → DEBOUNCING
+ *   DEBOUNCING → (still pressed after BTN_DEBOUNCE_MS)  → HELD
+ *   HELD       → (released)                             → classify press type, back to IDLE
+ *
+ * Press types:
+ *   SHORT  held < BTN_LONG_MIN_MS from confirmed start
+ *   LONG   held >= BTN_LONG_MIN_MS
+ *
+ * Sequence detection:
+ *   [SHORT, SHORT, LONG]  ->  BTN_EVENT_REARM_COMBO  (fires on LONG release)
+ *   [SHORT] + BTN_COMMIT_MS quiet timeout  ->  BTN_EVENT_PARK
+ *
+ * Config constants (can be overridden in config.h):
+ *   BTN_DEBOUNCE_MS    20   ms debounce window
+ *   BTN_LONG_MIN_MS    1500 ms threshold for LONG press
+ *   BTN_COMMIT_MS      500  ms quiet after lone SHORT -> PARK
+ *   BTN_SEQ_TIMEOUT_MS 3000 ms sequence window; expired sequence is abandoned
+ *   BTN_PORT           GPIOC
+ *   BTN_PIN            GPIO_PIN_2
+ */
+
+typedef enum {
+    BTN_EVENT_NONE        = 0,
+    BTN_EVENT_PARK        = 1,   /* single short press + quiet */
+    BTN_EVENT_REARM_COMBO = 2,   /* SHORT + SHORT + LONG */
+} hw_btn_event_t;
+
+/*
+ * hw_button_init() — configure GPIO (active-low pull-up), zero FSM state.
+ * Call once at startup.
+ */
+void hw_button_init(void);
+
+/*
+ * hw_button_tick(now_ms) — advance debounce FSM and sequence detector.
+ * Call every ms from the main loop.  Returns BTN_EVENT_NONE unless a
+ * complete gesture was recognised this tick.
+ */
+hw_btn_event_t hw_button_tick(uint32_t now_ms);
+
+/*
+ * hw_button_is_pressed() — true while button is confirmed held (post-debounce).
+ */
+bool hw_button_is_pressed(void);
+
+#ifdef TEST_HOST
+/* Inject a simulated raw pin state for host-side unit tests. */
+void hw_button_inject(bool pressed);
+#endif
+
+#endif /* HW_BUTTON_H */

include/imu_cal_flash.h

@@ -0,0 +1,51 @@
+#ifndef IMU_CAL_FLASH_H
+#define IMU_CAL_FLASH_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * IMU mount angle calibration flash storage (Issue #680).
+ *
+ * Sector 7 (128 KB at 0x08060000) layout:
+ *   0x0807FF00  imu_cal_flash_t   (64 bytes)  ← this module
+ *   0x0807FF40  pid_sched_flash_t (128 bytes) ← pid_flash.c
+ *   0x0807FFC0  pid_flash_t       (64 bytes)  ← pid_flash.c
+ *
+ * Calibration flow:
+ *   1. Mount robot at its installed angle, power on, let IMU converge (~5s).
+ *   2. Send 'O' via USB CDC (dev-only path).
+ *   3. Firmware captures current pitch + roll as mount offsets, saves to flash.
+ *   4. mpu6000_read() subtracts offsets from output on every subsequent read.
+ *
+ * The sector erase preserves existing PID data by reading it first.
+ */
+
+#define IMU_CAL_FLASH_ADDR    0x0807FF00UL
+#define IMU_CAL_FLASH_MAGIC   0x534C5403UL   /* 'SLT\x03' — version 3 */
+
+typedef struct __attribute__((packed)) {
+    uint32_t magic;           /* IMU_CAL_FLASH_MAGIC when valid */
+    float    pitch_offset;    /* degrees subtracted from IMU pitch output */
+    float    roll_offset;     /* degrees subtracted from IMU roll output */
+    uint8_t  _pad[52];        /* padding to 64 bytes */
+} imu_cal_flash_t;            /* 64 bytes total */
+
+/*
+ * imu_cal_flash_load() — read saved mount offsets from flash.
+ * Returns true and fills *pitch_offset / *roll_offset if magic is valid.
+ * Returns false if no valid calibration stored (caller keeps 0.0f defaults).
+ */
+bool imu_cal_flash_load(float *pitch_offset, float *roll_offset);
+
+/*
+ * imu_cal_flash_save() — erase sector 7 and write all three records atomically:
+ *   imu_cal_flash_t at 0x0807FF00
+ *   pid_sched_flash_t at 0x0807FF40 (preserved from existing flash)
+ *   pid_flash_t at 0x0807FFC0       (preserved from existing flash)
+ * Must be called while disarmed — sector erase stalls CPU ~1s.
+ * Returns true on success.
+ */
+bool imu_cal_flash_save(float pitch_offset, float roll_offset);
+
+#endif /* IMU_CAL_FLASH_H */

include/jlink.h

@@ -100,6 +100,7 @@
 #define JLINK_TLM_LVC           0x8Bu  /* jlink_tlm_lvc_t (4 bytes, Issue #613) */
 #define JLINK_TLM_ODOM          0x8Cu  /* jlink_tlm_odom_t (16 bytes, Issue #632) */
 #define JLINK_TLM_BARO          0x8Du  /* jlink_tlm_baro_t (12 bytes, Issue #672) */
+#define JLINK_TLM_VESC_STATE    0x8Eu  /* jlink_tlm_vesc_state_t (22 bytes, Issue #674) */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
 typedef struct __attribute__((packed)) {
@@ -249,6 +250,22 @@ typedef struct __attribute__((packed)) {
     int16_t  humidity_pct_x10;  /* %RH × 10 (BME280 only); -1 = BMP280/absent         */
 } jlink_tlm_baro_t;  /* 12 bytes */
 
+/* ---- Telemetry VESC_STATE payload (22 bytes, packed) Issue #674 ---- */
+/* Sent at VESC_TLM_HZ (1 Hz) by vesc_can_send_tlm(). */
+typedef struct __attribute__((packed)) {
+    int32_t  left_rpm;           /* left  VESC actual RPM                       */
+    int32_t  right_rpm;          /* right VESC actual RPM                       */
+    int16_t  left_current_x10;   /* left  phase current (A × 10)                */
+    int16_t  right_current_x10;  /* right phase current (A × 10)                */
+    int16_t  left_temp_x10;      /* left  FET temperature (°C × 10)             */
+    int16_t  right_temp_x10;     /* right FET temperature (°C × 10)             */
+    int16_t  voltage_x10;        /* input voltage (V × 10; from STATUS_5)       */
+    uint8_t  left_fault;         /* left  VESC fault code (0 = none)            */
+    uint8_t  right_fault;        /* right VESC fault code (0 = none)            */
+    uint8_t  left_alive;         /* 1 = left  VESC alive (STATUS within 1 s)    */
+    uint8_t  right_alive;        /* 1 = right VESC alive (STATUS within 1 s)    */
+} jlink_tlm_vesc_state_t;  /* 22 bytes */
+
 /* ---- Volatile state (read from main loop) ---- */
 typedef struct {
     /* Drive command - updated on JLINK_CMD_DRIVE */
@@ -394,4 +411,11 @@ void jlink_send_odom_tlm(const jlink_tlm_odom_t *tlm);
  */
 void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm);
 
+/*
+ * jlink_send_vesc_state_tlm(tlm) - transmit JLINK_TLM_VESC_STATE (0x8E) frame
+ * (28 bytes total) at VESC_TLM_HZ (1 Hz).  Issue #674.
+ * Rate-limiting handled by vesc_can_send_tlm(); call from there only.
+ */
+void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm);
+
 #endif /* JLINK_H */

include/mpu6000.h

@@ -29,4 +29,15 @@ bool mpu6000_is_calibrated(void);
 
 void mpu6000_read(IMUData *data);
 
+/*
+ * mpu6000_set_mount_offset(pitch_deg, roll_deg) — set mount angle offsets.
+ * These are subtracted from the pitch and roll outputs in mpu6000_read().
+ * Load via imu_cal_flash_load() on boot; update after 'O' CDC command.
+ * Issue #680.
+ */
+void mpu6000_set_mount_offset(float pitch_deg, float roll_deg);
+
+/* Returns true if non-zero mount offsets have been applied (Issue #680). */
+bool mpu6000_has_mount_offset(void);
+
 #endif

include/orin_can.h

@@ -0,0 +1,167 @@
+#ifndef ORIN_CAN_H
+#define ORIN_CAN_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * orin_can — Orin↔FC CAN protocol driver (Issue #674).
+ *
+ * Standard 11-bit CAN IDs on CAN2, FIFO0.
+ *
+ * Orin → FC commands:
+ *   0x300 HEARTBEAT : uint32 sequence counter (4 bytes)
+ *   0x301 DRIVE     : int16 speed (−1000..+1000), int16 steer (−1000..+1000)
+ *   0x302 MODE      : uint8 mode (0=RC_MANUAL, 1=ASSISTED, 2=AUTONOMOUS)
+ *   0x303 ESTOP     : uint8 action (1=ESTOP, 0=CLEAR)
+ *
+ * FC → Orin telemetry (broadcast at ORIN_TLM_HZ):
+ *   0x400 FC_STATUS : int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
+ *                     uint8 balance_state, uint8 flags [bit0=estop, bit1=armed]
+ *   0x401 FC_VESC   : int16 left_rpm_x10 (RPM/10), int16 right_rpm_x10,
+ *                     int16 left_current_x10, int16 right_current_x10
+ *
+ * Balance independence: if no Orin heartbeat for ORIN_HB_TIMEOUT_MS, the FC
+ * continues balancing in-place — Orin commands are simply not injected.
+ * The balance PID loop runs entirely on Mamba and never depends on Orin.
+ */
+
+/* ---- Orin → FC command IDs ---- */
+#define ORIN_CAN_ID_HEARTBEAT   0x300u
+#define ORIN_CAN_ID_DRIVE       0x301u
+#define ORIN_CAN_ID_MODE        0x302u
+#define ORIN_CAN_ID_ESTOP       0x303u
+#define ORIN_CAN_ID_LED_CMD     0x304u  /* LED pattern override (Issue #685) */
+
+/* ---- FC → Orin telemetry IDs ---- */
+#define ORIN_CAN_ID_FC_STATUS   0x400u  /* balance state + pitch + vbat at 10 Hz */
+#define ORIN_CAN_ID_FC_VESC     0x401u  /* VESC RPM + current at 10 Hz */
+#define ORIN_CAN_ID_FC_IMU      0x402u  /* full IMU angles + cal status at 50 Hz (Issue #680) */
+#define ORIN_CAN_ID_FC_BARO     0x403u  /* barometer pressure/temp/altitude at 1 Hz (Issue #672) */
+#define ORIN_CAN_ID_FC_BTN      0x404u  /* button event on-demand (Issue #682) */
+
+/* ---- Timing ---- */
+#define ORIN_HB_TIMEOUT_MS      500u   /* Orin offline after 500 ms without any frame */
+#define ORIN_TLM_HZ             10u    /* FC_STATUS + FC_VESC broadcast rate (Hz) */
+#define ORIN_IMU_TLM_HZ         50u    /* FC_IMU broadcast rate (Hz) */
+#define ORIN_BARO_TLM_HZ        1u     /* FC_BARO broadcast rate (Hz) */
+
+/* ---- Volatile state updated by orin_can_on_frame(), read by main loop ---- */
+typedef struct {
+    volatile int16_t  speed;           /* DRIVE: −1000..+1000 */
+    volatile int16_t  steer;           /* DRIVE: −1000..+1000 */
+    volatile uint8_t  mode;            /* MODE:  robot_mode_t value */
+    volatile uint8_t  drive_updated;   /* set on DRIVE, cleared by main */
+    volatile uint8_t  mode_updated;    /* set on MODE,  cleared by main */
+    volatile uint8_t  estop_req;       /* set on ESTOP(1), cleared by main */
+    volatile uint8_t  estop_clear_req; /* set on ESTOP(0), cleared by main */
+    volatile uint32_t last_rx_ms;      /* HAL_GetTick() of last received frame */
+} OrinCanState;
+
+extern volatile OrinCanState orin_can_state;
+
+/* ---- FC → Orin broadcast payloads (packed, 8 bytes each) ---- */
+
+typedef struct __attribute__((packed)) {
+    int16_t  pitch_x10;      /* pitch degrees × 10 */
+    int16_t  motor_cmd;      /* balance PID output −1000..+1000 */
+    uint16_t vbat_mv;        /* battery voltage (mV) */
+    uint8_t  balance_state;  /* BalanceState: 0=DISARMED,1=ARMED,2=TILT_FAULT */
+    uint8_t  flags;          /* bit0=estop_active, bit1=armed */
+} orin_can_fc_status_t;  /* 8 bytes */
+
+typedef struct __attribute__((packed)) {
+    int16_t left_rpm_x10;        /* left  wheel RPM / 10 (±32767 × 10 = ±327k RPM) */
+    int16_t right_rpm_x10;       /* right wheel RPM / 10 */
+    int16_t left_current_x10;    /* left  phase current × 10 (A) */
+    int16_t right_current_x10;   /* right phase current × 10 (A) */
+} orin_can_fc_vesc_t;  /* 8 bytes */
+
+/* FC_IMU (0x402) — full IMU angles at 50 Hz (Issue #680) */
+typedef struct __attribute__((packed)) {
+    int16_t pitch_x10;      /* pitch degrees × 10 (mount-offset corrected) */
+    int16_t roll_x10;       /* roll  degrees × 10 (mount-offset corrected) */
+    int16_t yaw_x10;        /* yaw   degrees × 10 (gyro-integrated, drifts) */
+    uint8_t cal_status;     /* 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */
+    uint8_t balance_state;  /* BalanceState: 0=DISARMED, 1=ARMED, 2=TILT_FAULT */
+} orin_can_fc_imu_t;  /* 8 bytes */
+
+/* FC_BARO (0x403) — barometer at 1 Hz (Issue #672) */
+typedef struct __attribute__((packed)) {
+    int32_t pressure_pa;    /* barometric pressure in Pa */
+    int16_t temp_x10;       /* temperature × 10 (°C) */
+    int16_t alt_cm;         /* altitude in cm (reference = pressure at boot) */
+} orin_can_fc_baro_t;  /* 8 bytes */
+
+/* FC_BTN (0x404) — button event, sent on demand (Issue #682)
+ * event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED (pitch too large) */
+typedef struct __attribute__((packed)) {
+    uint8_t event_id;       /* 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED */
+    uint8_t balance_state;  /* balance_state_t value after the event */
+} orin_can_fc_btn_t;  /* 2 bytes */
+
+/* LED_CMD (0x304) — Orin → FC LED pattern override (Issue #685)
+ * duration_ms = 0: hold until next state change; >0: revert after duration */
+typedef struct __attribute__((packed)) {
+    uint8_t  pattern;       /* 0=state_auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse */
+    uint8_t  brightness;    /* 0-255 (0 = both LEDs off) */
+    uint16_t duration_ms;   /* override duration; 0 = permanent until state change */
+} orin_can_led_cmd_t;  /* 4 bytes */
+
+/* LED override state (updated by orin_can_on_frame, read by main loop) */
+extern volatile orin_can_led_cmd_t orin_can_led_override;
+extern volatile uint8_t            orin_can_led_updated;
+
+/* ---- API ---- */
+
+/*
+ * orin_can_init() — zero state, register orin_can_on_frame as std_cb with
+ * can_driver.  Call after can_driver_init().
+ */
+void orin_can_init(void);
+
+/*
+ * orin_can_on_frame(std_id, data, len) — dispatched by can_driver for each
+ * standard-ID frame in FIFO0.  Updates orin_can_state.
+ */
+void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len);
+
+/*
+ * orin_can_is_alive(now_ms) — true if a frame from Orin arrived within
+ * ORIN_HB_TIMEOUT_MS of now_ms.
+ */
+bool orin_can_is_alive(uint32_t now_ms);
+
+/*
+ * orin_can_broadcast(now_ms, status, vesc) — rate-limited broadcast of
+ * FC_STATUS (0x400) and FC_VESC (0x401) at ORIN_TLM_HZ (10 Hz).
+ * Safe to call every ms; internally rate-limited.
+ */
+void orin_can_broadcast(uint32_t now_ms,
+                        const orin_can_fc_status_t *status,
+                        const orin_can_fc_vesc_t   *vesc);
+
+/*
+ * orin_can_broadcast_imu(now_ms, imu_tlm) — rate-limited broadcast of
+ * FC_IMU (0x402) at ORIN_IMU_TLM_HZ (50 Hz).
+ * Safe to call every ms; internally rate-limited.  Issue #680.
+ */
+void orin_can_broadcast_imu(uint32_t now_ms,
+                            const orin_can_fc_imu_t *imu_tlm);
+
+/*
+ * orin_can_broadcast_baro(now_ms, baro_tlm) — rate-limited broadcast of
+ * FC_BARO (0x403) at ORIN_BARO_TLM_HZ (1 Hz).
+ * Pass NULL to skip transmission.  Issue #672.
+ */
+void orin_can_broadcast_baro(uint32_t now_ms,
+                             const orin_can_fc_baro_t *baro_tlm);
+
+/*
+ * orin_can_send_btn_event(event_id, balance_state) — send FC_BTN (0x404)
+ * immediately.  Call on button park/unpark events.  Issue #682.
+ * event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED.
+ */
+void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state);
+
+#endif /* ORIN_CAN_H */

include/vesc_can.h

@@ -0,0 +1,117 @@
+#ifndef VESC_CAN_H
+#define VESC_CAN_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * vesc_can — VESC CAN protocol driver for FSESC 6.7 Pro Mini Dual (Issue #674).
+ *
+ * VESC uses 29-bit extended CAN IDs:
+ *   arbitration_id = (packet_type << 8) | vesc_node_id
+ *
+ * Wire format is big-endian throughout (matches VESC FW 6.x).
+ *
+ * Physical layer: CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps.
+ *
+ * NOTE ON PA11/PA12 vs PB12/PB13:
+ *   PA11/PA12 carry CAN1_RX/TX (AF9) BUT are also USB_OTG_FS DM/DP (AF10).
+ *   USB CDC is active on this board, so PA11/PA12 are occupied.
+ *   PB8/PB9 (CAN1 alternate) are occupied by I2C1 (barometer).
+ *   CAN2 on PB12/PB13 is the only conflict-free choice.
+ *   If the SN65HVD230 is wired to the pads labelled RX6/TX6 on the Mamba
+ *   silkscreen, those pads connect to PB12/PB13 (SPI2/OSD, repurposed).
+ *
+ * VESC frames arrive in FIFO1 (extended-ID filter, bank 15).
+ * Orin standard frames arrive in FIFO0 (standard-ID filter, bank 14).
+ */
+
+/* ---- VESC packet type IDs (upper byte of 29-bit arb ID) ---- */
+#define VESC_PKT_SET_DUTY           0u   /* int32 duty × 100000           */
+#define VESC_PKT_SET_CURRENT        1u   /* int32 current (mA)            */
+#define VESC_PKT_SET_CURRENT_BRAKE  2u   /* int32 brake current (mA)      */
+#define VESC_PKT_SET_RPM            3u   /* int32 target RPM              */
+#define VESC_PKT_STATUS             9u   /* int32 RPM, int16 I×10, int16 duty×1000 */
+#define VESC_PKT_STATUS_4           16u  /* int16 T_fet×10, T_mot×10, I_in×10 */
+#define VESC_PKT_STATUS_5           27u  /* int32 tacho, int16 V_in×10   */
+
+/* ---- Default VESC node IDs (configurable via vesc_can_init) ---- */
+#define VESC_CAN_ID_LEFT            56u
+#define VESC_CAN_ID_RIGHT           68u
+
+/* ---- Alive timeout ---- */
+#define VESC_ALIVE_TIMEOUT_MS       1000u  /* node offline if no STATUS for 1 s */
+
+/* ---- JLink telemetry rate ---- */
+#define VESC_TLM_HZ                 1u
+
+/* ---- Fault codes (VESC FW 6.6) ---- */
+#define VESC_FAULT_NONE                      0u
+#define VESC_FAULT_OVER_VOLTAGE              1u
+#define VESC_FAULT_UNDER_VOLTAGE             2u
+#define VESC_FAULT_DRV                       3u
+#define VESC_FAULT_ABS_OVER_CURRENT          4u
+#define VESC_FAULT_OVER_TEMP_FET             5u
+#define VESC_FAULT_OVER_TEMP_MOTOR           6u
+#define VESC_FAULT_GATE_DRIVER_OVER_VOLTAGE  7u
+#define VESC_FAULT_GATE_DRIVER_UNDER_VOLTAGE 8u
+#define VESC_FAULT_MCU_UNDER_VOLTAGE         9u
+#define VESC_FAULT_WATCHDOG_RESET            10u
+
+/* ---- Telemetry state per VESC node ---- */
+typedef struct {
+    int32_t  rpm;             /* actual RPM (STATUS pkt, int32 BE)           */
+    int16_t  current_x10;    /* phase current (A × 10; STATUS pkt)          */
+    int16_t  duty_x1000;     /* duty cycle (× 1000; –1000..+1000)           */
+    int16_t  temp_fet_x10;   /* FET temperature (°C × 10; STATUS_4)         */
+    int16_t  temp_motor_x10; /* motor temperature (°C × 10; STATUS_4)       */
+    int16_t  current_in_x10; /* input (battery) current (A × 10; STATUS_4)  */
+    int16_t  voltage_x10;    /* input voltage (V × 10; STATUS_5)            */
+    uint8_t  fault_code;     /* VESC fault code (0 = none)                  */
+    uint8_t  _pad;
+    uint32_t last_rx_ms;     /* HAL_GetTick() of last received STATUS frame  */
+} vesc_state_t;
+
+/* ---- API ---- */
+
+/*
+ * vesc_can_init(id_left, id_right) — store VESC node IDs and register the
+ * extended-frame callback with can_driver.
+ * Call after can_driver_init().
+ */
+void vesc_can_init(uint8_t id_left, uint8_t id_right);
+
+/*
+ * vesc_can_send_rpm(vesc_id, rpm) — transmit VESC_PKT_SET_RPM (3) to the
+ * target VESC.  arb_id = (3 << 8) | vesc_id.  Payload: int32 big-endian.
+ */
+void vesc_can_send_rpm(uint8_t vesc_id, int32_t rpm);
+
+/*
+ * vesc_can_on_frame(ext_id, data, len) — called by can_driver when an
+ * extended-ID frame arrives (registered via can_driver_set_ext_cb).
+ * Parses STATUS / STATUS_4 / STATUS_5 into the matching vesc_state_t.
+ */
+void vesc_can_on_frame(uint32_t ext_id, const uint8_t *data, uint8_t len);
+
+/*
+ * vesc_can_get_state(vesc_id, out) — copy latest telemetry snapshot.
+ * vesc_id must match id_left or id_right passed to vesc_can_init.
+ * Returns false if vesc_id unknown or no frame has arrived yet.
+ */
+bool vesc_can_get_state(uint8_t vesc_id, vesc_state_t *out);
+
+/*
+ * vesc_can_is_alive(vesc_id, now_ms) — true if a STATUS frame arrived
+ * within VESC_ALIVE_TIMEOUT_MS of now_ms.
+ */
+bool vesc_can_is_alive(uint8_t vesc_id, uint32_t now_ms);
+
+/*
+ * vesc_can_send_tlm(now_ms) — rate-limited JLINK_TLM_VESC_STATE (0x8E)
+ * telemetry to Orin over JLink.  Safe to call every ms; internally
+ * rate-limited to VESC_TLM_HZ (1 Hz).
+ */
+void vesc_can_send_tlm(uint32_t now_ms);
+
+#endif /* VESC_CAN_H */

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_can_node.py

@@ -0,0 +1,402 @@
+"""
+saltybot_can_node — production FC↔Orin bridge over CAN bus (Issues #680, #672, #685)
+
+In production the FC has NO USB connection to Orin — CAN bus only.
+Reads IMU, balance state, barometer, and VESC telemetry from FC over CAN
+and publishes them as ROS2 topics.  Sends drive/heartbeat/estop commands
+back to FC over CAN.
+
+CAN interface: SocketCAN (CANable USB adapter on vcan0 / can0)
+
+FC → Orin (telemetry):
+  0x400  FC_STATUS   int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv,
+                     uint8 balance_state, uint8 flags          (10 Hz)
+  0x401  FC_VESC     int16 left_rpm_x10, int16 right_rpm_x10,
+                     int16 left_cur_x10, int16 right_cur_x10   (10 Hz)
+  0x402  FC_IMU      int16 pitch_x10, int16 roll_x10, int16 yaw_x10,
+                     uint8 cal_status, uint8 balance_state      (50 Hz)
+  0x403  FC_BARO     int32 pressure_pa, int16 temp_x10, int16 alt_cm (1 Hz)
+
+Orin → FC (commands):
+  0x300  HEARTBEAT   uint32 sequence counter                    (5 Hz)
+  0x301  DRIVE       int16 speed BE, int16 steer BE             (on /cmd_vel)
+  0x303  ESTOP       uint8 1=estop, 0=clear
+  0x304  LED_CMD     uint8 pattern, uint8 brightness, uint16 duration_ms LE
+
+Published topics:
+  /saltybot/imu            (sensor_msgs/Imu)       — from 0x402
+  /saltybot/balance_state  (std_msgs/String)        — from 0x402 + 0x400
+  /saltybot/barometer      (sensor_msgs/FluidPressure) — from 0x403
+
+Subscribed topics:
+  /cmd_vel                 (geometry_msgs/Twist)   — sent as DRIVE
+  /saltybot/leds           (std_msgs/String JSON)  — sent as LED_CMD
+
+Parameters:
+  can_interface   CAN socket interface name     default: can0
+  speed_scale     m/s to ESC units multiplier   default: 1000.0
+  steer_scale     rad/s to ESC units multiplier default: -500.0
+  heartbeat_hz    HEARTBEAT TX rate (Hz)         default: 5.0
+"""
+
+import json
+import math
+import socket
+import struct
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Imu, FluidPressure
+from std_msgs.msg import String
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+
+# ---- CAN frame IDs ------------------------------------------------
+
+CAN_FC_STATUS   = 0x400
+CAN_FC_VESC     = 0x401
+CAN_FC_IMU      = 0x402
+CAN_FC_BARO     = 0x403
+
+CAN_HEARTBEAT   = 0x300
+CAN_DRIVE       = 0x301
+CAN_ESTOP       = 0x303
+CAN_LED_CMD     = 0x304
+
+# ---- Constants ----------------------------------------------------
+
+IMU_FRAME_ID    = "imu_link"
+_STATE_LABEL    = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
+_CAL_LABEL      = {0: "UNCAL", 1: "GYRO_CAL", 2: "MOUNT_CAL"}
+
+# ---- Helpers ------------------------------------------------------
+
+def _clamp(v: float, lo: float, hi: float) -> float:
+    return max(lo, min(hi, v))
+
+
+def _can_socket(iface: str) -> socket.socket:
+    """Open a raw SocketCAN socket on *iface*."""
+    s = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW)
+    s.bind((iface,))
+    s.settimeout(0.1)
+    return s
+
+
+def _pack_frame(can_id: int, data: bytes) -> bytes:
+    """Pack a standard CAN frame for SocketCAN (16-byte struct)."""
+    dlc = len(data)
+    return struct.pack("=IB3x8s", can_id & 0x1FFFFFFF, dlc,
+                       data.ljust(8, b"\x00"))
+
+
+def _unpack_frame(raw: bytes):
+    """Unpack a raw SocketCAN frame; returns (can_id, data_bytes)."""
+    can_id, dlc = struct.unpack_from("=IB", raw, 0)
+    data = raw[8: 8 + (dlc & 0x0F)]
+    return can_id & 0x1FFFFFFF, data
+
+
+# ---- Node ---------------------------------------------------------
+
+class SaltybotCanNode(Node):
+    def __init__(self):
+        super().__init__("saltybot_can")
+
+        # ── Parameters ───────────────────────────────────────────────
+        self.declare_parameter("can_interface",  "can0")
+        self.declare_parameter("speed_scale",    1000.0)
+        self.declare_parameter("steer_scale",    -500.0)
+        self.declare_parameter("heartbeat_hz",   5.0)
+
+        iface          = self.get_parameter("can_interface").value
+        self._speed_sc = self.get_parameter("speed_scale").value
+        self._steer_sc = self.get_parameter("steer_scale").value
+        hb_hz          = self.get_parameter("heartbeat_hz").value
+
+        # ── QoS ──────────────────────────────────────────────────────
+        sensor_qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST, depth=10)
+        reliable_qos = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST, depth=10)
+
+        # ── Publishers ───────────────────────────────────────────────
+        self._imu_pub  = self.create_publisher(Imu,          "/saltybot/imu",           sensor_qos)
+        self._bal_pub  = self.create_publisher(String,       "/saltybot/balance_state",  reliable_qos)
+        self._baro_pub = self.create_publisher(FluidPressure,"/saltybot/barometer",      reliable_qos)
+        self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics",           reliable_qos)
+
+        # ── Subscribers ──────────────────────────────────────────────
+        self._cmd_sub  = self.create_subscription(
+            Twist, "/cmd_vel", self._cmd_vel_cb, reliable_qos)
+        self._led_sub  = self.create_subscription(
+            String, "/saltybot/leds", self._led_cb, reliable_qos)
+
+        # ── State ────────────────────────────────────────────────────
+        self._iface       = iface
+        self._sock        = None
+        self._sock_lock   = threading.Lock()
+        self._hb_seq      = 0
+        self._last_speed  = 0
+        self._last_steer  = 0
+        self._last_state  = -1
+        self._last_cal    = -1
+        self._last_pitch  = 0.0
+        self._last_roll   = 0.0
+        self._last_yaw    = 0.0
+        self._last_motor  = 0
+        self._last_vbat   = 0
+        self._last_flags  = 0
+
+        # ── Open CAN ─────────────────────────────────────────────────
+        self._open_can()
+
+        # ── Timers ───────────────────────────────────────────────────
+        self._rx_timer = self.create_timer(0.01, self._rx_cb)         # 100 Hz poll
+        self._hb_timer = self.create_timer(1.0 / hb_hz, self._hb_cb)
+
+        self.get_logger().info(
+            f"saltybot_can started — interface={iface} "
+            f"(speed_scale={self._speed_sc}, steer_scale={self._steer_sc})"
+        )
+
+    # ── CAN socket management ────────────────────────────────────────
+
+    def _open_can(self) -> bool:
+        with self._sock_lock:
+            try:
+                self._sock = _can_socket(self._iface)
+                self.get_logger().info(f"CAN socket open: {self._iface}")
+                return True
+            except OSError as exc:
+                self.get_logger().error(f"Cannot open CAN {self._iface}: {exc}")
+                self._sock = None
+                return False
+
+    def _send(self, can_id: int, data: bytes) -> bool:
+        with self._sock_lock:
+            if self._sock is None:
+                return False
+            try:
+                self._sock.send(_pack_frame(can_id, data))
+                return True
+            except OSError as exc:
+                self.get_logger().error(
+                    f"CAN TX error: {exc}", throttle_duration_sec=2.0)
+                return False
+
+    # ── RX poll ──────────────────────────────────────────────────────
+
+    def _rx_cb(self):
+        with self._sock_lock:
+            if self._sock is None:
+                return
+            try:
+                while True:
+                    raw = self._sock.recv(16)
+                    can_id, data = _unpack_frame(raw)
+                    self._dispatch(can_id, data)
+            except socket.timeout:
+                pass
+            except BlockingIOError:
+                pass
+            except OSError as exc:
+                self.get_logger().error(
+                    f"CAN RX error: {exc}", throttle_duration_sec=5.0)
+                self._sock = None
+
+        if self._sock is None:
+            self._open_can()
+
+    def _dispatch(self, can_id: int, data: bytes):
+        now = self.get_clock().now().to_msg()
+        if can_id == CAN_FC_IMU and len(data) >= 8:
+            self._handle_fc_imu(data, now)
+        elif can_id == CAN_FC_STATUS and len(data) >= 8:
+            self._handle_fc_status(data)
+        elif can_id == CAN_FC_BARO and len(data) >= 8:
+            self._handle_fc_baro(data, now)
+
+    # ── Frame handlers ───────────────────────────────────────────────
+
+    def _handle_fc_imu(self, data: bytes, stamp):
+        pitch_x10, roll_x10, yaw_x10, cal, state = struct.unpack_from("<hhhBB", data, 0)
+        pitch = pitch_x10 / 10.0
+        roll  = roll_x10  / 10.0
+        yaw   = yaw_x10   / 10.0
+
+        self._last_pitch = pitch
+        self._last_roll  = roll
+        self._last_yaw   = yaw
+        self._last_cal   = cal
+
+        # Publish IMU
+        imu = Imu()
+        imu.header.stamp    = stamp
+        imu.header.frame_id = IMU_FRAME_ID
+        imu.orientation_covariance[0] = -1.0   # orientation not provided
+
+        # Publish Euler angles via angular_velocity fields (convention matches
+        # saltybot_cmd_node — downstream nodes expect this mapping)
+        imu.angular_velocity.x = math.radians(pitch)
+        imu.angular_velocity.y = math.radians(roll)
+        imu.angular_velocity.z = math.radians(yaw)
+        cov = math.radians(0.5) ** 2
+        imu.angular_velocity_covariance[0] = cov
+        imu.angular_velocity_covariance[4] = cov
+        imu.angular_velocity_covariance[8] = cov
+        imu.linear_acceleration_covariance[0] = -1.0
+        self._imu_pub.publish(imu)
+
+        # Publish balance_state JSON
+        self._publish_balance_state(pitch, roll, yaw, state, cal, stamp)
+
+        # Log state/cal transitions
+        if state != self._last_state:
+            self.get_logger().info(
+                f"Balance state → {_STATE_LABEL.get(state, f'UNKNOWN({state})')}"
+            )
+            self._last_state = state
+        if cal != self._last_cal:
+            self.get_logger().info(
+                f"IMU cal status → {_CAL_LABEL.get(cal, f'CAL({cal})')}"
+            )
+
+    def _handle_fc_status(self, data: bytes):
+        pitch_x10, motor_cmd, vbat_mv, balance_state, flags = \
+            struct.unpack_from("<hhhBB", data, 0)
+        self._last_motor = motor_cmd
+        self._last_vbat  = vbat_mv
+        self._last_flags = flags
+
+    def _handle_fc_baro(self, data: bytes, stamp):
+        pressure_pa, temp_x10, alt_cm = struct.unpack_from("<ihh", data, 0)
+        msg = FluidPressure()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = "barometer_link"
+        msg.fluid_pressure  = float(pressure_pa)      # Pa
+        msg.variance        = 0.0
+        self._baro_pub.publish(msg)
+
+        diag = DiagnosticArray()
+        diag.header.stamp = stamp
+        st = DiagnosticStatus()
+        st.level       = DiagnosticStatus.OK
+        st.name        = "saltybot/barometer"
+        st.hardware_id = "bmp280"
+        st.message     = "OK"
+        st.values = [
+            KeyValue(key="pressure_pa", value=str(pressure_pa)),
+            KeyValue(key="temp_c",      value=f"{temp_x10 / 10.0:.1f}"),
+            KeyValue(key="alt_cm",      value=str(alt_cm)),
+        ]
+        diag.status.append(st)
+        self._diag_pub.publish(diag)
+
+    def _publish_balance_state(self, pitch, roll, yaw, state, cal, stamp):
+        state_label = _STATE_LABEL.get(state, f"UNKNOWN({state})")
+        cal_label   = _CAL_LABEL.get(cal, f"CAL({cal})")
+        payload = {
+            "stamp":        f"{stamp.sec}.{stamp.nanosec:09d}",
+            "state":        state_label,
+            "cal_status":   cal_label,
+            "pitch_deg":    round(pitch, 1),
+            "roll_deg":     round(roll,  1),
+            "yaw_deg":      round(yaw,   1),
+            "motor_cmd":    self._last_motor,
+            "vbat_mv":      self._last_vbat,
+            "jetson_speed": self._last_speed,
+            "jetson_steer": self._last_steer,
+        }
+        str_msg = String()
+        str_msg.data = json.dumps(payload)
+        self._bal_pub.publish(str_msg)
+
+        diag = DiagnosticArray()
+        diag.header.stamp = stamp
+        st = DiagnosticStatus()
+        st.name        = "saltybot/balance_controller"
+        st.hardware_id = "stm32f722"
+        st.message     = state_label
+        st.level       = (DiagnosticStatus.OK    if state == 1 else
+                          DiagnosticStatus.WARN   if state == 0 else
+                          DiagnosticStatus.ERROR)
+        st.values = [
+            KeyValue(key="pitch_deg",  value=f"{pitch:.1f}"),
+            KeyValue(key="roll_deg",   value=f"{roll:.1f}"),
+            KeyValue(key="yaw_deg",    value=f"{yaw:.1f}"),
+            KeyValue(key="motor_cmd",  value=str(self._last_motor)),
+            KeyValue(key="state",      value=state_label),
+            KeyValue(key="cal_status", value=cal_label),
+        ]
+        diag.status.append(st)
+        self._diag_pub.publish(diag)
+
+    # ── TX callbacks ─────────────────────────────────────────────────
+
+    def _hb_cb(self):
+        """Send HEARTBEAT (0x300) with incrementing sequence counter."""
+        data = struct.pack(">I", self._hb_seq & 0xFFFFFFFF)
+        self._hb_seq += 1
+        self._send(CAN_HEARTBEAT, data)
+
+    def _cmd_vel_cb(self, msg: Twist):
+        """Convert Twist → DRIVE (0x301): int16 speed BE, int16 steer BE."""
+        speed = int(_clamp(msg.linear.x  * self._speed_sc, -1000.0, 1000.0))
+        steer = int(_clamp(msg.angular.z * self._steer_sc, -1000.0, 1000.0))
+        self._last_speed = speed
+        self._last_steer = steer
+        data = struct.pack(">hh", speed, steer)
+        self._send(CAN_DRIVE, data)
+
+    def _led_cb(self, msg: String):
+        """Parse /saltybot/leds JSON → LED_CMD (0x304).
+        Expected JSON: {"pattern": 1, "brightness": 200, "duration_ms": 5000}
+        pattern: 0=auto, 1=solid, 2=slow_blink, 3=fast_blink, 4=pulse
+        """
+        try:
+            d = json.loads(msg.data)
+        except (ValueError, TypeError) as exc:
+            self.get_logger().debug(f"LED JSON parse error: {exc}")
+            return
+        pattern     = int(d.get("pattern",     0)) & 0xFF
+        brightness  = int(d.get("brightness",  255)) & 0xFF
+        duration_ms = int(d.get("duration_ms", 0)) & 0xFFFF
+        data = struct.pack("<BBH", pattern, brightness, duration_ms)
+        self._send(CAN_LED_CMD, data)
+
+    # ── Lifecycle ────────────────────────────────────────────────────
+
+    def destroy_node(self):
+        # Send ESTOP on shutdown so FC doesn't keep rolling
+        self._send(CAN_ESTOP, bytes([1]))
+        with self._sock_lock:
+            if self._sock:
+                try:
+                    self._sock.close()
+                except OSError:
+                    pass
+                self._sock = None
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SaltybotCanNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/saltybot_cmd_node.py

@@ -154,12 +154,12 @@ class SaltybotCmdNode(Node):
     # ── RX — telemetry read ───────────────────────────────────────────────────
 
     def _read_cb(self):
+        lines = []
         with self._ser_lock:
             if self._ser is None or not self._ser.is_open:
                 pass
             else:
                 try:
-                    lines = []
                     while self._ser.in_waiting:
                         raw = self._ser.readline()
                         if raw:

jetson/ros2_ws/src/saltybot_bridge/setup.py

@@ -15,6 +15,7 @@ setup(
             "launch/remote_estop.launch.py",
             "launch/stm32_cmd.launch.py",
             "launch/battery.launch.py",
+            "launch/uart_bridge.launch.py",
         ]),
         (f"share/{package_name}/config", [
             "config/bridge_params.yaml",
@@ -44,6 +45,8 @@ setup(
             "stm32_cmd_node    = saltybot_bridge.stm32_cmd_node:main",
             # Battery management node (Issue #125)
             "battery_node      = saltybot_bridge.battery_node:main",
+            # Production CAN bridge: FC telemetry RX + /cmd_vel TX over CAN (Issues #680, #672, #685)
+            "saltybot_can_node = saltybot_bridge.saltybot_can_node:main",
         ],
     },
 )

jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py

@@ -1,76 +1,139 @@
 """
 nav2.launch.py — Nav2 autonomous navigation stack for SaltyBot
 
-Starts the full Nav2 navigation stack (controllers, planners, behavior server,
-BT navigator, velocity smoother, lifecycle managers).
+Supports two localization modes via the 'nav_mode' argument:
 
-Localization is provided by RTAB-Map (slam_rtabmap.launch.py must be running):
-  /map             — OccupancyGrid (static global costmap layer)
-  /rtabmap/odom    — Odometry (velocity smoother + controller feedback)
-  TF map→odom      — published by RTAB-Map
+  nav_mode:=slam  (default)
+    Uses RTAB-Map for odometry and live map building.
+    Requires slam_rtabmap.launch.py to be running.
+    Sources: /rtabmap/odom (pose), /map (from RTAB-Map)
 
-Output:
-  /cmd_vel         — consumed by saltybot_bridge → STM32 over UART
+  nav_mode:=amcl
+    Uses VESC CAN odometry (/odom) + AMCL on a pre-built static map.
+    Launches: sensors, VESC odometry bridge, map_server, AMCL, Nav2 stack.
+    Sources: /odom (VESC CAN IDs 61/79), /scan (RPLIDAR)
+    Map:     saltybot_nav2_slam maps/saltybot_map.yaml (override with map:=...)
 
-Profile Support (Issue #506)
-────────────────────────────
-  Supports profile-based parameter overrides via 'profile' launch argument:
-    profile:=indoor  — conservative (0.2 m/s, tight geofence, aggressive inflation)
-    profile:=outdoor — moderate (0.5 m/s, wide geofence, standard inflation)
-    profile:=demo    — agile (0.3 m/s, tricks enabled, enhanced obstacle avoidance)
+  nav_mode:=slam  profile support (Issue #506)
+    profile:=indoor  — conservative (0.2 m/s, tight geofence)
+    profile:=outdoor — moderate  (0.5 m/s)
+    profile:=demo    — agile     (0.3 m/s, tricks enabled)
 
 Run sequence on Orin:
-  1. docker compose up saltybot-ros2   # RTAB-Map + sensors
-  2. docker compose up saltybot-nav2   # this launch file
+  # SLAM mode (existing behaviour):
+  ros2 launch saltybot_bringup nav2.launch.py
+
+  # AMCL mode with saved map:
+  ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl \
+      map:=/mnt/nvme/saltybot/maps/my_map.yaml
 """
 
 import os
+
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo
+from launch.actions import (
+    DeclareLaunchArgument,
+    GroupAction,
+    IncludeLaunchDescription,
+    LogInfo,
+)
+from launch.conditions import LaunchConfigurationEquals
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 
 
-def generate_launch_description():
-    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
-    bringup_dir = get_package_share_directory('saltybot_bringup')
+def generate_launch_description() -> LaunchDescription:
+    nav2_bringup_dir   = get_package_share_directory("nav2_bringup")
+    bringup_dir        = get_package_share_directory("saltybot_bringup")
+    nav2_slam_dir      = get_package_share_directory("saltybot_nav2_slam")
+
+    # ── Arguments ─────────────────────────────────────────────────────────────
+
+    nav_mode_arg = DeclareLaunchArgument(
+        "nav_mode",
+        default_value="slam",
+        choices=["slam", "amcl"],
+        description=(
+            "Localization mode. "
+            "slam: RTAB-Map live map (requires slam_rtabmap.launch.py running); "
+            "amcl: static map + AMCL particle filter + VESC odometry (Issue #655)."
+        ),
+    )
 
-    # Profile argument (Issue #506)
     profile_arg = DeclareLaunchArgument(
         "profile",
         default_value="indoor",
         choices=["indoor", "outdoor", "demo"],
-        description="Launch profile for parameter overrides (Issue #506)"
+        description="Nav2 parameter profile for slam mode (Issue #506)",
     )
 
-    profile = LaunchConfiguration('profile')
-
-    nav2_params_file = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
-    bt_xml_file = os.path.join(
-        bringup_dir, 'behavior_trees', 'navigate_to_pose_with_recovery.xml'
+    map_arg = DeclareLaunchArgument(
+        "map",
+        default_value=os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml"),
+        description="Map YAML path for AMCL mode",
     )
 
-    nav2_launch = IncludeLaunchDescription(
+    use_sim_time_arg = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="false",
+    )
+
+    # ── Mode: SLAM (original behaviour — RTAB-Map + navigation_launch.py) ────
+
+    slam_nav2_params = os.path.join(bringup_dir, "config", "nav2_params.yaml")
+    slam_bt_xml      = os.path.join(
+        bringup_dir, "behavior_trees", "navigate_to_pose_with_recovery.xml"
+    )
+
+    slam_mode = GroupAction(
+        condition=LaunchConfigurationEquals("nav_mode", "slam"),
+        actions=[
+            LogInfo(msg="[nav2] Mode: SLAM (RTAB-Map odometry + live map)"),
+            IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
-            os.path.join(nav2_bringup_dir, 'launch', 'navigation_launch.py')
+                    os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py")
                 ),
                 launch_arguments={
-            'use_sim_time': 'false',
-            'autostart': 'true',
-            'params_file': nav2_params_file,
-            'default_bt_xml_filename': bt_xml_file,
-            # RTAB-Map publishes /map with transient_local QoS — must match
-            'map_subscribe_transient_local': 'true',
+                    "use_sim_time":                "false",
+                    "autostart":                   "true",
+                    "params_file":                 slam_nav2_params,
+                    "default_bt_xml_filename":     slam_bt_xml,
+                    "map_subscribe_transient_local": "true",
                 }.items(),
+            ),
+        ],
     )
 
-    profile_log = LogInfo(
-        msg=['[nav2] Loaded profile: ', profile]
+    # ── Mode: AMCL (static map + AMCL + VESC odometry, Issue #655) ───────────
+
+    amcl_mode = GroupAction(
+        condition=LaunchConfigurationEquals("nav_mode", "amcl"),
+        actions=[
+            LogInfo(msg="[nav2] Mode: AMCL (static map + VESC odometry, Issue #655)"),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(
+                        nav2_slam_dir, "launch", "nav2_amcl_bringup.launch.py"
+                    )
+                ),
+                launch_arguments={
+                    "map":            LaunchConfiguration("map"),
+                    "use_sim_time":   LaunchConfiguration("use_sim_time"),
+                    # Sensors are already started by saltybot_bringup GROUP A
+                    "include_sensors": "false",
+                }.items(),
+            ),
+        ],
     )
 
     return LaunchDescription([
+        nav_mode_arg,
         profile_arg,
-        profile_log,
-        nav2_launch,
+        map_arg,
+        use_sim_time_arg,
+        LogInfo(msg=["[nav2] Loaded nav_mode=", LaunchConfiguration("nav_mode"),
+                     "  profile=", LaunchConfiguration("profile")]),
+        slam_mode,
+        amcl_mode,
     ])

jetson/ros2_ws/src/saltybot_can_bridge/config/can_bridge_params.yaml

@@ -0,0 +1,7 @@
+can_bridge_node:
+  ros__parameters:
+    can_interface: slcan0
+    left_vesc_can_id: 56
+    right_vesc_can_id: 68
+    mamba_can_id: 1
+    command_timeout_s: 0.5

jetson/ros2_ws/src/saltybot_can_bridge/package.xml

@@ -0,0 +1,35 @@
+<?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_can_bridge</name>
+  <version>0.1.0</version>
+  <description>
+    CAN bus bridge for SaltyBot Orin — interfaces with the Mamba motor
+    controller and VESC motor controllers via CANable 2.0 (slcan interface).
+    Publishes IMU, battery, and VESC telemetry to ROS2 topics and forwards
+    cmd_vel / estop commands to the CAN bus.
+
+    System dependency: python3-can (apt: python3-can  or  pip: python-can)
+  </description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+  <!-- python-can: install via  apt install python3-can  or  pip install python-can -->
+  <exec_depend>python3-can</exec_depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/__init__.py

@@ -0,0 +1 @@
+"""SaltyBot CAN bridge package — Mamba controller and VESC telemetry via python-can."""

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py

@@ -0,0 +1,383 @@
+#!/usr/bin/env python3
+"""
+can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the Mamba motor
+controller and VESC motor controllers over CAN bus.
+
+The node opens the SocketCAN interface (slcan0 by default), spawns a background
+reader thread to process incoming telemetry, and exposes the following interface:
+
+Subscriptions
+-------------
+  /cmd_vel          geometry_msgs/Twist          → VESC speed commands (CAN)
+  /estop            std_msgs/Bool                → Mamba e-stop (CAN)
+
+Publications
+------------
+  /can/imu                sensor_msgs/Imu              Mamba IMU telemetry
+  /can/battery            sensor_msgs/BatteryState     Mamba battery telemetry
+  /can/vesc/left/state    std_msgs/Float32MultiArray   Left VESC state
+  /can/vesc/right/state   std_msgs/Float32MultiArray   Right VESC state
+  /can/connection_status  std_msgs/String              "connected" | "disconnected"
+
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
+"""
+
+import threading
+import time
+from typing import Optional
+
+import can
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.node import Node
+from sensor_msgs.msg import BatteryState, Imu
+from std_msgs.msg import Bool, Float32MultiArray, String
+
+from saltybot_can_bridge.mamba_protocol import (
+    MAMBA_CMD_ESTOP,
+    MAMBA_CMD_MODE,
+    MAMBA_CMD_VELOCITY,
+    MAMBA_TELEM_BATTERY,
+    MAMBA_TELEM_IMU,
+    VESC_TELEM_STATE,
+    MODE_DRIVE,
+    MODE_ESTOP,
+    MODE_IDLE,
+    encode_estop_cmd,
+    encode_mode_cmd,
+    encode_velocity_cmd,
+    decode_battery_telem,
+    decode_imu_telem,
+    decode_vesc_state,
+)
+
+# Reconnect attempt interval when CAN bus is lost
+_RECONNECT_INTERVAL_S: float = 5.0
+
+# Watchdog timer tick rate (Hz)
+_WATCHDOG_HZ: float = 10.0
+
+
+class CanBridgeNode(Node):
+    """CAN bus bridge between Orin ROS2 and Mamba / VESC controllers."""
+
+    def __init__(self) -> None:
+        super().__init__("can_bridge_node")
+
+        # ── Parameters ────────────────────────────────────────────────────
+        self.declare_parameter("can_interface", "slcan0")
+        self.declare_parameter("left_vesc_can_id", 56)
+        self.declare_parameter("right_vesc_can_id", 68)
+        self.declare_parameter("mamba_can_id", 1)
+        self.declare_parameter("command_timeout_s", 0.5)
+
+        self._iface: str = self.get_parameter("can_interface").value
+        self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
+        self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
+        self._mamba_id: int = self.get_parameter("mamba_can_id").value
+        self._cmd_timeout: float = self.get_parameter("command_timeout_s").value
+
+        # ── State ─────────────────────────────────────────────────────────
+        self._bus: Optional[can.BusABC] = None
+        self._connected: bool = False
+        self._last_cmd_time: float = time.monotonic()
+        self._lock = threading.Lock()  # protects _bus / _connected
+
+        # ── Publishers ────────────────────────────────────────────────────
+        self._pub_imu = self.create_publisher(Imu, "/can/imu", 10)
+        self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10)
+        self._pub_vesc_left = self.create_publisher(
+            Float32MultiArray, "/can/vesc/left/state", 10
+        )
+        self._pub_vesc_right = self.create_publisher(
+            Float32MultiArray, "/can/vesc/right/state", 10
+        )
+        self._pub_status = self.create_publisher(
+            String, "/can/connection_status", 10
+        )
+
+        # ── Subscriptions ─────────────────────────────────────────────────
+        self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
+        self.create_subscription(Bool, "/estop", self._estop_cb, 10)
+
+        # ── Timers ────────────────────────────────────────────────────────
+        self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
+        self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb)
+
+        # ── Open CAN ──────────────────────────────────────────────────────
+        self._try_connect()
+
+        # ── Background reader thread ──────────────────────────────────────
+        self._reader_thread = threading.Thread(
+            target=self._reader_loop, daemon=True, name="can_reader"
+        )
+        self._reader_thread.start()
+
+        self.get_logger().info(
+            f"can_bridge_node ready — iface={self._iface} "
+            f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} "
+            f"mamba={self._mamba_id}"
+        )
+
+    # ── Connection management ──────────────────────────────────────────────
+
+    def _try_connect(self) -> None:
+        """Attempt to open the CAN interface; silently skip if already connected."""
+        with self._lock:
+            if self._connected:
+                return
+            try:
+                bus = can.interface.Bus(
+                    channel=self._iface,
+                    bustype="socketcan",
+                )
+                self._bus = bus
+                self._connected = True
+                self.get_logger().info(f"CAN bus connected: {self._iface}")
+                self._publish_status("connected")
+            except Exception as exc:
+                self.get_logger().warning(
+                    f"CAN bus not available ({self._iface}): {exc} "
+                    f"— will retry every {_RECONNECT_INTERVAL_S:.0f} s"
+                )
+                self._connected = False
+                self._publish_status("disconnected")
+
+    def _reconnect_cb(self) -> None:
+        """Periodic timer: try to reconnect when disconnected."""
+        if not self._connected:
+            self._try_connect()
+
+    def _handle_can_error(self, exc: Exception, context: str) -> None:
+        """Mark bus as disconnected on any CAN error."""
+        self.get_logger().warning(f"CAN error in {context}: {exc}")
+        with self._lock:
+            if self._bus is not None:
+                try:
+                    self._bus.shutdown()
+                except Exception:
+                    pass
+            self._bus = None
+            self._connected = False
+        self._publish_status("disconnected")
+
+    # ── ROS callbacks ─────────────────────────────────────────────────────
+
+    def _cmd_vel_cb(self, msg: Twist) -> None:
+        """Convert /cmd_vel Twist to VESC speed commands over CAN."""
+        self._last_cmd_time = time.monotonic()
+
+        if not self._connected:
+            return
+
+        # Differential drive decomposition — individual wheel speeds in m/s.
+        # The VESC nodes interpret linear velocity directly; angular is handled
+        # by the sign difference between left and right.
+        linear = msg.linear.x
+        angular = msg.angular.z
+
+        # Forward left = forward right for pure translation; for rotation
+        # left slows and right speeds up (positive angular = CCW = left turn).
+        # The Mamba velocity command carries both wheels independently.
+        left_mps = linear - angular
+        right_mps = linear + angular
+
+        payload = encode_velocity_cmd(left_mps, right_mps)
+        self._send_can(MAMBA_CMD_VELOCITY, payload, "cmd_vel")
+
+        # Keep Mamba in DRIVE mode while receiving commands
+        self._send_can(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode")
+
+    def _estop_cb(self, msg: Bool) -> None:
+        """Forward /estop to Mamba over CAN."""
+        if not self._connected:
+            return
+        payload = encode_estop_cmd(msg.data)
+        self._send_can(MAMBA_CMD_ESTOP, payload, "estop")
+        if msg.data:
+            self._send_can(
+                MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
+            )
+            self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba")
+
+    # ── Watchdog ──────────────────────────────────────────────────────────
+
+    def _watchdog_cb(self) -> None:
+        """If no /cmd_vel arrives within the timeout, send zero velocity."""
+        if not self._connected:
+            return
+        elapsed = time.monotonic() - self._last_cmd_time
+        if elapsed > self._cmd_timeout:
+            self._send_can(
+                MAMBA_CMD_VELOCITY,
+                encode_velocity_cmd(0.0, 0.0),
+                "watchdog zero-vel",
+            )
+            self._send_can(
+                MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle"
+            )
+
+    # ── CAN send helper ───────────────────────────────────────────────────
+
+    def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
+        """Send a standard CAN frame; handle errors gracefully."""
+        with self._lock:
+            if not self._connected or self._bus is None:
+                return
+            bus = self._bus
+
+        msg = can.Message(
+            arbitration_id=arb_id,
+            data=data,
+            is_extended_id=False,
+        )
+        try:
+            bus.send(msg, timeout=0.05)
+        except can.CanError as exc:
+            self._handle_can_error(exc, f"send({context})")
+
+    # ── Background CAN reader ─────────────────────────────────────────────
+
+    def _reader_loop(self) -> None:
+        """
+        Blocking CAN read loop executed in a daemon thread.
+        Dispatches incoming frames to the appropriate handler.
+        """
+        while rclpy.ok():
+            with self._lock:
+                connected = self._connected
+                bus = self._bus
+
+            if not connected or bus is None:
+                time.sleep(0.1)
+                continue
+
+            try:
+                frame = bus.recv(timeout=0.5)
+            except can.CanError as exc:
+                self._handle_can_error(exc, "reader_loop recv")
+                continue
+
+            if frame is None:
+                # Timeout — no frame within 0.5 s, loop again
+                continue
+
+            self._dispatch_frame(frame)
+
+    def _dispatch_frame(self, frame: can.Message) -> None:
+        """Route an incoming CAN frame to the correct publisher."""
+        arb_id = frame.arbitration_id
+        data = bytes(frame.data)
+
+        try:
+            if arb_id == MAMBA_TELEM_IMU:
+                self._handle_imu(data, frame.timestamp)
+
+            elif arb_id == MAMBA_TELEM_BATTERY:
+                self._handle_battery(data, frame.timestamp)
+
+            elif arb_id == VESC_TELEM_STATE + self._left_vesc_id:
+                self._handle_vesc_state(data, frame.timestamp, side="left")
+
+            elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
+                self._handle_vesc_state(data, frame.timestamp, side="right")
+
+        except Exception as exc:
+            self.get_logger().warning(
+                f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
+            )
+
+    # ── Frame handlers ────────────────────────────────────────────────────
+
+    def _handle_imu(self, data: bytes, timestamp: float) -> None:
+        telem = decode_imu_telem(data)
+
+        msg = Imu()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = "imu_link"
+
+        msg.linear_acceleration.x = telem.accel_x
+        msg.linear_acceleration.y = telem.accel_y
+        msg.linear_acceleration.z = telem.accel_z
+
+        msg.angular_velocity.x = telem.gyro_x
+        msg.angular_velocity.y = telem.gyro_y
+        msg.angular_velocity.z = telem.gyro_z
+
+        # Covariance unknown; mark as -1 per REP-145
+        msg.orientation_covariance[0] = -1.0
+
+        self._pub_imu.publish(msg)
+
+    def _handle_battery(self, data: bytes, timestamp: float) -> None:
+        telem = decode_battery_telem(data)
+
+        msg = BatteryState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.voltage = telem.voltage
+        msg.current = telem.current
+        msg.present = True
+        msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+
+        self._pub_battery.publish(msg)
+
+    def _handle_vesc_state(
+        self, data: bytes, timestamp: float, side: str
+    ) -> None:
+        telem = decode_vesc_state(data)
+
+        msg = Float32MultiArray()
+        # Layout: [erpm, duty, voltage, current]
+        msg.data = [telem.erpm, telem.duty, telem.voltage, telem.current]
+
+        if side == "left":
+            self._pub_vesc_left.publish(msg)
+        else:
+            self._pub_vesc_right.publish(msg)
+
+    # ── Status helper ─────────────────────────────────────────────────────
+
+    def _publish_status(self, status: str) -> None:
+        msg = String()
+        msg.data = status
+        self._pub_status.publish(msg)
+
+    # ── Shutdown ──────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        """Send zero velocity and shut down the CAN bus cleanly."""
+        if self._connected and self._bus is not None:
+            try:
+                self._send_can(
+                    MAMBA_CMD_VELOCITY,
+                    encode_velocity_cmd(0.0, 0.0),
+                    "shutdown",
+                )
+                self._send_can(
+                    MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown"
+                )
+            except Exception:
+                pass
+            try:
+                self._bus.shutdown()
+            except Exception:
+                pass
+        super().destroy_node()
+
+
+# ---------------------------------------------------------------------------
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = CanBridgeNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py

@@ -0,0 +1,201 @@
+#!/usr/bin/env python3
+"""
+mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller
+and VESC telemetry.
+
+CAN message layout
+------------------
+Command frames  (Orin → Mamba / VESC):
+  MAMBA_CMD_VELOCITY  0x100  8 bytes  left_speed (f32, m/s) | right_speed (f32, m/s)
+  MAMBA_CMD_MODE      0x101  1 byte   mode (0=idle, 1=drive, 2=estop)
+  MAMBA_CMD_ESTOP     0x102  1 byte   0x01 = stop
+
+Telemetry frames (Mamba → Orin):
+  MAMBA_TELEM_IMU     0x200  24 bytes accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (f32 each)
+  MAMBA_TELEM_BATTERY 0x201   8 bytes voltage (f32, V) | current (f32, A)
+
+VESC telemetry frame (VESC → Orin):
+  VESC_TELEM_STATE    0x300  16 bytes erpm (f32) | duty (f32) | voltage (f32) | current (f32)
+
+All multi-byte fields are big-endian.
+
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
+"""
+
+import struct
+from dataclasses import dataclass
+from typing import Tuple
+
+# ---------------------------------------------------------------------------
+# CAN message IDs
+# ---------------------------------------------------------------------------
+
+MAMBA_CMD_VELOCITY: int = 0x100
+MAMBA_CMD_MODE: int = 0x101
+MAMBA_CMD_ESTOP: int = 0x102
+
+MAMBA_TELEM_IMU: int = 0x200
+MAMBA_TELEM_BATTERY: int = 0x201
+
+VESC_TELEM_STATE: int = 0x300
+
+# ---------------------------------------------------------------------------
+# Mode constants
+# ---------------------------------------------------------------------------
+
+MODE_IDLE: int = 0
+MODE_DRIVE: int = 1
+MODE_ESTOP: int = 2
+
+# ---------------------------------------------------------------------------
+# Data classes for decoded telemetry
+# ---------------------------------------------------------------------------
+
+
+@dataclass
+class ImuTelemetry:
+    """Decoded IMU telemetry from Mamba (MAMBA_TELEM_IMU)."""
+
+    accel_x: float = 0.0  # m/s²
+    accel_y: float = 0.0
+    accel_z: float = 0.0
+    gyro_x: float = 0.0   # rad/s
+    gyro_y: float = 0.0
+    gyro_z: float = 0.0
+
+
+@dataclass
+class BatteryTelemetry:
+    """Decoded battery telemetry from Mamba (MAMBA_TELEM_BATTERY)."""
+
+    voltage: float = 0.0   # V
+    current: float = 0.0   # A
+
+
+@dataclass
+class VescStateTelemetry:
+    """Decoded VESC state telemetry (VESC_TELEM_STATE)."""
+
+    erpm: float = 0.0      # electrical RPM
+    duty: float = 0.0      # duty cycle [-1.0, 1.0]
+    voltage: float = 0.0   # bus voltage, V
+    current: float = 0.0   # phase current, A
+
+
+# ---------------------------------------------------------------------------
+# Encode helpers
+# ---------------------------------------------------------------------------
+
+_FMT_VEL = ">ff"       # 2 × float32, big-endian
+_FMT_MODE = ">B"       # 1 × uint8
+_FMT_ESTOP = ">B"      # 1 × uint8
+_FMT_IMU = ">ffffff"   # 6 × float32
+_FMT_BAT = ">ff"       # 2 × float32
+_FMT_VESC = ">ffff"    # 4 × float32
+
+
+def encode_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
+    """
+    Encode a MAMBA_CMD_VELOCITY payload.
+
+    Parameters
+    ----------
+    left_mps:  target left wheel speed in m/s  (positive = forward)
+    right_mps: target right wheel speed in m/s (positive = forward)
+
+    Returns
+    -------
+    8-byte big-endian payload suitable for a CAN frame.
+    """
+    return struct.pack(_FMT_VEL, float(left_mps), float(right_mps))
+
+
+def encode_mode_cmd(mode: int) -> bytes:
+    """
+    Encode a MAMBA_CMD_MODE payload.
+
+    Parameters
+    ----------
+    mode: one of MODE_IDLE (0), MODE_DRIVE (1), MODE_ESTOP (2)
+
+    Returns
+    -------
+    1-byte payload.
+    """
+    if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
+        raise ValueError(f"Invalid mode {mode!r}; expected 0, 1, or 2")
+    return struct.pack(_FMT_MODE, mode)
+
+
+def encode_estop_cmd(stop: bool = True) -> bytes:
+    """
+    Encode a MAMBA_CMD_ESTOP payload.
+
+    Parameters
+    ----------
+    stop: True to assert e-stop, False to clear.
+
+    Returns
+    -------
+    1-byte payload (0x01 = stop, 0x00 = clear).
+    """
+    return struct.pack(_FMT_ESTOP, 0x01 if stop else 0x00)
+
+
+# ---------------------------------------------------------------------------
+# Decode helpers
+# ---------------------------------------------------------------------------
+
+def decode_imu_telem(data: bytes) -> ImuTelemetry:
+    """
+    Decode a MAMBA_TELEM_IMU payload.
+
+    Parameters
+    ----------
+    data: exactly 24 bytes (6 × float32, big-endian).
+
+    Returns
+    -------
+    ImuTelemetry dataclass instance.
+
+    Raises
+    ------
+    struct.error if data is the wrong length.
+    """
+    ax, ay, az, gx, gy, gz = struct.unpack(_FMT_IMU, data)
+    return ImuTelemetry(
+        accel_x=ax, accel_y=ay, accel_z=az,
+        gyro_x=gx, gyro_y=gy, gyro_z=gz,
+    )
+
+
+def decode_battery_telem(data: bytes) -> BatteryTelemetry:
+    """
+    Decode a MAMBA_TELEM_BATTERY payload.
+
+    Parameters
+    ----------
+    data: exactly 8 bytes (2 × float32, big-endian).
+
+    Returns
+    -------
+    BatteryTelemetry dataclass instance.
+    """
+    voltage, current = struct.unpack(_FMT_BAT, data)
+    return BatteryTelemetry(voltage=voltage, current=current)
+
+
+def decode_vesc_state(data: bytes) -> VescStateTelemetry:
+    """
+    Decode a VESC_TELEM_STATE payload.
+
+    Parameters
+    ----------
+    data: exactly 16 bytes (4 × float32, big-endian).
+
+    Returns
+    -------
+    VescStateTelemetry dataclass instance.
+    """
+    erpm, duty, voltage, current = struct.unpack(_FMT_VESC, data)
+    return VescStateTelemetry(erpm=erpm, duty=duty, voltage=voltage, current=current)

jetson/ros2_ws/src/saltybot_can_bridge/setup.cfg

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

jetson/ros2_ws/src/saltybot_can_bridge/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = "saltybot_can_bridge"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/config", ["config/can_bridge_params.yaml"]),
+    ],
+    install_requires=["setuptools", "python-can"],
+    zip_safe=True,
+    maintainer="sl-controls",
+    maintainer_email="sl-controls@saltylab.local",
+    description="CAN bus bridge for Mamba controller and VESC telemetry",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "can_bridge_node = saltybot_can_bridge.can_bridge_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_can_bridge/test/test_can_bridge.py

@@ -0,0 +1,254 @@
+#!/usr/bin/env python3
+"""
+Unit tests for saltybot_can_bridge.mamba_protocol.
+
+No ROS2 or CAN hardware required — tests exercise encode/decode round-trips
+and boundary conditions entirely in Python.
+
+Run with:  pytest test/test_can_bridge.py -v
+"""
+
+import struct
+import unittest
+
+from saltybot_can_bridge.mamba_protocol import (
+    MAMBA_CMD_ESTOP,
+    MAMBA_CMD_MODE,
+    MAMBA_CMD_VELOCITY,
+    MAMBA_TELEM_BATTERY,
+    MAMBA_TELEM_IMU,
+    VESC_TELEM_STATE,
+    MODE_DRIVE,
+    MODE_ESTOP,
+    MODE_IDLE,
+    BatteryTelemetry,
+    ImuTelemetry,
+    VescStateTelemetry,
+    decode_battery_telem,
+    decode_imu_telem,
+    decode_vesc_state,
+    encode_estop_cmd,
+    encode_mode_cmd,
+    encode_velocity_cmd,
+)
+
+
+class TestMessageIDs(unittest.TestCase):
+    """Verify the CAN message ID constants are correct."""
+
+    def test_command_ids(self):
+        self.assertEqual(MAMBA_CMD_VELOCITY, 0x100)
+        self.assertEqual(MAMBA_CMD_MODE, 0x101)
+        self.assertEqual(MAMBA_CMD_ESTOP, 0x102)
+
+    def test_telemetry_ids(self):
+        self.assertEqual(MAMBA_TELEM_IMU, 0x200)
+        self.assertEqual(MAMBA_TELEM_BATTERY, 0x201)
+        self.assertEqual(VESC_TELEM_STATE, 0x300)
+
+
+class TestVelocityEncode(unittest.TestCase):
+    """Tests for encode_velocity_cmd."""
+
+    def test_zero_velocity(self):
+        payload = encode_velocity_cmd(0.0, 0.0)
+        self.assertEqual(len(payload), 8)
+        left, right = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(left, 0.0, places=5)
+        self.assertAlmostEqual(right, 0.0, places=5)
+
+    def test_forward_velocity(self):
+        payload = encode_velocity_cmd(1.5, 1.5)
+        left, right = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(left, 1.5, places=5)
+        self.assertAlmostEqual(right, 1.5, places=5)
+
+    def test_differential_velocity(self):
+        payload = encode_velocity_cmd(-0.5, 0.5)
+        left, right = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(left, -0.5, places=5)
+        self.assertAlmostEqual(right, 0.5, places=5)
+
+    def test_large_velocity(self):
+        # No clamping at the protocol layer — values are sent as-is
+        payload = encode_velocity_cmd(10.0, -10.0)
+        left, right = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(left, 10.0, places=3)
+        self.assertAlmostEqual(right, -10.0, places=3)
+
+    def test_payload_is_big_endian(self):
+        # Sanity check: first 4 bytes encode left speed
+        payload = encode_velocity_cmd(1.0, 0.0)
+        (left,) = struct.unpack(">f", payload[:4])
+        self.assertAlmostEqual(left, 1.0, places=5)
+
+
+class TestModeEncode(unittest.TestCase):
+    """Tests for encode_mode_cmd."""
+
+    def test_idle_mode(self):
+        payload = encode_mode_cmd(MODE_IDLE)
+        self.assertEqual(payload, b"\x00")
+
+    def test_drive_mode(self):
+        payload = encode_mode_cmd(MODE_DRIVE)
+        self.assertEqual(payload, b"\x01")
+
+    def test_estop_mode(self):
+        payload = encode_mode_cmd(MODE_ESTOP)
+        self.assertEqual(payload, b"\x02")
+
+    def test_invalid_mode_raises(self):
+        with self.assertRaises(ValueError):
+            encode_mode_cmd(99)
+
+    def test_invalid_mode_negative_raises(self):
+        with self.assertRaises(ValueError):
+            encode_mode_cmd(-1)
+
+
+class TestEstopEncode(unittest.TestCase):
+    """Tests for encode_estop_cmd."""
+
+    def test_estop_assert(self):
+        self.assertEqual(encode_estop_cmd(True), b"\x01")
+
+    def test_estop_clear(self):
+        self.assertEqual(encode_estop_cmd(False), b"\x00")
+
+    def test_estop_default_is_stop(self):
+        self.assertEqual(encode_estop_cmd(), b"\x01")
+
+
+class TestImuDecodeRoundTrip(unittest.TestCase):
+    """Round-trip tests for IMU telemetry."""
+
+    def _encode_imu(self, ax, ay, az, gx, gy, gz) -> bytes:
+        return struct.pack(">ffffff", ax, ay, az, gx, gy, gz)
+
+    def test_zero_imu(self):
+        data = self._encode_imu(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+        telem = decode_imu_telem(data)
+        self.assertIsInstance(telem, ImuTelemetry)
+        self.assertAlmostEqual(telem.accel_x, 0.0, places=5)
+        self.assertAlmostEqual(telem.gyro_z, 0.0, places=5)
+
+    def test_nominal_imu(self):
+        data = self._encode_imu(0.1, -0.2, 9.81, 0.01, -0.02, 0.03)
+        telem = decode_imu_telem(data)
+        self.assertAlmostEqual(telem.accel_x, 0.1, places=4)
+        self.assertAlmostEqual(telem.accel_y, -0.2, places=4)
+        self.assertAlmostEqual(telem.accel_z, 9.81, places=3)
+        self.assertAlmostEqual(telem.gyro_x, 0.01, places=5)
+        self.assertAlmostEqual(telem.gyro_y, -0.02, places=5)
+        self.assertAlmostEqual(telem.gyro_z, 0.03, places=5)
+
+    def test_imu_bad_length_raises(self):
+        with self.assertRaises(struct.error):
+            decode_imu_telem(b"\x00" * 10)  # too short
+
+
+class TestBatteryDecodeRoundTrip(unittest.TestCase):
+    """Round-trip tests for battery telemetry."""
+
+    def _encode_bat(self, voltage, current) -> bytes:
+        return struct.pack(">ff", voltage, current)
+
+    def test_nominal_battery(self):
+        data = self._encode_bat(24.6, 3.2)
+        telem = decode_battery_telem(data)
+        self.assertIsInstance(telem, BatteryTelemetry)
+        self.assertAlmostEqual(telem.voltage, 24.6, places=3)
+        self.assertAlmostEqual(telem.current, 3.2, places=4)
+
+    def test_zero_battery(self):
+        data = self._encode_bat(0.0, 0.0)
+        telem = decode_battery_telem(data)
+        self.assertAlmostEqual(telem.voltage, 0.0, places=5)
+        self.assertAlmostEqual(telem.current, 0.0, places=5)
+
+    def test_max_voltage(self):
+        # 6S LiPo max ~25.2 V; test with a high value
+        data = self._encode_bat(25.2, 0.0)
+        telem = decode_battery_telem(data)
+        self.assertAlmostEqual(telem.voltage, 25.2, places=3)
+
+    def test_battery_bad_length_raises(self):
+        with self.assertRaises(struct.error):
+            decode_battery_telem(b"\x00" * 4)  # too short
+
+
+class TestVescStateDecodeRoundTrip(unittest.TestCase):
+    """Round-trip tests for VESC state telemetry."""
+
+    def _encode_vesc(self, erpm, duty, voltage, current) -> bytes:
+        return struct.pack(">ffff", erpm, duty, voltage, current)
+
+    def test_nominal_vesc(self):
+        data = self._encode_vesc(3000.0, 0.25, 24.0, 5.5)
+        telem = decode_vesc_state(data)
+        self.assertIsInstance(telem, VescStateTelemetry)
+        self.assertAlmostEqual(telem.erpm, 3000.0, places=2)
+        self.assertAlmostEqual(telem.duty, 0.25, places=5)
+        self.assertAlmostEqual(telem.voltage, 24.0, places=4)
+        self.assertAlmostEqual(telem.current, 5.5, places=4)
+
+    def test_zero_vesc(self):
+        data = self._encode_vesc(0.0, 0.0, 0.0, 0.0)
+        telem = decode_vesc_state(data)
+        self.assertAlmostEqual(telem.erpm, 0.0)
+        self.assertAlmostEqual(telem.duty, 0.0)
+
+    def test_reverse_erpm(self):
+        data = self._encode_vesc(-1500.0, -0.15, 23.0, 2.0)
+        telem = decode_vesc_state(data)
+        self.assertAlmostEqual(telem.erpm, -1500.0, places=2)
+        self.assertAlmostEqual(telem.duty, -0.15, places=5)
+
+    def test_vesc_bad_length_raises(self):
+        with self.assertRaises(struct.error):
+            decode_vesc_state(b"\x00" * 8)  # too short (need 16)
+
+
+class TestEncodeDecodeIdentity(unittest.TestCase):
+    """Cross-module identity tests: encode then decode must be identity."""
+
+    def test_velocity_identity(self):
+        """encode_velocity_cmd raw bytes must decode to the same floats."""
+        left, right = 0.75, -0.3
+        payload = encode_velocity_cmd(left, right)
+        decoded_l, decoded_r = struct.unpack(">ff", payload)
+        self.assertAlmostEqual(decoded_l, left, places=5)
+        self.assertAlmostEqual(decoded_r, right, places=5)
+
+    def test_imu_identity(self):
+        accel = (0.5, -0.5, 9.8)
+        gyro = (0.1, -0.1, 0.2)
+        raw = struct.pack(">ffffff", *accel, *gyro)
+        telem = decode_imu_telem(raw)
+        self.assertAlmostEqual(telem.accel_x, accel[0], places=4)
+        self.assertAlmostEqual(telem.accel_y, accel[1], places=4)
+        self.assertAlmostEqual(telem.accel_z, accel[2], places=3)
+        self.assertAlmostEqual(telem.gyro_x, gyro[0], places=5)
+        self.assertAlmostEqual(telem.gyro_y, gyro[1], places=5)
+        self.assertAlmostEqual(telem.gyro_z, gyro[2], places=5)
+
+    def test_battery_identity(self):
+        voltage, current = 22.4, 8.1
+        raw = struct.pack(">ff", voltage, current)
+        telem = decode_battery_telem(raw)
+        self.assertAlmostEqual(telem.voltage, voltage, places=3)
+        self.assertAlmostEqual(telem.current, current, places=4)
+
+    def test_vesc_identity(self):
+        erpm, duty, voltage, current = 5000.0, 0.5, 24.5, 10.0
+        raw = struct.pack(">ffff", erpm, duty, voltage, current)
+        telem = decode_vesc_state(raw)
+        self.assertAlmostEqual(telem.erpm, erpm, places=2)
+        self.assertAlmostEqual(telem.duty, duty, places=5)
+        self.assertAlmostEqual(telem.voltage, voltage, places=3)
+        self.assertAlmostEqual(telem.current, current, places=4)
+
+
+if __name__ == "__main__":
+    unittest.main()

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/config/aggregator_params.yaml

@@ -0,0 +1,15 @@
+# Diagnostics Aggregator — Issue #658
+# Unified health dashboard aggregating telemetry from all SaltyBot subsystems.
+
+diagnostics_aggregator:
+  ros__parameters:
+    # Publish rate for /saltybot/system_health (Hz)
+    heartbeat_hz: 1.0
+
+    # Seconds without a topic update before a subsystem is marked STALE
+    # Increase for sensors with lower publish rates (e.g. UWB at 5 Hz)
+    stale_timeout_s: 5.0
+
+    # Maximum number of state transitions kept in the in-memory ring buffer
+    # Last 10 transitions are included in each /saltybot/system_health publish
+    transition_log_size: 50

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/launch/diagnostics_aggregator.launch.py

@@ -0,0 +1,44 @@
+"""Launch file for diagnostics aggregator node."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("saltybot_diagnostics_aggregator")
+    config_file = os.path.join(pkg_dir, "config", "aggregator_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "config_file",
+            default_value=config_file,
+            description="Path to aggregator_params.yaml",
+        ),
+        DeclareLaunchArgument(
+            "heartbeat_hz",
+            default_value="1.0",
+            description="Publish rate for /saltybot/system_health (Hz)",
+        ),
+        DeclareLaunchArgument(
+            "stale_timeout_s",
+            default_value="5.0",
+            description="Seconds without update before subsystem goes STALE",
+        ),
+        Node(
+            package="saltybot_diagnostics_aggregator",
+            executable="diagnostics_aggregator_node",
+            name="diagnostics_aggregator",
+            output="screen",
+            parameters=[
+                LaunchConfiguration("config_file"),
+                {
+                    "heartbeat_hz":    LaunchConfiguration("heartbeat_hz"),
+                    "stale_timeout_s": LaunchConfiguration("stale_timeout_s"),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_diagnostics_aggregator</name>
+  <version>0.1.0</version>
+  <description>
+    Diagnostics aggregator for SaltyBot — unified health dashboard node (Issue #658).
+    Subscribes to /vesc/health, /diagnostics, /saltybot/safety_zone/status,
+    /saltybot/pose/status, /saltybot/uwb/status. Aggregates into
+    /saltybot/system_health JSON at 1 Hz. Tracks motors, battery, imu, uwb,
+    lidar, camera, can_bus, estop subsystems with state-transition logging.
+  </description>
+  <maintainer email="sl-firmware@saltylab.local">sl-firmware</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/saltybot_diagnostics_aggregator/aggregator_node.py

@@ -0,0 +1,312 @@
+#!/usr/bin/env python3
+"""Diagnostics aggregator — unified health dashboard node (Issue #658).
+
+Subscribes to telemetry and diagnostics topics from all subsystems, aggregates
+them into a single /saltybot/system_health JSON publish at 1 Hz.
+
+Subscribed topics
+-----------------
+  /vesc/health                    (std_msgs/String)            — motor VESC health JSON
+  /diagnostics                    (diagnostic_msgs/DiagnosticArray) — ROS diagnostics bus
+  /saltybot/safety_zone/status    (std_msgs/String)            — safety / estop state
+  /saltybot/pose/status           (std_msgs/String)            — IMU / pose estimate state
+  /saltybot/uwb/status            (std_msgs/String)            — UWB positioning state
+
+Published topics
+----------------
+  /saltybot/system_health  (std_msgs/String) — JSON health dashboard at 1 Hz
+
+JSON schema for /saltybot/system_health
+----------------------------------------
+{
+  "overall_status": "OK" | "WARN" | "ERROR" | "STALE",
+  "uptime_s": <float>,
+  "subsystems": {
+    "motors":   { "status": ..., "message": ..., "age_s": ..., "previous_status": ... },
+    "battery":  { ... },
+    "imu":      { ... },
+    "uwb":      { ... },
+    "lidar":    { ... },
+    "camera":   { ... },
+    "can_bus":  { ... },
+    "estop":    { ... }
+  },
+  "last_error": { "subsystem": ..., "message": ..., "timestamp": ... } | null,
+  "recent_transitions": [ { "subsystem", "from_status", "to_status",
+                             "message", "timestamp_iso" }, ... ],
+  "timestamp": "<ISO-8601>"
+}
+
+Parameters
+----------
+  heartbeat_hz          float  1.0    Publish rate (Hz)
+  stale_timeout_s       float  5.0    Seconds without update → STALE
+  transition_log_size   int    50     Max recent transitions kept in memory
+"""
+
+import json
+import time
+from collections import deque
+from datetime import datetime, timezone
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from diagnostic_msgs.msg import DiagnosticArray
+
+from .subsystem import (
+    SubsystemState,
+    Transition,
+    STATUS_OK,
+    STATUS_WARN,
+    STATUS_ERROR,
+    STATUS_STALE,
+    STATUS_UNKNOWN,
+    worse,
+    ros_level_to_status,
+    SUBSYSTEM_NAMES as _SUBSYSTEM_NAMES,
+    keyword_to_subsystem as _keyword_to_subsystem,
+)
+
+
+class DiagnosticsAggregatorNode(Node):
+    """Unified health dashboard node."""
+
+    def __init__(self):
+        super().__init__("diagnostics_aggregator")
+
+        # ----------------------------------------------------------------
+        # Parameters
+        # ----------------------------------------------------------------
+        self.declare_parameter("heartbeat_hz",        1.0)
+        self.declare_parameter("stale_timeout_s",     5.0)
+        self.declare_parameter("transition_log_size", 50)
+
+        hz              = float(self.get_parameter("heartbeat_hz").value)
+        stale_timeout   = float(self.get_parameter("stale_timeout_s").value)
+        log_size        = int(self.get_parameter("transition_log_size").value)
+
+        # ----------------------------------------------------------------
+        # Subsystem state table
+        # ----------------------------------------------------------------
+        self._subsystems: dict[str, SubsystemState] = {
+            name: SubsystemState(name=name, stale_timeout_s=stale_timeout)
+            for name in _SUBSYSTEM_NAMES
+        }
+        self._transitions: deque[Transition] = deque(maxlen=log_size)
+        self._last_error: Optional[dict] = None
+        self._start_mono = time.monotonic()
+
+        # ----------------------------------------------------------------
+        # Subscriptions
+        # ----------------------------------------------------------------
+        self.create_subscription(String,        "/vesc/health",
+                                 self._on_vesc_health,       10)
+        self.create_subscription(DiagnosticArray, "/diagnostics",
+                                 self._on_diagnostics,       10)
+        self.create_subscription(String,        "/saltybot/safety_zone/status",
+                                 self._on_safety_zone,       10)
+        self.create_subscription(String,        "/saltybot/pose/status",
+                                 self._on_pose_status,       10)
+        self.create_subscription(String,        "/saltybot/uwb/status",
+                                 self._on_uwb_status,        10)
+
+        # ----------------------------------------------------------------
+        # Publisher
+        # ----------------------------------------------------------------
+        self._pub = self.create_publisher(String, "/saltybot/system_health", 1)
+
+        # ----------------------------------------------------------------
+        # 1 Hz heartbeat timer
+        # ----------------------------------------------------------------
+        self.create_timer(1.0 / max(0.1, hz), self._on_timer)
+
+        self.get_logger().info(
+            f"diagnostics_aggregator: {len(_SUBSYSTEM_NAMES)} subsystems, "
+            f"heartbeat={hz} Hz, stale_timeout={stale_timeout} s"
+        )
+
+    # ----------------------------------------------------------------
+    # Topic callbacks
+    # ----------------------------------------------------------------
+
+    def _on_vesc_health(self, msg: String) -> None:
+        """Parse /vesc/health JSON → motors subsystem."""
+        now = time.monotonic()
+        try:
+            d = json.loads(msg.data)
+            fault = d.get("fault_code", 0)
+            alive = d.get("alive", True)
+            if not alive:
+                status, message = STATUS_STALE, "VESC offline"
+            elif fault != 0:
+                status  = STATUS_ERROR
+                message = f"VESC fault {d.get('fault_name', fault)}"
+            else:
+                status  = STATUS_OK
+                message = (
+                    f"RPM={d.get('rpm', '?')} "
+                    f"I={d.get('current_a', '?')} A "
+                    f"V={d.get('voltage_v', '?')} V"
+                )
+            self._update("motors", status, message, now)
+        except Exception as exc:
+            self.get_logger().debug(f"/vesc/health parse error: {exc}")
+
+    def _on_diagnostics(self, msg: DiagnosticArray) -> None:
+        """Fan /diagnostics entries out to per-subsystem states."""
+        now = time.monotonic()
+        # Accumulate worst status per subsystem across all entries in this array
+        pending: dict[str, tuple[str, str]] = {}  # subsystem → (status, message)
+
+        for ds in msg.status:
+            subsystem = _keyword_to_subsystem(ds.name)
+            if subsystem is None:
+                continue
+            status  = ros_level_to_status(ds.level)
+            message = ds.message or ""
+            if subsystem not in pending:
+                pending[subsystem] = (status, message)
+            else:
+                prev_s, prev_m = pending[subsystem]
+                new_worst = worse(status, prev_s)
+                pending[subsystem] = (
+                    new_worst,
+                    message if new_worst == status else prev_m,
+                )
+
+        for subsystem, (status, message) in pending.items():
+            self._update(subsystem, status, message, now)
+
+    def _on_safety_zone(self, msg: String) -> None:
+        """Parse /saltybot/safety_zone/status JSON → estop subsystem."""
+        now = time.monotonic()
+        try:
+            d = json.loads(msg.data)
+            triggered = d.get("estop_triggered", d.get("triggered", False))
+            active    = d.get("safety_zone_active", d.get("active", True))
+            if triggered:
+                status, message = STATUS_ERROR, "E-stop triggered"
+            elif not active:
+                status, message = STATUS_WARN, "Safety zone inactive"
+            else:
+                status, message = STATUS_OK, "Safety zone active"
+            self._update("estop", status, message, now)
+        except Exception as exc:
+            self.get_logger().debug(f"/saltybot/safety_zone/status parse error: {exc}")
+
+    def _on_pose_status(self, msg: String) -> None:
+        """Parse /saltybot/pose/status JSON → imu subsystem."""
+        now = time.monotonic()
+        try:
+            d = json.loads(msg.data)
+            status_str = d.get("status", "OK").upper()
+            if status_str not in (STATUS_OK, STATUS_WARN, STATUS_ERROR):
+                status_str = STATUS_WARN
+            message = d.get("message", d.get("msg", ""))
+            self._update("imu", status_str, message, now)
+        except Exception as exc:
+            self.get_logger().debug(f"/saltybot/pose/status parse error: {exc}")
+
+    def _on_uwb_status(self, msg: String) -> None:
+        """Parse /saltybot/uwb/status JSON → uwb subsystem."""
+        now = time.monotonic()
+        try:
+            d = json.loads(msg.data)
+            status_str = d.get("status", "OK").upper()
+            if status_str not in (STATUS_OK, STATUS_WARN, STATUS_ERROR):
+                status_str = STATUS_WARN
+            message = d.get("message", d.get("msg", ""))
+            self._update("uwb", status_str, message, now)
+        except Exception as exc:
+            self.get_logger().debug(f"/saltybot/uwb/status parse error: {exc}")
+
+    # ----------------------------------------------------------------
+    # Internal helpers
+    # ----------------------------------------------------------------
+
+    def _update(self, subsystem: str, status: str, message: str, now: float) -> None:
+        """Update subsystem state and record any transition."""
+        s = self._subsystems.get(subsystem)
+        if s is None:
+            return
+        transition = s.update(status, message, now)
+        if transition is not None:
+            self._transitions.append(transition)
+            self.get_logger().info(
+                f"[{subsystem}] {transition.from_status} → {transition.to_status}: {message}"
+            )
+            if transition.to_status == STATUS_ERROR:
+                self._last_error = {
+                    "subsystem": subsystem,
+                    "message":   message,
+                    "timestamp": transition.timestamp_iso,
+                }
+
+    def _apply_stale_checks(self, now: float) -> None:
+        for s in self._subsystems.values():
+            transition = s.apply_stale_check(now)
+            if transition is not None:
+                self._transitions.append(transition)
+                self.get_logger().warn(
+                    f"[{transition.subsystem}] went STALE (no data)"
+                )
+
+    # ----------------------------------------------------------------
+    # Heartbeat timer
+    # ----------------------------------------------------------------
+
+    def _on_timer(self) -> None:
+        now = time.monotonic()
+        self._apply_stale_checks(now)
+
+        # Overall status = worst across all subsystems
+        overall = STATUS_OK
+        for s in self._subsystems.values():
+            overall = worse(overall, s.status)
+        # UNKNOWN does not degrade overall if at least one subsystem is known
+        known = [s for s in self._subsystems.values() if s.status != STATUS_UNKNOWN]
+        if not known:
+            overall = STATUS_UNKNOWN
+
+        uptime = now - self._start_mono
+
+        payload = {
+            "overall_status": overall,
+            "uptime_s":       round(uptime, 1),
+            "subsystems":     {
+                name: s.to_dict(now)
+                for name, s in self._subsystems.items()
+            },
+            "last_error": self._last_error,
+            "recent_transitions": [
+                {
+                    "subsystem":   t.subsystem,
+                    "from_status": t.from_status,
+                    "to_status":   t.to_status,
+                    "message":     t.message,
+                    "timestamp":   t.timestamp_iso,
+                }
+                for t in list(self._transitions)[-10:]  # last 10 in the JSON
+            ],
+            "timestamp": datetime.now(timezone.utc).isoformat(),
+        }
+
+        self._pub.publish(String(data=json.dumps(payload)))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DiagnosticsAggregatorNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/saltybot_diagnostics_aggregator/subsystem.py

@@ -0,0 +1,148 @@
+"""Subsystem state tracking for the diagnostics aggregator.
+
+Each SubsystemState tracks one logical subsystem (motors, battery, etc.)
+across potentially multiple source topics.  Status priority:
+  ERROR > WARN > STALE > OK > UNKNOWN
+"""
+
+import time
+from dataclasses import dataclass, field
+from typing import Optional
+
+# ---------------------------------------------------------------------------
+# Status constants — ordered by severity (higher index = more severe)
+# ---------------------------------------------------------------------------
+STATUS_UNKNOWN = "UNKNOWN"
+STATUS_OK      = "OK"
+STATUS_STALE   = "STALE"
+STATUS_WARN    = "WARN"
+STATUS_ERROR   = "ERROR"
+
+# ---------------------------------------------------------------------------
+# Subsystem registry and diagnostic keyword routing
+# ---------------------------------------------------------------------------
+SUBSYSTEM_NAMES: list[str] = [
+    "motors", "battery", "imu", "uwb", "lidar", "camera", "can_bus", "estop"
+]
+
+# (keywords-tuple, subsystem-name) — first match wins, lower-cased comparison
+DIAG_KEYWORD_MAP: list[tuple[tuple[str, ...], str]] = [
+    (("vesc", "motor", "esc", "fsesc"),               "motors"),
+    (("battery", "ina219", "lvc", "coulomb", "vbat"),  "battery"),
+    (("imu", "mpu6000", "bno055", "icm42688", "gyro", "accel"), "imu"),
+    (("uwb", "dw1000", "dw3000"),                      "uwb"),
+    (("lidar", "rplidar", "sick", "laser"),             "lidar"),
+    (("camera", "realsense", "oak", "webcam"),          "camera"),
+    (("can", "can_bus", "can_driver"),                  "can_bus"),
+    (("estop", "safety", "emergency", "e-stop"),        "estop"),
+]
+
+
+def keyword_to_subsystem(name: str) -> Optional[str]:
+    """Map a diagnostic status name to a subsystem key, or None."""
+    lower = name.lower()
+    for keywords, subsystem in DIAG_KEYWORD_MAP:
+        if any(kw in lower for kw in keywords):
+            return subsystem
+    return None
+
+
+_SEVERITY: dict[str, int] = {
+    STATUS_UNKNOWN: 0,
+    STATUS_OK:      1,
+    STATUS_STALE:   2,
+    STATUS_WARN:    3,
+    STATUS_ERROR:   4,
+}
+
+
+def worse(a: str, b: str) -> str:
+    """Return the more severe of two status strings."""
+    return a if _SEVERITY.get(a, 0) >= _SEVERITY.get(b, 0) else b
+
+
+def ros_level_to_status(level: int) -> str:
+    """Convert diagnostic_msgs/DiagnosticStatus.level to a status string."""
+    return {0: STATUS_OK, 1: STATUS_WARN, 2: STATUS_ERROR}.get(level, STATUS_UNKNOWN)
+
+
+# ---------------------------------------------------------------------------
+# Transition log entry
+# ---------------------------------------------------------------------------
+@dataclass
+class Transition:
+    """A logged status change for one subsystem."""
+    subsystem: str
+    from_status: str
+    to_status: str
+    message: str
+    timestamp_iso: str      # ISO-8601 wall-clock
+    monotonic_s: float      # time.monotonic() at the transition
+
+
+# ---------------------------------------------------------------------------
+# Per-subsystem state
+# ---------------------------------------------------------------------------
+@dataclass
+class SubsystemState:
+    """Live health state for one logical subsystem."""
+
+    name: str
+    stale_timeout_s: float = 5.0    # seconds without update → STALE
+
+    # Mutable state
+    status: str = STATUS_UNKNOWN
+    message: str = ""
+    last_updated_mono: float = field(default_factory=lambda: 0.0)
+    last_change_mono: float  = field(default_factory=lambda: 0.0)
+    previous_status: str = STATUS_UNKNOWN
+
+    def update(self, new_status: str, message: str, now_mono: float) -> Optional[Transition]:
+        """Apply a new status, record a transition if the status changed.
+
+        Returns a Transition if the status changed, else None.
+        """
+        from datetime import datetime, timezone
+
+        self.last_updated_mono = now_mono
+
+        if new_status == self.status:
+            self.message = message
+            return None
+
+        transition = Transition(
+            subsystem=self.name,
+            from_status=self.status,
+            to_status=new_status,
+            message=message,
+            timestamp_iso=datetime.now(timezone.utc).isoformat(),
+            monotonic_s=now_mono,
+        )
+
+        self.previous_status = self.status
+        self.status = new_status
+        self.message = message
+        self.last_change_mono = now_mono
+        return transition
+
+    def apply_stale_check(self, now_mono: float) -> Optional[Transition]:
+        """Mark STALE if no update received within stale_timeout_s.
+
+        Returns a Transition if newly staled, else None.
+        """
+        if self.last_updated_mono == 0.0:
+            # Never received any data — leave as UNKNOWN
+            return None
+        if (now_mono - self.last_updated_mono) > self.stale_timeout_s:
+            if self.status not in (STATUS_STALE, STATUS_UNKNOWN):
+                return self.update(STATUS_STALE, "No data received", now_mono)
+        return None
+
+    def to_dict(self, now_mono: float) -> dict:
+        age = (now_mono - self.last_updated_mono) if self.last_updated_mono > 0 else None
+        return {
+            "status":           self.status,
+            "message":          self.message,
+            "age_s":            round(age, 2) if age is not None else None,
+            "previous_status":  self.previous_status,
+        }

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/setup.cfg

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

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/setup.py

@@ -0,0 +1,28 @@
+from setuptools import setup
+
+package_name = "saltybot_diagnostics_aggregator"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/launch", ["launch/diagnostics_aggregator.launch.py"]),
+        (f"share/{package_name}/config", ["config/aggregator_params.yaml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-firmware",
+    maintainer_email="sl-firmware@saltylab.local",
+    description="Diagnostics aggregator — unified health dashboard for SaltyBot",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "diagnostics_aggregator_node = "
+            "saltybot_diagnostics_aggregator.aggregator_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_diagnostics_aggregator/test/test_aggregator.py

@@ -0,0 +1,272 @@
+"""Unit tests for diagnostics aggregator — subsystem logic and routing.
+
+All pure-function tests; no ROS2 or live topics required.
+"""
+
+import time
+import json
+import pytest
+
+from saltybot_diagnostics_aggregator.subsystem import (
+    SubsystemState,
+    Transition,
+    STATUS_OK,
+    STATUS_WARN,
+    STATUS_ERROR,
+    STATUS_STALE,
+    STATUS_UNKNOWN,
+    worse,
+    ros_level_to_status,
+    keyword_to_subsystem as _keyword_to_subsystem,
+    SUBSYSTEM_NAMES as _SUBSYSTEM_NAMES,
+)
+
+
+# ---------------------------------------------------------------------------
+# worse()
+# ---------------------------------------------------------------------------
+
+class TestWorse:
+    def test_error_beats_warn(self):
+        assert worse(STATUS_ERROR, STATUS_WARN) == STATUS_ERROR
+
+    def test_warn_beats_ok(self):
+        assert worse(STATUS_WARN, STATUS_OK) == STATUS_WARN
+
+    def test_stale_beats_ok(self):
+        assert worse(STATUS_STALE, STATUS_OK) == STATUS_STALE
+
+    def test_warn_beats_stale(self):
+        assert worse(STATUS_WARN, STATUS_STALE) == STATUS_WARN
+
+    def test_error_beats_stale(self):
+        assert worse(STATUS_ERROR, STATUS_STALE) == STATUS_ERROR
+
+    def test_ok_vs_ok(self):
+        assert worse(STATUS_OK, STATUS_OK) == STATUS_OK
+
+    def test_error_vs_error(self):
+        assert worse(STATUS_ERROR, STATUS_ERROR) == STATUS_ERROR
+
+    def test_unknown_loses_to_ok(self):
+        assert worse(STATUS_OK, STATUS_UNKNOWN) == STATUS_OK
+
+    def test_symmetric(self):
+        for a in (STATUS_OK, STATUS_WARN, STATUS_ERROR, STATUS_STALE):
+            for b in (STATUS_OK, STATUS_WARN, STATUS_ERROR, STATUS_STALE):
+                assert worse(a, b) == worse(b, a) or True  # just ensure no crash
+
+
+# ---------------------------------------------------------------------------
+# ros_level_to_status()
+# ---------------------------------------------------------------------------
+
+class TestRosLevelToStatus:
+    def test_level_0_is_ok(self):
+        assert ros_level_to_status(0) == STATUS_OK
+
+    def test_level_1_is_warn(self):
+        assert ros_level_to_status(1) == STATUS_WARN
+
+    def test_level_2_is_error(self):
+        assert ros_level_to_status(2) == STATUS_ERROR
+
+    def test_unknown_level(self):
+        assert ros_level_to_status(99) == STATUS_UNKNOWN
+
+    def test_negative_level(self):
+        assert ros_level_to_status(-1) == STATUS_UNKNOWN
+
+
+# ---------------------------------------------------------------------------
+# _keyword_to_subsystem()
+# ---------------------------------------------------------------------------
+
+class TestKeywordToSubsystem:
+    def test_vesc_maps_to_motors(self):
+        assert _keyword_to_subsystem("VESC/left (CAN ID 56)") == "motors"
+
+    def test_motor_maps_to_motors(self):
+        assert _keyword_to_subsystem("motor_controller") == "motors"
+
+    def test_battery_maps_to_battery(self):
+        assert _keyword_to_subsystem("battery_monitor") == "battery"
+
+    def test_ina219_maps_to_battery(self):
+        assert _keyword_to_subsystem("INA219 current sensor") == "battery"
+
+    def test_lvc_maps_to_battery(self):
+        assert _keyword_to_subsystem("lvc_cutoff") == "battery"
+
+    def test_imu_maps_to_imu(self):
+        assert _keyword_to_subsystem("IMU/mpu6000") == "imu"
+
+    def test_mpu6000_maps_to_imu(self):
+        assert _keyword_to_subsystem("mpu6000 driver") == "imu"
+
+    def test_uwb_maps_to_uwb(self):
+        assert _keyword_to_subsystem("UWB anchor 0") == "uwb"
+
+    def test_rplidar_maps_to_lidar(self):
+        assert _keyword_to_subsystem("rplidar_node") == "lidar"
+
+    def test_lidar_maps_to_lidar(self):
+        assert _keyword_to_subsystem("lidar/scan") == "lidar"
+
+    def test_realsense_maps_to_camera(self):
+        assert _keyword_to_subsystem("RealSense D435i") == "camera"
+
+    def test_camera_maps_to_camera(self):
+        assert _keyword_to_subsystem("camera_health") == "camera"
+
+    def test_can_maps_to_can_bus(self):
+        assert _keyword_to_subsystem("can_driver stats") == "can_bus"
+
+    def test_estop_maps_to_estop(self):
+        assert _keyword_to_subsystem("estop_monitor") == "estop"
+
+    def test_safety_maps_to_estop(self):
+        assert _keyword_to_subsystem("safety_zone") == "estop"
+
+    def test_unknown_returns_none(self):
+        assert _keyword_to_subsystem("totally_unrelated_sensor") is None
+
+    def test_case_insensitive(self):
+        assert _keyword_to_subsystem("RPLIDAR A2") == "lidar"
+        assert _keyword_to_subsystem("IMU_CALIBRATION") == "imu"
+
+
+# ---------------------------------------------------------------------------
+# SubsystemState.update()
+# ---------------------------------------------------------------------------
+
+class TestSubsystemStateUpdate:
+    def _make(self) -> SubsystemState:
+        return SubsystemState(name="motors", stale_timeout_s=5.0)
+
+    def test_initial_state(self):
+        s = self._make()
+        assert s.status == STATUS_UNKNOWN
+        assert s.message == ""
+        assert s.previous_status == STATUS_UNKNOWN
+
+    def test_first_update_creates_transition(self):
+        s = self._make()
+        t = s.update(STATUS_OK, "all good", time.monotonic())
+        assert t is not None
+        assert t.from_status == STATUS_UNKNOWN
+        assert t.to_status   == STATUS_OK
+        assert t.subsystem   == "motors"
+
+    def test_same_status_no_transition(self):
+        s = self._make()
+        s.update(STATUS_OK, "good", time.monotonic())
+        t = s.update(STATUS_OK, "still good", time.monotonic())
+        assert t is None
+
+    def test_status_change_produces_transition(self):
+        s = self._make()
+        now = time.monotonic()
+        s.update(STATUS_OK, "good", now)
+        t = s.update(STATUS_ERROR, "fault", now + 1)
+        assert t is not None
+        assert t.from_status == STATUS_OK
+        assert t.to_status   == STATUS_ERROR
+        assert t.message     == "fault"
+
+    def test_previous_status_saved(self):
+        s = self._make()
+        now = time.monotonic()
+        s.update(STATUS_OK,   "good",  now)
+        s.update(STATUS_WARN, "warn",  now + 1)
+        assert s.previous_status == STATUS_OK
+        assert s.status          == STATUS_WARN
+
+    def test_last_updated_advances(self):
+        s = self._make()
+        t1 = time.monotonic()
+        s.update(STATUS_OK, "x", t1)
+        assert s.last_updated_mono == pytest.approx(t1)
+        t2 = t1 + 1.0
+        s.update(STATUS_OK, "y", t2)
+        assert s.last_updated_mono == pytest.approx(t2)
+
+    def test_transition_has_iso_timestamp(self):
+        s = self._make()
+        t = s.update(STATUS_OK, "good", time.monotonic())
+        assert t is not None
+        assert "T" in t.timestamp_iso  # ISO-8601 contains 'T'
+
+
+# ---------------------------------------------------------------------------
+# SubsystemState.apply_stale_check()
+# ---------------------------------------------------------------------------
+
+class TestSubsystemStateStale:
+    def test_never_updated_stays_unknown(self):
+        s = SubsystemState(name="imu", stale_timeout_s=2.0)
+        t = s.apply_stale_check(time.monotonic() + 100)
+        assert t is None
+        assert s.status == STATUS_UNKNOWN
+
+    def test_fresh_data_not_stale(self):
+        s = SubsystemState(name="imu", stale_timeout_s=5.0)
+        now = time.monotonic()
+        s.update(STATUS_OK, "good", now)
+        t = s.apply_stale_check(now + 3.0)  # 3s < 5s timeout
+        assert t is None
+        assert s.status == STATUS_OK
+
+    def test_old_data_goes_stale(self):
+        s = SubsystemState(name="imu", stale_timeout_s=5.0)
+        now = time.monotonic()
+        s.update(STATUS_OK, "good", now)
+        t = s.apply_stale_check(now + 6.0)  # 6s > 5s timeout
+        assert t is not None
+        assert t.to_status == STATUS_STALE
+
+    def test_already_stale_no_duplicate_transition(self):
+        s = SubsystemState(name="imu", stale_timeout_s=5.0)
+        now = time.monotonic()
+        s.update(STATUS_OK, "good", now)
+        s.apply_stale_check(now + 6.0)     # → STALE
+        t2 = s.apply_stale_check(now + 7.0)  # already STALE
+        assert t2 is None
+
+
+# ---------------------------------------------------------------------------
+# SubsystemState.to_dict()
+# ---------------------------------------------------------------------------
+
+class TestSubsystemStateToDict:
+    def test_unknown_state(self):
+        s = SubsystemState(name="uwb")
+        d = s.to_dict(time.monotonic())
+        assert d["status"] == STATUS_UNKNOWN
+        assert d["age_s"] is None
+
+    def test_known_state_has_age(self):
+        s = SubsystemState(name="uwb", stale_timeout_s=5.0)
+        now = time.monotonic()
+        s.update(STATUS_OK, "ok", now)
+        d = s.to_dict(now + 1.5)
+        assert d["status"]  == STATUS_OK
+        assert d["age_s"]   == pytest.approx(1.5, abs=0.01)
+        assert d["message"] == "ok"
+
+
+# ---------------------------------------------------------------------------
+# _SUBSYSTEM_NAMES completeness
+# ---------------------------------------------------------------------------
+
+class TestSubsystemNames:
+    def test_all_required_subsystems_present(self):
+        required = {"motors", "battery", "imu", "uwb", "lidar", "camera", "can_bus", "estop"}
+        assert required.issubset(set(_SUBSYSTEM_NAMES))
+
+    def test_no_duplicates(self):
+        assert len(_SUBSYSTEM_NAMES) == len(set(_SUBSYSTEM_NAMES))
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_nav2_slam/config/amcl_nav2_params.yaml

@@ -0,0 +1,331 @@
+# amcl_nav2_params.yaml — Complete Nav2 + AMCL parameter file (Issue #655)
+#
+# AMCL localises on a pre-built static map using /scan (RPLIDAR A1M8).
+# Odometry source: /odom from vesc_can_odometry (VESC CAN IDs 61/79, Issue #646).
+#
+# Launch with:
+#   ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py
+#   ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \
+#       map:=/mnt/nvme/saltybot/maps/my_map.yaml
+#
+# Costmaps (inline — required by nav2_bringup navigation_launch.py):
+#   global: static_layer + obstacle_layer (LiDAR) + inflation_layer
+#   local:  obstacle_layer (LiDAR) + inflation_layer, 4m rolling window
+
+# ── AMCL ─────────────────────────────────────────────────────────────────────
+amcl:
+  ros__parameters:
+    use_sim_time: false
+
+    # Particle filter bounds
+    min_particles:   500
+    max_particles:   3000
+
+    # Motion model noise (alpha1–5: low = trust odometry more)
+    # Tuned for VESC differential drive (Issue #646): encoder slip ~5%
+    alpha1: 0.10   # rot noise from rot
+    alpha2: 0.10   # rot noise from trans
+    alpha3: 0.05   # trans noise from trans
+    alpha4: 0.05   # trans noise from rot
+    alpha5: 0.10   # (omni only)
+
+    # Sensor model — likelihood field
+    laser_model_type:        "likelihood_field"
+    laser_likelihood_max_dist: 2.0
+    max_beams:               60
+    sigma_hit:               0.2
+    z_hit:                   0.50   # beam hit weight
+    z_rand:                  0.45   # random noise weight
+    z_max:                   0.025  # max-range weight  (z_hit+z_rand+z_max+z_short = 1.0)
+    z_short:                 0.025  # short-read weight
+    sigma_short:             0.05
+    do_beamskip:             false
+    beam_search_increment:   0.1
+
+    # Frame IDs
+    global_frame_id:  "map"
+    odom_frame_id:    "odom"
+    base_frame_id:    "base_link"
+
+    # Update triggers
+    update_min_d:  0.20   # m — update after moving this far
+    update_min_a:  0.15   # rad — update after rotating this much
+    resample_interval: 1
+
+    # Particle filter convergence
+    pf_err: 0.05
+    pf_z:   0.99
+
+    # Transform
+    tf_broadcast:       true
+    transform_tolerance: 0.5    # s — relaxed for slower Nav2 loop
+    save_pose_rate:     0.5     # Hz — save last pose for fast re-init
+
+    # Recovery particle injection (0 = disabled)
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+
+    # Robot model
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+
+    # Scan topic
+    scan_topic: /scan
+
+# ── Map Server ────────────────────────────────────────────────────────────────
+map_server:
+  ros__parameters:
+    use_sim_time: false
+    yaml_filename: ""   # overridden by launch argument --map
+
+# ── BT Navigator ─────────────────────────────────────────────────────────────
+bt_navigator:
+  ros__parameters:
+    use_sim_time: false
+    global_frame:      map
+    robot_base_frame:  base_link
+    odom_topic:        /odom
+    enable_groot_monitoring: false
+    default_nav_to_pose_bt_xml: >-
+      $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
+    default_nav_through_poses_bt_xml: >-
+      $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_planner_selector_bt_node
+
+# ── Controller Server (DWB local planner) ─────────────────────────────────────
+controller_server:
+  ros__parameters:
+    use_sim_time: false
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins:    ["general_goal_checker"]
+    controller_plugins:      ["FollowPath"]
+
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.3
+      movement_time_allowance: 15.0
+
+    general_goal_checker:
+      stateful: true
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance:  0.20
+      yaw_goal_tolerance: 0.20
+
+    # DWB — tuned for VESC differential drive on Saltybot
+    FollowPath:
+      plugin:                       "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details:     false
+      min_vel_x:                    0.0
+      min_vel_y:                    0.0
+      max_vel_x:                    1.0    # m/s — conservative indoor nav
+      max_vel_y:                    0.0    # diff drive: no lateral
+      max_vel_theta:                1.5    # rad/s
+      min_speed_xy:                 0.0
+      max_speed_xy:                 1.0
+      min_speed_theta:              0.0
+      acc_lim_x:                    2.0
+      acc_lim_y:                    0.0
+      acc_lim_theta:                2.0
+      decel_lim_x:                 -2.0
+      decel_lim_theta:             -2.0
+      vx_samples:                   20
+      vy_samples:                   1      # diff drive: no lateral
+      vtheta_samples:               40
+      sim_time:                     1.7
+      linear_granularity:           0.05
+      angular_granularity:          0.025
+      transform_tolerance:          0.2
+      trans_stopped_velocity:       0.25
+      short_circuit_trajectory_evaluation: true
+      stateful:                     true
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign",
+                "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale:   0.02
+      PathAlign.scale:      32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale:      24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale:       32.0
+      GoalDist.scale:       24.0
+      RotateToGoal.scale:   32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+# ── Planner Server (NavFn A*) ─────────────────────────────────────────────────
+planner_server:
+  ros__parameters:
+    use_sim_time: false
+    expected_planner_frequency: 20.0
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin:        "nav2_navfn_planner/NavfnPlanner"
+      tolerance:     0.5
+      use_astar:     true
+      allow_unknown: true
+
+# ── Behavior Server (recovery behaviors) ──────────────────────────────────────
+behavior_server:
+  ros__parameters:
+    use_sim_time:      false
+    costmap_topic:     local_costmap/costmap_raw
+    footprint_topic:   local_costmap/published_footprint
+    cycle_frequency:   10.0
+    global_frame:      odom
+    robot_base_frame:  base_link
+    transform_timeout: 0.1
+    simulate_ahead_time: 2.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin:           "nav2_behaviors/Spin"
+    backup:
+      plugin:           "nav2_behaviors/BackUp"
+    wait:
+      plugin:           "nav2_behaviors/Wait"
+    # Spin recovery
+    max_rotations:       2.0    # full rotations before giving up
+    # Backup recovery
+    max_backup_dist:     0.30   # m
+    backup_speed:        0.15   # m/s — slow for a balancing robot
+
+# ── Global Costmap ────────────────────────────────────────────────────────────
+# Uses /scan (RPLIDAR) for obstacle detection.
+# Layers: static (from /map) + obstacle (LiDAR) + inflation.
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time:        false
+      global_frame:        map
+      robot_base_frame:    base_link
+      update_frequency:    5.0    # Hz
+      publish_frequency:   2.0    # Hz
+      rolling_window:      false
+      resolution:          0.05   # m/cell
+      robot_radius:        0.35   # m — inscribed circle
+      track_unknown_space: true
+      unknown_cost_value:  -1
+      lethal_cost_threshold: 100
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      static_layer:
+        plugin:                     "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: true
+        enabled:                    true
+      obstacle_layer:
+        plugin:           "nav2_costmap_2d::ObstacleLayer"
+        enabled:          true
+        observation_sources: scan
+        scan:
+          topic:                /scan
+          data_type:            "LaserScan"
+          max_obstacle_height:  2.0
+          min_obstacle_height:  0.0
+          obstacle_max_range:   9.5
+          obstacle_min_range:   0.0
+          raytrace_max_range:   10.0
+          raytrace_min_range:   0.0
+          clearing:             true
+          marking:              true
+      inflation_layer:
+        plugin:               "nav2_costmap_2d::InflationLayer"
+        enabled:              true
+        inflation_radius:     0.55   # m — clears obstacles wider than robot
+        cost_scaling_factor:  10.0
+        inflate_unknown:      false
+        inflate_around_unknown: true
+
+# ── Local Costmap ─────────────────────────────────────────────────────────────
+# Rolling 4m window in odom frame.  LiDAR obstacle layer + inflation.
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time:        false
+      global_frame:        odom
+      robot_base_frame:    base_link
+      update_frequency:    10.0   # Hz
+      publish_frequency:   5.0    # Hz
+      rolling_window:      true
+      width:               4      # m
+      height:              4      # m
+      resolution:          0.05   # m/cell
+      robot_radius:        0.35
+      track_unknown_space: true
+      unknown_cost_value:  0      # treat unknown as free locally
+      lethal_cost_threshold: 100
+      plugins: ["obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin:           "nav2_costmap_2d::ObstacleLayer"
+        enabled:          true
+        observation_sources: scan
+        scan:
+          topic:                /scan
+          data_type:            "LaserScan"
+          max_obstacle_height:  2.0
+          min_obstacle_height:  0.0
+          obstacle_max_range:   5.0
+          obstacle_min_range:   0.0
+          raytrace_max_range:   6.0
+          raytrace_min_range:   0.0
+          clearing:             true
+          marking:              true
+          observation_persistence: 0.0
+          expected_update_rate: 5.5   # RPLIDAR A1M8 scan rate
+      inflation_layer:
+        plugin:               "nav2_costmap_2d::InflationLayer"
+        enabled:              true
+        inflation_radius:     0.55
+        cost_scaling_factor:  10.0
+        inflate_unknown:      false
+        inflate_around_unknown: false
+
+# ── Velocity Smoother ─────────────────────────────────────────────────────────
+velocity_smoother:
+  ros__parameters:
+    use_sim_time:        false
+    smoothing_frequency: 20.0
+    scale_velocities:    false
+    feedback:            "OPEN_LOOP"
+    max_velocity:        [1.0, 0.0, 1.5]
+    min_velocity:       [-1.0, 0.0, -1.5]
+    max_accel:           [2.0, 0.0, 2.0]
+    max_decel:          [-2.0, 0.0, -2.0]
+    odom_topic:          "odom"
+    odom_duration:       0.1
+    deadband_velocity:   [0.0, 0.0, 0.0]
+    velocity_timeout:    1.0
+
+# ── Lifecycle Managers ────────────────────────────────────────────────────────
+lifecycle_manager_localization:
+  ros__parameters:
+    use_sim_time: false
+    autostart:    true
+    node_names:   ["map_server", "amcl"]
+
+lifecycle_manager_navigation:
+  ros__parameters:
+    use_sim_time: false
+    autostart:    true
+    node_names:   ["controller_server", "planner_server", "behavior_server",
+                   "bt_navigator", "velocity_smoother"]

jetson/ros2_ws/src/saltybot_nav2_slam/config/vesc_odometry_params.yaml

@@ -1,8 +1,12 @@
 vesc_can_odometry:
   ros__parameters:
-    # ── CAN motor IDs ────────────────────────────────────────────────────────
-    left_can_id:       61      # left  motor VESC CAN ID → /vesc/can_61/state
-    right_can_id:      79      # right motor VESC CAN ID → /vesc/can_79/state
+    # ── CAN motor IDs (used for CAN addressing) ───────────────────────────────
+    left_can_id:        56      # left  motor VESC CAN ID (Mamba F722S)
+    right_can_id:       68      # right motor VESC CAN ID (Mamba F722S)
+
+    # ── State topic names (must match VESC telemetry publisher) ──────────────
+    left_state_topic:   /vesc/left/state
+    right_state_topic:  /vesc/right/state
 
     # ── Drive geometry ───────────────────────────────────────────────────────
     wheel_radius:      0.10    # wheel radius (m)

jetson/ros2_ws/src/saltybot_nav2_slam/launch/nav2_amcl_bringup.launch.py

@@ -0,0 +1,188 @@
+"""
+nav2_amcl_bringup.launch.py — Nav2 with AMCL localization (Issue #655).
+
+Full autonomous navigation stack for SaltyBot using a pre-built static map
+and AMCL particle-filter localization.
+
+Odometry: /odom from vesc_can_odometry (VESC CAN IDs 61/79, Issue #646)
+LiDAR:    /scan from RPLIDAR A1M8 (via saltybot_bringup sensors.launch.py)
+Map:      static OccupancyGrid loaded by map_server (defaults to placeholder)
+
+Startup sequence
+────────────────
+  1. Sensors         — RealSense D435i + RPLIDAR A1M8 drivers + static TF
+  2. VESC odometry   — /odom (differential drive from dual CAN motors)
+  3. Localization    — map_server + AMCL (nav2_bringup localization_launch.py)
+  4. Navigation      — controller + planner + behaviors + BT navigator
+                       (nav2_bringup navigation_launch.py)
+
+Arguments
+─────────
+  map            path to map YAML (default: maps/saltybot_map.yaml placeholder)
+  use_sim_time   (default: false)
+  autostart      lifecycle manager autostart (default: true)
+  params_file    Nav2 params override (default: config/amcl_nav2_params.yaml)
+  include_sensors  launch sensors.launch.py (default: true — set false if
+                   sensors are already running)
+
+Usage
+─────
+  # Minimal — sensors included, placeholder map:
+  ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py
+
+  # With a real saved map:
+  ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \
+      map:=/mnt/nvme/saltybot/maps/my_map.yaml
+
+  # Without sensor bringup (sensors already running):
+  ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \
+      include_sensors:=false
+
+Integration with saltybot_bringup
+──────────────────────────────────
+  The saltybot_bringup orchestrator (saltybot_bringup.launch.py) calls
+  nav2.launch.py at t=22 s in GROUP C.  To use AMCL instead of RTAB-Map:
+    ros2 launch saltybot_bringup nav2.launch.py nav_mode:=amcl
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    GroupAction,
+    IncludeLaunchDescription,
+    LogInfo,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description() -> LaunchDescription:
+    nav2_slam_dir    = get_package_share_directory("saltybot_nav2_slam")
+    bringup_dir      = get_package_share_directory("saltybot_bringup")
+    nav2_bringup_dir = get_package_share_directory("nav2_bringup")
+
+    default_map    = os.path.join(nav2_slam_dir, "maps", "saltybot_map.yaml")
+    default_params = os.path.join(nav2_slam_dir, "config", "amcl_nav2_params.yaml")
+
+    # ── Launch arguments ──────────────────────────────────────────────────────
+
+    map_arg = DeclareLaunchArgument(
+        "map",
+        default_value=default_map,
+        description="Path to map YAML file (Nav2 map_server format)",
+    )
+
+    use_sim_time_arg = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="false",
+        description="Use simulated clock from /clock topic",
+    )
+
+    autostart_arg = DeclareLaunchArgument(
+        "autostart",
+        default_value="true",
+        description="Automatically start lifecycle nodes after launch",
+    )
+
+    params_file_arg = DeclareLaunchArgument(
+        "params_file",
+        default_value=default_params,
+        description="Path to Nav2 parameter YAML file",
+    )
+
+    include_sensors_arg = DeclareLaunchArgument(
+        "include_sensors",
+        default_value="true",
+        description="Launch sensors.launch.py (set false if sensors already running)",
+    )
+
+    # ── Substitutions ─────────────────────────────────────────────────────────
+
+    map_file       = LaunchConfiguration("map")
+    use_sim_time   = LaunchConfiguration("use_sim_time")
+    autostart      = LaunchConfiguration("autostart")
+    params_file    = LaunchConfiguration("params_file")
+    include_sensors = LaunchConfiguration("include_sensors")
+
+    # ── Sensor bringup (conditional) ─────────────────────────────────────────
+    # RealSense D435i + RPLIDAR A1M8 drivers + base_link→laser/camera_link TF
+
+    sensors_launch = GroupAction(
+        condition=IfCondition(include_sensors),
+        actions=[
+            LogInfo(msg="[nav2_amcl] Starting sensors (RealSense + RPLIDAR)"),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(bringup_dir, "launch", "sensors.launch.py")
+                ),
+            ),
+        ],
+    )
+
+    # ── VESC CAN odometry ────────────────────────────────────────────────────
+    # Differential drive from CAN IDs 61 (left) + 79 (right) → /odom + TF
+
+    odom_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py")
+        ),
+        launch_arguments={"use_sim_time": use_sim_time}.items(),
+    )
+
+    # ── Localization: map_server + AMCL ──────────────────────────────────────
+    # Publishes: /map, TF map→odom (AMCL), /amcl_pose, /particlecloud
+
+    localization_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, "launch", "localization_launch.py")
+        ),
+        launch_arguments={
+            "map":           map_file,
+            "use_sim_time":  use_sim_time,
+            "autostart":     autostart,
+            "params_file":   params_file,
+            "use_lifecycle_mgr": "true",
+        }.items(),
+    )
+
+    # ── Navigation: controller + planner + behaviors + BT navigator ──────────
+    # Subscribes: /odom, /scan, /map, /cmd_vel_nav (→ velocity_smoother → /cmd_vel)
+    # Provides: action servers for NavigateToPose + NavigateThroughPoses
+
+    navigation_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, "launch", "navigation_launch.py")
+        ),
+        launch_arguments={
+            "use_sim_time":                use_sim_time,
+            "autostart":                   autostart,
+            "params_file":                 params_file,
+            "map_subscribe_transient_local": "true",
+            "use_lifecycle_mgr":           "true",
+        }.items(),
+    )
+
+    return LaunchDescription([
+        # Arguments
+        map_arg,
+        use_sim_time_arg,
+        autostart_arg,
+        params_file_arg,
+        include_sensors_arg,
+
+        # Banner
+        LogInfo(msg="[nav2_amcl] Nav2 AMCL bringup starting (Issue #655)"),
+
+        # Startup sequence
+        sensors_launch,                             # step 1 — sensors
+        odom_launch,                                # step 2 — /odom from VESC
+        LogInfo(msg="[nav2_amcl] Starting AMCL localization"),
+        localization_launch,                        # step 3 — map_server + AMCL
+        LogInfo(msg="[nav2_amcl] Starting Nav2 navigation stack"),
+        navigation_launch,                          # step 4 — full nav stack
+    ])

jetson/ros2_ws/src/saltybot_nav2_slam/maps/saltybot_map.yaml

@@ -0,0 +1,16 @@
+# SaltyBot placeholder map — 10m × 10m all free space at 0.05 m/cell.
+#
+# Replace with a real map saved by slam_toolbox or RTAB-Map:
+#   ros2 run nav2_map_server map_saver_cli -f /mnt/nvme/saltybot/maps/my_map
+#
+# Then launch with:
+#   ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py \
+#       map:=/mnt/nvme/saltybot/maps/my_map.yaml
+
+image:      saltybot_map.pgm
+mode:       trinary
+resolution: 0.05          # m/cell — matches costmap resolution
+origin:     [-5.0, -5.0, 0.0]   # lower-left corner (x, y, yaw)
+negate:     0             # 0: white=free, black=occupied
+occupied_thresh:  0.65    # p ≥ 0.65 → lethal
+free_thresh:      0.25    # p ≤ 0.25 → free

jetson/ros2_ws/src/saltybot_nav2_slam/package.xml

@@ -4,9 +4,10 @@
   <name>saltybot_nav2_slam</name>
   <version>0.1.0</version>
   <description>
-    Nav2 SLAM integration for SaltyBot autonomous navigation.
-    Combines SLAM Toolbox (RPLIDAR 2D SLAM), RealSense depth for obstacle avoidance,
-    VESC odometry, and DWB planner for autonomous navigation up to 20km/h.
+    Nav2 SLAM + AMCL integration for SaltyBot autonomous navigation (Issues #422, #655).
+    Combines SLAM Toolbox (RPLIDAR 2D SLAM), VESC CAN differential-drive odometry
+    (Issue #646), AMCL particle-filter localization on static maps, DWB local planner,
+    NavFn A* global planner, RealSense depth costmap, and spin/backup/wait recovery.
   </description>
   <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
   <license>MIT</license>
@@ -20,6 +21,14 @@
   <depend>tf2_ros</depend>
 
   <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>nav2_amcl</exec_depend>
+  <exec_depend>nav2_map_server</exec_depend>
+  <exec_depend>nav2_bt_navigator</exec_depend>
+  <exec_depend>nav2_controller</exec_depend>
+  <exec_depend>nav2_planner</exec_depend>
+  <exec_depend>nav2_behaviors</exec_depend>
+  <exec_depend>dwb_core</exec_depend>
+  <exec_depend>nav2_navfn_planner</exec_depend>
   <exec_depend>slam_toolbox</exec_depend>
   <exec_depend>rplidar_ros</exec_depend>
   <exec_depend>realsense2_camera</exec_depend>

jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/vesc_odometry_bridge.py

@@ -2,7 +2,9 @@
 """
 vesc_odometry_bridge.py — Differential drive odometry from dual VESC CAN motors (Issue #646).
 
-Subscribes to per-motor VESC telemetry topics for CAN IDs 61 (left) and 79 (right),
+Subscribes to per-motor VESC telemetry topics (configurable, defaulting to
+/vesc/left/state and /vesc/right/state as published by the telemetry node)
+for CAN IDs 56 (left) and 68 (right),
 applies differential drive kinematics via DiffDriveOdometry, and publishes:
 
   /odom                  nav_msgs/Odometry   — consumed by Nav2 / slam_toolbox
@@ -10,8 +12,11 @@ applies differential drive kinematics via DiffDriveOdometry, and publishes:
   TF odom → base_link
 
 Input topics (std_msgs/String, JSON):
-  /vesc/can_61/state  {"rpm": <eRPM>, "voltage_v": ..., "current_a": ..., ...}
-  /vesc/can_79/state  same schema
+  /vesc/left/state   {"rpm": <eRPM>, "voltage_v": ..., "current_a": ..., ...}
+  /vesc/right/state  same schema
+
+Topic names are configurable via left_state_topic / right_state_topic params.
+CAN IDs (left_can_id / right_can_id) are retained for CAN addressing purposes.
 
 Replaces the old single-motor vesc_odometry_bridge that used /vesc/state.
 """
@@ -52,8 +57,11 @@ class VESCCANOdometryNode(Node):
         super().__init__("vesc_can_odometry")
 
         # ── Parameters ────────────────────────────────────────────────────────
-        self.declare_parameter("left_can_id",       61)
-        self.declare_parameter("right_can_id",      79)
+        self.declare_parameter("left_can_id",       56)      # CAN ID for left motor
+        self.declare_parameter("right_can_id",      68)      # CAN ID for right motor
+        # Configurable state topic names — default to the names telemetry publishes
+        self.declare_parameter("left_state_topic",  "/vesc/left/state")
+        self.declare_parameter("right_state_topic", "/vesc/right/state")
         self.declare_parameter("wheel_radius",       0.10)   # metres
         self.declare_parameter("wheel_separation",   0.35)   # metres (track width)
         self.declare_parameter("motor_poles",        7)      # BLDC pole pairs
@@ -72,6 +80,8 @@ class VESCCANOdometryNode(Node):
 
         left_id         = self.get_parameter("left_can_id").value
         right_id        = self.get_parameter("right_can_id").value
+        left_topic      = self.get_parameter("left_state_topic").value
+        right_topic     = self.get_parameter("right_state_topic").value
         freq            = self.get_parameter("update_frequency").value
 
         self._odom_frame      = self.get_parameter("odom_frame_id").value
@@ -115,13 +125,13 @@ class VESCCANOdometryNode(Node):
         # ── Subscriptions ──────────────────────────────────────────────────────
         self.create_subscription(
             String,
-            f"/vesc/can_{left_id}/state",
+            left_topic,
             self._on_left,
             10,
         )
         self.create_subscription(
             String,
-            f"/vesc/can_{right_id}/state",
+            right_topic,
             self._on_right,
             10,
         )
@@ -130,7 +140,8 @@ class VESCCANOdometryNode(Node):
         self.create_timer(1.0 / freq, self._on_timer)
 
         self.get_logger().info(
-            f"vesc_can_odometry: left=CAN{left_id} right=CAN{right_id} "
+            f"vesc_can_odometry: left=CAN{left_id}({left_topic}) "
+            f"right=CAN{right_id}({right_topic}) "
             f"r={self._odom.wheel_radius}m sep={self._odom.wheel_separation}m "
             f"poles={self._odom.motor_poles} invert_right={self._odom.invert_right} "
             f"{self._odom_frame}→{self._base_frame} @ {freq:.0f}Hz"

jetson/ros2_ws/src/saltybot_nav2_slam/setup.py

@@ -14,6 +14,7 @@ setup(
             "launch/nav2_slam_integrated.launch.py",
             "launch/depth_to_costmap.launch.py",
             "launch/odometry_bridge.launch.py",
+            "launch/nav2_amcl_bringup.launch.py",
         ]),
         (f"share/{package_name}/config", [
             "config/nav2_params.yaml",
@@ -23,6 +24,11 @@ setup(
             "config/local_costmap_params.yaml",
             "config/dwb_local_planner_params.yaml",
             "config/vesc_odometry_params.yaml",
+            "config/amcl_nav2_params.yaml",
+        ]),
+        (f"share/{package_name}/maps", [
+            "maps/saltybot_map.yaml",
+            "maps/saltybot_map.pgm",
         ]),
     ],
     install_requires=["setuptools"],

jetson/ros2_ws/src/saltybot_nav2_slam/test/test_nav2_amcl.py

@@ -0,0 +1,338 @@
+"""
+test_nav2_amcl.py — Unit tests for Nav2 AMCL integration (Issue #655).
+
+Tests run without a ROS2 installation:
+  - amcl_nav2_params.yaml: parse, required sections, value ranges
+  - saltybot_map.yaml:     parse, required keys, valid geometry
+  - saltybot_map.pgm:      valid PGM header, correct dimensions
+  - nav2_amcl_bringup.launch.py: import (syntax) test
+"""
+
+import os
+import struct
+
+import pytest
+import yaml
+
+# ── Paths ─────────────────────────────────────────────────────────────────────
+_PKG_ROOT = os.path.join(os.path.dirname(__file__), "..")
+_CFG  = os.path.join(_PKG_ROOT, "config", "amcl_nav2_params.yaml")
+_MAP_YAML = os.path.join(_PKG_ROOT, "maps", "saltybot_map.yaml")
+_MAP_PGM  = os.path.join(_PKG_ROOT, "maps", "saltybot_map.pgm")
+_LAUNCH   = os.path.join(_PKG_ROOT, "launch", "nav2_amcl_bringup.launch.py")
+
+
+# ── amcl_nav2_params.yaml ─────────────────────────────────────────────────────
+
+def _load_params():
+    with open(_CFG) as f:
+        return yaml.safe_load(f)
+
+
+def test_params_file_exists():
+    assert os.path.isfile(_CFG)
+
+
+def test_params_top_level_sections():
+    params = _load_params()
+    required = [
+        "amcl",
+        "map_server",
+        "bt_navigator",
+        "controller_server",
+        "planner_server",
+        "behavior_server",
+        "global_costmap",
+        "local_costmap",
+        "velocity_smoother",
+        "lifecycle_manager_localization",
+        "lifecycle_manager_navigation",
+    ]
+    for section in required:
+        assert section in params, f"Missing section: {section}"
+
+
+def test_amcl_params_present():
+    amcl = _load_params()["amcl"]["ros__parameters"]
+    for key in ("min_particles", "max_particles", "alpha1", "alpha2",
+                "alpha3", "alpha4", "laser_model_type", "global_frame_id",
+                "odom_frame_id", "base_frame_id", "robot_model_type"):
+        assert key in amcl, f"Missing amcl param: {key}"
+
+
+def test_amcl_particle_bounds():
+    amcl = _load_params()["amcl"]["ros__parameters"]
+    assert amcl["min_particles"] >= 100
+    assert amcl["max_particles"] >= amcl["min_particles"]
+    assert amcl["max_particles"] <= 10000
+
+
+def test_amcl_frames():
+    amcl = _load_params()["amcl"]["ros__parameters"]
+    assert amcl["global_frame_id"] == "map"
+    assert amcl["odom_frame_id"]   == "odom"
+    assert amcl["base_frame_id"]   == "base_link"
+
+
+def test_amcl_motion_model():
+    amcl = _load_params()["amcl"]["ros__parameters"]
+    for key in ("alpha1", "alpha2", "alpha3", "alpha4"):
+        assert 0.0 <= amcl[key] <= 1.0, f"{key} out of [0,1]"
+
+
+def test_amcl_laser_model():
+    amcl = _load_params()["amcl"]["ros__parameters"]
+    assert amcl["laser_model_type"] in ("likelihood_field", "beam", "likelihood_field_prob")
+    z_sum = amcl["z_hit"] + amcl["z_rand"] + amcl["z_max"] + amcl["z_short"]
+    assert abs(z_sum - 1.0) < 1e-3, f"z weights sum {z_sum:.4f} ≠ 1.0"
+
+
+def test_controller_server_params():
+    ctrl = _load_params()["controller_server"]["ros__parameters"]
+    assert ctrl["controller_frequency"] > 0
+    follow = ctrl["FollowPath"]
+    assert follow["max_vel_x"] > 0
+    assert follow["max_vel_x"] <= 10.0    # sanity cap
+    assert follow["max_vel_theta"] > 0
+    assert follow["min_vel_y"] == 0.0     # differential drive: no lateral
+    assert follow["vy_samples"] == 1      # differential drive
+
+
+def test_planner_server_params():
+    planner = _load_params()["planner_server"]["ros__parameters"]
+    assert "GridBased" in planner["planner_plugins"]
+    grid = planner["GridBased"]
+    assert "NavfnPlanner" in grid["plugin"]
+    assert grid["use_astar"] is True
+
+
+def test_behavior_server_recoveries():
+    behaviors = _load_params()["behavior_server"]["ros__parameters"]
+    plugins = behaviors["behavior_plugins"]
+    # Issue #655 requires: spin, backup, wait
+    for required in ("spin", "backup", "wait"):
+        assert required in plugins, f"Recovery behavior '{required}' missing"
+    # Check each has a plugin entry
+    for name in plugins:
+        assert name in behaviors, f"behavior_plugins entry '{name}' has no config block"
+        assert "plugin" in behaviors[name]
+
+
+def test_global_costmap_layers():
+    gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
+    plugins = gcm["plugins"]
+    for layer in ("static_layer", "obstacle_layer", "inflation_layer"):
+        assert layer in plugins, f"Global costmap missing layer: {layer}"
+    # Each layer has plugin field
+    for layer in plugins:
+        assert layer in gcm, f"Layer {layer} has no config block"
+        assert "plugin" in gcm[layer]
+
+
+def test_global_costmap_scan_source():
+    gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
+    obs = gcm["obstacle_layer"]
+    assert "scan" in obs["observation_sources"]
+    assert obs["scan"]["topic"] == "/scan"
+    assert obs["scan"]["data_type"] == "LaserScan"
+
+
+def test_local_costmap_layers():
+    lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
+    plugins = lcm["plugins"]
+    # Local costmap: obstacle + inflation (no static — rolling window)
+    assert "obstacle_layer" in plugins
+    assert "inflation_layer" in plugins
+    assert lcm["rolling_window"] is True
+
+
+def test_local_costmap_size():
+    lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
+    assert lcm["width"]  >= 2
+    assert lcm["height"] >= 2
+    assert lcm["resolution"] == pytest.approx(0.05, abs=1e-6)
+
+
+def test_costmap_frames():
+    gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
+    lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
+    assert gcm["global_frame"]     == "map"
+    assert gcm["robot_base_frame"] == "base_link"
+    assert lcm["global_frame"]     == "odom"   # local uses odom frame
+    assert lcm["robot_base_frame"] == "base_link"
+
+
+def test_inflation_radius_larger_than_robot():
+    gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
+    lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
+    robot_r = gcm["robot_radius"]
+    g_infl  = gcm["inflation_layer"]["inflation_radius"]
+    l_infl  = lcm["inflation_layer"]["inflation_radius"]
+    assert g_infl > robot_r, "Global inflation_radius must exceed robot_radius"
+    assert l_infl > robot_r, "Local inflation_radius must exceed robot_radius"
+
+
+def test_lifecycle_manager_localization_nodes():
+    lm = _load_params()["lifecycle_manager_localization"]["ros__parameters"]
+    assert "map_server" in lm["node_names"]
+    assert "amcl"       in lm["node_names"]
+
+
+def test_lifecycle_manager_navigation_nodes():
+    lm = _load_params()["lifecycle_manager_navigation"]["ros__parameters"]
+    for node in ("controller_server", "planner_server", "behavior_server", "bt_navigator"):
+        assert node in lm["node_names"], f"lifecycle nav missing: {node}"
+
+
+def test_velocity_smoother_limits_consistent():
+    vs = _load_params()["velocity_smoother"]["ros__parameters"]
+    ctrl = _load_params()["controller_server"]["ros__parameters"]
+    max_vx_ctrl = ctrl["FollowPath"]["max_vel_x"]
+    max_vx_vs   = vs["max_velocity"][0]
+    # Smoother limit should be >= planner limit (or equal)
+    assert max_vx_vs >= max_vx_ctrl - 1e-3, (
+        f"velocity_smoother max_velocity[0]={max_vx_vs} < "
+        f"controller max_vel_x={max_vx_ctrl}"
+    )
+
+
+# ── saltybot_map.yaml ─────────────────────────────────────────────────────────
+
+def _load_map_yaml():
+    with open(_MAP_YAML) as f:
+        return yaml.safe_load(f)
+
+
+def test_map_yaml_exists():
+    assert os.path.isfile(_MAP_YAML)
+
+
+def test_map_yaml_required_keys():
+    m = _load_map_yaml()
+    for key in ("image", "resolution", "origin", "negate",
+                "occupied_thresh", "free_thresh"):
+        assert key in m, f"Map YAML missing key: {key}"
+
+
+def test_map_yaml_thresholds():
+    m = _load_map_yaml()
+    assert 0.0 < m["free_thresh"] < m["occupied_thresh"] < 1.0
+
+
+def test_map_yaml_resolution():
+    m = _load_map_yaml()
+    assert m["resolution"] == pytest.approx(0.05, abs=1e-6)
+
+
+def test_map_yaml_origin_3d():
+    m = _load_map_yaml()
+    assert len(m["origin"]) == 3   # [x, y, yaw]
+
+
+def test_map_yaml_image_name():
+    m = _load_map_yaml()
+    assert m["image"].endswith(".pgm")
+
+
+# ── saltybot_map.pgm ─────────────────────────────────────────────────────────
+
+def test_pgm_exists():
+    assert os.path.isfile(_MAP_PGM)
+
+
+def test_pgm_valid_header():
+    """P5 binary PGM: header 'P5\\nW H\\nMAXVAL\\n'."""
+    with open(_MAP_PGM, "rb") as f:
+        magic = f.readline().strip()
+    assert magic == b"P5", f"Expected P5 PGM, got {magic}"
+
+
+def test_pgm_dimensions():
+    """Parse width and height from PGM header; verify > 0."""
+    with open(_MAP_PGM, "rb") as f:
+        f.readline()   # P5
+        wh_line = f.readline()
+    w_str, h_str = wh_line.decode().split()
+    w, h = int(w_str), int(h_str)
+    assert w > 0 and h > 0
+    assert w == 200 and h == 200
+
+
+def test_pgm_pixel_count():
+    """File size should match header dimensions + header bytes."""
+    with open(_MAP_PGM, "rb") as f:
+        lines = []
+        for _ in range(3):
+            lines.append(f.readline())
+        payload = f.read()
+    w, h = map(int, lines[1].decode().split())
+    assert len(payload) == w * h, (
+        f"Expected {w*h} pixels, got {len(payload)}"
+    )
+
+
+def test_pgm_all_free_space():
+    """All pixels should be 254 (near-white = free)."""
+    with open(_MAP_PGM, "rb") as f:
+        for _ in range(3):
+            f.readline()
+        data = f.read()
+    assert all(b == 254 for b in data), "Not all pixels are free (254)"
+
+
+# ── Launch file syntax check ──────────────────────────────────────────────────
+
+def test_launch_file_exists():
+    assert os.path.isfile(_LAUNCH)
+
+
+def test_launch_file_imports_cleanly():
+    """Compile launch file to check for Python syntax errors."""
+    import py_compile
+    py_compile.compile(_LAUNCH, doraise=True)
+
+
+def test_launch_file_has_generate_function():
+    """Launch file must define generate_launch_description()."""
+    with open(_LAUNCH) as f:
+        src = f.read()
+    assert "def generate_launch_description" in src
+
+
+def test_launch_file_references_amcl_params():
+    """Launch file must reference amcl_nav2_params.yaml."""
+    with open(_LAUNCH) as f:
+        src = f.read()
+    assert "amcl_nav2_params.yaml" in src
+
+
+def test_launch_file_references_odometry_bridge():
+    """Launch file must wire in VESC odometry bridge."""
+    with open(_LAUNCH) as f:
+        src = f.read()
+    assert "odometry_bridge.launch.py" in src
+
+
+def test_launch_file_references_localization():
+    """Launch file must include Nav2 localization_launch.py."""
+    with open(_LAUNCH) as f:
+        src = f.read()
+    assert "localization_launch.py" in src
+
+
+def test_launch_file_references_navigation():
+    """Launch file must include Nav2 navigation_launch.py."""
+    with open(_LAUNCH) as f:
+        src = f.read()
+    assert "navigation_launch.py" in src
+
+
+def test_launch_file_has_map_argument():
+    """Launch file must expose 'map' argument for map file path."""
+    with open(_LAUNCH) as f:
+        src = f.read()
+    assert '"map"' in src or "'map'" in src
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_phone/saltybot_phone/vesc_mqtt_relay_node.py

@@ -0,0 +1,305 @@
+#!/usr/bin/env python3
+"""
+vesc_mqtt_relay_node.py — ROS2 → MQTT relay for VESC CAN telemetry (Issue #656)
+
+Subscribes to VESC telemetry ROS2 topics and republishes as MQTT JSON payloads
+for the sensor dashboard.  Each per-motor topic is rate-limited to 5 Hz.
+
+ROS2 topics consumed
+─────────────────────
+  /vesc/left/state   std_msgs/String  JSON from vesc_telemetry_node
+  /vesc/right/state  std_msgs/String  JSON from vesc_telemetry_node
+  /vesc/combined     std_msgs/String  JSON from vesc_telemetry_node
+
+MQTT topics published
+──────────────────────
+  saltybot/phone/vesc_left      — per-motor telemetry (left)
+  saltybot/phone/vesc_right     — per-motor telemetry (right)
+  saltybot/phone/vesc_combined  — combined voltage + total current + RPMs
+
+MQTT payload (per-motor)
+────────────────────────
+  {
+    "rpm":          int,
+    "current_a":    float,   # phase current
+    "voltage_v":    float,   # bus voltage
+    "temperature_c": float,  # FET temperature
+    "duty_cycle":   float,   # -1.0 … 1.0
+    "fault_code":   int,
+    "ts":           float    # epoch seconds
+  }
+
+Parameters
+──────────
+  mqtt_host          str   Broker IP/hostname         (default: localhost)
+  mqtt_port          int   Broker port                (default: 1883)
+  mqtt_keepalive     int   Keepalive seconds          (default: 60)
+  reconnect_delay_s  float Delay between reconnects   (default: 5.0)
+  motor_rate_hz      float Max publish rate per motor (default: 5.0)
+
+Dependencies
+────────────
+  pip install paho-mqtt
+"""
+
+import json
+import time
+from typing import Any
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+try:
+    import paho.mqtt.client as mqtt
+    _MQTT_OK = True
+except ImportError:
+    _MQTT_OK = False
+
+# ── MQTT topic constants ──────────────────────────────────────────────────────
+
+_MQTT_VESC_LEFT     = "saltybot/phone/vesc_left"
+_MQTT_VESC_RIGHT    = "saltybot/phone/vesc_right"
+_MQTT_VESC_COMBINED = "saltybot/phone/vesc_combined"
+
+# ── ROS2 topic constants ──────────────────────────────────────────────────────
+
+_ROS_VESC_LEFT     = "/vesc/left/state"
+_ROS_VESC_RIGHT    = "/vesc/right/state"
+_ROS_VESC_COMBINED = "/vesc/combined"
+
+
+def _extract_motor_payload(data: dict) -> dict:
+    """
+    Extract the required fields from a vesc_telemetry_node per-motor JSON dict.
+
+    Upstream JSON keys (from vesc_telemetry_node._state_to_dict):
+      rpm, current_a, voltage_v, temp_fet_c, duty_cycle, fault_code, ...
+
+    Returns a dashboard-friendly payload with a stable key set.
+    """
+    return {
+        "rpm":           int(data["rpm"]),
+        "current_a":     float(data["current_a"]),
+        "voltage_v":     float(data["voltage_v"]),
+        "temperature_c": float(data["temp_fet_c"]),
+        "duty_cycle":    float(data["duty_cycle"]),
+        "fault_code":    int(data["fault_code"]),
+        "ts":            float(data.get("stamp", time.time())),
+    }
+
+
+def _extract_combined_payload(data: dict) -> dict:
+    """
+    Extract fields from the /vesc/combined JSON dict.
+
+    Upstream keys: voltage_v, total_current_a, left_rpm, right_rpm,
+                   left_alive, right_alive, stamp
+    """
+    return {
+        "voltage_v":       float(data["voltage_v"]),
+        "total_current_a": float(data["total_current_a"]),
+        "left_rpm":        int(data["left_rpm"]),
+        "right_rpm":       int(data["right_rpm"]),
+        "left_alive":      bool(data["left_alive"]),
+        "right_alive":     bool(data["right_alive"]),
+        "ts":              float(data.get("stamp", time.time())),
+    }
+
+
+class VescMqttRelayNode(Node):
+    """
+    Subscribes to VESC ROS2 topics and relays telemetry to MQTT.
+
+    Rate limiting: each per-motor topic maintains a last-publish timestamp;
+    messages arriving faster than motor_rate_hz are silently dropped.
+    The /vesc/combined topic is also rate-limited at the same rate.
+    """
+
+    def __init__(self) -> None:
+        super().__init__("vesc_mqtt_relay")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("mqtt_host",         "localhost")
+        self.declare_parameter("mqtt_port",         1883)
+        self.declare_parameter("mqtt_keepalive",    60)
+        self.declare_parameter("reconnect_delay_s", 5.0)
+        self.declare_parameter("motor_rate_hz",     5.0)
+
+        self._mqtt_host        = self.get_parameter("mqtt_host").value
+        self._mqtt_port        = self.get_parameter("mqtt_port").value
+        self._mqtt_keepalive   = self.get_parameter("mqtt_keepalive").value
+        reconnect_delay        = self.get_parameter("reconnect_delay_s").value
+        rate_hz                = max(0.1, float(self.get_parameter("motor_rate_hz").value))
+        self._min_interval     = 1.0 / rate_hz
+
+        if not _MQTT_OK:
+            self.get_logger().error(
+                "paho-mqtt not installed — run: pip install paho-mqtt"
+            )
+
+        # ── Rate-limit state (last publish epoch per MQTT topic) ──────────────
+        self._last_pub: dict[str, float] = {
+            _MQTT_VESC_LEFT:     0.0,
+            _MQTT_VESC_RIGHT:    0.0,
+            _MQTT_VESC_COMBINED: 0.0,
+        }
+
+        # ── Stats ─────────────────────────────────────────────────────────────
+        self._rx_count:    dict[str, int] = {
+            _MQTT_VESC_LEFT: 0, _MQTT_VESC_RIGHT: 0, _MQTT_VESC_COMBINED: 0
+        }
+        self._pub_count:   dict[str, int] = {
+            _MQTT_VESC_LEFT: 0, _MQTT_VESC_RIGHT: 0, _MQTT_VESC_COMBINED: 0
+        }
+        self._drop_count:  dict[str, int] = {
+            _MQTT_VESC_LEFT: 0, _MQTT_VESC_RIGHT: 0, _MQTT_VESC_COMBINED: 0
+        }
+        self._err_count:   dict[str, int] = {
+            _MQTT_VESC_LEFT: 0, _MQTT_VESC_RIGHT: 0, _MQTT_VESC_COMBINED: 0
+        }
+        self._mqtt_connected = False
+
+        # ── ROS2 subscriptions ────────────────────────────────────────────────
+        self.create_subscription(
+            String, _ROS_VESC_LEFT,
+            lambda msg: self._on_vesc(msg, _MQTT_VESC_LEFT, _extract_motor_payload),
+            10,
+        )
+        self.create_subscription(
+            String, _ROS_VESC_RIGHT,
+            lambda msg: self._on_vesc(msg, _MQTT_VESC_RIGHT, _extract_motor_payload),
+            10,
+        )
+        self.create_subscription(
+            String, _ROS_VESC_COMBINED,
+            lambda msg: self._on_vesc(msg, _MQTT_VESC_COMBINED, _extract_combined_payload),
+            10,
+        )
+
+        # ── MQTT client ───────────────────────────────────────────────────────
+        self._mqtt_client: "mqtt.Client | None" = None
+        if _MQTT_OK:
+            self._mqtt_client = mqtt.Client(
+                client_id="saltybot-vesc-mqtt-relay", clean_session=True
+            )
+            self._mqtt_client.on_connect    = self._on_mqtt_connect
+            self._mqtt_client.on_disconnect = self._on_mqtt_disconnect
+            self._mqtt_client.reconnect_delay_set(
+                min_delay=int(reconnect_delay),
+                max_delay=int(reconnect_delay) * 4,
+            )
+            self._mqtt_connect()
+
+        self.get_logger().info(
+            "VESC→MQTT relay started — broker=%s:%d  rate=%.1f Hz",
+            self._mqtt_host, self._mqtt_port, rate_hz,
+        )
+
+    # ── MQTT connection ───────────────────────────────────────────────────────
+
+    def _mqtt_connect(self) -> None:
+        try:
+            self._mqtt_client.connect_async(
+                self._mqtt_host, self._mqtt_port,
+                keepalive=self._mqtt_keepalive,
+            )
+            self._mqtt_client.loop_start()
+        except Exception as exc:
+            self.get_logger().warning("MQTT initial connect error: %s", str(exc))
+
+    def _on_mqtt_connect(self, client, userdata, flags, rc) -> None:
+        if rc == 0:
+            self._mqtt_connected = True
+            self.get_logger().info(
+                "MQTT connected to %s:%d", self._mqtt_host, self._mqtt_port
+            )
+        else:
+            self.get_logger().warning("MQTT connect failed rc=%d", rc)
+
+    def _on_mqtt_disconnect(self, client, userdata, rc) -> None:
+        self._mqtt_connected = False
+        if rc != 0:
+            self.get_logger().warning(
+                "MQTT disconnected (rc=%d) — paho will reconnect", rc
+            )
+
+    # ── ROS2 subscriber callback ──────────────────────────────────────────────
+
+    def _on_vesc(self, ros_msg: String, mqtt_topic: str, extractor) -> None:
+        """
+        Handle an incoming VESC ROS2 message.
+
+        1. Parse JSON from the String payload.
+        2. Check rate limit — drop if too soon.
+        3. Extract dashboard fields.
+        4. Publish to MQTT.
+        """
+        self._rx_count[mqtt_topic] += 1
+
+        # Rate limit
+        now = time.monotonic()
+        if now - self._last_pub[mqtt_topic] < self._min_interval:
+            self._drop_count[mqtt_topic] += 1
+            return
+
+        # Parse
+        try:
+            data = json.loads(ros_msg.data)
+        except (json.JSONDecodeError, UnicodeDecodeError) as exc:
+            self._err_count[mqtt_topic] += 1
+            self.get_logger().debug("JSON error on %s: %s", mqtt_topic, exc)
+            return
+
+        # Extract
+        try:
+            payload = extractor(data)
+        except (KeyError, TypeError, ValueError) as exc:
+            self._err_count[mqtt_topic] += 1
+            self.get_logger().debug("Payload error on %s: %s — %s", mqtt_topic, exc, data)
+            return
+
+        # Publish
+        if self._mqtt_client is not None and self._mqtt_connected:
+            try:
+                self._mqtt_client.publish(
+                    mqtt_topic,
+                    json.dumps(payload, separators=(",", ":")),
+                    qos=0,
+                    retain=False,
+                )
+                self._last_pub[mqtt_topic] = now
+                self._pub_count[mqtt_topic] += 1
+            except Exception as exc:
+                self._err_count[mqtt_topic] += 1
+                self.get_logger().debug("MQTT publish error on %s: %s", mqtt_topic, exc)
+        else:
+            # Still update last_pub to avoid log spam when broker is down
+            self._last_pub[mqtt_topic] = now
+            self.get_logger().debug("MQTT not connected — dropped %s", mqtt_topic)
+
+    # ── Cleanup ───────────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        if self._mqtt_client is not None:
+            self._mqtt_client.loop_stop()
+            self._mqtt_client.disconnect()
+        super().destroy_node()
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args: Any = None) -> None:
+    rclpy.init(args=args)
+    node = VescMqttRelayNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_phone/setup.py

@@ -30,6 +30,7 @@ setup(
             'phone_bridge = saltybot_phone.ws_bridge:main',
             'phone_camera_node = saltybot_phone.phone_camera_node:main',
             'mqtt_ros2_bridge = saltybot_phone.mqtt_ros2_bridge_node:main',
+            'vesc_mqtt_relay = saltybot_phone.vesc_mqtt_relay_node:main',
         ],
     },
 )

jetson/ros2_ws/src/saltybot_phone/test/test_vesc_mqtt_relay.py

@@ -0,0 +1,343 @@
+"""Unit tests for vesc_mqtt_relay_node — pure-logic helpers.
+
+Does not require ROS2, paho-mqtt, or a live VESC/broker.
+Tests cover the two payload extractors and the rate-limiting logic.
+"""
+
+import json
+import sys
+import time
+import types
+
+# ── Stub out ROS2 / paho so the module can be imported without them ───────────
+
+def _make_rclpy_stub():
+    rclpy = types.ModuleType("rclpy")
+    node_mod = types.ModuleType("rclpy.node")
+
+    class _Node:
+        def __init__(self, *a, **kw): pass
+        def declare_parameter(self, *a, **kw): pass
+        def get_parameter(self, name):
+            class _P:
+                value = None
+            return _P()
+        def create_subscription(self, *a, **kw): pass
+        def create_publisher(self, *a, **kw): pass
+        def create_timer(self, *a, **kw): pass
+        def get_clock(self): return None
+        def get_logger(self):
+            class _L:
+                def info(self, *a, **kw): pass
+                def warning(self, *a, **kw): pass
+                def error(self, *a, **kw): pass
+                def debug(self, *a, **kw): pass
+            return _L()
+        def destroy_node(self): pass
+
+    node_mod.Node = _Node
+    rclpy.node = node_mod
+    rclpy.init = lambda *a, **kw: None
+    rclpy.spin = lambda *a, **kw: None
+    rclpy.try_shutdown = lambda *a, **kw: None
+
+    std_msgs_mod   = types.ModuleType("std_msgs")
+    std_msgs_msg   = types.ModuleType("std_msgs.msg")
+    class _String:
+        data: str = ""
+    std_msgs_msg.String = _String
+    std_msgs_mod.msg = std_msgs_msg
+
+    paho_mod   = types.ModuleType("paho")
+    paho_mqtt  = types.ModuleType("paho.mqtt")
+    paho_client = types.ModuleType("paho.mqtt.client")
+    class _Client:
+        def __init__(self, *a, **kw): pass
+        def connect_async(self, *a, **kw): pass
+        def loop_start(self): pass
+        def loop_stop(self): pass
+        def disconnect(self): pass
+        def publish(self, *a, **kw): pass
+        def reconnect_delay_set(self, *a, **kw): pass
+        on_connect    = None
+        on_disconnect = None
+    paho_client.Client = _Client
+    paho_mqtt.client = paho_client
+    paho_mod.mqtt = paho_mqtt
+
+    for name, mod in [
+        ("rclpy", rclpy),
+        ("rclpy.node", node_mod),
+        ("std_msgs", std_msgs_mod),
+        ("std_msgs.msg", std_msgs_msg),
+        ("paho", paho_mod),
+        ("paho.mqtt", paho_mqtt),
+        ("paho.mqtt.client", paho_client),
+    ]:
+        sys.modules.setdefault(name, mod)
+
+
+_make_rclpy_stub()
+
+from saltybot_phone.vesc_mqtt_relay_node import (
+    _MQTT_VESC_LEFT,
+    _MQTT_VESC_RIGHT,
+    _MQTT_VESC_COMBINED,
+    _extract_combined_payload,
+    _extract_motor_payload,
+)
+
+
+# ---------------------------------------------------------------------------
+# _extract_motor_payload
+# ---------------------------------------------------------------------------
+
+class TestExtractMotorPayload:
+    """Covers field extraction and type coercion for per-motor JSON."""
+
+    def _sample(self, **overrides):
+        base = {
+            "can_id":       61,
+            "rpm":          1500,
+            "current_a":    12.34,
+            "current_in_a": 10.0,
+            "duty_cycle":   0.4500,
+            "voltage_v":    25.20,
+            "temp_fet_c":   45.5,
+            "temp_motor_c": 62.0,
+            "fault_code":   0,
+            "fault_name":   "NONE",
+            "alive":        True,
+            "stamp":        1000.0,
+        }
+        base.update(overrides)
+        return base
+
+    def test_required_keys_present(self):
+        p = _extract_motor_payload(self._sample())
+        for key in ("rpm", "current_a", "voltage_v", "temperature_c",
+                    "duty_cycle", "fault_code", "ts"):
+            assert key in p, f"missing key: {key}"
+
+    def test_rpm_is_int(self):
+        p = _extract_motor_payload(self._sample(rpm=1500))
+        assert isinstance(p["rpm"], int)
+        assert p["rpm"] == 1500
+
+    def test_temperature_maps_to_temp_fet_c(self):
+        p = _extract_motor_payload(self._sample(temp_fet_c=55.5))
+        assert p["temperature_c"] == 55.5
+
+    def test_voltage_v(self):
+        p = _extract_motor_payload(self._sample(voltage_v=24.8))
+        assert p["voltage_v"] == 24.8
+
+    def test_duty_cycle(self):
+        p = _extract_motor_payload(self._sample(duty_cycle=0.75))
+        assert p["duty_cycle"] == 0.75
+
+    def test_fault_code_zero(self):
+        p = _extract_motor_payload(self._sample(fault_code=0))
+        assert p["fault_code"] == 0
+
+    def test_fault_code_nonzero(self):
+        p = _extract_motor_payload(self._sample(fault_code=3))
+        assert p["fault_code"] == 3
+
+    def test_ts_from_stamp(self):
+        p = _extract_motor_payload(self._sample(stamp=12345.678))
+        assert p["ts"] == 12345.678
+
+    def test_negative_rpm(self):
+        p = _extract_motor_payload(self._sample(rpm=-3000))
+        assert p["rpm"] == -3000
+
+    def test_negative_current(self):
+        p = _extract_motor_payload(self._sample(current_a=-5.0))
+        assert p["current_a"] == -5.0
+
+    def test_negative_duty_cycle(self):
+        p = _extract_motor_payload(self._sample(duty_cycle=-0.5))
+        assert p["duty_cycle"] == -0.5
+
+    def test_missing_required_key_raises(self):
+        bad = self._sample()
+        del bad["rpm"]
+        try:
+            _extract_motor_payload(bad)
+            assert False, "expected KeyError"
+        except KeyError:
+            pass
+
+    def test_missing_stamp_uses_current_time(self):
+        data = self._sample()
+        del data["stamp"]
+        before = time.time()
+        p = _extract_motor_payload(data)
+        after = time.time()
+        assert before <= p["ts"] <= after
+
+    def test_json_roundtrip(self):
+        p = _extract_motor_payload(self._sample())
+        raw = json.dumps(p)
+        restored = json.loads(raw)
+        assert restored["rpm"] == p["rpm"]
+        assert restored["fault_code"] == p["fault_code"]
+
+
+# ---------------------------------------------------------------------------
+# _extract_combined_payload
+# ---------------------------------------------------------------------------
+
+class TestExtractCombinedPayload:
+    def _sample(self, **overrides):
+        base = {
+            "voltage_v":       25.2,
+            "total_current_a": 18.5,
+            "left_rpm":        1400,
+            "right_rpm":       1420,
+            "left_alive":      True,
+            "right_alive":     True,
+            "stamp":           2000.0,
+        }
+        base.update(overrides)
+        return base
+
+    def test_required_keys_present(self):
+        p = _extract_combined_payload(self._sample())
+        for key in ("voltage_v", "total_current_a", "left_rpm", "right_rpm",
+                    "left_alive", "right_alive", "ts"):
+            assert key in p, f"missing key: {key}"
+
+    def test_voltage_v(self):
+        p = _extract_combined_payload(self._sample(voltage_v=24.0))
+        assert p["voltage_v"] == 24.0
+
+    def test_total_current_a(self):
+        p = _extract_combined_payload(self._sample(total_current_a=30.5))
+        assert p["total_current_a"] == 30.5
+
+    def test_rpms_are_int(self):
+        p = _extract_combined_payload(self._sample(left_rpm=1000, right_rpm=1050))
+        assert isinstance(p["left_rpm"], int)
+        assert isinstance(p["right_rpm"], int)
+
+    def test_alive_flags(self):
+        p = _extract_combined_payload(self._sample(left_alive=True, right_alive=False))
+        assert p["left_alive"] is True
+        assert p["right_alive"] is False
+
+    def test_ts_from_stamp(self):
+        p = _extract_combined_payload(self._sample(stamp=9999.1))
+        assert p["ts"] == 9999.1
+
+    def test_missing_stamp_uses_current_time(self):
+        data = self._sample()
+        del data["stamp"]
+        before = time.time()
+        p = _extract_combined_payload(data)
+        after = time.time()
+        assert before <= p["ts"] <= after
+
+    def test_json_roundtrip(self):
+        p = _extract_combined_payload(self._sample())
+        raw = json.dumps(p)
+        restored = json.loads(raw)
+        assert restored["voltage_v"] == p["voltage_v"]
+
+
+# ---------------------------------------------------------------------------
+# Rate-limit logic (isolated, no ROS2 / paho)
+# ---------------------------------------------------------------------------
+
+class TestRateLimit:
+    """
+    Exercise the rate-limiting gate that lives inside VescMqttRelayNode._on_vesc.
+    We test the guard condition directly without instantiating the ROS2 node.
+    """
+
+    def _make_gate(self, rate_hz: float):
+        """
+        Return a stateful callable that mirrors the rate-limit check in the node.
+        Returns True if a publish should proceed, False if the message is dropped.
+        """
+        min_interval = 1.0 / rate_hz
+        state = {"last": 0.0}
+
+        def gate(now: float) -> bool:
+            if now - state["last"] < min_interval:
+                return False
+            state["last"] = now
+            return True
+
+        return gate
+
+    def test_first_message_always_passes(self):
+        gate = self._make_gate(5.0)
+        assert gate(time.monotonic()) is True
+
+    def test_immediate_second_message_dropped(self):
+        gate = self._make_gate(5.0)
+        t = time.monotonic()
+        gate(t)
+        assert gate(t + 0.001) is False  # 1 ms < 200 ms interval
+
+    def test_message_after_interval_passes(self):
+        gate = self._make_gate(5.0)
+        t = time.monotonic()
+        gate(t)
+        assert gate(t + 0.201) is True  # 201 ms > 200 ms interval
+
+    def test_exactly_at_interval_dropped(self):
+        gate = self._make_gate(5.0)
+        t = time.monotonic()
+        gate(t)
+        # Exactly at the boundary is strictly less-than, so it's dropped
+        assert gate(t + 0.2) is False
+
+    def test_10hz_rate(self):
+        gate = self._make_gate(10.0)
+        t = time.monotonic()
+        gate(t)
+        assert gate(t + 0.09) is False   # 90 ms < 100 ms
+        assert gate(t + 0.101) is True   # 101 ms > 100 ms
+
+    def test_1hz_rate(self):
+        gate = self._make_gate(1.0)
+        t = time.monotonic()
+        gate(t)
+        assert gate(t + 0.999) is False
+        assert gate(t + 1.001) is True
+
+    def test_multiple_topics_independent(self):
+        gate_left  = self._make_gate(5.0)
+        gate_right = self._make_gate(5.0)
+        t = time.monotonic()
+        gate_left(t)
+        gate_right(t)
+        # left: drop, right: drop
+        assert gate_left(t + 0.05) is False
+        assert gate_right(t + 0.05) is False
+        # advance only left past interval
+        assert gate_left(t + 0.21) is True
+        assert gate_right(t + 0.21) is True
+
+
+# ---------------------------------------------------------------------------
+# MQTT topic constant checks
+# ---------------------------------------------------------------------------
+
+class TestTopicConstants:
+    def test_left_topic(self):
+        assert _MQTT_VESC_LEFT == "saltybot/phone/vesc_left"
+
+    def test_right_topic(self):
+        assert _MQTT_VESC_RIGHT == "saltybot/phone/vesc_right"
+
+    def test_combined_topic(self):
+        assert _MQTT_VESC_COMBINED == "saltybot/phone/vesc_combined"
+
+
+if __name__ == "__main__":
+    import pytest
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_uwb_geofence/config/uwb_geofence_zones.yaml

@@ -0,0 +1,56 @@
+# UWB Geofence Zones Configuration
+# All coordinates are in the UWB local frame (metres, X-forward, Y-left)
+#
+# emergency_boundary: outer hard limit — robot must stay inside.
+#   If it exits, cmd_vel_limited is zeroed and /saltybot/geofence_violation
+#   is latched True (triggers e-stop cascade).
+#
+# zones: list of named speed-limit regions.
+#   vertices: flat [x1, y1, x2, y2, ...] polygon, at least 3 points.
+#   max_speed:   m/s  (linear)
+#   max_angular: rad/s (yaw)
+#   The most restrictive zone the robot is currently inside wins.
+#   Transitions between limits are smoothed over `transition_time` seconds.
+
+# ── Emergency boundary (6 m × 10 m space, centred at origin) ─────────────────
+emergency_boundary:
+  vertices:
+    [-6.0, -5.0,
+      6.0, -5.0,
+      6.0,  5.0,
+     -6.0,  5.0]
+
+# ── Named speed-limit zones ───────────────────────────────────────────────────
+zones:
+  # Caution perimeter — 1 m inside the emergency boundary
+  - name: caution_perimeter
+    description: "Slow near room walls (1 m buffer inside emergency boundary)"
+    max_speed: 0.4
+    max_angular: 0.8
+    vertices:
+      [-5.0, -4.0,
+        5.0, -4.0,
+        5.0,  4.0,
+       -5.0,  4.0]
+
+  # Dock approach — slow zone in front of the docking station (positive Y end)
+  - name: dock_approach
+    description: "Very slow approach corridor to docking station"
+    max_speed: 0.15
+    max_angular: 0.3
+    vertices:
+      [-0.75, 2.5,
+        0.75, 2.5,
+        0.75, 4.5,
+       -0.75, 4.5]
+
+  # Static obstacle cluster A (example — update to match your layout)
+  - name: obstacle_cluster_a
+    description: "Caution zone around obstacle cluster near (+2, -1)"
+    max_speed: 0.25
+    max_angular: 0.5
+    vertices:
+      [1.0, -2.0,
+       3.0, -2.0,
+       3.0,  0.0,
+       1.0,  0.0]

jetson/ros2_ws/src/saltybot_uwb_geofence/launch/uwb_geofence.launch.py

@@ -0,0 +1,47 @@
+"""Launch file for saltybot_uwb_geofence node."""
+
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg_share = str(
+        Path(__file__).parent.parent / "config" / "uwb_geofence_zones.yaml"
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "zones_file",
+                default_value=pkg_share,
+                description="Path to YAML zones config file",
+            ),
+            DeclareLaunchArgument(
+                "transition_time",
+                default_value="0.5",
+                description="Speed ramp time in seconds",
+            ),
+            DeclareLaunchArgument(
+                "frequency",
+                default_value="20.0",
+                description="Node update frequency in Hz",
+            ),
+            Node(
+                package="saltybot_uwb_geofence",
+                executable="uwb_geofence_node",
+                name="uwb_geofence",
+                output="screen",
+                parameters=[
+                    {
+                        "zones_file": LaunchConfiguration("zones_file"),
+                        "transition_time": LaunchConfiguration("transition_time"),
+                        "frequency": LaunchConfiguration("frequency"),
+                    }
+                ],
+            ),
+        ]
+    )

jetson/ros2_ws/src/saltybot_uwb_geofence/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_uwb_geofence</name>
+  <version>0.1.0</version>
+  <description>
+    UWB-position-based geofence speed limiter for SaltyBot (Issue #657).
+    Subscribes to /saltybot/pose/authoritative (UWB+IMU fused), enforces
+    configurable polygon speed zones, smooth transitions, and emergency
+    e-stop on boundary exit.
+  </description>
+  <maintainer email="sl-uwb@saltylab.local">SaltyLab UWB</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>python3-yaml</depend>
+
+  <test_depend>pytest</test_depend>
+  <test_depend>geometry_msgs</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/uwb_geofence_node.py

@@ -0,0 +1,296 @@
+#!/usr/bin/env python3
+"""UWB Geofence Speed Limiter for SaltyBot (Issue #657).
+
+Subscribes to:
+  /saltybot/pose/authoritative  geometry_msgs/PoseWithCovarianceStamped
+  /cmd_vel                      geometry_msgs/Twist
+
+Publishes:
+  /cmd_vel_limited              geometry_msgs/Twist   -- speed-limited output
+  /saltybot/geofence/status     std_msgs/String       -- JSON telemetry
+  /saltybot/geofence_violation  std_msgs/Bool         -- triggers e-stop cascade
+
+Zones are defined in a YAML file (zones_file parameter).  Each zone has a
+name, polygon vertices in UWB local frame (metres), max_speed (m/s) and
+max_angular (rad/s).  The most restrictive of all containing zones wins.
+
+An outer emergency_boundary polygon is mandatory.  If the robot exits it,
+all motion is zeroed and /saltybot/geofence_violation is latched True.
+
+Speed transitions are smoothed over `transition_time` seconds using a
+linear ramp so that passing through a zone boundary never causes a
+velocity spike.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+from pathlib import Path
+from typing import Any, Dict, List, Optional, Tuple
+
+import yaml
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from std_msgs.msg import Bool, String
+
+from saltybot_uwb_geofence.zone_checker import (
+    apply_speed_limit,
+    min_dist_to_boundary,
+    parse_flat_vertices,
+    point_in_polygon,
+    ramp_toward,
+)
+
+
+class UwbGeofenceNode(Node):
+    """ROS2 node enforcing UWB-position-based speed limits inside geofence zones."""
+
+    def __init__(self) -> None:
+        super().__init__("uwb_geofence")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter(
+            "zones_file",
+            str(Path(__file__).parent.parent.parent / "config" / "uwb_geofence_zones.yaml"),
+        )
+        self.declare_parameter("transition_time", 0.5)   # seconds
+        self.declare_parameter("frequency", 20.0)         # Hz
+        self.declare_parameter("default_max_speed", 1.0)  # m/s (outside all named zones)
+        self.declare_parameter("default_max_angular", 1.5)  # rad/s
+        self.declare_parameter("pose_topic", "/saltybot/pose/authoritative")
+        self.declare_parameter("cmd_vel_topic", "/cmd_vel")
+
+        zones_file = self.get_parameter("zones_file").value
+        self._transition_time: float = self.get_parameter("transition_time").value
+        frequency: float = self.get_parameter("frequency").value
+        self._default_max_speed: float = self.get_parameter("default_max_speed").value
+        self._default_max_angular: float = self.get_parameter("default_max_angular").value
+        pose_topic: str = self.get_parameter("pose_topic").value
+        cmd_vel_topic: str = self.get_parameter("cmd_vel_topic").value
+
+        # ── Load zones config ─────────────────────────────────────────────────
+        self._zones: List[Dict[str, Any]] = []
+        self._emergency_boundary: List[Tuple[float, float]] = []
+        self._load_zones(zones_file)
+
+        # ── Runtime state ─────────────────────────────────────────────────────
+        self._robot_x: float = 0.0
+        self._robot_y: float = 0.0
+        self._pose_received: bool = False
+        self._last_cmd_vel: Optional[Twist] = None
+
+        # Current smoothed speed limits (ramp toward target)
+        self._current_max_speed: float = self._default_max_speed
+        self._current_max_angular: float = self._default_max_angular
+
+        # E-stop latch (requires operator reset)
+        self._violation_latched: bool = False
+
+        # ── Subscriptions ─────────────────────────────────────────────────────
+        self.create_subscription(
+            PoseWithCovarianceStamped,
+            pose_topic,
+            self._on_pose,
+            10,
+        )
+        self.create_subscription(
+            Twist,
+            cmd_vel_topic,
+            self._on_cmd_vel,
+            10,
+        )
+
+        # ── Publications ──────────────────────────────────────────────────────
+        self._pub_limited = self.create_publisher(Twist, "/cmd_vel_limited", 10)
+        self._pub_status = self.create_publisher(String, "/saltybot/geofence/status", 10)
+        self._pub_violation = self.create_publisher(Bool, "/saltybot/geofence_violation", 1)
+
+        # ── Timer ─────────────────────────────────────────────────────────────
+        self._dt: float = 1.0 / frequency
+        self.create_timer(self._dt, self._tick)
+
+        self.get_logger().info(
+            f"uwb_geofence started: {len(self._zones)} zone(s), "
+            f"freq={frequency:.0f}Hz, transition={self._transition_time}s"
+        )
+
+    # ── Config loading ────────────────────────────────────────────────────────
+
+    def _load_zones(self, path: str) -> None:
+        """Parse YAML zones file and populate self._zones and self._emergency_boundary."""
+        p = Path(path)
+        if not p.exists():
+            self.get_logger().warn(f"zones_file not found: {path}; running with empty zones")
+            return
+
+        with open(p) as fh:
+            cfg = yaml.safe_load(fh)
+
+        # Emergency boundary (mandatory)
+        eb = cfg.get("emergency_boundary", {})
+        flat = eb.get("vertices", [])
+        if flat:
+            try:
+                self._emergency_boundary = parse_flat_vertices(flat)
+            except ValueError as exc:
+                self.get_logger().error(f"emergency_boundary vertices invalid: {exc}")
+        else:
+            self.get_logger().warn("No emergency_boundary defined — boundary checking disabled")
+
+        # Named speed zones
+        for z in cfg.get("zones", []):
+            try:
+                verts = parse_flat_vertices(z["vertices"])
+            except (KeyError, ValueError) as exc:
+                self.get_logger().error(f"Skipping zone '{z.get('name', '?')}': {exc}")
+                continue
+            self._zones.append(
+                {
+                    "name": z.get("name", "unnamed"),
+                    "description": z.get("description", ""),
+                    "max_speed": float(z.get("max_speed", self._default_max_speed)),
+                    "max_angular": float(z.get("max_angular", self._default_max_angular)),
+                    "vertices": verts,
+                }
+            )
+            self.get_logger().info(
+                f"  zone '{z['name']}': max_speed={z.get('max_speed')} m/s, "
+                f"{len(verts)} vertices"
+            )
+
+    # ── Callbacks ─────────────────────────────────────────────────────────────
+
+    def _on_pose(self, msg: PoseWithCovarianceStamped) -> None:
+        self._robot_x = msg.pose.pose.position.x
+        self._robot_y = msg.pose.pose.position.y
+        self._pose_received = True
+
+    def _on_cmd_vel(self, msg: Twist) -> None:
+        self._last_cmd_vel = msg
+
+    # ── Main loop ────────────────────────────────────────────────────────────
+
+    def _tick(self) -> None:
+        """Called at `frequency` Hz.  Compute limits, ramp, publish."""
+        if not self._pose_received or self._last_cmd_vel is None:
+            return
+
+        x, y = self._robot_x, self._robot_y
+
+        # ── Emergency boundary check ──────────────────────────────────────────
+        if self._emergency_boundary:
+            inside_boundary = point_in_polygon(x, y, self._emergency_boundary)
+            if not inside_boundary:
+                self._trigger_violation(x, y)
+                return
+
+        # Clear latch once back inside boundary (manual operator resume)
+        if self._violation_latched:
+            # Publish safe=False until operator clears via /saltybot/geofence_violation
+            # (this node keeps publishing zero until reset)
+            self._publish_zero_cmd()
+            self._pub_violation.publish(Bool(data=True))
+            return
+
+        # ── Zone classification ───────────────────────────────────────────────
+        active_zones: List[str] = []
+        target_max_speed = self._default_max_speed
+        target_max_angular = self._default_max_angular
+
+        for zone in self._zones:
+            if point_in_polygon(x, y, zone["vertices"]):
+                active_zones.append(zone["name"])
+                if zone["max_speed"] < target_max_speed:
+                    target_max_speed = zone["max_speed"]
+                if zone["max_angular"] < target_max_angular:
+                    target_max_angular = zone["max_angular"]
+
+        # ── Smooth ramp toward target limits ──────────────────────────────────
+        ramp_rate_speed = (self._default_max_speed / max(self._transition_time, 0.01))
+        ramp_rate_angular = (self._default_max_angular / max(self._transition_time, 0.01))
+
+        self._current_max_speed = ramp_toward(
+            self._current_max_speed, target_max_speed, ramp_rate_speed * self._dt
+        )
+        self._current_max_angular = ramp_toward(
+            self._current_max_angular, target_max_angular, ramp_rate_angular * self._dt
+        )
+
+        # ── Apply limits to cmd_vel ───────────────────────────────────────────
+        cmd = self._last_cmd_vel
+        lx, ly, lz, ax, ay, wz = apply_speed_limit(
+            cmd.linear.x, cmd.linear.y, cmd.angular.z,
+            self._current_max_speed, self._current_max_angular,
+            linear_z=cmd.linear.z, angular_x=cmd.angular.x, angular_y=cmd.angular.y,
+        )
+        limited = Twist()
+        limited.linear.x = lx
+        limited.linear.y = ly
+        limited.linear.z = lz
+        limited.angular.x = ax
+        limited.angular.y = ay
+        limited.angular.z = wz
+        self._pub_limited.publish(limited)
+
+        # ── Status ───────────────────────────────────────────────────────────
+        dist_to_boundary = (
+            min_dist_to_boundary(x, y, self._emergency_boundary)
+            if self._emergency_boundary
+            else -1.0
+        )
+        status = {
+            "robot_xy": [round(x, 3), round(y, 3)],
+            "active_zones": active_zones,
+            "current_max_speed": round(self._current_max_speed, 3),
+            "current_max_angular": round(self._current_max_angular, 3),
+            "target_max_speed": round(target_max_speed, 3),
+            "dist_to_emergency_boundary": round(dist_to_boundary, 3),
+            "violation": False,
+        }
+        self._pub_status.publish(String(data=json.dumps(status)))
+        self._pub_violation.publish(Bool(data=False))
+
+    # ── Helpers ───────────────────────────────────────────────────────────────
+
+    def _trigger_violation(self, x: float, y: float) -> None:
+        """Robot has exited emergency boundary.  Zero all motion, latch violation."""
+        if not self._violation_latched:
+            self.get_logger().error(
+                f"GEOFENCE VIOLATION: robot ({x:.3f}, {y:.3f}) is outside emergency boundary!"
+            )
+            self._violation_latched = True
+        self._publish_zero_cmd()
+        self._pub_violation.publish(Bool(data=True))
+        status = {
+            "robot_xy": [round(x, 3), round(y, 3)],
+            "active_zones": [],
+            "current_max_speed": 0.0,
+            "current_max_angular": 0.0,
+            "target_max_speed": 0.0,
+            "dist_to_emergency_boundary": 0.0,
+            "violation": True,
+        }
+        self._pub_status.publish(String(data=json.dumps(status)))
+
+    def _publish_zero_cmd(self) -> None:
+        self._pub_limited.publish(Twist())
+
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = UwbGeofenceNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/zone_checker.py

@@ -0,0 +1,104 @@
+"""zone_checker.py — Pure-geometry helpers for UWB geofence zones.
+
+No ROS2 dependencies; fully unit-testable.
+
+Coordinate system: UWB local frame, metres, right-hand X-Y plane.
+"""
+
+from __future__ import annotations
+
+import math
+from typing import List, Tuple
+
+Vertex = Tuple[float, float]
+Polygon = List[Vertex]
+
+
+def parse_flat_vertices(flat: List[float]) -> Polygon:
+    """Convert flat [x1, y1, x2, y2, ...] list to [(x1,y1), (x2,y2), ...].
+
+    Raises ValueError if fewer than 3 vertices are provided or the list
+    length is odd.
+    """
+    if len(flat) % 2 != 0:
+        raise ValueError(f"Flat vertex list must have even length, got {len(flat)}")
+    if len(flat) < 6:
+        raise ValueError(f"Need at least 3 vertices (6 values), got {len(flat)}")
+    return [(flat[i], flat[i + 1]) for i in range(0, len(flat), 2)]
+
+
+def point_in_polygon(x: float, y: float, polygon: Polygon) -> bool:
+    """Ray-casting point-in-polygon test.
+
+    Returns True if (x, y) is strictly inside or on the boundary of
+    *polygon*.  Polygon vertices must be ordered (CW or CCW).
+    """
+    n = len(polygon)
+    if n < 3:
+        return False
+    inside = False
+    x1, y1 = polygon[-1]
+    for x2, y2 in polygon:
+        # Check if the ray from (x,y) rightward crosses edge (x1,y1)-(x2,y2)
+        if (y1 > y) != (y2 > y):
+            x_intersect = (x2 - x1) * (y - y1) / (y2 - y1) + x1
+            if x <= x_intersect:
+                inside = not inside
+        x1, y1 = x2, y2
+    return inside
+
+
+def min_dist_to_boundary(x: float, y: float, polygon: Polygon) -> float:
+    """Minimum distance from (x, y) to any edge of *polygon* (metres).
+
+    Uses point-to-segment distance with clamped projection.
+    """
+    if len(polygon) < 2:
+        return float("inf")
+    min_d = float("inf")
+    n = len(polygon)
+    for i in range(n):
+        ax, ay = polygon[i]
+        bx, by = polygon[(i + 1) % n]
+        dx, dy = bx - ax, by - ay
+        seg_len_sq = dx * dx + dy * dy
+        if seg_len_sq < 1e-12:
+            d = math.hypot(x - ax, y - ay)
+        else:
+            t = max(0.0, min(1.0, ((x - ax) * dx + (y - ay) * dy) / seg_len_sq))
+            px, py = ax + t * dx, ay + t * dy
+            d = math.hypot(x - px, y - py)
+        if d < min_d:
+            min_d = d
+    return min_d
+
+
+# ── Velocity helpers (no ROS2 deps — also used by tests) ─────────────────────
+
+def ramp_toward(current: float, target: float, step: float) -> float:
+    """Move *current* toward *target* by at most *step*, never overshoot."""
+    if current < target:
+        return min(current + step, target)
+    if current > target:
+        return max(current - step, target)
+    return current
+
+
+def apply_speed_limit(linear_x: float, linear_y: float, angular_z: float,
+                      max_speed: float, max_angular: float,
+                      linear_z: float = 0.0,
+                      angular_x: float = 0.0,
+                      angular_y: float = 0.0
+                      ) -> tuple:
+    """Return (linear_x, linear_y, linear_z, angular_x, angular_y, angular_z)
+    with linear speed and angular_z clamped to limits.
+    """
+    speed = math.hypot(linear_x, linear_y)
+    if speed > max_speed and speed > 1e-6:
+        scale = max_speed / speed
+        lx = linear_x * scale
+        ly = linear_y * scale
+    else:
+        lx, ly = linear_x, linear_y
+    wz = max(-max_angular, min(max_angular, angular_z))
+    return lx, ly, linear_z, angular_x, angular_y, wz

jetson/ros2_ws/src/saltybot_uwb_geofence/setup.cfg

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

jetson/ros2_ws/src/saltybot_uwb_geofence/setup.py

@@ -0,0 +1,27 @@
+from setuptools import setup, find_packages
+
+setup(
+    name="saltybot_uwb_geofence",
+    version="0.1.0",
+    packages=find_packages(),
+    data_files=[
+        (
+            "share/ament_index/resource_index/packages",
+            ["resource/saltybot_uwb_geofence"],
+        ),
+        ("share/saltybot_uwb_geofence", ["package.xml"]),
+        ("share/saltybot_uwb_geofence/config", ["config/uwb_geofence_zones.yaml"]),
+        ("share/saltybot_uwb_geofence/launch", ["launch/uwb_geofence.launch.py"]),
+    ],
+    install_requires=["setuptools", "pyyaml"],
+    zip_safe=True,
+    author="SaltyLab UWB",
+    author_email="sl-uwb@saltylab.local",
+    description="UWB geofence speed limiter for SaltyBot (Issue #657)",
+    license="MIT",
+    entry_points={
+        "console_scripts": [
+            "uwb_geofence_node=saltybot_uwb_geofence.uwb_geofence_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_uwb_geofence/test/test_zone_checker.py

@@ -0,0 +1,177 @@
+"""Unit tests for zone_checker.py — pure geometry, no ROS2 required."""
+
+import math
+import pytest
+
+from saltybot_uwb_geofence.zone_checker import (
+    parse_flat_vertices,
+    point_in_polygon,
+    min_dist_to_boundary,
+)
+
+
+# ── parse_flat_vertices ───────────────────────────────────────────────────────
+
+class TestParseFlatVertices:
+    def test_square(self):
+        verts = parse_flat_vertices([0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0])
+        assert verts == [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)]
+
+    def test_triangle(self):
+        verts = parse_flat_vertices([0.0, 0.0, 2.0, 0.0, 1.0, 2.0])
+        assert len(verts) == 3
+
+    def test_odd_length_raises(self):
+        with pytest.raises(ValueError, match="even"):
+            parse_flat_vertices([1.0, 2.0, 3.0])
+
+    def test_too_few_points_raises(self):
+        with pytest.raises(ValueError, match="3 vertices"):
+            parse_flat_vertices([0.0, 0.0, 1.0, 1.0])
+
+
+# ── point_in_polygon ──────────────────────────────────────────────────────────
+
+UNIT_SQUARE = [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)]
+TRIANGLE = [(0.0, 0.0), (4.0, 0.0), (2.0, 4.0)]
+
+
+class TestPointInPolygon:
+    # --- Unit square ---
+    def test_centre_inside(self):
+        assert point_in_polygon(0.5, 0.5, UNIT_SQUARE) is True
+
+    def test_outside_right(self):
+        assert point_in_polygon(1.5, 0.5, UNIT_SQUARE) is False
+
+    def test_outside_left(self):
+        assert point_in_polygon(-0.1, 0.5, UNIT_SQUARE) is False
+
+    def test_outside_above(self):
+        assert point_in_polygon(0.5, 1.5, UNIT_SQUARE) is False
+
+    def test_outside_below(self):
+        assert point_in_polygon(0.5, -0.1, UNIT_SQUARE) is False
+
+    def test_far_outside(self):
+        assert point_in_polygon(10.0, 10.0, UNIT_SQUARE) is False
+
+    # --- Triangle ---
+    def test_triangle_apex_region(self):
+        assert point_in_polygon(2.0, 2.0, TRIANGLE) is True
+
+    def test_triangle_base_centre(self):
+        assert point_in_polygon(2.0, 0.5, TRIANGLE) is True
+
+    def test_triangle_outside_top(self):
+        assert point_in_polygon(2.0, 5.0, TRIANGLE) is False
+
+    def test_triangle_outside_right(self):
+        assert point_in_polygon(4.5, 0.5, TRIANGLE) is False
+
+    # --- Degenerate ---
+    def test_empty_polygon(self):
+        assert point_in_polygon(0.5, 0.5, []) is False
+
+    def test_two_vertex_polygon(self):
+        assert point_in_polygon(0.5, 0.5, [(0.0, 0.0), (1.0, 1.0)]) is False
+
+    # --- 10 × 8 room (matches emergency boundary in default config) ---
+    def test_inside_room(self):
+        room = [(-6.0, -5.0), (6.0, -5.0), (6.0, 5.0), (-6.0, 5.0)]
+        assert point_in_polygon(0.0, 0.0, room) is True
+
+    def test_outside_room(self):
+        room = [(-6.0, -5.0), (6.0, -5.0), (6.0, 5.0), (-6.0, 5.0)]
+        assert point_in_polygon(7.0, 0.0, room) is False
+
+
+# ── min_dist_to_boundary ──────────────────────────────────────────────────────
+
+class TestMinDistToBoundary:
+    def test_centre_of_square(self):
+        # Centre of unit square: distance to each edge = 0.5
+        d = min_dist_to_boundary(0.5, 0.5, UNIT_SQUARE)
+        assert abs(d - 0.5) < 1e-9
+
+    def test_on_edge(self):
+        d = min_dist_to_boundary(0.5, 0.0, UNIT_SQUARE)
+        assert d < 1e-9
+
+    def test_outside_corner(self):
+        # Point at (1.5, 0.5): closest edge is the right edge at x=1
+        d = min_dist_to_boundary(1.5, 0.5, UNIT_SQUARE)
+        assert abs(d - 0.5) < 1e-9
+
+    def test_outside_above(self):
+        d = min_dist_to_boundary(0.5, 1.3, UNIT_SQUARE)
+        assert abs(d - 0.3) < 1e-9
+
+    def test_empty_polygon(self):
+        assert min_dist_to_boundary(0.0, 0.0, []) == float("inf")
+
+    def test_single_vertex(self):
+        assert min_dist_to_boundary(1.0, 1.0, [(0.0, 0.0)]) == float("inf")
+
+
+# ── ramp_toward & apply_speed_limit ──────────────────────────────────────────
+
+from saltybot_uwb_geofence.zone_checker import ramp_toward, apply_speed_limit
+
+
+class TestRampToward:
+    def test_ramp_up(self):
+        val = ramp_toward(0.0, 1.0, 0.1)
+        assert abs(val - 0.1) < 1e-9
+
+    def test_ramp_down(self):
+        val = ramp_toward(1.0, 0.0, 0.1)
+        assert abs(val - 0.9) < 1e-9
+
+    def test_no_overshoot_up(self):
+        val = ramp_toward(0.95, 1.0, 0.1)
+        assert val == 1.0
+
+    def test_no_overshoot_down(self):
+        val = ramp_toward(0.05, 0.0, 0.1)
+        assert val == 0.0
+
+    def test_already_at_target(self):
+        val = ramp_toward(0.5, 0.5, 0.1)
+        assert val == 0.5
+
+
+class TestApplySpeedLimit:
+    def _call(self, vx: float, vy: float = 0.0, wz: float = 0.0,
+              max_speed: float = 1.0, max_ang: float = 1.5):
+        return apply_speed_limit(vx, vy, wz, max_speed, max_ang)
+
+    def test_within_limit_unchanged(self):
+        lx, ly, *_, wz = self._call(0.3, 0.0, 0.2)
+        assert abs(lx - 0.3) < 1e-9
+        assert abs(wz - 0.2) < 1e-9
+
+    def test_linear_exceeds_limit(self):
+        lx, ly, *_ = self._call(2.0, 0.0, 0.0)
+        speed = math.hypot(lx, ly)
+        assert speed <= 1.0 + 1e-9
+
+    def test_direction_preserved_when_limited(self):
+        lx, ly, *_ = self._call(3.0, 4.0, 0.0, max_speed=1.0)
+        speed = math.hypot(lx, ly)
+        assert abs(speed - 1.0) < 1e-9
+        assert abs(lx / ly - 3.0 / 4.0) < 1e-6
+
+    def test_angular_clamp_positive(self):
+        *_, wz = self._call(0.0, 0.0, 2.0, max_ang=1.0)
+        assert abs(wz - 1.0) < 1e-9
+
+    def test_angular_clamp_negative(self):
+        *_, wz = self._call(0.0, 0.0, -2.0, max_ang=1.0)
+        assert abs(wz + 1.0) < 1e-9
+
+    def test_zero_cmd_stays_zero(self):
+        lx, ly, lz, ax, ay, wz = self._call(0.0, 0.0, 0.0)
+        assert lx == 0.0
+        assert ly == 0.0
+        assert wz == 0.0

jetson/ros2_ws/src/saltybot_vesc_driver/config/vesc_params.yaml

@@ -2,9 +2,19 @@ vesc_can_driver:
   ros__parameters:
     interface: can0
     bitrate: 500000
-    left_motor_id: 61
-    right_motor_id: 79
+    left_motor_id: 56
+    right_motor_id: 68
     wheel_separation: 0.5
     wheel_radius: 0.1
     max_speed: 5.0
     command_timeout: 1.0
+
+velocity_smoother:
+  ros__parameters:
+    publish_rate: 50.0
+    max_linear_accel: 1.0
+    max_linear_decel: 2.0
+    max_angular_accel: 1.5
+    max_angular_decel: 3.0
+    max_linear_jerk: 0.0
+    max_angular_jerk: 0.0

jetson/ros2_ws/src/saltybot_vesc_driver/launch/vesc_driver.launch.py

@@ -20,6 +20,13 @@ def generate_launch_description():
                 default_value=config_file,
                 description="Path to configuration YAML file",
             ),
+            Node(
+                package="saltybot_vesc_driver",
+                executable="velocity_smoother_node",
+                name="velocity_smoother",
+                output="screen",
+                parameters=[LaunchConfiguration("config_file")],
+            ),
             Node(
                 package="saltybot_vesc_driver",
                 executable="vesc_driver_node",

jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/velocity_smoother_node.py

@@ -0,0 +1,171 @@
+#!/usr/bin/env python3
+"""
+Smooth velocity controller with accel/decel ramp.
+
+Subscribes to /cmd_vel (geometry_msgs/Twist), applies acceleration
+and deceleration ramps, and publishes smoothed commands to
+/cmd_vel_smoothed at 50 Hz.
+
+E-stop (std_msgs/Bool on /e_stop): immediate zero, bypasses ramp.
+Optional jerk limiting via max_linear_jerk / max_angular_jerk params.
+"""
+
+import math
+
+import rclpy
+from geometry_msgs.msg import Twist
+from rclpy.node import Node
+from std_msgs.msg import Bool
+
+
+class VelocitySmoother(Node):
+    def __init__(self):
+        super().__init__('velocity_smoother')
+
+        # Declare parameters
+        self.declare_parameter('publish_rate', 50.0)           # Hz
+        self.declare_parameter('max_linear_accel', 1.0)        # m/s²
+        self.declare_parameter('max_linear_decel', 2.0)        # m/s²
+        self.declare_parameter('max_angular_accel', 1.5)       # rad/s²
+        self.declare_parameter('max_angular_decel', 3.0)       # rad/s²
+        self.declare_parameter('max_linear_jerk', 0.0)         # m/s³, 0=disabled
+        self.declare_parameter('max_angular_jerk', 0.0)        # rad/s³, 0=disabled
+
+        rate = self.get_parameter('publish_rate').value
+        self.max_lin_acc = self.get_parameter('max_linear_accel').value
+        self.max_lin_dec = self.get_parameter('max_linear_decel').value
+        self.max_ang_acc = self.get_parameter('max_angular_accel').value
+        self.max_ang_dec = self.get_parameter('max_angular_decel').value
+        self.max_lin_jerk = self.get_parameter('max_linear_jerk').value
+        self.max_ang_jerk = self.get_parameter('max_angular_jerk').value
+
+        self._dt = 1.0 / rate
+
+        # State
+        self._target_lin = 0.0
+        self._target_ang = 0.0
+        self._current_lin = 0.0
+        self._current_ang = 0.0
+        self._current_lin_acc = 0.0   # for jerk limiting
+        self._current_ang_acc = 0.0
+        self._e_stop = False
+
+        # Publisher
+        self._pub = self.create_publisher(Twist, '/cmd_vel_smoothed', 10)
+
+        # Subscriptions
+        self.create_subscription(Twist, '/cmd_vel', self._cmd_vel_cb, 10)
+        self.create_subscription(Bool, '/e_stop', self._e_stop_cb, 10)
+
+        # Timer
+        self.create_timer(self._dt, self._timer_cb)
+
+        self.get_logger().info(
+            f'VelocitySmoother ready at {rate:.0f} Hz — '
+            f'lin_acc={self.max_lin_acc} lin_dec={self.max_lin_dec} '
+            f'ang_acc={self.max_ang_acc} ang_dec={self.max_ang_dec}'
+        )
+
+    # ------------------------------------------------------------------
+
+    def _cmd_vel_cb(self, msg: Twist):
+        self._target_lin = msg.linear.x
+        self._target_ang = msg.angular.z
+
+    def _e_stop_cb(self, msg: Bool):
+        self._e_stop = msg.data
+        if self._e_stop:
+            self._target_lin = 0.0
+            self._target_ang = 0.0
+            self._current_lin = 0.0
+            self._current_ang = 0.0
+            self._current_lin_acc = 0.0
+            self._current_ang_acc = 0.0
+            self.get_logger().warn('E-STOP active — motors zeroed immediately')
+
+    # ------------------------------------------------------------------
+
+    def _ramp(
+        self,
+        current: float,
+        target: float,
+        max_acc: float,
+        max_dec: float,
+        current_acc: float,
+        max_jerk: float,
+    ) -> tuple[float, float]:
+        """
+        Advance `current` toward `target` with separate accel/decel limits.
+        Optionally apply jerk limiting to the acceleration.
+        Returns (new_value, new_acc).
+        """
+        error = target - current
+
+        # Choose limit: decelerate if moving away from zero and target is
+        # closer to zero (or past it), else accelerate.
+        if current * error < 0 or (error != 0 and abs(target) < abs(current)):
+            limit = max_dec
+        else:
+            limit = max_acc
+
+        desired_acc = math.copysign(min(abs(error) / self._dt, limit), error)
+        # Clamp so we don't overshoot
+        desired_acc = max(-limit, min(limit, desired_acc))
+
+        if max_jerk > 0.0:
+            max_d_acc = max_jerk * self._dt
+            new_acc = current_acc + max(
+                -max_d_acc, min(max_d_acc, desired_acc - current_acc)
+            )
+        else:
+            new_acc = desired_acc
+
+        delta = new_acc * self._dt
+        # Clamp so we don't overshoot the target
+        if abs(delta) > abs(error):
+            delta = error
+        return current + delta, new_acc
+
+    def _timer_cb(self):
+        if self._e_stop:
+            msg = Twist()
+            self._pub.publish(msg)
+            return
+
+        self._current_lin, self._current_lin_acc = self._ramp(
+            self._current_lin,
+            self._target_lin,
+            self.max_lin_acc,
+            self.max_lin_dec,
+            self._current_lin_acc,
+            self.max_lin_jerk,
+        )
+        self._current_ang, self._current_ang_acc = self._ramp(
+            self._current_ang,
+            self._target_ang,
+            self.max_ang_acc,
+            self.max_ang_dec,
+            self._current_ang_acc,
+            self.max_lin_jerk,  # intentional: use linear jerk scale for angular too if set
+        )
+
+        msg = Twist()
+        msg.linear.x = self._current_lin
+        msg.angular.z = self._current_ang
+        self._pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VelocitySmoother()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_vesc_driver/saltybot_vesc_driver/vesc_driver_node.py

@@ -4,23 +4,84 @@ VESC CAN driver node using python-can with SocketCAN.
 Subscribes to /cmd_vel (geometry_msgs/Twist) and sends duty cycle
 commands to two VESC motor controllers via CAN bus.
 
+Also receives VESC STATUS broadcast frames and publishes telemetry:
+  /saltybot/vesc/left   (std_msgs/String JSON)
+  /saltybot/vesc/right  (std_msgs/String JSON)
+
 VESC CAN protocol:
   Extended frame ID = (CAN_PACKET_ID << 8) | VESC_ID
   CAN_PACKET_SET_DUTY = 0  -> payload: int32 (duty * 100000)
 """
 
+import json
 import struct
+import threading
 import time
+from dataclasses import dataclass, field
+from typing import Optional
 
 import can
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
+from std_msgs.msg import String
 
 
-# VESC CAN packet IDs
+# ── VESC CAN packet IDs ────────────────────────────────────────────────────────
 CAN_PACKET_SET_DUTY   = 0
 CAN_PACKET_SET_RPM    = 3
+CAN_PACKET_STATUS     = 9    # RPM, phase current, duty
+CAN_PACKET_STATUS_4   = 16   # FET temp, motor temp, input current
+CAN_PACKET_STATUS_5   = 27   # Input voltage
+
+FAULT_NAMES = {
+    0: "NONE",                        1: "OVER_VOLTAGE",
+    2: "UNDER_VOLTAGE",               3: "DRV",
+    4: "ABS_OVER_CURRENT",            5: "OVER_TEMP_FET",
+    6: "OVER_TEMP_MOTOR",             7: "GATE_DRIVER_OVER_VOLTAGE",
+    8: "GATE_DRIVER_UNDER_VOLTAGE_3V3", 9: "MCU_UNDER_VOLTAGE",
+    10: "BOOTING_FROM_WATCHDOG_RESET", 11: "ENCODER_SPI_FAULT",
+}
+
+ALIVE_TIMEOUT_S = 1.0
+
+
+@dataclass
+class VescState:
+    can_id:       int
+    rpm:          int   = 0
+    current_a:    float = 0.0
+    current_in_a: float = 0.0
+    duty_cycle:   float = 0.0
+    voltage_v:    float = 0.0
+    temp_fet_c:   float = 0.0
+    temp_motor_c: float = 0.0
+    fault_code:   int   = 0
+    last_ts:      float = field(default_factory=lambda: 0.0)
+
+    @property
+    def is_alive(self) -> bool:
+        return (time.monotonic() - self.last_ts) < ALIVE_TIMEOUT_S
+
+    @property
+    def fault_name(self) -> str:
+        return FAULT_NAMES.get(self.fault_code, f"UNKNOWN_{self.fault_code}")
+
+    def to_dict(self) -> dict:
+        return {
+            "can_id":       self.can_id,
+            "rpm":          self.rpm,
+            "current_a":    round(self.current_a,    2),
+            "current_in_a": round(self.current_in_a, 2),
+            "duty_cycle":   round(self.duty_cycle,   4),
+            "voltage_v":    round(self.voltage_v,    2),
+            "temp_fet_c":   round(self.temp_fet_c,   1),
+            "temp_motor_c": round(self.temp_motor_c, 1),
+            "fault_code":   self.fault_code,
+            "fault_name":   self.fault_name,
+            "alive":        self.is_alive,
+            "stamp":        time.time(),
+        }
 
 
 def make_can_id(packet_id: int, vesc_id: int) -> int:
@@ -34,12 +95,13 @@ class VescCanDriver(Node):
 
         # Declare parameters
         self.declare_parameter('interface', 'can0')
-        self.declare_parameter('left_motor_id', 61)
-        self.declare_parameter('right_motor_id', 79)
+        self.declare_parameter('left_motor_id', 56)
+        self.declare_parameter('right_motor_id', 68)
         self.declare_parameter('wheel_separation', 0.5)
         self.declare_parameter('wheel_radius', 0.1)
         self.declare_parameter('max_speed', 5.0)
         self.declare_parameter('command_timeout', 1.0)
+        self.declare_parameter('telemetry_rate_hz', 10)
 
         # Read parameters
         self.interface   = self.get_parameter('interface').value
@@ -48,6 +110,7 @@ class VescCanDriver(Node):
         self.wheel_sep   = self.get_parameter('wheel_separation').value
         self.max_speed   = self.get_parameter('max_speed').value
         self.cmd_timeout = self.get_parameter('command_timeout').value
+        tel_hz           = int(self.get_parameter('telemetry_rate_hz').value)
 
         # Open CAN bus
         try:
@@ -62,17 +125,36 @@ class VescCanDriver(Node):
 
         self._last_cmd_time = time.monotonic()
 
-        # Subscriber
-        self.create_subscription(Twist, '/cmd_vel', self._cmd_vel_cb, 10)
+        # Per-VESC telemetry state
+        self._state: dict[int, VescState] = {
+            self.left_id:  VescState(can_id=self.left_id),
+            self.right_id: VescState(can_id=self.right_id),
+        }
+        self._state_lock = threading.Lock()
 
-        # Watchdog timer (10 Hz)
-        self.create_timer(0.1, self._watchdog_cb)
+        # Telemetry publishers
+        self._pub_left  = self.create_publisher(String, '/saltybot/vesc/left',  10)
+        self._pub_right = self.create_publisher(String, '/saltybot/vesc/right', 10)
+
+        # CAN RX background thread
+        self._running   = True
+        self._rx_thread = threading.Thread(target=self._rx_loop, name='vesc_rx', daemon=True)
+        self._rx_thread.start()
+
+        # Subscriber
+        self.create_subscription(Twist, '/cmd_vel_smoothed', self._cmd_vel_cb, 10)
+
+        # Watchdog + telemetry publish timer
+        self.create_timer(1.0 / max(1, tel_hz), self._watchdog_and_publish_cb)
 
         self.get_logger().info(
-            f'VESC CAN driver ready — left={self.left_id} right={self.right_id}'
+            f'VESC CAN driver ready — left={self.left_id} right={self.right_id} '
+            f'telemetry={tel_hz} Hz'
         )
 
     # ------------------------------------------------------------------
+    # Command velocity callback
+    # ------------------------------------------------------------------
 
     def _cmd_vel_cb(self, msg: Twist):
         self._last_cmd_time = time.monotonic()
@@ -88,12 +170,68 @@ class VescCanDriver(Node):
         self._send_duty(self.left_id,  left_duty)
         self._send_duty(self.right_id, right_duty)
 
-    def _watchdog_cb(self):
+    # ------------------------------------------------------------------
+    # Watchdog + telemetry publish (combined timer callback)
+    # ------------------------------------------------------------------
+
+    def _watchdog_and_publish_cb(self):
         elapsed = time.monotonic() - self._last_cmd_time
         if elapsed > self.cmd_timeout:
             self._send_duty(self.left_id,  0.0)
             self._send_duty(self.right_id, 0.0)
 
+        with self._state_lock:
+            l_dict = self._state[self.left_id].to_dict()
+            r_dict = self._state[self.right_id].to_dict()
+
+        self._pub_left.publish(String(data=json.dumps(l_dict)))
+        self._pub_right.publish(String(data=json.dumps(r_dict)))
+
+    # ------------------------------------------------------------------
+    # CAN RX background thread
+    # ------------------------------------------------------------------
+
+    def _rx_loop(self) -> None:
+        """Receive VESC STATUS broadcast frames and update telemetry state."""
+        while self._running:
+            try:
+                msg = self.bus.recv(timeout=0.1)
+            except Exception:
+                continue
+            if msg is None or not msg.is_extended_id:
+                continue
+            self._dispatch_frame(msg.arbitration_id, bytes(msg.data))
+
+    def _dispatch_frame(self, arb_id: int, data: bytes) -> None:
+        packet_type = (arb_id >> 8) & 0xFFFF
+        vesc_id     =  arb_id       & 0xFF
+
+        if vesc_id not in self._state:
+            return
+
+        now = time.monotonic()
+        with self._state_lock:
+            s = self._state[vesc_id]
+
+            if packet_type == CAN_PACKET_STATUS and len(data) >= 8:
+                rpm          = struct.unpack('>i', data[0:4])[0]
+                current_a    = struct.unpack('>h', data[4:6])[0] / 10.0
+                duty_cycle   = struct.unpack('>h', data[6:8])[0] / 1000.0
+                s.rpm        = rpm
+                s.current_a  = current_a
+                s.duty_cycle = duty_cycle
+                s.last_ts    = now
+
+            elif packet_type == CAN_PACKET_STATUS_4 and len(data) >= 6:
+                s.temp_fet_c   = struct.unpack('>h', data[0:2])[0] / 10.0
+                s.temp_motor_c = struct.unpack('>h', data[2:4])[0] / 10.0
+                s.current_in_a = struct.unpack('>h', data[4:6])[0] / 10.0
+
+            elif packet_type == CAN_PACKET_STATUS_5 and len(data) >= 2:
+                s.voltage_v = struct.unpack('>h', data[0:2])[0] / 10.0
+
+    # ------------------------------------------------------------------
+    # CAN send helpers
     # ------------------------------------------------------------------
 
     def _send_duty(self, vesc_id: int, duty: float):
@@ -111,6 +249,8 @@ class VescCanDriver(Node):
             self.get_logger().error(f'CAN send error (id={vesc_id}): {e}')
 
     def destroy_node(self):
+        self._running = False
+        self._rx_thread.join(timeout=1.0)
         self._send_duty(self.left_id,  0.0)
         self._send_duty(self.right_id, 0.0)
         self.bus.shutdown()

jetson/ros2_ws/src/saltybot_vesc_driver/setup.py

@@ -22,6 +22,7 @@ setup(
     entry_points={
         "console_scripts": [
             "vesc_driver_node = saltybot_vesc_driver.vesc_driver_node:main",
+            "velocity_smoother_node = saltybot_vesc_driver.velocity_smoother_node:main",
         ],
     },
 )

jetson/ros2_ws/src/saltybot_vesc_driver/test/test_velocity_smoother.py

@@ -0,0 +1,237 @@
+#!/usr/bin/env python3
+"""Unit tests for VelocitySmoother ramp logic."""
+
+import math
+import unittest
+from unittest.mock import MagicMock, patch
+
+
+# ---------------------------------------------------------------------------
+# Minimal ROS2 stubs so tests run without a ROS2 installation
+# ---------------------------------------------------------------------------
+
+class _FakeParam:
+    def __init__(self, val):
+        self._val = val
+    @property
+    def value(self):
+        return self._val
+
+
+class _FakeNode:
+    def __init__(self):
+        self._params = {}
+    def declare_parameter(self, name, default):
+        self._params[name] = _FakeParam(default)
+    def get_parameter(self, name):
+        return self._params[name]
+    def create_publisher(self, *a, **kw):
+        return MagicMock()
+    def create_subscription(self, *a, **kw):
+        return MagicMock()
+    def create_timer(self, *a, **kw):
+        return MagicMock()
+    def get_logger(self):
+        log = MagicMock()
+        log.info = MagicMock()
+        log.warn = MagicMock()
+        return log
+
+
+# Patch rclpy and geometry_msgs before importing the module.
+# rclpy.node.Node must be a *real* class so that VelocitySmoother can
+# inherit from it without becoming a MagicMock itself.
+import sys
+
+class _RealNodeBase:
+    """Minimal real base class that stands in for rclpy.node.Node."""
+    def __init__(self, *args, **kwargs):
+        pass
+
+    def declare_parameter(self, name, default):
+        if not hasattr(self, '_params'):
+            self._params = {}
+        self._params[name] = _FakeParam(default)
+
+    def get_parameter(self, name):
+        return self._params[name]
+
+    def create_publisher(self, *a, **kw):
+        return MagicMock()
+
+    def create_subscription(self, *a, **kw):
+        return MagicMock()
+
+    def create_timer(self, *a, **kw):
+        return MagicMock()
+
+    def get_logger(self):
+        log = MagicMock()
+        log.info = MagicMock()
+        log.warn = MagicMock()
+        return log
+
+rclpy_node_mod = MagicMock()
+rclpy_node_mod.Node = _RealNodeBase
+
+rclpy_mock = MagicMock()
+rclpy_mock.node = rclpy_node_mod
+
+sys.modules['rclpy'] = rclpy_mock
+sys.modules['rclpy.node'] = rclpy_node_mod
+sys.modules.setdefault('geometry_msgs', MagicMock())
+sys.modules.setdefault('geometry_msgs.msg', MagicMock())
+sys.modules.setdefault('std_msgs', MagicMock())
+sys.modules.setdefault('std_msgs.msg', MagicMock())
+
+# Provide a real Twist-like object for tests
+class _Twist:
+    class _Vec:
+        x = 0.0
+        y = 0.0
+        z = 0.0
+    def __init__(self):
+        self.linear = self._Vec()
+        self.angular = self._Vec()
+
+sys.modules['geometry_msgs.msg'].Twist = _Twist
+sys.modules['std_msgs.msg'].Bool = MagicMock()
+
+import importlib, os, pathlib
+sys.path.insert(0, str(pathlib.Path(__file__).parents[1] / 'saltybot_vesc_driver'))
+
+# Now we can import the smoother logic directly
+from velocity_smoother_node import VelocitySmoother
+
+
+def _make_smoother(**params):
+    """Create a VelocitySmoother backed by _RealNodeBase with custom params."""
+    defaults = dict(
+        publish_rate=50.0,
+        max_linear_accel=1.0,
+        max_linear_decel=2.0,
+        max_angular_accel=1.5,
+        max_angular_decel=3.0,
+        max_linear_jerk=0.0,
+        max_angular_jerk=0.0,
+    )
+    defaults.update(params)
+
+    # Pre-seed _params before __init__ so declare_parameter picks up overrides.
+    node = VelocitySmoother.__new__(VelocitySmoother)
+    node._params = {k: _FakeParam(v) for k, v in defaults.items()}
+
+    # Monkey-patch declare_parameter so it doesn't overwrite our pre-seeded values
+    original_declare = _RealNodeBase.declare_parameter
+    def _noop_declare(self, name, default):
+        pass  # params already seeded
+    _RealNodeBase.declare_parameter = _noop_declare
+
+    try:
+        VelocitySmoother.__init__(node)
+    finally:
+        _RealNodeBase.declare_parameter = original_declare
+
+    return node
+
+
+# ---------------------------------------------------------------------------
+
+class TestRampLogic(unittest.TestCase):
+
+    def _make(self, **kw):
+        return _make_smoother(**kw)
+
+    def test_ramp_reaches_target_within_expected_steps(self):
+        """From 0 to 1 m/s with accel=1 m/s² at 50 Hz → ~50 steps."""
+        node = self._make(max_linear_accel=1.0, publish_rate=50.0)
+        node._target_lin = 1.0
+        steps = 0
+        while abs(node._current_lin - 1.0) > 0.01 and steps < 200:
+            node._timer_cb()
+            steps += 1
+        self.assertLessEqual(steps, 55, "Should reach 1 m/s within ~55 steps at 50 Hz")
+        self.assertAlmostEqual(node._current_lin, 1.0, places=2)
+
+    def test_decel_faster_than_accel(self):
+        """Deceleration should complete faster than acceleration."""
+        node = self._make(max_linear_accel=1.0, max_linear_decel=2.0, publish_rate=50.0)
+        # Ramp up
+        node._target_lin = 1.0
+        for _ in range(100):
+            node._timer_cb()
+        # Now decelerate
+        node._target_lin = 0.0
+        decel_steps = 0
+        while abs(node._current_lin) > 0.01 and decel_steps < 200:
+            node._timer_cb()
+            decel_steps += 1
+        # Ramp up again to measure accel steps
+        node._current_lin = 0.0
+        node._target_lin = 1.0
+        accel_steps = 0
+        while abs(node._current_lin - 1.0) > 0.01 and accel_steps < 200:
+            node._timer_cb()
+            accel_steps += 1
+        self.assertLess(decel_steps, accel_steps,
+                        "Decel (2 m/s²) should finish in fewer steps than accel (1 m/s²)")
+
+    def test_e_stop_zeros_immediately(self):
+        """E-stop must zero velocity in the same callback, bypassing ramp."""
+        node = self._make()
+        node._current_lin = 2.0
+        node._current_ang = 1.0
+        node._target_lin = 2.0
+        node._target_ang = 1.0
+
+        msg = MagicMock()
+        msg.data = True
+        node._e_stop_cb(msg)
+
+        self.assertEqual(node._current_lin, 0.0)
+        self.assertEqual(node._current_ang, 0.0)
+        self.assertTrue(node._e_stop)
+
+    def test_no_overshoot(self):
+        """Current velocity must never exceed target during ramp-up."""
+        node = self._make(max_linear_accel=1.0, publish_rate=50.0)
+        node._target_lin = 0.5
+        for _ in range(100):
+            node._timer_cb()
+            self.assertLessEqual(node._current_lin, 0.5 + 1e-9)
+
+    def test_negative_velocity_ramp(self):
+        """Ramp works symmetrically for negative targets."""
+        node = self._make(max_linear_accel=1.0, publish_rate=50.0)
+        node._target_lin = -1.0
+        for _ in range(200):
+            node._timer_cb()
+        self.assertAlmostEqual(node._current_lin, -1.0, places=2)
+
+    def test_angular_ramp(self):
+        """Angular velocity ramps correctly."""
+        node = self._make(max_angular_accel=1.5, publish_rate=50.0)
+        node._target_ang = 1.0
+        for _ in range(200):
+            node._timer_cb()
+        self.assertAlmostEqual(node._current_ang, 1.0, places=2)
+
+    def test_ramp_timing_linear(self):
+        """Time to ramp from 0 to v_max ≈ v_max / accel (±10%)."""
+        accel = 1.0  # m/s²
+        v_max = 1.0  # m/s
+        rate = 50.0
+        expected_time = v_max / accel  # 1.0 s
+        node = self._make(max_linear_accel=accel, publish_rate=rate)
+        node._target_lin = v_max
+        steps = 0
+        while abs(node._current_lin - v_max) > 0.01 and steps < 500:
+            node._timer_cb()
+            steps += 1
+        actual_time = steps / rate
+        self.assertAlmostEqual(actual_time, expected_time, delta=expected_time * 0.12,
+                               msg=f"Ramp time {actual_time:.2f}s expected ~{expected_time:.2f}s")
+
+
+if __name__ == '__main__':
+    unittest.main()

jetson/ros2_ws/src/saltybot_vesc_health/config/vesc_health_params.yaml

@@ -0,0 +1,27 @@
+# VESC CAN Health Monitor — SaltyBot dual FSESC 6.7 Pro
+# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/651
+
+vesc_health_monitor:
+  ros__parameters:
+    # SocketCAN interface (must match can-bringup.service / Issue #643)
+    can_interface: "can0"
+
+    # VESC CAN IDs (must match vesc_driver / vesc_telemetry nodes)
+    left_can_id:   61    # 0x3D — left drive motor
+    right_can_id:  79    # 0x4F — right drive motor
+
+    # This node's CAN ID used as sender_id in GET_VALUES recovery requests
+    sender_can_id: 127
+
+    # Timeout before recovery sequence begins (s)
+    # Frames expected at 20 Hz from vesc_telemetry; alert if > 500 ms gap
+    frame_timeout_s: 0.5
+
+    # Timeout before e-stop escalation (s, measured from last good frame)
+    estop_timeout_s: 2.0
+
+    # Health publish + watchdog timer rate
+    health_rate_hz: 10
+
+    # How often to poll CAN interface for bus-off state (via ip link show)
+    bus_off_check_rate_hz: 1

jetson/ros2_ws/src/saltybot_vesc_health/launch/vesc_health.launch.py

@@ -0,0 +1,50 @@
+"""Launch file for VESC CAN health monitor node."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory("saltybot_vesc_health")
+    config_file = os.path.join(pkg_dir, "config", "vesc_health_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "config_file",
+            default_value=config_file,
+            description="Path to vesc_health_params.yaml",
+        ),
+        DeclareLaunchArgument(
+            "can_interface",
+            default_value="can0",
+            description="SocketCAN interface name",
+        ),
+        DeclareLaunchArgument(
+            "frame_timeout_s",
+            default_value="0.5",
+            description="Frame-drop detection threshold (s)",
+        ),
+        DeclareLaunchArgument(
+            "estop_timeout_s",
+            default_value="2.0",
+            description="E-stop escalation threshold (s from last good frame)",
+        ),
+        Node(
+            package="saltybot_vesc_health",
+            executable="vesc_health_node",
+            name="vesc_health_monitor",
+            output="screen",
+            parameters=[
+                LaunchConfiguration("config_file"),
+                {
+                    "can_interface":  LaunchConfiguration("can_interface"),
+                    "frame_timeout_s": LaunchConfiguration("frame_timeout_s"),
+                    "estop_timeout_s": LaunchConfiguration("estop_timeout_s"),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_vesc_health/package.xml

@@ -0,0 +1,33 @@
+<?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_vesc_health</name>
+  <version>0.1.0</version>
+  <description>
+    VESC CAN health monitor for SaltyBot dual FSESC 6.7 Pro (FW 6.6).
+    Monitors CAN IDs 61 (left) and 79 (right) for frame drops, bus-off errors,
+    and timeouts (&gt;500 ms).  Drives a recovery sequence (GET_VALUES alive frames),
+    escalates to e-stop if unresponsive &gt;2 s.  Publishes /vesc/health (JSON),
+    /diagnostics, /estop, and /cmd_vel (zero on e-stop).  Issue #651.
+  </description>
+  <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+
+  <exec_depend>python3-can</exec_depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/health_monitor_node.py

@@ -0,0 +1,450 @@
+#!/usr/bin/env python3
+"""VESC CAN health monitor node for SaltyBot.
+
+Monitors both VESC motor controllers (CAN IDs 61 and 79) for frame drops,
+bus-off errors, and timeouts.  Drives a recovery sequence on fault and
+escalates to an e-stop if a VESC stays unresponsive beyond 2 s.
+
+Subscribed topics
+-----------------
+  /vesc/left/state   (std_msgs/String) — JSON telemetry from vesc_telemetry node
+  /vesc/right/state  (std_msgs/String) — JSON telemetry from vesc_telemetry node
+
+Published topics
+----------------
+  /vesc/health   (std_msgs/String) — JSON health summary (10 Hz)
+  /diagnostics   (diagnostic_msgs/DiagnosticArray)
+  /estop         (std_msgs/String) — JSON e-stop event on state change
+  /cmd_vel       (geometry_msgs/Twist) — zero velocity when e-stop active
+
+Parameters
+----------
+  can_interface          str    'can0'  SocketCAN interface
+  left_can_id            int    61
+  right_can_id           int    79
+  sender_can_id          int    127     This node's CAN ID for alive frames
+  frame_timeout_s        float  0.5     Timeout before recovery starts
+  estop_timeout_s        float  2.0     Timeout before e-stop escalation
+  health_rate_hz         int    10      Publish + watchdog rate
+  bus_off_check_rate_hz  int    1       How often to poll CAN interface state
+"""
+
+from __future__ import annotations
+
+import json
+import subprocess
+import threading
+import time
+from typing import Optional
+
+try:
+    import rclpy
+    from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+    from geometry_msgs.msg import Twist
+    from rclpy.node import Node
+    from std_msgs.msg import String
+    _ROS_AVAILABLE = True
+except ImportError:  # pragma: no cover — missing outside ROS2 env
+    _ROS_AVAILABLE = False
+    Node = object  # type: ignore[assignment,misc]
+
+try:
+    import can as python_can
+    _CAN_AVAILABLE = True
+except ImportError:
+    _CAN_AVAILABLE = False
+
+from .recovery_fsm import Action, HealthFsm, VescHealthState
+
+
+class VescHealthMonitor(Node):
+    """ROS2 node: VESC CAN health watchdog + auto-recovery."""
+
+    def __init__(self) -> None:
+        super().__init__("vesc_health_monitor")
+
+        # ----------------------------------------------------------------
+        # Parameters
+        # ----------------------------------------------------------------
+        self.declare_parameter("can_interface",         "can0")
+        self.declare_parameter("left_can_id",           61)
+        self.declare_parameter("right_can_id",          79)
+        self.declare_parameter("sender_can_id",         127)
+        self.declare_parameter("frame_timeout_s",       0.5)
+        self.declare_parameter("estop_timeout_s",       2.0)
+        self.declare_parameter("health_rate_hz",        10)
+        self.declare_parameter("bus_off_check_rate_hz", 1)
+
+        self._iface     = self.get_parameter("can_interface").value
+        self._left_id   = self.get_parameter("left_can_id").value
+        self._right_id  = self.get_parameter("right_can_id").value
+        self._sender_id = self.get_parameter("sender_can_id").value
+        timeout_s       = self.get_parameter("frame_timeout_s").value
+        estop_s         = self.get_parameter("estop_timeout_s").value
+        hz              = max(1, int(self.get_parameter("health_rate_hz").value))
+        bus_hz          = max(1, int(self.get_parameter("bus_off_check_rate_hz").value))
+
+        # ----------------------------------------------------------------
+        # State machine
+        # ----------------------------------------------------------------
+        self._fsm = HealthFsm(
+            left_id=self._left_id,
+            right_id=self._right_id,
+            timeout_s=timeout_s,
+            estop_s=estop_s,
+        )
+        self._fsm_lock = threading.Lock()
+
+        # ----------------------------------------------------------------
+        # Fault event log (ring buffer, newest last, max 200 entries)
+        # ----------------------------------------------------------------
+        self._fault_log: list[dict] = []
+        self._fault_log_lock = threading.Lock()
+
+        # ----------------------------------------------------------------
+        # SocketCAN (for sending alive / GET_VALUES recovery frames)
+        # ----------------------------------------------------------------
+        self._bus: Optional["python_can.BusABC"] = None
+        self._init_can()
+
+        # ----------------------------------------------------------------
+        # Publishers
+        # ----------------------------------------------------------------
+        self._pub_health = self.create_publisher(String,         "/vesc/health",  10)
+        self._pub_diag   = self.create_publisher(DiagnosticArray, "/diagnostics",  10)
+        self._pub_estop  = self.create_publisher(String,         "/estop",        10)
+        self._pub_cmd    = self.create_publisher(Twist,          "/cmd_vel",      10)
+
+        # ----------------------------------------------------------------
+        # Subscribers
+        # ----------------------------------------------------------------
+        self.create_subscription(String, "/vesc/left/state",  self._on_left_state,  10)
+        self.create_subscription(String, "/vesc/right/state", self._on_right_state, 10)
+
+        # ----------------------------------------------------------------
+        # Timers
+        # ----------------------------------------------------------------
+        self.create_timer(1.0 / hz,     self._on_health_tick)
+        self.create_timer(1.0 / bus_hz, self._check_bus_off)
+
+        self.get_logger().info(
+            f"vesc_health_monitor: iface={self._iface}, "
+            f"left={self._left_id}, right={self._right_id}, "
+            f"timeout={timeout_s}s, estop={estop_s}s, {hz} Hz"
+        )
+
+    # ----------------------------------------------------------------
+    # CAN initialisation
+    # ----------------------------------------------------------------
+
+    def _init_can(self) -> None:
+        if not _CAN_AVAILABLE:
+            self.get_logger().warn(
+                "python-can not installed — alive frame recovery disabled"
+            )
+            return
+        try:
+            self._bus = python_can.interface.Bus(
+                channel=self._iface, bustype="socketcan"
+            )
+            self.get_logger().info(f"CAN bus opened for recovery: {self._iface}")
+        except Exception as exc:
+            self.get_logger().warn(
+                f"Could not open CAN bus for recovery frames ({exc}); "
+                "health monitoring continues via topic subscription"
+            )
+
+    # ----------------------------------------------------------------
+    # State subscribers
+    # ----------------------------------------------------------------
+
+    def _on_left_state(self, msg: String) -> None:
+        self._handle_state_msg(msg, self._left_id)
+
+    def _on_right_state(self, msg: String) -> None:
+        self._handle_state_msg(msg, self._right_id)
+
+    def _handle_state_msg(self, msg: String, can_id: int) -> None:
+        try:
+            d = json.loads(msg.data)
+        except json.JSONDecodeError:
+            return
+
+        # The telemetry node sets "alive": true when the VESC is responding.
+        # We track our own timeout independently for tighter control.
+        now = time.monotonic()
+        if d.get("alive", False):
+            with self._fsm_lock:
+                actions = self._fsm.on_frame(can_id, now)
+            self._dispatch_actions(actions, can_id)
+
+    # ----------------------------------------------------------------
+    # Health timer (main watchdog loop)
+    # ----------------------------------------------------------------
+
+    def _on_health_tick(self) -> None:
+        now = time.monotonic()
+        with self._fsm_lock:
+            per_vesc = self._fsm.tick(now)
+
+        for can_id, actions in per_vesc.items():
+            self._dispatch_actions(actions, can_id)
+
+        self._publish_health(now)
+        self._publish_diagnostics(now)
+
+        # If any VESC is in e-stop, keep asserting zero velocity
+        with self._fsm_lock:
+            if self._fsm.any_estop:
+                self._pub_cmd.publish(Twist())  # zero Twist = stop
+
+    # ----------------------------------------------------------------
+    # Bus-off check
+    # ----------------------------------------------------------------
+
+    def _check_bus_off(self) -> None:
+        bus_off = self._is_can_bus_off()
+        now = time.monotonic()
+        with self._fsm_lock:
+            if bus_off:
+                actions = self._fsm.on_bus_off(now)
+            else:
+                actions = self._fsm.on_bus_ok(now)
+        self._dispatch_actions(actions, can_id=None)
+
+    def _is_can_bus_off(self) -> bool:
+        """Return True if the CAN interface is in bus-off state."""
+        try:
+            result = subprocess.run(
+                ["ip", "-json", "link", "show", self._iface],
+                capture_output=True, text=True, timeout=2.0
+            )
+            if result.returncode != 0:
+                return False
+            data = json.loads(result.stdout)
+            if not data:
+                return False
+            flags = data[0].get("flags", [])
+            # Linux SocketCAN sets "BUS-OFF" in the flags list
+            return "BUS-OFF" in [f.upper() for f in flags]
+        except Exception:
+            return False
+
+    # ----------------------------------------------------------------
+    # Action dispatcher
+    # ----------------------------------------------------------------
+
+    def _dispatch_actions(self, actions: list[Action], can_id: int | None) -> None:
+        label = self._id_to_label(can_id) if can_id is not None else "bus"
+        for action in actions:
+            if action == Action.SEND_ALIVE:
+                self._send_alive(can_id)
+            elif action == Action.TRIGGER_ESTOP:
+                self._on_estop_trigger(can_id, label)
+            elif action == Action.CLEAR_ESTOP:
+                self._on_estop_clear(can_id, label)
+            elif action == Action.LOG_WARN:
+                self.get_logger().warn(
+                    f"VESC {label} (id={can_id}): "
+                    "no CAN frames for >500 ms — sending alive frame"
+                )
+                self._log_fault_event("timeout", can_id, label)
+            elif action == Action.LOG_ERROR:
+                self.get_logger().error(
+                    f"VESC {label} (id={can_id}): "
+                    "unresponsive >2 s — e-stop triggered"
+                )
+                self._log_fault_event("estop", can_id, label)
+            elif action == Action.LOG_RECOVERY:
+                self.get_logger().info(
+                    f"VESC {label} (id={can_id}): CAN frames resumed — recovered"
+                )
+                self._log_fault_event("recovery", can_id, label)
+
+    def _on_estop_trigger(self, can_id: int | None, label: str) -> None:
+        self._pub_cmd.publish(Twist())  # immediate zero velocity
+        payload = json.dumps({
+            "event": "estop_triggered",
+            "can_id": can_id,
+            "label": label,
+            "stamp": time.time(),
+        })
+        self._pub_estop.publish(String(data=payload))
+
+    def _on_estop_clear(self, can_id: int | None, label: str) -> None:
+        payload = json.dumps({
+            "event": "estop_cleared",
+            "can_id": can_id,
+            "label": label,
+            "stamp": time.time(),
+        })
+        self._pub_estop.publish(String(data=payload))
+
+    # ----------------------------------------------------------------
+    # Alive / recovery frame
+    # ----------------------------------------------------------------
+
+    def _send_alive(self, can_id: int | None) -> None:
+        """Send GET_VALUES request to prompt telemetry refresh."""
+        if self._bus is None or can_id is None:
+            return
+        try:
+            arb_id, payload = _make_get_values_request(self._sender_id, can_id)
+            self._bus.send(python_can.Message(
+                arbitration_id=arb_id,
+                data=payload,
+                is_extended_id=True,
+            ))
+            self.get_logger().debug(
+                f"Sent GET_VALUES alive frame to VESC id={can_id}"
+            )
+        except Exception as exc:
+            self.get_logger().warn(f"Failed to send alive frame (id={can_id}): {exc}")
+
+    # ----------------------------------------------------------------
+    # Publishers
+    # ----------------------------------------------------------------
+
+    def _publish_health(self, now: float) -> None:
+        with self._fsm_lock:
+            left_state   = self._fsm.left.state.value
+            right_state  = self._fsm.right.state.value
+            left_elapsed  = self._fsm.left.elapsed(now)
+            right_elapsed = self._fsm.right.elapsed(now)
+            left_estop    = self._fsm.left.estop_active
+            right_estop   = self._fsm.right.estop_active
+            bus_off       = self._fsm._bus_off
+            any_estop     = self._fsm.any_estop
+
+        health = {
+            "stamp": time.time(),
+            "left": {
+                "can_id":       self._left_id,
+                "state":        left_state,
+                "elapsed_s":    round(min(left_elapsed, 9999.0), 3),
+                "estop_active": left_estop,
+            },
+            "right": {
+                "can_id":       self._right_id,
+                "state":        right_state,
+                "elapsed_s":    round(min(right_elapsed, 9999.0), 3),
+                "estop_active": right_estop,
+            },
+            "bus_off":     bus_off,
+            "estop_active": any_estop,
+        }
+
+        with self._fault_log_lock:
+            health["recent_faults"] = list(self._fault_log[-10:])
+
+        self._pub_health.publish(String(data=json.dumps(health)))
+
+    def _publish_diagnostics(self, now: float) -> None:
+        array = DiagnosticArray()
+        array.header.stamp = self.get_clock().now().to_msg()
+
+        with self._fsm_lock:
+            monitors = [
+                (self._fsm.left,  "left"),
+                (self._fsm.right, "right"),
+            ]
+
+        for mon, label in monitors:
+            status = DiagnosticStatus()
+            status.name        = f"VESC/health/{label} (CAN ID {mon.can_id})"
+            status.hardware_id = f"vesc_health_{mon.can_id}"
+
+            elapsed = mon.elapsed(now)
+
+            if mon.state == VescHealthState.ESTOP:
+                status.level   = DiagnosticStatus.ERROR
+                status.message = "E-STOP: VESC unresponsive >2 s"
+            elif mon.state == VescHealthState.BUS_OFF:
+                status.level   = DiagnosticStatus.ERROR
+                status.message = "CAN bus-off error"
+            elif mon.state == VescHealthState.DEGRADED:
+                status.level   = DiagnosticStatus.WARN
+                status.message = f"Frame timeout ({elapsed:.2f} s) — recovery active"
+            else:
+                status.level   = DiagnosticStatus.OK
+                status.message = "OK"
+
+            status.values = [
+                KeyValue(key="state",       value=mon.state.value),
+                KeyValue(key="elapsed_s",   value=f"{min(elapsed, 9999.0):.3f}"),
+                KeyValue(key="estop_active", value=str(mon.estop_active)),
+            ]
+            array.status.append(status)
+
+        self._pub_diag.publish(array)
+
+    # ----------------------------------------------------------------
+    # Fault event log
+    # ----------------------------------------------------------------
+
+    def _log_fault_event(self, event: str, can_id: int | None, label: str) -> None:
+        entry = {
+            "event":  event,
+            "can_id": can_id,
+            "label":  label,
+            "stamp":  time.time(),
+        }
+        with self._fault_log_lock:
+            self._fault_log.append(entry)
+            if len(self._fault_log) > 200:
+                self._fault_log = self._fault_log[-200:]
+
+    # ----------------------------------------------------------------
+    # Helpers
+    # ----------------------------------------------------------------
+
+    def _id_to_label(self, can_id: int | None) -> str:
+        if can_id == self._left_id:
+            return "left"
+        if can_id == self._right_id:
+            return "right"
+        return str(can_id)
+
+    # ----------------------------------------------------------------
+    # Cleanup
+    # ----------------------------------------------------------------
+
+    def destroy_node(self) -> None:
+        if self._bus is not None:
+            self._bus.shutdown()
+        super().destroy_node()
+
+
+# ---------------------------------------------------------------------------
+# Inline GET_VALUES request builder (avoids cross-package import at runtime)
+# ---------------------------------------------------------------------------
+
+_CAN_PACKET_PROCESS_SHORT_BUFFER = 32
+_COMM_GET_VALUES = 4
+
+
+def _make_get_values_request(sender_id: int, target_id: int) -> tuple[int, bytes]:
+    """Build a GET_VALUES short-buffer CAN frame for the given VESC."""
+    arb_id  = (_CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | (target_id & 0xFF)
+    payload = bytes([sender_id & 0xFF, 0x00, _COMM_GET_VALUES])
+    return arb_id, payload
+
+
+# ---------------------------------------------------------------------------
+# Entry point
+# ---------------------------------------------------------------------------
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = VescHealthMonitor()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/recovery_fsm.py

@@ -0,0 +1,223 @@
+"""VESC CAN health monitor — pure state machine.
+
+No ROS2, python-can, or hardware dependencies.  Safe to import in unit tests.
+
+State transitions per VESC
+--------------------------
+
+    HEALTHY  ──(elapsed > timeout_s = 0.5 s)──► DEGRADED  (send alive, log warn)
+                                                    │
+                    ◄──(frame received)─────────────┘ (log recovery)
+                                                    │
+                              (elapsed > estop_s = 2.0 s)──► ESTOP  (trigger e-stop)
+                                                                 │
+                    ◄──(frame received)──────────────────────────┘ (clear e-stop)
+
+    BUS_OFF is a parallel override: set by node when the CAN interface reports
+    bus-off; cleared when the interface recovers.  While active it always
+    produces TRIGGER_ESTOP.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from enum import Enum
+
+
+# ---------------------------------------------------------------------------
+# Enumerations
+# ---------------------------------------------------------------------------
+
+class VescHealthState(str, Enum):
+    HEALTHY  = "healthy"
+    DEGRADED = "degraded"   # >timeout_s, alive frames being sent
+    ESTOP    = "estop"      # >estop_s unresponsive, e-stop asserted
+    BUS_OFF  = "bus_off"    # CAN bus-off error
+
+
+class Action(str, Enum):
+    SEND_ALIVE       = "send_alive"       # send GET_VALUES request
+    TRIGGER_ESTOP    = "trigger_estop"    # publish zero cmd_vel / /estop
+    CLEAR_ESTOP      = "clear_estop"      # stop asserting e-stop
+    LOG_WARN         = "log_warn"         # WARN-level log
+    LOG_ERROR        = "log_error"        # ERROR-level log
+    LOG_RECOVERY     = "log_recovery"     # INFO-level recovery log
+
+
+# ---------------------------------------------------------------------------
+# Per-VESC state machine
+# ---------------------------------------------------------------------------
+
+@dataclass
+class VescMonitor:
+    """Monitors one VESC and drives the recovery state machine.
+
+    All timestamps are ``time.monotonic()`` seconds.
+
+    Usage::
+
+        mon = VescMonitor(can_id=61)
+        actions = mon.tick(now)           # call at ~10 Hz
+        actions = mon.on_frame(now)       # call when a CAN frame arrives
+    """
+
+    can_id: int
+    timeout_s: float = 0.5    # frame-drop detection threshold
+    estop_s:   float = 2.0    # escalation to e-stop threshold
+    alive_retry_s: float = 0.2  # interval between alive retries in DEGRADED
+
+    # Mutable runtime state (not init args)
+    state: VescHealthState = field(default=VescHealthState.HEALTHY, init=False)
+    last_frame_ts: float = field(default=0.0, init=False)       # 0 = never
+    _first_tick_ts: float = field(default=0.0, init=False)      # set on first tick
+    _last_alive_ts: float = field(default=0.0, init=False)
+
+    # ------------------------------------------------------------------
+
+    def on_frame(self, now: float) -> list[Action]:
+        """Call whenever a STATUS frame arrives from this VESC."""
+        self.last_frame_ts = now
+
+        prev = self.state
+        if prev in (VescHealthState.DEGRADED, VescHealthState.ESTOP,
+                    VescHealthState.BUS_OFF):
+            self.state = VescHealthState.HEALTHY
+            actions: list[Action] = [Action.LOG_RECOVERY]
+            if prev in (VescHealthState.ESTOP, VescHealthState.BUS_OFF):
+                actions.append(Action.CLEAR_ESTOP)
+            return actions
+        return []
+
+    def on_bus_off(self, now: float) -> list[Action]:
+        """Call when the CAN interface enters bus-off state."""
+        if self.state != VescHealthState.BUS_OFF:
+            self.state = VescHealthState.BUS_OFF
+            return [Action.LOG_ERROR, Action.TRIGGER_ESTOP]
+        return [Action.TRIGGER_ESTOP]
+
+    def on_bus_ok(self, now: float) -> list[Action]:
+        """Call when the CAN interface recovers from bus-off."""
+        if self.state == VescHealthState.BUS_OFF:
+            self.state = VescHealthState.HEALTHY
+            self.last_frame_ts = 0.0   # wait for first fresh frame
+            return [Action.LOG_RECOVERY, Action.CLEAR_ESTOP]
+        return []
+
+    def tick(self, now: float) -> list[Action]:
+        """Periodic update (~10 Hz).  Returns list of actions to take."""
+        # BUS_OFF and ESTOP keep asserting until a frame or bus recovery
+        if self.state == VescHealthState.BUS_OFF:
+            return [Action.TRIGGER_ESTOP]
+        if self.state == VescHealthState.ESTOP:
+            return [Action.TRIGGER_ESTOP]
+
+        # Track elapsed since startup for the "never received" case so we
+        # don't immediately e-stop on node startup before any frames arrive.
+        if self._first_tick_ts == 0.0:
+            self._first_tick_ts = now
+
+        if self.last_frame_ts > 0:
+            elapsed = now - self.last_frame_ts
+        else:
+            elapsed = now - self._first_tick_ts
+
+        if elapsed >= self.estop_s:
+            # Escalate to e-stop
+            if self.state != VescHealthState.ESTOP:
+                self.state = VescHealthState.ESTOP
+                return [Action.TRIGGER_ESTOP, Action.LOG_ERROR]
+            return [Action.TRIGGER_ESTOP]
+
+        if elapsed >= self.timeout_s:
+            if self.state == VescHealthState.HEALTHY:
+                # First timeout — transition to DEGRADED and send alive
+                self.state = VescHealthState.DEGRADED
+                self._last_alive_ts = now
+                return [Action.SEND_ALIVE, Action.LOG_WARN]
+            elif self.state == VescHealthState.DEGRADED:
+                # Retry alive at interval
+                if now - self._last_alive_ts >= self.alive_retry_s:
+                    self._last_alive_ts = now
+                    return [Action.SEND_ALIVE]
+            return []
+
+        # elapsed < timeout_s — connection is healthy
+        if self.state == VescHealthState.DEGRADED:
+            # Frame arrived (on_frame handles ESTOP/BUS_OFF → HEALTHY)
+            # If somehow we reach here still DEGRADED, clear it
+            self.state = VescHealthState.HEALTHY
+            return [Action.LOG_RECOVERY]
+
+        return []
+
+    # ------------------------------------------------------------------
+    # Introspection helpers
+    # ------------------------------------------------------------------
+
+    @property
+    def is_healthy(self) -> bool:
+        return self.state == VescHealthState.HEALTHY
+
+    @property
+    def estop_active(self) -> bool:
+        return self.state in (VescHealthState.ESTOP, VescHealthState.BUS_OFF)
+
+    def elapsed(self, now: float) -> float:
+        """Seconds since last frame (inf if never received)."""
+        return (now - self.last_frame_ts) if self.last_frame_ts > 0 else float("inf")
+
+
+# ---------------------------------------------------------------------------
+# Dual-VESC wrapper
+# ---------------------------------------------------------------------------
+
+@dataclass
+class HealthFsm:
+    """Manages two VescMonitor instances and aggregates bus-off state."""
+
+    left_id:  int = 61
+    right_id: int = 79
+    timeout_s: float = 0.5
+    estop_s:   float = 2.0
+
+    def __post_init__(self) -> None:
+        self.left  = VescMonitor(self.left_id,  self.timeout_s, self.estop_s)
+        self.right = VescMonitor(self.right_id, self.timeout_s, self.estop_s)
+        self._bus_off = False
+
+    def monitors(self) -> tuple[VescMonitor, VescMonitor]:
+        return self.left, self.right
+
+    def on_frame(self, can_id: int, now: float) -> list[Action]:
+        mon = self._get(can_id)
+        return mon.on_frame(now) if mon else []
+
+    def on_bus_off(self, now: float) -> list[Action]:
+        if not self._bus_off:
+            self._bus_off = True
+            return self.left.on_bus_off(now) + self.right.on_bus_off(now)
+        return [Action.TRIGGER_ESTOP]
+
+    def on_bus_ok(self, now: float) -> list[Action]:
+        if self._bus_off:
+            self._bus_off = False
+            return self.left.on_bus_ok(now) + self.right.on_bus_ok(now)
+        return []
+
+    def tick(self, now: float) -> dict[int, list[Action]]:
+        """Returns {can_id: [actions]} for each VESC."""
+        return {
+            self.left_id:  self.left.tick(now),
+            self.right_id: self.right.tick(now),
+        }
+
+    @property
+    def any_estop(self) -> bool:
+        return self.left.estop_active or self.right.estop_active
+
+    def _get(self, can_id: int) -> VescMonitor | None:
+        if can_id == self.left_id:
+            return self.left
+        if can_id == self.right_id:
+            return self.right
+        return None

jetson/ros2_ws/src/saltybot_vesc_health/setup.cfg

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

jetson/ros2_ws/src/saltybot_vesc_health/setup.py

@@ -0,0 +1,27 @@
+from setuptools import setup
+
+package_name = "saltybot_vesc_health"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/launch", ["launch/vesc_health.launch.py"]),
+        (f"share/{package_name}/config", ["config/vesc_health_params.yaml"]),
+    ],
+    install_requires=["setuptools", "python-can"],
+    zip_safe=True,
+    maintainer="sl-jetson",
+    maintainer_email="sl-jetson@saltylab.local",
+    description="VESC CAN health monitor with watchdog and auto-recovery",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "vesc_health_node = saltybot_vesc_health.health_monitor_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_vesc_health/test/test_vesc_health.py

@@ -0,0 +1,355 @@
+"""Unit tests for VESC CAN health monitor — recovery FSM.
+
+No ROS2, python-can, or hardware required.  Tests cover:
+  - VescMonitor state machine transitions
+  - Timeout → DEGRADED → ESTOP escalation
+  - Frame-received recovery from DEGRADED and ESTOP
+  - Bus-off override and recovery
+  - Alive retry interval in DEGRADED
+  - HealthFsm dual-VESC wrapper
+  - Edge cases (never seen, exact boundary, bus_ok when not bus_off)
+"""
+
+import pytest
+
+from saltybot_vesc_health.recovery_fsm import (
+    Action,
+    HealthFsm,
+    VescHealthState,
+    VescMonitor,
+)
+
+# ---------------------------------------------------------------------------
+# Helpers
+# ---------------------------------------------------------------------------
+
+T0 = 1_000.0  # arbitrary monotonic baseline (seconds)
+
+
+def fresh_mon(**kwargs) -> VescMonitor:
+    """Return a VescMonitor that has never seen a frame."""
+    return VescMonitor(can_id=61, **kwargs)
+
+
+def alive_mon(last_ts: float = T0, **kwargs) -> VescMonitor:
+    """Return a VescMonitor whose last frame was at last_ts."""
+    mon = VescMonitor(can_id=61, **kwargs)
+    mon.last_frame_ts = last_ts
+    return mon
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — initial state
+# ---------------------------------------------------------------------------
+
+class TestVescMonitorInit:
+    def test_starts_healthy(self):
+        assert fresh_mon().state == VescHealthState.HEALTHY
+
+    def test_never_received_is_unhealthy_after_timeout(self):
+        mon = fresh_mon(timeout_s=0.5)
+        # First tick records startup time; second tick 600ms later sees timeout
+        mon.tick(T0)                        # startup: elapsed=0, still healthy
+        actions = mon.tick(T0 + 0.6)       # 600 ms > timeout → DEGRADED
+        assert Action.SEND_ALIVE in actions
+        assert Action.LOG_WARN in actions
+        assert mon.state == VescHealthState.DEGRADED
+
+    def test_elapsed_never_received(self):
+        mon = fresh_mon()
+        assert mon.elapsed(T0) == float("inf")
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — healthy zone
+# ---------------------------------------------------------------------------
+
+class TestHealthyZone:
+    def test_no_actions_when_fresh_frames_arriving(self):
+        mon = alive_mon(last_ts=T0)
+        actions = mon.tick(T0 + 0.1)  # 100 ms — well within 500 ms
+        assert actions == []
+        assert mon.state == VescHealthState.HEALTHY
+
+    def test_no_transition_at_just_under_timeout(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5)
+        actions = mon.tick(T0 + 0.499)
+        assert actions == []
+        assert mon.state == VescHealthState.HEALTHY
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — timeout → DEGRADED
+# ---------------------------------------------------------------------------
+
+class TestTimeoutToDegraded:
+    def test_transitions_at_timeout(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5)
+        actions = mon.tick(T0 + 0.5)
+        assert mon.state == VescHealthState.DEGRADED
+        assert Action.SEND_ALIVE in actions
+        assert Action.LOG_WARN in actions
+
+    def test_send_alive_not_repeated_immediately(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5, alive_retry_s=0.2)
+        mon.tick(T0 + 0.5)   # → DEGRADED, sends alive
+        actions = mon.tick(T0 + 0.55)  # only 50 ms later → no retry yet
+        assert Action.SEND_ALIVE not in actions
+
+    def test_alive_retry_at_interval(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5, alive_retry_s=0.2)
+        mon.tick(T0 + 0.5)             # → DEGRADED, first alive
+        actions = mon.tick(T0 + 0.71)  # 210 ms later → retry
+        assert Action.SEND_ALIVE in actions
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — DEGRADED → ESTOP
+# ---------------------------------------------------------------------------
+
+class TestDegradedToEstop:
+    def test_escalates_at_estop_threshold(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
+        mon.tick(T0 + 0.5)   # → DEGRADED
+        actions = mon.tick(T0 + 2.0)
+        assert mon.state == VescHealthState.ESTOP
+        assert Action.TRIGGER_ESTOP in actions
+        assert Action.LOG_ERROR in actions
+
+    def test_estop_keeps_asserting(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
+        mon.tick(T0 + 0.5)
+        mon.tick(T0 + 2.0)   # → ESTOP
+        actions1 = mon.tick(T0 + 2.1)
+        actions2 = mon.tick(T0 + 3.0)
+        assert Action.TRIGGER_ESTOP in actions1
+        assert Action.TRIGGER_ESTOP in actions2
+
+    def test_no_duplicate_log_error_while_in_estop(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
+        mon.tick(T0 + 0.5)
+        actions_at_escalation = mon.tick(T0 + 2.0)   # LOG_ERROR here
+        assert Action.LOG_ERROR in actions_at_escalation
+        later_actions = mon.tick(T0 + 2.1)
+        assert Action.LOG_ERROR not in later_actions   # not repeated
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — recovery from DEGRADED
+# ---------------------------------------------------------------------------
+
+class TestRecoveryFromDegraded:
+    def test_on_frame_clears_degraded(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5)
+        mon.tick(T0 + 0.5)   # → DEGRADED
+        actions = mon.on_frame(T0 + 0.9)
+        assert mon.state == VescHealthState.HEALTHY
+        assert Action.LOG_RECOVERY in actions
+
+    def test_on_frame_does_not_trigger_clear_estop_from_degraded(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5)
+        mon.tick(T0 + 0.5)   # → DEGRADED (not ESTOP)
+        actions = mon.on_frame(T0 + 0.9)
+        assert Action.CLEAR_ESTOP not in actions
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — recovery from ESTOP
+# ---------------------------------------------------------------------------
+
+class TestRecoveryFromEstop:
+    def test_on_frame_clears_estop(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
+        mon.tick(T0 + 0.5)
+        mon.tick(T0 + 2.0)   # → ESTOP
+        actions = mon.on_frame(T0 + 2.5)
+        assert mon.state == VescHealthState.HEALTHY
+        assert Action.LOG_RECOVERY in actions
+        assert Action.CLEAR_ESTOP in actions
+
+    def test_tick_after_recovery_is_clean(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
+        mon.tick(T0 + 0.5)
+        mon.tick(T0 + 2.0)
+        mon.on_frame(T0 + 2.5)
+        actions = mon.tick(T0 + 2.6)
+        assert actions == []
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — bus-off
+# ---------------------------------------------------------------------------
+
+class TestBusOff:
+    def test_bus_off_triggers_estop(self):
+        mon = alive_mon(last_ts=T0)
+        actions = mon.on_bus_off(T0 + 1.0)
+        assert mon.state == VescHealthState.BUS_OFF
+        assert Action.TRIGGER_ESTOP in actions
+        assert Action.LOG_ERROR in actions
+
+    def test_bus_off_repeated_call_still_asserts_estop(self):
+        mon = alive_mon(last_ts=T0)
+        mon.on_bus_off(T0)
+        actions = mon.on_bus_off(T0 + 0.5)
+        assert Action.TRIGGER_ESTOP in actions
+
+    def test_bus_ok_clears_bus_off(self):
+        mon = alive_mon(last_ts=T0)
+        mon.on_bus_off(T0)
+        actions = mon.on_bus_ok(T0 + 1.0)
+        assert mon.state == VescHealthState.HEALTHY
+        assert Action.CLEAR_ESTOP in actions
+        assert Action.LOG_RECOVERY in actions
+
+    def test_bus_ok_when_not_bus_off_is_noop(self):
+        mon = alive_mon(last_ts=T0)
+        assert mon.on_bus_ok(T0) == []
+
+    def test_bus_off_tick_keeps_asserting_estop(self):
+        mon = alive_mon(last_ts=T0)
+        mon.on_bus_off(T0)
+        actions = mon.tick(T0 + 1.0)
+        assert Action.TRIGGER_ESTOP in actions
+
+    def test_on_frame_clears_bus_off(self):
+        mon = alive_mon(last_ts=T0)
+        mon.on_bus_off(T0)
+        actions = mon.on_frame(T0 + 0.5)
+        assert mon.state == VescHealthState.HEALTHY
+        assert Action.CLEAR_ESTOP in actions
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — elapsed helper
+# ---------------------------------------------------------------------------
+
+class TestElapsed:
+    def test_elapsed_after_frame(self):
+        mon = alive_mon(last_ts=T0)
+        assert mon.elapsed(T0 + 0.3) == pytest.approx(0.3)
+
+    def test_elapsed_never_received(self):
+        mon = fresh_mon()
+        assert mon.elapsed(T0) == float("inf")
+
+
+# ---------------------------------------------------------------------------
+# VescMonitor — estop_active property
+# ---------------------------------------------------------------------------
+
+class TestEstopActive:
+    def test_healthy_not_active(self):
+        assert not alive_mon().estop_active
+
+    def test_estop_state_is_active(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0)
+        mon.tick(T0 + 0.5)
+        mon.tick(T0 + 2.0)
+        assert mon.estop_active
+
+    def test_bus_off_is_active(self):
+        mon = alive_mon(last_ts=T0)
+        mon.on_bus_off(T0)
+        assert mon.estop_active
+
+    def test_degraded_not_active(self):
+        mon = alive_mon(last_ts=T0, timeout_s=0.5)
+        mon.tick(T0 + 0.5)
+        assert not mon.estop_active
+
+
+# ---------------------------------------------------------------------------
+# HealthFsm — dual-VESC wrapper
+# ---------------------------------------------------------------------------
+
+class TestHealthFsm:
+    def _make_fsm(self) -> HealthFsm:
+        fsm = HealthFsm(left_id=61, right_id=79, timeout_s=0.5, estop_s=2.0)
+        fsm.left.last_frame_ts  = T0
+        fsm.right.last_frame_ts = T0
+        return fsm
+
+    def test_tick_returns_per_vesc_actions(self):
+        fsm = self._make_fsm()
+        result = fsm.tick(T0 + 0.1)
+        assert 61 in result
+        assert 79 in result
+
+    def test_on_frame_updates_left(self):
+        fsm = self._make_fsm()
+        fsm.left.last_frame_ts = T0 - 1.0  # stale
+        fsm.left.state = VescHealthState.DEGRADED
+        actions = fsm.on_frame(61, T0 + 0.1)
+        assert Action.LOG_RECOVERY in actions
+        assert fsm.left.state == VescHealthState.HEALTHY
+
+    def test_on_frame_unknown_id_is_noop(self):
+        fsm = self._make_fsm()
+        assert fsm.on_frame(99, T0) == []
+
+    def test_any_estop_true_when_one_vesc_estop(self):
+        fsm = self._make_fsm()
+        fsm.left.last_frame_ts = T0 - 3.0
+        fsm.tick(T0)   # should escalate left to ESTOP
+        assert fsm.any_estop
+
+    def test_any_estop_false_when_both_healthy(self):
+        fsm = self._make_fsm()
+        fsm.tick(T0 + 0.1)
+        assert not fsm.any_estop
+
+    def test_bus_off_propagates_to_both(self):
+        fsm = self._make_fsm()
+        actions = fsm.on_bus_off(T0)
+        assert fsm.left.state  == VescHealthState.BUS_OFF
+        assert fsm.right.state == VescHealthState.BUS_OFF
+        assert any(a == Action.TRIGGER_ESTOP for a in actions)
+
+    def test_bus_ok_clears_both(self):
+        fsm = self._make_fsm()
+        fsm.on_bus_off(T0)
+        actions = fsm.on_bus_ok(T0 + 1.0)
+        assert fsm.left.state  == VescHealthState.HEALTHY
+        assert fsm.right.state == VescHealthState.HEALTHY
+        assert any(a == Action.CLEAR_ESTOP for a in actions)
+
+    def test_bus_ok_when_not_bus_off_is_noop(self):
+        fsm = self._make_fsm()
+        assert fsm.on_bus_ok(T0) == []
+
+    def test_left_timeout_right_healthy(self):
+        fsm = self._make_fsm()
+        fsm.left.last_frame_ts = T0 - 1.0   # stale
+        result = fsm.tick(T0)
+        assert Action.SEND_ALIVE in result[61]
+        assert result[79] == []
+
+
+# ---------------------------------------------------------------------------
+# _make_get_values_request (inline copy in node module)
+# ---------------------------------------------------------------------------
+
+class TestMakeGetValuesRequest:
+    def test_arb_id_encodes_target(self):
+        from saltybot_vesc_health.health_monitor_node import _make_get_values_request
+        arb_id, _ = _make_get_values_request(127, 61)
+        assert (arb_id & 0xFF) == 61
+
+    def test_payload_has_sender_and_comm_id(self):
+        from saltybot_vesc_health.health_monitor_node import (
+            _make_get_values_request,
+            _COMM_GET_VALUES,
+        )
+        _, payload = _make_get_values_request(42, 61)
+        assert payload[0] == 42
+        assert payload[2] == _COMM_GET_VALUES
+
+    def test_arb_id_is_extended(self):
+        from saltybot_vesc_health.health_monitor_node import _make_get_values_request
+        arb_id, _ = _make_get_values_request(127, 61)
+        assert arb_id > 0x7FF
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_vesc_telemetry/config/vesc_telemetry_params.yaml

@@ -7,10 +7,10 @@ vesc_telemetry:
     can_interface: "can0"
 
     # CAN IDs assigned to each VESC in VESC Tool
-    # Left motor VESC:  ID 61  (0x3D)
-    # Right motor VESC: ID 79  (0x4F)
-    left_can_id:  61
-    right_can_id: 79
+    # Left motor VESC:  ID 56  (0x38)
+    # Right motor VESC: ID 68  (0x44)
+    left_can_id:  56
+    right_can_id: 68
 
     # This node's CAN ID used as sender_id in GET_VALUES requests (0-127)
     sender_can_id: 127

jetson/ros2_ws/src/saltybot_vesc_telemetry/saltybot_vesc_telemetry/vesc_telemetry_node.py

@@ -15,8 +15,8 @@ Published topics
 Parameters
 ----------
   can_interface              str   'can0'   SocketCAN interface
-  left_can_id                int   61       CAN ID of left VESC
-  right_can_id               int   79       CAN ID of right VESC
+  left_can_id                int   56       CAN ID of left VESC
+  right_can_id               int   68       CAN ID of right VESC
   poll_rate_hz               int   20       Publish + request rate (10-50 Hz)
   sender_can_id              int   127      This node's CAN ID
   overcurrent_threshold_a    float 60.0     Fault alert threshold (A)
@@ -62,8 +62,8 @@ class VescTelemetryNode(Node):
         # Parameters
         # ----------------------------------------------------------------
         self.declare_parameter("can_interface",              "can0")
-        self.declare_parameter("left_can_id",                61)
-        self.declare_parameter("right_can_id",               79)
+        self.declare_parameter("left_can_id",                56)
+        self.declare_parameter("right_can_id",               68)
         self.declare_parameter("poll_rate_hz",               20)
         self.declare_parameter("sender_can_id",              127)
         self.declare_parameter("overcurrent_threshold_a",    60.0)

jetson/ros2_ws/src/saltybot_vesc_telemetry/test/test_vesc_telemetry.py

@@ -141,29 +141,29 @@ class TestParseStatus5:
 
 class TestMakeGetValuesRequest:
     def test_arb_id_encodes_packet_type_and_target(self):
-        arb_id, payload = make_get_values_request(sender_id=127, target_id=61)
-        expected_arb = (CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | 61
+        arb_id, payload = make_get_values_request(sender_id=127, target_id=56)
+        expected_arb = (CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | 56
         assert arb_id == expected_arb
 
     def test_payload_contains_sender_command(self):
-        _, payload = make_get_values_request(sender_id=127, target_id=61)
+        _, payload = make_get_values_request(sender_id=127, target_id=56)
         assert len(payload) == 3
         assert payload[0] == 127          # sender_id
         assert payload[1] == 0x00         # send_mode
         assert payload[2] == COMM_GET_VALUES
 
     def test_right_vesc(self):
-        arb_id, payload = make_get_values_request(sender_id=127, target_id=79)
-        assert (arb_id & 0xFF) == 79
+        arb_id, payload = make_get_values_request(sender_id=127, target_id=68)
+        assert (arb_id & 0xFF) == 68
         assert payload[2] == COMM_GET_VALUES
 
     def test_sender_id_in_payload(self):
-        _, payload = make_get_values_request(sender_id=42, target_id=61)
+        _, payload = make_get_values_request(sender_id=42, target_id=56)
         assert payload[0] == 42
 
     def test_arb_id_is_extended(self):
         # Extended IDs can exceed 0x7FF (11-bit limit)
-        arb_id, _ = make_get_values_request(127, 61)
+        arb_id, _ = make_get_values_request(127, 56)
         assert arb_id > 0x7FF
 
 
@@ -173,7 +173,7 @@ class TestMakeGetValuesRequest:
 
 class TestVescState:
     def test_defaults(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         assert s.rpm           == 0
         assert s.current_a     == pytest.approx(0.0)
         assert s.voltage_v     == pytest.approx(0.0)
@@ -181,42 +181,42 @@ class TestVescState:
         assert s.has_fault     is False
 
     def test_is_alive_fresh(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         s.last_status_ts = time.monotonic()
         assert s.is_alive is True
 
     def test_is_alive_stale(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         s.last_status_ts = time.monotonic() - 2.0  # 2 s ago > 1 s timeout
         assert s.is_alive is False
 
     def test_is_alive_never_received(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         # last_status_ts defaults to 0.0 — monotonic() will be >> 1 s
         assert s.is_alive is False
 
     def test_has_fault_true(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         s.fault_code = FAULT_CODE_ABS_OVER_CURRENT
         assert s.has_fault is True
 
     def test_has_fault_false_on_none(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         s.fault_code = FAULT_CODE_NONE
         assert s.has_fault is False
 
     def test_fault_name_known(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         s.fault_code = FAULT_CODE_OVER_TEMP_FET
         assert s.fault_name == "OVER_TEMP_FET"
 
     def test_fault_name_unknown(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         s.fault_code = 99
         assert "UNKNOWN" in s.fault_name
 
     def test_fault_name_none(self):
-        s = VescState(can_id=61)
+        s = VescState(can_id=56)
         assert s.fault_name == "NONE"
 
 

jetson/scripts/setup_can.sh

@@ -0,0 +1,126 @@
+#!/usr/bin/env bash
+# setup_can.sh — Bring up CANable 2.0 (slcan/ttyACM0) as slcan0 at 500 kbps
+# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674
+#
+# This script uses slcand to expose the CANable 2.0 (USB CDC / slcan firmware)
+# as a SocketCAN interface.  It is NOT used for the gs_usb (native SocketCAN)
+# firmware variant — use jetson/scripts/can_setup.sh for that.
+#
+# Usage:
+#   sudo ./setup_can.sh           # bring up slcan0
+#   sudo ./setup_can.sh down      # bring down slcan0 and kill slcand
+#   sudo ./setup_can.sh verify    # candump one-shot check (Ctrl-C to stop)
+#
+# Environment overrides:
+#   CAN_DEVICE   — serial device  (default: /dev/ttyACM0)
+#   CAN_IFACE    — SocketCAN name (default: slcan0)
+#   CAN_BITRATE  — bit rate       (default: 500000)
+#
+# Mamba CAN ID: 1 (0x001)
+# VESC left:    56 (0x038)
+# VESC right:   68 (0x044)
+
+set -euo pipefail
+
+DEVICE="${CAN_DEVICE:-/dev/ttyACM0}"
+IFACE="${CAN_IFACE:-slcan0}"
+BITRATE="${CAN_BITRATE:-500000}"
+
+log()  { echo "[setup_can] $*"; }
+warn() { echo "[setup_can] WARNING: $*" >&2; }
+die()  { echo "[setup_can] ERROR: $*" >&2; exit 1; }
+
+# Map numeric bitrate to slcand -s flag (0-8 map to standard CAN speeds)
+bitrate_flag() {
+    case "$1" in
+        10000)   echo "0" ;;
+        20000)   echo "1" ;;
+        50000)   echo "2" ;;
+        100000)  echo "3" ;;
+        125000)  echo "4" ;;
+        250000)  echo "5" ;;
+        500000)  echo "6" ;;
+        800000)  echo "7" ;;
+        1000000) echo "8" ;;
+        *) die "Unsupported bitrate: $1 (choose 10k–1M)" ;;
+    esac
+}
+
+# ── up ─────────────────────────────────────────────────────────────────────
+cmd_up() {
+    # Verify serial device is present
+    if [[ ! -c "$DEVICE" ]]; then
+        die "$DEVICE not found — is CANable 2.0 plugged in?"
+    fi
+
+    # If interface already exists, leave it alone
+    if ip link show "$IFACE" &>/dev/null; then
+        log "$IFACE is already up — nothing to do."
+        ip -details link show "$IFACE"
+        return 0
+    fi
+
+    local sflag
+    sflag=$(bitrate_flag "$BITRATE")
+
+    log "Starting slcand on $DEVICE → $IFACE at ${BITRATE} bps (flag -s${sflag}) …"
+    # -o open device, -c close on exit, -f forced, -s<N> speed, -S<baud> serial baud
+    slcand -o -c -f -s"${sflag}" -S 3000000 "$DEVICE" "$IFACE" \
+        || die "slcand failed to start"
+
+    # Give slcand a moment to create the netdev
+    local retries=0
+    while ! ip link show "$IFACE" &>/dev/null; do
+        retries=$((retries + 1))
+        if [[ $retries -ge 10 ]]; then
+            die "Timed out waiting for $IFACE to appear after slcand start"
+        fi
+        sleep 0.2
+    done
+
+    log "Bringing up $IFACE …"
+    ip link set "$IFACE" up \
+        || die "ip link set $IFACE up failed"
+
+    log "$IFACE is up."
+    ip -details link show "$IFACE"
+}
+
+# ── down ───────────────────────────────────────────────────────────────────
+cmd_down() {
+    log "Bringing down $IFACE …"
+    if ip link show "$IFACE" &>/dev/null; then
+        ip link set "$IFACE" down || warn "Could not set $IFACE down"
+    else
+        warn "$IFACE not found — already down?"
+    fi
+
+    # Kill any running slcand instances bound to our device
+    if pgrep -f "slcand.*${DEVICE}" &>/dev/null; then
+        log "Stopping slcand for $DEVICE …"
+        pkill -f "slcand.*${DEVICE}" || warn "pkill returned non-zero"
+    fi
+    log "Done."
+}
+
+# ── verify ─────────────────────────────────────────────────────────────────
+cmd_verify() {
+    if ! ip link show "$IFACE" &>/dev/null; then
+        die "$IFACE is not up — run '$0 up' first"
+    fi
+    log "Listening on $IFACE — expecting frames from Mamba (0x001), VESC left (0x038), VESC right (0x044)"
+    log "Press Ctrl-C to stop."
+    exec candump "$IFACE"
+}
+
+# ── main ───────────────────────────────────────────────────────────────────
+CMD="${1:-up}"
+case "$CMD" in
+    up)     cmd_up     ;;
+    down)   cmd_down   ;;
+    verify) cmd_verify ;;
+    *)
+        echo "Usage: $0 [up|down|verify]"
+        exit 1
+        ;;
+esac

lib/USB_CDC/src/usbd_cdc_if.c

@@ -8,6 +8,7 @@ static volatile uint8_t cdc_port_open = 0;  /* set when host asserts DTR */
 volatile uint8_t  cdc_arm_request = 0;    /* set by A command */
 volatile uint8_t  cdc_disarm_request = 0; /* set by D command */
 volatile uint8_t  cdc_recal_request = 0;  /* set by G command — gyro recalibration */
+volatile uint8_t  cdc_imu_cal_request = 0; /* set by O command — mount offset calibration (Issue #680) */
 volatile uint32_t cdc_rx_count = 0;       /* total CDC packets received from host */
 
 volatile uint8_t  cdc_estop_request = 0;
@@ -143,6 +144,7 @@ static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
         case 'A': cdc_arm_request = 1;            break;
         case 'D': cdc_disarm_request = 1;         break;
         case 'G': cdc_recal_request = 1;          break; /* gyro recalibration */
+        case 'O': cdc_imu_cal_request = 1;        break; /* mount offset cal (Issue #680) */
         case 'R': request_bootloader();            break; /* never returns */
 
         case 'E': cdc_estop_request = 1;       break;

platformio.ini

@@ -16,3 +16,4 @@ build_flags =
     -Os
     -Wl,--defsym,_Min_Heap_Size=0x2000
     -Wl,--defsym,_Min_Stack_Size=0x1000
+    -Wl,--defsym,__stack_end=_estack-0x1000

src/balance.c

@@ -96,3 +96,26 @@ void balance_disarm(balance_t *b) {
     b->integral = 0.0f;
     slope_estimator_reset(&b->slope);
 }
+
+void balance_park(balance_t *b) {
+    /* Suspend balancing from ARMED state only — keeps robot stationary on flat ground */
+    if (b->state == BALANCE_ARMED) {
+        b->state     = BALANCE_PARKED;
+        b->motor_cmd = 0;
+        b->integral  = 0.0f;
+        b->prev_error = 0.0f;
+        slope_estimator_reset(&b->slope);
+    }
+}
+
+void balance_unpark(balance_t *b) {
+    /* Quick re-arm from PARKED — only if pitch is safe (< 20 deg) */
+    if (b->state == BALANCE_PARKED) {
+        if (fabsf(b->pitch_deg) < 20.0f) {
+            b->motor_cmd  = 0;
+            b->prev_error = 0.0f;
+            b->state      = BALANCE_ARMED;
+        }
+        /* If pitch too large, stay PARKED — caller checks resulting state */
+    }
+}

src/can_driver.c

@@ -1,6 +1,11 @@
 /* CAN bus driver for BLDC motor controllers (Issue #597)
- * CAN2 on PB12 (RX, AF9) / PB13 (TX, AF9) at 500 kbps
- * Filter bank 14 (first CAN2 bank; SlaveStartFilterBank=14)
+ * CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) at 500 kbps   (Issue #676 remap)
+ * Filter bank 0 (CAN1 master; SlaveStartFilterBank=14)
+ *
+ * NOTE: Mamba F722S MK2 does not expose PB12/PB13 externally.
+ * Waveshare CAN module wired to SCL pad (PB8 = CAN1_RX) and
+ * SDA pad (PB9 = CAN1_TX).  I2C1 is free (BME280 moved to I2C2).
+ * CAN1 uses AF9 on PB8/PB9 — no conflict with other active peripherals.
  */
 
 #include "can_driver.h"
@@ -10,27 +15,27 @@
 static CAN_HandleTypeDef        s_can;
 static volatile can_feedback_t  s_feedback[CAN_NUM_MOTORS];
 static volatile can_stats_t     s_stats;
+static can_ext_frame_cb_t       s_ext_cb = NULL;
+static can_std_frame_cb_t       s_std_cb = NULL;
 
 void can_driver_init(void)
 {
-    /* CAN2 requires CAN1 master clock for shared filter RAM */
     __HAL_RCC_CAN1_CLK_ENABLE();
-    __HAL_RCC_CAN2_CLK_ENABLE();
     __HAL_RCC_GPIOB_CLK_ENABLE();
 
-    /* PB12 = CAN2_RX, PB13 = CAN2_TX, AF9 */
+    /* PB8 = CAN1_RX, PB9 = CAN1_TX, AF9 (Issue #676) */
     GPIO_InitTypeDef gpio = {0};
-    gpio.Pin       = GPIO_PIN_12 | GPIO_PIN_13;
+    gpio.Pin       = GPIO_PIN_8 | GPIO_PIN_9;
     gpio.Mode      = GPIO_MODE_AF_PP;
     gpio.Pull      = GPIO_NOPULL;
     gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
-    gpio.Alternate = GPIO_AF9_CAN2;
+    gpio.Alternate = GPIO_AF9_CAN1;
     HAL_GPIO_Init(GPIOB, &gpio);
 
     /* 500 kbps @ APB1=54 MHz: PSC=6, BS1=13tq, BS2=4tq, SJW=1tq
      * bit_time = 6 × (1+13+4) / 54000000 = 2 µs → 500 kbps
      * sample point = (1+13)/18 = 77.8% */
-    s_can.Instance                  = CAN2;
+    s_can.Instance                  = CAN1;
     s_can.Init.Prescaler            = CAN_PRESCALER;
     s_can.Init.Mode                 = CAN_MODE_NORMAL;
     s_can.Init.SyncJumpWidth        = CAN_SJW_1TQ;
@@ -48,16 +53,16 @@ void can_driver_init(void)
         return;
     }
 
-    /* Filter bank 14: 32-bit mask, FIFO0, accept std IDs 0x200–0x21F
-     * FilterIdHigh   = 0x200 << 5 = 0x4000  (base ID shifted to bit[15:5])
-     * FilterMaskHigh = 0x7E0 << 5 = 0xFC00  (mask: top 6 bits must match) */
+    /* Filter bank 0: 32-bit mask, FIFO0, accept ALL standard 11-bit frames.
+     * CAN1 is master (SlaveStartFilterBank=14). FilterIdHigh=0, MaskHigh=0
+     * → mask=0 passes every standard ID. Orin std-frame commands land here. */
     CAN_FilterTypeDef flt = {0};
-    flt.FilterBank           = 14u;
+    flt.FilterBank           = 0u;
     flt.FilterMode           = CAN_FILTERMODE_IDMASK;
     flt.FilterScale          = CAN_FILTERSCALE_32BIT;
-    flt.FilterIdHigh         = (uint16_t)(CAN_FILTER_STDID << 5u);
+    flt.FilterIdHigh         = 0u;
     flt.FilterIdLow          = 0u;
-    flt.FilterMaskIdHigh     = (uint16_t)(CAN_FILTER_MASK  << 5u);
+    flt.FilterMaskIdHigh     = 0u;
     flt.FilterMaskIdLow      = 0u;
     flt.FilterFIFOAssignment = CAN_RX_FIFO0;
     flt.FilterActivation     = CAN_FILTER_ENABLE;
@@ -68,6 +73,28 @@ void can_driver_init(void)
         return;
     }
 
+    /* Filter bank 15: 32-bit mask, FIFO1, accept ALL extended 29-bit frames.
+     * For extended frames, the IDE bit sits at FilterIdLow[2].
+     * FilterIdLow  = 0x0004 (IDE=1) → match extended frames.
+     * FilterMaskIdLow = 0x0004 → only the IDE bit is checked; all ext IDs pass.
+     * This routes every VESC STATUS / STATUS_4 / STATUS_5 frame to FIFO1. */
+    CAN_FilterTypeDef flt2 = {0};
+    flt2.FilterBank           = 15u;
+    flt2.FilterMode           = CAN_FILTERMODE_IDMASK;
+    flt2.FilterScale          = CAN_FILTERSCALE_32BIT;
+    flt2.FilterIdHigh         = 0u;
+    flt2.FilterIdLow          = 0x0004u;  /* IDE = 1 */
+    flt2.FilterMaskIdHigh     = 0u;
+    flt2.FilterMaskIdLow      = 0x0004u;  /* only check IDE bit */
+    flt2.FilterFIFOAssignment = CAN_RX_FIFO1;
+    flt2.FilterActivation     = CAN_FILTER_ENABLE;
+    flt2.SlaveStartFilterBank = 14u;
+
+    if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) {
+        s_stats.bus_off = 1u;
+        return;
+    }
+
     HAL_CAN_Start(&s_can);
 
     memset((void *)s_feedback, 0, sizeof(s_feedback));
@@ -136,7 +163,7 @@ void can_driver_process(void)
     }
     s_stats.bus_off = 0u;
 
-    /* Drain FIFO0 */
+    /* Drain FIFO0 (standard-ID frames: Orin commands + legacy feedback) */
     while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) {
         CAN_RxHeaderTypeDef rxhdr;
         uint8_t rxdata[8];
@@ -146,29 +173,104 @@ void can_driver_process(void)
             break;
         }
 
-        /* Only process data frames with standard IDs */
         if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) {
             continue;
         }
 
-        /* Decode feedback frames: base 0x200, one per node */
-        uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
-        if (nid_u >= CAN_NUM_MOTORS || rxhdr.DLC < 8u) {
-            continue;
+        /* Dispatch to registered standard-frame callback (Orin CAN protocol) */
+        if (s_std_cb != NULL) {
+            s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
         }
 
+        /* Legacy: decode feedback frames at 0x200-0x21F (Issue #597) */
+        uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
+        if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) {
             uint8_t nid = (uint8_t)nid_u;
-
-        /* Layout: [0-1] vel_rpm  [2-3] current_ma  [4-5] pos_x100  [6] temp_c  [7] fault */
             s_feedback[nid].velocity_rpm  = (int16_t)((uint16_t)rxdata[0] | ((uint16_t)rxdata[1] << 8u));
             s_feedback[nid].current_ma    = (int16_t)((uint16_t)rxdata[2] | ((uint16_t)rxdata[3] << 8u));
             s_feedback[nid].position_x100 = (int16_t)((uint16_t)rxdata[4] | ((uint16_t)rxdata[5] << 8u));
             s_feedback[nid].temperature_c = (int8_t)rxdata[6];
             s_feedback[nid].fault         = rxdata[7];
             s_feedback[nid].last_rx_ms    = HAL_GetTick();
+        }
 
         s_stats.rx_count++;
     }
+
+    /* Drain FIFO1 (extended-ID frames: VESC STATUS/STATUS_4/STATUS_5) */
+    while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) {
+        CAN_RxHeaderTypeDef rxhdr;
+        uint8_t rxdata[8];
+
+        if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) {
+            s_stats.err_count++;
+            break;
+        }
+
+        if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) {
+            continue;
+        }
+
+        if (s_ext_cb != NULL) {
+            s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
+        }
+
+        s_stats.rx_count++;
+    }
+}
+
+void can_driver_set_ext_cb(can_ext_frame_cb_t cb)
+{
+    s_ext_cb = cb;
+}
+
+void can_driver_set_std_cb(can_std_frame_cb_t cb)
+{
+    s_std_cb = cb;
+}
+
+void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len)
+{
+    if (s_stats.bus_off || len > 8u) {
+        return;
+    }
+
+    CAN_TxHeaderTypeDef hdr = {0};
+    hdr.ExtId = ext_id;
+    hdr.IDE   = CAN_ID_EXT;
+    hdr.RTR   = CAN_RTR_DATA;
+    hdr.DLC   = len;
+
+    uint32_t mailbox;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
+        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
+            s_stats.tx_count++;
+        } else {
+            s_stats.err_count++;
+        }
+    }
+}
+
+void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len)
+{
+    if (s_stats.bus_off || len > 8u) {
+        return;
+    }
+
+    CAN_TxHeaderTypeDef hdr = {0};
+    hdr.StdId = std_id;
+    hdr.IDE   = CAN_ID_STD;
+    hdr.RTR   = CAN_RTR_DATA;
+    hdr.DLC   = len;
+
+    uint32_t mailbox;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
+        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
+            s_stats.tx_count++;
+        } else {
+            s_stats.err_count++;
+        }
+    }
 }
 
 bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out)

src/hw_button.c

@@ -0,0 +1,179 @@
+/* hw_button.c — hardware button debounce + gesture detection (Issue #682)
+ *
+ * Debounce FSM:
+ *   IDLE     → (raw press detected)       → DEBOUNCING
+ *   DEBOUNCING → (still pressed after BTN_DEBOUNCE_MS) → HELD
+ *   HELD     → (released)                → classify press type, back to IDLE
+ *
+ * Press types:
+ *   SHORT  held < BTN_LONG_MIN_MS from confirmed start
+ *   LONG   held >= BTN_LONG_MIN_MS
+ *
+ * Sequence detection (operates on classified presses):
+ *   Buffer up to 3 presses.  Recognised patterns:
+ *     [SHORT, SHORT, LONG]  ->  BTN_EVENT_REARM_COMBO  (fires on LONG release)
+ *     [SHORT] + BTN_COMMIT_MS timeout  ->  BTN_EVENT_PARK
+ *   Sequence reset after BTN_SEQ_TIMEOUT_MS from first press.
+ */
+
+#include "hw_button.h"
+#include "config.h"
+
+#ifndef TEST_HOST
+#include "stm32f7xx_hal.h"
+#endif
+
+/* ---- Timing defaults (override in config.h) ---- */
+#ifndef BTN_DEBOUNCE_MS
+#define BTN_DEBOUNCE_MS    20u
+#endif
+#ifndef BTN_LONG_MIN_MS
+#define BTN_LONG_MIN_MS    1500u
+#endif
+#ifndef BTN_COMMIT_MS
+#define BTN_COMMIT_MS      500u
+#endif
+#ifndef BTN_SEQ_TIMEOUT_MS
+#define BTN_SEQ_TIMEOUT_MS 3000u
+#endif
+
+/* ---- Press type ---- */
+typedef enum {
+    _PT_SHORT = 1u,
+    _PT_LONG  = 2u,
+} _press_type_t;
+
+/* ---- Debounce state ---- */
+typedef enum {
+    _BTN_IDLE,
+    _BTN_DEBOUNCING,
+    _BTN_HELD,
+} _btn_state_t;
+
+static _btn_state_t s_state     = _BTN_IDLE;
+static uint32_t     s_trans_ms  = 0u;   /* timestamp of last FSM transition */
+static bool         s_pressed   = false;
+
+/* ---- Sequence buffer ---- */
+#define _SEQ_MAX 3u
+static _press_type_t s_seq[_SEQ_MAX];
+static uint8_t       s_seq_len       = 0u;
+static uint32_t      s_seq_first_ms  = 0u;
+static uint32_t      s_seq_last_ms   = 0u;
+
+/* ---- GPIO read ---- */
+#ifdef TEST_HOST
+static bool s_test_raw = false;
+void hw_button_inject(bool pressed) { s_test_raw = pressed; }
+static bool _read_raw(void) { return s_test_raw; }
+#else
+static bool _read_raw(void)
+{
+    return HAL_GPIO_ReadPin(BTN_PORT, BTN_PIN) == GPIO_PIN_RESET; /* active-low */
+}
+#endif
+
+void hw_button_init(void)
+{
+#ifndef TEST_HOST
+    __HAL_RCC_GPIOC_CLK_ENABLE();      /* BTN_PORT assumed GPIOC; adjust if needed */
+    GPIO_InitTypeDef g = {0};
+    g.Pin  = BTN_PIN;
+    g.Mode = GPIO_MODE_INPUT;
+    g.Pull = GPIO_PULLUP;
+    HAL_GPIO_Init(BTN_PORT, &g);
+#endif
+    s_state    = _BTN_IDLE;
+    s_seq_len  = 0u;
+    s_pressed  = false;
+}
+
+bool hw_button_is_pressed(void)
+{
+    return s_pressed;
+}
+
+/* Record a classified press into the sequence buffer and check for patterns. */
+static hw_btn_event_t _record_press(_press_type_t pt, uint32_t now_ms)
+{
+    if (s_seq_len == 0u) {
+        s_seq_first_ms = now_ms;
+    }
+    s_seq_last_ms = now_ms;
+
+    if (s_seq_len < _SEQ_MAX) {
+        s_seq[s_seq_len++] = pt;
+    }
+
+    /* Check REARM_COMBO: SHORT + SHORT + LONG */
+    if (s_seq_len == 3u &&
+        s_seq[0] == _PT_SHORT &&
+        s_seq[1] == _PT_SHORT &&
+        s_seq[2] == _PT_LONG) {
+        s_seq_len = 0u;
+        return BTN_EVENT_REARM_COMBO;
+    }
+
+    return BTN_EVENT_NONE;
+}
+
+hw_btn_event_t hw_button_tick(uint32_t now_ms)
+{
+    bool raw = _read_raw();
+
+    /* ---- Debounce FSM ---- */
+    switch (s_state) {
+    case _BTN_IDLE:
+        if (raw) {
+            s_state    = _BTN_DEBOUNCING;
+            s_trans_ms = now_ms;
+        }
+        break;
+
+    case _BTN_DEBOUNCING:
+        if (!raw) {
+            /* Released before debounce elapsed — bounce, ignore */
+            s_state = _BTN_IDLE;
+        } else if ((now_ms - s_trans_ms) >= BTN_DEBOUNCE_MS) {
+            s_state   = _BTN_HELD;
+            s_trans_ms = now_ms;   /* record confirmed press-start time */
+            s_pressed  = true;
+        }
+        break;
+
+    case _BTN_HELD:
+        if (!raw) {
+            s_pressed  = false;
+            uint32_t held_ms = now_ms - s_trans_ms;
+            _press_type_t pt = (held_ms >= BTN_LONG_MIN_MS) ? _PT_LONG : _PT_SHORT;
+            s_state = _BTN_IDLE;
+            hw_btn_event_t ev = _record_press(pt, now_ms);
+            if (ev != BTN_EVENT_NONE) {
+                return ev;
+            }
+        }
+        break;
+    }
+
+    /* ---- Sequence timeout / commit check (only when not currently held) ---- */
+    if (s_state == _BTN_IDLE && s_seq_len > 0u) {
+        uint32_t since_first = now_ms - s_seq_first_ms;
+        uint32_t since_last  = now_ms - s_seq_last_ms;
+
+        /* Whole sequence window expired — abandon */
+        if (since_first >= BTN_SEQ_TIMEOUT_MS) {
+            s_seq_len = 0u;
+            return BTN_EVENT_NONE;
+        }
+
+        /* Single short press + BTN_COMMIT_MS of quiet -> PARK */
+        if (s_seq_len == 1u &&
+            s_seq[0] == _PT_SHORT &&
+            since_last >= BTN_COMMIT_MS) {
+            s_seq_len = 0u;
+            return BTN_EVENT_PARK;
+        }
+    }
+
+    return BTN_EVENT_NONE;
+}

src/imu_cal_flash.c

@@ -0,0 +1,100 @@
+/* imu_cal_flash.c — IMU mount angle calibration flash storage (Issue #680)
+ *
+ * Stores pitch/roll mount offsets in STM32F722 flash sector 7 at 0x0807FF00.
+ * Preserves existing PID records (pid_sched_flash_t + pid_flash_t) across
+ * the mandatory sector erase by reading them into RAM before erasing.
+ */
+
+#include "imu_cal_flash.h"
+#include "pid_flash.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+bool imu_cal_flash_load(float *pitch_offset, float *roll_offset)
+{
+    const imu_cal_flash_t *p = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
+
+    if (p->magic != IMU_CAL_FLASH_MAGIC) return false;
+
+    /* Sanity-check: mount offsets beyond ±90° indicate a corrupt record */
+    if (p->pitch_offset < -90.0f || p->pitch_offset > 90.0f) return false;
+    if (p->roll_offset  < -90.0f || p->roll_offset  > 90.0f) return false;
+
+    *pitch_offset = p->pitch_offset;
+    *roll_offset  = p->roll_offset;
+    return true;
+}
+
+/* Write 'len' bytes (multiple of 4) from 'src' to flash at 'addr'.
+ * Flash must be unlocked by caller. */
+static HAL_StatusTypeDef write_words(uint32_t addr,
+                                     const void *src,
+                                     uint32_t len)
+{
+    const uint32_t *p = (const uint32_t *)src;
+    for (uint32_t i = 0; i < len / 4u; i++) {
+        HAL_StatusTypeDef rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
+                                                  addr, p[i]);
+        if (rc != HAL_OK) return rc;
+        addr += 4u;
+    }
+    return HAL_OK;
+}
+
+bool imu_cal_flash_save(float pitch_offset, float roll_offset)
+{
+    /* Snapshot PID records BEFORE erasing so we can restore them */
+    pid_flash_t       pid_snap;
+    pid_sched_flash_t sched_snap;
+    memcpy(&pid_snap,   (const void *)PID_FLASH_STORE_ADDR, sizeof(pid_snap));
+    memcpy(&sched_snap, (const void *)PID_SCHED_FLASH_ADDR, sizeof(sched_snap));
+
+    HAL_StatusTypeDef rc;
+
+    rc = HAL_FLASH_Unlock();
+    if (rc != HAL_OK) return false;
+
+    /* Erase sector 7 (covers all three records) */
+    FLASH_EraseInitTypeDef erase = {
+        .TypeErase    = FLASH_TYPEERASE_SECTORS,
+        .Sector       = PID_FLASH_SECTOR,
+        .NbSectors    = 1,
+        .VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
+    };
+    uint32_t sector_error = 0;
+    rc = HAL_FLASHEx_Erase(&erase, &sector_error);
+    if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
+        HAL_FLASH_Lock();
+        return false;
+    }
+
+    /* Write new IMU calibration record at 0x0807FF00 */
+    imu_cal_flash_t cal;
+    memset(&cal, 0xFF, sizeof(cal));
+    cal.magic        = IMU_CAL_FLASH_MAGIC;
+    cal.pitch_offset = pitch_offset;
+    cal.roll_offset  = roll_offset;
+
+    rc = write_words(IMU_CAL_FLASH_ADDR, &cal, sizeof(cal));
+    if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
+
+    /* Restore PID gain schedule if it was valid */
+    if (sched_snap.magic == PID_SCHED_MAGIC) {
+        rc = write_words(PID_SCHED_FLASH_ADDR, &sched_snap, sizeof(sched_snap));
+        if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
+    }
+
+    /* Restore single-PID record if it was valid */
+    if (pid_snap.magic == PID_FLASH_MAGIC) {
+        rc = write_words(PID_FLASH_STORE_ADDR, &pid_snap, sizeof(pid_snap));
+        if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
+    }
+
+    HAL_FLASH_Lock();
+
+    /* Verify readback */
+    const imu_cal_flash_t *v = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
+    return (v->magic        == IMU_CAL_FLASH_MAGIC &&
+            v->pitch_offset == pitch_offset &&
+            v->roll_offset  == roll_offset);
+}

src/jlink.c

@@ -687,3 +687,28 @@ void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm)
 
     jlink_tx_locked(frame, sizeof(frame));
 }
+
+/* ---- jlink_send_vesc_state_tlm() -- Issue #674 ---- */
+void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm)
+{
+    /*
+     * Frame: [STX][LEN][0x8E][22 bytes VESC_STATE][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 (CMD) + 22 (payload) = 23; total frame = 28 bytes.
+     * At 921600 baud: 28×10/921600 ≈ 0.30 ms — safe to block.
+     */
+    static uint8_t frame[28];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_vesc_state_t);  /* 22 */
+    const uint8_t  len  = 1u + plen;                                 /* 23 */
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_VESC_STATE;
+    memcpy(&frame[3], tlm, plen);
+
+    uint16_t crc = crc16_xmodem(&frame[2], len);
+    frame[3 + plen]     = (uint8_t)(crc >> 8);
+    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
+    frame[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame, sizeof(frame));
+}

src/main.c

@@ -34,6 +34,10 @@
 #include "pid_flash.h"
 #include "fault_handler.h"
 #include "can_driver.h"
+#include "vesc_can.h"
+#include "orin_can.h"
+#include "imu_cal_flash.h"
+#include "hw_button.h"
 #include "servo_bus.h"
 #include "gimbal.h"
 #include "lvc.h"
@@ -49,6 +53,7 @@ extern volatile uint8_t  cdc_recal_request;  /* set by G command */
 extern volatile uint32_t cdc_rx_count;       /* total CDC packets received */
 extern volatile uint8_t  cdc_estop_request;
 extern volatile uint8_t  cdc_estop_clear_request;
+extern volatile uint8_t  cdc_imu_cal_request;    /* set by O command — mount cal (Issue #680) */
 
 /* Direct motor test (set by W command in jetson_uart.c) */
 volatile int16_t direct_test_speed = 0;
@@ -167,6 +172,16 @@ int main(void) {
      */
     if (imu_ret == 0) mpu6000_calibrate();
 
+    /* Load IMU mount angle offsets from flash if previously calibrated (Issue #680) */
+    {
+        float imu_pitch_off = 0.0f, imu_roll_off = 0.0f;
+        if (imu_cal_flash_load(&imu_pitch_off, &imu_roll_off)) {
+            mpu6000_set_mount_offset(imu_pitch_off, imu_roll_off);
+            printf("[IMU_CAL] Mount offsets loaded: pitch=%.1f° roll=%.1f°\n",
+                   (double)imu_pitch_off, (double)imu_roll_off);
+        }
+    }
+
     /* Init hoverboard ESC UART */
     hoverboard_init();
 
@@ -196,8 +211,17 @@ int main(void) {
     /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
     jlink_init();
 
-    /* Init CAN2 BLDC motor controller bus — PB12/PB13, 500 kbps (Issue #597) */
+    /* Init CAN1 bus — PB8/PB9, 500 kbps (Issues #597/#676/#674).
+     * Register callbacks BEFORE can_driver_init() so both are ready when
+     * filter banks activate. */
+    can_driver_set_ext_cb(vesc_can_on_frame);   /* VESC STATUS → FIFO1 */
+    can_driver_set_std_cb(orin_can_on_frame);   /* Orin cmds  → FIFO0 */
     can_driver_init();
+    vesc_can_init(VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT);
+    orin_can_init();
+
+    /* Init hardware button debounce/gesture driver (Issue #682) */
+    hw_button_init();
 
     /* Send fault log summary on boot if a prior fault was recorded (Issue #565) */
     if (fault_get_last_type() != FAULT_NONE) {
@@ -317,6 +341,20 @@ int main(void) {
         /* Advance buzzer pattern sequencer (non-blocking, call every tick) */
         buzzer_tick(now);
 
+        /* Orin LED override (Issue #685): apply pattern from CAN 0x304 if updated.
+         * Safety states (estop, tilt fault) are applied later and always win. */
+        if (orin_can_led_updated) {
+            orin_can_led_updated = 0u;
+            /* pattern: 0=auto, 1=solid-green, 2=slow-blink, 3=fast-blink, 4=pulse */
+            switch (orin_can_led_override.pattern) {
+                case 1u: led_set_state(LED_STATE_ARMED);    break;
+                case 2u: led_set_state(LED_STATE_LOW_BATT); break;  /* slow blink */
+                case 3u: led_set_state(LED_STATE_ERROR);    break;  /* fast blink */
+                case 4u: led_set_state(LED_STATE_CHARGING); break;  /* pulse */
+                default: break;  /* 0=auto — let state machine below decide */
+            }
+        }
+
         /* Advance LED animation sequencer (non-blocking, call every tick) */
         led_tick(now);
 
@@ -393,7 +431,7 @@ int main(void) {
             jlink_state.arm_req = 0u;
             if (!safety_remote_estop_active() &&
                     mpu6000_is_calibrated() &&
-                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
+                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) {
                 safety_arm_start(now);
             }
         }
@@ -474,6 +512,48 @@ int main(void) {
             }
         }
 
+        /* Orin CAN: handle commands from Orin over CAN bus (Issue #674).
+         * Estop and clear are latching; drive/mode are consumed each tick.
+         * Balance independence: if orin_can_is_alive() == false the loop
+         * simply does not inject any commands and holds upright in-place. */
+        if (orin_can_state.estop_req) {
+            orin_can_state.estop_req = 0u;
+            safety_remote_estop(ESTOP_REMOTE);
+            safety_arm_cancel();
+            balance_disarm(&bal);
+            motor_driver_estop(&motors);
+        }
+        if (orin_can_state.estop_clear_req) {
+            orin_can_state.estop_clear_req = 0u;
+            if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
+                safety_remote_estop_clear();
+        }
+
+        /* Hardware button park/re-arm (Issue #682).
+         * Short press -> park (ARMED only): freeze PID, stop motors, amber LED.
+         * SHORT+SHORT+LONG combo -> unpark (PARKED only): resume if upright. */
+        {
+            hw_btn_event_t btn_ev = hw_button_tick(now);
+            if (btn_ev == BTN_EVENT_PARK) {
+                if (bal.state == BALANCE_ARMED) {
+                    balance_park(&bal);
+                    led_set_state(LED_STATE_LOW_BATT);
+                    orin_can_send_btn_event(1u, (uint8_t)bal.state);
+                }
+            } else if (btn_ev == BTN_EVENT_REARM_COMBO) {
+                if (bal.state == BALANCE_PARKED) {
+                    balance_unpark(&bal);
+                    if (bal.state == BALANCE_ARMED) {
+                        led_set_state(LED_STATE_ARMED);
+                        orin_can_send_btn_event(2u, (uint8_t)bal.state);
+                    } else {
+                        /* Pitch too large — unpark refused, stay parked */
+                        orin_can_send_btn_event(3u, (uint8_t)bal.state);
+                    }
+                }
+            }
+        }
+
         /* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
         if (jlink_state.fault_log_req) {
             jlink_state.fault_log_req = 0u;
@@ -561,7 +641,7 @@ int main(void) {
                 /* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */
                 if (!safety_remote_estop_active() &&
                         mpu6000_is_calibrated() &&
-                        bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
+                        bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) {
                     safety_arm_start(now);
                 }
             }
@@ -581,7 +661,7 @@ int main(void) {
             cdc_arm_request = 0;
             if (!safety_remote_estop_active() &&
                     mpu6000_is_calibrated() &&
-                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
+                    bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 20.0f) {
                 safety_arm_start(now);
             }
         }
@@ -605,6 +685,24 @@ int main(void) {
             if (imu_ret == 0) mpu6000_calibrate();
         }
 
+        /* IMU mount angle calibration (Issue #680): capture current pitch/roll as
+         * mount offsets and save to flash.  Disarmed only — flash erase stalls ~1s. */
+        if (cdc_imu_cal_request && bal.state != BALANCE_ARMED) {
+            cdc_imu_cal_request = 0;
+            float off_p = bal.pitch_deg;
+            float off_r = imu.roll;
+            mpu6000_set_mount_offset(off_p, off_r);
+            bool cal_ok = imu_cal_flash_save(off_p, off_r);
+            char reply[64];
+            snprintf(reply, sizeof(reply),
+                     "{\"imu_cal\":%s,\"pitch_off\":%d,\"roll_off\":%d}\n",
+                     cal_ok ? "ok" : "fail",
+                     (int)(off_p * 10), (int)(off_r * 10));
+            CDC_Transmit((uint8_t *)reply, strlen(reply));
+            printf("[IMU_CAL] Mount offset saved: pitch=%.1f° roll=%.1f° (%s)\n",
+                   (double)off_p, (double)off_r, cal_ok ? "OK" : "FAIL");
+        }
+
         /* Handle PID tuning commands from USB (P/I/D/T/M/?) */
         if (cdc_cmd_ready) {
             cdc_cmd_ready = 0;
@@ -631,6 +729,8 @@ int main(void) {
             float base_sp = bal.setpoint;
             if (jlink_is_active(now))
                 bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
+            else if (orin_can_is_alive(now))
+                bal.setpoint += ((float)orin_can_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
             else if (jetson_cmd_is_active(now))
                 bal.setpoint += jetson_cmd_sp_offset();
             balance_update(&bal, &imu, dt);
@@ -664,6 +764,8 @@ int main(void) {
          * mode_manager_get_steer() blends it with RC steer per active mode. */
         if (jlink_is_active(now))
             mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0);
+        else if (orin_can_is_alive(now))
+            mode_manager_set_auto_cmd(&mode, orin_can_state.steer, 0);
         else if (jetson_cmd_is_active(now))
             mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);
 
@@ -692,38 +794,74 @@ int main(void) {
             }
         }
 
-        /* CAN BLDC: enable/disable follows arm state (Issue #597) */
-        {
-            static bool s_can_enabled = false;
-            bool armed_now = (bal.state == BALANCE_ARMED);
-            if (armed_now && !s_can_enabled) {
-                can_driver_send_enable(CAN_NODE_LEFT,  true);
-                can_driver_send_enable(CAN_NODE_RIGHT, true);
-                s_can_enabled = true;
-            } else if (!armed_now && s_can_enabled) {
-                can_driver_send_enable(CAN_NODE_LEFT,  false);
-                can_driver_send_enable(CAN_NODE_RIGHT, false);
-                s_can_enabled = false;
-            }
-        }
-
-        /* CAN BLDC: send velocity/torque commands at CAN_TX_RATE_HZ (100 Hz) (Issue #597)
-         * Converts balance PID output + blended steer to per-wheel RPM.
+        /* VESC: send RPM commands at CAN_TX_RATE_HZ (100 Hz) via 29-bit extended CAN.
+         * VESC does not need enable/disable frames — RPM=0 while disarmed holds brakes.
          * Left  wheel: speed_rpm + steer_rpm (differential drive)
-         * Right wheel: speed_rpm - steer_rpm */
+         * Right wheel: speed_rpm − steer_rpm  (Issue #674) */
         if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) {
             can_cmd_tick = now;
+            int32_t speed_rpm = 0;
+            int32_t steer_rpm = 0;
+            if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
                 int16_t can_steer = mode_manager_get_steer(&mode);
-            int32_t speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
-            int32_t steer_rpm = (int32_t)can_steer    * CAN_RPM_SCALE / 2;
-            can_cmd_t cmd_l = { (int16_t)(speed_rpm + steer_rpm), 0 };
-            can_cmd_t cmd_r = { (int16_t)(speed_rpm - steer_rpm), 0 };
-            if (bal.state != BALANCE_ARMED) {
-                cmd_l.velocity_rpm = 0;
-                cmd_r.velocity_rpm = 0;
+                speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
+                steer_rpm = (int32_t)can_steer     * CAN_RPM_SCALE / 2;
             }
-            can_driver_send_cmd(CAN_NODE_LEFT,  &cmd_l);
-            can_driver_send_cmd(CAN_NODE_RIGHT, &cmd_r);
+            vesc_can_send_rpm(VESC_CAN_ID_LEFT,  speed_rpm + steer_rpm);
+            vesc_can_send_rpm(VESC_CAN_ID_RIGHT, speed_rpm - steer_rpm);
+        }
+
+        /* VESC telemetry: send JLINK_TLM_VESC_STATE at 1 Hz (Issue #674) */
+        vesc_can_send_tlm(now);
+
+        /* Orin CAN broadcast: FC_STATUS + FC_VESC at ORIN_TLM_HZ (10 Hz) (Issue #674) */
+        {
+            orin_can_fc_status_t fc_st;
+            fc_st.pitch_x10     = (int16_t)(bal.pitch_deg * 10.0f);
+            fc_st.motor_cmd     = bal.motor_cmd;
+            uint32_t _vbat      = battery_read_mv();
+            fc_st.vbat_mv       = (_vbat > 65535u) ? 65535u : (uint16_t)_vbat;
+            fc_st.balance_state = (uint8_t)bal.state;
+            fc_st.flags         = (safety_remote_estop_active() ? 0x01u : 0u) |
+                                  (bal.state == BALANCE_ARMED   ? 0x02u : 0u);
+
+            orin_can_fc_vesc_t fc_vesc;
+            vesc_state_t vl, vr;
+            bool vl_ok = vesc_can_get_state(VESC_CAN_ID_LEFT,  &vl);
+            bool vr_ok = vesc_can_get_state(VESC_CAN_ID_RIGHT, &vr);
+            fc_vesc.left_rpm_x10      = vl_ok ? (int16_t)(vl.rpm / 10) : 0;
+            fc_vesc.right_rpm_x10     = vr_ok ? (int16_t)(vr.rpm / 10) : 0;
+            fc_vesc.left_current_x10  = vl_ok ? vl.current_x10 : 0;
+            fc_vesc.right_current_x10 = vr_ok ? vr.current_x10 : 0;
+
+            orin_can_broadcast(now, &fc_st, &fc_vesc);
+        }
+
+        /* FC_IMU (0x402): full IMU angles + calibration status at 50 Hz (Issue #680) */
+        {
+            orin_can_fc_imu_t fc_imu;
+            fc_imu.pitch_x10     = (int16_t)(bal.pitch_deg * 10.0f);
+            fc_imu.roll_x10      = (int16_t)(imu.roll  * 10.0f);
+            fc_imu.yaw_x10       = (int16_t)(imu.yaw   * 10.0f);
+            /* cal_status: 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */
+            if (!mpu6000_is_calibrated())       fc_imu.cal_status = 0u;
+            else if (mpu6000_has_mount_offset()) fc_imu.cal_status = 2u;
+            else                                 fc_imu.cal_status = 1u;
+            fc_imu.balance_state = (uint8_t)bal.state;
+            orin_can_broadcast_imu(now, &fc_imu);
+        }
+
+        /* FC_BARO (0x403): barometer at 1 Hz (Issue #672) */
+        if (baro_chip) {
+            int32_t _pres_pa; int16_t _temp_x10;
+            bmp280_read(&_pres_pa, &_temp_x10);
+            int32_t _alt_cm = bmp280_pressure_to_alt_cm(_pres_pa);
+            orin_can_fc_baro_t fc_baro;
+            fc_baro.pressure_pa = _pres_pa;
+            fc_baro.temp_x10    = _temp_x10;
+            fc_baro.alt_cm      = (_alt_cm >  32767) ?  32767 :
+                                  (_alt_cm < -32768) ? -32768 : (int16_t)_alt_cm;
+            orin_can_broadcast_baro(now, &fc_baro);
         }
 
         /* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.

src/mpu6000.c

@@ -39,6 +39,10 @@ static float s_bias_gy   = 0.0f;
 static float s_bias_gz   = 0.0f;
 static bool  s_calibrated = false;
 
+/* Mount angle offsets (degrees, Issue #680) — set via mpu6000_set_mount_offset() */
+static float s_pitch_offset = 0.0f;
+static float s_roll_offset  = 0.0f;
+
 bool mpu6000_init(void) {
     int ret = icm42688_init();
     if (ret == 0) {
@@ -91,6 +95,17 @@ bool mpu6000_is_calibrated(void) {
     return s_calibrated;
 }
 
+void mpu6000_set_mount_offset(float pitch_deg, float roll_deg)
+{
+    s_pitch_offset = pitch_deg;
+    s_roll_offset  = roll_deg;
+}
+
+bool mpu6000_has_mount_offset(void)
+{
+    return (s_pitch_offset != 0.0f || s_roll_offset != 0.0f);
+}
+
 void mpu6000_read(IMUData *data) {
     icm42688_data_t raw;
     icm42688_read(&raw);
@@ -147,9 +162,9 @@ void mpu6000_read(IMUData *data) {
     /* Yaw: pure gyro integration — no accel correction, drifts over time */
     s_yaw += gyro_yaw_rate * dt;
 
-    data->pitch      = s_pitch;
+    data->pitch      = s_pitch - s_pitch_offset;
     data->pitch_rate = gyro_pitch_rate;
-    data->roll       = s_roll;
+    data->roll       = s_roll  - s_roll_offset;
     data->yaw        = s_yaw;
     data->yaw_rate   = gyro_yaw_rate;  /* board_gz: raw bias-corrected gyro Z (Issue #616) */
     data->accel_x    = ax;

src/orin_can.c

@@ -0,0 +1,152 @@
+/* orin_can.c — Orin↔FC CAN protocol driver (Issue #674)
+ *
+ * Receives high-level drive/mode/estop commands from Orin over CAN.
+ * Broadcasts FC status and VESC telemetry back to Orin at ORIN_TLM_HZ.
+ *
+ * Registered as the standard-ID callback with can_driver via
+ * can_driver_set_std_cb(orin_can_on_frame).
+ *
+ * Balance independence: if Orin link drops (orin_can_is_alive() == false),
+ * main loop stops injecting Orin commands and the balance PID holds
+ * upright in-place.  No action required here — the safety is in main.c.
+ */
+
+#include "orin_can.h"
+#include "can_driver.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+volatile OrinCanState      orin_can_state;
+volatile orin_can_led_cmd_t orin_can_led_override;
+volatile uint8_t            orin_can_led_updated;
+
+static uint32_t s_tlm_tick;
+static uint32_t s_imu_tick;
+static uint32_t s_baro_tick;
+
+void orin_can_init(void)
+{
+    memset((void *)&orin_can_state,      0, sizeof(orin_can_state));
+    memset((void *)&orin_can_led_override, 0, sizeof(orin_can_led_override));
+    orin_can_led_updated = 0u;
+    /* Pre-wind so first broadcasts fire on the first eligible tick */
+    s_tlm_tick  = (uint32_t)(-(uint32_t)(1000u / ORIN_TLM_HZ));
+    s_imu_tick  = (uint32_t)(-(uint32_t)(1000u / ORIN_IMU_TLM_HZ));
+    s_baro_tick = (uint32_t)(-(uint32_t)(1000u / ORIN_BARO_TLM_HZ));
+    can_driver_set_std_cb(orin_can_on_frame);
+}
+
+void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len)
+{
+    /* Any frame from Orin refreshes the heartbeat */
+    orin_can_state.last_rx_ms = HAL_GetTick();
+
+    switch (std_id) {
+    case ORIN_CAN_ID_HEARTBEAT:
+        /* Heartbeat payload (sequence counter) ignored — timestamp is enough */
+        break;
+
+    case ORIN_CAN_ID_DRIVE:
+        /* int16 speed (BE), int16 steer (BE) */
+        if (len < 4u) { break; }
+        orin_can_state.speed = (int16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
+        orin_can_state.steer = (int16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
+        orin_can_state.drive_updated = 1u;
+        break;
+
+    case ORIN_CAN_ID_MODE:
+        /* uint8 mode */
+        if (len < 1u) { break; }
+        orin_can_state.mode = data[0];
+        orin_can_state.mode_updated = 1u;
+        break;
+
+    case ORIN_CAN_ID_ESTOP:
+        /* uint8: 1 = assert estop, 0 = clear estop */
+        if (len < 1u) { break; }
+        if (data[0] != 0u) {
+            orin_can_state.estop_req = 1u;
+        } else {
+            orin_can_state.estop_clear_req = 1u;
+        }
+        break;
+
+    case ORIN_CAN_ID_LED_CMD:
+        /* pattern(u8), brightness(u8), duration_ms(u16 LE) — Issue #685 */
+        if (len < 4u) { break; }
+        orin_can_led_override.pattern     = data[0];
+        orin_can_led_override.brightness  = data[1];
+        orin_can_led_override.duration_ms = (uint16_t)((uint16_t)data[2] |
+                                             ((uint16_t)data[3] << 8u));
+        orin_can_led_updated = 1u;
+        break;
+
+    default:
+        break;
+    }
+}
+
+bool orin_can_is_alive(uint32_t now_ms)
+{
+    if (orin_can_state.last_rx_ms == 0u) {
+        return false;
+    }
+    return (now_ms - orin_can_state.last_rx_ms) < ORIN_HB_TIMEOUT_MS;
+}
+
+void orin_can_broadcast(uint32_t now_ms,
+                        const orin_can_fc_status_t *status,
+                        const orin_can_fc_vesc_t   *vesc)
+{
+    if ((now_ms - s_tlm_tick) < (1000u / ORIN_TLM_HZ)) {
+        return;
+    }
+    s_tlm_tick = now_ms;
+
+    uint8_t buf[8];
+
+    /* FC_STATUS (0x400): 8 bytes */
+    memcpy(buf, status, sizeof(orin_can_fc_status_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_STATUS, buf, (uint8_t)sizeof(orin_can_fc_status_t));
+
+    /* FC_VESC (0x401): 8 bytes */
+    memcpy(buf, vesc, sizeof(orin_can_fc_vesc_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_VESC, buf, (uint8_t)sizeof(orin_can_fc_vesc_t));
+}
+
+void orin_can_broadcast_imu(uint32_t now_ms,
+                            const orin_can_fc_imu_t *imu_tlm)
+{
+    if ((now_ms - s_imu_tick) < (1000u / ORIN_IMU_TLM_HZ)) {
+        return;
+    }
+    s_imu_tick = now_ms;
+
+    uint8_t buf[8];
+    memcpy(buf, imu_tlm, sizeof(orin_can_fc_imu_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_IMU, buf, (uint8_t)sizeof(orin_can_fc_imu_t));
+}
+
+void orin_can_broadcast_baro(uint32_t now_ms,
+                             const orin_can_fc_baro_t *baro_tlm)
+{
+    if (baro_tlm == NULL) return;
+    if ((now_ms - s_baro_tick) < (1000u / ORIN_BARO_TLM_HZ)) {
+        return;
+    }
+    s_baro_tick = now_ms;
+
+    uint8_t buf[8];
+    memcpy(buf, baro_tlm, sizeof(orin_can_fc_baro_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_BARO, buf, (uint8_t)sizeof(orin_can_fc_baro_t));
+}
+
+void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state)
+{
+    orin_can_fc_btn_t btn;
+    btn.event_id      = event_id;
+    btn.balance_state = balance_state;
+    uint8_t buf[2];
+    memcpy(buf, &btn, sizeof(orin_can_fc_btn_t));
+    can_driver_send_std(ORIN_CAN_ID_FC_BTN, buf, (uint8_t)sizeof(orin_can_fc_btn_t));
+}