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:
@@ -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))
|
||||
|
||||
|
||||
Reference in New Issue
Block a user