docs: full SAUL-TEE ESP32-S3 spec — pins, CAN, UART, RC mapping

Complete hardware reference from hal@Orin spec (2026-04-04):
- docs/SAUL-TEE-SYSTEM-REFERENCE.md: authoritative pin/protocol/CAN reference
  ESP32-S3 BALANCE: QMI8658 SPI(IO38-42), GC9A01 LCD, SN65HVD230 CAN(IO43/44),
  inter-board UART(IO17/18)
  ESP32-S3 IO: Crossfire UART0(IO43/44), ELRS UART2(IO16/17), BTS7960(IO1-8),
  I2C(IO11/12), WS2812(IO13), buzzer/headlight/fan, arming btn, kill-sw, UART(IO18/21)
- Inter-board binary protocol: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud
- CAN: VESC L=68, R=56; Orin cmds 0x300-0x303; telemetry 0x400-0x401 @ 10Hz
- RC: CH5=ARM, CH6=ESTOP, CH7=speed-limit; CRSF loss >100ms = motors cut
- CLAUDE.md, TEAM.md, docs/AGENTS.md, docs/SALTYLAB.md updated with full spec

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

AUTONOMOUS_ARMING.md

@@ -7,7 +7,7 @@ The robot can now be armed and operated autonomously from the Jetson without req
 
 ### Jetson Autonomous Arming
 - Command: `A\n` (single byte 'A' followed by newline)
-- Sent via USB CDC to the STM32 firmware
+- Sent via USB CDC to the ESP32 BALANCE firmware
 - Robot arms after ARMING_HOLD_MS (~500ms) safety hold period
 - Works even when RC is not connected or not armed
 
@@ -42,7 +42,7 @@ The robot can now be armed and operated autonomously from the Jetson without req
 
 ## Command Protocol
 
-### From Jetson to STM32 (USB CDC)
+### From Jetson to ESP32 BALANCE (USB CDC)
 ```
 A           — Request arm (triggers safety hold, then motors enable)
 D           — Request disarm (immediate motor stop)
@@ -52,7 +52,7 @@ H           — Heartbeat (refresh timeout timer, every 500ms)
 C<spd>,<str> — Drive command: speed, steer (also refreshes heartbeat)
 ```
 
-### From STM32 to Jetson (USB CDC)
+### From ESP32 BALANCE to Jetson (USB CDC)
 Motor commands are gated by `bal.state == BALANCE_ARMED`:
 - When ARMED: Motor commands sent every 20ms (50 Hz)
 - When DISARMED: Zero sent every 20ms (prevents ESC timeout)

CLAUDE.md

@@ -1,14 +1,23 @@
 # SaltyLab Firmware — Agent Playbook
 
 ## Project
-Self-balancing two-wheeled robot: STM32F722 flight controller, hoverboard hub motors, Jetson Nano for AI/SLAM.
+**SAUL-TEE** — 4-wheel wagon (870×510×550 mm, 23 kg).
+Two ESP32-S3 boards + Jetson Orin via CAN. Full spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
+
+| Board | Role |
+|-------|------|
+| **ESP32-S3 BALANCE** | QMI8658 IMU, PID balance, CAN→VESC (L:68 / R:56), GC9A01 LCD (Waveshare Touch LCD 1.28) |
+| **ESP32-S3 IO** | TBS Crossfire RC, ELRS failover, BTS7960 motors, NFC/baro/ToF, WS2812 |
+| **Jetson Orin** | AI/SLAM, CANable2 USB→CAN, cmds 0x300–0x303, telemetry 0x400–0x401 |
+
+> **Legacy:** `src/` and `include/` = archived STM32 HAL — do not extend. New firmware in `esp32/`.
 
 ## Team
 | Agent | Role | Focus |
 |-------|------|-------|
-| **sl-firmware** | Embedded Firmware Lead | STM32 HAL, USB CDC debugging, SPI/UART, PlatformIO, DFU bootloader |
-| **sl-controls** | Control Systems Engineer | PID tuning, IMU sensor fusion, real-time control loops, safety systems |
-| **sl-perception** | Perception / SLAM Engineer | Jetson Nano, RealSense D435i, RPLIDAR, ROS2, Nav2 |
+| **sl-firmware** | Embedded Firmware Lead | ESP32-S3, ESP-IDF, QMI8658, CAN/UART protocol, BTS7960 |
+| **sl-controls** | Control Systems Engineer | PID tuning, IMU fusion, balance loop, safety |
+| **sl-perception** | Perception / SLAM Engineer | Jetson Orin, RealSense D435i, RPLIDAR, ROS2, Nav2 |
 
 ## Status
 USB CDC TX bug resolved (PR #10 — DCache MPU non-cacheable region + IWDG ordering fix).

TEAM.md

@@ -1,12 +1,13 @@
 # SaltyLab — Ideal Team
 
 ## Project
-Self-balancing two-wheeled robot using a drone flight controller (STM32F722), hoverboard hub motors, and eventually a Jetson Nano for AI/SLAM.
+**SAUL-TEE** — 4-wheel wagon (870×510×550 mm, 23 kg).
+Two ESP32-S3 boards (BALANCE + IO) + Jetson Orin. See `docs/SAUL-TEE-SYSTEM-REFERENCE.md`.
 
 ## Current Status
-- **Hardware:** Assembled — FC, motors, ESC, IMU, battery, RC all on hand
-- **Firmware:** Balance PID + hoverboard ESC protocol written, but blocked by USB CDC bug
-- **Blocker:** USB CDC TX stops working when peripheral inits (SPI/UART/GPIO) are added alongside USB OTG FS — see `USB_CDC_BUG.md`
+- **Hardware:** ESP32-S3 BALANCE (Waveshare Touch LCD 1.28, CH343 USB) + ESP32-S3 IO (bare devkit, JTAG USB)
+- **Firmware:** ESP-IDF/PlatformIO target; legacy `src/` STM32 HAL archived
+- **Comms:** UART 460800 baud inter-board; CANable2 USB→CAN for Orin; CAN 500 kbps to VESCs (L:68 / R:56)
 
 ---
 
@@ -14,10 +15,10 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
 
 ### 1. Embedded Firmware Engineer (Lead)
 **Must-have:**
-- Deep STM32 HAL experience (F7 series specifically)
+- Deep ESP32 (Arduino/ESP-IDF) or STM32 HAL experience
 - USB OTG FS / CDC ACM debugging (TxState, endpoint management, DMA conflicts)
-- SPI + UART + USB coexistence on STM32
-- PlatformIO or bare-metal STM32 toolchain
+- SPI + UART + USB coexistence on ESP32
+- PlatformIO or bare-metal ESP32 toolchain
 - DFU bootloader implementation
 
 **Nice-to-have:**
@@ -25,7 +26,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
 - PID control loop tuning for balance robots
 - FOC motor control (hoverboard ESC protocol)
 
-**Why:** The immediate blocker is a USB peripheral conflict. Need someone who's debugged STM32 USB issues before — this is not a software logic bug, it's a hardware peripheral interaction issue.
+**Why:** The immediate blocker is a USB peripheral conflict. Need someone who's debugged STM32 USB issues before — ESP32 firmware for the balance loop and I/O needs to be written from scratch.
 
 ### 2. Control Systems / Robotics Engineer
 **Must-have:**
@@ -61,7 +62,7 @@ Self-balancing two-wheeled robot using a drone flight controller (STM32F722), ho
 ## Hardware Reference
 | Component | Details |
 |-----------|---------|
-| FC | MAMBA F722S (STM32F722RET6, MPU6000) |
+| FC | ESP32 BALANCE (ESP32RET6, MPU6000) |
 | Motors | 2x 8" pneumatic hoverboard hub motors |
 | ESC | Hoverboard ESC (EFeru FOC firmware) |
 | Battery | 36V pack |

USB_CDC_BUG.md

@@ -127,7 +127,7 @@ loop — USB would never enumerate cleanly.
 | LED2 | PC15 | GPIO |
 | Buzzer | PB2 | GPIO/TIM4_CH3 |
 
-MCU: STM32F722RET6 (MAMBA F722S FC, Betaflight target DIAT-MAMBAF722_2022B)
+MCU: ESP32RET6 (ESP32 BALANCE FC, Betaflight target DIAT-MAMBAF722_2022B)
 
 ---
 

android/build.gradle

@@ -0,0 +1,46 @@
+plugins {
+    id 'com.android.application'
+    id 'kotlin-android'
+}
+
+android {
+    compileSdk 34
+    namespace 'com.saltylab.uwbtag'
+
+    defaultConfig {
+        applicationId "com.saltylab.uwbtag"
+        minSdk 26
+        targetSdk 34
+        versionCode 1
+        versionName "1.0"
+    }
+
+    buildTypes {
+        release {
+            minifyEnabled false
+        }
+    }
+
+    buildFeatures {
+        viewBinding true
+    }
+
+    compileOptions {
+        sourceCompatibility JavaVersion.VERSION_17
+        targetCompatibility JavaVersion.VERSION_17
+    }
+
+    kotlinOptions {
+        jvmTarget = '17'
+    }
+}
+
+dependencies {
+    implementation 'androidx.core:core-ktx:1.12.0'
+    implementation 'androidx.appcompat:appcompat:1.6.1'
+    implementation 'com.google.android.material:material:1.11.0'
+    implementation 'androidx.recyclerview:recyclerview:1.3.2'
+    implementation 'androidx.lifecycle:lifecycle-runtime-ktx:2.7.0'
+    implementation 'org.jetbrains.kotlinx:kotlinx-coroutines-android:1.7.3'
+    implementation 'com.google.code.gson:gson:2.10.1'
+}

android/src/main/AndroidManifest.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0" encoding="utf-8"?>
+<manifest xmlns:android="http://schemas.android.com/apk/res/android">
+
+    <!-- BLE permissions (API 31+) -->
+    <uses-permission android:name="android.permission.BLUETOOTH_SCAN"
+        android:usesPermissionFlags="neverForLocation" />
+    <uses-permission android:name="android.permission.BLUETOOTH_CONNECT" />
+    <uses-permission android:name="android.permission.BLUETOOTH_ADVERTISE" />
+
+    <!-- Legacy BLE (API < 31) -->
+    <uses-permission android:name="android.permission.BLUETOOTH"
+        android:maxSdkVersion="30" />
+    <uses-permission android:name="android.permission.BLUETOOTH_ADMIN"
+        android:maxSdkVersion="30" />
+    <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION"
+        android:maxSdkVersion="30" />
+
+    <uses-feature android:name="android.hardware.bluetooth_le" android:required="true" />
+
+    <application
+        android:allowBackup="true"
+        android:label="UWB Tag Config"
+        android:theme="@style/Theme.MaterialComponents.DayNight.DarkActionBar">
+
+        <activity
+            android:name=".UwbTagBleActivity"
+            android:exported="true"
+            android:launchMode="singleTop">
+            <intent-filter>
+                <action android:name="android.intent.action.MAIN" />
+                <category android:name="android.intent.category.LAUNCHER" />
+            </intent-filter>
+        </activity>
+
+    </application>
+
+</manifest>

android/src/main/kotlin/com/saltylab/uwbtag/UwbTagBleActivity.kt

@@ -0,0 +1,444 @@
+package com.saltylab.uwbtag
+
+import android.Manifest
+import android.annotation.SuppressLint
+import android.bluetooth.*
+import android.bluetooth.le.*
+import android.content.Context
+import android.content.pm.PackageManager
+import android.os.Build
+import android.os.Bundle
+import android.os.Handler
+import android.os.Looper
+import android.view.LayoutInflater
+import android.view.View
+import android.view.ViewGroup
+import android.widget.Button
+import android.widget.TextView
+import android.widget.Toast
+import androidx.appcompat.app.AppCompatActivity
+import androidx.core.app.ActivityCompat
+import androidx.core.content.ContextCompat
+import androidx.recyclerview.widget.LinearLayoutManager
+import androidx.recyclerview.widget.RecyclerView
+import com.google.android.material.card.MaterialCardView
+import com.google.android.material.switchmaterial.SwitchMaterial
+import com.google.android.material.textfield.TextInputEditText
+import com.google.gson.Gson
+import com.saltylab.uwbtag.databinding.ActivityUwbTagBleBinding
+import java.util.UUID
+
+// ---------------------------------------------------------------------------
+// GATT service / characteristic UUIDs
+// ---------------------------------------------------------------------------
+private val SERVICE_UUID     = UUID.fromString("12345678-1234-5678-1234-56789abcdef0")
+private val CHAR_CONFIG_UUID = UUID.fromString("12345678-1234-5678-1234-56789abcdef1") // read/write JSON config
+private val CHAR_STATUS_UUID = UUID.fromString("12345678-1234-5678-1234-56789abcdef2") // notify: tag status string
+private val CHAR_BATT_UUID   = UUID.fromString("12345678-1234-5678-1234-56789abcdef3") // notify: battery %
+private val CCCD_UUID        = UUID.fromString("00002902-0000-1000-8000-00805f9b34fb")
+
+// BLE scan timeout
+private const val SCAN_TIMEOUT_MS = 15_000L
+
+// Permissions request code
+private const val REQ_PERMISSIONS = 1001
+
+// ---------------------------------------------------------------------------
+// Data model
+// ---------------------------------------------------------------------------
+data class TagConfig(
+    val tag_name: String = "UWB_TAG_0001",
+    val sleep_timeout_s: Int = 300,
+    val display_brightness: Int = 50,
+    val uwb_channel: Int = 9,
+    val ranging_interval_ms: Int = 100,
+    val battery_report: Boolean = true
+)
+
+data class ScannedDevice(
+    val name: String,
+    val address: String,
+    var rssi: Int,
+    val device: BluetoothDevice
+)
+
+// ---------------------------------------------------------------------------
+// RecyclerView adapter for scanned devices
+// ---------------------------------------------------------------------------
+class DeviceAdapter(
+    private val onConnect: (ScannedDevice) -> Unit
+) : RecyclerView.Adapter<DeviceAdapter.VH>() {
+
+    private val items = mutableListOf<ScannedDevice>()
+
+    fun update(device: ScannedDevice) {
+        val idx = items.indexOfFirst { it.address == device.address }
+        if (idx >= 0) {
+            items[idx] = device
+            notifyItemChanged(idx)
+        } else {
+            items.add(device)
+            notifyItemInserted(items.size - 1)
+        }
+    }
+
+    fun clear() {
+        items.clear()
+        notifyDataSetChanged()
+    }
+
+    override fun onCreateViewHolder(parent: ViewGroup, viewType: Int): VH {
+        val view = LayoutInflater.from(parent.context)
+            .inflate(R.layout.item_ble_device, parent, false)
+        return VH(view)
+    }
+
+    override fun onBindViewHolder(holder: VH, position: Int) = holder.bind(items[position])
+    override fun getItemCount() = items.size
+
+    inner class VH(view: View) : RecyclerView.ViewHolder(view) {
+        private val tvName    = view.findViewById<TextView>(R.id.tvDeviceName)
+        private val tvAddress = view.findViewById<TextView>(R.id.tvDeviceAddress)
+        private val tvRssi    = view.findViewById<TextView>(R.id.tvRssi)
+        private val btnConn   = view.findViewById<Button>(R.id.btnConnect)
+
+        fun bind(item: ScannedDevice) {
+            tvName.text    = item.name
+            tvAddress.text = item.address
+            tvRssi.text    = "${item.rssi} dBm"
+            btnConn.setOnClickListener { onConnect(item) }
+        }
+    }
+}
+
+// ---------------------------------------------------------------------------
+// Activity
+// ---------------------------------------------------------------------------
+@SuppressLint("MissingPermission") // permissions checked at runtime before any BLE call
+class UwbTagBleActivity : AppCompatActivity() {
+
+    private lateinit var binding: ActivityUwbTagBleBinding
+    private val gson = Gson()
+    private val mainHandler = Handler(Looper.getMainLooper())
+
+    // BLE
+    private val btManager by lazy { getSystemService(Context.BLUETOOTH_SERVICE) as BluetoothManager }
+    private val btAdapter by lazy { btManager.adapter }
+    private var bleScanner: BluetoothLeScanner? = null
+    private var gatt: BluetoothGatt? = null
+    private var configChar: BluetoothGattCharacteristic? = null
+    private var statusChar: BluetoothGattCharacteristic? = null
+    private var battChar: BluetoothGattCharacteristic? = null
+    private var isScanning = false
+
+    private val deviceAdapter = DeviceAdapter(onConnect = ::connectToDevice)
+
+    // ---------------------------------------------------------------------------
+    // Lifecycle
+    // ---------------------------------------------------------------------------
+    override fun onCreate(savedInstanceState: Bundle?) {
+        super.onCreate(savedInstanceState)
+        binding = ActivityUwbTagBleBinding.inflate(layoutInflater)
+        setContentView(binding.root)
+        setSupportActionBar(binding.toolbar)
+
+        binding.rvDevices.layoutManager = LinearLayoutManager(this)
+        binding.rvDevices.adapter = deviceAdapter
+
+        binding.btnScan.setOnClickListener {
+            if (isScanning) stopScan() else startScanIfPermitted()
+        }
+        binding.btnDisconnect.setOnClickListener { disconnectGatt() }
+        binding.btnReadConfig.setOnClickListener { readConfig() }
+        binding.btnWriteConfig.setOnClickListener { writeConfig() }
+
+        requestBlePermissions()
+    }
+
+    override fun onDestroy() {
+        super.onDestroy()
+        stopScan()
+        disconnectGatt()
+    }
+
+    // ---------------------------------------------------------------------------
+    // Permissions
+    // ---------------------------------------------------------------------------
+    private fun requestBlePermissions() {
+        val needed = mutableListOf<String>()
+        if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.S) {
+            if (!hasPermission(Manifest.permission.BLUETOOTH_SCAN))
+                needed += Manifest.permission.BLUETOOTH_SCAN
+            if (!hasPermission(Manifest.permission.BLUETOOTH_CONNECT))
+                needed += Manifest.permission.BLUETOOTH_CONNECT
+        } else {
+            if (!hasPermission(Manifest.permission.ACCESS_FINE_LOCATION))
+                needed += Manifest.permission.ACCESS_FINE_LOCATION
+        }
+        if (needed.isNotEmpty()) {
+            ActivityCompat.requestPermissions(this, needed.toTypedArray(), REQ_PERMISSIONS)
+        }
+    }
+
+    private fun hasPermission(perm: String) =
+        ContextCompat.checkSelfPermission(this, perm) == PackageManager.PERMISSION_GRANTED
+
+    override fun onRequestPermissionsResult(
+        requestCode: Int, permissions: Array<out String>, grantResults: IntArray
+    ) {
+        super.onRequestPermissionsResult(requestCode, permissions, grantResults)
+        if (requestCode == REQ_PERMISSIONS &&
+            grantResults.any { it != PackageManager.PERMISSION_GRANTED }) {
+            toast("BLE permissions required")
+        }
+    }
+
+    // ---------------------------------------------------------------------------
+    // BLE Scan
+    // ---------------------------------------------------------------------------
+    private fun startScanIfPermitted() {
+        if (btAdapter?.isEnabled != true) { toast("Bluetooth is off"); return }
+        bleScanner = btAdapter.bluetoothLeScanner
+        val filter = ScanFilter.Builder()
+            .setDeviceNamePattern("UWB_TAG_.*".toRegex().toPattern())
+            .build()
+        val settings = ScanSettings.Builder()
+            .setScanMode(ScanSettings.SCAN_MODE_LOW_LATENCY)
+            .build()
+        deviceAdapter.clear()
+        bleScanner?.startScan(listOf(filter), settings, scanCallback)
+        isScanning = true
+        binding.btnScan.text = "Stop"
+        binding.tvScanStatus.text = "Scanning…"
+        mainHandler.postDelayed({ stopScan() }, SCAN_TIMEOUT_MS)
+    }
+
+    private fun stopScan() {
+        bleScanner?.stopScan(scanCallback)
+        isScanning = false
+        binding.btnScan.text = "Scan"
+        binding.tvScanStatus.text = "Scan stopped"
+    }
+
+    private val scanCallback = object : ScanCallback() {
+        override fun onScanResult(callbackType: Int, result: ScanResult) {
+            val name = result.device.name ?: return
+            if (!name.startsWith("UWB_TAG_")) return
+            val dev = ScannedDevice(
+                name    = name,
+                address = result.device.address,
+                rssi    = result.rssi,
+                device  = result.device
+            )
+            mainHandler.post { deviceAdapter.update(dev) }
+        }
+
+        override fun onScanFailed(errorCode: Int) {
+            mainHandler.post {
+                binding.tvScanStatus.text = "Scan failed (code $errorCode)"
+                isScanning = false
+                binding.btnScan.text = "Scan"
+            }
+        }
+    }
+
+    // ---------------------------------------------------------------------------
+    // GATT Connection
+    // ---------------------------------------------------------------------------
+    private fun connectToDevice(scanned: ScannedDevice) {
+        stopScan()
+        binding.tvScanStatus.text = "Connecting to ${scanned.name}…"
+        gatt = scanned.device.connectGatt(this, false, gattCallback, BluetoothDevice.TRANSPORT_LE)
+    }
+
+    private fun disconnectGatt() {
+        gatt?.disconnect()
+        gatt?.close()
+        gatt = null
+        configChar = null
+        statusChar = null
+        battChar = null
+        mainHandler.post {
+            binding.cardConfig.visibility = View.GONE
+            binding.tvScanStatus.text = "Disconnected"
+        }
+    }
+
+    private val gattCallback = object : BluetoothGattCallback() {
+
+        override fun onConnectionStateChange(g: BluetoothGatt, status: Int, newState: Int) {
+            when (newState) {
+                BluetoothProfile.STATE_CONNECTED -> {
+                    mainHandler.post { binding.tvScanStatus.text = "Connected — discovering services…" }
+                    g.discoverServices()
+                }
+                BluetoothProfile.STATE_DISCONNECTED -> {
+                    mainHandler.post {
+                        binding.cardConfig.visibility = View.GONE
+                        binding.tvScanStatus.text = "Disconnected"
+                        toast("Tag disconnected")
+                    }
+                    gatt?.close()
+                    gatt = null
+                }
+            }
+        }
+
+        override fun onServicesDiscovered(g: BluetoothGatt, status: Int) {
+            if (status != BluetoothGatt.GATT_SUCCESS) {
+                mainHandler.post { toast("Service discovery failed") }
+                return
+            }
+            val service = g.getService(SERVICE_UUID)
+            if (service == null) {
+                mainHandler.post { toast("UWB config service not found on tag") }
+                return
+            }
+            configChar = service.getCharacteristic(CHAR_CONFIG_UUID)
+            statusChar = service.getCharacteristic(CHAR_STATUS_UUID)
+            battChar   = service.getCharacteristic(CHAR_BATT_UUID)
+
+            // Subscribe to status notifications
+            statusChar?.let { enableNotifications(g, it) }
+            battChar?.let   { enableNotifications(g, it) }
+
+            // Initial config read
+            configChar?.let { g.readCharacteristic(it) }
+
+            mainHandler.post {
+                val devName = g.device.name ?: g.device.address
+                binding.tvConnectedName.text = "Connected: $devName"
+                binding.cardConfig.visibility = View.VISIBLE
+                binding.tvScanStatus.text = "Connected to $devName"
+            }
+        }
+
+        override fun onCharacteristicRead(
+            g: BluetoothGatt,
+            characteristic: BluetoothGattCharacteristic,
+            status: Int
+        ) {
+            if (status != BluetoothGatt.GATT_SUCCESS) return
+            if (characteristic.uuid == CHAR_CONFIG_UUID) {
+                val json = characteristic.value?.toString(Charsets.UTF_8) ?: return
+                val cfg = runCatching { gson.fromJson(json, TagConfig::class.java) }.getOrNull() ?: return
+                mainHandler.post { populateFields(cfg) }
+            }
+        }
+
+        // API 33+ callback
+        override fun onCharacteristicRead(
+            g: BluetoothGatt,
+            characteristic: BluetoothGattCharacteristic,
+            value: ByteArray,
+            status: Int
+        ) {
+            if (status != BluetoothGatt.GATT_SUCCESS) return
+            if (characteristic.uuid == CHAR_CONFIG_UUID) {
+                val json = value.toString(Charsets.UTF_8)
+                val cfg = runCatching { gson.fromJson(json, TagConfig::class.java) }.getOrNull() ?: return
+                mainHandler.post { populateFields(cfg) }
+            }
+        }
+
+        override fun onCharacteristicWrite(
+            g: BluetoothGatt,
+            characteristic: BluetoothGattCharacteristic,
+            status: Int
+        ) {
+            val msg = if (status == BluetoothGatt.GATT_SUCCESS) "Config written" else "Write failed ($status)"
+            mainHandler.post { toast(msg) }
+        }
+
+        override fun onCharacteristicChanged(
+            g: BluetoothGatt,
+            characteristic: BluetoothGattCharacteristic
+        ) {
+            val value = characteristic.value ?: return
+            handleNotification(characteristic.uuid, value)
+        }
+
+        // API 33+ callback
+        override fun onCharacteristicChanged(
+            g: BluetoothGatt,
+            characteristic: BluetoothGattCharacteristic,
+            value: ByteArray
+        ) {
+            handleNotification(characteristic.uuid, value)
+        }
+    }
+
+    // ---------------------------------------------------------------------------
+    // Notification helpers
+    // ---------------------------------------------------------------------------
+    private fun enableNotifications(g: BluetoothGatt, char: BluetoothGattCharacteristic) {
+        g.setCharacteristicNotification(char, true)
+        val descriptor = char.getDescriptor(CCCD_UUID) ?: return
+        descriptor.value = BluetoothGattDescriptor.ENABLE_NOTIFICATION_VALUE
+        g.writeDescriptor(descriptor)
+    }
+
+    private fun handleNotification(uuid: UUID, value: ByteArray) {
+        val text = value.toString(Charsets.UTF_8)
+        mainHandler.post {
+            when (uuid) {
+                CHAR_STATUS_UUID -> binding.tvTagStatus.text = "Status: $text"
+                CHAR_BATT_UUID   -> {
+                    val pct = text.toIntOrNull() ?: return@post
+                    binding.tvTagStatus.text = binding.tvTagStatus.text.toString()
+                        .replace(Regex("\\| Batt:.*"), "")
+                        .trimEnd() + " | Batt: $pct%"
+                }
+            }
+        }
+    }
+
+    // ---------------------------------------------------------------------------
+    // Config read / write
+    // ---------------------------------------------------------------------------
+    private fun readConfig() {
+        val g = gatt ?: run { toast("Not connected"); return }
+        val c = configChar ?: run { toast("Config char not found"); return }
+        g.readCharacteristic(c)
+    }
+
+    private fun writeConfig() {
+        val g = gatt ?: run { toast("Not connected"); return }
+        val c = configChar ?: run { toast("Config char not found"); return }
+        val cfg = buildConfigFromFields()
+        val json = gson.toJson(cfg)
+        if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.TIRAMISU) {
+            g.writeCharacteristic(c, json.toByteArray(Charsets.UTF_8),
+                BluetoothGattCharacteristic.WRITE_TYPE_DEFAULT)
+        } else {
+            @Suppress("DEPRECATION")
+            c.value = json.toByteArray(Charsets.UTF_8)
+            @Suppress("DEPRECATION")
+            g.writeCharacteristic(c)
+        }
+    }
+
+    // ---------------------------------------------------------------------------
+    // UI helpers
+    // ---------------------------------------------------------------------------
+    private fun populateFields(cfg: TagConfig) {
+        binding.etTagName.setText(cfg.tag_name)
+        binding.etSleepTimeout.setText(cfg.sleep_timeout_s.toString())
+        binding.etBrightness.setText(cfg.display_brightness.toString())
+        binding.etUwbChannel.setText(cfg.uwb_channel.toString())
+        binding.etRangingInterval.setText(cfg.ranging_interval_ms.toString())
+        binding.switchBatteryReport.isChecked = cfg.battery_report
+    }
+
+    private fun buildConfigFromFields() = TagConfig(
+        tag_name           = binding.etTagName.text?.toString() ?: "UWB_TAG_0001",
+        sleep_timeout_s    = binding.etSleepTimeout.text?.toString()?.toIntOrNull() ?: 300,
+        display_brightness = binding.etBrightness.text?.toString()?.toIntOrNull() ?: 50,
+        uwb_channel        = binding.etUwbChannel.text?.toString()?.toIntOrNull() ?: 9,
+        ranging_interval_ms = binding.etRangingInterval.text?.toString()?.toIntOrNull() ?: 100,
+        battery_report     = binding.switchBatteryReport.isChecked
+    )
+
+    private fun toast(msg: String) =
+        Toast.makeText(this, msg, Toast.LENGTH_SHORT).show()
+}

android/src/main/res/layout/activity_uwb_tag_ble.xml

