diff --git a/src/KSP_tools/krpc_explore.py b/src/KSP_tools/krpc_explore.py index ab694ab..1463e3f 100644 --- a/src/KSP_tools/krpc_explore.py +++ b/src/KSP_tools/krpc_explore.py @@ -62,7 +62,7 @@ def 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): +def auto_land_predictive(bottom_clearance_margin=12, host='localhost', port=50000, target_altitude=0.5, target_vspeed=-2.0, safety_margin=5.0, verbose=True): """预测式自动着陆（suicide burn 思路）。 关键点： @@ -81,6 +81,8 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ 注：这里 v 取“相对地表速度”的标量（surface speed），更贴近你提到的 get_surface_speed。 若需要更精细，可改成取 surface_velocity 向量的径向分量/或用垂直速度替代。 + + bottom_clearance_margin = 12 # m：按载具尺寸调整（越大越早开始末端减速） """ try: import math @@ -100,13 +102,15 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ status = {'result': 'failed', 'logs': []} # WAIT 阶段按折减推力估算刹车距离（提前点火，补偿点火/推力建立延迟） - thrust_discount_wait = 0.9 + thrust_discount_wait = 0.85 # 低空/接地参数（用于“最后几米慢下来”） touchdown_vspeed_tol = 0.6 # m/s：认为“软着陆”的竖直速度容差 low_altitude_pid_switch = 10.0 # m：低于该高度切换到竖直速度控制 vspeed_p_gain = 1.2 # 比例增益：越大越积极（建议 0.6~2.0） low_altitude_ff_gain = 0.3 # 低空前馈增益：按“需要的减速距离/剩余距离”做额外加速度 + # 由于 surface_altitude 近似是“载具质心高度”，末端会偏乐观。 + # 用 bedrock_altitude（雷达高度）并减去一个底部几何余量，近似得到“最低点离地高度”。 def _get_g(local_vessel): try: @@ -140,9 +144,10 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ ap = None def _read_state(): - """从接口读取状态：alt, surface_speed(标量), vertical_speed, mass, available_thrust, g, situation_name""" + """从接口读取状态：alt(surface_altitude), radar_alt(bedrock_altitude), surface_speed, vertical_speed, mass, available_thrust, g, situation_name""" flight = vessel.flight(surface_frame) if surface_frame is not None else vessel.flight() alt = float(getattr(flight, 'surface_altitude', 0.0)) + radar_alt = float(getattr(flight, 'bedrock_altitude', alt)) srf_speed = float(getattr(flight, 'speed', 0.0)) v_speed = float(getattr(flight, 'vertical_speed', 0.0)) mass = float(getattr(vessel, 'mass', 0.0)) @@ -152,7 +157,7 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ 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 + return alt, radar_alt, srf_speed, v_speed, mass, avail_thrust, g_local, situation_name try: # 初始确保不点火 @@ -164,8 +169,10 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ # 倒计时/等待点火 last_log_t = 0.0 while True: - alt, srf_speed, v_speed, mass, avail_thrust, g_local, situation_name = _read_state() - remaining_alt = max(0.0, alt - float(target_altitude)) + alt, radar_alt, srf_speed, v_speed, mass, avail_thrust, g_local, situation_name = _read_state() + # 用“雷达高度-底部余量”近似最低点离地高度 + alt_eff = max(0.0, float(radar_alt) - float(bottom_clearance_margin)) + remaining_alt = max(0.0, alt_eff - float(target_altitude)) # v：使用 surface speed（标量）作为“需要刹掉的速度” v = max(0.0, float(srf_speed)) @@ -192,7 +199,7 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ if verbose and now - last_log_t > 0.2: last_log_t = now msg = ( - f'WAIT alt={alt:.1f} rem={remaining_alt:.1f} ' + f'WAIT alt={alt:.1f} radar={radar_alt:.1f} alt_eff={alt_eff:.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} thr_disc={thrust_discount_wait:.2f} ' f'countdown={(countdown if countdown is not None else float("nan")):.2f}' @@ -212,16 +219,17 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ pass time.sleep(0.01) - # 点火 - try: - vessel.control.activate_next_stage() - except Exception: - pass + # # 点火 + # try: + # vessel.control.activate_next_stage() + # except Exception: + # pass # 燃烧闭环：根据 remaining_alt 反推需要的减速度 -> 油门 while True: - alt, srf_speed, v_speed, mass, avail_thrust, g_local, situation_name = _read_state() - remaining_alt = max(0.0, alt - float(target_altitude)) + alt, radar_alt, srf_speed, v_speed, mass, avail_thrust, g_local, situation_name = _read_state() + alt_eff = max(0.0, float(radar_alt) - float(bottom_clearance_margin)) + remaining_alt = max(0.0, alt_eff - float(target_altitude)) # BURN 阶段以“竖直下降速度”为控制对象：下降为负，统一转成向下为正的标量 v_down v_down = max(0.0, -float(v_speed)) @@ -254,7 +262,7 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ break # 低空策略：切换到“竖直速度跟踪”以确保最后几米把速度压到目标值 - if alt <= float(low_altitude_pid_switch): + if alt_eff <= float(low_altitude_pid_switch): # 低空：比例 + 前馈（用当前速度估算“需要多少距离才能刹到目标速度”） # a_p：把竖直速度拉向目标 a_p = float(vspeed_p_gain) * max(0.0, (v_down - vf_down)) @@ -280,7 +288,7 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ throttle = max(0.0, min(1.0, float(throttle))) # 低空兜底：高度<5m 且仍很快 -> 满油门 - if alt < 5.0 and v_down > 10.0: + if alt_eff < 5.0 and v_down > 10.0: throttle = 1.0 # 若已经触发 LANDED 但竖直速度仍偏大，避免油门过低导致“硬砸/反弹” @@ -294,7 +302,7 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ if verbose: msg = ( - f'BURN alt={alt:.1f} rem={remaining_alt:.1f} ' + f'BURN alt={alt:.1f} radar={radar_alt:.1f} alt_eff={alt_eff:.1f} rem={remaining_alt:.1f} ' f'srf_speed={srf_speed:.2f} vs={v_speed:.2f} ' f'v_down={v_down:.2f} vf_down={vf_down:.2f} ' f'a_req_up={a_req_up:.2f} throttle={throttle:.2f} ' @@ -322,6 +330,6 @@ def auto_land_predictive(host='localhost', port=50000, target_altitude=0.5, targ return status if __name__ == "__main__": - xxx = auto_land_predictive() + xxx = auto_land_predictive(bottom_clearance_margin=20) print(xxx) print('done') \ No newline at end of file