sl-firmware
1f88835fac
Add active VESC probing so the Orin binary protocol reports CAN RX health: - vesc_can_ping(): sends CAN_PACKET_PING (17) to each VESC at startup - vesc_can_rx_task: handles CAN_PACKET_PONG (18) → sets g_vesc_alive[i] - g_can_bus_active: set on any extended CAN frame received - STATUS flags now include bit4=can_bus_active, bit5=vesc_a_alive, bit6=vesc_b_alive - Test script decodes and reports twai_state, can_bus_active, vesc_a/b_alive - Fix cosmetic: VESC IDs 56=LEFT 68=RIGHT (was wrong 61/79 in print line) Confirmed diagnostic: can_bus_active=False — VESCs ACK SET_RPM commands (TWAI stays RUNNING) but broadcast zero data frames. Root cause: VESC CAN Status Message Mode is Disabled. Fix: set mode ≥ 1 in VESC Tool. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
129 lines
4.5 KiB
C
129 lines
4.5 KiB
C
/* main.c — ESP32-S3 BALANCE app_main (bd-66hx + OTA beads) */
|
||
|
||
#include "orin_serial.h"
|
||
#include "vesc_can.h"
|
||
#include "gc9a01.h"
|
||
#include "gitea_ota.h"
|
||
#include "ota_self.h"
|
||
#include "uart_ota.h"
|
||
#include "ota_display.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 "driver/twai.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, bit1=hb_timeout, bits[3:2]=twai_state,
|
||
* bit4=can_bus_active, bit5=vesc_a_alive, bit6=vesc_b_alive */
|
||
twai_status_info_t twai_info = {0};
|
||
twai_get_status_info(&twai_info);
|
||
uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
|
||
(hb_timeout ? 0x02u : 0x00u) |
|
||
((twai_info.state & 0x03u) << 2u) |
|
||
(g_can_bus_active ? 0x10u : 0x00u) |
|
||
(g_vesc_alive[0] ? 0x20u : 0x00u) |
|
||
(g_vesc_alive[1] ? 0x40u : 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_can_send_rpm(VESC_ID_A, left_erpm); /* VESC 56 = left */
|
||
vesc_can_send_rpm(VESC_ID_B, right_erpm); /* VESC 68 = right */
|
||
}
|
||
}
|
||
|
||
void app_main(void)
|
||
{
|
||
ESP_LOGI(TAG, "ESP32-S3 BALANCE starting");
|
||
|
||
/* OTA rollback health check — must be called within OTA_ROLLBACK_WINDOW_S */
|
||
ota_self_health_check();
|
||
|
||
/* Init peripherals — gc9a01 before vesc_can so BL/GPIO2 is high before TWAI takes it */
|
||
gc9a01_init();
|
||
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);
|
||
|
||
/* PING both VESCs after vesc_rx task is running — PONG sets g_vesc_alive[] */
|
||
vTaskDelay(pdMS_TO_TICKS(100));
|
||
vesc_can_ping(VESC_ID_A);
|
||
vesc_can_ping(VESC_ID_B);
|
||
|
||
/* OTA subsystem — WiFi version checker + display overlay */
|
||
gitea_ota_init();
|
||
ota_display_init();
|
||
|
||
ESP_LOGI(TAG, "all tasks started");
|
||
/* app_main returns — FreeRTOS scheduler continues */
|
||
}
|