This commit is contained in:
2026-01-14 23:48:11 +08:00
parent d7a93b88ef
commit 915335ecc5
+15 -3
View File
@@ -105,7 +105,8 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
# 低空/接地参数(用于“最后几米慢下来”)
touchdown_vspeed_tol = 0.6 # m/s:认为“软着陆”的竖直速度容差
low_altitude_pid_switch = 10.0 # m:低于该高度切换到竖直速度控制
vspeed_p_gain = 0.6 # 比例增益:越大越积极(0.3~1.0 之间可调
vspeed_p_gain = 1.2 # 比例增益:越大越积极(建议 0.6~2.0
low_altitude_ff_gain = 0.3 # 低空前馈增益:按“需要的减速距离/剩余距离”做额外加速度
def _get_g(local_vessel):
try:
@@ -254,8 +255,15 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
# 低空策略:切换到“竖直速度跟踪”以确保最后几米把速度压到目标值
if alt <= float(low_altitude_pid_switch):
# 需要的净向上加速度:a_net_up = k * (v_down - vf_down)
a_req_up = max(0.0, float(vspeed_p_gain) * max(0.0, (v_down - vf_down)))
# 低空:比例 + 前馈(用当前速度估算“需要多少距离才能刹到目标速度”)
# a_p:把竖直速度拉向目标
a_p = float(vspeed_p_gain) * max(0.0, (v_down - vf_down))
# a_ff:基于能量/距离的前馈,防止在最后几米才突然补救
if remaining_alt <= 1e-3:
a_ff = 0.0
else:
a_ff = float(low_altitude_ff_gain) * max(0.0, (v_down * v_down - vf_down * vf_down) / (2.0 * remaining_alt))
a_req_up = max(0.0, a_p + a_ff)
else:
# 中高空:用距离反推所需净向上加速度(使用竖直下降速度)
if remaining_alt <= 1e-3:
@@ -275,6 +283,10 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
if alt < 5.0 and v_down > 10.0:
throttle = 1.0
# 若已经触发 LANDED 但竖直速度仍偏大,避免油门过低导致“硬砸/反弹”
if is_landed and v_down > (vf_down + float(touchdown_vspeed_tol)):
throttle = max(throttle, 0.8)
try:
vessel.control.throttle = throttle
except Exception: