Merge branch 'wenchun'

march 11 alignment

README.md

@@ -16,5 +16,15 @@
 
 === 
 Package 簡述
+1. unitdev 為各自協作者做開發時的測試區
+    01 -> 晉凱(ken)
+    02 -> 其宇(chiyu)
+    03 -> 文鈞
+    04 -> 倫渝
+2. fc_network_adapter 
+    建立、維護與飛控韌體的連接 
+    構築 mavlink 封包
+    同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息
+    處理無線模組的通訊格式 (XBee)
 
 

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -0,0 +1,197 @@
+import threading
+from pymavlink import mavutil
+import queue
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+
+import time
+
+
+'''
+
+# 不同的匯流排只有單一種通訊協定 
+# 匯流排接到訊息後透過 queue stack 傳送到分析器
+# 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器
+# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue
+# 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去
+
+# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排
+# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面
+# 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function
+
+
+'''
+
+fixed_stream_analyzer_queue = queue.Queue()
+return_packet_processor_queue = queue.Queue()
+converte_format_queues = []
+
+class MavlinkObject():
+    def __init__(self, mavlink_socket, socket_id = None):
+        self.socket_id = socket_id
+        self.mavlink_socket = mavlink_socket
+        self.msg_count = {}
+        self.multiplexingList = []
+
+        self.multiplexingToAnalysis = [
+            # 0, # HEARTBEAT
+            30, # ATTITUDE
+            33, # GLOBAL_POSITION_INT
+            # 147, # BATTERY_STATUS
+        ]
+        self.multiplexingToReturn = [
+            0, # HEARTBEAT
+            # 30, # ATTITUDE
+        ]
+        self.multiplexingToConvert = [
+            [74, ] # VFR_HUD 
+        ]
+    def __del__(self):
+        self.mavlink_socket.close()
+        self.stop()
+
+    def run(self):
+        if not self.socket_id:
+            print('[mavlinkObject.py] Please set socket id before running')
+            return
+        self.thread = threading.Thread(target=self._run_thread)
+        self.running = self.updateMultiplexingList()
+        self.thread.start()
+
+    def stop(self):
+        self.running = False
+
+    def _run_thread(self):
+        start_time = time.time()
+        while self.running:
+            try:
+                mavlinkMsg = self.mavlink_socket.recv_msg()
+            except Exception as e:
+                print(f"[mavlinkObject.py] Error receiving mavlink data: {e}")
+                print(mavlinkMsg)
+                self.running = False
+                break
+
+            if mavlinkMsg:
+                self.count_mavlink_type(mavlinkMsg) # 統計收到的訊息
+                # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug
+                # break # Debug
+
+                # 將訊息依照 multiplexing list 分發到不同的 queue
+                for i in range(len(self.multiplexingList)):
+                    if mavlinkMsg.get_msgId() in self.multiplexingList[i]:
+                        if i == 0:
+                            fixed_stream_analyzer_queue.put((self.socket_id,mavlinkMsg))
+                        elif i == 1:
+                            return_packet_processor_queue.put((self.socket_id,mavlinkMsg))
+                        else:
+                            convert_queue = converte_format_queues[i-2]
+                            convert_queue.put((self.socket_id,mavlinkMsg))
+
+            # 每秒中 統計一次 收到的訊息總量
+            elapsed_time = time.time() - start_time
+            if elapsed_time > 1:
+                print('[mavlinkObject.py] Messages Type received (in about 1 sec) :')
+                print(self.msg_count)
+                start_time = time.time()
+                self.msg_count = {}
+        
+        # thread 結束
+        print('[mavlinkObject.py] End of _run_thread')
+
+    def count_mavlink_type(self, mavlinkMsg):
+        msg_type = mavlinkMsg.get_type()
+        if msg_type in self.msg_count:
+            self.msg_count[msg_type] += 1
+        else:
+            self.msg_count[msg_type] = 1
+
+    def publish_mavlink_data(self, mavlinkMsg):
+        msg = String()
+        msg.data = str(mavlinkMsg)
+        self.publisher_.publish(msg)
+
+    def updateMultiplexingList(self):
+        # 更新 multiplexing list 
+        self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert
+
+        check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList)
+
+        # 若有錯誤則回傳 False
+        if not check:
+            print('[mavlinkObject.py] Multiplexing List Error, Please check the list')
+            return False
+        return True
+
+class mavlink_analyzer(Node):
+    '''
+    這個 class 是用來接收 mavlink 訊息 並進行分析
+    記錄有 mavlink bus 上有那些 system id 和 component id
+    為了每個 system id 都有一個對應的 device object
+    並且看是否有重複 system id
+
+    也許可以用 node 去創建 這樣就可以直接進到 ros2 執行緒?
+    '''
+    def __init__(self):
+        super().__init__('mavlink_analyzer')
+
+        # self.publisher_ = self.create_publisher(String, 'mavlink_data', 10)
+        # self.mavlink_object = None
+        # self.create_mavlink_object()
+
+    def create_mavlink_object(self):
+        # connection_string="udp:
+        pass
+
+class mavlink_device():
+    def __init__(self):
+        self.socket_id = None # 記錄來自於哪個 socket
+        self.sysid = None
+        self.component_count = 1
+        self.compid_list = []
+        
+
+
+def main(args=None):
+
+    connection_string="udp:127.0.0.1:14550"
+    mavlink_socket = mavutil.mavlink_connection(connection_string)
+
+    # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來
+    mavlink_object = MavlinkObject(mavlink_socket, 1)
+    mavlink_object.multiplexingToAnalysis = [30] # only ATTITUDE
+    mavlink_object.multiplexingToReturn = [0] # only HEARTBEAT
+    mavlink_object.multiplexingToConvert = [[74,]] # only VFR_HUD 
+    
+    converte_format_queues.append(queue.Queue())
+
+    mavlink_object.run()
+    start_time = time.time()
+    while time.time() - start_time < 2:
+        
+        while not fixed_stream_analyzer_queue.empty():
+            print('fixed_stream_analyzer_queue:')
+            t = fixed_stream_analyzer_queue.get()
+            print('from {} : {}'.format(t[0],t[1]))
+        while not return_packet_processor_queue.empty():
+            print('return_packet_processor_queue:')
+            t = return_packet_processor_queue.get()
+            print('from {} : {}'.format(t[0],t[1]))
+        
+        for q in converte_format_queues:
+            while not q.empty():
+                print('converte_format_queues:')
+                t = q.get()
+                print('from {} : {}'.format(t[0],t[1]))
+
+
+    mavlink_object.running = False
+    mavlink_object.thread.join()
+    print('End of Program')
+
+
+
+if __name__ == '__main__':
+    main()

