python/hitl/geo.py
@@ -161,10 +161,14 @@ def heading_math_to_nav(heading_rad: float) -> float: """ 将数学坐标系 (东为0°, CCW为正) 的航向角转换为导航坐标系 (北为0°, 顺时针为正)。 与STM32代码保持一致：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 # 与STM32代码保持一致：先计算，再包装到[0, 360) while nav >= 360.0: nav -= 360.0 while nav < 0.0: nav += 360.0 return nav
