Upload files to 'src/unitdev01/unitdev01'

src/unitdev01/unitdev01/mavlink.py

@@ -1,7 +1,6 @@
 import sys
 import time
 import math
-import rclpy
 from PyQt5 import QtWidgets
 from PyQt5.QtCore import QThread, pyqtSignal
 from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox
@@ -20,10 +19,11 @@ class MAVLinkWorker(QThread):
     loss_signal = pyqtSignal(str, float)
     frequency_signal = pyqtSignal(str, float)
     param_signal = pyqtSignal(str, int)
+    kbps_signal = pyqtSignal(str, float)
 
     def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'):
         super().__init__()
-        self.namespaces = ['UAV1', 'UAV4', 'UAV5']
+        self.namespaces = ['UAV1', 'UAV2', 'UAV3']
         self.connection = mavutil.mavlink_connection(usb, baud=57600)
         self.connection.wait_heartbeat()
         for sysid in self.namespaces:
@@ -31,38 +31,48 @@ class MAVLinkWorker(QThread):
             self.set_sr_params(sysid)
         self.running = True
 
-        # 設置需要監控的訊息類型
+        # 用於計算頻率丟包
-        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 = {}
+        self.total_bytes_received = {}
+        self.throughput_KBps = {}
 
         for namespace in self.namespaces:
             self.request_param(namespace, "SR1_EXTRA1")
+        self.connection.mav.timesync_send( 
+            0, #tc1
+            int(time.time() * 1e9) #ts1
+        )
 
     def run(self):
         while self.running:
             try:
-                current_time = time.time()
                 msg = self.connection.recv_msg()
+                current_time = time.time()
                 if not msg:
                     continue
                 sysid = msg.get_srcSystem()
                 if sysid == 0:
                     continue
                 namespace = f"UAV{sysid}"
-                print(sysid)
                 msg_type = msg.get_type()
                 if msg_type =="BAD_DATA":
                     continue
                 
-                print(msg_type)
+
+                if sysid not in self.total_bytes_received:
+                    self.total_bytes_received[sysid] = 0
+                    self.throughput_KBps[sysid] = 0
+
+                # 計算訊息大小
+                msg_bytes = msg.get_msgbuf()  # 取得封包的 bytes
+                if msg_bytes:
+                    self.total_bytes_received[sysid] += len(msg_bytes)
                 
                 # Packet loss calculation
                 if sysid not in self.seq_numbers:
@@ -71,7 +81,7 @@ class MAVLinkWorker(QThread):
                     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
+                    expected_seq = (self.seq_numbers[sysid] + 1) % 256
                     self.total_packet_count[sysid] += 1
 
                     if current_seq != expected_seq: # Packet(s) lost
@@ -91,27 +101,32 @@ class MAVLinkWorker(QThread):
                 self.message_count[sysid] += 1
 
                 # 每隔一段時間更新
-                if current_time - self.start_time[sysid] >= 1:
+                elapsed_time = current_time - self.start_time[sysid]
+                if elapsed_time >= 1:
                     # 每秒頻率
-                    self.frequency[sysid] = self.message_count[sysid] / (current_time - self.start_time[sysid])
+                    self.frequency[sysid] = self.message_count[sysid] / elapsed_time
-                    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
+                        int(current_time * 1e9) #ts1
                     )
-                    #self.send_heartbeat
+                    #吞吐量
+                    self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time 
+                    self.kbps_signal.emit(namespace, self.throughput_KBps[sysid])
+
+                    self.start_time[sysid] = current_time
+                    self.message_count[sysid] = 0
+                    self.total_bytes_received[sysid] = 0
 
                 if msg_type == "HEARTBEAT":
                     mode = mavutil.mode_string_v10(msg)
                     self.state_signal.emit(namespace, mode)
 
                 elif msg_type == "BATTERY_STATUS":
