Compare commits
8 Commits
sl-firmwar
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
a6e7c4a550 | ||
| ac2e9d00d6 | |||
| d1e3a3cbd1 | |||
| cd84ee82fa | |||
| 98494a98c7 | |||
| b353a2ba29 | |||
| 329797d43c | |||
| 26e71d7a14 |
@ -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
|
||||
|
||||
@ -28,6 +28,14 @@
|
||||
#define VESC_ID_A 56u /* TELEM_VESC_LEFT (0x81) */
|
||||
#define VESC_ID_B 68u /* TELEM_VESC_RIGHT (0x82) */
|
||||
|
||||
/* ── GC9A01 240×240 round display (Waveshare ESP32-S3-Touch-LCD-1.28) ── */
|
||||
#define DISP_DC_GPIO 8
|
||||
#define DISP_CS_GPIO 9
|
||||
#define DISP_SCK_GPIO 10
|
||||
#define DISP_MOSI_GPIO 11
|
||||
#define DISP_RST_GPIO 14
|
||||
#define DISP_BL_GPIO 2
|
||||
|
||||
/* ── Safety / timing ── */
|
||||
#define HB_TIMEOUT_MS 500u /* heartbeat watchdog: disarm if exceeded */
|
||||
#define DRIVE_TIMEOUT_MS 500u /* drive command staleness timeout */
|
||||
|
||||
269
esp32s3/balance/main/gc9a01.c
Normal file
269
esp32s3/balance/main/gc9a01.c
Normal file
@ -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);
|
||||
}
|
||||
}
|
||||
24
esp32s3/balance/main/gc9a01.h
Normal file
24
esp32s3/balance/main/gc9a01.h
Normal file
@ -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
|
||||
5
esp32s3/balance/main/idf_component.yml
Normal file
5
esp32s3/balance/main/idf_component.yml
Normal file
@ -0,0 +1,5 @@
|
||||
dependencies:
|
||||
idf:
|
||||
version: '>=5.0'
|
||||
espressif/cjson:
|
||||
version: "^1.7.19~2"
|
||||
@ -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"
|
||||
@ -87,7 +88,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();
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -39,11 +39,7 @@ Modes
|
||||
─ UWB driver (2-anchor DW3000, publishes /uwb/target)
|
||||
─ YOLOv8n person detection (TensorRT)
|
||||
─ Person follower with UWB+camera fusion
|
||||
<<<<<<< HEAD
|
||||
─ cmd_vel bridge → ESP32 BALANCE (deadman + ramp + AUTONOMOUS gate)
|
||||
=======
|
||||
─ cmd_vel bridge → ESP32-S3 (deadman + ramp + AUTONOMOUS gate)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
─ rosbridge WebSocket (port 9090)
|
||||
|
||||
outdoor
|
||||
@ -61,13 +57,8 @@ Modes
|
||||
Launch sequence (wall-clock delays — conservative for cold start)
|
||||
─────────────────────────────────────────────────────────────────
|
||||
t= 0s robot_description (URDF + TF tree)
|
||||
<<<<<<< HEAD
|
||||
t= 0s ESP32 bridge (serial port owner — must be first)
|
||||
t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32 bridge up)
|
||||
=======
|
||||
t= 0s ESP32-S3 bridge (serial port owner — must be first)
|
||||
t= 2s cmd_vel bridge (consumes /cmd_vel, needs ESP32-S3 bridge up)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
t= 2s sensors (RPLIDAR + RealSense)
|
||||
t= 4s UWB driver (independent serial device)
|
||||
t= 4s CSI cameras (optional, independent)
|
||||
@ -80,17 +71,10 @@ Launch sequence (wall-clock delays — conservative for cold start)
|
||||
|
||||
Safety wiring
|
||||
─────────────
|
||||
<<<<<<< HEAD
|
||||
• ESP32 bridge must be up before cmd_vel bridge sends any command.
|
||||
• cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
|
||||
• ESP32 BALANCE AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
|
||||
until ESP32 BALANCE firmware is in AUTONOMOUS mode regardless of /cmd_vel.
|
||||
=======
|
||||
• ESP32-S3 bridge must be up before cmd_vel bridge sends any command.
|
||||
• cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent.
|
||||
• ESP32-S3 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still
|
||||
until ESP32-S3 firmware is in AUTONOMOUS mode regardless of /cmd_vel.
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
• follow_enabled:=false disables person follower without stopping the node.
|
||||
• To e-stop at runtime: ros2 topic pub /saltybot/estop std_msgs/Bool '{data: true}'
|
||||
|
||||
@ -107,11 +91,7 @@ Topics published by this stack
|
||||
/person/target PoseStamped (camera position, base_link)
|
||||
/person/detections Detection2DArray
|
||||
/cmd_vel Twist (from follower or Nav2)
|
||||
<<<<<<< HEAD
|
||||
/saltybot/cmd String (to ESP32 BALANCE)
|
||||
=======
|
||||
/saltybot/cmd String (to ESP32-S3)
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
/saltybot/imu Imu
|
||||
/saltybot/balance_state String
|
||||
"""
|
||||
@ -229,11 +209,7 @@ def generate_launch_description():
|
||||
enable_bridge_arg = DeclareLaunchArgument(
|
||||
"enable_bridge",
|
||||
default_value="true",
|
||||
<<<<<<< HEAD
|
||||
description="Launch ESP32 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
|
||||
=======
|
||||
description="Launch ESP32-S3 serial bridge + cmd_vel bridge (disable for sim/rosbag)",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
)
|
||||
|
||||
enable_rosbridge_arg = DeclareLaunchArgument(
|
||||
@ -242,7 +218,7 @@ def generate_launch_description():
|
||||
description="Launch rosbridge WebSocket server (port 9090)",
|
||||
)
|
||||
|
||||
enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
"enable_mission_logging",
|
||||
default_value="true",
|
||||
description="Launch ROS2 bag recorder for mission logging (Issue #488)",
|
||||
@ -294,11 +270,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
esp32_port_arg = DeclareLaunchArgument(
|
||||
"esp32_port",
|
||||
default_value="/dev/esp32-bridge",
|
||||
<<<<<<< HEAD
|
||||
description="ESP32 USB CDC serial port",
|
||||
=======
|
||||
description="ESP32-S3 USB CDC serial port",
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
)
|
||||
|
||||
# ── Shared substitution handles ───────────────────────────────────────────
|
||||
@ -318,11 +290,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
launch_arguments={"use_sim_time": use_sim_time}.items(),
|
||||
)
|
||||
|
||||
<<<<<<< HEAD
|
||||
# ── t=0s ESP32 bidirectional serial bridge ────────────────────────────────
|
||||
=======
|
||||
# ── t=0s ESP32-S3 bidirectional serial bridge ────────────────────────────────
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
# ── t=0s ESP32-S3 bidirectional serial bridge ───────────────────────────────
|
||||
esp32_bridge = GroupAction(
|
||||
condition=IfCondition(LaunchConfiguration("enable_bridge")),
|
||||
actions=[
|
||||
@ -352,11 +320,7 @@ enable_mission_logging_arg = DeclareLaunchArgument(
|
||||
],
|
||||
)
|
||||
|
||||
<<<<<<< HEAD
|
||||
# ── t=2s cmd_vel safety bridge (depends on ESP32 bridge) ────────────────
|
||||
=======
|
||||
# ── t=2s cmd_vel safety bridge (depends on ESP32-S3 bridge) ────────────────
|
||||
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
|
||||
cmd_vel_bridge = TimerAction(
|
||||
period=2.0,
|
||||
actions=[
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
[Unit]
|
||||
Description=CANable 2.0 CAN bus bringup (can0, 500 kbps)
|
||||
Description=CANable 2.0 CAN bus bringup (can0, 1 Mbps DroneCAN — Here4 GPS)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/643
|
||||
# Wait until the gs_usb net device appears; udev fires After=sys-subsystem-net-devices-can0.device
|
||||
After=network.target sys-subsystem-net-devices-can0.device
|
||||
@ -10,7 +10,7 @@ BindsTo=sys-subsystem-net-devices-can0.device
|
||||
Type=oneshot
|
||||
RemainAfterExit=yes
|
||||
|
||||
ExecStart=/usr/sbin/ip link set can0 up type can bitrate 500000
|
||||
ExecStart=/usr/sbin/ip link set can0 up type can bitrate 1000000
|
||||
ExecStop=/usr/sbin/ip link set can0 down
|
||||
|
||||
StandardOutput=journal
|
||||
|
||||
@ -2,6 +2,10 @@
|
||||
# Exposes the adapter as can0 via the SocketCAN subsystem.
|
||||
# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/643
|
||||
#
|
||||
# Bitrate is set by can-bringup.service (1 Mbps DroneCAN for Here4 GPS, bd-p47c).
|
||||
# This rule only assigns the interface name and tags it for systemd; it does NOT
|
||||
# bring up the interface — that is handled by can-bringup.service.
|
||||
#
|
||||
# Install:
|
||||
# sudo cp 70-canable.rules /etc/udev/rules.d/
|
||||
# sudo udevadm control --reload && sudo udevadm trigger
|
||||
@ -16,4 +20,4 @@
|
||||
SUBSYSTEM=="net", ACTION=="add", \
|
||||
ATTRS{idVendor}=="1d50", ATTRS{idProduct}=="606f", \
|
||||
NAME="can0", \
|
||||
RUN+="/sbin/ip link set can0 up type can bitrate 500000"
|
||||
TAG+="systemd"
|
||||
|
||||
31
jetson/ros2_ws/src/saltybot_bringup/udev/80-esp32.rules
Normal file
31
jetson/ros2_ws/src/saltybot_bringup/udev/80-esp32.rules
Normal file
@ -0,0 +1,31 @@
|
||||
# ESP32-S3 USB serial devices (bd-wim1)
|
||||
#
|
||||
# ESP32-S3 BALANCE (Waveshare LCD 1.28) — USB CDC via CH343 USB-UART chip
|
||||
# Appears as /dev/ttyACM0, symlinked to /dev/esp32-balance
|
||||
# idVendor = 1a86 (QinHeng Electronics / WCH)
|
||||
# idProduct = 55d4 (CH343 USB-UART)
|
||||
#
|
||||
# ESP32-S3 IO (bare board JTAG USB) — native USB CDC
|
||||
# Appears as /dev/ttyACM1, symlinked to /dev/esp32-io
|
||||
# idVendor = 303a (Espressif)
|
||||
# idProduct = 1001 (ESP32-S3 USB CDC)
|
||||
#
|
||||
# Install:
|
||||
# sudo cp 80-esp32.rules /etc/udev/rules.d/
|
||||
# sudo udevadm control --reload && sudo udevadm trigger
|
||||
#
|
||||
# Verify:
|
||||
# ls -la /dev/esp32-*
|
||||
# python3 -c "import serial; s=serial.Serial('/dev/esp32-balance', 460800); s.close(); print('OK')"
|
||||
|
||||
# ESP32-S3 BALANCE — CH343 USB-UART (bd-wim1 UART serial bridge)
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d4", \
|
||||
SYMLINK+="esp32-balance", \
|
||||
TAG+="systemd", \
|
||||
MODE="0660", GROUP="dialout"
|
||||
|
||||
# ESP32-S3 IO — Espressif native USB CDC
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001", \
|
||||
SYMLINK+="esp32-io", \
|
||||
TAG+="systemd", \
|
||||
MODE="0660", GROUP="dialout"
|
||||
38
jetson/scripts/ros2-launch.sh
Executable file
38
jetson/scripts/ros2-launch.sh
Executable file
@ -0,0 +1,38 @@
|
||||
#!/usr/bin/env bash
|
||||
# ros2-launch.sh — Source ROS2 Humble + workspace overlay, then exec ros2 launch
|
||||
#
|
||||
# Used by saltybot-*.service units so that ExecStart= does not need to inline
|
||||
# bash source chains.
|
||||
#
|
||||
# Usage (from a service file):
|
||||
# ExecStart=/opt/saltybot/scripts/ros2-launch.sh <pkg> <launch_file> [args...]
|
||||
#
|
||||
# Env vars respected:
|
||||
# ROS_DISTRO default: humble
|
||||
# SALTYBOT_WS default: /opt/saltybot/jetson/ros2_ws/install
|
||||
# ROS_DOMAIN_ID default: 42
|
||||
|
||||
set -euo pipefail
|
||||
|
||||
ROS_DISTRO="${ROS_DISTRO:-humble}"
|
||||
SALTYBOT_WS="${SALTYBOT_WS:-/opt/saltybot/jetson/ros2_ws/install}"
|
||||
|
||||
ROS_SETUP="/opt/ros/${ROS_DISTRO}/setup.bash"
|
||||
WS_SETUP="${SALTYBOT_WS}/setup.bash"
|
||||
|
||||
if [[ ! -f "${ROS_SETUP}" ]]; then
|
||||
echo "[ros2-launch] ERROR: ROS2 setup not found: ${ROS_SETUP}" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# shellcheck disable=SC1090
|
||||
source "${ROS_SETUP}"
|
||||
|
||||
if [[ -f "${WS_SETUP}" ]]; then
|
||||
# shellcheck disable=SC1090
|
||||
source "${WS_SETUP}"
|
||||
fi
|
||||
|
||||
export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}"
|
||||
|
||||
exec ros2 launch "$@"
|
||||
@ -1,55 +1,194 @@
|
||||
#!/usr/bin/env bash
|
||||
# install_systemd.sh — Install saltybot systemd services on Orin
|
||||
# Run as root: sudo ./systemd/install_systemd.sh
|
||||
# install_systemd.sh — Deploy and enable saltybot systemd services on Orin
|
||||
#
|
||||
# Run as root: sudo ./systemd/install_systemd.sh
|
||||
#
|
||||
# What this does:
|
||||
# 1. Deploy repo to /opt/saltybot/jetson
|
||||
# 2. Build ROS2 workspace (colcon)
|
||||
# 3. Install systemd unit files
|
||||
# 4. Install udev rules (CAN, ESP32)
|
||||
# 5. Enable and optionally start all services
|
||||
#
|
||||
# Services installed (start order):
|
||||
# can-bringup.service CANable2 @ 1 Mbps DroneCAN (Here4 GPS)
|
||||
# saltybot-esp32-serial.service ESP32-S3 BALANCE UART bridge (bd-wim1)
|
||||
# saltybot-here4.service Here4 GPS DroneCAN bridge (bd-p47c)
|
||||
# saltybot-ros2.service Full ROS2 stack (perception + nav)
|
||||
# saltybot-dashboard.service Web dashboard on port 8080
|
||||
# saltybot-social.service Social-bot stack (speech + LLM + face)
|
||||
# tailscale-vpn.service Tailscale VPN for remote access
|
||||
#
|
||||
# Prerequisites:
|
||||
# apt install ros-humble-desktop python3-colcon-common-extensions
|
||||
# pip install dronecan (for Here4 GPS node)
|
||||
# usermod -aG dialout orin (for serial port access)
|
||||
set -euo pipefail
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
REPO_DIR="$(dirname "${SCRIPT_DIR}")"
|
||||
SYSTEMD_DIR="/etc/systemd/system"
|
||||
DEPLOY_DIR="/opt/saltybot/jetson"
|
||||
SCRIPTS_DIR="${DEPLOY_DIR}/scripts"
|
||||
UDEV_DIR="/etc/udev/rules.d"
|
||||
BRINGUP_SYSTEMD="${REPO_DIR}/ros2_ws/src/saltybot_bringup/systemd"
|
||||
BRINGUP_UDEV="${REPO_DIR}/ros2_ws/src/saltybot_bringup/udev"
|
||||
WS_SRC="${REPO_DIR}/ros2_ws/src"
|
||||
WS_BUILD="${DEPLOY_DIR}/ros2_ws"
|
||||
|
||||
log() { echo "[install_systemd] $*"; }
|
||||
ROS_DISTRO="${ROS_DISTRO:-humble}"
|
||||
ROS_SETUP="/opt/ros/${ROS_DISTRO}/setup.bash"
|
||||
ORIN_USER="${SALTYBOT_USER:-orin}"
|
||||
|
||||
[[ "$(id -u)" == "0" ]] || { echo "Run as root"; exit 1; }
|
||||
log() { echo "[install_systemd] $*"; }
|
||||
warn() { echo "[install_systemd] WARN: $*" >&2; }
|
||||
die() { echo "[install_systemd] ERROR: $*" >&2; exit 1; }
|
||||
|
||||
# Deploy repo to /opt/saltybot/jetson
|
||||
log "Deploying to ${DEPLOY_DIR}..."
|
||||
[[ "$(id -u)" == "0" ]] || die "Run as root (sudo $0)"
|
||||
[[ -f "${ROS_SETUP}" ]] || die "ROS2 ${ROS_DISTRO} not found at ${ROS_SETUP}"
|
||||
|
||||
# ── 1. Deploy repo ─────────────────────────────────────────────────────────────
|
||||
log "Deploying repo to ${DEPLOY_DIR}..."
|
||||
mkdir -p "${DEPLOY_DIR}"
|
||||
rsync -a --exclude='.git' --exclude='__pycache__' \
|
||||
rsync -a \
|
||||
--exclude='.git' \
|
||||
--exclude='__pycache__' \
|
||||
--exclude='*.pyc' \
|
||||
--exclude='.pytest_cache' \
|
||||
--exclude='build/' \
|
||||
--exclude='install/' \
|
||||
--exclude='log/' \
|
||||
"${REPO_DIR}/" "${DEPLOY_DIR}/"
|
||||
|
||||
# Install service files
|
||||
log "Installing systemd units..."
|
||||
cp "${SCRIPT_DIR}/saltybot.target" "${SYSTEMD_DIR}/"
|
||||
cp "${SCRIPT_DIR}/saltybot-social.service" "${SYSTEMD_DIR}/"
|
||||
cp "${SCRIPT_DIR}/tailscale-vpn.service" "${SYSTEMD_DIR}/"
|
||||
cp "${BRINGUP_SYSTEMD}/can-bringup.service" "${SYSTEMD_DIR}/"
|
||||
# Install launch wrapper script
|
||||
log "Installing ros2-launch.sh..."
|
||||
install -m 755 "${SCRIPT_DIR}/../scripts/ros2-launch.sh" "/opt/saltybot/scripts/ros2-launch.sh"
|
||||
|
||||
# Install udev rules
|
||||
# ── 2. Build ROS2 workspace ────────────────────────────────────────────────────
|
||||
if command -v colcon &>/dev/null; then
|
||||
log "Building ROS2 workspace (colcon)..."
|
||||
# shellcheck disable=SC1090
|
||||
source "${ROS_SETUP}"
|
||||
(
|
||||
cd "${WS_BUILD}"
|
||||
colcon build \
|
||||
--symlink-install \
|
||||
--cmake-args -DCMAKE_BUILD_TYPE=Release \
|
||||
2>&1 | tee /tmp/colcon-build.log
|
||||
)
|
||||
log "Workspace build complete."
|
||||
else
|
||||
warn "colcon not found — skipping workspace build."
|
||||
warn "Run manually: cd ${WS_BUILD} && colcon build --symlink-install"
|
||||
fi
|
||||
|
||||
# ── 3. Install systemd units ───────────────────────────────────────────────────
|
||||
log "Installing systemd units..."
|
||||
|
||||
# Units from jetson/systemd/
|
||||
for unit in \
|
||||
saltybot.target \
|
||||
saltybot-ros2.service \
|
||||
saltybot-esp32-serial.service \
|
||||
saltybot-here4.service \
|
||||
saltybot-dashboard.service \
|
||||
saltybot-social.service \
|
||||
tailscale-vpn.service
|
||||
do
|
||||
if [[ -f "${SCRIPT_DIR}/${unit}" ]]; then
|
||||
cp "${SCRIPT_DIR}/${unit}" "${SYSTEMD_DIR}/"
|
||||
log " Installed ${unit}"
|
||||
else
|
||||
warn " ${unit} not found in ${SCRIPT_DIR}/ — skipping"
|
||||
fi
|
||||
done
|
||||
|
||||
# Units from saltybot_bringup/systemd/
|
||||
for unit in \
|
||||
can-bringup.service \
|
||||
chromium-kiosk.service \
|
||||
magedok-display.service \
|
||||
salty-face-server.service
|
||||
do
|
||||
if [[ -f "${BRINGUP_SYSTEMD}/${unit}" ]]; then
|
||||
cp "${BRINGUP_SYSTEMD}/${unit}" "${SYSTEMD_DIR}/"
|
||||
log " Installed ${unit} (from bringup)"
|
||||
else
|
||||
warn " ${unit} not found in bringup systemd/ — skipping"
|
||||
fi
|
||||
done
|
||||
|
||||
# ── 4. Install udev rules ──────────────────────────────────────────────────────
|
||||
log "Installing udev rules..."
|
||||
mkdir -p "${UDEV_DIR}"
|
||||
cp "${BRINGUP_UDEV}/70-canable.rules" "${UDEV_DIR}/"
|
||||
cp "${BRINGUP_UDEV}/90-magedok-touch.rules" "${UDEV_DIR}/"
|
||||
|
||||
for rule in \
|
||||
70-canable.rules \
|
||||
80-esp32.rules \
|
||||
90-magedok-touch.rules
|
||||
do
|
||||
if [[ -f "${BRINGUP_UDEV}/${rule}" ]]; then
|
||||
cp "${BRINGUP_UDEV}/${rule}" "${UDEV_DIR}/"
|
||||
log " Installed ${rule}"
|
||||
else
|
||||
warn " ${rule} not found — skipping"
|
||||
fi
|
||||
done
|
||||
|
||||
udevadm control --reload
|
||||
udevadm trigger --subsystem-match=net --action=add
|
||||
udevadm trigger --subsystem-match=net --action=add
|
||||
udevadm trigger --subsystem-match=tty --action=add
|
||||
log "udev rules reloaded."
|
||||
|
||||
# Reload and enable
|
||||
# ── 5. Set permissions ─────────────────────────────────────────────────────────
|
||||
log "Ensuring '${ORIN_USER}' is in dialout group (serial port access)..."
|
||||
if id "${ORIN_USER}" &>/dev/null; then
|
||||
usermod -aG dialout "${ORIN_USER}" || warn "usermod failed — add ${ORIN_USER} to dialout manually"
|
||||
else
|
||||
warn "User '${ORIN_USER}' not found — skip usermod"
|
||||
fi
|
||||
|
||||
# ── 6. Reload systemd and enable services ─────────────────────────────────────
|
||||
log "Reloading systemd daemon..."
|
||||
systemctl daemon-reload
|
||||
systemctl enable saltybot.target
|
||||
systemctl enable saltybot-social.service
|
||||
systemctl enable tailscale-vpn.service
|
||||
systemctl enable can-bringup.service
|
||||
|
||||
log "Services installed. Start with:"
|
||||
log " systemctl start saltybot-social"
|
||||
log " systemctl start tailscale-vpn"
|
||||
log " systemctl start can-bringup"
|
||||
log " journalctl -fu saltybot-social"
|
||||
log " journalctl -fu tailscale-vpn"
|
||||
log " journalctl -fu can-bringup"
|
||||
log "Enabling services..."
|
||||
systemctl enable \
|
||||
saltybot.target \
|
||||
can-bringup.service \
|
||||
saltybot-esp32-serial.service \
|
||||
saltybot-here4.service \
|
||||
saltybot-ros2.service \
|
||||
saltybot-dashboard.service \
|
||||
saltybot-social.service \
|
||||
tailscale-vpn.service
|
||||
|
||||
# Enable mosquitto (MQTT broker) if installed
|
||||
if systemctl list-unit-files mosquitto.service &>/dev/null; then
|
||||
systemctl enable mosquitto.service
|
||||
log " Enabled mosquitto.service"
|
||||
fi
|
||||
|
||||
# ── 7. Summary ────────────────────────────────────────────────────────────────
|
||||
log ""
|
||||
log "Verify CAN bus: candump can0"
|
||||
log " VESC CAN IDs: 61 (0x3D) and 79 (0x4F)"
|
||||
log "Installation complete."
|
||||
log ""
|
||||
log "Services will start automatically on next reboot."
|
||||
log "To start now without rebooting:"
|
||||
log " sudo systemctl start saltybot.target"
|
||||
log ""
|
||||
log "Check status:"
|
||||
log " systemctl status can-bringup saltybot-esp32-serial saltybot-here4 saltybot-ros2 saltybot-dashboard"
|
||||
log ""
|
||||
log "Live logs:"
|
||||
log " journalctl -fu can-bringup"
|
||||
log " journalctl -fu saltybot-esp32-serial"
|
||||
log " journalctl -fu saltybot-here4"
|
||||
log " journalctl -fu saltybot-ros2"
|
||||
log " journalctl -fu saltybot-dashboard"
|
||||
log ""
|
||||
log "Dashboard: http://<orin-ip>:8080"
|
||||
log "rosbridge: ws://<orin-ip>:9090"
|
||||
log ""
|
||||
log "Note: saltybot-esp32-serial and saltybot-here4 require packages"
|
||||
log " from bd-wim1 (PR #727) and bd-p47c (PR #728) to be merged."
|
||||
|
||||
38
jetson/systemd/saltybot-dashboard.service
Normal file
38
jetson/systemd/saltybot-dashboard.service
Normal file
@ -0,0 +1,38 @@
|
||||
[Unit]
|
||||
Description=SaltyBot Web Dashboard (saltybot_dashboard — port 8080)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
|
||||
#
|
||||
# Serves the live monitoring dashboard on http://<orin-ip>:8080
|
||||
# Subscribes to ROS2 topics over the local DomainID (ROS_DOMAIN_ID=42).
|
||||
# Starts after the ROS2 stack but does not hard-depend on it —
|
||||
# the dashboard handles missing topics gracefully and will show data as
|
||||
# nodes come up.
|
||||
After=network-online.target saltybot-ros2.service
|
||||
Wants=saltybot-ros2.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=orin
|
||||
Group=orin
|
||||
|
||||
Environment="ROS_DOMAIN_ID=42"
|
||||
Environment="ROS_DISTRO=humble"
|
||||
Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
|
||||
|
||||
ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
|
||||
saltybot_dashboard dashboard.launch.py
|
||||
|
||||
Restart=on-failure
|
||||
RestartSec=5s
|
||||
StartLimitInterval=60s
|
||||
StartLimitBurst=5
|
||||
|
||||
TimeoutStartSec=30s
|
||||
TimeoutStopSec=10s
|
||||
|
||||
StandardOutput=journal
|
||||
StandardError=journal
|
||||
SyslogIdentifier=saltybot-dashboard
|
||||
|
||||
[Install]
|
||||
WantedBy=saltybot.target
|
||||
54
jetson/systemd/saltybot-esp32-serial.service
Normal file
54
jetson/systemd/saltybot-esp32-serial.service
Normal file
@ -0,0 +1,54 @@
|
||||
[Unit]
|
||||
Description=SaltyBot ESP32-S3 BALANCE UART Serial Bridge (bd-wim1)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/bd-wim1
|
||||
# Requires package: saltybot_esp32_serial (merged in bd-wim1 → PR #727)
|
||||
#
|
||||
# Publishes:
|
||||
# /saltybot/attitude String JSON (pitch, motor_cmd, vbat_mv, state)
|
||||
# /saltybot/balance_state String
|
||||
# /can/battery BatteryState
|
||||
# /can/vesc/left/state Float32MultiArray [erpm, voltage_v, current_a, temp_c]
|
||||
# /can/vesc/right/state Float32MultiArray
|
||||
# /can/connection_status String
|
||||
#
|
||||
# Subscribes:
|
||||
# /cmd_vel Twist → DRIVE command to ESP32 BALANCE
|
||||
# /estop Bool → ESTOP command
|
||||
# /saltybot/arm Bool → ARM command
|
||||
# /saltybot/pid_update String → PID gains update
|
||||
After=network-online.target
|
||||
# Wait for /dev/esp32-balance (created by 80-esp32.rules udev rule).
|
||||
# TAG+="systemd" in the udev rule makes systemd track dev-esp32\x2dbalance.device.
|
||||
# If device is not present at boot the node's auto-reconnect loop handles it.
|
||||
Wants=network-online.target
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=orin
|
||||
Group=dialout
|
||||
|
||||
# ROS2 Humble environment + workspace overlay
|
||||
Environment="ROS_DOMAIN_ID=42"
|
||||
Environment="ROS_DISTRO=humble"
|
||||
Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
|
||||
|
||||
ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
|
||||
saltybot_esp32_serial esp32_balance.launch.py
|
||||
|
||||
# The node auto-reconnects on serial disconnect (2 s retry).
|
||||
# Restart=on-failure covers node crash; RestartSec gives the device time to re-enumerate.
|
||||
Restart=on-failure
|
||||
RestartSec=5s
|
||||
StartLimitInterval=60s
|
||||
StartLimitBurst=5
|
||||
|
||||
TimeoutStartSec=30s
|
||||
TimeoutStopSec=10s
|
||||
|
||||
# Logging
|
||||
StandardOutput=journal
|
||||
StandardError=journal
|
||||
SyslogIdentifier=saltybot-esp32-serial
|
||||
|
||||
[Install]
|
||||
WantedBy=saltybot.target
|
||||
50
jetson/systemd/saltybot-here4.service
Normal file
50
jetson/systemd/saltybot-here4.service
Normal file
@ -0,0 +1,50 @@
|
||||
[Unit]
|
||||
Description=SaltyBot Here4 GPS DroneCAN Bridge (bd-p47c)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware/issues/bd-p47c
|
||||
# Requires package: saltybot_dronecan_gps (merged in bd-p47c → PR #728)
|
||||
# Requires: can0 at 1 Mbps (set by can-bringup.service)
|
||||
#
|
||||
# Publishes:
|
||||
# /gps/fix NavSatFix 5 Hz → navsat_transform EKF
|
||||
# /gps/velocity TwistWithCovarianceStamped 5 Hz
|
||||
# /here4/fix NavSatFix 5 Hz (alias)
|
||||
# /here4/imu Imu 50 Hz
|
||||
# /here4/mag MagneticField 10 Hz
|
||||
# /here4/baro FluidPressure 10 Hz
|
||||
# /here4/status String JSON 1 Hz (node health, fix quality)
|
||||
# /here4/node_id Int32 once (auto-discovered DroneCAN ID)
|
||||
#
|
||||
# Subscribes:
|
||||
# /rtcm ByteMultiArray on demand (RTCM corrections)
|
||||
# /rtcm_hex String on demand (hex fallback)
|
||||
After=network-online.target can-bringup.service
|
||||
Requires=can-bringup.service
|
||||
# If can-bringup stops (CANable2 unplugged), stop this service too
|
||||
BindsTo=can-bringup.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=orin
|
||||
Group=dialout
|
||||
|
||||
Environment="ROS_DOMAIN_ID=42"
|
||||
Environment="ROS_DISTRO=humble"
|
||||
Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
|
||||
|
||||
ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
|
||||
saltybot_dronecan_gps here4.launch.py
|
||||
|
||||
Restart=on-failure
|
||||
RestartSec=5s
|
||||
StartLimitInterval=60s
|
||||
StartLimitBurst=5
|
||||
|
||||
TimeoutStartSec=30s
|
||||
TimeoutStopSec=10s
|
||||
|
||||
StandardOutput=journal
|
||||
StandardError=journal
|
||||
SyslogIdentifier=saltybot-here4
|
||||
|
||||
[Install]
|
||||
WantedBy=saltybot.target
|
||||
59
jetson/systemd/saltybot-ros2.service
Normal file
59
jetson/systemd/saltybot-ros2.service
Normal file
@ -0,0 +1,59 @@
|
||||
[Unit]
|
||||
Description=SaltyBot ROS2 Full Stack (perception + navigation + SLAM)
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
|
||||
#
|
||||
# Launches: saltybot_bringup full_stack.launch.py (indoor mode by default)
|
||||
# enable_bridge:=false — ESP32-S3 serial bridge is managed by saltybot-esp32-serial.service
|
||||
#
|
||||
# Stack launched (t=0s → t=17s):
|
||||
# t= 0s robot_description (URDF + TF)
|
||||
# t= 2s RPLIDAR A1M8 + RealSense D435i
|
||||
# t= 3s LIDAR obstacle avoidance
|
||||
# t= 4s 4× IMX219 CSI cameras (optional)
|
||||
# t= 4s UWB driver
|
||||
# t= 5s Audio pipeline (wake word + STT + TTS)
|
||||
# t= 6s RTAB-Map SLAM (indoor) / GPS nav (outdoor)
|
||||
# t= 6s YOLOv8n person + object detection (TensorRT)
|
||||
# t= 7s Autonomous docking
|
||||
# t= 9s Person follower
|
||||
# t=14s Nav2 navigation stack
|
||||
# t=17s rosbridge WebSocket (ws://0.0.0.0:9090)
|
||||
#
|
||||
# Hard deps: can-bringup (Here4 GPS topics), esp32-serial (VESC telemetry)
|
||||
After=network-online.target can-bringup.service saltybot-esp32-serial.service saltybot-here4.service
|
||||
Wants=can-bringup.service saltybot-esp32-serial.service saltybot-here4.service
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=orin
|
||||
Group=orin
|
||||
|
||||
Environment="ROS_DOMAIN_ID=42"
|
||||
Environment="ROS_DISTRO=humble"
|
||||
Environment="SALTYBOT_WS=/opt/saltybot/jetson/ros2_ws/install"
|
||||
|
||||
# enable_bridge:=false — ESP32 serial bridge runs as its own service (saltybot-esp32-serial)
|
||||
# mode can be overridden via environment: Environment="SALTYBOT_MODE=outdoor"
|
||||
ExecStart=/opt/saltybot/scripts/ros2-launch.sh \
|
||||
saltybot_bringup full_stack.launch.py \
|
||||
enable_bridge:=false \
|
||||
mode:=${SALTYBOT_MODE:-indoor}
|
||||
|
||||
Restart=on-failure
|
||||
RestartSec=10s
|
||||
StartLimitInterval=120s
|
||||
StartLimitBurst=3
|
||||
|
||||
TimeoutStartSec=120s
|
||||
TimeoutStopSec=30s
|
||||
|
||||
# ROS2 launch spawns many child processes; kill them on stop
|
||||
KillMode=control-group
|
||||
|
||||
# Logging
|
||||
StandardOutput=journal
|
||||
StandardError=journal
|
||||
SyslogIdentifier=saltybot-ros2
|
||||
|
||||
[Install]
|
||||
WantedBy=saltybot.target
|
||||
@ -1,8 +1,22 @@
|
||||
[Unit]
|
||||
Description=Saltybot Full Stack Target
|
||||
Description=SaltyBot Full Stack Target
|
||||
Documentation=https://gitea.vayrette.com/seb/saltylab-firmware
|
||||
After=docker.service network-online.target
|
||||
Requires=docker.service
|
||||
#
|
||||
# Start everything: systemctl start saltybot.target
|
||||
# Stop everything: systemctl stop saltybot.target
|
||||
# Check all: systemctl status 'saltybot-*' can-bringup
|
||||
#
|
||||
# Boot order (dependency chain):
|
||||
# network-online.target
|
||||
# → can-bringup.service (CANable2 @ 1 Mbps DroneCAN)
|
||||
# → saltybot-here4.service (Here4 GPS → /gps/fix, /here4/*)
|
||||
# → saltybot-esp32-serial.service (ESP32-S3 UART → /can/vesc/*, /saltybot/attitude)
|
||||
# → saltybot-ros2.service (full_stack.launch.py, perception + nav)
|
||||
# → saltybot-dashboard.service (port 8080)
|
||||
# (independent) saltybot-social.service
|
||||
# (independent) tailscale-vpn.service
|
||||
After=network-online.target
|
||||
Wants=network-online.target
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user