From 275b03224aa6170d4dc8c661c1cd949dd88c1fcb Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期一, 24 十一月 2025 15:10:21 +0800
Subject: [PATCH] 转向问题解决，方向定义问题，开始走了
---
python/hitl/geo.py | 8 ++++++--
1 files changed, 6 insertions(+), 2 deletions(-)
diff --git a/python/hitl/geo.py b/python/hitl/geo.py
index 1ddafcb..9d0e414 100644
--- a/python/hitl/geo.py
+++ b/python/hitl/geo.py
@@ -161,10 +161,14 @@
def heading_math_to_nav(heading_rad: float) -> float:
"""
灏嗘暟瀛﹀潗鏍囩郴 (涓滀负0掳, CCW涓烘) 鐨勮埅鍚戣杞崲涓哄鑸潗鏍囩郴 (鍖椾负0掳, 椤烘椂閽堜负姝�)銆�
+ 涓嶴TM32浠ｇ爜淇濇寔涓�鑷达細compass_deg = 90.0f - (heading_rad * RAD2DEG)
"""
heading_deg = math.degrees(heading_rad)
- nav = (90.0 - heading_deg) % 360.0
- if nav < 0:
+ nav = 90.0 - heading_deg
+ # 涓嶴TM32浠ｇ爜淇濇寔涓�鑷达細鍏堣绠楋紝鍐嶅寘瑁呭埌[0, 360)
+ while nav >= 360.0:
+ nav -= 360.0
+ while nav < 0.0:
nav += 360.0
return nav
--
Gitblit v1.10.0