diff --git a/esp32s3/balance/main/main.c b/esp32s3/balance/main/main.c index 1704c1f..7a6968e 100644 --- a/esp32s3/balance/main/main.c +++ b/esp32s3/balance/main/main.c @@ -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); diff --git a/esp32s3/balance/main/vesc_can.c b/esp32s3/balance/main/vesc_can.c index 866016a..290993a 100644 --- a/esp32s3/balance/main/vesc_can.c +++ b/esp32s3/balance/main/vesc_can.c @@ -12,11 +12,13 @@ #include "esp_timer.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" +#include #include 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 */ } diff --git a/esp32s3/balance/main/vesc_can.h b/esp32s3/balance/main/vesc_can.h index b5d82c7..ab1cf17 100644 --- a/esp32s3/balance/main/vesc_can.h +++ b/esp32s3/balance/main/vesc_can.h @@ -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);