docs: Update chassis docs for ESP32 architecture (retire Mamba F722S)

Replace Mamba F722S / STM32F722 references in BOM.md and ASSEMBLY.md
with ESP32 BALANCE + ESP32 IO. Board dimensions marked TBD pending
spec from max.

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

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):** Mamba F722S 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):** Mamba F722S (STM32F722) 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 Mamba F722S |
+| 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/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();
+}

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/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/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_can_bridge/saltybot_can_bridge/can_bridge_node.py

@@ -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,
 )
 
@@ -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:
@@ -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

@@ -38,6 +38,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
@@ -82,6 +84,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 +152,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 +216,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_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↔Mamba↔VESC integration test suite.
+
+All IDs and payload formats are derived from:
+  include/orin_can.h   — Orin↔FC (Mamba) protocol
+  include/vesc_can.h   — VESC CAN protocol
+  saltybot_can_bridge/mamba_protocol.py — existing bridge constants
+
+CAN IDs used in tests
+---------------------
+Orin → FC (Mamba) 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 (Mamba) → 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
+
+Mamba ↔ 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 (Mamba) 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 (Mamba) → Orin telemetry IDs  (from orin_can.h)
+# ---------------------------------------------------------------------------
+
+FC_STATUS: int = 0x400
+FC_VESC: int   = 0x401
+FC_IMU: int    = 0x402
+FC_BARO: int   = 0x403
+
+# ---------------------------------------------------------------------------
+# Mamba → 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 — Mamba 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↔Mamba↔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 → Mamba 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 Mamba 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 Mamba 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 Mamba → 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 Mamba 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_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)

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

@@ -0,0 +1,143 @@
+"""
+map_persistence.launch.py — Map save/load lifecycle nodes (Issue #696).
+
+Launches:
+  1. map_saver_server  — nav2_map_server lifecycle node; exposes
+                         /map_saver/save_map action (nav2_msgs/action/SaveMap)
+  2. saltybot_map_saver — helper node: /saltybot/save_map Trigger service,
+                          startup check, optional auto-save on shutdown
+  3. lifecycle_manager_map_saver — manages map_saver_server transitions
+
+Arguments
+─────────
+  map_dir      path to map storage directory
+               (default: /mnt/nvme/saltybot/maps)
+  map_name     base filename for saved map, no extension
+               (default: saltybot_map)
+  use_sim_time (default: false)
+  autostart    lifecycle manager autostart (default: true)
+
+Save a map (CLI shortcut while SLAM is running):
+  ros2 service call /saltybot/save_map std_srvs/srv/Trigger {}
+
+Or directly via map_saver_cli:
+  ros2 run nav2_map_server map_saver_cli \\
+      -f /mnt/nvme/saltybot/maps/saltybot_map --map_subscribe_transient_local
+
+Load on next startup:
+  ros2 launch saltybot_nav2_slam nav2_amcl_bringup.launch.py
+      # auto-detects /mnt/nvme/saltybot/maps/saltybot_map.yaml if present
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam")
+    map_saver_params = os.path.join(nav2_slam_dir, "config", "map_saver_params.yaml")
+
+    # ── Arguments ─────────────────────────────────────────────────────────────
+
+    map_dir_arg = DeclareLaunchArgument(
+        "map_dir",
+        default_value="/mnt/nvme/saltybot/maps",
+        description="Directory for saving/loading map files (.yaml + .pgm)",
+    )
+
+    map_name_arg = DeclareLaunchArgument(
+        "map_name",
+        default_value="saltybot_map",
+        description="Base filename for saved map (no extension)",
+    )
+
+    use_sim_time_arg = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="false",
+        description="Use simulated clock",
+    )
+
+    autostart_arg = DeclareLaunchArgument(
+        "autostart",
+        default_value="true",
+        description="Automatically configure/activate lifecycle nodes",
+    )
+
+    # ── Substitutions ─────────────────────────────────────────────────────────
+
+    map_dir      = LaunchConfiguration("map_dir")
+    map_name     = LaunchConfiguration("map_name")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    autostart    = LaunchConfiguration("autostart")
+
+    # ── map_saver_server (nav2_map_server lifecycle node) ─────────────────────
+    # Exposes /map_saver/save_map action server when active.
+
+    map_saver_server = Node(
+        package="nav2_map_server",
+        executable="map_saver_server",
+        name="map_saver",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        parameters=[
+            map_saver_params,
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
+    # ── saltybot_map_saver helper node ────────────────────────────────────────
+    # /saltybot/save_map Trigger service + startup reporting + auto-save.
+
+    map_saver_helper = Node(
+        package="saltybot_nav2_slam",
+        executable="map_saver_node",
+        name="saltybot_map_saver",
+        output="screen",
+        parameters=[
+            {
+                "map_dir":              map_dir,
+                "map_name":             map_name,
+                "auto_save_on_shutdown": True,
+                "use_sim_time":         use_sim_time,
+            }
+        ],
+    )
+
+    # ── Lifecycle manager for map_saver_server ────────────────────────────────
+
+    lifecycle_manager = Node(
+        package="nav2_lifecycle_manager",
+        executable="lifecycle_manager",
+        name="lifecycle_manager_map_saver",
+        output="screen",
+        parameters=[
+            {
+                "use_sim_time": use_sim_time,
+                "autostart":    autostart,
+                "node_names":   ["map_saver"],
+            }
+        ],
+    )
+
+    return LaunchDescription([
+        # Arguments
+        map_dir_arg,
+        map_name_arg,
+        use_sim_time_arg,
+        autostart_arg,
+
+        # Banner
+        LogInfo(msg="[map_persistence] Starting map_saver_server + helper (Issue #696)"),
+
+        # Nodes
+        map_saver_server,
+        map_saver_helper,
+        lifecycle_manager,
+    ])

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

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

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

@@ -1,66 +1,65 @@
-"""Nav2 SLAM Bringup for SaltyBot (Issue #422)"""
+"""nav2_slam_bringup.launch.py (Issue #422, #696) — SLAM + map persistence."""
 
 import os
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-from ament_index_python.packages import get_package_share_directory
 
 
 def generate_launch_description():
-    nav2_slam_dir = get_package_share_directory('saltybot_nav2_slam')
-    bringup_dir = get_package_share_directory('saltybot_bringup')
+    nav2_slam_dir = get_package_share_directory("saltybot_nav2_slam")
+    bringup_dir = get_package_share_directory("saltybot_bringup")
+    nav2_params = os.path.join(nav2_slam_dir, "config", "nav2_params.yaml")
+    slam_params = os.path.join(nav2_slam_dir, "config", "slam_toolbox_params.yaml")
 
-    nav2_params = os.path.join(nav2_slam_dir, 'config', 'nav2_params.yaml')
-    slam_params = os.path.join(nav2_slam_dir, 'config', 'slam_toolbox_params.yaml')
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    map_dir = LaunchConfiguration("map_dir")
+    map_name = LaunchConfiguration("map_name")
 
     return LaunchDescription([
-        DeclareLaunchArgument('use_sim_time', default_value='false'),
-
+        DeclareLaunchArgument("use_sim_time", default_value="false"),
+        DeclareLaunchArgument(
+            "map_dir",
+            default_value="/mnt/nvme/saltybot/maps",
+            description="Directory for saving/loading map files (Issue #696)",
+        ),
+        DeclareLaunchArgument(
+            "map_name",
+            default_value="saltybot_map",
+            description="Base filename for saved map (no extension)",
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(bringup_dir, "launch", "sensors.launch.py")),
+            launch_arguments={"use_sim_time": use_sim_time}.items(),
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "odometry_bridge.launch.py")),
+            launch_arguments={"use_sim_time": use_sim_time}.items(),
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "nav2_slam_integrated.launch.py")),
+            launch_arguments={"slam_params_file": slam_params, "use_sim_time": use_sim_time}.items(),
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "depth_to_costmap.launch.py")),
+            launch_arguments={"use_sim_time": use_sim_time}.items(),
+        ),
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                os.path.join(bringup_dir, 'launch', 'sensors.launch.py')
-            ),
-            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
-        ),
-
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                os.path.join(nav2_slam_dir, 'launch', 'odometry_bridge.launch.py')
-            ),
-            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
-        ),
-
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                os.path.join(nav2_slam_dir, 'launch', 'nav2_slam_integrated.launch.py')
+                os.path.join(get_package_share_directory("nav2_bringup"), "launch", "navigation_launch.py")
             ),
             launch_arguments={
-                'slam_params_file': slam_params,
-                'use_sim_time': LaunchConfiguration('use_sim_time'),
+                "use_sim_time": use_sim_time,
+                "params_file": nav2_params,
+                "autostart": "true",
+                "map_subscribe_transient_local": "true",
             }.items(),
         ),
-
+        # Map persistence (Issue #696): map_saver_server + /saltybot/save_map service
         IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                os.path.join(nav2_slam_dir, 'launch', 'depth_to_costmap.launch.py')
-            ),
-            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time')}.items(),
-        ),
-
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                os.path.join(
-                    get_package_share_directory('nav2_bringup'),
-                    'launch', 'navigation_launch.py'
-                )
-            ),
-            launch_arguments={
-                'use_sim_time': LaunchConfiguration('use_sim_time'),
-                'params_file': nav2_params,
-                'autostart': 'true',
-                'map_subscribe_transient_local': 'true',
-            }.items(),
+            PythonLaunchDescriptionSource(os.path.join(nav2_slam_dir, "launch", "map_persistence.launch.py")),
+            launch_arguments={"map_dir": map_dir, "map_name": map_name, "use_sim_time": use_sim_time}.items(),
         ),
     ])

jetson/ros2_ws/src/saltybot_nav2_slam/saltybot_nav2_slam/map_saver_node.py

@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+"""
+map_saver_node.py — Map persistence helper for SLAM → AMCL hand-off (Issue #696).
+
+Responsibilities:
+  1. On startup: report whether a saved map exists at map_dir/map_name.yaml
+     (the path that nav2_amcl_bringup.launch.py will load on next boot).
+  2. Provide /saltybot/save_map (std_srvs/Trigger) — triggers map_saver_cli
+     to save the current /map topic to map_dir/map_name.{yaml,pgm}.
+  3. On shutdown: if auto_save_on_shutdown is true, save the final map.
+
+Parameters:
+  map_dir              (str)  default /mnt/nvme/saltybot/maps
+  map_name             (str)  default saltybot_map
+  auto_save_on_shutdown (bool) default true
+
+The saved map is loaded automatically by nav2_amcl_bringup.launch.py on the
+next startup when the file is present at map_dir/map_name.yaml.
+"""
+
+import os
+import subprocess
+
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import Trigger
+
+
+class MapSaverNode(Node):
+    """Helper node for SLAM map persistence and AMCL pre-load."""
+
+    def __init__(self) -> None:
+        super().__init__("saltybot_map_saver")
+
+        self.declare_parameter("map_dir",              "/mnt/nvme/saltybot/maps")
+        self.declare_parameter("map_name",             "saltybot_map")
+        self.declare_parameter("auto_save_on_shutdown", True)
+
+        self._map_dir   = self.get_parameter("map_dir").value
+        self._map_name  = self.get_parameter("map_name").value
+        self._auto_save = self.get_parameter("auto_save_on_shutdown").value
+
+        self._map_prefix = os.path.join(self._map_dir, self._map_name)
+        self._map_yaml   = self._map_prefix + ".yaml"
+
+        # ── Ensure map directory exists ────────────────────────────────────────
+        os.makedirs(self._map_dir, exist_ok=True)
+
+        # ── Report saved map status ────────────────────────────────────────────
+        if os.path.isfile(self._map_yaml):
+            self.get_logger().info(
+                f"[map_saver] Saved map found: {self._map_yaml} — "
+                "AMCL will load this map on next startup."
+            )
+        else:
+            self.get_logger().warn(
+                f"[map_saver] No saved map at {self._map_yaml} — "
+                "AMCL will use the placeholder map. "
+                "Run SLAM, then call /saltybot/save_map to persist."
+            )
+
+        # ── Service: /saltybot/save_map ────────────────────────────────────────
+        self._save_srv = self.create_service(
+            Trigger,
+            "/saltybot/save_map",
+            self._on_save_map,
+        )
+
+        self.get_logger().info(
+            f"[map_saver] Ready. map_dir={self._map_dir} map_name={self._map_name} "
+            f"auto_save_on_shutdown={self._auto_save}"
+        )
+
+    # ── Service callback ───────────────────────────────────────────────────────
+
+    def _on_save_map(
+        self, _request: Trigger.Request, response: Trigger.Response
+    ) -> Trigger.Response:
+        """Save current /map to map_dir/map_name.{yaml,pgm} via map_saver_cli."""
+        success, message = self._save_map()
+        response.success = success
+        response.message = message
+        return response
+
+    # ── Map save helper ────────────────────────────────────────────────────────
+
+    def _save_map(self) -> tuple[bool, str]:
+        """
+        Call nav2_map_server map_saver_cli to write the current /map.
+
+        Returns (success, message).
+        """
+        cmd = [
+            "ros2", "run", "nav2_map_server", "map_saver_cli",
+            "-f", self._map_prefix,
+            "--map_subscribe_transient_local",
+        ]
+        self.get_logger().info(f"[map_saver] Saving map to {self._map_prefix}.*")
+        try:
+            result = subprocess.run(
+                cmd,
+                timeout=10,
+                capture_output=True,
+                text=True,
+            )
+            if result.returncode == 0:
+                msg = f"Map saved to {self._map_yaml}"
+                self.get_logger().info(f"[map_saver] {msg}")
+                return True, msg
+            else:
+                msg = f"map_saver_cli failed (rc={result.returncode}): {result.stderr.strip()}"
+                self.get_logger().error(f"[map_saver] {msg}")
+                return False, msg
+        except subprocess.TimeoutExpired:
+            msg = "map_saver_cli timed out after 10 s (is /map published?)"
+            self.get_logger().error(f"[map_saver] {msg}")
+            return False, msg
+        except Exception as exc:  # noqa: BLE001
+            msg = f"map_saver_cli exception: {exc}"
+            self.get_logger().error(f"[map_saver] {msg}")
+            return False, msg
+
+    # ── Lifecycle ──────────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        if self._auto_save:
+            self.get_logger().info("[map_saver] Shutdown — auto-saving map...")
+            self._save_map()
+        super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = MapSaverNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_nav2_slam/setup.py

