Lawnmower_STM32H7.git

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


			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 l_au8BTUartRxBuf[BT_UART_RX_BUF_SIZE];
			static HIDO_UINT8 l_au8BTUartTxBuf[BT_UART_TX_BUF_SIZE];

			// 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;

			// 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;
			@@ -60,122 +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();
			}

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

			// 使能 IDLE 中断
			__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);

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

			/**
			* @brief 发送字符串
			* @brief Bluetooth Poll Function (Circular DMA Mode with GetChar)
			*/
			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
			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)
			{
			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);
			dma_cnt = __HAL_DMA_GET_COUNTER(huart6.hdmarx);
			}

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