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/simulator.py | 16 ++++++++++++----
1 files changed, 12 insertions(+), 4 deletions(-)
diff --git a/python/hitl/simulator.py b/python/hitl/simulator.py
index d5ddde2..aa2c66b 100644
--- a/python/hitl/simulator.py
+++ b/python/hitl/simulator.py
@@ -291,6 +291,13 @@
dt = max(now - last, 1e-4)
last = now
with self._state_lock:
+ # 娉ㄦ剰锛氳繖閲岀殑 self._target_linear/angular 鏄粠 STM32 鍙戝洖鏉ョ殑 PWM 杞崲鍚庣殑浼扮畻鍊�
+ # 瀹為檯涓婃槸 STM32 璁や负鐨勫懡浠わ紝鎴栬�呭鏋滄垜浠彧鏀� PWM 鐨勮瘽锛屽畠灏辨槸瀹為檯鎺у埗閲�
+ # STM32 鍙戦�佺殑鏄� target_mps 鍜� target_turn锛屾垨鑰呮槸 PWM
+ # 鍦� protocols.py 涓紝濡傛灉鎴戜滑鏀跺埌浜� PWM锛屽氨浼氳浆鎹负閫熷害
+ # 濡傛灉鏀跺埌鐨勬槸 target_mps锛岄偅灏辨槸 target_mps
+ # 鐜板湪鐨勯�昏緫鏄細Simulator 鏀跺埌 PythonLinkFrame锛岄噷闈㈠彲鑳芥槸 PWM 鍙嶇畻鐨� velocity
+ # 杩欐牱灏辨瀯鎴愪簡闂幆锛歋TM32 璁＄畻 PWM -> Python 鎺ユ敹 PWM -> 鍙嶇畻閫熷害 -> 鐗╃悊妯″瀷绉垎 -> 浼犳劅鍣ㄦ暟鎹� -> STM32
state = self.model.step(self._target_linear, self._target_angular, dt).copy()
self._latest_state = state
self._sim_time += timedelta(seconds=dt)
@@ -303,7 +310,6 @@
frame, gps_time_s = self._build_nav_frame()
if frame:
self.uart2.write(frame)
- self._log_binary("PY->STM32 UART2 IM23A_NAV", frame, gps_time_s=gps_time_s, source_rank=0)
self._sleep_remaining(start, period)
def _loop_imu(self):
@@ -313,7 +319,6 @@
frame, gps_time_s = self._build_imu_frame()
if frame:
self.uart2.write(frame)
- self._log_binary("PY->STM32 UART2 IM23A_IMU", frame, gps_time_s=gps_time_s, source_rank=0)
self._sleep_remaining(start, period)
def _loop_control(self):
@@ -389,7 +394,10 @@
def _build_nav_frame(self) -> tuple[bytes | None, float]:
state, timestamp = self._snapshot()
lat, lon, alt = geo.enu_to_lla(state.east, state.north, state.up, self.origin)
- heading_nav = geo.heading_math_to_nav(state.heading)
+ heading_nav_deg = geo.heading_math_to_nav(state.heading)
+ # 灏嗗鑸潗鏍囩郴瑙掑害锛�0-360搴︼級杞崲涓哄姬搴︼紙-蟺鍒跋�锛夛紝0搴︽鍖楀搴�0寮у害
+ # 0掳(姝ｅ寳)鈫�0rad, 90掳(姝ｄ笢)鈫捪�/2rad, 180掳(姝ｅ崡)鈫捪�rad, 270掳(姝ｈタ)鈫�-蟺/2rad
+ heading_nav_rad = math.radians(heading_nav_deg) if heading_nav_deg <= 180.0 else math.radians(heading_nav_deg - 360.0)
gps_time_s = _seconds_of_day(timestamp)
status_flags = int(self.config.position_quality) & 0xFF
frame = build_im23a_nav_frame(
@@ -400,7 +408,7 @@
east_vel=state.east_velocity,
north_vel=state.north_velocity,
up_vel=state.up_velocity,
- heading_deg=heading_nav,
+ heading_rad=heading_nav_rad,
pitch_deg=state.pitch_deg,
roll_deg=state.roll_deg,
accel_bias=(0.0, 0.0, 0.0),
--
Gitblit v1.10.0