From b53fff11e6f0d560594834de32886239cbba90a3 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期二, 16 十二月 2025 15:48:58 +0800
Subject: [PATCH] 外部调完，可以解析下发的MQTT数据了，但是路径文件太大准备换成http模式
---
python/hitl/simulator.py | 98 ++++++++++++++++++++++++++++--------------------
1 files changed, 57 insertions(+), 41 deletions(-)
diff --git a/python/hitl/simulator.py b/python/hitl/simulator.py
index b209cb0..aa2c66b 100644
--- a/python/hitl/simulator.py
+++ b/python/hitl/simulator.py
@@ -1,5 +1,5 @@
"""
-纭欢鍦ㄧ幆 (HITL) 浠跨湡鍣細鐢熸垚 $GPRMI/$GPIMU 浼犳劅鍣ㄦ暟鎹紝骞朵笌 STM32H7 閫氳繃 PythonLink 闂幆銆�
+纭欢鍦ㄧ幆 (HITL) 浠跨湡鍣細鐢熸垚 IM23A fmin/fmim 浼犳劅鍣ㄥ抚锛屽苟涓� STM32H7 閫氳繃 PythonLink 闂幆銆�
"""
from __future__ import annotations
@@ -26,8 +26,8 @@
PythonLinkFrame,
StackStatus,
StateStatus,
- build_gpimu_sentence,
- build_gprmi_sentence,
+ build_im23a_imu_frame,
+ build_im23a_nav_frame,
decode_control_status,
decode_pose_status,
decode_stack_status,
@@ -130,6 +130,7 @@
baseline_distance: float = 0.9
max_linear_speed: float = 2.0
max_angular_speed_deg: float = 140.0
+ position_quality: int = 4
class SerialEndpoint:
@@ -262,8 +263,8 @@
self._running.set()
self._threads = [
threading.Thread(target=self._loop_physics, name="hitl-phys", daemon=True),
- threading.Thread(target=self._loop_gprmi, name="hitl-gprmi", daemon=True),
- threading.Thread(target=self._loop_gpimu, name="hitl-gpimu", daemon=True),
+ threading.Thread(target=self._loop_nav, name="hitl-nav", daemon=True),
+ threading.Thread(target=self._loop_imu, name="hitl-imu", daemon=True),
threading.Thread(target=self._loop_control, name="hitl-ctrl", daemon=True),
threading.Thread(target=self._loop_log, name="hitl-log", daemon=True),
]
@@ -290,29 +291,34 @@
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)
time.sleep(0.005)
- def _loop_gprmi(self):
+ def _loop_nav(self):
period = 0.1 # 10 Hz
while self._running.is_set():
start = time.perf_counter()
- sentence, gps_time_s = self._build_gprmi_sentence()
- if sentence:
- self.uart2.write(sentence)
- self._log_ascii("PY->STM32 UART2 GPFMI", sentence, gps_time_s=gps_time_s, source_rank=0)
+ frame, gps_time_s = self._build_nav_frame()
+ if frame:
+ self.uart2.write(frame)
self._sleep_remaining(start, period)
- def _loop_gpimu(self):
+ def _loop_imu(self):
period = 0.01 # 100 Hz
while self._running.is_set():
start = time.perf_counter()
- sentence, gps_time_s = self._build_gpimu_sentence()
- if sentence:
- self.uart2.write(sentence)
- self._log_ascii("PY->STM32 UART2 GPIMU", sentence, gps_time_s=gps_time_s, source_rank=0)
+ frame, gps_time_s = self._build_imu_frame()
+ if frame:
+ self.uart2.write(frame)
self._sleep_remaining(start, period)
def _loop_control(self):
@@ -351,6 +357,7 @@
gps_time_s=_ascii_timestamp_to_seconds(ctrl.timestamp_ms),
source_rank=1,
)
+ self._apply_ascii_control(ctrl)
handled = True
pose = decode_pose_status(msg)
if pose and self.on_pose_status:
@@ -384,40 +391,42 @@
# ------------------------------------------------------------------ #
# 鏋勯�犲抚
# ------------------------------------------------------------------ #
- def _build_gprmi_sentence(self) -> tuple[bytes | None, float]:
+ 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)
- return (
- build_gprmi_sentence(
- timestamp=timestamp,
- lat_deg=lat,
- lon_deg=lon,
- alt_m=alt,
- east_vel=state.east_velocity,
- north_vel=state.north_velocity,
- up_vel=state.up_velocity,
- heading_deg=heading_nav,
- pitch_deg=state.pitch_deg,
- roll_deg=state.roll_deg,
- baseline_m=self.config.baseline_distance,
- ),
- gps_time_s,
+ status_flags = int(self.config.position_quality) & 0xFF
+ frame = build_im23a_nav_frame(
+ timestamp=timestamp,
+ lat_deg=lat,
+ lon_deg=lon,
+ alt_m=alt,
+ east_vel=state.east_velocity,
+ north_vel=state.north_velocity,
+ up_vel=state.up_velocity,
+ heading_rad=heading_nav_rad,
+ pitch_deg=state.pitch_deg,
+ roll_deg=state.roll_deg,
+ accel_bias=(0.0, 0.0, 0.0),
+ gyro_bias=(0.0, 0.0, 0.0),
+ temperature_c=state.temperature_c,
+ status_flags=status_flags,
)
+ return frame, gps_time_s
- def _build_gpimu_sentence(self) -> tuple[bytes | None, float]:
+ def _build_imu_frame(self) -> tuple[bytes | None, float]:
state, timestamp = self._snapshot()
gps_time_s = _seconds_of_day(timestamp)
- return (
- build_gpimu_sentence(
- timestamp=timestamp,
- accel_g=state.body_accel_g,
- gyro_deg_s=state.gyro_deg_s,
- temperature_c=state.temperature_c,
- ),
- gps_time_s,
+ frame = build_im23a_imu_frame(
+ timestamp=timestamp,
+ accel_g=state.body_accel_g,
+ gyro_deg_s=state.gyro_deg_s,
)
+ return frame, gps_time_s
# ------------------------------------------------------------------ #
# 宸ュ叿
@@ -469,6 +478,13 @@
# ------------------------------------------------------------------ #
# 鏃ュ織宸ュ叿
# ------------------------------------------------------------------ #
+ def _apply_ascii_control(self, ctrl: ControlStatus):
+ with self._state_lock:
+ self._target_linear = ctrl.forward_mps
+ self._target_angular = ctrl.turn_rate
+ if self.on_control:
+ self.on_control(ctrl.forward_mps, ctrl.turn_rate)
+
def _log_ascii(
self,
prefix: str,
--
Gitblit v1.10.0