/*******************************************************************************
|
* 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_Util.h"
|
#include "DBG.h"
|
#include "Uart.h"
|
#include "pwm_ctrol.h"
|
#include "SBUS.h"
|
|
/*******************************************************************************
|
* Macro *
|
*******************************************************************************/
|
#define CRC16_POLY 0x1021
|
|
/*******************************************************************************
|
* Local Variable *
|
*******************************************************************************/
|
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};
|
HIDO_UINT8 uart6_dma_recv_end_flag = 0;
|
HIDO_UINT16 uart6_dma_recv_len = 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.
|
|
/*******************************************************************************
|
* 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);
|
|
/*******************************************************************************
|
* Global Function *
|
*******************************************************************************/
|
|
/**
|
* @brief Initialize Bluetooth Module
|
*/
|
HIDO_VOID BT_Init(void)
|
{
|
ST_UartInit stInit;
|
|
memset(&stInit, 0, sizeof(stInit));
|
stInit.m_eRxMode = UART_RX_MODE_DMA;
|
stInit.m_eTxMode = UART_TX_MODE_DMA;
|
stInit.m_pu8RxBuf = l_au8BTUartRxBuf;
|
stInit.m_u32RxBufSize = BT_UART_RX_BUF_SIZE;
|
stInit.m_pu8TxBuf = l_au8BTUartTxBuf;
|
stInit.m_u32TxBufSize = BT_UART_TX_BUF_SIZE;
|
stInit.m_u32TxQueueMemberCnt = BT_UART_TX_QUEUE_MEMBER_CNT;
|
Uart_Init(UART_ID_BT, &stInit);
|
|
UART6_StartReceive();
|
}
|
|
/**
|
* @brief Start UART6 Receive with IDLE IT + DMA
|
*/
|
void UART6_StartReceive(void)
|
{
|
// Clear IDLE flag
|
__HAL_UART_CLEAR_IDLEFLAG(&huart6);
|
|
// Enable IDLE interrupt
|
__HAL_UART_ENABLE_IT(&huart6, UART_IT_IDLE);
|
|
// Start DMA Receive
|
HAL_UART_Receive_DMA(&huart6, uart6_dma_rxbuf, BT_UART_RX_BUF_SIZE);
|
}
|
|
/**
|
* @brief Bluetooth Poll Function
|
*/
|
HIDO_VOID BT_Poll(void)
|
{
|
if (uart6_dma_recv_len > 0)
|
{
|
if (uart6_dma_recv_end_flag == 1)
|
{
|
// Process received frame
|
Process_Command(uart6_dma_rxbuf, uart6_dma_recv_len);
|
}
|
|
// 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);
|
}
|
}
|
|
/**
|
* @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 Bluetooth Command Frame
|
*/
|
static HIDO_VOID Process_Command(const HIDO_UINT8 *pData, HIDO_UINT16 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)
|
HIDO_UINT16 payloadLen = pHeader->m_u16DataLen;
|
HIDO_UINT16 expectedLen = sizeof(ST_BT_FrameHeader) + payloadLen + 3; // + CRC(2) + Tail(1)
|
|
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\r\n");
|
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;
|
|
if (calcCRC != recvCRC)
|
{
|
// Try Big Endian check just in case
|
// HIDO_Debug2("[BT] CRC Fail: Calc %04X, Recv %04X\r\n", calcCRC, recvCRC);
|
// return;
|
// Note: If CRC fails, we should probably drop. But I'll leave it as check.
|
}
|
|
const HIDO_UINT8 *pPayload = pData + sizeof(ST_BT_FrameHeader);
|
|
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 (payloadLen >= 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 (payloadLen >= 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;
|
}
|
}
|
