<?xml version="1.0"?>
<!--

  Author:   Jon Berndt
  Date:     18 August 2002
  Function: C-172 autopilot test file

  Note: this file represents a test only of the new autopilot
        setup in JSBSim. The same components used in the FCS
        can also be used in the autopilot section. There are
        some new FCS input and output identifiers directly
        related to the autopilot:

          ap/elevator_cmd,
          ap/aileron_cmd,
          ap/attitude_hold,
          ap/altitude_hold,
          ap/heading_hold,
          ap/altitude_setpoint,
          ap/heading_setpoint

-->
<autopilot name="C-172X Autopilot">

<!-- INTERFACE PROPERTIES -->

  <property>ap/attitude_hold</property>
  <property>ap/altitude_hold</property>
  <property>ap/heading_hold</property>
  <property>ap/altitude_setpoint</property>
  <property>ap/heading_setpoint</property>
  <property>ap/aileron_cmd</property>
  <property>ap/elevator_cmd</property>
  <property>ap/airspeed_setpoint</property>
  <property>ap/airspeed_hold</property>
  <property>ap/throttle-cmd-norm</property>

<!-- INITIAL GAIN VALUES -->
  
  <property value="0.5"> ap/hdg-roll-err-c1 </property>
  <property value="50.0"> ap/roll-pid-kp </property>
  <property value="5.0"> ap/roll-pid-ki </property>
  <property value="17.0"> ap/roll-pid-kd </property>

  <!--  <property>attitude/sensor/phi-rad</property> -->

<!--
=====================================================
ROLL CHANNEL
=====================================================
-->

<!-- Wing leveler -->

  <property value="0"> simulation/input1 </property>
  <property value="0"> simulation/input2 </property>

<channel name="Roll wing leveler">

  <sensor name="fcs/attitude/sensor/phi-rad">
    <input> attitude/phi-rad </input>
    <lag> 0.5 </lag>
    <delay> 2 </delay>
    <noise variation="PERCENT" distribution="GAUSSIAN"> 0.05 </noise>
    <quantization name="attitude/sensor/quantized/phi-rad">
      <bits> 12 </bits>
      <min> -3.1416 </min>
      <max>  3.1416 </max>
    </quantization>
    <bias> 0.001 </bias>
  </sensor>

  <switch name="simulation/test">
    <default value="-1"/>
    <test value="0">
      simulation/input1 eq 1
      simulation/input2 gt 2
    </test>
    <test value="1" logic="OR">
      simulation/input1 eq 3
      simulation/input2 gt 4
    </test>
    <test value="2">
      <test logic="AND">
        simulation/input1 eq 4
        simulation/input2 gt 5
      </test>
      <test logic="OR">
        simulation/input1 eq 6
        simulation/input2 gt 7
      </test>
    </test>
    <test value="3">
      <test logic="AND">
        simulation/input1 eq 8
        simulation/input2 gt 9
      </test>
      <test logic="OR">
        simulation/input1 eq 10
        simulation/input2 gt 11
      </test>
    </test>
  </switch>
    
  <switch name="fcs/wing-leveler-ap-on-off">
    <default value="-1"/>
    <test value="0">
      ap/attitude_hold == 1
    </test>
  </switch>

  <pid name="fcs/roll-ap-error-pid">
    <input>attitude/phi-rad</input>
    <kp> ap/roll-pid-kp </kp>
    <ki> ap/roll-pid-ki </ki>
    <kd> ap/roll-pid-kd </kd>
    <trigger> fcs/wing-leveler-ap-on-off </trigger>
  </pid>

  <switch name="fcs/roll-ap-autoswitch">
    <default value="0.0"/>
    <test value="-fcs/roll-ap-error-pid">
      ap/attitude_hold == 1
    </test>
  </switch>

</channel>

<!-- Heading hold -->

<channel name="Roll heading hold">
  <pure_gain name="fcs/heading-true-degrees">
    <input>attitude/heading-true-rad</input>
    <gain>57.3</gain> <!-- convert to degrees -->
  </pure_gain>

  <summer name="fcs/heading-error">
    <input> -fcs/heading-true-degrees</input>
    <input> ap/heading_setpoint </input>
  </summer>

  <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>-30</min>
      <max>30</max>
    </clipto>
  </summer>

  <pure_gain name="fcs/heading-command">
    <input> fcs/heading-corrected </input>
    <gain> 0.01745 </gain>
  </pure_gain>

  <lag_filter name="fcs/heading-roll-error-lag">
    <input> fcs/heading-command </input>
    <c1> ap/hdg-roll-err-c1 </c1>
  </lag_filter>

  <summer name="fcs/heading-roll-error">
    <input> fcs/heading-roll-error-lag </input>
    <input> -attitude/phi-rad </input>
  </summer>

  <switch name="fcs/heading-roll-error-switch">
    <default value="0.0"/>
    <test value="fcs/heading-roll-error">
      ap/heading_hold == 1
    </test>
  </switch>

  <pid name="fcs/heading-pi-controller">
    <input> fcs/heading-roll-error-switch </input>
    <kp> 6.0 </kp>
    <ki> 0.13 </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
      gear/unit[2]/WOW == 0
    </test>
    <test value="fcs/roll-ap-autoswitch">
      ap/attitude_hold == 1
      gear/unit[2]/WOW == 0
    </test>
    <output>ap/aileron_cmd</output>
  </switch>
