From 275b03224aa6170d4dc8c661c1cd949dd88c1fcb Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期一, 24 十一月 2025 15:10:21 +0800
Subject: [PATCH] 转向问题解决，方向定义问题，开始走了
---
STM32H743/APL/motion_control_task.c | 7 +++++--
1 files changed, 5 insertions(+), 2 deletions(-)
diff --git a/STM32H743/APL/motion_control_task.c b/STM32H743/APL/motion_control_task.c
index a93c5f0..8543cae 100644
--- a/STM32H743/APL/motion_control_task.c
+++ b/STM32H743/APL/motion_control_task.c
@@ -36,6 +36,7 @@
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;
@@ -121,8 +122,9 @@
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));
@@ -131,9 +133,10 @@
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;
--
Gitblit v1.10.0