sl-firmware
8d411e2603
drive_task stack 2048→4096 (ESP_LOGI with 7 args overflowed 2048 frame). vesc_can_send_rpm: ESP_LOGI→ESP_LOGD (was logging 100x/sec at 50Hz×2). sdkconfig.defaults: add CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 (SPI init call chain overflowed default 3584-byte main task stack). Firmware confirmed stable on bd-66hx: 1 boot cycle in 12 seconds. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
179 lines
6.4 KiB
C
179 lines
6.4 KiB
C
/* 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 "gc9a01.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 <stdio.h>
|
||
#include <string.h>
|
||
#include <stdlib.h>
|
||
|
||
static const char *TAG = "main";
|
||
|
||
static QueueHandle_t s_orin_tx_q;
|
||
|
||
/* ── HUD task: SAULT brand + battery voltage on GC9A01 display ── */
|
||
static void hud_task(void *arg)
|
||
{
|
||
/* Draw static "SAULT" header in orange — scale=3: each char 15×21 px */
|
||
const int hs = 3;
|
||
int hx = (240 - 5 * 6 * hs) / 2;
|
||
display_draw_string_s(hx, 16, "SAULT", COL_ORANGE, COL_BG, hs);
|
||
|
||
char prev[16] = "";
|
||
for (;;) {
|
||
vTaskDelay(pdMS_TO_TICKS(1000));
|
||
|
||
int16_t v10 = g_vesc[0].voltage_x10;
|
||
char vstr[16];
|
||
if (v10 <= 0) {
|
||
snprintf(vstr, sizeof(vstr), " --.-V");
|
||
} else {
|
||
snprintf(vstr, sizeof(vstr), "%2d.%dV", v10 / 10, abs(v10 % 10));
|
||
}
|
||
|
||
if (strcmp(vstr, prev) != 0) {
|
||
const int vs = 4; /* scale=4: each char 20×28 px */
|
||
int nch = (int)strlen(vstr);
|
||
int vx = (240 - nch * 6 * vs) / 2;
|
||
display_fill_rect(0, 95, 240, 7 * vs + 2, COL_BG);
|
||
display_draw_string_s(vx, 97, vstr, COL_WHITE, COL_BG, vs);
|
||
strncpy(prev, vstr, sizeof(prev) - 1);
|
||
}
|
||
}
|
||
}
|
||
|
||
/* ── 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, bit2=twai_bus_off */
|
||
uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 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);
|
||
|
||
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)
|
||
{
|
||
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 (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 */
|
||
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");
|
||
|
||
orin_serial_init();
|
||
vesc_can_init();
|
||
gc9a01_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 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);
|
||
xTaskCreate(orin_serial_tx_task, "orin_tx", 2048, s_orin_tx_q, 9, NULL);
|
||
if (!g_twai_bus_off) {
|
||
xTaskCreate(vesc_can_rx_task, "vesc_rx", 4096, s_orin_tx_q, 10, NULL);
|
||
xTaskCreate(drive_task, "drive", 4096, NULL, 8, NULL);
|
||
} else {
|
||
ESP_LOGW(TAG, "CAN disabled — vesc_rx and drive tasks not started");
|
||
}
|
||
xTaskCreate(telem_task, "telem", 2048, NULL, 5, NULL);
|
||
xTaskCreate(hud_task, "hud", 4096, NULL, 3, NULL);
|
||
|
||
ESP_LOGI(TAG, "all tasks started");
|
||
/* app_main returns — FreeRTOS scheduler continues */
|
||
}
|