IM23数据解析成功，但是硬件仿真有问题，在解决。

yincheng.zhong

2025-11-29 dc713149918c980f23bc9405b8ffe7502d5f515e

 STM32H743/FML/GPS.c

@@ -6,40 +6,32 @@
#include "math.h"
#include "HIDO_Util.h"
#include "HIDO_Debug.h"
#include "HIDO_Timer.h"
#include "FreeRTOS.h"
#include "task.h"
#include "GPS.h"
#include "DBG.h"
#include "GPIO.h"
#include "Uart.h"
#include "UDPClient.h"
#include "PythonLink.h"
// #include "global_param.h"

#include "TTS.h"

#define GPS_DBG(level, fmt, ...) HIDO_Debug(fmt, __VA_ARGS__)

#define GPS_UART_RX_BUF_SIZE 1024
#define GPS_UART_TX_BUF_SIZE (2048 + 512)

typedef enum
{
    GPS_RECV_STATE_IDLE = 0,
    GPS_RECV_STATE_HEAD,
    GPS_RECV_STATE_CR,
    GPS_RECV_STATE_LF,
} E_GPSRecvState;
#define IM23A_HEADER_LEN                (4U)
#define IM23A_NAV_FRAME_LEN             (100U)   /* 包含头、负载、校验和及尾部 */
#define IM23A_IMU_FRAME_LEN             (52U)
#define IM23A_MAX_FRAME_LEN             IM23A_NAV_FRAME_LEN
#define IM23A_CHECKSUM_LEN              (2U)
#define IM23A_TAIL_LEN                  (2U)
#define IM23A_NAV_HEADER                "fmin"
#define IM23A_IMU_HEADER                "fmim"
#define IM23A_GPS_TIME_SCALE            (100.0)  /* hhmmss.ss -> centisecond */
#define IM23A_IMU_TIME_SCALE            (1000.0) /* hhmmss.ss -> millisecond */

typedef struct
{
    E_GPSRecvState m_eState;
    HIDO_CHAR m_acHeader[10], m_acHeader2[10], m_acHeader3[10], m_acHeader4[10], m_acHeader5[10];
    HIDO_UINT32 m_u32HeaderLen, m_u32Header2Len, m_u32Header3Len, m_u32Header4Len, m_u32Header5Len;

    HIDO_CHAR m_acRecvBuf[256];  // 增大缓冲区以容纳GPRMI的23个字段
    HIDO_UINT8 m_au8Buffer[IM23A_MAX_FRAME_LEN];
    HIDO_UINT32 m_u32RecvLen;
    HIDO_UINT32 m_u32ExpectedLen;
} ST_GPSRecv;

static HIDO_UINT8 l_au8GPSUartRxBuf[GPS_UART_RX_BUF_SIZE];
@@ -49,447 +41,288 @@
static ST_GPSRecv l_stGPSRecv;

static HIDO_UINT8 l_u8PosState = 0;
static HIDO_UINT8 l_u8GPS_HZ = 0;
static HIDO_BOOL l_bGPSConfig = HIDO_FALSE;
static HIDO_UINT32 l_u32QXTick = 0;
HIDO_UINT32 getRTK_Tick = 0;

/* 存储最新的GPRMI和GPIMU数据 */
static ST_GPRMI l_stGPRMI;
static ST_GPIMU l_stGPIMU;
static HIDO_UINT32 s_gprmi_log_idx = 0U;
/* GPS���� */
static HIDO_UINT8 l_au8CmdSave[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0x31, 0xBF};
static HIDO_UINT8 l_au8CmdRate1Hz[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00, 0x01, 0x39};
static HIDO_UINT8 l_au8CmdRate2Hz[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xF4, 0x01, 0x01, 0x00, 0x01, 0x00, 0x0B, 0x77};
static HIDO_UINT8 l_au8CmdRate5Hz[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A};
static HIDO_UINT8 l_au8CmdRate10Hz[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12};

/*******************************************************************************
 * Function Name     : GPS_RateConfig
 * Description       : GPS�������� ֧��1HZ��2HZ��5HZ��10HZ
 * Input             : _u8Rate ����
 * Output            : None
 * Return            : HIDO_OK �ɹ�, HIDO_ERR ʧ��
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��5��07��
 *                          IM23A Helper Declarations                          *
 *******************************************************************************/
static HIDO_INT32 GPS_RateConfig(HIDO_UINT8 _u8Rate)
static HIDO_VOID IM23A_ResetParser(ST_GPSRecv *parser);
static HIDO_BOOL IM23A_ValidateFrame(const HIDO_UINT8 *frame, HIDO_UINT32 len);
static HIDO_VOID IM23A_HandleFrame(const HIDO_UINT8 *frame, HIDO_UINT32 len);
static HIDO_VOID IM23A_HandleNavFrame(const HIDO_UINT8 *frame);
static HIDO_VOID IM23A_HandleImuFrame(const HIDO_UINT8 *frame);
static HIDO_DOUBLE IM23A_ReadDouble(const HIDO_UINT8 *ptr);
static HIDO_FLOAT IM23A_ReadFloat(const HIDO_UINT8 *ptr);
static HIDO_UINT32 IM23A_ReadU32(const HIDO_UINT8 *ptr);
static HIDO_UINT16 IM23A_ReadU16(const HIDO_UINT8 *ptr);
static HIDO_UINT32 IM23A_ConvertTime(double utc_val, double scale);