src/fc_network_adapter/fc_network_adapter/socketManager.py

@@ -0,0 +1,14 @@
+
+'''
+
+透過某個地方 得到 udp 或 uart 接口
+對於每個接口 視為一個獨立的物件 
+
+物件對於不同的接口 是為不同類型的物件
+
+每個類型的物件 創建一個獨立的執行緒 來處理資料
+關於執行緒的實作 是寫在另一個模組
+
+物件之間 也可以做資料轉換/轉拋
+
+'''

src/fc_network_adapter/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fc_network_adapter</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/fc_network_adapter/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/fc_network_adapter
+[install]
+install_scripts=$base/lib/fc_network_adapter

src/fc_network_adapter/setup.py

@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = 'fc_network_adapter'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='picars',
+    maintainer_email='chiyu1468@hotmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

src/unitdev01/unitdev01/mavlink.py

@@ -1,4 +1,6 @@
 import sys
+import time
+import math
 import rclpy
 from PyQt5 import QtWidgets
 from PyQt5.QtCore import QThread, pyqtSignal
@@ -14,26 +16,94 @@ class MAVLinkWorker(QThread):
     imu_signal = pyqtSignal(str, float)
     hdg_signal = pyqtSignal(str, float)
     groundspeed_signal = pyqtSignal(str, float)
+    ping_signal = pyqtSignal(str, float)
+    loss_signal = pyqtSignal(str, float)
+    frequency_signal = pyqtSignal(str, float)
+    param_signal = pyqtSignal(str, int)
 
-    def __init__(self, connection_string="udp:127.0.0.1:14550"):
+    def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'):
         super().__init__()
-        self.connection = mavutil.mavlink_connection(connection_string)
+        self.namespaces = ['UAV1', 'UAV4', 'UAV5']
+        self.connection = mavutil.mavlink_connection(usb, baud=57600)
+        self.connection.wait_heartbeat()
+        for sysid in self.namespaces:
+            sysid = int(sysid.replace('UAV', ''))
+            self.set_sr_params(sysid)
         self.running = True
-        self.uav_data = {}
+
+        # 設置需要監控的訊息類型
+        self.messages_to_monitor = ["HEARTBEAT", "BATTERY_STATUS", "GLOBAL_POSITION_INT", "GPS_RAW_INT", "LOCAL_POSITION_NED", "ATTITUDE", "VFR_HUD", "TIMESYNC"]
+        self.message_count = {}
+        self.frequency = {}
+        self.start_time = {}
+
+        # 用於計算丟包
+        self.seq_numbers = {}
+        self.packet_loss_count = {}
+        self.total_packet_count = {}
+        self.loss_percentage = {}
+
+        for namespace in self.namespaces:
+            self.request_param(namespace, "SR1_EXTRA1")
 
     def run(self):
         while self.running:
             try:
-                msg = self.connection.recv_match(blocking=True)
+                current_time = time.time()
+                msg = self.connection.recv_msg()
                 if not msg:
                     continue
-
-                msg_type = msg.get_type()
                 sysid = msg.get_srcSystem()
-                namespace = f"uav{sysid}"
+                if sysid == 0:
+                    continue
+                namespace = f"UAV{sysid}"
+                print(sysid)
+                msg_type = msg.get_type()
+                if msg_type =="BAD_DATA":
+                    continue
 
-                if sysid not in self.uav_data:
+                print(msg_type)
-                    self.uav_data[sysid] = {"sysid": sysid}
+                
+                # Packet loss calculation
+                if sysid not in self.seq_numbers:
+                    self.seq_numbers[sysid] = msg.get_seq()  # Initialize sequence number tracking
+                    self.packet_loss_count[sysid] = 0
+                    self.total_packet_count[sysid] = 1
+                else:
+                    current_seq = msg.get_seq()
+                    expected_seq = (self.seq_numbers[sysid] + 1) % 256  # MAVLink sequence numbers are modulo 256
+                    self.total_packet_count[sysid] += 1
+
+                    if current_seq != expected_seq: # Packet(s) lost
+                        lost_packets = (current_seq - expected_seq) % 256
+                        self.packet_loss_count[sysid] += lost_packets
+                        self.total_packet_count[sysid] += lost_packets
+
+                    self.seq_numbers[sysid] = current_seq
+                    self.loss_percentage[sysid] = (self.packet_loss_count[sysid] / self.total_packet_count[sysid]) * 100
+                    self.loss_signal.emit(namespace, self.loss_percentage[sysid])
+
+                # Frequency calculation
+                if sysid not in self.message_count:
+                    self.message_count[sysid] = 0
+                    self.start_time[sysid] = current_time
+
+                self.message_count[sysid] += 1
+
+                # 每隔一段時間更新
+                if current_time - self.start_time[sysid] >= 1:
+                    # 每秒頻率
+                    self.frequency[sysid] = self.message_count[sysid] / (current_time - self.start_time[sysid])
+                    self.start_time[sysid] = current_time
+                    self.message_count[sysid] = 0
+                    self.frequency_signal.emit(namespace, self.frequency[sysid])
+
+                    # 發送 timesync request
+                    self.connection.mav.timesync_send( 
+                        0, #tc1
+                        int(time.time() * 1e9) #ts1
+                    )
+                    #self.send_heartbeat
 
                 if msg_type == "HEARTBEAT":
                     mode = mavutil.mode_string_v10(msg)
@@ -60,7 +130,7 @@ class MAVLinkWorker(QThread):
                     self.local_position_signal.emit(namespace, x, y, z)
 
                 elif msg_type == "ATTITUDE":
