From 4adc78553c8d48ff122506195fa33641134bd7b1 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 13 十二月 2025 18:55:14 +0800
Subject: [PATCH] 路径点不重复的控制算法测试通了，但是好像不如之前路径点重复的版本好。增加了蓝牙接口部分。准备移植外包的MQTT。
---
python/hitl/run_sim.py | 81 ++++++++++++++++++++++++++++++++++++++--
1 files changed, 77 insertions(+), 4 deletions(-)
diff --git a/python/hitl/run_sim.py b/python/hitl/run_sim.py
index 6d8baa7..2e14e09 100644
--- a/python/hitl/run_sim.py
+++ b/python/hitl/run_sim.py
@@ -339,14 +339,27 @@
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)
- left_layout.addWidget(self.follow_checkbox, alignment=QtCore.Qt.AlignLeft)
+ 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;")
- left_layout.addWidget(self.mouse_pos_label, alignment=QtCore.Qt.AlignRight)
+ bottom_layout.addWidget(self.mouse_pos_label)
+
+ left_layout.addWidget(bottom_bar)
layout.addWidget(left_panel, stretch=3)
@@ -402,9 +415,11 @@
self.origin_text = QtWidgets.QLineEdit(default_origin_short)
self.origin_text.setPlaceholderText("3949.9120,N,11616.8544,E[,alt]")
self.origin_btn = QtWidgets.QPushButton("鏇存柊鍘熺偣")
+ 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] = []
@@ -428,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("鍫嗘爤鐩戞祴")
@@ -484,6 +500,7 @@
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)
@@ -492,6 +509,55 @@
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)
@@ -504,6 +570,12 @@
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}")
@@ -722,6 +794,7 @@
(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)
--
Gitblit v1.10.0