sl-firmware
3c895ae37b
Replaces Orin↔ESP32-S3 BALANCE CAN comms (0x300-0x303 / 0x400-0x401)
with binary serial framing over CH343 USB-CDC at 460800 baud.
Protocol matches bd-wim1 (sl-perception) exactly:
Frame: [0xAA][LEN][TYPE][PAYLOAD][CRC8-SMBUS]
CRC covers LEN+TYPE+PAYLOAD, big-endian multi-byte fields.
Commands (Orin→ESP32): HEARTBEAT/DRIVE/ESTOP/ARM/PID
Telemetry (ESP32→Orin): TELEM_STATUS, TELEM_VESC_LEFT (ID 56),
TELEM_VESC_RIGHT (ID 68), ACK/NACK
VESC CAN TWAI kept for motor control; drive commands from Orin
forwarded to VESCs via SET_RPM. Hardware note: SN65HVD230
rewired from IO43/44 to IO2/IO1 to free IO43/44 for CH343.
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
120 lines
3.8 KiB
C
120 lines
3.8 KiB
C
/* 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;
|
||
}
|
||
}
|
||
}
|