From 567085ead3f6adaabd884f16ab4b17c62e8f0403 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期日, 21 十二月 2025 22:28:09 +0800
Subject: [PATCH] OTA升级功能调通，准备增加boot的代码
---
python/hitl/dynamics.py | 11 +++++++++--
1 files changed, 9 insertions(+), 2 deletions(-)
diff --git a/python/hitl/dynamics.py b/python/hitl/dynamics.py
index f8161f0..0c33731 100644
--- a/python/hitl/dynamics.py
+++ b/python/hitl/dynamics.py
@@ -82,7 +82,12 @@
linear_acc = dv / dt
self.state.linear_velocity += dv
- dw = target_angular - self.state.angular_velocity
+ # 绗﹀彿瀹氫箟锛�
+ # - MCU宸蹭慨澶嶆柟鍚戝弽杞棶棰橈紙鍘绘帀浜嗛敊璇殑鍙栧弽锛夛紝鐜板湪鐩存帴浣跨敤MCU涓嬪彂鐨勮閫熷害
+ # - 鏁板鍧愭爣绯伙細涓滀负0掳锛岄�嗘椂閽堜负姝ｏ紙CCW>0锛�
+ target_angular_math = target_angular # 鐩存帴浣跨敤锛屼笉鍐嶅彇鍙�
+
+ dw = target_angular_math - self.state.angular_velocity
max_dw = self.max_angular_accel * dt
dw = _clamp(dw, -max_dw, max_dw)
angular_acc = dw / dt
@@ -116,10 +121,12 @@
self.state.pitch_deg = math.degrees(math.atan2(ax_g, 1.0))
self.state.roll_deg = math.degrees(math.atan2(-ay_g, 1.0))
+ # 鎯锛圛MU锛夌鍙峰畾涔夛細涓嶮CU鏂瑰悜瀹氫箟淇濇寔涓�鑷达紝涓嶅啀鍙栧弽
+ # MCU宸蹭慨澶嶆柟鍚戝弽杞棶棰橈紝鐜板湪鐩存帴浣跨敤angular_velocity
self.state.gyro_deg_s = (
self._rng.gauss(0.0, 0.01),
self._rng.gauss(0.0, 0.01),
- math.degrees(self.state.angular_velocity) + self._rng.gauss(0.0, 0.1),
+ math.degrees(self.state.angular_velocity) + self._rng.gauss(0.0, 0.1), # 鐩存帴浣跨敤锛屼笉鍐嶅彇鍙�
)
temp_drift = self._rng.gauss(0.0, 0.0005)
--
Gitblit v1.10.0