From 8c62f891a8d55d967d21ddfb7fc1d06540d88ceb Mon Sep 17 00:00:00 2001 From: wenchun Date: Mon, 25 May 2026 18:39:05 +0800 Subject: [PATCH] =?UTF-8?q?fix(GUI):=20=E4=BF=AE=E6=AD=A3=E5=AF=A6?= =?UTF-8?q?=E9=A3=9B=E4=B8=89=E5=80=8B=E9=97=9C=E9=8D=B5=20bug=20=E2=80=94?= =?UTF-8?q?=20=E9=AB=98=E5=BA=A6=E5=81=8F=E7=A7=BB=E7=B4=AF=E5=8A=A0?= =?UTF-8?q?=E3=80=81Leader-Follower=20=E4=B8=8D=E5=8B=95=E3=80=81=E8=B5=B7?= =?UTF-8?q?=E9=BB=9E=E9=87=8D=E7=96=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1. mission_planner: origin_alt 固定為 0，避免 GPS 橢球高度疊加到相對高度指令 2. mission_planner: 移除圓弧插值，直接用路徑點作為航點，消除微航點導致飛控無法執行 3. mission_planner: lookup 改為外推（extrapolate），解決偶數 rank 起終點重疊 4. communication: UDP/Serial 的 GLOBAL_POSITION_INT 補上 relative_alt 欄位 Co-Authored-By: Claude Opus 4.6 --- src/GUI/communication.py | 8 +- src/GUI/mission_planner.py | 247 +++++++------------------------------ 2 files changed, 52 insertions(+), 203 deletions(-) diff --git a/src/GUI/communication.py b/src/GUI/communication.py index a4f4102..adeec30 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -287,9 +287,11 @@ class UDPMavlinkReceiver(threading.Thread): elif msg_type == "GLOBAL_POSITION_INT": latitude = msg.lat / 1e7 longitude = msg.lon / 1e7 + relative_alt = msg.relative_alt / 1000.0 self.signals.update_signal.emit('gps', drone_id, { 'lat': latitude, - 'lon': longitude + 'lon': longitude, + 'alt': relative_alt, }) elif msg_type == "GPS_RAW_INT": @@ -499,9 +501,11 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): elif msg_type == "GLOBAL_POSITION_INT": latitude = msg.lat / 1e7 longitude = msg.lon / 1e7 + relative_alt = msg.relative_alt / 1000.0 self.signals.update_signal.emit('gps', drone_id, { 'lat': latitude, - 'lon': longitude + 'lon': longitude, + 'alt': relative_alt, }) elif msg_type == "GPS_RAW_INT": diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py index 29ed44e..c43d2c2 100644 --- a/src/GUI/mission_planner.py +++ b/src/GUI/mission_planner.py @@ -83,10 +83,11 @@ class FormationPlanner: center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions) center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions) - center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions) - self.current_origin = (center_lat, center_lon, center_alt) - self.converter = CoordinateConverter(center_lat, center_lon, center_alt) + # origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT + # 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移 + self.current_origin = (center_lat, center_lon, 0.0) + self.converter = CoordinateConverter(center_lat, center_lon, 0.0) drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions] target_local = self.converter.gps_to_local(*target_gps) @@ -206,29 +207,17 @@ class FormationPlanner: def _calculate_leader_follower(self, drone_positions, route_wps_local, params): """ - 路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length) - - 核心想法： - - center_path 建好後計算每個 sample 的累積弧長 s - - 每架 drone 給 (lat_k, lon_k) 的隊形偏移 - - 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k - - follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent) - - 超過 path 起點/終點以 clip 處理，避免倒退或衝出 - - 解決前版「空間偏移」的三個 bug： - 1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退 - 2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup - 3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線 - - Rank 定序規則： - **以 drone_positions 的輸入順序為準**（= GUI 選取順序）。 - drone_positions[0] = rank 0 = leader (lon=0)， - drone_positions[1] = rank 1 (lon=5)，依此類推。 - 刻意「不」用位置投影自動排序，避免浮點噪音造成 run-to-run - leader 漂移，以確保整個 mission 像軍隊縱隊那樣順序固定。 - - 隊形 (俯視, 以路徑行進方向為前): - D0 (lat=-L, lon=0) ← front-right (leader) + 路徑跟隨編隊 — 虛擬領隊 / 弧長參數化 + + center_path = 使用者畫的路徑點（直線段連接，不插弧）， + 每架 drone 沿弧長 s 參數化做隊形偏移： + s_follower = s_leader - lon_k + position = lookup(s_follower) + lat_k × perp(tangent) + s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。 + + Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。 + 隊形 (俯視): + D0 (lat=-L, lon=0) ← leader D1 (lat=+L, lon=5) D2 (lat=-L, lon=10) D3 (lat=+L, lon=15) @@ -237,29 +226,16 @@ class FormationPlanner: lateral_offset = params.get('lateral_offset', 3.0) longitudinal_spacing = params.get('longitudinal_spacing', 5.0) altitude = params.get('altitude', self.base_altitude) - min_inner_radius = params.get('min_inner_radius', 3.0) - arc_resolution = params.get('arc_resolution', 12) - sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0) # cos(90°)=0 - - # Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s） - center_path, barrier_indices = self._build_center_path( - route_wps_local, - formation_max_lateral=lateral_offset, - min_inner_radius=min_inner_radius, - arc_resolution=arc_resolution, - sharp_turn_cos_threshold=sharp_turn_cos, - ) + + # Step 1: 建立中心路徑（直線段連接，每個中間航點設 barrier） + center_path, barrier_indices = self._build_center_path(route_wps_local) s_list = [pt['s'] for pt in center_path] s_max = s_list[-1] n_pts = len(center_path) def lookup(s_target): - """ - 在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。 - s<0 → clip 到 start；s>s_max → clip 到 end。 - tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。 - """ + """在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。超出範圍時沿端點方向外推。""" if n_pts == 1: pt = center_path[0] return pt['x'], pt['y'], pt['dir'] @@ -270,7 +246,8 @@ class FormationPlanner: sdx, sdy = b['x'] - a['x'], b['y'] - a['y'] slen = math.hypot(sdx, sdy) if slen > 1e-6: - return a['x'], a['y'], (sdx / slen, sdy / slen) + dx, dy = sdx / slen, sdy / slen + return a['x'] + s_target * dx, a['y'] + s_target * dy, (dx, dy) return a['x'], a['y'], a['dir'] if s_target >= s_max: @@ -279,7 +256,9 @@ class FormationPlanner: sdx, sdy = b['x'] - a['x'], b['y'] - a['y'] slen = math.hypot(sdx, sdy) if slen > 1e-6: - return b['x'], b['y'], (sdx / slen, sdy / slen) + dx, dy = sdx / slen, sdy / slen + overshoot = s_target - s_max + return b['x'] + overshoot * dx, b['y'] + overshoot * dy, (dx, dy) return b['x'], b['y'], b['dir'] # Binary search：s_list[lo] <= s_target < s_list[hi] @@ -308,16 +287,6 @@ class FormationPlanner: # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定 all_waypoints = [None] * N - # 預先算好路徑終點的切線（給收尾點用） - end = center_path[-1] - end_dx, end_dy = end['dir'] - if n_pts >= 2: - a = center_path[-2] - sdx, sdy = end['x'] - a['x'], end['y'] - a['y'] - slen = math.hypot(sdx, sdy) - if slen > 1e-6: - end_dx, end_dy = sdx / slen, sdy / slen - for rank in range(N): lat_sign = -1 if rank % 2 == 0 else 1 lat = lat_sign * lateral_offset @@ -334,46 +303,16 @@ class FormationPlanner: altitude, )) - # 收尾：全員到 path 終點的 rank 位置（lon 歸零） - waypoints.append(( - end['x'] + lat * (-end_dy), - end['y'] + lat * end_dx, - altitude, - )) - all_waypoints[rank] = waypoints - # barrier 索引仍指向 center_path 內 (range [0, n_pts-1])， - # 不觸及末尾收尾點，直接沿用即可 return all_waypoints, barrier_indices - def _build_center_path(self, waypoints, - formation_max_lateral, - min_inner_radius, - arc_resolution, - sharp_turn_cos_threshold): + def _build_center_path(self, waypoints): """ - 建立以「圓弧」連接直線段的中心路徑。 - - 每個中間 waypoint 的處理： - 1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out) - 2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)， - barrier 放在該點，讓隊伍整體停下再繼續 - 3. 否則 → 平滑弧： - R_base = formation_max_lateral + min_inner_radius - d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限 - R_actual = d / tan(β/2) - tangent points T_in = WP - u_in·d, T_out = WP + u_out·d - arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW - arc 由 theta_in 到 theta_out 以 arc_resolution 等分 - path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)] - barrier 放在 T_out (轉完彎後的集合點) - - path 是一個 list[dict]，每個 dict 至少含: - {'seg': 'straight' | 'arc', - 'x': float, 'y': float, - 'dir': (dx, dy)} - 'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta' + 建立中心路徑 — 直接用路徑點連接，不插弧。 + + 每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。 + 飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。 Returns: (path, barrier_indices) @@ -385,129 +324,35 @@ class FormationPlanner: 'seg': 'straight', 'x': waypoints[0][0], 'y': waypoints[0][1], 'dir': (0.0, 1.0), + 's': 0.0, }], [] - segments = [] - for i in range(num_wps - 1): - dx = waypoints[i + 1][0] - waypoints[i][0] - dy = waypoints[i + 1][1] - waypoints[i][1] - length = math.hypot(dx, dy) - if length < 0.01: - segments.append(((0.0, 1.0), length)) - else: - segments.append(((dx / length, dy / length), length)) - - R_base = formation_max_lateral + min_inner_radius path = [] barrier_indices = [] - # 起點 - path.append({ - 'seg': 'straight', - 'x': waypoints[0][0], 'y': waypoints[0][1], - 'dir': segments[0][0], - }) - - for i in range(1, num_wps - 1): - u_in, len_in = segments[i - 1] - u_out, len_out = segments[i] - wp = waypoints[i] - - dot = u_in[0] * u_out[0] + u_in[1] * u_out[1] - dot = max(-1.0, min(1.0, dot)) - - # 銳角：hover + 原地轉向 - if dot < sharp_turn_cos_threshold: - avg_dx = u_in[0] + u_out[0] - avg_dy = u_in[1] + u_out[1] - avg_len = math.hypot(avg_dx, avg_dy) - if avg_len > 0.01: - avg_dir = (avg_dx / avg_len, avg_dy / avg_len) - else: - avg_dir = u_in - path.append({ - 'seg': 'straight', - 'x': wp[0], 'y': wp[1], - 'dir': avg_dir, - }) - barrier_indices.append(len(path) - 1) - continue - - # 平滑弧 - beta = math.acos(dot) - if beta < 1e-4: - # 幾乎直線，不插弧 - path.append({ - 'seg': 'straight', - 'x': wp[0], 'y': wp[1], - 'dir': u_in, - }) - continue - - half_tan = math.tan(beta / 2.0) - d_ideal = R_base * half_tan - d_max = 0.45 * min(len_in, len_out) - d = min(d_ideal, d_max) - R_actual = d / half_tan - - T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d) - T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d) - - # +1 CCW (左轉) / -1 CW (右轉) - cross = u_in[0] * u_out[1] - u_in[1] * u_out[0] - sign = 1 if cross > 0 else -1 - - # 入向左法線 - n_left = (-u_in[1], u_in[0]) - center = ( - T_in[0] + R_actual * sign * n_left[0], - T_in[1] + R_actual * sign * n_left[1], - ) - - theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0]) + for i in range(num_wps): + if i < num_wps - 1: + dx = waypoints[i + 1][0] - waypoints[i][0] + dy = waypoints[i + 1][1] - waypoints[i][1] + else: + dx = waypoints[i][0] - waypoints[i - 1][0] + dy = waypoints[i][1] - waypoints[i - 1][1] - # T_in (straight, 方向 u_in) - path.append({ - 'seg': 'straight', - 'x': T_in[0], 'y': T_in[1], - 'dir': u_in, - }) + length = math.hypot(dx, dy) + if length < 0.01: + direction = (0.0, 1.0) + else: + direction = (dx / length, dy / length) - # 弧段採樣 k=1..arc_resolution-1（不含兩端） - for k in range(1, arc_resolution): - t = k / arc_resolution - theta = theta_in + sign * beta * t - px = center[0] + R_actual * math.cos(theta) - py = center[1] + R_actual * math.sin(theta) - # 切線: sign × (-sin θ, cos θ) - tx = sign * (-math.sin(theta)) - ty = sign * math.cos(theta) - path.append({ - 'seg': 'arc', - 'x': px, 'y': py, - 'dir': (tx, ty), - 'arc_center': center, - 'arc_R': R_actual, - 'arc_sign': sign, - 'theta': theta, - }) - - # T_out (straight, 方向 u_out)，barrier 放這 path.append({ 'seg': 'straight', - 'x': T_out[0], 'y': T_out[1], - 'dir': u_out, + 'x': waypoints[i][0], 'y': waypoints[i][1], + 'dir': direction, }) - barrier_indices.append(len(path) - 1) - # 終點 - path.append({ - 'seg': 'straight', - 'x': waypoints[-1][0], 'y': waypoints[-1][1], - 'dir': segments[-1][0], - }) + if 0 < i < num_wps - 1: + barrier_indices.append(len(path) - 1) - # 後處理：為每個 sample 加上累積 polyline 弧長 s path[0]['s'] = 0.0 for i in range(1, len(path)): prev = path[i - 1]