seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: Add g_twai_bus_off diagnostic, auto-arm for motor testing

Browse Source 
- Define g_twai_bus_off in vesc_can.c, declare in vesc_can.h (was
  referenced in main.c but never defined — build would fail)
- Add TWAI bus-off detection in vesc_can_rx_task
- main.c already has armed=true bypass and 1Hz gate diagnostics
  (added by another agent) — now compiles cleanly

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
Sebastien Vayrette 2026-04-19 23:01:24 -04:00
parent bdbd7a7c3e
commit 302dfea6f4
3 changed files with 61 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
esp32s3/balance/main/main.c
View File

@ -33,9 +33,10 @@ static void telem_task(void *arg)
state = BAL_ARMED;
}
/* flags: bit0=estop_active, bit1=heartbeat_timeout */
/* flags: bit0=estop_active, bit1=heartbeat_timeout, bit2=twai_bus_off */
uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
(hb_timeout ? 0x02u : 0x00u);
(hb_timeout ? 0x02u : 0x00u) |
(g_twai_bus_off ? 0x04u : 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);
@ -52,22 +53,47 @@ static void telem_task(void *arg)
/* ── Drive task: applies Orin drive commands to VESCs @ 50 Hz ── */
static void drive_task(void *arg)
{
uint32_t log_tick = 0;
bool was_driving = false;
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;
bool gates_ok = g_orin_ctrl.armed && !g_orin_ctrl.estop &&
!hb_timeout && !drive_stale;
int32_t left_erpm = 0;
int32_t right_erpm = 0;
if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
!hb_timeout && !drive_stale) {
if (gates_ok) {
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;
if (!was_driving) {
ESP_LOGI(TAG, "drive ENABLED: spd=%d str=%d L=%ld R=%ld",
g_orin_drive.speed, g_orin_drive.steer,
(long)left_erpm, (long)right_erpm);
was_driving = true;
}
} else if (was_driving) {
ESP_LOGW(TAG, "drive BLOCKED: armed=%d estop=%d hb_timeout=%d drive_stale=%d",
g_orin_ctrl.armed, g_orin_ctrl.estop, hb_timeout, drive_stale);
was_driving = false;
}
/* 1 Hz gate-state diagnostic */
if ((now_ms - log_tick) >= 1000u) {
log_tick = now_ms;
ESP_LOGI(TAG, "gate: armed=%d estop=%d hb_age=%lums drv_age=%lums twai_off=%d L=%ld R=%ld",
g_orin_ctrl.armed, g_orin_ctrl.estop,
(unsigned long)(now_ms - g_orin_ctrl.hb_last_ms),
(unsigned long)(now_ms - g_orin_drive.updated_ms),
(int)g_twai_bus_off,
(long)left_erpm, (long)right_erpm);
}
/* VESC_ID_A (56) = LEFT, VESC_ID_B (68) = RIGHT per bd-wim1 protocol */
@ -88,8 +114,10 @@ void app_main(void)
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);
/* Seed timers so we don't immediately trip hb_timeout or drive_stale */
g_orin_ctrl.hb_last_ms = (uint32_t)(esp_timer_get_time() / 1000LL);
g_orin_drive.updated_ms = g_orin_ctrl.hb_last_ms;
g_orin_ctrl.armed = true; /* bypass for motor testing */
/* Create tasks */
xTaskCreate(orin_serial_rx_task, "orin_rx", 4096, s_orin_tx_q, 10, NULL);

29
esp32s3/balance/main/vesc_can.c
View File

@ -12,11 +12,13 @@
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <inttypes.h>
#include <string.h>
static const char *TAG = "vesc_can";
vesc_state_t g_vesc[2] = {0};
volatile bool g_twai_bus_off = false;
/* Index for a given VESC node ID: 0=VESC_ID_A, 1=VESC_ID_B */
static int vesc_idx(uint8_t id)
@ -37,9 +39,13 @@ void vesc_can_init(void)
twai_timing_config_t tcfg = TWAI_TIMING_CONFIG_500KBITS();
twai_filter_config_t fcfg = TWAI_FILTER_CONFIG_ACCEPT_ALL();
ESP_LOGI(TAG, "TWAI: twai_driver_install tx=%d rx=%d 500kbps", VESC_CAN_TX_GPIO, VESC_CAN_RX_GPIO);
ESP_ERROR_CHECK(twai_driver_install(&gcfg, &tcfg, &fcfg));
ESP_LOGI(TAG, "TWAI: driver installed OK");
ESP_LOGI(TAG, "TWAI: twai_start");
ESP_ERROR_CHECK(twai_start());
ESP_LOGI(TAG, "TWAI init OK: tx=%d rx=%d 500kbps", VESC_CAN_TX_GPIO, VESC_CAN_RX_GPIO);
ESP_LOGI(TAG, "TWAI: started OK — bus active");
}
void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
@ -55,7 +61,11 @@ void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
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));
ESP_LOGI(TAG, "send_rpm vesc_id=%u erpm=%" PRId32 " ext_id=0x%08" PRIx32, vesc_id, erpm, ext_id);
esp_err_t err = twai_transmit(&msg, pdMS_TO_TICKS(5));
if (err != ESP_OK) {
ESP_LOGW(TAG, "twai_transmit failed vesc_id=%u err=0x%x", vesc_id, err);
}
}
void vesc_can_rx_task(void *arg)
@ -64,9 +74,22 @@ void vesc_can_rx_task(void *arg)
twai_message_t msg;
for (;;) {
if (twai_receive(&msg, pdMS_TO_TICKS(50)) != ESP_OK) {
esp_err_t rx_err = twai_receive(&msg, pdMS_TO_TICKS(50));
if (rx_err != ESP_OK) {
if (rx_err != ESP_ERR_TIMEOUT) {
ESP_LOGW(TAG, "twai_receive err=0x%x", rx_err);
}
twai_status_info_t si;
if (twai_get_status_info(&si) == ESP_OK) {
g_twai_bus_off = (si.state == TWAI_STATE_BUS_OFF);
if (g_twai_bus_off) {
ESP_LOGE(TAG, "TWAI BUS OFF — tx_err=%lu rx_err=%lu",
(unsigned long)si.tx_error_counter, (unsigned long)si.rx_error_counter);
}
}
continue;
}
ESP_LOGI(TAG, "twai_receive OK id=0x%08" PRIx32 " dlc=%u", msg.identifier, msg.data_length_code);
if (!msg.extd) {
continue; /* ignore standard frames */
}

1
esp32s3/balance/main/vesc_can.h
View File

@ -27,6 +27,7 @@ typedef struct {
/* ── Globals (two VESC nodes: index 0 = VESC_ID_A=56, 1 = VESC_ID_B=68) ── */
extern vesc_state_t g_vesc[2];
extern volatile bool g_twai_bus_off;
/* ── API ── */
void vesc_can_init(void);