添加新的文件

yincheng.zhong

3 天以前 30303d366d1a0d857357c90bed876686f2d1e603

 STM32H743/FML/GPS.c

@@ -14,6 +14,7 @@
#include "GPIO.h"
#include "Uart.h"
#include "UDPClient.h"
#include "PythonLink.h"
// #include "global_param.h"

#include "TTS.h"
@@ -222,25 +223,28 @@
    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_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_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_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_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_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_u8ReservedFlags = (HIDO_UINT8)atoi((HIDO_CHAR *)astFields[21].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_bValid = HIDO_TRUE;
    
    // 立即发送GPS数据到Python (10Hz更新)
    PythonLink_SendGPSData(&l_stGPRMI);
    
    return HIDO_OK;
}
@@ -257,7 +261,7 @@
 *******************************************************************************/
static HIDO_INT32 GPS_ParseGPIMU(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
{
    HIDO_DataStruct astFields[9];
    HIDO_DataStruct astFields[8];  // 8个数据字段
    HIDO_UINT8 u8CalcChecksum = 0;
    HIDO_UINT32 i = 0;
    HIDO_CHAR *pCheckStart = HIDO_NULL;
@@ -266,12 +270,12 @@
    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>
    // 解析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,%p*%**",
        "$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],
        &astFields[8]) < 10)
        &astFields[4], &astFields[5], &astFields[6], &astFields[7]) < 8)
    {
        return HIDO_ERR;
    }
@@ -291,7 +295,7 @@
        }
    }
    
    // 解析各字段
    // 解析各字段: 时间、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);
@@ -301,14 +305,17 @@
    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);
    // 校验和已由HIDO_UtilParseFormat处理（格式字符串中的*%**）
    l_stGPIMU.m_u8Checksum = 0;  // 不需要单独存储
    
    // 验证校验和 (根据文档，这是异或校验)
    // 注意: 文档中显示校验值在最后，需要根据实际协议确定是否验证
    
    l_stGPIMU.m_bValid = HIDO_TRUE;
    
    // 立即发送IMU数据到Python (100Hz更新)
    PythonLink_SendIMUData(&l_stGPIMU);
    
    return HIDO_OK;
}
u16 g_spsum, g_snum;