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坐标有时候不更新
---
python/hitl/dynamics.py | 15 +++++++++++++--
1 files changed, 13 insertions(+), 2 deletions(-)
diff --git a/python/hitl/dynamics.py b/python/hitl/dynamics.py
index f8161f0..3a69d05 100644
--- a/python/hitl/dynamics.py
+++ b/python/hitl/dynamics.py
@@ -82,7 +82,14 @@
linear_acc = dv / dt
self.state.linear_velocity += dv
- dw = target_angular - self.state.angular_velocity
+ # 绗﹀彿瀹氫箟锛�
+ # - 缃楃洏鏂瑰悜锛氳礋鐨勬帶鍒朵俊鍙� 鈫� 閫嗘椂閽堟棆杞紝姝ｇ殑鎺у埗淇″彿 鈫� 椤烘椂閽堟棆杞�
+ # - 鏁板鍧愭爣绯伙細涓滀负0掳锛岄�嗘椂閽堜负姝ｏ紙CCW>0锛�
+ # - 鎵�浠ワ細璐熺殑turn_rate锛堥�嗘椂閽堬級鈫� angular_velocity涓烘锛屾鐨則urn_rate锛堥『鏃堕拡锛夆啋 angular_velocity涓鸿礋
+ # - 闇�瑕佸彇鍙嶏細angular_velocity = -target_angular
+ 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 +123,14 @@
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锛夌鍙峰畾涔夛細椤烘椂閽堟锛岄�嗘椂閽堣礋锛堜粠涓婂線涓嬬湅Z杞达紝鍙虫墜铻烘棆瀹氬垯锛�
+ # 鍦ㄦ暟瀛﹀潗鏍囩郴涓細椤烘椂閽� 鈫� angular_velocity涓鸿礋锛岄�嗘椂閽� 鈫� angular_velocity涓烘
+ # 鎵�浠ワ細GyroZ = -angular_velocity锛堝彇鍙嶄互鍖归厤鎯瀹氫箟锛�
+ # 渚嬪锛歛ngular_velocity = -0.8 rad/s锛堥『鏃堕拡锛夆啋 GyroZ = 0.8 deg/s锛堟鍊硷紝椤烘椂閽堬級
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