XBee解讀heartbeat資訊

src/unitdev04/receive.py

@@ -0,0 +1,60 @@
+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)