|
|
|
|
|
from xbee import XBee
|
|
|
|
|
|
import serial
|
|
|
|
|
|
from pymavlink import mavutil
|
|
|
|
|
|
import time
|
|
|
|
|
|
|
|
|
|
|
|
# 配置 XBee 串口連接
|
|
|
|
|
|
#ser = serial.Serial('COM5', 57600) # XBee 串口
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 無人機系統 ID（假設為 5）和目標系統 ID（假設為 1）
|
|
|
|
|
|
system_id = 5
|
|
|
|
|
|
target_system_id = 1 # 設定目標系統 ID，這裡假設目標系統 ID 是 1
|
|
|
|
|
|
|
|
|
|
|
|
# 目標設備的 64 位地址（請替換為實際的無人機地址）
|
|
|
|
|
|
target_address = b'\x00\x13\xa2\x00\x40\x5f\x88\x56' # 例如：00 13 A2 00 40 5F 88 56
|
|
|
|
|
|
|
|
|
|
|
|
def decode_mavlink_data(data,master):
|
|
|
|
|
|
"""解碼 MAVLink 的原始數據並處理 HEARTBEAT 訊息"""
|
|
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
|
msg = master.parse_char(data)
|
|
|
|
|
|
if msg:
|
|
|
|
|
|
if msg.get_type() == "HEARTBEAT":
|
|
|
|
|
|
print(f"Raw MAVLink Data (Hex): {data.hex()}")
|
|
|
|
|
|
print(f"Received MAVLink message: {msg}")
|
|
|
|
|
|
print(f"System status: {msg.system_status}")
|
|
|
|
|
|
print(f"Flight mode: {mavutil.mode_string_v10(msg)}")
|
|
|
|
|
|
print(f"System ID: {msg.get_srcSystem()}") # 使用 get_srcSystem() 獲取 system_id
|
|
|
|
|
|
except Exception as e:
|
|
|
|
|
|
print(f"Failed to decode MAVLink data: {e}")
|
|
|
|
|
|
|
|
|
|
|
|
def receive_packets(ser):
|
|
|
|
|
|
xbee = XBee(ser)
|
|
|
|
|
|
# 創建 MAVLink 解析器並與 XBee 串口連接
|
|
|
|
|
|
master = mavutil.mavlink.MAVLink(ser)
|
|
|
|
|
|
|
|
|
|
|
|
while True:
|
|
|
|
|
|
try:
|
|
|
|
|
|
# 從 XBee 接收數據
|
|
|
|
|
|
xbee_data = xbee.wait_read_frame()
|
|
|
|
|
|
|
|
|
|
|
|
# 讀取 `rf_data` 而非 `payload`
|
|
|
|
|
|
raw_data = xbee_data.get('rf_data', None)
|
|
|
|
|
|
if raw_data is None:
|
|
|
|
|
|
print("Warning: No 'rf_data' found in received XBee data!")
|
|
|
|
|
|
continue # 跳過此次循環
|
|
|
|
|
|
|
|
|
|
|
|
# 解碼 MAVLink 訊息
|
|
|
|
|
|
decode_mavlink_data(raw_data,master)
|
|
|
|
|
|
|
|
|
|
|
|
# 根據需要觸發模式切換，例如根據用戶輸入更改模式
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
except KeyboardInterrupt:
|
|
|
|
|
|
print("Exiting...")
|
|
|
|
|
|
break
|
|
|
|
|
|
except Exception as e:
|
|
|
|
|
|
print(f"Error: {e}")
|
|
|
|
|
|
|
|
|
|
|
|
time.sleep(0.1)
|