/* 旧版NMEA解析函数已移除，以下为IM23A专用解析工具函数 */

static HIDO_DOUBLE IM23A_ReadDouble(const HIDO_UINT8 *ptr)
{
    l_u8GPS_HZ = _u8Rate;

    switch (_u8Rate)
    {
    case 1:
    {
        Uart_Send(UART_ID_GPS, l_au8CmdRate1Hz, sizeof(l_au8CmdRate1Hz));
        Uart_Send(UART_ID_GPS, l_au8CmdSave, sizeof(l_au8CmdSave));
        break;
    }
    case 2:
    {
        Uart_Send(UART_ID_GPS, l_au8CmdRate2Hz, sizeof(l_au8CmdRate2Hz));
        Uart_Send(UART_ID_GPS, l_au8CmdSave, sizeof(l_au8CmdSave));
        break;
    }
    case 5:
    {
        Uart_Send(UART_ID_GPS, l_au8CmdRate5Hz, sizeof(l_au8CmdRate5Hz));
        Uart_Send(UART_ID_GPS, l_au8CmdSave, sizeof(l_au8CmdSave));
        break;
    }
    case 10:
    {
        Uart_Send(UART_ID_GPS, l_au8CmdRate10Hz, sizeof(l_au8CmdRate10Hz));
        Uart_Send(UART_ID_GPS, l_au8CmdSave, sizeof(l_au8CmdSave));
        break;
    }
    default:
    {
        break;
    }
    }

    return HIDO_OK;
    HIDO_DOUBLE val = 0.0;
    memcpy(&val, ptr, sizeof(val));
    return val;
}

/*******************************************************************************
 * Function Name     : GPS_DataCheck
 * Description       : GPS���ݸ�ʽ���
 * Input             : _pcData GPS����
 *                   : _u32Len GPS���ݳ���
 * Output            : None
 * Return            : HIDO_OK �ɹ�, HIDO_ERR ʧ��
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��5��07��
 *******************************************************************************/
static HIDO_INT32 GPS_DataCheck(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
static HIDO_FLOAT IM23A_ReadFloat(const HIDO_UINT8 *ptr)
{
    HIDO_DataStruct stData;
    HIDO_DataStruct stCheckValue;
    HIDO_UINT8 u8CheckValue = 0;
    HIDO_UINT8 u8CalcValue = 0;
    HIDO_UINT32 i = 0;

    if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len, "$%p*%p\r\n", &stData, &stCheckValue) != 2)
    {
        return HIDO_ERR;
    }

    u8CheckValue = HIDO_UtilHexStrBufToInt((HIDO_CHAR *)stCheckValue.m_pData, stCheckValue.m_u32Len);
    u8CalcValue = ((HIDO_UINT8 *)stData.m_pData)[0];
    for (i = 1; i < stData.m_u32Len; i++)
    {
        u8CalcValue ^= ((HIDO_UINT8 *)stData.m_pData)[i];
    }

    if (u8CalcValue != u8CheckValue)
    {
        return HIDO_ERR;
    }

    return HIDO_OK;
    HIDO_FLOAT val = 0.0f;
    memcpy(&val, ptr, sizeof(val));
    return val;
}

/*******************************************************************************
 * Function Name     : GPS_ParseGGA
 * Description       : GPS GGA���ݽ���(����������Ƿ���Ч)
 * Input             : _pcData GGA����
 *                   : _u32Len GGA���ݳ���
 * Output            : None
 * Return            : HIDO_OK �ɹ�, HIDO_ERR ʧ��
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��5��07��
 *******************************************************************************/
static HIDO_INT32 GPS_ParseGGA(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
static HIDO_UINT32 IM23A_ReadU32(const HIDO_UINT8 *ptr)
{
    ST_GPS stGPS;
    HIDO_DataStruct stPosState;

    memset(&stGPS, 0, sizeof(ST_GPS));
    if (GPS_DataCheck(_pcData, _u32Len) != HIDO_OK)
    {
        return HIDO_ERR;
    }

    if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len, "$%*,%*,%*,%*,%*,%*,%p,%*,%*,%*,%*,%*,%*,%*,%**", &stPosState) != 15)
    {
        return HIDO_ERR;
    }

    l_u8PosState = atoi((HIDO_CHAR *)stPosState.m_pData);

    return HIDO_OK;
    HIDO_UINT32 val = 0U;
    memcpy(&val, ptr, sizeof(val));
    return val;
}

