路径点不重复的控制算法测试通了，但是好像不如之前路径点重复的版本好。增加了蓝牙接口部分。准备移植外包的MQTT。

yincheng.zhong

10 天以前 4adc78553c8d48ff122506195fa33641134bd7b1

 STM32H743/FML/bluetooth.c

@@ -1,77 +1,83 @@
/*******************************************************************************
 * File Name         : BT.c
 * Description       :
 * Created on        : 2018年7月23日
 * Author            : 杜键
 * File Name         : bluetooth.c
 * Description       : Bluetooth Communication Protocol Implementation
 * Created on        : 2025-12-04
 * Author            : HIDO
 *******************************************************************************/

/*******************************************************************************
 *                              Include Files                                  *
 *******************************************************************************/
#include "bluetooth.h"
#include "stdio.h"
#include "stdarg.h"
#include "string.h"
#include "AppConfig.h"
#include "HIDO_VLQueue.h"
#include "HIDO_Input.h"
#include "HIDO_Timer.h"
#include "HIDO_Util.h"
#include "bluetooth.h"
#include "DBG.h"
#include "mainex.h"
#include "Uart.h"
#include "pwm_ctrol.h"
#include "SBUS.h"

/*******************************************************************************
 *                                  Macro                                      *
 *******************************************************************************/
// 全局状态标志
volatile uint8_t g_bt_configured = 0;    // 配置完成标志
volatile uint8_t g_bt_paired = 0;        // 配对完成标志
volatile uint8_t g_bt_at_moded = 0;        // AT_MODE

static BT_RevState bt_state = BT_STATE_IDLE;
static char bt_buffer[50];      // 接收缓冲区
static u8 bt_index = 0;       // 当前接收位置

static HIDO_UINT8 l_au8BTUartRxBuf[BT_UART_RX_BUF_SIZE];
static HIDO_UINT8 l_au8BTUartTxBuf[BT_UART_TX_BUF_SIZE];
HIDO_UINT8 uart6_dma_rxbuf[UART6_DMA_RX_BUF_SIZE] = {0};
HIDO_UINT8 uart6_dma_recv_end_flag = 0;
HIDO_UINT8 uart6_dma_recv_len = 0;
#define CRC16_POLY 0x1021
#define BT_RX_BUF_SIZE 128  // Internal parsing buffer size
#define ENABLE_BT_DEBUG_LOG 0  // Set to 1 to enable debug logs

/*******************************************************************************
 *                             Local Variable                                  *
 *******************************************************************************/
// 配置函数声明
static HIDO_UINT8 ConfigBluetoothMasterSlave(void);
static void Bluetooth_Config_Slave(void);
static HIDO_UINT8 l_au8BTUartRxBuf[BT_UART_RX_BUF_SIZE];
static HIDO_UINT8 l_au8BTUartTxBuf[BT_UART_TX_BUF_SIZE];

