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/run_sim.py | 625 +++++++++++++++++++++++++++++++++++++++++++++++++-------
1 files changed, 543 insertions(+), 82 deletions(-)
diff --git a/python/hitl/run_sim.py b/python/hitl/run_sim.py
index 99b60f3..2e14e09 100644
--- a/python/hitl/run_sim.py
+++ b/python/hitl/run_sim.py
@@ -23,7 +23,7 @@
except Exception: # pragma: no cover - serial 鍙兘涓嶅瓨鍦�
list_ports = None
-from .protocols import ControlStatus, PoseStatus, StackStatus, StateStatus
+from .protocols import ControlStatus, PoseStatus, StackStatus, StateStatus, nmea_checksum
from .simulator import HitlConfig, HitlSimulator, RunLogger
DEFAULT_UART2_PORT = "COM11"
@@ -33,7 +33,11 @@
DEFAULT_HEADING_DEG = 0.0
DEFAULT_UART2_BAUD = 115200
DEFAULT_UART5_BAUD = 921600
-RUN_LOG_PATH = Path(__file__).resolve().parent / "runlog.txt"
+BASE_DIR = Path(__file__).resolve().parent
+RUN_LOG_PATH = BASE_DIR / "runlog.txt"
+PATH_TEXT_FILE = BASE_DIR / "path_text_input.txt"
+PATH_EXPORT_FILE = PATH_TEXT_FILE.with_suffix(".c")
+STATE_FILE = BASE_DIR / "dashboard_state.json"
def load_path_points(path_file: Path) -> List[Tuple[float, float]]:
@@ -66,6 +70,89 @@
return points
+def parse_path_text(text: str) -> List[Tuple[float, float]]:
+ tokens = text.replace("\n", " ").split(";")
+ points: List[Tuple[float, float]] = []
+ for token in tokens:
+ token = token.strip()
+ if not token:
+ continue
+ parts = token.split(",")
+ if len(parts) < 2:
+ continue
+ try:
+ x = float(parts[0].strip())
+ y = float(parts[1].strip())
+ points.append((x, y))
+ except ValueError:
+ continue
+ return points
+
+
+def format_path_for_mcu(points: List[Tuple[float, float]], decimals: int = 2) -> str:
+ if not points:
+ return "const float g_motion_path_xy[] = {};\n"
+ fmt = f"{{:.{decimals}f}}"
+ lines = ["const float g_motion_path_xy[] = {"]
+ for idx, (x, y) in enumerate(points):
+ comma = "," if idx < len(points) - 1 else ""
+ lines.append(f" {fmt.format(x)}f, {fmt.format(y)}f{comma}")
+ lines.append("};")
+ return "\n".join(lines)
+
+
+def gga_to_short_origin(gga: str) -> str:
+ text = (gga or "").strip()
+ if not text:
+ return ""
+ body = text.split("*")[0]
+ parts = [p.strip() for p in body.split(",")]
+ if len(parts) < 6:
+ return ""
+ lat = parts[2]
+ lat_dir = parts[3]
+ lon = parts[4]
+ lon_dir = parts[5]
+ alt = parts[9] if len(parts) > 9 and parts[9] else ""
+ short = f"{lat},{lat_dir},{lon},{lon_dir}"
+ if alt:
+ short += f",{alt}"
+ return short
+
+
+def build_gga_from_short_origin(text: str, template: str) -> str:
+ tokens = [t.strip() for t in text.replace("\n", " ").split(",") if t.strip()]
+ if len(tokens) < 4:
+ raise ValueError("鍘熺偣鏍煎紡搴斾负 'lat,N,lon,E[,alt]'銆�")
+ lat_dm = tokens[0]
+ lat_dir = tokens[1].upper()
+ lon_dm = tokens[2]
+ lon_dir = tokens[3].upper()
+ alt = tokens[4] if len(tokens) >= 5 else None
+ if lat_dir not in ("N", "S") or lon_dir not in ("E", "W"):
+ raise ValueError("鏂瑰悜蹇呴』鏄� N/S/E/W銆�")
+
+ base = template or DEFAULT_ORIGIN_GGA
+ newline = ""
+ if base.endswith("\r\n"):
+ base = base[:-2]
+ newline = "\r\n"
+ body = base.split("*")[0]
+ fields = body.split(",")
+ while len(fields) <= 13:
+ fields.append("")
+ fields[0] = fields[0] or "$GNGGA"
+ fields[2] = lat_dm
+ fields[3] = lat_dir
+ fields[4] = lon_dm
+ fields[5] = lon_dir
+ if alt is not None:
+ fields[9] = alt
+ body = ",".join(fields)
+ checksum = nmea_checksum(body)
+ return f"{body}*{checksum}{newline}"
+
+
class HitlGuiBridge:
def __init__(self):
self.queue: queue.Queue = queue.Queue(maxsize=200)
@@ -97,6 +184,108 @@
pass
+class ZoomableGraphicsView(QtWidgets.QGraphicsView):
+ sceneMouseMoved = QtCore.pyqtSignal(float, float)
+ """鏀寔榧犳爣婊氳疆缂╂斁 + 鎸変綇宸﹂敭骞崇Щ鐨勮鍥�"""
+
+ def __init__(self, *args, **kwargs):
+ super().__init__(*args, **kwargs)
+ self.setRenderHint(QtGui.QPainter.Antialiasing, True)
+ self.setTransformationAnchor(QtWidgets.QGraphicsView.AnchorUnderMouse)
+ self.setResizeAnchor(QtWidgets.QGraphicsView.AnchorUnderMouse)
+ self.setDragMode(QtWidgets.QGraphicsView.NoDrag)
+ self.setCursor(QtCore.Qt.OpenHandCursor)
+ self._zoom_factor = 1.15 # 姣忔婊氳疆鐨勭缉鏀惧洜瀛�
+ self._panning = False
+ self._pan_last_pos = QtCore.QPoint()
+ self.on_user_pan = None # 鐢卞閮ㄦ敞鍏ョ殑鍥炶皟
+
+ def wheelEvent(self, event: QtGui.QWheelEvent):
+ delta = event.angleDelta().y()
+ scale_factor = self._zoom_factor if delta > 0 else 1.0 / self._zoom_factor
+ self.scale(scale_factor, scale_factor)
+
+ def mousePressEvent(self, event: QtGui.QMouseEvent):
+ if event.button() == QtCore.Qt.LeftButton:
+ self._panning = True
+ self._pan_last_pos = event.pos()
+ self.setCursor(QtCore.Qt.ClosedHandCursor)
+ event.accept()
+ return
+ super().mousePressEvent(event)
+
+ def mouseMoveEvent(self, event: QtGui.QMouseEvent):
+ if self._panning:
+ scene_pos = self.mapToScene(event.pos())
+ last_scene_pos = self.mapToScene(self._pan_last_pos)
+ delta = scene_pos - last_scene_pos
+ self._pan_last_pos = event.pos()
+ self.translate(-delta.x(), -delta.y())
+ if self.on_user_pan:
+ self.on_user_pan()
+ event.accept()
+ return
+ self.sceneMouseMoved.emit(*self._scene_coords(event.pos()))
+ super().mouseMoveEvent(event)
+
+ def _scene_coords(self, view_pos: QtCore.QPoint) -> Tuple[float, float]:
+ scene_pos = self.mapToScene(view_pos)
+ return scene_pos.x(), scene_pos.y()
+
+ def mouseReleaseEvent(self, event: QtGui.QMouseEvent):
+ if event.button() == QtCore.Qt.LeftButton and self._panning:
+ self._panning = False
+ self.setCursor(QtCore.Qt.OpenHandCursor)
+ event.accept()
+ return
+ super().mouseReleaseEvent(event)
+
+
+class DashboardStateStore:
+ def __init__(self, path: Path):
+ self.path = Path(path)
+ self.data: Dict[str, object] = {}
+ self._load()
+
+ def _load(self):
+ if not self.path.exists():
+ self.data = {}
+ return
+ try:
+ self.data = json.loads(self.path.read_text(encoding="utf-8"))
+ except Exception:
+ self.data = {}
+
+ def save(self):
+ try:
+ self.path.write_text(json.dumps(self.data, ensure_ascii=False, indent=2), encoding="utf-8")
+ except Exception:
+ pass
+
+ def get_pose(self) -> Optional[Tuple[float, float, float, float]]:
+ pose = self.data.get("pose")
+ if isinstance(pose, list) and len(pose) == 4:
+ try:
+ return tuple(float(v) for v in pose) # type: ignore
+ except (TypeError, ValueError):
+ return None
+ return None
+
+ def set_pose(self, east: float, north: float, up: float, heading: float):
+ self.data["pose"] = [east, north, up, heading]
+ self.save()
+
+ def get_origin(self) -> Optional[str]:
+ origin = self.data.get("origin")
+ if isinstance(origin, str):
+ return origin
+ return None
+
+ def set_origin(self, origin_str: str):
+ self.data["origin"] = origin_str
+ self.save()
+
+
class HitlDashboard(QtWidgets.QMainWindow):
def __init__(self,
simulator: HitlSimulator,
@@ -113,6 +302,7 @@
self.pose_status: Optional[PoseStatus] = None
self.state_status: Optional[StateStatus] = None
self.stack_status: Dict[str, StackStatus] = {}
+ self._auto_follow = True
self.setWindowTitle("HITL 浠跨湡鐘舵�侀潰鏉�")
self.resize(1280, 720)
@@ -120,7 +310,13 @@
self._serial_open = False
self._last_ports: List[str] = []
+ self.state_store = DashboardStateStore(STATE_FILE)
+
self._build_ui(initial_uart2, initial_uart5)
+ self._load_saved_path_text()
+ if not self.path_text.toPlainText().strip() and self.path_points:
+ self._update_path_text_from_points(self.path_points)
+ self._load_saved_pose()
self._init_scene()
self.timer = QtCore.QTimer(self)
@@ -133,11 +329,39 @@
self.setCentralWidget(central)
self.scene = QtWidgets.QGraphicsScene(self)
- self.view = QtWidgets.QGraphicsView(self.scene)
- self.view.setRenderHint(QtGui.QPainter.Antialiasing, True)
- self.view.setDragMode(QtWidgets.QGraphicsView.ScrollHandDrag)
- self.view.setTransformationAnchor(QtWidgets.QGraphicsView.AnchorUnderMouse)
- layout.addWidget(self.view, stretch=3)
+ left_panel = QtWidgets.QWidget()
+ left_layout = QtWidgets.QVBoxLayout(left_panel)
+ left_layout.setContentsMargins(0, 0, 0, 0)
+ left_layout.setSpacing(4)
+
+ self.view = ZoomableGraphicsView(self.scene)
+ self.view.on_user_pan = self._handle_user_pan
+ self.view.sceneMouseMoved.connect(self._update_mouse_pos)
+ left_layout.addWidget(self.view, stretch=1)
+
+ # Bottom bar for map controls
+ bottom_bar = QtWidgets.QWidget()
+ bottom_layout = QtWidgets.QHBoxLayout(bottom_bar)
+ bottom_layout.setContentsMargins(0, 4, 0, 0)
+
+ self.follow_checkbox = QtWidgets.QCheckBox("鑷姩璺熼殢杞﹁締")
+ self.follow_checkbox.setChecked(True)
+ self.follow_checkbox.toggled.connect(self._on_follow_toggled)
+ bottom_layout.addWidget(self.follow_checkbox)
+
+ self.clear_trail_btn = QtWidgets.QPushButton("娓呴櫎杞ㄨ抗")
+ self.clear_trail_btn.clicked.connect(self._clear_trail)
+ bottom_layout.addWidget(self.clear_trail_btn)
+
+ bottom_layout.addStretch(1)
+
+ self.mouse_pos_label = QtWidgets.QLabel("E: 0.00 N: 0.00")
+ self.mouse_pos_label.setStyleSheet("color:#222; background:rgba(255,255,255,190); padding:2px 6px; border-radius:4px;")
+ bottom_layout.addWidget(self.mouse_pos_label)
+
+ left_layout.addWidget(bottom_bar)
+
+ layout.addWidget(left_panel, stretch=3)
self._port_refresh_supported = list_ports is not None
@@ -169,25 +393,33 @@
self.port2_combo.setEditable(True)
self.port5_combo.setEditable(True)
- # Path
- path_group = QtWidgets.QGroupBox("璺緞鏂囦欢")
- path_layout = QtWidgets.QHBoxLayout(path_group)
- self.path_line = QtWidgets.QLineEdit()
- self.path_browse_btn = QtWidgets.QPushButton("娴忚")
- self.path_load_btn = QtWidgets.QPushButton("鍔犺浇")
- path_layout.addWidget(self.path_line)
- path_layout.addWidget(self.path_browse_btn)
- path_layout.addWidget(self.path_load_btn)
+ # Path text input
+ path_group = QtWidgets.QGroupBox("璺緞杞ㄨ抗")
+ path_layout = QtWidgets.QVBoxLayout(path_group)
+ self.path_text = QtWidgets.QPlainTextEdit()
+ self.path_text.setPlaceholderText("绀轰緥锛�23.36,-42.93;29.26,-52.99; ... ;")
+ self.path_text.setWordWrapMode(QtGui.QTextOption.NoWrap)
+ button_row = QtWidgets.QHBoxLayout()
+ self.path_load_btn = QtWidgets.QPushButton("鍔犺浇鍒板湴鍥�")
+ self.path_export_btn = QtWidgets.QPushButton("瀵煎嚭 MCU 鏍煎紡")
+ button_row.addWidget(self.path_load_btn)
+ button_row.addWidget(self.path_export_btn)
+ path_layout.addWidget(self.path_text)
+ path_layout.addLayout(button_row)
right_layout.addWidget(path_group)
# Origin / pose reset
origin_group = QtWidgets.QGroupBox("鍘熺偣/璧风偣")
grid = QtWidgets.QGridLayout(origin_group)
- self.origin_edit = QtWidgets.QLineEdit(DEFAULT_ORIGIN_GGA)
+ default_origin_short = gga_to_short_origin(self.simulator.config.origin_gga)
+ self.origin_text = QtWidgets.QLineEdit(default_origin_short)
+ self.origin_text.setPlaceholderText("3949.9120,N,11616.8544,E[,alt]")
self.origin_btn = QtWidgets.QPushButton("鏇存柊鍘熺偣")
- grid.addWidget(QtWidgets.QLabel("GGA:"), 0, 0)
- grid.addWidget(self.origin_edit, 0, 1, 1, 2)
- grid.addWidget(self.origin_btn, 0, 3)
+ self.copy_origin_btn = QtWidgets.QPushButton("澶嶅埗瀹忓畾涔�") # 鏂板鎸夐挳
+ grid.addWidget(QtWidgets.QLabel("鍧愭爣 (lat,N,lon,E):"), 0, 0, 1, 2)
+ grid.addWidget(self.origin_text, 0, 2, 1, 2)
+ grid.addWidget(self.origin_btn, 0, 4)
+ grid.addWidget(self.copy_origin_btn, 0, 5) # 娣诲姞鍒板竷灞�
labels = ["E (m)", "N (m)", "U (m)", "Heading (deg)"]
self.pos_spin: List[QtWidgets.QDoubleSpinBox] = []
@@ -211,12 +443,13 @@
status_layout = QtWidgets.QVBoxLayout(status_group)
self.info_label = QtWidgets.QLabel("绛夊緟鏁版嵁...")
self.info_label.setAlignment(QtCore.Qt.AlignTop | QtCore.Qt.AlignLeft)
- self.info_label.setStyleSheet("font-family: Consolas, 'Courier New'; font-size: 12px;")
+ self.info_label.setStyleSheet("font-family: Consolas, 'Courier New'; font-size: 14px;")
info_scroll = QtWidgets.QScrollArea()
info_scroll.setWidgetResizable(True)
info_scroll.setWidget(self.info_label)
+ info_scroll.setMinimumHeight(500) # 杩涗竴姝ュ鍔犳渶灏忛珮搴�
status_layout.addWidget(info_scroll)
- right_layout.addWidget(status_group, stretch=1)
+ right_layout.addWidget(status_group, stretch=4) # 杩涗竴姝ュ鍔犲竷灞�鏉冮噸锛屼粠2澧炲姞鍒�4
# Stack table
stack_group = QtWidgets.QGroupBox("鍫嗘爤鐩戞祴")
@@ -236,15 +469,38 @@
right_layout.addWidget(log_group, stretch=1)
# Scene items
- self.path_item = self.scene.addPath(QtGui.QPainterPath(), QtGui.QPen(QtGui.QColor("gray"), 0.8))
- self.trail_item = self.scene.addPath(QtGui.QPainterPath(), QtGui.QPen(QtGui.QColor("blue"), 1.0))
- self.robot_item = self.scene.addEllipse(-0.3, -0.3, 0.6, 0.6, QtGui.QPen(QtGui.QColor("red")), QtGui.QBrush(QtGui.QColor("red")))
- self.heading_item = self.scene.addLine(0, 0, 0, 0, QtGui.QPen(QtGui.QColor("red"), 2))
- self.target_item = self.scene.addEllipse(-0.2, -0.2, 0.4, 0.4, QtGui.QPen(QtGui.QColor("green")), QtGui.QBrush(QtGui.QColor("green")))
+ self.path_item = self.scene.addPath(QtGui.QPainterPath(), QtGui.QPen(QtGui.QColor("gray"), 0))
+ self.trail_item = self.scene.addPath(QtGui.QPainterPath(), QtGui.QPen(QtGui.QColor("blue"), 0))
+ self.robot_size = 0.25
+ self.robot_item = self.scene.addEllipse(
+ -self.robot_size,
+ -self.robot_size,
+ self.robot_size * 2,
+ self.robot_size * 2,
+ QtGui.QPen(QtGui.QColor("#d11d29"), 0),
+ QtGui.QBrush(QtGui.QColor("#ff4b5c")),
+ )
+ arrow_pen = QtGui.QPen(QtGui.QColor("#ff8c00"), 0)
+ arrow_pen.setJoinStyle(QtCore.Qt.RoundJoin)
+ self.heading_item = self.scene.addPath(
+ QtGui.QPainterPath(),
+ arrow_pen,
+ QtGui.QBrush(QtGui.QColor("#ff8c00")),
+ )
+ self.target_radius = 0.08
+ self.target_item = self.scene.addEllipse(
+ -self.target_radius,
+ -self.target_radius,
+ self.target_radius * 2,
+ self.target_radius * 2,
+ QtGui.QPen(QtGui.QColor("green")),
+ QtGui.QBrush(QtGui.QColor("green")),
+ )
- self.path_browse_btn.clicked.connect(self._browse_path)
- self.path_load_btn.clicked.connect(self._load_path)
+ self.path_load_btn.clicked.connect(self._load_path_from_text)
+ self.path_export_btn.clicked.connect(self._export_path_to_mcu)
self.origin_btn.clicked.connect(self._update_origin)
+ self.copy_origin_btn.clicked.connect(self._copy_origin_macro) # 缁戝畾浜嬩欢
self.reset_pose_btn.clicked.connect(self._reset_position)
self.port_refresh_btn.clicked.connect(self._refresh_serial_ports)
self.serial_toggle_btn.clicked.connect(self._toggle_serial)
@@ -253,6 +509,77 @@
self._set_controls_enabled(False)
self._update_serial_ui()
+ def _copy_origin_macro(self):
+ text = self.origin_text.text().strip()
+ if not text:
+ QtWidgets.QMessageBox.warning(self, "澶嶅埗", "鍘熺偣杈撳叆妗嗕负绌�")
+ return
+
+ # 鏍煎紡: lat_val,N,lon_val,E,alt
+ # 渚嬪: 3949.9120,N,11616.8544,E,47.5
+ parts = [p.strip() for p in text.split(",")]
+ if len(parts) < 4:
+ QtWidgets.QMessageBox.warning(self, "澶嶅埗", "鍘熺偣鏍煎紡閿欒锛岄渶鑷冲皯鍖呭惈缁忕含搴�")
+ return
+
+ try:
+ # 瑙ｆ瀽 DDMM.MMMM 鏍煎紡杞� DD.DDDD
+ # 绾害
+ lat_raw = parts[0]
+ lat_deg_int = int(float(lat_raw) / 100)
+ lat_min = float(lat_raw) % 100
+ lat_dd = lat_deg_int + lat_min / 60.0
+ if parts[1].upper() == 'S':
+ lat_dd = -lat_dd
+
+ # 缁忓害
+ lon_raw = parts[2]
+ lon_deg_int = int(float(lon_raw) / 100)
+ lon_min = float(lon_raw) % 100
+ lon_dd = lon_deg_int + lon_min / 60.0
+ if parts[3].upper() == 'W':
+ lon_dd = -lon_dd
+
+ # 楂樺害
+ alt = "0.0"
+ if len(parts) >= 5:
+ alt = parts[4]
+
+ macro = (
+ f"#define MC_CFG_ORIGIN_LAT_DEG ({lat_dd:.6f})\n"
+ f"#define MC_CFG_ORIGIN_LON_DEG ({lon_dd:.6f})\n"
+ f"#define MC_CFG_ORIGIN_ALT_M ({alt})"
+ )
+
+ clipboard = QtWidgets.QApplication.clipboard()
+ clipboard.setText(macro)
+ QtWidgets.QMessageBox.information(self, "澶嶅埗", "宸插鍒跺畯瀹氫箟鍒板壀璐存澘锛�")
+
+ except ValueError as e:
+ QtWidgets.QMessageBox.warning(self, "澶嶅埗", f"鏁板�艰В鏋愰敊璇�: {e}")
+
+ def _handle_user_pan(self):
+ if self._auto_follow:
+ self.follow_checkbox.blockSignals(True)
+ self.follow_checkbox.setChecked(False)
+ self.follow_checkbox.blockSignals(False)
+ self._auto_follow = False
+
+ def _on_follow_toggled(self, checked: bool):
+ self._auto_follow = checked
+ if checked and self.pose_status:
+ self.view.centerOn(self.pose_status.east, -self.pose_status.north)
+
+ def _clear_trail(self):
+ """娓呴櫎鍦板浘涓婄殑杞ㄨ抗鐐�"""
+ self.trail_points = []
+ # Update scene to remove trail
+ self._update_scene()
+
+ def _update_mouse_pos(self, east: float, scene_y: float):
+ north = -scene_y
+ self.mouse_pos_label.setText(f"E: {east: .2f} N: {north: .2f}")
+
def _refresh_serial_ports(self, initial_uart2: Optional[str] = None, initial_uart5: Optional[str] = None):
ports: List[str] = []
if list_ports:
@@ -283,17 +610,69 @@
self._last_ports = ports
def _set_controls_enabled(self, enabled: bool):
- for widget in (
- self.path_line,
- self.path_browse_btn,
- self.path_load_btn,
- self.origin_edit,
- self.origin_btn,
- self.reset_pose_btn,
- ):
- widget.setEnabled(enabled)
- self.path_line.setReadOnly(not enabled)
- self.origin_edit.setReadOnly(not enabled)
+ self.reset_pose_btn.setEnabled(enabled)
+ self.origin_text.setReadOnly(False)
+ self.origin_btn.setEnabled(True)
+
+ def _load_saved_path_text(self):
+ if PATH_TEXT_FILE.exists():
+ try:
+ text = PATH_TEXT_FILE.read_text(encoding="utf-8")
+ except Exception:
+ text = ""
+ self.path_text.setPlainText(text)
+ points = parse_path_text(text)
+ if points:
+ self.path_points = points
+
+ def _update_path_text_from_points(self, points: List[Tuple[float, float]]):
+ if not points:
+ self.path_text.clear()
+ return
+ text = ";".join(f"{x:.2f},{y:.2f}" for x, y in points) + ";"
+ self.path_text.setPlainText(text)
+
+ def _save_path_text(self, text: str):
+ try:
+ PATH_TEXT_FILE.write_text(text.strip(), encoding="utf-8")
+ except Exception as exc:
+ QtWidgets.QMessageBox.warning(self, "璺緞", f"淇濆瓨璺緞鏂囨湰澶辫触: {exc}")
+
+ def _load_saved_pose(self):
+ pose = self.state_store.get_pose()
+ if pose:
+ self.pos_spin[0].setValue(pose[0])
+ self.pos_spin[1].setValue(pose[1])
+ self.pos_spin[2].setValue(pose[2])
+ self.pos_spin[3].setValue(pose[3])
+
+ def _load_path_from_text(self):
+ text = self.path_text.toPlainText().strip()
+ points = parse_path_text(text)
+ if not points:
+ QtWidgets.QMessageBox.warning(self, "璺緞", "鏈В鏋愬埌鏈夋晥鐨勮建杩圭偣锛岃妫�鏌ヨ緭鍏ユ牸寮忋��")
+ return
+ self.path_points = points
+ self.trail_points.clear()
+ self._save_path_text(text)
+ self._init_scene()
+ self._refresh_view()
+ QtWidgets.QMessageBox.information(self, "璺緞", f"宸插姞杞� {len(points)} 涓矾寰勭偣銆�")
+
+ def _export_path_to_mcu(self):
+ text = self.path_text.toPlainText().strip()
+ points = parse_path_text(text)
+ if not points:
+ QtWidgets.QMessageBox.warning(self, "瀵煎嚭", "娌℃湁鍙鍑虹殑璺緞鐐癸紝璇峰厛鍔犺浇璺緞銆�")
+ return
+ formatted = format_path_for_mcu(points)
+ clipboard = QtWidgets.QApplication.clipboard()
+ clipboard.setText(formatted)
+ try:
+ PATH_EXPORT_FILE.write_text(formatted, encoding="utf-8")
+ except Exception as exc:
+ QtWidgets.QMessageBox.warning(self, "瀵煎嚭", f"鍐欏叆鏂囦欢澶辫触: {exc}")
+ QtWidgets.QMessageBox.information(self, "瀵煎嚭", "宸插鍒跺埌鍓创鏉匡紝骞跺啓鍏�:\n" + str(PATH_EXPORT_FILE))
def _update_serial_ui(self):
self.serial_toggle_btn.setText("鍏抽棴涓插彛" if self._serial_open else "鎵撳紑涓插彛")
@@ -357,37 +736,22 @@
self._close_serial()
super().closeEvent(event)
- def _browse_path(self):
- file_name, _ = QtWidgets.QFileDialog.getOpenFileName(self, "閫夋嫨璺緞鏂囦欢", str(Path.cwd()), "璺緞鏂囦欢 (*.txt *.json);;鎵�鏈夋枃浠� (*)")
- if file_name:
- self.path_line.setText(file_name)
-
- def _load_path(self):
- file_text = self.path_line.text().strip()
- if not file_text:
- QtWidgets.QMessageBox.warning(self, "璺緞", "璇峰厛閫夋嫨璺緞鏂囦欢銆�")
- return
- try:
- new_points = load_path_points(Path(file_text))
- except Exception as exc:
- QtWidgets.QMessageBox.warning(self, "璺緞", f"鍔犺浇澶辫触: {exc}")
- return
- if not new_points:
- QtWidgets.QMessageBox.warning(self, "璺緞", "鏈В鏋愬埌鏈夋晥璺緞鐐广��")
- return
- self.path_points = new_points
- self._init_scene()
- QtWidgets.QMessageBox.information(self, "璺緞", f"宸插姞杞� {len(new_points)} 涓矾寰勭偣銆�")
-
def _update_origin(self):
- gga = self.origin_edit.text().strip()
- if not gga:
- QtWidgets.QMessageBox.warning(self, "鍘熺偣", "璇疯緭鍏� GGA 瀛楃涓层��")
+ text = self.origin_text.text().strip()
+ if not text:
+ QtWidgets.QMessageBox.warning(self, "鍘熺偣", "璇疯緭鍏ュ潗鏍囷紝鏍煎紡锛歭at,N,lon,E[,alt]")
return
- if self.simulator.update_origin(gga):
- QtWidgets.QMessageBox.information(self, "鍘熺偣", "鍘熺偣宸叉洿鏂般��")
- else:
- QtWidgets.QMessageBox.warning(self, "鍘熺偣", "鍘熺偣鏇存柊澶辫触锛岃妫�鏌ユ牸寮忋��")
+ template = self.simulator.config.origin_gga or DEFAULT_ORIGIN_GGA
+ try:
+ gga = build_gga_from_short_origin(text, template)
+ except ValueError as exc:
+ QtWidgets.QMessageBox.warning(self, "鍘熺偣", f"鏍煎紡閿欒: {exc}")
+ return
+ self.simulator.update_origin(gga)
+ self.simulator.config.origin_gga = gga
+ self.origin_text.setText(gga_to_short_origin(gga))
+ self.state_store.set_origin(gga)
+ QtWidgets.QMessageBox.information(self, "鍘熺偣", "鍘熺偣宸叉洿鏂般��")
def _reset_position(self):
east = self.pos_spin[0].value()
@@ -395,11 +759,12 @@
up = self.pos_spin[2].value()
heading = self.pos_spin[3].value()
self.simulator.reset_state(east, north, up, heading)
+ self.state_store.set_pose(east, north, up, heading)
QtWidgets.QMessageBox.information(self, "浣嶇疆", "宸查噸缃豢鐪熺姸鎬併��")
def _init_scene(self):
if not self.path_points:
- self.scene.setSceneRect(-10, -10, 20, 20)
+ self.scene.setSceneRect(-250, -250, 500, 500)
return
path = QtGui.QPainterPath()
first = True
@@ -414,8 +779,22 @@
else:
path.lineTo(px, -py)
self.path_item.setPath(path)
- rect = QtCore.QRectF(min(xs) - 2, min(ys) - 2, (max(xs) - min(xs)) + 4, (max(ys) - min(ys)) + 4)
- self.scene.setSceneRect(rect)
+ min_x = min(xs)
+ max_x = max(xs)
+ min_y = min(ys)
+ max_y = max(ys)
+ width = max_x - min_x
+ height = max_y - min_y
+ pad_x = max(10.0, width * 0.75)
+ pad_y = max(10.0, height * 0.75)
+ scene_rect = QtCore.QRectF(
+ min_x - pad_x,
+ min_y - pad_y,
+ (width if width > 0 else 1.0) + pad_x * 2.0,
+ (height if height > 0 else 1.0) + pad_y * 2.0,
+ )
+ self.scene.setSceneRect(scene_rect)
+ self.view.fitInView(scene_rect, QtCore.Qt.KeepAspectRatio)
def _append_log(self, text: str):
self.log_view.appendPlainText(text)
@@ -515,13 +894,81 @@
pose = self.pose_status
x = pose.east
y = -pose.north
- self.robot_item.setRect(x - 0.3, y - 0.3, 0.6, 0.6)
- heading_rad = math.radians(pose.heading_deg)
- dx = math.cos(heading_rad)
- dy = math.sin(heading_rad)
- self.heading_item.setLine(x, y, x + dx, y - dy)
- self.target_item.setRect(pose.target_east - 0.2, -pose.target_north - 0.2, 0.4, 0.4)
- self.view.centerOn(x, y)
+ size = self.robot_size
+ self.robot_item.setRect(x - size, y - size, size * 2, size * 2)
+
+ # 缁樺埗鑸悜瑙掔澶达紙涓庤溅浣撳垎绂荤殑姗欒壊鎸囧悜绠ご锛�
+ # pose.heading_deg 涓虹綏鐩樿锛堝寳=0掳锛岄『鏃堕拡涓烘锛夛紝闇�杞崲鍒版暟瀛﹀潗鏍囷紙涓�=0掳锛岄�嗘椂閽堜负姝ｏ級
+ heading_rad = math.radians(90.0 - pose.heading_deg)
+ dir_x = math.cos(heading_rad)
+ dir_y = -math.sin(heading_rad) # 瑙嗗浘鍧愭爣 Y 鍚戜笅锛屽洜姝ゅ彇鍙�
+ perp_x = -dir_y
+ perp_y = dir_x
+
+ # 灏勭嚎鍙傛暟
+ offset = 0.25 # 绠ご绂昏溅浣撲腑蹇冪殑鍋忕Щ锛岄伩鍏嶉伄鎸�
+ shaft_length = 0.85
+ shaft_half_width = 0.07
+ head_length = 0.45
+ head_half_width = 0.28
+
+ start = (
+ x + dir_x * offset,
+ y + dir_y * offset,
+ )
+ shaft_end = (
+ start[0] + dir_x * shaft_length,
+ start[1] + dir_y * shaft_length,
+ )
+ tip = (
+ shaft_end[0] + dir_x * head_length,
+ shaft_end[1] + dir_y * head_length,
+ )
+
+ points = [
+ (
+ start[0] + perp_x * shaft_half_width,
+ start[1] + perp_y * shaft_half_width,
+ ),
+ (
+ shaft_end[0] + perp_x * shaft_half_width,
+ shaft_end[1] + perp_y * shaft_half_width,
+ ),
+ (
+ shaft_end[0] + perp_x * head_half_width,
+ shaft_end[1] + perp_y * head_half_width,
+ ),
+ tip,
+ (
+ shaft_end[0] - perp_x * head_half_width,
+ shaft_end[1] - perp_y * head_half_width,
+ ),
+ (
+ shaft_end[0] - perp_x * shaft_half_width,
+ shaft_end[1] - perp_y * shaft_half_width,
+ ),
+ (
+ start[0] - perp_x * shaft_half_width,
+ start[1] - perp_y * shaft_half_width,
+ ),
+ ]
+
+ arrow_path = QtGui.QPainterPath()
+ arrow_path.moveTo(*points[0])
+ for pt in points[1:]:
+ arrow_path.lineTo(*pt)
+ arrow_path.closeSubpath()
+
+ self.heading_item.setPath(arrow_path)
+ target_size = self.target_radius
+ self.target_item.setRect(
+ pose.target_east - target_size,
+ -pose.target_north - target_size,
+ target_size * 2,
+ target_size * 2,
+ )
+ if self._auto_follow:
+ self.view.centerOn(x, y)
def parse_args() -> argparse.Namespace:
@@ -542,12 +989,26 @@
def main():
args = parse_args()
+
+ # 浼樺厛浠� dashboard_state.json 鍔犺浇淇濆瓨鐨勫師鐐瑰拰鍒濆浣嶇疆
+ state_store = DashboardStateStore(STATE_FILE)
+ saved_origin = state_store.get_origin()
+ saved_pose = state_store.get_pose()
+
+ origin_gga = saved_origin if saved_origin else args.origin
+ if saved_pose:
+ initial_enu = (saved_pose[0], saved_pose[1], saved_pose[2])
+ initial_heading = saved_pose[3]
+ else:
+ initial_enu = (args.east, args.north, args.up)
+ initial_heading = args.heading
+
cfg = HitlConfig(
uart2_port=args.uart2,
uart5_port=args.uart5,
- origin_gga=args.origin,
- initial_enu=(args.east, args.north, args.up),
- initial_heading_deg=args.heading,
+ origin_gga=origin_gga,
+ initial_enu=initial_enu,
+ initial_heading_deg=initial_heading,
gps_baudrate=args.gps_baud,
log_baudrate=args.log_baud,
)
--
Gitblit v1.10.0