/*******************************************************************************
 * Function Name     : GPS_ParseGPRMI
 * Description       : 解析GPRMI数据包
 * Input             : _pcData GPRMI数据
 *                   : _u32Len GPRMI数据长度
 * Output            : None
 * Return            : HIDO_OK 成功, HIDO_ERR 失败
 * Author            : www.hido-studio.com
 * Modified Date:    : 2025年11月11日
 *******************************************************************************/
static HIDO_INT32 GPS_ParseGPRMI(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
static HIDO_UINT16 IM23A_ReadU16(const HIDO_UINT8 *ptr)
{
    HIDO_DataStruct astFields[23];
    
    HIDO_UINT16 val = 0U;
    memcpy(&val, ptr, sizeof(val));
    return val;
}

static HIDO_UINT32 IM23A_ConvertTime(double utc_val, double scale)
{
    if (!isfinite(utc_val) || utc_val < 0.0)
    {
        return 0U;
    }
    double scaled = utc_val * scale;
    if (scaled < 0.0)
    {
        scaled = 0.0;
    }
    return (HIDO_UINT32)(scaled + 0.5);
}

static HIDO_VOID IM23A_ResetParser(ST_GPSRecv *parser)
{
    memset(parser->m_au8Buffer, 0, sizeof(parser->m_au8Buffer));
    parser->m_u32RecvLen = 0U;
    parser->m_u32ExpectedLen = 0U;
}

static HIDO_BOOL IM23A_ValidateFrame(const HIDO_UINT8 *frame, HIDO_UINT32 len)
{
    if (frame == HIDO_NULL)
    {
        return HIDO_FALSE;
    }

    if ((len != IM23A_NAV_FRAME_LEN) && (len != IM23A_IMU_FRAME_LEN))
    {
        return HIDO_FALSE;
    }

    if (frame[len - 2U] != 'e' || frame[len - 1U] != 'd')
    {
        return HIDO_FALSE;
    }

    const HIDO_UINT32 sum_len = len - IM23A_TAIL_LEN - IM23A_CHECKSUM_LEN;
    HIDO_UINT16 calc = 0U;
    for (HIDO_UINT32 i = 0U; i < sum_len; ++i)
    {
        calc = (HIDO_UINT16)(calc + frame[i]);
    }

    const HIDO_UINT16 expect = IM23A_ReadU16(&frame[sum_len]);
    return (calc == expect);
}

static HIDO_VOID IM23A_HandleFrame(const HIDO_UINT8 *frame, HIDO_UINT32 len)
{
    if (len == IM23A_NAV_FRAME_LEN && memcmp(frame, IM23A_NAV_HEADER, IM23A_HEADER_LEN) == 0)
    {
        IM23A_HandleNavFrame(frame);
    }
    else if (len == IM23A_IMU_FRAME_LEN && memcmp(frame, IM23A_IMU_HEADER, IM23A_HEADER_LEN) == 0)
    {
        IM23A_HandleImuFrame(frame);
    }
}

static HIDO_VOID IM23A_HandleNavFrame(const HIDO_UINT8 *frame)
{
    if (IM23A_ValidateFrame(frame, IM23A_NAV_FRAME_LEN) == HIDO_FALSE)
    {
        return;
    }

    const HIDO_UINT8 *p = frame + IM23A_HEADER_LEN;

    double utc = IM23A_ReadDouble(p); p += 8U;
    double latitude = IM23A_ReadDouble(p); p += 8U;
    double longitude = IM23A_ReadDouble(p); p += 8U;
    double altitude = IM23A_ReadDouble(p); p += 8U;
    float vel_n = IM23A_ReadFloat(p); p += 4U;
    float vel_e = IM23A_ReadFloat(p); p += 4U;
    float vel_d = IM23A_ReadFloat(p); p += 4U;
    float roll = IM23A_ReadFloat(p); p += 4U;
    float pitch = IM23A_ReadFloat(p); p += 4U;
    float heading = IM23A_ReadFloat(p); p += 4U;
    float pos_accuracy = IM23A_ReadFloat(p); p += 4U;
    float accel_bias_x = IM23A_ReadFloat(p); p += 4U;
    float accel_bias_y = IM23A_ReadFloat(p); p += 4U;
    float accel_bias_z = IM23A_ReadFloat(p); p += 4U;
    float gyro_bias_x = IM23A_ReadFloat(p); p += 4U;
    float gyro_bias_y = IM23A_ReadFloat(p); p += 4U;
    float gyro_bias_z = IM23A_ReadFloat(p); p += 4U;
    float sensor_temp = IM23A_ReadFloat(p); p += 4U;
    HIDO_UINT32 status = IM23A_ReadU32(p);

    memset(&l_stGPRMI, 0, sizeof(ST_GPRMI));
    l_stGPRMI.m_bValid = HIDO_FALSE;
    
    // 检查数据格式校验
    if (GPS_DataCheck(_pcData, _u32Len) != HIDO_OK)
    {
        return HIDO_ERR;
    }
    
    // 解析23个字段: $GPRMI,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>,<13>,<14>,<15>,<16>,<17>,<18>,<19>,<20>,<21>,<22>,<23>*<CR><LF>
    if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len, 
        "$%*,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p,%p*%**",
        &astFields[0], &astFields[1], &astFields[2], &astFields[3], &astFields[4],
        &astFields[5], &astFields[6], &astFields[7], &astFields[8], &astFields[9],
        &astFields[10], &astFields[11], &astFields[12], &astFields[13], &astFields[14],
        &astFields[15], &astFields[16], &astFields[17], &astFields[18], &astFields[19],
        &astFields[20], &astFields[21], &astFields[22]) != 25)
    {
        return HIDO_ERR;
    }
    
    // 解析各字段
    l_stGPRMI.m_u32UTCTime = (HIDO_UINT32)(atof((HIDO_CHAR *)astFields[0].m_pData) * 100);  // 转换为整数
    l_stGPRMI.m_u16WeekNumber = (HIDO_UINT16)atoi((HIDO_CHAR *)astFields[1].m_pData);
    l_stGPRMI.m_u32TimeOfWeek = (HIDO_UINT32)(atof((HIDO_CHAR *)astFields[2].m_pData) * 1000);  // 转换为毫秒
    l_stGPRMI.m_dLatitude = atof((HIDO_CHAR *)astFields[3].m_pData);
    l_stGPRMI.m_dLongitude = atof((HIDO_CHAR *)astFields[4].m_pData);
    l_stGPRMI.m_fAltitude = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[5].m_pData);
    l_stGPRMI.m_fLatStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[6].m_pData);
    l_stGPRMI.m_fLonStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[7].m_pData);
    l_stGPRMI.m_fAltStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[8].m_pData);
    l_stGPRMI.m_fEastVelocity = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[9].m_pData);
    l_stGPRMI.m_fNorthVelocity = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[10].m_pData);
    l_stGPRMI.m_fUpVelocity = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[11].m_pData);
    l_stGPRMI.m_fHorizontalVelStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[12].m_pData);
    l_stGPRMI.m_fHeadingAngle = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[13].m_pData);
    l_stGPRMI.m_fPitchAngle = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[14].m_pData);
    l_stGPRMI.m_fRollAngle = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[15].m_pData);
    l_stGPRMI.m_fHeadingAngleStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[16].m_pData);
    l_stGPRMI.m_fPitchAngleStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[17].m_pData);
    l_stGPRMI.m_fRollAngleStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[18].m_pData);
    l_stGPRMI.m_fBaselineDistance = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[19].m_pData);
    l_stGPRMI.m_u8SatelliteCount = (HIDO_UINT8)atoi((HIDO_CHAR *)astFields[20].m_pData);
    l_stGPRMI.m_u8FixedAmbiguityCount = (HIDO_UINT8)atoi((HIDO_CHAR *)astFields[21].m_pData);
    l_stGPRMI.m_u8PositionQuality = (HIDO_UINT8)atoi((HIDO_CHAR *)astFields[22].m_pData);
    
    l_stGPRMI.m_u32UTCTime = IM23A_ConvertTime(utc, IM23A_GPS_TIME_SCALE);
    l_stGPRMI.m_dLatitude = latitude;
    l_stGPRMI.m_dLongitude = longitude;
    l_stGPRMI.m_fAltitude = (HIDO_FLOAT)altitude;
    l_stGPRMI.m_fNorthVelocity = vel_n;
    l_stGPRMI.m_fEastVelocity = vel_e;
    l_stGPRMI.m_fUpVelocity = (HIDO_FLOAT)(-vel_d);
    l_stGPRMI.m_fRollAngle = roll;
    l_stGPRMI.m_fPitchAngle = pitch;
    l_stGPRMI.m_fHeadingAngle = heading;
    l_stGPRMI.m_fHorizontalVelStdDev = pos_accuracy;
    l_stGPRMI.m_fAccelBiasX = accel_bias_x;
    l_stGPRMI.m_fAccelBiasY = accel_bias_y;
    l_stGPRMI.m_fAccelBiasZ = accel_bias_z;
    l_stGPRMI.m_fGyroBiasX = gyro_bias_x;
    l_stGPRMI.m_fGyroBiasY = gyro_bias_y;
    l_stGPRMI.m_fGyroBiasZ = gyro_bias_z;
    l_stGPRMI.m_fImuTemperature = sensor_temp;
    l_stGPRMI.m_u32StatusFlags = status;
    l_stGPRMI.m_u8PositionQuality = (HIDO_UINT8)(status & 0xFFU);
    l_stGPRMI.m_bValid = HIDO_TRUE;
    
    // 立即发送GPS数据到Python (10Hz更新)
    PythonLink_SendGPSData(&l_stGPRMI);
    
    return HIDO_OK;

    l_u8PosState = l_stGPRMI.m_u8PositionQuality;
}

