ASER-NAV/App/Can/snc_can_app.c

#include "snc_can_app.h"

#include <string.h>
#include <math.h>
#include "FreeRTOS.h"
#include "task.h"

/* ========================= 外部句柄 ========================= */
extern FDCAN_HandleTypeDef hfdcan1;

/* ========================= 在线判定参数 ========================= */
#define SNC_STATUS_TIMEOUT_MS      200U
#define SNC_ACTUAL_RPM_TIMEOUT_MS  200U
#define SNC_TARGET_RPM_TIMEOUT_MS  200U
#define SNC_COMM_DIAG_TIMEOUT_MS   300U

/* ========================= FDCAN 过滤器索引 ========================= */
#define SNC_FILTER_INDEX_STATUS      0U
#define SNC_FILTER_INDEX_ACTUAL_RPM  1U
#define SNC_FILTER_INDEX_TARGET_RPM  2U
#define SNC_FILTER_INDEX_COMM_DIAG   3U
#define SNC_FILTER_INDEX_ODOM        4U

SNC_CAN_AppContext_t g_snc_can_app;

/* ========================= CRC8-SAE J1850 查表 ========================= */
static const uint8_t s_crc8_j1850_table[256] =
{
    0x00U, 0x1DU, 0x3AU, 0x27U, 0x74U, 0x69U, 0x4EU, 0x53U, 0xE8U, 0xF5U, 0xD2U, 0xCFU, 0x9CU, 0x81U, 0xA6U, 0xBBU,
    0xCDU, 0xD0U, 0xF7U, 0xEAU, 0xB9U, 0xA4U, 0x83U, 0x9EU, 0x25U, 0x38U, 0x1FU, 0x02U, 0x51U, 0x4CU, 0x6BU, 0x76U,
    0x87U, 0x9AU, 0xBDU, 0xA0U, 0xF3U, 0xEEU, 0xC9U, 0xD4U, 0x6FU, 0x72U, 0x55U, 0x48U, 0x1BU, 0x06U, 0x21U, 0x3CU,
    0x4AU, 0x57U, 0x70U, 0x6DU, 0x3EU, 0x23U, 0x04U, 0x19U, 0xA2U, 0xBFU, 0x98U, 0x85U, 0xD6U, 0xCBU, 0xECU, 0xF1U,
    0x13U, 0x0EU, 0x29U, 0x34U, 0x67U, 0x7AU, 0x5DU, 0x40U, 0xFBU, 0xE6U, 0xC1U, 0xDCU, 0x8FU, 0x92U, 0xB5U, 0xA8U,
    0xDEU, 0xC3U, 0xE4U, 0xF9U, 0xAAU, 0xB7U, 0x90U, 0x8DU, 0x36U, 0x2BU, 0x0CU, 0x11U, 0x42U, 0x5FU, 0x78U, 0x65U,
    0x94U, 0x89U, 0xAEU, 0xB3U, 0xE0U, 0xFDU, 0xDAU, 0xC7U, 0x7CU, 0x61U, 0x46U, 0x5BU, 0x08U, 0x15U, 0x32U, 0x2FU,
    0x59U, 0x44U, 0x63U, 0x7EU, 0x2DU, 0x30U, 0x17U, 0x0AU, 0xB1U, 0xACU, 0x8BU, 0x96U, 0xC5U, 0xD8U, 0xFFU, 0xE2U,
    0x26U, 0x3BU, 0x1CU, 0x01U, 0x52U, 0x4FU, 0x68U, 0x75U, 0xCEU, 0xD3U, 0xF4U, 0xE9U, 0xBAU, 0xA7U, 0x80U, 0x9DU,
    0xEBU, 0xF6U, 0xD1U, 0xCCU, 0x9FU, 0x82U, 0xA5U, 0xB8U, 0x03U, 0x1EU, 0x39U, 0x24U, 0x77U, 0x6AU, 0x4DU, 0x50U,
    0xA1U, 0xBCU, 0x9BU, 0x86U, 0xD5U, 0xC8U, 0xEFU, 0xF2U, 0x49U, 0x54U, 0x73U, 0x6EU, 0x3DU, 0x20U, 0x07U, 0x1AU,
    0x6CU, 0x71U, 0x56U, 0x4BU, 0x18U, 0x05U, 0x22U, 0x3FU, 0x84U, 0x99U, 0xBEU, 0xA3U, 0xF0U, 0xEDU, 0xCAU, 0xD7U,
    0x35U, 0x28U, 0x0FU, 0x12U, 0x41U, 0x5CU, 0x7BU, 0x66U, 0xDDU, 0xC0U, 0xE7U, 0xFAU, 0xA9U, 0xB4U, 0x93U, 0x8EU,
    0xF8U, 0xE5U, 0xC2U, 0xDFU, 0x8CU, 0x91U, 0xB6U, 0xABU, 0x10U, 0x0DU, 0x2AU, 0x37U, 0x64U, 0x79U, 0x5EU, 0x43U,
    0xB2U, 0xAFU, 0x88U, 0x95U, 0xC6U, 0xDBU, 0xFCU, 0xE1U, 0x5AU, 0x47U, 0x60U, 0x7DU, 0x2EU, 0x33U, 0x14U, 0x09U,
    0x7FU, 0x62U, 0x45U, 0x58U, 0x0BU, 0x16U, 0x31U, 0x2CU, 0x97U, 0x8AU, 0xADU, 0xB0U, 0xE3U, 0xFEU, 0xD9U, 0xC4U
};

