| | |
|---|
| | | static ST_GeoOrigin g_motion_origin; |
|---|
| | | static HIDO_UINT32 g_last_gps_ms = 0U; |
|---|
| | | static HIDO_UINT32 g_last_imu_ms = 0U; |
|---|
| | | static HIDO_UINT32 g_last_gprmi_tow = 0U; |
|---|
| | | static HIDO_UINT32 g_last_control_report_ms = 0U; |
|---|
| | | static HIDO_UINT32 g_last_pose_report_ms = 0U; |
|---|
| | | static HIDO_UINT32 g_last_state_report_ms = 0U; |
|---|
| | |
|---|
| | | HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK); |
|---|
| | | HIDO_UINT32 now = HAL_GetTick(); |
|---|
| | | |
|---|
| | | if (gps_valid == HIDO_TRUE) |
|---|
| | | if (gps_valid == HIDO_TRUE && gprmi.m_u32TimeOfWeek != g_last_gprmi_tow) |
|---|
| | | { |
|---|
| | | /* 新的GPS数据到来（时间戳变化），才更新航向 */ |
|---|
| | | Geo_GprmiToENU(&gprmi, &g_motion_origin, enu); |
|---|
| | | MC_UpdateGps(&g_motion_state, enu, &gprmi); |
|---|
| | | memcpy(g_last_enu, enu, sizeof(enu)); |
|---|
| | |
|---|
| | | g_last_roll_deg = gprmi.m_fRollAngle; |
|---|
| | | g_last_pose_valid = HIDO_TRUE; |
|---|
| | | g_last_gps_ms = now; |
|---|
| | | g_last_gprmi_tow = gprmi.m_u32TimeOfWeek; |
|---|
| | | g_last_sensor_timestamp_ms = gprmi.m_u32TimeOfWeek; |
|---|
| | | } |
|---|
| | | else if ((now - g_last_gps_ms) > 200U) |
|---|
| | | else if (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U) |
|---|
| | | { |
|---|
| | | g_motion_state.pose_valid = HIDO_FALSE; |
|---|
| | | g_last_pose_valid = HIDO_FALSE; |
|---|