/*******************************************************************************
 * Function Name     : GPS_ParseGPIMU
 * Description       : 解析GPIMU数据包
 * Input             : _pcData GPIMU数据
 *                   : _u32Len GPIMU数据长度
 * Output            : None
 * Return            : HIDO_OK 成功, HIDO_ERR 失败
 * Author            : www.hido-studio.com
 * Modified Date:    : 2025年11月11日
 *******************************************************************************/
static HIDO_INT32 GPS_ParseGPIMU(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
static HIDO_VOID IM23A_HandleImuFrame(const HIDO_UINT8 *frame)
{
    HIDO_DataStruct astFields[8];  // 8个数据字段
    HIDO_UINT8 u8CalcChecksum = 0;
    HIDO_UINT32 i = 0;
    HIDO_CHAR *pCheckStart = HIDO_NULL;
    HIDO_CHAR *pCheckEnd = HIDO_NULL;
    
    if (IM23A_ValidateFrame(frame, IM23A_IMU_FRAME_LEN) == HIDO_FALSE)
    {
        return;
    }

    const HIDO_UINT8 *p = frame + IM23A_HEADER_LEN;

    double utc = IM23A_ReadDouble(p); p += 8U;
    float accel_x = IM23A_ReadFloat(p); p += 4U;
    float accel_y = IM23A_ReadFloat(p); p += 4U;
    float accel_z = IM23A_ReadFloat(p); p += 4U;
    float gyro_x = IM23A_ReadFloat(p); p += 4U;
    float gyro_y = IM23A_ReadFloat(p); p += 4U;
    float gyro_z = IM23A_ReadFloat(p); p += 4U;
    (void)IM23A_ReadFloat(p); p += 4U;
    (void)IM23A_ReadFloat(p); p += 4U;
    (void)IM23A_ReadFloat(p);

    memset(&l_stGPIMU, 0, sizeof(ST_GPIMU));
    l_stGPIMU.m_bValid = HIDO_FALSE;
    
    // 解析8个字段: $GPIMU,<时间>,<AccX>,<AccY>,<AccZ>,<GyroX>,<GyroY>,<GyroZ>,<温度>*<校验和>
    // 实际数据示例: $GPIMU,102728.808,-0.010,-0.281,-1.000,0.092,-0.214,-0.031,29.00*5B
    if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len,
        "$GPIMU,%p,%p,%p,%p,%p,%p,%p,%p*%**",
        &astFields[0], &astFields[1], &astFields[2], &astFields[3],
        &astFields[4], &astFields[5], &astFields[6], &astFields[7]) < 8)
    {
        return HIDO_ERR;
    }
    
    // 计算异或校验: 从第一个字段到第9个字段的异或和
    // 示例数据: $GPIMU, 054752.002, 0.000, 0.007, -1.032, -0.003, 0.053, -0.016,26.00@59
    // 格式说明: 语句示例中的异或校验和为0@59 (十六进制59)
    pCheckStart = strchr(_pcData, ',');  // 找到第一个逗号
    pCheckEnd = strrchr(_pcData, '*');    // 找到最后一个星号
    
    if (pCheckStart != HIDO_NULL && pCheckEnd != HIDO_NULL && pCheckEnd > pCheckStart)
    {
        // 计算从第一个逗号后到星号前的所有字符的异或
        for (i = 0; pCheckStart + i < pCheckEnd; i++)
        {
            u8CalcChecksum ^= (HIDO_UINT8)(pCheckStart[i]);
        }
    }
    
    // 解析各字段: 时间、3轴加速度、3轴角速度、温度
    l_stGPIMU.m_u32UTCTime = (HIDO_UINT32)(atof((HIDO_CHAR *)astFields[0].m_pData) * 1000);  // 转换为毫秒
    l_stGPIMU.m_fAccelX = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[1].m_pData);
    l_stGPIMU.m_fAccelY = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[2].m_pData);
    l_stGPIMU.m_fAccelZ = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[3].m_pData);
    l_stGPIMU.m_fGyroX = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[4].m_pData);
    l_stGPIMU.m_fGyroY = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[5].m_pData);
    l_stGPIMU.m_fGyroZ = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[6].m_pData);
    l_stGPIMU.m_fTemperature = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[7].m_pData);
    
    // 校验和已由HIDO_UtilParseFormat处理（格式字符串中的*%**）
    l_stGPIMU.m_u8Checksum = 0;  // 不需要单独存储
    
    // 验证校验和 (根据文档，这是异或校验)
    // 注意: 文档中显示校验值在最后，需要根据实际协议确定是否验证
    
    l_stGPIMU.m_u32UTCTime = IM23A_ConvertTime(utc, IM23A_IMU_TIME_SCALE);
    l_stGPIMU.m_fAccelX = accel_x;
    l_stGPIMU.m_fAccelY = accel_y;
    l_stGPIMU.m_fAccelZ = accel_z;
    l_stGPIMU.m_fGyroX = gyro_x;
    l_stGPIMU.m_fGyroY = gyro_y;
    l_stGPIMU.m_fGyroZ = gyro_z;
    l_stGPIMU.m_fTemperature = l_stGPRMI.m_fImuTemperature;
    l_stGPIMU.m_bValid = HIDO_TRUE;
    
    // 立即发送IMU数据到Python (100Hz更新)
    PythonLink_SendIMUData(&l_stGPIMU);
    
    return HIDO_OK;
}
u16 g_spsum, g_snum;
static HIDO_INT32 GPS_ParseGSV(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
{
    ST_GPS stGPS;
    HIDO_DataStruct spower[4];

    memset(&stGPS, 0, sizeof(ST_GPS));
    //    if (GPS_DataCheck(_pcData, _u32Len) != HIDO_OK)
    //    {
    //        return HIDO_ERR;
    //    }

    if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len, "$%*,%*,%*,%*,%*,%*,%*,%p,%*,%*,%*,%p,%*,%*,%*,%p,%*,%*,%*,%p,%**", &spower[0], &spower[1], &spower[2], &spower[3]) == 21)
    {
        g_snum += 4;
        g_spsum += atoi((HIDO_CHAR *)spower[0].m_pData) + atoi((HIDO_CHAR *)spower[1].m_pData) + atoi((HIDO_CHAR *)spower[2].m_pData) + atoi((HIDO_CHAR *)spower[3].m_pData);
    }
    else if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len, "$%*,%*,%*,%*,%*,%*,%*,%p,%*,%*,%*,%p,%*,%*,%*,%p,%**", &spower[0], &spower[1], &spower[2]) == 17)
    {
        g_snum += 3;
        g_spsum += atoi((HIDO_CHAR *)spower[0].m_pData) + atoi((HIDO_CHAR *)spower[1].m_pData) + atoi((HIDO_CHAR *)spower[2].m_pData);
    }
    else if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len, "$%*,%*,%*,%*,%*,%*,%*,%p,%*,%*,%*,%p,%**", &spower[0], &spower[1]) == 13)
    {
        g_snum += 2;
        g_spsum += atoi((HIDO_CHAR *)spower[0].m_pData) + atoi((HIDO_CHAR *)spower[1].m_pData);
    }
    else if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len, "$%*,%*,%*,%*,%*,%*,%*,%p,%**", &spower[0]) == 9)
    {
        g_snum += 1;
        g_spsum += atoi((HIDO_CHAR *)spower[0].m_pData);
    }
    // l_u8PosState = atoi((HIDO_CHAR *)stPosState.m_pData);

    return HIDO_OK;
}
/*******************************************************************************
 * Function Name     : GPS_RecvFsm
 * Description       : GPS ���ݽ���״̬��
 * Input             : _u8RecvChar һ�������ַ�
 * Output            : None
 * Return            : one
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��5��07��
 *******************************************************************************/
