Upload files to 'src/unitdev01/unitdev01'

src/unitdev01/unitdev01/gui.py

@@ -41,48 +41,82 @@ class DroneMonitor(Node):
         # 定義需要過濾的模式
         self.filtered_modes = ['Mode(0x000000c0)']
 
-        # 啟動 WebSocket client 執行緒
+        # 定義多個 WebSocket 連接配置
-        threading.Thread(target=self.start_ws_client, daemon=True).start()
+        self.websocket_connections = [
+            {
+                'name': 'local_1',
+                'url': 'ws://192.168.137.1:5163',
+                'enabled': True
+            },
+            {
+                'name': 'local_2', 
+                'url': 'ws://0.0.0.0:8756',
+                'enabled': True  # 新增的端口
+            },
+            {
+                'name': 'remote_8756',
+                'url': 'ws://192.168.50.48:8756', 
+                'enabled': False  # 可選擇啟用
+            }
+        ]   
+        
+        # 啟動多個 WebSocket client 執行緒
+        for connection in self.websocket_connections:
+            if connection['enabled']:
+                threading.Thread(
+                    target=self.start_ws_client, 
+                    args=(connection,),
+                    daemon=True,
+                    name=f"WebSocket-{connection['name']}"
+                ).start()
 
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
 
-    def start_ws_client(self):
+    def start_ws_client(self, connection_config):
+        """啟動單個 WebSocket 客戶端"""
         asyncio.set_event_loop(asyncio.new_event_loop())
-        asyncio.get_event_loop().run_until_complete(self.ws_client_loop())
+        asyncio.get_event_loop().run_until_complete(
+            self.ws_client_loop(connection_config)
+        )
 
-    async def ws_client_loop(self):
+    async def ws_client_loop(self, connection_config):
+        """單個 WebSocket 連接的主循環"""
         retry_count = 0
         max_retries = 5
         base_delay = 1.0
-        local = "ws://0.0.0.0:8765"  # 本地 WebSocket 地址
+        connection_name = connection_config['name']
-        remote = "ws://192.168.50.48:8756"
+        url = connection_config['url']
+        
+        print(f"Starting WebSocket client for {connection_name} at {url}")
+        
         while retry_count < max_retries:
             try:
-                async with websockets.connect(local) as websocket:
+                async with websockets.connect(url) as websocket:
-                    print("WebSocket connected")
+                    print(f"WebSocket {connection_name} connected to {url}")
                     retry_count = 0  # 重置重試計數
                     
                     async for message in websocket:
                         try:
                             data = json.loads(message)
+                            # 添加連接來源信息
+                            data['_connection_source'] = connection_name
                             self.process_websocket_message(data)
                         except json.JSONDecodeError as e:
-                            print(f"WebSocket JSON decode error: {e}")
+                            print(f"WebSocket {connection_name} JSON decode error: {e}")
                         except Exception as e:
-                            print(f"WebSocket message processing error: {e}")
+                            print(f"WebSocket {connection_name} message processing error: {e}")
                             
             except websockets.exceptions.ConnectionClosedError:
-                print("WebSocket connection closed")
+                print(f"WebSocket {connection_name} connection closed")
                 break
             except Exception as e:
                 retry_count += 1
                 delay = base_delay * (2 ** min(retry_count, 4))  # 指數退避
-                print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
+                print(f"WebSocket {connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
                 await asyncio.sleep(delay)
         
-        print("WebSocket client stopped after maximum retries")
+        print(f"WebSocket client {connection_name} stopped after maximum retries")
-
     
     def process_websocket_message(self, data):
         try:
@@ -446,7 +480,7 @@ class ControlStationUI(QMainWindow):
         scroll = QScrollArea()
         scroll.setWidget(self.drone_panel_container)
         scroll.setWidgetResizable(True)
-        self.left_tab.addTab(scroll, "無人機")
+        self.left_tab.addTab(scroll, "無人載具")
         
         # 底部控制按鈕區域
         bottom_control = QWidget()
@@ -594,14 +628,37 @@ class ControlStationUI(QMainWindow):
             <script src="https://unpkg.com/leaflet-rotatedmarker/leaflet.rotatedMarker.js"></script>
             <style>
                 html, body, #map { height: 100%; margin: 0; }
+                .map-controls {
+                    position: absolute;
+                    top: 10px;
+                    right: 10px;
+                    z-index: 1000;
+                }
+                .control-button {
+                    padding: 8px 12px;
+                    background-color: #f44336;
+                    color: white;
+                    border: none;
+                    border-radius: 4px;
+                    cursor: pointer;
+                    font-size: 12px;
+                    box-shadow: 0 2px 5px rgba(0,0,0,0.2);
+                }
+                .control-button:hover {
+                    background-color: #d32f2f;
+                }
             </style>
             </head>
             <body>
             <div id="map"></div>
+            <div class="map-controls">
+                <button class="control-button" onclick="clearAllTrajectories()">清除軌跡</button>
+            </div>
+
             <script>
-                var map = L.map('map').setView([0, 0], 20); 
+                var map = L.map('map').setView([0, 0], 19); 
                 L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
-                    maxZoom: 19,  // OpenStreetMap 支持的最大縮放級別
+                    maxZoom: 19,
                     attribution: '© OpenStreetMap contributors'
                 }).addTo(map);
 
