saltylab-firmware/esp32s3/balance/main/vesc_can.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};
volatile bool g_can_bus_active = false;
volatile bool g_vesc_alive[2]  = {false, false};

/* 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)
{
    /* Recover from BUS_OFF caused by TX errors when VESCs aren't on the bus yet */
    twai_status_info_t info;
    if (twai_get_status_info(&info) == ESP_OK &&
        info.state == TWAI_STATE_BUS_OFF) {
        ESP_LOGW(TAG, "TWAI BUS_OFF — restarting");
        twai_stop();
        twai_start();
        return;
    }

    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_ping(uint8_t vesc_id)
{
    uint32_t ext_id = ((uint32_t)VESC_PKT_PING << 8u) | vesc_id;
    twai_message_t msg = {
        .extd             = 1,
        .identifier       = ext_id,
        .data_length_code = 1,
    };
    msg.data[0] = VESC_CAN_ID_SELF;
    twai_transmit(&msg, pdMS_TO_TICKS(10));
    ESP_LOGI(TAG, "PING → VESC %u", vesc_id);
}

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 frame_id = (uint8_t)(msg.identifier & 0xFFu);

        g_can_bus_active = true;  /* any extended frame = CAN RX path alive */

        /* PONG frames are addressed to us (frame_id=VESC_CAN_ID_SELF).
         * data[0] = the responding VESC's own node ID. */
        if (pkt_type == VESC_PKT_PONG && frame_id == VESC_CAN_ID_SELF) {
            if (msg.data_length_code >= 1) {
                int pidx = vesc_idx(msg.data[0]);
                if (pidx >= 0) {
                    g_vesc_alive[pidx] = true;
                    ESP_LOGI(TAG, "PONG from VESC %u — alive", msg.data[0]);
                }
            }
            continue;
        }

        int idx = vesc_idx(frame_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 = (frame_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;
        }
    }
}