feat: 重构着陆控制逻辑，优化状态机和PID控制器实现

自动着陆.py

@@ -1,132 +1,148 @@
 from enum import Enum
 import time
+import math
 import krpc
 from pid import PID
-conn = krpc.connect(name='SpaceX Landing')
-if not conn.space_center:
-    print("No active vessel found.")
-    exit()
 
-# control = vessel.control
+class LandingState(str, Enum):
-vessel = conn.space_center.active_vessel
-celestial = vessel.orbit.body # 当前天体
-# 创建 PID 控制器
-# 外环：高度 -> 目标垂直速度（允许负值）
-altitude_pid = PID(kp=0.3, ki=0.03, kd=0.05, integral_limit=2.0, alpha=0.8, output_limit=(-10.0, 10.0))
-
-# 内环：垂直速度 -> 节流阀（0~1）
-speed_pid = PID(kp=0.06, ki=0.05, kd=0.05, integral_limit=10.0, alpha=0.5, output_limit=(0.0, 1.0))
-
-target_altitude = 30  # 目标高度，单位 m
-
-# 状态机定义
-class STATE(str, Enum):
     GLIDE = "GLIDE"      # 滑翔/自由落体
     LAND = "LAND"        # 动态减速
     FINAL = "FINAL"      # 10m以下匀速着陆
 
-current_state = STATE.GLIDE
+class SpaceX_Lander:
+    def __init__(self):
+        # 初始化连接
+        self.conn = krpc.connect(name="SpaceX Landing Controller")
+        if not self.conn.space_center:
+            print("No active vessel found.")
+            exit()
             
-last_time = time.monotonic()
+        self.vessel = self.conn.space_center.active_vessel
-loop_dt = 0.1 # 降低周期提高采样频率
+        self.celestial = self.vessel.orbit.body
         
-# 着陆控制参数
+        # 控制参数
-target_landing_speed = -1.0  # 最终落地目标垂直速度 (m/s)
+        self.target_landing_speed = -1.0  # m/s
-final_approach_alt = 10.0    # 切换到匀速下降的高度 (m)
+        self.final_approach_alt = 10.0    # m
-safety_factor = 0.8          # 冗余因素
+        self.safety_factor = 0.8          # 动力冗余
+        self.loop_dt = 0.05               # 控制周期
         
-try:
+        # PID 控制器
-    # 自动获取飞船高度偏移 (从质心到底部的距离)
+        self.speed_pid = PID(kp=0.06, ki=0.05, kd=0.05, integral_limit=10.0, alpha=0.5, output_limit=(0.0, 1.0))
-    # 在 vessel 坐标系中，通常 y 轴是纵向的
+        
+        # 状态
+        self.state = LandingState.GLIDE
+        self.bottom_offset = self._calculate_bottom_offset()
+        
+        print(f"SpaceX Lander 准备就绪。底部偏移: {self.bottom_offset:.2f}m")
+
+    def _calculate_bottom_offset(self):
+        """自动计算飞船质心到底部的距离"""
         try:
-        box = vessel.bounding_box(vessel.reference_frame)
+            box = self.vessel.bounding_box(self.vessel.reference_frame)
-        bottom_offset = abs(box[0][1]) 
+            return abs(box[0][1])
         except:
-        bottom_offset = 0.0
+            return 0.0
-    print(f"检测到飞船底部偏移量: {bottom_offset:.2f} m")
 
+    def set_steering_with_lateral_error(self, east_err, north_err, max_tilt=20.0):
+        """将横向误差转化为姿态"""
+        tilt = math.sqrt(east_err**2 + north_err**2)
+        if tilt > max_tilt:
+            scale = max_tilt / tilt
+            east_err *= scale
+            north_err *= scale
+            tilt = max_tilt
+
+        target_azimuth = math.degrees(math.atan2(east_err, north_err))
+        target_pitch = 90.0 - tilt
+        
+        self.vessel.auto_pilot.target_pitch_and_heading(target_pitch, target_azimuth)
+        self.vessel.auto_pilot.engage()
+
+    def run(self):
+        """主运行循环"""
+        last_time = time.monotonic()
+        
+        try:
             while True:
                 now = time.monotonic()
                 dt = now - last_time
                 last_time = now
 
-        flight = vessel.flight(vessel.orbit.body.reference_frame)
+                # 获取实时飞行数据
+                flight = self.vessel.flight(self.celestial.reference_frame)
                 current_altitude = flight.surface_altitude
+                current_velocity = flight.velocity
                 current_vertical_speed = flight.vertical_speed
+                h_err = max(0.0, current_altitude - self.bottom_offset)
 
-        # 计算转换距离与高度修正
+                # 计算环境与物理参数
-        available_thrust = vessel.available_thrust # 可用推力
+                available_thrust = self.vessel.available_thrust
-        gav = celestial.surface_gravity # 重力加速度
+                gav = self.celestial.surface_gravity
-        a_max = available_thrust / vessel.mass
+                a_max = available_thrust / self.vessel.mass if self.vessel.mass > 0 else 0
-        a_net_max = a_max - gav # 扣除重力后的最大净加速度
+                a_net_max = a_max - gav
 
-        # 修正高度：减掉底部偏移量 (h_err 是底部到地面的距离)
+                # 1. 状态切换逻辑与点火计算
-        h_err = max(0.0, current_altitude - bottom_offset)
+                new_state = self.state
-
+                transition_distance = 0
-        # 核心算法优化：计算点火距离时加入 safety_factor 提早点火
+                if self.state == LandingState.GLIDE:
                     if a_net_max > 0.5:
