From dc713149918c980f23bc9405b8ffe7502d5f515e Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 29 十一月 2025 18:57:26 +0800
Subject: [PATCH] IM23数据解析成功，但是硬件仿真有问题，在解决。
---
STM32H743/FML/GPS.c | 695 +++++++++++++++++++--------------------------------------
1 files changed, 237 insertions(+), 458 deletions(-)
diff --git a/STM32H743/FML/GPS.c b/STM32H743/FML/GPS.c
index bde14bb..8420b30 100644
--- a/STM32H743/FML/GPS.c
+++ b/STM32H743/FML/GPS.c
@@ -6,40 +6,32 @@
#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 "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) /* 鍖呭惈澶淬�佽礋杞姐�佹牎楠屽拰鍙婂熬閮� */
+#define IM23A_IMU_FRAME_LEN (52U)
+#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_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,8 +41,6 @@
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;
@@ -59,438 +49,280 @@
static ST_GPIMU l_stGPIMU;
static HIDO_UINT32 s_gprmi_log_idx = 0U;
/* 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_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))
+ {
+ 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);
+ }
+}
+
+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;
-
- // 妫�鏌ユ暟鎹牸寮忔牎楠�
- 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_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_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);
+ l_stGPRMI.m_fRollAngle = roll;
+ l_stGPRMI.m_fPitchAngle = pitch;
+ l_stGPRMI.m_fHeadingAngle = heading;
+ 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;
- // 绔嬪嵆鍙戦�丟PS鏁版嵁鍒癙ython (10Hz鏇存柊)
- // PythonLink_SendGPSData(&l_stGPRMI);
-
- return HIDO_OK;
+ l_u8PosState = l_stGPRMI.m_u8PositionQuality;
}
-/*******************************************************************************
- * 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)
-{
- ST_GPS stGPS;
- HIDO_DataStruct spower[4];
- memset(&stGPS, 0, sizeof(ST_GPS));
- // if (GPS_DataCheck(_pcData, _u32Len) != HIDO_OK)
- // {
- // return HIDO_ERR;
- // }
- 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);
-
- return HIDO_OK;
-}
-/*******************************************************************************
- * 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
{
- // 瑙ｆ瀽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;
- 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);
}
}
@@ -577,61 +409,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;
- }
}
}
@@ -646,7 +432,6 @@
*******************************************************************************/
HIDO_VOID GPS_Init(void)
{
- uint32_t gpsbaudrate;
GPS_PowerOn();
GPS_Rest();
@@ -661,13 +446,7 @@
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);
}
--
Gitblit v1.10.0