-                    pitch = msg.pitch
+                    pitch = math.degrees(msg.pitch)
                     self.imu_signal.emit(namespace, pitch)
 
                 elif msg_type == "VFR_HUD":
@@ -69,6 +139,16 @@ class MAVLinkWorker(QThread):
                     self.hdg_signal.emit(namespace, heading)
                     self.groundspeed_signal.emit(namespace, groundspeed)
 
+                elif msg_type == "TIMESYNC":
+                    round_trip_time = (int(time.time() * 1e9) - msg.ts1) / 1e6
+                    if(round_trip_time<1e6):
+                        self.ping_signal.emit(namespace, round_trip_time)
+                
+                elif msg.get_type() == 'PARAM_VALUE':
+                    param_name = "SR1_EXTRA1"
+                    if msg.param_id == param_name:
+                        self.param_signal.emit(namespace, msg.param_value)
+
             except Exception as e:
                 print(f"Error reading message: {e}")
 
@@ -77,7 +157,7 @@ class MAVLinkWorker(QThread):
         self.connection.close()
 
     def set_mode(self, namespace, mode):
-        sysid = int(namespace[-1])
+        sysid = int(namespace.replace('UAV', ''))
         if mode == 'STABILIZE':
             mode = 0
         elif mode == 'AUTO':
@@ -86,56 +166,47 @@ class MAVLinkWorker(QThread):
             mode = 4
         elif mode == 'LOITER':
             mode = 5
-        self.connection.mav.set_mode_send(
+        self.connection.mav.set_mode_send( #SET_MODE (11) — [DEP]
             sysid,
             mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
             mode
         )
 
     def arm(self, namespace, arm):
-        sysid = int(namespace[-1])
+        sysid = int(namespace.replace('UAV', ''))
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
-            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, #MAV_CMD_COMPONENT_ARM_DISARM (400)
             0,  # Confirmation
             1 if arm else 0,
             0, 0, 0, 0, 0, 0
         )
-        # self.connection.mav.command_long_send(
-        #     sysid,
-        #     1,  # Component ID
-        #     mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
-        #     0,
-        #     33,
-        #     -1, 
-        #     0, 0, 0, 0, 0
-        # )
 
     def takeoff(self, namespace, altitude):
-        sysid = int(namespace[-1])
+        sysid = int(namespace.replace('UAV', ''))
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
-            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,  # Command to execute
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,  #MAV_CMD_NAV_TAKEOFF (22)
             0,                                # Confirmation (0 = first transmission)
             0, 0, 0, 0,                      # Parameters 1-4 (Unused in takeoff command)
             0, 0, altitude            # Latitude, Longitude, Altitude (target_altitude in meters)
         )
 
     def land(self, namespace):
-        sysid = int(namespace[-1])
+        sysid = int(namespace.replace('UAV', ''))
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
-            mavutil.mavlink.MAV_CMD_NAV_LAND, 
+            mavutil.mavlink.MAV_CMD_NAV_LAND, #MAV_CMD_NAV_LAND (21)
             0,                                
             0, 0, 0, 0,                       
             0, 0, 0                           
         )
 
     def set_local_position(self, namespace, x, y, z):
-        sysid = int(namespace[-1])
+        sysid = int(namespace.replace('UAV', ''))
         self.connection.mav.set_position_target_local_ned_send(
             0, sysid, 1,  # time_boot_ms, sysid, compid
             mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # Frame
@@ -146,12 +217,70 @@ class MAVLinkWorker(QThread):
             0, 0  # Yaw, yaw_rate
         )
     
+    def reboot_drone(self, namespace):
+        sysid = int(namespace.replace('UAV', ''))
+        self.connection.mav.command_long_send(
+            sysid,
+            1,  # target_component (一般為1)
+            mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN,  # Reboot 命令
+            0,  # confirmation
+            1,  # 第一個參數 (1 = reboot，0 = 無操作，2 = 關機)
+            0, 0, 0, 0, 0, 0  # 其餘參數保留
+        )
+
+    def set_param(self, namespace, param_name, value):
+        sysid = int(namespace.replace('UAV', ''))
+        try:
+            self.connection.mav.param_set_send(
+                sysid,
+                1,
+                param_name.encode('utf-8'),
+                float(value),
+                mavutil.mavlink.MAV_PARAM_TYPE_REAL32
+            )
+        except Exception as e:
+            print(f"Failed to set parameter {param_name}: {e}")
+
+    def request_param(self, namespace, param_name):
+        sysid = int(namespace.replace('UAV', ''))
+        try:
+            self.connection.mav.param_request_read_send(
+                sysid,  # 系統 ID
+                1,  # 組件 ID
+                param_name.encode('utf-8'),  # 參數名稱
+                -1  # 參數索引，-1 表示根據名稱請求
+            )
+        except Exception as e:
+            print(f"Failed to set parameter {param_name}: {e}")
+
+    def send_heartbeat(self):
+        self.connection.mav.heartbeat_send(
+            mavutil.mavlink.MAV_TYPE_GCS,    
+            mavutil.mavlink.MAV_AUTOPILOT_INVALID,
+            0, 0, 0
+        )
+
+    def set_sr_params(self, sysid):
+        """ 直接設置 MAVLink 訊息頻率 """
+        freqs = [0, 2, 4]
+        params = {
+            "SR1_ADSB": freqs[0],
+            "SR1_EXT_STAT": freqs[1],
+            "SR1_EXTRA1": freqs[2],
+            "SR1_EXTRA2": freqs[2],
+            "SR1_EXTRA3": freqs[1],
+            "SR1_POSITION": freqs[1],
+            "SR1_RAW_SENS": freqs[1],
+            "SR1_RC_CHAN": freqs[1]
+        }
+        for param, value in params.items():
+            self.set_param(f"UAV{sysid}", param, value)
 
 class DroneControlApp(QMainWindow):
     def __init__(self):
         super().__init__()
-        self.namespaces = ['uav1', 'uav2', 'uav3']
         self.workers = MAVLinkWorker()
+        self.namespaces = self.workers.namespaces
         self.initUI()
         
         # Connect signals to update the UI
@@ -163,7 +292,10 @@ class DroneControlApp(QMainWindow):
         self.workers.imu_signal.connect(self.update_imu)
         self.workers.hdg_signal.connect(self.update_hdg)
         self.workers.groundspeed_signal.connect(self.update_groundspeed)