// 映射函数：将 [-100,100] 映射到 [1000,2000]
static uint32_t Map(int16_t input, int16_t in_min, int16_t in_max, uint32_t out_min, uint32_t out_max) {
    return (input - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}	
// 设置电机 PWM（前进/后退）
static void Set_Motor_PWM(int16_t speed) {
    uint32_t pulse = Map(speed, -100, 100, 1000, 2000);  // -100~100 → 1000~2000
    __HAL_TIM_SetCompare(&MOTOR_TIM, MOTOR_CHANNEL, pulse);
}
// DMA Buffer for UART6 (aligned for D-Cache operations)
__attribute__((aligned(32))) HIDO_UINT8 uart6_dma_rxbuf[BT_UART_RX_BUF_SIZE] = {0};
HIDO_UINT8 uart6_dma_recv_end_flag = 0;
HIDO_UINT16 uart6_dma_recv_len = 0;
volatile HIDO_UINT32 g_u32BtIdleIntCount = 0;

// 设置转向 PWM（左转/右转）
static void Set_Steering_PWM(int16_t steer) {
    uint32_t pulse = Map(steer, -100, 100, 1000, 2000);  // -100~100 → 1000~2000
    __HAL_TIM_SetCompare(&STEERING_TIM, STEERING_CHANNEL, pulse);
}
extern UART_HandleTypeDef huart6;
extern DMA_HandleTypeDef hdma_usart6_rx;

// Receive State Machine
typedef enum
{
    BT_FSM_IDLE = 0,
    BT_FSM_HEADER2,
    BT_FSM_CMD,
    BT_FSM_LEN_LOW,
    BT_FSM_LEN_HIGH,
    BT_FSM_DATA,
    BT_FSM_TAIL
} E_BT_FSM_State;

typedef struct
{
    E_BT_FSM_State state;
    HIDO_UINT8 cmd_type;
    HIDO_UINT16 data_len;
    HIDO_UINT16 data_idx;
    HIDO_UINT8 rx_buf[BT_RX_BUF_SIZE];
} ST_BT_RecvFSM;

static ST_BT_RecvFSM s_bt_fsm;

/*******************************************************************************
 * Function Name     : DBG_Init
 * Description       : 调试打印初始化
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : 杜键
 * Modified Date:    : 2018年7月23日
 *                        Local Function Declaration                           *
 *******************************************************************************/
static HIDO_UINT16 Calculate_CRC16(const HIDO_UINT8 *data, HIDO_UINT16 len);
static HIDO_VOID BT_RecvFSM(HIDO_UINT8 byte);
static HIDO_VOID BT_ProcessFrame(void);

/*******************************************************************************
 *                             Global Function                                 *
 *******************************************************************************/

/**
 * @brief Initialize Bluetooth Module
 */
HIDO_VOID BT_Init(void)
{
    ST_UartInit stInit;
@@ -85,248 +91,399 @@
    stInit.m_u32TxBufSize = BT_UART_TX_BUF_SIZE;
    stInit.m_u32TxQueueMemberCnt = BT_UART_TX_QUEUE_MEMBER_CNT;
    Uart_Init(UART_ID_BT, &stInit);


    Uart_ReConfigBaudRate(UART_ID_BT, 115200);
    UART6_StartReceive();
}

static void BTUsartParseDataHandler(uint8_t data)
/**
 * @brief Start UART6 Receive with Circular DMA
 */
void UART6_StartReceive(void)
{
    switch (bt_state)
   {
    case BT_STATE_IDLE:
        if(data!=NULL)
        {
         bt_state=BT_STATE_RECEIVING;
        }
        break;
    case BT_STATE_RECEIVING:
        bt_buffer[bt_index++] = data;
      if (bt_index >2 && bt_buffer[bt_index-2] == '\r' && bt_buffer[bt_index-1] == '\n') {
            // 检测到完整结束符 "\r\n"
            bt_buffer[bt_index] = '\0';  // 添加字符串终止符
            bt_index = 0;  // 重置缓冲区
            bt_state = BT_STATE_COMPLETE;
        }
        break;
    // Clear IDLE flag
    __HAL_UART_CLEAR_IDLEFLAG(&huart6);

    case BT_STATE_COMPLETE:
        // 状态机错误，重置状态
        bt_state = BT_STATE_IDLE;
        break;
    }
    // IDLE interrupt is optional for Circular DMA with GetChar polling
    // Uncomment if you want IDLE interrupt as a hint (not required for functionality)
    // __HAL_UART_ENABLE_IT(&huart6, UART_IT_IDLE);

    // Start DMA Receive (Circular Mode configured in HAL_MSP)
    HAL_UART_Receive_DMA(&huart6, uart6_dma_rxbuf, BT_UART_RX_BUF_SIZE);
}


// 蓝牙配置函数实现
static HIDO_UINT8  BT_Enter_ATmode(void)

{
   HIDO_UINT8 u8RecvChar = 0;
   //DBG_SerialPutString("AT+ENAT\r\n");
   Uart_Send(UART_ID_BT, (HIDO_UINT8 *) "AT+ENAT\r\n", strlen("AT+ENAT\r\n"));
    while (Uart_GetChar(UART_ID_BT, &u8RecvChar) == HIDO_OK)
    {
        BTUsartParseDataHandler(u8RecvChar);

        // 如果数据接收完成，处理数据帧
        if (bt_state == BT_STATE_COMPLETE)
        {
           if(0==strcmp(bt_buffer,"\r\nOK\r\n"))
            {
               memset(bt_buffer, 0, 50);	
            bt_state = BT_STATE_IDLE;
            g_bt_at_moded=1;
            return BT_CONFIG_SUCCESS;
            }
        }
    }
   return BT_CONFIG_FAIL;
}

// 蓝牙断开连接
void  BT_DisConnect(void)

{
   HIDO_UINT8 u8RecvChar = 0;
   //DBG_SerialPutString("AT+ENAT\r\n");
   Uart_Send(UART_ID_BT, (HIDO_UINT8 *) "AT+ENAT\r\n", strlen("AT+ENAT\r\n"));
// delay_ms(10);
   Uart_Send(UART_ID_BT, (HIDO_UINT8 *) "AT+LENC\r\n", strlen("AT+LENC\r\n"));
// delay_ms(10);
   Uart_Send(UART_ID_BT, (HIDO_UINT8 *) "AT+EXAT\r\n", strlen("AT+EXAT\r\n"));
}

// 蓝牙配置函数实现
static HIDO_UINT8  BT_Get_SLaveMode(void)

{
    HIDO_UINT8 u8RecvChar = 0;
// DBG_SerialPutString("AT+ROLE\r\n");
    while (Uart_GetChar(UART_ID_BT, &u8RecvChar) == HIDO_OK)
    {
        BTUsartParseDataHandler(u8RecvChar);

        // 如果数据接收完成，处理数据帧
        if (bt_state == BT_STATE_COMPLETE)
        {
           if(0==strcmp(bt_buffer,"LAVE\r\n"))
            {
               memset(bt_buffer, 0, 50);	
            bt_state = BT_STATE_IDLE;
            g_bt_configured=1;
//          DBG_SerialPutString(" AT+REST\r\n");//退出指令并进入透传模式
            return BT_CONFIG_SUCCESS;
            }
        }
    }
   return BT_CONFIG_FAIL;
}

// 蓝牙配置函数实现
static HIDO_UINT8  BT_wait_PairMode(void)

{
    HIDO_UINT8 u8RecvChar = 0;
    while (Uart_GetChar(UART_ID_BT, &u8RecvChar) == HIDO_OK)
    {
        BTUsartParseDataHandler(u8RecvChar);

        // 如果数据接收完成，处理数据帧
        if (bt_state == BT_STATE_COMPLETE)
        {
           if(0==strcmp(bt_buffer,"AT+LED1\r\n"))
            {
               memset(bt_buffer, 0, 50);	
            bt_state = BT_STATE_IDLE;
            g_bt_paired = 1;  
            return BT_CONFIG_SUCCESS;
            }
        }
    }
   return BT_CONFIG_FAIL;
}

// 蓝牙配置函数实现
static HIDO_UINT8  BT_connect_status(void)

{
    HIDO_UINT8 u8RecvChar = 0;
// DBG_SerialPutString("AT+CONN\r\n");
    while (Uart_GetChar(UART_ID_BT, &u8RecvChar) == HIDO_OK)
    {
        BTUsartParseDataHandler(u8RecvChar);

        // 如果数据接收完成，处理数据帧
        if (bt_state == BT_STATE_COMPLETE)
        {
           if(0==strcmp(bt_buffer,"1\r\n"))
            {
               memset(bt_buffer, 0, 50);	
            bt_state = BT_STATE_IDLE;
            g_bt_paired = 1;  
            return BT_CONFIG_SUCCESS;
            }
        }
    }
   return BT_CONFIG_FAIL;
}


// 蓝牙任务处理函数//DBG_SerialPutString("AT+SLAV\r\n");//配置从机
static void Bluetooth_Config_Slave(void)
{
   if(g_bt_configured==0)
      {
          if(g_bt_at_moded==0)
          {
            BT_Enter_ATmode();
          }
      else
      {
         if(BT_Get_SLaveMode())
             {
               g_bt_configured=1;
             }
      }	
      }
}

// 主处理函数
void Joystick_Process(Joystick_t *joy) {
    // 提取 y1 控制前进/后退
    int16_t motor_val = joy->y1;  // -100 ~ +100
    Set_Motor_PWM(motor_val);

    // 提取 x2 控制转向
    int16_t steering_val = joy->x2;  // -100 ~ +100
    Set_Steering_PWM(steering_val);
}

// 示例输入: "[joystick,-22,-2,0,0]"
void Parse_Joystick_Data(char *data) {
    Joystick_t joy = {0};

    if (strstr(data, "joystick") == NULL) return;

    // 跳过 "[joystick,"
    char *ptr = strstr(data, "joystick");
    if (!ptr) return;
    ptr += 10;  // 跳过 "joystick,"

    // 解析四个整数
    char *end;
    joy.x1 = strtol(ptr, &end, 10);
    if (end == ptr) return;
    ptr = end + 1;

    joy.y1 = strtol(ptr, &end, 10);
    if (end == ptr) return;
    ptr = end + 1;

    joy.x2 = strtol(ptr, &end, 10);
    if (end == ptr) return;
    ptr = end + 1;

    joy.y2 = strtol(ptr, &end, 10);

    // 处理数据
    Joystick_Process(&joy);
}


/*******************************************************************************
 * Function Name     : BT_Init
 * Description       : 调试打印轮询
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : 杜键
 * Modified Date:    : 2018年7月23日
 *******************************************************************************/
/**
 * @brief Bluetooth Poll Function (Circular DMA Mode with GetChar)
 */
HIDO_VOID BT_Poll(void)
{
  if (uart6_dma_recv_len>0)//HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_13) == GPIO_PIN_RESET
  {
       if (uart6_dma_recv_end_flag == 1) //接收完成标志
         {
              Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)uart6_dma_rxbuf,uart6_dma_recv_len);
            // 解析数据
            Parse_Joystick_Data(uart6_dma_rxbuf);
         }
       uart6_dma_recv_len = 0;//清除计数
        uart6_dma_recv_end_flag = 0;//清除接收结束标志位
        memset(uart6_dma_rxbuf, 0, UART6_DMA_RX_BUF_SIZE);
        HAL_UART_Receive_DMA(&huart6, uart6_dma_rxbuf, UART6_DMA_RX_BUF_SIZE); //重新打开DMA接收
  }
  #if 0
  else if(0==g_com_map[BT_SLAVE_STATUS])
      {
//    DBG_SerialPutString("AT+ENAT\r\n");
      //delay_ms(10);
       DBG_SerialPutString("AT+SLAV\r\n");
      g_com_map[BT_SLAVE_STATUS]=1;
      save_com_map_to_flash();
      }
  #endif
  }
    static HIDO_UINT32 s_last_poll_tick = 0;
    
    // Periodic debug log
    if (HAL_GetTick() - s_last_poll_tick > 1000)
    {
        s_last_poll_tick = HAL_GetTick();
#if ENABLE_BT_DEBUG_LOG
        HIDO_UINT32 dma_cnt = 0;
        if (huart6.hdmarx != NULL)
        {
            dma_cnt = __HAL_DMA_GET_COUNTER(huart6.hdmarx);
        }
        HIDO_UINT32 cr1 = huart6.Instance->CR1;
        HIDO_UINT32 isr = huart6.Instance->ISR;
        HIDO_Debug2("[BT] Poll: IntCnt=%u, DMA_CNDTR=%u, CR1=0x%X, ISR=0x%X\r\n", 
                    g_u32BtIdleIntCount, dma_cnt, cr1, isr);
#endif
    }

    // Read and process bytes from circular DMA buffer
    HIDO_UINT8 byte = 0;
    while (Uart_GetChar(UART_ID_BT, &byte) == HIDO_OK)
    {
        BT_RecvFSM(byte);
    }
}

