Compare commits
No commits in common. "main" and "sl-firmware/ota-esp32" have entirely different histories.
main
...
sl-firmwar
@ -3,7 +3,6 @@ idf_component_register(
|
||||
"main.c"
|
||||
"orin_serial.c"
|
||||
"vesc_can.c"
|
||||
"gc9a01.c"
|
||||
"gitea_ota.c"
|
||||
"ota_self.c"
|
||||
"uart_ota.c"
|
||||
@ -16,7 +15,7 @@ idf_component_register(
|
||||
nvs_flash
|
||||
app_update
|
||||
mbedtls
|
||||
espressif__cjson
|
||||
cJSON
|
||||
driver
|
||||
freertos
|
||||
esp_timer
|
||||
|
||||
@ -1,44 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
/* ── ESP32-S3 BALANCE board — Waveshare ESP32-S3-Touch-LCD-1.28 pinout ─────
|
||||
/* ── ESP32-S3 BALANCE board — bd-66hx pin/config definitions ───────────────
|
||||
*
|
||||
* 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
|
||||
* 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)
|
||||
*
|
||||
* 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).
|
||||
* 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 UART0 TX → CH343 RXD */
|
||||
#define ORIN_UART_RX_GPIO 44 /* CH343 TXD → ESP32 UART0 RX */
|
||||
#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
|
||||
|
||||
/* ── 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 */
|
||||
/* ── 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 */
|
||||
#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
|
||||
/* 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 */
|
||||
@ -48,3 +36,7 @@
|
||||
|
||||
/* ── 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
|
||||
|
||||
@ -1,269 +0,0 @@
|
||||
/* 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);
|
||||
}
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
#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
|
||||
@ -1,5 +0,0 @@
|
||||
dependencies:
|
||||
idf:
|
||||
version: '>=5.0'
|
||||
espressif/cjson:
|
||||
version: "^1.7.19~2"
|
||||
@ -2,7 +2,6 @@
|
||||
|
||||
#include "orin_serial.h"
|
||||
#include "vesc_can.h"
|
||||
#include "gc9a01.h"
|
||||
#include "gitea_ota.h"
|
||||
#include "ota_self.h"
|
||||
#include "uart_ota.h"
|
||||
@ -64,14 +63,20 @@ 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 front_erpm = 0;
|
||||
int32_t left_erpm = 0;
|
||||
int32_t right_erpm = 0;
|
||||
|
||||
if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
|
||||
!hb_timeout && !drive_stale) {
|
||||
front_erpm = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
|
||||
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_can_send_rpm(VESC_ID_A, front_erpm);
|
||||
/* 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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -82,8 +87,7 @@ void app_main(void)
|
||||
/* OTA rollback health check — must be called within OTA_ROLLBACK_WINDOW_S */
|
||||
ota_self_health_check();
|
||||
|
||||
/* Init peripherals — gc9a01 before vesc_can so BL/GPIO2 is high before TWAI takes it */
|
||||
gc9a01_init();
|
||||
/* Init peripherals */
|
||||
orin_serial_init();
|
||||
vesc_can_init();
|
||||
|
||||
|
||||
@ -17,8 +17,9 @@
|
||||
static const char *TAG = "orin";
|
||||
|
||||
/* ── Shared state ── */
|
||||
orin_drive_t g_orin_drive = {0};
|
||||
orin_control_t g_orin_ctrl = {.armed = false, .estop = false, .hb_last_ms = 0};
|
||||
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)
|
||||
@ -187,6 +188,25 @@ 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();
|
||||
|
||||
@ -23,11 +23,12 @@
|
||||
#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 61 (front) telemetry @ 10 Hz */
|
||||
#define TELEM_VESC_RIGHT 0x82u /* VESC ID 79 (rear) telemetry @ 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
|
||||
@ -50,11 +51,12 @@
|
||||
#define ERR_OTA_BUSY 0x05u
|
||||
#define ERR_OTA_NO_UPDATE 0x06u
|
||||
|
||||
/* ── Drive state (mirrored from TELEM_STATUS.balance_state) ── */
|
||||
/* ── Balance state (mirrored from TELEM_STATUS.balance_state) ── */
|
||||
typedef enum {
|
||||
BAL_DISARMED = 0,
|
||||
BAL_ARMED = 1,
|
||||
BAL_ESTOP = 3,
|
||||
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 ── */
|
||||
@ -64,6 +66,11 @@ 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;
|
||||
@ -79,6 +86,7 @@ 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 ── */
|
||||
|
||||
@ -116,7 +116,7 @@ void ota_display_update(void)
|
||||
|
||||
if (bal_avail || io_avail) {
|
||||
/* Show available versions on display when idle */
|
||||
char verline[48];
|
||||
char verline[32];
|
||||
if (bal_avail) {
|
||||
snprintf(verline, sizeof(verline), "Bal v%s rdy",
|
||||
g_balance_update.version);
|
||||
|
||||
@ -17,7 +17,3 @@ 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
|
||||
|
||||
@ -97,7 +97,11 @@ services:
|
||||
rgb_camera.profile:=640x480x30
|
||||
"
|
||||
|
||||
# ── ESP32-S3 bridge node (bidirectional serial<->ROS2) ───────────────────────
|
||||
<<<<<<< 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-bridge:
|
||||
image: saltybot/ros2-humble:jetson-orin
|
||||
build:
|
||||
@ -208,8 +212,13 @@ services:
|
||||
"
|
||||
|
||||
|
||||
# -- Remote e-stop bridge (MQTT over 4G -> ESP32-S3 CDC) ---------------------
|
||||
<<<<<<< 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) ----------------------
|
||||
# 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
|
||||
@ -357,50 +366,6 @@ 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
|
||||
|
||||
@ -1,64 +0,0 @@
|
||||
# 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;
|
||||
}
|
||||
}
|
||||
@ -39,7 +39,11 @@ 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
|
||||
@ -57,8 +61,13 @@ 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)
|
||||
@ -71,10 +80,17 @@ 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}'
|
||||
|
||||
@ -91,7 +107,11 @@ 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
|
||||
"""
|
||||
@ -209,7 +229,11 @@ 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(
|
||||
@ -218,7 +242,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)",
|
||||
@ -270,7 +294,11 @@ def generate_launch_description():
|
||||
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 ───────────────────────────────────────────
|
||||
@ -290,7 +318,11 @@ def generate_launch_description():
|
||||
launch_arguments={"use_sim_time": use_sim_time}.items(),
|
||||
)
|
||||
|
||||
# ── t=0s ESP32-S3 bidirectional serial bridge ───────────────────────────────
|
||||
<<<<<<< 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)
|
||||
esp32_bridge = GroupAction(
|
||||
condition=IfCondition(LaunchConfiguration("enable_bridge")),
|
||||
actions=[
|
||||
@ -320,7 +352,11 @@ def generate_launch_description():
|
||||
],
|
||||
)
|
||||
|
||||
<<<<<<< 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=[
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
[Unit]
|
||||
Description=CANable 2.0 CAN bus bringup (can0, 1 Mbps DroneCAN — Here4 GPS)
|
||||
Description=CANable 2.0 CAN bus bringup (can0, 500 kbps)
|
||||
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 1000000
|
||||
ExecStart=/usr/sbin/ip link set can0 up type can bitrate 500000
|
||||
ExecStop=/usr/sbin/ip link set can0 down
|
||||
|
||||
StandardOutput=journal
|
||||
|
||||
@ -2,10 +2,6 @@
|
||||
# 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
|
||||
@ -20,4 +16,4 @@
|
||||
SUBSYSTEM=="net", ACTION=="add", \
|
||||
ATTRS{idVendor}=="1d50", ATTRS{idProduct}=="606f", \
|
||||
NAME="can0", \
|
||||
TAG+="systemd"
|
||||
RUN+="/sbin/ip link set can0 up type can bitrate 500000"
|
||||
|
||||
@ -1,31 +0,0 @@
|
||||
# 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"
|
||||
@ -1,9 +0,0 @@
|
||||
# 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
|
||||
@ -1,110 +0,0 @@
|
||||
"""
|
||||
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'),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
@ -1 +0,0 @@
|
||||
saltybot_dronecan_gps
|
||||
@ -1,204 +0,0 @@
|
||||
#!/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()
|
||||
@ -1,18 +0,0 @@
|
||||
# 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
|
||||
@ -1,32 +0,0 @@
|
||||
<?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>
|
||||
@ -1 +0,0 @@
|
||||
saltybot_ntrip_client
|
||||
@ -1,207 +0,0 @@
|
||||
#!/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()
|
||||
@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_ntrip_client
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_ntrip_client
|
||||
@ -1,32 +0,0 @@
|
||||
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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,29 +1,22 @@
|
||||
# outdoor_nav_params.yaml — Outdoor navigation configuration for SaltyBot
|
||||
#
|
||||
# Hardware: Jetson Orin Nano Super / SIM7600X cellular GPS (±2.5 m CEP)
|
||||
# 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)
|
||||
# RTK upgrade: u-blox ZED-F9P → ±2 cm CEP (set use_rtk: true when installed)
|
||||
#
|
||||
# ── GPS quality notes ────────────────────────────────────────────────────────
|
||||
# SIM7600X reports STATUS_FIX (0) in open sky, STATUS_NO_FIX (-1) indoors.
|
||||
# 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
|
||||
# RTK ZED-F9P reports STATUS_GBAS_FIX (2) when corrections received.
|
||||
# Goal tolerance automatically tightens from 2.0m (cellular) to 0.3m (RTK).
|
||||
#
|
||||
# ── 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
|
||||
# ── 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
|
||||
#
|
||||
# References:
|
||||
# Here4 manual: https://docs.cubepilot.org/user-guides/here-4/here-4-manual
|
||||
# SparkFun RTK Express: https://docs.sparkfun.com/SparkFun_RTK_Everywhere_Firmware/
|
||||
# NTRIP caster list: https://rtk2go.com/sample-map/
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
|
||||
@ -103,26 +96,3 @@ 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)
|
||||
|
||||
@ -1,36 +1,22 @@
|
||||
#!/usr/bin/env bash
|
||||
# can_setup.sh — Bring up CANable 2.0 (gs_usb) as can0
|
||||
# can_setup.sh — Bring up CANable 2.0 (gs_usb) as can0 at 500 kbps
|
||||
# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/643
|
||||
#
|
||||
# Usage:
|
||||
# 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)
|
||||
# sudo ./can_setup.sh # bring up
|
||||
# sudo ./can_setup.sh down # bring down
|
||||
# sudo ./can_setup.sh verify # candump one-shot check (Ctrl-C to stop)
|
||||
#
|
||||
# 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.
|
||||
# VESCs on bus: CAN ID 61 (0x3D) and CAN ID 79 (0x4F), 500 kbps
|
||||
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)
|
||||
@ -39,7 +25,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 (mode: ${mode})..."
|
||||
log "Bringing up $IFACE at ${BITRATE} bps..."
|
||||
ip link set "$IFACE" down 2>/dev/null || true
|
||||
ip link set "$IFACE" up type can bitrate "$BITRATE"
|
||||
ip link set "$IFACE" up
|
||||
@ -54,17 +40,13 @@ 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 "Listening on $IFACE — expecting frames from VESC IDs 0x3D (61) and 0x4F (79)"
|
||||
log "Press Ctrl-C to stop."
|
||||
exec candump "$IFACE"
|
||||
;;
|
||||
|
||||
*)
|
||||
echo "Usage: $0 [up [vesc|dronecan]|down|verify [dronecan]]"
|
||||
echo "Usage: $0 [up|down|verify]"
|
||||
exit 1
|
||||
;;
|
||||
esac
|
||||
|
||||
@ -1,38 +0,0 @@
|
||||
#!/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 "$@"
|
||||
@ -1,194 +1,55 @@
|
||||
#!/usr/bin/env bash
|
||||
# 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)
|
||||
# install_systemd.sh — Install saltybot systemd services on Orin
|
||||
# Run as root: sudo ./systemd/install_systemd.sh
|
||||
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"
|
||||
|
||||
ROS_DISTRO="${ROS_DISTRO:-humble}"
|
||||
ROS_SETUP="/opt/ros/${ROS_DISTRO}/setup.bash"
|
||||
ORIN_USER="${SALTYBOT_USER:-orin}"
|
||||
log() { echo "[install_systemd] $*"; }
|
||||
|
||||
log() { echo "[install_systemd] $*"; }
|
||||
warn() { echo "[install_systemd] WARN: $*" >&2; }
|
||||
die() { echo "[install_systemd] ERROR: $*" >&2; exit 1; }
|
||||
[[ "$(id -u)" == "0" ]] || { echo "Run as root"; exit 1; }
|
||||
|
||||
[[ "$(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}..."
|
||||
# Deploy repo to /opt/saltybot/jetson
|
||||
log "Deploying to ${DEPLOY_DIR}..."
|
||||
mkdir -p "${DEPLOY_DIR}"
|
||||
rsync -a \
|
||||
--exclude='.git' \
|
||||
--exclude='__pycache__' \
|
||||
--exclude='*.pyc' \
|
||||
--exclude='.pytest_cache' \
|
||||
--exclude='build/' \
|
||||
--exclude='install/' \
|
||||
--exclude='log/' \
|
||||
rsync -a --exclude='.git' --exclude='__pycache__' \
|
||||
"${REPO_DIR}/" "${DEPLOY_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"
|
||||
|
||||
# ── 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 ───────────────────────────────────────────────────
|
||||
# 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}/"
|
||||
|
||||
# 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 ──────────────────────────────────────────────────────
|
||||
# Install udev rules
|
||||
log "Installing udev rules..."
|
||||
mkdir -p "${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
|
||||
|
||||
cp "${BRINGUP_UDEV}/70-canable.rules" "${UDEV_DIR}/"
|
||||
cp "${BRINGUP_UDEV}/90-magedok-touch.rules" "${UDEV_DIR}/"
|
||||
udevadm control --reload
|
||||
udevadm trigger --subsystem-match=net --action=add
|
||||
udevadm trigger --subsystem-match=tty --action=add
|
||||
log "udev rules reloaded."
|
||||
udevadm trigger --subsystem-match=net --action=add
|
||||
|
||||
# ── 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..."
|
||||
# Reload and enable
|
||||
systemctl daemon-reload
|
||||
systemctl enable saltybot.target
|
||||
systemctl enable saltybot-social.service
|
||||
systemctl enable tailscale-vpn.service
|
||||
systemctl enable can-bringup.service
|
||||
|
||||
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 "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 "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 " 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."
|
||||
log "Verify CAN bus: candump can0"
|
||||
log " VESC CAN IDs: 61 (0x3D) and 79 (0x4F)"
|
||||
|
||||
@ -1,38 +0,0 @@
|
||||
[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
|
||||
@ -1,54 +0,0 @@
|
||||
[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
|
||||
@ -1,50 +0,0 @@
|
||||
[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
|
||||
@ -1,59 +0,0 @@
|
||||
[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
|
||||
@ -1,22 +1,8 @@
|
||||
[Unit]
|
||||
Description=SaltyBot Full Stack Target
|
||||
Description=Saltybot Full Stack Target
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
|
||||
#
|
||||
# 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
|
||||
After=docker.service network-online.target
|
||||
Requires=docker.service
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
|
||||
@ -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="" placeholder="ws://host:port" />
|
||||
<input id="ws-input" type="text" value="ws://100.64.0.2:9090" placeholder="ws://host:port" />
|
||||
<button class="hbtn" id="btn-connect">CONNECT</button>
|
||||
<span id="conn-label">Not connected</span>
|
||||
</div>
|
||||
@ -732,10 +732,7 @@ map.on('dragstart', () => {
|
||||
// ── Init ──────────────────────────────────────────────────────────────────────
|
||||
|
||||
(function init() {
|
||||
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;
|
||||
const saved = localStorage.getItem('saul_tee_ws_url') || 'ws://100.64.0.2:9090';
|
||||
$('ws-input').value = saved;
|
||||
drawCompass(null);
|
||||
connectRos(saved);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user