-
+        self.workers.ping_signal.connect(self.update_ping)
+        self.workers.loss_signal.connect(self.update_loss)
+        self.workers.frequency_signal.connect(self.update_frequency)
+        self.workers.param_signal.connect(self.update_param)
         self.workers.start()
             
     def initUI(self):
@@ -193,15 +325,19 @@ class DroneControlApp(QMainWindow):
 
             # 狀態顯示
             self.uav_labels[namespace] = {
-                "status": QLabel(f"{namespace} 狀態：等待數據..."),
+                "status": QLabel("狀態：等待數據..."),
-                "battery": QLabel(f"{namespace} 電量：等待數據..."),
+                "battery": QLabel("電量：等待數據..."),
-                "gps": QLabel(f"{namespace} GPS位置：等待數據...\n\n"),
+                "gps": QLabel("GPS位置：等待數據...\n\n"),
-                "fix": QLabel(f"{namespace} Fix Type：等待數據..."),
+                "fix": QLabel("Fix Type：等待數據..."),
-                "satellites": QLabel(f"{namespace} 衛星數量：等待數據..."),
+                "satellites": QLabel("衛星數量：等待數據..."),
-                "local_position": QLabel(f"{namespace} Local Position：等待數據..."),
+                "local_position": QLabel("Local Position：等待數據..."),
-                "groundspeed": QLabel(f"{namespace} 地面速度：等待數據..."),
+                "groundspeed": QLabel("地面速度：等待數據..."),
-                "pitch": QLabel(f"{namespace} Pitch：等待數據..."),
+                "pitch": QLabel("Pitch：等待數據..."),
-                "heading": QLabel(f"{namespace} Heading：等待數據..."),
+                "heading": QLabel("Heading：等待數據..."),
+                "ping": QLabel("Ping：等待數據..."),
+                "loss": QLabel("丟包：等待數據..."),
+                "frequency": QLabel("頻率：等待數據..."),
+                "param": QLabel("SR1_EXTRA1：等待數據...")
             }
 
             # 把每個資訊添加到該 UAV 的垂直佈局中
@@ -213,6 +349,16 @@ class DroneControlApp(QMainWindow):
 
         main_layout.addLayout(uav_layout)
 
+        # SR1_EXTRA1參數設置
+        param_layout = QHBoxLayout()
+        self.paramInput = QLineEdit()
+        self.paramInput.setPlaceholderText("輸入SR1_EXTRA1的新值")
+        self.setParamButton = QPushButton("設置SR1_EXTRA1")
+        self.setParamButton.clicked.connect(self.set_SR1_EXTRA1)
+        param_layout.addWidget(self.paramInput)
+        param_layout.addWidget(self.setParamButton)
+        main_layout.addLayout(param_layout)
+
         # 模式切換區域
         mode_layout = QHBoxLayout()
         self.modeComboBox = QComboBox()
@@ -244,19 +390,26 @@ class DroneControlApp(QMainWindow):
         main_layout.addWidget(self.setPositionButton)
 
         # 解鎖按鈕
+        fly_layout = QHBoxLayout()
         self.armButton = QPushButton("解鎖")
         self.armButton.clicked.connect(self.arm_drone)
-        main_layout.addWidget(self.armButton)
 
         # 起飛按鈕
         self.takeoffButton = QPushButton("起飛")
         self.takeoffButton.clicked.connect(self.takeoff_drone)
-        main_layout.addWidget(self.takeoffButton)
 
         # 降落按鈕
         self.landButton = QPushButton("降落")
         self.landButton.clicked.connect(self.land_drone)
-        main_layout.addWidget(self.landButton)
+        fly_layout.addWidget(self.armButton)
+        fly_layout.addWidget(self.takeoffButton)
+        fly_layout.addWidget(self.landButton)
+        main_layout.addLayout(fly_layout)
+
+        #設置重啟按鈕
+        self.rebootButton = QPushButton("重啟")
+        self.rebootButton.clicked.connect(self.reboot_drone)
+        main_layout.addWidget(self.rebootButton)
 
         # 設置主佈局
         central_widget = QWidget(self)
@@ -271,13 +424,13 @@ class DroneControlApp(QMainWindow):
             event.accept()
 
     def update_state(self, namespace, mode):
-        self.uav_labels[namespace]["status"].setText(f"{namespace} 狀態：{mode}")
+        self.uav_labels[namespace]["status"].setText(f"狀態：{mode}")
 
     def update_battery(self, namespace, percentage):
-        self.uav_labels[namespace]["battery"].setText(f"{namespace} 電量：{percentage * 100:.2f}%")
+        self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%")
 
     def update_gps(self, namespace, latitude, longitude):
-        self.uav_labels[namespace]["gps"].setText(f"{namespace} GPS位置： \nLat:{latitude:.6f}° \nLon:{longitude:.6f}°")
+        self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n    Lat:{latitude:.6f}° \n    Lon:{longitude:.6f}°")
 
     def update_gps_status(self, namespace, fix_type, satellites_visible):
         fix_type_str = {
@@ -288,20 +441,32 @@ class DroneControlApp(QMainWindow):
             4: "RTK Float",
             5: "RTK Fix"
         }.get(fix_type, "Unknown")
-        self.uav_labels[namespace]["fix"].setText(f"{namespace} Fix Type：{fix_type_str}")
+        self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}")
-        self.uav_labels[namespace]["satellites"].setText(f"{namespace} 衛星數量：{satellites_visible}")
+        self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}")
 
     def update_local_position(self, namespace, x, y, z):
-        self.uav_labels[namespace]["local_position"].setText(f"{namespace} Local：({x:.2f}, {y:.2f}, {z:.2f})")
+        self.uav_labels[namespace]["local_position"].setText(f"Local：({x:.2f}, {y:.2f}, {z:.2f})")
 
     def update_imu(self, namespace, pitch):
-        self.uav_labels[namespace]["pitch"].setText(f"{namespace} Pitch：{pitch:.2f}°")
+        self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°")
 
     def update_hdg(self, namespace, heading):