@@ -0,0 +1,238 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+    xmlns:app="http://schemas.android.com/apk/res-auto"
+    android:layout_width="match_parent"
+    android:layout_height="match_parent"
+    android:orientation="vertical">
+
+    <androidx.appcompat.widget.Toolbar
+        android:id="@+id/toolbar"
+        android:layout_width="match_parent"
+        android:layout_height="?attr/actionBarSize"
+        android:background="?attr/colorPrimary"
+        android:elevation="4dp"
+        android:theme="@style/ThemeOverlay.AppCompat.Dark.ActionBar"
+        app:title="UWB Tag BLE Config" />
+
+    <!-- Scan controls -->
+    <LinearLayout
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:orientation="horizontal"
+        android:padding="12dp"
+        android:gravity="center_vertical">
+
+        <Button
+            android:id="@+id/btnScan"
+            style="@style/Widget.MaterialComponents.Button"
+            android:layout_width="wrap_content"
+            android:layout_height="wrap_content"
+            android:text="Scan" />
+
+        <TextView
+            android:id="@+id/tvScanStatus"
+            android:layout_width="0dp"
+            android:layout_height="wrap_content"
+            android:layout_weight="1"
+            android:layout_marginStart="12dp"
+            android:text="Tap Scan to find UWB tags"
+            android:textAppearance="@style/TextAppearance.MaterialComponents.Body2" />
+
+    </LinearLayout>
+
+    <!-- Scan results list -->
+    <TextView
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:paddingHorizontal="12dp"
+        android:text="Nearby Tags"
+        android:textAppearance="@style/TextAppearance.MaterialComponents.Subtitle1"
+        android:textStyle="bold" />
+
+    <androidx.recyclerview.widget.RecyclerView
+        android:id="@+id/rvDevices"
+        android:layout_width="match_parent"
+        android:layout_height="0dp"
+        android:layout_weight="1"
+        android:padding="8dp"
+        android:clipToPadding="false" />
+
+    <!-- Connected device config panel -->
+    <com.google.android.material.card.MaterialCardView
+        android:id="@+id/cardConfig"
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:layout_margin="8dp"
+        android:visibility="gone"
+        app:cardElevation="4dp">
+
+        <LinearLayout
+            android:layout_width="match_parent"
+            android:layout_height="wrap_content"
+            android:orientation="vertical"
+            android:padding="12dp">
+
+            <LinearLayout
+                android:layout_width="match_parent"
+                android:layout_height="wrap_content"
+                android:orientation="horizontal"
+                android:gravity="center_vertical">
+
+                <TextView
+                    android:id="@+id/tvConnectedName"
+                    android:layout_width="0dp"
+                    android:layout_height="wrap_content"
+                    android:layout_weight="1"
+                    android:text="Connected: —"
+                    android:textAppearance="@style/TextAppearance.MaterialComponents.Subtitle1"
+                    android:textStyle="bold" />
+
+                <Button
+                    android:id="@+id/btnDisconnect"
+                    style="@style/Widget.MaterialComponents.Button.OutlinedButton"
+                    android:layout_width="wrap_content"
+                    android:layout_height="wrap_content"
+                    android:text="Disconnect" />
+
+            </LinearLayout>
+
+            <!-- tag_name -->
+            <com.google.android.material.textfield.TextInputLayout
+                style="@style/Widget.MaterialComponents.TextInputLayout.OutlinedBox.Dense"
+                android:layout_width="match_parent"
+                android:layout_height="wrap_content"
+                android:layout_marginTop="8dp"
+                android:hint="Tag Name">
+                <com.google.android.material.textfield.TextInputEditText
+                    android:id="@+id/etTagName"
+                    android:layout_width="match_parent"
+                    android:layout_height="wrap_content"
+                    android:inputType="text" />
+            </com.google.android.material.textfield.TextInputLayout>
+
+            <!-- sleep_timeout_s and uwb_channel (row) -->
+            <LinearLayout
+                android:layout_width="match_parent"
+                android:layout_height="wrap_content"
+                android:orientation="horizontal"
+                android:layout_marginTop="4dp">
+
+                <com.google.android.material.textfield.TextInputLayout
+                    style="@style/Widget.MaterialComponents.TextInputLayout.OutlinedBox.Dense"
+                    android:layout_width="0dp"
+                    android:layout_height="wrap_content"
+                    android:layout_weight="1"
+                    android:layout_marginEnd="4dp"
+                    android:hint="Sleep Timeout (s)">
+                    <com.google.android.material.textfield.TextInputEditText
+                        android:id="@+id/etSleepTimeout"
+                        android:layout_width="match_parent"
+                        android:layout_height="wrap_content"
+                        android:inputType="number" />
+                </com.google.android.material.textfield.TextInputLayout>
+
+                <com.google.android.material.textfield.TextInputLayout
+                    style="@style/Widget.MaterialComponents.TextInputLayout.OutlinedBox.Dense"
+                    android:layout_width="0dp"
+                    android:layout_height="wrap_content"
+                    android:layout_weight="1"
+                    android:layout_marginStart="4dp"
+                    android:hint="UWB Channel">
+                    <com.google.android.material.textfield.TextInputEditText
+                        android:id="@+id/etUwbChannel"
+                        android:layout_width="match_parent"
+                        android:layout_height="wrap_content"
+                        android:inputType="number" />
+                </com.google.android.material.textfield.TextInputLayout>
+
+            </LinearLayout>
+
+            <!-- display_brightness and ranging_interval_ms (row) -->
+            <LinearLayout
+                android:layout_width="match_parent"
+                android:layout_height="wrap_content"
+                android:orientation="horizontal"
+                android:layout_marginTop="4dp">
+
+                <com.google.android.material.textfield.TextInputLayout
+                    style="@style/Widget.MaterialComponents.TextInputLayout.OutlinedBox.Dense"
+                    android:layout_width="0dp"
+                    android:layout_height="wrap_content"
+                    android:layout_weight="1"
+                    android:layout_marginEnd="4dp"
+                    android:hint="Brightness (0-100)">
+                    <com.google.android.material.textfield.TextInputEditText
+                        android:id="@+id/etBrightness"
+                        android:layout_width="match_parent"
+                        android:layout_height="wrap_content"
+                        android:inputType="number" />
+                </com.google.android.material.textfield.TextInputLayout>
+
+                <com.google.android.material.textfield.TextInputLayout
+                    style="@style/Widget.MaterialComponents.TextInputLayout.OutlinedBox.Dense"
+                    android:layout_width="0dp"
+                    android:layout_height="wrap_content"
+                    android:layout_weight="1"
+                    android:layout_marginStart="4dp"
+                    android:hint="Ranging Interval (ms)">
+                    <com.google.android.material.textfield.TextInputEditText
+                        android:id="@+id/etRangingInterval"
+                        android:layout_width="match_parent"
+                        android:layout_height="wrap_content"
+                        android:inputType="number" />
+                </com.google.android.material.textfield.TextInputLayout>
+
+            </LinearLayout>
+
+            <!-- battery_report toggle -->
+            <com.google.android.material.switchmaterial.SwitchMaterial
+                android:id="@+id/switchBatteryReport"
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content"
+                android:layout_marginTop="8dp"
+                android:text="Battery Reporting" />
+
+            <!-- Action buttons -->
+            <LinearLayout
+                android:layout_width="match_parent"
+                android:layout_height="wrap_content"
+                android:orientation="horizontal"
+                android:layout_marginTop="8dp">
+
+                <Button
+                    android:id="@+id/btnReadConfig"
+                    style="@style/Widget.MaterialComponents.Button.OutlinedButton"
+                    android:layout_width="0dp"
+                    android:layout_height="wrap_content"
+                    android:layout_weight="1"
+                    android:layout_marginEnd="4dp"
+                    android:text="Read" />
+
+                <Button
+                    android:id="@+id/btnWriteConfig"
+                    style="@style/Widget.MaterialComponents.Button"
+                    android:layout_width="0dp"
+                    android:layout_height="wrap_content"
+                    android:layout_weight="1"
+                    android:layout_marginStart="4dp"
+                    android:text="Write" />
+
+            </LinearLayout>
+
+            <!-- Status notifications from tag -->
+            <TextView
+                android:id="@+id/tvTagStatus"
+                android:layout_width="match_parent"
+                android:layout_height="wrap_content"
+                android:layout_marginTop="8dp"
+                android:background="#1A000000"
+                android:fontFamily="monospace"
+                android:padding="8dp"
+                android:text="Tag status: —"
+                android:textAppearance="@style/TextAppearance.MaterialComponents.Caption" />
+
+        </LinearLayout>
+
+    </com.google.android.material.card.MaterialCardView>
+
+</LinearLayout>

android/src/main/res/layout/item_ble_device.xml

@@ -0,0 +1,60 @@
+<?xml version="1.0" encoding="utf-8"?>
+<com.google.android.material.card.MaterialCardView
+    xmlns:android="http://schemas.android.com/apk/res/android"
+    xmlns:app="http://schemas.android.com/apk/res-auto"
+    android:layout_width="match_parent"
+    android:layout_height="wrap_content"
+    android:layout_margin="4dp"
+    app:cardElevation="2dp"
+    android:clickable="true"
+    android:focusable="true">
+
+    <LinearLayout
+        android:layout_width="match_parent"
+        android:layout_height="wrap_content"
+        android:orientation="horizontal"
+        android:padding="12dp"
+        android:gravity="center_vertical">
+
+        <LinearLayout
+            android:layout_width="0dp"
+            android:layout_height="wrap_content"
+            android:layout_weight="1"
+            android:orientation="vertical">
+
+            <TextView
+                android:id="@+id/tvDeviceName"
+                android:layout_width="match_parent"
+                android:layout_height="wrap_content"
+                android:text="UWB_TAG_XXXX"
+                android:textAppearance="@style/TextAppearance.MaterialComponents.Subtitle2"
+                android:textStyle="bold" />
+
+            <TextView
+                android:id="@+id/tvDeviceAddress"
+                android:layout_width="match_parent"
+                android:layout_height="wrap_content"
+                android:text="XX:XX:XX:XX:XX:XX"
+                android:textAppearance="@style/TextAppearance.MaterialComponents.Caption" />
+
+        </LinearLayout>
+
+        <TextView
+            android:id="@+id/tvRssi"
+            android:layout_width="wrap_content"
+            android:layout_height="wrap_content"
+            android:text="-70 dBm"
+            android:textAppearance="@style/TextAppearance.MaterialComponents.Caption"
+            android:textColor="?attr/colorSecondary" />
+
+        <Button
+            android:id="@+id/btnConnect"
+            style="@style/Widget.MaterialComponents.Button.OutlinedButton"
+            android:layout_width="wrap_content"
+            android:layout_height="wrap_content"
+            android:layout_marginStart="8dp"
+            android:text="Connect" />
+
+    </LinearLayout>
+
+</com.google.android.material.card.MaterialCardView>

chassis/ASSEMBLY.md

@@ -56,10 +56,15 @@
 3. Fasten 4× M4×12 SHCS. Torque 2.5 N·m.
 4. Insert battery pack; route Velcro straps through slots and cinch.
 
-### 7  FC mount (MAMBA F722S)
+### 7  MCU mount (ESP32 BALANCE + ESP32 IO)
+
+> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** ESP32 BALANCE retired. Two ESP32 boards replace it.
+> Board dimensions and hole patterns TBD — await spec from max before machining mount plate.
+
 1. Place silicone anti-vibration grommets onto nylon M3 standoffs.
-2. Lower FC onto standoffs; secure with M3×6 BHCS. Snug only — do not over-torque.
-3. Orient USB-C port toward front of robot for cable access.
+2. Lower ESP32 BALANCE board onto standoffs; secure with M3×6 BHCS. Snug only.
+3. Mount ESP32 IO board adjacent — exact placement TBD pending board dimensions.
+4. Orient USB connectors toward front of robot for cable access.
 
 ### 8  Jetson Nano mount plate
 1. Press or thread M3 nylon standoffs (8mm) into plate holes.
@@ -86,7 +91,8 @@
 | Wheelbase (axle C/L to C/L) | 600 mm | ±1 mm |
 | Motor fork slot width | 24 mm | +0.5 / 0 |
 | Motor fork dropout depth | 60 mm | ±0.5 mm |
-| FC hole pattern | 30.5 × 30.5 mm | ±0.2 mm |
+| ESP32 BALANCE hole pattern | TBD — await spec from max | ±0.2 mm |
+| ESP32 IO hole pattern | TBD — await spec from max | ±0.2 mm |
 | Jetson hole pattern | 58 × 58 mm | ±0.2 mm |
 | Battery tray inner | 185 × 72 × 52 mm | +2 / 0 mm |
 

chassis/BOM.md

@@ -41,7 +41,7 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
 | 3 | Dropout clamp — upper | 2 | 8mm 6061-T6 Al | 90×70mm blank | D-cut bore; `RENDER="clamp_upper_2d"` |
 | 4 | Stem flange ring | 2 | 6mm Al or acrylic | Ø82mm disc | One above + one below plate; `RENDER="stem_flange_2d"` |
 | 5 | Vertical stem tube | 1 | 38.1mm OD × 1.5mm wall 6061-T6 Al | 1050mm length | 1.5" EMT conduit is a drop-in alternative |
-| 6 | FC standoff M3×6mm nylon | 4 | Nylon | — | MAMBA F722S vibration isolation |
+| 6 | MCU standoff M3×6mm nylon | 4 | Nylon | — | ESP32 BALANCE / IO board isolation (dimensions TBD) |
 | 7 | Ø4mm × 16mm alignment pin | 8 | Steel dowel | — | Dropout clamp-to-plate alignment |
 
 ### Battery Stem Clamp (`stem_battery_clamp.scad`) — Part B
@@ -88,12 +88,16 @@ PR #7 (`chassis_frame.scad`) used placeholder values. The table below records th
 
 ## Electronics Mounts
 
+> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** ESP32 BALANCE (ESP32) is retired.
+> Replaced by **ESP32 BALANCE** + **ESP32 IO**. Board dimensions and hole patterns TBD — await spec from max.
+
 | # | Part | Qty | Spec | Notes |
 |---|------|-----|------|-------|
-| 13 | STM32 MAMBA F722S FC | 1 | 36×36mm PCB, 30.5×30.5mm M3 mount | Oriented USB-C port toward front |
-| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | FC vibration isolation |
-| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under FC mount pads |
-| 16 | Jetson Nano B01 module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
+| 13 | ESP32 BALANCE board | 1 | TBD — mount pattern TBD | PID balance loop; replaces ESP32 BALANCE |
+| 13b | ESP32 IO board | 1 | TBD — mount pattern TBD | Motor/sensor/comms I/O |
+| 14 | Nylon M3 standoff 6mm | 4 | F/F nylon | ESP32 board isolation |
+| 15 | Anti-vibration grommet M3 | 4 | Ø6mm silicone | Under ESP32 mount pads |
+| 16 | Jetson Orin module | 1 | 69.6×45mm module + carrier | 58×58mm M3 carrier hole pattern |
 | 17 | Nylon M3 standoff 8mm | 4 | F/F nylon | Jetson board standoffs |
 
 ---
@@ -144,8 +148,8 @@ Slide entire carousel up/down the stem with M6 collar bolts loosened. Tighten at
 | 26 | M6×60 SHCS  | 4  | ISO 4762, SS | Collar clamping bolts |
 | 27 | M6 hex nut  | 4  | ISO 4032, SS | Captured in collar pockets |
 | 28 | M6×12 set screw | 2 | ISO 4026, SS cup-point | Stem height lock (1 per collar half) |
-| 29 | M3×10 SHCS | 12 | ISO 4762, SS | FC mount + miscellaneous |
-| 30 | M3×6 BHCS  | 4  | ISO 4762, SS | FC board bolts |
+| 29 | M3×10 SHCS | 12 | ISO 4762, SS | ESP32 mount + miscellaneous |
+| 30 | M3×6 BHCS  | 4  | ISO 4762, SS | ESP32 board bolts (qty TBD pending board spec) |
 | 31 | Axle lock nut (match axle tip thread) | 4 | Flanged, confirm thread | 2 per motor |
 | 32 | Flat washer M5 | 32 | SS | |
 | 33 | Flat washer M4 | 32 | SS | |

chassis/ip54_BOM.md

@@ -104,7 +104,7 @@ IP54-rated enclosures and sensor housings for all-weather outdoor robot operatio
 | Component | Thermal strategy | Max junction | Enclosure budget |
 |-----------|-----------------|-------------|-----------------|
 | Jetson Orin NX | Al pad → lid → fan forced convection | 95 °C Tj | Target ≤ 60 °C case |
-| FC (MAMBA F722S) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK |
+| FC (ESP32 BALANCE) | Passive; FC has own EMI shield | 85 °C | <60 °C ambient OK |
 | ESC × 2 | Al pad → lid | 100 °C Tj | Target ≤ 60 °C |
 | D435i | Passive; housing vent gap on rear cap | 45 °C surface | — |
 

chassis/vesc_mount.scad

@@ -0,0 +1,296 @@
+// ============================================================
+// vesc_mount.scad — FSESC 6.7 Pro Mini Dual ESC Mount Cradle
+// Issue #699 / sl-mechanical  2026-03-17
+// ============================================================
+// Open-top tray for Flipsky FSESC 6.7 Pro Mini Dual (~100 × 68 × 28 mm).
+// Attaches to 2020 aluminium T-slot rail via 4× M5 T-nuts
+// (2× per rail, two parallel rails, 60 mm bolt spacing in X,
+//  20 mm bolt spacing in Y matching 2020 slot pitch).
+//
+// Connector access:
+//   XT60 battery inputs    — X− end wall cutouts (2 connectors, side-by-side)
+//   XT30 motor outputs     — Y+ and Y− side wall cutouts (2 per side wall)
+//   CAN/UART terminal      — X+ end wall cutout (screw terminal, wire exit)
+//
+// Ventilation:
+//   Open top face          — heatsink fins fully exposed
+//   Floor grille slots     — under-board airflow
+//   Side vent louvres      — 4 slots on each Y± wall at heatsink height
+//
+// Retention: 4× M3 heat-set insert boss in floor — board screws down through
+// ESC mounting holes via M3×8 FHCS. Board sits on 4 mm raised posts for
+// under-board airflow.
+//
+// ⚠  VERIFY WITH CALIPERS BEFORE PRINTING:
+//    PCB_L, PCB_W                   board outline
+//    XT60_W, XT60_H                 XT60 shell at X− edge
+//    XT30_W, XT30_H                 XT30 shells at Y± edges
+//    TERM_W, TERM_H                 CAN screw terminal at X+ edge
+//    MOUNT_X1/X2, MOUNT_Y1/Y2      ESC board mounting hole pattern
+//
+// Print settings (PETG):
+//   3 perimeters, 40 % gyroid infill, no supports, 0.2 mm layer
+//   Print orientation: open face UP (as modelled)
+//
+// BOM:
+//   4 × M5×10 BHCS  +  4 × M5 slide-in T-nut  (2020 rail)
+//   4 × M3 heat-set insert (Voron-style, OD 4.5 mm × 4 mm deep)
+//   4 × M3×8 FHCS  (board retention)
+//
+// Export commands:
+//   openscad -D 'RENDER="mount"'    -o vesc_mount.stl    vesc_mount.scad
+//   openscad -D 'RENDER="assembly"' -o vesc_assembly.png vesc_mount.scad
+// ============================================================
+
+RENDER = "assembly"; // mount | assembly
+
+$fn = 48;
+EPS = 0.01;
+
+// ── ⚠ Verify before printing ─────────────────────────────────
+// FSESC 6.7 Pro Mini Dual PCB
+PCB_L    = 100.0;  // board length (X: XT60 end → CAN terminal end)
+PCB_W    =  68.0;  // board width  (Y)
+PCB_T    =  2.0;   // board thickness (incl. bottom-side components)
+COMP_H   =  26.0;  // tallest component above board top face (heatsink ~26 mm)
+
+// XT60 battery connectors at X− end (2 connectors, side-by-side)
+XT60_W   =  16.0;  // each XT60 shell width (Y)
+XT60_H   =  16.0;  // each XT60 shell height (Z) above board surface
+XT60_Z0  =   0.0;  // connector bottom offset above board surface
+// Y centres of each XT60 measured from PCB Y− edge
+XT60_Y1  =  16.0;
+XT60_Y2  =  52.0;
+
+// XT30 motor output connectors at Y± sides (2 per side)
+XT30_W   =  10.5;  // each XT30 shell width (X span)
+XT30_H   =  12.0;  // each XT30 shell height (Z) above board surface
+XT30_Z0  =   0.5;  // connector bottom offset above board surface
+// X centres measured from PCB X− edge (same layout both Y− and Y+ sides)
+XT30_X1  =  22.0;
+XT30_X2  =  78.0;
+
+// CAN / UART screw terminal block at X+ end (3-pos 3.5 mm pitch)
+TERM_W   =  14.0;  // terminal block Y span
+TERM_H   =  10.0;  // terminal block height above board surface
+TERM_Z0  =   0.5;  // terminal bottom offset above board surface
+TERM_Y_CTR = PCB_W / 2;
+
+// ── ESC board mounting hole pattern ──────────────────────────
+// 4 corner holes, 4 mm inset from each PCB edge
+MOUNT_INSET  =  4.0;
+MOUNT_X1     = MOUNT_INSET;
+MOUNT_X2     = PCB_L - MOUNT_INSET;
+MOUNT_Y1     = MOUNT_INSET;
+MOUNT_Y2     = PCB_W - MOUNT_INSET;
+
+M3_INSERT_OD =  4.6;   // Voron M3 heat-set insert press-fit OD
+M3_INSERT_H  =  4.0;   // insert depth
+M3_CLEAR_D   =  3.4;   // M3 clearance bore below insert
+
+// ── Cradle geometry ──────────────────────────────────────────
+WALL_T   =  2.8;   // side / end wall thickness
+FLOOR_T  =  4.5;   // floor plate thickness (fits M5 BHCS head pocket)
+POST_H   =  4.0;   // standoff post height (board lifts off floor for airflow)
+CL_SIDE  =  0.35;  // Y clearance per side
+CL_END   =  0.40;  // X clearance per end
+
+INN_W    = PCB_W + 2*CL_SIDE;
+INN_L    = PCB_L + 2*CL_END;
+INN_H    = POST_H + PCB_T + COMP_H + 1.5;
+
+OTR_W    = INN_W  + 2*WALL_T;
+OTR_L    = INN_L  + 2*WALL_T;
+OTR_H    = FLOOR_T + INN_H;
+
+PCB_X0   = WALL_T + CL_END;
+PCB_Y0   = WALL_T + CL_SIDE;
+PCB_Z0   = FLOOR_T + POST_H;
+
+// ── M5 T-nut mount (2020 rail) ────────────────────────────────
+// 4 bolts: 2 columns (X) × 2 rows (Y), centred on body
+M5_D       =  5.3;
+M5_HEAD_D  =  9.5;
+M5_HEAD_H  =  3.0;
+M5_SPAC_X  = 60.0;   // X bolt spacing
+M5_SPAC_Y  = 20.0;   // Y bolt spacing (2020 T-slot pitch)
+
+// ── Floor ventilation grille ──────────────────────────────────
+GRILLE_SLOT_W =  4.0;
+GRILLE_SLOT_T = FLOOR_T - 1.5;
+GRILLE_PITCH  = 10.0;
+GRILLE_X0     = WALL_T + 14;
+GRILLE_X_LEN  = OTR_L - 2*WALL_T - 28;
+GRILLE_N      = floor((INN_W - 10) / GRILLE_PITCH);
+
+// ── Side vent louvres on Y± walls ────────────────────────────
+LOUV_H     =  5.0;
+LOUV_W     = 12.0;
+LOUV_Z     = FLOOR_T + POST_H + PCB_T + 4.0;  // mid-heatsink height
+LOUV_N     =  4;
+LOUV_PITCH = (OTR_L - 2*WALL_T - 20) / max(LOUV_N - 1, 1);
+
+// ── CAN wire strain relief bosses (X+ end) ───────────────────
+SR_BOSS_OD =  7.0;
+SR_BOSS_H  =  6.0;
+SR_SLOT_W  =  3.5;
+SR_SLOT_T  =  2.2;
+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;
+
+// ─────────────────────────────────────────────────────────────
+module m3_insert_boss() {
+    // Solid post with heat-set insert bore from top
+    post_h = FLOOR_T + POST_H;
+    difference() {
+        cylinder(d = M3_INSERT_OD + 3.2, h = post_h);
+        // Insert bore from top
+        translate([0, 0, post_h - M3_INSERT_H])
+            cylinder(d = M3_INSERT_OD, h = M3_INSERT_H + EPS);
+        // Clearance bore from bottom
+        translate([0, 0, -EPS])
+            cylinder(d = M3_CLEAR_D, h = post_h - M3_INSERT_H + EPS);
+    }
+}
+
+module vesc_mount() {
+    difference() {
+        union() {
+            // Main body
+            cube([OTR_L, OTR_W, OTR_H]);
+
+            // M3 insert bosses at board mounting corners
+            for (mx = [MOUNT_X1, MOUNT_X2])
+            for (my = [MOUNT_Y1, MOUNT_Y2])
+                translate([PCB_X0 + mx, PCB_Y0 + my, 0])
+                    m3_insert_boss();
+
+            // CAN strain relief bosses on X+ end
+            for (sy = [SR_Y1, SR_Y2])
+                translate([SR_X, sy, 0])
+                    cylinder(d = SR_BOSS_OD, h = SR_BOSS_H);
+        }
+
+        // ── Interior cavity (open top) ─────────────────────────
+        translate([WALL_T, WALL_T, FLOOR_T])
+            cube([INN_L, INN_W, INN_H + EPS]);
+
+        // ── XT60 cutouts at X− end (2 connectors) ──────────────
+        for (yc = [XT60_Y1, XT60_Y2])
+            translate([-EPS,
+                       PCB_Y0 + yc - (XT60_W + 2.0)/2,
+                       PCB_Z0 + XT60_Z0 - 0.5])
+                cube([WALL_T + 2*EPS, XT60_W + 2.0, XT60_H + 3.0]);
+
+        // ── XT30 cutouts at Y− side (2 connectors) ─────────────
+        for (xc = [XT30_X1, XT30_X2])
+            translate([PCB_X0 + xc - (XT30_W + 2.0)/2,
+                       -EPS,
+                       PCB_Z0 + XT30_Z0 - 0.5])
+                cube([XT30_W + 2.0, WALL_T + 2*EPS, XT30_H + 3.0]);
+
+        // ── XT30 cutouts at Y+ side (2 connectors) ─────────────
+        for (xc = [XT30_X1, XT30_X2])
+            translate([PCB_X0 + xc - (XT30_W + 2.0)/2,
+                       OTR_W - WALL_T - EPS,
+                       PCB_Z0 + XT30_Z0 - 0.5])
+                cube([XT30_W + 2.0, WALL_T + 2*EPS, XT30_H + 3.0]);
+
+        // ── CAN terminal cutout at X+ end ──────────────────────
+        translate([OTR_L - WALL_T - EPS,
+                   PCB_Y0 + TERM_Y_CTR - (TERM_W + 3.0)/2,
+                   PCB_Z0 + TERM_Z0 - 0.5])
+            cube([WALL_T + 2*EPS, TERM_W + 3.0, TERM_H + 5.0]);
+
+        // ── Floor ventilation grille ───────────────────────────
+        for (i = [0 : GRILLE_N - 1]) {
+            sy = WALL_T + 5 + i * GRILLE_PITCH;
+            translate([GRILLE_X0, sy, -EPS])
+                cube([GRILLE_X_LEN, GRILLE_SLOT_W, GRILLE_SLOT_T + EPS]);
+        }
+
+        // ── Side vent louvres — Y− wall ────────────────────────
+        for (i = [0 : LOUV_N - 1]) {
+            lx = WALL_T + 10 + i * LOUV_PITCH;
+            translate([lx, -EPS, LOUV_Z])
+                cube([LOUV_W, WALL_T + 2*EPS, LOUV_H]);
+        }
+
+        // ── Side vent louvres — Y+ wall ────────────────────────
+        for (i = [0 : LOUV_N - 1]) {
+            lx = WALL_T + 10 + i * LOUV_PITCH;
+            translate([lx, OTR_W - WALL_T - EPS, LOUV_Z])
+                cube([LOUV_W, WALL_T + 2*EPS, LOUV_H]);
+        }
+
+        // ── M5 BHCS head pockets (4 bolts, bottom face) ────────
+        for (mx = [OTR_L/2 - M5_SPAC_X/2, OTR_L/2 + M5_SPAC_X/2])
+        for (my = [OTR_W/2 - M5_SPAC_Y/2, OTR_W/2 + M5_SPAC_Y/2])
+            translate([mx, my, -EPS]) {
+                cylinder(d = M5_D,      h = FLOOR_T + 2*EPS);
+                cylinder(d = M5_HEAD_D, h = M5_HEAD_H + EPS);
+            }
+
+        // ── Zip-tie slots through CAN 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 + 16, WALL_T + 6, -EPS])
+            cube([OTR_L - 2*WALL_T - 32, OTR_W - 2*WALL_T - 12,
+                  FLOOR_T - 2.0 + EPS]);
+    }
+}
+
+// ── Assembly preview ─────────────────────────────────────────
+if (RENDER == "assembly") {
+    color("DimGray", 0.93) vesc_mount();
+
+    // Phantom PCB
+    color("ForestGreen", 0.30)
+        translate([PCB_X0, PCB_Y0, PCB_Z0])
+            cube([PCB_L, PCB_W, PCB_T]);
+
+    // Phantom heatsink / component block
+    color("SlateGray", 0.22)
+        translate([PCB_X0, PCB_Y0, PCB_Z0 + PCB_T])
+            cube([PCB_L, PCB_W, COMP_H]);
+
+    // XT60 connector highlights (X− end)
+    for (yc = [XT60_Y1, XT60_Y2])
+        color("Gold", 0.85)
+            translate([-2,
+                       PCB_Y0 + yc - XT60_W/2,
+                       PCB_Z0 + XT60_Z0])
+                cube([WALL_T + 3, XT60_W, XT60_H]);
+
+    // XT30 connector highlights — Y− side
+    for (xc = [XT30_X1, XT30_X2])
+        color("OrangeRed", 0.80)
+            translate([PCB_X0 + xc - XT30_W/2,
+                       -2,
+                       PCB_Z0 + XT30_Z0])
+                cube([XT30_W, WALL_T + 3, XT30_H]);
+
+    // XT30 connector highlights — Y+ side
+    for (xc = [XT30_X1, XT30_X2])
+        color("OrangeRed", 0.80)
+            translate([PCB_X0 + xc - XT30_W/2,
+                       OTR_W - WALL_T - 1,
+                       PCB_Z0 + XT30_Z0])
+                cube([XT30_W, WALL_T + 3, XT30_H]);
+
+    // CAN terminal highlight
+    color("Tomato", 0.75)
+        translate([OTR_L - WALL_T - 1,
+                   PCB_Y0 + TERM_Y_CTR - TERM_W/2,
+                   PCB_Z0 + TERM_Z0])
+            cube([WALL_T + 3, TERM_W, TERM_H]);
+
+} else {
+    vesc_mount();
+}

docs/AGENTS.md

@@ -2,22 +2,29 @@
 
 You're working on **SaltyLab**, a self-balancing two-wheeled indoor robot. Read this entire file before touching anything.
 
-## Project Overview
+## ⚠️ ARCHITECTURE — SAUL-TEE (finalised 2026-04-04)
 
-A hoverboard-based balancing robot with two compute layers:
-1. **FC (Flight Controller)** — MAMBA F722S (STM32F722RET6 + MPU6000 IMU). Runs a lean C balance loop at up to 8kHz. Talks UART to the hoverboard ESC. This is the safety-critical layer.
-2. **Jetson Nano** — AI brain. ROS2, SLAM, person tracking. Sends velocity commands to FC via UART. Not safety-critical — FC operates independently.
+Full hardware spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md` — **read it before writing firmware.**
+
+| Board | Role |
+|-------|------|
+| **ESP32-S3 BALANCE** | Waveshare Touch LCD 1.28 (CH343 USB). QMI8658 IMU, PID loop, CAN→VESC L(68)/R(56), GC9A01 LCD |
+| **ESP32-S3 IO** | Bare devkit (JTAG USB). TBS Crossfire RC (UART0), ELRS failover (UART2), BTS7960 motors, NFC/baro/ToF, WS2812, buzzer/horn/headlight/fan |
+| **Jetson Orin** | CANable2 USB→CAN. Cmds on 0x300–0x303, telemetry on 0x400–0x401 |
 
 ```
-Jetson (speed+steer via UART1)  ←→  ELRS RC (UART3, kill switch)
-         │
-         ▼
-   MAMBA F722S (MPU6000 IMU, PID balance)
-         │
-         ▼ UART2
-   Hoverboard ESC (FOC) → 2× 8" hub motors
+Jetson Orin ──CANable2──► CAN 500kbps ◄───────────────────────┐
+                                │                               │
+                         ESP32-S3 BALANCE ←─UART 460800─► ESP32-S3 IO
+                         (QMI8658, PID loop)              (BTS7960, RC, sensors)
+                                │ CAN 500kbps
+                      ┌─────────┴──────────┐
+                 VESC Left (ID 68)    VESC Right (ID 56)
 ```
 
+Frame: `[0xAA][LEN][TYPE][PAYLOAD][CRC8]`  
+Legacy `src/` STM32 HAL code is **archived — do not extend.**
+
 ## ⚠️ SAFETY — READ THIS OR PEOPLE GET HURT
 
 This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, and launch the frame. Every firmware change must preserve these invariants:
@@ -35,7 +42,7 @@ This is not a toy. 8" hub motors + 36V battery can crush fingers, break toes, an
 ## Repository Layout
 
 ```
-firmware/              # STM32 HAL firmware (PlatformIO)
+firmware/              # Legacy ESP32/STM32 HAL firmware (PlatformIO, archived)
 ├── src/
 │   ├── main.c         # Entry point, clock config, main loop
 │   ├── icm42688.c     # ICM-42688-P SPI driver (backup IMU — currently broken)
@@ -82,11 +89,11 @@ PLATFORM.md            # Hardware platform reference
 
 ## Hardware Quick Reference
 
-### MAMBA F722S Flight Controller
+### ESP32 BALANCE Flight Controller
 
 | Spec | Value |
 |------|-------|
-| MCU | STM32F722RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) |
+| MCU | ESP32RET6 (Cortex-M7, 216MHz, 512KB flash, 256KB RAM) |
 | Primary IMU | MPU6000 (WHO_AM_I = 0x68) |
 | IMU Bus | SPI1: PA5=SCK, PA6=MISO, PA7=MOSI, CS=PA4 |
 | IMU EXTI | PC4 (data ready interrupt) |
@@ -160,7 +167,7 @@ PLATFORM.md            # Hardware platform reference
 ### Critical Lessons Learned (DON'T REPEAT THESE)
 
 1. **SysTick_Handler with HAL_IncTick() is MANDATORY** — without it, HAL_Delay() and every HAL timeout hangs forever. This bricked us multiple times.
-2. **DCache breaks SPI on STM32F7** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it.
+2. **DCache breaks SPI on ESP32** — disable DCache or use cache-aligned DMA buffers with clean/invalidate. We disable it.
 3. **`-(int)0 == 0`** — checking `if (-result)` to detect errors doesn't work when result is 0 (success and failure look the same). Always use explicit error codes.
 4. **NEVER auto-run untested code on_boot** — we bricked the NSPanel 3x doing this. Test manually first.
 5. **USB CDC needs ReceivePacket() primed in CDC_Init** — without it, the OUT endpoint never starts listening. No data reception.
@@ -172,7 +179,7 @@ The firmware supports reboot-to-DFU via USB command:
 2. Firmware writes `0xDEADBEEF` to RTC backup register 0
 3. `NVIC_SystemReset()` — clean hardware reset
 4. On boot, `checkForBootloader()` (called after `HAL_Init()`) reads the magic
-5. If magic found: clears it, remaps system memory, jumps to STM32 bootloader at `0x1FF00000`
+5. If magic found: clears it, remaps system memory, jumps to ESP32 BALANCE bootloader at `0x1FF00000`
 6. Board appears as DFU device, ready for `dfu-util` flash
 
 ### Build & Flash

docs/FACE_LCD_ANIMATION.md

@@ -1,6 +1,6 @@
 # Face LCD Animation System (Issue #507)
 
-Implements expressive face animations on an STM32 LCD display with 5 core emotions and smooth transitions.
+Implements expressive face animations on an ESP32 LCD display with 5 core emotions and smooth transitions.
 
 ## Features
 
@@ -82,7 +82,7 @@ STATUS      → Echo current emotion + idle state
 - Colors: Monochrome (1-bit) or RGB565
 
 ### Microcontroller
-- STM32F7xx (Mamba F722S)
+- ESP32xx (ESP32 BALANCE)
 - Available UART: USART3 (PB10=TX, PB11=RX)
 - Clock: 216 MHz
 

docs/SALTYLAB.md

@@ -1,6 +1,6 @@
-# SaltyLab — Self-Balancing Indoor Bot 🔬
+# SAUL-TEE — Self-Balancing Wagon Robot 🔬
 
-Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
+Four-wheel wagon (870×510×550 mm, 23 kg). Full spec: `docs/SAUL-TEE-SYSTEM-REFERENCE.md`
 
 ## ⚠️ SAFETY — TOP PRIORITY
 
@@ -32,8 +32,10 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
 |------|--------|
 | 2x 8" pneumatic hub motors (36 PSI) | ✅ Have |
 | 1x hoverboard ESC (FOC firmware) | ✅ Have |
-| 1x Drone FC (STM32F745 + MPU-6000) | ✅ Have — balance brain |
-| 1x Jetson Nano + Noctua fan | ✅ Have |
+| ~~1x Drone FC (ESP3245 + MPU-6000)~~ | ❌ RETIRED — replaced by ESP32 BALANCE |
+| 1x ESP32 BALANCE (PID loop) | ⬜ TBD — spec from max |
+| 1x ESP32 IO (motors/sensors/comms) | ⬜ TBD — spec from max |
+| 1x Jetson Orin + Noctua fan | ✅ Have |
 | 1x RealSense D435i | ✅ Have |
 | 1x RPLIDAR A1M8 | ✅ Have |
 | 1x battery pack (36V) | ✅ Have |
@@ -50,13 +52,13 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
 | 1x ELRS receiver (matching) | ✅ Have — mounts on FC UART |
 
 ### Drone FC Details — GEPRC GEP-F7 AIO
-- **MCU:** STM32F722RET6 (216MHz Cortex-M7, 512KB flash, 256KB RAM)
+- **MCU:** ESP32RET6 (216MHz Cortex-M7, 512KB flash, 256KB RAM)
 - **IMU:** TDK ICM-42688-P (6-axis, 32kHz gyro, ultra-low noise, SPI) ← the good one!
 - **Flash:** 8MB Winbond W25Q64 (blackbox, unused)
 - **OSD:** AT7456E (unused)
 - **4-in-1 ESC:** Built into AIO board (unused — we use hoverboard ESC)
 - **DFU mode:** Hold yellow BOOT button while plugging USB
-- **Firmware:** Custom balance firmware (PlatformIO + STM32 HAL)
+- **Firmware:** Custom balance firmware (PlatformIO + STM32 HAL) — LEGACY, see ESP32 BALANCE
 - **UART pads (confirmed from silkscreen):**
   - T1/R1 (bottom) → USART1 (PA9/PA10) → Jetson
   - T2/R2 (right top) → USART2 (PA2/PA3) → Hoverboard ESC
@@ -95,7 +97,7 @@ Two-wheeled, self-balancing robot for indoor AI/SLAM experiments.
 ## Self-Balancing Control — Custom Firmware on Drone FC
 
 ### Why a Drone FC?
-The F745 board is just a premium STM32 dev board with a high-quality IMU (MPU-6000) already soldered on, proper voltage regulation, and multiple UARTs broken out. We write a lean custom balance firmware (~50 lines of C).
+The F745 board was a premium STM32 dev board (legacy; now replaced by ESP32 BALANCE) with a high-quality IMU (MPU-6000) already soldered on, proper voltage regulation, and multiple UARTs broken out. We write a lean custom balance firmware (~50 lines of C).
 
 ### Architecture
 ```
@@ -142,7 +144,7 @@ GND          ──→   GND
 5V           ←──   5V
 ```
 
-### Custom Firmware (STM32 C)
+### Custom Firmware (Legacy STM32 C — archived)
 
 ```c
 // Core balance loop — runs in timer interrupt @ 1-8kHz
@@ -280,8 +282,8 @@ GND             ──→ Common ground
 ```
 
 ### Dev Tools
-- **Flashing:** STM32CubeProgrammer via USB (DFU mode) or SWD
-- **IDE:** PlatformIO + STM32 HAL, or STM32CubeIDE
+- **Flashing:** STM32CubeProgrammer via USB (DFU mode) or SWD (legacy)
+- **IDE:** PlatformIO + ESP-IDF (new) or STM32 HAL/STM32CubeIDE (legacy)
 - **Debug:** SWD via ST-Link (or use FC's USB as virtual COM for printf debug)
 
 ## Physical Design
@@ -375,7 +377,7 @@ GND             ──→ Common ground
 - [ ] Install hardware kill switch inline with 36V battery (NC — press to kill)
 - [ ] Set up ceiling tether point above test area (rated for >15kg)
 - [ ] Clear test area: 3m radius, no loose items, shoes on
-- [ ] Set up PlatformIO project for STM32F745 (STM32 HAL)
+- [ ] Set up PlatformIO project for ESP32 BALANCE (ESP-IDF)
 - [ ] Write MPU-6000 SPI driver (read gyro+accel, complementary filter)
 - [ ] Write PID balance loop with ALL safety checks:
   - ±25° tilt cutoff → disarm, require manual re-arm

docs/SAUL-TEE-SYSTEM-REFERENCE.md

@@ -0,0 +1,222 @@
+# SAUL-TEE System Reference — SaltyLab ESP32 Architecture
+*Authoritative source of truth for hardware, pins, protocols, and CAN assignments.*
+*Spec from hal@Orin, 2026-04-04.*
+
+---
+
+## Overview
+
+| Board | Role | MCU | USB chip |
+|-------|------|-----|----------|
+| **ESP32-S3 BALANCE** | PID balance loop, CAN→VESCs, LCD display | ESP32-S3 | CH343 USB-serial |
+| **ESP32-S3 IO** | RC input, motor drivers, sensors, LEDs, peripherals | ESP32-S3 | JTAG USB (native) |
+
+**Robot form factor:** 4-wheel wagon — 870 × 510 × 550 mm, ~23 kg
+**Power:** 36 V LiPo, DC-DC → 5 V and 12 V rails
+**Orin connection:** CANable2 USB → 500 kbps CAN (same bus as VESCs)
+
+---
+
+## ESP32-S3 BALANCE
+
+### Board
+Waveshare ESP32-S3 Touch LCD 1.28
+- GC9A01 round 240×240 LCD
+- CST816S capacitive touch
+- QMI8658 6-axis IMU (accel + gyro, SPI)
+- CH343 USB-to-serial chip
+
+### Pin Assignments
+
+| Function | GPIO | Notes |
+|----------|------|-------|
+| **QMI8658 IMU (SPI)** | | |
+| SCK | IO39 | |
+| MOSI | IO38 | |
+| MISO | IO40 | |
+| CS | IO41 | |
+| INT1 | IO42 | data-ready interrupt |
+| **GC9A01 LCD (shares SPI bus)** | | |
+| CS | IO12 | |
+| DC | IO11 | |
+| RST | IO10 | |
+| BL | IO9 | PWM backlight |
+| **CST816S Touch (I2C)** | | |
+| SDA | IO4 | |
+| SCL | IO5 | |
+| INT | IO6 | |
+| RST | IO7 | |
+| **CAN — SN65HVD230 transceiver** | | 500 kbps |
+| TX | IO43 | → SN65HVD230 TXD |
+| RX | IO44 | ← SN65HVD230 RXD |
+| **Inter-board UART (to IO board)** | | 460800 baud |
+| TX | IO17 | |
+| RX | IO18 | |
+
+### Responsibilities
+- Read QMI8658 @ 1 kHz (SPI, INT1-driven)
+- Complementary filter → pitch angle
+- PID balance loop (configurable Kp / Ki / Kd)
+- Send VESC speed commands via CAN (ID 68 = left, ID 56 = right)
+- Receive Orin velocity+mode commands via CAN (0x300–0x303)
+- Receive IO board status (arming, RC, faults) via UART protocol
+- Drive GC9A01 LCD: pitch, speed, battery %, error state
+- Enforce tilt cutoff at ±25°; IWDG 50 ms timeout
+- Publish telemetry on CAN 0x400–0x401 at 10 Hz
+
+---
+
+## ESP32-S3 IO
+
+### Board
+Bare ESP32-S3 devkit (JTAG USB)
+
+### Pin Assignments
+
+| Function | GPIO | Notes |
+|----------|------|-------|
+| **TBS Crossfire RC — UART0 (primary)** | | |
+| RX | IO44 | CRSF frames from Crossfire RX |
+| TX | IO43 | telemetry to Crossfire TX |
+| **ELRS failover — UART2** | | active if CRSF absent >100 ms |
+| RX | IO16 | |
+| TX | IO17 | |
+| **BTS7960 Motor Driver — Left** | | |
+| RPWM | IO1 | forward PWM |
+| LPWM | IO2 | reverse PWM |
+| R_EN | IO3 | right enable |
+| L_EN | IO4 | left enable |
+| **BTS7960 Motor Driver — Right** | | |
+| RPWM | IO5 | |
+| LPWM | IO6 | |
+| R_EN | IO7 | |
+| L_EN | IO8 | |
+| **I2C bus** | | |
+| SDA | IO11 | |
+| SCL | IO12 | |
+| NFC (PN532 or similar) | I2C | |
+| Barometer (BMP280/BMP388) | I2C | |
+| ToF (VL53L0X/VL53L1X) | I2C | |
+| **WS2812B LEDs** | | |
+| Data | IO13 | |
+| **Outputs** | | |
+| Horn / buzzer | IO14 | PWM tone |
+| Headlight | IO15 | PWM or digital |
+| Fan | IO16 | (if ELRS not fitted on UART2) |
+| **Inputs** | | |
+| Arming button | IO9 | active-low, hold 3 s to arm |
+| Kill switch sense | IO10 | hardware estop detect |
+| **Inter-board UART (to BALANCE board)** | | 460800 baud |
+| TX | IO18 | |
+| RX | IO21 | |
+
+### Responsibilities
+- Parse CRSF frames (TBS Crossfire, primary)
+- Parse ELRS frames (failover, activates if no CRSF for >100 ms)
+- Drive BTS7960 left/right PWM motor drivers
+- Read NFC, barometer, ToF via I2C
+- Drive WS2812B LEDs (armed/fault/idle patterns)
+- Control horn, headlight, fan, buzzer
+- Manage arming: hold button 3 s while upright → send ARM to BALANCE
+- Monitor kill switch input → immediate motor off + FAULT frame
+- Forward RC + sensor data to BALANCE via binary UART protocol
+- Report faults and RC-loss upstream
+
+---
+
+## Inter-Board Binary Protocol (UART @ 460800 baud)
+
+```
+[0xAA][LEN][TYPE][PAYLOAD × LEN bytes][CRC8]
+```
+- `0xAA` — start byte
+- `LEN` — payload length in bytes (uint8)
+- `TYPE` — message type (uint8)
+- `CRC8` — CRC-8/MAXIM over TYPE + PAYLOAD bytes
+
+### IO → BALANCE Messages
+
+| TYPE | Name | Payload | Description |
+|------|------|---------|-------------|
+| 0x01 | RC_CMD | int16 throttle, int16 steer, uint8 flags | flags: bit0=armed, bit1=kill |
+| 0x02 | SENSOR | uint16 tof_mm, int16 baro_delta_pa, uint8 nfc_present | |
+| 0x03 | FAULT | uint8 fault_flags | bit0=rc_loss, bit1=motor_fault, bit2=estop |
+
+### BALANCE → IO Messages
+
+| TYPE | Name | Payload | Description |
+|------|------|---------|-------------|
+| 0x10 | STATE | int16 pitch_x100, int16 pid_out, uint8 error_state | |
+| 0x11 | LED_CMD | uint8 pattern, uint8 r, uint8 g, uint8 b | |
+| 0x12 | BUZZER | uint8 tone_id, uint16 duration_ms | |
+
+---
+
+## CAN Bus — 500 kbps
+
+### Node Assignments
+
+| Node | CAN ID | Role |
+|------|--------|------|
+| VESC Left motor | **68** | Receives speed/duty via VESC CAN protocol |
+| VESC Right motor | **56** | Receives speed/duty via VESC CAN protocol |
+| ESP32-S3 BALANCE | — | Sends VESC commands; publishes telemetry |
+| Jetson Orin (CANable2) | — | Sends velocity commands; receives telemetry |
+
+### Frame Table
+
+| CAN ID | Direction | Description | Rate |
+|--------|-----------|-------------|------|
+| 0x300 | Orin → BALANCE | Velocity cmd: int16 speed_mmps, int16 steer_mrad | 20 Hz |
+| 0x301 | Orin → BALANCE | PID tuning: float Kp, float Ki, float Kd (3×4B IEEE-754) | on demand |
+| 0x302 | Orin → BALANCE | Mode: uint8 (0=off, 1=balance, 2=manual, 3=estop) | on demand |
+| 0x303 | Orin → BALANCE | Config: uint16 tilt_limit_x100, uint16 max_speed_mmps | on demand |
+| 0x400 | BALANCE → Orin | Telemetry A: int16 pitch_x100, int16 pid_out, int16 speed_mmps, uint8 state | 10 Hz |
+| 0x401 | BALANCE → Orin | Telemetry B: int16 vesc_l_rpm, int16 vesc_r_rpm, uint16 battery_mv, uint8 faults | 10 Hz |
+
+---
+
+## RC Channel Mapping (TBS Crossfire / ELRS CRSF)
+
+| CH | Function | Range (µs) | Notes |
+|----|----------|------------|-------|
+| 1 | Steer (Roll) | 988–2012 | ±100% → ±max steer |
+| 2 | Throttle (Pitch) | 988–2012 | forward / back speed |
+| 3 | Spare | 988–2012 | |
+| 4 | Spare | 988–2012 | |
+| 5 | ARM switch | <1500=disarm, >1500=arm | SB on TX |
+| 6 | **ESTOP** | <1500=normal, >1500=kill | SC on TX — checked first every loop |
+| 7 | Speed limit | 988–2012 | maps to 10–100% speed cap |
+| 8 | Spare | | |
+
+**RC loss:** No valid CRSF frame >100 ms → IO sends FAULT(rc_loss) → BALANCE cuts motors.
+
+---
+
+## Safety Invariants
+
+1. **Motors NEVER spin on power-on** — 3 s button hold required while upright
+2. **Tilt cutoff ±25°** — immediate motor zero, manual re-arm required
+3. **IWDG 50 ms** — firmware hang → motors cut
+4. **ESTOP RC channel** checked first in every loop iteration
+5. **Orin CAN timeout 500 ms** → revert to RC-only mode
+6. **Speed hard cap** — start at 10%, increase in 10% increments only after stable tethered testing
+7. **Never untethered** until stable for 5+ continuous minutes tethered
+
+---
+
+## USB Debug Commands (both boards, serial console)
+
+```
+help                     list commands
+status                   print pitch, PID state, CAN stats, UART stats
+pid <Kp> <Ki> <Kd>      set PID gains
+arm                      arm (if upright and safe)
+disarm                   disarm immediately
+estop                    emergency stop (requires re-arm)
+tilt_limit <deg>         set tilt cutoff angle (default 25)
+speed_limit <pct>        set speed cap percentage (default 10)
+can_stats                CAN bus counters (tx/rx/errors/busoff)
+uart_stats               inter-board UART frame counters
+reboot                   soft reboot
+```

docs/board-viz.html

@@ -2,7 +2,7 @@
 <html>
 <head>
 <meta charset="utf-8">
-<title>GEPRC GEP-F722-45A AIO — Board Layout</title>
+<title>GEPRC GEP-F722-45A AIO — Board Layout (Legacy / Archived)</title>
 <style>
 * { margin: 0; padding: 0; box-sizing: border-box; }
 body { background: #1a1a2e; color: #eee; font-family: 'Courier New', monospace; display: flex; flex-direction: column; align-items: center; padding: 20px; }
@@ -112,8 +112,8 @@ h1 { color: #e94560; margin-bottom: 5px; font-size: 1.4em; }
 </style>
 </head>
 <body>
-<h1>🤖 GEPRC GEP-F722-45A AIO — SaltyLab Pinout</h1>
-<p class="subtitle">STM32F722RET6 + ICM-42688-P | Betaflight target: GEPR-GEPRC_F722_AIO</p>
+<h1>🤖 GEPRC GEP-F722-45A AIO — SaltyLab Pinout (Legacy / Archived)</h1>
+<p class="subtitle">ESP32RET6 + ICM-42688-P | Betaflight target: GEPR-GEPRC_F722_AIO</p>
 
 <div class="container">
   <div class="board-wrap">
@@ -125,7 +125,7 @@ h1 { color: #e94560; margin-bottom: 5px; font-size: 1.4em; }
       <div class="mount br"></div>
 
       <!-- MCU -->
-      <div class="mcu"><div class="dot"></div>STM32<br>F722RET6<br>216MHz</div>
+      <div class="mcu"><div class="dot"></div>ESP32<br>(legacy:<br>F722RET6)</div>
 
       <!-- IMU -->
       <div class="imu">ICM<br>42688</div>

docs/wiring-diagram.md

@@ -1,6 +1,13 @@
-# SaltyLab Wiring Diagram
+# SaltyLab / SAUL-TEE Wiring Reference
 
-## System Overview
+> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32 retired.
+> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN.
+> **Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md)
+> Historical STM32/Mamba wiring below is **obsolete** — retained for reference only.
+
+---
+
+## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md)
 
 ```
 ┌─────────────────────────────────────────────────────────────────────┐
@@ -139,7 +146,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct)
 | 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` |
 
 
-## FC UART Summary (MAMBA F722S)
+## FC UART Summary (MAMBA F722S — OBSOLETE)
 
 | UART | Pins | Baud | Assignment | Notes |
 |------|------|------|------------|-------|

include/can_driver.h

@@ -1,101 +1,54 @@
 #ifndef CAN_DRIVER_H
 #define CAN_DRIVER_H
-
 #include <stdint.h>
 #include <stdbool.h>
-
-/* CAN bus driver for BLDC motor controllers (Issue #597)
- * 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
- */
-
-/* Node IDs */
 #define CAN_NUM_MOTORS          2u
 #define CAN_NODE_LEFT           0u
 #define CAN_NODE_RIGHT          1u
-
-/* CAN frame IDs */
-#define CAN_ID_VEL_CMD_BASE     0x100u   /* TX: 0x100 + node_id — velocity/torque command */
-#define CAN_ID_ENABLE_CMD_BASE  0x110u   /* TX: 0x110 + node_id — enable/disable */
-#define CAN_ID_FEEDBACK_BASE    0x200u   /* RX: 0x200 + node_id — position/velocity/current */
-
-/* Filter: accept standard IDs 0x200–0x21F */
+#define CAN_ID_VEL_CMD_BASE     0x100u
+#define CAN_ID_ENABLE_CMD_BASE  0x110u
+#define CAN_ID_FEEDBACK_BASE    0x200u
 #define CAN_FILTER_STDID        0x200u
 #define CAN_FILTER_MASK         0x7E0u
-
-/* Bit timing (500 kbps @ 54 MHz APB1) */
 #define CAN_PRESCALER           6u
-
-/* TX rate */
 #define CAN_TX_RATE_HZ          100u
-
-/* Node alive timeout */
 #define CAN_NODE_TIMEOUT_MS     100u
-
-/* TX command frame (8 bytes payload, DLC=4 for vel cmd) */
+#define CAN_WDOG_RESTART_MS     200u
+typedef struct { int16_t velocity_rpm; int16_t torque_x100; } can_cmd_t;
 typedef struct {
-    int16_t velocity_rpm;   /* target RPM (+/- = fwd/rev) */
-    int16_t torque_x100;    /* torque limit × 100 (0 = unlimited) */
-} can_cmd_t;
-
-/* RX feedback frame (DLC=8) */
-typedef struct {
-    int16_t  velocity_rpm;      /* actual RPM */
-    int16_t  current_ma;        /* phase current in mA */
-    int16_t  position_x100;     /* position × 100 (degrees or encoder counts) */
-    int8_t   temperature_c;     /* controller temperature °C */
-    uint8_t  fault;             /* fault flags (0 = healthy) */
-    uint32_t last_rx_ms;        /* HAL_GetTick() at last valid frame */
+    int16_t velocity_rpm; int16_t current_ma; int16_t position_x100;
+    int8_t temperature_c; uint8_t fault; uint32_t last_rx_ms;
 } can_feedback_t;
-
-/* Bus statistics */
 typedef struct {
-    uint32_t tx_count;   /* frames transmitted */
-    uint32_t rx_count;   /* frames received */
-    uint16_t err_count;  /* HAL-level errors */
-    uint8_t  bus_off;    /* 1 = bus-off state */
-    uint8_t  _pad;
+    uint32_t tx_count; uint32_t rx_count; uint16_t err_count;
+    uint8_t bus_off; uint8_t _pad;
 } can_stats_t;
-
-/* Initialise CAN2 peripheral, GPIO, and filter bank 14 */
+typedef enum {
+    CAN_ERR_NOMINAL = 0u, CAN_ERR_WARNING = 1u,
+    CAN_ERR_ERROR_PASSIVE = 2u, CAN_ERR_BUS_OFF = 3u,
+} can_error_state_t;
+typedef struct {
+    uint32_t restart_count; uint32_t busoff_count;
+    uint16_t errpassive_count; uint16_t errwarn_count;
+    can_error_state_t error_state; uint8_t tec; uint8_t rec; uint8_t busoff_pending;
+    uint32_t busoff_ms;
+} can_wdog_t;
 void can_driver_init(void);
-
-/* Send velocity+torque command to one node */
 void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd);
-
-/* Send enable/disable command to one node */
 void can_driver_send_enable(uint8_t node_id, bool enable);
-
-/* Copy latest feedback snapshot (returns false if node never heard from) */
 bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out);
-
-/* Returns true if node has been heard within CAN_NODE_TIMEOUT_MS */
 bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms);
-
-/* Copy bus statistics snapshot */
 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) */
+can_error_state_t can_driver_watchdog_tick(uint32_t now_ms);
+void can_driver_get_wdog(can_wdog_t *out);
+#ifdef TEST_HOST
+void can_driver_inject_esr(uint32_t esr_val);
+#endif
 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/jlink.h

@@ -101,6 +101,7 @@
 #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) */
+#define JLINK_TLM_CAN_WDOG      0x8Fu  /* jlink_tlm_can_wdog_t (16 bytes, Issue #694) */
 
 /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
 typedef struct __attribute__((packed)) {
@@ -250,6 +251,19 @@ typedef struct __attribute__((packed)) {
     int16_t  humidity_pct_x10;  /* %RH × 10 (BME280 only); -1 = BMP280/absent         */
 } jlink_tlm_baro_t;  /* 12 bytes */
 
+/* ---- Telemetry CAN_WDOG payload (16 bytes, packed) Issue #694 ---- */
+/* Sent at 1 Hz; reports CAN bus-error severity and restart history. */
+typedef struct __attribute__((packed)) {
+    uint32_t restart_count;     /* SW bus-off restarts since boot              */
+    uint32_t busoff_count;      /* lifetime bus-off entry events               */
+    uint16_t errpassive_count;  /* error-passive transitions                   */
+    uint16_t errwarn_count;     /* error-warning transitions                   */
+    uint8_t  error_state;       /* can_error_state_t: 0=OK,1=WARN,2=EP,3=BOFF */
+    uint8_t  tec;               /* transmit error counter (ESR[23:16])         */
+    uint8_t  rec;               /* receive  error counter (ESR[31:24])         */
+    uint8_t  _pad;              /* reserved                                    */
+} jlink_tlm_can_wdog_t;  /* 16 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)) {
@@ -418,4 +432,10 @@ void jlink_send_baro_tlm(const jlink_tlm_baro_t *tlm);
  */
 void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm);
 
+/*
+ * jlink_send_can_wdog_tlm(tlm) - transmit JLINK_TLM_CAN_WDOG (0x8F) frame
+ * (22 bytes total) at 1 Hz.  Issue #694.
+ */
+void jlink_send_can_wdog_tlm(const jlink_tlm_can_wdog_t *tlm);
+
 #endif /* JLINK_H */

include/orin_can.h

@@ -32,6 +32,7 @@
 #define ORIN_CAN_ID_MODE        0x302u
 #define ORIN_CAN_ID_ESTOP       0x303u
 #define ORIN_CAN_ID_LED_CMD     0x304u  /* LED pattern override (Issue #685) */
+#define ORIN_CAN_ID_PID_SET     0x305u  /* PID gain update: kp/ki/kd (Issue #693) */
 
 /* ---- FC → Orin telemetry IDs ---- */
 #define ORIN_CAN_ID_FC_STATUS   0x400u  /* balance state + pitch + vbat at 10 Hz */
@@ -39,6 +40,7 @@
 #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) */
+#define ORIN_CAN_ID_FC_PID_ACK  0x405u  /* PID gain ACK: echoes applied kp/ki/kd (Issue #693) */
 
 /* ---- Timing ---- */
 #define ORIN_HB_TIMEOUT_MS      500u   /* Orin offline after 500 ms without any frame */
@@ -56,6 +58,11 @@ typedef struct {
     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 */
+    /* PID_SET (Issue #693) -- set by orin_can_on_frame(), consumed by main */
+    volatile uint8_t  pid_updated;     /* set on PID_SET, cleared by main */
+    volatile uint16_t pid_kp_x100;     /* Kp * 100 (0..50000) */
+    volatile uint16_t pid_ki_x100;     /* Ki * 100 (0..5000) */
+    volatile uint16_t pid_kd_x100;     /* Kd * 100 (0..5000) */
 } OrinCanState;
 
 extern volatile OrinCanState orin_can_state;
@@ -164,4 +171,21 @@ void orin_can_broadcast_baro(uint32_t now_ms,
  */
 void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state);
 
+/* orin_can_send_pid_ack() -- send FC_PID_ACK (0x405). Issue #693. */
+void orin_can_send_pid_ack(float kp, float ki, float kd);
+
+/* PID_SET (0x305) -- 6-byte payload: kp*100, ki*100, kd*100 (uint16 BE each) */
+typedef struct __attribute__((packed)) {
+    uint16_t kp_x100;
+    uint16_t ki_x100;
+    uint16_t kd_x100;
+} orin_can_pid_set_t;
+
+/* FC_PID_ACK (0x405) -- FC -> Orin echo of applied gains */
+typedef struct __attribute__((packed)) {
+    uint16_t kp_x100;
+    uint16_t ki_x100;
+    uint16_t kd_x100;
+} orin_can_fc_pid_ack_t;
+
 #endif /* ORIN_CAN_H */

jetson/README.md

@@ -14,7 +14,7 @@ Self-balancing robot: Jetson Nano dev environment for ROS2 Humble + SLAM stack.
 | Nav | Nav2 |
 | Depth camera | Intel RealSense D435i |
 | LiDAR | RPLIDAR A1M8 |
-| MCU bridge | STM32F722 (USB CDC @ 921600) |
+| MCU bridge | ESP32 (USB CDC @ 921600) |
 
 ## Quick Start
 
@@ -42,7 +42,7 @@ bash scripts/build-and-run.sh shell
 ```
 jetson/
 ├── Dockerfile              # L4T base + ROS2 Humble + SLAM packages
-├── docker-compose.yml      # Multi-service stack (ROS2, RPLIDAR, D435i, STM32)
+├── docker-compose.yml      # Multi-service stack (ROS2, RPLIDAR, D435i, ESP32 BALANCE)
 ├── README.md               # This file
 ├── docs/
 │   ├── pinout.md           # GPIO/I2C/UART pinout reference

jetson/config/RECOVERY_BEHAVIORS.md

@@ -34,7 +34,7 @@ Recovery behaviors are triggered when Nav2 encounters navigation failures (path
 
 The emergency stop system (Issue #459, `saltybot_emergency` package) runs independently of Nav2 and takes absolute priority.
 
-Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the STM32 firmware.
+Recovery behaviors cannot interfere with E-stop because the emergency system operates at the motor driver level on the ESP32 BALANCE firmware.
 
 ## Behavior Tree Sequence
 

jetson/config/nav2_params.yaml

@@ -12,7 +12,7 @@
 #   /scan                    — RPLIDAR A1M8  (obstacle layer)
 #   /camera/depth/color/points — RealSense D435i (voxel layer)
 #
-# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
+# Output: /cmd_vel (Twist) — ESP32 bridge consumes this topic.
 
 bt_navigator:
   ros__parameters:

jetson/docker-compose.yml

@@ -31,7 +31,7 @@ services:
       - ./config:/config:ro
     devices:
       - /dev/rplidar:/dev/rplidar
-      - /dev/stm32-bridge:/dev/stm32-bridge
+      - /dev/esp32-bridge:/dev/esp32-bridge
       - /dev/bus/usb:/dev/bus/usb
       - /dev/i2c-7:/dev/i2c-7
       - /dev/video0:/dev/video0
@@ -97,13 +97,13 @@ services:
           rgb_camera.profile:=640x480x30
       "
 
-  # ── STM32 bridge node (bidirectional serial<->ROS2) ────────────────────────
-  stm32-bridge:
+  # ── ESP32 bridge node (bidirectional serial<->ROS2) ────────────────────────
+  esp32-bridge:
     image: saltybot/ros2-humble:jetson-orin
     build:
       context: .
       dockerfile: Dockerfile
-    container_name: saltybot-stm32-bridge
+    container_name: saltybot-esp32-bridge
     restart: unless-stopped
     runtime: nvidia
     network_mode: host
@@ -111,13 +111,13 @@ services:
       - ROS_DOMAIN_ID=42
       - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
     devices:
-      - /dev/stm32-bridge:/dev/stm32-bridge
+      - /dev/esp32-bridge:/dev/esp32-bridge
     command: >
       bash -c "
         source /opt/ros/humble/setup.bash &&
         ros2 launch saltybot_bridge bridge.launch.py
           mode:=bidirectional
-          serial_port:=/dev/stm32-bridge
+          serial_port:=/dev/esp32-bridge
       "
 
   # ── 4x IMX219 CSI cameras ──────────────────────────────────────────────────
@@ -192,7 +192,7 @@ services:
     network_mode: host
     depends_on:
       - saltybot-ros2
-      - stm32-bridge
+      - esp32-bridge
       - csi-cameras
     environment:
       - ROS_DOMAIN_ID=42
@@ -208,8 +208,8 @@ services:
       "
 
 
-  # -- Remote e-stop bridge (MQTT over 4G -> STM32 CDC) ----------------------
-  # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to STM32.
+  # -- Remote e-stop bridge (MQTT over 4G -> ESP32 CDC) ----------------------
+  # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32 BALANCE.
   # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT).
   remote-estop:
     image: saltybot/ros2-humble:jetson-orin
@@ -221,12 +221,12 @@ services:
     runtime: nvidia
     network_mode: host
     depends_on:
-      - stm32-bridge
+      - esp32-bridge
     environment:
       - ROS_DOMAIN_ID=42
       - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
     devices:
-      - /dev/stm32-bridge:/dev/stm32-bridge
+      - /dev/esp32-bridge:/dev/esp32-bridge
     volumes:
       - ./ros2_ws/src:/ros2_ws/src:rw
       - ./config:/config:ro
@@ -316,7 +316,7 @@ services:
     runtime: nvidia
     network_mode: host
     depends_on:
-      - stm32-bridge
+      - esp32-bridge
     environment:
       - NVIDIA_VISIBLE_DEVICES=all
       - NVIDIA_DRIVER_CAPABILITIES=all,audio

jetson/docs/pinout.md

@@ -1,5 +1,5 @@
 # Jetson Orin Nano Super — GPIO / I2C / UART / CSI Pinout Reference
-## Self-Balancing Robot: STM32F722 Bridge + RealSense D435i + RPLIDAR A1M8 + 4× IMX219
+## Self-Balancing Robot: ESP32 Bridge + RealSense D435i + RPLIDAR A1M8 + 4× IMX219
 
 Last updated: 2026-02-28
 JetPack version: 6.x (L4T R36.x / Ubuntu 22.04)
@@ -43,21 +43,21 @@ i2cdetect -l
 
 ---
 
-## 1. STM32F722 Bridge (USB CDC — Primary)
+## 1. ESP32 Bridge (USB CDC — Primary)
 
-The STM32 acts as a real-time motor + IMU controller. Communication is via **USB CDC serial**.
+The ESP32 BALANCE acts as a real-time motor + IMU controller. Communication is via **USB CDC serial**.
 
 ### USB CDC Connection
 | Connection | Detail |
 |-----------|--------|
-| Interface | USB Micro-B on STM32 dev board → USB-A on Jetson |
-| Device node | `/dev/ttyACM0` → symlink `/dev/stm32-bridge` (via udev) |
-| Baud rate | 921600 (configured in STM32 firmware) |
+| Interface | USB on ESP32 BALANCE board → USB-A on Jetson |
+| Device node | `/dev/ttyACM0` → symlink `/dev/esp32-bridge` (via udev) |
+| Baud rate | 921600 (configured in ESP32 BALANCE firmware) |
 | Protocol | JSON telemetry RX + ASCII command TX (see bridge docs) |
 | Power | Powered via robot 5V bus (data-only via USB) |
 
 ### Hardware UART (Fallback — 40-pin header)
-| Jetson Pin | Signal | STM32 Pin | Notes |
+| Jetson Pin | Signal | ESP32 Pin | Notes |
 |-----------|--------|-----------|-------|
 | Pin 8 (TXD0) | TX → | PA10 (UART1 RX) | Cross-connect TX→RX |
 | Pin 10 (RXD0) | RX ← | PA9 (UART1 TX) | Cross-connect RX→TX |
@@ -65,7 +65,7 @@ The STM32 acts as a real-time motor + IMU controller. Communication is via **USB
 
 **Jetson device node:** `/dev/ttyTHS0`
 **Baud rate:** 921600, 8N1
-**Voltage level:** 3.3V — both Jetson Orin and STM32F722 are 3.3V GPIO
+**Voltage level:** 3.3V — both Jetson Orin and ESP32 are 3.3V GPIO
 
 ```bash
 # Verify UART
@@ -75,13 +75,13 @@ sudo usermod -aG dialout $USER
 picocom -b 921600 /dev/ttyTHS0
 ```
 
-**ROS2 topics (STM32 bridge node):**
+**ROS2 topics (ESP32 bridge node):**
 | ROS2 Topic | Direction | Content |
 |-----------|-----------|---------
-| `/saltybot/imu` | STM32→Jetson | IMU data (accel, gyro) at 50Hz |
-| `/saltybot/balance_state` | STM32→Jetson | Motor cmd, pitch, state |
-| `/cmd_vel` | Jetson→STM32 | Velocity commands → `C<spd>,<str>\n` |
-| `/saltybot/estop` | Jetson→STM32 | Emergency stop |
+| `/saltybot/imu` | ESP32 BALANCE→Jetson | IMU data (accel, gyro) at 50Hz |
+| `/saltybot/balance_state` | ESP32 BALANCE→Jetson | Motor cmd, pitch, state |
+| `/cmd_vel` | Jetson→ESP32 BALANCE | Velocity commands → `C<spd>,<str>\n` |
+| `/saltybot/estop` | Jetson→ESP32 BALANCE | Emergency stop |
 
 ---
 
@@ -266,7 +266,7 @@ sudo mkdir -p /mnt/nvme
 |------|------|----------|
 | USB-A (top, blue) | USB 3.1 Gen 1 | RealSense D435i |
 | USB-A (bottom) | USB 2.0 | RPLIDAR (via USB-UART adapter) |
-| USB-C | USB 3.1 Gen 1 (+ DP) | STM32 CDC or host flash |
+| USB-C | USB 3.1 Gen 1 (+ DP) | ESP32 CDC or host flash |
 | Micro-USB | Debug/flash | JetPack flash only |
 
 ---
@@ -277,10 +277,10 @@ sudo mkdir -p /mnt/nvme
 |-------------|----------|---------|----------|
 | 3 | SDA1 | 3.3V | I2C data (i2c-7) |
 | 5 | SCL1 | 3.3V | I2C clock (i2c-7) |
-| 8 | TXD0 | 3.3V | UART TX → STM32 (fallback) |
-| 10 | RXD0 | 3.3V | UART RX ← STM32 (fallback) |
+| 8 | TXD0 | 3.3V | UART TX → ESP32 BALANCE (fallback) |
+| 10 | RXD0 | 3.3V | UART RX ← ESP32 BALANCE (fallback) |
 | USB-A ×2 | — | 5V | D435i, RPLIDAR |
-| USB-C | — | 5V | STM32 CDC |
+| USB-C | — | 5V | ESP32 CDC |
 | CSI-A (J5) | MIPI CSI-2 | — | Cameras front + left |
 | CSI-B (J8) | MIPI CSI-2 | — | Cameras rear + right |
 | M.2 Key M | PCIe Gen3 ×4 | — | NVMe SSD |
@@ -298,9 +298,9 @@ Apply stable device names:
 KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \
   SYMLINK+="rplidar", MODE="0666"
 
-# STM32 USB CDC (STMicroelectronics)
+# ESP32 USB CDC (STMicroelectronics)
 KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", \
-  SYMLINK+="stm32-bridge", MODE="0666"
+  SYMLINK+="esp32-bridge", MODE="0666"
 
 # Intel RealSense D435i
 SUBSYSTEM=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", \

jetson/docs/power-budget.md

@@ -56,7 +56,7 @@ sudo jtop
 |-----------|----------|------------|----------|-----------|-------|
 | RealSense D435i | 0.3 | 1.5 | 3.5 | USB 3.1 | Peak during boot/init |
 | RPLIDAR A1M8 | 0.4 | 2.6 | 3.0 | USB (UART adapter) | Motor spinning |
-| STM32F722 bridge | 0.0 | 0.0 | 0.0 | USB CDC | Self-powered from robot 5V |
+| ESP32 bridge | 0.0 | 0.0 | 0.0 | USB CDC | Self-powered from robot 5V |
 | 4× IMX219 cameras | 0.2 | 2.0 | 2.4 | MIPI CSI-2 | ~0.5W per camera active |
 | **Peripheral Subtotal** | **0.9** | **6.1** | **8.9** | | |
 
@@ -151,7 +151,7 @@ LiPo 4S (16.8V max)
        ├─► DC-DC Buck → 5V 6A ──► Jetson Orin barrel jack (30W)
        │   (e.g., XL4016E1)
        │
-       ├─► DC-DC Buck → 5V 3A ──► STM32 + logic 5V rail
+       ├─► DC-DC Buck → 5V 3A ──► ESP32 + logic 5V rail
        │
        └─► Hoverboard ESC ──► Hub motors (48V loop)
 ```

jetson/ros2_ws/src/saltybot_bridge/config/bridge_params.yaml

@@ -2,7 +2,7 @@
 # Used by both serial_bridge_node (RX-only) and saltybot_cmd_node (bidirectional)
 
 # ── Serial ─────────────────────────────────────────────────────────────────────
-# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied.
+# Use /dev/esp32-bridge if udev rule from jetson/docs/pinout.md is applied.
 serial_port:      /dev/ttyACM0
 baud_rate:        921600
 timeout:          0.05   # serial readline timeout (seconds)
@@ -11,7 +11,7 @@ reconnect_delay:  2.0    # seconds between reconnect attempts on serial disconne
 # ── saltybot_cmd_node (bidirectional) only ─────────────────────────────────────
 
 # Heartbeat: H\n sent every heartbeat_period seconds.
-# STM32 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.
+# ESP32 BALANCE reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.
 heartbeat_period: 0.2    # seconds (= 200ms)
 
 # Twist → ESC command scaling

jetson/ros2_ws/src/saltybot_bridge/config/cmd_vel_bridge_params.yaml

@@ -1,5 +1,5 @@
 # cmd_vel_bridge_params.yaml
-# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → STM32 autonomous drive.
+# Configuration for cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32 BALANCE autonomous drive.
 #
 # Run with:
 #   ros2 launch saltybot_bridge cmd_vel_bridge.launch.py
@@ -7,14 +7,14 @@
 #   ros2 launch saltybot_bridge cmd_vel_bridge.launch.py max_linear_vel:=0.3
 
 # ── Serial ─────────────────────────────────────────────────────────────────────
-# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied.
+# Use /dev/esp32-bridge if udev rule from jetson/docs/pinout.md is applied.
 serial_port:      /dev/ttyACM0
 baud_rate:        921600
 timeout:          0.05       # serial readline timeout (s)
 reconnect_delay:  2.0        # seconds between reconnect attempts
 
 # ── Heartbeat ──────────────────────────────────────────────────────────────────
-# STM32 jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms).
+# ESP32 BALANCE jetson_cmd module reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms).
 # Keep heartbeat well below that threshold.
 heartbeat_period: 0.2        # seconds (200ms)
 
@@ -50,5 +50,5 @@ ramp_rate:        500        # ESC units/second
 # ── Deadman switch ─────────────────────────────────────────────────────────────
 # If /cmd_vel is not received for this many seconds, target speed/steer are
 # zeroed immediately. The ramp then drives the robot to a stop.
-# 500ms matches the STM32 jetson heartbeat timeout for consistency.
+# 500ms matches the ESP32 BALANCE jetson heartbeat timeout for consistency.
 cmd_vel_timeout:  0.5        # seconds

jetson/ros2_ws/src/saltybot_bridge/config/estop_params.yaml

@@ -1,6 +1,6 @@
 remote_estop_node:
   ros__parameters:
-    serial_port:               /dev/stm32-bridge
+    serial_port:               /dev/esp32-bridge
     baud_rate:                 921600
     mqtt_host:                 "mqtt.example.com"
     mqtt_port:                 1883

jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml

@@ -1,8 +1,8 @@
 # stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
-# Binary-framed Jetson↔STM32 bridge at 921600 baud.
+# Binary-framed Jetson↔ESP32 bridge at 921600 baud.
 
 # ── Serial port ────────────────────────────────────────────────────────────────
-# Use /dev/stm32-bridge if the udev rule is applied:
+# Use /dev/esp32-bridge if the udev rule is applied:
 #   SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
 #   SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
 serial_port:      /dev/ttyACM0
@@ -12,7 +12,7 @@ reconnect_delay:  2.0        # seconds between USB reconnect attempts
 # ── Heartbeat ─────────────────────────────────────────────────────────────────
 # HEARTBEAT frame sent every heartbeat_period seconds.
 # STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
-heartbeat_period: 0.2        # 200ms → well within 500ms STM32 watchdog
+heartbeat_period: 0.2        # 200ms → well within 500ms ESP32 BALANCE watchdog
 
 # ── Watchdog (Jetson-side) ────────────────────────────────────────────────────
 # If no /cmd_vel message received for watchdog_timeout seconds,

jetson/ros2_ws/src/saltybot_bridge/launch/bridge.launch.py

@@ -6,7 +6,7 @@ Two deployment modes:
   1. Full bidirectional (recommended for Nav2):
        ros2 launch saltybot_bridge bridge.launch.py mode:=bidirectional
      Starts saltybot_cmd_node — owns serial port, handles both RX telemetry
-     and TX /cmd_vel → STM32 commands + heartbeat.
+     and TX /cmd_vel → ESP32 BALANCE commands + heartbeat.
 
   2. RX-only (telemetry monitor, no drive commands):
        ros2 launch saltybot_bridge bridge.launch.py mode:=rx_only
@@ -40,7 +40,7 @@ def _launch_nodes(context, *args, **kwargs):
         return [Node(
             package="saltybot_bridge",
             executable="serial_bridge_node",
-            name="stm32_serial_bridge",
+            name="esp32_serial_bridge",
             output="screen",
             parameters=[params],
         )]
@@ -65,7 +65,7 @@ def generate_launch_description():
         DeclareLaunchArgument("mode",         default_value="bidirectional",
                               description="bidirectional | rx_only"),
         DeclareLaunchArgument("serial_port",  default_value="/dev/ttyACM0",
-                              description="STM32 USB CDC device node"),
+                              description="ESP32 USB CDC device node"),
         DeclareLaunchArgument("baud_rate",    default_value="921600"),
         DeclareLaunchArgument("speed_scale",  default_value="1000.0",
                               description="m/s → ESC units (linear.x scale)"),

jetson/ros2_ws/src/saltybot_bridge/launch/cmd_vel_bridge.launch.py

@@ -1,10 +1,10 @@
 """
-cmd_vel_bridge.launch.py — Nav2 cmd_vel → STM32 autonomous drive bridge.
+cmd_vel_bridge.launch.py — Nav2 cmd_vel → ESP32 BALANCE autonomous drive bridge.
 
 Starts cmd_vel_bridge_node, which owns the serial port exclusively and provides:
   - /cmd_vel subscription with velocity limits + smooth ramp
   - Deadman switch (zero speed if /cmd_vel silent > cmd_vel_timeout)
-  - Mode gate (drives only when STM32 is in AUTONOMOUS mode, md=2)
+  - Mode gate (drives only when ESP32 BALANCE is in AUTONOMOUS mode, md=2)
   - Telemetry RX → /saltybot/imu, /saltybot/balance_state, /diagnostics
   - /saltybot/cmd publisher (observability)
 
@@ -72,12 +72,12 @@ def generate_launch_description():
             description="Full path to cmd_vel_bridge_params.yaml (overrides inline args)"),
         DeclareLaunchArgument(
             "serial_port",     default_value="/dev/ttyACM0",
-            description="STM32 USB CDC device node"),
+            description="ESP32 USB CDC device node"),
         DeclareLaunchArgument(
             "baud_rate",       default_value="921600"),
         DeclareLaunchArgument(
             "heartbeat_period",default_value="0.2",
-            description="Heartbeat interval (s); must be < STM32 HB timeout (0.5s)"),
+            description="Heartbeat interval (s); must be < ESP32 BALANCE HB timeout (0.5s)"),
         DeclareLaunchArgument(
             "max_linear_vel",  default_value="0.5",
             description="Hard speed cap before scaling (m/s)"),

jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py

@@ -1,4 +1,4 @@
-"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119).
+"""stm32_cmd.launch.py — Launch the binary-framed ESP32 BALANCE command node (Issue #119).
 
 Usage:
   # Default (binary protocol, bidirectional):

jetson/ros2_ws/src/saltybot_bridge/launch/uart_bridge.launch.py

@@ -2,7 +2,7 @@
 uart_bridge.launch.py — FC↔Orin UART bridge (Issue #362)
 
 Launches serial_bridge_node configured for Jetson Orin UART port.
-Bridges Flight Controller (STM32F722) telemetry from /dev/ttyTHS1 into ROS2.
+Bridges Flight Controller (ESP32) telemetry from /dev/ttyTHS1 into ROS2.
 
 Published topics (same as USB CDC bridge):
   /saltybot/imu              sensor_msgs/Imu         — pitch/roll/yaw as angular velocity
@@ -20,7 +20,7 @@ Usage:
 
 Prerequisites:
   - Flight Controller connected to /dev/ttyTHS1 @ 921600 baud
-  - STM32 firmware transmitting JSON telemetry frames (50 Hz)
+  - ESP32 BALANCE firmware transmitting JSON telemetry frames (50 Hz)
   - ROS2 environment sourced (source install/setup.bash)
 
 Note:

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/battery_node.py

@@ -14,7 +14,7 @@ Alert levels (SoC thresholds):
    5%  EMERGENCY — publish zero /cmd_vel, disarm, log + alert
 
 SoC source priority:
-  1. soc_pct field from STM32 BATTERY telemetry (fuel gauge or lookup on STM32)
+  1. soc_pct field from ESP32 BATTERY telemetry (fuel gauge or lookup on ESP32 BALANCE)
   2. Voltage-based lookup table (3S LiPo curve) if soc_pct == 0 and voltage known
 
 Parameters (config/battery_params.yaml):
@@ -320,7 +320,7 @@ class BatteryNode(Node):
         self._speed_limit_pub.publish(msg)
 
     def _execute_safe_stop(self) -> None:
-        """Send zero /cmd_vel and disarm the STM32."""
+        """Send zero /cmd_vel and disarm the ESP32 BALANCE."""
         self.get_logger().fatal("EMERGENCY: publishing zero /cmd_vel and disarming")
         # Publish zero velocity
         zero_twist = Twist()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/cmd_vel_bridge_node.py

@@ -1,5 +1,5 @@
 """
-cmd_vel_bridge_node — Nav2 /cmd_vel → STM32 drive command bridge.
+cmd_vel_bridge_node — Nav2 /cmd_vel → ESP32 BALANCE drive command bridge.
 
 Extends the basic saltybot_cmd_node with four additions required for safe
 autonomous operation on a self-balancing robot:
@@ -12,7 +12,7 @@ autonomous operation on a self-balancing robot:
   3. Deadman switch    — if /cmd_vel is silent for cmd_vel_timeout seconds,
                          zero targets immediately (Nav2 node crash / planner
                          stall → robot coasts to stop rather than running away).
-  4. Mode gate         — only issue non-zero drive commands when STM32 reports
+  4. Mode gate         — only issue non-zero drive commands when ESP32 BALANCE reports
                          md=2 (AUTONOMOUS). In any other mode (RC_MANUAL,
                          RC_ASSISTED) Jetson cannot override the RC pilot.
                          On mode re-entry current ramp state resets to 0 so
@@ -20,9 +20,9 @@ autonomous operation on a self-balancing robot:
 
 Serial protocol (C<speed>,<steer>\\n / H\\n — same as saltybot_cmd_node):
   C<spd>,<str>\\n  — drive command. speed/steer: -1000..+1000 integers.
-  H\\n             — heartbeat. STM32 reverts steer to 0 after 500ms silence.
+  H\\n             — heartbeat. ESP32 BALANCE reverts steer to 0 after 500ms silence.
 
-Telemetry (50 Hz from STM32):
+Telemetry (50 Hz from ESP32 BALANCE):
   Same RX/publish pipeline as saltybot_cmd_node.
   The "md" field (0=MANUAL,1=ASSISTED,2=AUTO) is parsed for the mode gate.
 
@@ -134,7 +134,7 @@ class CmdVelBridgeNode(Node):
         self._current_speed = 0   # ramped output actually sent
         self._current_steer = 0
         self._last_cmd_vel  = 0.0 # wall clock (seconds) of last /cmd_vel msg
-        self._stm32_mode    = 0   # parsed "md" field: 0=MANUAL,1=ASSISTED,2=AUTO
+        self._esp32_mode    = 0   # parsed "md" field: 0=MANUAL,1=ASSISTED,2=AUTO
         self._last_state    = -1
         self._frame_count   = 0
         self._error_count   = 0
@@ -150,7 +150,7 @@ class CmdVelBridgeNode(Node):
         self._open_serial()
 
         # ── Timers ────────────────────────────────────────────────────────────
-        # Telemetry read at 100 Hz (STM32 sends at 50 Hz)
+        # Telemetry read at 100 Hz (ESP32 BALANCE sends at 50 Hz)
         self._read_timer    = self.create_timer(0.01,             self._read_cb)
         # Control loop at 50 Hz: ramp + deadman + mode gate + send
         self._control_timer = self.create_timer(1.0 / _CONTROL_HZ, self._control_cb)
@@ -225,7 +225,7 @@ class CmdVelBridgeNode(Node):
 
         # Mode gate: in non-AUTONOMOUS mode, zero and reset ramp state so
         # re-entry always accelerates smoothly from 0.
-        if self._stm32_mode != MODE_AUTONOMOUS:
+        if self._esp32_mode != MODE_AUTONOMOUS:
             self._current_speed = 0
             self._current_steer = 0
             speed, steer = 0, 0
@@ -238,7 +238,7 @@ class CmdVelBridgeNode(Node):
             speed = self._current_speed
             steer = self._current_steer
 
-        # Send to STM32
+        # Send to ESP32 BALANCE
         frame = f"C{speed},{steer}\n".encode("ascii")
         if not self._write(frame):
             self.get_logger().warn(
@@ -256,7 +256,7 @@ class CmdVelBridgeNode(Node):
     # ── Heartbeat TX ──────────────────────────────────────────────────────────
 
     def _heartbeat_cb(self):
-        """H\\n keeps STM32 jetson_cmd heartbeat alive regardless of mode."""
+        """H\\n keeps ESP32 BALANCE jetson_cmd heartbeat alive regardless of mode."""
         self._write(b"H\n")
 
     # ── Telemetry RX ──────────────────────────────────────────────────────────
@@ -319,7 +319,7 @@ class CmdVelBridgeNode(Node):
         state     = int(data["s"])
         mode      = int(data.get("md", 0))  # 0=MANUAL if not present
 
-        self._stm32_mode = mode
+        self._esp32_mode = mode
         self._frame_count += 1
 
         self._publish_imu(pitch_deg, roll_deg, yaw_deg, now)
@@ -378,7 +378,7 @@ class CmdVelBridgeNode(Node):
         diag.header.stamp = stamp
         status = DiagnosticStatus()
         status.name        = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32"
         status.message     = f"{state_label} [{mode_label}]"
         status.level = (
             DiagnosticStatus.OK    if state == 1 else
@@ -406,11 +406,11 @@ class CmdVelBridgeNode(Node):
         status = DiagnosticStatus()
         status.level       = DiagnosticStatus.ERROR
         status.name        = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32"
         status.message     = f"IMU fault errno={errno}"
         diag.status.append(status)
         self._diag_pub.publish(diag)
-        self.get_logger().error(f"STM32 IMU fault: errno={errno}")
+        self.get_logger().error(f"ESP32 BALANCE IMU fault: errno={errno}")
 
     # ── Lifecycle ─────────────────────────────────────────────────────────────
 

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/remote_estop_node.py

@@ -1,8 +1,8 @@
 """
-remote_estop_node.py -- Remote e-stop bridge: MQTT -> STM32 USB CDC
+remote_estop_node.py -- Remote e-stop bridge: MQTT -> ESP32 USB CDC
 
-{"kill": true}  -> writes 'E\n' to STM32 (ESTOP_REMOTE, immediate motor cutoff)
-{"kill": false} -> writes 'Z\n' to STM32 (clear latch, robot can re-arm)
+{"kill": true}  -> writes 'E\n' to ESP32 BALANCE (ESTOP_REMOTE, immediate motor cutoff)
+{"kill": false} -> writes 'Z\n' to ESP32 BALANCE (clear latch, robot can re-arm)
 
 Cellular watchdog: if MQTT link drops for > cellular_timeout_s while in
 AUTO mode, automatically sends 'F\n' (ESTOP_CELLULAR_TIMEOUT).
@@ -26,7 +26,7 @@ class RemoteEstopNode(Node):
     def __init__(self):
         super().__init__('remote_estop_node')
 
-        self.declare_parameter('serial_port', '/dev/stm32-bridge')
+        self.declare_parameter('serial_port', '/dev/esp32-bridge')
         self.declare_parameter('baud_rate', 921600)
         self.declare_parameter('mqtt_host', 'mqtt.example.com')
         self.declare_parameter('mqtt_port', 1883)

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

@@ -322,7 +322,7 @@ class SaltybotCanNode(Node):
         diag.header.stamp = stamp
         st = DiagnosticStatus()
         st.name        = "saltybot/balance_controller"
-        st.hardware_id = "stm32f722"
+        st.hardware_id = "esp32"
         st.message     = state_label
         st.level       = (DiagnosticStatus.OK    if state == 1 else
                           DiagnosticStatus.WARN   if state == 0 else

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

@@ -1,20 +1,20 @@
 """
-saltybot_cmd_node — full bidirectional STM32↔Jetson bridge
+saltybot_cmd_node — full bidirectional ESP32 BALANCE↔Jetson bridge
 Combines telemetry RX (from serial_bridge_node) with drive command TX.
 
 Owns /dev/ttyACM0 exclusively — do NOT run alongside serial_bridge_node.
 
-RX path (50Hz from STM32):
+RX path (50Hz from ESP32 BALANCE):
   JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics
 
 TX path:
-  /cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n  → STM32
-  Heartbeat timer (200ms)        → H\\n                 → STM32
+  /cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n  → ESP32 BALANCE
+  Heartbeat timer (200ms)        → H\\n                 → ESP32 BALANCE
 
 Protocol:
-  H\\n              — heartbeat. STM32 reverts steer to 0 if gap > 500ms.
+  H\\n              — heartbeat. ESP32 BALANCE reverts steer to 0 if gap > 500ms.
   C<spd>,<str>\\n   — drive command. speed/steer: -1000..+1000 integers.
-                      C command also refreshes STM32 heartbeat timer.
+                      C command also refreshes ESP32 BALANCE heartbeat timer.
 
 Twist mapping (configurable via ROS2 params):
   speed = clamp(linear.x  * speed_scale, -1000, 1000)
@@ -100,7 +100,7 @@ class SaltybotCmdNode(Node):
         self._open_serial()
 
         # ── Timers ────────────────────────────────────────────────────────────
-        # Telemetry read at 100Hz (STM32 sends at 50Hz)
+        # Telemetry read at 100Hz (ESP32 BALANCE sends at 50Hz)
         self._read_timer = self.create_timer(0.01, self._read_cb)
         # Heartbeat TX at configured period (default 200ms)
         self._hb_timer   = self.create_timer(self._hb_period, self._heartbeat_cb)
@@ -266,7 +266,7 @@ class SaltybotCmdNode(Node):
         diag.header.stamp = stamp
         status = DiagnosticStatus()
         status.name = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32"
         status.message = state_label
         if state == 1:
             status.level = DiagnosticStatus.OK
@@ -294,11 +294,11 @@ class SaltybotCmdNode(Node):
         status = DiagnosticStatus()
         status.level = DiagnosticStatus.ERROR
         status.name = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32"
         status.message = f"IMU fault errno={errno}"
         diag.status.append(status)
         self._diag_pub.publish(diag)
-        self.get_logger().error(f"STM32 IMU fault: errno={errno}")
+        self.get_logger().error(f"ESP32 BALANCE IMU fault: errno={errno}")
 
     # ── TX — command send ─────────────────────────────────────────────────────
 
@@ -316,7 +316,7 @@ class SaltybotCmdNode(Node):
             )
 
     def _heartbeat_cb(self):
-        """Send H\\n heartbeat. STM32 reverts steer to 0 if gap > 500ms."""
+        """Send H\\n heartbeat. ESP32 BALANCE reverts steer to 0 if gap > 500ms."""
         self._write(b"H\n")
 
     # ── Lifecycle ─────────────────────────────────────────────────────────────

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/serial_bridge_node.py

@@ -1,6 +1,6 @@
 """
 saltybot_bridge — serial_bridge_node
-STM32F722 USB CDC → ROS2 topic publisher
+ESP32 USB CDC → ROS2 topic publisher
 
 Telemetry frame (50 Hz, newline-delimited JSON):
   {"p":<pitch×10>,"r":<roll×10>,"e":<err×10>,"ig":<integral×10>,
@@ -29,7 +29,7 @@ from sensor_msgs.msg import Imu
 from std_msgs.msg import String
 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
 
-# Balance state labels matching STM32 balance_state_t enum
+# Balance state labels matching ESP32 BALANCE balance_state_t enum
 _STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
 
 # Sensor frame_id published in Imu header
@@ -38,7 +38,7 @@ IMU_FRAME_ID = "imu_link"
 
 class SerialBridgeNode(Node):
     def __init__(self):
-        super().__init__("stm32_serial_bridge")
+        super().__init__("esp32_serial_bridge")
 
         # ── Parameters ────────────────────────────────────────────────────────
         self.declare_parameter("serial_port", "/dev/ttyACM0")
@@ -83,7 +83,7 @@ class SerialBridgeNode(Node):
 
         # ── Open serial and start read timer ──────────────────────────────────
         self._open_serial()
-        # Poll at 100 Hz — STM32 sends at 50 Hz, so we never miss a frame
+        # Poll at 100 Hz — ESP32 BALANCE sends at 50 Hz, so we never miss a frame
         self._timer = self.create_timer(0.01, self._read_cb)
 
         self.get_logger().info(
@@ -117,7 +117,7 @@ class SerialBridgeNode(Node):
 
     def write_serial(self, data: bytes) -> bool:
         """
-        Send raw bytes to STM32 over the open serial port.
+        Send raw bytes to ESP32 BALANCE over the open serial port.
         Returns False if port is not open (caller should handle gracefully).
         Note: for bidirectional use prefer saltybot_cmd_node which owns TX natively.
         """
@@ -206,7 +206,7 @@ class SerialBridgeNode(Node):
         """
         Publish sensor_msgs/Imu.
 
-        The STM32 IMU gives Euler angles (pitch/roll from accelerometer+gyro
+        The ESP32 BALANCE IMU gives Euler angles (pitch/roll from accelerometer+gyro
         fusion, yaw from gyro integration). We publish them as angular_velocity
         for immediate use by slam_toolbox / robot_localization.
 
@@ -264,7 +264,7 @@ class SerialBridgeNode(Node):
         diag.header.stamp = stamp
         status = DiagnosticStatus()
         status.name = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32"
         status.message = state_label
 
         if state == 1:   # ARMED
@@ -293,11 +293,11 @@ class SerialBridgeNode(Node):
         status = DiagnosticStatus()
         status.level = DiagnosticStatus.ERROR
         status.name = "saltybot/balance_controller"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32"
         status.message = f"IMU fault errno={errno}"
         diag.status.append(status)
         self._diag_pub.publish(diag)
-        self.get_logger().error(f"STM32 reported IMU fault: errno={errno}")
+        self.get_logger().error(f"ESP32 BALANCE reported IMU fault: errno={errno}")
 
     def destroy_node(self):
         self._close_serial()

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py

@@ -5,7 +5,7 @@ framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
 
 TX commands (Jetson → STM32):
   SPEED_STEER  — 50 Hz from /cmd_vel subscription
-  HEARTBEAT    — 200 ms timer (STM32 watchdog fires at 500 ms)
+  HEARTBEAT    — 200 ms timer (ESP32 BALANCE watchdog fires at 500 ms)
   ARM          — via /saltybot/arm service
   SET_MODE     — via /saltybot/set_mode service
   PID_UPDATE   — via /saltybot/pid_update topic
@@ -75,7 +75,7 @@ def _clamp(v: float, lo: float, hi: float) -> float:
 # ── Node ──────────────────────────────────────────────────────────────────────
 
 class Stm32CmdNode(Node):
-    """Binary-framed Jetson↔STM32 bridge node."""
+    """Binary-framed Jetson↔ESP32 bridge node."""
 
     def __init__(self) -> None:
         super().__init__("stm32_cmd_node")
@@ -283,7 +283,7 @@ class Stm32CmdNode(Node):
         msg.angular_velocity.x = math.radians(frame.pitch_deg)
         msg.angular_velocity.y = math.radians(frame.roll_deg)
         msg.angular_velocity.z = math.radians(frame.yaw_deg)
-        cov = math.radians(0.3) ** 2    # ±0.3° noise estimate from STM32 BMI088
+        cov = math.radians(0.3) ** 2    # ±0.3° noise estimate from ESP32 BMI088
         msg.angular_velocity_covariance[0] = cov
         msg.angular_velocity_covariance[4] = cov
         msg.angular_velocity_covariance[8] = cov
@@ -340,7 +340,7 @@ class Stm32CmdNode(Node):
 
     def _publish_error(self, frame: ErrorFrame, stamp) -> None:
         self.get_logger().error(
-            f"STM32 error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}"
+            f"ESP32 BALANCE error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}"
         )
         payload = {
             "error_code": frame.error_code,
@@ -432,7 +432,7 @@ class Stm32CmdNode(Node):
 
         status = DiagnosticStatus()
         status.name        = "saltybot/stm32_cmd_node"
-        status.hardware_id = "stm32f722"
+        status.hardware_id = "esp32"
 
         port_ok = self._ser is not None and self._ser.is_open
         if port_ok:

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_protocol.py

@@ -1,7 +1,11 @@
-"""stm32_protocol.py — Binary frame codec for Jetson↔STM32 communication.
+"""stm32_protocol.py — Binary frame codec for Jetson↔ESP32 BALANCE communication.
+
+# TODO(esp32-migration): This protocol was designed for STM32F722 USB CDC.
+# When ESP32 BALANCE protocol is defined, update frame layout and baud rate.
 
 Issue #119: defines the binary serial protocol between the Jetson Nano and the
-STM32F722 flight controller over USB CDC @ 921600 baud.
+ESP32 BALANCE over USB CDC @ 921600 baud.
+# TODO(esp32-migration): update when ESP32 BALANCE protocol is defined.
 
 Frame layout (all multi-byte fields are big-endian):
   ┌──────┬──────┬──────┬──────────────────┬───────────┬──────┐
@@ -12,14 +16,14 @@ Frame layout (all multi-byte fields are big-endian):
 CRC16 covers: TYPE + LEN + PAYLOAD (not STX, ETX, or CRC bytes themselves).
 CRC algorithm: CCITT-16, polynomial=0x1021, init=0xFFFF, no final XOR.
 
-Command types (Jetson → STM32):
+Command types (Jetson → ESP32 BALANCE):
   0x01  HEARTBEAT   — no payload              (len=0)
   0x02  SPEED_STEER — int16 speed + int16 steer (len=4)  range: -1000..+1000
   0x03  ARM         — uint8 (0=disarm, 1=arm)  (len=1)
   0x04  SET_MODE    — uint8 mode               (len=1)
   0x05  PID_UPDATE  — float32 kp + ki + kd     (len=12)
 
-Telemetry types (STM32 → Jetson):
+Telemetry types (ESP32 BALANCE → Jetson):
   0x10  IMU         — int16×6: pitch,roll,yaw (×100 deg), ax,ay,az (×100 m/s²) (len=12)
   0x11  BATTERY     — uint16 voltage_mv + int16 current_ma + uint8 soc_pct (len=5)
   0x12  MOTOR_RPM   — int16 left_rpm + int16 right_rpm     (len=4)
@@ -31,7 +35,7 @@ Usage:
   frame = encode_speed_steer(300, -150)
   ser.write(frame)
 
-  # Decoding (STM32 → Jetson), one byte at a time
+  # Decoding (ESP32 BALANCE → Jetson), one byte at a time
   parser = FrameParser()
   for byte in incoming_bytes:
       result = parser.feed(byte)
@@ -183,7 +187,7 @@ class ParseError(Exception):
 
 
 class FrameParser:
-    """Byte-by-byte streaming parser for STM32 telemetry frames.
+    """Byte-by-byte streaming parser for ESP32 BALANCE telemetry frames.
 
     Feed individual bytes via feed(); returns a decoded TelemetryFrame (or raw
     bytes tuple) when a complete valid frame is received.

jetson/ros2_ws/src/saltybot_bridge/setup.py

@@ -29,7 +29,7 @@ setup(
     zip_safe=True,
     maintainer="sl-jetson",
     maintainer_email="sl-jetson@saltylab.local",
-    description="STM32 USB CDC → ROS2 serial bridge for saltybot",
+    description="ESP32 USB CDC → ROS2 serial bridge for saltybot",
     license="MIT",
     tests_require=["pytest"],
     entry_points={
@@ -41,7 +41,7 @@ setup(
             # Nav2 cmd_vel bridge: velocity limits + ramp + deadman + mode gate
             "cmd_vel_bridge_node = saltybot_bridge.cmd_vel_bridge_node:main",
             "remote_estop_node  = saltybot_bridge.remote_estop_node:main",
-            # Binary-framed STM32 command node (Issue #119)
+            # Binary-framed ESP32 BALANCE command node (Issue #119)
             "stm32_cmd_node    = saltybot_bridge.stm32_cmd_node:main",
             # Battery management node (Issue #125)
             "battery_node      = saltybot_bridge.battery_node:main",

jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py

@@ -1,5 +1,5 @@
 """
-Unit tests for Jetson→STM32 command serialization logic.
+Unit tests for Jetson→ESP32 BALANCE command serialization logic.
 Tests Twist→speed/steer conversion and frame formatting.
 Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
 """

jetson/ros2_ws/src/saltybot_bridge/test/test_cmd_vel_bridge.py

@@ -139,10 +139,10 @@ class TestModeGate:
     MODE_ASSISTED  = 1
     MODE_AUTONOMOUS = 2
 
-    def _apply_mode_gate(self, stm32_mode, current_speed, current_steer,
+    def _apply_mode_gate(self, esp32_mode, current_speed, current_steer,
                           target_speed, target_steer, step=10):
         """Mirror of _control_cb mode gate logic."""
-        if stm32_mode != self.MODE_AUTONOMOUS:
+        if esp32_mode != self.MODE_AUTONOMOUS:
             # Reset ramp state, send zero
             return 0, 0, 0, 0  # (current_speed, current_steer, sent_speed, sent_steer)
         new_s = _ramp_toward(current_speed, target_speed, step)

jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py

@@ -1,5 +1,5 @@
 """
-Unit tests for STM32 telemetry parsing logic.
+Unit tests for ESP32 BALANCE telemetry parsing logic.
 Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
 """
 

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

@@ -19,7 +19,7 @@
 #   inflation_radius: 0.3m (robot_radius 0.15m + 0.15m padding)
 #   DepthCostmapLayer in-layer inflation: 0.10m (pre-inflation before inflation_layer)
 #
-# Output: /cmd_vel (Twist) — STM32 bridge consumes this topic.
+# Output: /cmd_vel (Twist) — ESP32 bridge consumes this topic.
 
 bt_navigator:
   ros__parameters:

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

@@ -49,6 +49,9 @@ rosbridge_websocket:
        "/cmd_vel",
        "/saltybot/imu",
        "/saltybot/balance_state",
+       "/saltybot/barometer",
+       "/vesc/left/state",
+       "/vesc/right/state",
        "/tf",
        "/tf_static"]
 

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

@@ -2,12 +2,12 @@
 # Master configuration for full stack bringup
 
 # ────────────────────────────────────────────────────────────────────────────
-# HARDWARE — STM32 Bridge & Motor Control
+# HARDWARE — ESP32 BALANCE Bridge & Motor Control
 # ────────────────────────────────────────────────────────────────────────────
 
 saltybot_bridge_node:
   ros__parameters:
-    serial_port: "/dev/stm32-bridge"
+    serial_port: "/dev/esp32-bridge"
     baud_rate: 921600
     timeout: 0.05
     reconnect_delay: 2.0

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

@@ -0,0 +1,70 @@
+"""
+can_monitor.launch.py — Lightweight rosbridge server for CAN sensor dashboard (Issue #697)
+
+Starts rosbridge_websocket on port 9090 with a whitelist limited to the five
+topics consumed by can_monitor_panel.html:
+
+  /saltybot/imu           sensor_msgs/Imu           — IMU attitude
+  /saltybot/balance_state std_msgs/String (JSON)    — balance PID state
+  /saltybot/barometer     std_msgs/String (JSON)    — pressure / temp / altitude
+  /vesc/left/state        std_msgs/String (JSON)    — left VESC telemetry
+  /vesc/right/state       std_msgs/String (JSON)    — right VESC telemetry
+
+Usage:
+  ros2 launch saltybot_bringup can_monitor.launch.py
+
+  # Override port if needed:
+  ros2 launch saltybot_bringup can_monitor.launch.py port:=9091
+
+Verify:
+  # From a browser on the same LAN:
+  #   var ros = new ROSLIB.Ros({ url: 'ws://<jetson-ip>:9090' });
+  #   ros.on('connection', () => console.log('connected'));
+"""
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+# Topics exposed to the CAN monitor WebUI panel.
+_TOPICS = [
+    '/saltybot/imu',
+    '/saltybot/balance_state',
+    '/saltybot/barometer',
+    '/vesc/left/state',
+    '/vesc/right/state',
+]
+
+_TOPICS_GLOB = '[' + ', '.join(f'"{t}"' for t in _TOPICS) + ']'
+
+
+def generate_launch_description():
+    port_arg = DeclareLaunchArgument(
+        'port',
+        default_value='9090',
+        description='WebSocket port for rosbridge (default 9090)',
+    )
+
+    rosbridge = Node(
+        package='rosbridge_server',
+        executable='rosbridge_websocket',
+        name='rosbridge_websocket',
+        parameters=[{
+            'port':               LaunchConfiguration('port'),
+            'host':               '0.0.0.0',
+            'authenticate':       False,
+            'max_message_size':   1000000,    # 1 MB — no large map payloads needed
+            'topics_glob':        _TOPICS_GLOB,
+            'services_glob':      '[]',
+            'params_glob':        '[]',
+            'unregister_timeout': 10.0,
+            'fragment_timeout':   600,
+            'delay_between_messages': 0,
+        }],
+        output='screen',
+    )
+
+    return LaunchDescription([port_arg, rosbridge])

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

@@ -39,7 +39,7 @@ Modes
     ─ UWB driver (2-anchor DW3000, publishes /uwb/target)
     ─ YOLOv8n person detection (TensorRT)
     ─ Person follower with UWB+camera fusion
-    ─ cmd_vel bridge → STM32 (deadman + ramp + AUTONOMOUS gate)
+    ─ cmd_vel bridge → ESP32 BALANCE (deadman + ramp + AUTONOMOUS gate)
     ─ rosbridge WebSocket (port 9090)
 
   outdoor
@@ -57,8 +57,8 @@ Modes
 Launch sequence (wall-clock delays — conservative for cold start)
 ─────────────────────────────────────────────────────────────────
    t= 0s  robot_description   (URDF + TF tree)
-   t= 0s  STM32 bridge        (serial port owner — must be first)
-   t= 2s  cmd_vel bridge      (consumes /cmd_vel, needs STM32 bridge up)
+   t= 0s  ESP32 bridge        (serial port owner — must be first)
+   t= 2s  cmd_vel bridge      (consumes /cmd_vel, needs ESP32 bridge up)
    t= 2s  sensors             (RPLIDAR + RealSense)
    t= 4s  UWB driver          (independent serial device)
    t= 4s  CSI cameras         (optional, independent)
@@ -71,10 +71,10 @@ Launch sequence (wall-clock delays — conservative for cold start)
 
 Safety wiring
 ─────────────
-  • STM32 bridge must be up before cmd_vel bridge sends any command.
+  • ESP32 bridge must be up before cmd_vel bridge sends any command.
   • cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
-  • STM32 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
-    until STM32 firmware is in AUTONOMOUS mode regardless of /cmd_vel.
+  • ESP32 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
+    until ESP32 BALANCE firmware is in AUTONOMOUS mode regardless of /cmd_vel.
   • follow_enabled:=false disables person follower without stopping the node.
   • To e-stop at runtime: ros2 topic pub /saltybot/estop std_msgs/Bool '{data: true}'
 
@@ -91,7 +91,7 @@ Topics published by this stack
   /person/target                   PoseStamped    (camera position, base_link)
   /person/detections               Detection2DArray
   /cmd_vel                         Twist          (from follower or Nav2)
-  /saltybot/cmd                    String         (to STM32)
+  /saltybot/cmd                    String         (to ESP32 BALANCE)
   /saltybot/imu                    Imu
   /saltybot/balance_state          String
 """
@@ -209,7 +209,7 @@ def generate_launch_description():
     enable_bridge_arg = DeclareLaunchArgument(
         "enable_bridge",
         default_value="true",
-        description="Launch STM32 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
+        description="Launch ESP32 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
     )
 
     enable_rosbridge_arg = DeclareLaunchArgument(
@@ -267,10 +267,10 @@ enable_mission_logging_arg = DeclareLaunchArgument(
         description="UWB anchor-1 serial port (starboard/right side)",
     )
 
-    stm32_port_arg = DeclareLaunchArgument(
-        "stm32_port",
-        default_value="/dev/stm32-bridge",
-        description="STM32 USB CDC serial port",
+    esp32_port_arg = DeclareLaunchArgument(
+        "esp32_port",
+        default_value="/dev/esp32-bridge",
+        description="ESP32 USB CDC serial port",
     )
 
     # ── Shared substitution handles ───────────────────────────────────────────
@@ -282,7 +282,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
     max_linear_vel  = LaunchConfiguration("max_linear_vel")
     uwb_port_a      = LaunchConfiguration("uwb_port_a")
     uwb_port_b      = LaunchConfiguration("uwb_port_b")
-    stm32_port      = LaunchConfiguration("stm32_port")
+    esp32_port      = LaunchConfiguration("esp32_port")
 
     # ── t=0s  Robot description (URDF + TF tree) ──────────────────────────────
     robot_description = IncludeLaunchDescription(
@@ -290,15 +290,15 @@ enable_mission_logging_arg = DeclareLaunchArgument(
         launch_arguments={"use_sim_time": use_sim_time}.items(),
     )
 
-    # ── t=0s  STM32 bidirectional serial bridge ────────────────────────────────
-    stm32_bridge = GroupAction(
+    # ── t=0s  ESP32 bidirectional serial bridge ────────────────────────────────
+    esp32_bridge = GroupAction(
         condition=IfCondition(LaunchConfiguration("enable_bridge")),
         actions=[
             IncludeLaunchDescription(
                 _launch("saltybot_bridge", "launch", "bridge.launch.py"),
                 launch_arguments={
                     "mode":        "bidirectional",
-                    "serial_port": stm32_port,
+                    "serial_port": esp32_port,
                 }.items(),
             ),
         ],
@@ -320,7 +320,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
         ],
     )
 
-    # ── t=2s  cmd_vel safety bridge (depends on STM32 bridge) ────────────────
+    # ── t=2s  cmd_vel safety bridge (depends on ESP32 bridge) ────────────────
     cmd_vel_bridge = TimerAction(
         period=2.0,
         actions=[
@@ -577,14 +577,14 @@ enable_mission_logging_arg,
         max_linear_vel_arg,
         uwb_port_a_arg,
         uwb_port_b_arg,
-        stm32_port_arg,
+        esp32_port_arg,
 
         # Startup banner
         banner,
 
         # t=0s
         robot_description,
-        stm32_bridge,
+        esp32_bridge,
 
         # t=0.5s
         mission_logging,

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

@@ -15,11 +15,11 @@ Usage
   ros2 launch saltybot_bringup saltybot_bringup.launch.py
   ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=minimal
   ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=debug
-  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full stm32_port:=/dev/ttyUSB0
+  ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full esp32_port:=/dev/ttyUSB0
 
 Startup sequence
 ────────────────
-  GROUP A — Drivers      t= 0 s  STM32 bridge, RealSense+RPLIDAR, motor daemon, IMU
+  GROUP A — Drivers      t= 0 s  ESP32 bridge, RealSense+RPLIDAR, motor daemon, IMU
   health gate ───────────────────────────────────────────────── t= 8 s (full/debug)
   GROUP B — Perception   t= 8 s  UWB, person detection, object detection, depth costmap, gimbal
   health gate ───────────────────────────────────────────────── t=16 s (full/debug)
@@ -35,7 +35,7 @@ Shutdown
 
 Hardware conditionals
 ─────────────────────
-  Missing devices (stm32_port, uwb_port_a/b, gimbal_port) skip that driver.
+  Missing devices (esp32_port, uwb_port_a/b, gimbal_port) skip that driver.
   All conditionals are evaluated at launch time via PathJoinSubstitution + IfCondition.
 """
 
@@ -120,10 +120,10 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
         description="Use /clock from rosbag/simulator",
     )
 
-    stm32_port_arg = DeclareLaunchArgument(
-        "stm32_port",
-        default_value="/dev/stm32-bridge",
-        description="STM32 USART bridge serial device",
+    esp32_port_arg = DeclareLaunchArgument(
+        "esp32_port",
+        default_value="/dev/esp32-bridge",
+        description="ESP32 UART bridge serial device",
     )
 
     uwb_port_a_arg = DeclareLaunchArgument(
@@ -160,7 +160,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
 
     profile         = LaunchConfiguration("profile")
     use_sim_time    = LaunchConfiguration("use_sim_time")
-    stm32_port      = LaunchConfiguration("stm32_port")
+    esp32_port      = LaunchConfiguration("esp32_port")
     uwb_port_a      = LaunchConfiguration("uwb_port_a")
     uwb_port_b      = LaunchConfiguration("uwb_port_b")
     gimbal_port     = LaunchConfiguration("gimbal_port")
@@ -198,7 +198,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
 
     # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
     # GROUP A — DRIVERS   (t = 0 s, all profiles)
-    # Dependency order: STM32 bridge first, then sensors, then motor daemon.
+    # Dependency order: ESP32 bridge first, then sensors, then motor daemon.
     # Health gate: subsequent groups delayed until t_perception (8 s full/debug).
     # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
 
@@ -212,12 +212,12 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
         launch_arguments={"use_sim_time": use_sim_time}.items(),
     )
 
-    # STM32 bidirectional bridge (JLINK USART1)
-    stm32_bridge = IncludeLaunchDescription(
+    # ESP32 BALANCE bridge
+    esp32_bridge = IncludeLaunchDescription(
         _launch("saltybot_bridge", "launch", "bridge.launch.py"),
         launch_arguments={
             "mode":        "bidirectional",
-            "serial_port": stm32_port,
+            "serial_port": esp32_port,
         }.items(),
     )
 
@@ -232,7 +232,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
         ],
     )
 
-    # Motor daemon: /cmd_vel → STM32 DRIVE frames (depends on bridge at t=0)
+    # Motor daemon: /cmd_vel → ESP32 BALANCE DRIVE frames (depends on bridge at t=0)
     motor_daemon = TimerAction(
         period=2.5,
         actions=[
@@ -541,7 +541,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
         # ── Arguments ──────────────────────────────────────────────────────────
         profile_arg,
         use_sim_time_arg,
-        stm32_port_arg,
+        esp32_port_arg,
         uwb_port_a_arg,
         uwb_port_b_arg,
         gimbal_port_arg,
@@ -559,7 +559,7 @@ def generate_launch_description() -> LaunchDescription:  # noqa: C901
 
         # ── GROUP A: Drivers (all profiles, t=0–4s) ───────────────────────────
         robot_description,
-        stm32_bridge,
+        esp32_bridge,
         sensors,
         motor_daemon,
         sensor_health,

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_wheel_odom.py

@@ -20,7 +20,7 @@ theta is kept in (−π, π] after every step.
 
 Int32 rollover
 --------------
-STM32 encoder counters are int32 and wrap at ±2^31.  `unwrap_delta` handles
+ESP32 BALANCE encoder counters are int32 and wrap at ±2^31.  `unwrap_delta` handles
 this by detecting jumps larger than half the int32 range and adjusting by the
 full range:
 

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py

@@ -29,7 +29,7 @@ class Profile:
     name: str
 
     # ── Group A: Drivers (always on in all profiles) ──────────────────────
-    enable_stm32_bridge: bool = True
+    enable_esp32_bridge: bool = True
     enable_sensors:      bool = True   # RealSense + RPLIDAR
     enable_motor_daemon: bool = True
     enable_imu:          bool = True
@@ -69,14 +69,14 @@ class Profile:
     t_ui:         float = 22.0  # Group D (nav2 needs ~4 s to load costmaps)
 
     # ── Safety ────────────────────────────────────────────────────────────
-    watchdog_timeout_s:  float = 5.0   # max silence from STM32 bridge (s)
+    watchdog_timeout_s:  float = 5.0   # max silence from ESP32 bridge (s)
     cmd_vel_deadman_s:   float = 0.5   # cmd_vel watchdog in bridge
     max_linear_vel:      float = 0.5   # m/s cap passed to bridge + follower
     follow_distance_m:   float = 1.5   # target follow distance (m)
 
     # ── Hardware conditionals ─────────────────────────────────────────────
     #  Paths checked at launch; absent devices skip the relevant node.
-    stm32_port:  str = "/dev/stm32-bridge"
+    esp32_port:  str = "/dev/esp32-bridge"
     uwb_port_a:  str = "/dev/uwb-anchor0"
     uwb_port_b:  str = "/dev/uwb-anchor1"
     gimbal_port: str = "/dev/ttyTHS1"
@@ -90,7 +90,7 @@ class Profile:
 # ── Profile factory ────────────────────────────────────────────────────────────
 
 def _minimal() -> Profile:
-    """Minimal: STM32 bridge + sensors + motor daemon.
+    """Minimal: ESP32 bridge + sensors + motor daemon.
 
     Safe drive control only.  No AI, no nav, no social.
     Boot time ~4 s.  RAM ~400 MB.
@@ -115,7 +115,7 @@ def _full() -> Profile:
     return Profile(
         name="full",
         # Drivers
-        enable_stm32_bridge=True,
+        enable_esp32_bridge=True,
         enable_sensors=True,
         enable_motor_daemon=True,
         enable_imu=True,

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/wheel_odom_node.py

@@ -1,7 +1,7 @@
 """
 wheel_odom_node.py — Differential drive wheel encoder odometry (Issue #184).
 
-Subscribes to raw encoder tick counts from the STM32 bridge, integrates
+Subscribes to raw encoder tick counts from the ESP32 bridge, integrates
 differential drive kinematics, and publishes nav_msgs/Odometry at 50 Hz.
 Optionally broadcasts the odom → base_link TF transform.
 

jetson/ros2_ws/src/saltybot_bringup/test/README_INTEGRATION_TESTS.md

@@ -61,7 +61,7 @@ kill %1
 
 ### Core System Components
 - Robot Description (URDF/TF tree)
-- STM32 Serial Bridge
+- ESP32 Serial Bridge
 - cmd_vel Bridge  
 - Rosbridge WebSocket
 
@@ -125,11 +125,11 @@ free -h
 
 ### cmd_vel bridge not responding
 ```bash
-# Verify STM32 bridge is running first
+# Verify ESP32 bridge is running first
 ros2 node list | grep bridge
 
 # Check serial port
-ls -l /dev/stm32-bridge
+ls -l /dev/esp32-bridge
 ```
 
 ## Performance Baseline

jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py

@@ -74,7 +74,7 @@ class TestMinimalProfile:
         assert self.p.name == "minimal"
 
     def test_drivers_enabled(self):
-        assert self.p.enable_stm32_bridge is True
+        assert self.p.enable_esp32_bridge is True
         assert self.p.enable_sensors is True
         assert self.p.enable_motor_daemon is True
         assert self.p.enable_imu is True
@@ -124,7 +124,7 @@ class TestFullProfile:
         assert self.p.name == "full"
 
     def test_drivers_enabled(self):
-        assert self.p.enable_stm32_bridge is True
+        assert self.p.enable_esp32_bridge is True
         assert self.p.enable_sensors is True
         assert self.p.enable_motor_daemon is True
         assert self.p.enable_imu is True
@@ -312,9 +312,9 @@ class TestSafetyDefaults:
 # ─── Hardware port defaults ────────────────────────────────────────────────────
 
 class TestHardwarePortDefaults:
-    def test_stm32_port_set(self):
+    def test_esp32_port_set(self):
         p = _minimal()
-        assert p.stm32_port.startswith("/dev/")
+        assert p.esp32_port.startswith("/dev/")
 
     def test_uwb_ports_set(self):
         p = _full()

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

@@ -1 +1 @@
-"""SaltyBot CAN bridge package — Mamba controller and VESC telemetry via python-can."""
+"""SaltyBot CAN bridge package — ESP32 IO motor controller and VESC telemetry via python-can."""

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

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 """
-can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the Mamba motor
+can_bridge_node.py — ROS2 node bridging the SaltyBot Orin to the ESP32 IO motor
 controller and VESC motor controllers over CAN bus.
 
 The node opens the SocketCAN interface (slcan0 by default), spawns a background
@@ -9,12 +9,12 @@ 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)
+  /estop            std_msgs/Bool                → ESP32 IO e-stop (CAN)
 
 Publications
 ------------
-  /can/imu                sensor_msgs/Imu              Mamba IMU telemetry
-  /can/battery            sensor_msgs/BatteryState     Mamba battery telemetry
+  /can/imu                sensor_msgs/Imu              ESP32 IO IMU telemetry
+  /can/battery            sensor_msgs/BatteryState     ESP32 IO 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"
@@ -30,6 +30,7 @@ import can
 import rclpy
 from geometry_msgs.msg import Twist
 from rclpy.node import Node
+from rcl_interfaces.msg import SetParametersResult
 from sensor_msgs.msg import BatteryState, Imu
 from std_msgs.msg import Bool, Float32MultiArray, String
 
@@ -40,14 +41,18 @@ from saltybot_can_bridge.mamba_protocol import (
     MAMBA_TELEM_BATTERY,
     MAMBA_TELEM_IMU,
     VESC_TELEM_STATE,
+    ORIN_CAN_ID_FC_PID_ACK,
+    ORIN_CAN_ID_PID_SET,
     MODE_DRIVE,
     MODE_ESTOP,
     MODE_IDLE,
     encode_estop_cmd,
     encode_mode_cmd,
     encode_velocity_cmd,
+    encode_pid_set_cmd,
     decode_battery_telem,
     decode_imu_telem,
+    decode_pid_ack,
     decode_vesc_state,
 )
 
@@ -59,7 +64,7 @@ _WATCHDOG_HZ: float = 10.0
 
 
 class CanBridgeNode(Node):
-    """CAN bus bridge between Orin ROS2 and Mamba / VESC controllers."""
+    """CAN bus bridge between Orin ROS2 and ESP32 IO / VESC controllers."""
 
     def __init__(self) -> None:
         super().__init__("can_bridge_node")
@@ -70,12 +75,18 @@ class CanBridgeNode(Node):
         self.declare_parameter("right_vesc_can_id", 68)
         self.declare_parameter("mamba_can_id", 1)
         self.declare_parameter("command_timeout_s", 0.5)
+        self.declare_parameter("pid/kp", 0.0)
+        self.declare_parameter("pid/ki", 0.0)
+        self.declare_parameter("pid/kd", 0.0)
 
         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
+        self._pid_kp: float = self.get_parameter("pid/kp").value
+        self._pid_ki: float = self.get_parameter("pid/ki").value
+        self._pid_kd: float = self.get_parameter("pid/kd").value
 
         # ── State ─────────────────────────────────────────────────────────
         self._bus: Optional[can.BusABC] = None
@@ -99,6 +110,7 @@ class CanBridgeNode(Node):
         # ── Subscriptions ─────────────────────────────────────────────────
         self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
         self.create_subscription(Bool, "/estop", self._estop_cb, 10)
+        self.add_on_set_parameters_callback(self._on_set_parameters)
 
         # ── Timers ────────────────────────────────────────────────────────
         self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
@@ -119,6 +131,30 @@ class CanBridgeNode(Node):
             f"mamba={self._mamba_id}"
         )
 
+    # -- PID parameter callback (Issue #693) --
+
+    def _on_set_parameters(self, params) -> SetParametersResult:
+        """Send new PID gains over CAN when pid/* params change."""
+        for p in params:
+            if p.name == "pid/kp":
+                self._pid_kp = float(p.value)
+            elif p.name == "pid/ki":
+                self._pid_ki = float(p.value)
+            elif p.name == "pid/kd":
+                self._pid_kd = float(p.value)
+            else:
+                continue
+            try:
+                payload = encode_pid_set_cmd(self._pid_kp, self._pid_ki, self._pid_kd)
+                self._send_can(ORIN_CAN_ID_PID_SET, payload, "pid_set")
+                self.get_logger().info(
+                    f"PID gains sent: Kp={self._pid_kp:.2f} "
+                    f"Ki={self._pid_ki:.2f} Kd={self._pid_kd:.2f}"
+                )
+            except ValueError as exc:
+                return SetParametersResult(successful=False, reason=str(exc))
+        return SetParametersResult(successful=True)
+
     # ── Connection management ──────────────────────────────────────────────
 
     def _try_connect(self) -> None:
@@ -178,18 +214,18 @@ class CanBridgeNode(Node):
 
         # 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.
+        # The ESP32 IO 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
+        # Keep ESP32 IO 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."""
+        """Forward /estop to ESP32 IO over CAN."""
         if not self._connected:
             return
         payload = encode_estop_cmd(msg.data)
@@ -198,7 +234,7 @@ class CanBridgeNode(Node):
             self._send_can(
                 MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode"
             )
-            self.get_logger().warning("E-stop asserted — sent ESTOP to Mamba")
+            self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 IO")
 
     # ── Watchdog ──────────────────────────────────────────────────────────
 
@@ -282,6 +318,12 @@ class CanBridgeNode(Node):
             elif arb_id == VESC_TELEM_STATE + self._right_vesc_id:
                 self._handle_vesc_state(data, frame.timestamp, side="right")
 
+            elif arb_id == ORIN_CAN_ID_FC_PID_ACK:
+                gains = decode_pid_ack(data)
+                self.get_logger().debug(
+                    f"FC PID ACK: Kp={gains.kp:.2f} Ki={gains.ki:.2f} Kd={gains.kd:.2f}"
+                )
+
         except Exception as exc:
             self.get_logger().warning(
                 f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"

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

@@ -1,16 +1,19 @@
 #!/usr/bin/env python3
 """
-mamba_protocol.py — CAN message encoding/decoding for the Mamba motor controller
+mamba_protocol.py — CAN message encoding/decoding for the ESP32 IO motor controller
 and VESC telemetry.
 
+# TODO(esp32-migration): CAN IDs and struct layouts below are for the legacy Mamba
+# controller. When ESP32 IO CAN protocol is defined, update CAN IDs and frame formats.
+
 CAN message layout
 ------------------
-Command frames  (Orin → Mamba / VESC):
+Command frames  (Orin → ESP32 IO / 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):
+Telemetry frames (ESP32 IO → 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)
 
@@ -38,6 +41,8 @@ MAMBA_TELEM_IMU: int = 0x200
 MAMBA_TELEM_BATTERY: int = 0x201
 
 VESC_TELEM_STATE: int = 0x300
+ORIN_CAN_ID_PID_SET: int = 0x305
+ORIN_CAN_ID_FC_PID_ACK: int = 0x405
 
 # ---------------------------------------------------------------------------
 # Mode constants
@@ -54,7 +59,7 @@ MODE_ESTOP: int = 2
 
 @dataclass
 class ImuTelemetry:
-    """Decoded IMU telemetry from Mamba (MAMBA_TELEM_IMU)."""
+    """Decoded IMU telemetry from ESP32 IO (MAMBA_TELEM_IMU)."""
 
     accel_x: float = 0.0  # m/s²
     accel_y: float = 0.0
@@ -66,7 +71,7 @@ class ImuTelemetry:
 
 @dataclass
 class BatteryTelemetry:
-    """Decoded battery telemetry from Mamba (MAMBA_TELEM_BATTERY)."""
+    """Decoded battery telemetry from ESP32 IO (MAMBA_TELEM_BATTERY)."""
 
     voltage: float = 0.0   # V
     current: float = 0.0   # A
@@ -82,6 +87,14 @@ class VescStateTelemetry:
     current: float = 0.0   # phase current, A
 
 
+@dataclass
+class PidGains:
+    """Balance PID gains (Issue #693)."""
+    kp: float = 0.0
+    ki: float = 0.0
+    kd: float = 0.0
+
+
 # ---------------------------------------------------------------------------
 # Encode helpers
 # ---------------------------------------------------------------------------
@@ -142,6 +155,13 @@ def encode_estop_cmd(stop: bool = True) -> bytes:
     return struct.pack(_FMT_ESTOP, 0x01 if stop else 0x00)
 
 
+def encode_pid_set_cmd(kp: float, ki: float, kd: float) -> bytes:
+    """Encode ORIN_CAN_ID_PID_SET (6 bytes, uint16 BE x3). Issue #693."""
+    if kp < 0.0 or ki < 0.0 or kd < 0.0:
+        raise ValueError("PID gains must be non-negative")
+    return struct.pack(_FMT_PID, round(min(kp,_PID_KP_MAX)*100), round(min(ki,_PID_KI_MAX)*100), round(min(kd,_PID_KD_MAX)*100))
+
+
 # ---------------------------------------------------------------------------
 # Decode helpers
 # ---------------------------------------------------------------------------
@@ -199,3 +219,9 @@ def decode_vesc_state(data: bytes) -> VescStateTelemetry:
     """
     erpm, duty, voltage, current = struct.unpack(_FMT_VESC, data)
     return VescStateTelemetry(erpm=erpm, duty=duty, voltage=voltage, current=current)
+
+
+def decode_pid_ack(data: bytes) -> PidGains:
+    """Decode ORIN_CAN_ID_FC_PID_ACK (6 bytes). Issue #693."""
+    kp_x100, ki_x100, kd_x100 = struct.unpack(_FMT_PID, data)
+    return PidGains(kp=kp_x100/100.0, ki=ki_x100/100.0, kd=kd_x100/100.0)

jetson/ros2_ws/src/saltybot_can_bridge/setup.py

@@ -15,7 +15,7 @@ setup(
     zip_safe=True,
     maintainer="sl-controls",
     maintainer_email="sl-controls@saltylab.local",
-    description="CAN bus bridge for Mamba controller and VESC telemetry",
+    description="CAN bus bridge for ESP32 IO motor controller and VESC telemetry",
     license="MIT",
     tests_require=["pytest"],
     entry_points={

jetson/ros2_ws/src/saltybot_can_e2e_test/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_can_e2e_test</name>
+  <version>0.1.0</version>
+  <description>
+    End-to-end CAN integration test suite for the SaltyBot Orin↔Mamba↔VESC full loop.
+
+    Tests verify the complete CAN pipeline: drive commands, heartbeat timeout,
+    e-stop escalation, mode switching, and FC_VESC status broadcasts.
+
+    No real hardware or a running ROS2 system is required.
+    Run with: python -m pytest test/ -v
+
+    Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/695
+  </description>
+  <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
+  <license>MIT</license>
+
+  <!-- Runtime dependency on saltybot_can_bridge for mamba_protocol -->
+  <exec_depend>saltybot_can_bridge</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_e2e_test/saltybot_can_e2e_test/__init__.py

@@ -0,0 +1 @@
+# saltybot_can_e2e_test — End-to-end CAN integration test helpers

jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/can_mock.py

@@ -0,0 +1,160 @@
+#!/usr/bin/env python3
+"""
+can_mock.py — Mock CAN bus for unit/integration tests.
+
+Implements the same minimal interface as python-can's Bus class so test code
+can inject frames and capture outbound traffic without real hardware or a
+running SocketCAN interface.
+
+Interface
+---------
+  MockCANBus.send(msg, timeout=None)     — capture frame; if loopback, also enqueue for recv
+  MockCANBus.recv(timeout=None)          — return next injected frame (or None on timeout)
+  MockCANBus.inject(arb_id, data,        — queue a frame as if received from the bus
+                    is_extended_id=False)
+  MockCANBus.get_sent_frames()           — return copy of all sent frames list
+  MockCANBus.reset()                     — clear all state
+  MockCANBus.shutdown()                  — mark as shut down
+"""
+
+import queue
+import threading
+import time
+from typing import List, Optional
+
+try:
+    import can
+    _Message = can.Message
+except ImportError:
+    # Lightweight stand-in when python-can is not installed
+    class _Message:  # type: ignore[no-redef]
+        def __init__(
+            self,
+            arbitration_id: int = 0,
+            data: bytes = b"",
+            is_extended_id: bool = False,
+            timestamp: Optional[float] = None,
+        ) -> None:
+            self.arbitration_id = arbitration_id
+            self.data = bytearray(data)
+            self.is_extended_id = is_extended_id
+            self.timestamp = timestamp if timestamp is not None else time.monotonic()
+
+
+class MockCANBus:
+    """
+    Thread-safe mock CAN bus.
+
+    Parameters
+    ----------
+    loopback: bool
+        When True, every frame passed to send() is also placed in the recv
+        queue, simulating a loopback interface.
+    filters: list[dict] or None
+        Optional list of {"can_id": int, "can_mask": int} dicts.  Only frames
+        matching at least one filter are returned by recv().  If None, all
+        frames are returned.
+    """
+
+    def __init__(self, loopback: bool = False, filters=None) -> None:
+        self._loopback = loopback
+        self._filters = filters
+        self._recv_q: queue.Queue = queue.Queue()
+        self._sent: List[_Message] = []
+        self._sent_lock = threading.Lock()
+        self._shutdown = False
+
+    # ------------------------------------------------------------------
+    # python-can Bus interface (subset used by CanBridgeNode)
+    # ------------------------------------------------------------------
+
+    def send(self, msg, timeout: Optional[float] = None) -> None:
+        """Record an outbound frame.  If loopback is enabled, also enqueue it."""
+        if self._shutdown:
+            raise RuntimeError("MockCANBus is shut down")
+        # Normalise to _Message so callers can pass any object with the right attrs
+        with self._sent_lock:
+            self._sent.append(msg)
+        if self._loopback:
+            self._recv_q.put(msg)
+
+    def recv(self, timeout: Optional[float] = None) -> Optional[_Message]:
+        """
+        Return the next injected frame.  Blocks up to timeout seconds.
+        Returns None if nothing arrives within the timeout.
+        """
+        if self._shutdown:
+            return None
+        try:
+            return self._recv_q.get(block=True, timeout=timeout)
+        except queue.Empty:
+            return None
+
+    def shutdown(self) -> None:
+        """Mark the bus as shut down; subsequent recv() returns None."""
+        self._shutdown = True
+
+    # ------------------------------------------------------------------
+    # Test helpers
+    # ------------------------------------------------------------------
+
+    def inject(
+        self,
+        arbitration_id: int,
+        data: bytes,
+        is_extended_id: bool = False,
+        timestamp: Optional[float] = None,
+    ) -> None:
+        """
+        Inject a frame into the receive queue as if it arrived from the bus.
+
+        Parameters
+        ----------
+        arbitration_id: CAN arbitration ID
+        data: frame payload bytes
+        is_extended_id: True for 29-bit extended frames (VESC style)
+        timestamp: optional monotonic timestamp; defaults to time.monotonic()
+        """
+        msg = _Message(
+            arbitration_id=arbitration_id,
+            data=data,
+            is_extended_id=is_extended_id,
+            timestamp=timestamp if timestamp is not None else time.monotonic(),
+        )
+        self._recv_q.put(msg)
+
+    def get_sent_frames(self) -> List[_Message]:
+        """Return a snapshot of all frames sent through this bus."""
+        with self._sent_lock:
+            return list(self._sent)
+
+    def get_sent_frames_by_id(self, arbitration_id: int) -> List[_Message]:
+        """Return only sent frames whose arbitration_id matches."""
+        with self._sent_lock:
+            return [f for f in self._sent if f.arbitration_id == arbitration_id]
+
+    def reset(self) -> None:
+        """Clear all sent frames and drain the receive queue."""
+        with self._sent_lock:
+            self._sent.clear()
+        while not self._recv_q.empty():
+            try:
+                self._recv_q.get_nowait()
+            except queue.Empty:
+                break
+        self._shutdown = False
+
+    def pending_recv(self) -> int:
+        """Return the number of frames waiting in the receive queue."""
+        return self._recv_q.qsize()
+
+    # ------------------------------------------------------------------
+    # Context manager support
+    # ------------------------------------------------------------------
+
+    def __enter__(self):
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.shutdown()
+        return False

jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py

@@ -0,0 +1,314 @@
+#!/usr/bin/env python3
+"""
+protocol_defs.py — CAN message ID constants and frame builders/parsers for the
+Orin↔ESP32 IO↔VESC integration test suite.
+
+All IDs and payload formats are derived from:
+  include/orin_can.h   — Orin↔FC (ESP32 IO) protocol
+  include/vesc_can.h   — VESC CAN protocol
+  saltybot_can_bridge/mamba_protocol.py — existing bridge constants
+
+CAN IDs used in tests
+---------------------
+Orin → FC (ESP32 IO) commands (standard 11-bit, matching orin_can.h):
+  ORIN_CMD_HEARTBEAT  0x300
+  ORIN_CMD_DRIVE      0x301    int16 speed (−1000..+1000), int16 steer (−1000..+1000)
+  ORIN_CMD_MODE       0x302    uint8 mode byte
+  ORIN_CMD_ESTOP      0x303    uint8 action (1=ESTOP, 0=CLEAR)
+
+FC (ESP32 IO) → Orin telemetry (standard 11-bit, matching orin_can.h):
+  FC_STATUS           0x400    8 bytes (see orin_can_fc_status_t)
+  FC_VESC             0x401    8 bytes (see orin_can_fc_vesc_t)
+  FC_IMU              0x402    8 bytes
+  FC_BARO             0x403    8 bytes
+
+ESP32 IO ↔ VESC internal commands (matching mamba_protocol.py):
+  MAMBA_CMD_VELOCITY  0x100    8 bytes  left_mps (f32) | right_mps (f32) big-endian
+  MAMBA_CMD_MODE      0x101    1 byte   mode (0=idle,1=drive,2=estop)
+  MAMBA_CMD_ESTOP     0x102    1 byte   0x01=stop
+
+VESC STATUS (extended 29-bit, matching vesc_can.h):
+  arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id  = (9 << 8) | node_id
+  Payload: int32 RPM (BE), int16 current×10 (BE), int16 duty×1000 (BE)
+"""
+
+import struct
+from typing import Tuple
+
+# ---------------------------------------------------------------------------
+# Orin → FC (ESP32 IO) command IDs  (from orin_can.h)
+# ---------------------------------------------------------------------------
+
+ORIN_CMD_HEARTBEAT: int = 0x300
+ORIN_CMD_DRIVE: int     = 0x301
+ORIN_CMD_MODE: int      = 0x302
+ORIN_CMD_ESTOP: int     = 0x303
+
+# ---------------------------------------------------------------------------
+# FC (ESP32 IO) → Orin telemetry IDs  (from orin_can.h)
+# ---------------------------------------------------------------------------
+
+FC_STATUS: int = 0x400
+FC_VESC: int   = 0x401
+FC_IMU: int    = 0x402
+FC_BARO: int   = 0x403
+
+# ---------------------------------------------------------------------------
+# ESP32 IO → VESC internal command IDs  (from mamba_protocol.py)
+# ---------------------------------------------------------------------------
+
+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
+
+# ---------------------------------------------------------------------------
+# VESC protocol constants  (from vesc_can.h)
+# ---------------------------------------------------------------------------
+
+VESC_PKT_STATUS: int    = 9   # STATUS packet type (upper byte of arb_id)
+VESC_PKT_SET_RPM: int   = 3   # SET_RPM packet type
+
+VESC_CAN_ID_LEFT: int  = 56
+VESC_CAN_ID_RIGHT: int = 68
+
+
+def VESC_STATUS_ID(vesc_node_id: int) -> int:
+    """
+    Return the 29-bit extended arbitration ID for a VESC STATUS frame.
+
+    Formula (from vesc_can.h):  arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id
+                                        = (9 << 8) | vesc_node_id
+    """
+    return (VESC_PKT_STATUS << 8) | vesc_node_id
+
+
+def VESC_SET_RPM_ID(vesc_node_id: int) -> int:
+    """
+    Return the 29-bit extended arbitration ID for a VESC SET_RPM command.
+
+    Formula:  arb_id = (VESC_PKT_SET_RPM << 8) | vesc_node_id
+                     = (3 << 8) | vesc_node_id
+    """
+    return (VESC_PKT_SET_RPM << 8) | vesc_node_id
+
+
+# ---------------------------------------------------------------------------
+# Frame builders — Orin → FC
+# ---------------------------------------------------------------------------
+
+def build_heartbeat(seq: int = 0) -> bytes:
+    """Build a HEARTBEAT payload: uint32 sequence counter (big-endian, 4 bytes)."""
+    return struct.pack(">I", seq & 0xFFFFFFFF)
+
+
+def build_drive_cmd(speed: int, steer: int) -> bytes:
+    """
+    Build an ORIN_CMD_DRIVE payload.
+
+    Parameters
+    ----------
+    speed: int, −1000..+1000  (mapped directly to int16)
+    steer: int, −1000..+1000
+    """
+    return struct.pack(">hh", int(speed), int(steer))
+
+
+def build_mode_cmd(mode: int) -> bytes:
+    """Build an ORIN_CMD_MODE payload (1 byte)."""
+    return struct.pack(">B", mode & 0xFF)
+
+
+def build_estop_cmd(action: int = 1) -> bytes:
+    """Build an ORIN_CMD_ESTOP payload.  action=1 → ESTOP, 0 → CLEAR."""
+    return struct.pack(">B", action & 0xFF)
+
+
+# ---------------------------------------------------------------------------
+# Frame builders — ESP32 IO velocity commands (mamba_protocol.py encoding)
+# ---------------------------------------------------------------------------
+
+def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes:
+    """
+    Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian).
+
+    Matches encode_velocity_cmd() in mamba_protocol.py.
+    """
+    return struct.pack(">ff", float(left_mps), float(right_mps))
+
+
+# ---------------------------------------------------------------------------
+# Frame builders — FC → Orin telemetry
+# ---------------------------------------------------------------------------
+
+def build_fc_status(
+    pitch_x10: int = 0,
+    motor_cmd: int = 0,
+    vbat_mv: int = 24000,
+    balance_state: int = 1,
+    flags: int = 0,
+) -> bytes:
+    """
+    Build an FC_STATUS (0x400) payload.
+
+    Layout (orin_can_fc_status_t, 8 bytes, big-endian):
+      int16  pitch_x10
+      int16  motor_cmd
+      uint16 vbat_mv
+      uint8  balance_state
+      uint8  flags  [bit0=estop_active, bit1=armed]
+    """
+    return struct.pack(
+        ">hhHBB",
+        int(pitch_x10),
+        int(motor_cmd),
+        int(vbat_mv) & 0xFFFF,
+        int(balance_state) & 0xFF,
+        int(flags) & 0xFF,
+    )
+
+
+def build_fc_vesc(
+    left_rpm_x10: int = 0,
+    right_rpm_x10: int = 0,
+    left_current_x10: int = 0,
+    right_current_x10: int = 0,
+) -> bytes:
+    """
+    Build an FC_VESC (0x401) payload.
+
+    Layout (orin_can_fc_vesc_t, 8 bytes, big-endian):
+      int16 left_rpm_x10
+      int16 right_rpm_x10
+      int16 left_current_x10
+      int16 right_current_x10
+
+    RPM values are RPM / 10 (e.g. 3000 RPM → 300).
+    Current values are A × 10 (e.g. 5.5 A → 55).
+    """
+    return struct.pack(
+        ">hhhh",
+        int(left_rpm_x10),
+        int(right_rpm_x10),
+        int(left_current_x10),
+        int(right_current_x10),
+    )
+
+
+def build_vesc_status(
+    rpm: int = 0,
+    current_x10: int = 0,
+    duty_x1000: int = 0,
+) -> bytes:
+    """
+    Build a VESC STATUS (packet type 9) payload.
+
+    Layout (from vesc_can.h / VESC FW 6.x, big-endian):
+      int32 rpm
+      int16 current × 10
+      int16 duty × 1000
+    Total: 8 bytes.
+    """
+    return struct.pack(
+        ">ihh",
+        int(rpm),
+        int(current_x10),
+        int(duty_x1000),
+    )
+
+
+# ---------------------------------------------------------------------------
+# Frame parsers
+# ---------------------------------------------------------------------------
+
+def parse_fc_status(data: bytes):
+    """
+    Parse an FC_STATUS (0x400) payload.
+
+    Returns
+    -------
+    dict with keys: pitch_x10, motor_cmd, vbat_mv, balance_state, flags,
+                    estop_active (bool), armed (bool)
+    """
+    if len(data) < 8:
+        raise ValueError(f"FC_STATUS needs 8 bytes, got {len(data)}")
+    pitch_x10, motor_cmd, vbat_mv, balance_state, flags = struct.unpack(
+        ">hhHBB", data[:8]
+    )
+    return {
+        "pitch_x10": pitch_x10,
+        "motor_cmd": motor_cmd,
+        "vbat_mv": vbat_mv,
+        "balance_state": balance_state,
+        "flags": flags,
+        "estop_active": bool(flags & 0x01),
+        "armed": bool(flags & 0x02),
+    }
+
+
+def parse_fc_vesc(data: bytes):
+    """
+    Parse an FC_VESC (0x401) payload.
+
+    Returns
+    -------
+    dict with keys: left_rpm_x10, right_rpm_x10, left_current_x10,
+                    right_current_x10, left_rpm (float), right_rpm (float)
+    """
+    if len(data) < 8:
+        raise ValueError(f"FC_VESC needs 8 bytes, got {len(data)}")
+    left_rpm_x10, right_rpm_x10, left_cur_x10, right_cur_x10 = struct.unpack(
+        ">hhhh", data[:8]
+    )
+    return {
+        "left_rpm_x10": left_rpm_x10,
+        "right_rpm_x10": right_rpm_x10,
+        "left_current_x10": left_cur_x10,
+        "right_current_x10": right_cur_x10,
+        "left_rpm": left_rpm_x10 * 10.0,
+        "right_rpm": right_rpm_x10 * 10.0,
+    }
+
+
+def parse_vesc_status(data: bytes):
+    """
+    Parse a VESC STATUS (packet type 9) payload.
+
+    Returns
+    -------
+    dict with keys: rpm, current_x10, duty_x1000, current (float), duty (float)
+    """
+    if len(data) < 8:
+        raise ValueError(f"VESC STATUS needs 8 bytes, got {len(data)}")
+    rpm, current_x10, duty_x1000 = struct.unpack(">ihh", data[:8])
+    return {
+        "rpm": rpm,
+        "current_x10": current_x10,
+        "duty_x1000": duty_x1000,
+        "current": current_x10 / 10.0,
+        "duty": duty_x1000 / 1000.0,
+    }
+
+
+def parse_velocity_cmd(data: bytes) -> Tuple[float, float]:
+    """
+    Parse a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian).
+
+    Returns
+    -------
+    (left_mps, right_mps)
+    """
+    if len(data) < 8:
+        raise ValueError(f"MAMBA_CMD_VELOCITY needs 8 bytes, got {len(data)}")
+    return struct.unpack(">ff", data[:8])

jetson/ros2_ws/src/saltybot_can_e2e_test/setup.cfg

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

jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py

@@ -0,0 +1,23 @@
+from setuptools import setup
+
+package_name = "saltybot_can_e2e_test"
+
+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"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-jetson",
+    maintainer_email="sl-jetson@saltylab.local",
+    description="End-to-end CAN integration tests for Orin↔ESP32 IO↔VESC full loop",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [],
+    },
+)

jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python3
+"""
+conftest.py — pytest fixtures for the saltybot_can_e2e_test suite.
+
+No ROS2 node infrastructure is started; all tests run purely in Python.
+"""
+
+import sys
+import os
+
+# Ensure the package root is on sys.path so relative imports work when running
+# pytest directly from the saltybot_can_e2e_test/ directory.
+_pkg_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+if _pkg_root not in sys.path:
+    sys.path.insert(0, _pkg_root)
+
+# Also add the saltybot_can_bridge package so we can import mamba_protocol.
+_bridge_pkg = os.path.join(
+    os.path.dirname(_pkg_root), "saltybot_can_bridge"
+)
+if _bridge_pkg not in sys.path:
+    sys.path.insert(0, _bridge_pkg)
+
+import pytest
+
+from saltybot_can_e2e_test.can_mock import MockCANBus
+from saltybot_can_e2e_test.protocol_defs import (
+    VESC_CAN_ID_LEFT,
+    VESC_CAN_ID_RIGHT,
+)
+
+
+# ---------------------------------------------------------------------------
+# Core fixtures
+# ---------------------------------------------------------------------------
+
+
+@pytest.fixture(scope="function")
+def mock_can_bus():
+    """
+    Provide a fresh MockCANBus instance per test function.
+    The bus is automatically shut down after each test.
+    """
+    bus = MockCANBus(loopback=False)
+    yield bus
+    bus.shutdown()
+
+
+@pytest.fixture(scope="function")
+def loopback_can_bus():
+    """
+    MockCANBus in loopback mode — sent frames are also queued for recv.
+    Useful for testing round-trip behaviour without a second node.
+    """
+    bus = MockCANBus(loopback=True)
+    yield bus
+    bus.shutdown()
+
+
+@pytest.fixture(scope="function")
+def bridge_components():
+    """
+    Return the mamba_protocol encode/decode callables and a fresh mock bus.
+
+    Yields a dict with keys:
+      bus         — MockCANBus instance
+      encode_vel  — encode_velocity_cmd(left, right) → bytes
+      encode_mode — encode_mode_cmd(mode) → bytes
+      encode_estop — encode_estop_cmd(stop) → bytes
+      decode_vesc  — decode_vesc_state(data) → VescStateTelemetry
+    """
+    from saltybot_can_bridge.mamba_protocol import (
+        encode_velocity_cmd,
+        encode_mode_cmd,
+        encode_estop_cmd,
+        decode_vesc_state,
+        decode_battery_telem,
+        decode_imu_telem,
+    )
+
+    bus = MockCANBus(loopback=False)
+    yield {
+        "bus": bus,
+        "encode_vel": encode_velocity_cmd,
+        "encode_mode": encode_mode_cmd,
+        "encode_estop": encode_estop_cmd,
+        "decode_vesc": decode_vesc_state,
+        "decode_battery": decode_battery_telem,
+        "decode_imu": decode_imu_telem,
+        "left_vesc_id": VESC_CAN_ID_LEFT,
+        "right_vesc_id": VESC_CAN_ID_RIGHT,
+    }
+    bus.shutdown()

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py

@@ -0,0 +1,193 @@
+#!/usr/bin/env python3
+"""
+test_drive_command.py — Integration tests for the drive command path.
+
+Tests verify:
+  DRIVE cmd → ESP32 IO receives velocity command frame → mock VESC status response
+  → FC_VESC broadcast contains correct RPMs.
+
+All tests run without real hardware or a running ROS2 system.
+Run with: python -m pytest test/test_drive_command.py -v
+"""
+
+import struct
+import pytest
+
+from saltybot_can_e2e_test.protocol_defs import (
+    MAMBA_CMD_VELOCITY,
+    MAMBA_CMD_MODE,
+    FC_VESC,
+    MODE_DRIVE,
+    MODE_IDLE,
+    VESC_CAN_ID_LEFT,
+    VESC_CAN_ID_RIGHT,
+    VESC_STATUS_ID,
+    build_velocity_cmd,
+    build_fc_vesc,
+    build_vesc_status,
+    parse_velocity_cmd,
+    parse_fc_vesc,
+)
+from saltybot_can_bridge.mamba_protocol import (
+    encode_velocity_cmd,
+    encode_mode_cmd,
+)
+
+
+# ---------------------------------------------------------------------------
+# Helper
+# ---------------------------------------------------------------------------
+
+def _send_drive(bus, left_mps: float, right_mps: float) -> None:
+    """Simulate the bridge encoding and sending a velocity command."""
+    from saltybot_can_e2e_test.can_mock import MockCANBus
+
+    payload = encode_velocity_cmd(left_mps, right_mps)
+    # Create a minimal message object compatible with our mock
+    class _Msg:
+        def __init__(self, arb_id, data):
+            self.arbitration_id = arb_id
+            self.data = bytearray(data)
+            self.is_extended_id = False
+
+    bus.send(_Msg(MAMBA_CMD_VELOCITY, payload))
+    bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
+
+
+# ---------------------------------------------------------------------------
+# Tests
+# ---------------------------------------------------------------------------
+
+class TestDriveForward:
+    def test_drive_forward_velocity_frame_sent(self, mock_can_bus):
+        """
+        Inject DRIVE cmd (1.0 m/s, 1.0 m/s) → verify ESP32 IO receives
+        a MAMBA_CMD_VELOCITY frame with correct payload.
+        """
+        _send_drive(mock_can_bus, 1.0, 1.0)
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 1, "Expected exactly one velocity command frame"
+
+        left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
+        assert abs(left - 1.0) < 1e-4, f"Left speed {left} != 1.0"
+        assert abs(right - 1.0) < 1e-4, f"Right speed {right} != 1.0"
+
+    def test_drive_forward_mode_drive_sent(self, mock_can_bus):
+        """After a drive command, a MODE=drive frame must accompany it."""
+        _send_drive(mock_can_bus, 1.0, 1.0)
+
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        assert len(mode_frames) >= 1, "Expected at least one MODE frame"
+        assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE])
+
+    def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus):
+        """
+        Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct.
+        (In the real loop ESP32 IO computes RPM from m/s and broadcasts FC_VESC.)
+        This test checks the FC_VESC frame format and parser.
+        """
+        # Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics)
+        fc_payload = build_fc_vesc(
+            left_rpm_x10=300, right_rpm_x10=300,
+            left_current_x10=50, right_current_x10=50,
+        )
+        mock_can_bus.inject(FC_VESC, fc_payload)
+
+        frame = mock_can_bus.recv(timeout=0.1)
+        assert frame is not None, "FC_VESC frame not received"
+        parsed = parse_fc_vesc(bytes(frame.data))
+        assert parsed["left_rpm_x10"] == 300
+        assert parsed["right_rpm_x10"] == 300
+        assert abs(parsed["left_rpm"] - 3000.0) < 0.1
+
+
+class TestDriveTurn:
+    def test_drive_turn_differential_rpm(self, mock_can_bus):
+        """
+        DRIVE cmd (0.5, −0.5) → verify differential RPM in velocity command.
+        """
+        _send_drive(mock_can_bus, 0.5, -0.5)
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 1
+
+        left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
+        assert abs(left - 0.5) < 1e-4,  f"Left speed {left} != 0.5"
+        assert abs(right - (-0.5)) < 1e-4, f"Right speed {right} != -0.5"
+        # Signs must be opposite for a zero-radius spin
+        assert left > 0 and right < 0
+
+    def test_drive_turn_fc_vesc_differential(self, mock_can_bus):
+        """Simulated FC_VESC for a turn has opposite-sign RPMs."""
+        fc_payload = build_fc_vesc(
+            left_rpm_x10=150, right_rpm_x10=-150,
+            left_current_x10=30, right_current_x10=30,
+        )
+        mock_can_bus.inject(FC_VESC, fc_payload)
+        frame = mock_can_bus.recv(timeout=0.1)
+        parsed = parse_fc_vesc(bytes(frame.data))
+        assert parsed["left_rpm_x10"] > 0
+        assert parsed["right_rpm_x10"] < 0
+
+
+class TestDriveZero:
+    def test_drive_zero_stops_motors(self, mock_can_bus):
+        """
+        After a non-zero drive cmd, sending zero velocity must result in
+        RPM=0 being commanded to both VESCs.
+        """
+        _send_drive(mock_can_bus, 1.0, 1.0)
+        mock_can_bus.reset()          # clear prior frames
+
+        _send_drive(mock_can_bus, 0.0, 0.0)
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 1
+        left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
+        assert abs(left) < 1e-5,  "Left motor not stopped"
+        assert abs(right) < 1e-5, "Right motor not stopped"
+
+
+class TestDriveCmdTimeout:
+    def test_drive_cmd_timeout_sends_zero(self, mock_can_bus):
+        """
+        Simulate the watchdog behaviour: if no DRIVE cmd arrives for >500 ms,
+        zero velocity is sent.  We test the encoding directly (without timers).
+        """
+        # The watchdog in CanBridgeNode calls encode_velocity_cmd(0.0, 0.0) and
+        # sends it on MAMBA_CMD_VELOCITY.  Replicate that here.
+        zero_payload = encode_velocity_cmd(0.0, 0.0)
+
+        class _Msg:
+            def __init__(self, arb_id, data):
+                self.arbitration_id = arb_id
+                self.data = bytearray(data)
+                self.is_extended_id = False
+
+        mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, zero_payload))
+        mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE)))
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 1
+        left, right = parse_velocity_cmd(bytes(vel_frames[0].data))
+        assert abs(left) < 1e-5
+        assert abs(right) < 1e-5
+
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        assert len(mode_frames) == 1
+        assert bytes(mode_frames[0].data) == bytes([MODE_IDLE])
+
+
+@pytest.mark.parametrize("left_mps,right_mps", [
+    (0.5, 0.5),
+    (1.0, 0.0),
+    (0.0, -1.0),
+    (-0.5, -0.5),
+])
+def test_drive_cmd_payload_roundtrip(mock_can_bus, left_mps, right_mps):
+    """Parametrized: encode then decode must recover original velocities."""
+    payload = encode_velocity_cmd(left_mps, right_mps)
+    l, r = parse_velocity_cmd(payload)
+    assert abs(l - left_mps) < 1e-4
+    assert abs(r - right_mps) < 1e-4

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py

@@ -0,0 +1,264 @@
+#!/usr/bin/env python3
+"""
+test_estop.py — E-stop command integration tests.
+
+Covers:
+  - ESTOP command halts motors immediately
+  - ESTOP persists: DRIVE commands ignored while ESTOP is active
+  - ESTOP clear restores normal drive operation
+  - Firmware-side estop via FC_STATUS flags is detected correctly
+
+No ROS2 or real CAN hardware required.
+Run with: python -m pytest test/test_estop.py -v
+"""
+
+import struct
+import pytest
+
+from saltybot_can_e2e_test.can_mock import MockCANBus
+from saltybot_can_e2e_test.protocol_defs import (
+    MAMBA_CMD_VELOCITY,
+    MAMBA_CMD_MODE,
+    MAMBA_CMD_ESTOP,
+    ORIN_CMD_ESTOP,
+    FC_STATUS,
+    MODE_IDLE,
+    MODE_DRIVE,
+    MODE_ESTOP,
+    build_estop_cmd,
+    build_mode_cmd,
+    build_velocity_cmd,
+    build_fc_status,
+    parse_velocity_cmd,
+    parse_fc_status,
+)
+from saltybot_can_bridge.mamba_protocol import (
+    encode_velocity_cmd,
+    encode_mode_cmd,
+    encode_estop_cmd,
+)
+
+
+# ---------------------------------------------------------------------------
+# Helpers
+# ---------------------------------------------------------------------------
+
+class _Msg:
+    """Minimal CAN message stand-in."""
+    def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False):
+        self.arbitration_id = arb_id
+        self.data = bytearray(data)
+        self.is_extended_id = is_extended_id
+
+
+class EstopStateMachine:
+    """
+    Lightweight state machine that mirrors the bridge estop logic.
+
+    Tracks whether ESTOP is active and gates velocity commands accordingly.
+    Sends frames to the supplied MockCANBus.
+    """
+
+    def __init__(self, bus: MockCANBus):
+        self._bus = bus
+        self._estop_active = False
+        self._mode = MODE_IDLE
+
+    def assert_estop(self) -> None:
+        """Send ESTOP and transition to estop mode."""
+        self._estop_active = True
+        self._mode = MODE_ESTOP
+        self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
+        self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP)))
+        self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
+
+    def clear_estop(self) -> None:
+        """Clear ESTOP and return to IDLE mode."""
+        self._estop_active = False
+        self._mode = MODE_IDLE
+        self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False)))
+        self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE)))
+
+    def send_drive(self, left_mps: float, right_mps: float) -> None:
+        """Send velocity command only if ESTOP is not active."""
+        if self._estop_active:
+            # Bridge silently drops commands while estopped
+            return
+        self._mode = MODE_DRIVE
+        self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps)))
+        self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
+
+    @property
+    def estop_active(self) -> bool:
+        return self._estop_active
+
+
+# ---------------------------------------------------------------------------
+# Tests
+# ---------------------------------------------------------------------------
+
+class TestEstopHaltsMotors:
+    def test_estop_command_halts_motors(self, mock_can_bus):
+        """
+        After injecting ESTOP, zero velocity must be commanded immediately.
+        """
+        sm = EstopStateMachine(mock_can_bus)
+        sm.assert_estop()
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) >= 1, "No velocity frame after ESTOP"
+        l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
+        assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP"
+        assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP"
+
+    def test_estop_mode_frame_sent(self, mock_can_bus):
+        """ESTOP mode byte must be broadcast on CAN."""
+        sm = EstopStateMachine(mock_can_bus)
+        sm.assert_estop()
+
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        assert any(
+            bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames
+        ), "MODE=ESTOP not found in sent frames"
+
+    def test_estop_flag_byte_is_0x01(self, mock_can_bus):
+        """MAMBA_CMD_ESTOP payload must be 0x01 when asserting e-stop."""
+        sm = EstopStateMachine(mock_can_bus)
+        sm.assert_estop()
+
+        estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
+        assert len(estop_frames) >= 1
+        assert bytes(estop_frames[-1].data) == b"\x01", \
+            f"ESTOP payload {estop_frames[-1].data!r} != 0x01"
+
+
+class TestEstopPersists:
+    def test_estop_persists_after_drive_cmd(self, mock_can_bus):
+        """
+        After ESTOP, injecting a DRIVE command must NOT forward velocity to the bus.
+        """
+        sm = EstopStateMachine(mock_can_bus)
+        sm.assert_estop()
+        mock_can_bus.reset()   # start fresh after initial ESTOP frames
+
+        sm.send_drive(1.0, 1.0)   # should be suppressed
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 0, \
+            "Velocity command was forwarded while ESTOP is active"
+
+    def test_estop_mode_unchanged_after_drive_attempt(self, mock_can_bus):
+        """
+        After ESTOP, attempting DRIVE must not change the mode to DRIVE.
+        """
+        sm = EstopStateMachine(mock_can_bus)
+        sm.assert_estop()
+        mock_can_bus.reset()
+
+        sm.send_drive(0.5, 0.5)
+
+        # No mode frames should have been emitted (drive was suppressed)
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        assert all(
+            bytes(f.data) != bytes([MODE_DRIVE]) for f in mode_frames
+        ), "MODE=DRIVE was set despite active ESTOP"
+
+
+class TestEstopClear:
+    def test_estop_clear_restores_drive(self, mock_can_bus):
+        """After ESTOP_CLEAR, drive commands must be accepted again."""
+        sm = EstopStateMachine(mock_can_bus)
+        sm.assert_estop()
+        sm.clear_estop()
+        mock_can_bus.reset()
+
+        sm.send_drive(0.8, 0.8)
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 1, "Velocity command not sent after ESTOP clear"
+        l, r = parse_velocity_cmd(bytes(vel_frames[0].data))
+        assert abs(l - 0.8) < 1e-4
+        assert abs(r - 0.8) < 1e-4
+
+    def test_estop_clear_flag_byte_is_0x00(self, mock_can_bus):
+        """MAMBA_CMD_ESTOP payload must be 0x00 when clearing e-stop."""
+        sm = EstopStateMachine(mock_can_bus)
+        sm.assert_estop()
+        sm.clear_estop()
+
+        estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
+        assert len(estop_frames) >= 2
+        # Last ESTOP frame should be the clear
+        assert bytes(estop_frames[-1].data) == b"\x00", \
+            f"ESTOP clear payload {estop_frames[-1].data!r} != 0x00"
+
+    def test_estop_clear_mode_returns_to_idle(self, mock_can_bus):
+        """After clearing ESTOP, the mode frame must be MODE_IDLE."""
+        sm = EstopStateMachine(mock_can_bus)
+        sm.assert_estop()
+        sm.clear_estop()
+
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        last_mode = bytes(mode_frames[-1].data)
+        assert last_mode == bytes([MODE_IDLE]), \
+            f"Mode after ESTOP clear is {last_mode!r}, expected MODE_IDLE"
+
+
+class TestFirmwareSideEstop:
+    def test_fc_status_estop_flag_detected(self, mock_can_bus):
+        """
+        Simulate firmware sending estop via FC_STATUS flags (bit0=estop_active).
+        Verify the Orin bridge side correctly parses the flag.
+        """
+        # Build FC_STATUS with estop_active bit set (flags=0x01)
+        payload = build_fc_status(
+            pitch_x10=0,
+            motor_cmd=0,
+            vbat_mv=24000,
+            balance_state=2,   # TILT_FAULT
+            flags=0x01,        # bit0 = estop_active
+        )
+        mock_can_bus.inject(FC_STATUS, payload)
+
+        frame = mock_can_bus.recv(timeout=0.1)
+        assert frame is not None, "FC_STATUS frame not received"
+        parsed = parse_fc_status(bytes(frame.data))
+        assert parsed["estop_active"] is True, \
+            "estop_active flag not set in FC_STATUS"
+        assert parsed["balance_state"] == 2
+
+    def test_fc_status_no_estop_flag(self, mock_can_bus):
+        """FC_STATUS with flags=0x00 must NOT set estop_active."""
+        payload = build_fc_status(flags=0x00)
+        mock_can_bus.inject(FC_STATUS, payload)
+        frame = mock_can_bus.recv(timeout=0.1)
+        parsed = parse_fc_status(bytes(frame.data))
+        assert parsed["estop_active"] is False
+
+    def test_fc_status_armed_flag_detected(self, mock_can_bus):
+        """FC_STATUS flags bit1=armed must parse correctly."""
+        payload = build_fc_status(flags=0x02)  # bit1 = armed
+        mock_can_bus.inject(FC_STATUS, payload)
+        frame = mock_can_bus.recv(timeout=0.1)
+        parsed = parse_fc_status(bytes(frame.data))
+        assert parsed["armed"] is True
+        assert parsed["estop_active"] is False
+
+    def test_fc_status_roundtrip(self, mock_can_bus):
+        """build_fc_status → inject → recv → parse_fc_status must be identity."""
+        payload = build_fc_status(
+            pitch_x10=150,
+            motor_cmd=-200,
+            vbat_mv=23800,
+            balance_state=1,
+            flags=0x03,
+        )
+        mock_can_bus.inject(FC_STATUS, payload)
+        frame = mock_can_bus.recv(timeout=0.1)
+        parsed = parse_fc_status(bytes(frame.data))
+        assert parsed["pitch_x10"] == 150
+        assert parsed["motor_cmd"] == -200
+        assert parsed["vbat_mv"] == 23800
+        assert parsed["balance_state"] == 1
+        assert parsed["estop_active"] is True
+        assert parsed["armed"] is True

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py

@@ -0,0 +1,315 @@
+#!/usr/bin/env python3
+"""
+test_fc_vesc_broadcast.py — FC_VESC broadcast and VESC STATUS integration tests.
+
+Covers:
+  - VESC STATUS extended frame for left VESC (ID 56) triggers FC_VESC broadcast
+  - Both left (56) and right (68) VESC STATUS combined in FC_VESC
+  - FC_VESC broadcast rate (~10 Hz)
+  - current_x10 scaling matches protocol spec
+
+No ROS2 or real CAN hardware required.
+Run with: python -m pytest test/test_fc_vesc_broadcast.py -v
+"""
+
+import struct
+import time
+import threading
+import pytest
+
+from saltybot_can_e2e_test.can_mock import MockCANBus
+from saltybot_can_e2e_test.protocol_defs import (
+    FC_VESC,
+    VESC_CAN_ID_LEFT,
+    VESC_CAN_ID_RIGHT,
+    VESC_STATUS_ID,
+    VESC_SET_RPM_ID,
+    VESC_TELEM_STATE,
+    build_vesc_status,
+    build_fc_vesc,
+    parse_fc_vesc,
+    parse_vesc_status,
+)
+from saltybot_can_bridge.mamba_protocol import (
+    VESC_TELEM_STATE as BRIDGE_VESC_TELEM_STATE,
+    decode_vesc_state,
+)
+
+
+# ---------------------------------------------------------------------------
+# Helpers
+# ---------------------------------------------------------------------------
+
+class VescStatusAggregator:
+    """
+    Simulates the firmware logic that:
+      1. Receives VESC STATUS extended frames from left/right VESCs
+      2. Builds an FC_VESC broadcast payload
+      3. Injects the FC_VESC frame onto the mock bus
+
+    This represents the ESP32 IO → Orin telemetry path.
+    """
+
+    def __init__(self, bus: MockCANBus):
+        self._bus = bus
+        self._left_rpm_x10 = 0
+        self._right_rpm_x10 = 0
+        self._left_current_x10 = 0
+        self._right_current_x10 = 0
+        self._left_seen = False
+        self._right_seen = False
+
+    def process_vesc_status(self, arb_id: int, data: bytes) -> None:
+        """
+        Process an incoming VESC STATUS frame (extended 29-bit ID).
+        Updates internal state; broadcasts FC_VESC when at least one side is known.
+        """
+        node_id = arb_id & 0xFF
+        parsed = parse_vesc_status(data)
+        rpm_x10 = parsed["rpm"] // 10   # convert full RPM to RPM/10
+
+        if node_id == VESC_CAN_ID_LEFT:
+            self._left_rpm_x10 = rpm_x10
+            self._left_current_x10 = parsed["current_x10"]
+            self._left_seen = True
+        elif node_id == VESC_CAN_ID_RIGHT:
+            self._right_rpm_x10 = rpm_x10
+            self._right_current_x10 = parsed["current_x10"]
+            self._right_seen = True
+
+        # Broadcast FC_VESC whenever we receive any update
+        self._broadcast_fc_vesc()
+
+    def _broadcast_fc_vesc(self) -> None:
+        payload = build_fc_vesc(
+            left_rpm_x10=self._left_rpm_x10,
+            right_rpm_x10=self._right_rpm_x10,
+            left_current_x10=self._left_current_x10,
+            right_current_x10=self._right_current_x10,
+        )
+        self._bus.inject(FC_VESC, payload)
+
+
+def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int,
+                        current_x10: int = 50, duty_x1000: int = 250) -> None:
+    """Inject a VESC STATUS extended frame for the given node ID."""
+    arb_id = VESC_STATUS_ID(vesc_id)
+    payload = build_vesc_status(rpm=rpm, current_x10=current_x10, duty_x1000=duty_x1000)
+    bus.inject(arb_id, payload, is_extended_id=True)
+
+
+# ---------------------------------------------------------------------------
+# Tests
+# ---------------------------------------------------------------------------
+
+class TestVescStatusToFcVesc:
+    def test_left_vesc_status_triggers_broadcast(self, mock_can_bus):
+        """
+        Inject VESC STATUS for left VESC (ID 56) → verify FC_VESC contains
+        the correct left RPM (rpm / 10).
+        """
+        agg = VescStatusAggregator(mock_can_bus)
+
+        # Left VESC: 3000 RPM → rpm_x10 = 300
+        arb_id = VESC_STATUS_ID(VESC_CAN_ID_LEFT)
+        payload = build_vesc_status(rpm=3000, current_x10=55)
+        agg.process_vesc_status(arb_id, payload)
+
+        frame = mock_can_bus.recv(timeout=0.1)
+        assert frame is not None, "No FC_VESC broadcast after left VESC STATUS"
+        parsed = parse_fc_vesc(bytes(frame.data))
+        assert parsed["left_rpm_x10"] == 300, \
+            f"left_rpm_x10 {parsed['left_rpm_x10']} != 300"
+        assert abs(parsed["left_rpm"] - 3000.0) < 1.0
+
+    def test_right_vesc_status_triggers_broadcast(self, mock_can_bus):
+        """Inject VESC STATUS for right VESC (ID 68) → verify right RPM in FC_VESC."""
+        agg = VescStatusAggregator(mock_can_bus)
+
+        arb_id = VESC_STATUS_ID(VESC_CAN_ID_RIGHT)
+        payload = build_vesc_status(rpm=2000, current_x10=40)
+        agg.process_vesc_status(arb_id, payload)
+
+        frame = mock_can_bus.recv(timeout=0.1)
+        assert frame is not None
+        parsed = parse_fc_vesc(bytes(frame.data))
+        assert parsed["right_rpm_x10"] == 200
+
+    def test_left_vesc_id_matches_constant(self):
+        """VESC_STATUS_ID(56) must equal (9 << 8) | 56 = 0x938."""
+        assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == (9 << 8) | 56
+        assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == 0x938
+
+    def test_right_vesc_id_matches_constant(self):
+        """VESC_STATUS_ID(68) must equal (9 << 8) | 68 = 0x944."""
+        assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == (9 << 8) | 68
+        assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == 0x944
+
+
+class TestBothVescStatusCombined:
+    def test_both_vesc_status_combined_in_fc_vesc(self, mock_can_bus):
+        """
+        Inject both left (56) and right (68) VESC STATUS frames.
+        Final FC_VESC must contain both RPMs.
+        """
+        agg = VescStatusAggregator(mock_can_bus)
+
+        # Left: 3000 RPM
+        agg.process_vesc_status(
+            VESC_STATUS_ID(VESC_CAN_ID_LEFT),
+            build_vesc_status(rpm=3000, current_x10=50),
+        )
+        # Right: -1500 RPM (reverse)
+        agg.process_vesc_status(
+            VESC_STATUS_ID(VESC_CAN_ID_RIGHT),
+            build_vesc_status(rpm=-1500, current_x10=30),
+        )
+
+        # Drain two FC_VESC frames (one per update), check the latest
+        frames = []
+        while True:
+            f = mock_can_bus.recv(timeout=0.05)
+            if f is None:
+                break
+            frames.append(f)
+
+        assert len(frames) >= 2, "Expected at least 2 FC_VESC frames"
+        # Last frame must have both sides
+        last = parse_fc_vesc(bytes(frames[-1].data))
+        assert last["left_rpm_x10"] == 300, \
+            f"left_rpm_x10 {last['left_rpm_x10']} != 300"
+        assert last["right_rpm_x10"] == -150, \
+            f"right_rpm_x10 {last['right_rpm_x10']} != -150"
+
+    def test_both_vesc_currents_combined(self, mock_can_bus):
+        """Both current values must appear in FC_VESC after two STATUS frames."""
+        agg = VescStatusAggregator(mock_can_bus)
+        agg.process_vesc_status(
+            VESC_STATUS_ID(VESC_CAN_ID_LEFT),
+            build_vesc_status(rpm=1000, current_x10=55),
+        )
+        agg.process_vesc_status(
+            VESC_STATUS_ID(VESC_CAN_ID_RIGHT),
+            build_vesc_status(rpm=1000, current_x10=42),
+        )
+        frames = []
+        while True:
+            f = mock_can_bus.recv(timeout=0.05)
+            if f is None:
+                break
+            frames.append(f)
+        last = parse_fc_vesc(bytes(frames[-1].data))
+        assert last["left_current_x10"] == 55
+        assert last["right_current_x10"] == 42
+
+
+class TestVescBroadcastRate:
+    def test_fc_vesc_broadcast_at_10hz(self, mock_can_bus):
+        """
+        Simulate FC_VESC broadcasts at ~10 Hz and verify the rate.
+        We inject 12 frames over ~120 ms, then verify count and average interval.
+        """
+        _FC_VESC_HZ = 10
+        _interval = 1.0 / _FC_VESC_HZ
+
+        timestamps = []
+        stop_event = threading.Event()
+
+        def broadcaster():
+            while not stop_event.is_set():
+                t = time.monotonic()
+                mock_can_bus.inject(
+                    FC_VESC,
+                    build_fc_vesc(100, -100, 30, 30),
+                    timestamp=t,
+                )
+                timestamps.append(t)
+                time.sleep(_interval)
+
+        t = threading.Thread(target=broadcaster, daemon=True)
+        t.start()
+        time.sleep(0.15)   # collect ~1.5 broadcasts
+        stop_event.set()
+        t.join(timeout=0.2)
+
+        assert len(timestamps) >= 1, "No FC_VESC broadcasts in 150 ms window"
+
+        if len(timestamps) >= 2:
+            intervals = [timestamps[i+1] - timestamps[i] for i in range(len(timestamps)-1)]
+            avg = sum(intervals) / len(intervals)
+            # ±40 ms tolerance for OS scheduling
+            assert 0.06 <= avg <= 0.14, \
+                f"FC_VESC broadcast interval {avg*1000:.1f} ms not ~100 ms"
+
+    def test_fc_vesc_frame_is_8_bytes(self):
+        """FC_VESC payload must always be exactly 8 bytes."""
+        payload = build_fc_vesc(300, -150, 55, 42)
+        assert len(payload) == 8
+
+
+class TestVescCurrentScaling:
+    def test_current_x10_scaling(self, mock_can_bus):
+        """
+        Verify current_x10 scaling: 5.5 A → current_x10=55.
+        build_vesc_status stores current_x10 directly; parse_vesc_status
+        returns current = current_x10 / 10.
+        """
+        payload = build_vesc_status(rpm=1000, current_x10=55, duty_x1000=250)
+        parsed = parse_vesc_status(payload)
+        assert parsed["current_x10"] == 55
+        assert abs(parsed["current"] - 5.5) < 0.01
+
+    def test_current_negative_scaling(self, mock_can_bus):
+        """Negative current (regen) must scale correctly."""
+        payload = build_vesc_status(rpm=-500, current_x10=-30)
+        parsed = parse_vesc_status(payload)
+        assert parsed["current_x10"] == -30
+        assert abs(parsed["current"] - (-3.0)) < 0.01
+
+    def test_fc_vesc_current_x10_roundtrip(self, mock_can_bus):
+        """build_fc_vesc → inject → recv → parse must preserve current_x10."""
+        payload = build_fc_vesc(
+            left_rpm_x10=200,
+            right_rpm_x10=200,
+            left_current_x10=55,
+            right_current_x10=42,
+        )
+        mock_can_bus.inject(FC_VESC, payload)
+        frame = mock_can_bus.recv(timeout=0.1)
+        parsed = parse_fc_vesc(bytes(frame.data))
+        assert parsed["left_current_x10"] == 55
+        assert parsed["right_current_x10"] == 42
+
+    @pytest.mark.parametrize("vesc_id", [VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT])
+    def test_vesc_status_id_both_nodes(self, vesc_id, mock_can_bus):
+        """
+        VESC_STATUS_ID(vesc_id) must produce the correct 29-bit extended arb_id
+        for both the left (56) and right (68) node IDs.
+        """
+        expected = (9 << 8) | vesc_id
+        assert VESC_STATUS_ID(vesc_id) == expected
+
+    @pytest.mark.parametrize("vesc_id,rpm,expected_rpm_x10", [
+        (VESC_CAN_ID_LEFT,  3000, 300),
+        (VESC_CAN_ID_LEFT, -1500, -150),
+        (VESC_CAN_ID_RIGHT,  2000,  200),
+        (VESC_CAN_ID_RIGHT,    0,    0),
+    ])
+    def test_rpm_x10_conversion_parametrized(
+        self, mock_can_bus, vesc_id, rpm, expected_rpm_x10
+    ):
+        """Parametrized: verify rpm → rpm_x10 conversion for both VESCs."""
+        agg = VescStatusAggregator(mock_can_bus)
+        agg.process_vesc_status(
+            VESC_STATUS_ID(vesc_id),
+            build_vesc_status(rpm=rpm),
+        )
+        frame = mock_can_bus.recv(timeout=0.05)
+        assert frame is not None
+        parsed = parse_fc_vesc(bytes(frame.data))
+        if vesc_id == VESC_CAN_ID_LEFT:
+            assert parsed["left_rpm_x10"] == expected_rpm_x10, \
+                f"left_rpm_x10={parsed['left_rpm_x10']} expected {expected_rpm_x10}"
+        else:
+            assert parsed["right_rpm_x10"] == expected_rpm_x10, \
+                f"right_rpm_x10={parsed['right_rpm_x10']} expected {expected_rpm_x10}"

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py

@@ -0,0 +1,238 @@
+#!/usr/bin/env python3
+"""
+test_heartbeat_timeout.py — Tests for heartbeat loss and recovery.
+
+Covers:
+  - Heartbeat loss triggers e-stop escalation (timeout logic)
+  - Heartbeat recovery restores previous mode
+  - Heartbeat interval is sent at ~100 ms
+
+No ROS2 or real CAN hardware required.
+Run with: python -m pytest test/test_heartbeat_timeout.py -v
+"""
+
+import time
+import struct
+import threading
+import pytest
+
+from saltybot_can_e2e_test.can_mock import MockCANBus
+from saltybot_can_e2e_test.protocol_defs import (
+    ORIN_CMD_HEARTBEAT,
+    ORIN_CMD_ESTOP,
+    ORIN_CMD_MODE,
+    MAMBA_CMD_VELOCITY,
+    MAMBA_CMD_MODE,
+    MAMBA_CMD_ESTOP,
+    MODE_IDLE,
+    MODE_DRIVE,
+    MODE_ESTOP,
+    build_heartbeat,
+    build_estop_cmd,
+    build_mode_cmd,
+    build_velocity_cmd,
+    parse_velocity_cmd,
+)
+from saltybot_can_bridge.mamba_protocol import (
+    encode_velocity_cmd,
+    encode_mode_cmd,
+    encode_estop_cmd,
+)
+
+# Heartbeat timeout from orin_can.h: 500 ms
+ORIN_HB_TIMEOUT_MS = 500
+ORIN_HB_TIMEOUT_S  = ORIN_HB_TIMEOUT_MS / 1000.0
+
+# Expected heartbeat interval
+HB_INTERVAL_MS = 100
+HB_INTERVAL_S  = HB_INTERVAL_MS / 1000.0
+
+
+# ---------------------------------------------------------------------------
+# Helpers
+# ---------------------------------------------------------------------------
+
+class HeartbeatSimulator:
+    """
+    Simulate periodic heartbeat injection into a MockCANBus.
+
+    Call start() to begin sending heartbeats every interval_s.
+    Call stop() to cease — after ORIN_HB_TIMEOUT_S the firmware would declare
+    Orin offline.
+    """
+
+    def __init__(self, bus: MockCANBus, interval_s: float = HB_INTERVAL_S):
+        self._bus = bus
+        self._interval_s = interval_s
+        self._seq = 0
+        self._running = False
+        self._thread: threading.Thread | None = None
+
+    def start(self):
+        self._running = True
+        self._thread = threading.Thread(target=self._run, daemon=True)
+        self._thread.start()
+
+    def stop(self):
+        self._running = False
+
+    def _run(self):
+        while self._running:
+            self._bus.inject(
+                ORIN_CMD_HEARTBEAT,
+                build_heartbeat(self._seq),
+                is_extended_id=False,
+            )
+            self._seq += 1
+            time.sleep(self._interval_s)
+
+
+def _simulate_estop_on_timeout(bus: MockCANBus) -> None:
+    """
+    Simulate the firmware-side logic: when heartbeat timeout expires,
+    the FC sends an e-stop command by setting estop mode on the ESP32 IO bus.
+    We model this as the bridge sending zero velocity + ESTOP mode.
+    """
+
+    class _Msg:
+        def __init__(self, arb_id, data):
+            self.arbitration_id = arb_id
+            self.data = bytearray(data)
+            self.is_extended_id = False
+
+    bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
+    bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP)))
+    bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
+
+
+# ---------------------------------------------------------------------------
+# Tests
+# ---------------------------------------------------------------------------
+
+class TestHeartbeatLoss:
+    def test_heartbeat_loss_triggers_estop(self, mock_can_bus):
+        """
+        After heartbeat ceases, the bridge must command zero velocity and
+        set ESTOP mode.  We simulate this directly using _simulate_estop_on_timeout.
+        """
+        # First confirm the bus is clean
+        assert len(mock_can_bus.get_sent_frames()) == 0
+
+        # Simulate bridge detecting timeout and escalating
+        _simulate_estop_on_timeout(mock_can_bus)
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) >= 1, "Zero velocity not sent after timeout"
+        l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
+        assert abs(l) < 1e-5, "Left not zero on timeout"
+        assert abs(r) < 1e-5, "Right not zero on timeout"
+
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        assert any(
+            bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames
+        ), "ESTOP mode not asserted on heartbeat timeout"
+
+        estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
+        assert len(estop_frames) >= 1, "ESTOP command not sent"
+        assert bytes(estop_frames[0].data) == b"\x01"
+
+    def test_heartbeat_loss_zero_velocity(self, mock_can_bus):
+        """Zero velocity frame must appear among sent frames after timeout."""
+        _simulate_estop_on_timeout(mock_can_bus)
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) >= 1
+        for f in vel_frames:
+            l, r = parse_velocity_cmd(bytes(f.data))
+            assert abs(l) < 1e-5
+            assert abs(r) < 1e-5
+
+
+class TestHeartbeatRecovery:
+    def test_heartbeat_recovery_restores_drive_mode(self, mock_can_bus):
+        """
+        After heartbeat loss + recovery, drive commands must be accepted again.
+        We simulate: ESTOP → clear estop → send drive → verify velocity frame.
+        """
+
+        class _Msg:
+            def __init__(self, arb_id, data):
+                self.arbitration_id = arb_id
+                self.data = bytearray(data)
+                self.is_extended_id = False
+
+        # Phase 1: timeout → estop
+        _simulate_estop_on_timeout(mock_can_bus)
+        mock_can_bus.reset()
+
+        # Phase 2: recovery — clear estop, restore drive mode
+        mock_can_bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False)))
+        mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE)))
+        mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.5, 0.5)))
+
+        estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP)
+        assert any(bytes(f.data) == b"\x00" for f in estop_frames), \
+            "ESTOP clear not sent on recovery"
+
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        assert any(
+            bytes(f.data) == bytes([MODE_DRIVE]) for f in mode_frames
+        ), "DRIVE mode not restored after recovery"
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) >= 1
+        l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
+        assert abs(l - 0.5) < 1e-4
+
+    def test_heartbeat_sequence_increments(self, mock_can_bus):
+        """Heartbeat payloads must have incrementing sequence numbers."""
+        payloads = []
+        for seq in range(5):
+            mock_can_bus.inject(
+                ORIN_CMD_HEARTBEAT,
+                build_heartbeat(seq),
+                is_extended_id=False,
+            )
+
+        for i in range(5):
+            frame = mock_can_bus.recv(timeout=0.05)
+            assert frame is not None
+            (seq_val,) = struct.unpack(">I", bytes(frame.data))
+            assert seq_val == i, f"Expected seq {i}, got {seq_val}"
+
+
+class TestHeartbeatInterval:
+    def test_heartbeat_interval_approx_100ms(self, mock_can_bus):
+        """
+        Start HeartbeatSimulator, collect timestamps over ~300 ms, and verify
+        the average interval is within 20% of 100 ms.
+        """
+        sim = HeartbeatSimulator(mock_can_bus, interval_s=0.1)
+        sim.start()
+        time.sleep(0.35)
+        sim.stop()
+
+        timestamps = []
+        while True:
+            frame = mock_can_bus.recv(timeout=0.01)
+            if frame is None:
+                break
+            if frame.arbitration_id == ORIN_CMD_HEARTBEAT:
+                timestamps.append(frame.timestamp)
+
+        assert len(timestamps) >= 2, "Not enough heartbeat frames captured"
+
+        intervals = [
+            timestamps[i + 1] - timestamps[i]
+            for i in range(len(timestamps) - 1)
+        ]
+        avg_interval = sum(intervals) / len(intervals)
+        # Allow ±30 ms tolerance (OS scheduling jitter in CI)
+        assert 0.07 <= avg_interval <= 0.13, \
+            f"Average heartbeat interval {avg_interval*1000:.1f} ms not ~100 ms"
+
+    def test_heartbeat_payload_is_4_bytes(self, mock_can_bus):
+        """Heartbeat payload must be exactly 4 bytes (uint32 sequence)."""
+        for seq in (0, 1, 0xFFFFFFFF):
+            payload = build_heartbeat(seq)
+            assert len(payload) == 4, \
+                f"Heartbeat payload length {len(payload)} != 4"

jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py

@@ -0,0 +1,236 @@
+#!/usr/bin/env python3
+"""
+test_mode_switching.py — Mode transition integration tests.
+
+Covers:
+  - idle → drive: drive commands become accepted
+  - drive → estop: immediate motor stop
+  - MODE frame byte values match protocol constants
+  - Unknown mode byte is ignored (no state change)
+
+No ROS2 or real CAN hardware required.
+Run with: python -m pytest test/test_mode_switching.py -v
+"""
+
+import struct
+import pytest
+
+from saltybot_can_e2e_test.can_mock import MockCANBus
+from saltybot_can_e2e_test.protocol_defs import (
+    MAMBA_CMD_VELOCITY,
+    MAMBA_CMD_MODE,
+    MAMBA_CMD_ESTOP,
+    MODE_IDLE,
+    MODE_DRIVE,
+    MODE_ESTOP,
+    build_mode_cmd,
+    build_velocity_cmd,
+    parse_velocity_cmd,
+)
+from saltybot_can_bridge.mamba_protocol import (
+    encode_velocity_cmd,
+    encode_mode_cmd,
+    encode_estop_cmd,
+)
+
+
+# ---------------------------------------------------------------------------
+# Helpers
+# ---------------------------------------------------------------------------
+
+class _Msg:
+    def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False):
+        self.arbitration_id = arb_id
+        self.data = bytearray(data)
+        self.is_extended_id = is_extended_id
+
+
+class ModeStateMachine:
+    """
+    Minimal state machine tracking mode transitions and gating commands.
+    """
+
+    def __init__(self, bus: MockCANBus):
+        self._bus = bus
+        self._mode = MODE_IDLE
+
+    def set_mode(self, mode: int) -> bool:
+        """
+        Transition to mode.  Returns True if accepted, False if invalid.
+        Invalid mode values (not 0, 1, 2) are ignored.
+        """
+        if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
+            return False  # silently ignore
+
+        prev_mode = self._mode
+        self._mode = mode
+        self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(mode)))
+
+        # Side-effects of entering ESTOP from DRIVE
+        if mode == MODE_ESTOP and prev_mode == MODE_DRIVE:
+            self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0)))
+            self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True)))
+
+        return True
+
+    def send_drive(self, left_mps: float, right_mps: float) -> bool:
+        """
+        Send a velocity command.  Returns True if forwarded, False if blocked.
+        """
+        if self._mode != MODE_DRIVE:
+            return False
+        self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps)))
+        return True
+
+    @property
+    def mode(self) -> int:
+        return self._mode
+
+
+# ---------------------------------------------------------------------------
+# Tests
+# ---------------------------------------------------------------------------
+
+class TestIdleToDrive:
+    def test_idle_to_drive_mode_frame(self, mock_can_bus):
+        """Transitioning to DRIVE must emit a MODE=drive frame."""
+        sm = ModeStateMachine(mock_can_bus)
+        sm.set_mode(MODE_DRIVE)
+
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        assert len(mode_frames) == 1
+        assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE])
+
+    def test_idle_blocks_drive_commands(self, mock_can_bus):
+        """In IDLE mode, drive commands must be suppressed."""
+        sm = ModeStateMachine(mock_can_bus)
+        # Attempt drive without entering DRIVE mode
+        forwarded = sm.send_drive(1.0, 1.0)
+        assert forwarded is False, "Drive cmd should be blocked in IDLE mode"
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 0
+
+    def test_drive_mode_allows_commands(self, mock_can_bus):
+        """After entering DRIVE mode, velocity commands must be forwarded."""
+        sm = ModeStateMachine(mock_can_bus)
+        sm.set_mode(MODE_DRIVE)
+        mock_can_bus.reset()
+
+        forwarded = sm.send_drive(0.5, 0.5)
+        assert forwarded is True
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 1
+        l, r = parse_velocity_cmd(bytes(vel_frames[0].data))
+        assert abs(l - 0.5) < 1e-4
+        assert abs(r - 0.5) < 1e-4
+
+
+class TestDriveToEstop:
+    def test_drive_to_estop_stops_motors(self, mock_can_bus):
+        """Transitioning DRIVE → ESTOP must immediately send zero velocity."""
+        sm = ModeStateMachine(mock_can_bus)
+        sm.set_mode(MODE_DRIVE)
+        sm.send_drive(1.0, 1.0)
+        mock_can_bus.reset()
+
+        sm.set_mode(MODE_ESTOP)
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) >= 1, "No velocity frame on DRIVE→ESTOP transition"
+        l, r = parse_velocity_cmd(bytes(vel_frames[-1].data))
+        assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP"
+        assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP"
+
+    def test_drive_to_estop_mode_frame(self, mock_can_bus):
+        """DRIVE → ESTOP must broadcast MODE=estop."""
+        sm = ModeStateMachine(mock_can_bus)
+        sm.set_mode(MODE_DRIVE)
+        sm.set_mode(MODE_ESTOP)
+
+        mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE)
+        assert any(bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames)
+
+    def test_estop_blocks_subsequent_drive(self, mock_can_bus):
+        """After DRIVE → ESTOP, drive commands must be blocked."""
+        sm = ModeStateMachine(mock_can_bus)
+        sm.set_mode(MODE_DRIVE)
+        sm.set_mode(MODE_ESTOP)
+        mock_can_bus.reset()
+
+        forwarded = sm.send_drive(1.0, 1.0)
+        assert forwarded is False
+
+        vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY)
+        assert len(vel_frames) == 0
+
+
+class TestModeCommandEncoding:
+    def test_mode_idle_byte(self, mock_can_bus):
+        """MODE_IDLE must encode as 0x00."""
+        assert encode_mode_cmd(MODE_IDLE) == b"\x00"
+
+    def test_mode_drive_byte(self, mock_can_bus):
+        """MODE_DRIVE must encode as 0x01."""
+        assert encode_mode_cmd(MODE_DRIVE) == b"\x01"
+
+    def test_mode_estop_byte(self, mock_can_bus):
+        """MODE_ESTOP must encode as 0x02."""
+        assert encode_mode_cmd(MODE_ESTOP) == b"\x02"
+
+    def test_mode_frame_length(self, mock_can_bus):
+        """Mode command payload must be exactly 1 byte."""
+        for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
+            payload = encode_mode_cmd(mode)
+            assert len(payload) == 1, f"Mode {mode} payload length {len(payload)} != 1"
+
+    def test_protocol_defs_build_mode_cmd_matches(self):
+        """build_mode_cmd in protocol_defs must produce identical bytes."""
+        for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
+            assert build_mode_cmd(mode) == encode_mode_cmd(mode), \
+                f"protocol_defs.build_mode_cmd({mode}) != mamba_protocol.encode_mode_cmd({mode})"
+
+
+class TestInvalidMode:
+    def test_invalid_mode_byte_ignored(self, mock_can_bus):
+        """Unknown mode byte (e.g. 0xFF) must be rejected — no state change."""
+        sm = ModeStateMachine(mock_can_bus)
+        sm.set_mode(MODE_DRIVE)
+        initial_mode = sm.mode
+        mock_can_bus.reset()
+
+        accepted = sm.set_mode(0xFF)
+        assert accepted is False, "Invalid mode 0xFF should be rejected"
+        assert sm.mode == initial_mode, "Mode changed despite invalid value"
+        assert len(mock_can_bus.get_sent_frames()) == 0, \
+            "Frames sent for invalid mode command"
+
+    def test_invalid_mode_99_ignored(self, mock_can_bus):
+        """Mode 99 must be rejected."""
+        sm = ModeStateMachine(mock_can_bus)
+        accepted = sm.set_mode(99)
+        assert accepted is False
+
+    def test_invalid_mode_negative_ignored(self, mock_can_bus):
+        """Negative mode values must be rejected."""
+        sm = ModeStateMachine(mock_can_bus)
+        accepted = sm.set_mode(-1)
+        assert accepted is False
+
+    def test_mamba_protocol_invalid_mode_raises(self):
+        """mamba_protocol.encode_mode_cmd must raise on invalid mode."""
+        with pytest.raises(ValueError):
+            encode_mode_cmd(99)
+        with pytest.raises(ValueError):
+            encode_mode_cmd(-1)
+
+
+@pytest.mark.parametrize("mode,expected_byte", [
+    (MODE_IDLE,  b"\x00"),
+    (MODE_DRIVE, b"\x01"),
+    (MODE_ESTOP, b"\x02"),
+])
+def test_mode_encoding_parametrized(mode, expected_byte):
+    """Parametrized check that all mode constants encode to the right byte."""
+    assert encode_mode_cmd(mode) == expected_byte

jetson/ros2_ws/src/saltybot_description/config/saltybot_properties.yaml

@@ -27,7 +27,7 @@ robot:
   stem_od:     0.0381          # m  STEM_OD = 38.1mm
   stem_height: 1.050           # m  nominal cut length
 
-  # ── FC / IMU (MAMBA F722S) ──────────────────────────────────────────────────
+  # ── FC / IMU (ESP32 BALANCE) ──────────────────────────────────────────────────
   # fc_x = -50mm in SCAD (front = -X SCAD = +X ROS REP-105)
   # z = deck_thickness/2 + mounting_pad(3mm) + standoff(6mm) = 12mm
   imu_x:  0.050                # m  forward of base_link center

jetson/ros2_ws/src/saltybot_diagnostics/README.md

@@ -5,7 +5,7 @@ Comprehensive hardware diagnostics and health monitoring for SaltyBot.
 ## Features
 
 ### Startup Checks
-- RPLIDAR, RealSense, VESC, Jabra mic, STM32, servos
+- RPLIDAR, RealSense, VESC, Jabra mic, ESP32 BALANCE, servos
 - WiFi, GPS, disk space, RAM
 - Boot result TTS + face animation
 - JSON logging

jetson/ros2_ws/src/saltybot_diagnostics/config/diagnostic_checks.yaml

@@ -6,7 +6,7 @@ startup_checks:
     - realsense
     - vesc
     - jabra_microphone
-    - stm32_bridge
+    - esp32_bridge
     - servos
     - wifi
     - gps

jetson/ros2_ws/src/saltybot_diagnostics/saltybot_diagnostics/diagnostics_node.py

@@ -138,7 +138,7 @@ class DiagnosticsNode(Node):
             self.hardware_checks["jabra"] = ("WARN", "Audio check failed", {})
 
     def _check_stm32(self):
-        self.hardware_checks["stm32"] = ("OK", "STM32 bridge online", {})
+        self.hardware_checks["stm32"] = ("OK", "ESP32 bridge online", {})
 
     def _check_servos(self):
         try:

jetson/ros2_ws/src/saltybot_follower/config/person_follower_params.yaml

@@ -7,7 +7,7 @@
 #   ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.2
 #
 # IMPORTANT: This node publishes raw /cmd_vel. The cmd_vel_bridge_node (PR #46)
-# applies the ESC ramp, deadman switch, and STM32 AUTONOMOUS mode gate.
+# applies the ESC ramp, deadman switch, and ESP32 BALANCE AUTONOMOUS mode gate.
 # Do not run this node without the cmd_vel bridge running on the same robot.
 
 # ── Follow geometry ────────────────────────────────────────────────────────────
@@ -70,5 +70,5 @@ control_rate:     20.0        # Hz — lower than cmd_vel bridge (50Hz) by desig
 # ── Mode integration ──────────────────────────────────────────────────────────
 # Master enable for the follow controller. When false, node publishes zero cmd_vel.
 # Toggle at runtime: ros2 param set /person_follower follow_enabled false
-# The cmd_vel bridge independently gates on STM32 AUTONOMOUS mode (md=2).
+# The cmd_vel bridge independently gates on ESP32 BALANCE AUTONOMOUS mode (md=2).
 follow_enabled:   true

jetson/ros2_ws/src/saltybot_follower/saltybot_follower/person_follower_node.py

@@ -28,7 +28,7 @@ State machine
 
 Safety wiring
 -------------
-  * cmd_vel bridge (PR #46) applies ramp + deadman + STM32 AUTONOMOUS mode gate --
+  * cmd_vel bridge (PR #46) applies ramp + deadman + ESP32 BALANCE AUTONOMOUS mode gate --
     this node publishes raw /cmd_vel, the bridge handles hardware safety.
   * follow_enabled param (default True) lets the operator disable the controller
     at runtime: ros2 param set /person_follower follow_enabled false

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

@@ -1,6 +1,6 @@
 gimbal_node:
   ros__parameters:
-    # Serial port connecting to STM32 over JLINK protocol
+    # Serial port connecting to ESP32 BALANCE over JLINK protocol
     serial_port: "/dev/ttyTHS1"
     baud_rate: 921600
 

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

@@ -14,7 +14,7 @@ def generate_launch_description() -> LaunchDescription:
     serial_port_arg = DeclareLaunchArgument(
         "serial_port",
         default_value="/dev/ttyTHS1",
-        description="JLINK serial port to STM32",
+        description="JLINK serial port to ESP32 BALANCE",
     )
     pan_limit_arg = DeclareLaunchArgument(
         "pan_limit_deg",

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

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 """gimbal_node.py — ROS2 gimbal control node for SaltyBot pan/tilt camera head (Issue #548).
 
-Controls pan/tilt gimbal via JLINK binary protocol over serial to STM32.
+Controls pan/tilt gimbal via JLINK binary protocol over serial to ESP32 BALANCE.
 Implements smooth trapezoidal motion profiles with configurable axis limits.
 
 Subscribed topics:

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

@@ -1,14 +1,14 @@
 """jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548).
 
-Matches the JLINK protocol defined in include/jlink.h (Issue #547 STM32 side).
+Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32 side).
 
-Command type (Jetson → STM32):
+Command type (Jetson → ESP32 BALANCE):
   0x0B  GIMBAL_POS  — int16 pan_x10 + int16 tilt_x10 + uint16 speed   (6 bytes)
                        pan_x10  = pan_deg  * 10  (±1500 for ±150°)
                        tilt_x10 = tilt_deg * 10  (±450  for ±45°)
                        speed    = servo speed register 0–4095 (0 = max)
 
-Telemetry type (STM32 → Jetson):
+Telemetry type (ESP32 BALANCE → Jetson):
   0x84  GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 +
                         uint16 pan_speed_raw + uint16 tilt_speed_raw +
                         uint8 torque_en + uint8 rx_err_pct             (10 bytes)
@@ -31,8 +31,8 @@ ETX = 0x03
 
 # ── Command / telemetry type codes ─────────────────────────────────────────────
 
-CMD_GIMBAL_POS   = 0x0B   # Jetson → STM32: set pan/tilt target
-TLM_GIMBAL_STATE = 0x84   # STM32 → Jetson: measured state
+CMD_GIMBAL_POS   = 0x0B   # Jetson → ESP32 BALANCE: set pan/tilt target
+TLM_GIMBAL_STATE = 0x84   # ESP32 BALANCE → Jetson: measured state
 
 # Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed.
 # Map deg/s to this register: speed_reg = max(0, 4095 - int(deg_s * 4095 / 360))

jetson/ros2_ws/src/saltybot_mode_switch/config/mode_switch_params.yaml

@@ -5,7 +5,7 @@
 #
 # Topic wiring:
 #   /rc/joy                          → mode_switch_node (CRSF channels)
-#   /saltybot/balance_state          → mode_switch_node (STM32 state)
+#   /saltybot/balance_state          → mode_switch_node (ESP32 BALANCE state)
 #   /slam_toolbox/pose_with_covariance_stamped → mode_switch_node (SLAM fix)
 #   /saltybot/control_mode           ← mode_switch_node (JSON mode + alpha)
 #   /saltybot/led_pattern            ← mode_switch_node (LED name)

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/cmd_vel_mux_node.py

@@ -13,7 +13,7 @@ Topic graph
 
 In RC mode (blend_alpha ≈ 0) the node publishes Twist(0,0) so the bridge
 receives zeros — this is harmless because the bridge's mode gate already
-prevents autonomous commands when the STM32 is in RC_MANUAL.
+prevents autonomous commands when the ESP32 BALANCE is in RC_MANUAL.
 
 The bridge's existing ESC ramp handles hardware-level smoothing;
 the blend_alpha here provides the higher-level cmd_vel policy ramp.

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_logic.py

@@ -6,9 +6,9 @@ state machine can be exercised in unit tests without a ROS2 runtime.
 
 Mode vocabulary
 ---------------
-  "RC"            — STM32 executing RC pilot commands; Jetson cmd_vel blocked.
+  "RC"            — ESP32 BALANCE executing RC pilot commands; Jetson cmd_vel blocked.
   "RAMP_TO_AUTO"  — Transitioning RC→AUTO; blend_alpha 0.0→1.0 over ramp_s.
-  "AUTO"          — STM32 executing Jetson cmd_vel; RC sticks idle.
+  "AUTO"          — ESP32 BALANCE executing Jetson cmd_vel; RC sticks idle.
   "RAMP_TO_RC"    — Transitioning AUTO→RC; blend_alpha 1.0→0.0 over ramp_s.
 
 Blend alpha

jetson/ros2_ws/src/saltybot_mode_switch/saltybot_mode_switch/mode_switch_node.py

@@ -9,7 +9,7 @@ Inputs
         axes[stick_axes...]   Roll/Pitch/Throttle/Yaw — override detection
 
   /saltybot/balance_state  (std_msgs/String JSON)
-      Parsed for RC link health (field "rc_link") and STM32 mode.
+      Parsed for RC link health (field "rc_link") and ESP32 BALANCE mode.
 
   <slam_fix_topic>         (geometry_msgs/PoseWithCovarianceStamped)
       Any message received within slam_fix_timeout_s → SLAM fix valid.
@@ -106,7 +106,7 @@ class ModeSwitchNode(Node):
         self._last_joy_t: float   = 0.0      # monotonic; 0 = never
         self._last_slam_t: float  = 0.0
         self._joy_axes: list      = []
-        self._stm32_mode: int     = 0        # from balance_state JSON
+        self._esp32_mode: int     = 0        # from balance_state JSON
 
         # ── QoS ───────────────────────────────────────────────────────────────
         best_effort = QoSProfile(
@@ -187,7 +187,7 @@ class ModeSwitchNode(Node):
             data = json.loads(msg.data)
             # "mode" is a label string; map back to int for reference
             mode_label = data.get("mode", "RC_MANUAL")
-            self._stm32_mode = {"RC_MANUAL": 0, "RC_ASSISTED": 1,
+            self._esp32_mode = {"RC_MANUAL": 0, "RC_ASSISTED": 1,
                                 "AUTONOMOUS": 2}.get(mode_label, 0)
         except (json.JSONDecodeError, TypeError):
             pass

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

@@ -0,0 +1,22 @@
+# map_saver_params.yaml — nav2_map_server map_saver_server config (Issue #696)
+#
+# map_saver_server is a lifecycle node from nav2_map_server that exposes the
+# /map_saver/save_map action (nav2_msgs/action/SaveMap).  It is included in
+# the SLAM bringup so operators can trigger a map save without restarting.
+#
+# Save a map (while SLAM is running):
+#   ros2 run nav2_map_server map_saver_cli \
+#       -f /mnt/nvme/saltybot/maps/saltybot_map --map_subscribe_transient_local
+#
+# Or via action:
+#   ros2 action send_goal /map_saver/save_map nav2_msgs/action/SaveMap \
+#       '{map_topic: "map", map_url: "/mnt/nvme/saltybot/maps/saltybot_map",
+#         image_format: "pgm", map_mode: "trinary",
+#         free_thresh: 0.25, occupied_thresh: 0.65}'
+
+map_saver:
+  ros__parameters:
+    save_map_timeout:          5.0    # s — time to wait for /map topic on save
+    free_thresh_default:       0.25   # p ≤ 0.25 → free cell
+    occupied_thresh_default:   0.65   # p ≥ 0.65 → occupied cell
+    map_subscribe_transient_local: true  # use transient-local QoS (required for /map)