Merge remote-tracking branch 'origin/sl-firmware/rm-balance-safety-cutoffs'

# Conflicts:
#	esp32s3/balance/main/config.h

esp32s3/balance/main/CMakeLists.txt

@@ -3,6 +3,7 @@ idf_component_register(
         "main.c"
         "orin_serial.c"
         "vesc_can.c"
+        "gc9a01.c"
         "gitea_ota.c"
         "ota_self.c"
         "uart_ota.c"
@@ -15,7 +16,7 @@ idf_component_register(
         nvs_flash
         app_update
         mbedtls
-        cJSON
+        espressif__cjson
         driver
         freertos
         esp_timer

esp32s3/balance/main/config.h

@@ -1,32 +1,44 @@
 #pragma once
 
-/* ── ESP32-S3 BALANCE board — bd-66hx pin/config definitions ───────────────
+/* ── ESP32-S3 BALANCE board — Waveshare ESP32-S3-Touch-LCD-1.28 pinout ─────
  *
- * 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)
+ * Orin comms:  CH343 USB-to-serial on UART0 (GPIO43/44) → /dev/ttyACM0 on Orin
+ * VESC CAN:    SN65HVD230 transceiver on GPIO15 (TX) / GPIO16 (RX)
+ * Display:     GC9A01 on SPI2 — BL=GPIO40, RST=GPIO12
  *
- * 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.
+ * GPIO2 is NOT used for CAN — it is free. Earlier versions had an erroneous
+ * conflict between DISP_BL (GPIO2) and VESC_CAN_TX (GPIO2); corrected here
+ * to match motor-test-firmware verified hardware layout (commit 8e66430).
  */
 
 /* ── 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_TX_GPIO       43      /* ESP32 UART0 TX → CH343 RXD */
+#define ORIN_UART_RX_GPIO       44      /* CH343 TXD → ESP32 UART0 RX */
 #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 */
+/* ── Inter-board UART (ESP32 BALANCE ↔ ESP32 IO) ── */
+#define IO_UART_TX_GPIO         17
+#define IO_UART_RX_GPIO         18
+
+/* ── VESC CAN TWAI (SN65HVD230 on Waveshare header GPIO15/16) ── */
+#define VESC_CAN_TX_GPIO        15      /* ESP32 TWAI TX → SN65HVD230 TXD */
+#define VESC_CAN_RX_GPIO        16      /* 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) */
+/* VESC node IDs */
+#define VESC_ID_A               61u     /* FRONT VESC — drive + telemetry (0x81) */
+#define VESC_ID_B               79u     /* REAR  VESC — telemetry only    (0x82) */
+
+/* ── GC9A01 240×240 round display (Waveshare ESP32-S3-Touch-LCD-1.28, SPI2) ── */
+#define DISP_DC_GPIO     8
+#define DISP_CS_GPIO     9
+#define DISP_SCK_GPIO   10
+#define DISP_MOSI_GPIO  11
+#define DISP_RST_GPIO   12
+#define DISP_BL_GPIO    40
 
 /* ── Safety / timing ── */
 #define HB_TIMEOUT_MS           500u    /* heartbeat watchdog: disarm if exceeded */
@@ -36,5 +48,3 @@
 
 /* ── Drive → VESC RPM scaling ── */
 #define RPM_PER_SPEED_UNIT      5       /* speed_units=1000 → 5000 ERPM */
-#define RPM_PER_STEER_UNIT      3       /* steer differential scale */
-

esp32s3/balance/main/gc9a01.c

@@ -0,0 +1,269 @@
+/* gc9a01.c — GC9A01 240×240 round LCD SPI driver (bd-1yr8 display bead) */
+
+#include "gc9a01.h"
+#include "config.h"
+#include "driver/spi_master.h"
+#include "driver/gpio.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "esp_log.h"
+#include <string.h>
+#include <math.h>
+
+static const char *TAG = "gc9a01";
+static spi_device_handle_t s_spi;
+
+/* ── 5×7 bitmap font, one byte per column (bit0 = top row), ASCII 32..126 ── */
+static const uint8_t s_font[95][5] = {
+    {0x00,0x00,0x00,0x00,0x00}, /* ' ' */  {0x00,0x00,0x5F,0x00,0x00}, /* '!' */
+    {0x00,0x07,0x00,0x07,0x00}, /* '"' */  {0x14,0x7F,0x14,0x7F,0x14}, /* '#' */
+    {0x24,0x2A,0x7F,0x2A,0x12}, /* '$' */  {0x23,0x13,0x08,0x64,0x62}, /* '%' */
+    {0x36,0x49,0x55,0x22,0x50}, /* '&' */  {0x00,0x05,0x03,0x00,0x00}, /* '\'' */
+    {0x00,0x1C,0x22,0x41,0x00}, /* '(' */  {0x00,0x41,0x22,0x1C,0x00}, /* ')' */
+    {0x14,0x08,0x3E,0x08,0x14}, /* '*' */  {0x08,0x08,0x3E,0x08,0x08}, /* '+' */
+    {0x00,0x50,0x30,0x00,0x00}, /* ',' */  {0x08,0x08,0x08,0x08,0x08}, /* '-' */
+    {0x00,0x60,0x60,0x00,0x00}, /* '.' */  {0x20,0x10,0x08,0x04,0x02}, /* '/' */
+    {0x3E,0x51,0x49,0x45,0x3E}, /* '0' */  {0x00,0x42,0x7F,0x40,0x00}, /* '1' */
+    {0x42,0x61,0x51,0x49,0x46}, /* '2' */  {0x21,0x41,0x45,0x4B,0x31}, /* '3' */
+    {0x18,0x14,0x12,0x7F,0x10}, /* '4' */  {0x27,0x45,0x45,0x45,0x39}, /* '5' */
+    {0x3C,0x4A,0x49,0x49,0x30}, /* '6' */  {0x01,0x71,0x09,0x05,0x03}, /* '7' */
+    {0x36,0x49,0x49,0x49,0x36}, /* '8' */  {0x06,0x49,0x49,0x29,0x1E}, /* '9' */
+    {0x00,0x36,0x36,0x00,0x00}, /* ':' */  {0x00,0x56,0x36,0x00,0x00}, /* ';' */
+    {0x08,0x14,0x22,0x41,0x00}, /* '<' */  {0x14,0x14,0x14,0x14,0x14}, /* '=' */
+    {0x00,0x41,0x22,0x14,0x08}, /* '>' */  {0x02,0x01,0x51,0x09,0x06}, /* '?' */
+    {0x32,0x49,0x79,0x41,0x3E}, /* '@' */  {0x7E,0x11,0x11,0x11,0x7E}, /* 'A' */
+    {0x7F,0x49,0x49,0x49,0x36}, /* 'B' */  {0x3E,0x41,0x41,0x41,0x22}, /* 'C' */
+    {0x7F,0x41,0x41,0x22,0x1C}, /* 'D' */  {0x7F,0x49,0x49,0x49,0x41}, /* 'E' */
+    {0x7F,0x09,0x09,0x09,0x01}, /* 'F' */  {0x3E,0x41,0x49,0x49,0x3A}, /* 'G' */
+    {0x7F,0x08,0x08,0x08,0x7F}, /* 'H' */  {0x00,0x41,0x7F,0x41,0x00}, /* 'I' */
+    {0x20,0x40,0x41,0x3F,0x01}, /* 'J' */  {0x7F,0x08,0x14,0x22,0x41}, /* 'K' */
+    {0x7F,0x40,0x40,0x40,0x40}, /* 'L' */  {0x7F,0x02,0x0C,0x02,0x7F}, /* 'M' */
+    {0x7F,0x04,0x08,0x10,0x7F}, /* 'N' */  {0x3E,0x41,0x41,0x41,0x3E}, /* 'O' */
+    {0x7F,0x09,0x09,0x09,0x06}, /* 'P' */  {0x3E,0x41,0x51,0x21,0x5E}, /* 'Q' */
+    {0x7F,0x09,0x19,0x29,0x46}, /* 'R' */  {0x46,0x49,0x49,0x49,0x31}, /* 'S' */
+    {0x01,0x01,0x7F,0x01,0x01}, /* 'T' */  {0x3F,0x40,0x40,0x40,0x3F}, /* 'U' */
+    {0x1F,0x20,0x40,0x20,0x1F}, /* 'V' */  {0x3F,0x40,0x38,0x40,0x3F}, /* 'W' */
+    {0x63,0x14,0x08,0x14,0x63}, /* 'X' */  {0x07,0x08,0x70,0x08,0x07}, /* 'Y' */
+    {0x61,0x51,0x49,0x45,0x43}, /* 'Z' */  {0x00,0x7F,0x41,0x41,0x00}, /* '[' */
+    {0x02,0x04,0x08,0x10,0x20}, /* '\\' */ {0x00,0x41,0x41,0x7F,0x00}, /* ']' */
+    {0x04,0x02,0x01,0x02,0x04}, /* '^' */  {0x40,0x40,0x40,0x40,0x40}, /* '_' */
+    {0x00,0x01,0x02,0x04,0x00}, /* '`' */  {0x20,0x54,0x54,0x54,0x78}, /* 'a' */
+    {0x7F,0x48,0x44,0x44,0x38}, /* 'b' */  {0x38,0x44,0x44,0x44,0x20}, /* 'c' */
+    {0x38,0x44,0x44,0x48,0x7F}, /* 'd' */  {0x38,0x54,0x54,0x54,0x18}, /* 'e' */
+    {0x08,0x7E,0x09,0x01,0x02}, /* 'f' */  {0x0C,0x52,0x52,0x52,0x3E}, /* 'g' */
+    {0x7F,0x08,0x04,0x04,0x78}, /* 'h' */  {0x00,0x44,0x7D,0x40,0x00}, /* 'i' */
+    {0x20,0x40,0x44,0x3D,0x00}, /* 'j' */  {0x7F,0x10,0x28,0x44,0x00}, /* 'k' */
+    {0x00,0x41,0x7F,0x40,0x00}, /* 'l' */  {0x7C,0x04,0x18,0x04,0x78}, /* 'm' */
+    {0x7C,0x08,0x04,0x04,0x78}, /* 'n' */  {0x38,0x44,0x44,0x44,0x38}, /* 'o' */
+    {0x7C,0x14,0x14,0x14,0x08}, /* 'p' */  {0x08,0x14,0x14,0x18,0x7C}, /* 'q' */
+    {0x7C,0x08,0x04,0x04,0x08}, /* 'r' */  {0x48,0x54,0x54,0x54,0x20}, /* 's' */
+    {0x04,0x3F,0x44,0x40,0x20}, /* 't' */  {0x3C,0x40,0x40,0x20,0x7C}, /* 'u' */
+    {0x1C,0x20,0x40,0x20,0x1C}, /* 'v' */  {0x3C,0x40,0x30,0x40,0x3C}, /* 'w' */
+    {0x44,0x28,0x10,0x28,0x44}, /* 'x' */  {0x0C,0x50,0x50,0x50,0x3C}, /* 'y' */
+    {0x44,0x64,0x54,0x4C,0x44}, /* 'z' */  {0x00,0x08,0x36,0x41,0x00}, /* '{' */
+    {0x00,0x00,0x7F,0x00,0x00}, /* '|' */  {0x00,0x41,0x36,0x08,0x00}, /* '}' */
+    {0x10,0x08,0x08,0x10,0x08},             /* '~' */
+};
+
+/* ── Static buffers (internal SRAM → DMA-safe) ── */
+static uint8_t s_line_buf[240 * 2];
+static uint8_t s_char_buf[5 * 5 * 7 * 5 * 2]; /* max scale=5: 25×35×2 */
+
+/* ── Low-level SPI helpers ── */
+static void write_cmd(uint8_t cmd)
+{
+    gpio_set_level(DISP_DC_GPIO, 0);
+    spi_transaction_t t = { .length = 8, .flags = SPI_TRANS_USE_TXDATA };
+    t.tx_data[0] = cmd;
+    spi_device_polling_transmit(s_spi, &t);
+}
+
+static void write_bytes(const uint8_t *data, size_t len)
+{
+    if (!len) return;
+    gpio_set_level(DISP_DC_GPIO, 1);
+    spi_transaction_t t = { .length = len * 8, .tx_buffer = data };
+    spi_device_polling_transmit(s_spi, &t);
+}
+
+static inline void write_byte(uint8_t b)  { write_bytes(&b, 1); }
+
+/* ── Address window ── */
+static void set_window(int x1, int y1, int x2, int y2)
+{
+    uint8_t d[4];
+    d[0] = x1 >> 8; d[1] = x1 & 0xFF; d[2] = x2 >> 8; d[3] = x2 & 0xFF;
+    write_cmd(0x2A); write_bytes(d, 4);
+    d[0] = y1 >> 8; d[1] = y1 & 0xFF; d[2] = y2 >> 8; d[3] = y2 & 0xFF;
+    write_cmd(0x2B); write_bytes(d, 4);
+}
+
+/* ── GC9A01 register init ── */
+static void init_regs(void)
+{
+    write_cmd(0xEF);
+    write_cmd(0xEB); write_byte(0x14);
+    write_cmd(0xFE);
+    write_cmd(0xEF);
+    write_cmd(0xEB); write_byte(0x14);
+    write_cmd(0x84); write_byte(0x40);
+    write_cmd(0x85); write_byte(0xFF);
+    write_cmd(0x86); write_byte(0xFF);
+    write_cmd(0x87); write_byte(0xFF);
+    write_cmd(0x88); write_byte(0x0A);
+    write_cmd(0x89); write_byte(0x21);
+    write_cmd(0x8A); write_byte(0x00);
+    write_cmd(0x8B); write_byte(0x80);
+    write_cmd(0x8C); write_byte(0x01);
+    write_cmd(0x8D); write_byte(0x01);
+    write_cmd(0x8E); write_byte(0xFF);
+    write_cmd(0x8F); write_byte(0xFF);
+    { uint8_t d[] = {0x00,0x20}; write_cmd(0xB6); write_bytes(d,2); }
+    write_cmd(0x36); write_byte(0x08);   /* MADCTL: normal, BGR */
+    write_cmd(0x3A); write_byte(0x05);   /* COLMOD: 16-bit RGB565 */
+    { uint8_t d[] = {0x08,0x08,0x08,0x08}; write_cmd(0x90); write_bytes(d,4); }
+    write_cmd(0xBD); write_byte(0x06);
+    write_cmd(0xBC); write_byte(0x00);
+    { uint8_t d[] = {0x60,0x01,0x04}; write_cmd(0xFF); write_bytes(d,3); }
+    write_cmd(0xC3); write_byte(0x13);
+    write_cmd(0xC4); write_byte(0x13);
+    write_cmd(0xC9); write_byte(0x22);
+    write_cmd(0xBE); write_byte(0x11);
+    { uint8_t d[] = {0x10,0x0E}; write_cmd(0xE1); write_bytes(d,2); }
+    { uint8_t d[] = {0x21,0x0C,0x02}; write_cmd(0xDF); write_bytes(d,3); }
+    { uint8_t d[] = {0x45,0x09,0x08,0x08,0x26,0x2A}; write_cmd(0xF0); write_bytes(d,6); }
+    { uint8_t d[] = {0x43,0x70,0x72,0x36,0x37,0x6F}; write_cmd(0xF1); write_bytes(d,6); }
+    { uint8_t d[] = {0x45,0x09,0x08,0x08,0x26,0x2A}; write_cmd(0xF2); write_bytes(d,6); }
+    { uint8_t d[] = {0x43,0x70,0x72,0x36,0x37,0x6F}; write_cmd(0xF3); write_bytes(d,6); }
+    { uint8_t d[] = {0x1B,0x0B}; write_cmd(0xED); write_bytes(d,2); }
+    write_cmd(0xAE); write_byte(0x77);
+    write_cmd(0xCD); write_byte(0x63);
+    { uint8_t d[] = {0x07,0x07,0x04,0x0E,0x0F,0x09,0x07,0x08,0x03};
+      write_cmd(0x70); write_bytes(d,9); }
+    write_cmd(0xE8); write_byte(0x34);
+    { uint8_t d[] = {0x18,0x0D,0xB7,0x18,0x0D,0x8B,0x88,0x08};
+      write_cmd(0x62); write_bytes(d,8); }
+    { uint8_t d[] = {0x18,0x0D,0xB7,0x58,0x1E,0x0B,0x00,0xA7,0x88,0x08};
+      write_cmd(0x63); write_bytes(d,10); }
+    { uint8_t d[] = {0x20,0x07,0x04}; write_cmd(0x64); write_bytes(d,3); }
+    { uint8_t d[] = {0x10,0x85,0x80,0x00,0x00,0x4E,0x00};
+      write_cmd(0x74); write_bytes(d,7); }
+    { uint8_t d[] = {0x3E,0x07}; write_cmd(0x98); write_bytes(d,2); }
+    write_cmd(0x35);           /* TEON */
+    write_cmd(0x21);           /* INVON */
+    write_cmd(0x11);           /* SLPOUT */
+    vTaskDelay(pdMS_TO_TICKS(120));
+    write_cmd(0x29);           /* DISPON */
+    vTaskDelay(pdMS_TO_TICKS(20));
+}
+
+/* ── Public init ── */
+void gc9a01_init(void)
+{
+    /* SPI bus */
+    spi_bus_config_t bus = {
+        .mosi_io_num     = DISP_MOSI_GPIO,
+        .miso_io_num     = -1,
+        .sclk_io_num     = DISP_SCK_GPIO,
+        .quadwp_io_num   = -1,
+        .quadhd_io_num   = -1,
+        .max_transfer_sz = 4096,
+    };
+    ESP_ERROR_CHECK(spi_bus_initialize(SPI2_HOST, &bus, SPI_DMA_CH_AUTO));
+
+    spi_device_interface_config_t dev = {
+        .clock_speed_hz = 40 * 1000 * 1000,
+        .mode           = 0,
+        .spics_io_num   = DISP_CS_GPIO,
+        .queue_size     = 1,
+    };
+    ESP_ERROR_CHECK(spi_bus_add_device(SPI2_HOST, &dev, &s_spi));
+
+    /* DC, RST, BL GPIOs */
+    gpio_set_direction(DISP_DC_GPIO,  GPIO_MODE_OUTPUT);
+    gpio_set_direction(DISP_RST_GPIO, GPIO_MODE_OUTPUT);
+    gpio_set_direction(DISP_BL_GPIO,  GPIO_MODE_OUTPUT);
+
+    /* Hardware reset */
+    gpio_set_level(DISP_RST_GPIO, 0); vTaskDelay(pdMS_TO_TICKS(10));
+    gpio_set_level(DISP_RST_GPIO, 1); vTaskDelay(pdMS_TO_TICKS(120));
+
+    init_regs();
+
+    /* Backlight on */
+    gpio_set_level(DISP_BL_GPIO, 1);
+    ESP_LOGI(TAG, "GC9A01 init OK: DC=%d CS=%d SCK=%d MOSI=%d RST=%d BL=%d",
+             DISP_DC_GPIO, DISP_CS_GPIO, DISP_SCK_GPIO, DISP_MOSI_GPIO,
+             DISP_RST_GPIO, DISP_BL_GPIO);
+}
+
+/* ── display_fill_rect ── */
+void display_fill_rect(int x, int y, int w, int h, uint16_t rgb565)
+{
+    if (w <= 0 || h <= 0) return;
+    if (x < 0) { w += x; x = 0; }
+    if (y < 0) { h += y; y = 0; }
+    if (x + w > 240) w = 240 - x;
+    if (y + h > 240) h = 240 - y;
+    if (w <= 0 || h <= 0) return;
+
+    set_window(x, y, x + w - 1, y + h - 1);
+    write_cmd(0x2C);
+
+    uint8_t hi = rgb565 >> 8, lo = rgb565 & 0xFF;
+    for (int i = 0; i < w * 2; i += 2) { s_line_buf[i] = hi; s_line_buf[i+1] = lo; }
+    for (int row = 0; row < h; row++) { write_bytes(s_line_buf, (size_t)(w * 2)); }
+}
+
+/* ── Glyph rasteriser (handles scale 1..5) ── */
+static void draw_char_s(int x, int y, char c, uint16_t fg, uint16_t bg, int scale)
+{
+    if ((uint8_t)c < 32 || (uint8_t)c > 126) return;
+    if (scale < 1) scale = 1;
+    if (scale > 5) scale = 5;
+    const uint8_t *g = s_font[(uint8_t)c - 32];
+    int cw = 5 * scale, ch = 7 * scale;
+
+    uint8_t *p = s_char_buf;
+    for (int row = 0; row < 7; row++) {
+        for (int sr = 0; sr < scale; sr++) {
+            for (int col = 0; col < 5; col++) {
+                uint16_t color = ((g[col] >> row) & 1) ? fg : bg;
+                uint8_t hi = color >> 8, lo = color & 0xFF;
+                for (int sc = 0; sc < scale; sc++) { *p++ = hi; *p++ = lo; }
+            }
+        }
+    }
+    set_window(x, y, x + cw - 1, y + ch - 1);
+    write_cmd(0x2C);
+    write_bytes(s_char_buf, (size_t)(cw * ch * 2));
+}
+
+/* ── display_draw_string / display_draw_string_s ── */
+void display_draw_string(int x, int y, const char *str, uint16_t fg, uint16_t bg)
+{
+    display_draw_string_s(x, y, str, fg, bg, 1);
+}
+
+void display_draw_string_s(int x, int y, const char *str,
+                           uint16_t fg, uint16_t bg, int scale)
+{
+    int cx = x;
+    while (*str) {
+        draw_char_s(cx, y, *str++, fg, bg, scale);
+        cx += 6 * scale;
+    }
+}
+
+/* ── display_draw_arc ── */
+void display_draw_arc(int cx, int cy, int r, int start_deg, int end_deg,
+                      int thickness, uint16_t color)
+{
+    for (int deg = start_deg; deg <= end_deg; deg++) {
+        float rad = (float)deg * (3.14159265f / 180.0f);
+        int px = cx + (int)((float)r * cosf(rad));
+        int py = cy + (int)((float)r * sinf(rad));
+        int half = thickness / 2;
+        display_fill_rect(px - half, py - half, thickness, thickness, color);
+    }
+}

esp32s3/balance/main/gc9a01.h

@@ -0,0 +1,24 @@
+#pragma once
+/* gc9a01.h — GC9A01 240×240 round LCD SPI driver (bd-1yr8 display bead) */
+
+#include <stdint.h>
+
+/* ── Initialise SPI bus + GC9A01. Call once from app_main. ── */
+void gc9a01_init(void);
+
+/* ── Display primitives (also satisfy ota_display.h contract) ── */
+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_string_s(int x, int y, const char *str,
+                           uint16_t fg, uint16_t bg, int scale);
+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
+#define COL_WHITE  0xFFFFu
+#define COL_GREEN  0x07E0u
+#define COL_YELLOW 0xFFE0u
+#define COL_RED    0xF800u
+#define COL_BLUE   0x001Fu
+#define COL_ORANGE 0xFD20u

esp32s3/balance/main/idf_component.yml

@@ -0,0 +1,5 @@
+dependencies:
+  idf:
+    version: '>=5.0'
+  espressif/cjson:
+    version: "^1.7.19~2"

esp32s3/balance/main/main.c

@@ -2,6 +2,7 @@
 
 #include "orin_serial.h"
 #include "vesc_can.h"
+#include "gc9a01.h"
 #include "gitea_ota.h"
 #include "ota_self.h"
 #include "uart_ota.h"
@@ -63,20 +64,14 @@ static void drive_task(void *arg)
         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;
+        int32_t front_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;
+            front_erpm = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
         }
 
-        /* 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);
+        vesc_can_send_rpm(VESC_ID_A, front_erpm);
     }
 }
 
@@ -87,7 +82,8 @@ void app_main(void)
     /* OTA rollback health check — must be called within OTA_ROLLBACK_WINDOW_S */
     ota_self_health_check();
 
-    /* Init peripherals */
+    /* Init peripherals — gc9a01 before vesc_can so BL/GPIO2 is high before TWAI takes it */
+    gc9a01_init();
     orin_serial_init();
     vesc_can_init();
 

esp32s3/balance/main/orin_serial.c

@@ -18,7 +18,6 @@ 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) ── */
@@ -188,25 +187,6 @@ static void dispatch_cmd(uint8_t type, const uint8_t *payload, uint8_t len,
         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();

esp32s3/balance/main/orin_serial.h

@@ -23,12 +23,11 @@
 #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_VESC_LEFT     0x81u   /* VESC ID 61 (front) telemetry @ 10 Hz */
+#define TELEM_VESC_RIGHT    0x82u   /* VESC ID 79 (rear)  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
@@ -51,7 +50,7 @@
 #define ERR_OTA_BUSY        0x05u
 #define ERR_OTA_NO_UPDATE   0x06u
 
-/* ── Balance state (mirrored from TELEM_STATUS.balance_state) ── */
+/* ── Drive state (mirrored from TELEM_STATUS.balance_state) ── */
 typedef enum {
     BAL_DISARMED = 0,
     BAL_ARMED    = 1,
@@ -65,11 +64,6 @@ typedef struct {
     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;
@@ -85,7 +79,6 @@ typedef struct {
 
 /* ── 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 ── */

esp32s3/balance/main/ota_display.c

@@ -116,7 +116,7 @@ void ota_display_update(void)
 
     if (bal_avail || io_avail) {
         /* Show available versions on display when idle */
-        char verline[32];
+        char verline[48];
         if (bal_avail) {
             snprintf(verline, sizeof(verline), "Bal v%s rdy",
                      g_balance_update.version);

esp32s3/balance/sdkconfig.defaults

@@ -17,3 +17,7 @@ CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE=y
 CONFIG_OTA_ALLOW_HTTP=y
 CONFIG_ESP_HTTPS_OTA_ALLOW_HTTP=y
 CONFIG_MBEDTLS_CERTIFICATE_BUNDLE=y
+
+# bd-66hx hardware: disable brownout detection — power supply dips during SPI/init
+# The gc9a01 display SPI init causes ~50mA transient that trips level-0 brownout
+CONFIG_ESP_BROWNOUT_DET=n

jetson/docker-compose.yml

@@ -97,11 +97,7 @@ services:
           rgb_camera.profile:=640x480x30
       "
 
-<<<<<<< HEAD
-  # ── ESP32 bridge node (bidirectional serial<->ROS2) ────────────────────────
-=======
-  # ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ────────────────────────
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
+  # ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ───────────────────────
   esp32-bridge:
     image: saltybot/ros2-humble:jetson-orin
     build:
@@ -212,13 +208,8 @@ services:
       "
 
 
-<<<<<<< HEAD
-  # -- Remote e-stop bridge (MQTT over 4G -> ESP32 CDC) ----------------------
-  # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32 BALANCE.
-=======
-  # -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ----------------------
+  # -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ---------------------
   # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to ESP32-S3.
->>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
   # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT).
   remote-estop:
     image: saltybot/ros2-humble:jetson-orin
@@ -366,6 +357,50 @@ services:
       "
 
 
+  # ── Here4 DroneCAN GPS + NTRIP RTK client ────────────────────────────────
+  # Issue #725 — CubePilot Here4 RTK GPS via DroneCAN (1Mbps, SocketCAN)
+  # Start: docker compose up -d here4-gps
+  # Monitor fix: docker compose exec here4-gps ros2 topic echo /gps/rtk_status
+  # Configure NTRIP: set NTRIP_MOUNT, NTRIP_USER, NTRIP_PASSWORD env vars
+  here4-gps:
+    image: saltybot/ros2-humble:jetson-orin
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: saltybot-here4-gps
+    restart: unless-stopped
+    runtime: nvidia
+    network_mode: host
+    depends_on:
+      - saltybot-nav2
+    environment:
+      - ROS_DOMAIN_ID=42
+      - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+      # NTRIP credentials — set these in your .env or override at runtime
+      - NTRIP_MOUNT=${NTRIP_MOUNT:-}
+      - NTRIP_USER=${NTRIP_USER:-}
+      - NTRIP_PASSWORD=${NTRIP_PASSWORD:-}
+    volumes:
+      - ./ros2_ws/src:/ros2_ws/src:rw
+      - ./config:/config:ro
+    devices:
+      - /dev/can0:/dev/can0
+    cap_add:
+      - NET_ADMIN    # needed for SocketCAN ip link set can0 up inside container
+    command: >
+      bash -c "
+        source /opt/ros/humble/setup.bash &&
+        source /ros2_ws/install/local_setup.bash 2>/dev/null || true &&
+        pip install python-dronecan --quiet 2>/dev/null || true &&
+        ros2 launch saltybot_dronecan_gps here4_gps.launch.py
+          can_interface:=can0
+          can_bitrate:=1000000
+          ntrip_caster:=rtk2go.com
+          ntrip_mount:=${NTRIP_MOUNT:-}
+          ntrip_user:=${NTRIP_USER:-}
+          ntrip_password:=${NTRIP_PASSWORD:-}
+      "
+
 volumes:
   saltybot-maps:
     driver: local

jetson/nginx/saul-tee.conf

@@ -0,0 +1,64 @@
+# saul-tee.conf — nginx site for saul-t-mote.evthings.app reverse proxy
+#
+# Replaces: python3 -m http.server 8080 --directory /home/seb
+# Router (ASUS Merlin / OpenResty) terminates TLS on :443 and proxies to Orin:8080
+#
+# Deploy:
+#   sudo cp saul-tee.conf /etc/nginx/sites-available/saul-tee
+#   sudo ln -sf /etc/nginx/sites-available/saul-tee /etc/nginx/sites-enabled/saul-tee
+#   sudo systemctl reload nginx
+#
+# ROUTER REQUIREMENT for WSS to work:
+#   The ASUS Merlin router's nginx/OpenResty must proxy /rosbridge with
+#   WebSocket headers.  Add to /jffs/configs/nginx.conf.add:
+#
+#     map $http_upgrade $connection_upgrade {
+#         default upgrade;
+#         ''      close;
+#     }
+#
+#   And in the server block that proxies to 192.168.86.158:8080, add:
+#
+#     location /rosbridge {
+#         proxy_pass         http://192.168.86.158:8080/rosbridge;
+#         proxy_http_version 1.1;
+#         proxy_set_header   Upgrade    $http_upgrade;
+#         proxy_set_header   Connection $connection_upgrade;
+#         proxy_read_timeout 86400;
+#     }
+
+# Infer WebSocket upgrade from Connection header.
+# Handles the case where an upstream proxy (e.g. ASUS Merlin OpenResty)
+# passes Connection: upgrade but strips the Upgrade: websocket header.
+map $http_connection $ws_upgrade {
+    "upgrade" "websocket";
+    default   "";
+}
+
+server {
+    listen 8080 default_server;
+    listen [::]:8080 default_server;
+
+    root /home/seb;
+    index index.html;
+    server_name _;
+
+    # Static files — replaces python3 -m http.server 8080 --directory /home/seb
+    location / {
+        try_files $uri $uri/ =404;
+        autoindex on;
+    }
+
+    # rosbridge WebSocket reverse proxy
+    # wss://saul-t-mote.evthings.app/rosbridge  -->  ws://127.0.0.1:9090
+    location /rosbridge {
+        proxy_pass         http://127.0.0.1:9090/;
+        proxy_http_version 1.1;
+        proxy_set_header   Upgrade    $ws_upgrade;
+        proxy_set_header   Connection "upgrade";
+        proxy_set_header   Host       $host;
+        proxy_set_header   X-Real-IP  $remote_addr;
+        proxy_read_timeout 86400;
+        proxy_send_timeout 86400;
+    }
+}

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

@@ -0,0 +1,9 @@
+# dronecan_gps_params.yaml — CubePilot Here4 DroneCAN GPS driver defaults
+# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
+
+dronecan_gps:
+  ros__parameters:
+    can_interface:    "can0"
+    can_bitrate:      1000000   # Here4 default: 1Mbps DroneCAN
+    node_id:          127       # DroneCAN local node ID (GPS driver)
+    publish_compass:  true      # publish MagneticFieldStrength if available

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

@@ -0,0 +1,110 @@
+"""
+here4_gps.launch.py — CubePilot Here4 RTK GPS full stack
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
+
+Launches:
+  - dronecan_gps_node   (saltybot_dronecan_gps)
+  - ntrip_client_node   (saltybot_ntrip_client)
+
+Usage (minimal):
+  ros2 launch saltybot_dronecan_gps here4_gps.launch.py \\
+    ntrip_mount:=RTCM3_GENERIC ntrip_user:=you@email.com
+
+Full options:
+  ros2 launch saltybot_dronecan_gps here4_gps.launch.py \\
+    can_interface:=can0 \\
+    can_bitrate:=1000000 \\
+    ntrip_caster:=rtk2go.com \\
+    ntrip_port:=2101 \\
+    ntrip_mount:=MYBASE \\
+    ntrip_user:=you@email.com \\
+    ntrip_password:=secret
+"""
+
+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:
+    gps_cfg = os.path.join(
+        get_package_share_directory('saltybot_dronecan_gps'),
+        'config', 'dronecan_gps_params.yaml',
+    )
+    ntrip_cfg = os.path.join(
+        get_package_share_directory('saltybot_ntrip_client'),
+        'config', 'ntrip_params.yaml',
+    )
+
+    return LaunchDescription([
+        # ── Shared CAN args ───────────────────────────────────────────────────
+        DeclareLaunchArgument(
+            'can_interface', default_value='can0',
+            description='SocketCAN interface name',
+        ),
+        DeclareLaunchArgument(
+            'can_bitrate', default_value='1000000',
+            description='CAN bus bitrate — Here4 default is 1000000 (1 Mbps)',
+        ),
+
+        # ── NTRIP args ────────────────────────────────────────────────────────
+        DeclareLaunchArgument(
+            'ntrip_caster', default_value='rtk2go.com',
+            description='NTRIP caster hostname',
+        ),
+        DeclareLaunchArgument(
+            'ntrip_port', default_value='2101',
+            description='NTRIP caster port',
+        ),
+        DeclareLaunchArgument(
+            'ntrip_mount', default_value='',
+            description='NTRIP mount point (REQUIRED)',
+        ),
+        DeclareLaunchArgument(
+            'ntrip_user', default_value='',
+            description='NTRIP username (rtk2go.com requires email address)',
+        ),
+        DeclareLaunchArgument(
+            'ntrip_password', default_value='',
+            description='NTRIP password',
+        ),
+
+        # ── DroneCAN GPS node ─────────────────────────────────────────────────
+        Node(
+            package='saltybot_dronecan_gps',
+            executable='dronecan_gps_node',
+            name='dronecan_gps',
+            output='screen',
+            parameters=[
+                gps_cfg,
+                {
+                    'can_interface': LaunchConfiguration('can_interface'),
+                    'can_bitrate':   LaunchConfiguration('can_bitrate'),
+                },
+            ],
+        ),
+
+        # ── NTRIP client node ─────────────────────────────────────────────────
+        Node(
+            package='saltybot_ntrip_client',
+            executable='ntrip_client_node',
+            name='ntrip_client',
+            output='screen',
+            parameters=[
+                ntrip_cfg,
+                {
+                    'can_interface':  LaunchConfiguration('can_interface'),
+                    'can_bitrate':    LaunchConfiguration('can_bitrate'),
+                    'ntrip_caster':   LaunchConfiguration('ntrip_caster'),
+                    'ntrip_port':     LaunchConfiguration('ntrip_port'),
+                    'ntrip_mount':    LaunchConfiguration('ntrip_mount'),
+                    'ntrip_user':     LaunchConfiguration('ntrip_user'),
+                    'ntrip_password': LaunchConfiguration('ntrip_password'),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_dronecan_gps/resource/saltybot_dronecan_gps

@@ -0,0 +1 @@
+saltybot_dronecan_gps

jetson/ros2_ws/src/saltybot_dronecan_gps/saltybot_dronecan_gps/dronecan_gps_node.py

@@ -0,0 +1,204 @@
+#!/usr/bin/env python3
+"""
+dronecan_gps_node.py — DroneCAN GPS driver for CubePilot Here4 RTK
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
+
+Subscribes to:
+  uavcan.equipment.gnss.Fix2            (msg ID 1063) — position + fix status
+  uavcan.equipment.ahrs.MagneticFieldStrength          — compass (optional)
+
+Publishes:
+  /gps/fix        sensor_msgs/NavSatFix
+  /gps/vel        geometry_msgs/TwistStamped
+  /gps/rtk_status std_msgs/String
+
+DroneCAN fix_type → sensor_msgs status mapping:
+  0 = NO_FIX     → STATUS_NO_FIX  (-1)
+  1 = TIME_ONLY  → STATUS_NO_FIX  (-1)
+  2 = 2D_FIX    → STATUS_FIX      (0)
+  3 = 3D_FIX    → STATUS_FIX      (0)
+  4 = DGPS       → STATUS_SBAS_FIX (1)
+  5 = RTK_FLOAT  → STATUS_GBAS_FIX (2)
+  6 = RTK_FIXED  → STATUS_GBAS_FIX (2)
+"""
+
+import math
+import threading
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from sensor_msgs.msg import NavSatFix, NavSatStatus
+from geometry_msgs.msg import TwistStamped
+from std_msgs.msg import String
+
+try:
+    import dronecan
+except ImportError:
+    dronecan = None
+
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+
+# DroneCAN fix_type → (NavSatStatus.status, rtk_label)
+_FIX_MAP = {
+    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'),
+    4: (NavSatStatus.STATUS_SBAS_FIX, 'DGPS'),
+    5: (NavSatStatus.STATUS_GBAS_FIX, 'RTK_FLOAT'),
+    6: (NavSatStatus.STATUS_GBAS_FIX, 'RTK_FIXED'),
+}
+
+
+class DroneCanGpsNode(Node):
+    def __init__(self) -> None:
+        super().__init__('dronecan_gps')
+
+        self.declare_parameter('can_interface', 'can0')
+        self.declare_parameter('can_bitrate', 1000000)
+        self.declare_parameter('node_id', 127)          # DroneCAN local node ID
+        self.declare_parameter('publish_compass', True)
+
+        self._iface = self.get_parameter('can_interface').value
+        self._bitrate = self.get_parameter('can_bitrate').value
+        self._node_id = self.get_parameter('node_id').value
+        self._publish_compass = self.get_parameter('publish_compass').value
+
+        self._fix_pub = self.create_publisher(NavSatFix, '/gps/fix', _SENSOR_QOS)
+        self._vel_pub = self.create_publisher(TwistStamped, '/gps/vel', _SENSOR_QOS)
+        self._rtk_pub = self.create_publisher(String, '/gps/rtk_status', 10)
+
+        if dronecan is None:
+            self.get_logger().error(
+                'python-dronecan not installed. '
+                'Run: pip install python-dronecan'
+            )
+            return
+
+        self._lock = threading.Lock()
+        self._dc_node = None
+        self._spin_thread = threading.Thread(
+            target=self._dronecan_spin, daemon=True
+        )
+        self._spin_thread.start()
+        self.get_logger().info(
+            f'DroneCanGpsNode started — interface={self._iface} '
+            f'bitrate={self._bitrate}'
+        )
+
+    # ── DroneCAN spin (runs in background thread) ─────────────────────────────
+
+    def _dronecan_spin(self) -> None:
+        try:
+            self._dc_node = dronecan.make_node(
+                self._iface,
+                node_id=self._node_id,
+                bitrate=self._bitrate,
+            )
+            self._dc_node.add_handler(
+                dronecan.uavcan.equipment.gnss.Fix2,
+                self._on_fix2,
+            )
+            if self._publish_compass:
+                self._dc_node.add_handler(
+                    dronecan.uavcan.equipment.ahrs.MagneticFieldStrength,
+                    self._on_mag,
+                )
+            self.get_logger().info(
+                f'DroneCAN node online on {self._iface}'
+            )
+            while rclpy.ok():
+                self._dc_node.spin(timeout=0.1)
+        except Exception as exc:
+            self.get_logger().error(f'DroneCAN spin error: {exc}')
+
+    # ── Message handlers ──────────────────────────────────────────────────────
+
+    def _on_fix2(self, event) -> None:
+        msg = event.message
+        now = self.get_clock().now().to_msg()
+
+        fix_type = int(msg.fix_type)
+        nav_status, rtk_label = _FIX_MAP.get(
+            fix_type, (NavSatStatus.STATUS_NO_FIX, 'UNKNOWN')
+        )
+
+        # NavSatFix
+        fix = NavSatFix()
+        fix.header.stamp = now
+        fix.header.frame_id = 'gps'
+        fix.status.status = nav_status
+        fix.status.service = NavSatStatus.SERVICE_GPS
+
+        fix.latitude = math.degrees(msg.latitude_deg_1e8 * 1e-8)
+        fix.longitude = math.degrees(msg.longitude_deg_1e8 * 1e-8)
+        fix.altitude = msg.height_msl_mm * 1e-3  # mm → m
+
+        # Covariance from position_covariance if available, else diagonal guess
+        if hasattr(msg, 'position_covariance') and len(msg.position_covariance) >= 9:
+            fix.position_covariance = list(msg.position_covariance)
+            fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_FULL
+        else:
+            h_var = (msg.horizontal_pos_accuracy_m_1e2 * 1e-2) ** 2 \
+                if hasattr(msg, 'horizontal_pos_accuracy_m_1e2') else 4.0
+            v_var = (msg.vertical_pos_accuracy_m_1e2 * 1e-2) ** 2 \
+                if hasattr(msg, 'vertical_pos_accuracy_m_1e2') else 4.0
+            fix.position_covariance = [
+                h_var, 0.0,   0.0,
+                0.0,   h_var, 0.0,
+                0.0,   0.0,   v_var,
+            ]
+            fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
+
+        self._fix_pub.publish(fix)
+
+        # TwistStamped velocity
+        if hasattr(msg, 'ned_velocity'):
+            vel = TwistStamped()
+            vel.header.stamp = now
+            vel.header.frame_id = 'gps'
+            vel.twist.linear.x = float(msg.ned_velocity[0])  # North m/s
+            vel.twist.linear.y = float(msg.ned_velocity[1])  # East  m/s
+            vel.twist.linear.z = float(msg.ned_velocity[2])  # Down  m/s (ROS: up+)
+            self._vel_pub.publish(vel)
+
+        # RTK status string
+        rtk_msg = String()
+        rtk_msg.data = rtk_label
+        self._rtk_pub.publish(rtk_msg)
+
+        self.get_logger().debug(
+            f'Fix2: {fix.latitude:.6f},{fix.longitude:.6f} '
+            f'alt={fix.altitude:.1f}m status={rtk_label}'
+        )
+
+    def _on_mag(self, event) -> None:
+        # Compass data logged; extend to publish /imu/mag if needed
+        msg = event.message
+        self.get_logger().debug(
+            f'Mag: {msg.magnetic_field_ga[0]:.3f} '
+            f'{msg.magnetic_field_ga[1]:.3f} '
+            f'{msg.magnetic_field_ga[2]:.3f} Ga'
+        )
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = DroneCanGpsNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_ntrip_client/config/ntrip_params.yaml

@@ -0,0 +1,18 @@
+# ntrip_params.yaml — NTRIP client configuration for Here4 RTK corrections
+# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
+#
+# Override at launch:
+#   ros2 launch saltybot_dronecan_gps here4_gps.launch.py \
+#     ntrip_mount:=MYBASE ntrip_user:=user ntrip_password:=pass
+
+ntrip_client:
+  ros__parameters:
+    ntrip_caster:     "rtk2go.com"
+    ntrip_port:       2101
+    ntrip_mount:      ""           # REQUIRED — set your mount point
+    ntrip_user:       ""           # empty = anonymous (rtk2go requires email)
+    ntrip_password:   ""
+    can_interface:    "can0"
+    can_bitrate:      1000000
+    can_node_id:      126          # DroneCAN local node ID (NTRIP node)
+    reconnect_delay:  5.0          # seconds between reconnect attempts

jetson/ros2_ws/src/saltybot_ntrip_client/package.xml

@@ -0,0 +1,32 @@
+<?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_ntrip_client</name>
+  <version>0.1.0</version>
+  <description>
+    NTRIP client for RTK corrections.
+    Connects to an NTRIP caster (default: rtk2go.com:2101), receives RTCM3
+    correction data, and forwards it to the Here4 via DroneCAN
+    (uavcan.equipment.gnss.RTCMStream) on the CAN bus.
+    Reconnects automatically on disconnect.
+    Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
+  </description>
+  <maintainer email="seb@vayrette.com">Sebastien Vayrette</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend>python3-pip</exec_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_ntrip_client/resource/saltybot_ntrip_client

@@ -0,0 +1 @@
+saltybot_ntrip_client

jetson/ros2_ws/src/saltybot_ntrip_client/saltybot_ntrip_client/ntrip_client_node.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python3
+"""
+ntrip_client_node.py — NTRIP client for Here4 RTK corrections
+Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/725
+
+Connects to an NTRIP caster (default: rtk2go.com:2101), streams RTCM3
+correction data, and forwards it to the Here4 GPS via DroneCAN
+(uavcan.equipment.gnss.RTCMStream) on the CAN bus.
+
+Publishes:
+  /ntrip/status  std_msgs/String  — CONNECTED / DISCONNECTED / ERROR:<reason>
+
+Parameters:
+  ntrip_caster    (str)   rtk2go.com
+  ntrip_port      (int)   2101
+  ntrip_mount     (str)   ''       — required, e.g. 'RTCM3_GENERIC'
+  ntrip_user      (str)   ''       — leave empty for anonymous casters
+  ntrip_password  (str)   ''
+  can_interface   (str)   can0
+  can_bitrate     (int)   1000000
+  can_node_id     (int)   126      — DroneCAN local node ID for NTRIP node
+  reconnect_delay (float) 5.0     — seconds between reconnect attempts
+"""
+
+import base64
+import socket
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+try:
+    import dronecan
+except ImportError:
+    dronecan = None
+
+_NTRIP_AGENT  = 'saltybot-ntrip/1.0'
+_RTCM_CHUNK   = 512   # bytes per DroneCAN RTCMStream message (max 128 per frame)
+_RECV_BUF     = 4096
+
+
+class NtripClientNode(Node):
+    def __init__(self) -> None:
+        super().__init__('ntrip_client')
+
+        self.declare_parameter('ntrip_caster',    'rtk2go.com')
+        self.declare_parameter('ntrip_port',      2101)
+        self.declare_parameter('ntrip_mount',     '')
+        self.declare_parameter('ntrip_user',      '')
+        self.declare_parameter('ntrip_password',  '')
+        self.declare_parameter('can_interface',   'can0')
+        self.declare_parameter('can_bitrate',     1000000)
+        self.declare_parameter('can_node_id',     126)
+        self.declare_parameter('reconnect_delay', 5.0)
+
+        self._caster   = self.get_parameter('ntrip_caster').value
+        self._port     = self.get_parameter('ntrip_port').value
+        self._mount    = self.get_parameter('ntrip_mount').value
+        self._user     = self.get_parameter('ntrip_user').value
+        self._password = self.get_parameter('ntrip_password').value
+        self._iface    = self.get_parameter('can_interface').value
+        self._bitrate  = self.get_parameter('can_bitrate').value
+        self._node_id  = self.get_parameter('can_node_id').value
+        self._reconnect_delay = self.get_parameter('reconnect_delay').value
+
+        self._status_pub = self.create_publisher(String, '/ntrip/status', 10)
+
+        if dronecan is None:
+            self.get_logger().error(
+                'python-dronecan not installed. Run: pip install python-dronecan'
+            )
+            self._publish_status('ERROR:dronecan_not_installed')
+            return
+
+        if not self._mount:
+            self.get_logger().error(
+                'ntrip_mount parameter is required (e.g. RTCM3_GENERIC)'
+            )
+            self._publish_status('ERROR:no_mount_point')
+            return
+
+        self._dc_node = dronecan.make_node(
+            self._iface,
+            node_id=self._node_id,
+            bitrate=self._bitrate,
+        )
+
+        self._stop_event = threading.Event()
+        self._thread = threading.Thread(
+            target=self._ntrip_loop, daemon=True
+        )
+        self._thread.start()
+
+        self.get_logger().info(
+            f'NTRIP client started — '
+            f'{self._caster}:{self._port}/{self._mount}'
+        )
+
+    # ── NTRIP loop (reconnects on any error) ──────────────────────────────────
+
+    def _ntrip_loop(self) -> None:
+        while not self._stop_event.is_set() and rclpy.ok():
+            try:
+                self._connect_and_stream()
+            except Exception as exc:
+                self.get_logger().warn(f'NTRIP disconnected: {exc}')
+                self._publish_status(f'DISCONNECTED')
+            if not self._stop_event.is_set() and rclpy.ok():
+                self.get_logger().info(
+                    f'Reconnecting in {self._reconnect_delay}s…'
+                )
+                time.sleep(self._reconnect_delay)
+
+    def _connect_and_stream(self) -> None:
+        sock = socket.create_connection(
+            (self._caster, self._port), timeout=10.0
+        )
+        sock.settimeout(30.0)
+
+        # Send NTRIP HTTP/1.1 GET request
+        request = self._build_request()
+        sock.sendall(request.encode('ascii'))
+
+        # Read response header
+        header = b''
+        while b'\r\n\r\n' not in header:
+            chunk = sock.recv(256)
+            if not chunk:
+                raise ConnectionError('Connection closed during header read')
+            header += chunk
+
+        header_str = header.split(b'\r\n\r\n')[0].decode('ascii', errors='replace')
+        if 'ICY 200 OK' not in header_str and '200 OK' not in header_str:
+            raise ConnectionError(f'NTRIP rejected: {header_str[:120]}')
+
+        self.get_logger().info(
+            f'NTRIP connected to {self._caster}:{self._port}/{self._mount}'
+        )
+        self._publish_status('CONNECTED')
+
+        # Any trailing bytes after header are RTCM data
+        leftover = header.split(b'\r\n\r\n', 1)[1] if b'\r\n\r\n' in header else b''
+        if leftover:
+            self._forward_rtcm(leftover)
+
+        while not self._stop_event.is_set() and rclpy.ok():
+            data = sock.recv(_RECV_BUF)
+            if not data:
+                raise ConnectionError('NTRIP stream closed by server')
+            self._forward_rtcm(data)
+
+    def _build_request(self) -> str:
+        lines = [
+            f'GET /{self._mount} HTTP/1.1',
+            f'Host: {self._caster}:{self._port}',
+            f'User-Agent: {_NTRIP_AGENT}',
+            'Ntrip-Version: Ntrip/2.0',
+            'Connection: close',
+        ]
+        if self._user:
+            creds = base64.b64encode(
+                f'{self._user}:{self._password}'.encode()
+            ).decode()
+            lines.append(f'Authorization: Basic {creds}')
+        lines += ['', '']
+        return '\r\n'.join(lines)
+
+    # ── DroneCAN RTCMStream forwarding ────────────────────────────────────────
+
+    def _forward_rtcm(self, data: bytes) -> None:
+        """Chunk RTCM data into DroneCAN RTCMStream messages."""
+        for i in range(0, len(data), _RTCM_CHUNK):
+            chunk = data[i:i + _RTCM_CHUNK]
+            try:
+                msg = dronecan.uavcan.equipment.gnss.RTCMStream()
+                msg.data = list(chunk)
+                self._dc_node.broadcast(msg)
+                self._dc_node.spin(timeout=0.0)
+            except Exception as exc:
+                self.get_logger().warn(f'DroneCAN send error: {exc}')
+
+    def _publish_status(self, status: str) -> None:
+        msg = String()
+        msg.data = status
+        self._status_pub.publish(msg)
+
+    def destroy_node(self) -> None:
+        self._stop_event.set()
+        super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = NtripClientNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_ntrip_client/setup.cfg

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

jetson/ros2_ws/src/saltybot_ntrip_client/setup.py

@@ -0,0 +1,32 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = 'saltybot_ntrip_client'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'),
+            glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'),
+            glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Sebastien Vayrette',
+    maintainer_email='seb@vayrette.com',
+    description='NTRIP client — RTCM3 corrections via DroneCAN to Here4',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'ntrip_client_node = saltybot_ntrip_client.ntrip_client_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_outdoor/config/outdoor_nav_params.yaml

@@ -1,22 +1,29 @@
 # outdoor_nav_params.yaml — Outdoor navigation configuration for SaltyBot
 #
 # Hardware: Jetson Orin Nano Super / SIM7600X cellular GPS (±2.5 m CEP)
-# RTK upgrade: u-blox ZED-F9P → ±2 cm CEP (set use_rtk: true when installed)
+# RTK (active): CubePilot Here4 → ±2 cm CEP via DroneCAN (Issue #725)
+#   - Connected via CANable 2.0 (SocketCAN can0) at 1 Mbps
+#   - RTK corrections from NTRIP caster via saltybot_ntrip_client
+#   - Launch: docker compose up -d here4-gps
+#             (set NTRIP_MOUNT / NTRIP_USER / NTRIP_PASSWORD env vars)
 #
 # ── GPS quality notes ────────────────────────────────────────────────────────
 # SIM7600X reports STATUS_FIX (0) in open sky, STATUS_NO_FIX (-1) indoors.
-# RTK ZED-F9P reports STATUS_GBAS_FIX (2) when corrections received.
+# Here4 DroneCAN fix_type mapping (saltybot_dronecan_gps):
+#   5 = RTK_FLOAT → STATUS_GBAS_FIX (2) — sub-metre
+#   6 = RTK_FIXED → STATUS_GBAS_FIX (2) — ±2 cm (best)
+# Monitor: ros2 topic echo /gps/rtk_status
 # Goal tolerance automatically tightens from 2.0m (cellular) to 0.3m (RTK).
 #
-# ── RTK upgrade procedure ────────────────────────────────────────────────────
-# 1. Connect ZED-F9P to /dev/ttyTHS0 (Orin 40-pin UART, pins 8/10)
-# 2. Set NTRIP credentials in rtk_gps.launch.py
-# 3. Run: ros2 launch saltybot_outdoor rtk_gps.launch.py
-# 4. Verify: ros2 topic echo /gps/fix | grep status
-#    → status.status == 2 (STATUS_GBAS_FIX) = RTK fixed
+# ── Here4 setup procedure ───────────────────────────────────────────────────
+# 1. can_setup.sh up dronecan         # bring up can0 at 1Mbps
+# 2. Set NTRIP_MOUNT, NTRIP_USER, NTRIP_PASSWORD in .env
+# 3. docker compose up -d here4-gps
+# 4. Verify: ros2 topic echo /gps/rtk_status  → RTK_FIXED
+# 5. saltybot-outdoor: set use_rtk:=true in docker-compose.yml
 #
 # References:
-#   SparkFun RTK Express: https://docs.sparkfun.com/SparkFun_RTK_Everywhere_Firmware/
+#   Here4 manual: https://docs.cubepilot.org/user-guides/here-4/here-4-manual
 #   NTRIP caster list: https://rtk2go.com/sample-map/
 # ─────────────────────────────────────────────────────────────────────────────
 
@@ -96,3 +103,26 @@ global_costmap:
         inflation_radius: 0.50
       obstacle_layer:
         observation_sources: scan surround_cameras
+
+# ── Here4 RTK GPS configuration (Issue #725) ────────────────────────────────
+# Active when here4-gps docker service is running.
+# The dronecan_gps node publishes on the same /gps/fix topic as SIM7600X,
+# so gps_waypoint_follower picks up RTK automatically.
+# Set use_rtk:=true in saltybot-outdoor docker command to tighten tolerances.
+here4_gps:
+  ros__parameters:
+    # CAN bus — must match can_setup.sh dronecan mode and docker-compose device
+    can_interface:    "can0"
+    can_bitrate:      1000000   # 1 Mbps — Here4 DroneCAN default
+    # DroneCAN node IDs (must be unique on bus; VESCs use 61 and 79)
+    gps_node_id:      127
+    ntrip_node_id:    126
+    # NTRIP — override via env vars NTRIP_MOUNT / NTRIP_USER / NTRIP_PASSWORD
+    ntrip_caster:     "rtk2go.com"
+    ntrip_port:       2101
+    ntrip_mount:      ""        # set your mount point
+    ntrip_user:       ""        # rtk2go.com requires a valid email address
+    ntrip_password:   ""
+    reconnect_delay:  5.0       # seconds between NTRIP reconnect attempts
+    # RTK goal tolerance — applied by gps_waypoint_follower when use_rtk:=true
+    goal_tolerance_xy_rtk: 0.3  # metres (vs 2.0m cellular)

jetson/scripts/can_setup.sh

@@ -1,22 +1,36 @@
 #!/usr/bin/env bash
-# can_setup.sh — Bring up CANable 2.0 (gs_usb) as can0 at 500 kbps
+# can_setup.sh — Bring up CANable 2.0 (gs_usb) as can0
 # Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/643
 #
 # Usage:
-#   sudo ./can_setup.sh          # bring up
-#   sudo ./can_setup.sh down     # bring down
+#   sudo ./can_setup.sh               # bring up (default: 500kbps for VESC)
+#   sudo ./can_setup.sh up dronecan   # bring up at 1Mbps for Here4 DroneCAN
+#   sudo ./can_setup.sh down
 #   sudo ./can_setup.sh verify        # candump one-shot check (Ctrl-C to stop)
 #
-# VESCs on bus: CAN ID 61 (0x3D) and CAN ID 79 (0x4F), 500 kbps
+# Bitrate modes:
+#   default / vesc     — 500 kbps  (VESC IDs 0x3D/0x4F)
+#   dronecan / here4   — 1000 kbps (CubePilot Here4 RTK GPS, DroneCAN default)
+#
+# CAN_BITRATE env var overrides both modes.
 set -euo pipefail
 
 IFACE="${CAN_IFACE:-can0}"
-BITRATE="${CAN_BITRATE:-500000}"
 
 log() { echo "[can_setup] $*"; }
 die() { echo "[can_setup] ERROR: $*" >&2; exit 1; }
 
 cmd="${1:-up}"
+mode="${2:-vesc}"
+
+# Resolve bitrate: env var wins, then mode keyword, then 500k default
+if [[ -n "${CAN_BITRATE:-}" ]]; then
+    BITRATE="$CAN_BITRATE"
+elif [[ "$mode" == "dronecan" || "$mode" == "here4" ]]; then
+    BITRATE=1000000
+else
+    BITRATE=500000
+fi
 
 case "$cmd" in
   up)
@@ -25,7 +39,7 @@ case "$cmd" in
         die "$IFACE not found — is CANable 2.0 plugged in and gs_usb loaded?"
     fi
 
-    log "Bringing up $IFACE at ${BITRATE} bps..."
+    log "Bringing up $IFACE at ${BITRATE} bps (mode: ${mode})..."
     ip link set "$IFACE" down 2>/dev/null || true
     ip link set "$IFACE" up type can bitrate "$BITRATE"
     ip link set "$IFACE" up
@@ -40,13 +54,17 @@ case "$cmd" in
     ;;
 
   verify)
+    if [[ "$mode" == "dronecan" || "$mode" == "here4" ]]; then
+        log "Listening on $IFACE — expecting DroneCAN frames from Here4 GPS (1Mbps)"
+    else
         log "Listening on $IFACE — expecting frames from VESC IDs 0x3D (61) and 0x4F (79)"
+    fi
     log "Press Ctrl-C to stop."
     exec candump "$IFACE"
     ;;
 
   *)
-    echo "Usage: $0 [up|down|verify]"
+    echo "Usage: $0 [up [vesc|dronecan]|down|verify [dronecan]]"
     exit 1
     ;;
 esac

ui/saul-tee-tracker.html

@@ -227,7 +227,7 @@ body {
   <div class="logo">⚡ SAUL-TEE TRACKER</div>
   <div id="conn-bar">
     <div id="ros-dot" title="ROS Bridge"></div>
-    <input id="ws-input" type="text" value="ws://100.64.0.2:9090" placeholder="ws://host:port" />
+    <input id="ws-input" type="text" value="" placeholder="ws://host:port" />
     <button class="hbtn" id="btn-connect">CONNECT</button>
     <span id="conn-label">Not connected</span>
   </div>
@@ -732,7 +732,10 @@ map.on('dragstart', () => {
 // ── Init ──────────────────────────────────────────────────────────────────────
 
 (function init() {
-  const saved = localStorage.getItem('saul_tee_ws_url') || 'ws://100.64.0.2:9090';
+  const defaultUrl = location.protocol === 'https:'
+    ? 'wss://saul-t-mote.evthings.app/rosbridge'
+    : 'ws://100.64.0.2:9090';
+  const saved = localStorage.getItem('saul_tee_ws_url') || defaultUrl;
   $('ws-input').value = saved;
   drawCompass(null);
   connectRos(saved);