/*******************************************************************************
|
* File Name : SBUS.c
|
* Description : SBUS RC receiver protocol parser implementation
|
* Created on : 2025-11-13
|
* Author : Auto-generated
|
*******************************************************************************/
|
|
/*******************************************************************************
|
* Include Files *
|
*******************************************************************************/
|
#include "SBUS.h"
|
#include "Uart.h"
|
#include "stm32h7xx_hal.h"
|
#include "stm32h7xx_hal_uart.h"
|
#include "stm32h7xx_hal_dma.h"
|
#include <string.h>
|
#include "pwm_ctrol.h"
|
|
/*******************************************************************************
|
* Macro *
|
*******************************************************************************/
|
#define SBUS_TIMEOUT_MS 100
|
|
// 调试开关：设置为 1 启用调试输出（需要 printf 重定向），设置为 0 禁用
|
#define SBUS_DEBUG_ENABLE 0
|
|
#if SBUS_DEBUG_ENABLE
|
#define SBUS_DEBUG_PRINT(...) printf(__VA_ARGS__)
|
#else
|
#define SBUS_DEBUG_PRINT(...) ((void)0)
|
#endif
|
|
/*******************************************************************************
|
* Global Variables *
|
*******************************************************************************/
|
// SBUS 接收缓冲区 (DMA 写入)
|
static HIDO_UINT8 g_au8SBUSRxBuf[SBUS_UART_RX_BUF_SIZE] = {0};
|
|
// SBUS 当前通道数据
|
static ST_SBUSData g_stSBUSData = {0};
|
|
// 临时解析缓冲区 (用于帧边界对齐)
|
static HIDO_UINT8 g_au8ParseBuf[SBUS_FRAME_SIZE] = {0};
|
static HIDO_UINT8 g_u8ParseBufIdx = 0;
|
|
// 调试统计
|
static HIDO_UINT32 g_u32DebugPollCount = 0;
|
static HIDO_UINT32 g_u32DebugLastDmaRemaining = 0;
|
// SBUS failsafe active flag
|
static volatile HIDO_UINT8 g_bSBUSFailsafeActive = 0;
|
|
/*******************************************************************************
|
* Local Function *
|
*******************************************************************************/
|
|
/**
|
* @brief 解析单个 SBUS 帧 (25 字节)
|
* @param _pu8Frame: 帧数据指针 (必须指向完整的 25 字节)
|
* @return HIDO_TRUE: 解析成功, HIDO_FALSE: 校验失败
|
*/
|
static HIDO_INT32 SBUS_ParseFrame(const HIDO_UINT8 *_pu8Frame)
|
{
|
// 1. 校验帧头和帧尾
|
if (_pu8Frame[0] != SBUS_HEADER || _pu8Frame[24] != SBUS_FOOTER)
|
{
|
g_stSBUSData.m_u32ErrorCount++;
|
return HIDO_FALSE;
|
}
|
|
// 2. 提取 16 个 11-bit 通道 (bit-packed in bytes 1~22)
|
// SBUS 协议: 176 bits (16 channels * 11 bits) 存储在 22 字节中
|
// Channel layout (LSB first):
|
// Byte 1: Ch0[0:7]
|
// Byte 2: Ch0[8:10] | Ch1[0:4]
|
// Byte 3: Ch1[5:10] | Ch2[0:1]
|
// ... (依次类推)
|
|
g_stSBUSData.m_au16Channels[0] = (HIDO_UINT16)((_pu8Frame[1] | _pu8Frame[2] << 8) & 0x07FF);
|
g_stSBUSData.m_au16Channels[1] = (HIDO_UINT16)((_pu8Frame[2] >> 3 | _pu8Frame[3] << 5) & 0x07FF);
|
g_stSBUSData.m_au16Channels[2] = (HIDO_UINT16)((_pu8Frame[3] >> 6 | _pu8Frame[4] << 2 | _pu8Frame[5] << 10) & 0x07FF);
|
g_stSBUSData.m_au16Channels[3] = (HIDO_UINT16)((_pu8Frame[5] >> 1 | _pu8Frame[6] << 7) & 0x07FF);
|
g_stSBUSData.m_au16Channels[4] = (HIDO_UINT16)((_pu8Frame[6] >> 4 | _pu8Frame[7] << 4) & 0x07FF);
|
g_stSBUSData.m_au16Channels[5] = (HIDO_UINT16)((_pu8Frame[7] >> 7 | _pu8Frame[8] << 1 | _pu8Frame[9] << 9) & 0x07FF);
|
g_stSBUSData.m_au16Channels[6] = (HIDO_UINT16)((_pu8Frame[9] >> 2 | _pu8Frame[10] << 6) & 0x07FF);
|
g_stSBUSData.m_au16Channels[7] = (HIDO_UINT16)((_pu8Frame[10] >> 5 | _pu8Frame[11] << 3) & 0x07FF);
|
g_stSBUSData.m_au16Channels[8] = (HIDO_UINT16)((_pu8Frame[12] | _pu8Frame[13] << 8) & 0x07FF);
|
g_stSBUSData.m_au16Channels[9] = (HIDO_UINT16)((_pu8Frame[13] >> 3 | _pu8Frame[14] << 5) & 0x07FF);
|
g_stSBUSData.m_au16Channels[10] = (HIDO_UINT16)((_pu8Frame[14] >> 6 | _pu8Frame[15] << 2 | _pu8Frame[16] << 10) & 0x07FF);
|
g_stSBUSData.m_au16Channels[11] = (HIDO_UINT16)((_pu8Frame[16] >> 1 | _pu8Frame[17] << 7) & 0x07FF);
|
g_stSBUSData.m_au16Channels[12] = (HIDO_UINT16)((_pu8Frame[17] >> 4 | _pu8Frame[18] << 4) & 0x07FF);
|
g_stSBUSData.m_au16Channels[13] = (HIDO_UINT16)((_pu8Frame[18] >> 7 | _pu8Frame[19] << 1 | _pu8Frame[20] << 9) & 0x07FF);
|
g_stSBUSData.m_au16Channels[14] = (HIDO_UINT16)((_pu8Frame[20] >> 2 | _pu8Frame[21] << 6) & 0x07FF);
|
g_stSBUSData.m_au16Channels[15] = (HIDO_UINT16)((_pu8Frame[21] >> 5 | _pu8Frame[22] << 3) & 0x07FF);
|
|
// 3. 提取标志位 (Byte 23)
|
// Bit 0: CH17 digital
|
// Bit 1: CH18 digital
|
// Bit 2: Frame Lost
|
// Bit 3: Failsafe
|
g_stSBUSData.m_u8Ch17 = (_pu8Frame[23] & 0x01) ? 1 : 0;
|
g_stSBUSData.m_u8Ch18 = (_pu8Frame[23] & 0x02) ? 1 : 0;
|
g_stSBUSData.m_u8FrameLost = (_pu8Frame[23] & 0x04) ? 1 : 0;
|
g_stSBUSData.m_u8Failsafe = (_pu8Frame[23] & 0x08) ? 1 : 0;
|
|
// 4. 更新时间戳和计数
|
g_stSBUSData.m_u32LastUpdateTick = HAL_GetTick();
|
g_stSBUSData.m_u32FrameCount++;
|
|
// 解析成功后立即触发 PWM 控制（最小延迟）
|
// 同时清除 failsafe 标志（如果之前触发过）
|
if (g_bSBUSFailsafeActive)
|
{
|
g_bSBUSFailsafeActive = 0;
|
}
|
SBUS_Control_PWM();
|
|
return HIDO_TRUE;
|
}
|
|
/**
|
* @brief 从 DMA 缓冲区中查找并解析 SBUS 帧
|
* @param _pu8Buf: DMA 缓冲区指针
|
* @param _u32Len: 可读数据长度
|
* @return 无
|
*/
|
static HIDO_VOID SBUS_ProcessBuffer(const HIDO_UINT8 *_pu8Buf, HIDO_UINT32 _u32Len)
|
{
|
static HIDO_UINT32 s_u32TotalBytesProcessed = 0;
|
|
for (HIDO_UINT32 i = 0; i < _u32Len; i++)
|
{
|
HIDO_UINT8 byte = _pu8Buf[i];
|
s_u32TotalBytesProcessed++;
|
|
// 状态机: 寻找帧头 0x0F
|
if (g_u8ParseBufIdx == 0)
|
{
|
if (byte == SBUS_HEADER)
|
{
|
g_au8ParseBuf[g_u8ParseBufIdx++] = byte;
|
}
|
// 否则丢弃非帧头字节
|
}
|
else
|
{
|
g_au8ParseBuf[g_u8ParseBufIdx++] = byte;
|
|
// 收满 25 字节后尝试解析
|
if (g_u8ParseBufIdx >= SBUS_FRAME_SIZE)
|
{
|
SBUS_ParseFrame(g_au8ParseBuf);
|
g_u8ParseBufIdx = 0; // 重置状态机
|
}
|
}
|
}
|
}
|
|
/*******************************************************************************
|
* Global Function *
|
*******************************************************************************/
|
|
/**
|
* @brief 初始化 SBUS 接收模块
|
*/
|
HIDO_INT32 SBUS_Init(HIDO_VOID)
|
{
|
HIDO_INT32 ret;
|
ST_UartInit stUartInit;
|
UART_HandleTypeDef *pUart = NULL;
|
|
// 先检查 UART 是否已经注册
|
ret = Uart_GetHandle(UART_ID_SBUS, (HIDO_VOID **)&pUart);
|
|
if (pUart == NULL)
|
{
|
return HIDO_FALSE;
|
}
|
|
// 初始化 SBUS 数据结构
|
memset(&g_stSBUSData, 0, sizeof(g_stSBUSData));
|
memset(g_au8ParseBuf, 0, sizeof(g_au8ParseBuf));
|
memset(g_au8SBUSRxBuf, 0, sizeof(g_au8SBUSRxBuf));
|
g_u8ParseBufIdx = 0;
|
g_u32DebugPollCount = 0;
|
g_u32DebugLastDmaRemaining = 0;
|
|
// 配置 UART4 接收 (DMA 模式)
|
stUartInit.m_eRxMode = UART_RX_MODE_DMA;
|
stUartInit.m_pu8RxBuf = g_au8SBUSRxBuf;
|
stUartInit.m_u32RxBufSize = SBUS_UART_RX_BUF_SIZE;
|
|
// 不使用发送功能 (SBUS 仅单向接收)
|
stUartInit.m_eTxMode = UART_TX_MODE_POLL;
|
stUartInit.m_pu8TxBuf = NULL;
|
stUartInit.m_u32TxBufSize = 0;
|
stUartInit.m_u32TxQueueMemberCnt = 0;
|
stUartInit.m_fnRxISR = NULL;
|
|
ret = Uart_Init(UART_ID_SBUS, &stUartInit);
|
if (ret != HIDO_OK)
|
{
|
return HIDO_FALSE;
|
}
|
|
// 获取 UART 句柄以验证初始化
|
ret = Uart_GetHandle(UART_ID_SBUS, (HIDO_VOID **)&pUart);
|
if (ret != HIDO_OK || pUart == NULL)
|
{
|
return HIDO_FALSE;
|
}
|
|
return HIDO_TRUE;
|
}
|
|
/**
|
* @brief SBUS 轮询函数 (主循环调用)
|
*/
|
HIDO_VOID SBUS_Poll(HIDO_VOID)
|
{
|
UART_HandleTypeDef *pUart = NULL;
|
HIDO_INT32 ret;
|
|
g_u32DebugPollCount++;
|
|
// 获取 UART 句柄
|
ret = Uart_GetHandle(UART_ID_SBUS, (HIDO_VOID **)&pUart);
|
if (ret != HIDO_OK || pUart == NULL)
|
{
|
return;
|
}
|
|
// 检查 DMA 句柄
|
if (pUart->hdmarx == NULL)
|
{
|
return;
|
}
|
|
// 计算 DMA 已接收的数据量
|
// DMA->CNDTR 返回剩余未传输的数据个数，因此已接收 = BufSize - CNDTR
|
HIDO_UINT32 dmaRemaining = __HAL_DMA_GET_COUNTER(pUart->hdmarx);
|
HIDO_UINT32 receivedBytes = SBUS_UART_RX_BUF_SIZE - dmaRemaining;
|
|
g_u32DebugLastDmaRemaining = dmaRemaining;
|
|
// 处理新接收的数据
|
// 注意: 这里使用环形缓冲区简单读取策略 (适合短周期轮询)
|
static HIDO_UINT32 s_u32LastProcessedIdx = 0;
|
|
if (receivedBytes > s_u32LastProcessedIdx)
|
{
|
HIDO_UINT32 newBytes = receivedBytes - s_u32LastProcessedIdx;
|
SBUS_ProcessBuffer(&g_au8SBUSRxBuf[s_u32LastProcessedIdx], newBytes);
|
s_u32LastProcessedIdx = receivedBytes;
|
}
|
|
// 处理 DMA 环形回绕 (当 CNDTR 重载时)
|
if (receivedBytes < s_u32LastProcessedIdx)
|
{
|
// 先处理缓冲区尾部
|
HIDO_UINT32 tailBytes = SBUS_UART_RX_BUF_SIZE - s_u32LastProcessedIdx;
|
if (tailBytes > 0)
|
{
|
SBUS_ProcessBuffer(&g_au8SBUSRxBuf[s_u32LastProcessedIdx], tailBytes);
|
}
|
// 再处理缓冲区头部
|
if (receivedBytes > 0)
|
{
|
SBUS_ProcessBuffer(&g_au8SBUSRxBuf[0], receivedBytes);
|
}
|
s_u32LastProcessedIdx = receivedBytes;
|
}
|
|
// Failsafe: 如果距离上次有效帧超过 1000ms，触发中立脉宽（1500us）
|
if (!g_bSBUSFailsafeActive)
|
{
|
HIDO_UINT32 currentTick = HAL_GetTick();
|
HIDO_UINT32 elapsed = currentTick - g_stSBUSData.m_u32LastUpdateTick;
|
if (elapsed >= 1000)
|
{
|
// 触发一次性 failsafe：将转向与电机脉宽设为中立 1500us
|
Set_Steering_Pulse(1500);
|
Set_Motor_Pulse(1500);
|
g_bSBUSFailsafeActive = 1;
|
}
|
}
|
}
|
|
/**
|
* @brief 获取 SBUS 通道数据副本
|
*/
|
HIDO_INT32 SBUS_GetData(ST_SBUSData *_pstData)
|
{
|
if (_pstData == NULL)
|
{
|
return HIDO_FALSE;
|
}
|
|
// 检查信号是否有效 (最近 100ms 内收到帧)
|
if (!SBUS_IsSignalValid(SBUS_TIMEOUT_MS))
|
{
|
return HIDO_FALSE;
|
}
|
|
// 拷贝数据 (简单拷贝, 若需线程安全可加临界区)
|
memcpy(_pstData, &g_stSBUSData, sizeof(ST_SBUSData));
|
return HIDO_TRUE;
|
}
|
|
/**
|
* @brief 获取单个通道值
|
*/
|
HIDO_UINT16 SBUS_GetChannel(HIDO_UINT8 _u8Channel)
|
{
|
if (_u8Channel >= SBUS_NUM_CHANNELS)
|
{
|
return SBUS_CENTER_VALUE;
|
}
|
|
if (!SBUS_IsSignalValid(SBUS_TIMEOUT_MS))
|
{
|
return SBUS_CENTER_VALUE;
|
}
|
|
return g_stSBUSData.m_au16Channels[_u8Channel];
|
}
|
|
/**
|
* @brief 获取数字通道状态
|
*/
|
HIDO_UINT8 SBUS_GetDigitalChannel(HIDO_UINT8 _u8DigitalChannel)
|
{
|
if (!SBUS_IsSignalValid(SBUS_TIMEOUT_MS))
|
{
|
return 0;
|
}
|
|
if (_u8DigitalChannel == 17)
|
{
|
return g_stSBUSData.m_u8Ch17;
|
}
|
else if (_u8DigitalChannel == 18)
|
{
|
return g_stSBUSData.m_u8Ch18;
|
}
|
|
return 0;
|
}
|
|
/**
|
* @brief 检查 SBUS 信号是否有效
|
*/
|
HIDO_INT32 SBUS_IsSignalValid(HIDO_UINT32 _u32TimeoutMs)
|
{
|
HIDO_UINT32 currentTick = HAL_GetTick();
|
HIDO_UINT32 elapsed = currentTick - g_stSBUSData.m_u32LastUpdateTick;
|
|
return (elapsed < _u32TimeoutMs) ? HIDO_TRUE : HIDO_FALSE;
|
}
|
|
/**
|
* @brief 获取接收统计信息
|
*/
|
HIDO_VOID SBUS_GetStats(HIDO_UINT32 *_pu32FrameCount, HIDO_UINT32 *_pu32ErrorCount)
|
{
|
if (_pu32FrameCount != NULL)
|
{
|
*_pu32FrameCount = g_stSBUSData.m_u32FrameCount;
|
}
|
if (_pu32ErrorCount != NULL)
|
{
|
*_pu32ErrorCount = g_stSBUSData.m_u32ErrorCount;
|
}
|
}
|
|
/**
|
* @brief 调试函数：打印 SBUS 状态和统计信息
|
*/
|
HIDO_VOID SBUS_PrintDebugInfo(HIDO_VOID)
|
{
|
UART_HandleTypeDef *pUart = NULL;
|
HIDO_INT32 ret;
|
|
HIDO_Debug2("\r\n========== SBUS Debug Info ==========\r\n");
|
HIDO_Debug2("Poll count: %u\r\n", g_u32DebugPollCount);
|
HIDO_Debug2("Frame count: %u\r\n", g_stSBUSData.m_u32FrameCount);
|
HIDO_Debug2("Error count: %u\r\n", g_stSBUSData.m_u32ErrorCount);
|
HIDO_Debug2("Last update: %u ms ago\r\n",
|
HAL_GetTick() - g_stSBUSData.m_u32LastUpdateTick);
|
HIDO_Debug2("Parse buf idx: %u\r\n", g_u8ParseBufIdx);
|
|
ret = Uart_GetHandle(UART_ID_SBUS, (HIDO_VOID **)&pUart);
|
if (ret == HIDO_OK && pUart != NULL)
|
{
|
if (pUart->hdmarx != NULL)
|
{
|
HIDO_UINT32 dmaRemaining = __HAL_DMA_GET_COUNTER(pUart->hdmarx);
|
HIDO_Debug2("DMA remaining: %u\r\n", dmaRemaining);
|
HIDO_Debug2("DMA received: %u\r\n", SBUS_UART_RX_BUF_SIZE - dmaRemaining);
|
HIDO_Debug2("DMA state: %u\r\n", HAL_DMA_GetState(pUart->hdmarx));
|
}
|
else
|
{
|
HIDO_Debug2("DMA handle: NULL!\r\n");
|
}
|
}
|
else
|
{
|
HIDO_Debug2("UART handle: NULL!\r\n");
|
}
|
|
if (g_stSBUSData.m_u32FrameCount > 0)
|
{
|
HIDO_Debug2("\r\nLast frame channels:\r\n");
|
for (HIDO_UINT8 i = 0; i < 8; i++)
|
{
|
HIDO_Debug2(" CH%02u: %4u", i, g_stSBUSData.m_au16Channels[i]);
|
if ((i + 1) % 4 == 0) HIDO_Debug2("\r\n");
|
}
|
HIDO_Debug2(" CH17: %u, CH18: %u\r\n",
|
g_stSBUSData.m_u8Ch17, g_stSBUSData.m_u8Ch18);
|
HIDO_Debug2(" FrameLost: %u, Failsafe: %u\r\n",
|
g_stSBUSData.m_u8FrameLost, g_stSBUSData.m_u8Failsafe);
|
}
|
|
HIDO_Debug2("\r\nRaw RX buffer (first 32 bytes):\r\n");
|
for (HIDO_UINT8 i = 0; i < 32; i++)
|
{
|
HIDO_Debug2("%02X ", g_au8SBUSRxBuf[i]);
|
if ((i + 1) % 16 == 0) HIDO_Debug2("\r\n");
|
}
|
HIDO_Debug2("====================================\r\n\r\n");
|
}
|
