1

WXK

9 天以前 aa14c757b47b2ef43ec8a6ced81fefe1e6594d2d

 STM32H743/FML/GPS.c

@@ -34,10 +34,10 @@
typedef struct
{
    E_GPSRecvState m_eState;
    HIDO_CHAR m_acHeader[10], m_acHeader2[10], m_acHeader3[10];
    HIDO_UINT32 m_u32HeaderLen, m_u32Header2Len, m_u32Header3Len;
    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[128];
    HIDO_CHAR m_acRecvBuf[256];  // 增大缓冲区以容纳GPRMI的23个字段
    HIDO_UINT32 m_u32RecvLen;
} ST_GPSRecv;

@@ -52,7 +52,11 @@
static HIDO_BOOL l_bGPSConfig = HIDO_FALSE;
static HIDO_UINT32 l_u32QXTick = 0;
HIDO_UINT32 getRTK_Tick = 0;
/* GPS命令 */

/* 存储最新的GPRMI和GPIMU数据 */
static ST_GPRMI l_stGPRMI;
static ST_GPIMU l_stGPIMU;
/* 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};
@@ -61,12 +65,12 @@

/*******************************************************************************
 * Function Name     : GPS_RateConfig
 * Description       : GPS速率配置 支持1HZ、2HZ、5HZ、10HZ
 * Input             : _u8Rate 速率
 * Description       : GPS�������� ֧��1HZ��2HZ��5HZ��10HZ
 * Input             : _u8Rate ����
 * Output            : None
 * Return            : HIDO_OK 成功, HIDO_ERR 失败
 * Return            : HIDO_OK �ɹ�, HIDO_ERR ʧ��
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年5月07日
 * Modified Date:    : 2021��5��07��
 *******************************************************************************/
