jsbsim源码中的f16飞机没有自动驾驶仪，但我想通过加载导航系统文件和自动驾驶仪实现指定航路点飞行

[fairyabcd]

jsbsim源码中的f16飞机没有自动驾驶仪，但我想通过加载导航系统文件和自动驾驶仪实现指定航路点飞行，目前我自己写的自动驾驶仪xml文件可以正确加载，但无法飞到指定的航路点一直在转圈飞行，f16的自动驾驶仪脚本如下：

ap/attitude_hold
ap/altitude_hold
ap/active-waypoint
ap/altitude_setpoint
ap/heading-setpoint-select
ap/aileron_cmd
ap/heading_hold
ap/yaw_damper
ap/velocity_hold

ap/roll-control-autopilot-on
ap/autopilot-roll-on
ap/roll-angle-saturation
ap/limited-roll-angle-rad

ap/heading-setpoint-select == 1

<!-- 计算航向误差 -->  
<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>-10</min>  
    <max>10</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>4.0</kp>  
  <ki>0.1</ki>  
  <kd>0.5</kd>
</pid>  
<output>ap/aileron_cmd</output>  
<!-- 选择滚转控制命令输出--> 
<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>    

ap/altitude_setpoint -position/h-sl-ft -150 150

<!-- 使用滞后滤波器平滑高度误差 -->  
<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>  

<switch name="ap/roll-control-autopilot-on">
  <default value="0"/>
  <test value="1">
    ap/autopilot-roll-on ne 0.0
  </test>
</switch>

<switch name="ap/roll-autopilot-windup-trigger">
  <default value="1"/>
  <test value="1">
    ap/roll-angle-saturation ne 0.0
  </test>
  <test value="0">
    ap/roll-control-autopilot-on gt 0
  </test>
</switch>

<pure_gain name="ap/limited-roll-angle-rad">
  <description>
    This component takes as input the angle to the target heading from the
    current heading, as calculated by the Angle to Heading channel in the
    GNC Utilities system file. The output of this component (stored) in the
    component property "ap/limited-roll-angle-rad" is the roll angle command,
    limited at 30 degrees (0.524 radians), though this can be overridden.
  </description> <!-- Autopilot中是guidance/angle-to-heading-rad -->
  <input> guidance/angle-to-heading-deg </input>
  <gain> 1.0 </gain>
  <clipto>
    <min> -guidance/roll-angle-limit </min>
    <max>  guidance/roll-angle-limit </max>
  </clipto>
</pure_gain>

<switch name="ap/roll-attitude-selector">
  <description>
    This component selects whether to use a roll angle of "0" - which
    is a wings-level attitude - or to use the roll angle commanded by the 
    ap/limited-roll-angle-rad component, above, which is used to acquire
    and hold a heading.
    Set ap/roll-attitude-mode to 1 for roll angle hold,
                              to 0 for wings level (roll attitude of zero).
  </description>
  <default value="0"/>
  <test value="ap/limited-roll-angle-rad">
    ap/roll-attitude-mode eq 1
  </test>
</switch>

<summer name="ap/limited-roll-angle-error">
  <description>
    This component computes the error between the calculated target roll
    angle and the actual roll angle. The output is interpreted as a roll
    rate command.
  </description>
  <input>  ap/roll-attitude-selector </input>
  <input> -attitude/phi-rad </input>
</summer>

<pid name="ap/roll-rate-pid-control">
  <input> ap/limited-roll-angle-error </input>
  <kp> 30 </kp>
  <ki> 0.1  </ki>
  <kd> 0.1  </kd>
  <trigger> ap/roll-autopilot-windup-trigger </trigger>
  <clipto>
    <min> -guidance/roll-rate-limit </min>
    <max>  guidance/roll-rate-limit </max>
  </clipto>
</pid>

<pure_gain name="ap/total-limited-roll-rate">
  <input>guidance/roll-rate-limit</input>
  <gain>2.0</gain>
</pure_gain>

<deadband name="ap/roll-rate-saturation">
  <input> ap/roll-rate-pid-control </input>
  <width> ap/total-limited-roll-rate </width>