static HIDO_VOID GPS_RecvFsm(HIDO_UINT8 _u8RecvChar)
{
    static int LastRTK = 0;
    ST_GPSRecv *parser = &l_stGPSRecv;

    HIDO_CHAR GPS_1[10] = "�豸����";
    HIDO_CHAR GPS_4[10] = "�豸�̶�";
    HIDO_CHAR GPS_5[10] = "�豸����";

    switch (l_stGPSRecv.m_eState)
    if (parser->m_u32ExpectedLen == 0U)
    {
    case GPS_RECV_STATE_IDLE:
    {
        if ('$' == _u8RecvChar)
        if (parser->m_u32RecvLen == 0U)
        {
            l_stGPSRecv.m_eState = GPS_RECV_STATE_HEAD;
            l_stGPSRecv.m_u32RecvLen = 0;
            l_stGPSRecv.m_acRecvBuf[l_stGPSRecv.m_u32RecvLen++] = _u8RecvChar;
            if (_u8RecvChar == 'f')
            {
                parser->m_au8Buffer[parser->m_u32RecvLen++] = _u8RecvChar;
            }
            return;
        }
        break;
    }
    case GPS_RECV_STATE_HEAD:
    {
        l_stGPSRecv.m_acRecvBuf[l_stGPSRecv.m_u32RecvLen++] = _u8RecvChar;
        if (l_stGPSRecv.m_u32RecvLen >= l_stGPSRecv.m_u32HeaderLen)

        parser->m_au8Buffer[parser->m_u32RecvLen++] = _u8RecvChar;

        if (parser->m_u32RecvLen == 2U && _u8RecvChar != 'm')
        {
            if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader, l_stGPSRecv.m_u32HeaderLen) == 0)
            if (_u8RecvChar == 'f')
            {
                l_stGPSRecv.m_eState = GPS_RECV_STATE_CR;
            }
            else if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader2, l_stGPSRecv.m_u32Header2Len) == 0)
            {
                l_stGPSRecv.m_eState = GPS_RECV_STATE_CR;
            }
            else if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader3, l_stGPSRecv.m_u32Header3Len) == 0)
            {
                l_stGPSRecv.m_eState = GPS_RECV_STATE_CR;
            }
            else if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader4, l_stGPSRecv.m_u32Header4Len) == 0)
            {
                l_stGPSRecv.m_eState = GPS_RECV_STATE_CR;
            }
            else if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader5, l_stGPSRecv.m_u32Header5Len) == 0)
            {
                l_stGPSRecv.m_eState = GPS_RECV_STATE_CR;
                parser->m_au8Buffer[0] = 'f';
                parser->m_u32RecvLen = 1U;
            }
            else
            {
                l_stGPSRecv.m_eState = GPS_RECV_STATE_IDLE;
                IM23A_ResetParser(parser);
            }
            return;
        }

        break;
    }
    case GPS_RECV_STATE_CR:
    {
        l_stGPSRecv.m_acRecvBuf[l_stGPSRecv.m_u32RecvLen++] = _u8RecvChar;

        if (l_stGPSRecv.m_u32RecvLen >= (sizeof(l_stGPSRecv.m_acRecvBuf) - 1))
        if (parser->m_u32RecvLen == 3U && _u8RecvChar != 'i')
        {
            l_stGPSRecv.m_eState = GPS_RECV_STATE_IDLE;
            break;
            if (_u8RecvChar == 'f')
            {
                parser->m_au8Buffer[0] = 'f';
                parser->m_u32RecvLen = 1U;
            }
            else
            {
                IM23A_ResetParser(parser);
            }
            return;
        }

        if ('\r' == _u8RecvChar)
        if (parser->m_u32RecvLen == IM23A_HEADER_LEN)
        {
            l_stGPSRecv.m_eState = GPS_RECV_STATE_LF;
        }

        break;
    }
    case GPS_RECV_STATE_LF:
    {
        static u8 gpsled_state = 0;
        l_stGPSRecv.m_acRecvBuf[l_stGPSRecv.m_u32RecvLen++] = _u8RecvChar;
        if ('\n' == _u8RecvChar)
        {

            /* ����GPS HZ */
            //                g_com_map[GPS_HZ] = 10;
            //if (l_bGPSConfig || l_u8GPS_HZ != g_com_map[GPS_HZ])
            if (memcmp(parser->m_au8Buffer, IM23A_NAV_HEADER, IM23A_HEADER_LEN) == 0)
            {
                l_bGPSConfig = HIDO_FALSE;
                //GPS_RateConfig(g_com_map[GPS_HZ]);
                parser->m_u32ExpectedLen = IM23A_NAV_FRAME_LEN;
            }

            if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader3, l_stGPSRecv.m_u32Header3Len) == 0)
            else if (memcmp(parser->m_au8Buffer, IM23A_IMU_HEADER, IM23A_HEADER_LEN) == 0)
            {
                GPS_ParseGSV(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_u32RecvLen);
                parser->m_u32ExpectedLen = IM23A_IMU_FRAME_LEN;
            }
            else if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader4, l_stGPSRecv.m_u32Header4Len) == 0)
            else
            {
                // 解析GPRMI数据
                GPS_ParseGPRMI(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_u32RecvLen);
            }
            else if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader5, l_stGPSRecv.m_u32Header5Len) == 0)
            {
                // 解析GPIMU数据
                GPS_ParseGPIMU(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_u32RecvLen);
            }
            
            if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader, l_stGPSRecv.m_u32HeaderLen) == 0)
            {
                static uint32_t gps_uploadtimer = 0;
                GPS_ParseGGA(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_u32RecvLen);
                l_stGPSRecv.m_acRecvBuf[l_stGPSRecv.m_u32RecvLen - 2] = '\0';
                //if (HIDO_TimerGetTick() - gps_uploadtimer > g_com_map[GPSUPLOADTIME_INDEX] * 1000)
                HIDO_UINT8 restart = parser->m_au8Buffer[IM23A_HEADER_LEN - 1U];
                IM23A_ResetParser(parser);
                if (restart == 'f')
                {
                    gps_uploadtimer = HIDO_TimerGetTick();
                    UDPClient_UploadGPS(l_stGPSRecv.m_acRecvBuf);
                    parser->m_au8Buffer[0] = 'f';
                    parser->m_u32RecvLen = 1U;
                }
            }
            if ((HIDO_TimerGetTick() - l_u32QXTick) >= 1000)
            {
                l_u32QXTick = HIDO_TimerGetTick();
#ifdef __USE_QXWZ__
                qxwz_app_upload_gga(l_stGPSRecv.m_acRecvBuf);
#endif
            }
        }
        return;
    }

        l_stGPSRecv.m_eState = GPS_RECV_STATE_IDLE;
        break;
    }
    default:
    if (parser->m_u32RecvLen >= IM23A_MAX_FRAME_LEN)
    {
        break;
        IM23A_ResetParser(parser);
        return;
    }

    parser->m_au8Buffer[parser->m_u32RecvLen++] = _u8RecvChar;

    if (parser->m_u32RecvLen == parser->m_u32ExpectedLen)
    {
        IM23A_HandleFrame(parser->m_au8Buffer, parser->m_u32ExpectedLen);
        IM23A_ResetParser(parser);
    }
}

