Merge pull request 'feat: systemd auto-start for ROS2 + dashboard on Orin boot (bd-1hyn)' (#732) from sl-perception/bd-1hyn-orin-autostart into main

Extends the bd-66hx serial protocol with two new Orin→ESP32 commands:
  CMD_OTA_CHECK  (0x10): triggers gitea_ota_check_now(), responds with
    TELEM_VERSION_INFO (0x84) for Balance and IO (current + available ver).
  CMD_OTA_UPDATE (0x11): uint8 target (0=balance, 1=io, 2=both) — triggers
    uart_ota_trigger() for IO or ota_self_trigger() for Balance.
    NACK with ERR_OTA_BUSY or ERR_OTA_NO_UPDATE on failure.
New telemetry: TELEM_OTA_STATUS (0x83, target+state+progress+err),
  TELEM_VERSION_INFO (0x84, target+current[16]+available[16]).
Wires OTA stack into app_main: ota_self_health_check on boot,
  gitea_ota_init + ota_display_init after peripherals ready.
CMakeLists updated with all OTA component dependencies.

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

.gitea/workflows/ota-release.yml

@@ -0,0 +1,162 @@
+# .gitea/workflows/ota-release.yml
+# Gitea Actions — ESP32 OTA firmware build & release (bd-9kod)
+#
+# Triggers on signed release tags:
+#   esp32-balance/vX.Y.Z  →  builds esp32s3/balance/   (ESP32-S3 Balance board)
+#   esp32-io/vX.Y.Z       →  builds esp32s3-io/         (ESP32-S3 IO board)
+#
+# Uses the official espressif/idf Docker image for reproducible builds.
+# Attaches <app>_<version>.bin + <app>_<version>.sha256 to the Gitea release.
+# The ESP32 Balance OTA system fetches the .bin from the release asset URL.
+
+name: OTA release — build & attach firmware
+
+on:
+  push:
+    tags:
+      - "esp32-balance/v*"
+      - "esp32-io/v*"
+
+permissions:
+  contents: write
+
+jobs:
+  build-and-release:
+    name: Build ${{ github.ref_name }}
+    runs-on: ubuntu-latest
+    container:
+      image: espressif/idf:v5.2.2
+      options: --user root
+
+    steps:
+      # ── 1. Checkout ───────────────────────────────────────────────────────────
+      - name: Checkout
+        uses: actions/checkout@v4
+
+      # ── 2. Resolve build target from tag ─────────────────────────────────────
+      # Tag format:  esp32-balance/v1.2.3  or  esp32-io/v1.2.3
+      - name: Resolve project from tag
+        id: proj
+        shell: bash
+        run: |
+          TAG="${GITHUB_REF_NAME}"
+          case "$TAG" in
+            esp32-balance/*)
+              DIR="esp32s3/balance"
+              APP="esp32s3_balance"
+              ;;
+            esp32-io/*)
+              DIR="esp32s3-io"
+              APP="esp32s3_io"
+              ;;
+            *)
+              echo "::error::Unrecognised tag prefix: ${TAG}"
+              exit 1
+              ;;
+          esac
+          VERSION="${TAG#*/}"
+          echo "dir=${DIR}"         >> "$GITHUB_OUTPUT"
+          echo "app=${APP}"         >> "$GITHUB_OUTPUT"
+          echo "version=${VERSION}" >> "$GITHUB_OUTPUT"
+          echo "tag=${TAG}"         >> "$GITHUB_OUTPUT"
+          echo "Build: ${APP} ${VERSION} from ${DIR}"
+
+      # ── 3. Build with ESP-IDF ─────────────────────────────────────────────────
+      - name: Build firmware (idf.py build)
+        shell: bash
+        run: |
+          . "${IDF_PATH}/export.sh"
+          cd "${{ steps.proj.outputs.dir }}"
+          idf.py build
+
+      # ── 4. Collect binary & generate checksum ────────────────────────────────
+      - name: Collect artifacts
+        id: art
+        shell: bash
+        run: |
+          APP="${{ steps.proj.outputs.app }}"
+          VER="${{ steps.proj.outputs.version }}"
+          BIN_SRC="${{ steps.proj.outputs.dir }}/build/${APP}.bin"
+          BIN_OUT="${APP}_${VER}.bin"
+          SHA_OUT="${APP}_${VER}.sha256"
+
+          cp "$BIN_SRC" "$BIN_OUT"
+          sha256sum "$BIN_OUT" > "$SHA_OUT"
+
+          echo "bin=${BIN_OUT}" >> "$GITHUB_OUTPUT"
+          echo "sha=${SHA_OUT}" >> "$GITHUB_OUTPUT"
+
+          echo "Binary:   ${BIN_OUT} ($(wc -c < "$BIN_OUT") bytes)"
+          echo "Checksum: $(cat "$SHA_OUT")"
+
+      # ── 5. Archive artifacts in CI workspace ─────────────────────────────────
+      - name: Upload build artifacts
+        uses: actions/upload-artifact@v4
+        with:
+          name: firmware-${{ steps.proj.outputs.app }}-${{ steps.proj.outputs.version }}
+          path: |
+            ${{ steps.art.outputs.bin }}
+            ${{ steps.art.outputs.sha }}
+
+      # ── 6. Create Gitea release (if needed) & upload assets ──────────────────
+      # Uses GITHUB_TOKEN (auto-provided, contents:write from permissions block).
+      # URL-encodes the tag to handle the slash in esp32-balance/vX.Y.Z.
+      - name: Publish assets to Gitea release
+        shell: bash
+        env:
+          GITEA_URL: https://gitea.vayrette.com
+          TOKEN: ${{ secrets.GITHUB_TOKEN }}
+          REPO: ${{ github.repository }}
+          TAG: ${{ steps.proj.outputs.tag }}
+          BIN: ${{ steps.art.outputs.bin }}
+          SHA: ${{ steps.art.outputs.sha }}
+        run: |
+          API="${GITEA_URL}/api/v1/repos/${REPO}"
+
+          # URL-encode the tag (slash in esp32-balance/vX.Y.Z must be escaped)
+          TAG_ENC=$(python3 -c "
+          import urllib.parse, sys
+          print(urllib.parse.quote(sys.argv[1], safe=''))
+          " "$TAG")
+
+          # Try to fetch an existing release for this tag
+          RELEASE=$(curl -sf \
+            -H "Authorization: token ${TOKEN}" \
+            "${API}/releases/tags/${TAG_ENC}") || true
+
+          # If no release yet, create it
+          if [ -z "$RELEASE" ]; then
+            echo "Creating release for tag: ${TAG}"
+            RELEASE=$(curl -sf \
+              -X POST \
+              -H "Authorization: token ${TOKEN}" \
+              -H "Content-Type: application/json" \
+              -d "$(python3 -c "
+          import json, sys
+          print(json.dumps({
+              'tag_name': sys.argv[1],
+              'name':     sys.argv[1],
+              'draft':    False,
+              'prerelease': False,
+          }))
+          " "$TAG")" \
+              "${API}/releases")
+          fi
+
+          RELEASE_ID=$(echo "$RELEASE" | python3 -c "
+          import sys, json; print(json.load(sys.stdin)['id'])
+          ")
+          echo "Release ID: ${RELEASE_ID}"
+
+          # Upload binary and checksum
+          for FILE in "$BIN" "$SHA"; do
+            FNAME=$(basename "$FILE")
+            echo "Uploading: ${FNAME}"
+            curl -sf \
+              -X POST \
+              -H "Authorization: token ${TOKEN}" \
+              -F "attachment=@${FILE}" \
+              "${API}/releases/${RELEASE_ID}/assets?name=${FNAME}"
+          done
+
+          echo "Published: ${BIN} + ${SHA} → release ${TAG}"

esp32s3/balance/CMakeLists.txt

@@ -0,0 +1,3 @@
+cmake_minimum_required(VERSION 3.16)
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(esp32s3_balance)

esp32s3/balance/main/CMakeLists.txt

@@ -0,0 +1,22 @@
+idf_component_register(
+    SRCS
+        "main.c"
+        "orin_serial.c"
+        "vesc_can.c"
+        "gitea_ota.c"
+        "ota_self.c"
+        "uart_ota.c"
+        "ota_display.c"
+    INCLUDE_DIRS "."
+    REQUIRES
+        esp_wifi
+        esp_http_client
+        esp_https_ota
+        nvs_flash
+        app_update
+        mbedtls
+        cJSON
+        driver
+        freertos
+        esp_timer
+)

esp32s3/balance/main/config.h

@@ -0,0 +1,42 @@
+#pragma once
+
+/* ── ESP32-S3 BALANCE board — bd-66hx pin/config definitions ───────────────
+ *
+ * Hardware change from pre-bd-66hx design:
+ *   Previously: IO43/IO44 = CAN SN65HVD230 (shared Orin+VESC bus via CANable2)
+ *   After bd-66hx: IO43/IO44 = CH343 UART0 (Orin serial comms)
+ *                  IO2/IO1   = CAN SN65HVD230 rewired (VESC-only bus)
+ *
+ * The SN65HVD230 transceiver physical wiring must be updated from IO43/44
+ * to IO2/IO1 when deploying this firmware.  See docs/SAUL-TEE-SYSTEM-REFERENCE.md.
+ */
+
+/* ── Orin serial (CH343 USB-to-UART, 1a86:55d3 on Orin side) ── */
+#define ORIN_UART_PORT          UART_NUM_0
+#define ORIN_UART_BAUD          460800
+#define ORIN_UART_TX_GPIO       43      /* ESP32→CH343 RXD */
+#define ORIN_UART_RX_GPIO       44      /* CH343 TXD→ESP32 */
+#define ORIN_UART_RX_BUF        1024
+#define ORIN_TX_QUEUE_DEPTH     16
+
+/* ── VESC CAN TWAI (SN65HVD230 transceiver, rewired for bd-66hx) ── */
+#define VESC_CAN_TX_GPIO        2       /* ESP32 TWAI TX → SN65HVD230 TXD */
+#define VESC_CAN_RX_GPIO        1       /* SN65HVD230 RXD → ESP32 TWAI RX */
+#define VESC_CAN_RX_QUEUE       32
+
+/* VESC node IDs — matched to bd-wim1 TELEM_VESC_LEFT/RIGHT mapping */
+#define VESC_ID_A               56u     /* TELEM_VESC_LEFT  (0x81) */
+#define VESC_ID_B               68u     /* TELEM_VESC_RIGHT (0x82) */
+
+/* ── Safety / timing ── */
+#define HB_TIMEOUT_MS           500u    /* heartbeat watchdog: disarm if exceeded */
+#define DRIVE_TIMEOUT_MS        500u    /* drive command staleness timeout */
+#define TELEM_STATUS_PERIOD_MS  100u    /* 10 Hz status telemetry to Orin */
+#define TELEM_VESC_PERIOD_MS    100u    /* 10 Hz VESC telemetry to Orin */
+
+/* ── Drive → VESC RPM scaling ── */
+#define RPM_PER_SPEED_UNIT      5       /* speed_units=1000 → 5000 ERPM */
+#define RPM_PER_STEER_UNIT      3       /* steer differential scale */
+
+/* ── Tilt cutoff ── */
+#define TILT_CUTOFF_DEG         25.0f

esp32s3/balance/main/gitea_ota.c

@@ -0,0 +1,285 @@
+/* gitea_ota.c — Gitea version checker (bd-3hte)
+ *
+ * Uses esp_http_client + cJSON to query:
+ *   GET /api/v1/repos/{repo}/releases?limit=10
+ * Filters releases by tag prefix, extracts version and download URLs.
+ */
+
+#include "gitea_ota.h"
+#include "version.h"
+#include "esp_log.h"
+#include "esp_wifi.h"
+#include "esp_event.h"
+#include "esp_netif.h"
+#include "esp_http_client.h"
+#include "nvs_flash.h"
+#include "nvs.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "freertos/event_groups.h"
+#include "cJSON.h"
+#include <string.h>
+#include <stdio.h>
+
+static const char *TAG = "gitea_ota";
+
+ota_update_info_t g_balance_update = {0};
+ota_update_info_t g_io_update      = {0};
+
+/* ── WiFi connection ── */
+#define WIFI_CONNECTED_BIT  BIT0
+#define WIFI_FAIL_BIT       BIT1
+#define WIFI_MAX_RETRIES    5
+
+/* Compile-time WiFi fallback (override in NVS "wifi"/"ssid","pass") */
+#define DEFAULT_WIFI_SSID   "saltylab"
+#define DEFAULT_WIFI_PASS   ""
+
+static EventGroupHandle_t s_wifi_eg;
+static int s_wifi_retries = 0;
+
+static void wifi_event_handler(void *arg, esp_event_base_t base,
+                                int32_t id, void *data)
+{
+    if (base == WIFI_EVENT && id == WIFI_EVENT_STA_START) {
+        esp_wifi_connect();
+    } else if (base == WIFI_EVENT && id == WIFI_EVENT_STA_DISCONNECTED) {
+        if (s_wifi_retries < WIFI_MAX_RETRIES) {
+            esp_wifi_connect();
+            s_wifi_retries++;
+        } else {
+            xEventGroupSetBits(s_wifi_eg, WIFI_FAIL_BIT);
+        }
+    } else if (base == IP_EVENT && id == IP_EVENT_STA_GOT_IP) {
+        s_wifi_retries = 0;
+        xEventGroupSetBits(s_wifi_eg, WIFI_CONNECTED_BIT);
+    }
+}
+
+static bool wifi_connect(void)
+{
+    char ssid[64] = DEFAULT_WIFI_SSID;
+    char pass[64] = DEFAULT_WIFI_PASS;
+
+    /* Try to read credentials from NVS */
+    nvs_handle_t nvs;
+    if (nvs_open("wifi", NVS_READONLY, &nvs) == ESP_OK) {
+        size_t sz = sizeof(ssid);
+        nvs_get_str(nvs, "ssid", ssid, &sz);
+        sz = sizeof(pass);
+        nvs_get_str(nvs, "pass", pass, &sz);
+        nvs_close(nvs);
+    }
+
+    s_wifi_eg = xEventGroupCreate();
+    s_wifi_retries = 0;
+
+    ESP_ERROR_CHECK(esp_netif_init());
+    ESP_ERROR_CHECK(esp_event_loop_create_default());
+    esp_netif_create_default_wifi_sta();
+
+    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
+    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
+
+    esp_event_handler_instance_t h1, h2;
+    ESP_ERROR_CHECK(esp_event_handler_instance_register(
+        WIFI_EVENT, ESP_EVENT_ANY_ID, wifi_event_handler, NULL, &h1));
+    ESP_ERROR_CHECK(esp_event_handler_instance_register(
+        IP_EVENT, IP_EVENT_STA_GOT_IP, wifi_event_handler, NULL, &h2));
+
+    wifi_config_t wcfg = {0};
+    strlcpy((char *)wcfg.sta.ssid,     ssid, sizeof(wcfg.sta.ssid));
+    strlcpy((char *)wcfg.sta.password, pass, sizeof(wcfg.sta.password));
+
+    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
+    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wcfg));
+    ESP_ERROR_CHECK(esp_wifi_start());
+
+    EventBits_t bits = xEventGroupWaitBits(s_wifi_eg,
+        WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, pdFALSE, pdFALSE,
+        pdMS_TO_TICKS(15000));
+
+    esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, h2);
+    esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, h1);
+    vEventGroupDelete(s_wifi_eg);
+
+    if (bits & WIFI_CONNECTED_BIT) {
+        ESP_LOGI(TAG, "WiFi connected SSID=%s", ssid);
+        return true;
+    }
+    ESP_LOGW(TAG, "WiFi connect failed SSID=%s", ssid);
+    return false;
+}
+
+/* ── HTTP fetch into a heap buffer ── */
+#define HTTP_RESP_MAX   (8 * 1024)
+
+typedef struct { char *buf; int len; int cap; } http_buf_t;
+
+static esp_err_t http_event_cb(esp_http_client_event_t *evt)
+{
+    http_buf_t *b = (http_buf_t *)evt->user_data;
+    if (evt->event_id == HTTP_EVENT_ON_DATA && b) {
+        if (b->len + evt->data_len < b->cap) {
+            memcpy(b->buf + b->len, evt->data, evt->data_len);
+            b->len += evt->data_len;
+        }
+    }
+    return ESP_OK;
+}
+
+static char *http_get(const char *url)
+{
+    char *buf = malloc(HTTP_RESP_MAX);
+    if (!buf) return NULL;
+    http_buf_t b = {.buf = buf, .len = 0, .cap = HTTP_RESP_MAX};
+    buf[0] = '\0';
+
+    esp_http_client_config_t cfg = {
+        .url            = url,
+        .event_handler  = http_event_cb,
+        .user_data      = &b,
+        .timeout_ms     = GITEA_API_TIMEOUT_MS,
+        .skip_cert_common_name_check = true,
+    };
+    esp_http_client_handle_t client = esp_http_client_init(&cfg);
+    esp_err_t err = esp_http_client_perform(client);
+    int status = esp_http_client_get_status_code(client);
+    esp_http_client_cleanup(client);
+
+    if (err != ESP_OK || status != 200) {
+        ESP_LOGW(TAG, "HTTP GET %s → err=%d status=%d", url, err, status);
+        free(buf);
+        return NULL;
+    }
+    buf[b.len] = '\0';
+    return buf;
+}
+
+/* ── Version comparison: returns true if remote > local ── */
+static bool version_newer(const char *local, const char *remote)
+{
+    int la=0,lb=0,lc=0, ra=0,rb=0,rc=0;
+    sscanf(local,  "%d.%d.%d", &la, &lb, &lc);
+    sscanf(remote, "%d.%d.%d", &ra, &rb, &rc);
+    if (ra != la) return ra > la;
+    if (rb != lb) return rb > lb;
+    return rc > lc;
+}
+
+/* ── Parse releases JSON array, fill ota_update_info_t ── */
+static void parse_releases(const char *json, const char *tag_prefix,
+                             const char *bin_asset, const char *sha_asset,
+                             const char *local_version,
+                             ota_update_info_t *out)
+{
+    cJSON *arr = cJSON_Parse(json);
+    if (!arr || !cJSON_IsArray(arr)) {
+        ESP_LOGW(TAG, "JSON parse failed");
+        cJSON_Delete(arr);
+        return;
+    }
+
+    cJSON *rel;
+    cJSON_ArrayForEach(rel, arr) {
+        cJSON *tag_j = cJSON_GetObjectItem(rel, "tag_name");
+        if (!cJSON_IsString(tag_j)) continue;
+
+        const char *tag = tag_j->valuestring;
+        if (strncmp(tag, tag_prefix, strlen(tag_prefix)) != 0) continue;
+
+        /* Extract version after prefix */
+        const char *ver = tag + strlen(tag_prefix);
+        if (*ver == 'v') ver++;  /* strip leading 'v' */
+
+        if (!version_newer(local_version, ver)) continue;
+
+        /* Found a newer release — extract asset URLs */
+        cJSON *assets = cJSON_GetObjectItem(rel, "assets");
+        if (!cJSON_IsArray(assets)) continue;
+
+        out->available = false;
+        out->download_url[0] = '\0';
+        out->sha256[0] = '\0';
+        strlcpy(out->version, ver, sizeof(out->version));
+
+        cJSON *asset;
+        cJSON_ArrayForEach(asset, assets) {
+            cJSON *name_j = cJSON_GetObjectItem(asset, "name");
+            cJSON *url_j  = cJSON_GetObjectItem(asset, "browser_download_url");
+            if (!cJSON_IsString(name_j) || !cJSON_IsString(url_j)) continue;
+
+            if (strcmp(name_j->valuestring, bin_asset) == 0) {
+                strlcpy(out->download_url, url_j->valuestring,
+                        sizeof(out->download_url));
+                out->available = true;
+            } else if (strcmp(name_j->valuestring, sha_asset) == 0) {
+                /* Download the SHA256 asset inline */
+                char *sha = http_get(url_j->valuestring);
+                if (sha) {
+                    /* sha file is just hex+newline */
+                    size_t n = strspn(sha, "0123456789abcdefABCDEF");
+                    if (n == 64) {
+                        memcpy(out->sha256, sha, 64);
+                        out->sha256[64] = '\0';
+                    }
+                    free(sha);
+                }
+            }
+        }
+
+        if (out->available) {
+            ESP_LOGI(TAG, "update: tag=%s ver=%s", tag, out->version);
+        }
+        break;  /* use first matching release */
+    }
+
+    cJSON_Delete(arr);
+}
+
+/* ── Main check ── */
+void gitea_ota_check_now(void)
+{
+    char url[512];
+    snprintf(url, sizeof(url),
+             "%s/api/v1/repos/%s/releases?limit=10",
+             GITEA_BASE_URL, GITEA_REPO);
+
+    char *json = http_get(url);
+    if (!json) {
+        ESP_LOGW(TAG, "releases fetch failed");
+        return;
+    }
+
+    parse_releases(json, BALANCE_TAG_PREFIX, BALANCE_BIN_ASSET,
+                   BALANCE_SHA256_ASSET, BALANCE_FW_VERSION, &g_balance_update);
+    parse_releases(json, IO_TAG_PREFIX, IO_BIN_ASSET,
+                   IO_SHA256_ASSET, IO_FW_VERSION, &g_io_update);
+    free(json);
+}
+
+/* ── Background task ── */
+static void version_check_task(void *arg)
+{
+    /* Initial check immediately after WiFi up */
+    vTaskDelay(pdMS_TO_TICKS(2000));
+    gitea_ota_check_now();
+
+    for (;;) {
+        vTaskDelay(pdMS_TO_TICKS(VERSION_CHECK_PERIOD_MS));
+        gitea_ota_check_now();
+    }
+}
+
+void gitea_ota_init(void)
+{
+    ESP_ERROR_CHECK(nvs_flash_init());
+
+    if (!wifi_connect()) {
+        ESP_LOGW(TAG, "WiFi unavailable — version checks disabled");
+        return;
+    }
+
+    xTaskCreate(version_check_task, "ver_check", 6144, NULL, 3, NULL);
+    ESP_LOGI(TAG, "version check task started");
+}

esp32s3/balance/main/gitea_ota.h

@@ -0,0 +1,42 @@
+#pragma once
+/* gitea_ota.h — Gitea release version checker (bd-3hte)
+ *
+ * WiFi task: on boot and every 30 min, queries Gitea releases API,
+ * compares tag version against embedded FW_VERSION, stores update info.
+ *
+ * WiFi credentials read from NVS namespace "wifi" keys "ssid"/"pass".
+ * Fall back to compile-time defaults if NVS is empty.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* Gitea instance */
+#define GITEA_BASE_URL      "http://gitea.vayrette.com"
+#define GITEA_REPO          "seb/saltylab-firmware"
+#define GITEA_API_TIMEOUT_MS 10000
+
+/* Version check interval */
+#define VERSION_CHECK_PERIOD_MS  (30u * 60u * 1000u)  /* 30 minutes */
+
+/* Max URL/version string lengths */
+#define OTA_URL_MAX     384
+#define OTA_VER_MAX     32
+#define OTA_SHA256_MAX  65
+
+typedef struct {
+    bool    available;
+    char    version[OTA_VER_MAX];      /* remote version string, e.g. "1.2.3" */
+    char    download_url[OTA_URL_MAX]; /* direct download URL for .bin */
+    char    sha256[OTA_SHA256_MAX];    /* hex SHA256 (from .sha256 asset), or "" */
+} ota_update_info_t;
+
+/* Shared state — written by gitea_ota_check_task, read by display/OTA tasks */
+extern ota_update_info_t g_balance_update;
+extern ota_update_info_t g_io_update;
+
+/* Initialize WiFi and start version check task */
+void gitea_ota_init(void);
+
+/* One-shot sync check (can be called from any task) */
+void gitea_ota_check_now(void);

esp32s3/balance/main/main.c

@@ -0,0 +1,114 @@
+/* main.c — ESP32-S3 BALANCE app_main (bd-66hx + OTA beads) */
+
+#include "orin_serial.h"
+#include "vesc_can.h"
+#include "gitea_ota.h"
+#include "ota_self.h"
+#include "uart_ota.h"
+#include "ota_display.h"
+#include "config.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "freertos/queue.h"
+#include "esp_log.h"
+#include "esp_timer.h"
+#include <string.h>
+
+static const char *TAG = "main";
+
+static QueueHandle_t s_orin_tx_q;
+
+/* ── Telemetry task: sends TELEM_STATUS to Orin at 10 Hz ── */
+static void telem_task(void *arg)
+{
+    for (;;) {
+        vTaskDelay(pdMS_TO_TICKS(TELEM_STATUS_PERIOD_MS));
+
+        uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
+        bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
+
+        /* Determine balance state for telemetry */
+        bal_state_t state;
+        if (g_orin_ctrl.estop) {
+            state = BAL_ESTOP;
+        } else if (!g_orin_ctrl.armed) {
+            state = BAL_DISARMED;
+        } else {
+            state = BAL_ARMED;
+        }
+
+        /* flags: bit0=estop_active, bit1=heartbeat_timeout */
+        uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
+                        (hb_timeout         ? 0x02u : 0x00u);
+
+        /* Battery voltage from VESC_ID_A STATUS_5 (V×10 → mV) */
+        uint16_t vbat_mv = (uint16_t)((int32_t)g_vesc[0].voltage_x10 * 100);
+
+        orin_send_status(s_orin_tx_q,
+                         0,      /* pitch_x10: stub — full IMU in future bead */
+                         0,      /* motor_cmd: stub */
+                         vbat_mv,
+                         state,
+                         flags);
+    }
+}
+
+/* ── Drive task: applies Orin drive commands to VESCs @ 50 Hz ── */
+static void drive_task(void *arg)
+{
+    for (;;) {
+        vTaskDelay(pdMS_TO_TICKS(20));  /* 50 Hz */
+
+        uint32_t now_ms   = (uint32_t)(esp_timer_get_time() / 1000LL);
+        bool hb_timeout   = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
+        bool drive_stale  = (now_ms - g_orin_drive.updated_ms) > DRIVE_TIMEOUT_MS;
+
+        int32_t left_erpm  = 0;
+        int32_t right_erpm = 0;
+
+        if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
+            !hb_timeout && !drive_stale) {
+            int32_t spd = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
+            int32_t str = (int32_t)g_orin_drive.steer * RPM_PER_STEER_UNIT;
+            left_erpm  = spd + str;
+            right_erpm = spd - str;
+        }
+
+        /* VESC_ID_A (56) = LEFT, VESC_ID_B (68) = RIGHT per bd-wim1 protocol */
+        vesc_can_send_rpm(VESC_ID_A, left_erpm);
+        vesc_can_send_rpm(VESC_ID_B, right_erpm);
+    }
+}
+
+void app_main(void)
+{
+    ESP_LOGI(TAG, "ESP32-S3 BALANCE starting");
+
+    /* OTA rollback health check — must be called within OTA_ROLLBACK_WINDOW_S */
+    ota_self_health_check();
+
+    /* Init peripherals */
+    orin_serial_init();
+    vesc_can_init();
+
+    /* TX queue for outbound serial frames */
+    s_orin_tx_q = xQueueCreate(ORIN_TX_QUEUE_DEPTH, sizeof(orin_tx_frame_t));
+    configASSERT(s_orin_tx_q);
+
+    /* Seed heartbeat timer so we don't immediately timeout */
+    g_orin_ctrl.hb_last_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
+
+    /* Create tasks */
+    xTaskCreate(orin_serial_rx_task, "orin_rx",  4096, s_orin_tx_q, 10, NULL);
+    xTaskCreate(orin_serial_tx_task, "orin_tx",  2048, s_orin_tx_q,  9, NULL);
+    xTaskCreate(vesc_can_rx_task,    "vesc_rx",  4096, s_orin_tx_q, 10, NULL);
+    xTaskCreate(telem_task,          "telem",    2048, NULL,          5, NULL);
+    xTaskCreate(drive_task,          "drive",    2048, NULL,          8, NULL);
+
+    /* OTA subsystem — WiFi version checker + display overlay */
+    gitea_ota_init();
+    ota_display_init();
+
+    ESP_LOGI(TAG, "all tasks started");
+    /* app_main returns — FreeRTOS scheduler continues */
+}

esp32s3/balance/main/orin_serial.c

