update
This commit is contained in:
@@ -0,0 +1,377 @@
|
|||||||
|
"""
|
||||||
|
KRPC integration helpers
|
||||||
|
提供一个函数 run_flight_sequence 来连接本地 kRPC 服务,并按照用户需求控制当前活动载具。
|
||||||
|
需要安装 krpc: pip install krpc
|
||||||
|
|
||||||
|
行为:
|
||||||
|
1) 点火并将油门设为 70%,垂直向上飞行 40s
|
||||||
|
2) 计算并设定维持 1G 的油门(若计算失败回退到 50%),航向 90,俯仰 45°,维持 20s
|
||||||
|
3) 将油门提升到 100%,航向 270,俯仰 70°,维持 15s
|
||||||
|
|
||||||
|
注意:不同飞行环境(行星/轨道)下重力与最大推力不同,计算油门只是近似。
|
||||||
|
"""
|
||||||
|
|
||||||
|
import time
|
||||||
|
|
||||||
|
try:
|
||||||
|
import krpc
|
||||||
|
except Exception: # 保持导入失败时的可读性
|
||||||
|
krpc = None
|
||||||
|
|
||||||
|
|
||||||
|
def run_flight_sequence(host='localhost', port=50000, ignite_stage=True, verbose=True):
|
||||||
|
"""连接 kRPC 并执行飞行序列。
|
||||||
|
|
||||||
|
参数:
|
||||||
|
- host, port: krpc 服务器地址和端口
|
||||||
|
- ignite_stage: 是否在开始时执行一次 stage 激活以点火(默认 True)
|
||||||
|
- verbose: 是否打印运行信息
|
||||||
|
|
||||||
|
返回: dict 状态信息
|
||||||
|
"""
|
||||||
|
if krpc is None:
|
||||||
|
raise RuntimeError('krpc 库未安装或不可用,运行前请 pip install krpc')
|
||||||
|
|
||||||
|
conn = krpc.connect(name='general_tools_flight_sequence', address=host, rpc_port=port)
|
||||||
|
sc = conn.space_center
|
||||||
|
|
||||||
|
vessel = sc.active_vessel
|
||||||
|
if vessel is None:
|
||||||
|
conn.close()
|
||||||
|
raise RuntimeError('未找到活动载具')
|
||||||
|
|
||||||
|
# 预先包含 result 字段,使用字符串默认值以保持类型一致
|
||||||
|
status = {'steps': [], 'result': ''}
|
||||||
|
|
||||||
|
try:
|
||||||
|
# 保险起见先把油门设为 0
|
||||||
|
vessel.control.throttle = 0.0
|
||||||
|
time.sleep(0.1)
|
||||||
|
|
||||||
|
if ignite_stage:
|
||||||
|
if verbose:
|
||||||
|
print('激活第一个 stage 以点火(如适用)')
|
||||||
|
try:
|
||||||
|
# 在分级前先设置目标油门,这样发动机激活后会立刻使用该油门
|
||||||
|
vessel.control.throttle = 0.7
|
||||||
|
vessel.control.activate_next_stage()
|
||||||
|
# activate_next_stage 可能改变当前活动载具(比如 decouple 导致控制权转移),
|
||||||
|
# 重新获取 active_vessel 保证后续写入作用在当前载具上
|
||||||
|
time.sleep(0.05)
|
||||||
|
vessel = sc.active_vessel
|
||||||
|
except Exception:
|
||||||
|
# 某些情况下 activate_next_stage 可能无效,继续也没问题
|
||||||
|
if verbose:
|
||||||
|
print('activate_next_stage 失败,继续执行')
|
||||||
|
|
||||||
|
# 使用 streams 优先监控并确保油门生效
|
||||||
|
thrust_ok = _ensure_throttle_applied(conn, sc, desired_throttle=0.7, timeout=10.0, verbose=verbose)
|
||||||
|
if not thrust_ok:
|
||||||
|
if verbose:
|
||||||
|
print('警告:通过 streams/轮询仍未检测到油门生效,尝试更激进的写入(_force_apply_throttle)并输出发动机诊断')
|
||||||
|
try:
|
||||||
|
forced = _force_apply_throttle(conn, sc, desired_throttle=0.7, timeout=5.0, verbose=verbose)
|
||||||
|
except Exception:
|
||||||
|
forced = False
|
||||||
|
if verbose:
|
||||||
|
print(f'_force_apply_throttle 返回: {forced}')
|
||||||
|
try:
|
||||||
|
print(_diagnose_engines(sc.active_vessel))
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
time.sleep(0.15)
|
||||||
|
|
||||||
|
# 1) 推力 70%,垂直向上 40s
|
||||||
|
t0 = time.time()
|
||||||
|
vessel.control.throttle = 0.7
|
||||||
|
# 设置自动驾驶为垂直向上(俯仰 90°),保持当前航向
|
||||||
|
flight = vessel.flight()
|
||||||
|
heading = flight.heading if hasattr(flight, 'heading') else 0.0
|
||||||
|
ap = vessel.auto_pilot
|
||||||
|
try:
|
||||||
|
ap.engage()
|
||||||
|
ap.target_pitch_and_heading(90.0, heading)
|
||||||
|
except Exception:
|
||||||
|
# 如果 target_pitch_and_heading 不可用,尝试设置朝向为航向不变,俯仰 90
|
||||||
|
if verbose:
|
||||||
|
print('auto_pilot.target_pitch_and_heading 不可用,尝试基础设置')
|
||||||
|
if verbose:
|
||||||
|
print('阶段1: 70% 推力,垂直上升,持续 40s')
|
||||||
|
status['steps'].append({'step': 1, 'throttle': 0.7, 'pitch': 90, 'heading': heading, 'duration_s': 40})
|
||||||
|
_sleep_with_progress(40)
|
||||||
|
|
||||||
|
# 2) 计算维持 1G 所需油门,航向 90,俯仰 45,持续 20s
|
||||||
|
desired_g = 9.81
|
||||||
|
throttle_for_1g = None
|
||||||
|
try:
|
||||||
|
# vessel.mass 与 vessel.available_thrust 在多数 kRPC 版本可用
|
||||||
|
mass = vessel.mass
|
||||||
|
available_thrust = vessel.available_thrust
|
||||||
|
if available_thrust is None or available_thrust == 0:
|
||||||
|
raise RuntimeError('available_thrust 无效')
|
||||||
|
# 所需油门比例 = (质量 * 目标加速度) / 可用推力
|
||||||
|
throttle_for_1g = (mass * desired_g) / available_thrust
|
||||||
|
throttle_for_1g = max(0.0, min(1.0, throttle_for_1g))
|
||||||
|
except Exception:
|
||||||
|
# 计算失败时回退到一个保守值
|
||||||
|
throttle_for_1g = 0.5
|
||||||
|
if verbose:
|
||||||
|
print('无法精确计算 1G 所需油门,回退到 50%')
|
||||||
|
|
||||||
|
vessel.control.throttle = float(throttle_for_1g)
|
||||||
|
try:
|
||||||
|
ap.target_pitch_and_heading(45.0, 90.0)
|
||||||
|
except Exception:
|
||||||
|
if verbose:
|
||||||
|
print('设置 45°/90° 方向到 auto_pilot 失败')
|
||||||
|
if verbose:
|
||||||
|
print(f'阶段2: 维持 1G (估计油门 {throttle_for_1g:.3f}),航向 90,俯仰 45,持续 20s')
|
||||||
|
status['steps'].append({'step': 2, 'throttle': throttle_for_1g, 'pitch': 45, 'heading': 90, 'duration_s': 20})
|
||||||
|
_sleep_with_progress(20)
|
||||||
|
|
||||||
|
# 3) 推力 100%,航向 270,俯仰 70,持续 15s
|
||||||
|
vessel.control.throttle = 1.0
|
||||||
|
try:
|
||||||
|
ap.target_pitch_and_heading(70.0, 270.0)
|
||||||
|
except Exception:
|
||||||
|
if verbose:
|
||||||
|
print('设置 70°/270° 方向到 auto_pilot 失败')
|
||||||
|
if verbose:
|
||||||
|
print('阶段3: 100% 推力,航向 270,俯仰 70,持续 15s')
|
||||||
|
status['steps'].append({'step': 3, 'throttle': 1.0, 'pitch': 70, 'heading': 270, 'duration_s': 15})
|
||||||
|
_sleep_with_progress(15)
|
||||||
|
|
||||||
|
if verbose:
|
||||||
|
print('飞行序列完成')
|
||||||
|
status['result'] = 'ok'
|
||||||
|
finally:
|
||||||
|
try:
|
||||||
|
# 结束时不改变油门状态,仅可选地切换到 0
|
||||||
|
vessel.control.throttle = 0.0
|
||||||
|
if hasattr(vessel.auto_pilot, 'disengage'):
|
||||||
|
try:
|
||||||
|
vessel.auto_pilot.disengage()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
try:
|
||||||
|
conn.close()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
return status
|
||||||
|
|
||||||
|
|
||||||
|
def _sleep_with_progress(duration_s):
|
||||||
|
# 小循环以便能快速响应(比如可拓展为监听中断)
|
||||||
|
end = time.time() + duration_s
|
||||||
|
while True:
|
||||||
|
now = time.time()
|
||||||
|
if now >= end:
|
||||||
|
break
|
||||||
|
time.sleep(min(0.5, end - now))
|
||||||
|
|
||||||
|
|
||||||
|
def _ensure_throttle_applied(conn, sc, desired_throttle=0.7, timeout=10.0, verbose=False):
|
||||||
|
"""尝试确保对当前活动载具的油门设置被游戏接受。
|
||||||
|
|
||||||
|
优先使用 kRPC streams 订阅 control.throttle、available_thrust、thrust 的回显。
|
||||||
|
在分级后调用,函数会在超时前反复写入 throttle 并检查回显或推力变化。
|
||||||
|
返回:布尔,表示油门是否被接受。
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
start = time.time()
|
||||||
|
vessel = sc.active_vessel
|
||||||
|
# 尝试建立 streams
|
||||||
|
streams = {}
|
||||||
|
try:
|
||||||
|
streams['throttle'] = conn.add_stream(getattr, vessel.control, 'throttle')
|
||||||
|
streams['available_thrust'] = conn.add_stream(getattr, vessel, 'available_thrust')
|
||||||
|
streams['thrust'] = conn.add_stream(getattr, vessel, 'thrust')
|
||||||
|
except Exception:
|
||||||
|
streams = {}
|
||||||
|
|
||||||
|
while time.time() - start < timeout:
|
||||||
|
try:
|
||||||
|
vessel = sc.active_vessel
|
||||||
|
vessel.control.throttle = desired_throttle
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
# 读取 stream 回显
|
||||||
|
try:
|
||||||
|
tval = streams.get('throttle').read() if 'throttle' in streams else getattr(vessel.control, 'throttle', None)
|
||||||
|
except Exception:
|
||||||
|
try:
|
||||||
|
tval = getattr(vessel.control, 'throttle', None)
|
||||||
|
except Exception:
|
||||||
|
tval = None
|
||||||
|
|
||||||
|
try:
|
||||||
|
thrust_val = streams.get('thrust').read() if 'thrust' in streams else getattr(vessel, 'thrust', None)
|
||||||
|
except Exception:
|
||||||
|
thrust_val = None
|
||||||
|
|
||||||
|
try:
|
||||||
|
avail = streams.get('available_thrust').read() if 'available_thrust' in streams else getattr(vessel, 'available_thrust', None)
|
||||||
|
except Exception:
|
||||||
|
avail = None
|
||||||
|
|
||||||
|
# 额外调试输出,帮助定位为何油门被限制
|
||||||
|
if verbose:
|
||||||
|
try:
|
||||||
|
vs = getattr(vessel, 'name', None)
|
||||||
|
except Exception:
|
||||||
|
vs = None
|
||||||
|
print(f"_ensure_throttle_applied: vessel={vs} desired={desired_throttle:.3f} tval={tval} thrust={thrust_val} avail={avail}")
|
||||||
|
|
||||||
|
if tval is not None and tval >= desired_throttle * 0.95:
|
||||||
|
# 回显已接近目标
|
||||||
|
_cleanup_streams(streams)
|
||||||
|
return True
|
||||||
|
if thrust_val is not None and thrust_val > 0:
|
||||||
|
_cleanup_streams(streams)
|
||||||
|
return True
|
||||||
|
if avail is not None and avail > 0 and tval is not None and tval > 0:
|
||||||
|
_cleanup_streams(streams)
|
||||||
|
return True
|
||||||
|
|
||||||
|
time.sleep(0.1)
|
||||||
|
|
||||||
|
# 超时仍失败,清理并返回 False
|
||||||
|
_cleanup_streams(streams)
|
||||||
|
return False
|
||||||
|
except Exception:
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
def _cleanup_streams(streams):
|
||||||
|
try:
|
||||||
|
for s in list(streams.values()):
|
||||||
|
try:
|
||||||
|
s.remove()
|
||||||
|
except Exception:
|
||||||
|
try:
|
||||||
|
s.close()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
def _diagnose_engines(vessel):
|
||||||
|
"""收集并返回当前载具的一些发动机/推进相关诊断信息(尽量安全)。"""
|
||||||
|
info = {}
|
||||||
|
try:
|
||||||
|
info['control_throttle'] = getattr(vessel.control, 'throttle', 'N/A')
|
||||||
|
except Exception:
|
||||||
|
info['control_throttle'] = 'N/A'
|
||||||
|
try:
|
||||||
|
info['available_thrust'] = getattr(vessel, 'available_thrust', 'N/A')
|
||||||
|
except Exception:
|
||||||
|
info['available_thrust'] = 'N/A'
|
||||||
|
try:
|
||||||
|
info['thrust'] = getattr(vessel, 'thrust', 'N/A')
|
||||||
|
except Exception:
|
||||||
|
info['thrust'] = 'N/A'
|
||||||
|
# 尝试获取部件计数和发动机计数
|
||||||
|
try:
|
||||||
|
parts = vessel.parts
|
||||||
|
info['parts_count'] = getattr(parts, 'count', 'N/A')
|
||||||
|
engines = getattr(parts, 'engines', None)
|
||||||
|
if engines is not None:
|
||||||
|
info['engines_count'] = getattr(engines, 'count', 'N/A')
|
||||||
|
# 尝试收集每个 engine 的状态
|
||||||
|
engines_list = []
|
||||||
|
try:
|
||||||
|
for i, eng in enumerate(engines):
|
||||||
|
ed = {'index': i}
|
||||||
|
try:
|
||||||
|
ed['part_name'] = getattr(eng, 'part', None)
|
||||||
|
except Exception:
|
||||||
|
ed['part_name'] = 'N/A'
|
||||||
|
# 常见 engine 属性,安全读取
|
||||||
|
for attr in ('active', 'ignited', 'has_fuel', 'thrust_limit', 'thrust', 'available_thrust', 'fuel_flow', 'flameout'):
|
||||||
|
try:
|
||||||
|
ed[attr] = getattr(eng, attr)
|
||||||
|
except Exception:
|
||||||
|
try:
|
||||||
|
# 有时属性在 engine 上不可用,但可在 part 或 module 上
|
||||||
|
ed[attr] = getattr(eng, attr, 'N/A')
|
||||||
|
except Exception:
|
||||||
|
ed[attr] = 'N/A'
|
||||||
|
engines_list.append(ed)
|
||||||
|
except Exception:
|
||||||
|
engines_list = 'N/A'
|
||||||
|
info['engines'] = engines_list
|
||||||
|
else:
|
||||||
|
info['engines_count'] = 'N/A'
|
||||||
|
info['engines'] = 'N/A'
|
||||||
|
except Exception:
|
||||||
|
info['parts_count'] = 'N/A'
|
||||||
|
info['engines_count'] = 'N/A'
|
||||||
|
info['engines'] = 'N/A'
|
||||||
|
|
||||||
|
return info
|
||||||
|
|
||||||
|
|
||||||
|
def _force_apply_throttle(conn, sc, desired_throttle, timeout=5.0, verbose=False):
|
||||||
|
"""在 _ensure_throttle_applied 失败后尝试更激进地应用油门:
|
||||||
|
- 设置每个发动机的 thrust_limit 到目标(如果属性存在)
|
||||||
|
- 多次写入 vessel.control.throttle
|
||||||
|
返回是否成功检测到油门生效。
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
start = time.time()
|
||||||
|
vessel = sc.active_vessel
|
||||||
|
# 尝试设置各发动机的 thrust_limit
|
||||||
|
try:
|
||||||
|
parts = vessel.parts
|
||||||
|
engines = getattr(parts, 'engines', None)
|
||||||
|
if engines is not None:
|
||||||
|
for eng in engines:
|
||||||
|
try:
|
||||||
|
# 有些 engine 对象暴露 thrust_limit 属性
|
||||||
|
if hasattr(eng, 'thrust_limit'):
|
||||||
|
eng.thrust_limit = desired_throttle
|
||||||
|
except Exception:
|
||||||
|
# 忽略单个发动机失败
|
||||||
|
pass
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
# 重复写入并检测
|
||||||
|
while time.time() - start < timeout:
|
||||||
|
try:
|
||||||
|
vessel = sc.active_vessel
|
||||||
|
vessel.control.throttle = float(desired_throttle)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
try:
|
||||||
|
tval = getattr(vessel.control, 'throttle', None)
|
||||||
|
except Exception:
|
||||||
|
tval = None
|
||||||
|
try:
|
||||||
|
thrust_val = getattr(vessel, 'thrust', None)
|
||||||
|
except Exception:
|
||||||
|
thrust_val = None
|
||||||
|
|
||||||
|
if verbose:
|
||||||
|
print(f"_force_apply_throttle: desired={desired_throttle:.3f} tval={tval} thrust={thrust_val}")
|
||||||
|
|
||||||
|
if tval is not None and tval >= desired_throttle * 0.9:
|
||||||
|
return True
|
||||||
|
if thrust_val is not None and thrust_val > 0:
|
||||||
|
return True
|
||||||
|
|
||||||
|
time.sleep(0.1)
|
||||||
|
|
||||||
|
return False
|
||||||
|
except Exception:
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
run_flight_sequence(verbose=True)
|
||||||
@@ -27,3 +27,256 @@ def get_surface_velocity():
|
|||||||
velocity = vessel.flight(ref_frame).velocity
|
velocity = vessel.flight(ref_frame).velocity
|
||||||
print('Surface velocity = (%.1f, %.1f, %.1f)' % velocity)
|
print('Surface velocity = (%.1f, %.1f, %.1f)' % velocity)
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
|
|
||||||
|
|
||||||
|
def surface_prograde():
|
||||||
|
conn = krpc.connect(name='Surface prograde')
|
||||||
|
vessel = conn.space_center.active_vessel
|
||||||
|
ap = vessel.auto_pilot
|
||||||
|
|
||||||
|
ap.reference_frame = vessel.surface_velocity_reference_frame
|
||||||
|
ap.target_direction = (0, 1, 0)
|
||||||
|
ap.engage()
|
||||||
|
ap.wait()
|
||||||
|
print('Auto-pilot now pointing prograde in surface reference frame')
|
||||||
|
ap.disengage()
|
||||||
|
|
||||||
|
def surface_retrograde():
|
||||||
|
conn = krpc.connect(name='Surface retrograde')
|
||||||
|
vessel = conn.space_center.active_vessel
|
||||||
|
ap = vessel.auto_pilot
|
||||||
|
|
||||||
|
ap.reference_frame = vessel.surface_velocity_reference_frame
|
||||||
|
ap.target_direction = (0, -1, 0)
|
||||||
|
ap.engage()
|
||||||
|
ap.wait()
|
||||||
|
print('Auto-pilot now pointing retrograde in surface reference frame')
|
||||||
|
ap.disengage()
|
||||||
|
|
||||||
|
def vessel_available_thrust():
|
||||||
|
conn = krpc.connect(name='Vessel available thrust')
|
||||||
|
vessel = conn.space_center.active_vessel
|
||||||
|
|
||||||
|
while True:
|
||||||
|
thrust = vessel.available_thrust
|
||||||
|
print('Vessel available thrust = %.1f kN' % (thrust / 1000))
|
||||||
|
time.sleep(1)
|
||||||
|
|
||||||
|
def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, target_vspeed=-2.0, safety_margin=5.0, verbose=True):
|
||||||
|
"""预测式自动着陆(suicide burn 思路)。
|
||||||
|
|
||||||
|
关键点:
|
||||||
|
1) 速度从接口获取:surface speed 用 flight(surface_frame).speed(标量),垂直速度用 flight(surface_frame).vertical_speed。
|
||||||
|
不再用“高度差/时间差”估算。
|
||||||
|
2) 不再脚本激活即点火;先计算 braking distance,根据可用推力决定何时点火(countdown)。
|
||||||
|
|
||||||
|
近似模型(忽略空气阻力、推力变化、TWR变化导致的积分误差):
|
||||||
|
a_max_up = max(0, available_thrust/mass - g)
|
||||||
|
braking_distance ≈ (v^2 - vf^2) / (2*a_max_up) + safety_margin
|
||||||
|
当 remaining_alt <= braking_distance 时开始点火。
|
||||||
|
|
||||||
|
燃烧阶段按剩余距离反推所需净向上加速度:
|
||||||
|
a_req_up = max(0, (v^2 - vf^2) / (2*remaining_alt))
|
||||||
|
throttle = (a_req_up + g) * mass / available_thrust
|
||||||
|
|
||||||
|
注:这里 v 取“相对地表速度”的标量(surface speed),更贴近你提到的 get_surface_speed。
|
||||||
|
若需要更精细,可改成取 surface_velocity 向量的径向分量/或用垂直速度替代。
|
||||||
|
"""
|
||||||
|
try:
|
||||||
|
import math
|
||||||
|
except Exception:
|
||||||
|
math = None
|
||||||
|
|
||||||
|
if krpc is None:
|
||||||
|
raise RuntimeError('krpc 未安装')
|
||||||
|
|
||||||
|
conn = krpc.connect(name='auto_land_predictive', address=host, rpc_port=port)
|
||||||
|
sc = conn.space_center
|
||||||
|
vessel = sc.active_vessel
|
||||||
|
if vessel is None:
|
||||||
|
conn.close()
|
||||||
|
raise RuntimeError('未找到活动载具')
|
||||||
|
|
||||||
|
status = {'result': 'failed', 'logs': []}
|
||||||
|
|
||||||
|
def _get_g(local_vessel):
|
||||||
|
try:
|
||||||
|
body = local_vessel.orbit.body
|
||||||
|
g = float(getattr(body, 'surface_gravity', 9.81))
|
||||||
|
if g > 0:
|
||||||
|
return g
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
try:
|
||||||
|
g = float(getattr(local_vessel.flight(), 'surface_gravity', 9.81))
|
||||||
|
if g > 0:
|
||||||
|
return g
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
return 9.81
|
||||||
|
|
||||||
|
# 使用 body 的 reference_frame 读取 surface speed / vertical_speed(对应 get_surface_speed 思路)
|
||||||
|
try:
|
||||||
|
surface_frame = vessel.orbit.body.reference_frame
|
||||||
|
except Exception:
|
||||||
|
surface_frame = None
|
||||||
|
|
||||||
|
# autopilot 指向 surface retrograde
|
||||||
|
try:
|
||||||
|
ap = vessel.auto_pilot
|
||||||
|
ap.reference_frame = vessel.surface_velocity_reference_frame
|
||||||
|
ap.target_direction = (0, -1, 0)
|
||||||
|
ap.engage()
|
||||||
|
except Exception:
|
||||||
|
ap = None
|
||||||
|
|
||||||
|
def _read_state():
|
||||||
|
"""从接口读取状态:alt, surface_speed(标量), vertical_speed, mass, available_thrust, g"""
|
||||||
|
flight = vessel.flight(surface_frame) if surface_frame is not None else vessel.flight()
|
||||||
|
alt = float(getattr(flight, 'surface_altitude', 0.0))
|
||||||
|
srf_speed = float(getattr(flight, 'speed', 0.0))
|
||||||
|
v_speed = float(getattr(flight, 'vertical_speed', 0.0))
|
||||||
|
mass = float(getattr(vessel, 'mass', 0.0))
|
||||||
|
avail_thrust = float(getattr(vessel, 'available_thrust', 0.0))
|
||||||
|
g_local = _get_g(vessel)
|
||||||
|
return alt, srf_speed, v_speed, mass, avail_thrust, g_local
|
||||||
|
|
||||||
|
try:
|
||||||
|
# 初始确保不点火
|
||||||
|
try:
|
||||||
|
vessel.control.throttle = 0.0
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
# 倒计时/等待点火
|
||||||
|
ignited = False
|
||||||
|
last_log_t = 0.0
|
||||||
|
while True:
|
||||||
|
alt, srf_speed, v_speed, mass, avail_thrust, g_local = _read_state()
|
||||||
|
remaining_alt = max(0.0, alt - float(target_altitude))
|
||||||
|
|
||||||
|
# v:使用 surface speed(标量)作为“需要刹掉的速度”
|
||||||
|
v = max(0.0, float(srf_speed))
|
||||||
|
vf = max(0.0, abs(float(target_vspeed)))
|
||||||
|
|
||||||
|
# 若推力不可用,直接点火尝试(避免永远等不到)
|
||||||
|
if mass <= 0 or avail_thrust <= 1e-6:
|
||||||
|
a_max_up = 0.0
|
||||||
|
else:
|
||||||
|
a_max_up = max(0.0, avail_thrust / mass - g_local)
|
||||||
|
|
||||||
|
if a_max_up <= 1e-6:
|
||||||
|
braking_dist = float('inf')
|
||||||
|
else:
|
||||||
|
braking_dist = max(0.0, (v * v - vf * vf) / (2.0 * a_max_up)) + float(safety_margin)
|
||||||
|
|
||||||
|
countdown = None
|
||||||
|
if v > 1e-6 and braking_dist != float('inf'):
|
||||||
|
# 近似把“还剩多少距离”除以当前速度当作倒计时(粗略)
|
||||||
|
countdown = (remaining_alt - braking_dist) / v
|
||||||
|
|
||||||
|
now = time.time()
|
||||||
|
if verbose and now - last_log_t > 0.2:
|
||||||
|
last_log_t = now
|
||||||
|
msg = (
|
||||||
|
f'WAIT alt={alt:.1f} rem={remaining_alt:.1f} '
|
||||||
|
f'srf_speed={srf_speed:.2f} vs={v_speed:.2f} '
|
||||||
|
f'a_max_up={a_max_up:.2f} braking={braking_dist:.1f} '
|
||||||
|
f'countdown={(countdown if countdown is not None else float("nan")):.2f}'
|
||||||
|
)
|
||||||
|
status['logs'].append(msg)
|
||||||
|
print(msg)
|
||||||
|
|
||||||
|
# 点火条件:进入刹车距离 or 推力未知(inf)但高度很低
|
||||||
|
should_ignite = (remaining_alt <= braking_dist) or (braking_dist == float('inf') and remaining_alt < 200.0)
|
||||||
|
if should_ignite:
|
||||||
|
break
|
||||||
|
|
||||||
|
# 确保仍在滑行
|
||||||
|
try:
|
||||||
|
vessel.control.throttle = 0.0
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
time.sleep(0.05)
|
||||||
|
|
||||||
|
# 点火
|
||||||
|
try:
|
||||||
|
vessel.control.activate_next_stage()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
ignited = True
|
||||||
|
|
||||||
|
# 燃烧闭环:根据 remaining_alt 反推需要的减速度 -> 油门
|
||||||
|
while True:
|
||||||
|
alt, srf_speed, v_speed, mass, avail_thrust, g_local = _read_state()
|
||||||
|
remaining_alt = max(0.0, alt - float(target_altitude))
|
||||||
|
|
||||||
|
v = max(0.0, float(srf_speed))
|
||||||
|
vf = max(0.0, abs(float(target_vspeed)))
|
||||||
|
|
||||||
|
# 终止条件(接地附近 + 速度已足够小)
|
||||||
|
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'] = 'landed'
|
||||||
|
status['logs'].append('预测着陆完成')
|
||||||
|
if verbose:
|
||||||
|
print('预测着陆完成')
|
||||||
|
break
|
||||||
|
|
||||||
|
# 计算所需净向上加速度
|
||||||
|
if remaining_alt <= 1e-3:
|
||||||
|
a_req_up = 0.0
|
||||||
|
else:
|
||||||
|
a_req_up = max(0.0, (v * v - vf * vf) / (2.0 * remaining_alt))
|
||||||
|
|
||||||
|
# 推力不足时给满油门
|
||||||
|
if mass <= 0 or avail_thrust <= 1e-6:
|
||||||
|
throttle = 1.0
|
||||||
|
else:
|
||||||
|
throttle = (a_req_up + g_local) * mass / avail_thrust
|
||||||
|
|
||||||
|
throttle = max(0.0, min(1.0, float(throttle)))
|
||||||
|
|
||||||
|
# 低空兜底:高度<5m 且仍很快 -> 满油门
|
||||||
|
if alt < 5.0 and v > 10.0:
|
||||||
|
throttle = 1.0
|
||||||
|
|
||||||
|
try:
|
||||||
|
vessel.control.throttle = throttle
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
if verbose:
|
||||||
|
msg = (
|
||||||
|
f'BURN alt={alt:.1f} rem={remaining_alt:.1f} '
|
||||||
|
f'srf_speed={srf_speed:.2f} vs={v_speed:.2f} '
|
||||||
|
f'a_req_up={a_req_up:.2f} throttle={throttle:.2f}'
|
||||||
|
)
|
||||||
|
status['logs'].append(msg)
|
||||||
|
print(msg)
|
||||||
|
|
||||||
|
time.sleep(0.05)
|
||||||
|
|
||||||
|
finally:
|
||||||
|
try:
|
||||||
|
if ap is not None and hasattr(ap, 'disengage'):
|
||||||
|
try:
|
||||||
|
ap.disengage()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
try:
|
||||||
|
conn.close()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
return status
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
xxx = auto_land_predictive()
|
||||||
|
print(xxx)
|
||||||
|
print('done')
|
||||||
@@ -243,7 +243,7 @@ def upload_didi_saved_message_to_db():
|
|||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
tg = MessageUploader(
|
tg = MessageUploader(
|
||||||
local_logger=LoggerClass().get_log(logging.DEBUG),
|
local_logger=LoggerClass().get_log(logging.DEBUG),
|
||||||
local_load_path='E:\\SNSFiles\\ChatExport_2026-01-01',
|
local_load_path='E:\\SNSFiles\\ChatExport_2026-01-02',
|
||||||
local_file_db_path=project_config['file_db_path'],
|
local_file_db_path=project_config['file_db_path'],
|
||||||
size_limit=0.25
|
size_limit=0.25
|
||||||
)
|
)
|
||||||
|
|||||||
Reference in New Issue
Block a user