diff --git a/esp32s3/balance/CMakeLists.txt b/esp32s3/balance/CMakeLists.txt new file mode 100644 index 0000000..ce51cdf --- /dev/null +++ b/esp32s3/balance/CMakeLists.txt @@ -0,0 +1,3 @@ +cmake_minimum_required(VERSION 3.16) +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +project(esp32s3_balance) diff --git a/esp32s3/balance/main/CMakeLists.txt b/esp32s3/balance/main/CMakeLists.txt new file mode 100644 index 0000000..2fd31d5 --- /dev/null +++ b/esp32s3/balance/main/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRCS "main.c" "orin_serial.c" "vesc_can.c" + INCLUDE_DIRS "." +) diff --git a/esp32s3/balance/main/config.h b/esp32s3/balance/main/config.h new file mode 100644 index 0000000..dd0b6af --- /dev/null +++ b/esp32s3/balance/main/config.h @@ -0,0 +1,42 @@ +#pragma once + +/* ── ESP32-S3 BALANCE board — bd-66hx pin/config definitions ─────────────── + * + * Hardware change from pre-bd-66hx design: + * Previously: IO43/IO44 = CAN SN65HVD230 (shared Orin+VESC bus via CANable2) + * After bd-66hx: IO43/IO44 = CH343 UART0 (Orin serial comms) + * IO2/IO1 = CAN SN65HVD230 rewired (VESC-only bus) + * + * The SN65HVD230 transceiver physical wiring must be updated from IO43/44 + * to IO2/IO1 when deploying this firmware. See docs/SAUL-TEE-SYSTEM-REFERENCE.md. + */ + +/* ── Orin serial (CH343 USB-to-UART, 1a86:55d3 on Orin side) ── */ +#define ORIN_UART_PORT UART_NUM_0 +#define ORIN_UART_BAUD 460800 +#define ORIN_UART_TX_GPIO 43 /* ESP32→CH343 RXD */ +#define ORIN_UART_RX_GPIO 44 /* CH343 TXD→ESP32 */ +#define ORIN_UART_RX_BUF 1024 +#define ORIN_TX_QUEUE_DEPTH 16 + +/* ── VESC CAN TWAI (SN65HVD230 transceiver, rewired for bd-66hx) ── */ +#define VESC_CAN_TX_GPIO 2 /* ESP32 TWAI TX → SN65HVD230 TXD */ +#define VESC_CAN_RX_GPIO 1 /* SN65HVD230 RXD → ESP32 TWAI RX */ +#define VESC_CAN_RX_QUEUE 32 + +/* VESC node IDs — matched to bd-wim1 TELEM_VESC_LEFT/RIGHT mapping */ +#define VESC_ID_A 56u /* TELEM_VESC_LEFT (0x81) */ +#define VESC_ID_B 68u /* TELEM_VESC_RIGHT (0x82) */ + +/* ── Safety / timing ── */ +#define HB_TIMEOUT_MS 500u /* heartbeat watchdog: disarm if exceeded */ +#define DRIVE_TIMEOUT_MS 500u /* drive command staleness timeout */ +#define TELEM_STATUS_PERIOD_MS 100u /* 10 Hz status telemetry to Orin */ +#define TELEM_VESC_PERIOD_MS 100u /* 10 Hz VESC telemetry to Orin */ + +/* ── Drive → VESC RPM scaling ── */ +#define RPM_PER_SPEED_UNIT 5 /* speed_units=1000 → 5000 ERPM */ +#define RPM_PER_STEER_UNIT 3 /* steer differential scale */ + +/* ── Tilt cutoff ── */ +#define TILT_CUTOFF_DEG 25.0f diff --git a/esp32s3/balance/main/main.c b/esp32s3/balance/main/main.c new file mode 100644 index 0000000..c382c04 --- /dev/null +++ b/esp32s3/balance/main/main.c @@ -0,0 +1,111 @@ +/* main.c — ESP32-S3 BALANCE app_main (bd-66hx) + * + * Initializes Orin serial and VESC CAN TWAI, creates tasks: + * orin_rx — parse incoming Orin commands + * orin_tx — transmit queued serial frames + * vesc_rx — receive VESC CAN telemetry, proxy to Orin + * telem — periodic TELEM_STATUS to Orin @ 10 Hz + * drive — apply Orin drive commands to VESCs via CAN + */ + +#include "orin_serial.h" +#include "vesc_can.h" +#include "config.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/queue.h" +#include "esp_log.h" +#include "esp_timer.h" +#include + +static const char *TAG = "main"; + +static QueueHandle_t s_orin_tx_q; + +/* ── Telemetry task: sends TELEM_STATUS to Orin at 10 Hz ── */ +static void telem_task(void *arg) +{ + for (;;) { + vTaskDelay(pdMS_TO_TICKS(TELEM_STATUS_PERIOD_MS)); + + uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL); + bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS; + + /* Determine balance state for telemetry */ + bal_state_t state; + if (g_orin_ctrl.estop) { + state = BAL_ESTOP; + } else if (!g_orin_ctrl.armed) { + state = BAL_DISARMED; + } else { + state = BAL_ARMED; + } + + /* flags: bit0=estop_active, bit1=heartbeat_timeout */ + uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) | + (hb_timeout ? 0x02u : 0x00u); + + /* Battery voltage from VESC_ID_A STATUS_5 (V×10 → mV) */ + uint16_t vbat_mv = (uint16_t)((int32_t)g_vesc[0].voltage_x10 * 100); + + orin_send_status(s_orin_tx_q, + 0, /* pitch_x10: stub — full IMU in future bead */ + 0, /* motor_cmd: stub */ + vbat_mv, + state, + flags); + } +} + +/* ── Drive task: applies Orin drive commands to VESCs @ 50 Hz ── */ +static void drive_task(void *arg) +{ + for (;;) { + vTaskDelay(pdMS_TO_TICKS(20)); /* 50 Hz */ + + uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL); + bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS; + bool drive_stale = (now_ms - g_orin_drive.updated_ms) > DRIVE_TIMEOUT_MS; + + int32_t left_erpm = 0; + int32_t right_erpm = 0; + + if (g_orin_ctrl.armed && !g_orin_ctrl.estop && + !hb_timeout && !drive_stale) { + int32_t spd = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT; + int32_t str = (int32_t)g_orin_drive.steer * RPM_PER_STEER_UNIT; + left_erpm = spd + str; + right_erpm = spd - str; + } + + /* VESC_ID_A (56) = LEFT, VESC_ID_B (68) = RIGHT per bd-wim1 protocol */ + vesc_can_send_rpm(VESC_ID_A, left_erpm); + vesc_can_send_rpm(VESC_ID_B, right_erpm); + } +} + +void app_main(void) +{ + ESP_LOGI(TAG, "ESP32-S3 BALANCE bd-66hx starting"); + + /* Init peripherals */ + orin_serial_init(); + vesc_can_init(); + + /* TX queue for outbound serial frames */ + s_orin_tx_q = xQueueCreate(ORIN_TX_QUEUE_DEPTH, sizeof(orin_tx_frame_t)); + configASSERT(s_orin_tx_q); + + /* Seed heartbeat timer so we don't immediately timeout */ + g_orin_ctrl.hb_last_ms = (uint32_t)(esp_timer_get_time() / 1000LL); + + /* Create tasks */ + xTaskCreate(orin_serial_rx_task, "orin_rx", 4096, s_orin_tx_q, 10, NULL); + xTaskCreate(orin_serial_tx_task, "orin_tx", 2048, s_orin_tx_q, 9, NULL); + xTaskCreate(vesc_can_rx_task, "vesc_rx", 4096, s_orin_tx_q, 10, NULL); + xTaskCreate(telem_task, "telem", 2048, NULL, 5, NULL); + xTaskCreate(drive_task, "drive", 2048, NULL, 8, NULL); + + ESP_LOGI(TAG, "all tasks started"); + /* app_main returns — FreeRTOS scheduler continues */ +} diff --git a/esp32s3/balance/main/orin_serial.c b/esp32s3/balance/main/orin_serial.c new file mode 100644 index 0000000..2cb3ef4 --- /dev/null +++ b/esp32s3/balance/main/orin_serial.c @@ -0,0 +1,292 @@ +/* orin_serial.c — Orin↔ESP32-S3 serial protocol implementation (bd-66hx) + * + * Implements the binary framing protocol matching bd-wim1 (Orin side). + * CRC8-SMBUS: poly=0x07, init=0x00, covers LEN+TYPE+PAYLOAD bytes. + */ + +#include "orin_serial.h" +#include "config.h" +#include "driver/uart.h" +#include "esp_log.h" +#include "esp_timer.h" +#include "freertos/FreeRTOS.h" +#include "freertos/queue.h" +#include + +static const char *TAG = "orin"; + +/* ── Shared state ── */ +orin_drive_t g_orin_drive = {0}; +orin_pid_t g_orin_pid = {0}; +orin_control_t g_orin_ctrl = {.armed = false, .estop = false, .hb_last_ms = 0}; + +/* ── CRC8-SMBUS (poly=0x07, init=0x00) ── */ +static uint8_t crc8(const uint8_t *data, uint8_t len) +{ + uint8_t crc = 0x00u; + for (uint8_t i = 0; i < len; i++) { + crc ^= data[i]; + for (uint8_t b = 0; b < 8u; b++) { + crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u); + } + } + return crc; +} + +/* ── Frame builder ── */ +static void build_frame(orin_tx_frame_t *f, uint8_t out[/* ORIN_MAX_PAYLOAD + 4 */], uint8_t *out_len) +{ + /* [SYNC][LEN][TYPE][PAYLOAD...][CRC] */ + uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD]; + crc_buf[0] = f->len; + crc_buf[1] = f->type; + memcpy(&crc_buf[2], f->payload, f->len); + uint8_t crc = crc8(crc_buf, (uint8_t)(2u + f->len)); + + out[0] = ORIN_SYNC; + out[1] = f->len; + out[2] = f->type; + memcpy(&out[3], f->payload, f->len); + out[3u + f->len] = crc; + *out_len = (uint8_t)(4u + f->len); +} + +/* ── Enqueue helpers ── */ +static void enqueue(QueueHandle_t q, uint8_t type, const uint8_t *payload, uint8_t len) +{ + orin_tx_frame_t f = {.type = type, .len = len}; + if (len > 0u && payload) { + memcpy(f.payload, payload, len); + } + if (xQueueSend(q, &f, 0) != pdTRUE) { + ESP_LOGW(TAG, "tx queue full, dropped type=0x%02x", type); + } +} + +void orin_send_ack(QueueHandle_t q, uint8_t cmd_type) +{ + enqueue(q, RESP_ACK, &cmd_type, 1u); +} + +void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err) +{ + uint8_t p[2] = {cmd_type, err}; + enqueue(q, RESP_NACK, p, 2u); +} + +void orin_send_status(QueueHandle_t q, + int16_t pitch_x10, int16_t motor_cmd, + uint16_t vbat_mv, bal_state_t state, uint8_t flags) +{ + /* int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, uint8 state, uint8 flags — BE */ + uint8_t p[8]; + p[0] = (uint8_t)((uint16_t)pitch_x10 >> 8u); + p[1] = (uint8_t)((uint16_t)pitch_x10); + p[2] = (uint8_t)((uint16_t)motor_cmd >> 8u); + p[3] = (uint8_t)((uint16_t)motor_cmd); + p[4] = (uint8_t)(vbat_mv >> 8u); + p[5] = (uint8_t)(vbat_mv); + p[6] = (uint8_t)state; + p[7] = flags; + enqueue(q, TELEM_STATUS, p, 8u); +} + +void orin_send_vesc(QueueHandle_t q, uint8_t telem_type, + int32_t erpm, uint16_t voltage_mv, + int16_t current_ma, uint16_t temp_c_x10) +{ + /* int32 erpm, uint16 voltage_mv, int16 current_ma, uint16 temp_c_x10 — BE */ + uint8_t p[10]; + uint32_t u = (uint32_t)erpm; + p[0] = (uint8_t)(u >> 24u); + p[1] = (uint8_t)(u >> 16u); + p[2] = (uint8_t)(u >> 8u); + p[3] = (uint8_t)(u); + p[4] = (uint8_t)(voltage_mv >> 8u); + p[5] = (uint8_t)(voltage_mv); + p[6] = (uint8_t)((uint16_t)current_ma >> 8u); + p[7] = (uint8_t)((uint16_t)current_ma); + p[8] = (uint8_t)(temp_c_x10 >> 8u); + p[9] = (uint8_t)(temp_c_x10); + enqueue(q, telem_type, p, 10u); +} + +/* ── UART init ── */ +void orin_serial_init(void) +{ + uart_config_t cfg = { + .baud_rate = ORIN_UART_BAUD, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_DISABLE, + .stop_bits = UART_STOP_BITS_1, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + }; + ESP_ERROR_CHECK(uart_param_config(ORIN_UART_PORT, &cfg)); + ESP_ERROR_CHECK(uart_set_pin(ORIN_UART_PORT, + ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO, + UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE)); + ESP_ERROR_CHECK(uart_driver_install(ORIN_UART_PORT, ORIN_UART_RX_BUF, 0, + 0, NULL, 0)); + ESP_LOGI(TAG, "UART%d init OK: tx=%d rx=%d baud=%d", + ORIN_UART_PORT, ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO, ORIN_UART_BAUD); +} + +/* ── RX parser state machine ── */ +typedef enum { + WAIT_SYNC, + WAIT_LEN, + WAIT_TYPE, + WAIT_PAYLOAD, + WAIT_CRC, +} rx_state_t; + +static void dispatch_cmd(uint8_t type, const uint8_t *payload, uint8_t len, + QueueHandle_t tx_q) +{ + uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL); + + switch (type) { + case CMD_HEARTBEAT: + g_orin_ctrl.hb_last_ms = now_ms; + orin_send_ack(tx_q, type); + break; + + case CMD_DRIVE: + if (len < 4u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; } + if (g_orin_ctrl.estop) { orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE); break; } + if (!g_orin_ctrl.armed) { orin_send_nack(tx_q, type, ERR_DISARMED); break; } + g_orin_drive.speed = (int16_t)(((uint16_t)payload[0] << 8u) | payload[1]); + g_orin_drive.steer = (int16_t)(((uint16_t)payload[2] << 8u) | payload[3]); + g_orin_drive.updated_ms = now_ms; + g_orin_ctrl.hb_last_ms = now_ms; /* drive counts as heartbeat */ + orin_send_ack(tx_q, type); + break; + + case CMD_ESTOP: + if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; } + g_orin_ctrl.estop = (payload[0] != 0u); + if (g_orin_ctrl.estop) { + g_orin_drive.speed = 0; + g_orin_drive.steer = 0; + } + orin_send_ack(tx_q, type); + break; + + case CMD_ARM: + if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; } + if (g_orin_ctrl.estop && payload[0] != 0u) { + /* cannot arm while estop is active */ + orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE); + break; + } + g_orin_ctrl.armed = (payload[0] != 0u); + if (!g_orin_ctrl.armed) { + g_orin_drive.speed = 0; + g_orin_drive.steer = 0; + } + orin_send_ack(tx_q, type); + break; + + case CMD_PID: + if (len < 12u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; } + /* float32 big-endian: copy and swap bytes */ + { + uint32_t raw; + raw = ((uint32_t)payload[0] << 24u) | ((uint32_t)payload[1] << 16u) | + ((uint32_t)payload[2] << 8u) | (uint32_t)payload[3]; + memcpy((void*)&g_orin_pid.kp, &raw, 4u); + raw = ((uint32_t)payload[4] << 24u) | ((uint32_t)payload[5] << 16u) | + ((uint32_t)payload[6] << 8u) | (uint32_t)payload[7]; + memcpy((void*)&g_orin_pid.ki, &raw, 4u); + raw = ((uint32_t)payload[8] << 24u) | ((uint32_t)payload[9] << 16u) | + ((uint32_t)payload[10] << 8u) | (uint32_t)payload[11]; + memcpy((void*)&g_orin_pid.kd, &raw, 4u); + g_orin_pid.updated = true; + } + orin_send_ack(tx_q, type); + break; + + default: + ESP_LOGW(TAG, "unknown cmd type=0x%02x", type); + break; + } +} + +void orin_serial_rx_task(void *arg) +{ + QueueHandle_t tx_q = (QueueHandle_t)arg; + rx_state_t state = WAIT_SYNC; + uint8_t rx_len = 0; + uint8_t rx_type = 0; + uint8_t payload[ORIN_MAX_PAYLOAD]; + uint8_t pay_idx = 0; + + uint8_t byte; + for (;;) { + int r = uart_read_bytes(ORIN_UART_PORT, &byte, 1, pdMS_TO_TICKS(10)); + if (r <= 0) { + continue; + } + + switch (state) { + case WAIT_SYNC: + if (byte == ORIN_SYNC) { state = WAIT_LEN; } + break; + + case WAIT_LEN: + if (byte > ORIN_MAX_PAYLOAD) { + /* oversize — send NACK and reset */ + orin_send_nack(tx_q, 0x00u, ERR_BAD_LEN); + state = WAIT_SYNC; + } else { + rx_len = byte; + state = WAIT_TYPE; + } + break; + + case WAIT_TYPE: + rx_type = byte; + pay_idx = 0u; + state = (rx_len == 0u) ? WAIT_CRC : WAIT_PAYLOAD; + break; + + case WAIT_PAYLOAD: + payload[pay_idx++] = byte; + if (pay_idx == rx_len) { state = WAIT_CRC; } + break; + + case WAIT_CRC: { + /* Verify CRC over [LEN, TYPE, PAYLOAD] */ + uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD]; + crc_buf[0] = rx_len; + crc_buf[1] = rx_type; + memcpy(&crc_buf[2], payload, rx_len); + uint8_t expected = crc8(crc_buf, (uint8_t)(2u + rx_len)); + if (byte != expected) { + ESP_LOGW(TAG, "CRC fail type=0x%02x got=0x%02x exp=0x%02x", + rx_type, byte, expected); + orin_send_nack(tx_q, rx_type, ERR_BAD_CRC); + } else { + dispatch_cmd(rx_type, payload, rx_len, tx_q); + } + state = WAIT_SYNC; + break; + } + } + } +} + +void orin_serial_tx_task(void *arg) +{ + QueueHandle_t tx_q = (QueueHandle_t)arg; + orin_tx_frame_t f; + uint8_t wire[4u + ORIN_MAX_PAYLOAD]; + uint8_t wire_len; + + for (;;) { + if (xQueueReceive(tx_q, &f, portMAX_DELAY) == pdTRUE) { + build_frame(&f, wire, &wire_len); + uart_write_bytes(ORIN_UART_PORT, (const char *)wire, wire_len); + } + } +} diff --git a/esp32s3/balance/main/orin_serial.h b/esp32s3/balance/main/orin_serial.h new file mode 100644 index 0000000..d533794 --- /dev/null +++ b/esp32s3/balance/main/orin_serial.h @@ -0,0 +1,94 @@ +#pragma once +/* orin_serial.h — Orin↔ESP32-S3 BALANCE USB/UART serial protocol (bd-66hx) + * + * Frame layout (matches bd-wim1 esp32_balance_protocol.py exactly): + * [0xAA][LEN][TYPE][PAYLOAD × LEN bytes][CRC8-SMBUS] + * CRC covers LEN + TYPE + PAYLOAD bytes. + * All multi-byte payload fields are big-endian. + * + * Physical: UART0 → CH343 USB-serial → Orin /dev/esp32-balance @ 460800 baud + */ + +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/queue.h" + +/* ── Frame constants ── */ +#define ORIN_SYNC 0xAAu +#define ORIN_MAX_PAYLOAD 62u + +/* ── Command types: Orin → ESP32 ── */ +#define CMD_HEARTBEAT 0x01u +#define CMD_DRIVE 0x02u /* int16 speed + int16 steer, BE */ +#define CMD_ESTOP 0x03u /* uint8: 1=assert, 0=clear */ +#define CMD_ARM 0x04u /* uint8: 1=arm, 0=disarm */ +#define CMD_PID 0x05u /* float32 kp, ki, kd, BE */ + +/* ── Telemetry types: ESP32 → Orin ── */ +#define TELEM_STATUS 0x80u /* status @ 10 Hz */ +#define TELEM_VESC_LEFT 0x81u /* VESC ID 56 telemetry @ 10 Hz */ +#define TELEM_VESC_RIGHT 0x82u /* VESC ID 68 telemetry @ 10 Hz */ +#define RESP_ACK 0xA0u +#define RESP_NACK 0xA1u + +/* ── NACK error codes ── */ +#define ERR_BAD_CRC 0x01u +#define ERR_BAD_LEN 0x02u +#define ERR_ESTOP_ACTIVE 0x03u +#define ERR_DISARMED 0x04u + +/* ── Balance state (mirrored from TELEM_STATUS.balance_state) ── */ +typedef enum { + BAL_DISARMED = 0, + BAL_ARMED = 1, + BAL_TILT_FAULT = 2, + BAL_ESTOP = 3, +} bal_state_t; + +/* ── Shared state written by RX task, consumed by main/vesc tasks ── */ +typedef struct { + volatile int16_t speed; /* -1000..+1000 */ + volatile int16_t steer; /* -1000..+1000 */ + volatile uint32_t updated_ms; /* esp_timer tick at last CMD_DRIVE */ +} orin_drive_t; + +typedef struct { + volatile float kp, ki, kd; + volatile bool updated; +} orin_pid_t; + +typedef struct { + volatile bool armed; + volatile bool estop; + volatile uint32_t hb_last_ms; /* esp_timer tick at last CMD_HEARTBEAT/CMD_DRIVE */ +} orin_control_t; + +/* ── TX frame queue item ── */ +typedef struct { + uint8_t type; + uint8_t len; + uint8_t payload[ORIN_MAX_PAYLOAD]; +} orin_tx_frame_t; + +/* ── Globals (defined in orin_serial.c, extern here) ── */ +extern orin_drive_t g_orin_drive; +extern orin_pid_t g_orin_pid; +extern orin_control_t g_orin_ctrl; + +/* ── API ── */ +void orin_serial_init(void); + +/* Tasks — pass tx_queue as arg to both */ +void orin_serial_rx_task(void *arg); /* arg = QueueHandle_t tx_queue */ +void orin_serial_tx_task(void *arg); /* arg = QueueHandle_t tx_queue */ + +/* Enqueue outbound frames */ +void orin_send_status(QueueHandle_t q, + int16_t pitch_x10, int16_t motor_cmd, + uint16_t vbat_mv, bal_state_t state, uint8_t flags); +void orin_send_vesc(QueueHandle_t q, uint8_t telem_type, + int32_t erpm, uint16_t voltage_mv, + int16_t current_ma, uint16_t temp_c_x10); +void orin_send_ack(QueueHandle_t q, uint8_t cmd_type); +void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err); diff --git a/esp32s3/balance/main/vesc_can.c b/esp32s3/balance/main/vesc_can.c new file mode 100644 index 0000000..866016a --- /dev/null +++ b/esp32s3/balance/main/vesc_can.c @@ -0,0 +1,119 @@ +/* vesc_can.c — VESC CAN TWAI driver (bd-66hx) + * + * Receives VESC STATUS/4/5 frames via TWAI, proxies to Orin over serial. + * Transmits SET_RPM commands from Orin drive requests. + */ + +#include "vesc_can.h" +#include "orin_serial.h" +#include "config.h" +#include "driver/twai.h" +#include "esp_log.h" +#include "esp_timer.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include + +static const char *TAG = "vesc_can"; + +vesc_state_t g_vesc[2] = {0}; + +/* Index for a given VESC node ID: 0=VESC_ID_A, 1=VESC_ID_B */ +static int vesc_idx(uint8_t id) +{ + if (id == VESC_ID_A) return 0; + if (id == VESC_ID_B) return 1; + return -1; +} + +void vesc_can_init(void) +{ + twai_general_config_t gcfg = TWAI_GENERAL_CONFIG_DEFAULT( + (gpio_num_t)VESC_CAN_TX_GPIO, + (gpio_num_t)VESC_CAN_RX_GPIO, + TWAI_MODE_NORMAL); + gcfg.rx_queue_len = VESC_CAN_RX_QUEUE; + + twai_timing_config_t tcfg = TWAI_TIMING_CONFIG_500KBITS(); + twai_filter_config_t fcfg = TWAI_FILTER_CONFIG_ACCEPT_ALL(); + + ESP_ERROR_CHECK(twai_driver_install(&gcfg, &tcfg, &fcfg)); + ESP_ERROR_CHECK(twai_start()); + ESP_LOGI(TAG, "TWAI init OK: tx=%d rx=%d 500kbps", VESC_CAN_TX_GPIO, VESC_CAN_RX_GPIO); +} + +void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm) +{ + uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | vesc_id; + twai_message_t msg = { + .extd = 1, + .identifier = ext_id, + .data_length_code = 4, + }; + uint32_t u = (uint32_t)erpm; + msg.data[0] = (uint8_t)(u >> 24u); + msg.data[1] = (uint8_t)(u >> 16u); + msg.data[2] = (uint8_t)(u >> 8u); + msg.data[3] = (uint8_t)(u); + twai_transmit(&msg, pdMS_TO_TICKS(5)); +} + +void vesc_can_rx_task(void *arg) +{ + QueueHandle_t tx_q = (QueueHandle_t)arg; + twai_message_t msg; + + for (;;) { + if (twai_receive(&msg, pdMS_TO_TICKS(50)) != ESP_OK) { + continue; + } + if (!msg.extd) { + continue; /* ignore standard frames */ + } + + uint8_t pkt_type = (uint8_t)(msg.identifier >> 8u); + uint8_t vesc_id = (uint8_t)(msg.identifier & 0xFFu); + int idx = vesc_idx(vesc_id); + if (idx < 0) { + continue; /* not our VESC */ + } + + uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL); + vesc_state_t *s = &g_vesc[idx]; + + switch (pkt_type) { + case VESC_PKT_STATUS: + if (msg.data_length_code < 8u) { break; } + s->erpm = (int32_t)( + ((uint32_t)msg.data[0] << 24u) | ((uint32_t)msg.data[1] << 16u) | + ((uint32_t)msg.data[2] << 8u) | (uint32_t)msg.data[3]); + s->current_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]); + s->last_rx_ms = now_ms; + /* Proxy to Orin: voltage from STATUS_5 (may be zero until received) */ + { + uint8_t ttype = (vesc_id == VESC_ID_A) ? TELEM_VESC_LEFT : TELEM_VESC_RIGHT; + /* voltage_mv: V×10 → mV (/10 * 1000 = *100); current_ma: A×10 → mA (*100) */ + uint16_t vmv = (uint16_t)((int32_t)s->voltage_x10 * 100); + int16_t ima = (int16_t)((int32_t)s->current_x10 * 100); + orin_send_vesc(tx_q, ttype, s->erpm, vmv, ima, + (uint16_t)s->temp_mot_x10); + } + break; + + case VESC_PKT_STATUS_4: + if (msg.data_length_code < 6u) { break; } + /* T_fet×10, T_mot×10, I_in×10 */ + s->temp_mot_x10 = (int16_t)(((uint16_t)msg.data[2] << 8u) | msg.data[3]); + break; + + case VESC_PKT_STATUS_5: + if (msg.data_length_code < 6u) { break; } + /* int32 tacho (ignored), int16 V_in×10 */ + s->voltage_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]); + break; + + default: + break; + } + } +} diff --git a/esp32s3/balance/main/vesc_can.h b/esp32s3/balance/main/vesc_can.h new file mode 100644 index 0000000..b5d82c7 --- /dev/null +++ b/esp32s3/balance/main/vesc_can.h @@ -0,0 +1,36 @@ +#pragma once +/* vesc_can.h — VESC CAN TWAI driver for ESP32-S3 BALANCE (bd-66hx) + * + * VESC extended CAN ID: (packet_type << 8) | vesc_node_id + * Physical layer: TWAI peripheral → SN65HVD230 → 500 kbps shared bus + */ + +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/queue.h" + +/* ── VESC packet types ── */ +#define VESC_PKT_SET_RPM 3u +#define VESC_PKT_STATUS 9u /* int32 erpm, int16 I×10, int16 duty×1000 */ +#define VESC_PKT_STATUS_4 16u /* int16 T_fet×10, T_mot×10, I_in×10 */ +#define VESC_PKT_STATUS_5 27u /* int32 tacho, int16 V_in×10 */ + +/* ── VESC telemetry snapshot ── */ +typedef struct { + int32_t erpm; /* electrical RPM (STATUS) */ + int16_t current_x10; /* phase current A×10 (STATUS) */ + int16_t voltage_x10; /* bus voltage V×10 (STATUS_5) */ + int16_t temp_mot_x10; /* motor temp °C×10 (STATUS_4) */ + uint32_t last_rx_ms; /* esp_timer ms of last STATUS frame */ +} vesc_state_t; + +/* ── Globals (two VESC nodes: index 0 = VESC_ID_A=56, 1 = VESC_ID_B=68) ── */ +extern vesc_state_t g_vesc[2]; + +/* ── API ── */ +void vesc_can_init(void); +void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm); + +/* RX task — pass tx_queue as arg; forwards STATUS frames to Orin over serial */ +void vesc_can_rx_task(void *arg); /* arg = QueueHandle_t orin_tx_queue */ diff --git a/esp32s3/balance/sdkconfig.defaults b/esp32s3/balance/sdkconfig.defaults new file mode 100644 index 0000000..fe9981c --- /dev/null +++ b/esp32s3/balance/sdkconfig.defaults @@ -0,0 +1,11 @@ +CONFIG_IDF_TARGET="esp32s3" +CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y +CONFIG_FREERTOS_HZ=1000 +CONFIG_ESP_TASK_WDT_EN=y +CONFIG_ESP_TASK_WDT_TIMEOUT_S=5 +CONFIG_TWAI_ISR_IN_IRAM=y +CONFIG_UART_ISR_IN_IRAM=y +CONFIG_ESP_CONSOLE_UART_DEFAULT=y +CONFIG_ESP_CONSOLE_UART_NUM=0 +CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200 +CONFIG_LOG_DEFAULT_LEVEL_INFO=y