From 8d0d8dec18cc9170f3fb82a4dba18160dd8e5233 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期三, 10 十二月 2025 23:24:59 +0800
Subject: [PATCH] 还原到路径第一次调通的版本，
---
STM32H743/FML/GPS.c | 17 ++++++++++++++---
1 files changed, 14 insertions(+), 3 deletions(-)
diff --git a/STM32H743/FML/GPS.c b/STM32H743/FML/GPS.c
index ebdde76..e5db9e9 100644
--- a/STM32H743/FML/GPS.c
+++ b/STM32H743/FML/GPS.c
@@ -206,9 +206,20 @@
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;
+ /* 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;
--
Gitblit v1.10.0