parent
7af138b02a
commit
6a71e4530f
File diff suppressed because it is too large
Load Diff
@ -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)
|
||||
Loading…
Reference in New Issue