@@ -0,0 +1,354 @@
+/* orin_serial.c — Orin↔ESP32-S3 serial protocol (bd-66hx + bd-1s1s OTA cmds) */
+
+#include "orin_serial.h"
+#include "config.h"
+#include "gitea_ota.h"
+#include "ota_self.h"
+#include "uart_ota.h"
+#include "version.h"
+#include "driver/uart.h"
+#include "esp_log.h"
+#include "esp_timer.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+#include <string.h>
+#include <stdio.h>
+
+static const char *TAG = "orin";
+
+/* ── Shared state ── */
+orin_drive_t   g_orin_drive   = {0};
+orin_pid_t     g_orin_pid     = {0};
+orin_control_t g_orin_ctrl    = {.armed = false, .estop = false, .hb_last_ms = 0};
+
+/* ── CRC8-SMBUS (poly=0x07, init=0x00) ── */
+static uint8_t crc8(const uint8_t *data, uint8_t len)
+{
+    uint8_t crc = 0x00u;
+    for (uint8_t i = 0; i < len; i++) {
+        crc ^= data[i];
+        for (uint8_t b = 0; b < 8u; b++) {
+            crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u);
+        }
+    }
+    return crc;
+}
+
+/* ── Frame builder ── */
+static void build_frame(orin_tx_frame_t *f, uint8_t out[/* ORIN_MAX_PAYLOAD + 4 */], uint8_t *out_len)
+{
+    /* [SYNC][LEN][TYPE][PAYLOAD...][CRC] */
+    uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD];
+    crc_buf[0] = f->len;
+    crc_buf[1] = f->type;
+    memcpy(&crc_buf[2], f->payload, f->len);
+    uint8_t crc = crc8(crc_buf, (uint8_t)(2u + f->len));
+
+    out[0] = ORIN_SYNC;
+    out[1] = f->len;
+    out[2] = f->type;
+    memcpy(&out[3], f->payload, f->len);
+    out[3u + f->len] = crc;
+    *out_len = (uint8_t)(4u + f->len);
+}
+
+/* ── Enqueue helpers ── */
+static void enqueue(QueueHandle_t q, uint8_t type, const uint8_t *payload, uint8_t len)
+{
+    orin_tx_frame_t f = {.type = type, .len = len};
+    if (len > 0u && payload) {
+        memcpy(f.payload, payload, len);
+    }
+    if (xQueueSend(q, &f, 0) != pdTRUE) {
+        ESP_LOGW(TAG, "tx queue full, dropped type=0x%02x", type);
+    }
+}
+
+void orin_send_ack(QueueHandle_t q, uint8_t cmd_type)
+{
+    enqueue(q, RESP_ACK, &cmd_type, 1u);
+}
+
+void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err)
+{
+    uint8_t p[2] = {cmd_type, err};
+    enqueue(q, RESP_NACK, p, 2u);
+}
+
+void orin_send_status(QueueHandle_t q,
+                      int16_t pitch_x10, int16_t motor_cmd,
+                      uint16_t vbat_mv, bal_state_t state, uint8_t flags)
+{
+    /* int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, uint8 state, uint8 flags — BE */
+    uint8_t p[8];
+    p[0] = (uint8_t)((uint16_t)pitch_x10 >> 8u);
+    p[1] = (uint8_t)((uint16_t)pitch_x10);
+    p[2] = (uint8_t)((uint16_t)motor_cmd >> 8u);
+    p[3] = (uint8_t)((uint16_t)motor_cmd);
+    p[4] = (uint8_t)(vbat_mv >> 8u);
+    p[5] = (uint8_t)(vbat_mv);
+    p[6] = (uint8_t)state;
+    p[7] = flags;
+    enqueue(q, TELEM_STATUS, p, 8u);
+}
+
+void orin_send_vesc(QueueHandle_t q, uint8_t telem_type,
+                    int32_t erpm, uint16_t voltage_mv,
+                    int16_t current_ma, uint16_t temp_c_x10)
+{
+    /* int32 erpm, uint16 voltage_mv, int16 current_ma, uint16 temp_c_x10 — BE */
+    uint8_t p[10];
+    uint32_t u = (uint32_t)erpm;
+    p[0] = (uint8_t)(u >> 24u);
+    p[1] = (uint8_t)(u >> 16u);
+    p[2] = (uint8_t)(u >>  8u);
+    p[3] = (uint8_t)(u);
+    p[4] = (uint8_t)(voltage_mv >> 8u);
+    p[5] = (uint8_t)(voltage_mv);
+    p[6] = (uint8_t)((uint16_t)current_ma >> 8u);
+    p[7] = (uint8_t)((uint16_t)current_ma);
+    p[8] = (uint8_t)(temp_c_x10 >> 8u);
+    p[9] = (uint8_t)(temp_c_x10);
+    enqueue(q, telem_type, p, 10u);
+}
+
+/* ── UART init ── */
+void orin_serial_init(void)
+{
+    uart_config_t cfg = {
+        .baud_rate  = ORIN_UART_BAUD,
+        .data_bits  = UART_DATA_8_BITS,
+        .parity     = UART_PARITY_DISABLE,
+        .stop_bits  = UART_STOP_BITS_1,
+        .flow_ctrl  = UART_HW_FLOWCTRL_DISABLE,
+    };
+    ESP_ERROR_CHECK(uart_param_config(ORIN_UART_PORT, &cfg));
+    ESP_ERROR_CHECK(uart_set_pin(ORIN_UART_PORT,
+                                  ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO,
+                                  UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
+    ESP_ERROR_CHECK(uart_driver_install(ORIN_UART_PORT, ORIN_UART_RX_BUF, 0,
+                                        0, NULL, 0));
+    ESP_LOGI(TAG, "UART%d init OK: tx=%d rx=%d baud=%d",
+             ORIN_UART_PORT, ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO, ORIN_UART_BAUD);
+}
+
+/* ── RX parser state machine ── */
+typedef enum {
+    WAIT_SYNC,
+    WAIT_LEN,
+    WAIT_TYPE,
+    WAIT_PAYLOAD,
+    WAIT_CRC,
+} rx_state_t;
+
+static void dispatch_cmd(uint8_t type, const uint8_t *payload, uint8_t len,
+                          QueueHandle_t tx_q)
+{
+    uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
+
+    switch (type) {
+    case CMD_HEARTBEAT:
+        g_orin_ctrl.hb_last_ms = now_ms;
+        orin_send_ack(tx_q, type);
+        break;
+
+    case CMD_DRIVE:
+        if (len < 4u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
+        if (g_orin_ctrl.estop) { orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE); break; }
+        if (!g_orin_ctrl.armed) { orin_send_nack(tx_q, type, ERR_DISARMED); break; }
+        g_orin_drive.speed      = (int16_t)(((uint16_t)payload[0] << 8u) | payload[1]);
+        g_orin_drive.steer      = (int16_t)(((uint16_t)payload[2] << 8u) | payload[3]);
+        g_orin_drive.updated_ms = now_ms;
+        g_orin_ctrl.hb_last_ms  = now_ms;  /* drive counts as heartbeat */
+        orin_send_ack(tx_q, type);
+        break;
+
+    case CMD_ESTOP:
+        if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
+        g_orin_ctrl.estop = (payload[0] != 0u);
+        if (g_orin_ctrl.estop) {
+            g_orin_drive.speed = 0;
+            g_orin_drive.steer = 0;
+        }
+        orin_send_ack(tx_q, type);
+        break;
+
+    case CMD_ARM:
+        if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
+        if (g_orin_ctrl.estop && payload[0] != 0u) {
+            /* cannot arm while estop is active */
+            orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE);
+            break;
+        }
+        g_orin_ctrl.armed = (payload[0] != 0u);
+        if (!g_orin_ctrl.armed) {
+            g_orin_drive.speed = 0;
+            g_orin_drive.steer = 0;
+        }
+        orin_send_ack(tx_q, type);
+        break;
+
+    case CMD_PID:
+        if (len < 12u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
+        /* float32 big-endian: copy and swap bytes */
+        {
+            uint32_t raw;
+            raw = ((uint32_t)payload[0] << 24u) | ((uint32_t)payload[1] << 16u) |
+                  ((uint32_t)payload[2] <<  8u) |  (uint32_t)payload[3];
+            memcpy((void*)&g_orin_pid.kp, &raw, 4u);
+            raw = ((uint32_t)payload[4] << 24u) | ((uint32_t)payload[5] << 16u) |
+                  ((uint32_t)payload[6] <<  8u) |  (uint32_t)payload[7];
+            memcpy((void*)&g_orin_pid.ki, &raw, 4u);
+            raw = ((uint32_t)payload[8] << 24u) | ((uint32_t)payload[9] << 16u) |
+                  ((uint32_t)payload[10] << 8u) |  (uint32_t)payload[11];
+            memcpy((void*)&g_orin_pid.kd, &raw, 4u);
+            g_orin_pid.updated = true;
+        }
+        orin_send_ack(tx_q, type);
+        break;
+
+    case CMD_OTA_CHECK:
+        /* Trigger an immediate Gitea version check */
+        gitea_ota_check_now();
+        orin_send_version_info(tx_q, OTA_TARGET_BALANCE,
+                                BALANCE_FW_VERSION,
+                                g_balance_update.available
+                                    ? g_balance_update.version : "");
+        orin_send_version_info(tx_q, OTA_TARGET_IO,
+                                IO_FW_VERSION,
+                                g_io_update.available
+                                    ? g_io_update.version : "");
+        orin_send_ack(tx_q, type);
+        break;
+
+    case CMD_OTA_UPDATE:
+        if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
+        {
+            uint8_t target = payload[0];
+            bool triggered = false;
+            if (target == OTA_TARGET_IO || target == OTA_TARGET_BOTH) {
+                if (!uart_ota_trigger()) {
+                    orin_send_nack(tx_q, type,
+                        g_io_update.available ? ERR_OTA_BUSY : ERR_OTA_NO_UPDATE);
+                    break;
+                }
+                triggered = true;
+            }
+            if (target == OTA_TARGET_BALANCE || target == OTA_TARGET_BOTH) {
+                if (!ota_self_trigger()) {
+                    if (!triggered) {
+                        orin_send_nack(tx_q, type,
+                            g_balance_update.available ? ERR_OTA_BUSY : ERR_OTA_NO_UPDATE);
+                        break;
+                    }
+                }
+            }
+            orin_send_ack(tx_q, type);
+        }
+        break;
+
+    default:
+        ESP_LOGW(TAG, "unknown cmd type=0x%02x", type);
+        break;
+    }
+}
+
+void orin_serial_rx_task(void *arg)
+{
+    QueueHandle_t tx_q = (QueueHandle_t)arg;
+    rx_state_t state   = WAIT_SYNC;
+    uint8_t    rx_len  = 0;
+    uint8_t    rx_type = 0;
+    uint8_t    payload[ORIN_MAX_PAYLOAD];
+    uint8_t    pay_idx = 0;
+
+    uint8_t byte;
+    for (;;) {
+        int r = uart_read_bytes(ORIN_UART_PORT, &byte, 1, pdMS_TO_TICKS(10));
+        if (r <= 0) {
+            continue;
+        }
+
+        switch (state) {
+        case WAIT_SYNC:
+            if (byte == ORIN_SYNC) { state = WAIT_LEN; }
+            break;
+
+        case WAIT_LEN:
+            if (byte > ORIN_MAX_PAYLOAD) {
+                /* oversize — send NACK and reset */
+                orin_send_nack(tx_q, 0x00u, ERR_BAD_LEN);
+                state = WAIT_SYNC;
+            } else {
+                rx_len = byte;
+                state  = WAIT_TYPE;
+            }
+            break;
+
+        case WAIT_TYPE:
+            rx_type = byte;
+            pay_idx = 0u;
+            state   = (rx_len == 0u) ? WAIT_CRC : WAIT_PAYLOAD;
+            break;
+
+        case WAIT_PAYLOAD:
+            payload[pay_idx++] = byte;
+            if (pay_idx == rx_len) { state = WAIT_CRC; }
+            break;
+
+        case WAIT_CRC: {
+            /* Verify CRC over [LEN, TYPE, PAYLOAD] */
+            uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD];
+            crc_buf[0] = rx_len;
+            crc_buf[1] = rx_type;
+            memcpy(&crc_buf[2], payload, rx_len);
+            uint8_t expected = crc8(crc_buf, (uint8_t)(2u + rx_len));
+            if (byte != expected) {
+                ESP_LOGW(TAG, "CRC fail type=0x%02x got=0x%02x exp=0x%02x",
+                         rx_type, byte, expected);
+                orin_send_nack(tx_q, rx_type, ERR_BAD_CRC);
+            } else {
+                dispatch_cmd(rx_type, payload, rx_len, tx_q);
+            }
+            state = WAIT_SYNC;
+            break;
+        }
+        }
+    }
+}
+
+void orin_serial_tx_task(void *arg)
+{
+    QueueHandle_t tx_q = (QueueHandle_t)arg;
+    orin_tx_frame_t f;
+    uint8_t wire[4u + ORIN_MAX_PAYLOAD];
+    uint8_t wire_len;
+
+    for (;;) {
+        if (xQueueReceive(tx_q, &f, portMAX_DELAY) == pdTRUE) {
+            build_frame(&f, wire, &wire_len);
+            uart_write_bytes(ORIN_UART_PORT, (const char *)wire, wire_len);
+        }
+    }
+}
+
+/* ── OTA telemetry helpers (bd-1s1s) ── */
+
+void orin_send_ota_status(QueueHandle_t q, uint8_t target,
+                           uint8_t state, uint8_t progress, uint8_t err)
+{
+    /* TELEM_OTA_STATUS: uint8 target, uint8 state, uint8 progress, uint8 err */
+    uint8_t p[4] = {target, state, progress, err};
+    enqueue(q, TELEM_OTA_STATUS, p, 4u);
+}
+
+void orin_send_version_info(QueueHandle_t q, uint8_t target,
+                             const char *current, const char *available)
+{
+    /* TELEM_VERSION_INFO: uint8 target, char current[16], char available[16] */
+    uint8_t p[33];
+    p[0] = target;
+    strncpy((char *)&p[1],  current,   16); p[16] = '\0';
+    strncpy((char *)&p[17], available ? available : "", 16); p[32] = '\0';
+    enqueue(q, TELEM_VERSION_INFO, p, 33u);
+}

esp32s3/balance/main/orin_serial.h

@@ -0,0 +1,113 @@
+#pragma once
+/* orin_serial.h — Orin↔ESP32-S3 BALANCE USB/UART serial protocol (bd-66hx)
+ *
+ * Frame layout (matches bd-wim1 esp32_balance_protocol.py exactly):
+ *   [0xAA][LEN][TYPE][PAYLOAD × LEN bytes][CRC8-SMBUS]
+ *   CRC covers LEN + TYPE + PAYLOAD bytes.
+ *   All multi-byte payload fields are big-endian.
+ *
+ * Physical: UART0 → CH343 USB-serial → Orin /dev/esp32-balance  @ 460800 baud
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+
+/* ── Frame constants ── */
+#define ORIN_SYNC           0xAAu
+#define ORIN_MAX_PAYLOAD    62u
+
+/* ── Command types: Orin → ESP32 ── */
+#define CMD_HEARTBEAT       0x01u
+#define CMD_DRIVE           0x02u   /* int16 speed + int16 steer, BE */
+#define CMD_ESTOP           0x03u   /* uint8: 1=assert, 0=clear */
+#define CMD_ARM             0x04u   /* uint8: 1=arm, 0=disarm */
+#define CMD_PID             0x05u   /* float32 kp, ki, kd, BE */
+
+/* ── Telemetry types: ESP32 → Orin ── */
+#define TELEM_STATUS        0x80u   /* status @ 10 Hz */
+#define TELEM_VESC_LEFT     0x81u   /* VESC ID 56 telemetry @ 10 Hz */
+#define TELEM_VESC_RIGHT    0x82u   /* VESC ID 68 telemetry @ 10 Hz */
+#define TELEM_OTA_STATUS    0x83u   /* OTA state + progress (bd-1s1s) */
+#define TELEM_VERSION_INFO  0x84u   /* firmware version report (bd-1s1s) */
+#define RESP_ACK            0xA0u
+#define RESP_NACK           0xA1u
+
+/* ── OTA commands (Orin → ESP32, bd-1s1s) ── */
+#define CMD_OTA_CHECK       0x10u   /* no payload: trigger Gitea version check */
+#define CMD_OTA_UPDATE      0x11u   /* uint8 target: 0=balance, 1=io, 2=both */
+
+/* ── OTA target constants ── */
+#define OTA_TARGET_BALANCE  0x00u
+#define OTA_TARGET_IO       0x01u
+#define OTA_TARGET_BOTH     0x02u
+
+/* ── NACK error codes ── */
+#define ERR_BAD_CRC         0x01u
+#define ERR_BAD_LEN         0x02u
+#define ERR_ESTOP_ACTIVE    0x03u
+#define ERR_DISARMED        0x04u
+#define ERR_OTA_BUSY        0x05u
+#define ERR_OTA_NO_UPDATE   0x06u
+
+/* ── Balance state (mirrored from TELEM_STATUS.balance_state) ── */
+typedef enum {
+    BAL_DISARMED   = 0,
+    BAL_ARMED      = 1,
+    BAL_TILT_FAULT = 2,
+    BAL_ESTOP      = 3,
+} bal_state_t;
+
+/* ── Shared state written by RX task, consumed by main/vesc tasks ── */
+typedef struct {
+    volatile int16_t  speed;        /* -1000..+1000 */
+    volatile int16_t  steer;        /* -1000..+1000 */
+    volatile uint32_t updated_ms;   /* esp_timer tick at last CMD_DRIVE */
+} orin_drive_t;
+
+typedef struct {
+    volatile float    kp, ki, kd;
+    volatile bool     updated;
+} orin_pid_t;
+
+typedef struct {
+    volatile bool     armed;
+    volatile bool     estop;
+    volatile uint32_t hb_last_ms;   /* esp_timer tick at last CMD_HEARTBEAT/CMD_DRIVE */
+} orin_control_t;
+
+/* ── TX frame queue item ── */
+typedef struct {
+    uint8_t type;
+    uint8_t len;
+    uint8_t payload[ORIN_MAX_PAYLOAD];
+} orin_tx_frame_t;
+
+/* ── Globals (defined in orin_serial.c, extern here) ── */
+extern orin_drive_t   g_orin_drive;
+extern orin_pid_t     g_orin_pid;
+extern orin_control_t g_orin_ctrl;
+
+/* ── API ── */
+void orin_serial_init(void);
+
+/* Tasks — pass tx_queue as arg to both */
+void orin_serial_rx_task(void *arg);   /* arg = QueueHandle_t tx_queue */
+void orin_serial_tx_task(void *arg);   /* arg = QueueHandle_t tx_queue */
+
+/* Enqueue outbound frames */
+void orin_send_status(QueueHandle_t q,
+                      int16_t pitch_x10, int16_t motor_cmd,
+                      uint16_t vbat_mv, bal_state_t state, uint8_t flags);
+void orin_send_vesc(QueueHandle_t q, uint8_t telem_type,
+                    int32_t erpm, uint16_t voltage_mv,
+                    int16_t current_ma, uint16_t temp_c_x10);
+void orin_send_ack(QueueHandle_t q, uint8_t cmd_type);
+void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err);
+
+/* OTA telemetry helpers (bd-1s1s) */
+void orin_send_ota_status(QueueHandle_t q, uint8_t target,
+                           uint8_t state, uint8_t progress, uint8_t err);
+void orin_send_version_info(QueueHandle_t q, uint8_t target,
+                             const char *current, const char *available);

esp32s3/balance/main/ota_display.c

@@ -0,0 +1,150 @@
+/* ota_display.c — OTA notification/progress UI on GC9A01 (bd-1yr8)
+ *
+ * Renders OTA state overlaid on the 240×240 round HUD display:
+ *   - BADGE: small dot on top-right when update available (idle state)
+ *   - UPDATE SCREEN: version compare, Update Balance / Update IO / Update All
+ *   - PROGRESS: arc around display perimeter + % + status text
+ *   - ERROR: red banner + "RETRY" prompt
+ *
+ * The display_draw_* primitives must be provided by the GC9A01 driver.
+ * Actual SPI driver implementation is in a separate driver bead.
+ */
+
+#include "ota_display.h"
+#include "gitea_ota.h"
+#include "version.h"
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include <stdio.h>
+#include <string.h>
+
+static const char *TAG = "ota_disp";
+
+/* Display centre and radius for the 240×240 GC9A01 */
+#define CX   120
+#define CY   120
+#define RAD  110
+
+/* ── Availability badge: 8×8 dot at top-right of display ── */
+static void draw_badge(bool balance_avail, bool io_avail)
+{
+    uint16_t col = (balance_avail || io_avail) ? COL_ORANGE : COL_BG;
+    display_fill_rect(200, 15, 12, 12, col);
+}
+
+/* ── Progress arc: sweeps 0→360° proportional to progress% ── */
+static void draw_progress_arc(uint8_t pct, uint16_t color)
+{
+    int end_deg = (int)(360 * pct / 100);
+    display_draw_arc(CX, CY, RAD, 0, end_deg, 6, color);
+}
+
+/* ── Status banner: 2 lines of text centred on display ── */
+static void draw_status(const char *line1, const char *line2,
+                         uint16_t fg, uint16_t bg)
+{
+    display_fill_rect(20, 90, 200, 60, bg);
+    if (line1 && line1[0])
+        display_draw_string(CX - (int)(strlen(line1) * 6 / 2), 96,
+                             line1, fg, bg);
+    if (line2 && line2[0])
+        display_draw_string(CX - (int)(strlen(line2) * 6 / 2), 116,
+                             line2, fg, bg);
+}
+
+/* ── Main render logic ── */
+void ota_display_update(void)
+{
+    /* Determine dominant OTA state */
+    ota_self_state_t  self  = g_ota_self_state;
+    uart_ota_send_state_t io_s = g_uart_ota_state;
+
+    switch (self) {
+    case OTA_SELF_DOWNLOADING:
+    case OTA_SELF_VERIFYING:
+    case OTA_SELF_APPLYING: {
+        /* Balance self-update in progress */
+        char pct_str[16];
+        snprintf(pct_str, sizeof(pct_str), "%d%%", g_ota_self_progress);
+        const char *phase = (self == OTA_SELF_VERIFYING) ? "Verifying..." :
+                            (self == OTA_SELF_APPLYING)  ? "Applying..." :
+                                                            "Downloading...";
+        draw_progress_arc(g_ota_self_progress, COL_BLUE);
+        draw_status("Updating Balance", pct_str, COL_WHITE, COL_BG);
+        ESP_LOGD(TAG, "balance OTA %s %d%%", phase, g_ota_self_progress);
+        return;
+    }
+    case OTA_SELF_REBOOTING:
+        draw_status("Update complete", "Rebooting...", COL_GREEN, COL_BG);
+        return;
+    case OTA_SELF_FAILED:
+        draw_progress_arc(0, COL_RED);
+        draw_status("Balance update", "FAILED  RETRY?", COL_RED, COL_BG);
+        return;
+    default:
+        break;
+    }
+
+    switch (io_s) {
+    case UART_OTA_S_DOWNLOADING:
+        draw_progress_arc(g_uart_ota_progress, COL_YELLOW);
+        draw_status("Downloading IO", "firmware...", COL_WHITE, COL_BG);
+        return;
+    case UART_OTA_S_SENDING: {
+        char pct_str[16];
+        snprintf(pct_str, sizeof(pct_str), "%d%%", g_uart_ota_progress);
+        draw_progress_arc(g_uart_ota_progress, COL_YELLOW);
+        draw_status("Updating IO", pct_str, COL_WHITE, COL_BG);
+        return;
+    }
+    case UART_OTA_S_DONE:
+        draw_status("IO update done", "", COL_GREEN, COL_BG);
+        return;
+    case UART_OTA_S_FAILED:
+        draw_progress_arc(0, COL_RED);
+        draw_status("IO update", "FAILED  RETRY?", COL_RED, COL_BG);
+        return;
+    default:
+        break;
+    }
+
+    /* Idle — show badge if update available */
+    bool bal_avail = g_balance_update.available;
+    bool io_avail  = g_io_update.available;
+    draw_badge(bal_avail, io_avail);
+
+    if (bal_avail || io_avail) {
+        /* Show available versions on display when idle */
+        char verline[32];
+        if (bal_avail) {
+            snprintf(verline, sizeof(verline), "Bal v%s rdy",
+                     g_balance_update.version);
+            draw_status(verline, io_avail ? "IO update rdy" : "",
+                         COL_ORANGE, COL_BG);
+        } else if (io_avail) {
+            snprintf(verline, sizeof(verline), "IO v%s rdy",
+                     g_io_update.version);
+            draw_status(verline, "", COL_ORANGE, COL_BG);
+        }
+    } else {
+        /* Clear OTA overlay area */
+        display_fill_rect(20, 90, 200, 60, COL_BG);
+        draw_badge(false, false);
+    }
+}
+
+/* ── Background display task (5 Hz) ── */
+static void ota_display_task(void *arg)
+{
+    for (;;) {
+        vTaskDelay(pdMS_TO_TICKS(200));
+        ota_display_update();
+    }
+}
+
+void ota_display_init(void)
+{
+    xTaskCreate(ota_display_task, "ota_disp", 2048, NULL, 3, NULL);
+    ESP_LOGI(TAG, "OTA display task started");
+}

esp32s3/balance/main/ota_display.h

@@ -0,0 +1,33 @@
+#pragma once
+/* ota_display.h — OTA notification UI on GC9A01 round LCD (bd-1yr8)
+ *
+ * GC9A01 240×240 round display via SPI (IO12 CS, IO11 DC, IO10 RST, IO9 BL).
+ * Calls into display_draw_* primitives (provided by display driver layer).
+ * This module owns the "OTA notification overlay" rendered over the HUD.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "ota_self.h"
+#include "uart_ota.h"
+
+/* ── Display primitives API (must be provided by display driver) ── */
+void display_fill_rect(int x, int y, int w, int h, uint16_t rgb565);
+void display_draw_string(int x, int y, const char *str, uint16_t fg, uint16_t bg);
+void display_draw_arc(int cx, int cy, int r, int start_deg, int end_deg,
+                       int thickness, uint16_t color);
+
+/* ── Colour palette (RGB565) ── */
+#define COL_BG       0x0000u  /* black */
+#define COL_WHITE    0xFFFFu
+#define COL_GREEN    0x07E0u
+#define COL_YELLOW   0xFFE0u
+#define COL_RED      0xF800u
+#define COL_BLUE     0x001Fu
+#define COL_ORANGE   0xFD20u
+
+/* ── OTA display task: runs at 5 Hz, overlays OTA state on HUD ── */
+void ota_display_init(void);
+
+/* Called from main loop or display task to render the OTA overlay */
+void ota_display_update(void);

esp32s3/balance/main/ota_self.c