-            # 预留更多余量 (safety_factor 越小，预估点火高度越高)
+                        a_plan = max(0.1, a_net_max * self.safety_factor)
-            a_plan = max(0.1, a_net_max * safety_factor)
+                        transition_distance = (current_vertical_speed**2 - self.target_landing_speed**2) / (2 * a_plan)
-            transition_distance = (current_vertical_speed**2 - target_landing_speed**2) / (2 * a_plan)
                     else:
                         transition_distance = 1e6
                     
-        # 状态切换逻辑
-        new_state = current_state
-        if current_state == STATE.GLIDE: 
                     if h_err <= transition_distance:
-                new_state = STATE.LAND
+                        new_state = LandingState.LAND
-        elif current_state == STATE.LAND:
-            if h_err <= final_approach_alt:
-                new_state = STATE.FINAL
                 
-        if new_state != current_state:
+                elif self.state == LandingState.LAND:
-            print(f"\n状态切换: {current_state} -> {new_state}")
+                    if h_err <= self.final_approach_alt:
-            current_state = new_state
+                        new_state = LandingState.FINAL
-            # 在状态切换时重置 PID，避免积分风up
-            try:
-                altitude_pid.reset()
-                speed_pid.reset()
-            except:
-                pass
 
-        if current_state == STATE.GLIDE:
+                if new_state != self.state:
-            # 滑翔：不做具体控制，油门设为 0
+                    self._switch_state(new_state)
-            vessel.control.throttle = 0.0
-            print(f"高度: {h_err:.2f} m, 预计点火点: {transition_distance:.2f} m, 状态: GLIDE, 油门: 0.00"," "*10, end='\r')
-        elif current_state == STATE.LAND:
-            # 动态轨迹：根据当前高度计算理想减速梯度
-            a_plan = max(0.1, a_net_max * safety_factor)
-            # 目标速度公式: 随高度平滑减速
-            ideal_speed = -( max(0, target_landing_speed**2 + 2 * a_plan * (h_err - final_approach_alt))**0.5 )
 
-            # 内环：垂直速度误差 -> 节流阀
+                # 2. 状态执行逻辑
-            speed_error = ideal_speed - current_vertical_speed
+                throttle = 0.0
-            throttle = speed_pid.update(speed_error, dt)
                 
-            vessel.control.throttle = throttle
+                if self.state == LandingState.GLIDE:
-            print(f"高度: {h_err:.2f} m, 理想速: {ideal_speed:.2f} m/s, 当前速: {current_vertical_speed:.2f} m/s, 状态: LAND, 节流阀: {throttle:.2f}"," "*10, end='\r')
+                    throttle = 0.0
+                    print(f"高度: {h_err:.1f}m, 点火距: {transition_distance:.1f}m, 东速: {current_velocity[1]:.1f}m/s, 北速: {current_velocity[0]:.1f}m/s, 状态: GLIDE", " "*10, end="\r")
                 
-        elif current_state == STATE.FINAL:
+                elif self.state == LandingState.LAND:
-            # 10m 以下：匀速缓慢下降
+                    # 动态减速曲线
-            target_v = target_landing_speed
+                    a_plan = max(0.1, a_net_max * self.safety_factor)
-            speed_error = target_v - current_vertical_speed
+                    ideal_speed = -(max(0, self.target_landing_speed**2 + 2 * a_plan * (h_err - self.final_approach_alt))**0.5)
-            throttle = speed_pid.update(speed_error, dt)
+                    throttle = self.speed_pid.update(ideal_speed - current_vertical_speed, dt)
+                    print(f"高度: {h_err:.1f}m, 理想速: {ideal_speed:.1f}m/s, 当前速: {current_vertical_speed:.1f}m/s, 状态: LAND", " "*10, end="\r")
                 
-            vessel.control.throttle = throttle
+                elif self.state == LandingState.FINAL:
-            # 到达极低高度或检测到接触（垂直速度接近0）时关闭引擎
+                    # 匀速下降阶段
+                    throttle = self.speed_pid.update(self.target_landing_speed - current_vertical_speed, dt)
+                    print(f"高度: {h_err:.1f}m, 目标速: {self.target_landing_speed:.1f}m/s, 当前速: {current_vertical_speed:.1f}m/s, 状态: FINAL", " "*10, end="\r")
+                    
+                    # 触地检测
                     if h_err < 0.5 and abs(current_vertical_speed) < 0.5:
-                vessel.control.throttle = 0.0
+                        self.vessel.control.throttle = 0.0
-                print("\n检测到触地，引擎关闭。")
+                        print("\n触地成功，任务完成。")
                         break
 
-            print(f"高度: {h_err:.2f} m, 目标速: {target_v:.2f} m/s, 当前速: {current_vertical_speed:.2f} m/s, 状态: FINAL, 节流阀: {throttle:.2f}"," "*10, end='\r')
+                self.vessel.control.throttle = throttle
 
-        # 保持大致的控制周期
+                # 频率控制
-        sleep_time = loop_dt - (time.monotonic() - now)
+                sleep_time = self.loop_dt - (time.monotonic() - now)
                 if sleep_time > 0:
                     time.sleep(sleep_time)
 
-except KeyboardInterrupt:
+        except KeyboardInterrupt:
-    vessel.control.throttle = 0.0
+            self.vessel.control.throttle = 0.0
-    print("已停止控制，节流设为 0。")
+            print("\n程序手动停止，油门已归零。")
+
+    def _switch_state(self, new_state):
+        print(f"\n[状态切换]: {self.state} -> {new_state}")
+        self.state = new_state
+        self.speed_pid.reset()
+
+if __name__ == "__main__":
+    lander = SpaceX_Lander()
+    lander.run()
+