-        self.uav_labels[namespace]["heading"].setText(f"{namespace} Heading：{heading:.2f}°")
+        self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°")
 
     def update_groundspeed(self, namespace, groundspeed):
-        self.uav_labels[namespace]["groundspeed"].setText(f"{namespace} 地面速度：{groundspeed:.2f} m/s")
+        self.uav_labels[namespace]["groundspeed"].setText(f"地面速度：{groundspeed:.2f} m/s")
+    
+    def update_ping(self, namespace, ping):
+        self.uav_labels[namespace]["ping"].setText(f"Ping：{ping:.2f} ms")
+
+    def update_loss(self, namespace, loss):
+        self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%")
+
+    def update_frequency(self, namespace, frequency):
+        self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz")
+
+    def update_param(self, namespace, value):
+        self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}")
 
     def change_mode(self):
         try:
@@ -346,6 +511,24 @@ class DroneControlApp(QMainWindow):
         except ValueError:
             QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
 
+    def reboot_drone(self):
+        try:
+            for namespace in self.namespaces:
+                if self.uav_checkboxes[namespace].isChecked():
+                    self.workers.reboot_drone(namespace)
+        except Exception as e:
+            QtWidgets.QMessageBox.warning(self, "錯誤", f"重啟失敗：{e}")
+
+    def set_SR1_EXTRA1(self):
+        param_value = self.paramInput.text().strip()
+        try:
+            for namespace in self.namespaces:
+                if self.uav_checkboxes[namespace].isChecked():
+                    self.workers.set_param(namespace, "SR1_EXTRA1", param_value)
+                    self.workers.request_param(namespace, "SR1_EXTRA1")
+        except ValueError:
+            QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
+
 def main(args=None):
     rclpy.init(args=args)
     app = QtWidgets.QApplication(sys.argv)

src/unitdev02/setup.py

@@ -21,7 +21,8 @@ setup(
     entry_points={
         'console_scripts': [
             'showmavsimple = unitdev02.test01:mavlink_analyzer_simple',
-            'fdm_switch_one = unitdev02.test01:fdm_swicth_example_one',
+            'fdm_switch_one = unitdev02.test01:fdm_switch_example_one',
+            'fdm_switch_two = unitdev02.test01:fdm_switch_example_two',
         ],
     },
 )

src/unitdev02/unitdev02/test01.py

@@ -6,6 +6,10 @@ import time
 import socket
 import struct
 
+# used at fdm_switch_example_two
+import threading
+import queue
+
 def mavlink_analyzer_simple(count = 500):
     # rclpy.init()
     # node = rclpy.create_node('mavlink_analyzer_simple')
@@ -32,11 +36,17 @@ def mavlink_analyzer_simple(count = 500):
 
             if msg.get_type() == 'SERVO_OUTPUT_RAW':
                 print(msg)
+                pass
 
         else:
             print('No message yet, 1 second for data to fill')
             sleep(1)
 
+    print("markA ------")
+    print(msg.get_type())
+    print(msg.ordered_fieldnames)
+    print("markA ------ END")
+
     print('Messages Type received:')
     print(msg_count)
 
@@ -52,7 +62,6 @@ def fdm_parser_example(data=None):
         data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
     # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
 
-
     parse_format = 'HHI16H'
 
     if len(data) != struct.calcsize(parse_format):
@@ -103,7 +112,8 @@ def fdm_switch_example_one():
         elapsed_time = current_time - start_time
 
         if elapsed_time >= 1.0:
-            print(f"Packets received in the last second: {packet_count}")
+            # print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack : {data_g2s}')
             packet_count = 0
             start_time = current_time
             fdm_parser_example(data_s2g)
@@ -113,34 +123,108 @@ def fdm_switch_example_two():
     '''
     這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
     有轉換數據格式
+
+    time_usec
+    servo1_raw 
+    servo16_raw
     '''
     # read info from SITL via MAVLink
-    connection_string="udp:127.0.0.1:14550"
+    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
     connection = mavutil.mavlink_connection(connection_string)
 
     # set a target link pass JSON to Gazebo, as a client
     sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
     server_address = ('127.0.0.1', 19002)
 
+    data_queue = queue.Queue()
+    servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....]
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
+            sock.sendto(data_fdm, server_address)
+            frame_count_fdm += 3
+            # fdm_parser_example(data_fdm) # debug
+
+            elapsed_time = time.time() - start_time
+            sleep_time = interval - elapsed_time
+            if sleep_time > 0:
+                sleep(sleep_time)
+
+    Running = True
+    thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+    thread.start()
+    print('Start sending data to Gazebo')
+
+    while Running:
+        msgb1 = None
+        msg = connection.recv_msg()
+        while msg :
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                msgb1 = msg
+                # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+            msg = connection.recv_msg()
+
+        if msgb1:
+            for i in range(16):
+                servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+                data_queue.put(servo_out)
+            msgb1 = None
+            # Running = False # debug
+        else:
+            # print('No message yet, 10 ms for data to fill')
+            sleep(0.01)
+
+        # Example of putting data into the queue
+        # data_queue.put(data)
+        # To stop the thread, you can put None into the queue
+        # data_queue.put(None)
+
+from pymavlink.dialects.v20 import common  # 使用 MAVLink 2.0
+
+def mavlink_btye_generator_test():
+
+    # 創建一個 MAVLink 對象
+    mav = common.MAVLink(None)  # 不透過 connection，直接使用 None
+
+    # 創建一個 HEARTBEAT 訊息
+    msg = mav.heartbeat_encode(
+        type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
+        autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+        base_mode=0,
+        custom_mode=0,
+        system_status=mavutil.mavlink.MAV_STATE_ACTIVE
+    )
+
+    # 取得 MAVLink 訊息的 bytes
+    mavlink_bytes = msg.pack(mav)
+
+    # 回傳 bytes
+    print(mavlink_bytes)
+    print(type(mavlink_bytes))
+
+    mav2 = common.MAVLink(None)
+    # my_msg = mav2.decode(mavlink_bytes)
+    print("========")
+    print(dir(mav2))
+    # print(my_msg)
+
+
 
 print('Start')