@@ -0,0 +1,183 @@
+/* ota_self.c — Balance self-OTA (bd-18nb)
+ *
+ * Uses esp_https_ota / esp_ota_ops to download from Gitea release URL,
+ * stream-verify SHA256 with mbedTLS, set new boot partition, and reboot.
+ * CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE in sdkconfig allows auto-rollback
+ * if the new image doesn't call esp_ota_mark_app_valid_cancel_rollback()
+ * within OTA_ROLLBACK_WINDOW_S seconds.
+ */
+
+#include "ota_self.h"
+#include "gitea_ota.h"
+#include "esp_log.h"
+#include "esp_ota_ops.h"
+#include "esp_http_client.h"
+#include "esp_timer.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "mbedtls/sha256.h"
+#include <string.h>
+#include <stdio.h>
+
+static const char *TAG = "ota_self";
+
+volatile ota_self_state_t g_ota_self_state   = OTA_SELF_IDLE;
+volatile uint8_t          g_ota_self_progress = 0;
+
+#define OTA_CHUNK_SIZE  4096
+
+/* ── SHA256 verify helper ── */
+static bool sha256_matches(const uint8_t *digest, const char *expected_hex)
+{
+    if (!expected_hex || expected_hex[0] == '\0') {
+        ESP_LOGW(TAG, "no SHA256 to verify — skipping");
+        return true;
+    }
+    char got[65] = {0};
+    for (int i = 0; i < 32; i++) {
+        snprintf(&got[i*2], 3, "%02x", digest[i]);
+    }
+    bool ok = (strncasecmp(got, expected_hex, 64) == 0);
+    if (!ok) {
+        ESP_LOGE(TAG, "SHA256 mismatch: got=%s exp=%s", got, expected_hex);
+    }
+    return ok;
+}
+
+/* ── OTA download + flash task ── */
+static void ota_self_task(void *arg)
+{
+    const char *url    = g_balance_update.download_url;
+    const char *sha256 = g_balance_update.sha256;
+
+    g_ota_self_state    = OTA_SELF_DOWNLOADING;
+    g_ota_self_progress = 0;
+
+    ESP_LOGI(TAG, "OTA start: %s", url);
+
+    esp_ota_handle_t       handle     = 0;
+    const esp_partition_t *ota_part   = esp_ota_get_next_update_partition(NULL);
+    if (!ota_part) {
+        ESP_LOGE(TAG, "no OTA partition");
+        g_ota_self_state = OTA_SELF_FAILED;
+        vTaskDelete(NULL);
+        return;
+    }
+
+    esp_err_t err = esp_ota_begin(ota_part, OTA_WITH_SEQUENTIAL_WRITES, &handle);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "ota_begin: %s", esp_err_to_name(err));
+        g_ota_self_state = OTA_SELF_FAILED;
+        vTaskDelete(NULL);
+        return;
+    }
+
+    /* Setup HTTP client */
+    esp_http_client_config_t hcfg = {
+        .url        = url,
+        .timeout_ms = 30000,
+        .buffer_size = OTA_CHUNK_SIZE,
+        .skip_cert_common_name_check = true,
+    };
+    esp_http_client_handle_t client = esp_http_client_init(&hcfg);
+    err = esp_http_client_open(client, 0);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "http_open: %s", esp_err_to_name(err));
+        esp_ota_abort(handle);
+        esp_http_client_cleanup(client);
+        g_ota_self_state = OTA_SELF_FAILED;
+        vTaskDelete(NULL);
+        return;
+    }
+
+    int content_len = esp_http_client_fetch_headers(client);
+    ESP_LOGI(TAG, "content-length: %d", content_len);
+
+    mbedtls_sha256_context sha_ctx;
+    mbedtls_sha256_init(&sha_ctx);
+    mbedtls_sha256_starts(&sha_ctx, 0);  /* 0 = SHA-256 */
+
+    static uint8_t buf[OTA_CHUNK_SIZE];
+    int total = 0;
+    int rd;
+    while ((rd = esp_http_client_read(client, (char *)buf, sizeof(buf))) > 0) {
+        mbedtls_sha256_update(&sha_ctx, buf, rd);
+        err = esp_ota_write(handle, buf, rd);
+        if (err != ESP_OK) {
+            ESP_LOGE(TAG, "ota_write: %s", esp_err_to_name(err));
+            esp_ota_abort(handle);
+            goto cleanup;
+        }
+        total += rd;
+        if (content_len > 0) {
+            g_ota_self_progress = (uint8_t)((total * 100) / content_len);
+        }
+    }
+    esp_http_client_close(client);
+
+    /* Verify SHA256 */
+    g_ota_self_state = OTA_SELF_VERIFYING;
+    uint8_t digest[32];
+    mbedtls_sha256_finish(&sha_ctx, digest);
+    if (!sha256_matches(digest, sha256)) {
+        ESP_LOGE(TAG, "SHA256 verification failed");
+        esp_ota_abort(handle);
+        g_ota_self_state = OTA_SELF_FAILED;
+        goto cleanup;
+    }
+
+    /* Finalize + set boot partition */
+    g_ota_self_state = OTA_SELF_APPLYING;
+    err = esp_ota_end(handle);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "ota_end: %s", esp_err_to_name(err));
+        g_ota_self_state = OTA_SELF_FAILED;
+        goto cleanup;
+    }
+
+    err = esp_ota_set_boot_partition(ota_part);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "set_boot_partition: %s", esp_err_to_name(err));
+        g_ota_self_state = OTA_SELF_FAILED;
+        goto cleanup;
+    }
+
+    g_ota_self_state    = OTA_SELF_REBOOTING;
+    g_ota_self_progress = 100;
+    ESP_LOGI(TAG, "OTA success — rebooting");
+    vTaskDelay(pdMS_TO_TICKS(500));
+    esp_restart();
+
+cleanup:
+    mbedtls_sha256_free(&sha_ctx);
+    esp_http_client_cleanup(client);
+    handle = 0;
+    vTaskDelete(NULL);
+}
+
+bool ota_self_trigger(void)
+{
+    if (!g_balance_update.available) {
+        ESP_LOGW(TAG, "no update available");
+        return false;
+    }
+    if (g_ota_self_state != OTA_SELF_IDLE) {
+        ESP_LOGW(TAG, "OTA already in progress (state=%d)", g_ota_self_state);
+        return false;
+    }
+    xTaskCreate(ota_self_task, "ota_self", 8192, NULL, 5, NULL);
+    return true;
+}
+
+void ota_self_health_check(void)
+{
+    /* Mark running image as valid — prevents rollback */
+    esp_err_t err = esp_ota_mark_app_valid_cancel_rollback();
+    if (err == ESP_OK) {
+        ESP_LOGI(TAG, "image marked valid");
+    } else if (err == ESP_ERR_NOT_SUPPORTED) {
+        /* Not an OTA image (e.g., flashed via JTAG) — ignore */
+    } else {
+        ESP_LOGW(TAG, "mark_valid: %s", esp_err_to_name(err));
+    }
+}

esp32s3/balance/main/ota_self.h

@@ -0,0 +1,34 @@
+#pragma once
+/* ota_self.h — Balance self-OTA (bd-18nb)
+ *
+ * Downloads balance-firmware.bin from Gitea release URL to the inactive
+ * OTA partition, verifies SHA256, sets boot partition, reboots.
+ * Auto-rollback if health check not called within ROLLBACK_WINDOW_S seconds.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define OTA_ROLLBACK_WINDOW_S  30
+
+typedef enum {
+    OTA_SELF_IDLE      = 0,
+    OTA_SELF_CHECKING,    /* (unused — gitea_ota handles this) */
+    OTA_SELF_DOWNLOADING,
+    OTA_SELF_VERIFYING,
+    OTA_SELF_APPLYING,
+    OTA_SELF_REBOOTING,
+    OTA_SELF_FAILED,
+} ota_self_state_t;
+
+extern volatile ota_self_state_t g_ota_self_state;
+extern volatile uint8_t          g_ota_self_progress; /* 0-100 % */
+
+/* Trigger a Balance self-update.
+ * Uses g_balance_update (from gitea_ota). Non-blocking: starts in a task.
+ * Returns false if no update available or OTA already in progress. */
+bool ota_self_trigger(void);
+
+/* Called from app_main after boot to mark the running image as valid.
+ * Must be called within OTA_ROLLBACK_WINDOW_S after boot or rollback fires. */
+void ota_self_health_check(void);

esp32s3/balance/main/uart_ota.c

@@ -0,0 +1,241 @@
+/* uart_ota.c — UART OTA sender: Balance→IO board (bd-21hv)
+ *
+ * Downloads io-firmware.bin from Gitea, then sends to IO board via UART1.
+ * IO board must update itself BEFORE Balance self-update (per spec).
+ */
+
+#include "uart_ota.h"
+#include "gitea_ota.h"
+#include "esp_log.h"
+#include "esp_http_client.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "mbedtls/sha256.h"
+#include <string.h>
+#include <stdio.h>
+
+static const char *TAG = "uart_ota";
+
+volatile uart_ota_send_state_t g_uart_ota_state    = UART_OTA_S_IDLE;
+volatile uint8_t               g_uart_ota_progress  = 0;
+
+/* ── CRC8-SMBUS ── */
+static uint8_t crc8(const uint8_t *d, uint16_t len)
+{
+    uint8_t crc = 0;
+    for (uint16_t i = 0; i < len; i++) {
+        crc ^= d[i];
+        for (uint8_t b = 0; b < 8; b++)
+            crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u);
+    }
+    return crc;
+}
+
+/* ── Build and send one UART OTA frame ── */
+static void send_frame(uint8_t type, uint16_t seq,
+                        const uint8_t *payload, uint16_t plen)
+{
+    /* [TYPE:1][SEQ:2 BE][LEN:2 BE][PAYLOAD][CRC8:1] */
+    uint8_t hdr[5];
+    hdr[0] = type;
+    hdr[1] = (uint8_t)(seq >> 8u);
+    hdr[2] = (uint8_t)(seq);
+    hdr[3] = (uint8_t)(plen >> 8u);
+    hdr[4] = (uint8_t)(plen);
+
+    /* CRC over hdr + payload */
+    uint8_t crc_buf[5 + OTA_UART_CHUNK_SIZE];
+    memcpy(crc_buf, hdr, 5);
+    if (plen > 0 && payload) memcpy(crc_buf + 5, payload, plen);
+    uint8_t crc = crc8(crc_buf, (uint16_t)(5 + plen));
+
+    uart_write_bytes(UART_OTA_PORT, (char *)hdr, 5);
+    if (plen > 0 && payload)
+        uart_write_bytes(UART_OTA_PORT, (char *)payload, plen);
+    uart_write_bytes(UART_OTA_PORT, (char *)&crc, 1);
+}
+
+/* ── Wait for ACK/NACK from IO board ── */
+static bool wait_ack(uint16_t expected_seq)
+{
+    /* Response frame: [TYPE:1][SEQ:2][LEN:2][PAYLOAD][CRC:1] */
+    uint8_t buf[16];
+    int timeout = OTA_UART_ACK_TIMEOUT_MS;
+    int got = 0;
+
+    while (timeout > 0 && got < 6) {
+        int r = uart_read_bytes(UART_OTA_PORT, buf + got, 1, pdMS_TO_TICKS(50));
+        if (r > 0) got++;
+        else timeout -= 50;
+    }
+
+    if (got < 3) return false;
+
+    uint8_t type = buf[0];
+    uint16_t seq = (uint16_t)((buf[1] << 8u) | buf[2]);
+
+    if (type == UART_OTA_ACK && seq == expected_seq) return true;
+    if (type == UART_OTA_NACK) {
+        uint8_t err = (got >= 6) ? buf[5] : 0;
+        ESP_LOGW(TAG, "NACK seq=%u err=%u", seq, err);
+    }
+    return false;
+}
+
+/* ── Download firmware to RAM buffer (max 1.75 MB) ── */
+static uint8_t *download_io_firmware(uint32_t *out_size)
+{
+    const char *url = g_io_update.download_url;
+    ESP_LOGI(TAG, "downloading IO fw: %s", url);
+
+    esp_http_client_config_t cfg = {
+        .url = url, .timeout_ms = 30000,
+        .skip_cert_common_name_check = true,
+    };
+    esp_http_client_handle_t client = esp_http_client_init(&cfg);
+    if (esp_http_client_open(client, 0) != ESP_OK) {
+        esp_http_client_cleanup(client);
+        return NULL;
+    }
+
+    int content_len = esp_http_client_fetch_headers(client);
+    if (content_len <= 0 || content_len > (int)(0x1B0000)) {
+        ESP_LOGE(TAG, "bad content-length: %d", content_len);
+        esp_http_client_cleanup(client);
+        return NULL;
+    }
+
+    uint8_t *buf = malloc(content_len);
+    if (!buf) {
+        ESP_LOGE(TAG, "malloc %d failed", content_len);
+        esp_http_client_cleanup(client);
+        return NULL;
+    }
+
+    int total = 0, rd;
+    while ((rd = esp_http_client_read(client, (char *)buf + total,
+                                       content_len - total)) > 0) {
+        total += rd;
+        g_uart_ota_progress = (uint8_t)((total * 50) / content_len); /* 0-50% for download */
+    }
+    esp_http_client_cleanup(client);
+
+    if (total != content_len) {
+        free(buf);
+        return NULL;
+    }
+    *out_size = (uint32_t)total;
+    return buf;
+}
+
+/* ── UART OTA send task ── */
+static void uart_ota_task(void *arg)
+{
+    g_uart_ota_state    = UART_OTA_S_DOWNLOADING;
+    g_uart_ota_progress = 0;
+
+    uint32_t fw_size = 0;
+    uint8_t *fw = download_io_firmware(&fw_size);
+    if (!fw) {
+        ESP_LOGE(TAG, "download failed");
+        g_uart_ota_state = UART_OTA_S_FAILED;
+        vTaskDelete(NULL);
+        return;
+    }
+
+    /* Compute SHA256 of downloaded firmware */
+    uint8_t digest[32];
+    mbedtls_sha256_context sha;
+    mbedtls_sha256_init(&sha);
+    mbedtls_sha256_starts(&sha, 0);
+    mbedtls_sha256_update(&sha, fw, fw_size);
+    mbedtls_sha256_finish(&sha, digest);
+    mbedtls_sha256_free(&sha);
+
+    g_uart_ota_state = UART_OTA_S_SENDING;
+
+    /* Send OTA_BEGIN: uint32 size + uint8[32] sha256 */
+    uint8_t begin_payload[36];
+    begin_payload[0] = (uint8_t)(fw_size >> 24u);
+    begin_payload[1] = (uint8_t)(fw_size >> 16u);
+    begin_payload[2] = (uint8_t)(fw_size >>  8u);
+    begin_payload[3] = (uint8_t)(fw_size);
+    memcpy(&begin_payload[4], digest, 32);
+
+    for (int retry = 0; retry < OTA_UART_MAX_RETRIES; retry++) {
+        send_frame(UART_OTA_BEGIN, 0, begin_payload, 36);
+        if (wait_ack(0)) goto send_data;
+        ESP_LOGW(TAG, "BEGIN retry %d", retry);
+    }
+    ESP_LOGE(TAG, "BEGIN failed");
+    free(fw);
+    g_uart_ota_state = UART_OTA_S_FAILED;
+    vTaskDelete(NULL);
+    return;
+
+send_data: {
+    uint32_t offset = 0;
+    uint16_t seq    = 1;
+    while (offset < fw_size) {
+        uint16_t chunk = (uint16_t)((fw_size - offset) < OTA_UART_CHUNK_SIZE
+                          ? (fw_size - offset) : OTA_UART_CHUNK_SIZE);
+        bool acked = false;
+        for (int retry = 0; retry < OTA_UART_MAX_RETRIES; retry++) {
+            send_frame(UART_OTA_DATA, seq, fw + offset, chunk);
+            if (wait_ack(seq)) { acked = true; break; }
+            ESP_LOGW(TAG, "DATA seq=%u retry=%d", seq, retry);
+        }
+        if (!acked) {
+            ESP_LOGE(TAG, "DATA seq=%u failed", seq);
+            send_frame(UART_OTA_ABORT, seq, NULL, 0);
+            free(fw);
+            g_uart_ota_state = UART_OTA_S_FAILED;
+            vTaskDelete(NULL);
+            return;
+        }
+        offset += chunk;
+        seq++;
+        /* 50-100% for sending phase */
+        g_uart_ota_progress = (uint8_t)(50u + (offset * 50u) / fw_size);
+    }
+
+    /* Send OTA_END */
+    for (int retry = 0; retry < OTA_UART_MAX_RETRIES; retry++) {
+        send_frame(UART_OTA_END, seq, NULL, 0);
+        if (wait_ack(seq)) break;
+    }
+    }
+
+    free(fw);
+    g_uart_ota_progress = 100;
+    g_uart_ota_state    = UART_OTA_S_DONE;
+    ESP_LOGI(TAG, "IO OTA complete — %lu bytes sent", (unsigned long)fw_size);
+    vTaskDelete(NULL);
+}
+
+bool uart_ota_trigger(void)
+{
+    if (!g_io_update.available) {
+        ESP_LOGW(TAG, "no IO update available");
+        return false;
+    }
+    if (g_uart_ota_state != UART_OTA_S_IDLE) {
+        ESP_LOGW(TAG, "UART OTA busy (state=%d)", g_uart_ota_state);
+        return false;
+    }
+    /* Init UART1 for OTA */
+    uart_config_t ucfg = {
+        .baud_rate  = UART_OTA_BAUD,
+        .data_bits  = UART_DATA_8_BITS,
+        .parity     = UART_PARITY_DISABLE,
+        .stop_bits  = UART_STOP_BITS_1,
+        .flow_ctrl  = UART_HW_FLOWCTRL_DISABLE,
+    };
+    uart_param_config(UART_OTA_PORT, &ucfg);
+    uart_set_pin(UART_OTA_PORT, UART_OTA_TX_GPIO, UART_OTA_RX_GPIO,
+                  UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+    uart_driver_install(UART_OTA_PORT, 2048, 0, 0, NULL, 0);
+
+    xTaskCreate(uart_ota_task, "uart_ota", 16384, NULL, 4, NULL);
+    return true;
+}

esp32s3/balance/main/uart_ota.h

@@ -0,0 +1,64 @@
+#pragma once
+/* uart_ota.h — UART OTA protocol for Balance→IO firmware update (bd-21hv)
+ *
+ * Balance downloads io-firmware.bin from Gitea, then streams it to the IO
+ * board over UART1 (GPIO17/18, 460800 baud) in 1 KB chunks with ACK.
+ *
+ * Protocol frame format (both directions):
+ *   [TYPE:1][SEQ:2 BE][LEN:2 BE][PAYLOAD:LEN][CRC8:1]
+ *   CRC8-SMBUS over TYPE+SEQ+LEN+PAYLOAD.
+ *
+ * Balance→IO:
+ *   OTA_BEGIN  (0xC0) payload: uint32 total_size BE + uint8[32] sha256
+ *   OTA_DATA   (0xC1) payload: uint8[] chunk (up to 1024 bytes)
+ *   OTA_END    (0xC2) no payload
+ *   OTA_ABORT  (0xC3) no payload
+ *
+ * IO→Balance:
+ *   OTA_ACK    (0xC4) payload: uint16 acked_seq BE
+ *   OTA_NACK   (0xC5) payload: uint16 failed_seq BE + uint8 err_code
+ *   OTA_STATUS (0xC6) payload: uint8 state + uint8 progress%
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* UART for Balance→IO OTA */
+#include "driver/uart.h"
+#define UART_OTA_PORT       UART_NUM_1
+#define UART_OTA_BAUD       460800
+#define UART_OTA_TX_GPIO    17
+#define UART_OTA_RX_GPIO    18
+
+#define OTA_UART_CHUNK_SIZE  1024
+#define OTA_UART_ACK_TIMEOUT_MS  3000
+#define OTA_UART_MAX_RETRIES     3
+
+/* Frame type bytes */
+#define UART_OTA_BEGIN   0xC0u
+#define UART_OTA_DATA    0xC1u
+#define UART_OTA_END     0xC2u
+#define UART_OTA_ABORT   0xC3u
+#define UART_OTA_ACK     0xC4u
+#define UART_OTA_NACK    0xC5u
+#define UART_OTA_STATUS  0xC6u
+
+/* NACK error codes */
+#define OTA_ERR_BAD_CRC  0x01u
+#define OTA_ERR_WRITE    0x02u
+#define OTA_ERR_SIZE     0x03u
+
+typedef enum {
+    UART_OTA_S_IDLE        = 0,
+    UART_OTA_S_DOWNLOADING,  /* downloading from Gitea */
+    UART_OTA_S_SENDING,      /* sending to IO board */
+    UART_OTA_S_DONE,
+    UART_OTA_S_FAILED,
+} uart_ota_send_state_t;
+
+extern volatile uart_ota_send_state_t g_uart_ota_state;
+extern volatile uint8_t               g_uart_ota_progress;
+
+/* Trigger IO firmware update. Uses g_io_update (from gitea_ota).
+ * Downloads bin, then streams via UART. Returns false if busy or no update. */
+bool uart_ota_trigger(void);

esp32s3/balance/main/version.h

@@ -0,0 +1,14 @@
+#pragma once
+/* Embedded firmware version — bump on each release */
+#define BALANCE_FW_VERSION      "1.0.0"
+#define IO_FW_VERSION           "1.0.0"
+
+/* Gitea release tag prefixes */
+#define BALANCE_TAG_PREFIX      "esp32-balance/"
+#define IO_TAG_PREFIX           "esp32-io/"
+
+/* Gitea release asset filenames */
+#define BALANCE_BIN_ASSET       "balance-firmware.bin"
+#define IO_BIN_ASSET            "io-firmware.bin"
+#define BALANCE_SHA256_ASSET    "balance-firmware.sha256"
+#define IO_SHA256_ASSET         "io-firmware.sha256"

esp32s3/balance/main/vesc_can.c

@@ -0,0 +1,119 @@
+/* vesc_can.c — VESC CAN TWAI driver (bd-66hx)
+ *
+ * Receives VESC STATUS/4/5 frames via TWAI, proxies to Orin over serial.
+ * Transmits SET_RPM commands from Orin drive requests.
+ */
+
+#include "vesc_can.h"
+#include "orin_serial.h"
+#include "config.h"
+#include "driver/twai.h"
+#include "esp_log.h"
+#include "esp_timer.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include <string.h>
+
+static const char *TAG = "vesc_can";
+
+vesc_state_t g_vesc[2] = {0};
+
+/* Index for a given VESC node ID: 0=VESC_ID_A, 1=VESC_ID_B */
+static int vesc_idx(uint8_t id)
+{
+    if (id == VESC_ID_A) return 0;
+    if (id == VESC_ID_B) return 1;
+    return -1;
+}
+
+void vesc_can_init(void)
+{
+    twai_general_config_t gcfg = TWAI_GENERAL_CONFIG_DEFAULT(
+        (gpio_num_t)VESC_CAN_TX_GPIO,
+        (gpio_num_t)VESC_CAN_RX_GPIO,
+        TWAI_MODE_NORMAL);
+    gcfg.rx_queue_len = VESC_CAN_RX_QUEUE;
+
+    twai_timing_config_t  tcfg = TWAI_TIMING_CONFIG_500KBITS();
+    twai_filter_config_t  fcfg = TWAI_FILTER_CONFIG_ACCEPT_ALL();
+
+    ESP_ERROR_CHECK(twai_driver_install(&gcfg, &tcfg, &fcfg));
+    ESP_ERROR_CHECK(twai_start());
+    ESP_LOGI(TAG, "TWAI init OK: tx=%d rx=%d 500kbps", VESC_CAN_TX_GPIO, VESC_CAN_RX_GPIO);
+}
+
+void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
+{
+    uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | vesc_id;
+    twai_message_t msg = {
+        .extd            = 1,
+        .identifier      = ext_id,
+        .data_length_code = 4,
+    };
+    uint32_t u = (uint32_t)erpm;
+    msg.data[0] = (uint8_t)(u >> 24u);
+    msg.data[1] = (uint8_t)(u >> 16u);
+    msg.data[2] = (uint8_t)(u >>  8u);
+    msg.data[3] = (uint8_t)(u);
+    twai_transmit(&msg, pdMS_TO_TICKS(5));
+}
+
+void vesc_can_rx_task(void *arg)
+{
+    QueueHandle_t tx_q = (QueueHandle_t)arg;
+    twai_message_t msg;
+
+    for (;;) {
+        if (twai_receive(&msg, pdMS_TO_TICKS(50)) != ESP_OK) {
+            continue;
+        }
+        if (!msg.extd) {
+            continue;  /* ignore standard frames */
+        }
+
+        uint8_t pkt_type = (uint8_t)(msg.identifier >> 8u);
+        uint8_t vesc_id  = (uint8_t)(msg.identifier & 0xFFu);
+        int idx = vesc_idx(vesc_id);
+        if (idx < 0) {
+            continue;  /* not our VESC */
+        }
+
+        uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
+        vesc_state_t *s = &g_vesc[idx];
+
+        switch (pkt_type) {
+        case VESC_PKT_STATUS:
+            if (msg.data_length_code < 8u) { break; }
+            s->erpm = (int32_t)(
+                ((uint32_t)msg.data[0] << 24u) | ((uint32_t)msg.data[1] << 16u) |
+                ((uint32_t)msg.data[2] <<  8u) |  (uint32_t)msg.data[3]);
+            s->current_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]);
+            s->last_rx_ms  = now_ms;
+            /* Proxy to Orin: voltage from STATUS_5 (may be zero until received) */
+            {
+                uint8_t ttype = (vesc_id == VESC_ID_A) ? TELEM_VESC_LEFT : TELEM_VESC_RIGHT;
+                /* voltage_mv: V×10 → mV (/10 * 1000 = *100); current_ma: A×10 → mA (*100) */
+                uint16_t vmv = (uint16_t)((int32_t)s->voltage_x10 * 100);
+                int16_t  ima = (int16_t)((int32_t)s->current_x10 * 100);
+                orin_send_vesc(tx_q, ttype, s->erpm, vmv, ima,
+                               (uint16_t)s->temp_mot_x10);
+            }
+            break;
+
+        case VESC_PKT_STATUS_4:
+            if (msg.data_length_code < 6u) { break; }
+            /* T_fet×10, T_mot×10, I_in×10 */
+            s->temp_mot_x10 = (int16_t)(((uint16_t)msg.data[2] << 8u) | msg.data[3]);
+            break;
+
+        case VESC_PKT_STATUS_5:
+            if (msg.data_length_code < 6u) { break; }
+            /* int32 tacho (ignored), int16 V_in×10 */
+            s->voltage_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]);
+            break;
+
+        default:
+            break;
+        }
+    }
+}

esp32s3/balance/main/vesc_can.h

@@ -0,0 +1,36 @@
+#pragma once
+/* vesc_can.h — VESC CAN TWAI driver for ESP32-S3 BALANCE (bd-66hx)
+ *
+ * VESC extended CAN ID: (packet_type << 8) | vesc_node_id
+ * Physical layer: TWAI peripheral → SN65HVD230 → 500 kbps shared bus
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+
+/* ── VESC packet types ── */
+#define VESC_PKT_SET_RPM    3u
+#define VESC_PKT_STATUS     9u    /* int32 erpm, int16 I×10, int16 duty×1000 */
+#define VESC_PKT_STATUS_4   16u   /* int16 T_fet×10, T_mot×10, I_in×10 */
+#define VESC_PKT_STATUS_5   27u   /* int32 tacho, int16 V_in×10 */
+
+/* ── VESC telemetry snapshot ── */
+typedef struct {
+    int32_t  erpm;          /* electrical RPM (STATUS) */
+    int16_t  current_x10;   /* phase current A×10 (STATUS) */
+    int16_t  voltage_x10;   /* bus voltage V×10 (STATUS_5) */
+    int16_t  temp_mot_x10;  /* motor temp °C×10 (STATUS_4) */
+    uint32_t last_rx_ms;    /* esp_timer ms of last STATUS frame */
+} vesc_state_t;
+
+/* ── Globals (two VESC nodes: index 0 = VESC_ID_A=56, 1 = VESC_ID_B=68) ── */
+extern vesc_state_t g_vesc[2];
+
+/* ── API ── */
+void vesc_can_init(void);
+void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm);
+
+/* RX task — pass tx_queue as arg; forwards STATUS frames to Orin over serial */
+void vesc_can_rx_task(void *arg);   /* arg = QueueHandle_t orin_tx_queue */

esp32s3/balance/partitions.csv

@@ -0,0 +1,7 @@
+# ESP32-S3 BALANCE — 4 MB flash, dual OTA partitions
+# Name,     Type,  SubType,  Offset,    Size
+nvs,        data,  nvs,      0x9000,    0x5000,
+otadata,    data,  ota,      0xe000,    0x2000,
+app0,       app,   ota_0,    0x10000,   0x1B0000,
+app1,       app,   ota_1,    0x1C0000,  0x1B0000,
+nvs_user,   data,  nvs,      0x370000,  0x50000,

esp32s3/balance/sdkconfig.defaults

@@ -0,0 +1,19 @@
+CONFIG_IDF_TARGET="esp32s3"
+CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
+CONFIG_FREERTOS_HZ=1000
+CONFIG_ESP_TASK_WDT_EN=y
+CONFIG_ESP_TASK_WDT_TIMEOUT_S=5
+CONFIG_TWAI_ISR_IN_IRAM=y
+CONFIG_UART_ISR_IN_IRAM=y
+CONFIG_ESP_CONSOLE_UART_DEFAULT=y
+CONFIG_ESP_CONSOLE_UART_NUM=0
+CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200
+CONFIG_LOG_DEFAULT_LEVEL_INFO=y
+
+# OTA — bd-3gwo: dual OTA partitions + rollback
+CONFIG_PARTITION_TABLE_CUSTOM=y
+CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
+CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE=y
+CONFIG_OTA_ALLOW_HTTP=y
+CONFIG_ESP_HTTPS_OTA_ALLOW_HTTP=y
+CONFIG_MBEDTLS_CERTIFICATE_BUNDLE=y

esp32s3/io/CMakeLists.txt

@@ -0,0 +1,3 @@
+cmake_minimum_required(VERSION 3.16)
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(esp32s3_io)

esp32s3/io/main/CMakeLists.txt

@@ -0,0 +1,10 @@
+idf_component_register(
+    SRCS "main.c" "uart_ota_recv.c"
+    INCLUDE_DIRS "."
+    REQUIRES
+        app_update
+        mbedtls
+        driver
+        freertos
+        esp_timer
+)

esp32s3/io/main/config.h

