|
|
#!/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() |