-mavlink_analyzer_simple()
+# fdm_switch_example_two()
+# fdm_parser_example()
+# mavlink_analyzer_simple(8)
+# mavlink_btye_generator_test()
 
-# connection_string="udp:127.0.0.1:14550"
-# connection = mavutil.mavlink_connection(connection_string)
 
-# connection.mav.command_long_send(
-#     1,
-#     1,
-#     mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
-#     0, 33, 1000000, 0, 0, 0, 0, 0
-# )
-
-# connection.mav.command_long_send(
-#         1,
-#         1,  # Component ID
-#         mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-#         0,  # Confirmation
-#         1,  # 0: disarm, 1: arm
-#         0, 0, 0, 0, 0, 0
-#     )

src/unitdev03/gazebo_native_manager.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+import subprocess
+import argparse
+import time
+import psutil
+import h5py
+import numpy as np
+
+class GazeboModelManager:
+    def __init__(self):
+        self.base_cmd = "gz service -s /world/map"
+        self.timeout = 5000
+
+    def add_iris(self, number, x_pos, y_pos, yaw_angle):
+        cmd = f'''gz service -s /world/map/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 5000 --req 'sdf: "<?xml version=\\"1.0\\" ?><sdf version=\\"1.9\\"><model name=\\"iris{number}\\"><include><uri>model://iris{number}</uri><pose degrees=\\"true\\">{x_pos} {y_pos} 0.195 0 0 {yaw_angle}</pose></include></model></sdf>"' '''
+
+        try:
+            print(f"正在新增 iris{number}...")
+            print(f"位置: x={x_pos}, y={y_pos}, yaw={yaw_angle}度")
+            subprocess.run(cmd, shell=True, check=True)
+            print(f"成功新增 iris{number}")
+            
+            sitl_instance = number - 1
+            port = 14550 + sitl_instance
+            
+            sitl_cmd = f"sim_vehicle.py -v ArduCopter -f gazebo-iris{sitl_instance} -L NCHU --model JSON --map --console -I{sitl_instance} --out=127.0.0.1:{port}"
+            terminal_cmd = f'gnome-terminal -- bash -c "{sitl_cmd}; exec bash"'
+            print(f"正在啟動 SITL...")
+            subprocess.Popen(terminal_cmd, shell=True)
+            
+            return True
+        except subprocess.CalledProcessError as e:
+            print(f"新增失敗: {e}")
+            return False
+
+    def cleanup_sitl(self, number):
+        instance_number = number - 1
+        print(f"正在清理 SITL 進程 {number}...")
+        
+        cleanup_commands = [
+            f"pkill -f 'sim_vehicle.py.*-I{instance_number}'",
+            f"pkill -f 'arducopter.*-I{instance_number}'",
+            "pkill -f 'mavproxy.py.*map'",
+            "pkill -f 'mavproxy.py.*console'",
+            f"pkill -f 'mavproxy.*{instance_number}'",
+            "pkill -f 'python.*map.py'",
+            "pkill -f 'MAVProxy.py.*map'"
+        ]
+
+        for cmd in cleanup_commands:
+            try:
+                subprocess.run(cmd, shell=True)
+            except subprocess.CalledProcessError:
+                pass
+
+        time.sleep(2)
+        print(f"SITL 進程清理完成")
+
+    def remove_iris(self, number):
+        cmd = f'gz service -s /world/map/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 1000 --req \'name: "iris{number}"\''
+        
+        try:
+            print(f"正在移除 iris{number}...")
+            subprocess.run(cmd, shell=True, check=True)
+            print(f"成功移除 iris{number}")
+            return True
+        except subprocess.CalledProcessError as e:
+            print(f"移除失敗: {e}")
+            return False
+
+    def add_obstacle(self, number, x_pos, y_pos, radius=0.5, height=2.0):
+        cmd = f'''gz service -s /world/map/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 5000 --req 'sdf: "<?xml version=\\"1.0\\" ?><sdf version=\\"1.9\\"><model name=\\"obstacle_{number}\\"><static>true</static><pose>{x_pos} {y_pos} {height/2} 0 0 0</pose><link name=\\"link\\"><collision name=\\"collision\\"><geometry><cylinder><radius>{radius}</radius><length>{height}</length></cylinder></geometry></collision><visual name=\\"visual\\"><geometry><cylinder><radius>{radius}</radius><length>{height}</length></cylinder></geometry><material><ambient>0.3 0.3 0.3 1</ambient><diffuse>0.7 0.7 0.7 1</diffuse></material></visual></link></model></sdf>"' '''
+
+        try:
+            print(f"正在新增障礙物 {number}...")
+            print(f"位置: x={x_pos}, y={y_pos}, radius={radius}, height={height}")
+            subprocess.run(cmd, shell=True, check=True)
+            return True
+        except subprocess.CalledProcessError as e:
+            print(f"新增失敗: {e}")
+            return False
+
+    def remove_obstacle(self, number):
+        cmd = f'gz service -s /world/map/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 1000 --req \'name: "obstacle_{number}"\''
+        
+        try:
+            print(f"正在移除障礙物 {number}...")
+            subprocess.run(cmd, shell=True, check=True)
+            print(f"成功移除障礙物 {number}")
+            return True
+        except subprocess.CalledProcessError as e:
+            print(f"移除失敗: {e}")
+            return False
+
+    def add_obstacles_from_mat(self, mat_file):
+        """從 .mat 檔案逐一生成障礙物"""
+        try:
+            with h5py.File(mat_file, 'r') as f:
+                x_data = f['#refs#/d'][()]
+                y_data = f['#refs#/e'][()]
+                z_data = f['#refs#/f'][()]
+                radius_data = f['#refs#/g'][()]
+                height_data = f['#refs#/h'][()]
+                
+                data_matrix = np.vstack((x_data, y_data, z_data, radius_data, height_data)).T
+                
+                for i, obstacle in enumerate(data_matrix):
+                    x, y, _, radius, height = obstacle
+                    success = self.add_obstacle(i+1, float(x), float(y), float(radius), float(height))
+                    if not success:
+                        print(f"警告：障礙物 {i+1} 新增失敗")
+                    time.sleep(0.1)
+                
+                print("所有障礙物新增完成")
+                return True
+                
+        except Exception as e:
+            print(f"錯誤：讀取或處理 .mat 檔案時發生問題: {e}")
+            return False
+
+def main():
+    parser = argparse.ArgumentParser(description='Gazebo Iris and Obstacle Manager')
+    parser.add_argument('action', choices=['add', 'remove', 'cleanup', 'add_from_mat'], 
+                       help='Action to perform (add, remove, cleanup, or add_from_mat)')
+    parser.add_argument('type', choices=['iris', 'obstacle'],
+                       help='Type of object to manage (iris or obstacle)')
+    parser.add_argument('number', type=int, nargs='?',
+                       help='Object number (e.g., 4 for iris4 or obstacle_4)')
+    parser.add_argument('--x-pos', type=float, default=15.0,
+                       help='X position for new object')
+    parser.add_argument('--y-pos', type=float, default=0.0,
+                       help='Y position for new object')
+    parser.add_argument('--yaw', type=float, default=90.0,
+                       help='Yaw angle in degrees for iris')
+    parser.add_argument('--radius', type=float, default=0.5,
+                       help='Radius for obstacle cylinder')
+    parser.add_argument('--height', type=float, default=2.0,
+                       help='Height for obstacle cylinder')
+    parser.add_argument('--mat-file', type=str,
+                       help='Path to .mat file containing obstacles data')
+    
+    args = parser.parse_args()
+    manager = GazeboModelManager()
+    
+    if args.action == 'add_from_mat':
+        if args.type == 'obstacle' and args.mat_file:
+            manager.add_obstacles_from_mat(args.mat_file)
+        else:
+            print("錯誤：使用 add_from_mat 時需指定 --mat-file 參數")
+    elif args.action == 'add':
+        if args.number is None:
+            print("錯誤：使用 add 時需指定 number 參數")
+            return
+        if args.type == 'iris':
+            manager.add_iris(args.number, args.x_pos, args.y_pos, args.yaw)
+        else:  # obstacle
+            manager.add_obstacle(args.number, args.x_pos, args.y_pos, args.radius, args.height)
+    elif args.action == 'cleanup':
+        if args.number is None:
+            print("錯誤：使用 cleanup 時需指定 number 參數")
+            return
+        if args.type == 'iris':
+            manager.cleanup_sitl(args.number)
+    else:  # remove
+        if args.number is None:
+            print("錯誤：使用 remove 時需指定 number 參數")
+            return
+        if args.type == 'iris':
+            manager.remove_iris(args.number)
+        else:
+            manager.remove_obstacle(args.number)
+
+if __name__ == "__main__":
+    main()

