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 | 320 +++++++++++++++++++++++++++++++++++++++++++++++++----
1 files changed, 296 insertions(+), 24 deletions(-)
diff --git a/python/hitl/simulator.py b/python/hitl/simulator.py
index c6d5bdf..aa2c66b 100644
--- a/python/hitl/simulator.py
+++ b/python/hitl/simulator.py
@@ -1,14 +1,18 @@
"""
-纭欢鍦ㄧ幆 (HITL) 浠跨湡鍣細鐢熸垚 $GPRMI/$GPIMU 浼犳劅鍣ㄦ暟鎹紝骞朵笌 STM32H7 閫氳繃 PythonLink 闂幆銆�
+纭欢鍦ㄧ幆 (HITL) 浠跨湡鍣細鐢熸垚 IM23A fmin/fmim 浼犳劅鍣ㄥ抚锛屽苟涓� STM32H7 閫氳繃 PythonLink 闂幆銆�
"""
from __future__ import annotations
import dataclasses
+import heapq
+import itertools
import math
+import queue
import threading
import time
from datetime import datetime, timedelta, timezone
+from pathlib import Path
from typing import Callable
import serial
@@ -16,12 +20,100 @@
from . import geo
from .dynamics import DifferentialDriveModel, DifferentialDriveState
from .protocols import (
+ ControlStatus,
+ PoseStatus,
PythonLinkDecoder,
PythonLinkFrame,
- build_gpimu_sentence,
- build_gprmi_sentence,
+ StackStatus,
+ StateStatus,
+ build_im23a_imu_frame,
+ build_im23a_nav_frame,
+ decode_control_status,
+ decode_pose_status,
+ decode_stack_status,
+ decode_state_status,
+ parse_ascii_message,
)
+GPS_EPOCH = datetime(1980, 1, 6, tzinfo=timezone.utc)
+
+
+class RunLogger:
+ """寮傛鏃ュ織鍣細鎸� GPS 鏃堕棿浼樺厛鎺掑簭鍐欏叆锛涙棤 GPS 鏃堕棿鍒欏洖閫�鍒颁富鏈烘椂闂淬��"""
+
+ def __init__(self, path: Path | str, flush_delay_ms: float = 500.0):
+ self.path = Path(path)
+ self.path.parent.mkdir(parents=True, exist_ok=True)
+ self._file = self.path.open("w", encoding="utf-8")
+ self._queue: queue.Queue = queue.Queue()
+ self._buffer: list[
+ tuple[tuple[int, float], int, float, str, str]
+ ] = []
+ self._stop = threading.Event()
+ self._counter = itertools.count()
+ self._flush_delay = flush_delay_ms / 1000.0
+ self._worker = threading.Thread(target=self._loop, name="hitl-runlog", daemon=True)
+ self._worker.start()
+
+ def log(
+ self,
+ prefix: str,
+ content: str,
+ *,
+ gps_time_s: float | None = None,
+ source_rank: int = 0,
+ ):
+ arrival = time.time()
+ priority = 0 if gps_time_s is not None else 1
+ key_time = float(gps_time_s if gps_time_s is not None else arrival)
+ self._queue.put(
+ (
+ (priority, key_time, source_rank),
+ next(self._counter),
+ arrival,
+ prefix,
+ content,
+ )
+ )
+
+ def close(self):
+ self._stop.set()
+ self._worker.join()
+ self._flush_buffer(force=True)
+ if not self._file.closed:
+ self._file.close()
+
+ def _loop(self):
+ while not self._stop.is_set() or not self._queue.empty() or self._buffer:
+ try:
+ item = self._queue.get(timeout=0.05)
+ heapq.heappush(self._buffer, item)
+ except queue.Empty:
+ pass
+ self._flush_buffer()
+
+ def _flush_buffer(self, force: bool = False):
+ now = time.time()
+ while self._buffer:
+ key, _seq, arrival, prefix, content = self._buffer[0]
+ if not force and (now - arrival) < self._flush_delay and not self._stop.is_set():
+ break
+ heapq.heappop(self._buffer)
+ self._write_line(key, prefix, content)
+
+ def _write_line(
+ self, key: tuple[int, float, int], prefix: str, content: str
+ ):
+ priority, timestamp_s, source_rank = key
+ if priority == 0:
+ ts_label = f"GPS={timestamp_s:09.3f}s"
+ else:
+ ts_label = f"HOST={timestamp_s:012.3f}s"
+ line = f"[{ts_label} r={source_rank}] {prefix}: {content}\n"
+ self._file.write(line)
+ self._file.flush()
+
+
@dataclasses.dataclass
class HitlConfig:
@@ -32,12 +124,13 @@
origin_gga: str
initial_enu: tuple[float, float, float] = (0.0, 0.0, 0.0)
initial_heading_deg: float = 0.0
- gps_baudrate: int = 460800
+ gps_baudrate: int = 115200
log_baudrate: int = 921600
track_width: float = 0.78
baseline_distance: float = 0.9
max_linear_speed: float = 2.0
max_angular_speed_deg: float = 140.0
+ position_quality: int = 4
class SerialEndpoint:
@@ -118,8 +211,12 @@
_threads: list[threading.Thread]
on_control: Callable[[float, float], None] | None
on_log: Callable[[str], None] | None
+ on_control_status: Callable[[ControlStatus], None] | None
+ on_pose_status: Callable[[PoseStatus], None] | None
+ on_state_status: Callable[[StateStatus], None] | None
+ on_stack_status: Callable[[StackStatus], None] | None
- def __init__(self, config: HitlConfig):
+ def __init__(self, config: HitlConfig, run_logger: RunLogger | None = None):
self.config = config
self.origin = geo.parse_origin(config.origin_gga)
self.model = DifferentialDriveModel(
@@ -145,9 +242,15 @@
self._decoder = PythonLinkDecoder(self._handle_control_frame)
self._running = threading.Event()
self._threads = []
+ self.run_logger = run_logger
self.on_control = None
self.on_log = None
+ self.on_control_status = None
+ self.on_pose_status = None
+ self.on_state_status = None
+ self.on_stack_status = None
+ self.on_ascii = None
# ------------------------------------------------------------------ #
# 鐢熷懡鍛ㄦ湡
@@ -160,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),
]
@@ -188,33 +291,43 @@
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 = self._build_gprmi_sentence()
- if sentence:
- self.uart2.write(sentence)
+ 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 = self._build_gpimu_sentence()
- if sentence:
- self.uart2.write(sentence)
+ frame, gps_time_s = self._build_imu_frame()
+ if frame:
+ self.uart2.write(frame)
self._sleep_remaining(start, period)
def _loop_control(self):
while self._running.is_set():
data = self.uart2.read(128)
if data:
+ self._log_binary(
+ "STM32->PY UART2 CTRL_RAW", data, source_rank=1
+ )
self._decoder.feed(data)
else:
time.sleep(0.002)
@@ -230,17 +343,64 @@
time.sleep(0.01)
continue
text = line.decode("utf-8", errors="replace").strip()
- if text and self.on_log:
+ if not text:
+ continue
+ handled = False
+ msg = parse_ascii_message(text)
+ if msg:
+ ctrl = decode_control_status(msg)
+ if ctrl and self.on_control_status:
+ self.on_control_status(ctrl)
+ self._log_text(
+ "STM32 UART5 CTRL",
+ text,
+ 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:
+ self.on_pose_status(pose)
+ self._log_text(
+ "STM32 UART5 POSE",
+ text,
+ gps_time_s=_ascii_timestamp_to_seconds(pose.timestamp_ms),
+ source_rank=1,
+ )
+ handled = True
+ state = decode_state_status(msg)
+ if state and self.on_state_status:
+ self.on_state_status(state)
+ self._log_text(
+ "STM32 UART5 STATE",
+ text,
+ gps_time_s=_ascii_timestamp_to_seconds(state.timestamp_ms),
+ source_rank=1,
+ )
+ handled = True
+ stack = decode_stack_status(msg)
+ if stack and self.on_stack_status:
+ self.on_stack_status(stack)
+ handled = True
+ if not handled and self.on_log:
self.on_log(text)
+ if not handled:
+ self._log_text("STM32 UART5", text, source_rank=1)
# ------------------------------------------------------------------ #
# 鏋勯�犲抚
# ------------------------------------------------------------------ #
- def _build_gprmi_sentence(self) -> bytes | None:
+ 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)
- return build_gprmi_sentence(
+ 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(
timestamp=timestamp,
lat_deg=lat,
lon_deg=lon,
@@ -248,20 +408,25 @@
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,
- baseline_m=self.config.baseline_distance,
+ 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) -> bytes | None:
+ def _build_imu_frame(self) -> tuple[bytes | None, float]:
state, timestamp = self._snapshot()
- return build_gpimu_sentence(
+ gps_time_s = _seconds_of_day(timestamp)
+ frame = build_im23a_imu_frame(
timestamp=timestamp,
accel_g=state.body_accel_g,
gyro_deg_s=state.gyro_deg_s,
- temperature_c=state.temperature_c,
)
+ return frame, gps_time_s
# ------------------------------------------------------------------ #
# 宸ュ叿
@@ -276,6 +441,11 @@
self._target_angular = frame.turn
if self.on_control:
self.on_control(frame.forward, frame.turn)
+ if self.run_logger:
+ self.run_logger.log(
+ "STM32->PY CTRL_FRAME",
+ f"forward={frame.forward:.3f} turn={frame.turn:.3f} pwm={frame.steering_pwm}/{frame.throttle_pwm}",
+ )
@staticmethod
def _sleep_remaining(start: float, period: float):
@@ -284,6 +454,108 @@
if remaining > 0:
time.sleep(remaining)
+ # ------------------------------------------------------------------ #
+ # 澶栭儴鎺у埗
+ # ------------------------------------------------------------------ #
+ def update_origin(self, origin_gga: str):
+ if not origin_gga:
+ return
+ try:
+ new_origin = geo.parse_origin(origin_gga)
+ except ValueError:
+ return
+ self.config.origin_gga = origin_gga
+ self.origin = new_origin
+ self._sim_time = _initial_timestamp_from_gga(origin_gga)
+
+ def reset_state(self, east: float, north: float, up: float, heading_deg: float):
+ with self._state_lock:
+ self.model.reset(east=east, north=north, up=up, heading_deg=heading_deg)
+ self._latest_state = self.model.state.copy()
+ self._target_linear = 0.0
+ self._target_angular = 0.0
+
+ # ------------------------------------------------------------------ #
+ # 鏃ュ織宸ュ叿
+ # ------------------------------------------------------------------ #
+ 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,
+ payload: bytes,
+ *,
+ gps_time_s: float | None = None,
+ source_rank: int = 0,
+ ):
+ if not self.run_logger or not payload:
+ return
+ text = payload.decode("utf-8", errors="replace").strip()
+ self.run_logger.log(prefix, text, gps_time_s=gps_time_s, source_rank=source_rank)
+
+ def _log_binary(
+ self,
+ prefix: str,
+ payload: bytes,
+ *,
+ gps_time_s: float | None = None,
+ source_rank: int = 0,
+ ):
+ if not self.run_logger or not payload:
+ return
+ self.run_logger.log(
+ prefix, payload.hex(), gps_time_s=gps_time_s, source_rank=source_rank
+ )
+
+ def _log_text(
+ self,
+ prefix: str,
+ text: str,
+ *,
+ gps_time_s: float | None = None,
+ source_rank: int = 0,
+ ):
+ if not self.run_logger:
+ return
+ self.run_logger.log(prefix, text, gps_time_s=gps_time_s, source_rank=source_rank)
+
+def _ensure_utc_datetime(dt: datetime) -> datetime:
+ if dt.tzinfo is None:
+ return dt.replace(tzinfo=timezone.utc)
+ return dt.astimezone(timezone.utc)
+
+
+def _seconds_of_day(dt: datetime) -> float:
+ utc = _ensure_utc_datetime(dt)
+ return (
+ utc.hour * 3600
+ + utc.minute * 60
+ + utc.second
+ + utc.microsecond / 1_000_000.0
+ )
+
+
+def _ascii_timestamp_to_seconds(raw: float | int | None) -> float | None:
+ if raw is None:
+ return None
+ try:
+ value = float(raw)
+ except (TypeError, ValueError):
+ return None
+ text = f"{int(abs(value)):09d}"
+ hh = int(text[0:2])
+ mm = int(text[2:4])
+ ss = int(text[4:6])
+ ms = int(text[6:9])
+ if 0 <= hh < 24 and 0 <= mm < 60 and 0 <= ss < 60:
+ return hh * 3600 + mm * 60 + ss + ms / 1000.0
+ return value / 1000.0
+
def _initial_timestamp_from_gga(gga: str) -> datetime:
parts = (gga or "").split(",")
--
Gitblit v1.10.0