"""
|
硬件在环 (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
|
|
from . import geo
|
from .dynamics import DifferentialDriveModel, DifferentialDriveState
|
from .protocols import (
|
ControlStatus,
|
PoseStatus,
|
PythonLinkDecoder,
|
PythonLinkFrame,
|
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:
|
"""HITL 仿真运行必需的配置项。"""
|
|
uart2_port: str # STM32 UART2 (GPS/IMU 输入 + 控制输出)
|
uart5_port: str | None # STM32 UART5 日志输出
|
origin_gga: str
|
initial_enu: tuple[float, float, float] = (0.0, 0.0, 0.0)
|
initial_heading_deg: float = 0.0
|
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:
|
"""线程安全的串口包装。"""
|
|
port: str | None
|
baudrate: int
|
timeout: float
|
_serial: serial.Serial | None
|
_lock: threading.Lock
|
|
def __init__(self, port: str | None, baudrate: int, timeout: float = 0.0):
|
self.port = port
|
self.baudrate = baudrate
|
self.timeout = timeout
|
self._serial = None
|
self._lock = threading.Lock()
|
|
def open(self):
|
if not self.port:
|
return
|
if self._serial and self._serial.is_open:
|
return
|
self._serial = serial.Serial(
|
self.port,
|
self.baudrate,
|
timeout=self.timeout,
|
write_timeout=0,
|
)
|
self._serial.reset_input_buffer()
|
self._serial.reset_output_buffer()
|
|
def close(self):
|
if self._serial:
|
try:
|
self._serial.close()
|
finally:
|
self._serial = None
|
|
def write(self, data: bytes):
|
if not data or not self._serial:
|
return
|
with self._lock:
|
_ = self._serial.write(data)
|
|
def read(self, size: int = 1) -> bytes:
|
if not self._serial:
|
return b""
|
try:
|
return self._serial.read(size)
|
except serial.SerialException:
|
return b""
|
|
def readline(self) -> bytes:
|
if not self._serial:
|
return b""
|
try:
|
return self._serial.readline()
|
except serial.SerialException:
|
return b""
|
|
|
class HitlSimulator:
|
"""电脑仿真环境与 STM32H7 控制板之间的桥梁。"""
|
|
config: HitlConfig
|
origin: geo.Origin
|
model: DifferentialDriveModel
|
_state_lock: threading.Lock
|
_latest_state: DifferentialDriveState
|
_target_linear: float
|
_target_angular: float
|
_sim_time: datetime
|
uart2: SerialEndpoint
|
log_uart: SerialEndpoint
|
_decoder: PythonLinkDecoder
|
_running: threading.Event
|
_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, run_logger: RunLogger | None = None):
|
self.config = config
|
self.origin = geo.parse_origin(config.origin_gga)
|
self.model = DifferentialDriveModel(
|
track_width=config.track_width,
|
max_linear_speed=config.max_linear_speed,
|
max_angular_speed=math.radians(config.max_angular_speed_deg),
|
)
|
self.model.reset(
|
east=config.initial_enu[0],
|
north=config.initial_enu[1],
|
up=config.initial_enu[2],
|
heading_deg=config.initial_heading_deg,
|
)
|
self._state_lock = threading.Lock()
|
self._latest_state = self.model.state.copy()
|
self._target_linear = 0.0
|
self._target_angular = 0.0
|
self._sim_time = _initial_timestamp_from_gga(config.origin_gga)
|
|
self.uart2 = SerialEndpoint(config.uart2_port, config.gps_baudrate, timeout=0.0)
|
self.log_uart = SerialEndpoint(config.uart5_port, config.log_baudrate, timeout=0.1)
|
|
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
|
|
# ------------------------------------------------------------------ #
|
# 生命周期
|
# ------------------------------------------------------------------ #
|
def start(self):
|
if self._running.is_set():
|
return
|
self.uart2.open()
|
self.log_uart.open()
|
self._running.set()
|
self._threads = [
|
threading.Thread(target=self._loop_physics, name="hitl-phys", 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),
|
]
|
for t in self._threads:
|
t.start()
|
|
def stop(self):
|
if not self._running.is_set():
|
return
|
self._running.clear()
|
for t in self._threads:
|
t.join(timeout=1.0)
|
self._threads.clear()
|
self.uart2.close()
|
self.log_uart.close()
|
|
# ------------------------------------------------------------------ #
|
# 线程
|
# ------------------------------------------------------------------ #
|
def _loop_physics(self):
|
last = time.perf_counter()
|
while self._running.is_set():
|
now = time.perf_counter()
|
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
|
# 这样就构成了闭环：STM32 计算 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_nav(self):
|
period = 0.1 # 10 Hz
|
while self._running.is_set():
|
start = time.perf_counter()
|
frame, gps_time_s = self._build_nav_frame()
|
if frame:
|
self.uart2.write(frame)
|
self._sleep_remaining(start, period)
|
|
def _loop_imu(self):
|
period = 0.01 # 100 Hz
|
while self._running.is_set():
|
start = time.perf_counter()
|
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)
|
|
def _loop_log(self):
|
if not self.config.uart5_port:
|
while self._running.is_set():
|
time.sleep(0.5)
|
return
|
while self._running.is_set():
|
line = self.log_uart.readline()
|
if not line:
|
time.sleep(0.01)
|
continue
|
text = line.decode("utf-8", errors="replace").strip()
|
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_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_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,
|
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_imu_frame(self) -> tuple[bytes | None, float]:
|
state, timestamp = self._snapshot()
|
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,
|
)
|
return frame, gps_time_s
|
|
# ------------------------------------------------------------------ #
|
# 工具
|
# ------------------------------------------------------------------ #
|
def _snapshot(self) -> tuple[DifferentialDriveState, datetime]:
|
with self._state_lock:
|
return self._latest_state.copy(), self._sim_time
|
|
def _handle_control_frame(self, frame: PythonLinkFrame):
|
with self._state_lock:
|
self._target_linear = frame.forward
|
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):
|
elapsed = time.perf_counter() - start
|
remaining = period - elapsed
|
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(",")
|
if len(parts) > 1 and parts[1]:
|
time_str = parts[1]
|
try:
|
hh = int(time_str[0:2])
|
mm = int(time_str[2:4])
|
ss = int(time_str[4:6])
|
frac = float("0." + time_str.split(".")[1]) if "." in time_str else 0.0
|
today = datetime.now(timezone.utc).date()
|
base = datetime(today.year, today.month, today.day, tzinfo=timezone.utc)
|
return base.replace(hour=hh, minute=mm, second=ss, microsecond=int(frac * 1_000_000))
|
except (ValueError, IndexError):
|
pass
|
return datetime.now(timezone.utc)
|