@@ -576,61 +409,15 @@

/*******************************************************************************
 * Function Name     : GPS_Poll
 * Description       : GPS��ѯ����
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��1��8��
 * Description       : GPS轮询，读取UART数据并送入IM23A解析器
 *******************************************************************************/
HIDO_VOID GPS_Poll(void)
{
    static HIDO_UINT8 l_u8GPSBuff[512];
    static HIDO_UINT32 l_u8GPSLen = 0;
    static HIDO_UINT32 l_u8GPSRecvTick = 0;
    HIDO_UINT8 u8RecvChar = 0U;

    volatile HIDO_INT32 i32Result = HIDO_OK;
    HIDO_UINT8 u8RecvChar = 0;
    UART_HandleTypeDef *pstUartHandle = HIDO_NULL;

    Uart_GetHandle(UART_ID_DBG, (HIDO_VOID **)&pstUartHandle);

    while ((i32Result = Uart_GetChar(UART_ID_GPS, &u8RecvChar)) == HIDO_OK)
    while (Uart_GetChar(UART_ID_GPS, &u8RecvChar) == HIDO_OK)
    {
#if 0
        if (DBG_GetMode() == DBG_MODE_GPS || DBG_GetMode() == DBG_MODE_CFG)
        {
            HAL_UART_Transmit(pstUartHandle, &u8RecvChar, 1, 1000);
        }
#endif
        GPS_RecvFsm(u8RecvChar);
        l_u8GPSBuff[l_u8GPSLen++] = u8RecvChar;
        if (l_u8GPSLen >= sizeof(l_u8GPSBuff))
        {
#ifdef __USE_TCP_RTCM__
            RTKClient_ReportData(l_u8GPSBuff, l_u8GPSLen);
#endif
#ifdef __USE_NTRIP__
            NTRIPApp_ReportGGA(l_u8GPSBuff, l_u8GPSLen);
#endif
            l_u8GPSLen = 0;
        }

        l_u8GPSRecvTick = HIDO_TimerGetTick();
    }

    if (l_u8GPSLen > 0)
    {
        if ((HIDO_TimerGetTick() - l_u8GPSRecvTick) > 50)
        {
#ifdef __USE_TCP_RTCM__
            RTKClient_ReportData(l_u8GPSBuff, l_u8GPSLen);
#endif
#ifdef __USE_NTRIP__
            NTRIPApp_ReportGGA(l_u8GPSBuff, l_u8GPSLen);
#endif
            l_u8GPSLen = 0;
        }
    }
}

