路径点不重复的控制算法测试通了，但是好像不如之前路径点重复的版本好。增加了蓝牙接口部分。准备移植外包的MQTT。

yincheng.zhong

10 天以前 4adc78553c8d48ff122506195fa33641134bd7b1

 python/hitl/run_sim.py

@@ -23,17 +23,21 @@
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 = "COM35"
DEFAULT_UART2_PORT = "COM11"
DEFAULT_UART5_PORT = "COM17"
DEFAULT_ORIGIN_GGA = "$GNGGA,080112.000,3949.8105069,N,11616.6876082,E,4,44,0.42,48.502,M,-6.684,M,1.0,0409*73"
DEFAULT_ENU = (0.0, 0.0, 0.0)
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)
@@ -98,6 +185,7 @@


class ZoomableGraphicsView(QtWidgets.QGraphicsView):
    sceneMouseMoved = QtCore.pyqtSignal(float, float)
    """支持鼠标滚轮缩放 + 按住左键平移的视图"""

    def __init__(self, *args, **kwargs):
@@ -137,8 +225,13 @@
                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
@@ -148,6 +241,51 @@
        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,
@@ -172,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)
@@ -192,12 +336,30 @@

        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)
        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;")
        bottom_layout.addWidget(self.mouse_pos_label)

        left_layout.addWidget(bottom_bar)

        layout.addWidget(left_panel, stretch=3)

@@ -231,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] = []
@@ -273,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("堆栈监测")
@@ -298,14 +469,15 @@
        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.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(
            -0.3,
            -0.3,
            0.6,
            0.6,
            QtGui.QPen(QtGui.QColor("#d11d29"), 1.0),
            -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)
@@ -315,11 +487,20 @@
            arrow_pen,
            QtGui.QBrush(QtGui.QColor("#ff8c00")),
        )
        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.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)
@@ -328,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)
@@ -340,6 +570,16 @@
        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:
@@ -370,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 "打开串口")
@@ -444,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, "原点", "请输入坐标，格式：lat,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()
@@ -482,6 +759,7 @@
        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):
@@ -516,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)
@@ -615,7 +894,8 @@
            pose = self.pose_status
            x = pose.east
            y = -pose.north
            self.robot_item.setRect(x - 0.3, y - 0.3, 0.6, 0.6)
            size = self.robot_size
            self.robot_item.setRect(x - size, y - size, size * 2, size * 2)
            
            # 绘制航向角箭头（与车体分离的橙色指向箭头）
            # pose.heading_deg 为罗盘角（北=0°，顺时针为正），需转换到数学坐标（东=0°，逆时针为正）
@@ -680,7 +960,13 @@
            arrow_path.closeSubpath()

            self.heading_item.setPath(arrow_path)
            self.target_item.setRect(pose.target_east - 0.2, -pose.target_north - 0.2, 0.4, 0.4)
            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)

@@ -703,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,
    )