/**
 * @brief CRC16 Calculation (Poly 0x1021)
 */
static HIDO_UINT16 Calculate_CRC16(const HIDO_UINT8 *data, HIDO_UINT16 len)
{
    HIDO_UINT16 crc = 0xFFFF;
    for (HIDO_UINT16 i = 0; i < len; i++)
    {
        crc ^= (HIDO_UINT16)data[i] << 8;
        for (HIDO_UINT8 j = 0; j < 8; j++)
        {
            if (crc & 0x8000)
            {
                crc = (crc << 1) ^ CRC16_POLY;
            }
            else
            {
                crc <<= 1;
            }
        }
    }
    return crc;
}

/**
 * @brief Process a complete BT frame (called after FSM receives full frame)
 */
static HIDO_VOID BT_ProcessFrame(void)
{
    // Frame is stored in s_bt_fsm.rx_buf
    // Structure: Header(5) + SeqNum(2) + Payload(4) + CRC(2) + Tail(1) = 14 bytes total
    // rx_buf[0..4]: Header (AA 55 Cmd Len_L Len_H)
    // rx_buf[5..6]: SeqNum
    // rx_buf[7..10]: Payload (Steer + Speed + Reserved)
    // rx_buf[11..12]: CRC
    // rx_buf[13]: Tail
    
    HIDO_UINT8 *pBuf = s_bt_fsm.rx_buf;
    HIDO_UINT16 total_len = 5 + s_bt_fsm.data_len; // Header(5) + Data(SeqNum + Payload + CRC + Tail)
    
    if (total_len < 14 || total_len > BT_RX_BUF_SIZE)
    {
#if ENABLE_BT_DEBUG_LOG
        HIDO_Debug2("[BT] Invalid frame length: %u\r\n", total_len);
#endif
        return;
    }
    
#if ENABLE_BT_DEBUG_LOG
    // Print entire frame in hex for debugging
    HIDO_Debug2("[BT] Frame (%u bytes): ", total_len);
    for (HIDO_UINT16 i = 0; i < total_len; i++)
    {
        HIDO_Debug2("%02X ", pBuf[i]);
    }
    HIDO_Debug2("\r\n");
#endif
    
    // Check CRC - Try different ranges to find the correct one
    HIDO_UINT16 crc_calc_len = total_len - 3; // Current: Header + SeqNum + Payload (11 bytes)
    HIDO_UINT16 calc_crc1 = Calculate_CRC16(pBuf, crc_calc_len); // Full: AA 55 03 09 00 67 00 00 0A 00 00
    HIDO_UINT16 calc_crc2 = Calculate_CRC16(pBuf + 3, crc_calc_len - 3); // From DataLen: 09 00 67 00 00 0A 00 00
    HIDO_UINT16 calc_crc3 = Calculate_CRC16(pBuf + 5, crc_calc_len - 5); // From SeqNum: 67 00 00 0A 00 00
    
    HIDO_UINT8 crc_low = pBuf[crc_calc_len];
    HIDO_UINT8 crc_high = pBuf[crc_calc_len + 1];
    HIDO_UINT16 recv_crc_le = (HIDO_UINT16)(crc_low | (crc_high << 8)); // Little Endian
    HIDO_UINT16 recv_crc_be = (HIDO_UINT16)((crc_low << 8) | crc_high); // Big Endian
    
#if ENABLE_BT_DEBUG_LOG
    HIDO_Debug2("[BT] CRC Test: Recv(LE)=%04X, Recv(BE)=%04X\r\n", recv_crc_le, recv_crc_be);
    HIDO_Debug2("  Calc1(Full,11B)=%04X, Calc2(FromLen,8B)=%04X, Calc3(FromSeq,6B)=%04X\r\n", 
                calc_crc1, calc_crc2, calc_crc3);
#endif
    
    // Use calc_crc1 for now (original logic)
    if (calc_crc1 != recv_crc_le && calc_crc1 != recv_crc_be)
    {
#if ENABLE_BT_DEBUG_LOG
        HIDO_Debug2("[BT] CRC Fail (all methods)\r\n");
#endif
        // Continue processing for debug
        // return;
    }
    
    // Check Tail
    HIDO_UINT8 tail = pBuf[total_len - 1];
    if (tail != BT_FRAME_TAIL)
    {
#if ENABLE_BT_DEBUG_LOG
        HIDO_Debug2("[BT] Invalid Tail: %02X\r\n", tail);
#endif
        return;
    }
    
    // Parse command based on type
    switch (s_bt_fsm.cmd_type)
    {
        case BT_CMD_CONTROL:
        {
            // Payload starts at offset 7 (after Header(5) + SeqNum(2))
            ST_BT_ControlData *pCtrl = (ST_BT_ControlData *)&pBuf[7];
            
            // Check RC signal status
            if (SBUS_IsSignalValid(500) == HIDO_FALSE)
            {
#if ENABLE_BT_DEBUG_LOG
                HIDO_Debug2("[BT] Control: Steer %d, Speed %d\r\n", pCtrl->m_i8SteerSpeed, pCtrl->m_i8TravelSpeed);
#endif
                
                Set_Steering_PWM(pCtrl->m_i8SteerSpeed);
                Set_Motor_PWM(pCtrl->m_i8TravelSpeed);
            }
            else
            {
                // HIDO_Debug2("[BT] Ignored (RC Active)\r\n");
            }
            break;
        }
        default:
#if ENABLE_BT_DEBUG_LOG
            HIDO_Debug2("[BT] Unknown Cmd: 0x%02X\r\n", s_bt_fsm.cmd_type);
#endif
            break;
    }
}

