38 lines
999 B
Python
38 lines
999 B
Python
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)
|