src/unitdev03/pymavlink_01.py

@@ -0,0 +1,232 @@
+#!/usr/bin/env python3
+from pymavlink import mavutil
+import time
+import math
+import threading
+
+class DroneController:
+    def __init__(self, connection_string="udp:127.0.0.1:14550"):
+        # 建立 MAVLink 連接
+        self.master = mavutil.mavlink_connection(connection_string)
+        
+        # 等待心跳包
+        self.master.wait_heartbeat()
+        print("已建立連接!")
+
+    def set_mode(self, mode):
+        """設置飛行模式"""
+        mode_id = self.master.mode_mapping()[mode]
+        self.master.mav.set_mode_send(
+            self.master.target_system,
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            mode_id
+        )
+        
+        for _ in range(5):
+            msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            if msg and msg.custom_mode == mode_id:
+                return True
+        return False
+
+    def arm(self, arm_state=True):
+        """解鎖/上鎖無人機"""
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0,
+            1 if arm_state else 0, 0, 0, 0, 0, 0, 0
+        )
+        
+        for _ in range(5):
+            msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            if msg:
+                armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
+                if armed == arm_state:
+                    return True
+        return False
+
+    def takeoff(self, altitude):
+        """起飛到指定高度"""
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            0,
+            0, 0, 0, 0, 0, 0,
+            altitude
+        )
+        
+        while True:
+            msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
+            if msg:
+                current_alt = msg.relative_alt / 1000.0
+                if abs(current_alt - altitude) < 0.5:
+                    return True
+            time.sleep(0.1)
+
+    def goto_position_local(self, x, y, z, vx=2, vy=2, vz=0, yaw=0, yaw_rate=0):
+        """
+        飛到指定的本地座標位置
+        x, y, z: 目標位置(公尺)
+        """
+        # NED系統中，向上飛需要給負值
+        z = -z  # 將輸入的z值轉換為NED系統
+
+        # 重要：type_mask 設為 0 表示使用該維度
+        # 我們只想使用位置控制 (x,y,z)，所以其他都設為 ignore
+        type_mask = (
+            # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |  # 8
+            # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |  # 16
+            # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE |  # 32
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |  # 64
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |  # 128
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |  # 256
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE | # 1024
+            mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE  # 2048
+        )
+        
+        # 或直接使用二進制: type_mask = 0b0000111111111000
+        
+        print(f"發送位置指令: x={x}, y={y}, z={-z}")
+        
+        self.master.mav.set_position_target_local_ned_send(
+            0,       # timestamp
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+            type_mask,
+            x, y, z,    # 位置
+            0, 0, 0,    # 速度 (設為 0)
+            5, 5, 0,    # 加速度 (設為 0)
+            0, 0        # yaw, yaw_rate (設為 0)
+        )
+
+        # 監控位置變化
+        print("開始監控位置變化...")
+        start_time = time.time()
+        while time.time() - start_time < 20:  # 20秒超時
+            msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+            if msg:
+                dist = math.sqrt((msg.x - x)**2 + (msg.y - y)**2 + (msg.z - z)**2)
+                print(f"當前位置: x={msg.x:.2f}, y={msg.y:.2f}, z={-msg.z:.2f}, 距離目標: {dist:.2f}m")
+                if dist < 0.5:
+                    print("到達目標位置")
+                    return True
+            time.sleep(0.1)
+        
+        print("移動超時")
+        return False
+
+    def get_current_state(self):
+        """獲取當前狀態"""
+        msg = self.master.recv_match(type='HEARTBEAT', blocking=True)
+        if msg:
+            mode = list(self.master.mode_mapping().keys())[list(self.master.mode_mapping().values()).index(msg.custom_mode)]
+            armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
+            return {
+                'mode': mode,
+                'armed': armed
+            }
+        return None
+
+def print_menu():
+    """列印功能表"""
+    menu = [
+        "\n=== 無人機控制界面 ===",
+        "1. 切換至 GUIDED 模式",
+        "2. 切換至 LAND 模式",
+        "3. 解鎖",
+        "4. 上鎖",
+        "5. 起飛",
+        "6. 飛到指定位置",
+        "7. 切換至 RTL 模式(返航)",
+        "8. 查看當前狀態",
+        "9. 退出",
+        "0. 顯示此清單",
+        "===================="
+    ]
+    for item in menu:
+        print(item)
+
+def main():
+    drone = DroneController()
+    print_menu()
+
+    while True:
+        try:
+            choice = input("\n請輸入指令編號: ")
+
+            if choice == '0':
+                print_menu()
+                
+            elif choice == '1':
+                if drone.set_mode("GUIDED"):
+                    print("已切換至 GUIDED 模式")
+                else:
+                    print("切換模式失敗")
+                    
+            elif choice == '2':
+                if drone.set_mode("LAND"):
+                    print("已切換至 LAND 模式")
+                else:
+                    print("切換模式失敗")
+                    
+            elif choice == '3':
+                if drone.arm(True):
+                    print("無人機已解鎖")
+                else:
+                    print("解鎖失敗")
+                    
+            elif choice == '4':
+                if drone.arm(False):
+                    print("無人機已上鎖")
+                else:
+                    print("上鎖失敗")
+                    
+            elif choice == '5':
+                altitude = float(input("請輸入起飛高度(公尺): "))
+                if drone.takeoff(altitude):
+                    print(f"已到達目標高度: {altitude}公尺")
+                else:
+                    print("起飛失敗")
+                    
+            elif choice == '6':
+                x = float(input("請輸入X座標(公尺): "))
+                y = float(input("請輸入Y座標(公尺): "))
+                z = float(input("請輸入Z座標(公尺,負值代表向下): "))
+                if drone.goto_position_local(x, y, z):
+                    print("已到達目標位置")
+                else:
+                    print("移動失敗")
+                    
+            elif choice == '7':
+                if drone.set_mode("RTL"):
+                    print("已切換至 RTL 模式，無人機正在返航")
+                else:
+                    print("切換至 RTL 模式失敗")
+                    
+            elif choice == '8':
+                state = drone.get_current_state()
+                if state:
+                    print("\n=== 無人機狀態 ===")
+                    print(f"模式: {state['mode']}")
+                    print(f"解鎖狀態: {'已解鎖' if state['armed'] else '已上鎖'}")
+                    print("==================")
+                else:
+                    print("無法獲取狀態")
+                    
+            elif choice == '9':
+                print("程式結束")
+                break
+                
+            else:
+                print("無效的指令，請重新輸入")
+                
+        except ValueError as e:
+            print(f"輸入錯誤: {e}")
+        except Exception as e:
+            print(f"發生錯誤: {e}")
+
+if __name__ == "__main__":
+    main()