/**
 * @brief Receive State Machine (called for each byte)
 */
static HIDO_VOID BT_RecvFSM(HIDO_UINT8 byte)
{
    switch (s_bt_fsm.state)
    {
        case BT_FSM_IDLE:
            if (byte == BT_FRAME_HEADER1) // 0xAA
            {
                s_bt_fsm.rx_buf[0] = byte;
                s_bt_fsm.data_idx = 1;
                s_bt_fsm.state = BT_FSM_HEADER2;
            }
            break;
            
        case BT_FSM_HEADER2:
            if (byte == BT_FRAME_HEADER2) // 0x55
            {
                s_bt_fsm.rx_buf[1] = byte;
                s_bt_fsm.data_idx = 2;
                s_bt_fsm.state = BT_FSM_CMD;
            }
            else
            {
                s_bt_fsm.state = BT_FSM_IDLE;
            }
            break;
            
        case BT_FSM_CMD:
            s_bt_fsm.rx_buf[2] = byte;
            s_bt_fsm.cmd_type = byte;
            s_bt_fsm.data_idx = 3;
            s_bt_fsm.state = BT_FSM_LEN_LOW;
            break;
            
        case BT_FSM_LEN_LOW:
            s_bt_fsm.rx_buf[3] = byte;
            s_bt_fsm.data_len = byte; // Low byte first
            s_bt_fsm.data_idx = 4;
            s_bt_fsm.state = BT_FSM_LEN_HIGH;
            break;
            
        case BT_FSM_LEN_HIGH:
            s_bt_fsm.rx_buf[4] = byte;
            s_bt_fsm.data_len |= (HIDO_UINT16)byte << 8; // High byte
            s_bt_fsm.data_idx = 5;
            
            // Sanity check on length
            if (s_bt_fsm.data_len > (BT_RX_BUF_SIZE - 5))
            {
#if ENABLE_BT_DEBUG_LOG
                HIDO_Debug2("[BT] FSM: Length too large (%u), resetting\r\n", s_bt_fsm.data_len);
#endif
                s_bt_fsm.state = BT_FSM_IDLE;
            }
            else
            {
                s_bt_fsm.state = BT_FSM_DATA;
            }
            break;
            
        case BT_FSM_DATA:
            s_bt_fsm.rx_buf[s_bt_fsm.data_idx++] = byte;
            
            // Check if we've received all data bytes
            // data_len includes: SeqNum(2) + Payload(N) + CRC(2) + Tail(1)
            // Total frame = Header(5) + data_len
            // We've already stored Header(5), now need data_len more bytes
            if (s_bt_fsm.data_idx >= (5 + s_bt_fsm.data_len))
            {
                // Frame complete, process it
                BT_ProcessFrame();
                s_bt_fsm.state = BT_FSM_IDLE;
            }
            
            // Safety check: prevent buffer overflow
            if (s_bt_fsm.data_idx >= BT_RX_BUF_SIZE)
            {
#if ENABLE_BT_DEBUG_LOG
                HIDO_Debug2("[BT] FSM: Buffer overflow, resetting\r\n");
#endif
                s_bt_fsm.state = BT_FSM_IDLE;
            }
            break;
            
        default:
            s_bt_fsm.state = BT_FSM_IDLE;
            break;
    }
}