</deadband>

<summer name="ap/limited-roll-rate-error">
  <description>
    This component computes the error between the calculated target roll
    rate and the actual roll rate. The output is interpreted as an aileron
    command.
  </description>
  <input> ap/roll-rate-pid-control </input>
  <input> -velocities/p-aero-rad_sec </input> <!-- -飞行器绕机体y轴的角速度 -->
</summer>

<pid name="ap/roll-command-pid-control">
  <input> ap/limited-roll-rate-error </input>
  <kp>2.5</kp>
  <ki>0.0</ki>
  <kd>0.0</kd>
  <trigger> ap/roll-autopilot-windup-trigger </trigger>
</pid>

<lag_filter name="ap/roll-cmd-smoother">
  <input> ap/roll-command-pid-control </input>
  <c1> 100.0 </c1>
</lag_filter>

<switch name="ap/roll-cmd-norm-output">
  <default value="0"/>
  <test value="ap/roll-cmd-smoother">
    ap/roll-control-autopilot-on ne 0.0
  </test>
</switch>

<switch name="ap/roll-command-selector-steering">
  <default value="0.0"/>
  <test value="ap/roll-cmd-norm-output">
    ap/roll-control-autopilot-on == 1
    gear/unit[0]/WOW == 1
  </test>
  <output>fcs/steer-cmd-norm</output> <!-- 输出-->
</switch>

aero/beta-rad velocities/ve-kts 30 0.00 60 -40.00 ap/yaw_damper -1.0 1.0 ap/rudder_cmd 设置f16航路点的脚本如下， simulation/notify-time-trigger simulation/run_id propulsion/tank[0]/contents-lbs propulsion/tank[1]/contents-lbs propulsion/tank[2]/contents-lbs propulsion/tank[3]/contents-lbs

<event name="Engine start">
  <condition> simulation/sim-time-sec ge 0.0 </condition> 
  <set name="fcs/throttle-cmd-norm" value="1.0" action="FG_RAMP" tc="0.5"/>
  <set name="fcs/mixture-cmd-norm" value="1"/>
  <set name="propulsion/starter_cmd" value="1"/>
  <set name="position/h-sl-ft" value="10000" action="EXP"/>
<!--  <set name="ap/attitude_hold" value="1"/>-->
  <set name="ap/heading-setpoint-select" value="1"/> 
  <set name="ap/heading_hold" value="1"/>
  <set name="ap/altitude_setpoint" value="1"/>
  <set name="ap/altitude_hold" value="1"/>
  <set name="ap/roll-control-autopilot-on" value="1"/>
  <!--<set name="fcs/fbw-override" value="1"/> -->
  <notify>
    <property caption="Latitude(gc-deg)  "> position/lat-gc-deg </property>
    <property caption="Longitude(gc-deg)   "> position/long-gc-deg </property>
  </notify>
</event>

<event name="Set altitude hold">
  <description> set altitude-altitude and set steady flight </description>
  <condition> simulation/sim-time-sec gt 0.0 </condition>
  <!--<set name="ap/altitude_hold" value="1"/> -->
  <notify>
    <property caption="Latitude(gc-deg)  "> position/lat-gc-deg </property>
    <property caption="Longitude(gc-deg)   "> position/long-gc-deg </property>
  </notify>
</event>

<!-- 起落架收起，距地面xx英尺则gear/gear-cmd-norm==0 -->
<event name="release lading gear">
  <condition>position/h-agl-ft  ge  40 </condition>
  <set name="gear/gear-cmd-norm" value="0"/>
  <notify> 
    <property caption="Latitude(gc-deg)  "> position/lat-gc-deg </property>
    <property caption="Longitude(gc-deg)   "> position/long-gc-deg </property>
  </notify>
</event>

