update
This commit is contained in:
@@ -131,7 +131,7 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
|
|||||||
ap = None
|
ap = None
|
||||||
|
|
||||||
def _read_state():
|
def _read_state():
|
||||||
"""从接口读取状态:alt, surface_speed(标量), vertical_speed, mass, available_thrust, g"""
|
"""从接口读取状态:alt, surface_speed(标量), vertical_speed, mass, available_thrust, g, situation_name"""
|
||||||
flight = vessel.flight(surface_frame) if surface_frame is not None else vessel.flight()
|
flight = vessel.flight(surface_frame) if surface_frame is not None else vessel.flight()
|
||||||
alt = float(getattr(flight, 'surface_altitude', 0.0))
|
alt = float(getattr(flight, 'surface_altitude', 0.0))
|
||||||
srf_speed = float(getattr(flight, 'speed', 0.0))
|
srf_speed = float(getattr(flight, 'speed', 0.0))
|
||||||
@@ -139,7 +139,11 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
|
|||||||
mass = float(getattr(vessel, 'mass', 0.0))
|
mass = float(getattr(vessel, 'mass', 0.0))
|
||||||
avail_thrust = float(getattr(vessel, 'available_thrust', 0.0))
|
avail_thrust = float(getattr(vessel, 'available_thrust', 0.0))
|
||||||
g_local = _get_g(vessel)
|
g_local = _get_g(vessel)
|
||||||
return alt, srf_speed, v_speed, mass, avail_thrust, g_local
|
try:
|
||||||
|
situation_name = str(getattr(getattr(vessel, 'situation', None), 'name', ''))
|
||||||
|
except Exception:
|
||||||
|
situation_name = ''
|
||||||
|
return alt, srf_speed, v_speed, mass, avail_thrust, g_local, situation_name
|
||||||
|
|
||||||
try:
|
try:
|
||||||
# 初始确保不点火
|
# 初始确保不点火
|
||||||
@@ -152,7 +156,7 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
|
|||||||
ignited = False
|
ignited = False
|
||||||
last_log_t = 0.0
|
last_log_t = 0.0
|
||||||
while True:
|
while True:
|
||||||
alt, srf_speed, v_speed, mass, avail_thrust, g_local = _read_state()
|
alt, srf_speed, v_speed, mass, avail_thrust, g_local, situation_name = _read_state()
|
||||||
remaining_alt = max(0.0, alt - float(target_altitude))
|
remaining_alt = max(0.0, alt - float(target_altitude))
|
||||||
|
|
||||||
# v:使用 surface speed(标量)作为“需要刹掉的速度”
|
# v:使用 surface speed(标量)作为“需要刹掉的速度”
|
||||||
@@ -208,22 +212,34 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
|
|||||||
|
|
||||||
# 燃烧闭环:根据 remaining_alt 反推需要的减速度 -> 油门
|
# 燃烧闭环:根据 remaining_alt 反推需要的减速度 -> 油门
|
||||||
while True:
|
while True:
|
||||||
alt, srf_speed, v_speed, mass, avail_thrust, g_local = _read_state()
|
alt, srf_speed, v_speed, mass, avail_thrust, g_local, situation_name = _read_state()
|
||||||
remaining_alt = max(0.0, alt - float(target_altitude))
|
remaining_alt = max(0.0, alt - float(target_altitude))
|
||||||
|
|
||||||
v = max(0.0, float(srf_speed))
|
v = max(0.0, float(srf_speed))
|
||||||
vf = max(0.0, abs(float(target_vspeed)))
|
vf = max(0.0, abs(float(target_vspeed)))
|
||||||
|
|
||||||
# 终止条件(接地附近 + 速度已足够小)
|
# 终止条件:以 vessel.situation.name == 'landed' 为准
|
||||||
if remaining_alt <= max(0.2, float(target_altitude)) and v <= (vf + 0.5):
|
if str(situation_name).strip().lower() == 'landed':
|
||||||
try:
|
try:
|
||||||
vessel.control.throttle = 0.0
|
vessel.control.throttle = 0.0
|
||||||
except Exception:
|
except Exception:
|
||||||
pass
|
pass
|
||||||
status['result'] = 'landed'
|
status['result'] = 'landed'
|
||||||
status['logs'].append('预测着陆完成')
|
status['logs'].append('检测到 vessel.situation=LANDED,着陆完成')
|
||||||
if verbose:
|
if verbose:
|
||||||
print('预测着陆完成')
|
print('检测到 vessel.situation=LANDED,着陆完成')
|
||||||
|
break
|
||||||
|
|
||||||
|
# 兜底:极低高度 + 速度足够小(防止 situation 迟迟不切换导致卡死)
|
||||||
|
if remaining_alt <= max(0.2, float(target_altitude)) and v <= (vf + 0.5):
|
||||||
|
try:
|
||||||
|
vessel.control.throttle = 0.0
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
status['result'] = 'touchdown_pending'
|
||||||
|
status['logs'].append('达到高度/速度阈值,但 vessel.situation 仍未变为 LANDED')
|
||||||
|
if verbose:
|
||||||
|
print('达到高度/速度阈值,但 vessel.situation 仍未变为 LANDED')
|
||||||
break
|
break
|
||||||
|
|
||||||
# 计算所需净向上加速度
|
# 计算所需净向上加速度
|
||||||
@@ -258,7 +274,7 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ
|
|||||||
status['logs'].append(msg)
|
status['logs'].append(msg)
|
||||||
print(msg)
|
print(msg)
|
||||||
|
|
||||||
time.sleep(0.05)
|
time.sleep(0.01)
|
||||||
|
|
||||||
finally:
|
finally:
|
||||||
try:
|
try:
|
||||||
|
|||||||
Reference in New Issue
Block a user