<?xml version="1.0"?>
<autopilot name="f16 Autopilot for Waypoint Navigation">
<!-- 基础自动驾驶属性 -->
<property>ap/attitude_hold</property>
<property>ap/altitude_hold</property>
<property>ap/active-waypoint</property>
<property>ap/altitude_setpoint</property>
<property>ap/heading-setpoint-select</property>
<property>ap/aileron_cmd</property>
<property>ap/heading_hold</property>
<property>ap/yaw_damper</property>

<!-- 航路点导航通道 -->
<channel name="AP Waypoint Navigation">
<!-- 选择航路点作为航向设定点 -->
<switch name="fcs/heading-setpoint-selector">
<default value="ic/psi-true-deg"/>
<test value="guidance/wp-heading-deg">
ap/heading-setpoint-select == 1
</test>
</switch>

<!-- 计算航向误差 -->  
<pure_gain name="fcs/heading-true-degrees">  
  <input>attitude/heading-true-rad</input>  
  <gain>57.2957795</gain> <!-- 将弧度转换为度 -->  
</pure_gain>  
<summer name="fcs/heading-error">  
  <input>-fcs/heading-true-degrees</input>  
  <input>fcs/heading-setpoint-selector</input>  
</summer>  
  
<!-- 修正航向误差（处理360度问题） -->  
<switch name="fcs/heading-error-bias-switch">  
  <default value="0.0"/>  
  <test value="360.0">  
    fcs/heading-error lt -180  
  </test>  
  <test value="-360.0">  
    fcs/heading-error gt 180  
  </test>  
</switch>  
<summer name="fcs/heading-corrected">  
  <input>fcs/heading-error-bias-switch</input>  
  <input>fcs/heading-error</input>  
  <clipto>  
    <min>-33</min>  
    <max>33</max>  
  </clipto>  
</summer>  
  
<!-- 将航向误差转换为控制命令 -->  
<pure_gain name="fcs/heading-command">  
  <input>fcs/heading-corrected</input>  
  <gain>0.01745</gain> <!-- 将度转换为弧度 -->  
</pure_gain>  
  
<!-- 使用PI控制器生成滚转控制命令 -->  
<pid name="fcs/heading-pi-controller">  
  <input>fcs/heading-command</input>  
  <kp>7.0</kp>  
  <ki>0.20</ki>  
  <kd>6.0</kd>
</pid>  
  
<!-- 选择滚转控制命令输出 -->  
<switch name="fcs/roll-command-selector">  
  <default value="0.0"/>  
  <test value="fcs/heading-pi-controller">  
    ap/heading_hold == 1  
  </test>  
  <output>ap/aileron_cmd</output>  
</switch>  

</channel>

<!-- 高度保持通道 -->
<channel name="AP Pitch Altitude Hold">
<!-- 计算高度误差 -->
<summer name="fcs/altitude-error">
<input>ap/altitude_setpoint</input>
<input>-position/h-sl-ft</input>
<clipto>
<min>-150</min>
<max>150</max>
</clipto>
</summer>

<!-- 使用滞后滤波器平滑高度误差 -->  
<lag_filter name="fcs/alt-error-lag">  
  <input>fcs/altitude-error</input>  
  <c1>1</c1>  
</lag_filter>  
  
<!-- 计算垂直速度命令 -->  
<fcs_function name="fcs/hdot-command">  
  <input>fcs/alt-error-lag</input>  
  <function>  
    <quotient>  
      <difference>  
        <value>45.0</value>  
        <quotient>  
          <property>position/h-sl-ft</property>  
          <value>833.33</value>  
        </quotient>  
      </difference>  
      <value>100.0</value>  
    </quotient>  
  </function>  
</fcs_function>  
  
<!-- 计算垂直速度误差 -->  
<summer name="fcs/hdot-error">  
  <input>fcs/hdot-command</input>  
  <input>-velocities/h-dot-fps</input>  
</summer>  
  
<!-- 高度保持开关 -->  
<switch name="fcs/ap-alt-hold-switch">  
  <default value="0.0"/>  
  <test value="fcs/hdot-error">  
    ap/altitude_hold == 1  
  </test>  
</switch>  
  
<!-- 使用积分器和比例增益生成俯仰控制命令 -->  
<integrator name="fcs/integral">  
  <input>fcs/ap-alt-hold-switch</input>  
  <c1>0.001</c1>  
</integrator>  
<pure_gain name="fcs/proportional">  
  <input>fcs/ap-alt-hold-switch</input>  
  <gain>0.03</gain>  
</pure_gain>  
<summer name="fcs/control-summer">  
  <input>fcs/integral</input>  
  <input>fcs/proportional</input>  
  <clipto>  
    <min>-1.0</min>  
    <max>1.0</max>  
  </clipto>  
</summer>  
<pure_gain name="fcs/elevator">  
  <input>fcs/control-summer</input>  
  <gain>-1.0</gain>  
  <output>ap/elevator_cmd</output>  
</pure_gain>  

</channel>

<!--

YAW CHANNEL

-->

<!-- Yaw Damper偏航阻尼器 -->

<channel name="Yaw Damper">
<scheduled_gain name="ap/rudder_control">
<input>aero/beta-rad</input>
<table>
<independentVar lookup="row">velocities/ve-kts</independentVar>
<tableData>
30 0.00
60 -40.00
</tableData>
</table>
<gain>ap/yaw_damper</gain>
<clipto>
<min>-1.0</min>
<max>1.0</max>
</clipto>
<output>ap/rudder_cmd</output>
</scheduled_gain>
</channel>
</autopilot>