static inline int16_t snc_read_i16_le(uint8_t lo, uint8_t hi)
{
    return (int16_t)((uint16_t)lo | ((uint16_t)hi << 8));
}

static inline void snc_write_i16_le(uint8_t *dst, int16_t val)
{
    dst[0] = (uint8_t)(val & 0xFF);
    dst[1] = (uint8_t)((uint16_t)val >> 8);
}

static int16_t snc_saturate_float_to_i16(float x)
{
    if (x > 32767.0f) return 32767;
    if (x < -32768.0f) return -32768;

    // 手动实现四舍五入，避免调用 lroundf 库函数
    return (int16_t)(x >= 0.0f ? (x + 0.5f) : (x - 0.5f));
}

uint8_t SNC_CAN_Crc8J1850(const uint8_t *data, uint16_t len)
{
    uint8_t crc = 0xFFU;

    while (len-- > 0U)
    {
        crc = s_crc8_j1850_table[(uint8_t)(crc ^ *data++)];
    }

    return (uint8_t)(crc ^ 0xFFU);
}

static uint32_t snc_fdcan_dlc_from_bytes(uint8_t dlc_bytes)
{
    switch (dlc_bytes)
    {
        case 0: return FDCAN_DLC_BYTES_0;
        case 1: return FDCAN_DLC_BYTES_1;
        case 2: return FDCAN_DLC_BYTES_2;
        case 3: return FDCAN_DLC_BYTES_3;
        case 4: return FDCAN_DLC_BYTES_4;
        case 5: return FDCAN_DLC_BYTES_5;
        case 6: return FDCAN_DLC_BYTES_6;
        case 7: return FDCAN_DLC_BYTES_7;
        case 8: return FDCAN_DLC_BYTES_8;
        default: return FDCAN_DLC_BYTES_8;
    }
}

static HAL_StatusTypeDef snc_fdcan_add_tx_std(uint16_t std_id, const uint8_t *data, uint8_t dlc_bytes)
{
    FDCAN_TxHeaderTypeDef txHeader;

    memset(&txHeader, 0, sizeof(txHeader));

    txHeader.Identifier = std_id;
    txHeader.IdType = FDCAN_STANDARD_ID;
    txHeader.TxFrameType = FDCAN_DATA_FRAME;
    txHeader.DataLength = snc_fdcan_dlc_from_bytes(dlc_bytes);
    txHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    txHeader.BitRateSwitch = FDCAN_BRS_OFF;
    txHeader.FDFormat = FDCAN_CLASSIC_CAN;
    txHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    txHeader.MessageMarker = 0U;

    /* 先判断 TX FIFO/Queue 是否有空位，避免异常状态下继续硬塞 */
    if (HAL_FDCAN_GetTxFifoFreeLevel(&hfdcan1) == 0U)
    {
        return HAL_BUSY;
    }

    return HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &txHeader, (uint8_t *)data);
}

static void snc_fdcan_config_filter(uint32_t index, uint16_t std_id)
{
    FDCAN_FilterTypeDef sFilter;

    sFilter.IdType = FDCAN_STANDARD_ID;
    sFilter.FilterIndex = index;
    sFilter.FilterType = FDCAN_FILTER_MASK;
    sFilter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
    sFilter.FilterID1 = std_id;
    sFilter.FilterID2 = 0x7FFU; /* 完全匹配 11-bit ID */

    if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilter) != HAL_OK)
    {
        Error_Handler();
    }
}

