484 lines
16 KiB
C
484 lines
16 KiB
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;
|
||
}
|