#include "stdio.h"
|
#include "stdarg.h"
|
#include "stdlib.h"
|
#include "string.h"
|
#include "stm32h7xx_hal.h"
|
#include "math.h"
|
#include "HIDO_Util.h"
|
#include "HIDO_Debug.h"
|
#include "FreeRTOS.h"
|
#include "task.h"
|
#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) /* 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
|
{
|
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];
|
static HIDO_UINT8 l_au8GPSUartTxBuf[GPS_UART_TX_BUF_SIZE];
|
|
static ST_GPIO l_astGPSPin[GPS_PIN_LAST];
|
static ST_GPSRecv l_stGPSRecv;
|
|
static HIDO_UINT8 l_u8PosState = 0;
|
static HIDO_UINT32 l_u32QXTick = 0;
|
HIDO_UINT32 getRTK_Tick = 0;
|
|
/* 存储最新的GPRMI、GPIMU和GPGIG数据 */
|
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 *
|
*******************************************************************************/
|
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解析函数已移除，以下为IM23A专用解析工具函数 */
|
|
static HIDO_DOUBLE IM23A_ReadDouble(const HIDO_UINT8 *ptr)
|
{
|
HIDO_DOUBLE val = 0.0;
|
memcpy(&val, ptr, sizeof(val));
|
return val;
|
}
|
|
static HIDO_FLOAT IM23A_ReadFloat(const HIDO_UINT8 *ptr)
|
{
|
HIDO_FLOAT val = 0.0f;
|
memcpy(&val, ptr, sizeof(val));
|
return val;
|
}
|
|
static HIDO_UINT32 IM23A_ReadU32(const HIDO_UINT8 *ptr)
|
{
|
HIDO_UINT32 val = 0U;
|
memcpy(&val, ptr, sizeof(val));
|
return val;
|
}
|
|
static HIDO_UINT16 IM23A_ReadU16(const HIDO_UINT8 *ptr)
|
{
|
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_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
|
|
/* Heading 转换为 0-360 范围 */
|
while (l_stGPRMI.m_fHeadingAngle < 0.0f)
|
{
|
l_stGPRMI.m_fHeadingAngle += 360.0f;
|
}
|
while (l_stGPRMI.m_fHeadingAngle >= 360.0f)
|
{
|
l_stGPRMI.m_fHeadingAngle -= 360.0f;
|
}
|
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;
|
|
/* 如果是第一个固定解（解状态=4），初始化ENU原点 */
|
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)
|
{
|
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_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;
|
}
|
|
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)
|
{
|
ST_GPSRecv *parser = &l_stGPSRecv;
|
|
if (parser->m_u32ExpectedLen == 0U)
|
{
|
if (parser->m_u32RecvLen == 0U)
|
{
|
if (_u8RecvChar == 'f')
|
{
|
parser->m_au8Buffer[parser->m_u32RecvLen++] = _u8RecvChar;
|
}
|
return;
|
}
|
|
parser->m_au8Buffer[parser->m_u32RecvLen++] = _u8RecvChar;
|
|
if (parser->m_u32RecvLen == 2U && _u8RecvChar != 'm')
|
{
|
if (_u8RecvChar == 'f')
|
{
|
parser->m_au8Buffer[0] = 'f';
|
parser->m_u32RecvLen = 1U;
|
}
|
else
|
{
|
IM23A_ResetParser(parser);
|
}
|
return;
|
}
|
|
if (parser->m_u32RecvLen == 3U && _u8RecvChar != 'i')
|
{
|
if (_u8RecvChar == 'f')
|
{
|
parser->m_au8Buffer[0] = 'f';
|
parser->m_u32RecvLen = 1U;
|
}
|
else
|
{
|
IM23A_ResetParser(parser);
|
}
|
return;
|
}
|
|
if (parser->m_u32RecvLen == IM23A_HEADER_LEN)
|
{
|
if (memcmp(parser->m_au8Buffer, IM23A_NAV_HEADER, IM23A_HEADER_LEN) == 0)
|
{
|
parser->m_u32ExpectedLen = IM23A_NAV_FRAME_LEN;
|
}
|
else if (memcmp(parser->m_au8Buffer, IM23A_IMU_HEADER, IM23A_HEADER_LEN) == 0)
|
{
|
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];
|
IM23A_ResetParser(parser);
|
if (restart == 'f')
|
{
|
parser->m_au8Buffer[0] = 'f';
|
parser->m_u32RecvLen = 1U;
|
}
|
}
|
}
|
return;
|
}
|
|
if (parser->m_u32RecvLen >= IM23A_MAX_FRAME_LEN)
|
{
|
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);
|
}
|
}
|
|
/*******************************************************************************
|
* Function Name : GPS_Rest
|
* Description : GPS��λ
|
* Input : None
|
* Output : None
|
* Return : None
|
* Author : www.hido-studio.com
|
* Modified Date: : 2021��1��8��
|
*******************************************************************************/
|
static HIDO_VOID GPS_Rest(void)
|
{
|
GPIO_SET(&l_astGPSPin[GPS_PIN_REST]);
|
vTaskDelay(pdMS_TO_TICKS(10));
|
GPIO_RESET(&l_astGPSPin[GPS_PIN_REST]);
|
vTaskDelay(pdMS_TO_TICKS(10));
|
GPIO_SET(&l_astGPSPin[GPS_PIN_REST]);
|
}
|
|
/*******************************************************************************
|
* Function Name : GPS_PowerOn
|
* Description : GPS�ϵ�
|
* Input : None
|
* Output : None
|
* Return : None
|
* Author : www.hido-studio.com
|
* Modified Date: : 2021��1��8��
|
*******************************************************************************/
|
static HIDO_VOID GPS_PowerOn(void)
|
{
|
GPIO_SET(&l_astGPSPin[GPS_PIN_EN]);
|
}
|
|
/*******************************************************************************
|
* Function Name : GPS_PowerOff
|
* Description : GPS����
|
* Input : None
|
* Output : None
|
* Return : None
|
* Author : www.hido-studio.com
|
* Modified Date: : 2021��1��8��
|
*******************************************************************************/
|
HIDO_VOID GPS_PowerOff(void)
|
{
|
GPIO_RESET(&l_astGPSPin[GPS_PIN_EN]);
|
}
|
|
/*******************************************************************************
|
* Global Function *
|
*******************************************************************************/
|
|
/*******************************************************************************
|
* Function Name : GPS_GetState
|
* Description : ��ȡGPS��λ״̬
|
* Input : None
|
* Output : None
|
* Return : ��λ״̬ 0 1 2 3
|
* Author : www.hido-studio.com
|
* Modified Date: : 2021��1��8��
|
*******************************************************************************/
|
HIDO_UINT8 GPS_GetState(HIDO_VOID)
|
{
|
return l_u8PosState;
|
}
|
|
/*******************************************************************************
|
* Function Name : GPS_PinRegister
|
* Description : GPSģ��ܽ�ע��
|
* Input : _ePin �ܽŶ���
|
* : _pstGPIOx GPIOx
|
* : _u16GPIOPin GPIO_PIN_x
|
* Output : None
|
* Return : None
|
* Author : www.hido-studio.com
|
* Modified Date: : 2021��1��8��
|
*******************************************************************************/
|
HIDO_VOID GPS_PinRegister(E_GPSPin _ePin, GPIO_TypeDef *_pstGPIOx, HIDO_UINT16 _u16GPIOPin)
|
{
|
l_astGPSPin[_ePin].m_pstGPIOx = _pstGPIOx;
|
l_astGPSPin[_ePin].m_u16GPIOPin = _u16GPIOPin;
|
}
|
|
/*******************************************************************************
|
* Function Name : GPS_Poll
|
* Description : GPS轮询，读取UART数据并送入IM23A解析器
|
*******************************************************************************/
|
HIDO_VOID GPS_Poll(void)
|
{
|
HIDO_UINT8 u8RecvChar = 0U;
|
|
while (Uart_GetChar(UART_ID_GPS, &u8RecvChar) == HIDO_OK)
|
{
|
GPS_RecvFsm(u8RecvChar);
|
}
|
}
|
|
/*******************************************************************************
|
* Function Name : GPS_Init
|
* Description : GPSģ���ʼ��
|
* Input : None
|
* Output : None
|
* Return : None
|
* Author : www.hido-studio.com
|
* Modified Date: : 2021��1��8��
|
*******************************************************************************/
|
HIDO_VOID GPS_Init(void)
|
{
|
GPS_PowerOn();
|
GPS_Rest();
|
|
ST_UartInit stInit;
|
|
stInit.m_eRxMode = UART_RX_MODE_DMA;
|
stInit.m_eTxMode = UART_TX_MODE_DMA;
|
stInit.m_pu8RxBuf = l_au8GPSUartRxBuf;
|
stInit.m_u32RxBufSize = GPS_UART_RX_BUF_SIZE;
|
stInit.m_pu8TxBuf = l_au8GPSUartTxBuf;
|
stInit.m_u32TxBufSize = GPS_UART_TX_BUF_SIZE;
|
stInit.m_u32TxQueueMemberCnt = 2;
|
Uart_Init(UART_ID_GPS, &stInit);
|
|
IM23A_ResetParser(&l_stGPSRecv);
|
Uart_ReConfigBaudRate(UART_ID_GPS, 115200);
|
|
/* 延时等待GPS模块启动完成 */
|
vTaskDelay(pdMS_TO_TICKS(1000));
|
|
/* 发送AT命令配置GPS模块输出到UART2 */
|
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));
|
}
|
|
/*******************************************************************************
|
* Function Name : GPS_GetGPRMI
|
* Description : 获取最新的GPRMI数据
|
* Input : _pstGPRMI - 存储GPRMI数据的结构体指针
|
* Output : None
|
* Return : HIDO_OK - 成功, HIDO_ERR - 失败或数据无效
|
* Author : www.hido-studio.com
|
* Modified Date: : 2025年11月11日
|
*******************************************************************************/
|
HIDO_INT32 GPS_GetGPRMI(ST_GPRMI *_pstGPRMI)
|
{
|
if (_pstGPRMI == HIDO_NULL)
|
{
|
return HIDO_ERR;
|
}
|
|
if (l_stGPRMI.m_bValid == HIDO_FALSE)
|
{
|
return HIDO_ERR;
|
}
|
|
memcpy(_pstGPRMI, &l_stGPRMI, sizeof(ST_GPRMI));
|
|
return HIDO_OK;
|
}
|
|
/*******************************************************************************
|
* Function Name : GPS_GetGPIMU
|
* Description : 获取最新的GPIMU数据
|
* Input : _pstGPIMU - 存储GPIMU数据的结构体指针
|
* Output : None
|
* Return : HIDO_OK - 成功, HIDO_ERR - 失败或数据无效
|
* Author : www.hido-studio.com
|
* Modified Date: : 2025年11月11日
|
*******************************************************************************/
|
HIDO_INT32 GPS_GetGPIMU(ST_GPIMU *_pstGPIMU)
|
{
|
if (_pstGPIMU == HIDO_NULL)
|
{
|
return HIDO_ERR;
|
}
|
|
if (l_stGPIMU.m_bValid == HIDO_FALSE)
|
{
|
return HIDO_ERR;
|
}
|
|
memcpy(_pstGPIMU, &l_stGPIMU, sizeof(ST_GPIMU));
|
|
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数据（fmig报文，解状态和差分龄期）
|
* 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 : 将fmin和fmig数据格式化为标准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无效时回退到fmin数据 */
|
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 : 每秒上传一次GGA报文到4G（拼接fmin和fmig数据）
|
* 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];
|
|
/* 检查fmin和fmig数据有效性 */
|
if (l_stGPRMI.m_bValid == HIDO_FALSE || l_stGPGIG.m_bValid == HIDO_FALSE)
|
{
|
return;
|
}
|
|
/* 格式化GGA报文 */
|
HIDO_UINT32 ggaLen = GPS_FormatGGA(&l_stGPRMI, &l_stGPGIG, ggaBuffer, sizeof(ggaBuffer));
|
|
if (ggaLen > 0U)
|
{
|
/* 调用4G上传函数 */
|
UDPClient_UploadGPS(ggaBuffer);
|
}
|
}
|