static void snc_parse_status(const uint8_t *d)
{
    g_snc_can_app.status.state = d[0];
    g_snc_can_app.status.health = d[1];
    g_snc_can_app.status.diag_bits =
        ((uint32_t)d[2]) |
        ((uint32_t)d[3] << 8) |
        ((uint32_t)d[4] << 16) |
        ((uint32_t)d[5] << 24);
    g_snc_can_app.status.cmd_age_10ms = d[6];
    g_snc_can_app.status.status_counter = d[7];
    g_snc_can_app.status.last_update_ms = HAL_GetTick();
    g_snc_can_app.status.online = true;
}

static void snc_parse_actual_rpm(const uint8_t *d)
{
    g_snc_can_app.actual_rpm.fl = snc_read_i16_le(d[0], d[1]);
    g_snc_can_app.actual_rpm.rl = snc_read_i16_le(d[2], d[3]);
    g_snc_can_app.actual_rpm.fr = snc_read_i16_le(d[4], d[5]);
    g_snc_can_app.actual_rpm.rr = snc_read_i16_le(d[6], d[7]);
    g_snc_can_app.actual_rpm.last_update_ms = HAL_GetTick();
}

static void snc_parse_target_rpm(const uint8_t *d)
{
    g_snc_can_app.target_rpm.fl = snc_read_i16_le(d[0], d[1]);
    g_snc_can_app.target_rpm.rl = snc_read_i16_le(d[2], d[3]);
    g_snc_can_app.target_rpm.fr = snc_read_i16_le(d[4], d[5]);
    g_snc_can_app.target_rpm.rr = snc_read_i16_le(d[6], d[7]);
    g_snc_can_app.target_rpm.last_update_ms = HAL_GetTick();
}

static void snc_parse_comm_diag(const uint8_t *d)
{
    g_snc_can_app.comm_diag.valid_cmd_total_lsb = d[0];
    g_snc_can_app.comm_diag.crc_error_total_lsb = d[1];
    g_snc_can_app.comm_diag.counter_reject_total_lsb = d[2];
    g_snc_can_app.comm_diag.can_tx_drop_total_lsb = d[3];
    g_snc_can_app.comm_diag.busoff_total_lsb = d[4];
    g_snc_can_app.comm_diag.rx_overrun_total_lsb = d[5];
    g_snc_can_app.comm_diag.last_accepted_counter = d[6];
    g_snc_can_app.comm_diag.consecutive_counter_errors = (uint8_t)((d[7] >> 4) & 0x0F);
    g_snc_can_app.comm_diag.consecutive_crc_errors = (uint8_t)(d[7] & 0x0F);
    g_snc_can_app.comm_diag.last_update_ms = HAL_GetTick();
}

static void snc_parse_odom_delta(const uint8_t *d)
{
    uint32_t now = HAL_GetTick();
    uint32_t prev_update_ms = g_snc_can_app.odom_delta.last_update_ms;

    /* 保留最近一帧快照（兼容性，调试用） */
    g_snc_can_app.odom_delta.fl_delta = snc_read_i16_le(d[0], d[1]);
    g_snc_can_app.odom_delta.rl_delta = snc_read_i16_le(d[2], d[3]);
    g_snc_can_app.odom_delta.fr_delta = snc_read_i16_le(d[4], d[5]);
    g_snc_can_app.odom_delta.rr_delta = snc_read_i16_le(d[6], d[7]);
    g_snc_can_app.odom_delta.last_update_ms = now;

    /* ---- 累加到待消费缓冲区，解决漏积分问题 ---- */
    SNC_OdomDeltaAccum_t *acc = &g_snc_can_app.odom_accum;

    acc->fl_accum += (int32_t)snc_read_i16_le(d[0], d[1]);
    acc->rl_accum += (int32_t)snc_read_i16_le(d[2], d[3]);
    acc->fr_accum += (int32_t)snc_read_i16_le(d[4], d[5]);
    acc->rr_accum += (int32_t)snc_read_i16_le(d[6], d[7]);

    if (acc->frame_count == 0U) {
        acc->first_frame_ms = now;
    }
    acc->last_frame_ms = now;

    /*
     * 累加增量真实覆盖的时间窗。
     * 每个 0x200 帧表示“自上一帧以来”的编码器增量，
     * 因此应累加相邻帧之间的时间差，而不是简单用 last-first。
     */
    if (prev_update_ms != 0U) {
        uint32_t frame_dt_ms = now - prev_update_ms;
        if (frame_dt_ms <= 1000U) {
            acc->span_ms += frame_dt_ms;
        }
    }

    if (acc->frame_count < 255U) {
        acc->frame_count++;
    }
}