<event name="Arrive last wp">
  <condition>
    guidance/wp-distance lt 5
    <!--ap/active-waypoint eq 5 -->
    ap/active-waypoint eq 3
  </condition> 
  <!--<set name="ap/altitude_hold" value="1"/> -->
  <notify>
    <property caption="Latitude(gc-deg)   "> position/lat-gc-deg </property>
    <property caption="Longitude(gc-deg)    "> position/long-gc-deg </property>
    <property caption="Distance to WP    "> guidance/wp-distance  </property>
    <property caption="waypoint latitude "> guidance/target_wp_latitude_deg </property>
    <property caption="waypoint longitude"> guidance/target_wp_longitude_deg </property>
  </notify>
</event>

<event name="Arrive No.2 wp/set No.3 wp">
  <condition>
    guidance/wp-distance lt 5
    ap/active-waypoint eq 2
  </condition> 
  <set name="ap/active-waypoint" value="3"/>
  <set name="guidance/target_wp_latitude_deg" value="38.994656"/>
  <set name="guidance/target_wp_longitude_deg" value="83.667239"/>
  <notify>
    <property caption="Latitude(gc-deg)   "> position/lat-gc-deg </property>
    <property caption="Longitude(gc-deg)    "> position/long-gc-deg </property>
    <property caption="Distance to current WP "> guidance/wp-distance  </property>
    <property caption="next waypoint latitude "> guidance/target_wp_latitude_deg </property>
    <property caption="next waypoint longitude"> guidance/target_wp_longitude_deg </property>
  </notify>
</event>
   
<event name="Arrive No.1 wp/set No.2 wp">
  <description> When the distance arrives to first wp less than 5meters,set the second wp.</description>
  <condition>
    guidance/wp-distance lt 100
    ap/active-waypoint eq 1
  </condition> 
  
  <set name="ap/active-waypoint" value="2"/>
  <set name="guidance/target_wp_latitude_deg" value="39.094656"/>
  <set name="guidance/target_wp_longitude_deg" value="84.267239"/>
  <notify>
    <property caption="Latitude(gc-deg)   "> position/lat-gc-deg </property>
    <property caption="Longitude(gc-deg)    "> position/long-gc-deg </property>
    <property caption="Distance to current WP "> guidance/wp-distance  </property>
    <property caption="next waypoint latitude "> guidance/target_wp_latitude_deg </property>
    <property caption="next waypoint longitude"> guidance/target_wp_longitude_deg </property>
  </notify>
</event>

<event name="Head to first waypoint">
  <condition> simulation/sim-time-sec ge 0.0 </condition>
  <set name="ap/active-waypoint" value="1"/>
  <set name="guidance/target_wp_latitude_deg" value="39.594656"/>
  <set name="guidance/target_wp_longitude_deg" value="84.267239"/>
  <notify>
    <property caption="Latitude(gc-deg)   "> position/lat-gc-deg </property>
    <property caption="Longitude(gc-deg)    "> position/long-gc-deg </property>
    <property caption="Distance to WP         "> guidance/wp-distance  </property>
    <property caption="next waypoint latitude "> guidance/target_wp_latitude_deg </property>
    <property caption="next waypoint longitude"> guidance/target_wp_longitude_deg </property>
  </notify>
</event>

propulsion/total-fuel-lbs eq 0 propulsion/total-fuel-lbs

<!--   remember delete this event[Time Notify]-->
<event name="Time Notify" persistent="true"> 
  <description>Output message at xx seconds intervals</description>
  <condition> simulation/sim-time-sec ge simulation/notify-time-trigger </condition>
  <set name="simulation/notify-time-trigger" value="300" type="FG_DELTA"/>
  <notify>
    <property> velocities/mach </property>
    <property> propulsion/total-fuel-lbs </property>
    <property> position/long-gc-deg </property>
    <property> position/lat-gc-deg </property>
    <property> guidance/wp-distance </property>
    <property> guidance/target_wp_longitude_deg </property>
    <property> guidance/target_wp_latitude_deg </property>
    <property> fcs/hdot-command </property>
    <property> velocities/u-fps</property>
    <property> velocities/vt-fps </property>
    <property> ic/u-fps </property>
    <property> ic/vt-fps </property>
  </notify>
</event>

[agodemar]

@fairyabcd would you be so kind as to type your posts in Roman characters?