@@ -0,0 +1,35 @@
+#pragma once
+/* ESP32-S3 IO board — pin assignments (SAUL-TEE-SYSTEM-REFERENCE.md) */
+
+/* ── Inter-board UART (to/from BALANCE board) ── */
+#define IO_UART_PORT        UART_NUM_0
+#define IO_UART_BAUD        460800
+#define IO_UART_TX_GPIO     43   /* IO board UART0_TXD → BALANCE RX */
+#define IO_UART_RX_GPIO     44   /* IO board UART0_RXD ← BALANCE TX */
+/* Note: SAUL-TEE spec says IO TX=IO18, RX=IO21; BALANCE TX=IO17, RX=IO18.
+ * This is UART0 on the IO devkit (GPIO43/44). Adjust to match actual wiring. */
+
+/* ── BTS7960 Left motor driver ── */
+#define MOTOR_L_RPWM    1
+#define MOTOR_L_LPWM    2
+#define MOTOR_L_EN_R    3
+#define MOTOR_L_EN_L    4
+
+/* ── BTS7960 Right motor driver ── */
+#define MOTOR_R_RPWM    5
+#define MOTOR_R_LPWM    6
+#define MOTOR_R_EN_R    7
+#define MOTOR_R_EN_L    8
+
+/* ── Arming button / kill switch ── */
+#define ARM_BTN_GPIO    9
+#define KILL_GPIO       10
+
+/* ── WS2812B LED strip ── */
+#define LED_DATA_GPIO   13
+
+/* ── OTA UART — receives firmware from BALANCE (bd-21hv) ── */
+/* Uses same IO_UART_PORT since Balance drives OTA over the inter-board link */
+
+/* ── Firmware version ── */
+#define IO_FW_VERSION   "1.0.0"

esp32s3/io/main/main.c

@@ -0,0 +1,42 @@
+/* main.c — ESP32-S3 IO board app_main */
+
+#include "uart_ota_recv.h"
+#include "config.h"
+#include "esp_log.h"
+#include "esp_ota_ops.h"
+#include "driver/uart.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+static const char *TAG = "io_main";
+
+static void uart_init(void)
+{
+    uart_config_t cfg = {
+        .baud_rate  = IO_UART_BAUD,
+        .data_bits  = UART_DATA_8_BITS,
+        .parity     = UART_PARITY_DISABLE,
+        .stop_bits  = UART_STOP_BITS_1,
+        .flow_ctrl  = UART_HW_FLOWCTRL_DISABLE,
+    };
+    uart_param_config(IO_UART_PORT, &cfg);
+    uart_set_pin(IO_UART_PORT, IO_UART_TX_GPIO, IO_UART_RX_GPIO,
+                  UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+    uart_driver_install(IO_UART_PORT, 4096, 0, 0, NULL, 0);
+}
+
+void app_main(void)
+{
+    ESP_LOGI(TAG, "ESP32-S3 IO v%s starting", IO_FW_VERSION);
+
+    /* Mark running image valid (OTA rollback support) */
+    esp_ota_mark_app_valid_cancel_rollback();
+
+    uart_init();
+    uart_ota_recv_init();
+
+    /* IO board main loop placeholder — RC/motor/sensor tasks added in later beads */
+    while (1) {
+        vTaskDelay(pdMS_TO_TICKS(1000));
+    }
+}

esp32s3/io/main/uart_ota_recv.c

@@ -0,0 +1,210 @@
+/* uart_ota_recv.c — IO board OTA receiver (bd-21hv)
+ *
+ * Listens on UART0 for OTA frames from Balance board.
+ * Writes incoming chunks to the inactive OTA partition, verifies SHA256,
+ * then reboots into new firmware.
+ */
+
+#include "uart_ota_recv.h"
+#include "config.h"
+#include "esp_log.h"
+#include "esp_ota_ops.h"
+#include "driver/uart.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "mbedtls/sha256.h"
+#include <string.h>
+
+static const char *TAG = "io_ota";
+
+volatile io_ota_state_t g_io_ota_state    = IO_OTA_IDLE;
+volatile uint8_t        g_io_ota_progress = 0;
+
+/* Frame type bytes (same as uart_ota.h sender side) */
+#define OTA_BEGIN   0xC0u
+#define OTA_DATA    0xC1u
+#define OTA_END     0xC2u
+#define OTA_ABORT   0xC3u
+#define OTA_ACK     0xC4u
+#define OTA_NACK    0xC5u
+
+#define CHUNK_MAX   1024
+
+static uint8_t crc8(const uint8_t *d, uint16_t len)
+{
+    uint8_t crc = 0;
+    for (uint16_t i = 0; i < len; i++) {
+        crc ^= d[i];
+        for (uint8_t b = 0; b < 8; b++)
+            crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u);
+    }
+    return crc;
+}
+
+static void send_ack(uint16_t seq)
+{
+    uint8_t frame[6];
+    frame[0] = OTA_ACK;
+    frame[1] = (uint8_t)(seq >> 8u);
+    frame[2] = (uint8_t)(seq);
+    frame[3] = 0; frame[4] = 0;  /* LEN=0 */
+    uint8_t crc = crc8(frame, 5);
+    frame[5] = crc;
+    uart_write_bytes(IO_UART_PORT, (char *)frame, 6);
+}
+
+static void send_nack(uint16_t seq, uint8_t err)
+{
+    uint8_t frame[8];
+    frame[0] = OTA_NACK;
+    frame[1] = (uint8_t)(seq >> 8u);
+    frame[2] = (uint8_t)(seq);
+    frame[3] = 0; frame[4] = 1;  /* LEN=1 */
+    frame[5] = err;
+    uint8_t crc = crc8(frame, 6);
+    frame[6] = crc;
+    uart_write_bytes(IO_UART_PORT, (char *)frame, 7);
+}
+
+/* Read exact n bytes with timeout */
+static bool uart_read_exact(uint8_t *buf, int n, int timeout_ms)
+{
+    int got = 0;
+    while (got < n && timeout_ms > 0) {
+        int r = uart_read_bytes(IO_UART_PORT, buf + got, n - got,
+                                pdMS_TO_TICKS(50));
+        if (r > 0) got += r;
+        else timeout_ms -= 50;
+    }
+    return got == n;
+}
+
+static void ota_recv_task(void *arg)
+{
+    esp_ota_handle_t       handle    = 0;
+    const esp_partition_t *ota_part  = esp_ota_get_next_update_partition(NULL);
+    mbedtls_sha256_context sha;
+    mbedtls_sha256_init(&sha);
+    uint32_t expected_size = 0;
+    uint8_t  expected_digest[32] = {0};
+    uint32_t received = 0;
+    bool     ota_started = false;
+    static uint8_t payload[CHUNK_MAX];
+
+    for (;;) {
+        /* Read frame header: TYPE(1) + SEQ(2) + LEN(2) = 5 bytes */
+        uint8_t hdr[5];
+        if (!uart_read_exact(hdr, 5, 5000)) continue;
+
+        uint8_t  type = hdr[0];
+        uint16_t seq  = (uint16_t)((hdr[1] << 8u) | hdr[2]);
+        uint16_t plen = (uint16_t)((hdr[3] << 8u) | hdr[4]);
+
+        if (plen > CHUNK_MAX + 36) {
+            ESP_LOGW(TAG, "oversized frame plen=%u", plen);
+            continue;
+        }
+
+        /* Read payload + CRC */
+        if (plen > 0 && !uart_read_exact(payload, plen, 2000)) continue;
+        uint8_t crc_rx;
+        if (!uart_read_exact(&crc_rx, 1, 500)) continue;
+
+        /* Verify CRC over hdr+payload */
+        uint8_t crc_buf[5 + CHUNK_MAX + 36];
+        memcpy(crc_buf, hdr, 5);
+        if (plen > 0) memcpy(crc_buf + 5, payload, plen);
+        uint8_t expected_crc = crc8(crc_buf, (uint16_t)(5 + plen));
+        if (crc_rx != expected_crc) {
+            ESP_LOGW(TAG, "CRC fail seq=%u", seq);
+            send_nack(seq, 0x01u);  /* OTA_ERR_BAD_CRC */
+            continue;
+        }
+
+        switch (type) {
+        case OTA_BEGIN:
+            if (plen < 36) { send_nack(seq, 0x03u); break; }
+            expected_size = ((uint32_t)payload[0] << 24u) |
+                            ((uint32_t)payload[1] << 16u) |
+                            ((uint32_t)payload[2] <<  8u) |
+                             (uint32_t)payload[3];
+            memcpy(expected_digest, &payload[4], 32);
+
+            if (!ota_part || esp_ota_begin(ota_part, OTA_WITH_SEQUENTIAL_WRITES,
+                                            &handle) != ESP_OK) {
+                send_nack(seq, 0x02u);
+                break;
+            }
+            mbedtls_sha256_starts(&sha, 0);
+            received   = 0;
+            ota_started = true;
+            g_io_ota_state    = IO_OTA_RECEIVING;
+            g_io_ota_progress = 0;
+            ESP_LOGI(TAG, "OTA begin: %lu bytes", (unsigned long)expected_size);
+            send_ack(seq);
+            break;
+
+        case OTA_DATA:
+            if (!ota_started) { send_nack(seq, 0x02u); break; }
+            if (esp_ota_write(handle, payload, plen) != ESP_OK) {
+                send_nack(seq, 0x02u);
+                esp_ota_abort(handle);
+                ota_started = false;
+                g_io_ota_state = IO_OTA_FAILED;
+                break;
+            }
+            mbedtls_sha256_update(&sha, payload, plen);
+            received += plen;
+            if (expected_size > 0)
+                g_io_ota_progress = (uint8_t)((received * 100u) / expected_size);
+            send_ack(seq);
+            break;
+
+        case OTA_END: {
+            if (!ota_started) { send_nack(seq, 0x02u); break; }
+            g_io_ota_state = IO_OTA_VERIFYING;
+
+            uint8_t digest[32];
+            mbedtls_sha256_finish(&sha, digest);
+            if (memcmp(digest, expected_digest, 32) != 0) {
+                ESP_LOGE(TAG, "SHA256 mismatch");
+                esp_ota_abort(handle);
+                send_nack(seq, 0x01u);
+                g_io_ota_state = IO_OTA_FAILED;
+                break;
+            }
+
+            if (esp_ota_end(handle) != ESP_OK ||
+                esp_ota_set_boot_partition(ota_part) != ESP_OK) {
+                send_nack(seq, 0x02u);
+                g_io_ota_state = IO_OTA_FAILED;
+                break;
+            }
+
+            g_io_ota_state    = IO_OTA_REBOOTING;
+            g_io_ota_progress = 100;
+            ESP_LOGI(TAG, "OTA done — rebooting");
+            send_ack(seq);
+            vTaskDelay(pdMS_TO_TICKS(500));
+            esp_restart();
+            break;
+        }
+
+        case OTA_ABORT:
+            if (ota_started) { esp_ota_abort(handle); ota_started = false; }
+            g_io_ota_state = IO_OTA_IDLE;
+            ESP_LOGW(TAG, "OTA aborted");
+            break;
+
+        default:
+            break;
+        }
+    }
+}
+
+void uart_ota_recv_init(void)
+{
+    /* UART0 already initialized for inter-board comms; just create the task */
+    xTaskCreate(ota_recv_task, "io_ota_recv", 8192, NULL, 6, NULL);
+    ESP_LOGI(TAG, "OTA receiver task started");
+}

esp32s3/io/main/uart_ota_recv.h

@@ -0,0 +1,20 @@
+#pragma once
+/* uart_ota_recv.h — IO board: receives OTA firmware from Balance (bd-21hv) */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef enum {
+    IO_OTA_IDLE       = 0,
+    IO_OTA_RECEIVING,
+    IO_OTA_VERIFYING,
+    IO_OTA_APPLYING,
+    IO_OTA_REBOOTING,
+    IO_OTA_FAILED,
+} io_ota_state_t;
+
+extern volatile io_ota_state_t g_io_ota_state;
+extern volatile uint8_t        g_io_ota_progress;
+
+/* Start listening for OTA frames on UART0 */
+void uart_ota_recv_init(void);

esp32s3/io/partitions.csv

@@ -0,0 +1,7 @@
+# ESP32-S3 IO — 4 MB flash, dual OTA partitions
+# Name,     Type,  SubType,  Offset,    Size
+nvs,        data,  nvs,      0x9000,    0x5000,
+otadata,    data,  ota,      0xe000,    0x2000,
+app0,       app,   ota_0,    0x10000,   0x1B0000,
+app1,       app,   ota_1,    0x1C0000,  0x1B0000,
+nvs_user,   data,  nvs,      0x370000,  0x50000,

esp32s3/io/sdkconfig.defaults

@@ -0,0 +1,13 @@
+CONFIG_IDF_TARGET="esp32s3"
+CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
+CONFIG_FREERTOS_HZ=1000
+CONFIG_ESP_TASK_WDT_EN=y
+CONFIG_ESP_TASK_WDT_TIMEOUT_S=5
+CONFIG_UART_ISR_IN_IRAM=y
+CONFIG_ESP_CONSOLE_UART_DEFAULT=y
+CONFIG_LOG_DEFAULT_LEVEL_INFO=y
+
+# OTA — bd-3gwo: dual OTA partitions + rollback
+CONFIG_PARTITION_TABLE_CUSTOM=y
+CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
+CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE=y

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

@@ -39,11 +39,7 @@ Modes
     ─ UWB driver (2-anchor DW3000, publishes /uwb/target)
     ─ YOLOv8n person detection (TensorRT)
     ─ Person follower with UWB+camera fusion
-<<<<<<< HEAD
-    ─ cmd_vel bridge → ESP32 BALANCE (deadman + ramp + AUTONOMOUS gate)
-=======
     ─ cmd_vel bridge → ESP32-S3 (deadman + ramp + AUTONOMOUS gate)
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
     ─ rosbridge WebSocket (port 9090)
 
   outdoor
@@ -61,13 +57,8 @@ Modes
 Launch sequence (wall-clock delays — conservative for cold start)
 ─────────────────────────────────────────────────────────────────
    t= 0s  robot_description   (URDF + TF tree)
-<<<<<<< HEAD
-   t= 0s  ESP32 bridge        (serial port owner — must be first)
-   t= 2s  cmd_vel bridge      (consumes /cmd_vel, needs ESP32 bridge up)
-=======
    t= 0s  ESP32-S3 bridge        (serial port owner — must be first)
    t= 2s  cmd_vel bridge      (consumes /cmd_vel, needs ESP32-S3 bridge up)
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
    t= 2s  sensors             (RPLIDAR + RealSense)
    t= 4s  UWB driver          (independent serial device)
    t= 4s  CSI cameras         (optional, independent)
@@ -80,17 +71,10 @@ Launch sequence (wall-clock delays — conservative for cold start)
 
 Safety wiring
 ─────────────
-<<<<<<< HEAD
-  • ESP32 bridge must be up before cmd_vel bridge sends any command.
-  • cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
-  • ESP32 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
-    until ESP32 BALANCE firmware is in AUTONOMOUS mode regardless of /cmd_vel.
-=======
   • ESP32-S3 bridge must be up before cmd_vel bridge sends any command.
   • cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
   • ESP32-S3 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
     until ESP32-S3 firmware is in AUTONOMOUS mode regardless of /cmd_vel.
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
   • follow_enabled:=false disables person follower without stopping the node.
   • To e-stop at runtime: ros2 topic pub /saltybot/estop std_msgs/Bool '{data: true}'
 
@@ -107,11 +91,7 @@ Topics published by this stack
   /person/target                   PoseStamped    (camera position, base_link)
   /person/detections               Detection2DArray
   /cmd_vel                         Twist          (from follower or Nav2)
-<<<<<<< HEAD
-  /saltybot/cmd                    String         (to ESP32 BALANCE)
-=======
   /saltybot/cmd                    String         (to ESP32-S3)
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
   /saltybot/imu                    Imu
   /saltybot/balance_state          String
 """
@@ -229,11 +209,7 @@ def generate_launch_description():
     enable_bridge_arg = DeclareLaunchArgument(
         "enable_bridge",
         default_value="true",
-<<<<<<< HEAD
-        description="Launch ESP32 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
-=======
         description="Launch ESP32-S3 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
     )
 
     enable_rosbridge_arg = DeclareLaunchArgument(
@@ -242,7 +218,7 @@ def generate_launch_description():
         description="Launch rosbridge WebSocket server (port 9090)",
     )
 
-enable_mission_logging_arg = DeclareLaunchArgument(
+    enable_mission_logging_arg = DeclareLaunchArgument(
         "enable_mission_logging",
         default_value="true",
         description="Launch ROS2 bag recorder for mission logging (Issue #488)",
@@ -294,11 +270,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
     esp32_port_arg = DeclareLaunchArgument(
         "esp32_port",
         default_value="/dev/esp32-bridge",
-<<<<<<< HEAD
-        description="ESP32 USB CDC serial port",
-=======
         description="ESP32-S3 USB CDC serial port",
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
     )
 
     # ── Shared substitution handles ───────────────────────────────────────────
@@ -318,11 +290,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
         launch_arguments={"use_sim_time": use_sim_time}.items(),
     )
 
-<<<<<<< HEAD
-    # ── t=0s  ESP32 bidirectional serial bridge ────────────────────────────────
-=======
-    # ── t=0s  ESP32-S3 bidirectional serial bridge ────────────────────────────────
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
+    # ── t=0s  ESP32-S3 bidirectional serial bridge ───────────────────────────────
     esp32_bridge = GroupAction(
         condition=IfCondition(LaunchConfiguration("enable_bridge")),
         actions=[
@@ -352,11 +320,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
         ],
     )
 
-<<<<<<< HEAD
-    # ── t=2s  cmd_vel safety bridge (depends on ESP32 bridge) ────────────────
-=======
     # ── t=2s  cmd_vel safety bridge (depends on ESP32-S3 bridge) ────────────────
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
     cmd_vel_bridge = TimerAction(
         period=2.0,
         actions=[

jetson/ros2_ws/src/saltybot_bringup/systemd/can-bringup.service

@@ -1,5 +1,5 @@
 [Unit]
-Description=CANable 2.0 CAN bus bringup (can0, 500 kbps)
+Description=CANable 2.0 CAN bus bringup (can0, 1 Mbps DroneCAN — Here4 GPS)
 Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/643
 # Wait until the gs_usb net device appears; udev fires After=sys-subsystem-net-devices-can0.device
 After=network.target sys-subsystem-net-devices-can0.device
@@ -10,7 +10,7 @@ BindsTo=sys-subsystem-net-devices-can0.device
 Type=oneshot
 RemainAfterExit=yes
 
-ExecStart=/usr/sbin/ip link set can0 up type can bitrate 500000
+ExecStart=/usr/sbin/ip link set can0 up type can bitrate 1000000
 ExecStop=/usr/sbin/ip link set can0 down
 
 StandardOutput=journal

jetson/ros2_ws/src/saltybot_bringup/udev/70-canable.rules

@@ -2,6 +2,10 @@
 # Exposes the adapter as can0 via the SocketCAN subsystem.
 # Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/643
 #
+# Bitrate is set by can-bringup.service (1 Mbps DroneCAN for Here4 GPS, bd-p47c).
+# This rule only assigns the interface name and tags it for systemd; it does NOT
+# bring up the interface — that is handled by can-bringup.service.
+#
 # Install:
 #   sudo cp 70-canable.rules /etc/udev/rules.d/
 #   sudo udevadm control --reload && sudo udevadm trigger
@@ -16,4 +20,4 @@
 SUBSYSTEM=="net", ACTION=="add", \
     ATTRS{idVendor}=="1d50", ATTRS{idProduct}=="606f", \
     NAME="can0", \
-    RUN+="/sbin/ip link set can0 up type can bitrate 500000"
+    TAG+="systemd"

jetson/ros2_ws/src/saltybot_bringup/udev/80-esp32.rules

@@ -0,0 +1,31 @@
+# ESP32-S3 USB serial devices (bd-wim1)
+#
+# ESP32-S3 BALANCE (Waveshare LCD 1.28) — USB CDC via CH343 USB-UART chip
+#   Appears as /dev/ttyACM0, symlinked to /dev/esp32-balance
+#   idVendor  = 1a86  (QinHeng Electronics / WCH)
+#   idProduct = 55d4  (CH343 USB-UART)
+#
+# ESP32-S3 IO (bare board JTAG USB) — native USB CDC
+#   Appears as /dev/ttyACM1, symlinked to /dev/esp32-io
+#   idVendor  = 303a  (Espressif)
+#   idProduct = 1001  (ESP32-S3 USB CDC)
+#
+# Install:
+#   sudo cp 80-esp32.rules /etc/udev/rules.d/
+#   sudo udevadm control --reload && sudo udevadm trigger
+#
+# Verify:
+#   ls -la /dev/esp32-*
+#   python3 -c "import serial; s=serial.Serial('/dev/esp32-balance', 460800); s.close(); print('OK')"
+
+# ESP32-S3 BALANCE — CH343 USB-UART (bd-wim1 UART serial bridge)
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", \
+    SYMLINK+="esp32-balance", \
+    TAG+="systemd", \
+    MODE="0660", GROUP="dialout"
+
+# ESP32-S3 IO — Espressif native USB CDC
+SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001", \
+    SYMLINK+="esp32-io", \
+    TAG+="systemd", \
+    MODE="0660", GROUP="dialout"

jetson/ros2_ws/src/saltybot_dronecan_gps/config/here4_params.yaml

@@ -0,0 +1,22 @@
+here4_can_node:
+  ros__parameters:
+    # SocketCAN interface — freed from ESP32 BALANCE comms by bd-wim1
+    can_interface: can0
+
+    # DroneCAN bitrate — Here4 runs at 1 Mbps (not 500 kbps used previously)
+    bitrate: 1000000
+
+    # Local DroneCAN node ID for this Orin instance (1-127, must be unique on bus)
+    local_node_id: 126
+
+    # Set true to run: ip link set can0 type can bitrate 1000000 && ip link set can0 up
+    # Requires: sudo setcap cap_net_admin+eip $(which python3)  or  run as root
+    # Default false — manage interface externally (systemd-networkd or udev)
+    bring_up_can: false
+
+    # Here4 DroneCAN node ID — 0 means auto-detect from first Fix2 message
+    node_id_filter: 0
+
+    # TF frame IDs
+    fix_frame_id: gps
+    imu_frame_id: here4_imu

jetson/ros2_ws/src/saltybot_dronecan_gps/launch/here4.launch.py

@@ -0,0 +1,109 @@
+"""here4.launch.py — Launch the Here4 GPS DroneCAN bridge on CANable2.
+
+bd-p47c: CANable2 freed by bd-wim1 (ESP32 moved to UART/USB) now used for
+Here4 GPS at 1 Mbps DroneCAN/UAVCAN v0.
+
+CAN setup (one-time, survives reboot via systemd-networkd or udev)
+------------------------------------------------------------------
+  # Option A — manual (quick test):
+  sudo ip link set can0 down
+  sudo ip link set can0 type can bitrate 1000000
+  sudo ip link set can0 up
+
+  # Option B — let the node do it (requires CAP_NET_ADMIN):
+  ros2 launch saltybot_dronecan_gps here4.launch.py bring_up_can:=true
+
+  # Option C — systemd-networkd (/etc/systemd/network/80-can0.network):
+  [Match]
+  Name=can0
+  [CAN]
+  BitRate=1M
+
+Published topics
+----------------
+  /gps/fix             sensor_msgs/NavSatFix     → navsat_transform_node (EKF)
+  /gps/velocity        geometry_msgs/TwistWithCovarianceStamped
+  /here4/fix           sensor_msgs/NavSatFix     (alias)
+  /here4/imu           sensor_msgs/Imu
+  /here4/mag           sensor_msgs/MagneticField
+  /here4/baro          sensor_msgs/FluidPressure
+  /here4/status        std_msgs/String JSON
+  /here4/node_id       std_msgs/Int32
+
+Subscribed topics
+-----------------
+  /rtcm                std_msgs/ByteMultiArray   RTCM corrections (NTRIP client)
+  /rtcm_hex            std_msgs/String           hex-encoded RTCM (fallback)
+
+Usage
+-----
+  # Default (interface already up):
+  ros2 launch saltybot_dronecan_gps here4.launch.py
+
+  # Bring up interface automatically (CAP_NET_ADMIN required):
+  ros2 launch saltybot_dronecan_gps here4.launch.py bring_up_can:=true
+
+  # Override interface (e.g. second CAN adapter):
+  ros2 launch saltybot_dronecan_gps here4.launch.py can_interface:=can1
+
+  # Pin to specific Here4 node ID (skip auto-detect):
+  ros2 launch saltybot_dronecan_gps here4.launch.py node_id_filter:=10
+
+System dependency
+-----------------
+  pip install dronecan          # DroneCAN/UAVCAN v0 Python library
+  apt install can-utils         # optional: candump, cansend for debugging
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg_share   = get_package_share_directory("saltybot_dronecan_gps")
+    params_file = os.path.join(pkg_share, "config", "here4_params.yaml")
+
+    args = [
+        DeclareLaunchArgument(
+            "can_interface",
+            default_value="can0",
+            description="SocketCAN interface (CANable2 @ 1 Mbps)",
+        ),
+        DeclareLaunchArgument(
+            "bring_up_can",
+            default_value="false",
+            description="Bring up can_interface via ip link (requires CAP_NET_ADMIN)",
+        ),
+        DeclareLaunchArgument(
+            "node_id_filter",
+            default_value="0",
+            description="DroneCAN node ID of Here4 (0=auto-detect from first Fix2)",
+        ),
+        DeclareLaunchArgument(
+            "local_node_id",
+            default_value="126",
+            description="DroneCAN node ID for this Orin (must be unique on bus)",
+        ),
+    ]
+
+    node = Node(
+        package="saltybot_dronecan_gps",
+        executable="here4_node",
+        name="here4_can_node",
+        output="screen",
+        parameters=[
+            params_file,
+            {
+                "can_interface":  LaunchConfiguration("can_interface"),
+                "bring_up_can":   LaunchConfiguration("bring_up_can"),
+                "node_id_filter": LaunchConfiguration("node_id_filter"),
+                "local_node_id":  LaunchConfiguration("local_node_id"),
+            },
+        ],
+    )
+
+    return LaunchDescription([*args, node])

jetson/ros2_ws/src/saltybot_dronecan_gps/package.xml

@@ -0,0 +1,37 @@
+<?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_dronecan_gps</name>
+  <version>0.1.0</version>
+  <description>
+    DroneCAN/UAVCAN v0 bridge for Here4 GPS on the CANable2 SocketCAN adapter.
+    Publishes GPS fix, IMU, magnetometer, and barometer data to ROS2.
+    Injects RTCM corrections for RTK.
+
+    bd-p47c: CANable2 freed from ESP32 BALANCE comms by bd-wim1.
+    Here4 operates at 1 Mbps on can0 (was 500 kbps for VESC comms).
+
+    System dependency: dronecan Python library (pip install dronecan)
+  </description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <!-- dronecan: pip install dronecan  (DroneCAN/UAVCAN v0 Python library) -->
+  <!-- python3-can is a transitive dependency of dronecan -->
+
+  <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_dronecan_gps/saltybot_dronecan_gps/__init__.py

@@ -0,0 +1 @@
+# saltybot_dronecan_gps — Here4 GPS DroneCAN bridge for CANable2 (bd-p47c)

jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/dronecan_parser.py