void SNC_CAN_AppInit(void)
{
    memset(&g_snc_can_app, 0, sizeof(g_snc_can_app));

    /* 配置 5 个标准滤波器，只接收协议相关 ID */
    snc_fdcan_config_filter(SNC_FILTER_INDEX_STATUS, SNC_CAN_ID_STATUS);
    snc_fdcan_config_filter(SNC_FILTER_INDEX_ACTUAL_RPM, SNC_CAN_ID_ACTUAL_RPM);
    snc_fdcan_config_filter(SNC_FILTER_INDEX_TARGET_RPM, SNC_CAN_ID_TARGET_RPM);
    snc_fdcan_config_filter(SNC_FILTER_INDEX_COMM_DIAG, SNC_CAN_ID_COMM_DIAG);
    snc_fdcan_config_filter(SNC_FILTER_INDEX_ODOM, SNC_CAN_ID_ODOM);

    /* 其余标准帧/扩展帧全部拒收 */
    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1,
                                     FDCAN_REJECT,
                                     FDCAN_REJECT,
                                     FDCAN_REJECT_REMOTE,
                                     FDCAN_REJECT_REMOTE) != HAL_OK)
    {
        Error_Handler();
    }

    if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK)
    {
        Error_Handler();
    }

    if (HAL_FDCAN_ActivateNotification(&hfdcan1,
                                       FDCAN_IT_RX_FIFO0_NEW_MESSAGE,
                                       0U) != HAL_OK)
    {
        Error_Handler();
    }
}

HAL_StatusTypeDef SNC_CAN_SendHeartbeat(void)
{
    uint8_t data[0];
    HAL_StatusTypeDef ret = snc_fdcan_add_tx_std(SNC_CAN_ID_HEARTBEAT, data, 0U);

    if (ret == HAL_OK)
    {
        g_snc_can_app.tx_total++;
    }
    else
    {
        g_snc_can_app.tx_fail_total++;
    }

    return ret;
}

HAL_StatusTypeDef SNC_CAN_SendCmdVel(float vx_mps, float wz_radps, uint8_t ctrl_flags)
{
    uint8_t data[8];
    int16_t vx_i16 = snc_saturate_float_to_i16(vx_mps * 1000.0f);
    int16_t wz_i16 = snc_saturate_float_to_i16(wz_radps * 1000.0f);
    HAL_StatusTypeDef ret;

    g_snc_can_app.tx_counter = (uint8_t)(g_snc_can_app.tx_counter + 1U);
    g_snc_can_app.tx_ctrl_flags = ctrl_flags;

    snc_write_i16_le(&data[0], vx_i16);
    snc_write_i16_le(&data[2], wz_i16);
    data[4] = ctrl_flags;
    data[5] = 0U;
    data[6] = g_snc_can_app.tx_counter;
    data[7] = SNC_CAN_Crc8J1850(data, 7U);

    ret = snc_fdcan_add_tx_std(SNC_CAN_ID_CMD_VEL, data, 8U);

    if (ret == HAL_OK)
    {
        g_snc_can_app.tx_total++;
    }
    else
    {
        g_snc_can_app.tx_fail_total++;
    }

    return ret;
}

void SNC_CAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t rxFifo0ITs)
{
    FDCAN_RxHeaderTypeDef rxHeader;
    uint8_t data[8];

    if (hfdcan != &hfdcan1)
    {
        return;
    }

    if ((rxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == 0U)
    {
        return;
    }

    while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0) > 0U)
    {
        if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &rxHeader, data) != HAL_OK)
        {
            break;
        }

        if ((rxHeader.IdType != FDCAN_STANDARD_ID) ||
            (rxHeader.RxFrameType != FDCAN_DATA_FRAME))
        {
            continue;
        }

        g_snc_can_app.rx_total++;

        switch (rxHeader.Identifier)
        {
            case SNC_CAN_ID_STATUS:
                if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
                {
                    snc_parse_status(data);
                }
                break;

            case SNC_CAN_ID_ACTUAL_RPM:
                if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
                {
                    snc_parse_actual_rpm(data);
                }
                break;

            case SNC_CAN_ID_TARGET_RPM:
                if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
                {
                    snc_parse_target_rpm(data);
                }
                break;

            case SNC_CAN_ID_COMM_DIAG:
                if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
                {
                    snc_parse_comm_diag(data);
                }
                break;

            case SNC_CAN_ID_ODOM:
                if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
                {
                    snc_parse_odom_delta(data);
                }
                break;

            default:
                break;
        }
    }
}

