fix: 修正實飛 Circle 高度爆衝與 Leader-Follower 卡點

1. mavlinkROS2Nodes: position_gnss topic 改發 relative_altitude 而非 AMSL,
   讓 drone_gps['alt'] 在實飛與 SITL 都代表「相對 home 高度」
2. mission_planner: Circle stage1.z 改用 final_z(不再用 current_z 做中點插值),
   避免實飛時 current_z 是 AMSL 而把過渡高度衝到海拔值的一半
3. mission_executor: 到達判定改為 hover-stable(radius * 1.5 hysteresis +
   穩定計時),預設 arrival_radius 2→4m,解決實飛 GPS 抖動永遠進不了
   2m radius 的卡死問題
4. mission_executor: 新增 progress log,每 3 秒印一行距離與穩定度,
   方便實飛 debug 卡點問題
5. mission_group + gui: 暴露 arrival_radius / hover_stable_sec 給 GUI 任務參數面板,
   每次 start 重讀以支援中途調整

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
2026-06-08 16:51:19 +08:00
co-authored by Claude Opus 4.7
parent 5231cffcb2
commit 415aed088a
5 changed files with 125 additions and 13 deletions
+4 -2
View File
@@ -193,11 +193,13 @@ class FormationPlanner:
final_y = center_y + radius * math.sin(angle_rad)
final_z = altitude
current_x, current_y, current_z = drone_positions[i]
current_x, current_y, _current_z = drone_positions[i]
# stage1 高度直接用 final_z:先爬升到目標高度再水平集合,
# 避免用 current_z 做中點插值(current_z 在實飛時可能是 AMSL 而非相對 home,會導致衝高)
stage1_positions.append((
current_x + (final_x - current_x) * 0.5,
current_y + (final_y - current_y) * 0.5,
current_z + (final_z - current_z) * 0.5
final_z,
))
stage2_positions.append((final_x, final_y, final_z))