From b53fff11e6f0d560594834de32886239cbba90a3 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期二, 16 十二月 2025 15:48:58 +0800
Subject: [PATCH] 外部调完，可以解析下发的MQTT数据了，但是路径文件太大准备换成http模式
---
STM32H743/FML/GPS.c | 1016 +++++++++++++++++++++++++++++++++--------------------------
1 files changed, 569 insertions(+), 447 deletions(-)
diff --git a/STM32H743/FML/GPS.c b/STM32H743/FML/GPS.c
index 43103c1..f921989 100644
--- a/STM32H743/FML/GPS.c
+++ b/STM32H743/FML/GPS.c
@@ -6,40 +6,36 @@
#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 "geo_utils.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) /* 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 */
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 +45,386 @@
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鍜孏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锟斤拷锟斤拷 */
-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_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);
+static HIDO_UINT16 IM23A_ReadU16(const HIDO_UINT8 *ptr);
+static HIDO_UINT32 IM23A_ConvertTime(double utc_val, double scale);
+
+/* 鏃х増NMEA瑙ｆ瀽鍑芥暟宸茬Щ闄わ紝浠ヤ笅涓篒M23A涓撶敤瑙ｆ瀽宸ュ叿鍑芥暟 */
+
+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) && (len != IM23A_GIG_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);
+ }
+ 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)
+{
+ 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;
+ 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);
+ /* IM23A鍗忚涓� roll/pitch/heading 鍧囦负寮у害锛岄渶杞崲涓鸿搴� */
+ l_stGPRMI.m_fRollAngle = roll * 57.29577951308232f; // RAD2DEG
+ l_stGPRMI.m_fPitchAngle = pitch * 57.29577951308232f; // RAD2DEG
+ l_stGPRMI.m_fHeadingAngle = heading * 57.29577951308232f; // RAD2DEG
- // 妫�鏌ユ暟鎹牸寮忔牎楠�
- if (GPS_DataCheck(_pcData, _u32Len) != HIDO_OK)
+ /* Heading 杞崲涓� 0-360 鑼冨洿 */
+ while (l_stGPRMI.m_fHeadingAngle < 0.0f)
{
- return HIDO_ERR;
+ l_stGPRMI.m_fHeadingAngle += 360.0f;
}
-
- // 瑙ｆ瀽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)
+ while (l_stGPRMI.m_fHeadingAngle >= 360.0f)
{
- return HIDO_ERR;
+ l_stGPRMI.m_fHeadingAngle -= 360.0f;
}
-
- // 瑙ｆ瀽鍚勫瓧娈�
- 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_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;
+
+ // l_u8PosState = l_stGPRMI.m_u8PositionQuality;
- // 绔嬪嵆鍙戦�丟PS鏁版嵁鍒癙ython (10Hz鏇存柊)
- // PythonLink_SendGPSData(&l_stGPRMI);
+ /* 濡傛灉鏄涓�涓浐瀹氳В锛堣В鐘舵��=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);
+ }
- return HIDO_OK;
+ /* 璁＄畻褰撳墠ENU鍧愭爣 */
+ if (l_stGeoOrigin.initialized == HIDO_TRUE)
+ {
+ Geo_GprmiToENU(&l_stGPRMI, &l_stGeoOrigin, l_fCurrentENU);
+ }
}
-/*******************************************************************************
- * 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);
-
- // 鏍￠獙鍜屽凡鐢盚IDO_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;
-
- // 绔嬪嵆鍙戦�両MU鏁版嵁鍒癙ython (100Hz鏇存柊)
- // PythonLink_SendIMUData(&l_stGPIMU);
-
- return HIDO_OK;
}
-u16 g_spsum, g_snum;
-static HIDO_INT32 GPS_ParseGSV(HIDO_CHAR *_pcData, HIDO_UINT32 _u32Len)
+
+static HIDO_VOID IM23A_HandleGigFrame(const HIDO_UINT8 *frame)
{
- ST_GPS stGPS;
- HIDO_DataStruct spower[4];
+ if (IM23A_ValidateFrame(frame, IM23A_GIG_FRAME_LEN) == HIDO_FALSE)
+ {
+ return;
+ }
- memset(&stGPS, 0, sizeof(ST_GPS));
- // if (GPS_DataCheck(_pcData, _u32Len) != HIDO_OK)
- // {
- // return HIDO_ERR;
- // }
+ /* fmig鍗忚瑙ｆ瀽锛氳В鏋愪綅缃俊鎭�佽В鐘舵�佸拰宸垎榫勬湡淇℃伅 */
+ const HIDO_UINT8 *base = frame;
- 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);
+ /* 瀛楁1: 鏃堕棿 hhmmss.ss (double, 浣嶇疆5) */
+ double utc = IM23A_ReadDouble(&base[4]);
- return HIDO_OK;
+ /* 瀛楁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;
}
-/*******************************************************************************
- * 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 if (memcmp(parser->m_au8Buffer, IM23A_GIG_HEADER, IM23A_HEADER_LEN) == 0)
{
- // 瑙ｆ瀽GPRMI鏁版嵁
- GPS_ParseGPRMI(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_u32RecvLen);
+ parser->m_u32ExpectedLen = IM23A_GIG_FRAME_LEN;
}
- else if (memcmp(l_stGPSRecv.m_acRecvBuf, l_stGPSRecv.m_acHeader5, l_stGPSRecv.m_u32Header5Len) == 0)
+ else
{
- // 瑙ｆ瀽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 +511,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杞锛岃鍙朥ART鏁版嵁骞堕�佸叆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 +534,6 @@
*******************************************************************************/
HIDO_VOID GPS_Init(void)
{
- uint32_t gpsbaudrate;
GPS_PowerOn();
GPS_Rest();
@@ -660,14 +548,20 @@
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;
+ 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));
}
/*******************************************************************************
@@ -722,3 +616,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鏃�
+ *******************************************************************************/
+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