#!/usr/bin/env python3 from pymavlink import mavutil import socket import re import time import math import threading import json import os import datetime import queue import argparse from select import select class Drone: def __init__(self, instance_id, mavlink_port, udp_port): self.instance_id = instance_id self.mavlink_port = mavlink_port self.udp_port = udp_port self.running = True self.connection = None # 創建數據緩存 self.latest_position = None # 創建日誌佇列 self.log_queue = queue.Queue() # 創建日誌文件目錄 self.log_dir = "drone_logs" if not os.path.exists(self.log_dir): os.makedirs(self.log_dir) # 創建一個含時間戳的日誌文件名 timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S") self.log_file = os.path.join(self.log_dir, f"drone_{self.instance_id}_{timestamp}.log") # 通訊統計 self.packet_counter = 0 self.start_time = time.time() self.packets_per_second = 0 def connect(self): """僅建立MAVLink連接""" print(f"Drone {self.instance_id}: 建立MAVLink連接 (Port {self.mavlink_port})...") connection_string = f"udp:127.0.0.1:{self.mavlink_port}" self.connection = mavutil.mavlink_connection(connection_string) self.connection.wait_heartbeat() print(f"Drone {self.instance_id}: MAVLink連接已建立") def set_guided_mode(self): """切換到GUIDED模式""" self.connection.mav.set_mode_send( self.connection.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 4 # GUIDED模式ID ) print(f"Drone {self.instance_id}: 已切換到GUIDED模式") def arm(self): """解鎖無人機""" self.connection.mav.command_long_send( self.connection.target_system, self.connection.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, 0, 0, 0, 0, 0, 0 # 1表示解鎖 ) print(f"Drone {self.instance_id}: 無人機已解鎖") def takeoff(self, altitude=3.0): """起飛到指定高度""" self.connection.mav.command_long_send( self.connection.target_system, self.connection.target_component, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, altitude ) print(f"Drone {self.instance_id}: 起飛命令已發送，目標高度{altitude}公尺") def mavlink_listener(self): """事件驅動方式監聽MAVLink訊息""" # 獲取底層socket mavlink_socket = self.connection.port.fileno() while self.running: # 使用select等待數據，無須sleep readable, _, _ = select([mavlink_socket], [], [], 0.001) if readable: msg = self.connection.recv_msg() if msg and msg.get_type() == 'LOCAL_POSITION_NED': self.latest_position = msg def log_worker(self): """事件驅動方式處理日誌寫入""" while self.running: if not self.log_queue.empty(): try: # 批量處理日誌 batch = [] while not self.log_queue.empty() and len(batch) < 100: try: entry = self.log_queue.get_nowait() batch.append(entry) except queue.Empty: break # 寫入日誌 if batch: with open(self.log_file, 'a', encoding='utf-8') as f: for entry in batch: f.write(json.dumps(entry, ensure_ascii=False) + "\n") except Exception as e: print(f"Drone {self.instance_id}: 日誌寫入錯誤: {e}") # 每秒計算一次統計數據 now = time.time() if now - self.start_time >= 1.0: self.packets_per_second = self.packet_counter print(f"Drone {self.instance_id} - 每秒封包數: {self.packets_per_second}") self.packet_counter = 0 self.start_time = now # 使用select等待短暫時間而非sleep select([], [], [], 0.01) # 10ms的微小等待 def log_packet(self, packet_data, packet_num): """將數據加入日誌佇列""" try: timestamp = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3] log_entry = { "timestamp": timestamp, "packet_number": packet_num, "data": packet_data } self.log_queue.put(log_entry) except Exception as e: print(f"Drone {self.instance_id}: 佇列添加錯誤: {e}") def run(self): """使用事件驅動方式處理UDP通訊""" # 啟動背景線程 mavlink_thread = threading.Thread(target=self.mavlink_listener) mavlink_thread.daemon = True mavlink_thread.start() log_thread = threading.Thread(target=self.log_worker) log_thread.daemon = True log_thread.start() # 設置UDP服務器 server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 262144) # 增大接收緩衝區 server_socket.bind(('0.0.0.0', self.udp_port)) server_socket.setblocking(0) # 非阻塞模式 print(f"Drone {self.instance_id}: UDP服務器已啟動，監聽端口{self.udp_port}") print(f"Drone {self.instance_id}: 日誌將保存至 {self.log_file}") # 主事件循環 while self.running: # 使用select監聽UDP readable, _, _ = select([server_socket], [], [], 0.001) if readable: try: data, _ = server_socket.recvfrom(4096) decoded_data = data.decode('utf-8') # 增加計數器 self.packet_counter += 1 total_packets = self.packet_counter try: # 解析JSON json_data = json.loads(decoded_data) # 寫入日誌 self.log_packet(json_data, total_packets) # 提取速度數據 vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data) if vel_match: velocity_str = vel_match.group(1) velocity = [float(x) for x in velocity_str.split(',')] vx, vy, vz = velocity # 設置type_mask - 僅使用速度控制 type_mask = ( mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE | mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE ) # 直接發送速度指令，不需等待位置數據 self.connection.mav.set_position_target_local_ned_send( 0, self.connection.target_system, self.connection.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, type_mask, 0, 0, 0, # 位置 vx, vy, -vz, # 速度 0, 0, 0, # 加速度 0, 0 # 航向 ) print(f"Drone {self.instance_id}: 發送速度指令: vx={vx}, vy={vy}, vz={-vz}") except json.JSONDecodeError as je: print(f"Drone {self.instance_id}: JSON解析錯誤: {je}") self.log_packet({"raw_data": decoded_data}, total_packets) except Exception as e: print(f"Drone {self.instance_id}: 處理錯誤: {e}") # 清理資源 server_socket.close() print(f"Drone {self.instance_id}: 線程結束") def main(): # 解析命令行參數 parser = argparse.ArgumentParser(description='多無人機控制程序') parser.add_argument('--drones', type=str, default='1,2,3', help='要控制的無人機ID，用逗號分隔 (例如: 1,2,3)') args = parser.parse_args() # 所有可用的無人機配置 all_drone_configs = [ (1, 14550, 5003), (2, 14560, 5013), (3, 14570, 5023) ] # 解析要使用的無人機ID drone_ids = [int(id_str) for id_str in args.drones.split(',')] drone_configs = [config for config in all_drone_configs if config[0] in drone_ids] drones = [] threads = [] try: # 創建所有無人機實例 for instance_id, mavlink_port, udp_port in drone_configs: drone = Drone(instance_id, mavlink_port, udp_port) drones.append(drone) # 步驟1: 所有無人機連接 for drone in drones: drone.connect() # 適當延遲確保連接穩定 time.sleep(1) # 步驟2: 所有無人機同時切換到GUIDED模式 for drone in drones: drone.set_guided_mode() time.sleep(1) # 步驟3: 所有無人機同時解鎖 for drone in drones: drone.arm() time.sleep(1) # 步驟4: 所有無人機同時起飛 for drone in drones: drone.takeoff() # 啟動監聽線程 for drone in drones: thread = threading.Thread(target=drone.run) thread.daemon = True threads.append(thread) thread.start() print(f"已啟動 {len(drones)} 台無人機 (ID: {args.drones})，按Ctrl+C結束程序") # 主線程事件循環 while True: select([], [], [], 0.1) # 使用select代替time.sleep except KeyboardInterrupt: print("正在停止所有無人機...") for drone in drones: drone.running = False for thread in threads: thread.join(timeout=2) print("程序已結束") if __name__ == "__main__": main()