/**
 * @brief Process Bluetooth Command Frame (Legacy, kept for reference)
 */
static HIDO_VOID Process_Command(const HIDO_UINT8 *pData, HIDO_UINT16 u16Len)
{
    // HIDO_Debug2("[BT] Processing %u bytes\r\n", u16Len);
    if (u16Len < sizeof(ST_BT_FrameHeader) + 3) // Header + CRC + Tail min
    {
        return;
    }

    ST_BT_FrameHeader *pHeader = (ST_BT_FrameHeader *)pData;

    // Check Header
    if (pHeader->m_u8Header1 != BT_FRAME_HEADER1 || pHeader->m_u8Header2 != BT_FRAME_HEADER2)
    {
        HIDO_Debug2("[BT] Invalid Header: %02X %02X\r\n", pHeader->m_u8Header1, pHeader->m_u8Header2);
        return;
    }

    // Check Length (Total frame size check)
    // Protocol Definition: DataLen (9) includes Seq(2) + Payload(4) + CRC(2) + Tail(1)
    // Frame Structure: Header(5) + DataLen(9) = 14 bytes
    // ST_BT_FrameHeader(7) includes SeqNum(2) at the end.
    // So HeaderBase (AA 55 Cmd Len) is 5 bytes.
    HIDO_UINT16 headerBaseSize = 5; 
    HIDO_UINT16 declaredLen = pHeader->m_u16DataLen;
    HIDO_UINT16 expectedLen = headerBaseSize + declaredLen;

    if (u16Len < expectedLen)
    {
        HIDO_Debug2("[BT] Incomplete Frame: Recv %d, Expected %d\r\n", u16Len, expectedLen);
        return;
    }

    // Check Tail
    if (pData[expectedLen - 1] != BT_FRAME_TAIL)
    {
        HIDO_Debug2("[BT] Invalid Tail: %02X\r\n", pData[expectedLen - 1]);
        return;
    }

    // Check CRC
    // CRC is calculated over Header + Payload (excluding CRC and Tail)
    // Range: 0 to (TotalLen - CRC(2) - Tail(1)) = TotalLen - 3
    HIDO_UINT16 calcLen = expectedLen - 3;
    HIDO_UINT16 calcCRC = Calculate_CRC16(pData, calcLen);
    
    // CRC is at offset: expectedLen - 3 (Low byte), expectedLen - 2 (High byte) - assuming Little Endian in stream
    // User Example: ... C3 7B 0D. CRC is C3 7B. Tail 0D.
    // 7B is at -2, C3 is at -3.
    // If recvCRC is uint16, let's read it.
    HIDO_UINT8 crcLow = pData[expectedLen - 3];
    HIDO_UINT8 crcHigh = pData[expectedLen - 2];
    HIDO_UINT16 recvCRC = (HIDO_UINT16)(crcLow | (crcHigh << 8)); 

    if (calcCRC != recvCRC)
    {
        HIDO_Debug2("[BT] CRC Fail: Calc %04X, Recv %04X\r\n", calcCRC, recvCRC);
        // Continue processing for debugging, but typically should return
        // return; 
    }

    // Payload starts after SeqNum. 
    // ST_BT_FrameHeader includes SeqNum. sizeof is 7.
    // So pPayload points to Data after SeqNum.
    const HIDO_UINT8 *pPayload = pData + sizeof(ST_BT_FrameHeader);
    
    // Payload length for Command logic (excluding Seq, CRC, Tail)
    // DataLen(9) - Seq(2) - CRC(2) - Tail(1) = 4 bytes.
    HIDO_INT16 realPayloadLen = declaredLen - 2 - 3; 

    switch (pHeader->m_u8CmdType)
    {
        case BT_CMD_PATH_COORDS:
        {
            HIDO_UINT8 pathCount = pPayload[0];
            HIDO_Debug2("[BT] Path Coords: Count %d\r\n", pathCount);
            ST_BT_PathPoint *pPoints = (ST_BT_PathPoint *)(pPayload + 1);
            for(int i=0; i<pathCount; i++)
            {
                HIDO_Debug2("  Pt%d: %.2f, %.2f\r\n", i, pPoints[i].m_dX, pPoints[i].m_dY);
            }
            // TODO: Store path points
            break;
        }
        case BT_CMD_REF_POINT:
        {
            if (realPayloadLen >= sizeof(ST_BT_RefPointData))
            {
                ST_BT_RefPointData *pRef = (ST_BT_RefPointData *)pPayload;
                HIDO_Debug2("[BT] Ref Point: Lat %.8f %c, Lon %.8f %c\r\n", 
                            pRef->m_dLat, pRef->m_cLatDir, pRef->m_dLon, pRef->m_cLonDir);
                // TODO: Store ref point
            }
            break;
        }
        case BT_CMD_CONTROL:
        {
            if (realPayloadLen >= sizeof(ST_BT_ControlData))
            {
                ST_BT_ControlData *pCtrl = (ST_BT_ControlData *)pPayload;
                
                // Check RC signal status
                // "Bluetooth control car, requirement is can only execute if remote control signal is NOT received."
                // SBUS_IsSignalValid returns HIDO_TRUE if valid (connected)
                if (SBUS_IsSignalValid(500) == HIDO_FALSE)
                {
                    HIDO_Debug2("[BT] Control: Steer %d, Speed %d\r\n", pCtrl->m_i8SteerSpeed, pCtrl->m_i8TravelSpeed);
                    
                    Set_Steering_PWM(pCtrl->m_i8SteerSpeed);
                    Set_Motor_PWM(pCtrl->m_i8TravelSpeed);
                }
                else
                {
                    // HIDO_Debug2("[BT] Ignored (RC Active)\r\n");
                }
            }
            break;
        }
        default:
            HIDO_Debug2("[BT] Unknown Cmd: 0x%02X\r\n", pHeader->m_u8CmdType);
            break;
    }
}