@@ -0,0 +1,333 @@
+"""dronecan_parser.py — Pure-Python conversion of DroneCAN/UAVCAN v0 message
+objects to plain Python dicts, independent of ROS2.
+
+Keeps the dronecan library dependency isolated so that unit tests can run
+without the dronecan package or CAN hardware.
+
+DroneCAN message types handled
+-------------------------------
+  uavcan.equipment.gnss.Fix2               DTID 1063
+  uavcan.equipment.gnss.Auxiliary          DTID 1060
+  uavcan.equipment.ahrs.RawIMU             DTID 1003
+  uavcan.equipment.ahrs.MagneticFieldStrength2  DTID 1001
+  uavcan.equipment.air_data.StaticPressure DTID 1028
+  uavcan.equipment.air_data.StaticTemperature DTID 1029
+  uavcan.protocol.NodeStatus               DTID 341
+  uavcan.equipment.gnss.RTCMStream         DTID 1062  (TX: RTCM injection)
+
+Fix2 status values
+------------------
+  0  NO_FIX
+  1  TIME_ONLY
+  2  2D_FIX
+  3  3D_FIX
+
+NodeStatus health / mode
+-------------------------
+  health  0=OK 1=WARNING 2=ERROR 3=CRITICAL
+  mode    0=OPERATIONAL 1=INITIALIZATION 2=MAINTENANCE 3=SOFTWARE_UPDATE 7=OFFLINE
+"""
+
+from __future__ import annotations
+
+from typing import Any, Dict, List, Optional
+
+# ── DSDL constants ────────────────────────────────────────────────────────────
+
+DTID_FIX2              = 1063
+DTID_AUXILIARY         = 1060
+DTID_RAW_IMU           = 1003
+DTID_MAG_FIELD         = 1001
+DTID_STATIC_PRESSURE   = 1028
+DTID_STATIC_TEMPERATURE = 1029
+DTID_NODE_STATUS       = 341
+DTID_RTCM_STREAM       = 1062
+
+# Fix2 status
+FIX_NO_FIX    = 0
+FIX_TIME_ONLY = 1
+FIX_2D        = 2
+FIX_3D        = 3
+
+FIX_LABEL = {FIX_NO_FIX: "NO_FIX", FIX_TIME_ONLY: "TIME_ONLY",
+             FIX_2D: "2D_FIX", FIX_3D: "3D_FIX"}
+
+# NodeStatus health
+HEALTH_OK       = 0
+HEALTH_WARNING  = 1
+HEALTH_ERROR    = 2
+HEALTH_CRITICAL = 3
+
+HEALTH_LABEL = {0: "OK", 1: "WARNING", 2: "ERROR", 3: "CRITICAL"}
+MODE_LABEL   = {0: "OPERATIONAL", 1: "INITIALIZATION", 2: "MAINTENANCE",
+                3: "SOFTWARE_UPDATE", 7: "OFFLINE"}
+
+# RTCM stream: max bytes per DroneCAN frame
+RTCM_MAX_CHUNK = 128
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _getf(obj: Any, *attrs: str, default: Any = None) -> Any:
+    """Get nested attribute with fallback, tolerant of dronecan proxy objects."""
+    for attr in attrs:
+        try:
+            obj = getattr(obj, attr)
+        except AttributeError:
+            return default
+    return obj
+
+
+def _list3(vec: Any, default: float = 0.0) -> List[float]:
+    """Convert a dronecan 3-element array to a plain list of floats."""
+    try:
+        return [float(vec[0]), float(vec[1]), float(vec[2])]
+    except (TypeError, IndexError):
+        return [default, default, default]
+
+
+def _list9(mat: Any, default: float = 0.0) -> List[float]:
+    """Convert a dronecan 9-element array (row-major 3×3) to a plain list."""
+    try:
+        return [float(mat[i]) for i in range(9)]
+    except (TypeError, IndexError):
+        return [default] * 9
+
+
+# ── Fix2 ─────────────────────────────────────────────────────────────────────
+
+def parse_fix2(msg: Any) -> Optional[Dict[str, Any]]:
+    """Parse a uavcan.equipment.gnss.Fix2 message.
+
+    Parameters
+    ----------
+    msg : dronecan message object (event.message / event.transfer.payload)
+
+    Returns
+    -------
+    dict with keys:
+        lat_deg        float   degrees (WGS84)
+        lon_deg        float   degrees (WGS84)
+        alt_msl_m      float   metres above MSL
+        alt_ellipsoid_m float  metres above WGS84 ellipsoid
+        ned_vel        list[3] [north, east, down] m/s
+        fix_type       int     0..3
+        fix_label      str
+        sats_used      int
+        pdop           float
+        hdop           float
+        vdop           float
+        pos_cov        list[9] row-major 3×3 position covariance, m²
+        undulation_m   float   WGS84 geoid undulation (m)
+        gnss_timestamp_us int  microseconds (GPS epoch)
+    """
+    if msg is None:
+        return None
+    try:
+        lat = float(_getf(msg, "latitude_deg_1e8",  default=0)) * 1e-8
+        lon = float(_getf(msg, "longitude_deg_1e8", default=0)) * 1e-8
+        alt_msl = float(_getf(msg, "height_msl_mm",       default=0)) * 1e-3
+        alt_ell = float(_getf(msg, "height_ellipsoid_mm",  default=0)) * 1e-3
+
+        ned = _list3(_getf(msg, "ned_velocity"))
+
+        fix_type = int(_getf(msg, "status", default=FIX_NO_FIX))
+
+        # sats_used is uint7, may be named differently across DSDL revisions
+        sats = int(_getf(msg, "sats_used", default=0))
+
+        pdop = float(_getf(msg, "pdop", default=99.0))
+        hdop = float(_getf(msg, "hdop", default=99.0))
+        vdop = float(_getf(msg, "vdop", default=99.0))
+        undulation = float(_getf(msg, "undulation", default=0.0))
+
+        # position_covariance: optional, may be absent in older DSDL
+        pos_cov_raw = _getf(msg, "position_covariance")
+        pos_cov = _list9(pos_cov_raw) if pos_cov_raw is not None else [0.0] * 9
+
+        gnss_ts = int(_getf(msg, "gnss_timestamp", default=0))
+
+        return {
+            "lat_deg":          lat,
+            "lon_deg":          lon,
+            "alt_msl_m":        alt_msl,
+            "alt_ellipsoid_m":  alt_ell,
+            "ned_vel":          ned,
+            "fix_type":         fix_type,
+            "fix_label":        FIX_LABEL.get(fix_type, f"UNKNOWN({fix_type})"),
+            "sats_used":        sats,
+            "pdop":             pdop,
+            "hdop":             hdop,
+            "vdop":             vdop,
+            "pos_cov":          pos_cov,
+            "undulation_m":     undulation,
+            "gnss_timestamp_us": gnss_ts,
+        }
+    except Exception:
+        return None
+
+
+# ── RawIMU ────────────────────────────────────────────────────────────────────
+
+def parse_raw_imu(msg: Any) -> Optional[Dict[str, Any]]:
+    """Parse a uavcan.equipment.ahrs.RawIMU message.
+
+    Returns
+    -------
+    dict with keys:
+        gyro_xyz     list[3]  rad/s
+        accel_xyz    list[3]  m/s²
+        dt_s         float    integration interval (seconds)
+        gyro_cov     list[9]  row-major 3×3 covariance
+        accel_cov    list[9]
+    """
+    if msg is None:
+        return None
+    try:
+        gyro  = _list3(_getf(msg, "rate_gyro_latest"))
+        accel = _list3(_getf(msg, "accelerometer_latest"))
+        dt    = float(_getf(msg, "integration_interval", default=0.0))
+
+        gyro_cov  = _list9(_getf(msg, "rate_gyro_covariance"))
+        accel_cov = _list9(_getf(msg, "accelerometer_covariance"))
+
+        return {
+            "gyro_xyz":  gyro,
+            "accel_xyz": accel,
+            "dt_s":      dt,
+            "gyro_cov":  gyro_cov,
+            "accel_cov": accel_cov,
+        }
+    except Exception:
+        return None
+
+
+# ── MagneticFieldStrength2 ────────────────────────────────────────────────────
+
+def parse_mag(msg: Any) -> Optional[Dict[str, Any]]:
+    """Parse a uavcan.equipment.ahrs.MagneticFieldStrength2 message.
+
+    Returns
+    -------
+    dict with keys:
+        sensor_id     int
+        field_ga      list[3]  [x, y, z] Gauss
+        field_cov     list[9]  row-major 3×3 covariance (Gauss²)
+    """
+    if msg is None:
+        return None
+    try:
+        sensor_id = int(_getf(msg, "sensor_id", default=0))
+        field_ga  = _list3(_getf(msg, "magnetic_field_ga"))
+        field_cov = _list9(_getf(msg, "magnetic_field_covariance"))
+        return {
+            "sensor_id": sensor_id,
+            "field_ga":  field_ga,
+            "field_cov": field_cov,
+        }
+    except Exception:
+        return None
+
+
+# ── StaticPressure / StaticTemperature ───────────────────────────────────────
+
+def parse_static_pressure(msg: Any) -> Optional[Dict[str, Any]]:
+    """Parse a uavcan.equipment.air_data.StaticPressure message.
+
+    Returns
+    -------
+    dict with keys:
+        pressure_pa       float  Pascals
+        pressure_variance float  Pa²
+    """
+    if msg is None:
+        return None
+    try:
+        return {
+            "pressure_pa":       float(_getf(msg, "static_pressure",          default=0.0)),
+            "pressure_variance": float(_getf(msg, "static_pressure_variance", default=0.0)),
+        }
+    except Exception:
+        return None
+
+
+def parse_static_temperature(msg: Any) -> Optional[Dict[str, Any]]:
+    """Parse a uavcan.equipment.air_data.StaticTemperature message.
+
+    Returns
+    -------
+    dict with keys:
+        temperature_k float  Kelvin
+    """
+    if msg is None:
+        return None
+    try:
+        return {
+            "temperature_k": float(_getf(msg, "static_temperature", default=273.15)),
+        }
+    except Exception:
+        return None
+
+
+# ── NodeStatus ────────────────────────────────────────────────────────────────
+
+def parse_node_status(msg: Any, source_node_id: int) -> Optional[Dict[str, Any]]:
+    """Parse a uavcan.protocol.NodeStatus message.
+
+    Returns
+    -------
+    dict with keys:
+        node_id        int
+        uptime_sec     int
+        health         int  0=OK .. 3=CRITICAL
+        health_label   str
+        mode           int
+        mode_label     str
+        vendor_code    int
+    """
+    if msg is None:
+        return None
+    try:
+        health      = int(_getf(msg, "health",                        default=0))
+        mode        = int(_getf(msg, "mode",                          default=0))
+        uptime      = int(_getf(msg, "uptime_sec",                    default=0))
+        vendor_code = int(_getf(msg, "vendor_specific_status_code",   default=0))
+        return {
+            "node_id":      source_node_id,
+            "uptime_sec":   uptime,
+            "health":       health,
+            "health_label": HEALTH_LABEL.get(health, f"UNKNOWN({health})"),
+            "mode":         mode,
+            "mode_label":   MODE_LABEL.get(mode, f"UNKNOWN({mode})"),
+            "vendor_code":  vendor_code,
+        }
+    except Exception:
+        return None
+
+
+# ── RTCM stream chunking ──────────────────────────────────────────────────────
+
+def chunk_rtcm(data: bytes, sequence_id: int) -> List[Dict[str, Any]]:
+    """Split a raw RTCM byte string into DroneCAN RTCMStream message dicts.
+
+    Each chunk carries up to RTCM_MAX_CHUNK bytes.  The caller must convert
+    these dicts into actual dronecan.uavcan.equipment.gnss.RTCMStream objects
+    and broadcast them on the bus.
+
+    Parameters
+    ----------
+    data        : raw RTCM correction bytes
+    sequence_id : monotonically increasing counter (uint16, wraps at 65535)
+
+    Returns
+    -------
+    List of dicts:  {"sequence_id": int, "data": bytes}
+    """
+    chunks = []
+    for i in range(0, len(data), RTCM_MAX_CHUNK):
+        chunks.append({
+            "sequence_id": sequence_id & 0xFFFF,
+            "data": data[i:i + RTCM_MAX_CHUNK],
+        })
+        sequence_id += 1
+    return chunks

jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/here4_node.py