void SNC_CAN_20msTask(void)
{
    /* 这里默认发心跳。
       速度命令建议由你的上层控制逻辑每 20ms 调 SNC_CAN_SendCmdVel()。 */
    (void)SNC_CAN_SendHeartbeat();
}

void SNC_CAN_100msTask(void)
{
    /* 使用静态变量记录上一次的接收总数 */
    static uint32_t last_rx_total = 0;

    /* 如果当前总数和上次记录的总数不一致，说明这 100ms 内成功收到了新消息 */
    if (g_snc_can_app.rx_total != last_rx_total)
    {
        last_rx_total = g_snc_can_app.rx_total;

        // 翻转 PE2 电平，产生肉眼可见的闪烁效果
        HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_3);
    }
    else
    {
        /* 如果 100ms 内没有收到任何新消息（CAN 断开或下位机死机）
           强制关闭 LED。
           注意：这里假设高电平 (SET) 是熄灭。如果是低电平熄灭，请改为 GPIO_PIN_RESET */
        HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_SET);
    }
}

void SNC_CAN_PollOnlineState(uint32_t now_ms)
{
    if ((now_ms - g_snc_can_app.status.last_update_ms) > SNC_STATUS_TIMEOUT_MS)
    {
        g_snc_can_app.status.online = false;
    }
}

const SNC_CAN_AppContext_t *SNC_CAN_GetContext(void)
{
    return &g_snc_can_app;
}

uint8_t SNC_CAN_ConsumeOdomDelta(int16_t *fl, int16_t *rl,
                                  int16_t *fr, int16_t *rr,
                                  uint32_t *dt_ms)
{
    /*
     * 原子取走累加器内容并清零。
     *
     * 本函数由 monitorTask (普通任务上下文) 调用，
     * 而写端 snc_parse_odom_delta() 运行在 CAN Rx ISR 中。
     *
     * 用 taskENTER_CRITICAL / taskEXIT_CRITICAL 保证取走 + 清零
     * 的原子性：临界区会关中断，防止 ISR 在取走过程中写入新增量。
     */
    SNC_OdomDeltaAccum_t snapshot;
    uint8_t count;

    taskENTER_CRITICAL();
    {
        snapshot = g_snc_can_app.odom_accum;   /* 结构体拷贝 */
        /* 清零累加器，等待下一批帧 */
        g_snc_can_app.odom_accum.fl_accum      = 0;
        g_snc_can_app.odom_accum.rl_accum      = 0;
        g_snc_can_app.odom_accum.fr_accum      = 0;
        g_snc_can_app.odom_accum.rr_accum      = 0;
        g_snc_can_app.odom_accum.first_frame_ms = 0U;
        g_snc_can_app.odom_accum.last_frame_ms  = 0U;
        g_snc_can_app.odom_accum.span_ms        = 0U;
        g_snc_can_app.odom_accum.frame_count    = 0U;
    }
    taskEXIT_CRITICAL();

    count = snapshot.frame_count;

    if (count == 0U) {
        /* 期间没有新帧到达 */
        *fl = 0;  *rl = 0;  *fr = 0;  *rr = 0;
        *dt_ms = 0U;
        return 0U;
    }

    /* int32 → int16 饱和截断（正常工况下不会溢出） */
    *fl = (int16_t)((snapshot.fl_accum > 32767) ? 32767 :
                    (snapshot.fl_accum < -32768) ? -32768 : snapshot.fl_accum);
    *rl = (int16_t)((snapshot.rl_accum > 32767) ? 32767 :
                    (snapshot.rl_accum < -32768) ? -32768 : snapshot.rl_accum);
    *fr = (int16_t)((snapshot.fr_accum > 32767) ? 32767 :
                    (snapshot.fr_accum < -32768) ? -32768 : snapshot.fr_accum);
    *rr = (int16_t)((snapshot.rr_accum > 32767) ? 32767 :
                    (snapshot.rr_accum < -32768) ? -32768 : snapshot.rr_accum);

    /* 实际累计时间窗 */
    if (snapshot.span_ms == 0U) {
        /* 首帧或刚恢复时无法可靠计算，返回 0 让调用方使用保底值 */
        *dt_ms = 0U;
    } else {
        *dt_ms = snapshot.span_ms;
    }

    return count;
}