-                    percentage = msg.battery_remaining/100
+                    voltage = msg.voltages[0] / 1000
-                    self.battery_signal.emit(namespace, percentage)
+                    self.battery_signal.emit(namespace, voltage)
 
                 elif msg_type == "GLOBAL_POSITION_INT":
                     latitude = msg.lat / 1e7
@@ -140,11 +155,11 @@ class MAVLinkWorker(QThread):
                     self.groundspeed_signal.emit(namespace, groundspeed)
 
                 elif msg_type == "TIMESYNC":
-                    round_trip_time = (int(time.time() * 1e9) - msg.ts1) / 1e6
+                    round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6
                     if(round_trip_time<1e6):
                         self.ping_signal.emit(namespace, round_trip_time)
                 
-                elif msg.get_type() == 'PARAM_VALUE':
+                elif msg_type == 'PARAM_VALUE':
                     param_name = "SR1_EXTRA1"
                     if msg.param_id == param_name:
                         self.param_signal.emit(namespace, msg.param_value)
@@ -253,13 +268,7 @@ class MAVLinkWorker(QThread):
         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]
@@ -275,6 +284,22 @@ class MAVLinkWorker(QThread):
         }
         for param, value in params.items():
             self.set_param(f"UAV{sysid}", param, value)
+    '''
+    def set_sr_params(self, sysid):
+        """ 直接設置 MAVLink 訊息頻率 """
+        freqs = [0, 1, 1]
+        params = {
+            "SR1_ADSB": freqs[0],
+            "SR1_EXT_STAT": freqs[0],
+            "SR1_EXTRA1": freqs[2],
+            "SR1_EXTRA2": freqs[2],
+            "SR1_EXTRA3": freqs[0],
+            "SR1_POSITION": freqs[1],
+            "SR1_RAW_SENS": freqs[1],
+            "SR1_RC_CHAN": freqs[0]
+        }
+        for param, value in params.items():
+            self.set_param(f"UAV{sysid}", param, value)
 
 class DroneControlApp(QMainWindow):
     def __init__(self):
@@ -296,6 +321,7 @@ class DroneControlApp(QMainWindow):
         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.kbps_signal.connect(self.update_kbps)
         self.workers.start()
             
     def initUI(self):
@@ -326,17 +352,18 @@ class DroneControlApp(QMainWindow):
             # 狀態顯示
             self.uav_labels[namespace] = {
                 "status": QLabel("狀態：等待數據..."),
-                "battery": QLabel("電量：等待數據..."),
+                "battery": QLabel("電壓：等待數據..."),
+                "local_position": QLabel("Local Position：等待數據..."),
                 "gps": QLabel("GPS位置：等待數據...\n\n"),
                 "fix": QLabel("Fix Type：等待數據..."),
                 "satellites": QLabel("衛星數量：等待數據..."),
-                "local_position": QLabel("Local Position：等待數據..."),
                 "groundspeed": QLabel("地面速度：等待數據..."),
                 "pitch": QLabel("Pitch：等待數據..."),
                 "heading": QLabel("Heading：等待數據..."),
                 "ping": QLabel("Ping：等待數據..."),
                 "loss": QLabel("丟包：等待數據..."),
                 "frequency": QLabel("頻率：等待數據..."),
+                "kbps": QLabel("吞吐量：等待數據..."),
                 "param": QLabel("SR1_EXTRA1：等待數據...")
             }
 
@@ -370,25 +397,6 @@ class DroneControlApp(QMainWindow):
         mode_layout.addWidget(self.modeButton)
         main_layout.addLayout(mode_layout)
 
-        # XYZ 設置欄位
-        xyz_layout = QHBoxLayout()
-        self.positionX = QLineEdit()
-        self.positionX.setPlaceholderText("X")
-        self.positionY = QLineEdit()
-        self.positionY.setPlaceholderText("Y")
-        self.positionZ = QLineEdit()
-        self.positionZ.setPlaceholderText("Z")
-        xyz_layout.addWidget(QLabel("輸入位置："))
-        xyz_layout.addWidget(self.positionX)
-        xyz_layout.addWidget(self.positionY)
-        xyz_layout.addWidget(self.positionZ)
-        main_layout.addLayout(xyz_layout)
-
-        # 設置 XYZ 的按鈕
-        self.setPositionButton = QPushButton("設置位置")
-        self.setPositionButton.clicked.connect(self.set_local_position)
-        main_layout.addWidget(self.setPositionButton)
-
         # 解鎖按鈕
         fly_layout = QHBoxLayout()
         self.armButton = QPushButton("解鎖")
@@ -406,6 +414,25 @@ class DroneControlApp(QMainWindow):
         fly_layout.addWidget(self.landButton)
         main_layout.addLayout(fly_layout)
 
+        # XYZ 設置欄位
+        xyz_layout = QHBoxLayout()
+        self.positionX = QLineEdit()
+        self.positionX.setPlaceholderText("X")
+        self.positionY = QLineEdit()
+        self.positionY.setPlaceholderText("Y")
+        self.positionZ = QLineEdit()
+        self.positionZ.setPlaceholderText("Z")
+        self.setPositionButton = QPushButton("設置位置")
+        self.setPositionButton.clicked.connect(self.set_local_position)
+        xyz_layout.addWidget(QLabel("輸入位置："))
+        xyz_layout.addWidget(self.positionX)
+        xyz_layout.addWidget(self.positionY)
+        xyz_layout.addWidget(self.positionZ)
+        xyz_layout.addWidget(self.setPositionButton)
+        main_layout.addLayout(xyz_layout)
+
+        # 設置 XYZ 的按鈕
+
         #設置重啟按鈕
         self.rebootButton = QPushButton("重啟")
         self.rebootButton.clicked.connect(self.reboot_drone)
@@ -426,8 +453,8 @@ class DroneControlApp(QMainWindow):
     def update_state(self, namespace, mode):
         self.uav_labels[namespace]["status"].setText(f"狀態：{mode}")
 
-    def update_battery(self, namespace, percentage):
+    def update_battery(self, namespace, voltage):
-        self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%")
+        self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V")
 
     def update_gps(self, namespace, latitude, longitude):
         self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n    Lat:{latitude:.6f}° \n    Lon:{longitude:.6f}°")
@@ -465,6 +492,9 @@ class DroneControlApp(QMainWindow):
     def update_frequency(self, namespace, frequency):
         self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz")
 
+    def update_kbps(self, namespace, kbps):
+        self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s")
+
     def update_param(self, namespace, value):
         self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}")
 
@@ -488,10 +518,12 @@ class DroneControlApp(QMainWindow):
     def takeoff_drone(self):
         try:
             z_text = self.positionZ.text().strip()
-            altitude = float(z_text) if z_text else 5.0
+            z = float(z_text) if z_text else 5.0
-            for namespace in self.namespaces:
+            h = 2
+            for i, namespace in enumerate(self.namespaces):
                 if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.takeoff(namespace, altitude)
+                    self.workers.takeoff(namespace, z + h * i)
+            
         except ValueError:
             QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
 
@@ -505,9 +537,11 @@ class DroneControlApp(QMainWindow):
             x = float(self.positionX.text().strip())
             y = float(self.positionY.text().strip())
             z = float(self.positionZ.text().strip())
-            for namespace in self.namespaces:
+            h = 2
+            for i, namespace in enumerate(self.namespaces):
                 if self.uav_checkboxes[namespace].isChecked():
-                    self.workers.set_local_position(namespace, x, y, z)
+                    self.workers.set_local_position(namespace, x, y, z + h * i)
+            
         except ValueError:
             QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
 
@@ -529,8 +563,7 @@ class DroneControlApp(QMainWindow):
         except ValueError:
             QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！")
 
-def main(args=None):
+def main():
-    rclpy.init(args=args)
     app = QtWidgets.QApplication(sys.argv)
     window = DroneControlApp()
     window.show()