update
This commit is contained in:
@@ -105,7 +105,8 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
|
|||||||
# 低空/接地参数(用于“最后几米慢下来”)
|
# 低空/接地参数(用于“最后几米慢下来”)
|
||||||
touchdown_vspeed_tol = 0.6 # m/s:认为“软着陆”的竖直速度容差
|
touchdown_vspeed_tol = 0.6 # m/s:认为“软着陆”的竖直速度容差
|
||||||
low_altitude_pid_switch = 10.0 # m:低于该高度切换到竖直速度控制
|
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):
|
def _get_g(local_vessel):
|
||||||
try:
|
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):
|
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:
|
else:
|
||||||
# 中高空:用距离反推所需净向上加速度(使用竖直下降速度)
|
# 中高空:用距离反推所需净向上加速度(使用竖直下降速度)
|
||||||
if remaining_alt <= 1e-3:
|
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:
|
if alt < 5.0 and v_down > 10.0:
|
||||||
throttle = 1.0
|
throttle = 1.0
|
||||||
|
|
||||||
|
# 若已经触发 LANDED 但竖直速度仍偏大,避免油门过低导致“硬砸/反弹”
|
||||||
|
if is_landed and v_down > (vf_down + float(touchdown_vspeed_tol)):
|
||||||
|
throttle = max(throttle, 0.8)
|
||||||
|
|
||||||
try:
|
try:
|
||||||
vessel.control.throttle = throttle
|
vessel.control.throttle = throttle
|
||||||
except Exception:
|
except Exception:
|
||||||
|
|||||||
Reference in New Issue
Block a user