@@ -15,6 +15,7 @@ setup(
             "launch/depth_to_costmap.launch.py",
             "launch/odometry_bridge.launch.py",
             "launch/nav2_amcl_bringup.launch.py",
+            "launch/map_persistence.launch.py",
         ]),
         (f"share/{package_name}/config", [
             "config/nav2_params.yaml",
@@ -25,6 +26,7 @@ setup(
             "config/dwb_local_planner_params.yaml",
             "config/vesc_odometry_params.yaml",
             "config/amcl_nav2_params.yaml",
+            "config/map_saver_params.yaml",
         ]),
         (f"share/{package_name}/maps", [
             "maps/saltybot_map.yaml",
@@ -42,6 +44,7 @@ setup(
         "console_scripts": [
             "vesc_odometry_bridge = saltybot_nav2_slam.vesc_odometry_bridge:main",
             "vesc_can_odometry = saltybot_nav2_slam.vesc_odometry_bridge:main",
+            "map_saver_node = saltybot_nav2_slam.map_saver_node:main",
         ],
     },
 )

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

@@ -334,5 +334,125 @@ def test_launch_file_has_map_argument():
     assert '"map"' in src or "'map'" in src
 
 
+
+# ── map_saver_params.yaml + map_persistence files (Issue #696) ───────────────
+
+import os as _os
+_PKG_ROOT2 = _os.path.join(_os.path.dirname(__file__), "..")
+_MAP_SAVER_CFG  = _os.path.join(_PKG_ROOT2, "config", "map_saver_params.yaml")
+_SLAM_LAUNCH    = _os.path.join(_PKG_ROOT2, "launch", "nav2_slam_bringup.launch.py")
+_PERSIST_LAUNCH = _os.path.join(_PKG_ROOT2, "launch", "map_persistence.launch.py")
+_SAVER_NODE     = _os.path.join(_PKG_ROOT2, "saltybot_nav2_slam", "map_saver_node.py")
+
+
+def _load_map_saver_params():
+    import yaml
+    with open(_MAP_SAVER_CFG) as f:
+        return yaml.safe_load(f)
+
+
+def test_map_saver_params_file_exists():
+    assert _os.path.isfile(_MAP_SAVER_CFG)
+
+
+def test_map_saver_params_required_keys():
+    p = _load_map_saver_params()["map_saver"]["ros__parameters"]
+    for key in ("save_map_timeout", "free_thresh_default",
+                "occupied_thresh_default", "map_subscribe_transient_local"):
+        assert key in p, f"map_saver_params missing: {key}"
+
+
+def test_map_saver_thresholds_valid():
+    p = _load_map_saver_params()["map_saver"]["ros__parameters"]
+    assert 0.0 < p["free_thresh_default"] < p["occupied_thresh_default"] < 1.0
+
+
+def test_map_saver_timeout_positive():
+    p = _load_map_saver_params()["map_saver"]["ros__parameters"]
+    assert p["save_map_timeout"] > 0.0
+
+
+def test_map_persistence_launch_exists():
+    assert _os.path.isfile(_PERSIST_LAUNCH)
+
+
+def test_map_persistence_launch_syntax():
+    import py_compile
+    py_compile.compile(_PERSIST_LAUNCH, doraise=True)
+
+
+def test_map_persistence_launch_has_map_saver_server():
+    with open(_PERSIST_LAUNCH) as f:
+        src = f.read()
+    assert "map_saver_server" in src
+
+
+def test_map_persistence_launch_has_lifecycle_manager():
+    with open(_PERSIST_LAUNCH) as f:
+        src = f.read()
+    assert "lifecycle_manager" in src
+
+
+def test_map_persistence_launch_has_map_dir_arg():
+    with open(_PERSIST_LAUNCH) as f:
+        src = f.read()
+    assert "map_dir" in src
+
+
+def test_map_saver_node_exists():
+    assert _os.path.isfile(_SAVER_NODE)
+
+
+def test_map_saver_node_syntax():
+    import py_compile
+    py_compile.compile(_SAVER_NODE, doraise=True)
+
+
+def test_map_saver_node_has_save_service():
+    with open(_SAVER_NODE) as f:
+        src = f.read()
+    assert "/saltybot/save_map" in src
+
+
+def test_map_saver_node_calls_map_saver_cli():
+    with open(_SAVER_NODE) as f:
+        src = f.read()
+    assert "map_saver_cli" in src
+
+
+def test_map_saver_node_has_auto_save_shutdown():
+    with open(_SAVER_NODE) as f:
+        src = f.read()
+    assert "auto_save_on_shutdown" in src
+
+
+def test_slam_bringup_includes_map_persistence():
+    with open(_SLAM_LAUNCH) as f:
+        src = f.read()
+    assert "map_persistence.launch.py" in src
+
+
+def test_slam_bringup_has_map_dir_arg():
+    with open(_SLAM_LAUNCH) as f:
+        src = f.read()
+    assert "map_dir" in src
+
+
+def test_amcl_launch_has_map_dir_arg():
+    import os as __os
+    _LAUNCH2 = __os.path.join(__os.path.dirname(__file__), "..", "launch", "nav2_amcl_bringup.launch.py")
+    with open(_LAUNCH2) as f:
+        src = f.read()
+    assert "map_dir" in src
+
+
+def test_amcl_launch_has_auto_detect_logic():
+    import os as __os
+    _LAUNCH2 = __os.path.join(__os.path.dirname(__file__), "..", "launch", "nav2_amcl_bringup.launch.py")
+    with open(_LAUNCH2) as f:
+        src = f.read()
+    assert "/mnt/nvme/saltybot/maps" in src
+    assert "isfile" in src or "os.path" in src
+
 if __name__ == "__main__":
     pytest.main([__file__, "-v"])

src/can_driver.c

@@ -1,13 +1,4 @@
-/* CAN bus driver for BLDC motor controllers (Issue #597)
- * CAN1 on PB8 (RX, AF9) / PB9 (TX, AF9) at 500 kbps   (Issue #676 remap)
- * Filter bank 0 (CAN1 master; SlaveStartFilterBank=14)
- *
- * NOTE: Mamba F722S MK2 does not expose PB12/PB13 externally.
- * Waveshare CAN module wired to SCL pad (PB8 = CAN1_RX) and
- * SDA pad (PB9 = CAN1_TX).  I2C1 is free (BME280 moved to I2C2).
- * CAN1 uses AF9 on PB8/PB9 — no conflict with other active peripherals.
- */
-
+/* CAN bus driver (Issues #597, #676, #674, #694) */
 #include "can_driver.h"
 #include "stm32f7xx_hal.h"
 #include <string.h>
@@ -17,24 +8,31 @@ static volatile can_feedback_t  s_feedback[CAN_NUM_MOTORS];
 static volatile can_stats_t     s_stats;
 static can_ext_frame_cb_t       s_ext_cb = NULL;
 static can_std_frame_cb_t       s_std_cb = NULL;
+static volatile can_wdog_t      s_wdog;
+
+#ifdef TEST_HOST
+static volatile uint32_t s_test_esr = 0u;
+void can_driver_inject_esr(uint32_t v) { s_test_esr = v; }
+static uint32_t _read_esr(void) { return s_test_esr; }
+static HAL_StatusTypeDef _can_restart(void) {
+    HAL_CAN_Stop(&s_can); s_test_esr = 0u; return HAL_CAN_Start(&s_can);
+}
+#else
+static uint32_t _read_esr(void) { return s_can.Instance->ESR; }
+static HAL_StatusTypeDef _can_restart(void) {
+    HAL_CAN_Stop(&s_can); return HAL_CAN_Start(&s_can);
+}
+#endif
 
 void can_driver_init(void)
 {
     __HAL_RCC_CAN1_CLK_ENABLE();
     __HAL_RCC_GPIOB_CLK_ENABLE();
-
-    /* PB8 = CAN1_RX, PB9 = CAN1_TX, AF9 (Issue #676) */
     GPIO_InitTypeDef gpio = {0};
     gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9;
-    gpio.Mode      = GPIO_MODE_AF_PP;
-    gpio.Pull      = GPIO_NOPULL;
-    gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
-    gpio.Alternate = GPIO_AF9_CAN1;
+    gpio.Mode = GPIO_MODE_AF_PP; gpio.Pull = GPIO_NOPULL;
+    gpio.Speed = GPIO_SPEED_FREQ_HIGH; gpio.Alternate = GPIO_AF9_CAN1;
     HAL_GPIO_Init(GPIOB, &gpio);
-
-    /* 500 kbps @ APB1=54 MHz: PSC=6, BS1=13tq, BS2=4tq, SJW=1tq
-     * bit_time = 6 × (1+13+4) / 54000000 = 2 µs → 500 kbps
-     * sample point = (1+13)/18 = 77.8% */
     s_can.Instance = CAN1;
     s_can.Init.Prescaler = CAN_PRESCALER;
     s_can.Init.Mode = CAN_MODE_NORMAL;
@@ -42,147 +40,83 @@ void can_driver_init(void)
     s_can.Init.TimeSeg1 = CAN_BS1_13TQ;
     s_can.Init.TimeSeg2 = CAN_BS2_4TQ;
     s_can.Init.TimeTriggeredMode = DISABLE;
-    s_can.Init.AutoBusOff           = ENABLE;   /* HW recovery after 128×11 bits */
+    s_can.Init.AutoBusOff = ENABLE;
     s_can.Init.AutoWakeUp = DISABLE;
     s_can.Init.AutoRetransmission = ENABLE;
     s_can.Init.ReceiveFifoLocked = DISABLE;
     s_can.Init.TransmitFifoPriority = DISABLE;
-
-    if (HAL_CAN_Init(&s_can) != HAL_OK) {
-        s_stats.bus_off = 1u;
-        return;
-    }
-
-    /* Filter bank 0: 32-bit mask, FIFO0, accept ALL standard 11-bit frames.
-     * CAN1 is master (SlaveStartFilterBank=14). FilterIdHigh=0, MaskHigh=0
-     * → mask=0 passes every standard ID. Orin std-frame commands land here. */
+    if (HAL_CAN_Init(&s_can) != HAL_OK) { s_stats.bus_off = 1u; return; }
     CAN_FilterTypeDef flt = {0};
-    flt.FilterBank           = 0u;
-    flt.FilterMode           = CAN_FILTERMODE_IDMASK;
+    flt.FilterBank = 0u; flt.FilterMode = CAN_FILTERMODE_IDMASK;
     flt.FilterScale = CAN_FILTERSCALE_32BIT;
-    flt.FilterIdHigh         = 0u;
-    flt.FilterIdLow          = 0u;
-    flt.FilterMaskIdHigh     = 0u;
-    flt.FilterMaskIdLow      = 0u;
     flt.FilterFIFOAssignment = CAN_RX_FIFO0;
     flt.FilterActivation = CAN_FILTER_ENABLE;
     flt.SlaveStartFilterBank = 14u;
-
-    if (HAL_CAN_ConfigFilter(&s_can, &flt) != HAL_OK) {
-        s_stats.bus_off = 1u;
-        return;
-    }
-
-    /* Filter bank 15: 32-bit mask, FIFO1, accept ALL extended 29-bit frames.
-     * For extended frames, the IDE bit sits at FilterIdLow[2].
-     * FilterIdLow  = 0x0004 (IDE=1) → match extended frames.
-     * FilterMaskIdLow = 0x0004 → only the IDE bit is checked; all ext IDs pass.
-     * This routes every VESC STATUS / STATUS_4 / STATUS_5 frame to FIFO1. */
+    if (HAL_CAN_ConfigFilter(&s_can, &flt) != HAL_OK) { s_stats.bus_off = 1u; return; }
     CAN_FilterTypeDef flt2 = {0};
-    flt2.FilterBank           = 15u;
-    flt2.FilterMode           = CAN_FILTERMODE_IDMASK;
+    flt2.FilterBank = 15u; flt2.FilterMode = CAN_FILTERMODE_IDMASK;
     flt2.FilterScale = CAN_FILTERSCALE_32BIT;
-    flt2.FilterIdHigh         = 0u;
-    flt2.FilterIdLow          = 0x0004u;  /* IDE = 1 */
-    flt2.FilterMaskIdHigh     = 0u;
-    flt2.FilterMaskIdLow      = 0x0004u;  /* only check IDE bit */
+    flt2.FilterIdLow = 0x0004u; flt2.FilterMaskIdLow = 0x0004u;
     flt2.FilterFIFOAssignment = CAN_RX_FIFO1;
     flt2.FilterActivation = CAN_FILTER_ENABLE;
     flt2.SlaveStartFilterBank = 14u;
-
-    if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) {
-        s_stats.bus_off = 1u;
-        return;
-    }
-
+    if (HAL_CAN_ConfigFilter(&s_can, &flt2) != HAL_OK) { s_stats.bus_off = 1u; return; }
     HAL_CAN_Start(&s_can);
-
+#ifndef TEST_HOST
+    HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 5u, 0u);
+    HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn);
+    HAL_CAN_ActivateNotification(&s_can, CAN_IT_BUSOFF | CAN_IT_ERROR_PASSIVE | CAN_IT_ERROR_WARNING);
+#endif
     memset((void *)s_feedback, 0, sizeof(s_feedback));
     memset((void *)&s_stats, 0, sizeof(s_stats));
+    memset((void *)&s_wdog, 0, sizeof(s_wdog));
 }
 
 void can_driver_send_cmd(uint8_t node_id, const can_cmd_t *cmd)
 {
-    if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) {
-        return;
-    }
-
-    /* Payload: [vel_lo, vel_hi, torque_lo, torque_hi] — little-endian */
+    if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) return;
     uint8_t data[4];
     data[0] = (uint8_t)((uint16_t)cmd->velocity_rpm & 0xFFu);
     data[1] = (uint8_t)((uint16_t)cmd->velocity_rpm >> 8u);
     data[2] = (uint8_t)((uint16_t)cmd->torque_x100 & 0xFFu);
     data[3] = (uint8_t)((uint16_t)cmd->torque_x100 >> 8u);
-
     CAN_TxHeaderTypeDef hdr = {0};
     hdr.StdId = CAN_ID_VEL_CMD_BASE + (uint32_t)node_id;
-    hdr.IDE   = CAN_ID_STD;
-    hdr.RTR   = CAN_RTR_DATA;
-    hdr.DLC   = 4u;
-
+    hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = 4u;
     uint32_t mailbox;
     if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
-        if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK) {
+        if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK)
             s_stats.tx_count++;
-        } else {
-            s_stats.err_count++;
-        }
+        else s_stats.err_count++;
     }
 }
 
 void can_driver_send_enable(uint8_t node_id, bool enable)
 {
-    if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) {
-        return;
-    }
-
+    if (node_id >= CAN_NUM_MOTORS || s_stats.bus_off) return;
     uint8_t data[1] = { enable ? 1u : 0u };
-
     CAN_TxHeaderTypeDef hdr = {0};
     hdr.StdId = CAN_ID_ENABLE_CMD_BASE + (uint32_t)node_id;
-    hdr.IDE   = CAN_ID_STD;
-    hdr.RTR   = CAN_RTR_DATA;
-    hdr.DLC   = 1u;
-
+    hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = 1u;
     uint32_t mailbox;
     if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
-        if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK) {
+        if (HAL_CAN_AddTxMessage(&s_can, &hdr, data, &mailbox) == HAL_OK)
             s_stats.tx_count++;
-        } else {
-            s_stats.err_count++;
-        }
+        else s_stats.err_count++;
     }
 }
 
 void can_driver_process(void)
 {
-    /* Check for bus-off */
-    if (s_can.Instance->ESR & CAN_ESR_BOFF) {
-        s_stats.bus_off = 1u;
-        return;
-    }
+    if (_read_esr() & CAN_ESR_BOFF) { s_stats.bus_off = 1u; return; }
     s_stats.bus_off = 0u;
-
-    /* Drain FIFO0 (standard-ID frames: Orin commands + legacy feedback) */
     while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO0) > 0u) {
-        CAN_RxHeaderTypeDef rxhdr;
-        uint8_t rxdata[8];
-
+        CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8];
         if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO0, &rxhdr, rxdata) != HAL_OK) {
-            s_stats.err_count++;
-            break;
+            s_stats.err_count++; break;
         }
-
-        if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) {
-            continue;
-        }
-
-        /* Dispatch to registered standard-frame callback (Orin CAN protocol) */
-        if (s_std_cb != NULL) {
-            s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
-        }
-
-        /* Legacy: decode feedback frames at 0x200-0x21F (Issue #597) */
+        if (rxhdr.IDE != CAN_ID_STD || rxhdr.RTR != CAN_RTR_DATA) continue;
+        if (s_std_cb != NULL) s_std_cb((uint16_t)rxhdr.StdId, rxdata, (uint8_t)rxhdr.DLC);
         uint32_t nid_u = rxhdr.StdId - CAN_ID_FEEDBACK_BASE;
         if (nid_u < CAN_NUM_MOTORS && rxhdr.DLC >= 8u) {
             uint8_t nid = (uint8_t)nid_u;
@@ -193,113 +127,127 @@ void can_driver_process(void)
             s_feedback[nid].fault         = rxdata[7];
             s_feedback[nid].last_rx_ms    = HAL_GetTick();
         }
-
         s_stats.rx_count++;
     }
-
-    /* Drain FIFO1 (extended-ID frames: VESC STATUS/STATUS_4/STATUS_5) */
     while (HAL_CAN_GetRxFifoFillLevel(&s_can, CAN_RX_FIFO1) > 0u) {
-        CAN_RxHeaderTypeDef rxhdr;
-        uint8_t rxdata[8];
-
+        CAN_RxHeaderTypeDef rxhdr; uint8_t rxdata[8];
         if (HAL_CAN_GetRxMessage(&s_can, CAN_RX_FIFO1, &rxhdr, rxdata) != HAL_OK) {
-            s_stats.err_count++;
-            break;
+            s_stats.err_count++; break;
         }
-
-        if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) {
-            continue;
-        }
-
-        if (s_ext_cb != NULL) {
-            s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
-        }
-
+        if (rxhdr.IDE != CAN_ID_EXT || rxhdr.RTR != CAN_RTR_DATA) continue;
+        if (s_ext_cb != NULL) s_ext_cb(rxhdr.ExtId, rxdata, (uint8_t)rxhdr.DLC);
         s_stats.rx_count++;
     }
 }
 
-void can_driver_set_ext_cb(can_ext_frame_cb_t cb)
-{
-    s_ext_cb = cb;
-}
-
-void can_driver_set_std_cb(can_std_frame_cb_t cb)
-{
-    s_std_cb = cb;
-}
+void can_driver_set_ext_cb(can_ext_frame_cb_t cb) { s_ext_cb = cb; }
+void can_driver_set_std_cb(can_std_frame_cb_t cb) { s_std_cb = cb; }
 
 void can_driver_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t len)
 {
-    if (s_stats.bus_off || len > 8u) {
-        return;
-    }
-
+    if (s_stats.bus_off || len > 8u) return;
     CAN_TxHeaderTypeDef hdr = {0};
-    hdr.ExtId = ext_id;
-    hdr.IDE   = CAN_ID_EXT;
-    hdr.RTR   = CAN_RTR_DATA;
-    hdr.DLC   = len;
-
+    hdr.ExtId = ext_id; hdr.IDE = CAN_ID_EXT; hdr.RTR = CAN_RTR_DATA; hdr.DLC = len;
     uint32_t mailbox;
     if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
-        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
+        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK)
             s_stats.tx_count++;
-        } else {
-            s_stats.err_count++;
-        }
+        else s_stats.err_count++;
     }
 }
 
 void can_driver_send_std(uint16_t std_id, const uint8_t *data, uint8_t len)
 {
-    if (s_stats.bus_off || len > 8u) {
-        return;
-    }
-
+    if (s_stats.bus_off || len > 8u) return;
     CAN_TxHeaderTypeDef hdr = {0};
-    hdr.StdId = std_id;
-    hdr.IDE   = CAN_ID_STD;
-    hdr.RTR   = CAN_RTR_DATA;
-    hdr.DLC   = len;
-
+    hdr.StdId = std_id; hdr.IDE = CAN_ID_STD; hdr.RTR = CAN_RTR_DATA; hdr.DLC = len;
     uint32_t mailbox;
     if (HAL_CAN_GetTxMailboxesFreeLevel(&s_can) > 0u) {
-        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK) {
+        if (HAL_CAN_AddTxMessage(&s_can, &hdr, (uint8_t *)data, &mailbox) == HAL_OK)
             s_stats.tx_count++;
-        } else {
-            s_stats.err_count++;
-        }
+        else s_stats.err_count++;
     }
 }
 
 bool can_driver_get_feedback(uint8_t node_id, can_feedback_t *out)
 {
-    if (node_id >= CAN_NUM_MOTORS || out == NULL) {
-        return false;
-    }
-    if (s_feedback[node_id].last_rx_ms == 0u) {
-        return false;
-    }
+    if (node_id >= CAN_NUM_MOTORS || out == NULL) return false;
+    if (s_feedback[node_id].last_rx_ms == 0u) return false;
     memcpy(out, (const void *)&s_feedback[node_id], sizeof(can_feedback_t));
     return true;
 }
 
 bool can_driver_is_alive(uint8_t node_id, uint32_t now_ms)
 {
-    if (node_id >= CAN_NUM_MOTORS) {
-        return false;
-    }
-    if (s_feedback[node_id].last_rx_ms == 0u) {
-        return false;
-    }
+    if (node_id >= CAN_NUM_MOTORS || s_feedback[node_id].last_rx_ms == 0u) return false;
     return (now_ms - s_feedback[node_id].last_rx_ms) < CAN_NODE_TIMEOUT_MS;
 }
 
 void can_driver_get_stats(can_stats_t *out)
 {
-    if (out == NULL) {
-        return;
-    }
+    if (out == NULL) return;
     memcpy(out, (const void *)&s_stats, sizeof(can_stats_t));
 }
+
+/* SCE interrupt handler -- Issue #694 */
+#ifndef TEST_HOST
+void CAN1_SCE_IRQHandler(void)
+{
+    uint32_t esr = s_can.Instance->ESR;
+    if (esr & CAN_ESR_BOFF) {
+        if (s_wdog.error_state != CAN_ERR_BUS_OFF) s_wdog.busoff_count++;
+        s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u;
+    } else if (esr & CAN_ESR_EPVF) {
+        if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) {
+            s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE;
+        }
+    } else if (esr & CAN_ESR_EWGF) {
+        if (s_wdog.error_state < CAN_ERR_WARNING) {
+            s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING;
+        }
+    }
+    __HAL_CAN_CLEAR_FLAG(&s_can, CAN_FLAG_ERRI);
+    HAL_CAN_IRQHandler(&s_can);
+}
+#endif
+
+/* Watchdog tick -- Issue #694 */
+can_error_state_t can_driver_watchdog_tick(uint32_t now_ms)
+{
+    uint32_t esr = _read_esr();
+    if (esr & CAN_ESR_BOFF) {
+        if (s_wdog.error_state != CAN_ERR_BUS_OFF) {
+            s_wdog.busoff_count++; s_wdog.busoff_ms = now_ms; s_wdog.busoff_pending = 1u;
+        }
+        s_wdog.error_state = CAN_ERR_BUS_OFF; s_stats.bus_off = 1u;
+    } else if (esr & CAN_ESR_EPVF) {
+        if (s_wdog.error_state < CAN_ERR_ERROR_PASSIVE) {
+            s_wdog.errpassive_count++; s_wdog.error_state = CAN_ERR_ERROR_PASSIVE;
+        }
+    } else if (esr & CAN_ESR_EWGF) {
+        if (s_wdog.error_state < CAN_ERR_WARNING) {
+            s_wdog.errwarn_count++; s_wdog.error_state = CAN_ERR_WARNING;
+        }
+    } else {
+        if (s_wdog.error_state > CAN_ERR_NOMINAL && s_wdog.error_state < CAN_ERR_BUS_OFF)
+            s_wdog.error_state = CAN_ERR_NOMINAL;
+    }
+    if (s_wdog.busoff_pending && s_wdog.error_state == CAN_ERR_BUS_OFF &&
+        (now_ms - s_wdog.busoff_ms) >= CAN_WDOG_RESTART_MS) {
+        if (_can_restart() == HAL_OK) {
+            s_wdog.restart_count++; s_wdog.error_state = CAN_ERR_NOMINAL;
+            s_wdog.busoff_pending = 0u; s_stats.bus_off = 0u;
+        } else {
+            s_wdog.busoff_ms = now_ms;
+        }
+    }
+    s_wdog.tec = (uint8_t)((esr >> CAN_ESR_TEC_Pos) & 0xFFu);
+    s_wdog.rec = (uint8_t)((esr >> CAN_ESR_REC_Pos) & 0xFFu);
+    return s_wdog.error_state;
+}
+
+void can_driver_get_wdog(can_wdog_t *out)
+{
+    if (out == NULL) return;
+    memcpy(out, (const void *)&s_wdog, sizeof(can_wdog_t));
+}

src/jlink.c

@@ -712,3 +712,27 @@ void jlink_send_vesc_state_tlm(const jlink_tlm_vesc_state_t *tlm)
 
     jlink_tx_locked(frame, sizeof(frame));
 }
+
+/* ---- jlink_send_can_wdog_tlm() -- Issue #694 ---- */
+void jlink_send_can_wdog_tlm(const jlink_tlm_can_wdog_t *tlm)
+{
+    /*
+     * Frame: [STX][LEN][0x8F][16 bytes CAN_WDOG][CRC_hi][CRC_lo][ETX]
+     * LEN = 1 (CMD) + 16 (payload) = 17; total frame = 22 bytes.
+     */
+    static uint8_t frame[22];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_can_wdog_t);  /* 16 */
+    const uint8_t  len  = 1u + plen;                               /* 17 */
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_CAN_WDOG;
+    memcpy(&frame[3], tlm, plen);
+
+    uint16_t crc = crc16_xmodem(&frame[2], len);
+    frame[3 + plen]     = (uint8_t)(crc >> 8);
+    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
+    frame[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame, sizeof(frame));
+}

src/main.c

@@ -322,6 +322,7 @@ int main(void) {
     uint32_t pm_tlm_tick    = 0;   /* JLINK_TLM_POWER transmit timer */
     uint32_t can_cmd_tick   = 0;   /* CAN velocity command TX timer (Issue #597) */
     uint32_t can_tlm_tick   = 0;   /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */
+    uint32_t wdog_tlm_tick  = 0;   /* JLINK_TLM_CAN_WDOG transmit timer (Issue #694) */
     uint32_t lvc_tlm_tick   = 0;   /* JLINK_TLM_LVC transmit timer (Issue #613) */
     uint32_t uart_status_tick = 0;  /* UART protocol STATUS frame timer (Issue #629) */
     uint8_t  pm_pwm_phase   = 0;   /* Software PWM counter for sleep LED */
@@ -416,6 +417,9 @@ int main(void) {
         /* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */
         can_driver_process();
 
+        /* CAN watchdog: detect bus errors, auto-restart (Issue #694) */
+        can_driver_watchdog_tick(now);
+
         /* UART command protocol RX: parse Jetson frames (Issue #629) */
         uart_protocol_process();
 
@@ -528,6 +532,23 @@ int main(void) {
             if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
                 safety_remote_estop_clear();
         }
+        /* PID_SET (Issue #693): apply new gains from Orin immediately.
+         * Gains persist in RAM until reboot; not saved to flash. */
+        if (orin_can_state.pid_updated) {
+            orin_can_state.pid_updated = 0u;
+            float new_kp = (float)orin_can_state.pid_kp_x100 / 100.0f;
+            float new_ki = (float)orin_can_state.pid_ki_x100 / 100.0f;
+            float new_kd = (float)orin_can_state.pid_kd_x100 / 100.0f;
+            if (new_kp > 500.0f) new_kp = 500.0f;
+            if (new_ki > 50.0f)  new_ki = 50.0f;
+            if (new_kd > 50.0f)  new_kd = 50.0f;
+            bal.kp = new_kp;
+            bal.ki = new_ki;
+            bal.kd = new_kd;
+            orin_can_send_pid_ack(bal.kp, bal.ki, bal.kd);
+            printf("[PID] Orin set Kp=%.3f Ki=%.3f Kd=%.3f\n",
+                   (double)bal.kp, (double)bal.ki, (double)bal.kd);
+        }
 
         /* Hardware button park/re-arm (Issue #682).
          * Short press -> park (ARMED only): freeze PID, stop motors, amber LED.
@@ -604,6 +625,23 @@ int main(void) {
             }
         }
 
+        /* CAN_WDOG: send watchdog telemetry at 1 Hz (Issue #694) */
+        if (now - wdog_tlm_tick >= 1000u) {
+            wdog_tlm_tick = now;
+            can_wdog_t wd;
+            can_driver_get_wdog(&wd);
+            jlink_tlm_can_wdog_t wtlm;
+            memset(&wtlm, 0, sizeof(wtlm));
+            wtlm.restart_count    = wd.restart_count;
+            wtlm.busoff_count     = wd.busoff_count;
+            wtlm.errpassive_count = wd.errpassive_count;
+            wtlm.errwarn_count    = wd.errwarn_count;
+            wtlm.error_state      = (uint8_t)wd.error_state;
+            wtlm.tec              = wd.tec;
+            wtlm.rec              = wd.rec;
+            jlink_send_can_wdog_tlm(&wtlm);
+        }
+
         /* Power management: CRSF/JLink activity or armed state resets idle timer */
         if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) ||
                 jlink_is_active(now) ||

src/orin_can.c

@@ -81,11 +81,34 @@ void orin_can_on_frame(uint16_t std_id, const uint8_t *data, uint8_t len)
         orin_can_led_updated = 1u;
         break;
 
+    case ORIN_CAN_ID_PID_SET:
+        /* kp_x100(u16 BE), ki_x100(u16 BE), kd_x100(u16 BE) -- Issue #693 */
+        if (len < 6u) { break; }
+        orin_can_state.pid_kp_x100 = (uint16_t)(((uint16_t)data[0] << 8u) | (uint16_t)data[1]);
+        orin_can_state.pid_ki_x100 = (uint16_t)(((uint16_t)data[2] << 8u) | (uint16_t)data[3]);
+        orin_can_state.pid_kd_x100 = (uint16_t)(((uint16_t)data[4] << 8u) | (uint16_t)data[5]);
+        orin_can_state.pid_updated = 1u;
+        break;
+
     default:
         break;
     }
 }
 
