校准完成，准备修改一下运动控制算法，转向的时候有直行分量，保存一版

yincheng.zhong

2025-12-05 18f1d1afd16ae159b9f20cef640a594c848ad249

 STM32H743/FML/bluetooth.c

@@ -1,52 +1,56 @@
/*******************************************************************************
 * 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                                      *
 *******************************************************************************/
// 全局状态标志


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

/*******************************************************************************
 *                             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.

/*******************************************************************************
 * 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 Process_Command(const HIDO_UINT8 *pData, HIDO_UINT16 u16Len);

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

/**
 * @brief Initialize Bluetooth Module
 */
HIDO_VOID BT_Init(void)
{
    ST_UartInit stInit;
@@ -61,121 +65,183 @@
    stInit.m_u32TxQueueMemberCnt = BT_UART_TX_QUEUE_MEMBER_CNT;
    Uart_Init(UART_ID_BT, &stInit);


    UART6_StartReceive();
}

/**
 * @brief 启动 UART6 接收（启用 IDLE 中断 + DMA）
 * @brief Start UART6 Receive with IDLE IT + DMA
 */
void UART6_StartReceive(void)
{
    // 清除标志位，防止上电误触发
    // Clear IDLE flag
    __HAL_UART_CLEAR_IDLEFLAG(&huart6);

    // 使能 IDLE 中断
    // Enable IDLE interrupt
    __HAL_UART_ENABLE_IT(&huart6, UART_IT_IDLE);

    // 启动 DMA 接收（循环模式）
    HAL_UART_Receive_DMA(&huart6, uart6_dma_rxbuf, UART6_DMA_RX_BUF_SIZE);
    // Start DMA Receive
    HAL_UART_Receive_DMA(&huart6, uart6_dma_rxbuf, BT_UART_RX_BUF_SIZE);
}

/**
 * @brief 发送字符串
 * @brief Bluetooth Poll Function
 */
void UART6_SendString(const char *str)
{
    HAL_UART_Transmit_DMA(&huart6, (uint8_t *)str, strlen(str));
}

/**
 * @brief 发送数据数组
 */
void UART6_SendArray(uint8_t *data, uint16_t len)
{
    HAL_UART_Transmit_DMA(&huart6, data, len);
}

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日
 *******************************************************************************/
HIDO_VOID BT_Poll(void)
{
        static HIDO_UINT8 l_uart6_dma_rxbuf[100];
    if (uart6_dma_recv_len > 0) //HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_13) == GPIO_PIN_RESET
    if (uart6_dma_recv_len > 0)
    {
        if (uart6_dma_recv_end_flag == 1) //接收完成标志
        if (uart6_dma_recv_end_flag == 1)
        {
            HIDO_UtilSnprintf((HIDO_CHAR *)l_uart6_dma_rxbuf, sizeof(l_uart6_dma_rxbuf), "buff=%s\r\n", uart6_dma_rxbuf);
            Uart_Send(UART_ID_DBG, (HIDO_UINT8 *)l_uart6_dma_rxbuf, strlen(l_uart6_dma_rxbuf));
            // 解析数据
            Parse_Joystick_Data(uart6_dma_rxbuf);
            // Process received frame
            Process_Command(uart6_dma_rxbuf, uart6_dma_recv_len);
        }

        uart6_dma_recv_len = 0;//清除计数
        uart6_dma_recv_end_flag = 0;//清除接收结束标志位
        memset(uart6_dma_rxbuf, 0, UART6_DMA_RX_BUF_SIZE);
        // 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, UART6_DMA_RX_BUF_SIZE); //重新打开DMA接收
        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;
    }
}