/*******************************************************************************
|
* File Name : PythonLink.c
|
* Description : Python通信链路模块实现
|
* Created on : 2025-11-13
|
* Author : Auto-generated
|
*******************************************************************************/
|
|
/*******************************************************************************
|
* Include Files *
|
*******************************************************************************/
|
#include "PythonLink.h"
|
#include "Uart.h"
|
#include "HIDO_Debug.h"
|
#include <string.h>
|
#include <math.h>
|
|
/*******************************************************************************
|
* Global Variables *
|
*******************************************************************************/
|
// UART 缓冲区
|
static HIDO_UINT8 g_au8PythonLinkTxFrameBuf[PYTHONLINK_TX_BUF_SIZE]; // 用于组帧
|
static HIDO_UINT8 g_au8PythonLinkTxQueueBuf[PYTHONLINK_TX_BUF_SIZE]; // 提供给UART FIFO
|
static HIDO_UINT8 g_au8PythonLinkRxBuf[PYTHONLINK_RX_BUF_SIZE];
|
typedef struct
|
{
|
HIDO_UINT32 m_u32LastPollTick;
|
HIDO_UINT32 m_u32LastDataTick;
|
HIDO_UINT32 m_u32TotalBytes;
|
HIDO_UINT32 m_u32ParsedFrames;
|
HIDO_UINT32 m_u32HandleFailCount;
|
HIDO_UINT32 m_u32DmaNullCount;
|
HIDO_UINT32 m_u32NoDataWarningTick;
|
HIDO_UINT32 m_u32LastLogTick;
|
} ST_PythonLink_RxDebug;
|
|
static ST_PythonLink_RxDebug g_stPythonLinkRxDebug;
|
|
// 统计计数
|
static HIDO_UINT32 g_u32GPSPacketCount = 0;
|
static HIDO_UINT32 g_u32IMUPacketCount = 0;
|
static HIDO_UINT32 g_u32ErrorCount = 0;
|
static HIDO_UINT32 g_u32ControlRxCount = 0;
|
|
// 控制命令缓存
|
static volatile HIDO_UINT16 g_u16LatestSteeringPWM = 1500; // 中位值
|
static volatile HIDO_UINT16 g_u16LatestThrottlePWM = 1500; // 中位值
|
static volatile HIDO_UINT32 g_u32ControlTimestamp = 0; // 命令时间戳
|
static volatile HIDO_BOOL g_bControlValid = HIDO_FALSE; // 命令有效标志
|
|
// DMA接收位置跟踪
|
static HIDO_UINT16 g_u16LastDMAPos = 0;
|
|
/*******************************************************************************
|
* Local Function *
|
*******************************************************************************/
|
|
/**
|
* @brief 计算16位累加校验和 (Checksum_u16)
|
* @param _pu8Data: 数据指针
|
* @param _u32Len: 数据长度
|
* @return 16位校验和
|
*/
|
static HIDO_UINT16 PythonLink_CalcChecksum(const HIDO_UINT8 *_pu8Data, HIDO_UINT32 _u32Len)
|
{
|
HIDO_UINT32 sum = 0;
|
HIDO_UINT32 i;
|
|
// 16位累加和
|
for (i = 0; i < _u32Len; i++)
|
{
|
sum += _pu8Data[i];
|
}
|
|
// 返回低16位
|
return (HIDO_UINT16)(sum & 0xFFFF);
|
}
|
|
#if (PYTHONLINK_FORCE_FIXED_PAYLOAD != 0)
|
/**
|
* @brief 调试: 生成固定内容的负载数据，便于观测串口是否乱码
|
* @param _pu8Dst 负载存放位置
|
* @param _u16Len 负载长度
|
* @param _u8Type 数据类型, 用于区分不同包
|
*/
|
static HIDO_VOID PythonLink_DebugFillFixedPayload(HIDO_UINT8 *_pu8Dst, HIDO_UINT16 _u16Len, HIDO_UINT8 _u8Type)
|
{
|
static const HIDO_UINT8 s_au8TagGPS[] = "GPSDBG";
|
static const HIDO_UINT8 s_au8TagIMU[] = "IMUDBG";
|
static const HIDO_UINT8 s_au8TagCTRL[] = "CTRDBG";
|
const HIDO_UINT8 *pTag = HIDO_NULL;
|
HIDO_UINT8 tagLen = 0;
|
HIDO_UINT16 i;
|
|
switch (_u8Type)
|
{
|
case PYTHONLINK_TYPE_GPS:
|
pTag = s_au8TagGPS;
|
tagLen = sizeof(s_au8TagGPS) - 1;
|
break;
|
case PYTHONLINK_TYPE_IMU:
|
pTag = s_au8TagIMU;
|
tagLen = sizeof(s_au8TagIMU) - 1;
|
break;
|
case PYTHONLINK_TYPE_CONTROL:
|
pTag = s_au8TagCTRL;
|
tagLen = sizeof(s_au8TagCTRL) - 1;
|
break;
|
default:
|
tagLen = 0;
|
break;
|
}
|
|
for (i = 0; i < _u16Len; i++)
|
{
|
if (i < tagLen && pTag != HIDO_NULL)
|
{
|
_pu8Dst[i] = pTag[i];
|
}
|
else
|
{
|
_pu8Dst[i] = (HIDO_UINT8)((_u8Type << 4) + (i & 0x0F));
|
}
|
}
|
}
|
#endif
|
|
/**
|
* @brief 解析并处理接收到的控制命令帧
|
* @param _pu8Data: 帧数据指针 (从帧头开始)
|
* @param _u16Len: 数据长度
|
* @return HIDO_OK: 解析成功, HIDO_ERR: 失败
|
*/
|
static HIDO_INT32 PythonLink_ParseControlFrame(const HIDO_UINT8 *_pu8Data, HIDO_UINT16 _u16Len)
|
{
|
ST_PythonLink_FrameHeader *pHeader;
|
ST_PythonLink_Control *pControl;
|
HIDO_UINT16 checksum_received, checksum_calc;
|
HIDO_UINT16 payload_len;
|
|
// 检查最小长度: 帧头(5) + 负载(4) + 校验和(2) + 帧尾(2) = 13字节
|
if (_u16Len < 13)
|
{
|
return HIDO_ERR;
|
}
|
|
// 解析帧头
|
pHeader = (ST_PythonLink_FrameHeader *)_pu8Data;
|
|
// 检查帧头
|
if (pHeader->m_u8Header1 != PYTHONLINK_FRAME_HEADER1 ||
|
pHeader->m_u8Header2 != PYTHONLINK_FRAME_HEADER2)
|
{
|
return HIDO_ERR;
|
}
|
|
// 检查类型
|
if (pHeader->m_u8Type != PYTHONLINK_TYPE_CONTROL)
|
{
|
return HIDO_ERR;
|
}
|
|
// 检查负载长度
|
payload_len = pHeader->m_u16Length;
|
if (payload_len != sizeof(ST_PythonLink_Control))
|
{
|
return HIDO_ERR;
|
}
|
|
// 检查总长度
|
HIDO_UINT16 expected_len = sizeof(ST_PythonLink_FrameHeader) + payload_len +
|
sizeof(ST_PythonLink_FrameFooter);
|
if (_u16Len < expected_len)
|
{
|
return HIDO_ERR;
|
}
|
|
// 校验Checksum (从类型字节开始，跳过帧头AA 55)
|
// 校验范围: Type + Length + Payload
|
checksum_calc = PythonLink_CalcChecksum(_pu8Data + 2,
|
sizeof(ST_PythonLink_FrameHeader) - 2 + payload_len);
|
checksum_received = *((HIDO_UINT16 *)(_pu8Data + sizeof(ST_PythonLink_FrameHeader) + payload_len));
|
|
if (checksum_received != checksum_calc)
|
{
|
g_u32ErrorCount++;
|
return HIDO_ERR;
|
}
|
|
// 检查帧尾
|
HIDO_UINT8 footer1 = _pu8Data[sizeof(ST_PythonLink_FrameHeader) + payload_len + 2];
|
HIDO_UINT8 footer2 = _pu8Data[sizeof(ST_PythonLink_FrameHeader) + payload_len + 3];
|
if (footer1 != PYTHONLINK_FRAME_FOOTER1 || footer2 != PYTHONLINK_FRAME_FOOTER2)
|
{
|
g_u32ErrorCount++;
|
return HIDO_ERR;
|
}
|
|
// 解析控制命令
|
pControl = (ST_PythonLink_Control *)(_pu8Data + sizeof(ST_PythonLink_FrameHeader));
|
|
// 限制PWM范围并更新全局变量
|
g_u16LatestSteeringPWM = (pControl->m_u16SteeringPWM < 1000) ? 1000 :
|
(pControl->m_u16SteeringPWM > 2000) ? 2000 :
|
pControl->m_u16SteeringPWM;
|
|
g_u16LatestThrottlePWM = (pControl->m_u16ThrottlePWM < 1000) ? 1000 :
|
(pControl->m_u16ThrottlePWM > 2000) ? 2000 :
|
pControl->m_u16ThrottlePWM;
|
|
g_u32ControlTimestamp = HAL_GetTick();
|
g_bControlValid = HIDO_TRUE;
|
g_u32ControlRxCount++;
|
|
return HIDO_OK;
|
}
|
|
/**
|
* @brief 在DMA接收缓冲区中查找并解析控制命令帧
|
* @return 解析成功的帧数量
|
*/
|
static HIDO_UINT32 PythonLink_ProcessRxBuffer(HIDO_VOID)
|
{
|
UART_HandleTypeDef *pUart = NULL;
|
HIDO_UINT16 dma_pos, available_bytes;
|
HIDO_UINT32 parsed_count = 0;
|
HIDO_INT32 ret;
|
|
// 获取UART句柄
|
HIDO_UINT32 tick_now = HAL_GetTick();
|
g_stPythonLinkRxDebug.m_u32LastPollTick = tick_now;
|
|
ret = Uart_GetHandle(UART_ID_PYTHON, (HIDO_VOID **)&pUart);
|
if (ret != HIDO_OK || pUart == NULL)
|
{
|
g_stPythonLinkRxDebug.m_u32HandleFailCount++;
|
return 0;
|
}
|
if (pUart->hdmarx == NULL)
|
{
|
g_stPythonLinkRxDebug.m_u32DmaNullCount++;
|
return 0;
|
}
|
|
// 获取DMA当前写入位置
|
dma_pos = PYTHONLINK_RX_BUF_SIZE - __HAL_DMA_GET_COUNTER(pUart->hdmarx);
|
|
// 计算可读字节数
|
if (dma_pos >= g_u16LastDMAPos)
|
{
|
available_bytes = dma_pos - g_u16LastDMAPos;
|
}
|
else
|
{
|
// DMA循环缓冲区回绕
|
available_bytes = PYTHONLINK_RX_BUF_SIZE - g_u16LastDMAPos + dma_pos;
|
}
|
|
// 如果没有新数据，直接返回
|
if (available_bytes == 0)
|
{
|
if ((tick_now - g_stPythonLinkRxDebug.m_u32LastDataTick) > 1000 &&
|
(tick_now - g_stPythonLinkRxDebug.m_u32NoDataWarningTick) > 1000)
|
{
|
g_stPythonLinkRxDebug.m_u32NoDataWarningTick = tick_now;
|
}
|
return 0;
|
}
|
|
g_stPythonLinkRxDebug.m_u32LastDataTick = tick_now;
|
g_stPythonLinkRxDebug.m_u32TotalBytes += available_bytes;
|
|
// 查找帧头并解析
|
HIDO_UINT16 search_pos = g_u16LastDMAPos;
|
while (available_bytes >= 13) // 最小帧长度
|
{
|
// 查找帧头 AA 55
|
HIDO_UINT8 byte1 = g_au8PythonLinkRxBuf[search_pos];
|
HIDO_UINT8 byte2 = g_au8PythonLinkRxBuf[(search_pos + 1) % PYTHONLINK_RX_BUF_SIZE];
|
|
if (byte1 == PYTHONLINK_FRAME_HEADER1 && byte2 == PYTHONLINK_FRAME_HEADER2)
|
{
|
// 找到帧头，尝试解析
|
HIDO_UINT8 temp_buf[64]; // 临时缓冲区
|
HIDO_UINT16 copy_len = (available_bytes > 64) ? 64 : available_bytes;
|
|
// 从循环缓冲区拷贝数据到临时缓冲区
|
for (HIDO_UINT16 i = 0; i < copy_len; i++)
|
{
|
temp_buf[i] = g_au8PythonLinkRxBuf[(search_pos + i) % PYTHONLINK_RX_BUF_SIZE];
|
}
|
|
// 尝试解析
|
if (PythonLink_ParseControlFrame(temp_buf, copy_len) == HIDO_OK)
|
{
|
// 解析成功，跳过这个帧
|
HIDO_UINT16 frame_len = 13; // 控制帧固定长度
|
search_pos = (search_pos + frame_len) % PYTHONLINK_RX_BUF_SIZE;
|
available_bytes -= frame_len;
|
parsed_count++;
|
g_stPythonLinkRxDebug.m_u32ParsedFrames++;
|
}
|
else
|
{
|
// 解析失败，跳过一个字节继续搜索
|
search_pos = (search_pos + 1) % PYTHONLINK_RX_BUF_SIZE;
|
available_bytes--;
|
}
|
}
|
else
|
{
|
// 不是帧头，继续搜索
|
search_pos = (search_pos + 1) % PYTHONLINK_RX_BUF_SIZE;
|
available_bytes--;
|
}
|
}
|
|
// 更新读取位置
|
g_u16LastDMAPos = search_pos;
|
|
return parsed_count;
|
}
|
|
/**
|
* @brief 发送数据帧
|
* @param _u8Type: 数据类型 (GPS/IMU)
|
* @param _pu8Payload: 数据负载指针
|
* @param _u16PayloadLen: 负载长度
|
* @return HIDO_OK: 成功, HIDO_ERR: 失败
|
*/
|
static HIDO_INT32 PythonLink_SendFrame(HIDO_UINT8 _u8Type,
|
const HIDO_UINT8 *_pu8Payload,
|
HIDO_UINT16 _u16PayloadLen)
|
{
|
HIDO_UINT32 frameLen = 0;
|
HIDO_UINT16 crc = 0;
|
ST_PythonLink_FrameHeader header;
|
ST_PythonLink_FrameFooter footer;
|
|
// 检查参数
|
if (_pu8Payload == NULL || _u16PayloadLen == 0)
|
{
|
g_u32ErrorCount++;
|
return HIDO_ERR;
|
}
|
|
// 检查缓冲区空间
|
if ((sizeof(header) + _u16PayloadLen + sizeof(footer)) > PYTHONLINK_TX_BUF_SIZE)
|
{
|
g_u32ErrorCount++;
|
return HIDO_ERR;
|
}
|
|
// 构造帧头
|
header.m_u8Header1 = PYTHONLINK_FRAME_HEADER1;
|
header.m_u8Header2 = PYTHONLINK_FRAME_HEADER2;
|
header.m_u8Type = _u8Type;
|
header.m_u16Length = _u16PayloadLen;
|
|
// 拷贝帧头到发送缓冲区
|
memcpy(&g_au8PythonLinkTxFrameBuf[frameLen], &header, sizeof(header));
|
frameLen += sizeof(header);
|
|
// 拷贝数据负载
|
#if (PYTHONLINK_FORCE_FIXED_PAYLOAD != 0)
|
PythonLink_DebugFillFixedPayload(&g_au8PythonLinkTxFrameBuf[frameLen], _u16PayloadLen, _u8Type);
|
#else
|
memcpy(&g_au8PythonLinkTxFrameBuf[frameLen], _pu8Payload, _u16PayloadLen);
|
#endif
|
frameLen += _u16PayloadLen;
|
|
// 计算Checksum (从类型字节开始，跳过帧头AA 55)
|
// 校验范围: Type + Length + Payload
|
crc = PythonLink_CalcChecksum(&g_au8PythonLinkTxFrameBuf[2], frameLen - 2);
|
|
// 构造帧尾
|
footer.m_u16CRC = crc;
|
footer.m_u8Footer1 = PYTHONLINK_FRAME_FOOTER1;
|
footer.m_u8Footer2 = PYTHONLINK_FRAME_FOOTER2;
|
|
// 拷贝帧尾
|
memcpy(&g_au8PythonLinkTxFrameBuf[frameLen], &footer, sizeof(footer));
|
frameLen += sizeof(footer);
|
|
// 通过UART发送
|
if (Uart_Send(UART_ID_PYTHON, g_au8PythonLinkTxFrameBuf, frameLen) != HIDO_OK)
|
{
|
g_u32ErrorCount++;
|
return HIDO_ERR;
|
}
|
|
return HIDO_OK;
|
}
|
|
/*******************************************************************************
|
* Global Function *
|
*******************************************************************************/
|
|
/**
|
* @brief 初始化 PythonLink 模块
|
*/
|
HIDO_INT32 PythonLink_Init(HIDO_VOID)
|
{
|
ST_UartInit stUartInit;
|
UART_HandleTypeDef *pUart = NULL;
|
HIDO_INT32 ret;
|
|
// 检查 UART 是否已注册
|
ret = Uart_GetHandle(UART_ID_PYTHON, (HIDO_VOID **)&pUart);
|
if (ret != HIDO_OK || pUart == NULL)
|
{
|
HIDO_Debug2("[PythonLink] UART_ID_PYTHON not registered!\r\n");
|
return HIDO_ERR;
|
}
|
|
// 清空缓冲区
|
memset(g_au8PythonLinkTxFrameBuf, 0, sizeof(g_au8PythonLinkTxFrameBuf));
|
memset(g_au8PythonLinkTxQueueBuf, 0, sizeof(g_au8PythonLinkTxQueueBuf));
|
memset(g_au8PythonLinkRxBuf, 0, sizeof(g_au8PythonLinkRxBuf));
|
|
// 配置 UART (DMA 模式)
|
stUartInit.m_eTxMode = UART_TX_MODE_DMA;
|
stUartInit.m_pu8TxBuf = g_au8PythonLinkTxQueueBuf;
|
stUartInit.m_u32TxBufSize = PYTHONLINK_TX_BUF_SIZE;
|
stUartInit.m_u32TxQueueMemberCnt = 16; // 发送队列深度 (增大以缓冲更多 DMA 事务)
|
|
stUartInit.m_eRxMode = UART_RX_MODE_DMA;
|
stUartInit.m_pu8RxBuf = g_au8PythonLinkRxBuf;
|
stUartInit.m_u32RxBufSize = PYTHONLINK_RX_BUF_SIZE;
|
stUartInit.m_fnRxISR = NULL;
|
|
ret = Uart_Init(UART_ID_PYTHON, &stUartInit);
|
if (ret != HIDO_OK)
|
{
|
HIDO_Debug2("[PythonLink] UART init failed!\r\n");
|
return HIDO_ERR;
|
}
|
|
// 重置统计计数
|
g_u32GPSPacketCount = 0;
|
g_u32IMUPacketCount = 0;
|
g_u32ErrorCount = 0;
|
|
HIDO_Debug2("[PythonLink] Init success (UART5, 921600 bps, DMA)\r\n");
|
return HIDO_OK;
|
}
|
|
/**
|
* @brief PythonLink 轮询函数
|
*/
|
HIDO_VOID PythonLink_Poll(HIDO_VOID)
|
{
|
// 处理接收缓冲区中的控制命令
|
PythonLink_ProcessRxBuffer();
|
|
// 检查控制命令超时 (1秒无新命令则标记为无效)
|
if (g_bControlValid == HIDO_TRUE)
|
{
|
HIDO_UINT32 elapsed = HAL_GetTick() - g_u32ControlTimestamp;
|
if (elapsed > 1000)
|
{
|
g_bControlValid = HIDO_FALSE;
|
// 超时后恢复到中位值
|
g_u16LatestSteeringPWM = 1500;
|
g_u16LatestThrottlePWM = 1500;
|
}
|
}
|
}
|
|
/**
|
* @brief 发送GPS数据包
|
*/
|
HIDO_INT32 PythonLink_SendGPSData(const ST_GPRMI *_pstGPRMI)
|
{
|
ST_PythonLink_GPS gpsPacket;
|
HIDO_INT32 ret;
|
|
// 检查参数
|
if (_pstGPRMI == NULL || _pstGPRMI->m_bValid == HIDO_FALSE)
|
{
|
return HIDO_ERR;
|
}
|
|
// 填充GPS数据包
|
memset(&gpsPacket, 0, sizeof(gpsPacket));
|
gpsPacket.m_dLatitude = _pstGPRMI->m_dLatitude;
|
gpsPacket.m_dLongitude = _pstGPRMI->m_dLongitude;
|
|
// 默认使用GPRMI提供的航向角, 若不可用则回退到速度计算
|
HIDO_FLOAT heading = _pstGPRMI->m_fHeadingAngle;
|
if (!isfinite(heading))
|
{
|
heading = 0.0f;
|
}
|
|
if ((!isfinite(heading) || heading == 0.0f) &&
|
(_pstGPRMI->m_fEastVelocity != 0.0f || _pstGPRMI->m_fNorthVelocity != 0.0f))
|
{
|
// atan2(东,北) 得到相对正北方向的角度
|
heading = atan2f(_pstGPRMI->m_fEastVelocity, _pstGPRMI->m_fNorthVelocity) * 180.0f / 3.14159265f;
|
if (heading < 0.0f)
|
{
|
heading += 360.0f;
|
}
|
}
|
|
gpsPacket.m_fHeadingAngle = heading;
|
gpsPacket.m_fPitchAngle = _pstGPRMI->m_fPitchAngle;
|
gpsPacket.m_fRollAngle = _pstGPRMI->m_fRollAngle;
|
if (!isfinite(gpsPacket.m_fPitchAngle))
|
{
|
gpsPacket.m_fPitchAngle = 0.0f;
|
}
|
if (!isfinite(gpsPacket.m_fRollAngle))
|
{
|
gpsPacket.m_fRollAngle = 0.0f;
|
}
|
|
gpsPacket.m_fEastVelocity = _pstGPRMI->m_fEastVelocity;
|
gpsPacket.m_fNorthVelocity = _pstGPRMI->m_fNorthVelocity;
|
gpsPacket.m_fUpVelocity = _pstGPRMI->m_fUpVelocity;
|
gpsPacket.m_fAltitude = _pstGPRMI->m_fAltitude;
|
gpsPacket.m_u32UTCTime = _pstGPRMI->m_u32UTCTime;
|
gpsPacket.m_u8PositionQuality = _pstGPRMI->m_u8PositionQuality;
|
gpsPacket.m_u8SatelliteCount = _pstGPRMI->m_u8SatelliteCount;
|
|
// 发送数据帧
|
ret = PythonLink_SendFrame(PYTHONLINK_TYPE_GPS,
|
(const HIDO_UINT8 *)&gpsPacket,
|
sizeof(gpsPacket));
|
|
if (ret == HIDO_OK)
|
{
|
g_u32GPSPacketCount++;
|
}
|
|
return ret;
|
}
|
|
/**
|
* @brief 发送IMU数据包
|
*/
|
HIDO_INT32 PythonLink_SendIMUData(const ST_GPIMU *_pstGPIMU)
|
{
|
ST_PythonLink_IMU imuPacket;
|
HIDO_INT32 ret;
|
|
// 检查参数
|
if (_pstGPIMU == NULL || _pstGPIMU->m_bValid == HIDO_FALSE)
|
{
|
return HIDO_ERR;
|
}
|
|
// 填充IMU数据包
|
memset(&imuPacket, 0, sizeof(imuPacket));
|
imuPacket.m_fAccelX = _pstGPIMU->m_fAccelX;
|
imuPacket.m_fAccelY = _pstGPIMU->m_fAccelY;
|
imuPacket.m_fAccelZ = _pstGPIMU->m_fAccelZ;
|
imuPacket.m_fGyroX = _pstGPIMU->m_fGyroX;
|
imuPacket.m_fGyroY = _pstGPIMU->m_fGyroY;
|
imuPacket.m_fGyroZ = _pstGPIMU->m_fGyroZ;
|
imuPacket.m_fTemperature = _pstGPIMU->m_fTemperature;
|
imuPacket.m_u32UTCTime = _pstGPIMU->m_u32UTCTime;
|
|
// 发送数据帧
|
ret = PythonLink_SendFrame(PYTHONLINK_TYPE_IMU,
|
(const HIDO_UINT8 *)&imuPacket,
|
sizeof(imuPacket));
|
|
if (ret == HIDO_OK)
|
{
|
g_u32IMUPacketCount++;
|
}
|
|
return ret;
|
}
|
|
/**
|
* @brief 获取统计信息
|
*/
|
HIDO_VOID PythonLink_GetStats(HIDO_UINT32 *_pu32GPSCount,
|
HIDO_UINT32 *_pu32IMUCount,
|
HIDO_UINT32 *_pu32ErrorCount)
|
{
|
if (_pu32GPSCount != NULL)
|
{
|
*_pu32GPSCount = g_u32GPSPacketCount;
|
}
|
if (_pu32IMUCount != NULL)
|
{
|
*_pu32IMUCount = g_u32IMUPacketCount;
|
}
|
if (_pu32ErrorCount != NULL)
|
{
|
*_pu32ErrorCount = g_u32ErrorCount;
|
}
|
}
|
|
/**
|
* @brief 打印调试信息
|
*/
|
HIDO_VOID PythonLink_PrintDebugInfo(HIDO_VOID)
|
{
|
HIDO_Debug2("\r\n========== PythonLink Debug Info ==========\r\n");
|
HIDO_Debug2("GPS packets sent: %u\r\n", g_u32GPSPacketCount);
|
HIDO_Debug2("IMU packets sent: %u\r\n", g_u32IMUPacketCount);
|
HIDO_Debug2("Control RX count: %u\r\n", g_u32ControlRxCount);
|
HIDO_Debug2("Error count: %u\r\n", g_u32ErrorCount);
|
HIDO_Debug2("RX total bytes: %u\r\n", g_stPythonLinkRxDebug.m_u32TotalBytes);
|
HIDO_Debug2("RX parsed frames: %u\r\n", g_stPythonLinkRxDebug.m_u32ParsedFrames);
|
HIDO_Debug2("RX last data age: %u ms\r\n",
|
HAL_GetTick() - g_stPythonLinkRxDebug.m_u32LastDataTick);
|
HIDO_Debug2("RX handle fails: %u\r\n", g_stPythonLinkRxDebug.m_u32HandleFailCount);
|
HIDO_Debug2("RX DMA null count: %u\r\n", g_stPythonLinkRxDebug.m_u32DmaNullCount);
|
HIDO_Debug2("Control valid: %s\r\n", g_bControlValid ? "YES" : "NO");
|
if (g_bControlValid)
|
{
|
HIDO_Debug2(" Steering PWM: %u us\r\n", g_u16LatestSteeringPWM);
|
HIDO_Debug2(" Throttle PWM: %u us\r\n", g_u16LatestThrottlePWM);
|
HIDO_Debug2(" Age: %u ms\r\n", HAL_GetTick() - g_u32ControlTimestamp);
|
}
|
HIDO_Debug2("===========================================\r\n\r\n");
|
}
|
|
HIDO_VOID PythonLink_PrintRxDebug(HIDO_VOID)
|
{
|
HIDO_Debug2("\r\n----- PythonLink RX Debug Dump -----\r\n");
|
HIDO_Debug2("Last poll tick: %u\r\n", g_stPythonLinkRxDebug.m_u32LastPollTick);
|
HIDO_Debug2("Last data tick: %u\r\n", g_stPythonLinkRxDebug.m_u32LastDataTick);
|
HIDO_Debug2("Total bytes: %u\r\n", g_stPythonLinkRxDebug.m_u32TotalBytes);
|
HIDO_Debug2("Parsed frames: %u\r\n", g_stPythonLinkRxDebug.m_u32ParsedFrames);
|
HIDO_Debug2("Handle fail count: %u\r\n", g_stPythonLinkRxDebug.m_u32HandleFailCount);
|
HIDO_Debug2("DMA null count: %u\r\n", g_stPythonLinkRxDebug.m_u32DmaNullCount);
|
HIDO_Debug2("Last log tick: %u\r\n", g_stPythonLinkRxDebug.m_u32LastLogTick);
|
HIDO_Debug2("---------------------------------------\r\n\r\n");
|
}
|
|
/**
|
* @brief 获取最新的控制命令
|
*/
|
HIDO_BOOL PythonLink_GetControl(HIDO_UINT16 *_pu16SteeringPWM,
|
HIDO_UINT16 *_pu16ThrottlePWM,
|
HIDO_UINT32 *_pu32Timestamp)
|
{
|
if (g_bControlValid == HIDO_FALSE)
|
{
|
return HIDO_FALSE;
|
}
|
|
if (_pu16SteeringPWM != NULL)
|
{
|
*_pu16SteeringPWM = g_u16LatestSteeringPWM;
|
}
|
|
if (_pu16ThrottlePWM != NULL)
|
{
|
*_pu16ThrottlePWM = g_u16LatestThrottlePWM;
|
}
|
|
if (_pu32Timestamp != NULL)
|
{
|
*_pu32Timestamp = g_u32ControlTimestamp;
|
}
|
|
return HIDO_TRUE;
|
}
|