+void orin_can_send_pid_ack(float kp, float ki, float kd)
+{
+    orin_can_fc_pid_ack_t ack;
+    if (kp < 0.0f) kp = 0.0f;
+    if (ki < 0.0f) ki = 0.0f;
+    if (kd < 0.0f) kd = 0.0f;
+    ack.kp_x100 = (uint16_t)(kp * 100.0f + 0.5f);
+    ack.ki_x100 = (uint16_t)(ki * 100.0f + 0.5f);
+    ack.kd_x100 = (uint16_t)(kd * 100.0f + 0.5f);
+    uint8_t buf[6];
+    memcpy(buf, &ack, sizeof(ack));
+    can_driver_send_std(ORIN_CAN_ID_FC_PID_ACK, buf, (uint8_t)sizeof(ack));
+}
+
 bool orin_can_is_alive(uint32_t now_ms)
 {
     if (orin_can_state.last_rx_ms == 0u) {

test/stubs/stm32f7xx_hal.h

@@ -1,86 +1,40 @@
-/* test/stubs/stm32f7xx_hal.h — minimal HAL stub for host-side unit tests.
- *
- * Provides just enough types and functions so that the firmware source files
- * compile on a host (Linux/macOS) with -DTEST_HOST.
- * Each test file must define the actual behavior of HAL_GetTick() etc.
- */
-
+/* test/stubs/stm32f7xx_hal.h - minimal HAL stub for host-side unit tests. */
 #ifndef STM32F7XX_HAL_H
 #define STM32F7XX_HAL_H
-
 #include <stdint.h>
 #include <stdbool.h>
 #include <stddef.h>
-
-/* ---- Return codes ---- */
 #define HAL_OK      0
 #define HAL_ERROR   1
 #define HAL_BUSY    2
 #define HAL_TIMEOUT 3
 typedef uint32_t HAL_StatusTypeDef;
-
-/* ---- Enable / Disable ---- */
 #define ENABLE  1
 #define DISABLE 0
-
-/* ---- HAL_GetTick: each test declares its own implementation ---- */
 uint32_t HAL_GetTick(void);
-
-/* ---- Minimal CAN types used by can_driver.c / vesc_can.c ---- */
-typedef struct { uint32_t dummy; } CAN_TypeDef;
-
+typedef struct { uint32_t dummy; uint32_t ESR; } CAN_TypeDef;
 typedef struct {
-    uint32_t Prescaler;
-    uint32_t Mode;
-    uint32_t SyncJumpWidth;
-    uint32_t TimeSeg1;
-    uint32_t TimeSeg2;
-    uint32_t TimeTriggeredMode;
-    uint32_t AutoBusOff;
-    uint32_t AutoWakeUp;
-    uint32_t AutoRetransmission;
-    uint32_t ReceiveFifoLocked;
-    uint32_t TransmitFifoPriority;
+    uint32_t Prescaler; uint32_t Mode; uint32_t SyncJumpWidth;
+    uint32_t TimeSeg1;  uint32_t TimeSeg2; uint32_t TimeTriggeredMode;
+    uint32_t AutoBusOff; uint32_t AutoWakeUp; uint32_t AutoRetransmission;
+    uint32_t ReceiveFifoLocked; uint32_t TransmitFifoPriority;
 } CAN_InitTypeDef;
-
+typedef struct { CAN_TypeDef *Instance; CAN_InitTypeDef Init; } CAN_HandleTypeDef;
 typedef struct {
-    CAN_TypeDef    *Instance;
-    CAN_InitTypeDef Init;
-    /* opaque HAL internals omitted */
-} CAN_HandleTypeDef;
-
-typedef struct {
-    uint32_t StdId;
-    uint32_t ExtId;
-    uint32_t IDE;
-    uint32_t RTR;
-    uint32_t DLC;
-    uint32_t Timestamp;
-    uint32_t FilterMatchIndex;
+    uint32_t StdId; uint32_t ExtId; uint32_t IDE; uint32_t RTR;
+    uint32_t DLC; uint32_t Timestamp; uint32_t FilterMatchIndex;
 } CAN_RxHeaderTypeDef;
-
 typedef struct {
-    uint32_t StdId;
-    uint32_t ExtId;
-    uint32_t IDE;
-    uint32_t RTR;
-    uint32_t DLC;
-    uint32_t TransmitGlobalTime;
+    uint32_t StdId; uint32_t ExtId; uint32_t IDE; uint32_t RTR;
+    uint32_t DLC; uint32_t TransmitGlobalTime;
 } CAN_TxHeaderTypeDef;
-
 typedef struct {
-    uint32_t FilterIdHigh;
-    uint32_t FilterIdLow;
-    uint32_t FilterMaskIdHigh;
-    uint32_t FilterMaskIdLow;
-    uint32_t FilterFIFOAssignment;
-    uint32_t FilterBank;
-    uint32_t FilterMode;
-    uint32_t FilterScale;
-    uint32_t FilterActivation;
-    uint32_t SlaveStartFilterBank;
+    uint32_t FilterIdHigh; uint32_t FilterIdLow;
+    uint32_t FilterMaskIdHigh; uint32_t FilterMaskIdLow;
+    uint32_t FilterFIFOAssignment; uint32_t FilterBank;
+    uint32_t FilterMode; uint32_t FilterScale;
+    uint32_t FilterActivation; uint32_t SlaveStartFilterBank;
 } CAN_FilterTypeDef;
-
 #define CAN_ID_STD              0x00000000u
 #define CAN_ID_EXT              0x00000004u
 #define CAN_RTR_DATA            0x00000000u
@@ -94,33 +48,54 @@ typedef struct {
 #define CAN_SJW_1TQ             0u
 #define CAN_BS1_13TQ            0u
 #define CAN_BS2_4TQ             0u
+#define CAN_ESR_EWGF            0x00000001u
+#define CAN_ESR_EPVF            0x00000002u
 #define CAN_ESR_BOFF            0x00000004u
-
+#define CAN_ESR_TEC_Pos         16u
+#define CAN_ESR_REC_Pos         24u
+#define CAN_IT_ERROR_WARNING    0x00000100u
+#define CAN_IT_ERROR_PASSIVE    0x00000200u
+#define CAN_IT_BUSOFF           0x00000400u
+#define CAN_FLAG_ERRI           0x00060000u
+typedef uint32_t IRQn_Type;
+#define CAN1_SCE_IRQn           ((IRQn_Type)22u)
+static inline void HAL_NVIC_SetPriority(IRQn_Type i,uint32_t p,uint32_t s){(void)i;(void)p;(void)s;}
+static inline void HAL_NVIC_EnableIRQ(IRQn_Type i){(void)i;}
+#define CAN1                    ((CAN_TypeDef *)0)
 static inline HAL_StatusTypeDef HAL_CAN_Init(CAN_HandleTypeDef *h){(void)h;return HAL_OK;}
 static inline HAL_StatusTypeDef HAL_CAN_ConfigFilter(CAN_HandleTypeDef *h,CAN_FilterTypeDef *f){(void)h;(void)f;return HAL_OK;}
 static inline HAL_StatusTypeDef HAL_CAN_Start(CAN_HandleTypeDef *h){(void)h;return HAL_OK;}
+static inline HAL_StatusTypeDef HAL_CAN_Stop(CAN_HandleTypeDef *h){(void)h;return HAL_OK;}
+static inline HAL_StatusTypeDef HAL_CAN_ActivateNotification(CAN_HandleTypeDef *h,uint32_t it){(void)h;(void)it;return HAL_OK;}
+static inline void HAL_CAN_IRQHandler(CAN_HandleTypeDef *h){(void)h;}
 static inline uint32_t HAL_CAN_GetTxMailboxesFreeLevel(CAN_HandleTypeDef *h){(void)h;return 3u;}
 static inline HAL_StatusTypeDef HAL_CAN_AddTxMessage(CAN_HandleTypeDef *h,CAN_TxHeaderTypeDef *hdr,uint8_t *d,uint32_t *mb){(void)h;(void)hdr;(void)d;(void)mb;return HAL_OK;}
 static inline uint32_t HAL_CAN_GetRxFifoFillLevel(CAN_HandleTypeDef *h,uint32_t f){(void)h;(void)f;return 0u;}
 static inline HAL_StatusTypeDef HAL_CAN_GetRxMessage(CAN_HandleTypeDef *h,uint32_t f,CAN_RxHeaderTypeDef *hdr,uint8_t *d){(void)h;(void)f;(void)hdr;(void)d;return HAL_OK;}
-
-/* ---- GPIO (minimal, for can_driver GPIO init) ---- */
+#define __HAL_CAN_CLEAR_FLAG(h,f) ((void)(h),(void)(f))
 typedef struct { uint32_t dummy; } GPIO_TypeDef;
-typedef struct {
-    uint32_t Pin; uint32_t Mode; uint32_t Pull; uint32_t Speed; uint32_t Alternate;
-} GPIO_InitTypeDef;
+typedef struct { uint32_t Pin; uint32_t Mode; uint32_t Pull; uint32_t Speed; uint32_t Alternate; } GPIO_InitTypeDef;
+typedef uint32_t GPIO_PinState;
+#define GPIO_PIN_RESET  0u
+#define GPIO_PIN_SET    1u
 static inline void HAL_GPIO_Init(GPIO_TypeDef *p,GPIO_InitTypeDef *g){(void)p;(void)g;}
+static inline GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *p,uint32_t pin){(void)p;(void)pin;return GPIO_PIN_SET;}
 #define GPIOB ((GPIO_TypeDef *)0)
+#define GPIOC ((GPIO_TypeDef *)0)
+#define GPIO_PIN_2              (1u<<2)
+#define GPIO_PIN_8              (1u<<8)
+#define GPIO_PIN_9              (1u<<9)
 #define GPIO_PIN_12             (1u<<12)
 #define GPIO_PIN_13             (1u<<13)
 #define GPIO_MODE_AF_PP         0u
+#define GPIO_MODE_INPUT         1u
 #define GPIO_NOPULL             0u
+#define GPIO_PULLUP             1u
 #define GPIO_SPEED_FREQ_HIGH    0u
+#define GPIO_AF9_CAN1           9u
 #define GPIO_AF9_CAN2           9u
-
-/* ---- RCC stubs ---- */
 #define __HAL_RCC_CAN1_CLK_ENABLE()  ((void)0)
 #define __HAL_RCC_CAN2_CLK_ENABLE()  ((void)0)
 #define __HAL_RCC_GPIOB_CLK_ENABLE() ((void)0)
-
+#define __HAL_RCC_GPIOC_CLK_ENABLE() ((void)0)
 #endif /* STM32F7XX_HAL_H */

test/test_can_watchdog.c

@@ -0,0 +1,224 @@
+/*
+ * test_can_watchdog.c — Unit tests for CAN bus watchdog and error recovery (Issue #694).
+ *
+ * Build (host, no hardware):
+ *   gcc -I include -I test/stubs -DTEST_HOST \
+ *       -o /tmp/test_can_watchdog src/can_driver.c test/test_can_watchdog.c
+ *
+ * All timing driven through now_ms argument passed to can_driver_watchdog_tick().
+ * ESR state injected via can_driver_inject_esr().
+ */
+
+/* Include implementation directly (pulls stm32 stub via -I test/stubs) */
+#include "../src/can_driver.c"
+
+/* ---- Test framework ---- */
+#include <stdio.h>
+#include <string.h>
+
+static int g_pass = 0;
+static int g_fail = 0;
+
+#define ASSERT(cond, msg) \
+    do { \
+        if (cond) { g_pass++; } \
+        else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \
+    } while (0)
+
+/* HAL_GetTick stub */
+uint32_t HAL_GetTick(void) { return 0u; }
+
+/* Reset driver state between tests */
+static void _reset(void)
+{
+    can_driver_inject_esr(0u);
+    can_driver_init();
+}
+
+/* ---- Tests ---- */
+
+static void test_nominal_no_errors(void)
+{
+    _reset();
+    can_error_state_t st = can_driver_watchdog_tick(0u);
+    ASSERT(st == CAN_ERR_NOMINAL, "clean ESR: state is NOMINAL");
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.restart_count == 0u, "nominal: restart_count 0");
+    ASSERT(wd.busoff_count  == 0u, "nominal: busoff_count 0");
+}
+
+static void test_errwarn_detection(void)
+{
+    _reset();
+    can_driver_inject_esr(CAN_ESR_EWGF);
+    can_error_state_t st = can_driver_watchdog_tick(1000u);
+    ASSERT(st == CAN_ERR_WARNING, "EWGF set: state is WARNING");
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.errwarn_count == 1u, "errwarn_count incremented");
+}
+
+static void test_errwarn_cleared_returns_nominal(void)
+{
+    _reset();
+    /* Enter warning state */
+    can_driver_inject_esr(CAN_ESR_EWGF);
+    can_driver_watchdog_tick(100u);
+
+    /* Clear ESR - should de-escalate */
+    can_driver_inject_esr(0u);
+    can_error_state_t st = can_driver_watchdog_tick(200u);
+    ASSERT(st == CAN_ERR_NOMINAL, "ESR cleared: de-escalate to NOMINAL");
+}
+
+static void test_errpassive_detection(void)
+{
+    _reset();
+    can_driver_inject_esr(CAN_ESR_EPVF);
+    can_error_state_t st = can_driver_watchdog_tick(500u);
+    ASSERT(st == CAN_ERR_ERROR_PASSIVE, "EPVF set: state is ERROR_PASSIVE");
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.errpassive_count == 1u, "errpassive_count incremented");
+}
+
+static void test_busoff_detection(void)
+{
+    _reset();
+    can_driver_inject_esr(CAN_ESR_BOFF);
+    can_error_state_t st = can_driver_watchdog_tick(1000u);
+    ASSERT(st == CAN_ERR_BUS_OFF, "BOFF set: state is BUS_OFF");
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.busoff_count == 1u, "busoff_count incremented");
+    ASSERT(wd.busoff_ms == 1000u, "busoff_ms recorded");
+}
+
+static void test_busoff_sets_stats_bus_off(void)
+{
+    _reset();
+    can_driver_inject_esr(CAN_ESR_BOFF);
+    can_driver_watchdog_tick(500u);
+
+    can_stats_t cs;
+    can_driver_get_stats(&cs);
+    ASSERT(cs.bus_off == 1u, "bus-off: stats.bus_off = 1");
+}
+
+static void test_busoff_no_restart_before_timeout(void)
+{
+    _reset();
+    can_driver_inject_esr(CAN_ESR_BOFF);
+    can_driver_watchdog_tick(0u);
+
+    /* Call again just before CAN_WDOG_RESTART_MS */
+    can_error_state_t st = can_driver_watchdog_tick(CAN_WDOG_RESTART_MS - 1u);
+    ASSERT(st == CAN_ERR_BUS_OFF, "before restart timeout: still BUS_OFF");
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.restart_count == 0u, "no restart yet");
+}
+
+static void test_busoff_auto_restart_after_timeout(void)
+{
+    _reset();
+    /* Inject bus-off at t=0 */
+    can_driver_inject_esr(CAN_ESR_BOFF);
+    can_driver_watchdog_tick(0u);
+
+    /* After restart timeout, HAL_CAN_Stop/Start clears s_test_esr internally */
+    can_error_state_t st = can_driver_watchdog_tick(CAN_WDOG_RESTART_MS);
+    ASSERT(st == CAN_ERR_NOMINAL, "after restart timeout: state is NOMINAL");
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.restart_count == 1u, "restart_count incremented to 1");
+    ASSERT(wd.busoff_ms == 0u,    "busoff_ms cleared after restart");
+}
+
+static void test_busoff_stats_cleared_after_restart(void)
+{
+    _reset();
+    can_driver_inject_esr(CAN_ESR_BOFF);
+    can_driver_watchdog_tick(0u);
+    can_driver_watchdog_tick(CAN_WDOG_RESTART_MS);
+
+    can_stats_t cs;
+    can_driver_get_stats(&cs);
+    ASSERT(cs.bus_off == 0u, "after restart: stats.bus_off cleared");
+}
+
+static void test_busoff_count_only_increments_once_per_event(void)
+{
+    _reset();
+    can_driver_inject_esr(CAN_ESR_BOFF);
+    /* Call tick multiple times while still in bus-off */
+    can_driver_watchdog_tick(0u);
+    can_driver_watchdog_tick(10u);
+    can_driver_watchdog_tick(50u);
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.busoff_count == 1u, "repeated BOFF ticks: count stays 1");
+}
+
+static void test_multiple_restart_cycles(void)
+{
+    _reset();
+    /* Cycle 1 */
+    can_driver_inject_esr(CAN_ESR_BOFF);
+    can_driver_watchdog_tick(0u);
+    can_driver_watchdog_tick(CAN_WDOG_RESTART_MS);  /* restart */
+
+    /* Cycle 2: inject bus-off again */
+    can_driver_inject_esr(CAN_ESR_BOFF);
+    can_driver_watchdog_tick(CAN_WDOG_RESTART_MS + 1u);
+    can_driver_watchdog_tick(CAN_WDOG_RESTART_MS * 2u + 1u);  /* restart */
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.restart_count == 2u, "two restart cycles: restart_count = 2");
+    ASSERT(wd.busoff_count  == 2u, "two restart cycles: busoff_count = 2");
+}
+
+static void test_tec_rec_extracted(void)
+{
+    _reset();
+    /* Inject ESR with TEC=0xAB (bits 23:16) and REC=0xCD (bits 31:24) */
+    uint32_t fake_esr = (0xABu << 16) | (0xCDu << 24);
+    can_driver_inject_esr(fake_esr);
+    can_driver_watchdog_tick(100u);
+
+    can_wdog_t wd;
+    can_driver_get_wdog(&wd);
+    ASSERT(wd.tec == 0xABu, "TEC extracted from ESR correctly");
+    ASSERT(wd.rec == 0xCDu, "REC extracted from ESR correctly");
+}
+
+/* ---- main ---- */
+int main(void)
+{
+    printf("=== test_can_watchdog ===\n");
+
+    test_nominal_no_errors();
+    test_errwarn_detection();
+    test_errwarn_cleared_returns_nominal();
+    test_errpassive_detection();
+    test_busoff_detection();
+    test_busoff_sets_stats_bus_off();
+    test_busoff_no_restart_before_timeout();
+    test_busoff_auto_restart_after_timeout();
+    test_busoff_stats_cleared_after_restart();
+    test_busoff_count_only_increments_once_per_event();
+    test_multiple_restart_cycles();
+    test_tec_rec_extracted();
+
+    printf("\nResults: %d passed, %d failed\n", g_pass, g_fail);
+    return g_fail ? 1 : 0;
+}

ui/can_monitor_panel.js

