From f88f3da8f132cd1dd321dfc584a1fe68b6eb2138 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期四, 04 十二月 2025 21:49:01 +0800
Subject: [PATCH] 在跑校准了，还是有些问题,GPS坐标有时候不更新
---
STM32H743/FML/GPS.c | 341 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 336 insertions(+), 5 deletions(-)
diff --git a/STM32H743/FML/GPS.c b/STM32H743/FML/GPS.c
index 8420b30..ebdde76 100644
--- a/STM32H743/FML/GPS.c
+++ b/STM32H743/FML/GPS.c
@@ -11,19 +11,23 @@
#include "GPS.h"
#include "GPIO.h"
#include "Uart.h"
+#include "geo_utils.h"
+#include "UDPClient.h"
// #include "global_param.h"
#define GPS_UART_RX_BUF_SIZE 1024
#define GPS_UART_TX_BUF_SIZE (2048 + 512)
#define IM23A_HEADER_LEN (4U)
-#define IM23A_NAV_FRAME_LEN (100U) /* 鍖呭惈澶淬�佽礋杞姐�佹牎楠屽拰鍙婂熬閮� */
-#define IM23A_IMU_FRAME_LEN (52U)
+#define IM23A_NAV_FRAME_LEN (100U) /* fmin: 鍖呭惈澶淬�佽礋杞姐�佹牎楠屽拰鍙婂熬閮� */
+#define IM23A_IMU_FRAME_LEN (52U) /* fmim: IMU鏁版嵁甯� */
+#define IM23A_GIG_FRAME_LEN (95U) /* fmig: GPS瀵艰埅鏁版嵁甯� */
#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_GIG_HEADER "fmig"
#define IM23A_GPS_TIME_SCALE (100.0) /* hhmmss.ss -> centisecond */
#define IM23A_IMU_TIME_SCALE (1000.0) /* hhmmss.ss -> millisecond */
@@ -44,10 +48,16 @@
static HIDO_UINT32 l_u32QXTick = 0;
HIDO_UINT32 getRTK_Tick = 0;
-/* 瀛樺偍鏈�鏂扮殑GPRMI鍜孏PIMU鏁版嵁 */
+/* 瀛樺偍鏈�鏂扮殑GPRMI銆丟PIMU鍜孏PGIG鏁版嵁 */
static ST_GPRMI l_stGPRMI;
static ST_GPIMU l_stGPIMU;
+static ST_GPGIG l_stGPGIG;
static HIDO_UINT32 s_gprmi_log_idx = 0U;
+
+/* ENU鍧愭爣绯诲師鐐癸紙寮�鏈哄悗绗竴涓浐瀹氳В鏃跺垵濮嬪寲锛� */
+static ST_GeoOrigin l_stGeoOrigin = {0};
+static float l_fCurrentENU[3] = {0.0f, 0.0f, 0.0f};
+
/* GPS锟斤拷锟斤拷 */
/*******************************************************************************
* IM23A Helper Declarations *
@@ -57,6 +67,7 @@
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_VOID IM23A_HandleGigFrame(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);
@@ -121,7 +132,7 @@
return HIDO_FALSE;
}
- if ((len != IM23A_NAV_FRAME_LEN) && (len != IM23A_IMU_FRAME_LEN))
+ if ((len != IM23A_NAV_FRAME_LEN) && (len != IM23A_IMU_FRAME_LEN) && (len != IM23A_GIG_FRAME_LEN))
{
return HIDO_FALSE;
}
@@ -152,6 +163,10 @@
{
IM23A_HandleImuFrame(frame);
}
+ else if (len == IM23A_GIG_FRAME_LEN && memcmp(frame, IM23A_GIG_HEADER, IM23A_HEADER_LEN) == 0)
+ {
+ IM23A_HandleGigFrame(frame);
+ }
}
static HIDO_VOID IM23A_HandleNavFrame(const HIDO_UINT8 *frame)
@@ -206,7 +221,21 @@
l_stGPRMI.m_u8PositionQuality = (HIDO_UINT8)(status & 0xFFU);
l_stGPRMI.m_bValid = HIDO_TRUE;
- l_u8PosState = l_stGPRMI.m_u8PositionQuality;
+ // l_u8PosState = l_stGPRMI.m_u8PositionQuality;
+
+ /* 濡傛灉鏄涓�涓浐瀹氳В锛堣В鐘舵��=4锛夛紝鍒濆鍖朎NU鍘熺偣 */
+ if (l_stGeoOrigin.initialized == HIDO_FALSE && l_u8PosState == 4U)
+ {
+ Geo_OriginInit(&l_stGeoOrigin, l_stGPRMI.m_dLatitude, l_stGPRMI.m_dLongitude, (double)l_stGPRMI.m_fAltitude);
+ HIDO_Debug2("[GPS] ENU origin initialized: lat=%.6f, lon=%.6f, alt=%.2f\r\n",
+ l_stGeoOrigin.lat_deg, l_stGeoOrigin.lon_deg, l_stGeoOrigin.alt_m);
+ }
+
+ /* 璁＄畻褰撳墠ENU鍧愭爣 */
+ if (l_stGeoOrigin.initialized == HIDO_TRUE)
+ {
+ Geo_GprmiToENU(&l_stGPRMI, &l_stGeoOrigin, l_fCurrentENU);
+ }
}
static HIDO_VOID IM23A_HandleImuFrame(const HIDO_UINT8 *frame)
@@ -241,6 +270,64 @@
l_stGPIMU.m_bValid = HIDO_TRUE;
}
+static HIDO_VOID IM23A_HandleGigFrame(const HIDO_UINT8 *frame)
+{
+ if (IM23A_ValidateFrame(frame, IM23A_GIG_FRAME_LEN) == HIDO_FALSE)
+ {
+ return;
+ }
+
+ /* fmig鍗忚瑙ｆ瀽锛氳В鏋愪綅缃俊鎭�佽В鐘舵�佸拰宸垎榫勬湡淇℃伅 */
+ const HIDO_UINT8 *base = frame;
+
+ /* 瀛楁1: 鏃堕棿 hhmmss.ss (double, 浣嶇疆5) */
+ double utc = IM23A_ReadDouble(&base[4]);
+
+ /* 瀛楁2: 绾害 (double, 浣嶇疆13) */
+ double latitude = IM23A_ReadDouble(&base[12]);
+
+ /* 瀛楁3: 缁忓害 (double, 浣嶇疆21) */
+ double longitude = IM23A_ReadDouble(&base[20]);
+
+ /* 瀛楁14: HRMS (float, 浣嶇疆73) */
+ float hrms = IM23A_ReadFloat(&base[72]);
+
+ /* 瀛楁15: VRMS (float, 浣嶇疆77) */
+ float vrms = IM23A_ReadFloat(&base[76]);
+
+ /* 瀛楁16: HDOP (float, 浣嶇疆81) */
+ float hdop = IM23A_ReadFloat(&base[80]);
+
+ /* 瀛楁17: VDOP (float, 浣嶇疆85) */
+ float vdop = IM23A_ReadFloat(&base[84]);
+
+ /* 瀛楁18: 鍗槦鏁伴噺 (uint8_t, 浣嶇疆89) */
+ HIDO_UINT8 sat_count = base[88];
+
+ /* 瀛楁19: 瑙ｇ姸鎬� (uint8_t, 浣嶇疆90) */
+ HIDO_UINT8 sol_status = base[89];
+
+ /* 瀛楁20: 宸垎榫勬湡 (uint8_t, 浣嶇疆91) */
+ HIDO_UINT8 diff_age = base[90];
+
+ /* 鏇存柊GPGIG缁撴瀯浣擄紝鍖呭惈浣嶇疆淇℃伅 */
+ memset(&l_stGPGIG, 0, sizeof(ST_GPGIG));
+ l_stGPGIG.m_u32UTCTime = IM23A_ConvertTime(utc, IM23A_GPS_TIME_SCALE);
+ l_stGPGIG.m_dLatitude = latitude;
+ l_stGPGIG.m_dLongitude = longitude;
+ l_stGPGIG.m_u8SatelliteCount = sat_count;
+ l_stGPGIG.m_u8SolutionStatus = sol_status;
+ l_stGPGIG.m_u8DifferentialAge = diff_age;
+ l_stGPGIG.m_fHRMS = hrms;
+ l_stGPGIG.m_fVRMS = vrms;
+ l_stGPGIG.m_fHDOP = hdop;
+ l_stGPGIG.m_fVDOP = vdop;
+ l_stGPGIG.m_bValid = HIDO_TRUE;
+
+ /* 鏇存柊鍏ㄥ眬瑙ｇ姸鎬侊紙鐢ㄤ簬鍏煎鏃т唬鐮侊級 */
+ l_u8PosState = sol_status;
+}
+
static HIDO_VOID GPS_RecvFsm(HIDO_UINT8 _u8RecvChar)
{
@@ -297,6 +384,10 @@
{
parser->m_u32ExpectedLen = IM23A_IMU_FRAME_LEN;
}
+ else if (memcmp(parser->m_au8Buffer, IM23A_GIG_HEADER, IM23A_HEADER_LEN) == 0)
+ {
+ parser->m_u32ExpectedLen = IM23A_GIG_FRAME_LEN;
+ }
else
{
HIDO_UINT8 restart = parser->m_au8Buffer[IM23A_HEADER_LEN - 1U];
@@ -448,6 +539,18 @@
IM23A_ResetParser(&l_stGPSRecv);
Uart_ReConfigBaudRate(UART_ID_GPS, 115200);
+
+ /* 寤舵椂绛夊緟GPS妯″潡鍚姩瀹屾垚 */
+ vTaskDelay(pdMS_TO_TICKS(1000));
+
+ /* 鍙戦�丄T鍛戒护閰嶇疆GPS妯″潡杈撳嚭鍒癠ART2 */
+ const HIDO_CHAR *at_cmd = "AT+GNSS_OUTPUT=UART2,ON\r\n";
+ Uart_Send(UART_ID_GPS, (HIDO_UINT8 *)at_cmd, strlen(at_cmd));
+ vTaskDelay(pdMS_TO_TICKS(100));
+ const HIDO_CHAR *at_cmd2 = "AT+SAVE_ALL\r\n";
+ Uart_Send(UART_ID_GPS, (HIDO_UINT8 *)at_cmd2, strlen(at_cmd2));
+ /* 绛夊緟GPS妯″潡鍝嶅簲 */
+ vTaskDelay(pdMS_TO_TICKS(100));
}
/*******************************************************************************
@@ -502,3 +605,231 @@
return HIDO_OK;
}
+/*******************************************************************************
+ * Function Name : GPS_GetCurrentENU
+ * Description : 鑾峰彇褰撳墠ENU鍧愭爣锛堢浉瀵逛簬寮�鏈哄悗绗竴涓浐瀹氳В锛�
+ * Input : _enu - 瀛樺偍ENU鍧愭爣鐨勬暟缁勬寚閽� [涓�, 鍖�, 澶
+ * Output : None
+ * Return : HIDO_OK - 鎴愬姛, HIDO_ERR - 澶辫触鎴栧師鐐规湭鍒濆鍖�
+ * Author : www.hido-studio.com
+ * Modified Date: : 2025骞�12鏈�3鏃�
+ *******************************************************************************/
+HIDO_INT32 GPS_GetCurrentENU(float _enu[3])
+{
+ if (_enu == HIDO_NULL)
+ {
+ return HIDO_ERR;
+ }
+
+ if (l_stGeoOrigin.initialized == HIDO_FALSE)
+ {
+ _enu[0] = 0.0f;
+ _enu[1] = 0.0f;
+ _enu[2] = 0.0f;
+ return HIDO_ERR;
+ }
+
+ _enu[0] = l_fCurrentENU[0];
+ _enu[1] = l_fCurrentENU[1];
+ _enu[2] = l_fCurrentENU[2];
+
+ return HIDO_OK;
+}
+
+/*******************************************************************************
+ * Function Name : GPS_GetGPGIG
+ * Description : 鑾峰彇鏈�鏂扮殑GPGIG鏁版嵁锛坒mig鎶ユ枃锛岃В鐘舵�佸拰宸垎榫勬湡锛�
+ * Input : _pstGPGIG - 瀛樺偍GPGIG鏁版嵁鐨勭粨鏋勪綋鎸囬拡
+ * Output : None
+ * Return : HIDO_OK - 鎴愬姛, HIDO_ERR - 澶辫触鎴栨暟鎹棤鏁�
+ * Author : www.hido-studio.com
+ * Modified Date: : 2025骞�12鏈�3鏃�
+ *******************************************************************************/
+HIDO_INT32 GPS_GetGPGIG(ST_GPGIG *_pstGPGIG)
+{
+ if (_pstGPGIG == HIDO_NULL)
+ {
+ return HIDO_ERR;
+ }
+
+ if (l_stGPGIG.m_bValid == HIDO_FALSE)
+ {
+ return HIDO_ERR;
+ }
+
+ memcpy(_pstGPGIG, &l_stGPGIG, sizeof(ST_GPGIG));
+
+ return HIDO_OK;
+}
+
+/*******************************************************************************
+ * Function Name : GPS_ConvertLatToDDMM
+ * Description : 灏嗙含搴︿粠搴︽暟鏍煎紡杞崲涓哄害鍒嗘牸寮�
+ * Input : _dLatDeg - 绾害(搴�)锛屾鏁颁负鍖楃含锛岃礋鏁颁负鍗楃含
+ * : _pcSign - 杈撳嚭绗﹀彿瀛楃 'N' 鎴� 'S'
+ * Output : _pcSign
+ * Return : 搴﹀垎鏍煎紡鍊� (ddmm.mmmm)
+ * Author : www.hido-studio.com
+ * Modified Date: : 2025骞�12鏈�4鏃�
+ *******************************************************************************/
+static HIDO_DOUBLE GPS_ConvertLatToDDMM(HIDO_DOUBLE _dLatDeg, HIDO_CHAR *_pcSign)
+{
+ HIDO_DOUBLE absLat = (_dLatDeg < 0.0) ? -_dLatDeg : _dLatDeg;
+ *_pcSign = (_dLatDeg >= 0.0) ? 'N' : 'S';
+
+ HIDO_INT32 degrees = (HIDO_INT32)absLat;
+ HIDO_DOUBLE minutes = (absLat - degrees) * 60.0;
+
+ return (HIDO_DOUBLE)degrees * 100.0 + minutes;
+}
+
+/*******************************************************************************
+ * Function Name : GPS_ConvertLonToDDDMM
+ * Description : 灏嗙粡搴︿粠搴︽暟鏍煎紡杞崲涓哄害鍒嗘牸寮�
+ * Input : _dLonDeg - 缁忓害(搴�)锛屾鏁颁负涓滅粡锛岃礋鏁颁负瑗跨粡
+ * : _pcSign - 杈撳嚭绗﹀彿瀛楃 'E' 鎴� 'W'
+ * Output : _pcSign
+ * Return : 搴﹀垎鏍煎紡鍊� (dddmm.mmmm)
+ * Author : www.hido-studio.com
+ * Modified Date: : 2025骞�12鏈�4鏃�
+ *******************************************************************************/
+static HIDO_DOUBLE GPS_ConvertLonToDDDMM(HIDO_DOUBLE _dLonDeg, HIDO_CHAR *_pcSign)
+{
+ HIDO_DOUBLE absLon = (_dLonDeg < 0.0) ? -_dLonDeg : _dLonDeg;
+ *_pcSign = (_dLonDeg >= 0.0) ? 'E' : 'W';
+
+ HIDO_INT32 degrees = (HIDO_INT32)absLon;
+ HIDO_DOUBLE minutes = (absLon - degrees) * 60.0;
+
+ return (HIDO_DOUBLE)degrees * 100.0 + minutes;
+}
+
+/*******************************************************************************
+ * Function Name : GPS_CalculateNMEAChecksum
+ * Description : 璁＄畻NMEA鍗忚鏍￠獙鍜岋紙XOR锛�
+ * Input : _pcData - 鏁版嵁瀛楃涓诧紙涓嶅寘鍚�$鍜�*锛�
+ * : _u32Len - 鏁版嵁闀垮害
+ * Output : None
+ * Return : 鏍￠獙鍜岋紙0-255锛�
+ * Author : www.hido-studio.com
+ * Modified Date: : 2025骞�12鏈�4鏃�
+ *******************************************************************************/
+static HIDO_UINT8 GPS_CalculateNMEAChecksum(const HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
+{
+ HIDO_UINT8 checksum = 0U;
+ for (HIDO_UINT32 i = 0U; i < _u32Len; i++)
+ {
+ checksum ^= (HIDO_UINT8)_pcData[i];
+ }
+ return checksum;
+}
+
+/*******************************************************************************
+ * Function Name : GPS_FormatGGA
+ * Description : 灏唂min鍜宖mig鏁版嵁鏍煎紡鍖栦负鏍囧噯GGA鎶ユ枃锛堢粡绾害浼樺厛浣跨敤fmig鏁版嵁锛�
+ * Input : _pstGPRMI - fmin鏁版嵁
+ * : _pstGPGIG - fmig鏁版嵁锛堝寘鍚粡绾害淇℃伅锛�
+ * : _pcBuffer - 杈撳嚭缂撳啿鍖�
+ * : _u32BufferSize - 缂撳啿鍖哄ぇ灏�
+ * Output : _pcBuffer
+ * Return : 瀹為檯鍐欏叆鐨勫瓧绗︽暟锛屽け璐ヨ繑鍥�0
+ * Author : www.hido-studio.com
+ * Modified Date: : 2025骞�12鏈�4鏃�
+ *******************************************************************************/
+static HIDO_UINT32 GPS_FormatGGA(const ST_GPRMI *_pstGPRMI, const ST_GPGIG *_pstGPGIG,
+ HIDO_CHAR *_pcBuffer, HIDO_UINT32 _u32BufferSize)
+{
+ if (_pstGPRMI == HIDO_NULL || _pstGPGIG == HIDO_NULL || _pcBuffer == HIDO_NULL)
+ {
+ return 0U;
+ }
+
+ /* 杞崲UTC鏃堕棿锛氬帢绉�(hhmmss.ss) 鈫� 瀛楃涓叉牸寮� */
+ HIDO_UINT32 utc = _pstGPRMI->m_u32UTCTime;
+ HIDO_UINT32 hh = utc / 1000000U;
+ HIDO_UINT32 mm = (utc / 10000U) % 100U;
+ HIDO_UINT32 ss = (utc / 100U) % 100U;
+ HIDO_UINT32 cs = utc % 100U;
+
+ /* 杞崲缁忕含搴︼細浼樺厛浣跨敤fmig鏁版嵁锛屽害鏁� 鈫� 搴﹀垎鏍煎紡 */
+ HIDO_CHAR latSign, lonSign;
+ HIDO_DOUBLE latitude, longitude;
+
+ /* 浼樺厛浣跨敤fmig涓殑缁忕含搴︿俊鎭� */
+ if (_pstGPGIG->m_bValid && _pstGPGIG->m_dLatitude != 0.0 && _pstGPGIG->m_dLongitude != 0.0)
+ {
+ latitude = _pstGPGIG->m_dLatitude;
+ longitude = _pstGPGIG->m_dLongitude;
+ }
+ else
+ {
+ /* fmig鏃犳晥鏃跺洖閫�鍒癴min鏁版嵁 */
+ latitude = _pstGPRMI->m_dLatitude;
+ longitude = _pstGPRMI->m_dLongitude;
+ }
+
+ HIDO_DOUBLE latDDMM = GPS_ConvertLatToDDMM(latitude, &latSign);
+ HIDO_DOUBLE lonDDDMM = GPS_ConvertLonToDDDMM(longitude, &lonSign);
+
+ /* 鏋勫缓GGA鎶ユ枃涓讳綋锛堜笉鍚�$鍜屾牎楠屽拰锛� */
+ HIDO_CHAR ggaBody[256];
+ HIDO_INT32 bodyLen = snprintf(ggaBody, sizeof(ggaBody),
+ "GNGGA,%02u%02u%02u.%02u,%013.8f,%c,%014.8f,%c,%u,%02u,%.1f,%.1f,M,0.0,M,%.1f,",
+ hh, mm, ss, cs, /* UTC鏃堕棿 */
+ latDDMM, latSign, /* 绾害 (8浣嶅皬鏁�) */
+ lonDDDMM, lonSign, /* 缁忓害 (8浣嶅皬鏁�) */
+ (HIDO_UINT32)_pstGPGIG->m_u8SolutionStatus, /* 瀹氫綅璐ㄩ噺 */
+ (HIDO_UINT32)_pstGPGIG->m_u8SatelliteCount, /* 鍗槦鏁伴噺 */
+ (double)_pstGPGIG->m_fHDOP, /* HDOP */
+ (double)_pstGPRMI->m_fAltitude, /* 娴锋嫈 */
+ (double)_pstGPGIG->m_u8DifferentialAge /* 宸垎榫勬湡 */
+ );
+
+ if (bodyLen <= 0 || bodyLen >= (HIDO_INT32)sizeof(ggaBody))
+ {
+ return 0U;
+ }
+
+ /* 璁＄畻鏍￠獙鍜� */
+ HIDO_UINT8 checksum = GPS_CalculateNMEAChecksum(ggaBody, (HIDO_UINT32)bodyLen);
+
+ /* 缁勮瀹屾暣GGA鎶ユ枃锛�$GPGGA,...*CS */
+ HIDO_INT32 totalLen = snprintf(_pcBuffer, _u32BufferSize, "$%s*%02X", ggaBody, checksum);
+
+ if (totalLen <= 0 || totalLen >= (HIDO_INT32)_u32BufferSize)
+ {
+ return 0U;
+ }
+
+ return (HIDO_UINT32)totalLen;
+}
+
+/*******************************************************************************
+ * Function Name : GPS_UploadGGA
+ * Description : 姣忕涓婁紶涓�娆GA鎶ユ枃鍒�4G锛堟嫾鎺min鍜宖mig鏁版嵁锛�
+ * Input : None
+ * Output : None
+ * Return : None
+ * Author : www.hido-studio.com
+ * Modified Date: : 2025骞�12鏈�4鏃�
+ *******************************************************************************/
+HIDO_VOID GPS_UploadGGA(HIDO_VOID)
+{
+ static HIDO_CHAR ggaBuffer[256];
+
+ /* 妫�鏌min鍜宖mig鏁版嵁鏈夋晥鎬� */
+ if (l_stGPRMI.m_bValid == HIDO_FALSE || l_stGPGIG.m_bValid == HIDO_FALSE)
+ {
+ return;
+ }
+
+ /* 鏍煎紡鍖朑GA鎶ユ枃 */
+ HIDO_UINT32 ggaLen = GPS_FormatGGA(&l_stGPRMI, &l_stGPGIG, ggaBuffer, sizeof(ggaBuffer));
+
+ if (ggaLen > 0U)
+ {
+ /* 璋冪敤4G涓婁紶鍑芥暟 */
+ UDPClient_UploadGPS(ggaBuffer);
+ }
+}
+
--
Gitblit v1.10.0