@@ -645,7 +432,6 @@
 *******************************************************************************/
HIDO_VOID GPS_Init(void)
{
    uint32_t gpsbaudrate;
    GPS_PowerOn();
    GPS_Rest();

@@ -660,19 +446,8 @@
    stInit.m_u32TxQueueMemberCnt = 2;
    Uart_Init(UART_ID_GPS, &stInit);

    HIDO_UtilBzero(&l_stGPSRecv, sizeof(ST_GPSRecv));
    l_stGPSRecv.m_u32HeaderLen = HIDO_UtilSnprintf(l_stGPSRecv.m_acHeader, sizeof(l_stGPSRecv.m_acHeader), "$GNGGA");
    l_stGPSRecv.m_u32Header2Len = HIDO_UtilSnprintf(l_stGPSRecv.m_acHeader2, sizeof(l_stGPSRecv.m_acHeader2), "$GPGSV");
    l_stGPSRecv.m_u32Header3Len = HIDO_UtilSnprintf(l_stGPSRecv.m_acHeader3, sizeof(l_stGPSRecv.m_acHeader3), "$GBGSV");
    l_stGPSRecv.m_u32Header4Len = HIDO_UtilSnprintf(l_stGPSRecv.m_acHeader4, sizeof(l_stGPSRecv.m_acHeader4), "$GPFMI");
    l_stGPSRecv.m_u32Header5Len = HIDO_UtilSnprintf(l_stGPSRecv.m_acHeader5, sizeof(l_stGPSRecv.m_acHeader5), "$GPIMU");
    l_bGPSConfig = HIDO_TRUE;
    //gpsbaudrate = (g_com_map[GPSBAUDRATE1_INDEX] << 8) | g_com_map[GPSBAUDRATE2_INDEX];
    if (gpsbaudrate > 921600 || gpsbaudrate < 9600)
    {
        gpsbaudrate = 115200;
    }
    Uart_ReConfigBaudRate(UART_ID_GPS, gpsbaudrate);
    IM23A_ResetParser(&l_stGPSRecv);
    Uart_ReConfigBaudRate(UART_ID_GPS, 115200);
}

/*******************************************************************************