From 4adc78553c8d48ff122506195fa33641134bd7b1 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 13 十二月 2025 18:55:14 +0800
Subject: [PATCH] 路径点不重复的控制算法测试通了，但是好像不如之前路径点重复的版本好。增加了蓝牙接口部分。准备移植外包的MQTT。
---
STM32H743/FML/bluetooth.c | 330 +++++++++++++++++++++++++++++++++++++++++++++++-------
1 files changed, 286 insertions(+), 44 deletions(-)
diff --git a/STM32H743/FML/bluetooth.c b/STM32H743/FML/bluetooth.c
index d98fcd5..daef5e6 100644
--- a/STM32H743/FML/bluetooth.c
+++ b/STM32H743/FML/bluetooth.c
@@ -23,6 +23,8 @@
* Macro *
*******************************************************************************/
#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 *
@@ -30,19 +32,44 @@
static HIDO_UINT8 l_au8BTUartRxBuf[BT_UART_RX_BUF_SIZE];
static HIDO_UINT8 l_au8BTUartTxBuf[BT_UART_TX_BUF_SIZE];
-// DMA Buffer for UART6
-HIDO_UINT8 uart6_dma_rxbuf[BT_UART_RX_BUF_SIZE] = {0};
+// 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;
extern UART_HandleTypeDef huart6;
-extern DMA_HandleTypeDef hdma_usart6_rx; // Also needed for __HAL_DMA_GET_COUNTER if used here, but it's used in IT. Wait, IT uses it. Bluetooth.c uses 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;
/*******************************************************************************
* Local Function Declaration *
*******************************************************************************/
static HIDO_UINT16 Calculate_CRC16(const HIDO_UINT8 *data, HIDO_UINT16 len);
-static HIDO_VOID Process_Command(const HIDO_UINT8 *pData, HIDO_UINT16 u16Len);
+static HIDO_VOID BT_RecvFSM(HIDO_UINT8 byte);
+static HIDO_VOID BT_ProcessFrame(void);
/*******************************************************************************
* Global Function *
@@ -64,46 +91,55 @@
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();
}
/**
- * @brief Start UART6 Receive with IDLE IT + DMA
+ * @brief Start UART6 Receive with Circular DMA
*/
void UART6_StartReceive(void)
{
// Clear IDLE flag
__HAL_UART_CLEAR_IDLEFLAG(&huart6);
- // Enable IDLE interrupt
- __HAL_UART_ENABLE_IT(&huart6, UART_IT_IDLE);
+ // 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
+ // Start DMA Receive (Circular Mode configured in HAL_MSP)
HAL_UART_Receive_DMA(&huart6, uart6_dma_rxbuf, BT_UART_RX_BUF_SIZE);
}
/**
- * @brief Bluetooth Poll Function
+ * @brief Bluetooth Poll Function (Circular DMA Mode with GetChar)
*/
HIDO_VOID BT_Poll(void)
{
- if (uart6_dma_recv_len > 0)
+ static HIDO_UINT32 s_last_poll_tick = 0;
+
+ // Periodic debug log
+ if (HAL_GetTick() - s_last_poll_tick > 1000)
{
- if (uart6_dma_recv_end_flag == 1)
+ s_last_poll_tick = HAL_GetTick();
+#if ENABLE_BT_DEBUG_LOG
+ HIDO_UINT32 dma_cnt = 0;
+ if (huart6.hdmarx != NULL)
{
- // Process received frame
- Process_Command(uart6_dma_rxbuf, uart6_dma_recv_len);
+ 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
+ }
- // Reset buffer and flags
- uart6_dma_recv_len = 0;
- uart6_dma_recv_end_flag = 0;
- memset(uart6_dma_rxbuf, 0, BT_UART_RX_BUF_SIZE);
-
- // Restart reception
- __HAL_UART_CLEAR_IDLEFLAG(&huart6);
- HAL_UART_Receive_DMA(&huart6, uart6_dma_rxbuf, BT_UART_RX_BUF_SIZE);
+ // Read and process bytes from circular DMA buffer
+ HIDO_UINT8 byte = 0;
+ while (Uart_GetChar(UART_ID_BT, &byte) == HIDO_OK)
+ {
+ BT_RecvFSM(byte);
}
}
@@ -132,10 +168,206 @@
}
/**
- * @brief Process Bluetooth Command Frame
+ * @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;
@@ -151,8 +383,13 @@
}
// Check Length (Total frame size check)
- HIDO_UINT16 payloadLen = pHeader->m_u16DataLen;
- HIDO_UINT16 expectedLen = sizeof(ST_BT_FrameHeader) + payloadLen + 3; // + CRC(2) + Tail(1)
+ // 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)
{
@@ -163,34 +400,39 @@
// Check Tail
if (pData[expectedLen - 1] != BT_FRAME_TAIL)
{
- HIDO_Debug2("[BT] Invalid Tail\r\n");
+ HIDO_Debug2("[BT] Invalid Tail: %02X\r\n", pData[expectedLen - 1]);
return;
}
// Check CRC
- // CRC is calculated over Header + Payload
- HIDO_UINT16 calcCRC = Calculate_CRC16(pData, sizeof(ST_BT_FrameHeader) + payloadLen);
- HIDO_UINT16 recvCRC = (HIDO_UINT16)(pData[expectedLen - 3] | (pData[expectedLen - 2] << 8)); // Little Endian from struct? No, usually network order or defined.
- // The CSV doesn't specify endianness, but usually STM32 is Little Endian.
- // However, protocols often use Big Endian (Network Byte Order).
- // Let's assume Little Endian for now as it's simpler with structs on STM32.
- // Wait, CSV says "CRC16, 2, uint16_t".
- // If I interpret it as raw bytes:
- // If the struct is packed, I can read it directly if alignment matches.
- // Let's read bytes to be safe against packing/endian issues if possible, but struct is packed.
- // Re-reading the CRC from the buffer:
- HIDO_UINT16 *pCrcPtr = (HIDO_UINT16 *)&pData[sizeof(ST_BT_FrameHeader) + payloadLen];
- recvCRC = *pCrcPtr;
+ // 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)
{
- // Try Big Endian check just in case
- // HIDO_Debug2("[BT] CRC Fail: Calc %04X, Recv %04X\r\n", calcCRC, recvCRC);
+ HIDO_Debug2("[BT] CRC Fail: Calc %04X, Recv %04X\r\n", calcCRC, recvCRC);
+ // Continue processing for debugging, but typically should return
// return;
- // Note: If CRC fails, we should probably drop. But I'll leave it as check.
}
+ // 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)
{
@@ -208,7 +450,7 @@
}
case BT_CMD_REF_POINT:
{
- if (payloadLen >= sizeof(ST_BT_RefPointData))
+ 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",
@@ -219,7 +461,7 @@
}
case BT_CMD_CONTROL:
{
- if (payloadLen >= sizeof(ST_BT_ControlData))
+ if (realPayloadLen >= sizeof(ST_BT_ControlData))
{
ST_BT_ControlData *pCtrl = (ST_BT_ControlData *)pPayload;
--
Gitblit v1.10.0