@@ -293,11 +293,51 @@ function drawCompass(yaw) {
 
 // ── ROS connection ────────────────────────────────────────────────────────────
 
-function connect(url) {
+// ── Auto-reconnect state ──────────────────────────────────────────────────────
+
+const RECONNECT_BASE_MS = 2000;
+const RECONNECT_MAX_MS  = 30000;
+const RECONNECT_FACTOR  = 1.5;
+
+let reconnectDelay  = RECONNECT_BASE_MS;
+let reconnectTimer  = null;
+let reconnectTick   = null;
+let reconnectTarget = null;
+
+function cancelReconnect() {
+  if (reconnectTimer) { clearTimeout(reconnectTimer);  reconnectTimer = null; }
+  if (reconnectTick)  { clearInterval(reconnectTick);  reconnectTick  = null; }
+}
+
+function scheduleReconnect(url) {
+  cancelReconnect();
+  let secs = Math.round(reconnectDelay / 1000);
+  $('conn-label').textContent = `Retry in ${secs}s…`;
+
+  reconnectTick = setInterval(() => {
+    secs = Math.max(0, secs - 1);
+    if (secs > 0) $('conn-label').textContent = `Retry in ${secs}s…`;
+  }, 1000);
+
+  reconnectTimer = setTimeout(() => {
+    reconnectTimer = null;
+    connect(url, true);
+  }, reconnectDelay);
+
+  reconnectDelay = Math.min(reconnectDelay * RECONNECT_FACTOR, RECONNECT_MAX_MS);
+}
+
+function connect(url, isAutoRetry) {
+  cancelReconnect();
+  reconnectTarget = url;
+  if (!isAutoRetry) reconnectDelay = RECONNECT_BASE_MS;
+
   if (ros) { try { ros.close(); } catch(_) {} }
   ros = new ROSLIB.Ros({ url });
 
   ros.on('connection', () => {
+    reconnectDelay = RECONNECT_BASE_MS;
+    cancelReconnect();
     $('conn-dot').className = 'connected';
     $('conn-label').style.color = '#22c55e';
     $('conn-label').textContent = 'Connected';
@@ -315,7 +355,7 @@ function connect(url) {
   ros.on('close', () => {
     $('conn-dot').className = '';
     $('conn-label').style.color = '#6b7280';
-    $('conn-label').textContent = 'Disconnected';
+    scheduleReconnect(reconnectTarget || $('ws-input').value.trim());
   });
 }
 
@@ -405,8 +445,8 @@ function setupTopics() {
 
 // ── UI wiring ─────────────────────────────────────────────────────────────────
 
-$('btn-connect').addEventListener('click', () => connect($('ws-input').value.trim()));
-$('ws-input').addEventListener('keydown', e => { if (e.key === 'Enter') connect($('ws-input').value.trim()); });
+$('btn-connect').addEventListener('click', () => connect($('ws-input').value.trim(), false));
+$('ws-input').addEventListener('keydown', e => { if (e.key === 'Enter') connect($('ws-input').value.trim(), false); });
 
 // ── Auto-connect on load ──────────────────────────────────────────────────────
 

ui/dashboard.js

@@ -22,6 +22,7 @@ const PANELS = [
   { id: 'settings', watchTopic: null,                              msgType: null },  // service-based
   { id: 'gimbal',   watchTopic: '/gimbal/state',                   msgType: 'geometry_msgs/Vector3' },
   { id: 'can',      watchTopic: '/vesc/left/state',               msgType: 'std_msgs/String' },
+  { id: 'gps',      watchTopic: 'saltybot/gps/fix',              msgType: 'std_msgs/String' },
 ];
 
 // ── State ──────────────────────────────────────────────────────────────────
@@ -188,6 +189,13 @@ function setupTopics() {
   });
   vescWatch.subscribe(() => { markPanelLive('can'); });
 
+  // ── GPS fix (for GPS map card liveness) ──
+  const gpsWatch = new ROSLIB.Topic({
+    ros, name: 'saltybot/gps/fix',
+    messageType: 'std_msgs/String', throttle_rate: 2000,
+  });
+  gpsWatch.subscribe(() => { markPanelLive('gps'); });
+
   // ── cmd_vel monitor (for gamepad card liveness) ──
   const cmdVelWatch = new ROSLIB.Topic({
     ros, name: '/cmd_vel',

ui/gps_map_panel.css

@@ -0,0 +1,165 @@
+/* gps_map_panel.css — Robot GPS Live Map (Issue #709 companion) */
+
+*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
+
+:root {
+  --bg0:       #050510;
+  --bg1:       #070712;
+  --bg2:       #0a0a1a;
+  --border:    #0c2a3a;
+  --border2:   #1e3a5f;
+  --text-dim:  #374151;
+  --text-mid:  #6b7280;
+  --text-base: #9ca3af;
+  --cyan:      #06b6d4;
+  --cyan-dim:  #0e4f69;
+  --green:     #22c55e;
+  --amber:     #f59e0b;
+  --red:       #ef4444;
+  --orange:    #f97316;
+}
+
+html, body {
+  height: 100%;
+  overflow: hidden;
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: var(--bg0);
+  color: var(--text-base);
+  display: flex;
+  flex-direction: column;
+}
+
+/* ── Header ── */
+#header {
+  display: flex; align-items: center; gap: 10px;
+  padding: 6px 14px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--border);
+  flex-shrink: 0; flex-wrap: wrap;
+}
+.logo { color: var(--orange); font-weight: bold; letter-spacing: .15em; font-size: 13px; }
+.logo a { color: inherit; text-decoration: none; }
+.logo a:hover { text-decoration: underline; }
+
+#conn-bar { display: flex; align-items: center; gap: 6px; flex: 1; min-width: 240px; }
+#conn-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--text-dim); flex-shrink: 0; transition: background .3s;
+}
+#conn-dot.connected { background: var(--green); }
+#conn-dot.error     { background: var(--red); animation: blink 1s infinite; }
+
+@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.4} }
+
+#ws-input {
+  flex: 1; min-width: 160px; max-width: 240px;
+  background: var(--bg2); border: 1px solid var(--border2); border-radius: 4px;
+  color: #67e8f9; padding: 2px 8px; font-family: monospace; font-size: 11px;
+}
+#ws-input:focus { outline: none; border-color: var(--cyan); }
+
+.hdr-btn {
+  padding: 3px 10px; border-radius: 4px; border: 1px solid var(--border2);
+  background: var(--bg2); color: #67e8f9; font-family: monospace;
+  font-size: 10px; font-weight: bold; cursor: pointer;
+}
+.hdr-btn:hover { background: var(--cyan-dim); }
+#conn-label { font-size: 10px; color: var(--text-dim); }
+
+/* ── Status bar ── */
+#status-bar {
+  display: flex; gap: 8px; align-items: center; flex-wrap: wrap;
+  padding: 4px 14px; background: var(--bg1);
+  border-bottom: 1px solid var(--border); font-size: 10px;
+}
+.sys-badge {
+  padding: 2px 8px; border-radius: 3px; font-weight: bold;
+  border: 1px solid; letter-spacing: .05em;
+}
+.badge-ok    { background: #052e16; border-color: #166534; color: #4ade80; }
+.badge-warn  { background: #451a03; border-color: #92400e; color: #fcd34d; }
+.badge-error { background: #450a0a; border-color: #991b1b; color: #f87171; animation: blink 1s infinite; }
+.badge-stale { background: #111827; border-color: #374151; color: #6b7280; }
+#last-update { color: var(--text-mid); margin-left: auto; }
+
+/* ── Main layout ── */
+#main {
+  flex: 1; display: flex; min-height: 0;
+}
+
+/* ── Map ── */
+#map-wrap { flex: 1; position: relative; min-width: 0; }
+#map      { width: 100%; height: 100%; }
+
+/* dark tile filter */
+.leaflet-tile { filter: brightness(.8) invert(1) hue-rotate(200deg) saturate(.65); }
+.leaflet-container { background: #090915; }
+
+/* ── Map controls ── */
+#map-controls {
+  position: absolute; top: 10px; right: 10px; z-index: 1000;
+  display: flex; flex-direction: column; gap: 4px;
+}
+.map-btn {
+  padding: 4px 9px; border-radius: 4px; border: 1px solid var(--border);
+  background: rgba(7,7,18,.88); color: var(--cyan);
+  font-family: monospace; font-size: 10px; font-weight: bold;
+  cursor: pointer; backdrop-filter: blur(4px);
+}
+.map-btn:hover { background: rgba(14,79,105,.88); }
+.map-btn.active { background: rgba(6,182,212,.22); border-color: var(--cyan); }
+
+/* ── Sidebar ── */
+#sidebar {
+  width: 210px; flex-shrink: 0;
+  background: var(--bg1); border-left: 1px solid var(--border);
+  display: flex; flex-direction: column; overflow-y: auto;
+}
+
+.stat-section { padding: 10px 12px; border-bottom: 1px solid var(--border); }
+.section-title {
+  font-size: 8px; font-weight: bold; letter-spacing: .15em;
+  color: #0891b2; text-transform: uppercase; margin-bottom: 8px;
+}
+
+.big-stat { display: flex; flex-direction: column; align-items: center; padding: 4px 0; }
+.big-val  { font-size: 36px; font-weight: bold; font-family: monospace; line-height: 1; }
+.big-unit { font-size: 11px; color: var(--text-mid); margin-top: 2px; }
+.big-lbl  { font-size: 8px; color: var(--text-dim); margin-top: 4px; letter-spacing: .1em; }
+
+.kv-grid  { display: grid; grid-template-columns: 1fr 1fr; gap: 4px; }
+.kv-cell  { background: var(--bg2); border: 1px solid var(--border); border-radius: 5px; padding: 5px 7px; }
+.kv-lbl   { font-size: 8px; color: var(--text-dim); margin-bottom: 2px; }
+.kv-val   { font-size: 13px; font-weight: bold; font-family: monospace; }
+
+.coord-row { font-size: 10px; color: var(--text-mid); margin-bottom: 2px; }
+.coord-val { color: var(--cyan); font-family: monospace; }
+
+/* accuracy bar */
+.acc-wrap { height: 4px; background: var(--bg2); border-radius: 2px; margin-top: 6px; overflow: hidden; }
+.acc-bar  { height: 100%; border-radius: 2px; transition: width .4s, background .4s; }
+
+/* compass */
+#compass-wrap { display: flex; justify-content: center; padding: 6px 0; }
+
+/* trail log */
+#trail-log {
+  flex: 1; overflow-y: auto; padding: 6px 12px;
+  display: flex; flex-direction: column; gap: 3px;
+}
+.trail-entry { font-size: 9px; color: var(--text-dim); border-bottom: 1px solid #0b1a28; padding-bottom: 3px; }
+.te-coord { color: var(--cyan); }
+.te-spd   { color: var(--orange); }
+
+/* bottom bar */
+#bottombar {
+  display: flex; align-items: center; gap: 12px; flex-wrap: wrap;
+  padding: 4px 14px; background: var(--bg1);
+  border-top: 1px solid var(--border); font-size: 10px;
+  flex-shrink: 0;
+}
+.bb-lbl { color: var(--text-dim); }
+#last-msg { margin-left: auto; color: var(--text-dim); }
+
+@media (max-width: 600px) { #sidebar { display: none; } }

ui/gps_map_panel.html

@@ -0,0 +1,138 @@
+<!DOCTYPE html>
+<html lang="en">
+<head>
+<meta charset="UTF-8">
+<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
+<title>Saltybot — GPS Map</title>
+<link rel="stylesheet" href="gps_map_panel.css">
+<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css" />
+<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
+<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
+</head>
+<body>
+
+<!-- ── Header ── -->
+<div id="header">
+  <div class="logo">⚡ <a href="index.html">SALTYBOT</a> — GPS MAP</div>
+  <div id="conn-bar">
+    <div id="conn-dot"></div>
+    <input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
+    <button class="hdr-btn" id="btn-connect">CONNECT</button>
+    <span id="conn-label">Not connected</span>
+  </div>
+</div>
+
+<!-- ── Status bar ── -->
+<div id="status-bar">
+  <span style="color:var(--text-dim);font-size:10px">ROBOT FIX</span>
+  <span class="sys-badge badge-stale" id="badge-fix">STALE</span>
+  <span style="color:var(--text-dim)">│</span>
+  <span style="color:var(--text-dim);font-size:10px">VELOCITY</span>
+  <span class="sys-badge badge-stale" id="badge-vel">STALE</span>
+  <span style="color:var(--text-dim)">│</span>
+  <span style="color:var(--text-dim);font-size:10px">PHONE</span>
+  <span class="sys-badge badge-stale" id="badge-phone">NO DATA</span>
+  <span id="last-update">Awaiting data…</span>
+</div>
+
+<!-- ── Main ── -->
+<div id="main">
+
+  <!-- Map -->
+  <div id="map-wrap">
+    <div id="map"></div>
+    <div id="map-controls">
+      <button class="map-btn active" id="btn-follow">⊙ FOLLOW</button>
+      <button class="map-btn" id="btn-clear">✕ CLEAR</button>
+    </div>
+  </div>
+
+  <!-- Sidebar -->
+  <div id="sidebar">
+
+    <!-- Speed -->
+    <div class="stat-section">
+      <div class="section-title">Speed</div>
+      <div class="big-stat">
+        <div class="big-val" id="val-speed" style="color:#f97316">—</div>
+        <div class="big-unit">km/h</div>
+        <div class="big-lbl">GROUND SPEED</div>
+      </div>
+    </div>
+
+    <!-- Altitude + heading -->
+    <div class="stat-section">
+      <div class="section-title">Altitude &amp; Heading</div>
+      <div class="kv-grid">
+        <div class="kv-cell">
+          <div class="kv-lbl">ALT m</div>
+          <div class="kv-val" id="val-alt">—</div>
+        </div>
+        <div class="kv-cell">
+          <div class="kv-lbl">HDG °</div>
+          <div class="kv-val" id="val-hdg">—</div>
+        </div>
+      </div>
+      <div id="compass-wrap">
+        <canvas id="compass-canvas" width="110" height="110"></canvas>
+      </div>
+    </div>
+
+    <!-- Fix quality -->
+    <div class="stat-section">
+      <div class="section-title">Fix Quality</div>
+      <div class="kv-grid">
+        <div class="kv-cell">
+          <div class="kv-lbl">STATUS</div>
+          <div class="kv-val" id="val-stat">—</div>
+        </div>
+        <div class="kv-cell">
+          <div class="kv-lbl">FIXES</div>
+          <div class="kv-val" id="val-fixes">0</div>
+        </div>
+      </div>
+      <div style="margin-top:8px">
+        <div class="coord-row">LAT <span class="coord-val" id="val-lat">—</span></div>
+        <div class="coord-row">LON <span class="coord-val" id="val-lon">—</span></div>
+      </div>
+    </div>
+
+    <!-- Phone position -->
+    <div class="stat-section">
+      <div class="section-title" style="color:#3b82f6">📱 Phone / User</div>
+      <div style="margin-bottom:6px">
+        <div class="coord-row">LAT <span class="coord-val" id="phone-lat" style="color:#60a5fa">—</span></div>
+        <div class="coord-row">LON <span class="coord-val" id="phone-lon" style="color:#60a5fa">—</span></div>
+      </div>
+      <div class="kv-grid">
+        <div class="kv-cell">
+          <div class="kv-lbl">SPEED</div>
+          <div class="kv-val" id="phone-spd" style="color:#60a5fa">—</div>
+        </div>
+        <div class="kv-cell">
+          <div class="kv-lbl">ACCURACY</div>
+          <div class="kv-val" id="phone-acc">—</div>
+        </div>
+      </div>
+      <div class="coord-row" style="margin-top:6px">SRC <span id="phone-provider" style="color:#6b7280;font-family:monospace">—</span></div>
+    </div>
+
+    <!-- Trail log -->
+    <div class="section-title" style="padding:8px 12px 2px">Trail log</div>
+    <div id="trail-log"></div>
+
+  </div>
+</div>
+
+<!-- ── Bottom bar ── -->
+<div id="bottombar">
+  <span class="bb-lbl">FIXES</span><span id="bb-fixes">0</span>
+  <span class="bb-lbl">TRAIL PTS</span><span id="bb-trail">0</span>
+  <span class="bb-lbl">ROBOT MSGS</span><span id="bb-msgs">0</span>
+  <span class="bb-lbl">PHONE MSGS</span><span id="bb-phone">0</span>
+  <span id="last-msg">No data</span>
+</div>
+
+<script src="gps_map_panel.js"></script>
+</body>
+</html>

ui/gps_map_panel.js

@@ -0,0 +1,430 @@
+/**
+ * gps_map_panel.js — Robot GPS Live Map (Issue #709 companion)
+ *
+ * Subscribes via rosbridge to:
+ *   saltybot/gps/fix    std_msgs/String JSON — {lat, lon, alt, stat, t}
+ *   saltybot/gps/vel    std_msgs/String JSON — {spd, hdg, t}
+ *   saltybot/phone/gps  std_msgs/String JSON — {ts, lat, lon, alt_m, accuracy_m, speed_ms, bearing_deg, provider}
+ *
+ * Renders a Leaflet map with:
+ *   - Live robot position marker (cyan/orange) with heading arrow
+ *   - Live phone/user position marker (blue) with bearing arrow
+ *   - Orange robot trail + blue phone trail (up to MAX_TRAIL points each)
+ *   - Sidebar: speed, altitude, heading compass, fix status, coordinates, phone position
+ *   - FOLLOW mode (auto-pan) toggled by button or manual drag
+ *   - Stale detection at 5 s
+ */
+
+'use strict';
+
+// ── Constants ─────────────────────────────────────────────────────────────────
+
+const MAX_TRAIL      = 2000;
+const MAX_LOG        = 80;
+const STALE_MS       = 5000;
+const RECONNECT_BASE = 2000;
+const RECONNECT_MAX  = 30000;
+const RECONNECT_MUL  = 1.5;
+
+// ── State ─────────────────────────────────────────────────────────────────────
+
+let ros = null;
+
+const gps = {
+  lat: null, lon: null, alt: null, stat: null,
+  spd: null, hdg: null,
+  fixes: 0, msgs: 0,
+  tsfix: 0, tsvel: 0,
+};
+
+const phone = {
+  lat: null, lon: null, alt: null,
+  spd: null, bearing: null,
+  accuracy: null, provider: null,
+  msgs: 0, ts: 0,
+};
+
+let trail      = [];   // [{lat, lon}]
+let phoneTrail = [];   // [{lat, lon}]
+let logEntries = [];
+let followMode = true;
+
+let reconnDelay = RECONNECT_BASE;
+let reconnTimer = null;
+let reconnTick  = null;
+
+// ── DOM ───────────────────────────────────────────────────────────────────────
+
+const $ = id => document.getElementById(id);
+
+// ── Leaflet map ───────────────────────────────────────────────────────────────
+
+const map = L.map('map', { center: [43.45, -79.76], zoom: 15, zoomControl: true });
+
+L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
+  attribution: '© OpenStreetMap contributors',
+  maxZoom: 19,
+}).addTo(map);
+
+// Heading-aware SVG marker icon (rotated via CSS transform on the div)
+function makeIcon(hdg) {
+  const rot = (hdg != null && hdg >= 0) ? hdg : 0;
+  return L.divIcon({
+    className: '',
+    html: `<svg width="28" height="36" viewBox="0 0 28 36" xmlns="http://www.w3.org/2000/svg"
+              style="transform:rotate(${rot}deg);transform-origin:14px 20px">
+      <!-- body circle -->
+      <circle cx="14" cy="20" r="10" fill="#06b6d4" fill-opacity=".3" stroke="#06b6d4" stroke-width="2"/>
+      <circle cx="14" cy="20" r="4"  fill="#06b6d4"/>
+      <!-- heading arrow (points up = north when hdg=0) -->
+      <polygon points="14,2 10,14 18,14" fill="#f97316" opacity=".9"/>
+    </svg>`,
+    iconSize:   [28, 36],
+    iconAnchor: [14, 20],
+  });
+}
+
+// Robot marker (cyan body, orange heading arrow)
+let marker   = null;
+let polyline = L.polyline([], {
+  color: '#f97316', weight: 2.5, opacity: .8, lineJoin: 'round',
+}).addTo(map);
+
+// Phone/user marker (blue body, blue bearing arrow)
+function makePhoneIcon(bearing) {
+  const rot = (bearing != null && bearing >= 0) ? bearing : 0;
+  return L.divIcon({
+    className: '',
+    html: `<svg width="24" height="32" viewBox="0 0 24 32" xmlns="http://www.w3.org/2000/svg"
+              style="transform:rotate(${rot}deg);transform-origin:12px 17px">
+      <!-- body circle -->
+      <circle cx="12" cy="17" r="8" fill="#3b82f6" fill-opacity=".35" stroke="#3b82f6" stroke-width="2"/>
+      <circle cx="12" cy="17" r="3.5" fill="#3b82f6"/>
+      <!-- bearing arrow (points up = north when bearing=0) -->
+      <polygon points="12,2 9,12 15,12" fill="#60a5fa" opacity=".9"/>
+    </svg>`,
+    iconSize:   [24, 32],
+    iconAnchor: [12, 17],
+  });
+}
+
+let phoneMarker   = null;
+let phonePolyline = L.polyline([], {
+  color: '#3b82f6', weight: 2, opacity: .7, lineJoin: 'round',
+}).addTo(map);
+
+// ── Canvas compass ────────────────────────────────────────────────────────────
+
+function drawCompass(hdg) {
+  const canvas = $('compass-canvas');
+  const ctx = canvas.getContext('2d');
+  const W = canvas.width, H = canvas.height, cx = W / 2, cy = H / 2;
+  const r = cx - 4;
+
+  ctx.clearRect(0, 0, W, H);
+  ctx.beginPath(); ctx.arc(cx, cy, r, 0, Math.PI * 2);
+  ctx.fillStyle = '#070712'; ctx.fill();
+  ctx.strokeStyle = '#1e3a5f'; ctx.lineWidth = 1.5; ctx.stroke();
+
+  const cards = ['N', 'E', 'S', 'W'];
+  ctx.font = 'bold 8px monospace'; ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
+  cards.forEach((c, i) => {
+    const a  = i * Math.PI / 2;
+    const lx = cx + Math.sin(a) * (r - 9);
+    const ly = cy - Math.cos(a) * (r - 9);
+    ctx.fillStyle = c === 'N' ? '#ef4444' : '#4b5563';
+    ctx.fillText(c, lx, ly);
+  });
+
+  if (hdg == null || hdg < 0) {
+    ctx.fillStyle = '#374151'; ctx.font = '9px monospace';
+    ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
+    ctx.fillText('—', cx, cy);
+    return;
+  }
+
+  const rad     = (hdg - 90) * Math.PI / 180;
+  const tipLen  = r - 14;
+  const tailLen = 9;
+
+  ctx.save(); ctx.translate(cx, cy);
+  ctx.beginPath();
+  ctx.moveTo(Math.cos(rad) * tipLen,       Math.sin(rad) * tipLen);
+  ctx.lineTo(Math.cos(rad + Math.PI) * tailLen, Math.sin(rad + Math.PI) * tailLen);
+  ctx.strokeStyle = '#f97316'; ctx.lineWidth = 2.5; ctx.lineCap = 'round'; ctx.stroke();
+  ctx.beginPath(); ctx.arc(0, 0, 3, 0, Math.PI * 2);
+  ctx.fillStyle = '#9ca3af'; ctx.fill();
+  ctx.restore();
+}
+
+// ── Render ────────────────────────────────────────────────────────────────────
+
+function setSysBadge(id, level, text) {
+  const el = $(id); if (!el) return;
+  el.className = `sys-badge badge-${level}`;
+  if (text !== undefined) el.textContent = text;
+}
+
+function render() {
+  const now = Date.now();
+
+  // Stale badges
+  const fixStale   = now - gps.tsfix   > STALE_MS;
+  const velStale   = now - gps.tsvel   > STALE_MS;
+  const phoneStale = now - phone.ts    > STALE_MS;
+  setSysBadge('badge-fix',   fixStale   ? 'stale' : 'ok', fixStale   ? 'STALE' : 'LIVE');
+  setSysBadge('badge-vel',   velStale   ? 'stale' : 'ok', velStale   ? 'STALE' : 'LIVE');
+  setSysBadge('badge-phone', phone.ts === 0 ? 'stale' : phoneStale ? 'stale' : 'ok',
+    phone.ts === 0 ? 'NO DATA' : phoneStale ? 'STALE' : 'LIVE');
+
+  // Speed
+  const spdKmh = gps.spd != null ? gps.spd : null;
+  const spdTxt = spdKmh != null ? spdKmh.toFixed(1) : '—';
+  const spdColor = spdKmh == null ? '#6b7280'
+                 : spdKmh < 2    ? '#22c55e'
+                 : spdKmh < 30   ? '#f97316'
+                 :                  '#ef4444';
+  $('val-speed').textContent = spdTxt;
+  $('val-speed').style.color = spdColor;
+
+  // Altitude
+  $('val-alt').textContent = gps.alt != null ? gps.alt.toFixed(1) : '—';
+
+  // Heading
+  const hdg = gps.hdg != null && gps.hdg >= 0 ? Math.round(gps.hdg) : null;
+  $('val-hdg').textContent = hdg != null ? hdg + '°' : '—';
+  drawCompass(gps.hdg);
+
+  // Fix status
+  const statMap = { 0: 'NO FIX', 1: 'GPS', 2: 'DGPS', 3: 'PPS', 4: 'RTK', 5: 'RTK-F', 6: 'EST' };
+  $('val-stat').textContent = gps.stat != null ? (statMap[gps.stat] || gps.stat) : '—';
+  $('val-stat').style.color = gps.stat ? '#22c55e' : '#ef4444';
+
+  $('val-fixes').textContent = gps.fixes;
+  $('bb-fixes').textContent  = gps.fixes;
+  $('bb-msgs').textContent   = gps.msgs;
+  $('bb-trail').textContent  = trail.length;
+
+  // Coordinates
+  $('val-lat').textContent = gps.lat != null ? gps.lat.toFixed(6) : '—';
+  $('val-lon').textContent = gps.lon != null ? gps.lon.toFixed(6) : '—';
+
+  // Phone position
+  $('phone-lat').textContent = phone.lat != null ? phone.lat.toFixed(6) : '—';
+  $('phone-lon').textContent = phone.lon != null ? phone.lon.toFixed(6) : '—';
+  const phoneSpdMs = phone.spd;
+  $('phone-spd').textContent = phoneSpdMs != null ? (phoneSpdMs * 3.6).toFixed(1) + ' km/h' : '—';
+  $('phone-acc').textContent = phone.accuracy != null ? phone.accuracy.toFixed(1) + ' m' : '—';
+  $('phone-provider').textContent = phone.provider || '—';
+  $('bb-phone').textContent = phone.msgs;
+}
+
+function updateMap(lat, lon, hdg) {
+  const icon = makeIcon(hdg);
+
+  if (!marker) {
+    marker = L.marker([lat, lon], { icon }).addTo(map);
+    if (followMode) map.setView([lat, lon], 16);
+  } else {
+    marker.setIcon(icon);
+    marker.setLatLng([lat, lon]);
+    if (followMode) map.panTo([lat, lon]);
+  }
+
+  trail.push({ lat, lon });
+  if (trail.length > MAX_TRAIL) trail.shift();
+  polyline.setLatLngs(trail.map(p => [p.lat, p.lon]));
+}
+
+function updatePhoneMap(lat, lon, bearing) {
+  const icon = makePhoneIcon(bearing);
+
+  if (!phoneMarker) {
+    phoneMarker = L.marker([lat, lon], { icon }).addTo(map);
+  } else {
+    phoneMarker.setIcon(icon);
+    phoneMarker.setLatLng([lat, lon]);
+  }
+
+  phoneTrail.push({ lat, lon });
+  if (phoneTrail.length > MAX_TRAIL) phoneTrail.shift();
+  phonePolyline.setLatLngs(phoneTrail.map(p => [p.lat, p.lon]));
+}
+
+function addLog(lat, lon, spd, t) {
+  const time = new Date((t || Date.now() / 1000) * 1000)
+    .toLocaleTimeString('en-US', { hour12: false });
+  const spdTxt = spd != null ? spd.toFixed(1) + ' km/h' : '?';
+  logEntries.unshift({ time, lat, lon, spdTxt });
+  if (logEntries.length > MAX_LOG) logEntries.pop();
+
+  $('trail-log').innerHTML = logEntries.map(e =>
+    `<div class="trail-entry">${e.time} <span class="te-coord">${e.lat.toFixed(5)},${e.lon.toFixed(5)}</span> <span class="te-spd">${e.spdTxt}</span></div>`
+  ).join('');
+}
+
+// ── ROS / auto-reconnect ──────────────────────────────────────────────────────
+
+function cancelReconn() {
+  if (reconnTimer) { clearTimeout(reconnTimer);  reconnTimer = null; }
+  if (reconnTick)  { clearInterval(reconnTick);  reconnTick  = null; }
+}
+
+function scheduleReconn(url) {
+  cancelReconn();
+  let secs = Math.round(reconnDelay / 1000);
+  $('conn-label').textContent = `Retry in ${secs}s…`;
+  reconnTick = setInterval(() => {
+    secs = Math.max(0, secs - 1);
+    if (secs > 0) $('conn-label').textContent = `Retry in ${secs}s…`;
+  }, 1000);
+  reconnTimer = setTimeout(() => { reconnTimer = null; connect(url, true); }, reconnDelay);
+  reconnDelay = Math.min(reconnDelay * RECONNECT_MUL, RECONNECT_MAX);
+}
+
+function connect(url, isRetry) {
+  cancelReconn();
+  if (!isRetry) reconnDelay = RECONNECT_BASE;
+  if (ros) { try { ros.close(); } catch(_) {} }
+  ros = new ROSLIB.Ros({ url });
+
+  ros.on('connection', () => {
+    reconnDelay = RECONNECT_BASE;
+    cancelReconn();
+    $('conn-dot').className = 'connected';
+    $('conn-label').style.color = '#22c55e';
+    $('conn-label').textContent = 'Connected';
+    $('ws-input').value = url;
+    localStorage.setItem('gps_map_ws_url', url);
+    setupTopics();
+  });
+
+  ros.on('error', () => {
+    $('conn-dot').className = 'error';
+    $('conn-label').style.color = '#ef4444';
+    $('conn-label').textContent = 'Error';
+  });
+
+  ros.on('close', () => {
+    $('conn-dot').className = '';
+    $('conn-label').style.color = '#6b7280';
+    scheduleReconn($('ws-input').value.trim());
+  });
+}
+
+function setupTopics() {
+  // saltybot/gps/fix — {lat, lon, alt, stat, t}
+  new ROSLIB.Topic({
+    ros, name: 'saltybot/gps/fix',
+    messageType: 'std_msgs/String', throttle_rate: 500,
+  }).subscribe(msg => {
+    try {
+      const d = JSON.parse(msg.data);
+      gps.lat  = d.lat  ?? gps.lat;
+      gps.lon  = d.lon  ?? gps.lon;
+      gps.alt  = d.alt  ?? gps.alt;
+      gps.stat = d.stat ?? gps.stat;
+      gps.fixes++;
+      gps.msgs++;
+      gps.tsfix = Date.now();
+
+      if (gps.lat != null && gps.lon != null) {
+        updateMap(gps.lat, gps.lon, gps.hdg);
+        addLog(gps.lat, gps.lon, gps.spd, d.t);
+      }
+
+      $('last-update').textContent = 'Last: ' + new Date().toLocaleTimeString('en-US', { hour12: false });
+      $('last-msg').textContent    = 'fix @ ' + new Date().toLocaleTimeString('en-US', { hour12: false });
+      render();
+    } catch(_) {}
+  });
+
+  // saltybot/gps/vel — {spd, hdg, t}
+  new ROSLIB.Topic({
+    ros, name: 'saltybot/gps/vel',
+    messageType: 'std_msgs/String', throttle_rate: 500,
+  }).subscribe(msg => {
+    try {
+      const d = JSON.parse(msg.data);
+      gps.spd = d.spd ?? gps.spd;
+      gps.hdg = d.hdg ?? gps.hdg;
+      gps.msgs++;
+      gps.tsvel = Date.now();
+
+      // Update marker heading if we have a position
+      if (marker && gps.lat != null) {
+        marker.setIcon(makeIcon(gps.hdg));
+      }
+      render();
+    } catch(_) {}
+  });
+
+  // saltybot/phone/gps — {ts, lat, lon, alt_m, accuracy_m, speed_ms, bearing_deg, provider}
+  new ROSLIB.Topic({
+    ros, name: 'saltybot/phone/gps',
+    messageType: 'std_msgs/String', throttle_rate: 500,
+  }).subscribe(msg => {
+    try {
+      const d = JSON.parse(msg.data);
+      phone.lat      = d.lat       ?? phone.lat;
+      phone.lon      = d.lon       ?? phone.lon;
+      phone.alt      = d.alt_m     ?? phone.alt;
+      phone.accuracy = d.accuracy_m ?? phone.accuracy;
+      phone.spd      = d.speed_ms  ?? phone.spd;
+      phone.bearing  = d.bearing_deg ?? phone.bearing;
+      phone.provider = d.provider  ?? phone.provider;
+      phone.msgs++;
+      phone.ts = Date.now();
+
+      if (phone.lat != null && phone.lon != null) {
+        updatePhoneMap(phone.lat, phone.lon, phone.bearing);
+      }
+      render();
+    } catch(_) {}
+  });
+}
+
+// ── Stale checker (1 Hz) ──────────────────────────────────────────────────────
+
+setInterval(render, 1000);
+
+// ── Controls ──────────────────────────────────────────────────────────────────
+
+$('btn-connect').addEventListener('click', () => connect($('ws-input').value.trim(), false));
+$('ws-input').addEventListener('keydown', e => {
+  if (e.key === 'Enter') connect($('ws-input').value.trim(), false);
+});
+
+$('btn-follow').addEventListener('click', () => {
+  followMode = !followMode;
+  $('btn-follow').classList.toggle('active', followMode);
+  $('btn-follow').textContent = followMode ? '⊙ FOLLOW' : '⊙ FREE';
+  if (followMode && gps.lat != null) map.panTo([gps.lat, gps.lon]);
+});
+
+$('btn-clear').addEventListener('click', () => {
+  trail = []; phoneTrail = []; logEntries = []; gps.fixes = 0;
+  if (marker)      { marker.remove();      marker      = null; }
+  if (phoneMarker) { phoneMarker.remove(); phoneMarker = null; }
+  polyline.setLatLngs([]);
+  phonePolyline.setLatLngs([]);
+  $('trail-log').innerHTML = '';
+  render();
+});
+
+map.on('dragstart', () => {
+  if (followMode) {
+    followMode = false;
+    $('btn-follow').classList.remove('active');
+    $('btn-follow').textContent = '⊙ FREE';
+  }
+});
+
+// ── Init ──────────────────────────────────────────────────────────────────────
+
+(function init() {
+  const saved = localStorage.getItem('gps_map_ws_url') || 'ws://localhost:9090';
+  $('ws-input').value = saved;
+  drawCompass(null);
+  connect(saved, false);
+})();

