| | |
|---|
| | | def disconnect(self): |
|---|
| | | """断开连接""" |
|---|
| | | if self.receiver: |
|---|
| | | self.receiver.disconnect() |
|---|
| | | self.receiver.disconnect() |
|---|
| | | self.receiver = None |
|---|
| | | self.cmd_sender = None |
|---|
| | | |
|---|
| | |
|---|
| | | |
|---|
| | | if self._stop_event.is_set(): |
|---|
| | | return |
|---|
| | | |
|---|
| | | |
|---|
| | | print("[INFO] 开始控制循环") |
|---|
| | | |
|---|
| | | while not self._stop_event.is_set(): |
|---|
| | |
|---|
| | | # 数学角度转地理角度：heading_err_deg |
|---|
| | | # 注意 control_output['info']['heading_err'] 是弧度 |
|---|
| | | heading_err_deg = math.degrees(control_info.get('heading_err', 0)) |
|---|
| | | |
|---|
| | | |
|---|
| | | print(f"[LOG] POS_ENU: {x:.3f}, {y:.3f}, {z:.3f} | " |
|---|
| | | f"ATT(H/P/R): {gps_heading:.2f}°, {gps_pitch:.2f}°, {gps_roll:.2f}° | " |
|---|
| | | f"CTRL: S={status}, F={forward_signal}, T={turn_signal}, Err={xte:.2f}m") |
|---|
| | |
|---|
| | | self._stop_event.clear() |
|---|
| | | # 发送停止命令 (中位值) |
|---|
| | | if self.cmd_sender: |
|---|
| | | print("[INFO] 发送停止命令...") |
|---|
| | | for _ in range(5): |
|---|
| | | self.cmd_sender.send_control_command(1500, 1500) |
|---|
| | | time.sleep(0.02) |
|---|
| | | print("[INFO] 发送停止命令...") |
|---|
| | | for _ in range(5): |
|---|
| | | self.cmd_sender.send_control_command(1500, 1500) |
|---|
| | | time.sleep(0.02) |
|---|
| | | |
|---|
| | | # 打印最终统计 |
|---|
| | | print("\n========== 最终统计 ==========") |
|---|
| | |
|---|
| | | print(f"GPS数据包: {receiver.gps_count}") |
|---|
| | | print(f"IMU数据包: {receiver.imu_count}") |
|---|
| | | if self.cmd_sender: |
|---|
| | | print(f"命令发送: {self.cmd_sender.get_stats()}") |
|---|
| | | print(f"命令发送: {self.cmd_sender.get_stats()}") |
|---|
| | | if receiver: |
|---|
| | | print(f"错误计数: {receiver.error_count}") |
|---|
| | | print("==============================\n") |
|---|
| | |
|---|
| | | origin_alt_m=None, |
|---|
| | | gui_bridge=gui_bridge, |
|---|
| | | ) |
|---|
| | | |
|---|
| | | |
|---|
| | | # 预加载路径文件（若存在） |
|---|
| | | candidate_path = path_file or DEFAULT_PATH_FILE |
|---|
| | | if candidate_path and Path(candidate_path).exists(): |
|---|
| | |
|---|
| | | # 无Qt环境时，沿用旧的命令行工作流 |
|---|
| | | if not controller.path_points and Path(DEFAULT_PATH_FILE).exists(): |
|---|
| | | controller.load_path(DEFAULT_PATH_FILE) |
|---|
| | | if not controller.connect(): |
|---|
| | | print("[ERROR] 串口连接失败，退出") |
|---|
| | | return |
|---|
| | | try: |
|---|
| | | controller.run() |
|---|
| | | finally: |
|---|
| | | controller.disconnect() |
|---|
| | | if not controller.connect(): |
|---|
| | | print("[ERROR] 串口连接失败，退出") |
|---|
| | | return |
|---|
| | | try: |
|---|
| | | controller.run() |
|---|
| | | finally: |
|---|
| | | controller.disconnect() |
|---|
| | | |
|---|
| | | |
|---|
| | | if __name__ == "__main__": |
|---|