<!--
  <switch name="fcs/roll-command-selector-steering">
    <default value="0.0"/>
    <test value="fcs/heading-pi-controller">
      ap/heading_hold == 1
      gear/unit/WOW == 1
    </test>
    <output>fcs/steer-cmd-norm</output>
  </switch>
-->
</channel>
<!--
=====================================================
PITCH CHANNEL
=====================================================
-->

<!-- Altitude hold -->

<!-- The Altitude Error component below computes the altitude error, subtracting
     the desired altitude (altitude_setpoint) from the actual altitude above sea
     level (_not_ Above Ground Level).  This error signal is interpreted as an
     hdot command (hdot is time rate of change of altitude, or rate of climb). As
     such it is limited to a maximum absolute value of 12 fps here (720 fpm). The
     maximum achievable climb rate depends on altitude. The commanded climb rate
     is scheduled in the HDot Command component, below. For the given altitude
     (left column in the table), the commanded maaximum climb rate divided by 100
     is given in the right column.
-->

<channel name="Pitch altitude hold">

  <!--
  The difference between the desired altitude and the actual altitude
  is determined, and limited to 100. The output from this component is
  the desired climb rate in percent of maximum.
  -->
  <summer name="fcs/altitude-error">
    <input> ap/altitude_setpoint </input>
    <input> -position/h-agl-ft </input>
    <clipto>
      <min>-100</min>
      <max> 100</max>
    </clipto>
  </summer>

  <!--
  The desired climb rate is lagged slightly for stability.
  -->
  <lag_filter name="fcs/alt-error-lag">
    <input> fcs/altitude-error </input>
    <c1> 1 </c1>
  </lag_filter>

  <!--
  Dependent on altitude, the lagged (and limited) altitude error is multipled
  by the scheduled gain determined from the table, below. The output from this
  component is the absolute climb rate in feet/second. For example, if the desired
  climb rate is 100 percent of maximum and the current altitude is 1000.0 ft., then
  the output from this component would be 11 ft. sec.
  -->
  <scheduled_gain name="fcs/hdot-command">
    <input> fcs/alt-error-lag </input>
    <table>
      <independentVar>position/h-sl-ft</independentVar>
      <tableData>
           0.0   0.12
        1000.0   0.11
        2000.0   0.10
        3000.0   0.096
        4000.0   0.093
        5000.0   0.086
        6000.0   0.078
        7000.0   0.069
        8000.0   0.061
        9000.0   0.053
       10000.0   0.045
       11000.0   0.037
       12000.0   0.028
      </tableData>
    </table>
  </scheduled_gain>

  <!--
  This component calculates the climb rate error, taking the difference between
  the commanded climb rate (from the previous component) and actual climb rate
  in ft./sec.
  -->
  <summer name="fcs/hdot-error">
    <input> fcs/hdot-command </input>
    <input> -velocities/h-dot-fps </input>
  </summer>

  <!--
  If the altitude hold autopilot command is ON, then this switch component will
  pass through the climb rate error (from the previous component). Otherwise, it
  will pass zero.
  -->
  <switch name="fcs/ap-alt-hold-switch">
    <default value="0.0"/>
    <test value="fcs/hdot-error">
      ap/altitude_hold == 1
    </test>
  </switch>

  <!--
  The windup trigger below assumes the elevator will travel +/-23 degrees. The
  elevator, however, does not travel symmetrically. This will need to be addressed
  in a fix to the deadband component.
  -->
  <deadband name="fcs/windup-trigger">
    <input> fcs/elevator-pos-deg </input>
    <width>46.0</width>
  </deadband>

  <!--
  The integrator integrates the hdot error (when the switch component passes that
  signal through above when the altitude hold is selected ON). In the situation
  where the elevator becomes saturated, the integrator ceases to integrate. The
  windup protection is indicated below, with the windup-trigger property being
  the trigger to halt integration. When the windup trigger is non-zero (when the
  elevator position falls outside the range +/- 23 degrees - a deadband of 46
  degrees) then the deadband passes a non-zero value, triggering the anti-windup
  logic in the integrator.

  The proportional component multiplies the error signal by a constant, providing
  the proportional control action of this PI altitude hold controller.

  The pid component combines the proportional and integral control
  signals. It clips the sum to +/- 1.0.
  -->

  <pid name="fcs/altitude-hold-pid">
    <input> fcs/ap-alt-hold-switch </input>
    <kp> 0.01 </kp>
    <ki> 0.00015 </ki>
    <kd> 0.0003 </kd>
    <trigger> fcs/windup-trigger </trigger>
    <clipto> <min>-1.0</min>
             <max> 1.0</max> </clipto>
  </pid>

  <!--
  The elevator component flips the sign on the output of the control summer
  above and sets the ap/elevator_command property.
  -->
  <pure_gain name="fcs/elevator">
    <input> fcs/altitude-hold-pid </input>
    <gain> -1.0 </gain>
    <output> ap/elevator_cmd </output>
  </pure_gain>
</channel>

</autopilot>
