"""
|
硬件在环 (HITL) 仿真器：生成 $GPRMI/$GPIMU 传感器数据，并与 STM32H7 通过 PythonLink 闭环。
|
"""
|
|
from __future__ import annotations
|
|
import dataclasses
|
import math
|
import threading
|
import time
|
from datetime import datetime, timedelta, timezone
|
from typing import Callable
|
|
import serial
|
|
from . import geo
|
from .dynamics import DifferentialDriveModel, DifferentialDriveState
|
from .protocols import (
|
PythonLinkDecoder,
|
PythonLinkFrame,
|
build_gpimu_sentence,
|
build_gprmi_sentence,
|
)
|
|
|
@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 = 460800
|
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
|
|
|
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
|
|
def __init__(self, config: HitlConfig):
|
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.on_control = None
|
self.on_log = 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_gprmi, name="hitl-gprmi", daemon=True),
|
threading.Thread(target=self._loop_gpimu, name="hitl-gpimu", 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:
|
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):
|
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)
|
self._sleep_remaining(start, period)
|
|
def _loop_gpimu(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)
|
self._sleep_remaining(start, period)
|
|
def _loop_control(self):
|
while self._running.is_set():
|
data = self.uart2.read(128)
|
if data:
|
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 text and self.on_log:
|
self.on_log(text)
|
|
# ------------------------------------------------------------------ #
|
# 构造帧
|
# ------------------------------------------------------------------ #
|
def _build_gprmi_sentence(self) -> bytes | None:
|
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(
|
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,
|
)
|
|
def _build_gpimu_sentence(self) -> bytes | None:
|
state, timestamp = self._snapshot()
|
return build_gpimu_sentence(
|
timestamp=timestamp,
|
accel_g=state.body_accel_g,
|
gyro_deg_s=state.gyro_deg_s,
|
temperature_c=state.temperature_c,
|
)
|
|
# ------------------------------------------------------------------ #
|
# 工具
|
# ------------------------------------------------------------------ #
|
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)
|
|
@staticmethod
|
def _sleep_remaining(start: float, period: float):
|
elapsed = time.perf_counter() - start
|
remaining = period - elapsed
|
if remaining > 0:
|
time.sleep(remaining)
|
|
|
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)
|
