From f88f3da8f132cd1dd321dfc584a1fe68b6eb2138 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期四, 04 十二月 2025 21:49:01 +0800
Subject: [PATCH] 在跑校准了，还是有些问题,GPS坐标有时候不更新
---
STM32H743/APL/app.c | 45 +++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 45 insertions(+), 0 deletions(-)
diff --git a/STM32H743/APL/app.c b/STM32H743/APL/app.c
index ff76a00..26b3fff 100644
--- a/STM32H743/APL/app.c
+++ b/STM32H743/APL/app.c
@@ -16,6 +16,7 @@
#include "SBUS.h"
#include "pwm_ctrol.h"
#include "PythonLink.h"
+#include "motion_mode.h"
#include "motion_control_task.h"
#include "motion_calibration_task.h"
osSemaphoreId_t g_semaphoreHandle = NULL;
@@ -48,8 +49,21 @@
BT_Init();
SBUS_Init();
PythonLink_Init();
+#if (MOTION_ACTIVE_MODE == MOTION_MODE_CONTROL)
MotionControl_TaskInit();
+#elif (MOTION_ACTIVE_MODE == MOTION_MODE_CALIBRATION)
MotionCalibration_TaskInit();
+#else
+#error "Unsupported MOTION_ACTIVE_MODE"
+#endif
+ /* ENU璋冭瘯鎵撳嵃锛堥潪鏍″噯妯″紡锛� */
+ static HIDO_UINT32 debug_print_counter = 0;
+ const HIDO_UINT32 DEBUG_PRINT_INTERVAL = 100; /* 姣�100娆″惊鐜墦鍗颁竴娆� (绾�500ms) */
+
+ /* GPS GGA涓婁紶瀹氭椂鍣� */
+ static HIDO_UINT32 gps_upload_counter = 0;
+ const HIDO_UINT32 GPS_UPLOAD_INTERVAL = 200; /* 姣�200娆″惊鐜笂浼犱竴娆� (绾�1000ms) */
+
while (1)
{
//HIDO_UINT32 timeout = HIDO_TimerGetNearestTimeout(1000);
@@ -65,6 +79,37 @@
HIDO_ATLitePoll();
Internet_Poll();
GPS_Poll();
+
+ /* 瀹氭椂涓婁紶GGA鎶ユ枃鍒�4G锛堟瘡绉掍竴娆★級 */
+ if (++gps_upload_counter >= GPS_UPLOAD_INTERVAL)
+ {
+ gps_upload_counter = 0;
+ GPS_UploadGGA();
+ }
+
+ /* 鍛ㄦ湡鎬ф墦鍗癊NU鍧愭爣鍜孖MU鏁版嵁锛堢敤浜庤皟璇旼PS瑙ｆ瀽锛� */
+#if (MOTION_ACTIVE_MODE != MOTION_MODE_CALIBRATION)
+ if (++debug_print_counter >= DEBUG_PRINT_INTERVAL)
+ {
+ debug_print_counter = 0;
+
+ ST_GPRMI gprmi;
+ ST_GPIMU gpimu;
+ float enu[3];
+
+ if (GPS_GetGPRMI(&gprmi) == HIDO_OK &&
+ GPS_GetGPIMU(&gpimu) == HIDO_OK &&
+ GPS_GetCurrentENU(enu) == HIDO_OK)
+ {
+ HIDO_Debug2("$DEBUG,ENU,%.2f,%.2f,%.2f,HDG,%.2f,ACC,%.3f,%.3f,%.3f,GYRO,%.3f,%.3f,%.3f\r\n",
+ enu[0], enu[1], enu[2], /* ENU鍧愭爣 (m) */
+ gprmi.m_fHeadingAngle, /* 鑸悜瑙� (搴�) */
+ gpimu.m_fAccelX, gpimu.m_fAccelY, gpimu.m_fAccelZ, /* 鍔犻�熷害 (m/s虏) */
+ gpimu.m_fGyroX, gpimu.m_fGyroY, gpimu.m_fGyroZ); /* 瑙掗�熷害 (rad/s) */
+ }
+ }
+#endif
+
UDPClient_Poll();
TCPClient_Poll();
IdleTask();
--
Gitblit v1.10.0