static HIDO_INT32 GPS_RateConfig(HIDO_UINT8 _u8Rate)
{
@@ -109,13 +113,13 @@

/*******************************************************************************
 * Function Name     : GPS_DataCheck
 * Description       : GPS数据格式检查
 * Input             : _pcData GPS数据
 *                   : _u32Len GPS数据长度
 * Description       : GPS���ݸ�ʽ���
 * Input             : _pcData GPS����
 *                   : _u32Len GPS���ݳ���
 * Output            : None
 * Return            : HIDO_OK 成功, HIDO_ERR 失败
 * Return            : HIDO_OK �ɹ�, HIDO_ERR ʧ��
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年5月07日
 * Modified Date:    : 2021��5��07��
 *******************************************************************************/
static HIDO_INT32 GPS_DataCheck(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
{
@@ -147,13 +151,13 @@

/*******************************************************************************
 * Function Name     : GPS_ParseGGA
 * Description       : GPS GGA数据解析(仅检查数据是否有效)
 * Input             : _pcData GGA数据
 *                   : _u32Len GGA数据长度
 * Description       : GPS GGA���ݽ���(����������Ƿ���Ч)
 * Input             : _pcData GGA����
 *                   : _u32Len GGA���ݳ���
 * Output            : None
 * Return            : HIDO_OK 成功, HIDO_ERR 失败
 * Return            : HIDO_OK �ɹ�, HIDO_ERR ʧ��
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年5月07日
 * Modified Date:    : 2021��5��07��
 *******************************************************************************/
static HIDO_INT32 GPS_ParseGGA(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
{
@@ -173,6 +177,138 @@

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

    return HIDO_OK;
}

/*******************************************************************************
 * 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)
{
    HIDO_DataStruct astFields[23];
    
    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_fEastStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[6].m_pData);
    l_stGPRMI.m_fNorthStdDev = (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_fEastVelStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[12].m_pData);
    l_stGPRMI.m_fNorthVelStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[13].m_pData);
    l_stGPRMI.m_fUpVelStdDev = (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_fPitchAngleStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[16].m_pData);
    l_stGPRMI.m_fRollAngleStdDev = (HIDO_FLOAT)atof((HIDO_CHAR *)astFields[17].m_pData);
    l_stGPRMI.m_fHeadingAngleStdDev = (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_u8ReservedFlags = (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_bValid = HIDO_TRUE;
    
    return HIDO_OK;
}

/*******************************************************************************
 * 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)
{
    HIDO_DataStruct astFields[9];
    HIDO_UINT8 u8CalcChecksum = 0;
    HIDO_UINT32 i = 0;
    HIDO_CHAR *pCheckStart = HIDO_NULL;
    HIDO_CHAR *pCheckEnd = HIDO_NULL;
    
    memset(&l_stGPIMU, 0, sizeof(ST_GPIMU));
    l_stGPIMU.m_bValid = HIDO_FALSE;
    
    // 解析9个字段: $GPIMU,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>*<CR><LF>
    if (HIDO_UtilParseFormat((HIDO_UINT8 *)_pcData, _u32Len,
        "$GPIMU,%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]) < 10)
    {
        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]);
        }
    }
    
    // 解析各字段
    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);
    
    // 第9个字段是异或校验，直接解析
    l_stGPIMU.m_u8Checksum = (HIDO_UINT8)atoi((HIDO_CHAR *)astFields[8].m_pData);
    
    // 验证校验和 (根据文档，这是异或校验)
    // 注意: 文档中显示校验值在最后，需要根据实际协议确定是否验证
    
    l_stGPIMU.m_bValid = HIDO_TRUE;
    
    return HIDO_OK;
}
u16 g_spsum, g_snum;
@@ -213,20 +349,20 @@
}
/*******************************************************************************
 * Function Name     : GPS_RecvFsm
 * Description       : GPS 数据接收状态机
 * Input             : _u8RecvChar 一个接收字符
 * Description       : GPS ���ݽ���״̬��
 * Input             : _u8RecvChar һ�������ַ�
 * Output            : None
 * Return            : one
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年5月07日
 * Modified Date:    : 2021��5��07��
 *******************************************************************************/
static HIDO_VOID GPS_RecvFsm(HIDO_UINT8 _u8RecvChar)
{
    static int LastRTK = 0;

    HIDO_CHAR GPS_1[10] = "设备单点";
    HIDO_CHAR GPS_4[10] = "设备固定";
    HIDO_CHAR GPS_5[10] = "设备浮动";
    HIDO_CHAR GPS_1[10] = "�豸����";
    HIDO_CHAR GPS_4[10] = "�豸�̶�";
    HIDO_CHAR GPS_5[10] = "�豸����";

    switch (l_stGPSRecv.m_eState)
    {
@@ -254,6 +390,14 @@
                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;
            }
@@ -289,7 +433,7 @@
        if ('\n' == _u8RecvChar)
        {

            /* 配置GPS HZ */
            /* ����GPS HZ */
            //                g_com_map[GPS_HZ] = 10;
            //if (l_bGPSConfig || l_u8GPS_HZ != g_com_map[GPS_HZ])
            {
@@ -301,6 +445,17 @@
            {
                GPS_ParseGSV(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_u32RecvLen);
            }
            else if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader4, l_stGPSRecv.m_u32Header4Len) == 0)
            {
                // 解析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;
@@ -333,12 +488,12 @@

/*******************************************************************************
 * Function Name     : GPS_Rest
 * Description       : GPS复位
 * Description       : GPS��λ
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年1月8日
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
static HIDO_VOID GPS_Rest(void)
{
@@ -351,12 +506,12 @@

/*******************************************************************************
 * Function Name     : GPS_PowerOn
 * Description       : GPS上电
 * Description       : GPS�ϵ�
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年1月8日
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
static HIDO_VOID GPS_PowerOn(void)
{
@@ -365,12 +520,12 @@

/*******************************************************************************
 * Function Name     : GPS_PowerOff
 * Description       : GPS掉电
 * Description       : GPS����
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年1月8日
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_VOID GPS_PowerOff(void)
{
@@ -383,12 +538,12 @@

/*******************************************************************************
 * Function Name     : GPS_GetState
 * Description       : 获取GPS定位状态
 * Description       : ��ȡGPS��λ״̬
 * Input             : None
 * Output            : None
 * Return            : 定位状态 0 1 2 3
 * Return            : ��λ״̬ 0 1 2 3
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年1月8日
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_UINT8 GPS_GetState(HIDO_VOID)
{
@@ -397,14 +552,14 @@

/*******************************************************************************
 * Function Name     : GPS_PinRegister
 * Description       : GPS模块管脚注册
 * Input             : _ePin 管脚定义
 * Description       : GPSģ��ܽ�ע��
 * Input             : _ePin �ܽŶ���
 *                   : _pstGPIOx GPIOx
 *                   : _u16GPIOPin GPIO_PIN_x
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年1月8日
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_VOID GPS_PinRegister(E_GPSPin _ePin, GPIO_TypeDef *_pstGPIOx, HIDO_UINT16 _u16GPIOPin)
{
@@ -414,12 +569,12 @@

/*******************************************************************************
 * Function Name     : GPS_Poll
 * Description       : GPS轮询处理
 * Description       : GPS��ѯ����
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年1月8日
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_VOID GPS_Poll(void)
{
@@ -474,12 +629,12 @@

/*******************************************************************************
 * Function Name     : GPS_Init
 * Description       : GPS模块初始化
 * Description       : GPSģ���ʼ��
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021年1月8日
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_VOID GPS_Init(void)
{
@@ -502,6 +657,8 @@
    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), "$GPRMI");
    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)
@@ -510,3 +667,56 @@
    }
    Uart_ReConfigBaudRate(UART_ID_GPS, gpsbaudrate);
}

/*******************************************************************************
 * Function Name     : GPS_GetGPRMI
 * Description       : 获取最新的GPRMI数据
 * Input             : _pstGPRMI - 存储GPRMI数据的结构体指针
 * Output            : None
 * Return            : HIDO_OK - 成功, HIDO_ERR - 失败或数据无效
 * Author            : www.hido-studio.com
 * Modified Date:    : 2025年11月11日
 *******************************************************************************/
HIDO_INT32 GPS_GetGPRMI(ST_GPRMI *_pstGPRMI)
{
    if (_pstGPRMI == HIDO_NULL)
    {
        return HIDO_ERR;
    }
    
    if (l_stGPRMI.m_bValid == HIDO_FALSE)
    {
        return HIDO_ERR;
    }
    
    memcpy(_pstGPRMI, &l_stGPRMI, sizeof(ST_GPRMI));
    
    return HIDO_OK;
}

/*******************************************************************************
 * Function Name     : GPS_GetGPIMU
 * Description       : 获取最新的GPIMU数据
 * Input             : _pstGPIMU - 存储GPIMU数据的结构体指针
 * Output            : None
 * Return            : HIDO_OK - 成功, HIDO_ERR - 失败或数据无效
 * Author            : www.hido-studio.com
 * Modified Date:    : 2025年11月11日
 *******************************************************************************/
HIDO_INT32 GPS_GetGPIMU(ST_GPIMU *_pstGPIMU)
{
    if (_pstGPIMU == HIDO_NULL)
    {
        return HIDO_ERR;
    }
    
    if (l_stGPIMU.m_bValid == HIDO_FALSE)
    {
        return HIDO_ERR;
    }
    
    memcpy(_pstGPIMU, &l_stGPIMU, sizeof(ST_GPIMU));
    
    return HIDO_OK;
}