src/unitdev04/coordinator_api.py

@@ -0,0 +1,47 @@
+import threading
+import time
+from digi.xbee.devices import XBeeDevice
+
+# Configure XBee connection
+PORT = "/dev/ttyUSB1"
+BAUD_RATE = 57600
+
+# Initialize XBee device
+device = XBeeDevice(PORT, BAUD_RATE)
+device.open()
+
+# Callback function to handle incoming messages
+def data_received_callback(xbee_message):
+    sender = xbee_message.remote_device.get_64bit_addr()
+    try:
+        data = xbee_message.data.decode("utf-8")  # Attempt UTF-8 decoding
+    except UnicodeDecodeError:
+        data = xbee_message.data.hex()  # Fallback to HEX if decoding fails
+    print(f"\nReceived from {sender}: {data}\nEnter message: ", end="")
+
+# Register callback for incoming data
+device.add_data_received_callback(data_received_callback)
+
+# Function to send broadcast messages (runs in a separate thread)
+def send_messages():
+    while True:
+        user_input = input("\nEnter message: ")  # User input
+        status = device.send_data_broadcast(user_input)  # Broadcast message
+        if status:
+            print("Broadcast message sent successfully!")
+        else:
+            print("Failed to send broadcast message.")
+
+# Start the message sending thread
+send_thread = threading.Thread(target=send_messages, daemon=True)
+send_thread.start()
+
+print("Broadcast chat mode activated. Type a message and press Enter to send.\n")
+
+# Keep the program running
+try:
+    while True:
+        time.sleep(0.1)  # Reduce CPU usage and ensure stability
+except KeyboardInterrupt:
+    print("\nProgram terminated.")
+    device.close()

src/unitdev04/endpoint_at.py

@@ -0,0 +1,41 @@
+import serial
+import threading
+
+PORT = "COM15"  # AT Mode XBee COM Port
+BAUD_RATE = 57600
+
+# Initialize serial connection
+ser = serial.Serial(PORT, BAUD_RATE, timeout=1)
+
+# Function to receive messages from the Coordinator
+def receive_data():
+    while True:
+        data = ser.readline().decode("utf-8", errors="ignore").strip()  # Read incoming data
+        if data:
+            print(f"\nReceived from Coordinator: {data}")
+            print("Enter message: ", end="", flush=True)  # Keep input prompt intact
+
+# Function to send messages to the Coordinator
+def send_data():
+    while True:
+        user_input = input("\nEnter message: ")  # User input
+        ser.write(user_input.encode() + b'\r')  # Send message in AT Mode
+        print("Message sent.")
+
+# Start the receiving thread
+receive_thread = threading.Thread(target=receive_data, daemon=True)
+receive_thread.start()
+
+# Start the sending thread
+send_thread = threading.Thread(target=send_data, daemon=True)
+send_thread.start()
+
+print("AT Mode XBee communication started. Type a message and press Enter to send.\n")
+
+# Keep the program running
+try:
+    while True:
+        pass
+except KeyboardInterrupt:
+    print("\nProgram terminated.")
+    ser.close()