Compare commits
No commits in common. "d9e7acfa0d55fd7607eb6e76f6a078842a2b71ba" and "c02faf3ac2fb536343271728eef1d1bc445f6e00" have entirely different histories.
d9e7acfa0d
...
c02faf3ac2
@ -1,3 +0,0 @@
|
|||||||
cmake_minimum_required(VERSION 3.16)
|
|
||||||
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
|
|
||||||
project(esp32s3_balance)
|
|
||||||
@ -1,4 +0,0 @@
|
|||||||
idf_component_register(
|
|
||||||
SRCS "main.c" "orin_serial.c" "vesc_can.c"
|
|
||||||
INCLUDE_DIRS "."
|
|
||||||
)
|
|
||||||
@ -1,42 +0,0 @@
|
|||||||
#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
|
|
||||||
@ -1,111 +0,0 @@
|
|||||||
/* 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 <string.h>
|
|
||||||
|
|
||||||
static const char *TAG = "main";
|
|
||||||
|
|
||||||
static QueueHandle_t s_orin_tx_q;
|
|
||||||
|
|
||||||
/* ── Telemetry task: sends TELEM_STATUS to Orin at 10 Hz ── */
|
|
||||||
static void telem_task(void *arg)
|
|
||||||
{
|
|
||||||
for (;;) {
|
|
||||||
vTaskDelay(pdMS_TO_TICKS(TELEM_STATUS_PERIOD_MS));
|
|
||||||
|
|
||||||
uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
|
||||||
bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
|
|
||||||
|
|
||||||
/* Determine balance state for telemetry */
|
|
||||||
bal_state_t state;
|
|
||||||
if (g_orin_ctrl.estop) {
|
|
||||||
state = BAL_ESTOP;
|
|
||||||
} else if (!g_orin_ctrl.armed) {
|
|
||||||
state = BAL_DISARMED;
|
|
||||||
} else {
|
|
||||||
state = BAL_ARMED;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* flags: bit0=estop_active, bit1=heartbeat_timeout */
|
|
||||||
uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
|
|
||||||
(hb_timeout ? 0x02u : 0x00u);
|
|
||||||
|
|
||||||
/* Battery voltage from VESC_ID_A STATUS_5 (V×10 → mV) */
|
|
||||||
uint16_t vbat_mv = (uint16_t)((int32_t)g_vesc[0].voltage_x10 * 100);
|
|
||||||
|
|
||||||
orin_send_status(s_orin_tx_q,
|
|
||||||
0, /* pitch_x10: stub — full IMU in future bead */
|
|
||||||
0, /* motor_cmd: stub */
|
|
||||||
vbat_mv,
|
|
||||||
state,
|
|
||||||
flags);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ── Drive task: applies Orin drive commands to VESCs @ 50 Hz ── */
|
|
||||||
static void drive_task(void *arg)
|
|
||||||
{
|
|
||||||
for (;;) {
|
|
||||||
vTaskDelay(pdMS_TO_TICKS(20)); /* 50 Hz */
|
|
||||||
|
|
||||||
uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
|
||||||
bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
|
|
||||||
bool drive_stale = (now_ms - g_orin_drive.updated_ms) > DRIVE_TIMEOUT_MS;
|
|
||||||
|
|
||||||
int32_t left_erpm = 0;
|
|
||||||
int32_t right_erpm = 0;
|
|
||||||
|
|
||||||
if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
|
|
||||||
!hb_timeout && !drive_stale) {
|
|
||||||
int32_t spd = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
|
|
||||||
int32_t str = (int32_t)g_orin_drive.steer * RPM_PER_STEER_UNIT;
|
|
||||||
left_erpm = spd + str;
|
|
||||||
right_erpm = spd - str;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* VESC_ID_A (56) = LEFT, VESC_ID_B (68) = RIGHT per bd-wim1 protocol */
|
|
||||||
vesc_can_send_rpm(VESC_ID_A, left_erpm);
|
|
||||||
vesc_can_send_rpm(VESC_ID_B, right_erpm);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void app_main(void)
|
|
||||||
{
|
|
||||||
ESP_LOGI(TAG, "ESP32-S3 BALANCE 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 */
|
|
||||||
}
|
|
||||||
@ -1,292 +0,0 @@
|
|||||||
/* 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 <string.h>
|
|
||||||
|
|
||||||
static const char *TAG = "orin";
|
|
||||||
|
|
||||||
/* ── Shared state ── */
|
|
||||||
orin_drive_t g_orin_drive = {0};
|
|
||||||
orin_pid_t g_orin_pid = {0};
|
|
||||||
orin_control_t g_orin_ctrl = {.armed = false, .estop = false, .hb_last_ms = 0};
|
|
||||||
|
|
||||||
/* ── CRC8-SMBUS (poly=0x07, init=0x00) ── */
|
|
||||||
static uint8_t crc8(const uint8_t *data, uint8_t len)
|
|
||||||
{
|
|
||||||
uint8_t crc = 0x00u;
|
|
||||||
for (uint8_t i = 0; i < len; i++) {
|
|
||||||
crc ^= data[i];
|
|
||||||
for (uint8_t b = 0; b < 8u; b++) {
|
|
||||||
crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return crc;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ── Frame builder ── */
|
|
||||||
static void build_frame(orin_tx_frame_t *f, uint8_t out[/* ORIN_MAX_PAYLOAD + 4 */], uint8_t *out_len)
|
|
||||||
{
|
|
||||||
/* [SYNC][LEN][TYPE][PAYLOAD...][CRC] */
|
|
||||||
uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD];
|
|
||||||
crc_buf[0] = f->len;
|
|
||||||
crc_buf[1] = f->type;
|
|
||||||
memcpy(&crc_buf[2], f->payload, f->len);
|
|
||||||
uint8_t crc = crc8(crc_buf, (uint8_t)(2u + f->len));
|
|
||||||
|
|
||||||
out[0] = ORIN_SYNC;
|
|
||||||
out[1] = f->len;
|
|
||||||
out[2] = f->type;
|
|
||||||
memcpy(&out[3], f->payload, f->len);
|
|
||||||
out[3u + f->len] = crc;
|
|
||||||
*out_len = (uint8_t)(4u + f->len);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ── Enqueue helpers ── */
|
|
||||||
static void enqueue(QueueHandle_t q, uint8_t type, const uint8_t *payload, uint8_t len)
|
|
||||||
{
|
|
||||||
orin_tx_frame_t f = {.type = type, .len = len};
|
|
||||||
if (len > 0u && payload) {
|
|
||||||
memcpy(f.payload, payload, len);
|
|
||||||
}
|
|
||||||
if (xQueueSend(q, &f, 0) != pdTRUE) {
|
|
||||||
ESP_LOGW(TAG, "tx queue full, dropped type=0x%02x", type);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void orin_send_ack(QueueHandle_t q, uint8_t cmd_type)
|
|
||||||
{
|
|
||||||
enqueue(q, RESP_ACK, &cmd_type, 1u);
|
|
||||||
}
|
|
||||||
|
|
||||||
void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err)
|
|
||||||
{
|
|
||||||
uint8_t p[2] = {cmd_type, err};
|
|
||||||
enqueue(q, RESP_NACK, p, 2u);
|
|
||||||
}
|
|
||||||
|
|
||||||
void orin_send_status(QueueHandle_t q,
|
|
||||||
int16_t pitch_x10, int16_t motor_cmd,
|
|
||||||
uint16_t vbat_mv, bal_state_t state, uint8_t flags)
|
|
||||||
{
|
|
||||||
/* int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, uint8 state, uint8 flags — BE */
|
|
||||||
uint8_t p[8];
|
|
||||||
p[0] = (uint8_t)((uint16_t)pitch_x10 >> 8u);
|
|
||||||
p[1] = (uint8_t)((uint16_t)pitch_x10);
|
|
||||||
p[2] = (uint8_t)((uint16_t)motor_cmd >> 8u);
|
|
||||||
p[3] = (uint8_t)((uint16_t)motor_cmd);
|
|
||||||
p[4] = (uint8_t)(vbat_mv >> 8u);
|
|
||||||
p[5] = (uint8_t)(vbat_mv);
|
|
||||||
p[6] = (uint8_t)state;
|
|
||||||
p[7] = flags;
|
|
||||||
enqueue(q, TELEM_STATUS, p, 8u);
|
|
||||||
}
|
|
||||||
|
|
||||||
void orin_send_vesc(QueueHandle_t q, uint8_t telem_type,
|
|
||||||
int32_t erpm, uint16_t voltage_mv,
|
|
||||||
int16_t current_ma, uint16_t temp_c_x10)
|
|
||||||
{
|
|
||||||
/* int32 erpm, uint16 voltage_mv, int16 current_ma, uint16 temp_c_x10 — BE */
|
|
||||||
uint8_t p[10];
|
|
||||||
uint32_t u = (uint32_t)erpm;
|
|
||||||
p[0] = (uint8_t)(u >> 24u);
|
|
||||||
p[1] = (uint8_t)(u >> 16u);
|
|
||||||
p[2] = (uint8_t)(u >> 8u);
|
|
||||||
p[3] = (uint8_t)(u);
|
|
||||||
p[4] = (uint8_t)(voltage_mv >> 8u);
|
|
||||||
p[5] = (uint8_t)(voltage_mv);
|
|
||||||
p[6] = (uint8_t)((uint16_t)current_ma >> 8u);
|
|
||||||
p[7] = (uint8_t)((uint16_t)current_ma);
|
|
||||||
p[8] = (uint8_t)(temp_c_x10 >> 8u);
|
|
||||||
p[9] = (uint8_t)(temp_c_x10);
|
|
||||||
enqueue(q, telem_type, p, 10u);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ── UART init ── */
|
|
||||||
void orin_serial_init(void)
|
|
||||||
{
|
|
||||||
uart_config_t cfg = {
|
|
||||||
.baud_rate = ORIN_UART_BAUD,
|
|
||||||
.data_bits = UART_DATA_8_BITS,
|
|
||||||
.parity = UART_PARITY_DISABLE,
|
|
||||||
.stop_bits = UART_STOP_BITS_1,
|
|
||||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
|
||||||
};
|
|
||||||
ESP_ERROR_CHECK(uart_param_config(ORIN_UART_PORT, &cfg));
|
|
||||||
ESP_ERROR_CHECK(uart_set_pin(ORIN_UART_PORT,
|
|
||||||
ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO,
|
|
||||||
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
|
||||||
ESP_ERROR_CHECK(uart_driver_install(ORIN_UART_PORT, ORIN_UART_RX_BUF, 0,
|
|
||||||
0, NULL, 0));
|
|
||||||
ESP_LOGI(TAG, "UART%d init OK: tx=%d rx=%d baud=%d",
|
|
||||||
ORIN_UART_PORT, ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO, ORIN_UART_BAUD);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ── RX parser state machine ── */
|
|
||||||
typedef enum {
|
|
||||||
WAIT_SYNC,
|
|
||||||
WAIT_LEN,
|
|
||||||
WAIT_TYPE,
|
|
||||||
WAIT_PAYLOAD,
|
|
||||||
WAIT_CRC,
|
|
||||||
} rx_state_t;
|
|
||||||
|
|
||||||
static void dispatch_cmd(uint8_t type, const uint8_t *payload, uint8_t len,
|
|
||||||
QueueHandle_t tx_q)
|
|
||||||
{
|
|
||||||
uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
|
||||||
|
|
||||||
switch (type) {
|
|
||||||
case CMD_HEARTBEAT:
|
|
||||||
g_orin_ctrl.hb_last_ms = now_ms;
|
|
||||||
orin_send_ack(tx_q, type);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case CMD_DRIVE:
|
|
||||||
if (len < 4u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
|
||||||
if (g_orin_ctrl.estop) { orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE); break; }
|
|
||||||
if (!g_orin_ctrl.armed) { orin_send_nack(tx_q, type, ERR_DISARMED); break; }
|
|
||||||
g_orin_drive.speed = (int16_t)(((uint16_t)payload[0] << 8u) | payload[1]);
|
|
||||||
g_orin_drive.steer = (int16_t)(((uint16_t)payload[2] << 8u) | payload[3]);
|
|
||||||
g_orin_drive.updated_ms = now_ms;
|
|
||||||
g_orin_ctrl.hb_last_ms = now_ms; /* drive counts as heartbeat */
|
|
||||||
orin_send_ack(tx_q, type);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case CMD_ESTOP:
|
|
||||||
if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
|
||||||
g_orin_ctrl.estop = (payload[0] != 0u);
|
|
||||||
if (g_orin_ctrl.estop) {
|
|
||||||
g_orin_drive.speed = 0;
|
|
||||||
g_orin_drive.steer = 0;
|
|
||||||
}
|
|
||||||
orin_send_ack(tx_q, type);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case CMD_ARM:
|
|
||||||
if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
|
||||||
if (g_orin_ctrl.estop && payload[0] != 0u) {
|
|
||||||
/* cannot arm while estop is active */
|
|
||||||
orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
g_orin_ctrl.armed = (payload[0] != 0u);
|
|
||||||
if (!g_orin_ctrl.armed) {
|
|
||||||
g_orin_drive.speed = 0;
|
|
||||||
g_orin_drive.steer = 0;
|
|
||||||
}
|
|
||||||
orin_send_ack(tx_q, type);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case CMD_PID:
|
|
||||||
if (len < 12u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
|
|
||||||
/* float32 big-endian: copy and swap bytes */
|
|
||||||
{
|
|
||||||
uint32_t raw;
|
|
||||||
raw = ((uint32_t)payload[0] << 24u) | ((uint32_t)payload[1] << 16u) |
|
|
||||||
((uint32_t)payload[2] << 8u) | (uint32_t)payload[3];
|
|
||||||
memcpy((void*)&g_orin_pid.kp, &raw, 4u);
|
|
||||||
raw = ((uint32_t)payload[4] << 24u) | ((uint32_t)payload[5] << 16u) |
|
|
||||||
((uint32_t)payload[6] << 8u) | (uint32_t)payload[7];
|
|
||||||
memcpy((void*)&g_orin_pid.ki, &raw, 4u);
|
|
||||||
raw = ((uint32_t)payload[8] << 24u) | ((uint32_t)payload[9] << 16u) |
|
|
||||||
((uint32_t)payload[10] << 8u) | (uint32_t)payload[11];
|
|
||||||
memcpy((void*)&g_orin_pid.kd, &raw, 4u);
|
|
||||||
g_orin_pid.updated = true;
|
|
||||||
}
|
|
||||||
orin_send_ack(tx_q, type);
|
|
||||||
break;
|
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,94 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
/* orin_serial.h — Orin↔ESP32-S3 BALANCE USB/UART serial protocol (bd-66hx)
|
|
||||||
*
|
|
||||||
* Frame layout (matches bd-wim1 esp32_balance_protocol.py exactly):
|
|
||||||
* [0xAA][LEN][TYPE][PAYLOAD × LEN bytes][CRC8-SMBUS]
|
|
||||||
* CRC covers LEN + TYPE + PAYLOAD bytes.
|
|
||||||
* All multi-byte payload fields are big-endian.
|
|
||||||
*
|
|
||||||
* Physical: UART0 → CH343 USB-serial → Orin /dev/esp32-balance @ 460800 baud
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include "freertos/FreeRTOS.h"
|
|
||||||
#include "freertos/queue.h"
|
|
||||||
|
|
||||||
/* ── Frame constants ── */
|
|
||||||
#define ORIN_SYNC 0xAAu
|
|
||||||
#define ORIN_MAX_PAYLOAD 62u
|
|
||||||
|
|
||||||
/* ── Command types: Orin → ESP32 ── */
|
|
||||||
#define CMD_HEARTBEAT 0x01u
|
|
||||||
#define CMD_DRIVE 0x02u /* int16 speed + int16 steer, BE */
|
|
||||||
#define CMD_ESTOP 0x03u /* uint8: 1=assert, 0=clear */
|
|
||||||
#define CMD_ARM 0x04u /* uint8: 1=arm, 0=disarm */
|
|
||||||
#define CMD_PID 0x05u /* float32 kp, ki, kd, BE */
|
|
||||||
|
|
||||||
/* ── Telemetry types: ESP32 → Orin ── */
|
|
||||||
#define TELEM_STATUS 0x80u /* status @ 10 Hz */
|
|
||||||
#define TELEM_VESC_LEFT 0x81u /* VESC ID 56 telemetry @ 10 Hz */
|
|
||||||
#define TELEM_VESC_RIGHT 0x82u /* VESC ID 68 telemetry @ 10 Hz */
|
|
||||||
#define 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);
|
|
||||||
@ -1,119 +0,0 @@
|
|||||||
/* vesc_can.c — VESC CAN TWAI driver (bd-66hx)
|
|
||||||
*
|
|
||||||
* Receives VESC STATUS/4/5 frames via TWAI, proxies to Orin over serial.
|
|
||||||
* Transmits SET_RPM commands from Orin drive requests.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "vesc_can.h"
|
|
||||||
#include "orin_serial.h"
|
|
||||||
#include "config.h"
|
|
||||||
#include "driver/twai.h"
|
|
||||||
#include "esp_log.h"
|
|
||||||
#include "esp_timer.h"
|
|
||||||
#include "freertos/FreeRTOS.h"
|
|
||||||
#include "freertos/task.h"
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
static const char *TAG = "vesc_can";
|
|
||||||
|
|
||||||
vesc_state_t g_vesc[2] = {0};
|
|
||||||
|
|
||||||
/* Index for a given VESC node ID: 0=VESC_ID_A, 1=VESC_ID_B */
|
|
||||||
static int vesc_idx(uint8_t id)
|
|
||||||
{
|
|
||||||
if (id == VESC_ID_A) return 0;
|
|
||||||
if (id == VESC_ID_B) return 1;
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
void vesc_can_init(void)
|
|
||||||
{
|
|
||||||
twai_general_config_t gcfg = TWAI_GENERAL_CONFIG_DEFAULT(
|
|
||||||
(gpio_num_t)VESC_CAN_TX_GPIO,
|
|
||||||
(gpio_num_t)VESC_CAN_RX_GPIO,
|
|
||||||
TWAI_MODE_NORMAL);
|
|
||||||
gcfg.rx_queue_len = VESC_CAN_RX_QUEUE;
|
|
||||||
|
|
||||||
twai_timing_config_t tcfg = TWAI_TIMING_CONFIG_500KBITS();
|
|
||||||
twai_filter_config_t fcfg = TWAI_FILTER_CONFIG_ACCEPT_ALL();
|
|
||||||
|
|
||||||
ESP_ERROR_CHECK(twai_driver_install(&gcfg, &tcfg, &fcfg));
|
|
||||||
ESP_ERROR_CHECK(twai_start());
|
|
||||||
ESP_LOGI(TAG, "TWAI init OK: tx=%d rx=%d 500kbps", VESC_CAN_TX_GPIO, VESC_CAN_RX_GPIO);
|
|
||||||
}
|
|
||||||
|
|
||||||
void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
|
|
||||||
{
|
|
||||||
uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | vesc_id;
|
|
||||||
twai_message_t msg = {
|
|
||||||
.extd = 1,
|
|
||||||
.identifier = ext_id,
|
|
||||||
.data_length_code = 4,
|
|
||||||
};
|
|
||||||
uint32_t u = (uint32_t)erpm;
|
|
||||||
msg.data[0] = (uint8_t)(u >> 24u);
|
|
||||||
msg.data[1] = (uint8_t)(u >> 16u);
|
|
||||||
msg.data[2] = (uint8_t)(u >> 8u);
|
|
||||||
msg.data[3] = (uint8_t)(u);
|
|
||||||
twai_transmit(&msg, pdMS_TO_TICKS(5));
|
|
||||||
}
|
|
||||||
|
|
||||||
void vesc_can_rx_task(void *arg)
|
|
||||||
{
|
|
||||||
QueueHandle_t tx_q = (QueueHandle_t)arg;
|
|
||||||
twai_message_t msg;
|
|
||||||
|
|
||||||
for (;;) {
|
|
||||||
if (twai_receive(&msg, pdMS_TO_TICKS(50)) != ESP_OK) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
if (!msg.extd) {
|
|
||||||
continue; /* ignore standard frames */
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t pkt_type = (uint8_t)(msg.identifier >> 8u);
|
|
||||||
uint8_t vesc_id = (uint8_t)(msg.identifier & 0xFFu);
|
|
||||||
int idx = vesc_idx(vesc_id);
|
|
||||||
if (idx < 0) {
|
|
||||||
continue; /* not our VESC */
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
|
|
||||||
vesc_state_t *s = &g_vesc[idx];
|
|
||||||
|
|
||||||
switch (pkt_type) {
|
|
||||||
case VESC_PKT_STATUS:
|
|
||||||
if (msg.data_length_code < 8u) { break; }
|
|
||||||
s->erpm = (int32_t)(
|
|
||||||
((uint32_t)msg.data[0] << 24u) | ((uint32_t)msg.data[1] << 16u) |
|
|
||||||
((uint32_t)msg.data[2] << 8u) | (uint32_t)msg.data[3]);
|
|
||||||
s->current_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]);
|
|
||||||
s->last_rx_ms = now_ms;
|
|
||||||
/* Proxy to Orin: voltage from STATUS_5 (may be zero until received) */
|
|
||||||
{
|
|
||||||
uint8_t ttype = (vesc_id == VESC_ID_A) ? TELEM_VESC_LEFT : TELEM_VESC_RIGHT;
|
|
||||||
/* voltage_mv: V×10 → mV (/10 * 1000 = *100); current_ma: A×10 → mA (*100) */
|
|
||||||
uint16_t vmv = (uint16_t)((int32_t)s->voltage_x10 * 100);
|
|
||||||
int16_t ima = (int16_t)((int32_t)s->current_x10 * 100);
|
|
||||||
orin_send_vesc(tx_q, ttype, s->erpm, vmv, ima,
|
|
||||||
(uint16_t)s->temp_mot_x10);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case VESC_PKT_STATUS_4:
|
|
||||||
if (msg.data_length_code < 6u) { break; }
|
|
||||||
/* T_fet×10, T_mot×10, I_in×10 */
|
|
||||||
s->temp_mot_x10 = (int16_t)(((uint16_t)msg.data[2] << 8u) | msg.data[3]);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case VESC_PKT_STATUS_5:
|
|
||||||
if (msg.data_length_code < 6u) { break; }
|
|
||||||
/* int32 tacho (ignored), int16 V_in×10 */
|
|
||||||
s->voltage_x10 = (int16_t)(((uint16_t)msg.data[4] << 8u) | msg.data[5]);
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,36 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
/* vesc_can.h — VESC CAN TWAI driver for ESP32-S3 BALANCE (bd-66hx)
|
|
||||||
*
|
|
||||||
* VESC extended CAN ID: (packet_type << 8) | vesc_node_id
|
|
||||||
* Physical layer: TWAI peripheral → SN65HVD230 → 500 kbps shared bus
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include "freertos/FreeRTOS.h"
|
|
||||||
#include "freertos/queue.h"
|
|
||||||
|
|
||||||
/* ── VESC packet types ── */
|
|
||||||
#define VESC_PKT_SET_RPM 3u
|
|
||||||
#define VESC_PKT_STATUS 9u /* int32 erpm, int16 I×10, int16 duty×1000 */
|
|
||||||
#define VESC_PKT_STATUS_4 16u /* int16 T_fet×10, T_mot×10, I_in×10 */
|
|
||||||
#define VESC_PKT_STATUS_5 27u /* int32 tacho, int16 V_in×10 */
|
|
||||||
|
|
||||||
/* ── VESC telemetry snapshot ── */
|
|
||||||
typedef struct {
|
|
||||||
int32_t erpm; /* electrical RPM (STATUS) */
|
|
||||||
int16_t current_x10; /* phase current A×10 (STATUS) */
|
|
||||||
int16_t voltage_x10; /* bus voltage V×10 (STATUS_5) */
|
|
||||||
int16_t temp_mot_x10; /* motor temp °C×10 (STATUS_4) */
|
|
||||||
uint32_t last_rx_ms; /* esp_timer ms of last STATUS frame */
|
|
||||||
} vesc_state_t;
|
|
||||||
|
|
||||||
/* ── Globals (two VESC nodes: index 0 = VESC_ID_A=56, 1 = VESC_ID_B=68) ── */
|
|
||||||
extern vesc_state_t g_vesc[2];
|
|
||||||
|
|
||||||
/* ── API ── */
|
|
||||||
void vesc_can_init(void);
|
|
||||||
void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm);
|
|
||||||
|
|
||||||
/* RX task — pass tx_queue as arg; forwards STATUS frames to Orin over serial */
|
|
||||||
void vesc_can_rx_task(void *arg); /* arg = QueueHandle_t orin_tx_queue */
|
|
||||||
@ -1,11 +0,0 @@
|
|||||||
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
|
|
||||||
Loading…
x
Reference in New Issue
Block a user