chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

1. 大量添增終端機介面功能

Browse Source 
2. 優化 mavlink_object 與 manager 的流程
This commit is contained in:
chiyu1468
2025-12-01 14:53:55 +08:00
parent 7af138b02a
commit 6a71e4530f
7 changed files with 2263 additions and 1016 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_adapter/tests/demo_integration.py
+33 -223
View File
@@ -14,12 +14,13 @@ from pymavlink import mavutil
# 自定義的 import
from ..fc_network_adapter import mavlinkObject as mo
from ..fc_network_adapter import mavlinkDevice as md
from ..fc_network_adapter import mavlinkVehicleView as mvv
# from ..fc_network_adapter import mavlinkDevice as md
# ====================== 分割線 =====================
test_item = 10
running_time = 10000
running_time = 3
print('test_item : ', test_item)
@@ -44,10 +45,13 @@ if test_item == 10:
time.sleep(0.5) # 等待事件循環啟動
# 初始化輸入通道
connection_string="udp:127.0.0.1:14560"
connection_string="udp:127.0..1:14571"
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
sock = mavlink_socket1.port
print("Socket port:", sock)
manager.add_mavlink_object(mavlink_object1)
start_time = time.time()
@@ -65,7 +69,7 @@ if test_item == 10:
print("return_packet_ring is empty")
time.sleep(1)
manager.stop()
manager.shutdown()
print('<=== End of Program')
@@ -83,20 +87,22 @@ elif test_item == 11:
time.sleep(0.5) # 等待事件循環啟動
# 初始化輸入通道
connection_string="udp:127.0.0.1:14560"
connection_string="udp:127.0.0.1:14571"
mavlink_socket1 = mavutil.mavlink_connection(connection_string)
mavlink_object1 = mo.mavlink_object(mavlink_socket1)
manager.add_mavlink_object(mavlink_object1)
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
bridge = mo.mavlink_bridge()
bridge.start()
time.sleep(3)
# 印出目前所有 mavlink_systems 的內容
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
all_vehicles = mvv.vehicle_registry.get_all()
for sysid, vehicle in all_vehicles.items():
print(f" System {sysid}: {vehicle}")
start_time = time.time()
show_time = time.time()
@@ -105,14 +111,15 @@ elif test_item == 11:
# print("mark B")
show_time = time.time()
for sysid in analyzer.mavlink_systems:
for compid in analyzer.mavlink_systems[sysid].components:
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
for sysid, vehicle in all_vehicles.items():
for compid in vehicle.components:
comp = vehicle.get_component(compid)
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, comp.packet_stats.received_count))
comp.reset_packet_stats()
print("===================")
manager.stop()
analyzer.stop()
manager.shutdown()
bridge.stop()
print('<=== End of Program')
@@ -130,16 +137,16 @@ elif test_item == 12:
time.sleep(0.5) # 等待事件循環啟動
# 初始化輸入通道
connection_string="udp:127.0.0.1:14560"
connection_string="udp:127.0.0.1:14571"
mavlink_socket_in1 = mavutil.mavlink_connection(connection_string)
mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1)
connection_string="udp:127.0.0.1:14561"
connection_string="udp:127.0.0.1:14571"
mavlink_socket_in2 = mavutil.mavlink_connection(connection_string)
mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2)
# 初始化輸出通道
connection_string="udpout:127.0.0.1:14550"
connection_string="udpout:127.0.0.1:14551"
mavlink_socket_out = mavutil.mavlink_connection(connection_string)
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
@@ -160,66 +167,11 @@ elif test_item == 12:
time.sleep(1)
manager.stop()
manager.shutdown()
print('<=== End of Program')
# elif test_item == 20:
# # 這邊測試 node 生成 topic 的功能
# # 利用 空的通道 發出假的 heartbeat 封包
# print('===> Start of Program .Test ', test_item)
# rclpy.init() # 注意要初始化 rclpy 才能使用 node
# from pymavlink.dialects.v20 import common as mavlink2
# sysid = 1
# compid = 1
# # 啟動 mavlink_bridge
# analyzer = mo.mavlink_bridge()
# mav = mavlink2.MAVLink(None)
# mav.srcSystem = sysid
# mav.srcComponent = compid
# # 這是一個 heartbeat 封包
# fake_heartbeat = mavlink2.MAVLink_heartbeat_message(
# type=mavlink2.MAV_TYPE_QUADROTOR,
# autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA,
# base_mode=3,
# custom_mode=0,
# system_status=0,
# mavlink_version=3
# )
# mavlink_bytes = fake_heartbeat.pack(mav)
# fake_heartbeat_msg = mav.parse_char(mavlink_bytes)
# print(analyzer.mavlink_systems)
# mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data
# time.sleep(0.1)
# print(analyzer.mavlink_systems)
# # ROS2 初始化
# analyzer._init_node()
# # 創建 ROS2 topic
# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid
# print("topic created")
# time.sleep(5)
# # 丟出 topic
# analyzer.emit_info()
# # 結束程式
# analyzer.running = False
# analyzer.destroy_node()
# rclpy.shutdown()
# analyzer.stop()
# analyzer.thread.join()
# print('<=== End of Program')
elif test_item == 21:
# 需要開啟一個 ardupilot 的模擬器
@@ -291,160 +243,18 @@ elif test_item == 21:
mavlink_socket.close()
print('<=== End of Program')
# elif test_item == 22:
# # 需要開啟一個 ardupilot 的模擬器 與 GCS
# # 這邊是測試代碼 引入 rclpy 來測試 node 的運行
# print('===> Start of Program .Test ', test_item)
# rclpy.init() # 注意要初始化 rclpy 才能使用 node
# # 啟動 mavlink_bridge
# analyzer = mo.mavlink_bridge()
# # 關於 Node 的初始化
# show_time = time.time()
# analyzer._init_node() # 初始化 node
# print('初始化 node 完成 耗時 : ',time.time() - show_time)
elif test_item == 52:
print('===> Start of Program .Test ', test_item)
# # 初始化兩個通道
# connection_string_in="udp:127.0.0.1:15551"
# mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
# mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
# mavlink_object_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
manager = mo.async_io_manager()
manager.start()
# print(manager.thread.is_alive())
# connection_string_out="udpout:127.0.0.1:14553"
# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out)
# mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
# mavlink_object_out.multiplexingToAnalysis = []
manager.shutdown()
# # 讓兩個通道互相傳輸
# mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all
# mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all
# # 啟動通道
# mavlink_object_in.run()
# mavlink_object_out.run()
# print('waiting for mavlink data ...')
# time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
time.sleep(1)
# print('目前所有的系統 : ')
# for sysid in analyzer.mavlink_systems:
# print(analyzer.mavlink_systems[sysid])
print('manager stopped')
# compid = 1
# sysid = 1
# show_time = time.time()
# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
# print(f"Execution time for create_flightMode: {time.time() - show_time} seconds")
# print("start emit info")
# start_time = time.time()
# show_time = time.time()
# show_time2 = time.time()
# while time.time() - start_time < running_time:
# if (time.time() - show_time2) >= 1:
# analyzer.emit_info() # 這邊是測試 node 的運行
# # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode']
# fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode']
# sss = mavutil.mode_string_v10(fmsg)
# # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq()))
# print("目前的飛行模式 : {}".format(sss))
# show_time2 = time.time()
# # if (time.time() - show_time) >= 2:
# # show_time = time.time()
# # for sysid in analyzer.mavlink_systems:
# # for compid in analyzer.mavlink_systems[sysid].components:
# # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
# # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
# # print("===================")
# analyzer.destroy_node()
# rclpy.shutdown()
# # 結束程式 退出所有 thread
# mavlink_object_in.stop()
# mavlink_object_in.thread.join()
# mavlink_socket_in.close()
# mavlink_object_out.stop()
# mavlink_object_out.thread.join()
# mavlink_socket_out.close()
# analyzer.stop()
# analyzer.thread.join()
# print('<=== End of Program')
# elif test_item == 51:
# # 晉凱的測試項目
# print('===> Start of Program .Test ', test_item)
# rclpy.init() # 注意要初始化 rclpy 才能使用 node
# # 啟動 mavlink_bridge
# analyzer = mo.mavlink_bridge()
# # 關於 Node 的初始化
# show_time = time.time()
# analyzer._init_node() # 初始化 node
# print('初始化 node 完成 耗時 : ',time.time() - show_time)
# # 創建通道
# connection_string="udp:127.0.0.1:15551"
# mavlink_socket3 = mavutil.mavlink_connection(connection_string)
# mavlink_object3 = mo.mavlink_object(mavlink_socket3)
# # 設定通道流動
# mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
# mavlink_object3.multiplexingToReturn = [] #
# # mavlink_object3.multiplexingToSwap = [] #
# # 啟動通道
# mavlink_object3.run()
# print('waiting for mavlink data ...')
# time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
# compid = 1
# sysid = 1
# start_time = time.time()
# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
# analyzer.create_attitude(sysid, analyzer.mavlink_systems[sysid].components[compid])
# analyzer.create_local_position_pose(sysid, analyzer.mavlink_systems[sysid].components[compid])
# analyzer.create_local_position_velocity(sysid, analyzer.mavlink_systems[sysid].components[compid])
# analyzer.create_global_global(sysid, analyzer.mavlink_systems[sysid].components[compid])
# analyzer.create_vfr_hud(sysid, analyzer.mavlink_systems[sysid].components[compid])
# analyzer.create_battery(sysid, analyzer.mavlink_systems[sysid].components[compid])
# analyzer.create_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid])
# end_time = time.time()
# print(f"Execution time for create all topic: {end_time - start_time} seconds")
# print("start emit info")
# start_time = time.time()
# show_time = time.time()
# while time.time() - start_time < running_time:
# try:
# # rclpy.spin(analyzer)
# analyzer.emit_info() # 這邊是測試 node 的運行
# time.sleep(0.5)
# except KeyboardInterrupt:
# break
# analyzer.destroy_node()
# rclpy.shutdown()
# # 結束程式 退出所有 thread
# mavlink_object3.stop()
# mavlink_object3.thread.join()
# analyzer.stop()
# analyzer.thread.join()
# mavlink_socket3.close()
# print('<=== End of Program')
src/fc_network_adapter/tests/demo_mavlinkVehicleView.py
+331
View File
@@ -0,0 +1,331 @@
"""
VehicleView 使用範例
展示如何使用純狀態容器來管理 MAVLink 載具資訊
"""
import time
from ..fc_network_adapter.mavlinkVehicleView import (
VehicleView,
VehicleComponent,
RFModule,
vehicle_registry,
ConnectionType,
ComponentType,
RFModuleType
)
def example_basic_usage():
"""基本使用範例"""
print("=== 基本使用範例 ===\n")
# 1. 建立載具視圖
vehicle = VehicleView(sysid=1)
vehicle.kind = "Copter"
vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR
vehicle.connected_via = ConnectionType.UDP
print(f"建立載具: {vehicle}\n")
# 2. 新增 autopilot 組件
autopilot = vehicle.add_component(
component_id=1,
comp_type=ComponentType.AUTOPILOT
)
autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR
autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA
print(f"新增組件: {autopilot}\n")
# 3. 手動餵入位置資訊
autopilot.status.position.latitude = 25.0330
autopilot.status.position.longitude = 121.5654
autopilot.status.position.altitude = 100.5
autopilot.status.position.timestamp = time.time()
print(f"位置: 緯度={autopilot.status.position.latitude}, "
f"經度={autopilot.status.position.longitude}, "
f"高度={autopilot.status.position.altitude}m\n")
# 4. 手動餵入姿態資訊
autopilot.status.attitude.roll = 0.05 # 弧度
autopilot.status.attitude.pitch = -0.02
autopilot.status.attitude.yaw = 1.57
autopilot.status.attitude.timestamp = time.time()
print(f"姿態: Roll={autopilot.status.attitude.roll:.3f}, "
f"Pitch={autopilot.status.attitude.pitch:.3f}, "
f"Yaw={autopilot.status.attitude.yaw:.3f} rad\n")
# 5. 手動餵入飛行模式
autopilot.status.mode.base_mode = 89
autopilot.status.mode.custom_mode = 4
autopilot.status.mode.mode_name = "GUIDED"
autopilot.status.mode.timestamp = time.time()
print(f"飛行模式: {autopilot.status.mode.mode_name}\n")
# 6. 手動餵入電池資訊
autopilot.status.battery.voltage = 12.6
autopilot.status.battery.current = 15.2
autopilot.status.battery.remaining = 75
autopilot.status.battery.timestamp = time.time()
print(f"電池: 電壓={autopilot.status.battery.voltage}V, "
f"電流={autopilot.status.battery.current}A, "
f"剩餘={autopilot.status.battery.remaining}%\n")
def example_packet_tracking():
"""封包追蹤範例"""
print("\n=== 封包追蹤範例 ===\n")
vehicle = VehicleView(sysid=2)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
# 模擬接收封包
timestamp = time.time()
# 接收 HEARTBEAT (msg_type=0)
autopilot.update_packet_stats(seq=0, msg_type=0, timestamp=timestamp)
# 接收 ATTITUDE (msg_type=30)
autopilot.update_packet_stats(seq=1, msg_type=30, timestamp=timestamp+0.1)
# 接收 GLOBAL_POSITION_INT (msg_type=33)
autopilot.update_packet_stats(seq=2, msg_type=33, timestamp=timestamp+0.2)
# 模擬封包遺失 (seq 跳過 3, 4, 5)
autopilot.update_packet_stats(seq=6, msg_type=0, timestamp=timestamp+0.3)
stats = autopilot.packet_stats
print(f"封包統計:")
print(f" 接收: {stats.received_count}")
print(f" 遺失: {stats.lost_count}")
print(f" 最後序號: {stats.last_seq}")
print(f" 訊息類型計數: {stats.msg_type_count}\n")
def example_parameters():
"""參數管理範例"""
print("\n=== 參數管理範例 ===\n")
vehicle = VehicleView(sysid=3)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
# 手動設定參數 (不會主動下載)
autopilot.set_parameter("ARMING_CHECK", 1)
autopilot.set_parameter("ANGLE_MAX", 4500)
autopilot.set_parameter("WPNAV_SPEED", 500)
print(f"參數數量: {len(autopilot.parameters)}")
print(f"ARMING_CHECK = {autopilot.get_parameter('ARMING_CHECK')}")
print(f"ANGLE_MAX = {autopilot.get_parameter('ANGLE_MAX')}")
print(f"WPNAV_SPEED = {autopilot.get_parameter('WPNAV_SPEED')}\n")
def example_rf_module():
"""RF模組範例"""
print("\n=== RF模組範例 ===\n")
vehicle = VehicleView(sysid=4)
vehicle.connected_via = ConnectionType.SERIAL
# 設定 XBee RF 模組
rf = vehicle.set_rf_module(RFModuleType.XBEE)
# 更新 Socket 資訊
rf.update_socket_info(
ip="192.168.1.100",
port=14550,
local_ip="192.168.1.1",
local_port=14551,
connected=True
)
# 更新 RSSI
rf.update_rssi(rssi=-65, timestamp=time.time())
# 更新 AT 命令回應
rf.update_at_response("OK", timestamp=time.time())
# 自定義狀態
rf.status.custom_status['signal_quality'] = 'excellent'
rf.status.custom_status['packet_error_rate'] = 0.001
print(f"RF模組: {rf}")
print(f"Socket: {rf.socket_info.ip}:{rf.socket_info.port}")
print(f"RSSI: {rf.status.rssi} dBm")
print(f"AT回應: {rf.status.at_response}")
print(f"自定義狀態: {rf.status.custom_status}\n")
def example_multiple_components():
"""多組件範例"""
print("\n=== 多組件範例 ===\n")
vehicle = VehicleView(sysid=5)
vehicle.kind = "Copter with Gimbal"
# Autopilot 組件
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
autopilot.mav_type = 2
autopilot.status.mode.mode_name = "AUTO"
# Gimbal 組件
gimbal = vehicle.add_component(154, ComponentType.GIMBAL)
gimbal.mav_type = 26 # MAV_TYPE_GIMBAL
gimbal.status.attitude.pitch = -0.785 # 向下45度
gimbal.status.attitude.yaw = 0.0
# Camera 組件
camera = vehicle.add_component(100, ComponentType.CAMERA)
camera.mav_type = 30 # MAV_TYPE_CAMERA
camera.status.custom_status['recording'] = True
camera.status.custom_status['photo_interval'] = 2.0
print(f"載具: {vehicle}")
print(f"組件數量: {len(vehicle.components)}")
for cid, comp in vehicle.components.items():
print(f" 組件 {cid}: {comp.type.value}, MAV_TYPE={comp.mav_type}")
print()
def example_registry():
"""註冊表使用範例"""
print("\n=== 註冊表使用範例 ===\n")
# 註冊多個載具
v1 = vehicle_registry.register(sysid=1)
v1.kind = "Copter-1"
v1.add_component(1, ComponentType.AUTOPILOT)
v2 = vehicle_registry.register(sysid=2)
v2.kind = "Plane-1"
v2.add_component(1, ComponentType.AUTOPILOT)
v3 = vehicle_registry.register(sysid=3)
v3.kind = "Rover-1"
v3.add_component(1, ComponentType.AUTOPILOT)
print(f"註冊表中的載具數量: {len(vehicle_registry)}")
# 取得所有載具
all_vehicles = vehicle_registry.get_all()
for sysid, vehicle in all_vehicles.items():
print(f" System {sysid}: {vehicle.kind}")
# 檢查載具是否存在
print(f"\nSystem 2 存在? {2 in vehicle_registry}")
print(f"System 99 存在? {99 in vehicle_registry}")
# 取得特定載具
vehicle = vehicle_registry.get(2)
if vehicle:
print(f"\n取得載具: {vehicle}")
# 註銷載具
vehicle_registry.unregister(3)
print(f"\n註銷 System 3 後，剩餘載具: {len(vehicle_registry)}\n")
def example_serialization():
"""序列化範例 (除錯/日誌用)"""
print("\n=== 序列化範例 ===\n")
vehicle = VehicleView(sysid=10)
vehicle.kind = "Test Copter"
vehicle.connected_via = ConnectionType.UDP
vehicle.custom_meta['firmware'] = 'ArduCopter 4.3.0'
vehicle.custom_meta['frame_type'] = 'X'
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
autopilot.mav_type = 2
autopilot.status.position.altitude = 50.0
autopilot.status.battery.voltage = 12.4
autopilot.update_packet_stats(0, 0, time.time())
autopilot.update_packet_stats(1, 30, time.time())
rf = vehicle.set_rf_module(RFModuleType.UDP)
rf.update_rssi(-70)
rf.update_socket_info(ip="192.168.1.200", port=14550, connected=True)
# 轉換為字典
data = vehicle.to_dict()
print("載具資料 (字典格式):")
import json
print(json.dumps(data, indent=2, ensure_ascii=False))
def example_gps_ekf():
"""GPS 與 EKF 範例"""
print("\n\n=== GPS 與 EKF 範例 ===\n")
vehicle = VehicleView(sysid=11)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
# GPS 資訊
autopilot.status.gps.fix_type = 3 # 3D Fix
autopilot.status.gps.satellites_visible = 12
autopilot.status.gps.eph = 120 # HDOP = 1.2
autopilot.status.gps.epv = 180 # VDOP = 1.8
autopilot.status.gps.timestamp = time.time()
print(f"GPS:")
print(f" Fix Type: {autopilot.status.gps.fix_type}")
print(f" 衛星數: {autopilot.status.gps.satellites_visible}")
print(f" HDOP: {autopilot.status.gps.eph/100}")
# EKF 資訊
autopilot.status.ekf.flags = 0x1FF # 所有 flags 都 OK
autopilot.status.ekf.velocity_variance = 0.5
autopilot.status.ekf.pos_horiz_variance = 1.2
autopilot.status.ekf.pos_vert_variance = 2.0
autopilot.status.ekf.timestamp = time.time()
print(f"\nEKF:")
print(f" Flags: 0x{autopilot.status.ekf.flags:X}")
print(f" 速度變異: {autopilot.status.ekf.velocity_variance}")
print(f" 水平位置變異: {autopilot.status.ekf.pos_horiz_variance}")
print(f" 垂直位置變異: {autopilot.status.ekf.pos_vert_variance}\n")
def example_vfr_hud():
"""VFR HUD 範例"""
print("\n=== VFR HUD 範例 ===\n")
vehicle = VehicleView(sysid=12)
autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT)
# VFR HUD 資訊
autopilot.status.vfr.airspeed = 15.5 # m/s
autopilot.status.vfr.groundspeed = 14.8 # m/s
autopilot.status.vfr.heading = 90 # 東方
autopilot.status.vfr.throttle = 65 # %
autopilot.status.vfr.climb = 2.5 # m/s
autopilot.status.vfr.timestamp = time.time()
print(f"VFR HUD:")
print(f" 空速: {autopilot.status.vfr.airspeed} m/s")
print(f" 地速: {autopilot.status.vfr.groundspeed} m/s")
print(f" 航向: {autopilot.status.vfr.heading}°")
print(f" 油門: {autopilot.status.vfr.throttle}%")
print(f" 爬升率: {autopilot.status.vfr.climb} m/s\n")
if __name__ == "__main__":
# 執行所有範例
# example_basic_usage()
# example_packet_tracking()
# example_parameters()
# example_rf_module()
# example_multiple_components()
# example_registry()
# example_serialization()
# example_gps_ekf()
example_vfr_hud()
print("\n" + "="*50)
print("所有範例執行完成！")
print("="*50)
src/fc_network_adapter/tests/test_mavlinkObject.py
+253 -254
View File
@@ -13,7 +13,6 @@ import time
import threading
import socket
import asyncio
from unittest.mock import MagicMock, patch
# 導入要測試的模組
from ..fc_network_adapter.mavlinkObject import (
@@ -24,62 +23,100 @@ from ..fc_network_adapter.mavlinkObject import (
return_packet_ring
)
# 預先定義好的真實 MAVLink heartbeat 封包 (MAVLink 1.0 格式)
# Format: STX(0xFE) + LEN + SEQ + SYS + COMP + MSG_ID + PAYLOAD(9 bytes for heartbeat) + CRC(2 bytes)
HEARTBEAT_PACKET_1 = bytes([
0xFE, # STX (MAVLink 1.0)
0x09, # payload length (9 bytes)
0x00, # sequence
0x01, # system ID = 1
0x01, # component ID = 1
0x00, # message ID (HEARTBEAT = 0)
# Payload (9 bytes): custom_mode(4), type(1), autopilot(1), base_mode(1), system_status(1), mavlink_version(1)
0x00, 0x00, 0x00, 0x00, # custom_mode = 0
0x02, # type = MAV_TYPE_QUADROTOR (2)
0x03, # autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA (3)
0x40, # base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (64)
0x03, # system_status = MAV_STATE_STANDBY (3)
0x03, # mavlink_version = 3
0x62, 0x8E # CRC (simplified placeholder)
])
HEARTBEAT_PACKET_2 = bytes([
0xFE, # STX
0x09, # payload length
0x01, # sequence (增加)
0x01, # system ID = 1
0x01, # component ID = 1
0x00, # message ID (HEARTBEAT = 0)
0x00, 0x00, 0x00, 0x00,
0x02, 0x03, 0x41, 0x03, 0x03,
0x33, 0xEC
])
HEARTBEAT_PACKET_3 = bytes([
0xFE, # STX
0x09, # payload length
0x02, # sequence
0x02, # system ID = 2
0x01, # component ID = 1
0x00, # message ID (HEARTBEAT = 0)
0x00, 0x00, 0x00, 0x00,
0x02, 0x03, 0x42, 0x03, 0x03,
0x37, 0x44
])
class MockMavlinkSocket:
"""模擬 Mavlink Socket 的類別，用於測試"""
"""模擬 Mavlink Socket 的類別，用於測試
使用真實的 MAVLink 封包，而不是模擬的訊息對象
"""
def __init__(self, test_data=None):
def __init__(self, test_packets=None):
"""
Args:
test_packets: list of bytes，每個元素都是完整的 MAVLink 封包
"""
self.closed = False
self.test_data = test_data or []
self.data_index = 0
self.test_packets = test_packets or []
self.packet_index = 0
self.written_data = []
# 使用 pymavlink 來解析封包
from pymavlink import mavutil
self.mav_parser = mavutil.mavlink.MAVLink(self)
def recv_msg(self):
if not self.test_data or self.data_index >= len(self.test_data):
"""返回解析後的 MAVLink 訊息對象"""
if not self.test_packets or self.packet_index >= len(self.test_packets):
return None
data = self.test_data[self.data_index]
self.data_index += 1
return data
packet = self.test_packets[self.packet_index]
self.packet_index += 1
# 使用 pymavlink 解析封包
try:
for byte in packet:
msg = self.mav_parser.parse_char(bytes([byte]))
if msg:
return msg
except Exception as e:
print(f"Error parsing packet: {e}")
return None
return None
def write(self, data):
"""寫入數據（用於檢查轉發）"""
self.written_data.append(data)
def close(self):
"""關閉 socket"""
self.closed = True
class MockMavlinkMessage:
"""模擬 Mavlink 訊息的類別，用於測試"""
def __init__(self, msg_id, sysid, compid, seq=0):
self.msg_id = msg_id
self.sysid = sysid
self.compid = compid
self.seq = seq
self.msg_buf = bytes([msg_id, sysid, compid, seq])
def get_msgId(self):
return self.msg_id
def get_srcSystem(self):
return self.sysid
def get_srcComponent(self):
return self.compid
def get_seq(self):
return self.seq
def get_msgbuf(self):
return self.msg_buf
def get_type(self):
return f"MSG_ID_{self.msg_id}"
class TestMavlinkObject(unittest.TestCase):
"""測試 mavlink_object 類別"""
"""測試 mavlink_object 類別的獨立功能"""
def setUp(self):
"""在每個測試方法執行前準備環境"""
@@ -91,35 +128,27 @@ class TestMavlinkObject(unittest.TestCase):
stream_bridge_ring.clear()
return_packet_ring.clear()
# 創建測試訊息
self.heartbeat_msg = MockMavlinkMessage(msg_id=0, sysid=1, compid=1)
self.attitude_msg = MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
self.position_msg = MockMavlinkMessage(msg_id=32, sysid=1, compid=1)
# 創建模擬的 socket # 假的 Mavlink Socket
self.mock_socket = MockMavlinkSocket([
self.heartbeat_msg,
self.attitude_msg,
self.position_msg
])
# 創建模擬的 socket，使用真實封包
self.mock_socket = MockMavlinkSocket([HEARTBEAT_PACKET_1])
# 創建測試對象
self.mavlink_obj = mavlink_object(self.mock_socket)
def test_initialization(self):
"""測試 mavlink_object 初始化是否正確"""
# print("Testing mavlink_object initialization")
self.assertEqual(self.mavlink_obj.socket_id, 0)
self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT)
self.assertEqual(len(self.mavlink_obj.target_sockets), 0)
self.assertEqual(self.mavlink_obj.bridge_msg_types, [0])
self.assertEqual(self.mavlink_obj.return_msg_types, [])
def test_add_remove_target_socket(self):
"""測試添加和移除目標端口功能"""
# 添加目標端口
self.assertTrue(self.mavlink_obj.add_target_socket(1))
self.assertEqual(len(self.mavlink_obj.target_sockets), 1)
self.assertEqual(self.mavlink_obj.target_sockets[0], 1)
self.assertTrue(self.mavlink_obj.add_target_socket(2))
self.assertEqual(len(self.mavlink_obj.target_sockets), 2)
self.assertIn(2, self.mavlink_obj.target_sockets)
@@ -153,78 +182,31 @@ class TestMavlinkObject(unittest.TestCase):
self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid"))
self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"]))
def test_send_message(self):
"""測試 send_message 功能"""
# 創建一個新的 mavlink_object 實例
mock_socket = MockMavlinkSocket()
mavlink_obj = mavlink_object(mock_socket)
# 準備測試數據
test_message1 = b"test_message_1"
test_message2 = b"test_message_2"
# 測試初始狀態
self.assertEqual(len(mock_socket.written_data), 0)
def test_send_message_validation(self):
"""測試 send_message 的數據驗證功能（不需要啟動 manager）"""
# 測試非運行狀態下發送消息
self.assertFalse(mavlink_obj.send_message(test_message1))
self.assertEqual(len(mock_socket.written_data), 0)
self.assertFalse(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1))
# 啟動 manager
manager = async_io_manager()
manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 測試無效的數據類型
self.mavlink_obj.state = MavlinkObjectState.RUNNING # 臨時設置狀態
self.assertFalse(self.mavlink_obj.send_message("invalid"))
self.assertFalse(self.mavlink_obj.send_message(123))
# 添加對象到 manager
manager.add_mavlink_object(mavlink_obj)
time.sleep(0.1) # 等待對象啟動
# 測試太短的封包
self.assertFalse(self.mavlink_obj.send_message(bytes([0xFE, 0x00])))
# 確認對象狀態
self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING)
# 測試無效的起始標記
invalid_packet = bytes([0xFF, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00])
self.assertFalse(self.mavlink_obj.send_message(invalid_packet))
# 測試發送消息
self.assertTrue(mavlink_obj.send_message(test_message1))
time.sleep(0.2) # 等待消息處理
# 測試有效的封包可以加入佇列
self.assertTrue(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1))
self.assertEqual(len(self.mavlink_obj.outgoing_msgs), 1)
# 確認消息已發送
self.assertEqual(len(mock_socket.written_data), 1)
self.assertEqual(mock_socket.written_data[0], test_message1)
# 測試連續發送多條消息
self.assertTrue(mavlink_obj.send_message(test_message2))
time.sleep(0.2) # 等待消息處理
# 確認兩條消息都已發送
self.assertEqual(len(mock_socket.written_data), 2)
self.assertEqual(mock_socket.written_data[1], test_message2)
# 模擬發送出錯的情況
class ErrorWriteSocket(MockMavlinkSocket):
def write(self, data):
raise Exception("Write error")
error_socket = ErrorWriteSocket()
error_obj = mavlink_object(error_socket)
manager.add_mavlink_object(error_obj)
time.sleep(0.1) # 等待對象啟動
# 發送消息到錯誤的 socket
self.assertTrue(error_obj.send_message(test_message1))
time.sleep(0.2) # 等待消息處理
# 即使寫入失敗，send_message 應該也返回 True
# 因為消息已成功加入到佇列中，只是後續的實際發送失敗
# 停止 manager
manager.stop()
time.sleep(0.5) # 等待 manager 停止
# 測試對象已關閉後發送消息
self.assertFalse(mavlink_obj.send_message(test_message1))
self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加
self.mavlink_obj.state = MavlinkObjectState.INIT # 恢復狀態
class TestAsyncIOManager(unittest.TestCase):
"""測試 async_io_manager 類別"""
"""測試 async_io_manager 類別的獨立功能"""
def setUp(self):
"""在每個測試方法執行前準備環境"""
@@ -239,14 +221,9 @@ class TestAsyncIOManager(unittest.TestCase):
# 創建 async_io_manager 實例
self.manager = async_io_manager()
# 模擬 mavlink 對象
self.mock_socket1 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=1, compid=1),
MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
])
self.mock_socket2 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=2, compid=1)
])
# 創建模擬 mavlink 對象，使用真實封包
self.mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2])
self.mock_socket2 = MockMavlinkSocket([HEARTBEAT_PACKET_3])
self.mavlink_obj1 = mavlink_object(self.mock_socket1)
self.mavlink_obj2 = mavlink_object(self.mock_socket2)
@@ -254,10 +231,10 @@ class TestAsyncIOManager(unittest.TestCase):
def tearDown(self):
"""在每個測試方法執行後清理環境"""
if self.manager.running:
self.manager.stop()
self.manager.shutdown()
def test_singleton_pattern(self):
"""測試 async_io_manager 的單例模式 就是不會產生兩個 magager"""
"""測試 async_io_manager 的單例模式"""
manager1 = async_io_manager()
manager2 = async_io_manager()
self.assertIs(manager1, manager2)
@@ -275,7 +252,7 @@ class TestAsyncIOManager(unittest.TestCase):
self.assertIs(self.manager.thread, old_thread)
# 停止管理器
self.manager.stop()
self.manager.shutdown()
self.assertFalse(self.manager.running)
# 最多等待 5 秒讓線程結束
@@ -313,157 +290,179 @@ class TestAsyncIOManager(unittest.TestCase):
self.assertFalse(self.manager.remove_mavlink_object(999))
# 停止管理器
self.manager.stop()
self.manager.shutdown()
def test_data_processing(self):
"""測試數據處理"""
class TestIntegration(unittest.TestCase):
"""整合測試，測試多個 mavlink_object 之間的互動與資料流"""
def setUp(self):
"""在每個測試方法執行前準備環境"""
# 清空全局變數
mavlink_object.mavlinkObjects = {}
mavlink_object.socket_num = 0
# 清空 ring buffer
stream_bridge_ring.clear()
return_packet_ring.clear()
# 創建 async_io_manager 實例
self.manager = async_io_manager()
def tearDown(self):
"""在每個測試方法執行後清理環境"""
if self.manager.running:
self.manager.shutdown()
def test_send_message_with_manager(self):
"""測試透過 async_io_manager 發送訊息的完整流程"""
# 創建一個新的 mavlink_object 實例
mock_socket = MockMavlinkSocket()
mavlink_obj = mavlink_object(mock_socket)
# 測試初始狀態
self.assertEqual(len(mock_socket.written_data), 0)
# 測試非運行狀態下發送消息
self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
self.assertEqual(len(mock_socket.written_data), 0)
# 啟動 manager
self.manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 添加對象到 manager
self.manager.add_mavlink_object(mavlink_obj)
time.sleep(0.1) # 等待對象啟動
# 確認對象狀態
self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING)
# 測試發送消息
self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
time.sleep(0.2) # 等待消息處理
# 確認消息已發送
self.assertEqual(len(mock_socket.written_data), 1)
self.assertEqual(mock_socket.written_data[0], HEARTBEAT_PACKET_1)
# 測試連續發送多條消息
self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_2))
time.sleep(0.2) # 等待消息處理
# 確認兩條消息都已發送
self.assertEqual(len(mock_socket.written_data), 2)
self.assertEqual(mock_socket.written_data[1], HEARTBEAT_PACKET_2)
# 停止 manager
self.manager.shutdown()
time.sleep(0.5) # 等待 manager 停止
# 測試對象已關閉後發送消息
self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1))
self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加
def test_data_processing_and_forwarding(self):
"""測試數據處理與轉發流程"""
# 創建用於轉發的 mavlink_objects
mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2,])
mock_socket3 = MockMavlinkSocket()
mavlink_obj1 = mavlink_object(mock_socket1)
mavlink_obj3 = mavlink_object(mock_socket3)
# 設置轉發
self.mavlink_obj1.add_target_socket(2) # socket1 轉發到 socket3
# 設置訊息類型
self.mavlink_obj1.set_bridge_message_types([0, 30]) # HEARTBEAT, ATTITUDE
# self.mavlink_obj1.enable_return(True)
self.mavlink_obj1.set_return_message_types([30]) # ATTITUDE
mavlink_obj1.set_bridge_message_types([0]) # 只處理 HEARTBEAT
# 設置轉發: obj1 -> obj3
mavlink_obj1.add_target_socket(mavlink_obj3.socket_id) # socket1 轉發到 socket3 (socket_id=1)
# 啟動管理器並添加對象
self.manager.start()
time.sleep(0.5) # 等待事件循環啟動
self.manager.add_mavlink_object(self.mavlink_obj1)
"""
這邊出現很奇怪的狀況 應該說 設計時沒有考量 但是實測會發現
mavlink_obj3 是接收端 必需要被優先加入 manager 才能正確接收來自 mavlink_obj1 的轉發封包
若先把 mavlink_ojb1 加入 manger 則可能會導致前面幾個封包丟失
"""
self.manager.add_mavlink_object(mavlink_obj3)
self.manager.add_mavlink_object(mavlink_obj1)
# 等待處理完成
time.sleep(1.0)
time.sleep(0.5)
# print("testing Mark A")
# 檢查 Ring buffer 是否有正確的數據
self.assertEqual(stream_bridge_ring.size(), 2) # HEARTBEAT + ATTITUDE
self.assertEqual(return_packet_ring.size(), 1) # ATTITUDE
self.assertGreaterEqual(stream_bridge_ring.size(), 2) # 至少 2 個 HEARTBEAT
a = stream_bridge_ring.get()
print(f"stream_bridge_ring: {a}")
# 檢查是否正確轉發
self.assertEqual(len(mock_socket3.written_data), 2) # HEARTBEAT + ATTITUDE
# print("testing Mark B")
self.assertGreaterEqual(len(mock_socket3.written_data), 2) # 至少 2 個 HEARTBEAT
# 停止管理器
self.manager.stop()
self.manager.shutdown()
def test_bidirectional_forwarding(self):
"""測試雙向轉發"""
# 清空全局變數和 ring buffer
mavlink_object.mavlinkObjects = {}
mavlink_object.socket_num = 0
stream_bridge_ring.clear()
return_packet_ring.clear()
def test_error_handling(self):
"""測試錯誤處理情況"""
print("=== mark A ===")
# 創建一個會引發異常的 socket
class ErrorSocket(MockMavlinkSocket):
def recv_msg(self):
raise Exception("Test exception")
error_socket = ErrorSocket()
mavlink_obj_err = mavlink_object(error_socket)
# 啟動管理器並添加對象
# 創建三個 mavlink 對象，模擬三個通道
socket1 = MockMavlinkSocket()
socket2 = MockMavlinkSocket()
socket3 = MockMavlinkSocket()
obj1 = mavlink_object(socket1)
obj2 = mavlink_object(socket2)
obj3 = mavlink_object(socket3)
# 設置雙向轉發
# obj1 <-> obj2 <-> obj3
obj1.add_target_socket(1) # obj1 -> obj2
obj2.add_target_socket(0) # obj2 -> obj1
obj2.add_target_socket(2) # obj2 -> obj3
obj3.add_target_socket(1) # obj3 -> obj2
# 啟動 async_io_manager
self.manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 添加所有 mavlink_object
self.manager.add_mavlink_object(obj1)
self.manager.add_mavlink_object(obj2)
self.manager.add_mavlink_object(obj3)
self.manager.add_mavlink_object(mavlink_obj_err)
print("=== mark B ===")
# 等待錯誤處理
# 對三個對象添加數據
socket1.test_packets.append(HEARTBEAT_PACKET_1)
socket2.test_packets.append(HEARTBEAT_PACKET_2)
socket3.test_packets.append(HEARTBEAT_PACKET_3)
# 等待處理所有訊息
time.sleep(1.0)
# 對象應該進入錯誤狀態，但不會崩潰
# self.assertEqual(mavlink_obj_err.state, MavlinkObjectState.ERROR)
print("=== mark C ===")
# 管理器應該仍在運行
self.assertTrue(self.manager.running)
# 故意等一段時間 確認 socket 有被 manager 處理掉
time.sleep(3)
# 檢查轉發結果
# socket1 應該收到 socket2 的訊息
self.assertGreaterEqual(len(socket1.written_data), 1)
# socket2 應該收到 socket1 和 socket3 的訊息
self.assertGreaterEqual(len(socket2.written_data), 2)
# socket3 應該收到 socket2 的訊息
self.assertGreaterEqual(len(socket3.written_data), 1)
# 檢查 ring buffer 的數據
# 所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0]
self.assertGreaterEqual(stream_bridge_ring.size(), 3) # 至少 3 個 HEARTBEAT
# 停止管理器
self.manager.stop()
class TestIntegration(unittest.TestCase):
"""整合測試，測試多個 mavlink_object 之間的互動"""
def test_bidirectional_forwarding(self):
"""測試雙向轉發"""
# 清空全局變數和 ring buffer
mavlink_object.mavlinkObjects = {}
mavlink_object.socket_num = 0
stream_bridge_ring.clear()
return_packet_ring.clear()
# 創建三個 mavlink 對象，模擬三個通道
socket1 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=1, compid=1),
MockMavlinkMessage(msg_id=30, sysid=1, compid=1)
])
socket2 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=2, compid=1),
MockMavlinkMessage(msg_id=32, sysid=2, compid=1)
])
socket3 = MockMavlinkSocket([
MockMavlinkMessage(msg_id=0, sysid=3, compid=1),
MockMavlinkMessage(msg_id=33, sysid=3, compid=1)
])
obj1 = mavlink_object(socket1)
obj2 = mavlink_object(socket2)
obj3 = mavlink_object(socket3)
# 設置雙向轉發
# obj1 <-> obj2 <-> obj3
obj1.add_target_socket(1) # obj1 -> obj2
obj2.add_target_socket(0) # obj2 -> obj1
obj2.add_target_socket(2) # obj2 -> obj3
obj3.add_target_socket(1) # obj3 -> obj2
# 啟動 async_io_manager
manager = async_io_manager()
manager.start()
time.sleep(0.5) # 等待事件循環啟動
# 添加所有 mavlink_object
manager.add_mavlink_object(obj1)
manager.add_mavlink_object(obj2)
manager.add_mavlink_object(obj3)
# 等待處理所有訊息
time.sleep(1.5)
# 檢查轉發結果
# socket1 應該收到 socket2 和 socket3 的訊息
self.assertEqual(len(socket1.written_data), 4) # 2 from obj2, 2 from obj3 via obj2
# socket2 應該收到 socket1 和 socket3 的訊息
self.assertEqual(len(socket2.written_data), 4) # 2 from obj1, 2 from obj3
# socket3 應該收到 socket1 和 socket2 的訊息
self.assertEqual(len(socket3.written_data), 4) # 2 from obj1 via obj2, 2 from obj2
# 檢查 ring buffer 的數據
# 假設所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0]
# 應該有 3 個 HEARTBEAT 訊息
self.assertEqual(stream_bridge_ring.size(), 3) # 3 HEARTBEAT
# 停止管理器
manager.stop()
self.manager.shutdown()
if __name__ == "__main__":
unittest.main(defaultTest="TestMavlinkObject.test_send_message")
# unittest.main(defaultTest="TestAsyncIOManager")
# 可以指定要運行的測試
# unittest.main(defaultTest="TestMavlinkObject.test_send_message_validation")
# unittest.main(defaultTest="TestAsyncIOManager.test_add_remove_objects")
unittest.main(defaultTest="TestIntegration.test_bidirectional_forwarding")
unittest.main()