import time
import krpc, simple_pid


conn = krpc.connect(name='动态着陆 V4')
if not conn.space_center:
    print("No active vessel found.")
    exit()
vessel = conn.space_center.active_vessel
flight = vessel.flight(vessel.orbit.body.reference_frame)
# ================= 配置参数 (V4 - 动态着陆) =================



# ================= 飞控 =================

pid = simple_pid.PID(0.15, 0.05, 0.1, setpoint=0)
pid.output_limits = (0, 1)  # 节流阀范围
alpha = 0.3  # 节流阀平滑系数
last_throttle = 0.0
print("动态着陆 V4 已启动")
while True:
    alt = max(0, flight.surface_altitude - 9.50)  # 考虑雷达高度计偏差
    vel = flight.speed

    # 计算 PID 输出
    raw_throttle = pid(vel)
    if raw_throttle is None:
        raw_throttle = 0.0

    # 节流阀平滑处理
    smoothed_throttle = (1 - alpha) * last_throttle + alpha * raw_throttle
    last_throttle = smoothed_throttle

    vessel.control.throttle = smoothed_throttle

    time.sleep(0.1)