移除了无用的脚本
This commit is contained in:
@@ -1,82 +0,0 @@
|
|||||||
import time
|
|
||||||
import math
|
|
||||||
from drone_controller.qgc_router import QGCMissionReceiver
|
|
||||||
|
|
||||||
def gps_to_ned(lon, lat, alt, home_lon, home_lat):
|
|
||||||
"""
|
|
||||||
仅基于经纬度计算东向(E)和北向(N)，忽略高度。
|
|
||||||
home_lon, home_lat: 原点经度、纬度
|
|
||||||
lon, lat: 目标点经度、纬度
|
|
||||||
返回 (E, N) 单位：米
|
|
||||||
"""
|
|
||||||
from pyproj import Transformer
|
|
||||||
# 1. 创建 WGS84 经纬度 -> ECEF 的转换器（高度固定为0）
|
|
||||||
transformer = Transformer.from_crs(
|
|
||||||
{"proj": 'latlong', "ellps": 'WGS84', "datum": 'WGS84'},
|
|
||||||
{"proj": 'geocent', "ellps": 'WGS84', "datum": 'WGS84'},
|
|
||||||
always_xy=True
|
|
||||||
)
|
|
||||||
|
|
||||||
# 2. 目标点和 home 点的 ECEF 坐标（alt=0）
|
|
||||||
x_t, y_t, z_t = transformer.transform(lon, lat, 0)
|
|
||||||
x_h, y_h, z_h = transformer.transform(home_lon, home_lat, 0)
|
|
||||||
|
|
||||||
# 3. 差值
|
|
||||||
dx, dy, dz = x_t - x_h, y_t - y_h, z_t - z_h
|
|
||||||
|
|
||||||
# 4. 旋转到 ENU（只取 E,N）
|
|
||||||
phi = math.radians(home_lat)
|
|
||||||
lam = math.radians(home_lon)
|
|
||||||
sin_phi = math.sin(phi)
|
|
||||||
cos_phi = math.cos(phi)
|
|
||||||
sin_lam = math.sin(lam)
|
|
||||||
cos_lam = math.cos(lam)
|
|
||||||
|
|
||||||
E = -sin_lam * dx + cos_lam * dy
|
|
||||||
N = -sin_phi * cos_lam * dx - sin_phi * sin_lam * dy + cos_phi * dz
|
|
||||||
U = -alt
|
|
||||||
|
|
||||||
return E, N, U
|
|
||||||
|
|
||||||
def main():
|
|
||||||
HOME_LAT = 34
|
|
||||||
HOME_LONG = 113
|
|
||||||
|
|
||||||
# 创建一个用于存放本地局部坐标的列表
|
|
||||||
points = []
|
|
||||||
|
|
||||||
qgc_receiver = QGCMissionReceiver(connection_str="udpin:0.0.0.0:14550", home_lat=HOME_LAT, home_lon=HOME_LONG)
|
|
||||||
qgc_receiver.start()
|
|
||||||
|
|
||||||
print("\n[MainLoop] 核心控制线程（线程2）启动，等待地面站下发任务...")
|
|
||||||
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
current_mission = qgc_receiver.get_waypoints()
|
|
||||||
|
|
||||||
if not current_mission:
|
|
||||||
# 列表为空，原地挂起
|
|
||||||
print('[MainLoop] 当前无航点任务...', end='\r')
|
|
||||||
time.sleep(1.0)
|
|
||||||
continue
|
|
||||||
|
|
||||||
print(f"\n[MianLoop] 检测到新航线任务! 包含{len(current_mission)} 个航点, 准备进行香橙派自动巡点飞行控制。。。")
|
|
||||||
|
|
||||||
for wp in current_mission:
|
|
||||||
y, x, z = gps_to_ned(*wp,home_lon=HOME_LONG, home_lat=HOME_LAT)
|
|
||||||
points.append({'x': x, 'y': y, 'z': z, 'yaw':0})
|
|
||||||
print(f"--> 状态机驱动：开始前往目标航点 #{wp['seq']} -> 本地坐标 NED: X={x:.2f}米, Y={y:.2f}米, Z={z:.2f}米")
|
|
||||||
|
|
||||||
time.sleep(3.0)
|
|
||||||
print(f"--> 航点 #{wp['seq']} 任务处理完毕！")
|
|
||||||
|
|
||||||
print("[MainLoop] 航线列表中所有航点全部执行闭环,任务完毕,开始清空队列...")
|
|
||||||
print(points)
|
|
||||||
qgc_receiver.clear_waypoints()
|
|
||||||
points.clear()
|
|
||||||
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
qgc_receiver.stop()
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,17 +0,0 @@
|
|||||||
import time
|
|
||||||
from pymavlink import mavutil
|
|
||||||
|
|
||||||
# 根据你的实际 UDP 配置修改
|
|
||||||
# 如果飞控是作为 UDP 服务端（监听），则 Python 应作为客户端连接：
|
|
||||||
# connection_string = "udpout:飞控IP:14550"
|
|
||||||
# 如果飞控是作为 UDP 客户端（主动连接 Python），则 Python 应作为服务端监听：
|
|
||||||
connection_string = "udpin:127.0.0.1:14550"
|
|
||||||
|
|
||||||
master = mavutil.mavlink_connection(connection_string)
|
|
||||||
|
|
||||||
print("等待飞控心跳包...")
|
|
||||||
try:
|
|
||||||
master.wait_heartbeat(timeout=5)
|
|
||||||
print(f"收到心跳，系统 ID: {master.target_system}, 组件 ID: {master.target_component}")
|
|
||||||
except:
|
|
||||||
print("超时：未收到飞控心跳包，请检查 IP 和端口配置以及飞控端是否在发送心跳")
|
|
||||||
@@ -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 = []
|
|
||||||
Reference in New Issue
Block a user