@@ -0,0 +1,532 @@
+"""here4_node.py — ROS2 node for Here4 GPS via DroneCAN/UAVCAN v0 on CANable2.
+
+bd-p47c: CANable2 (freed from ESP32 BALANCE comms by bd-wim1) is now used
+for Here4 GPS at 1 Mbps DroneCAN.
+
+CAN setup (run once before launching, or use bring_up_can:=true arg)
+-----------------------------------------------------------------------
+  sudo ip link set can0 down
+  sudo ip link set can0 type can bitrate 1000000
+  sudo ip link set can0 up
+
+DroneCAN messages received from Here4
+--------------------------------------
+  uavcan.equipment.gnss.Fix2                 DTID 1063   ~5 Hz
+  uavcan.equipment.gnss.Auxiliary            DTID 1060   ~5 Hz
+  uavcan.equipment.ahrs.RawIMU               DTID 1003  ~50 Hz
+  uavcan.equipment.ahrs.MagneticFieldStrength2 DTID 1001 ~10 Hz
+  uavcan.equipment.air_data.StaticPressure   DTID 1028  ~10 Hz
+  uavcan.equipment.air_data.StaticTemperature DTID 1029  ~10 Hz
+  uavcan.protocol.NodeStatus                 DTID 341    ~1 Hz
+
+DroneCAN messages transmitted to Here4
+----------------------------------------
+  uavcan.equipment.gnss.RTCMStream           DTID 1062   on /rtcm input
+
+ROS2 topics published
+----------------------
+  /gps/fix              sensor_msgs/NavSatFix     5 Hz — feeds navsat_transform_node
+  /gps/velocity         geometry_msgs/TwistWithCovarianceStamped  5 Hz
+  /here4/fix            sensor_msgs/NavSatFix     alias of /gps/fix
+  /here4/imu            sensor_msgs/Imu          50 Hz
+  /here4/mag            sensor_msgs/MagneticField 10 Hz
+  /here4/baro           sensor_msgs/FluidPressure 10 Hz
+  /here4/status         std_msgs/String JSON       1 Hz (node ID, health, fix quality)
+  /here4/node_id        std_msgs/Int32             node ID once discovered
+
+ROS2 topics subscribed
+-----------------------
+  /rtcm                 std_msgs/ByteMultiArray   — RTCM corrections (from NTRIP client)
+                        or std_msgs/String (hex-encoded, fallback)
+
+Parameters
+----------
+  can_interface        str    SocketCAN interface name        default: can0
+  local_node_id        int    This node's DroneCAN ID (1-127) default: 126
+  bitrate              int    CAN bitrate, bps               default: 1000000
+  bring_up_can         bool   Bring up can_interface via ip   default: false
+  node_id_filter       int    Specific Here4 node ID (0=auto) default: 0
+  fix_frame_id         str    Frame ID for NavSatFix          default: gps
+  imu_frame_id         str    Frame ID for Imu                default: here4_imu
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import subprocess
+import threading
+import time
+from typing import Any, Optional
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TwistWithCovarianceStamped
+from sensor_msgs.msg import FluidPressure, Imu, MagneticField, NavSatFix, NavSatStatus
+from std_msgs.msg import ByteMultiArray, Int32, String
+
+from .dronecan_parser import (
+    FIX_3D,
+    FIX_NO_FIX,
+    chunk_rtcm,
+    parse_fix2,
+    parse_mag,
+    parse_raw_imu,
+    parse_static_pressure,
+    parse_static_temperature,
+    parse_node_status,
+)
+
+# Gauss → Tesla conversion
+_GA_TO_T = 1e-4
+
+# NavSatStatus mapping from DroneCAN fix type
+_FIX_TO_NAV_STATUS = {
+    0: NavSatStatus.STATUS_NO_FIX,   # NO_FIX
+    1: NavSatStatus.STATUS_NO_FIX,   # TIME_ONLY
+    2: NavSatStatus.STATUS_FIX,      # 2D_FIX
+    3: NavSatStatus.STATUS_FIX,      # 3D_FIX  (upgraded to SBAS/RTK when applicable)
+}
+
+# Here4 supports GPS+GLONASS+BeiDou+Galileo
+_SERVICE_FLAGS = (NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS |
+                  NavSatStatus.SERVICE_GALILEO | NavSatStatus.SERVICE_COMPASS)
+
+# RTCM injection sequence counter (per-session, wraps at 65535)
+_rtcm_seq = 0
+_rtcm_lock = threading.Lock()
+
+
+def _bring_up_can(iface: str, bitrate: int, logger) -> bool:
+    """Bring up the SocketCAN interface at the requested bitrate."""
+    try:
+        subprocess.run(["ip", "link", "set", iface, "down"],          check=False)
+        subprocess.run(["ip", "link", "set", iface, "type", "can",
+                        "bitrate", str(bitrate)],                      check=True)
+        subprocess.run(["ip", "link", "set", iface, "up"],             check=True)
+        logger.info(f"SocketCAN {iface} up at {bitrate} bps")
+        return True
+    except subprocess.CalledProcessError as exc:
+        logger.error(f"Failed to bring up {iface}: {exc} — check sudo/udev rules")
+        return False
+
+
+class Here4CanNode(Node):
+    """DroneCAN bridge publishing Here4 GPS, IMU, mag, and baro to ROS2."""
+
+    def __init__(self) -> None:
+        super().__init__("here4_can_node")
+
+        # ── Parameters ────────────────────────────────────────────────────
+        self.declare_parameter("can_interface",    "can0")
+        self.declare_parameter("local_node_id",    126)
+        self.declare_parameter("bitrate",          1_000_000)
+        self.declare_parameter("bring_up_can",     False)
+        self.declare_parameter("node_id_filter",   0)       # 0 = auto-detect
+        self.declare_parameter("fix_frame_id",     "gps")
+        self.declare_parameter("imu_frame_id",     "here4_imu")
+
+        self._iface        = self.get_parameter("can_interface").value
+        self._local_nid    = self.get_parameter("local_node_id").value
+        self._bitrate      = self.get_parameter("bitrate").value
+        self._bring_up     = self.get_parameter("bring_up_can").value
+        self._nid_filter   = self.get_parameter("node_id_filter").value
+        self._fix_frame    = self.get_parameter("fix_frame_id").value
+        self._imu_frame    = self.get_parameter("imu_frame_id").value
+
+        # ── State ─────────────────────────────────────────────────────────
+        self._here4_nid: Optional[int] = self._nid_filter if self._nid_filter else None
+        self._last_fix: Optional[dict] = None
+        self._last_temp_k: float = 293.15   # 20 °C default until first reading
+        self._dc_node = None                # dronecan node, set up after CAN is ready
+
+        # ── Publishers ────────────────────────────────────────────────────
+        self._pub_fix     = self.create_publisher(NavSatFix,
+                                "/gps/fix",       10)
+        self._pub_fix_h4  = self.create_publisher(NavSatFix,
+                                "/here4/fix",     10)
+        self._pub_vel     = self.create_publisher(TwistWithCovarianceStamped,
+                                "/gps/velocity",  10)
+        self._pub_imu     = self.create_publisher(Imu,
+                                "/here4/imu",     10)
+        self._pub_mag     = self.create_publisher(MagneticField,
+                                "/here4/mag",     10)
+        self._pub_baro    = self.create_publisher(FluidPressure,
+                                "/here4/baro",    10)
+        self._pub_status  = self.create_publisher(String,
+                                "/here4/status",  10)
+        self._pub_node_id = self.create_publisher(Int32,
+                                "/here4/node_id", 10)
+
+        # ── Subscriptions ─────────────────────────────────────────────────
+        self.create_subscription(ByteMultiArray, "/rtcm",
+                                 self._on_rtcm_bytes, 10)
+        self.create_subscription(String, "/rtcm_hex",
+                                 self._on_rtcm_hex, 10)
+
+        # ── Bring up CAN if requested ─────────────────────────────────────
+        if self._bring_up:
+            _bring_up_can(self._iface, self._bitrate, self.get_logger())
+
+        # ── Initialise DroneCAN node ──────────────────────────────────────
+        self._init_dronecan()
+
+        # ── Timers ────────────────────────────────────────────────────────
+        # Poll DroneCAN at 200 Hz (5 ms) to drain the RX queue in real-time
+        self.create_timer(0.005, self._spin_dronecan)
+        # Status publish at 1 Hz
+        self.create_timer(1.0,   self._publish_status)
+
+    # ── DroneCAN initialisation ───────────────────────────────────────────
+
+    def _init_dronecan(self) -> None:
+        try:
+            import dronecan  # type: ignore[import]
+        except ImportError:
+            self.get_logger().error(
+                "dronecan package not found — install with: pip install dronecan\n"
+                "Here4 node will not receive GPS data until dronecan is installed."
+            )
+            return
+
+        try:
+            self._dc_node = dronecan.make_node(
+                f"socketcan:{self._iface}",
+                node_id=self._local_nid,
+                bitrate=self._bitrate,
+            )
+        except Exception as exc:
+            self.get_logger().error(
+                f"Failed to open DroneCAN on {self._iface}: {exc}\n"
+                f"Verify: ip link show {self._iface}  and try bring_up_can:=true"
+            )
+            return
+
+        # ── Register message handlers ──────────────────────────────────────
+        dc = dronecan
+        self._dc_node.add_handler(dc.uavcan.equipment.gnss.Fix2,
+                                  self._on_fix2)
+        self._dc_node.add_handler(dc.uavcan.equipment.ahrs.RawIMU,
+                                  self._on_raw_imu)
+        self._dc_node.add_handler(dc.uavcan.equipment.ahrs.MagneticFieldStrength2,
+                                  self._on_mag)
+        self._dc_node.add_handler(dc.uavcan.equipment.air_data.StaticPressure,
+                                  self._on_pressure)
+        self._dc_node.add_handler(dc.uavcan.equipment.air_data.StaticTemperature,
+                                  self._on_temperature)
+        self._dc_node.add_handler(dc.uavcan.protocol.NodeStatus,
+                                  self._on_node_status)
+
+        self.get_logger().info(
+            f"DroneCAN node ready: {self._iface} @ {self._bitrate} bps "
+            f"local_id={self._local_nid}"
+        )
+
+    # ── DroneCAN spin (called from 5 ms timer) ────────────────────────────
+
+    def _spin_dronecan(self) -> None:
+        if self._dc_node is None:
+            return
+        try:
+            self._dc_node.spin(timeout=0)
+        except Exception as exc:
+            self.get_logger().warning(
+                f"DroneCAN spin error: {exc}",
+                throttle_duration_sec=5.0,
+            )
+
+    # ── Source node filter ────────────────────────────────────────────────
+
+    def _accept(self, event: Any) -> bool:  # type: ignore[valid-type]
+        """Return True if this event comes from the tracked Here4 node."""
+        src = getattr(getattr(event, "transfer", None), "source_node_id", None)
+        if src is None:
+            return True  # unknown source — accept
+        if self._here4_nid is None:
+            return True  # auto-detect mode — accept any
+        return src == self._here4_nid
+
+    # ── DroneCAN message handlers ─────────────────────────────────────────
+
+    def _on_fix2(self, event) -> None:
+        src = getattr(getattr(event, "transfer", None), "source_node_id", None)
+
+        # Auto-latch: first node that sends Fix2 becomes our Here4
+        if self._here4_nid is None and src is not None:
+            self._here4_nid = src
+            self.get_logger().info(
+                f"Here4 auto-discovered: DroneCAN node ID {src}"
+            )
+            nid_msg = Int32()
+            nid_msg.data = src
+            self._pub_node_id.publish(nid_msg)
+
+        if not self._accept(event):
+            return
+        d = parse_fix2(event.message)
+        if d is None:
+            return
+        self._last_fix = d
+        stamp = self.get_clock().now().to_msg()
+        self._publish_navsatfix(d, stamp)
+        self._publish_velocity(d, stamp)
+
+    def _on_raw_imu(self, event) -> None:
+        if not self._accept(event):
+            return
+        d = parse_raw_imu(event.message)
+        if d is None:
+            return
+        stamp = self.get_clock().now().to_msg()
+        msg = Imu()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = self._imu_frame
+
+        # Orientation: not provided by DroneCAN RawIMU — signal unknown
+        msg.orientation_covariance[0] = -1.0
+
+        gx, gy, gz = d["gyro_xyz"]
+        msg.angular_velocity.x = gx
+        msg.angular_velocity.y = gy
+        msg.angular_velocity.z = gz
+        _fill_cov9(msg.angular_velocity_covariance, d["gyro_cov"])
+
+        ax, ay, az = d["accel_xyz"]
+        msg.linear_acceleration.x = ax
+        msg.linear_acceleration.y = ay
+        msg.linear_acceleration.z = az
+        _fill_cov9(msg.linear_acceleration_covariance, d["accel_cov"])
+
+        self._pub_imu.publish(msg)
+
+    def _on_mag(self, event) -> None:
+        if not self._accept(event):
+            return
+        d = parse_mag(event.message)
+        if d is None:
+            return
+        stamp = self.get_clock().now().to_msg()
+        msg = MagneticField()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = self._imu_frame
+        gx, gy, gz = d["field_ga"]
+        msg.magnetic_field.x = gx * _GA_TO_T
+        msg.magnetic_field.y = gy * _GA_TO_T
+        msg.magnetic_field.z = gz * _GA_TO_T
+        _fill_cov9(msg.magnetic_field_covariance,
+                   [v * _GA_TO_T * _GA_TO_T for v in d["field_cov"]])
+        self._pub_mag.publish(msg)
+
+    def _on_pressure(self, event) -> None:
+        if not self._accept(event):
+            return
+        d = parse_static_pressure(event.message)
+        if d is None:
+            return
+        stamp = self.get_clock().now().to_msg()
+        msg = FluidPressure()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = self._imu_frame
+        msg.fluid_pressure  = d["pressure_pa"]
+        msg.variance        = d["pressure_variance"]
+        self._pub_baro.publish(msg)
+
+    def _on_temperature(self, event) -> None:
+        if not self._accept(event):
+            return
+        d = parse_static_temperature(event.message)
+        if d is None:
+            return
+        self._last_temp_k = d["temperature_k"]
+
+    def _on_node_status(self, event) -> None:
+        src = getattr(getattr(event, "transfer", None), "source_node_id", None)
+        if src is None:
+            return
+        d = parse_node_status(event.message, src)
+        if d is None:
+            return
+
+        # Auto-discovery: latch first node that sends GPS-relevant data
+        # NodeStatus alone is not enough — we track whichever source_node_id
+        # first delivered a Fix2 message; if none yet, log all nodes seen.
+        if self._here4_nid is None:
+            # Any node could be Here4 — we'll latch on first Fix2 source
+            self.get_logger().debug(
+                f"DroneCAN node {src} alive: health={d['health_label']} "
+                f"mode={d['mode_label']} uptime={d['uptime_sec']}s"
+            )
+        elif src == self._here4_nid:
+            self._last_node_status = d
+
+    # ── NavSatFix publishing ─────────────────────────────────────────────
+
+    def _publish_navsatfix(self, d: dict, stamp) -> None:
+        fix_type = d["fix_type"]
+
+        status = NavSatStatus()
+        status.service = _SERVICE_FLAGS
+
+        # Upgrade status for RTK: hdop < 0.5 and 3D fix → treat as SBAS/RTK
+        if fix_type == FIX_3D:
+            if d["hdop"] < 0.5:
+                status.status = NavSatStatus.STATUS_GBAS_FIX   # RTK fixed
+            elif d["hdop"] < 1.5:
+                status.status = NavSatStatus.STATUS_SBAS_FIX   # SBAS/DGPS
+            else:
+                status.status = NavSatStatus.STATUS_FIX
+        else:
+            status.status = _FIX_TO_NAV_STATUS.get(fix_type, NavSatStatus.STATUS_NO_FIX)
+
+        msg = NavSatFix()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = self._fix_frame
+        msg.status          = status
+        msg.latitude        = d["lat_deg"]
+        msg.longitude       = d["lon_deg"]
+        msg.altitude        = d["alt_msl_m"]
+
+        # Position covariance (diagonal from DroneCAN; full matrix if available)
+        pos_cov = d["pos_cov"]
+        msg.position_covariance_type = (
+            NavSatFix.COVARIANCE_TYPE_FULL if any(pos_cov) else
+            NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+        )
+        if any(pos_cov):
+            for i, v in enumerate(pos_cov):
+                msg.position_covariance[i] = v
+        else:
+            # Approximate from HDOP/VDOP: σ_h ≈ hdop × 3m, σ_v ≈ vdop × 5m
+            sh2 = (d["hdop"] * 3.0) ** 2
+            sv2 = (d["vdop"] * 5.0) ** 2
+            msg.position_covariance[0] = sh2   # east
+            msg.position_covariance[4] = sh2   # north
+            msg.position_covariance[8] = sv2   # up
+
+        self._pub_fix.publish(msg)
+        self._pub_fix_h4.publish(msg)
+
+    def _publish_velocity(self, d: dict, stamp) -> None:
+        vn, ve, vd = d["ned_vel"]
+        msg = TwistWithCovarianceStamped()
+        msg.header.stamp    = stamp
+        msg.header.frame_id = self._fix_frame
+        # NED → ENU: x=east=ve, y=north=vn, z=up=-vd
+        msg.twist.twist.linear.x = ve
+        msg.twist.twist.linear.y = vn
+        msg.twist.twist.linear.z = -vd
+        # Velocity covariance: approximate from HDOP
+        vel_var = (d["hdop"] * 0.2) ** 2
+        msg.twist.covariance[0]  = vel_var   # xx (east)
+        msg.twist.covariance[7]  = vel_var   # yy (north)
+        msg.twist.covariance[14] = (d["vdop"] * 0.3) ** 2  # zz (up)
+        self._pub_vel.publish(msg)
+
+    # ── Status publish (1 Hz) ────────────────────────────────────────────
+
+    def _publish_status(self) -> None:
+        fix = self._last_fix
+        ns = getattr(self, "_last_node_status", None)
+
+        payload: dict = {
+            "here4_node_id":  self._here4_nid,
+            "can_interface":  self._iface,
+            "dc_node_ready":  self._dc_node is not None,
+            "fix_type":       fix["fix_type"]    if fix else FIX_NO_FIX,
+            "fix_label":      fix["fix_label"]   if fix else "NO_FIX",
+            "sats_used":      fix["sats_used"]   if fix else 0,
+            "hdop":           round(fix["hdop"], 2) if fix else 99.0,
+            "lat_deg":        round(fix["lat_deg"],    7) if fix else 0.0,
+            "lon_deg":        round(fix["lon_deg"],    7) if fix else 0.0,
+            "alt_msl_m":      round(fix["alt_msl_m"],  2) if fix else 0.0,
+            "temp_c":         round(self._last_temp_k - 273.15, 1),
+            "node_health":    ns["health_label"] if ns else "UNKNOWN",
+            "node_uptime_s":  ns["uptime_sec"]   if ns else 0,
+        }
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._pub_status.publish(msg)
+
+        # Also publish discovered node ID
+        if self._here4_nid is not None:
+            nid_msg = Int32()
+            nid_msg.data = self._here4_nid
+            self._pub_node_id.publish(nid_msg)
+
+    # ── RTCM injection ────────────────────────────────────────────────────
+
+    def _on_rtcm_bytes(self, msg: ByteMultiArray) -> None:
+        """Receive RTCM bytes from /rtcm and inject via DroneCAN RTCMStream."""
+        raw = bytes(msg.data)
+        self._inject_rtcm(raw)
+
+    def _on_rtcm_hex(self, msg: String) -> None:
+        """Receive hex-encoded RTCM from /rtcm_hex (fallback topic)."""
+        try:
+            raw = bytes.fromhex(msg.data.strip())
+        except ValueError as exc:
+            self.get_logger().error(f"Bad /rtcm_hex: {exc}")
+            return
+        self._inject_rtcm(raw)
+
+    def _inject_rtcm(self, raw: bytes) -> None:
+        if self._dc_node is None or not raw:
+            return
+
+        global _rtcm_seq
+        try:
+            import dronecan  # type: ignore[import]
+        except ImportError:
+            return
+
+        with _rtcm_lock:
+            chunks = chunk_rtcm(raw, _rtcm_seq)
+            _rtcm_seq = (_rtcm_seq + len(chunks)) & 0xFFFF
+
+        for chunk in chunks:
+            try:
+                rtcm_msg = dronecan.uavcan.equipment.gnss.RTCMStream()
+                rtcm_msg.sequence_id = chunk["sequence_id"]
+                rtcm_msg.data = list(chunk["data"])
+                self._dc_node.broadcast(rtcm_msg)
+            except Exception as exc:
+                self.get_logger().warning(
+                    f"RTCM inject error (seq {chunk['sequence_id']}): {exc}",
+                    throttle_duration_sec=2.0,
+                )
+
+    # ── Shutdown ──────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        if self._dc_node is not None:
+            try:
+                self._dc_node.close()
+            except Exception:
+                pass
+        super().destroy_node()
+
+
+# ── Covariance helper ─────────────────────────────────────────────────────────
+
+def _fill_cov9(ros_cov, values) -> None:
+    """Fill a 9-element ROS2 covariance array from a list."""
+    for i, v in enumerate(values[:9]):
+        ros_cov[i] = float(v)
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = Here4CanNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_dronecan_gps/setup.cfg

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

jetson/ros2_ws/src/saltybot_dronecan_gps/setup.py

@@ -0,0 +1,27 @@
+from setuptools import setup
+
+package_name = "saltybot_dronecan_gps"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/config", ["config/here4_params.yaml"]),
+        (f"share/{package_name}/launch", ["launch/here4.launch.py"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-perception",
+    maintainer_email="sl-perception@saltylab.local",
+    description="DroneCAN/UAVCAN v0 bridge for Here4 GPS via CANable2 (bd-p47c)",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "here4_node = saltybot_dronecan_gps.here4_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_dronecan_gps/test/test_dronecan_parser.py

@@ -0,0 +1,367 @@
+#!/usr/bin/env python3
+"""Unit tests for saltybot_dronecan_gps.dronecan_parser.
+
+No dronecan library, ROS2, or CAN hardware required.
+Uses simple Python objects (namespaces) to simulate dronecan message objects.
+
+Run with:  pytest test/ -v
+"""
+
+import math
+import types
+import unittest
+
+from saltybot_dronecan_gps.dronecan_parser import (
+    DTID_FIX2,
+    DTID_RAW_IMU,
+    DTID_MAG_FIELD,
+    DTID_STATIC_PRESSURE,
+    DTID_STATIC_TEMPERATURE,
+    DTID_NODE_STATUS,
+    DTID_RTCM_STREAM,
+    FIX_NO_FIX,
+    FIX_TIME_ONLY,
+    FIX_2D,
+    FIX_3D,
+    RTCM_MAX_CHUNK,
+    chunk_rtcm,
+    parse_fix2,
+    parse_mag,
+    parse_node_status,
+    parse_raw_imu,
+    parse_static_pressure,
+    parse_static_temperature,
+    _getf,
+    _list3,
+    _list9,
+)
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _ns(**kwargs) -> types.SimpleNamespace:
+    """Build a simple namespace to mimic a dronecan message object."""
+    return types.SimpleNamespace(**kwargs)
+
+
+def _ns_vec(*values):
+    """Return an object that is indexable like a dronecan vector field."""
+    return list(values)
+
+
+# ── DTID constants ────────────────────────────────────────────────────────────
+
+class TestDtidConstants(unittest.TestCase):
+    def test_known_values(self):
+        self.assertEqual(DTID_FIX2,               1063)
+        self.assertEqual(DTID_RAW_IMU,             1003)
+        self.assertEqual(DTID_MAG_FIELD,           1001)
+        self.assertEqual(DTID_STATIC_PRESSURE,     1028)
+        self.assertEqual(DTID_STATIC_TEMPERATURE,  1029)
+        self.assertEqual(DTID_NODE_STATUS,          341)
+        self.assertEqual(DTID_RTCM_STREAM,         1062)
+
+    def test_rtcm_chunk_size(self):
+        self.assertEqual(RTCM_MAX_CHUNK, 128)
+
+
+# ── _getf / _list3 / _list9 ──────────────────────────────────────────────────
+
+class TestHelpers(unittest.TestCase):
+    def test_getf_simple(self):
+        obj = _ns(x=42)
+        self.assertEqual(_getf(obj, "x"), 42)
+
+    def test_getf_missing(self):
+        obj = _ns(x=1)
+        self.assertIsNone(_getf(obj, "y"))
+
+    def test_getf_default(self):
+        obj = _ns()
+        self.assertEqual(_getf(obj, "missing", default=99), 99)
+
+    def test_list3_ok(self):
+        self.assertEqual(_list3([1.0, 2.0, 3.0]), [1.0, 2.0, 3.0])
+
+    def test_list3_bad(self):
+        self.assertEqual(_list3(None), [0.0, 0.0, 0.0])
+
+    def test_list9_ok(self):
+        vals = list(range(9))
+        self.assertEqual(_list9(vals), [float(v) for v in vals])
+
+    def test_list9_short(self):
+        self.assertEqual(_list9(None), [0.0] * 9)
+
+
+# ── parse_fix2 ────────────────────────────────────────────────────────────────
+
+class TestParseFix2(unittest.TestCase):
+
+    def _make_fix(self, lat_1e8=0, lon_1e8=0, alt_msl_mm=0, alt_ell_mm=0,
+                  ned_vel=None, status=FIX_3D, sats=12,
+                  pdop=1.2, hdop=0.9, vdop=1.5,
+                  pos_cov=None, undulation=5.0, gnss_ts=0):
+        return _ns(
+            latitude_deg_1e8  = lat_1e8,
+            longitude_deg_1e8 = lon_1e8,
+            height_msl_mm     = alt_msl_mm,
+            height_ellipsoid_mm = alt_ell_mm,
+            ned_velocity      = ned_vel if ned_vel is not None else _ns_vec(0.0, 0.0, 0.0),
+            status            = status,
+            sats_used         = sats,
+            pdop              = pdop,
+            hdop              = hdop,
+            vdop              = vdop,
+            position_covariance = pos_cov,
+            undulation        = undulation,
+            gnss_timestamp    = gnss_ts,
+        )
+
+    def test_basic_3d_fix(self):
+        msg = self._make_fix(
+            lat_1e8    = 4_576_543_210,  # 45.7654321°
+            lon_1e8    = -7_345_678_901, # -73.45678901°
+            alt_msl_mm = 50_000,         # 50 m
+            alt_ell_mm = 52_000,         # 52 m
+            ned_vel    = _ns_vec(1.5, -0.3, 0.0),
+            sats       = 14,
+            hdop       = 0.8,
+        )
+        d = parse_fix2(msg)
+        self.assertIsNotNone(d)
+        self.assertAlmostEqual(d["lat_deg"],       45.7654321, places=5)
+        self.assertAlmostEqual(d["lon_deg"],       -73.45678901, places=5)
+        self.assertAlmostEqual(d["alt_msl_m"],     50.0, places=3)
+        self.assertAlmostEqual(d["alt_ellipsoid_m"], 52.0, places=3)
+        self.assertEqual(d["ned_vel"], [1.5, -0.3, 0.0])
+        self.assertEqual(d["fix_type"],  FIX_3D)
+        self.assertEqual(d["fix_label"], "3D_FIX")
+        self.assertEqual(d["sats_used"], 14)
+        self.assertAlmostEqual(d["hdop"], 0.8, places=5)
+
+    def test_no_fix(self):
+        msg = self._make_fix(status=FIX_NO_FIX, sats=0)
+        d = parse_fix2(msg)
+        self.assertIsNotNone(d)
+        self.assertEqual(d["fix_type"],  FIX_NO_FIX)
+        self.assertEqual(d["fix_label"], "NO_FIX")
+        self.assertEqual(d["sats_used"], 0)
+
+    def test_time_only(self):
+        msg = self._make_fix(status=FIX_TIME_ONLY)
+        d = parse_fix2(msg)
+        self.assertEqual(d["fix_label"], "TIME_ONLY")
+
+    def test_2d_fix(self):
+        msg = self._make_fix(status=FIX_2D)
+        d = parse_fix2(msg)
+        self.assertEqual(d["fix_label"], "2D_FIX")
+
+    def test_position_covariance(self):
+        cov = [float(i) for i in range(9)]
+        msg = self._make_fix(pos_cov=cov)
+        d = parse_fix2(msg)
+        self.assertEqual(d["pos_cov"], cov)
+
+    def test_no_covariance_returns_zeros(self):
+        msg = self._make_fix(pos_cov=None)
+        d = parse_fix2(msg)
+        self.assertEqual(d["pos_cov"], [0.0] * 9)
+
+    def test_zero_lat_lon(self):
+        msg = self._make_fix(lat_1e8=0, lon_1e8=0)
+        d = parse_fix2(msg)
+        self.assertAlmostEqual(d["lat_deg"], 0.0, places=8)
+        self.assertAlmostEqual(d["lon_deg"], 0.0, places=8)
+
+    def test_returns_none_on_bad_input(self):
+        self.assertIsNone(parse_fix2(None))
+
+    def test_gnss_timestamp(self):
+        msg = self._make_fix(gnss_ts=1_000_000_000)
+        d = parse_fix2(msg)
+        self.assertEqual(d["gnss_timestamp_us"], 1_000_000_000)
+
+
+# ── parse_raw_imu ─────────────────────────────────────────────────────────────
+
+class TestParseRawImu(unittest.TestCase):
+
+    def _make_imu(self, gyro=None, accel=None, dt=0.02,
+                  gyro_cov=None, accel_cov=None):
+        return _ns(
+            rate_gyro_latest         = gyro  if gyro  else _ns_vec(0.0, 0.0, 0.0),
+            accelerometer_latest     = accel if accel else _ns_vec(0.0, 0.0, 9.81),
+            integration_interval     = dt,
+            rate_gyro_covariance     = gyro_cov  if gyro_cov  else [0.0] * 9,
+            accelerometer_covariance = accel_cov if accel_cov else [0.0] * 9,
+        )
+
+    def test_basic(self):
+        msg = self._make_imu(
+            gyro  = _ns_vec(0.01, -0.02, 0.03),
+            accel = _ns_vec(0.1, -0.2, 9.8),
+            dt    = 0.02,
+        )
+        d = parse_raw_imu(msg)
+        self.assertIsNotNone(d)
+        self.assertEqual(d["gyro_xyz"],  [0.01, -0.02, 0.03])
+        self.assertEqual(d["accel_xyz"], [0.1, -0.2, 9.8])
+        self.assertAlmostEqual(d["dt_s"], 0.02)
+
+    def test_covariances_returned(self):
+        cov = [float(i * 0.001) for i in range(9)]
+        msg = self._make_imu(gyro_cov=cov, accel_cov=cov)
+        d = parse_raw_imu(msg)
+        self.assertEqual(d["gyro_cov"],  cov)
+        self.assertEqual(d["accel_cov"], cov)
+
+    def test_none_input(self):
+        self.assertIsNone(parse_raw_imu(None))
+
+
+# ── parse_mag ─────────────────────────────────────────────────────────────────
+
+class TestParseMag(unittest.TestCase):
+
+    def _make_mag(self, sensor_id=0, field=None, cov=None):
+        return _ns(
+            sensor_id              = sensor_id,
+            magnetic_field_ga      = field if field else _ns_vec(0.1, 0.2, -0.5),
+            magnetic_field_covariance = cov if cov else [0.0] * 9,
+        )
+
+    def test_basic(self):
+        msg = self._make_mag(sensor_id=1, field=_ns_vec(0.1, 0.2, -0.5))
+        d = parse_mag(msg)
+        self.assertIsNotNone(d)
+        self.assertEqual(d["sensor_id"],  1)
+        self.assertEqual(d["field_ga"],   [0.1, 0.2, -0.5])
+
+    def test_none_input(self):
+        self.assertIsNone(parse_mag(None))
+
+
+# ── parse_static_pressure ────────────────────────────────────────────────────
+
+class TestParseStaticPressure(unittest.TestCase):
+
+    def test_basic(self):
+        msg = _ns(static_pressure=101325.0, static_pressure_variance=100.0)
+        d = parse_static_pressure(msg)
+        self.assertIsNotNone(d)
+        self.assertAlmostEqual(d["pressure_pa"],       101325.0)
+        self.assertAlmostEqual(d["pressure_variance"], 100.0)
+
+    def test_none_input(self):
+        self.assertIsNone(parse_static_pressure(None))
+
+
+# ── parse_static_temperature ─────────────────────────────────────────────────
+
+class TestParseStaticTemperature(unittest.TestCase):
+
+    def test_celsius_to_kelvin(self):
+        msg = _ns(static_temperature=293.15)  # 20 °C
+        d = parse_static_temperature(msg)
+        self.assertIsNotNone(d)
+        self.assertAlmostEqual(d["temperature_k"], 293.15)
+
+    def test_none_input(self):
+        self.assertIsNone(parse_static_temperature(None))
+
+
+# ── parse_node_status ────────────────────────────────────────────────────────
+
+class TestParseNodeStatus(unittest.TestCase):
+
+    def _make_status(self, health=0, mode=0, uptime=120, vendor_code=0):
+        return _ns(
+            health                     = health,
+            mode                       = mode,
+            uptime_sec                 = uptime,
+            vendor_specific_status_code = vendor_code,
+        )
+
+    def test_ok_operational(self):
+        msg = self._make_status(health=0, mode=0, uptime=300)
+        d = parse_node_status(msg, source_node_id=10)
+        self.assertIsNotNone(d)
+        self.assertEqual(d["node_id"],     10)
+        self.assertEqual(d["health"],      0)
+        self.assertEqual(d["health_label"], "OK")
+        self.assertEqual(d["mode_label"],  "OPERATIONAL")
+        self.assertEqual(d["uptime_sec"],  300)
+
+    def test_error_state(self):
+        msg = self._make_status(health=2, mode=0)
+        d = parse_node_status(msg, source_node_id=5)
+        self.assertEqual(d["health_label"], "ERROR")
+
+    def test_critical_state(self):
+        msg = self._make_status(health=3)
+        d = parse_node_status(msg, 7)
+        self.assertEqual(d["health_label"], "CRITICAL")
+
+    def test_none_input(self):
+        self.assertIsNone(parse_node_status(None, 1))
+
+
+# ── chunk_rtcm ────────────────────────────────────────────────────────────────
+
+class TestChunkRtcm(unittest.TestCase):
+
+    def test_empty(self):
+        chunks = chunk_rtcm(b"", 0)
+        self.assertEqual(chunks, [])
+
+    def test_single_chunk(self):
+        data = bytes(range(64))
+        chunks = chunk_rtcm(data, 5)
+        self.assertEqual(len(chunks), 1)
+        self.assertEqual(chunks[0]["sequence_id"], 5)
+        self.assertEqual(chunks[0]["data"],        data)
+
+    def test_exact_chunk_boundary(self):
+        data = bytes(RTCM_MAX_CHUNK)
+        chunks = chunk_rtcm(data, 0)
+        self.assertEqual(len(chunks), 1)
+        self.assertEqual(len(chunks[0]["data"]), RTCM_MAX_CHUNK)
+
+    def test_two_chunks(self):
+        data = bytes(RTCM_MAX_CHUNK + 1)
+        chunks = chunk_rtcm(data, 0)
+        self.assertEqual(len(chunks), 2)
+        self.assertEqual(len(chunks[0]["data"]), RTCM_MAX_CHUNK)
+        self.assertEqual(len(chunks[1]["data"]), 1)
+
+    def test_sequence_id_increments(self):
+        data = bytes(RTCM_MAX_CHUNK * 3)
+        chunks = chunk_rtcm(data, 10)
+        for i, chunk in enumerate(chunks):
+            self.assertEqual(chunk["sequence_id"], 10 + i)
+
+    def test_sequence_id_wraps_at_16bit(self):
+        data = bytes(RTCM_MAX_CHUNK * 2)
+        chunks = chunk_rtcm(data, 0xFFFF)
+        self.assertEqual(chunks[0]["sequence_id"], 0xFFFF)
+        self.assertEqual(chunks[1]["sequence_id"], 0x0000)   # wraps
+
+    def test_data_round_trip(self):
+        original = bytes(range(200))
+        chunks = chunk_rtcm(original, 0)
+        reassembled = b"".join(c["data"] for c in chunks)
+        self.assertEqual(reassembled, original)
+
+    def test_large_message(self):
+        data = bytes(1024)
+        chunks = chunk_rtcm(data, 0)
+        total_bytes = sum(len(c["data"]) for c in chunks)
+        self.assertEqual(total_bytes, 1024)
+        for c in chunks:
+            self.assertLessEqual(len(c["data"]), RTCM_MAX_CHUNK)
+
+
+if __name__ == "__main__":
+    unittest.main()

jetson/ros2_ws/src/saltybot_esp32_serial/config/esp32_balance_params.yaml

@@ -0,0 +1,20 @@
+esp32_balance_node:
+  ros__parameters:
+    # USB-CDC serial port: CH343 chip, VID:PID 1a86:55d3
+    # Udev symlink set by jetson/docs/pinout.md udev rules
+    serial_port: /dev/esp32-balance
+    baud_rate: 460800
+
+    # /cmd_vel linear.x (m/s) → speed units sent to ESP32-S3 BALANCE
+    speed_scale: 1000.0
+    # /cmd_vel angular.z (rad/s) → steer units (negative = flip turn convention)
+    steer_scale: -500.0
+
+    # Safety: send CMD_DRIVE(0,0) if /cmd_vel silent for this many seconds
+    command_timeout_s: 0.5
+
+    # CMD_HEARTBEAT TX interval in seconds (ESP32-S3 watchdog fires at 500 ms)
+    heartbeat_period: 0.2
+
+    # Reconnect attempt interval when serial is lost
+    reconnect_delay: 2.0

jetson/ros2_ws/src/saltybot_esp32_serial/launch/esp32_balance.launch.py

@@ -0,0 +1,73 @@
+"""esp32_balance.launch.py — Launch the Orin↔ESP32-S3 BALANCE USB-serial bridge.
+
+bd-wim1: replaces can_bridge_node (CANable2/python-can) with USB-CDC serial.
+The freed CANable2 slot is used by Here4 GPS (bd-p47c).
+
+USB device
+----------
+  CH343 USB-CDC chip on ESP32-S3 BALANCE board
+  VID:PID  1a86:55d3
+  Symlink  /dev/esp32-balance  (via udev rule in jetson/docs/pinout.md)
+  Baud     460800
+
+Usage
+-----
+  # Default:
+  ros2 launch saltybot_esp32_serial esp32_balance.launch.py
+
+  # Override port (e.g. direct ACM path):
+  ros2 launch saltybot_esp32_serial esp32_balance.launch.py serial_port:=/dev/ttyACM0
+
+  # Custom velocity scales:
+  ros2 launch saltybot_esp32_serial esp32_balance.launch.py speed_scale:=800.0 steer_scale:=-400.0
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg_share = get_package_share_directory("saltybot_esp32_serial")
+    params_file = os.path.join(pkg_share, "config", "esp32_balance_params.yaml")
+
+    serial_port_arg = DeclareLaunchArgument(
+        "serial_port",
+        default_value="/dev/esp32-balance",
+        description="USB-CDC serial port for ESP32-S3 BALANCE (CH343, 1a86:55d3)",
+    )
+    speed_scale_arg = DeclareLaunchArgument(
+        "speed_scale",
+        default_value="1000.0",
+        description="linear.x (m/s) → motor speed units",
+    )
+    steer_scale_arg = DeclareLaunchArgument(
+        "steer_scale",
+        default_value="-500.0",
+        description="angular.z (rad/s) → steer units",
+    )
+
+    node = Node(
+        package="saltybot_esp32_serial",
+        executable="esp32_balance_node",
+        name="esp32_balance_node",
+        output="screen",
+        parameters=[
+            params_file,
+            {
+                "serial_port": LaunchConfiguration("serial_port"),
+                "speed_scale": LaunchConfiguration("speed_scale"),
+                "steer_scale": LaunchConfiguration("steer_scale"),
+            },
+        ],
+    )
+
+    return LaunchDescription([
+        serial_port_arg,
+        speed_scale_arg,
+        steer_scale_arg,
+        node,
+    ])

jetson/ros2_ws/src/saltybot_esp32_serial/package.xml

@@ -0,0 +1,40 @@
+<?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_esp32_serial</name>
+  <version>0.1.0</version>
+  <description>
+    UART/USB-CDC serial bridge for SaltyBot Orin — interfaces with the
+    ESP32-S3 BALANCE board (CH343, 1a86:55d3) using a binary framing protocol
+    [0xAA][LEN][TYPE][PAYLOAD][CRC8].  Replaces saltybot_can_bridge (CANable2)
+    to free the CAN bus for Here4 GPS (bd-p47c).
+
+    Publishes the same ROS2 topics as saltybot_can_bridge for backward compat:
+      /saltybot/attitude, /can/battery, /can/vesc/{left,right}/state,
+      /can/connection_status
+
+    System dependency: pyserial (apt: python3-serial  or  pip: pyserial)
+    Bead: bd-wim1  Counterpart firmware: bd-66hx (hal)
+  </description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+  <!-- pyserial: apt install python3-serial  or  pip install pyserial -->
+  <exec_depend>python3-serial</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_esp32_serial/saltybot_esp32_serial/__init__.py

@@ -0,0 +1 @@
+# saltybot_esp32_serial — UART/USB-CDC bridge for ESP32-S3 BALANCE (bd-wim1)

jetson/ros2_ws/src/saltybot_esp32_serial/saltybot_esp32_serial/esp32_balance_node.py