@@ -612,17 +669,16 @@ class ControlStationUI(QMainWindow):
                 });
 
                 function getColorBySocketId(id) {
-                    if (id.startsWith("s0_")) return "#00BFFF";  // DeepSkyBlue
+                    if (id.startsWith("s0_")) return "#00BFFF";
-                    if (id.startsWith("s1_")) return "#FFD700";  // Gold
+                    if (id.startsWith("s1_")) return "#FFD700";
-                    if (id.startsWith("s2_")) return "#FF69B4";  // HotPink
+                    if (id.startsWith("s2_")) return "#FF69B4";
-                    if (id.startsWith("s9_")) return "#7CFC00";  // LawnGreen
+                    if (id.startsWith("s9_")) return "#7CFC00";
-                    return "#AAAAAA";  // Default
+                    return "#AAAAAA";
                 }
 
-                // 新增 ID 標籤的圖標
                 function createIdIcon(id) {
                     const color = getColorBySocketId(id);
-                    const sysid = id.split('_')[1];  // e.g., 's3_2' → '2'
+                    const sysid = id.split('_')[1];
                     return L.divIcon({
                         className: 'drone-id',
                         html: `<div style="
@@ -644,12 +700,39 @@ class ControlStationUI(QMainWindow):
                     });
                 }
 
-                var markers = {};                // 儲存所有無人機
+                var markers = {};
-                var idLabels = {};    // 新增：儲存 ID 標籤
+                var idLabels = {};
-                var focusedId = null;           // 目前被鎖定的 sysid
+                var trajectories = {};
-                var initialized = false;        // 是否完成首次初始化
+                var trajectoryLines = {};
+                var focusedId = null;
+                var initialized = false;
+                var trajectoryGroup = L.layerGroup().addTo(map);
+
+                function initTrajectory(id) {
+                    if (!trajectories[id]) {
+                        trajectories[id] = [];
+                        const color = getColorBySocketId(id);
+                        trajectoryLines[id] = L.polyline([], {
+                            color: color,
+                            weight: 3,
+                            opacity: 0.7,
+                            smoothFactor: 1
+                        }).addTo(trajectoryGroup);
+                    }
+                }
+
+                function addTrajectoryPoint(id, lat, lon) {
+                    initTrajectory(id);
+                    const point = [lat, lon];
+                    trajectories[id].push(point);
+                    
+                    if (trajectories[id].length > 1000) {
+                        trajectories[id].shift();
+                    }
+                    
+                    trajectoryLines[id].setLatLngs([...trajectories[id]]);
+                }
 
-                // ⬆️ 鎖定指定無人機（置中地圖並彈出名稱）
                 function focusOn(id) {
                     if (!markers[id]) return;
                     focusedId = id;
@@ -657,23 +740,29 @@ class ControlStationUI(QMainWindow):
                     map.flyTo(latlng, map.getZoom());
                 }
 
-                // 🔁 定期重新鎖定 focusedId
                 setInterval(() => {
                     if (focusedId && markers[focusedId]) {
                         var latlng = markers[focusedId].getLatLng();
-                        map.panTo(latlng);  // 或 flyTo / setView
+                        map.panTo(latlng);
                     }
-                }, 1000); // 每秒更新一次視角
+                }, 1000);
 
-                // 📡 更新／新增無人機 marker
                 function updateDrone(lat, lon, id, heading) {
                     if (markers[id]) {
+                        const lastPos = markers[id].getLatLng();
+                        const distance = lastPos.distanceTo([lat, lon]);
+                        if (distance > 1) {
+                            addTrajectoryPoint(id, lat, lon);
+                        }
+                        
                         markers[id]
                             .setLatLng([lat, lon])
                             .setRotationAngle(heading);
-                        // 更新 ID 標籤位置
                         idLabels[id].setLatLng([lat, lon]);
                     } else {
+                        initTrajectory(id);
+                        addTrajectoryPoint(id, lat, lon);
+                        
                         markers[id] = L.marker([lat, lon], {
                             icon: arrowIcon,
                             rotationAngle: heading,
@@ -682,28 +771,32 @@ class ControlStationUI(QMainWindow):
                         .on('click', function () {
                             focusOn(id);
                         })
-                        .addTo(map)
+                        .addTo(map);
 
-                        // 新增 ID 標籤
                         idLabels[id] = L.marker([lat, lon], {
                             icon: createIdIcon(id),
-                            zIndexOffset: 1000  // 確保 ID 標籤在箭頭上方
+                            zIndexOffset: 1000
                         })
                         .on('click', function() {
                             focusOn(id);
                         })
                         .addTo(map);
 
-                        // 🧭 第一次加入 → 若未初始化，則以 sysid 最小的初始化
                         if (!initialized || id < focusedId) {
                             focusOn(id);
-                            markers[id]
-                            .setLatLng([lat, lon])
-                            .setRotationAngle(heading);
                             initialized = true;
                         }
                     }
                 }
+
+                function clearAllTrajectories() {
+                    trajectories = {};
+                    Object.values(trajectoryLines).forEach(line => {
+                        trajectoryGroup.removeLayer(line);
+                    });
+                    trajectoryLines = {};
+                    console.log('所有軌跡已清除');
+                }
             </script>
             </body>
             </html>
@@ -1167,18 +1260,16 @@ class ControlStationUI(QMainWindow):
         elif msg_type == 'battery':
             voltage = data.get('voltage', 16)
             
-            # 使用標準電壓判斷電池節數
+            # 判斷電池節數
-            cell_max = 4.2
             cells = round(voltage / 3.95)
-            max = cell_max * cells
 
             # 計算電量百分比
-            percentage = voltage / max * 100
+            percentage = (voltage / cells - 3.7) / 0.5 * 100
             
             # 根據百分比設置顏色
             if percentage < 20:
                 voltage_color = '#FF6464'  # 紅色 (低電量)
-            elif percentage < 40:
+            elif percentage < 50:
                 voltage_color = '#FFA500'  # 橘色 (中低電量)
             else:
                 voltage_color = '#FFFFFF'  # 白色 (正常電量)