songhaoyu/opi-yolo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

移除了无用的脚本

Browse Source
This commit is contained in:
songhaoyu
2026-06-12 11:14:12 +08:00
parent 47fbfe69ca
commit 5e71049e07
3 changed files with 0 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files
drone_controller/qgc_router.py
-199
View File
@@ -1,199 +0,0 @@
import threading
import time
from pymavlink import mavutil
class QGCMissionReceiver:
def __init__(self, connection_str='udpin:0.0.0.0:14550', sys_id=1, comp_id=1, home_lat=34.0, home_lon=113.0):
"""
QGC 航线拦截接收器 (模拟飞控节点)
:param connection_str: pymavlink连接字符串。udpin:0.0.0.0:14550 表示作为本地服务器等待QGC主动连接
:param sys_id: 伪装的系统ID (通常飞控为1)
:param comp_id: 伪装的组件ID (通常飞控为1)
:param home_lat: 模拟的起始点纬度（用于在没有真实GPS的室内环境让QGC地图定点）
:param home_lon: 模拟的起始点经度
"""
self.connection_str = connection_str
self.sys_id = sys_id
self.comp_id = comp_id
self.home_lat = home_lat
self.home_lon = home_lon
# 初始化 MAVLink 链路
self.master = mavutil.mavlink_connection(
self.connection_str,
source_system=self.sys_id,
source_component=self.comp_id
)
# 线程安全与航点存储
self.lock = threading.Lock()
self.waypoints_list = [] # 正式供外部控制脚本读取的航点列表
self._temp_waypoints = [] # 协议传输过程中的临时缓存缓存
# 协议状态控制变量
self.is_running = False
self.expected_count = 0
self.current_seq = 0
def start(self):
"""启动后台模拟线程"""
self.is_running = True
# 线程 1-A: 维持心跳与状态遥测广播 (5Hz)
self.telemetry_thread = threading.Thread(target=self._telemetry_loop, daemon=True)
self.telemetry_thread.start()
# 线程 1-B: 专门阻塞监听并应答 QGC 的航线握手协议
self.protocol_thread = threading.Thread(target=self._protocol_loop, daemon=True)
self.protocol_thread.start()
print(f"[QGCMissionProxy] 虚拟飞控线程已启动，正在监听 UDP 端口: {self.connection_str}")
def stop(self):
"""停止接收器"""
self.is_running = False
if self.master:
self.master.close()
print("[QGCMissionProxy] 虚拟飞控线程已关闭")
def _telemetry_loop(self):
"""高频向QGC广播心跳和定位，激活QGC的航线规划和上传按钮"""
boot_time = time.time()
while self.is_running:
try:
time_ms = int((time.time() - boot_time) * 1000)
# 1. 发送 HEARTBEAT (伪装成运行中的 ArduPilot 多旋翼，且处于 GUIDED 模式解锁状态)
self.master.mav.heartbeat_send(
mavutil.mavlink.MAV_TYPE_QUADROTOR, # 四轴/多旋翼
mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOT, # 固件类型
mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED | mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
0, # custom_mode
mavutil.mavlink.MAV_STATE_ACTIVE # 活动状态
)
# 2. 发送 GLOBAL_POSITION_INT (不发这个QGC地图无法显示飞机，也无法规划航线)
self.master.mav.global_position_int_send(
time_ms,
int(self.home_lat * 1e7), # 纬度 (degrees * 1E7)
int(self.home_lon * 1e7), # 经度 (degrees * 1E7)
10000, # 绝对高度 (毫米, 10米)
0, # 相对高度 (毫米, 0米)
0, 0, 0, # vx, vy, vz 速度 (cm/s)
9000 # hdg 偏航角 (cdeg, 90度朝东)
)
# 3. 发送系统状态 (维持QGC右上角图标正常显示，可选)
self.master.mav.sys_status_send(0, 0, 0, 500, 11100, 100, 80, 0, 0, 0, 0, 0, 0)
except Exception as e:
print(f"[QGCMissionProxy] 遥测发送异常: {e}")
time.sleep(0.2) # 5Hz 频率广播
def _protocol_loop(self):
"""标准 MAVLink 航线协议状态机逻辑"""
while self.is_running:
try:
# 阻塞读取消息
msg = self.master.recv_match(blocking=True, timeout=0.5)
if not msg:
continue
msg_type = msg.get_type()
src_sys = msg.get_srcSystem()
src_comp = msg.get_srcComponent()
# ---- 阶段 A: QGC点击了“发送航线”，通知飞机即将上传的总数 ----
if msg_type == 'MISSION_COUNT':
self.expected_count = msg.count
self.current_seq = 0
self._temp_waypoints = []
print(f"\n[QGCMissionProxy] 拦截到QGC航线上传请求！预计航点总数: {self.expected_count}")
if self.expected_count > 0:
# 核心回复：主动向地面站索要第 0 个航点数据
self.master.mav.mission_request_int_send(
src_sys, src_comp, self.current_seq, msg.mission_type
)
else:
# 总数为0则直接发送成功确认
self.master.mav.mission_ack_send(
src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED, msg.mission_type
)
# ---- 阶段 B: 接收具体的航点细节 (新版QGC通常使用 MISSION_ITEM_INT 整型坐标协议) ----
elif msg_type == 'MISSION_ITEM_INT':
if msg.seq == self.current_seq:
# 解析QGC发来的单条数据
wp = {
'seq': msg.seq,
'frame': msg.frame,
'command': msg.command,
'lat': msg.x / 1e7, # 还原浮点经纬度
'lon': msg.y / 1e7,
'alt': msg.z, # 高度
'param1': msg.param1, # 悬停时间等控制参数
'param2': msg.param2,
'param3': msg.param3,
'param4': msg.param4
}
self._temp_waypoints.append(wp)
print(
f"[QGCMissionProxy] 成功接收并缓存航点 [{msg.seq + 1}/{self.expected_count}]: Lat={wp['lat']:.6f}, Lon={wp['lon']:.6f}, Alt={wp['alt']:.2f}m")
self.current_seq += 1
if self.current_seq < self.expected_count:
# 继续索要下一个航点
self.master.mav.mission_request_int_send(
src_sys, src_comp, self.current_seq, msg.mission_type
)
else:
# ---- 阶段 C: 全部接收完毕，原子化存入正式列表，并向QGC回复“握手成功” ----
with self.lock:
self.waypoints_list = list(self._temp_waypoints)
print(f"🎉 [QGCMissionProxy] 航线完美拦截并填充！列表当前长度: {len(self.waypoints_list)}")
self.master.mav.mission_ack_send(
src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED, msg.mission_type
)
# ---- 兼容机制: 兼容老版本QGC使用的浮点数协议形式 ----
elif msg_type == 'MISSION_ITEM':
if msg.seq == self.current_seq:
wp = {
'seq': msg.seq, 'frame': msg.frame, 'command': msg.command,
'lat': msg.x, 'lon': msg.y, 'alt': msg.z,
'param1': msg.param1, 'param2': msg.param2, 'param3': msg.param3, 'param4': msg.param4
}
self._temp_waypoints.append(wp)
self.current_seq += 1
if self.current_seq < self.expected_count:
self.master.mav.mission_request_send(src_sys, src_comp, self.current_seq, msg.mission_type)
else:
with self.lock:
self.waypoints_list = list(self._temp_waypoints)
self.master.mav.mission_ack_send(src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED,
msg.mission_type)
# ---- 处理地面站点击“清除航线”的请求 ----
elif msg_type == 'MISSION_CLEAR_ALL':
print("[QGCMissionProxy] 拦截到QGC清除航线指令")
with self.lock:
self.waypoints_list = []
self.master.mav.mission_ack_send(src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED,
msg.mission_type)
except Exception as e:
print(f"[QGCMissionProxy] 协议状态机运行异常: {e}")
time.sleep(0.1)
def get_waypoints(self):
"""提供给控制主线程调用的线程安全方法：获取当前最新的航点列表"""
with self.lock:
return list(self.waypoints_list)
def clear_waypoints(self):
"""从香橙派端手动清空列表"""
with self.lock:
self.waypoints_list = []