@@ -0,0 +1,369 @@
+"""esp32_balance_node.py — ROS2 bridge: Orin ↔ ESP32-S3 BALANCE via USB-CDC serial.
+
+bd-wim1: replaces saltybot_can_bridge (CANable2/python-can) with USB serial.
+The CANable2 is freed for Here4 GPS (bd-p47c).
+
+Physical connection
+-------------------
+  Device  : /dev/esp32-balance  (CH343 USB-CDC, VID:PID 1a86:55d3)
+  Baud    : 460800, 8N1
+  Protocol: [0xAA][LEN][TYPE][PAYLOAD][CRC8]  (see esp32_balance_protocol.py)
+
+Subscriptions
+-------------
+  /cmd_vel               geometry_msgs/Twist   → CMD_DRIVE (at up to 50 Hz)
+  /estop                 std_msgs/Bool         → CMD_ESTOP
+  /saltybot/arm          std_msgs/Bool         → CMD_ARM
+  /saltybot/pid_update   std_msgs/String JSON  → CMD_PID  {"kp":…,"ki":…,"kd":…}
+
+Publications
+------------
+  /saltybot/attitude        std_msgs/String JSON  pitch, motor_cmd, state, etc.
+  /saltybot/balance_state   std_msgs/String       same as /saltybot/attitude (alias)
+  /can/battery              sensor_msgs/BatteryState  from TELEM_STATUS.vbat_mv
+  /can/vesc/left/state      std_msgs/Float32MultiArray  [erpm, duty≡0, voltage_mv/1000, current_ma/1000]
+  /can/vesc/right/state     std_msgs/Float32MultiArray  same layout
+  /can/connection_status    std_msgs/String       "connected" | "disconnected"
+
+  NOTE: /can/* topic names are preserved for backward compatibility with
+        nodes that subscribed to saltybot_can_bridge outputs.
+
+Parameters
+----------
+  serial_port       str    USB-CDC port           default: /dev/esp32-balance
+  baud_rate         int    Baud rate              default: 460800
+  speed_scale       float  linear.x (m/s) → units default: 1000.0
+  steer_scale       float  angular.z (rad/s) → units  default: -500.0
+  command_timeout_s float  /cmd_vel watchdog      default: 0.5
+  heartbeat_period  float  CMD_HEARTBEAT interval default: 0.2
+  reconnect_delay   float  Retry interval, s      default: 2.0
+"""
+
+from __future__ import annotations
+
+import json
+import threading
+import time
+from typing import Optional
+
+import rclpy
+import serial
+from geometry_msgs.msg import Twist
+from rclpy.node import Node
+from sensor_msgs.msg import BatteryState
+from std_msgs.msg import Bool, Float32MultiArray, String
+
+from .esp32_balance_protocol import (
+    BAUD_RATE,
+    SERIAL_PORT_DEFAULT,
+    AckFrame,
+    BalanceState,
+    BALANCE_STATE_LABEL,
+    FrameParser,
+    NackFrame,
+    StatusFrame,
+    VescFrame,
+    encode_arm,
+    encode_drive,
+    encode_estop,
+    encode_heartbeat,
+    encode_pid,
+)
+
+_WATCHDOG_HZ: float = 20.0   # TX watchdog check rate
+
+
+class Esp32BalanceNode(Node):
+    """Serial bridge replacing CANable2 CAN comms with USB-CDC to ESP32-S3 BALANCE."""
+
+    def __init__(self) -> None:
+        super().__init__("esp32_balance_node")
+
+        # ── Parameters ────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",       SERIAL_PORT_DEFAULT)
+        self.declare_parameter("baud_rate",         BAUD_RATE)
+        self.declare_parameter("speed_scale",       1000.0)
+        self.declare_parameter("steer_scale",       -500.0)
+        self.declare_parameter("command_timeout_s", 0.5)
+        self.declare_parameter("heartbeat_period",  0.2)
+        self.declare_parameter("reconnect_delay",   2.0)
+
+        self._port_name     = self.get_parameter("serial_port").value
+        self._baud          = self.get_parameter("baud_rate").value
+        self._speed_scale   = self.get_parameter("speed_scale").value
+        self._steer_scale   = self.get_parameter("steer_scale").value
+        self._cmd_timeout   = self.get_parameter("command_timeout_s").value
+        self._hb_period     = self.get_parameter("heartbeat_period").value
+        self._reconnect_delay = self.get_parameter("reconnect_delay").value
+
+        # ── State ─────────────────────────────────────────────────────────
+        self._ser: Optional[serial.Serial] = None
+        self._ser_lock     = threading.Lock()
+        self._parser       = FrameParser()
+        self._last_cmd_time = time.monotonic()
+        self._watchdog_sent = False
+
+        # ── Publishers ────────────────────────────────────────────────────
+        self._pub_attitude   = self.create_publisher(String,             "/saltybot/attitude",      10)
+        self._pub_balance    = self.create_publisher(String,             "/saltybot/balance_state", 10)
+        self._pub_battery    = self.create_publisher(BatteryState,       "/can/battery",            10)
+        self._pub_vesc_left  = self.create_publisher(Float32MultiArray,  "/can/vesc/left/state",    10)
+        self._pub_vesc_right = self.create_publisher(Float32MultiArray,  "/can/vesc/right/state",   10)
+        self._pub_status     = self.create_publisher(String,             "/can/connection_status",  10)
+
+        # ── Subscriptions ─────────────────────────────────────────────────
+        self.create_subscription(Twist,  "/cmd_vel",            self._cmd_vel_cb,  10)
+        self.create_subscription(Bool,   "/estop",              self._estop_cb,    10)
+        self.create_subscription(Bool,   "/saltybot/arm",       self._arm_cb,      10)
+        self.create_subscription(String, "/saltybot/pid_update", self._pid_cb,     10)
+
+        # ── Timers ────────────────────────────────────────────────────────
+        self.create_timer(self._hb_period,     self._heartbeat_cb)
+        self.create_timer(1.0 / _WATCHDOG_HZ,  self._watchdog_cb)
+        self.create_timer(self._reconnect_delay, self._reconnect_cb)
+
+        # ── Open port + start reader thread ───────────────────────────────
+        self._try_connect()
+        self._reader_thread = threading.Thread(
+            target=self._reader_loop, daemon=True, name="balance_serial_reader"
+        )
+        self._reader_thread.start()
+
+        self.get_logger().info(
+            f"esp32_balance_node — {self._port_name} @ {self._baud} baud | "
+            f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}"
+        )
+
+    # ── Serial management ─────────────────────────────────────────────────
+
+    def _try_connect(self) -> None:
+        with self._ser_lock:
+            if self._ser is not None and self._ser.is_open:
+                return
+            try:
+                self._ser = serial.Serial(
+                    port=self._port_name,
+                    baudrate=self._baud,
+                    timeout=0.05,
+                    write_timeout=0.1,
+                )
+                self._ser.reset_input_buffer()
+                self._parser.reset()
+                self.get_logger().info(f"Serial open: {self._port_name}")
+                self._publish_status("connected")
+            except serial.SerialException as exc:
+                self.get_logger().warning(
+                    f"Cannot open {self._port_name}: {exc} — retry in {self._reconnect_delay:.0f} s",
+                    throttle_duration_sec=self._reconnect_delay,
+                )
+                self._ser = None
+                self._publish_status("disconnected")
+
+    def _reconnect_cb(self) -> None:
+        with self._ser_lock:
+            open_ok = self._ser is not None and self._ser.is_open
+        if not open_ok:
+            self._try_connect()
+
+    def _handle_serial_error(self, exc: Exception, context: str) -> None:
+        self.get_logger().warning(f"Serial error in {context}: {exc}")
+        with self._ser_lock:
+            if self._ser is not None:
+                try:
+                    self._ser.close()
+                except Exception:
+                    pass
+            self._ser = None
+        self._publish_status("disconnected")
+
+    # ── Serial write helper ───────────────────────────────────────────────
+
+    def _write(self, data: bytes, context: str = "") -> bool:
+        with self._ser_lock:
+            if self._ser is None or not self._ser.is_open:
+                return False
+            ser = self._ser
+        try:
+            ser.write(data)
+            return True
+        except serial.SerialException as exc:
+            self._handle_serial_error(exc, f"write({context})")
+            return False
+
+    # ── ROS subscriptions ─────────────────────────────────────────────────
+
+    def _cmd_vel_cb(self, msg: Twist) -> None:
+        """Convert /cmd_vel to CMD_DRIVE at up to 50 Hz."""
+        self._last_cmd_time  = time.monotonic()
+        self._watchdog_sent  = False
+
+        speed = int(msg.linear.x  * self._speed_scale)
+        steer = int(msg.angular.z * self._steer_scale)
+        self._write(encode_drive(speed, steer), "cmd_vel")
+
+    def _estop_cb(self, msg: Bool) -> None:
+        frame = encode_estop(msg.data)
+        self._write(frame, "estop")
+        if msg.data:
+            self.get_logger().warning("E-stop asserted → CMD_ESTOP sent to ESP32-S3 BALANCE")
+
+    def _arm_cb(self, msg: Bool) -> None:
+        self._write(encode_arm(msg.data), "arm")
+
+    def _pid_cb(self, msg: String) -> None:
+        """Parse JSON {"kp":…,"ki":…,"kd":…} and send CMD_PID."""
+        try:
+            d = json.loads(msg.data)
+            kp = float(d["kp"])
+            ki = float(d["ki"])
+            kd = float(d["kd"])
+        except (KeyError, ValueError, json.JSONDecodeError) as exc:
+            self.get_logger().error(f"Bad /saltybot/pid_update JSON: {exc}")
+            return
+        self._write(encode_pid(kp, ki, kd), "pid_update")
+
+    # ── Timers ────────────────────────────────────────────────────────────
+
+    def _heartbeat_cb(self) -> None:
+        self._write(encode_heartbeat(), "heartbeat")
+
+    def _watchdog_cb(self) -> None:
+        """Send CMD_DRIVE(0,0) if /cmd_vel has been silent for command_timeout_s."""
+        if self._watchdog_sent:
+            return
+        if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
+            self._write(encode_drive(0, 0), "watchdog")
+            self._watchdog_sent = True
+
+    # ── Background reader ─────────────────────────────────────────────────
+
+    def _reader_loop(self) -> None:
+        while rclpy.ok():
+            with self._ser_lock:
+                ser = self._ser if (self._ser and self._ser.is_open) else None
+            if ser is None:
+                time.sleep(0.05)
+                continue
+            try:
+                n = ser.in_waiting
+                if n:
+                    raw = ser.read(n)
+                else:
+                    time.sleep(0.002)
+                    continue
+            except serial.SerialException as exc:
+                self._handle_serial_error(exc, "reader_loop")
+                continue
+
+            for byte in raw:
+                frame = self._parser.feed(byte)
+                if frame is not None:
+                    self._dispatch(frame)
+
+    def _dispatch(self, frame) -> None:
+        try:
+            if isinstance(frame, StatusFrame):
+                self._handle_status(frame)
+            elif isinstance(frame, VescFrame):
+                self._handle_vesc(frame)
+            elif isinstance(frame, AckFrame):
+                self.get_logger().debug(
+                    f"ACK for cmd type 0x{frame.echoed_type:02X}"
+                )
+            elif isinstance(frame, NackFrame):
+                self.get_logger().warning(
+                    f"NACK cmd=0x{frame.cmd_type:02X} err=0x{frame.error_code:02X}"
+                )
+            elif isinstance(frame, tuple):
+                type_code, _ = frame
+                self.get_logger().debug(f"Unknown telemetry type 0x{type_code:02X}")
+        except Exception as exc:
+            self.get_logger().warning(f"Error dispatching frame {type(frame).__name__}: {exc}")
+
+    # ── Frame handlers ────────────────────────────────────────────────────
+
+    def _handle_status(self, f: StatusFrame) -> None:
+        """Publish balance controller status — replaces CAN 0x400 handler."""
+        now = self.get_clock().now().to_msg()
+        payload = {
+            "pitch_deg":    round(f.pitch_deg, 2),
+            "motor_cmd":    f.motor_cmd,
+            "vbat_mv":      f.vbat_mv,
+            "balance_state": f.balance_state,
+            "state_label":  BALANCE_STATE_LABEL.get(f.balance_state,
+                                                     f"UNKNOWN({f.balance_state})"),
+            "flags":        f.flags,
+            "estop_active": bool(f.flags & 0x01),
+            "hb_timeout":   bool(f.flags & 0x02),
+            "ts":           f"{now.sec}.{now.nanosec:09d}",
+        }
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._pub_attitude.publish(msg)
+        self._pub_balance.publish(msg)
+
+        # Battery piggyback on STATUS frame for nodes that use /can/battery
+        bat = BatteryState()
+        bat.header.stamp = now
+        bat.voltage      = f.vbat_mv / 1000.0
+        bat.present      = True
+        bat.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+        self._pub_battery.publish(bat)
+
+    def _handle_vesc(self, f: VescFrame) -> None:
+        """Publish proxied VESC telemetry — replaces CAN VESC_STATUS_1 handler.
+
+        Float32MultiArray layout: [erpm, voltage_v, current_a, temp_c]
+        (duty not available via serial proxy; 0.0 placeholder)
+        """
+        arr = Float32MultiArray()
+        arr.data = [
+            float(f.erpm),
+            float(f.voltage_mv) / 1000.0,
+            float(f.current_ma) / 1000.0,
+            f.temp_c,
+        ]
+        if f.vesc_id == 56:
+            self._pub_vesc_left.publish(arr)
+        elif f.vesc_id == 68:
+            self._pub_vesc_right.publish(arr)
+        else:
+            self.get_logger().warning(f"Unknown VESC ID {f.vesc_id} in telemetry frame")
+
+    # ── Status helper ─────────────────────────────────────────────────────
+
+    def _publish_status(self, status: str) -> None:
+        msg = String()
+        msg.data = status
+        self._pub_status.publish(msg)
+
+    # ── Shutdown ──────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        # Send zero drive + estop before closing
+        self._write(encode_drive(0, 0), "shutdown")
+        self._write(encode_estop(True), "shutdown")
+        with self._ser_lock:
+            if self._ser is not None:
+                try:
+                    self._ser.close()
+                except Exception:
+                    pass
+            self._ser = None
+        super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = Esp32BalanceNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_esp32_serial/saltybot_esp32_serial/esp32_balance_protocol.py

@@ -0,0 +1,355 @@
+"""esp32_balance_protocol.py — Binary frame codec for Orin↔ESP32-S3 BALANCE UART/USB comms.
+
+bd-wim1: replaces CANable2/python-can ESP32 interface with USB-CDC serial.
+Counterpart firmware bead: bd-66hx (hal's ESP32-S3 BALANCE side).
+
+Frame layout:
+  ┌───────┬─────┬──────┬──────────────────┬───────┐
+  │  SYNC │ LEN │ TYPE │     PAYLOAD      │  CRC8 │
+  │  0xAA │  1B │  1B  │    LEN bytes     │   1B  │
+  └───────┴─────┴──────┴──────────────────┴───────┘
+
+  LEN   = number of payload bytes (0–62)
+  CRC8  = CRC8-SMBUS (poly=0x07, init=0x00), computed over LEN+TYPE+PAYLOAD
+
+Physical layer
+  Device  : /dev/esp32-balance  (CH343 USB-CDC, VID:PID 1a86:55d3)
+  Baud    : 460800, 8N1
+
+Command types (Orin → ESP32-S3 BALANCE):
+  0x01  CMD_HEARTBEAT  — no payload                               len=0
+  0x02  CMD_DRIVE      — int16 speed_units, int16 steer_units     len=4  range −1000..+1000
+  0x03  CMD_ESTOP      — uint8 (1=assert, 0=clear)               len=1
+  0x04  CMD_ARM        — uint8 (1=arm, 0=disarm)                 len=1
+  0x05  CMD_PID        — float32 kp, float32 ki, float32 kd       len=12
+
+Telemetry types (ESP32-S3 BALANCE → Orin):
+  0x80  TELEM_STATUS      — int16 pitch_x10, int16 motor_cmd,
+                            uint16 vbat_mv, uint8 balance_state, uint8 flags   len=8
+  0x81  TELEM_VESC_LEFT   — int32 erpm, uint16 voltage_mv,
+                            int16 current_ma, uint16 temp_c_x10                len=10
+  0x82  TELEM_VESC_RIGHT  — same layout as TELEM_VESC_LEFT                     len=10
+  0xA0  RESP_ACK   — uint8 echoed cmd type                       len=1
+  0xA1  RESP_NACK  — uint8 cmd type, uint8 error_code            len=2
+
+Balance state values (TELEM_STATUS.balance_state):
+  0  DISARMED
+  1  ARMED
+  2  TILT_FAULT
+  3  ESTOP
+
+NACK error codes:
+  0x01  ERR_BAD_CRC
+  0x02  ERR_BAD_LEN
+  0x03  ERR_ESTOP_ACTIVE
+  0x04  ERR_DISARMED
+
+NOTE: Message type IDs coordinated with hal (bd-66hx) on 2026-04-17.
+      Confirm against bd-66hx PR before deploying on hardware.
+"""
+
+from __future__ import annotations
+
+import struct
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Optional
+
+# ── Physical layer constants ──────────────────────────────────────────────────
+
+SERIAL_PORT_DEFAULT = "/dev/esp32-balance"
+BAUD_RATE           = 460800
+MAX_PAYLOAD_LEN     = 62       # hard cap; larger frames are considered corrupt
+
+# ── Framing ───────────────────────────────────────────────────────────────────
+
+SYNC_BYTE = 0xAA
+
+
+# ── Command type codes (Orin → ESP32-S3 BALANCE) ─────────────────────────────
+
+class CmdType(IntEnum):
+    HEARTBEAT = 0x01
+    DRIVE     = 0x02
+    ESTOP     = 0x03
+    ARM       = 0x04
+    PID       = 0x05
+
+
+# ── Telemetry type codes (ESP32-S3 BALANCE → Orin) ───────────────────────────
+
+class TelType(IntEnum):
+    STATUS     = 0x80
+    VESC_LEFT  = 0x81
+    VESC_RIGHT = 0x82
+    ACK        = 0xA0
+    NACK       = 0xA1
+
+
+# ── Balance state enum ────────────────────────────────────────────────────────
+
+class BalanceState(IntEnum):
+    DISARMED   = 0
+    ARMED      = 1
+    TILT_FAULT = 2
+    ESTOP      = 3
+
+
+BALANCE_STATE_LABEL = {
+    BalanceState.DISARMED:   "DISARMED",
+    BalanceState.ARMED:      "ARMED",
+    BalanceState.TILT_FAULT: "TILT_FAULT",
+    BalanceState.ESTOP:      "ESTOP",
+}
+
+
+# ── Parsed telemetry dataclasses ─────────────────────────────────────────────
+
+@dataclass
+class StatusFrame:
+    """TELEM_STATUS (0x80): balance controller status at ~10 Hz."""
+    pitch_deg:     float   # degrees (positive = forward tilt); raw pitch_x10 / 10.0
+    motor_cmd:     int     # ESC command, −1000..+1000
+    vbat_mv:       int     # battery voltage, millivolts
+    balance_state: int     # BalanceState enum
+    flags:         int     # bitmask — bit 0: estop_active, bit 1: heartbeat_timeout
+
+
+@dataclass
+class VescFrame:
+    """TELEM_VESC_LEFT (0x81) or TELEM_VESC_RIGHT (0x82): VESC proxied telemetry at ~10 Hz."""
+    vesc_id:    int    # 56 = left, 68 = right (maps to CAN IDs from hardware spec)
+    erpm:       int    # electrical RPM (signed)
+    voltage_mv: int    # bus voltage, millivolts
+    current_ma: int    # phase current, milliamps (signed)
+    temp_c:     float  # motor temperature, °C; raw temp_c_x10 / 10.0
+
+
+@dataclass
+class AckFrame:
+    """RESP_ACK (0xA0): acknowledgement for a successfully parsed command."""
+    echoed_type: int
+
+
+@dataclass
+class NackFrame:
+    """RESP_NACK (0xA1): rejection; contains original cmd type and error code."""
+    cmd_type:   int
+    error_code: int
+
+
+TelemetryFrame = StatusFrame | VescFrame | AckFrame | NackFrame
+
+
+# ── CRC8-SMBUS ────────────────────────────────────────────────────────────────
+
+def _crc8(data: bytes) -> int:
+    """CRC8-SMBUS: polynomial 0x07, init 0x00, no final XOR.
+
+    Coverage: LEN + TYPE + PAYLOAD bytes.
+    """
+    crc = 0
+    for byte in data:
+        crc ^= byte
+        for _ in range(8):
+            if crc & 0x80:
+                crc = (crc << 1) ^ 0x07
+            else:
+                crc <<= 1
+        crc &= 0xFF
+    return crc
+
+
+# ── Frame encoder ─────────────────────────────────────────────────────────────
+
+def _build_frame(cmd_type: int, payload: bytes) -> bytes:
+    """Build a complete binary frame: [0xAA][LEN][TYPE][PAYLOAD][CRC8]."""
+    if len(payload) > MAX_PAYLOAD_LEN:
+        raise ValueError(f"Payload length {len(payload)} exceeds max {MAX_PAYLOAD_LEN}")
+    length = len(payload)
+    crc_data = bytes([length, cmd_type]) + payload
+    crc = _crc8(crc_data)
+    return bytes([SYNC_BYTE, length, cmd_type]) + payload + bytes([crc])
+
+
+def encode_heartbeat() -> bytes:
+    """CMD_HEARTBEAT — no payload. Send every ≤500 ms to keep watchdog alive."""
+    return _build_frame(CmdType.HEARTBEAT, b"")
+
+
+def encode_drive(speed: int, steer: int) -> bytes:
+    """CMD_DRIVE — int16 speed_units + int16 steer_units (big-endian, −1000..+1000).
+
+    Parameters
+    ----------
+    speed : target longitudinal speed in motor units (positive = forward)
+    steer : steering demand in motor units (positive = right turn)
+    """
+    speed = max(-1000, min(1000, int(speed)))
+    steer = max(-1000, min(1000, int(steer)))
+    return _build_frame(CmdType.DRIVE, struct.pack(">hh", speed, steer))
+
+
+def encode_estop(assert_stop: bool) -> bytes:
+    """CMD_ESTOP — uint8, 1=assert, 0=clear."""
+    return _build_frame(CmdType.ESTOP, struct.pack("B", 1 if assert_stop else 0))
+
+
+def encode_arm(arm: bool) -> bytes:
+    """CMD_ARM — uint8, 1=arm, 0=disarm."""
+    return _build_frame(CmdType.ARM, struct.pack("B", 1 if arm else 0))
+
+
+def encode_pid(kp: float, ki: float, kd: float) -> bytes:
+    """CMD_PID — three float32 values, big-endian."""
+    if kp < 0.0 or ki < 0.0 or kd < 0.0:
+        raise ValueError("PID gains must be non-negative")
+    return _build_frame(CmdType.PID, struct.pack(">fff", kp, ki, kd))
+
+
+# ── Frame decoder (streaming state-machine) ───────────────────────────────────
+
+class _ParseState(IntEnum):
+    WAIT_SYNC = 0
+    WAIT_LEN  = 1
+    WAIT_TYPE = 2
+    PAYLOAD   = 3
+    WAIT_CRC  = 4
+
+
+class FrameParser:
+    """Byte-by-byte streaming parser for ESP32-S3 BALANCE telemetry frames.
+
+    Feed individual bytes via ``feed()``; returns a decoded dataclass when a
+    complete, CRC-valid frame arrives, otherwise returns ``None``.
+
+    Not thread-safe — guard with a lock if accessed from multiple threads.
+
+    Counters
+    --------
+    frames_ok     : successfully decoded frames
+    frames_error  : CRC failures or oversized payloads
+    """
+
+    def __init__(self) -> None:
+        self.frames_ok    = 0
+        self.frames_error = 0
+        self._reset()
+
+    def reset(self) -> None:
+        """Reset to initial state — call after serial reconnect."""
+        self._reset()
+
+    def _reset(self) -> None:
+        self._state   = _ParseState.WAIT_SYNC
+        self._length  = 0
+        self._type    = 0
+        self._payload = bytearray()
+
+    def feed(self, byte: int) -> Optional[TelemetryFrame | tuple]:
+        """Process one byte.
+
+        Returns a decoded dataclass on success, ``None`` while waiting for more
+        bytes, or a ``(type_code, raw_payload)`` tuple for unrecognised types.
+        """
+        s = self._state
+
+        if s == _ParseState.WAIT_SYNC:
+            if byte == SYNC_BYTE:
+                self._state = _ParseState.WAIT_LEN
+            return None
+
+        if s == _ParseState.WAIT_LEN:
+            if byte > MAX_PAYLOAD_LEN:
+                self.frames_error += 1
+                self._reset()
+                return None
+            self._length = byte
+            self._state  = _ParseState.WAIT_TYPE
+            return None
+
+        if s == _ParseState.WAIT_TYPE:
+            self._type    = byte
+            self._payload = bytearray()
+            if self._length == 0:
+                self._state = _ParseState.WAIT_CRC
+            else:
+                self._state = _ParseState.PAYLOAD
+            return None
+
+        if s == _ParseState.PAYLOAD:
+            self._payload.append(byte)
+            if len(self._payload) == self._length:
+                self._state = _ParseState.WAIT_CRC
+            return None
+
+        if s == _ParseState.WAIT_CRC:
+            # Verify CRC before resetting parser state
+            crc_data = bytes([self._length, self._type]) + self._payload
+            expected = _crc8(crc_data)
+            if byte != expected:
+                self.frames_error += 1
+                self._reset()
+                return None
+            self.frames_ok += 1
+            result = _decode(self._type, bytes(self._payload))
+            self._reset()
+            return result
+
+        self._reset()
+        return None
+
+
+# ── Telemetry decoder ─────────────────────────────────────────────────────────
+
+# Maps VESC_LEFT/RIGHT type codes to hardware CAN IDs (from memory)
+_VESC_ID_MAP = {
+    TelType.VESC_LEFT:  56,   # left VESC CAN ID
+    TelType.VESC_RIGHT: 68,   # right VESC CAN ID
+}
+
+
+def _decode(type_code: int, payload: bytes) -> Optional[TelemetryFrame | tuple]:
+    """Decode a validated telemetry payload into a typed dataclass."""
+    try:
+        if type_code == TelType.STATUS:
+            # int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, uint8 state, uint8 flags
+            if len(payload) < 8:
+                return None
+            pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hhHBB", payload)
+            return StatusFrame(
+                pitch_deg=pitch_x10 / 10.0,
+                motor_cmd=motor_cmd,
+                vbat_mv=vbat_mv,
+                balance_state=state,
+                flags=flags,
+            )
+
+        if type_code in (TelType.VESC_LEFT, TelType.VESC_RIGHT):
+            # int32 erpm, uint16 voltage_mv, int16 current_ma, uint16 temp_c_x10
+            if len(payload) < 10:
+                return None
+            erpm, voltage_mv, current_ma, temp_x10 = struct.unpack_from(">iHhH", payload)
+            return VescFrame(
+                vesc_id=_VESC_ID_MAP[type_code],
+                erpm=erpm,
+                voltage_mv=voltage_mv,
+                current_ma=current_ma,
+                temp_c=temp_x10 / 10.0,
+            )
+
+        if type_code == TelType.ACK:
+            if len(payload) < 1:
+                return None
+            return AckFrame(echoed_type=payload[0])
+
+        if type_code == TelType.NACK:
+            if len(payload) < 2:
+                return None
+            return NackFrame(cmd_type=payload[0], error_code=payload[1])
+
+    except struct.error:
+        return None
+
+    # Unknown type — return raw for forward-compatibility
+    return (type_code, payload)

jetson/ros2_ws/src/saltybot_esp32_serial/setup.cfg

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

jetson/ros2_ws/src/saltybot_esp32_serial/setup.py

@@ -0,0 +1,27 @@
+from setuptools import setup
+
+package_name = "saltybot_esp32_serial"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/config", ["config/esp32_balance_params.yaml"]),
+        (f"share/{package_name}/launch", ["launch/esp32_balance.launch.py"]),
+    ],
+    install_requires=["setuptools", "pyserial"],
+    zip_safe=True,
+    maintainer="sl-perception",
+    maintainer_email="sl-perception@saltylab.local",
+    description="UART/USB-CDC serial bridge for ESP32-S3 BALANCE (bd-wim1)",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "esp32_balance_node = saltybot_esp32_serial.esp32_balance_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_esp32_serial/test/test_esp32_balance_protocol.py