ui/index.html

@@ -193,6 +193,26 @@
       </div>
     </a>
 
+    <a class="panel-card" href="gps_map_panel.html" data-panel="gps">
+      <div class="card-header">
+        <div class="card-icon" style="color:#22c55e">🛰️</div>
+        <div>
+          <div class="card-title">GPS MAP</div>
+          <div class="card-sub">#709</div>
+        </div>
+        <div class="card-dot" id="dot-gps"></div>
+      </div>
+      <div class="card-desc">Live robot position · breadcrumb trail · speed · heading compass · fix quality</div>
+      <div class="card-topics">
+        <code>saltybot/gps/fix</code>
+        <code>saltybot/gps/vel</code>
+      </div>
+      <div class="card-footer">
+        <span class="card-status" id="status-gps">OFFLINE</span>
+        <span class="card-msg" id="msg-gps">No data</span>
+      </div>
+    </a>
+
     <a class="panel-card" href="can_monitor_panel.html" data-panel="can">
       <div class="card-header">
         <div class="card-icon" style="color:#06b6d4">📡</div>

ui/sultee-tracker.html

@@ -0,0 +1,818 @@
+<!DOCTYPE html>
+<html lang="en">
+<head>
+<meta charset="UTF-8">
+<meta name="viewport" content="width=device-width, initial-scale=1.0">
+<title>Sul-Tee GPS Tracker</title>
+
+<!-- Leaflet.js -->
+<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css" />
+<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
+<!-- ROSLIB for robot GPS via rosbridge -->
+<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
+
+<style>
+*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
+
+:root {
+  --bg0:    #050510;
+  --bg1:    #070712;
+  --bg2:    #0d0d20;
+  --border: #0c2a3a;
+  --cyan:   #06b6d4;
+  --blue:   #3b82f6;
+  --blue-hi:#60a5fa;
+  --green:  #22c55e;
+  --amber:  #f59e0b;
+  --red:    #ef4444;
+  --orange: #f97316;
+  --text:   #9ca3af;
+  --text-hi:#d1d5db;
+  --dim:    #374151;
+}
+
+html, body { height: 100%; overflow: hidden; }
+
+body {
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: var(--bg0);
+  color: var(--text);
+  display: flex;
+  flex-direction: column;
+}
+
+/* ── Header ──────────────────────────────────────────────────── */
+#header {
+  display: flex;
+  align-items: center;
+  gap: 10px;
+  padding: 5px 14px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--border);
+  flex-shrink: 0;
+  flex-wrap: wrap;
+}
+.logo { color: var(--orange); font-weight: bold; letter-spacing: .15em; font-size: 13px; white-space: nowrap; }
+#conn-bar { display: flex; align-items: center; gap: 6px; flex: 1; min-width: 240px; }
+#conn-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--dim); flex-shrink: 0; transition: background .3s;
+}
+#conn-dot.connected { background: var(--green); }
+#conn-dot.error     { background: var(--red); animation: blink 1s infinite; }
+#ros-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--dim); flex-shrink: 0; transition: background .3s;
+}
+#ros-dot.connected { background: var(--orange); }
+#ros-dot.error     { background: var(--red); animation: blink 1s infinite; }
+#ws-input {
+  flex: 1; background: var(--bg2); border: 1px solid #1e3a5f;
+  border-radius: 4px; color: #67e8f9; padding: 2px 8px;
+  font-family: monospace; font-size: 11px; min-width: 120px; max-width: 200px;
+}
+#ws-input:focus { outline: none; border-color: var(--cyan); }
+.hbtn {
+  padding: 3px 10px; border-radius: 4px; border: 1px solid #1e3a5f;
+  background: var(--bg2); color: #67e8f9; font-family: monospace;
+  font-size: 10px; font-weight: bold; cursor: pointer; white-space: nowrap;
+}
+.hbtn:hover { background: #0e4f69; }
+#conn-label { font-size: 10px; color: var(--dim); white-space: nowrap; }
+.hdr-badge {
+  font-size: 9px; padding: 2px 7px; border-radius: 3px; border: 1px solid;
+  font-weight: bold; white-space: nowrap;
+}
+.badge-phone { background: #0f172a; border-color: #1d4ed8; color: var(--blue-hi); }
+.badge-robot { background: #1a0f00; border-color: #9a3412; color: var(--orange); }
+
+@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.4} }
+
+/* ── Status bar ─────────────────────────────────────────────── */
+#status-bar {
+  display: flex; gap: 8px; align-items: center; flex-wrap: wrap;
+  padding: 3px 14px; background: var(--bg1);
+  border-bottom: 1px solid var(--border); font-size: 10px;
+}
+.sys-badge {
+  padding: 1px 7px; border-radius: 3px; font-weight: bold;
+  border: 1px solid; letter-spacing: .05em; font-size: 9px;
+}
+.badge-ok    { background: #052e16; border-color: #166534; color: #4ade80; }
+.badge-stale { background: #111827; border-color: var(--dim); color: #6b7280; }
+
+/* ── Main layout ─────────────────────────────────────────────── */
+#main { flex: 1; display: flex; min-height: 0; }
+
+/* ── Map ─────────────────────────────────────────────────────── */
+#map-wrap { flex: 1; position: relative; min-width: 0; }
+#map { width: 100%; height: 100%; }
+
+/* Leaflet dark-mode override */
+.leaflet-tile { filter: brightness(.85) invert(1) hue-rotate(200deg) saturate(.7); }
+.leaflet-container { background: #090915; }
+
+/* ── Stats sidebar ───────────────────────────────────────────── */
+#sidebar {
+  width: 220px;
+  flex-shrink: 0;
+  background: var(--bg1);
+  border-left: 1px solid var(--border);
+  display: flex;
+  flex-direction: column;
+  overflow-y: auto;
+}
+
+.stat-section {
+  padding: 9px 12px;
+  border-bottom: 1px solid var(--border);
+}
+.section-title {
+  font-size: 8px; font-weight: bold; letter-spacing: .15em;
+  color: #0891b2; text-transform: uppercase; margin-bottom: 7px;
+}
+.section-title.phone-title { color: #2563eb; }
+.section-title.robot-title { color: #c2410c; }
+
+.big-stat {
+  display: flex; flex-direction: column; align-items: center;
+  padding: 4px 0;
+}
+.big-val  { font-size: 36px; font-weight: bold; font-family: monospace; line-height: 1; }
+.big-unit { font-size: 11px; color: var(--text); margin-top: 2px; }
+.big-lbl  { font-size: 8px; color: var(--dim); margin-top: 4px; letter-spacing: .1em; }
+
+.kv-grid { display: grid; grid-template-columns: 1fr 1fr; gap: 4px; }
+.kv-cell {
+  background: var(--bg2);
+  border: 1px solid var(--border);
+  border-radius: 5px;
+  padding: 5px 7px;
+}
+.kv-lbl { font-size: 8px; color: var(--dim); margin-bottom: 2px; }
+.kv-val { font-size: 13px; font-weight: bold; font-family: monospace; }
+
+.coord-row { font-size: 10px; color: var(--text); margin-bottom: 2px; }
+.coord-val { font-family: monospace; }
+
+/* accuracy bar */
+.acc-bar-wrap { height: 4px; background: var(--bg2); border-radius: 2px; margin-top: 5px; overflow: hidden; }
+.acc-bar      { height: 100%; border-radius: 2px; transition: width .4s, background .4s; }
+
+/* compass */
+#compass-wrap { display: flex; justify-content: center; padding: 6px 0; }
+#compass-canvas { border-radius: 50%; border: 1px solid var(--border); }
+
+/* trail log */
+#trail-log {
+  flex: 1;
+  overflow-y: auto;
+  padding: 6px 12px;
+  display: flex;
+  flex-direction: column;
+  gap: 3px;
+}
+.trail-entry {
+  font-size: 9px;
+  color: var(--dim);
+  border-bottom: 1px solid #0c1a2a;
+  padding-bottom: 3px;
+}
+.trail-entry .te-lat { color: var(--blue-hi); }
+.trail-entry .te-spd { color: var(--orange); }
+
+/* ── Bottom bar ──────────────────────────────────────────────── */
+#bottombar {
+  display: flex; align-items: center; gap: 10px;
+  padding: 4px 14px;
+  background: var(--bg1);
+  border-top: 1px solid var(--border);
+  font-size: 10px; color: var(--text);
+  flex-shrink: 0; flex-wrap: wrap;
+}
+.bb-item { display: flex; align-items: center; gap: 4px; }
+.bb-lbl  { color: var(--dim); }
+#last-msg { margin-left: auto; color: var(--dim); }
+
+/* ── Map controls overlay ────────────────────────────────────── */
+#map-controls {
+  position: absolute; top: 10px; right: 10px; z-index: 1000;
+  display: flex; flex-direction: column; gap: 4px;
+}
+.map-btn {
+  padding: 4px 8px; border-radius: 4px; border: 1px solid var(--border);
+  background: rgba(7,7,18,.85); color: var(--cyan);
+  font-family: monospace; font-size: 10px; font-weight: bold;
+  cursor: pointer; backdrop-filter: blur(4px);
+  white-space: nowrap;
+}
+.map-btn:hover { background: rgba(14,79,105,.85); }
+.map-btn.active { background: rgba(6,182,212,.25); border-color: var(--cyan); }
+
+/* Legend overlay */
+#map-legend {
+  position: absolute; bottom: 26px; left: 10px; z-index: 1000;
+  background: rgba(7,7,18,.88); border: 1px solid var(--border);
+  border-radius: 5px; padding: 5px 9px;
+  display: flex; flex-direction: column; gap: 4px;
+  font-size: 10px; backdrop-filter: blur(4px);
+}
+.legend-item { display: flex; align-items: center; gap: 6px; }
+.legend-dot  { width: 10px; height: 10px; border-radius: 50%; flex-shrink: 0; }
+
+@media (max-width: 600px) {
+  #sidebar { display: none; }
+}
+</style>
+</head>
+<body>
+
+<!-- ── Header ── -->
+<div id="header">
+  <div class="logo">⚡ SUL-TEE TRACKER</div>
+  <div id="conn-bar">
+    <div id="conn-dot" title="Phone WS"></div>
+    <div id="ros-dot" title="Robot ROS"></div>
+    <input id="ws-input" type="text" value="ws://100.64.0.2:9090" placeholder="ws://host:port" />
+    <button class="hbtn" id="btn-connect">CONNECT</button>
+    <span id="conn-label">Not connected</span>
+  </div>
+  <span class="hdr-badge badge-phone" id="hdr-phone">📱 0 fixes</span>
+  <span class="hdr-badge badge-robot" id="hdr-robot">🤖 0 fixes</span>
+</div>
+
+<!-- ── Status bar ── -->
+<div id="status-bar">
+  <span style="color:var(--dim);font-size:9px">PHONE GPS</span>
+  <span class="sys-badge badge-stale" id="badge-phone">NO DATA</span>
+  <span style="color:var(--dim)">│</span>
+  <span style="color:var(--dim);font-size:9px">ROBOT FIX</span>
+  <span class="sys-badge badge-stale" id="badge-robot-fix">NO DATA</span>
+  <span style="color:var(--dim);font-size:9px">ROBOT VEL</span>
+  <span class="sys-badge badge-stale" id="badge-robot-vel">NO DATA</span>
+  <span id="status-last" style="color:var(--dim);margin-left:auto">Awaiting data…</span>
+</div>
+
+<!-- ── Main ── -->
+<div id="main">
+
+  <!-- Map -->
+  <div id="map-wrap">
+    <div id="map"></div>
+
+    <div id="map-controls">
+      <button class="map-btn active" id="btn-follow">⊙ FOLLOW</button>
+      <button class="map-btn" id="btn-fit">⊞ FIT ALL</button>
+      <button class="map-btn" id="btn-clear">✕ CLEAR</button>
+    </div>
+
+    <div id="map-legend">
+      <div class="legend-item"><div class="legend-dot" style="background:#3b82f6;border:2px solid #60a5fa"></div><span style="color:#60a5fa">Phone / User</span></div>
+      <div class="legend-item"><div class="legend-dot" style="background:#f97316;border:2px solid #fb923c"></div><span style="color:#f97316">Robot (SAUL-TEE)</span></div>
+    </div>
+  </div>
+
+  <!-- Sidebar -->
+  <div id="sidebar">
+
+    <!-- Phone speed (big) -->
+    <div class="stat-section">
+      <div class="section-title phone-title">📱 Phone Speed</div>
+      <div class="big-stat">
+        <div class="big-val" id="val-speed" style="color:#3b82f6">—</div>
+        <div class="big-unit">km/h</div>
+        <div class="big-lbl">GROUND SPEED</div>
+      </div>
+    </div>
+
+    <!-- Phone altitude + heading -->
+    <div class="stat-section">
+      <div class="section-title phone-title">Altitude &amp; Heading</div>
+      <div class="kv-grid">
+        <div class="kv-cell">
+          <div class="kv-lbl">ALT m</div>
+          <div class="kv-val" id="val-alt">—</div>
+        </div>
+        <div class="kv-cell">
+          <div class="kv-lbl">HEADING °</div>
+          <div class="kv-val" id="val-hdg">—</div>
+        </div>
+      </div>
+      <div id="compass-wrap" style="margin-top:6px">
+        <canvas id="compass-canvas" width="110" height="110"></canvas>
+      </div>
+    </div>
+
+    <!-- Phone fix quality -->
+    <div class="stat-section">
+      <div class="section-title phone-title">Phone Fix Quality</div>
+      <div class="kv-grid">
+        <div class="kv-cell">
+          <div class="kv-lbl">H-ACC m</div>
+          <div class="kv-val" id="val-acc">—</div>
+        </div>
+        <div class="kv-cell">
+          <div class="kv-lbl">FIXES</div>
+          <div class="kv-val" id="val-fixes">0</div>
+        </div>
+      </div>
+      <div class="acc-bar-wrap">
+        <div class="acc-bar" id="acc-bar"></div>
+      </div>
+      <div style="margin-top:6px">
+        <div class="coord-row">LAT <span class="coord-val" id="val-lat" style="color:#60a5fa">—</span></div>
+        <div class="coord-row">LON <span class="coord-val" id="val-lon" style="color:#60a5fa">—</span></div>
+      </div>
+    </div>
+
+    <!-- Robot position -->
+    <div class="stat-section">
+      <div class="section-title robot-title">🤖 Robot (SAUL-TEE)</div>
+      <div class="kv-grid">
+        <div class="kv-cell">
+          <div class="kv-lbl">SPEED km/h</div>
+          <div class="kv-val" id="robot-spd" style="color:#f97316">—</div>
+        </div>
+        <div class="kv-cell">
+          <div class="kv-lbl">FIXES</div>
+          <div class="kv-val" id="robot-fixes">0</div>
+        </div>
+      </div>
+      <div style="margin-top:6px">
+        <div class="coord-row">LAT <span class="coord-val" id="robot-lat" style="color:#fb923c">—</span></div>
+        <div class="coord-row">LON <span class="coord-val" id="robot-lon" style="color:#fb923c">—</span></div>
+      </div>
+      <div class="coord-row" style="margin-top:4px">
+        DIST <span id="robot-dist" style="color:var(--amber);font-family:monospace">—</span>
+      </div>
+    </div>
+
+    <!-- Trail log (phone) -->
+    <div class="section-title" style="padding:7px 12px 2px;color:#2563eb">Trail log (phone)</div>
+    <div id="trail-log"></div>
+
+  </div>
+</div>
+
+<!-- ── Bottom bar ── -->
+<div id="bottombar">
+  <div class="bb-item"><span class="bb-lbl" style="color:#3b82f6">●</span><span class="bb-lbl">PHONE</span><span id="bb-phone">0</span></div>
+  <div class="bb-item"><span class="bb-lbl" style="color:#f97316">●</span><span class="bb-lbl">ROBOT</span><span id="bb-robot">0</span></div>
+  <div class="bb-item"><span class="bb-lbl">TRAIL</span><span id="bb-trail">0</span></div>
+  <span id="last-msg">No data yet</span>
+</div>
+
+<script>
+'use strict';
+
+// ── Config ────────────────────────────────────────────────────────────────────
+
+const MAX_TRAIL      = 2000;
+const MAX_LOG        = 60;
+const STALE_MS       = 5000;
+const RECONNECT_BASE = 2000;
+const RECONNECT_MAX  = 30000;
+const RECONNECT_MUL  = 1.5;
+
+// ── State ─────────────────────────────────────────────────────────────────────
+
+const phone = {
+  lat: null, lon: null, speed: null, altitude: null,
+  course: null, accuracy: null, fixes: 0, ts: 0,
+};
+
+const robot = {
+  lat: null, lon: null, alt: null, stat: null,
+  spd: null, hdg: null, fixes: 0,
+  tsfix: 0, tsvel: 0,
+};
+
+let phoneTrail  = [];   // [{lat,lon}]
+let robotTrail  = [];
+let logEntries  = [];
+let followMode  = true;
+
+// WS (phone)
+let ws          = null;
+let wsDelay     = RECONNECT_BASE;
+let wsTimer     = null;
+let wsTick      = null;
+
+// ROS (robot)
+let ros         = null;
+
+// ── DOM ───────────────────────────────────────────────────────────────────────
+
+const $ = id => document.getElementById(id);
+
+// ── Leaflet map ───────────────────────────────────────────────────────────────
+
+const map = L.map('map', { center: [43.45, -79.76], zoom: 15, zoomControl: true });
+
+L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
+  attribution: '© OpenStreetMap contributors',
+  maxZoom: 19,
+}).addTo(map);
+
+// Phone marker — blue, bearing arrow
+function makePhoneIcon(course) {
+  const rot = (course != null && course >= 0) ? course : 0;
+  return L.divIcon({
+    className: '',
+    html: `<svg width="24" height="32" viewBox="0 0 24 32" xmlns="http://www.w3.org/2000/svg"
+              style="transform:rotate(${rot}deg);transform-origin:12px 17px">
+      <circle cx="12" cy="17" r="8" fill="#3b82f6" fill-opacity=".3" stroke="#3b82f6" stroke-width="2"/>
+      <circle cx="12" cy="17" r="3.5" fill="#3b82f6"/>
+      <polygon points="12,2 9,12 15,12" fill="#60a5fa" opacity=".9"/>
+    </svg>`,
+    iconSize:   [24, 32],
+    iconAnchor: [12, 17],
+  });
+}
+
+// Robot marker — orange, heading arrow
+function makeRobotIcon(hdg) {
+  const rot = (hdg != null && hdg >= 0) ? hdg : 0;
+  return L.divIcon({
+    className: '',
+    html: `<svg width="28" height="36" viewBox="0 0 28 36" xmlns="http://www.w3.org/2000/svg"
+              style="transform:rotate(${rot}deg);transform-origin:14px 20px">
+      <circle cx="14" cy="20" r="10" fill="#f97316" fill-opacity=".25" stroke="#f97316" stroke-width="2"/>
+      <circle cx="14" cy="20" r="4"  fill="#f97316"/>
+      <polygon points="14,2 10,14 18,14" fill="#fb923c" opacity=".9"/>
+    </svg>`,
+    iconSize:   [28, 36],
+    iconAnchor: [14, 20],
+  });
+}
+
+let phoneMarker   = null;
+let robotMarker   = null;
+
+let phonePolyline = L.polyline([], {
+  color: '#3b82f6', weight: 2.5, opacity: .8, lineJoin: 'round',
+}).addTo(map);
+
+let robotPolyline = L.polyline([], {
+  color: '#f97316', weight: 2.5, opacity: .8, lineJoin: 'round',
+}).addTo(map);
+
+// ── Canvas compass ────────────────────────────────────────────────────────────
+
+function drawCompass(hdg) {
+  const canvas = $('compass-canvas');
+  const ctx    = canvas.getContext('2d');
+  const W = canvas.width, H = canvas.height;
+  const cx = W / 2, cy = H / 2, r = W / 2 - 4;
+
+  ctx.clearRect(0, 0, W, H);
+  ctx.beginPath(); ctx.arc(cx, cy, r, 0, Math.PI * 2);
+  ctx.strokeStyle = '#1e3a5f'; ctx.lineWidth = 1.5; ctx.stroke();
+  ctx.fillStyle = '#070712'; ctx.fill();
+
+  const cards = ['N','E','S','W'];
+  ctx.font = 'bold 8px monospace'; ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
+  cards.forEach((c, i) => {
+    const a = i * Math.PI / 2;
+    const lx = cx + Math.sin(a) * (r - 10);
+    const ly = cy - Math.cos(a) * (r - 10);
+    ctx.fillStyle = c === 'N' ? '#ef4444' : '#4b5563';
+    ctx.fillText(c, lx, ly);
+  });
+
+  if (hdg == null || hdg < 0) {
+    ctx.fillStyle = '#374151'; ctx.font = '9px monospace';
+    ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
+    ctx.fillText('—', cx, cy);
+    return;
+  }
+
+  const rad = (hdg - 90) * Math.PI / 180;
+  const tipLen = r - 14, tailLen = 9;
+
+  ctx.save(); ctx.translate(cx, cy);
+  ctx.beginPath();
+  ctx.moveTo(Math.cos(rad) * tipLen,            Math.sin(rad) * tipLen);
+  ctx.lineTo(Math.cos(rad + Math.PI) * tailLen, Math.sin(rad + Math.PI) * tailLen);
+  ctx.strokeStyle = '#3b82f6'; ctx.lineWidth = 2.5; ctx.lineCap = 'round'; ctx.stroke();
+  ctx.beginPath(); ctx.arc(0, 0, 3, 0, Math.PI * 2);
+  ctx.fillStyle = '#9ca3af'; ctx.fill();
+  ctx.restore();
+}
+
+// ── Haversine distance (km) ───────────────────────────────────────────────────
+
+function haversine(lat1, lon1, lat2, lon2) {
+  const R = 6371;
+  const dLat = (lat2 - lat1) * Math.PI / 180;
+  const dLon = (lon2 - lon1) * Math.PI / 180;
+  const a = Math.sin(dLat/2)**2 +
+            Math.cos(lat1 * Math.PI/180) * Math.cos(lat2 * Math.PI/180) * Math.sin(dLon/2)**2;
+  return R * 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
+}
+
+// ── Stale badge helper ────────────────────────────────────────────────────────
+
+function setSysBadge(id, ok, text) {
+  const el = $(id); if (!el) return;
+  el.className = `sys-badge badge-${ok ? 'ok' : 'stale'}`;
+  el.textContent = text;
+}
+
+// ── Render ────────────────────────────────────────────────────────────────────
+
+function render() {
+  const now = Date.now();
+
+  // ─ Phone badges & stats ─
+  const phoneStale = phone.ts === 0 || (now - phone.ts > STALE_MS);
+  setSysBadge('badge-phone', !phoneStale,
+    phone.ts === 0 ? 'NO DATA' : phoneStale ? 'STALE' : 'LIVE');
+  $('hdr-phone').textContent = `📱 ${phone.fixes} fix${phone.fixes !== 1 ? 'es' : ''}`;
+
+  // Speed
+  const spdKmh = phone.speed != null ? phone.speed * 3.6 : null;
+  $('val-speed').textContent = spdKmh != null ? spdKmh.toFixed(1) : '—';
+  $('val-speed').style.color = spdKmh == null ? '#6b7280'
+    : spdKmh < 5 ? '#22c55e' : spdKmh < 30 ? '#3b82f6' : '#ef4444';
+
+  // Altitude
+  $('val-alt').textContent = phone.altitude != null ? phone.altitude.toFixed(1) : '—';
+
+  // Heading
+  const hdg = phone.course != null && phone.course >= 0 ? Math.round(phone.course) : null;
+  $('val-hdg').textContent = hdg != null ? hdg + '°' : '—';
+  drawCompass(phone.course);
+
+  // Accuracy
+  const acc = phone.accuracy;
+  $('val-acc').textContent = acc != null ? acc.toFixed(1) : '—';
+  const accNorm  = acc != null ? Math.min(1, acc / 50) : 0;
+  const accColor = acc == null ? '#374151' : acc < 5 ? '#22c55e' : acc < 15 ? '#f59e0b' : '#ef4444';
+  $('acc-bar').style.width = (accNorm * 100) + '%';
+  $('acc-bar').style.background = accColor;
+  $('val-acc').style.color = accColor;
+
+  // Coords
+  $('val-fixes').textContent = phone.fixes;
+  $('val-lat').textContent   = phone.lat != null ? phone.lat.toFixed(6) : '—';
+  $('val-lon').textContent   = phone.lon != null ? phone.lon.toFixed(6) : '—';
+
+  // ─ Robot badges & stats ─
+  const robotFixStale = robot.tsfix === 0 || (now - robot.tsfix > STALE_MS);
+  const robotVelStale = robot.tsvel === 0 || (now - robot.tsvel > STALE_MS);
+  setSysBadge('badge-robot-fix', !robotFixStale,
+    robot.tsfix === 0 ? 'NO DATA' : robotFixStale ? 'STALE' : 'LIVE');
+  setSysBadge('badge-robot-vel', !robotVelStale,
+    robot.tsvel === 0 ? 'NO DATA' : robotVelStale ? 'STALE' : 'LIVE');
+  $('hdr-robot').textContent = `🤖 ${robot.fixes} fix${robot.fixes !== 1 ? 'es' : ''}`;
+
+  const robotSpdKmh = robot.spd != null ? robot.spd : null;
+  $('robot-spd').textContent    = robotSpdKmh != null ? robotSpdKmh.toFixed(1) : '—';
+  $('robot-fixes').textContent  = robot.fixes;
+  $('robot-lat').textContent    = robot.lat != null ? robot.lat.toFixed(6) : '—';
+  $('robot-lon').textContent    = robot.lon != null ? robot.lon.toFixed(6) : '—';
+
+  // Distance between phone and robot
+  if (phone.lat != null && robot.lat != null) {
+    const km = haversine(phone.lat, phone.lon, robot.lat, robot.lon);
+    $('robot-dist').textContent = km < 1 ? (km * 1000).toFixed(0) + ' m' : km.toFixed(2) + ' km';
+  } else {
+    $('robot-dist').textContent = '—';
+  }
+
+  // ─ Bottom bar ─
+  $('bb-phone').textContent = phone.fixes;
+  $('bb-robot').textContent = robot.fixes;
+  $('bb-trail').textContent = phoneTrail.length;
+}
+
+// ── Map update functions ──────────────────────────────────────────────────────
+
+function updatePhoneMap(lat, lon, course) {
+  const icon = makePhoneIcon(course);
+  if (!phoneMarker) {
+    phoneMarker = L.marker([lat, lon], { icon }).addTo(map);
+    if (followMode) map.setView([lat, lon], 16);
+  } else {
+    phoneMarker.setIcon(icon);
+    phoneMarker.setLatLng([lat, lon]);
+    if (followMode) map.panTo([lat, lon]);
+  }
+  phoneTrail.push({ lat, lon });
+  if (phoneTrail.length > MAX_TRAIL) phoneTrail.shift();
+  phonePolyline.setLatLngs(phoneTrail.map(p => [p.lat, p.lon]));
+}
+
+function updateRobotMap(lat, lon, hdg) {
+  const icon = makeRobotIcon(hdg);
+  if (!robotMarker) {
+    robotMarker = L.marker([lat, lon], { icon }).addTo(map);
+  } else {
+    robotMarker.setIcon(icon);
+    robotMarker.setLatLng([lat, lon]);
+  }
+  robotTrail.push({ lat, lon });
+  if (robotTrail.length > MAX_TRAIL) robotTrail.shift();
+  robotPolyline.setLatLngs(robotTrail.map(p => [p.lat, p.lon]));
+}
+
+function fitAll() {
+  const pts = [];
+  if (phone.lat != null) pts.push([phone.lat, phone.lon]);
+  if (robot.lat != null) pts.push([robot.lat, robot.lon]);
+  if (pts.length > 0) map.fitBounds(pts, { padding: [40, 40], maxZoom: 17 });
+}
+
+// ── Trail log ─────────────────────────────────────────────────────────────────
+
+function addLogEntry(d, ts) {
+  const time = new Date(ts * 1000).toLocaleTimeString('en-US', { hour12: false });
+  const spd  = d.speed != null ? (d.speed * 3.6).toFixed(1) : '?';
+  logEntries.unshift({ time, lat: d.latitude, lon: d.longitude, spd });
+  if (logEntries.length > MAX_LOG) logEntries.pop();
+
+  $('trail-log').innerHTML = logEntries.map(e =>
+    `<div class="trail-entry">${e.time} <span class="te-lat">${e.lat.toFixed(5)},${e.lon.toFixed(5)}</span> <span class="te-spd">${e.spd} km/h</span></div>`
+  ).join('');
+}
+
+// ── WebSocket (phone GPS) ─────────────────────────────────────────────────────
+
+function cancelWsReconn() {
+  if (wsTimer) { clearTimeout(wsTimer);  wsTimer = null; }
+  if (wsTick)  { clearInterval(wsTick);  wsTick  = null; }
+}
+
+function scheduleWsReconn(url) {
+  cancelWsReconn();
+  let secs = Math.round(wsDelay / 1000);
+  $('conn-label').textContent = `Retry in ${secs}s…`;
+  $('conn-label').style.color = '#6b7280';
+  wsTick  = setInterval(() => { secs = Math.max(0, secs-1); if (secs>0) $('conn-label').textContent=`Retry in ${secs}s…`; }, 1000);
+  wsTimer = setTimeout(() => { wsTimer = null; doConnect(url, true); }, wsDelay);
+  wsDelay = Math.min(wsDelay * RECONNECT_MUL, RECONNECT_MAX);
+}
+
+function doConnect(url, isRetry) {
+  cancelWsReconn();
+  if (!isRetry) wsDelay = RECONNECT_BASE;
+  if (ws) { try { ws.close(); } catch(_) {} ws = null; }
+
+  $('conn-label').textContent = 'Connecting…';
+  $('conn-label').style.color = '#6b7280';
+  $('conn-dot').className = '';
+
+  try { ws = new WebSocket(url); }
+  catch(e) { $('conn-label').textContent = 'Bad URL'; $('conn-dot').className = 'error'; return; }
+
+  ws.onopen = () => {
+    wsDelay = RECONNECT_BASE; cancelWsReconn();
+    $('conn-dot').className = 'connected';
+    $('conn-label').textContent = 'WS connected';
+    $('conn-label').style.color = '#22c55e';
+    localStorage.setItem('sultee_ws_url', url);
+  };
+  ws.onclose = () => { $('conn-dot').className = ''; ws = null; scheduleWsReconn(url); };
+  ws.onerror = () => { $('conn-dot').className = 'error'; $('conn-label').style.color='#ef4444'; $('conn-label').textContent='WS error'; };
+
+  ws.onmessage = evt => {
+    let msg;
+    try { msg = JSON.parse(evt.data); } catch(_) { return; }
+    if (msg.type !== 'gps' || !msg.data) return;
+
+    const d  = msg.data;
+    const ts = msg.timestamp || Math.floor(Date.now() / 1000);
+
+    phone.lat      = d.latitude            ?? phone.lat;
+    phone.lon      = d.longitude           ?? phone.lon;
+    phone.speed    = d.speed               ?? phone.speed;
+    phone.altitude = d.altitude            ?? phone.altitude;
+    phone.course   = d.course              ?? phone.course;
+    phone.accuracy = d.horizontalAccuracy  ?? phone.accuracy;
+    phone.fixes++;
+    phone.ts = Date.now();
+
+    if (phone.lat != null && phone.lon != null) {
+      updatePhoneMap(phone.lat, phone.lon, phone.course);
+      addLogEntry(d, ts);
+    }
+
+    $('status-last').textContent = 'Phone: ' + new Date(ts * 1000).toLocaleTimeString('en-US', { hour12: false });
+    $('last-msg').textContent    = 'Phone: ' + new Date(ts * 1000).toLocaleTimeString('en-US', { hour12: false });
+    render();
+  };
+}
+
+// ── ROSLIB (robot GPS) ────────────────────────────────────────────────────────
+
+function connectRos(url) {
+  if (ros) { try { ros.close(); } catch(_) {} }
+  ros = new ROSLIB.Ros({ url });
+
+  ros.on('connection', () => {
+    $('ros-dot').className = 'connected';
+    setupRobotTopics();
+  });
+  ros.on('error', () => { $('ros-dot').className = 'error'; });
+  ros.on('close', () => {
+    $('ros-dot').className = '';
+    setTimeout(() => connectRos(url), 5000);
+  });
+}
+
+function setupRobotTopics() {
+  // /gps/fix — sensor_msgs/NavSatFix (SIM7600X / Pixel 5 GPS)
+  new ROSLIB.Topic({
+    ros, name: '/gps/fix',
+    messageType: 'sensor_msgs/NavSatFix', throttle_rate: 500,
+  }).subscribe(msg => {
+    robot.lat  = msg.latitude  ?? robot.lat;
+    robot.lon  = msg.longitude ?? robot.lon;
+    robot.alt  = msg.altitude  ?? robot.alt;
+    robot.stat = msg.status ? msg.status.status : robot.stat;
+    robot.fixes++;
+    robot.tsfix = Date.now();
+
+    if (robot.lat != null && robot.lon != null) {
+      updateRobotMap(robot.lat, robot.lon, robot.hdg);
+    }
+    $('last-msg').textContent = 'Robot: ' + new Date().toLocaleTimeString('en-US', { hour12: false });
+    render();
+  });
+
+  // /gps/vel — geometry_msgs/TwistStamped (east/north m/s + heading in radians)
+  new ROSLIB.Topic({
+    ros, name: '/gps/vel',
+    messageType: 'geometry_msgs/TwistStamped', throttle_rate: 500,
+  }).subscribe(msg => {
+    const vx  = msg.twist.linear.x;   // east  m/s
+    const vy  = msg.twist.linear.y;   // north m/s
+    const spd = Math.sqrt(vx * vx + vy * vy) * 3.6; // → km/h
+    const hdg = (msg.twist.angular.z * 180 / Math.PI + 360) % 360; // radians → degrees
+    robot.spd = spd;
+    robot.hdg = hdg;
+    robot.tsvel = Date.now();
+    if (robotMarker && robot.lat != null) {
+      robotMarker.setIcon(makeRobotIcon(robot.hdg));
+    }
+    render();
+  });
+}
+
+// ── Stale refresh ─────────────────────────────────────────────────────────────
+
+setInterval(render, 1000);
+
+// ── Controls ──────────────────────────────────────────────────────────────────
+
+$('btn-connect').addEventListener('click', () => {
+  const url = $('ws-input').value.trim();
+  doConnect(url, false);
+  connectRos(url);
+});
+$('ws-input').addEventListener('keydown', e => {
+  if (e.key === 'Enter') { const url = $('ws-input').value.trim(); doConnect(url, false); connectRos(url); }
+});
+
+$('btn-follow').addEventListener('click', () => {
+  followMode = !followMode;
+  $('btn-follow').classList.toggle('active', followMode);
+  $('btn-follow').textContent = followMode ? '⊙ FOLLOW' : '⊙ FREE';
+  if (followMode && phone.lat != null) map.panTo([phone.lat, phone.lon]);
+});
+
+$('btn-fit').addEventListener('click', fitAll);
+
+$('btn-clear').addEventListener('click', () => {
+  phoneTrail = []; robotTrail = [];
+  logEntries = []; phone.fixes = 0; robot.fixes = 0;
+  if (phoneMarker) { phoneMarker.remove(); phoneMarker = null; }
+  if (robotMarker) { robotMarker.remove(); robotMarker = null; }
+  phonePolyline.setLatLngs([]);
+  robotPolyline.setLatLngs([]);
+  $('trail-log').innerHTML = '';
+  render();
+});
+
+map.on('dragstart', () => {
+  if (followMode) {
+    followMode = false;
+    $('btn-follow').classList.remove('active');
+    $('btn-follow').textContent = '⊙ FREE';
+  }
+});
+
+// ── Init ──────────────────────────────────────────────────────────────────────
+
+(function init() {
+  const saved = localStorage.getItem('sultee_ws_url') || 'ws://100.64.0.2:9090';
+  $('ws-input').value = saved;
+  drawCompass(null);
+  doConnect(saved, false);
+  connectRos(saved);
+})();
+</script>
+</body>
+</html>