From 30303d366d1a0d857357c90bed876686f2d1e603 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期二, 18 十一月 2025 19:44:21 +0800
Subject: [PATCH] 添加新的文件
---
STM32H743/FML/GPS.c | 43 +++++++++++++++++++++++++------------------
1 files changed, 25 insertions(+), 18 deletions(-)
diff --git a/STM32H743/FML/GPS.c b/STM32H743/FML/GPS.c
index eaf611d..843e10b 100644
--- a/STM32H743/FML/GPS.c
+++ b/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;
+
+ // 绔嬪嵆鍙戦�丟PS鏁版嵁鍒癙ython (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,13 +305,16 @@
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);
+ // 鏍￠獙鍜屽凡鐢盚IDO_UtilParseFormat澶勭悊锛堟牸寮忓瓧绗︿覆涓殑*%**锛�
+ l_stGPIMU.m_u8Checksum = 0; // 涓嶉渶瑕佸崟鐙瓨鍌�
// 楠岃瘉鏍￠獙鍜� (鏍规嵁鏂囨。锛岃繖鏄紓鎴栨牎楠�)
// 娉ㄦ剰: 鏂囨。涓樉绀烘牎楠屽�煎湪鏈�鍚庯紝闇�瑕佹牴鎹疄闄呭崗璁‘瀹氭槸鍚﹂獙璇�
l_stGPIMU.m_bValid = HIDO_TRUE;
+
+ // 绔嬪嵆鍙戦�両MU鏁版嵁鍒癙ython (100Hz鏇存柊)
+ PythonLink_SendIMUData(&l_stGPIMU);
return HIDO_OK;
}
@@ -657,7 +664,7 @@
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_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];
--
Gitblit v1.9.3