@@ -0,0 +1,311 @@
+#!/usr/bin/env python3
+"""Unit tests for saltybot_esp32_serial.esp32_balance_protocol.
+
+No ROS2 or hardware required — exercises encode/decode round-trips,
+framing, CRC8, and parser state-machine in pure Python.
+
+Run with:  pytest test/ -v
+"""
+
+import struct
+import unittest
+
+from saltybot_esp32_serial.esp32_balance_protocol import (
+    SYNC_BYTE,
+    CmdType,
+    TelType,
+    BalanceState,
+    StatusFrame,
+    VescFrame,
+    AckFrame,
+    NackFrame,
+    FrameParser,
+    _crc8,
+    encode_heartbeat,
+    encode_drive,
+    encode_estop,
+    encode_arm,
+    encode_pid,
+)
+
+
+# ── CRC8 tests ────────────────────────────────────────────────────────────────
+
+class TestCrc8(unittest.TestCase):
+    """Verify CRC8-SMBUS implementation."""
+
+    def test_empty(self):
+        self.assertEqual(_crc8(b""), 0x00)
+
+    def test_single_byte_zero(self):
+        # CRC of 0x00: no XOR, no poly — stays 0
+        self.assertEqual(_crc8(b"\x00"), 0x00)
+
+    def test_single_byte_nonzero(self):
+        # Pre-computed expected value for 0xFF
+        result = _crc8(b"\xFF")
+        self.assertIsInstance(result, int)
+        self.assertGreaterEqual(result, 0)
+        self.assertLessEqual(result, 0xFF)
+
+    def test_deterministic(self):
+        data = b"\x01\x02\x04\x08"
+        self.assertEqual(_crc8(data), _crc8(data))
+
+    def test_different_data_different_crc(self):
+        self.assertNotEqual(_crc8(b"\x01"), _crc8(b"\x02"))
+
+    def test_byte_range(self):
+        for b in range(256):
+            r = _crc8(bytes([b]))
+            self.assertGreaterEqual(r, 0)
+            self.assertLessEqual(r, 255)
+
+
+# ── Frame structure tests ─────────────────────────────────────────────────────
+
+class TestFrameStructure(unittest.TestCase):
+    """Verify encode functions produce correctly structured frames."""
+
+    def _check_frame(self, data: bytes, expected_type: int, expected_payload: bytes):
+        """Assert frame matches [SYNC][LEN][TYPE][PAYLOAD][CRC8]."""
+        self.assertGreaterEqual(len(data), 4)
+        self.assertEqual(data[0], SYNC_BYTE, "SYNC byte mismatch")
+        length = data[1]
+        self.assertEqual(data[2], expected_type, "TYPE byte mismatch")
+        self.assertEqual(length, len(expected_payload), "LEN byte mismatch")
+        self.assertEqual(data[3:3 + length], expected_payload, "PAYLOAD mismatch")
+        # Verify CRC over [LEN][TYPE][PAYLOAD]
+        crc_data = bytes([length, expected_type]) + expected_payload
+        expected_crc = _crc8(crc_data)
+        self.assertEqual(data[-1], expected_crc, "CRC8 mismatch")
+
+    def test_heartbeat_frame(self):
+        frame = encode_heartbeat()
+        self._check_frame(frame, CmdType.HEARTBEAT, b"")
+
+    def test_drive_frame_zeros(self):
+        frame = encode_drive(0, 0)
+        payload = struct.pack(">hh", 0, 0)
+        self._check_frame(frame, CmdType.DRIVE, payload)
+
+    def test_drive_frame_values(self):
+        frame = encode_drive(500, -300)
+        payload = struct.pack(">hh", 500, -300)
+        self._check_frame(frame, CmdType.DRIVE, payload)
+
+    def test_drive_clamping(self):
+        # Values outside ±1000 must be clamped
+        frame = encode_drive(5000, -9999)
+        _, _, _, p0, p1, p2, p3, _ = frame
+        speed, steer = struct.unpack(">hh", bytes([p0, p1, p2, p3]))
+        self.assertEqual(speed, 1000)
+        self.assertEqual(steer, -1000)
+
+    def test_estop_assert(self):
+        frame = encode_estop(True)
+        self._check_frame(frame, CmdType.ESTOP, b"\x01")
+
+    def test_estop_clear(self):
+        frame = encode_estop(False)
+        self._check_frame(frame, CmdType.ESTOP, b"\x00")
+
+    def test_arm_frame(self):
+        for arm_val in (True, False):
+            frame = encode_arm(arm_val)
+            self._check_frame(frame, CmdType.ARM, bytes([1 if arm_val else 0]))
+
+    def test_pid_frame(self):
+        frame = encode_pid(1.0, 0.1, 0.01)
+        payload = struct.pack(">fff", 1.0, 0.1, 0.01)
+        self._check_frame(frame, CmdType.PID, payload)
+
+    def test_pid_negative_raises(self):
+        with self.assertRaises(ValueError):
+            encode_pid(-1.0, 0.1, 0.01)
+
+
+# ── Parser round-trip tests ───────────────────────────────────────────────────
+
+class TestFrameParser(unittest.TestCase):
+    """Feed encoded frames back through the parser and verify decode."""
+
+    def _parse_all(self, data: bytes):
+        """Feed all bytes; return list of decoded frames."""
+        parser = FrameParser()
+        results = []
+        for byte in data:
+            r = parser.feed(byte)
+            if r is not None:
+                results.append(r)
+        return parser, results
+
+    # ── Command echo round-trips (parser recognises unknown cmd types as raw tuples) ──
+
+    def test_heartbeat_roundtrip(self):
+        frame = encode_heartbeat()
+        parser, results = self._parse_all(frame)
+        self.assertEqual(len(results), 1)
+        # HEARTBEAT (0x01) is a command type — no dataclass decoder, returns raw tuple
+        self.assertIsInstance(results[0], tuple)
+        type_code, payload = results[0]
+        self.assertEqual(type_code, CmdType.HEARTBEAT)
+        self.assertEqual(payload, b"")
+
+    def test_drive_roundtrip(self):
+        frame = encode_drive(250, -100)
+        parser, results = self._parse_all(frame)
+        self.assertEqual(len(results), 1)
+        type_code, payload = results[0]
+        self.assertEqual(type_code, CmdType.DRIVE)
+        speed, steer = struct.unpack(">hh", payload)
+        self.assertEqual(speed, 250)
+        self.assertEqual(steer, -100)
+
+    # ── Telemetry decode round-trips ────────────────────────────────────────
+
+    def _build_telem_frame(self, type_code: int, payload: bytes) -> bytes:
+        length = len(payload)
+        crc = _crc8(bytes([length, type_code]) + payload)
+        return bytes([SYNC_BYTE, length, type_code]) + payload + bytes([crc])
+
+    def test_status_frame_decode(self):
+        raw = struct.pack(">hhHBB", 152, 300, 11400, BalanceState.ARMED, 0x00)
+        frame = self._build_telem_frame(TelType.STATUS, raw)
+        _, results = self._parse_all(frame)
+        self.assertEqual(len(results), 1)
+        f = results[0]
+        self.assertIsInstance(f, StatusFrame)
+        self.assertAlmostEqual(f.pitch_deg, 15.2, places=5)
+        self.assertEqual(f.motor_cmd, 300)
+        self.assertEqual(f.vbat_mv, 11400)
+        self.assertEqual(f.balance_state, BalanceState.ARMED)
+        self.assertEqual(f.flags, 0x00)
+
+    def test_status_estop_flag(self):
+        raw = struct.pack(">hhHBB", 0, 0, 10000, BalanceState.ESTOP, 0x01)
+        frame = self._build_telem_frame(TelType.STATUS, raw)
+        _, results = self._parse_all(frame)
+        f = results[0]
+        self.assertIsInstance(f, StatusFrame)
+        self.assertTrue(bool(f.flags & 0x01))  # estop_active flag set
+
+    def test_vesc_left_frame_decode(self):
+        # erpm=12000, voltage_mv=22400, current_ma=4500, temp_x10=352
+        raw = struct.pack(">iHhH", 12000, 22400, 4500, 352)
+        frame = self._build_telem_frame(TelType.VESC_LEFT, raw)
+        _, results = self._parse_all(frame)
+        self.assertEqual(len(results), 1)
+        f = results[0]
+        self.assertIsInstance(f, VescFrame)
+        self.assertEqual(f.vesc_id, 56)
+        self.assertEqual(f.erpm, 12000)
+        self.assertEqual(f.voltage_mv, 22400)
+        self.assertEqual(f.current_ma, 4500)
+        self.assertAlmostEqual(f.temp_c, 35.2, places=5)
+
+    def test_vesc_right_frame_decode(self):
+        raw = struct.pack(">iHhH", -6000, 22000, -200, 280)
+        frame = self._build_telem_frame(TelType.VESC_RIGHT, raw)
+        _, results = self._parse_all(frame)
+        f = results[0]
+        self.assertIsInstance(f, VescFrame)
+        self.assertEqual(f.vesc_id, 68)
+        self.assertEqual(f.erpm, -6000)
+        self.assertAlmostEqual(f.temp_c, 28.0, places=5)
+
+    def test_ack_frame_decode(self):
+        raw = bytes([CmdType.DRIVE])
+        frame = self._build_telem_frame(TelType.ACK, raw)
+        _, results = self._parse_all(frame)
+        f = results[0]
+        self.assertIsInstance(f, AckFrame)
+        self.assertEqual(f.echoed_type, CmdType.DRIVE)
+
+    def test_nack_frame_decode(self):
+        raw = bytes([CmdType.DRIVE, 0x03])  # ERR_ESTOP_ACTIVE
+        frame = self._build_telem_frame(TelType.NACK, raw)
+        _, results = self._parse_all(frame)
+        f = results[0]
+        self.assertIsInstance(f, NackFrame)
+        self.assertEqual(f.cmd_type, CmdType.DRIVE)
+        self.assertEqual(f.error_code, 0x03)
+
+    # ── Parser robustness ───────────────────────────────────────────────────
+
+    def test_bad_crc_discarded(self):
+        frame = bytearray(encode_heartbeat())
+        frame[-1] ^= 0xFF          # corrupt CRC
+        parser, results = self._parse_all(bytes(frame))
+        self.assertEqual(results, [])
+        self.assertEqual(parser.frames_error, 1)
+        self.assertEqual(parser.frames_ok, 0)
+
+    def test_garbage_before_sync(self):
+        garbage = bytes([0x00, 0xFF, 0x01, 0x22])
+        frame = encode_heartbeat()
+        parser, results = self._parse_all(garbage + frame)
+        self.assertEqual(len(results), 1)
+
+    def test_two_frames_sequential(self):
+        frames = encode_heartbeat() + encode_drive(100, 50)
+        parser, results = self._parse_all(frames)
+        self.assertEqual(len(results), 2)
+        self.assertEqual(parser.frames_ok, 2)
+
+    def test_oversized_payload_rejected(self):
+        # Craft a frame claiming LEN=63 (> MAX_PAYLOAD_LEN=62)
+        bad = bytes([SYNC_BYTE, 63, CmdType.DRIVE])
+        parser, results = self._parse_all(bad)
+        self.assertEqual(results, [])
+        self.assertEqual(parser.frames_error, 1)
+
+    def test_parser_counter_increments(self):
+        parser = FrameParser()
+        frame = encode_drive(10, 10)
+        for b in frame:
+            parser.feed(b)
+        self.assertEqual(parser.frames_ok, 1)
+        self.assertEqual(parser.frames_error, 0)
+
+    def test_reset_clears_state(self):
+        parser = FrameParser()
+        # Feed partial frame then reset
+        partial = encode_drive(0, 0)[:-2]
+        for b in partial:
+            parser.feed(b)
+        parser.reset()
+        # Now a clean frame should decode correctly
+        for b in encode_heartbeat():
+            r = parser.feed(b)
+        self.assertEqual(parser.frames_ok, 1)
+
+
+# ── Encode parameter edge cases ───────────────────────────────────────────────
+
+class TestEncodeEdgeCases(unittest.TestCase):
+    def test_drive_clamp_positive(self):
+        frame = encode_drive(9999, 9999)
+        payload = frame[3:7]
+        speed, steer = struct.unpack(">hh", payload)
+        self.assertEqual(speed, 1000)
+        self.assertEqual(steer, 1000)
+
+    def test_drive_clamp_negative(self):
+        frame = encode_drive(-9999, -9999)
+        payload = frame[3:7]
+        speed, steer = struct.unpack(">hh", payload)
+        self.assertEqual(speed, -1000)
+        self.assertEqual(steer, -1000)
+
+    def test_pid_zero_gains_valid(self):
+        frame = encode_pid(0.0, 0.0, 0.0)
+        self.assertIsNotNone(frame)
+
+    def test_pid_large_gains_valid(self):
+        frame = encode_pid(100.0, 50.0, 25.0)
+        self.assertIsNotNone(frame)
+
+
+if __name__ == "__main__":
+    unittest.main()

jetson/scripts/ros2-launch.sh

@@ -0,0 +1,38 @@
+#!/usr/bin/env bash
+# ros2-launch.sh — Source ROS2 Humble + workspace overlay, then exec ros2 launch
+#
+# Used by saltybot-*.service units so that ExecStart= does not need to inline
+# bash source chains.
+#
+# Usage (from a service file):
+#   ExecStart=/opt/saltybot/scripts/ros2-launch.sh <pkg> <launch_file> [args...]
+#
+# Env vars respected:
+#   ROS_DISTRO        default: humble
+#   SALTYBOT_WS       default: /opt/saltybot/jetson/ros2_ws/install
+#   ROS_DOMAIN_ID     default: 42
+
+set -euo pipefail
+
+ROS_DISTRO="${ROS_DISTRO:-humble}"
+SALTYBOT_WS="${SALTYBOT_WS:-/opt/saltybot/jetson/ros2_ws/install}"
+
+ROS_SETUP="/opt/ros/${ROS_DISTRO}/setup.bash"
+WS_SETUP="${SALTYBOT_WS}/setup.bash"
+
+if [[ ! -f "${ROS_SETUP}" ]]; then
+    echo "[ros2-launch] ERROR: ROS2 setup not found: ${ROS_SETUP}" >&2
+    exit 1
+fi
+
+# shellcheck disable=SC1090
+source "${ROS_SETUP}"
+
+if [[ -f "${WS_SETUP}" ]]; then
+    # shellcheck disable=SC1090
+    source "${WS_SETUP}"
+fi
+
+export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}"
+
+exec ros2 launch "$@"

jetson/systemd/install_systemd.sh

@@ -1,55 +1,194 @@
 #!/usr/bin/env bash
-# install_systemd.sh — Install saltybot systemd services on Orin
-# Run as root: sudo ./systemd/install_systemd.sh
+# install_systemd.sh — Deploy and enable saltybot systemd services on Orin
+#
+# Run as root:  sudo ./systemd/install_systemd.sh
+#
+# What this does:
+#   1. Deploy repo to /opt/saltybot/jetson
+#   2. Build ROS2 workspace (colcon)
+#   3. Install systemd unit files
+#   4. Install udev rules (CAN, ESP32)
+#   5. Enable and optionally start all services
+#
+# Services installed (start order):
+#   can-bringup.service           CANable2 @ 1 Mbps DroneCAN (Here4 GPS)
+#   saltybot-esp32-serial.service ESP32-S3 BALANCE UART bridge (bd-wim1)
+#   saltybot-here4.service        Here4 GPS DroneCAN bridge (bd-p47c)
+#   saltybot-ros2.service         Full ROS2 stack (perception + nav)
+#   saltybot-dashboard.service    Web dashboard on port 8080
+#   saltybot-social.service       Social-bot stack (speech + LLM + face)
+#   tailscale-vpn.service         Tailscale VPN for remote access
+#
+# Prerequisites:
+#   apt install ros-humble-desktop python3-colcon-common-extensions
+#   pip install dronecan  (for Here4 GPS node)
+#   usermod -aG dialout orin  (for serial port access)
 set -euo pipefail
 
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 REPO_DIR="$(dirname "${SCRIPT_DIR}")"
 SYSTEMD_DIR="/etc/systemd/system"
 DEPLOY_DIR="/opt/saltybot/jetson"
+SCRIPTS_DIR="${DEPLOY_DIR}/scripts"
 UDEV_DIR="/etc/udev/rules.d"
 BRINGUP_SYSTEMD="${REPO_DIR}/ros2_ws/src/saltybot_bringup/systemd"
 BRINGUP_UDEV="${REPO_DIR}/ros2_ws/src/saltybot_bringup/udev"
+WS_SRC="${REPO_DIR}/ros2_ws/src"
+WS_BUILD="${DEPLOY_DIR}/ros2_ws"
 
-log() { echo "[install_systemd] $*"; }
+ROS_DISTRO="${ROS_DISTRO:-humble}"
+ROS_SETUP="/opt/ros/${ROS_DISTRO}/setup.bash"
+ORIN_USER="${SALTYBOT_USER:-orin}"
 
-[[ "$(id -u)" == "0" ]] || { echo "Run as root"; exit 1; }
+log()  { echo "[install_systemd] $*"; }
+warn() { echo "[install_systemd] WARN: $*" >&2; }
+die()  { echo "[install_systemd] ERROR: $*" >&2; exit 1; }
 
-# Deploy repo to /opt/saltybot/jetson
-log "Deploying to ${DEPLOY_DIR}..."
+[[ "$(id -u)" == "0" ]] || die "Run as root (sudo $0)"
+[[ -f "${ROS_SETUP}" ]] || die "ROS2 ${ROS_DISTRO} not found at ${ROS_SETUP}"
+
+# ── 1. Deploy repo ─────────────────────────────────────────────────────────────
+log "Deploying repo to ${DEPLOY_DIR}..."
 mkdir -p "${DEPLOY_DIR}"
-rsync -a --exclude='.git' --exclude='__pycache__' \
+rsync -a \
+    --exclude='.git' \
+    --exclude='__pycache__' \
+    --exclude='*.pyc' \
+    --exclude='.pytest_cache' \
+    --exclude='build/' \
+    --exclude='install/' \
+    --exclude='log/' \
     "${REPO_DIR}/" "${DEPLOY_DIR}/"
 
-# Install service files
-log "Installing systemd units..."
-cp "${SCRIPT_DIR}/saltybot.target"        "${SYSTEMD_DIR}/"
-cp "${SCRIPT_DIR}/saltybot-social.service" "${SYSTEMD_DIR}/"
-cp "${SCRIPT_DIR}/tailscale-vpn.service"  "${SYSTEMD_DIR}/"
-cp "${BRINGUP_SYSTEMD}/can-bringup.service" "${SYSTEMD_DIR}/"
+# Install launch wrapper script
+log "Installing ros2-launch.sh..."
+install -m 755 "${SCRIPT_DIR}/../scripts/ros2-launch.sh" "/opt/saltybot/scripts/ros2-launch.sh"
 
-# Install udev rules
+# ── 2. Build ROS2 workspace ────────────────────────────────────────────────────
+if command -v colcon &>/dev/null; then
+    log "Building ROS2 workspace (colcon)..."
+    # shellcheck disable=SC1090
+    source "${ROS_SETUP}"
+    (
+        cd "${WS_BUILD}"
+        colcon build \
+            --symlink-install \
+            --cmake-args -DCMAKE_BUILD_TYPE=Release \
+            2>&1 | tee /tmp/colcon-build.log
+    )
+    log "Workspace build complete."
+else
+    warn "colcon not found — skipping workspace build."
+    warn "Run manually: cd ${WS_BUILD} && colcon build --symlink-install"
+fi
+
+# ── 3. Install systemd units ───────────────────────────────────────────────────
+log "Installing systemd units..."
+
+# Units from jetson/systemd/
+for unit in \
+    saltybot.target \
+    saltybot-ros2.service \
+    saltybot-esp32-serial.service \
+    saltybot-here4.service \
+    saltybot-dashboard.service \
+    saltybot-social.service \
+    tailscale-vpn.service
+do
+    if [[ -f "${SCRIPT_DIR}/${unit}" ]]; then
+        cp "${SCRIPT_DIR}/${unit}" "${SYSTEMD_DIR}/"
+        log "  Installed ${unit}"
+    else
+        warn "  ${unit} not found in ${SCRIPT_DIR}/ — skipping"
+    fi
+done
+
+# Units from saltybot_bringup/systemd/
+for unit in \
+    can-bringup.service \
+    chromium-kiosk.service \
+    magedok-display.service \
+    salty-face-server.service
+do
+    if [[ -f "${BRINGUP_SYSTEMD}/${unit}" ]]; then
+        cp "${BRINGUP_SYSTEMD}/${unit}" "${SYSTEMD_DIR}/"
+        log "  Installed ${unit} (from bringup)"
+    else
+        warn "  ${unit} not found in bringup systemd/ — skipping"
+    fi
+done
+
+# ── 4. Install udev rules ──────────────────────────────────────────────────────
 log "Installing udev rules..."
 mkdir -p "${UDEV_DIR}"
-cp "${BRINGUP_UDEV}/70-canable.rules"    "${UDEV_DIR}/"
-cp "${BRINGUP_UDEV}/90-magedok-touch.rules" "${UDEV_DIR}/"
+
+for rule in \
+    70-canable.rules \
+    80-esp32.rules \
+    90-magedok-touch.rules
+do
+    if [[ -f "${BRINGUP_UDEV}/${rule}" ]]; then
+        cp "${BRINGUP_UDEV}/${rule}" "${UDEV_DIR}/"
+        log "  Installed ${rule}"
+    else
+        warn "  ${rule} not found — skipping"
+    fi
+done
+
 udevadm control --reload
-udevadm trigger --subsystem-match=net --action=add
+udevadm trigger --subsystem-match=net   --action=add
+udevadm trigger --subsystem-match=tty   --action=add
+log "udev rules reloaded."
 
-# Reload and enable
+# ── 5. Set permissions ─────────────────────────────────────────────────────────
+log "Ensuring '${ORIN_USER}' is in dialout group (serial port access)..."
+if id "${ORIN_USER}" &>/dev/null; then
+    usermod -aG dialout "${ORIN_USER}" || warn "usermod failed — add ${ORIN_USER} to dialout manually"
+else
+    warn "User '${ORIN_USER}' not found — skip usermod"
+fi
+
+# ── 6. Reload systemd and enable services ─────────────────────────────────────
+log "Reloading systemd daemon..."
 systemctl daemon-reload
-systemctl enable saltybot.target
-systemctl enable saltybot-social.service
-systemctl enable tailscale-vpn.service
-systemctl enable can-bringup.service
 
-log "Services installed. Start with:"
-log "  systemctl start saltybot-social"
-log "  systemctl start tailscale-vpn"
-log "  systemctl start can-bringup"
-log "  journalctl -fu saltybot-social"
-log "  journalctl -fu tailscale-vpn"
-log "  journalctl -fu can-bringup"
+log "Enabling services..."
+systemctl enable \
+    saltybot.target \
+    can-bringup.service \
+    saltybot-esp32-serial.service \
+    saltybot-here4.service \
+    saltybot-ros2.service \
+    saltybot-dashboard.service \
+    saltybot-social.service \
+    tailscale-vpn.service
+
+# Enable mosquitto (MQTT broker) if installed
+if systemctl list-unit-files mosquitto.service &>/dev/null; then
+    systemctl enable mosquitto.service
+    log "  Enabled mosquitto.service"
+fi
+
+# ── 7. Summary ────────────────────────────────────────────────────────────────
 log ""
-log "Verify CAN bus: candump can0"
-log "  VESC CAN IDs: 61 (0x3D) and 79 (0x4F)"
+log "Installation complete."
+log ""
+log "Services will start automatically on next reboot."
+log "To start now without rebooting:"
+log "  sudo systemctl start saltybot.target"
+log ""
+log "Check status:"
+log "  systemctl status can-bringup saltybot-esp32-serial saltybot-here4 saltybot-ros2 saltybot-dashboard"
+log ""
+log "Live logs:"
+log "  journalctl -fu can-bringup"
+log "  journalctl -fu saltybot-esp32-serial"
+log "  journalctl -fu saltybot-here4"
+log "  journalctl -fu saltybot-ros2"
+log "  journalctl -fu saltybot-dashboard"
+log ""
+log "Dashboard: http://<orin-ip>:8080"
+log "rosbridge: ws://<orin-ip>:9090"
+log ""
+log "Note: saltybot-esp32-serial and saltybot-here4 require packages"
+log "  from bd-wim1 (PR #727) and bd-p47c (PR #728) to be merged."

jetson/systemd/saltybot-dashboard.service

@@ -0,0 +1,38 @@
+[Unit]
+Description=SaltyBot Web Dashboard (saltybot_dashboard — port 8080)
+Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
+#
+# Serves the live monitoring dashboard on http://<orin-ip>:8080
+# Subscribes to ROS2 topics over the local DomainID (ROS_DOMAIN_ID=42).
+# Starts after the ROS2 stack but does not hard-depend on it —
+# the dashboard handles missing topics gracefully and will show data as
+# nodes come up.
+After=network-online.target saltybot-ros2.service
+Wants=saltybot-ros2.service
+
+[Service]
+Type=simple
+User=orin
+Group=orin
+
+Environment="ROS_DOMAIN_ID=42"
+Environment="ROS_DISTRO=humble"
+Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
+
+ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
+    saltybot_dashboard dashboard.launch.py
+
+Restart=on-failure
+RestartSec=5s
+StartLimitInterval=60s
+StartLimitBurst=5
+
+TimeoutStartSec=30s
+TimeoutStopSec=10s
+
+StandardOutput=journal
+StandardError=journal
+SyslogIdentifier=saltybot-dashboard
+
+[Install]
+WantedBy=saltybot.target

jetson/systemd/saltybot-esp32-serial.service

@@ -0,0 +1,54 @@
+[Unit]
+Description=SaltyBot ESP32-S3 BALANCE UART Serial Bridge (bd-wim1)
+Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/bd-wim1
+# Requires package: saltybot_esp32_serial (merged in bd-wim1 → PR #727)
+#
+# Publishes:
+#   /saltybot/attitude     String JSON  (pitch, motor_cmd, vbat_mv, state)
+#   /saltybot/balance_state String
+#   /can/battery           BatteryState
+#   /can/vesc/left/state   Float32MultiArray  [erpm, voltage_v, current_a, temp_c]
+#   /can/vesc/right/state  Float32MultiArray
+#   /can/connection_status String
+#
+# Subscribes:
+#   /cmd_vel               Twist   → DRIVE command to ESP32 BALANCE
+#   /estop                 Bool    → ESTOP command
+#   /saltybot/arm          Bool    → ARM command
+#   /saltybot/pid_update   String  → PID gains update
+After=network-online.target
+# Wait for /dev/esp32-balance (created by 80-esp32.rules udev rule).
+# TAG+="systemd" in the udev rule makes systemd track dev-esp32\x2dbalance.device.
+# If device is not present at boot the node's auto-reconnect loop handles it.
+Wants=network-online.target
+
+[Service]
+Type=simple
+User=orin
+Group=dialout
+
+# ROS2 Humble environment + workspace overlay
+Environment="ROS_DOMAIN_ID=42"
+Environment="ROS_DISTRO=humble"
+Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
+
+ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
+    saltybot_esp32_serial esp32_balance.launch.py
+
+# The node auto-reconnects on serial disconnect (2 s retry).
+# Restart=on-failure covers node crash; RestartSec gives the device time to re-enumerate.
+Restart=on-failure
+RestartSec=5s
+StartLimitInterval=60s
+StartLimitBurst=5
+
+TimeoutStartSec=30s
+TimeoutStopSec=10s
+
+# Logging
+StandardOutput=journal
+StandardError=journal
+SyslogIdentifier=saltybot-esp32-serial
+
+[Install]
+WantedBy=saltybot.target

jetson/systemd/saltybot-here4.service

@@ -0,0 +1,50 @@
+[Unit]
+Description=SaltyBot Here4 GPS DroneCAN Bridge (bd-p47c)
+Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/bd-p47c
+# Requires package: saltybot_dronecan_gps (merged in bd-p47c → PR #728)
+# Requires: can0 at 1 Mbps (set by can-bringup.service)
+#
+# Publishes:
+#   /gps/fix           NavSatFix               5 Hz   → navsat_transform EKF
+#   /gps/velocity      TwistWithCovarianceStamped 5 Hz
+#   /here4/fix         NavSatFix               5 Hz   (alias)
+#   /here4/imu         Imu                    50 Hz
+#   /here4/mag         MagneticField          10 Hz
+#   /here4/baro        FluidPressure          10 Hz
+#   /here4/status      String JSON             1 Hz   (node health, fix quality)
+#   /here4/node_id     Int32                  once   (auto-discovered DroneCAN ID)
+#
+# Subscribes:
+#   /rtcm              ByteMultiArray          on demand  (RTCM corrections)
+#   /rtcm_hex          String                  on demand  (hex fallback)
+After=network-online.target can-bringup.service
+Requires=can-bringup.service
+# If can-bringup stops (CANable2 unplugged), stop this service too
+BindsTo=can-bringup.service
+
+[Service]
+Type=simple
+User=orin
+Group=dialout
+
+Environment="ROS_DOMAIN_ID=42"
+Environment="ROS_DISTRO=humble"
+Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
+
+ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
+    saltybot_dronecan_gps here4.launch.py
+
+Restart=on-failure
+RestartSec=5s
+StartLimitInterval=60s
+StartLimitBurst=5
+
+TimeoutStartSec=30s
+TimeoutStopSec=10s
+
+StandardOutput=journal
+StandardError=journal
+SyslogIdentifier=saltybot-here4
+
+[Install]
+WantedBy=saltybot.target

jetson/systemd/saltybot-ros2.service

@@ -0,0 +1,59 @@
+[Unit]
+Description=SaltyBot ROS2 Full Stack (perception + navigation + SLAM)
+Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
+#
+# Launches: saltybot_bringup full_stack.launch.py (indoor mode by default)
+#   enable_bridge:=false  — ESP32-S3 serial bridge is managed by saltybot-esp32-serial.service
+#
+# Stack launched (t=0s → t=17s):
+#   t= 0s  robot_description  (URDF + TF)
+#   t= 2s  RPLIDAR A1M8 + RealSense D435i
+#   t= 3s  LIDAR obstacle avoidance
+#   t= 4s  4× IMX219 CSI cameras (optional)
+#   t= 4s  UWB driver
+#   t= 5s  Audio pipeline (wake word + STT + TTS)
+#   t= 6s  RTAB-Map SLAM (indoor) / GPS nav (outdoor)
+#   t= 6s  YOLOv8n person + object detection (TensorRT)
+#   t= 7s  Autonomous docking
+#   t= 9s  Person follower
+#   t=14s  Nav2 navigation stack
+#   t=17s  rosbridge WebSocket (ws://0.0.0.0:9090)
+#
+# Hard deps: can-bringup (Here4 GPS topics), esp32-serial (VESC telemetry)
+After=network-online.target can-bringup.service saltybot-esp32-serial.service saltybot-here4.service
+Wants=can-bringup.service saltybot-esp32-serial.service saltybot-here4.service
+
+[Service]
+Type=simple
+User=orin
+Group=orin
+
+Environment="ROS_DOMAIN_ID=42"
+Environment="ROS_DISTRO=humble"
+Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
+
+# enable_bridge:=false — ESP32 serial bridge runs as its own service (saltybot-esp32-serial)
+# mode can be overridden via environment: Environment="SALTYBOT_MODE=outdoor"
+ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
+    saltybot_bringup full_stack.launch.py \
+    enable_bridge:=false \
+    mode:=${SALTYBOT_MODE:-indoor}
+
+Restart=on-failure
+RestartSec=10s
+StartLimitInterval=120s
+StartLimitBurst=3
+
+TimeoutStartSec=120s
+TimeoutStopSec=30s
+
+# ROS2 launch spawns many child processes; kill them on stop
+KillMode=control-group
+
+# Logging
+StandardOutput=journal
+StandardError=journal
+SyslogIdentifier=saltybot-ros2
+
+[Install]
+WantedBy=saltybot.target

jetson/systemd/saltybot.target

@@ -1,8 +1,22 @@
 [Unit]
-Description=Saltybot Full Stack Target
+Description=SaltyBot Full Stack Target
 Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
-After=docker.service network-online.target
-Requires=docker.service
+#
+# Start everything:  systemctl start saltybot.target
+# Stop everything:   systemctl stop  saltybot.target
+# Check all:         systemctl status 'saltybot-*' can-bringup
+#
+# Boot order (dependency chain):
+#   network-online.target
+#     → can-bringup.service          (CANable2 @ 1 Mbps DroneCAN)
+#         → saltybot-here4.service   (Here4 GPS → /gps/fix, /here4/*)
+#     → saltybot-esp32-serial.service (ESP32-S3 UART → /can/vesc/*, /saltybot/attitude)
+#     → saltybot-ros2.service        (full_stack.launch.py, perception + nav)
+#         → saltybot-dashboard.service (port 8080)
+#   (independent) saltybot-social.service
+#   (independent) tailscale-vpn.service
+After=network-online.target
+Wants=network-online.target
 
 [Install]
 WantedBy=multi-user.target