<?xml version="1.0"  encoding="UTF-8"?>

<!-- autopilot from Alouette-III-jsbsim Thorsten Renk 2017 -->
<!-- Modified by HH64 & Clm76 - 2021,2022,2023 -->

<system name="Autopilot">

  <!-- For this helicopter model, pitch (elevator) is appropriate for speed setting and throttle (collective) for altitude regulation. -->

  <!-- Radar Altitude - common to Autopilot and Autohover -->

  <channel name="Common">
    <fcs_function>
      <function name="autopilot/throttle/Alt-indicated-error">
        <difference>
          <property>autopilot/settings/target2-altitude-ft</property>
          <property>/position/altitude-ft</property>
        </difference>
      </function>
    </fcs_function>

    <fcs_function>
      <function name="autopilot/throttle/Alt-radar-error">
        <difference>
          <property>autopilot/settings/target2-altitude-ft</property>
          <property>/position/altitude-agl-ft</property>
        </difference>
      </function>
    </fcs_function>

    <switch name="autopilot/throttle/aph-altitude-error">
      <default value="autopilot/throttle/Alt-indicated-error"/>
      <test value="autopilot/throttle/Alt-radar-error">
        autopilot/lock/radar-lock-altitude == 1
      </test>
    </switch>
  </channel>

  <!-- Autopilot -->
    <!-- altitude -->
    <!-- For altitude regulation, throttle autotrim is used.
    To modify helicopter speed use elevator or elevator-trim -->

  <channel name="throttle">

    <deadband name="autopilot/throttle/ap-throttle-trigger1">
      <input>autopilot/throttle/aph-altitude-error</input>
      <width>20.0</width>
    </deadband>

    <deadband name="autopilot/throttle/ap-throttle-trigger2">
      <input>autopilot/throttle/aph-altitude-error</input>
      <width>2.0</width>
    </deadband>

    <switch name="autopilot/throttle/ap-throttle-trigger">
      <default value="autopilot/throttle/ap-throttle-trigger1"/>
      <test value="autopilot/throttle/ap-throttle-trigger2">
        autopilot/lock/radar-lock-altitude == 1
      </test>
    </switch>

    <pid name="autopilot/throttle/ap-throttle-autotrim-raw">
      <input>autopilot/throttle/aph-altitude-error</input>
      <kp>/_tuning/pid/autopilot-throttle-kp</kp>
      <ki>/_tuning/pid/autopilot-throttle-ki</ki>
      <kd>/_tuning/pid/autopilot-throttle-kd</kd>
      <trigger>autopilot/throttle/ap-throttle-trigger</trigger>
      <clipto>
        <min>/_tuning/trim/autopilot-throttle-min</min>
        <max>/_tuning/trim/autopilot-throttle-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/throttle/ap-throttle-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/throttle/ap-throttle-autotrim-raw">
        autopilot/lock/autopilot-engaged == 1
        <test logic="OR" value="autopilot/throttle/ap-throttle-autotrim-raw">
          autopilot/lock/autopilot-lock-altitude == 1
          autopilot/lock/radar-lock-altitude == 1
        </test>
      </test>
    </switch>

  </channel>

    <!-- heading -->
  <channel name="Roll">

    <switch name="autopilot/roll/channel-active">
      <default value="0"/>
      <test logic="OR" value="1">
        autopilot/lock/autopilot-lock-heading == 1
        autopilot/lock/autopilot-lock-nav1 == 1
        autopilot/lock/autopilot-lock-nav2 == 1
        autopilot/lock/autolanding-engaged == 1
      </test>
    </switch>

    <switch name="autopilot/roll/heading-error-deg">
      <default value="0.0"/>
      <test logic="AND" value="instruments/heading-indicator/heading-error-deg">
        autopilot/lock/autopilot-engaged == 1
        autopilot/roll/channel-active == 1
        autopilot/lock/autolanding-engaged == 0
        <test logic="OR" value="1">
          autopilot/lock/autopilot-lock-nav1 == 0
          <test logic="AND" value="1">
            autopilot/lock/autopilot-lock-nav1 == 1
            /instrumentation/nav/in-range == 0
          </test>
        </test>
        <test logic="OR" value="1">
          autopilot/lock/autopilot-lock-nav2 == 0
          <test logic="AND" value="1">
            autopilot/lock/autopilot-lock-nav2 == 1
            /instrumentation/nav[1]/slaved-to-gps == 0
          </test>
        </test>
      </test>
      <test logic="AND" value="autopilot/roll/course-offset">
        autopilot/lock/autopilot-engaged == 1
        autopilot/roll/channel-active == 1
        <test logic="OR" value="1">
          autopilot/lock/autolanding-engaged == 1
          <test logic="AND" value="1">
            autopilot/lock/autopilot-lock-nav1 == 1
            /instrumentation/nav/in-range == 1
          </test>
          <test logic="AND" value="1">
            autopilot/lock/autopilot-lock-nav2 == 1
            <test logic="OR" value="1">
              /instrumentation/nav[1]/in-range == 1
              /instrumentation/nav[1]/slaved-to-gps == 1
            </test>
          </test>
        </test>
      </test>
    </switch>

    <!-- Bank angle related to speed and heading error
        The data table incorporates compensation due to
        the roll inclination of the helicopter (~ 2.6° left) in flight.
    -->

    <fcs_function name="autopilot/roll/target-bank-angle">
      <function>
        <table>
          <independentVar lookup="row">autopilot/roll/heading-error-deg</independentVar>
          <independentVar lookup="column">velocities/uBody-kmh</independentVar>
          <tableData>
                  0     120   170
            -180  -5.8  -1.8 -5.8
            -2    -5.8  -1.8 -5.8
            0     0.0   0.0   0.0
            2     3.2   6.8  12.0
            180   3.2   6.8  12.0
          </tableData>
        </table>
      </function>
    </fcs_function>

    <fcs_function name="autopilot/roll/bank-angle-error-raw">
      <function>
        <product>
          <difference>
            <property>autopilot/roll/target-bank-angle</property>
            <property>/orientation/roll-deg</property>
          </difference>
          <property>autopilot/lock/autopilot-engaged</property>
          <property>autopilot/roll/channel-active</property>
        </product>
      </function>
    </fcs_function>

    <fcs_function name="autopilot/roll/bank-angle-error">
      <function>
        <max>
          <min>
            <property>autopilot/roll/bank-angle-error-raw</property>
            <value>5.0</value>
          </min>
          <value>-5.0</value>
        </max>
      </function>
    </fcs_function>

    <pid name="autopilot/roll/ap-roll-autotrim-raw">
      <input>autopilot/roll/bank-angle-error</input>
      <kp>/_tuning/pid/autopilot-roll-kp</kp>
      <ki>/_tuning/pid/autopilot-roll-ki</ki>
      <kd>/_tuning/pid/autopilot-roll-kd</kd>
      <clipto>
        <min>/_tuning/trim/autopilot-roll-min</min>
        <max>/_tuning/trim/autopilot-roll-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/roll/ap-roll-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/roll/ap-roll-autotrim-raw">
        autopilot/lock/autopilot-engaged == 1
        autopilot/roll/channel-active == 1
      </test>
    </switch>
  </channel>

  <channel name="Yaw">
    <switch name="autopilot/yaw/ap-beta-error">
      <default value="0.0"/>
      <test logic="AND" value="/instrumentation/gsdi/drift-angle-deg">
        autopilot/lock/autopilot-engaged == 1
      </test>
    </switch>

    <deadband name="autopilot/yaw/ap-beta-error-trigger">
      <input>autopilot/yaw/ap-beta-error</input>
      <width>5</width>
    </deadband>

    <pid name="autopilot/yaw/ap-yaw-autotrim-raw">
      <input>autopilot/yaw/ap-beta-error</input>
      <kp>/_tuning/pid/autopilot-yaw-kp</kp>
      <ki>/_tuning/pid/autopilot-yaw-ki</ki>
      <kd>/_tuning/pid/autopilot-yaw-kd</kd>
      <trigger>autopilot/yaw/ap-beta-error-trigger</trigger>
      <clipto>
        <min>/_tuning/trim/autopilot-yaw-min</min>
        <max>/_tuning/trim/autopilot-yaw-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/yaw/ap-yaw-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="0">
        autopilot/lock/autoyaw-engaged == 1
        autopilot/lock/autopilot-lock-heading == 1
      </test>
      <test logic="AND" value="autopilot/yaw/ap-yaw-autotrim-raw">
        autopilot/lock/autopilot-engaged == 1
        <test logic="OR" value="1">
          autopilot/lock/autopilot-lock-heading == 1
          autopilot/lock/autopilot-lock-nav1 == 1
          autopilot/lock/autopilot-lock-nav2 == 1
          autopilot/lock/autolanding-engaged == 1
        </test>
      </test>
    </switch>
  </channel>

  <!-- Auto-hover -->

  <channel name="Autohover">
    <!-- low_speed_trigger -->
    <switch name="autopilot/autohover/triggered">
      <default value="0"/>
      <test logic="OR" value="0">
        /instrumentation/gsdi/drift-speed-kt gt 100
        autopilot/lock/autohover-engaged == 0
      </test>
      <test logic="AND" value="1">
        autopilot/lock/autohover-engaged == 1
      </test>
    </switch>

    <!-- autohover roll control -->

    <fcs_function name="/instrumentation/gsdi/drift-v2-kt">
      <function>
        <sum>
          <property>/instrumentation/gsdi/drift-v-kt</property>
          <product>
            <table>
              <independentVar lookup="row">/controls/flight/aileron</independentVar>
              <tableData>
              -1 1
              -.2 0
              0 0
              .2 0
               1 -1
              </tableData>
            </table>
            <property>/_tuning/autohover/drift-v-kt</property>
          </product>
        </sum>
      </function>
    </fcs_function>

    <fcs_function name="autopilot/autohover/drift-v-kt">
      <function>
        <product>
          <property>/instrumentation/gsdi/drift-v2-kt</property>
          <property>autopilot/autohover/triggered</property>
        </product>
      </function>
    </fcs_function>

    <pid name="autopilot/autohover/roll-autotrim-raw">
      <input>-autopilot/autohover/drift-v-kt</input>
      <kp>/_tuning/pid/autohover-roll-kp</kp>
      <ki>/_tuning/pid/autohover-roll-ki</ki>
      <kd>/_tuning/pid/autohover-roll-kd</kd>
      <clipto>
        <min>/_tuning/trim/autohover-roll-min</min>
        <max>/_tuning/trim/autohover-roll-max</max>
      </clipto>
    </pid>

    <fcs_function name="autopilot/autohover/roll-autotrim">
      <function>
        <product>
          <property>autopilot/autohover/roll-autotrim-raw</property>
          <property>autopilot/autohover/triggered</property>
        </product>
      </function>
    </fcs_function>

    <!-- autohover pitch control -->
    <fcs_function name="/instrumentation/gsdi/drift-u2-kt">
      <function>
        <sum>
          <property>/instrumentation/gsdi/drift-u-kt</property>
          <product>
            <table>
              <independentVar lookup="row">/controls/flight/elevator</independentVar>
              <tableData>
              -1 1
              -.2 0
              0 0
              .2 0
               1 -1
              </tableData>
            </table>
            <property>/_tuning/autohover/drift-u-kt</property>
          </product>
        </sum>
      </function>
    </fcs_function>

    <fcs_function name="autopilot/autohover/drift-u-kt">
      <function>
        <product>
          <property>/instrumentation/gsdi/drift-u2-kt</property>
          <property>autopilot/autohover/triggered</property>
        </product>
      </function>
    </fcs_function>

    <fcs_function name="/_tuning/pid/autohover-pitch-ki">
      <function>
        <table>
          <independentVar lookup="row">autopilot/autohover/drift-u-kt</independentVar>
          <tableData>
          -5 5
          0 5
          0.1 5
          0.2 4
          0.3 3
          0.4 2
          0.5 1
          1 0.01
          3 0.01
          </tableData>
        </table>
      </function>
    </fcs_function>

    <pid name="autopilot/autohover/pitch-autotrim-raw">
      <input>-autopilot/autohover/drift-u-kt</input>
      <kp>/_tuning/pid/autohover-pitch-kp</kp>
      <ki>/_tuning/pid/autohover-pitch-ki</ki>
      <kd>/_tuning/pid/autohover-pitch-kd</kd>
      <clipto>
        <min>/_tuning/trim/autohover-pitch-min</min>
        <max>/_tuning/trim/autohover-pitch-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/autohover/pitch-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/autohover/pitch-autotrim-raw">
        autopilot/autohover/triggered == 1
      </test>
    </switch>

    <!-- autohover altitude control -->

    <deadband name="autopilot/autohover/throttle-trigger">
      <input>autopilot/throttle/aph-altitude-error</input>
      <width>0</width>
    </deadband>

    <pid name="autopilot/autohover/throttle-autotrim-raw">
      <input>autopilot/throttle/aph-altitude-error</input>
      <kp>/_tuning/pid/autohover-throttle-kp</kp>
      <ki>/_tuning/pid/autohover-throttle-ki</ki>
      <kd>/_tuning/pid/autohover-throttle-kd</kd>
      <trigger>autopilot/autohover/throttle-trigger</trigger>
      <clipto>
        <min>/_tuning/trim/autohover-throttle-min</min>
        <max>/_tuning/trim/autohover-throttle-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/autohover/throttle-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/autohover/throttle-autotrim-raw">
        autopilot/autohover/triggered == 1
        <test logic="OR" value="autopilot/autohover/throttle-autotrim-raw">
          autopilot/lock/autopilot-lock-altitude == 1
          autopilot/lock/radar-lock-altitude == 1
        </test>
      </test>
    </switch>

    <!-- autohover heading control -->
    <switch name="autopilot/yaw/ah-heading-error-deg">
      <default value="0.0"/>
      <test logic="AND" value="instruments/heading-indicator/heading-error-deg">
        autopilot/lock/autohover-engaged == 1
      </test>
    </switch>

    <deadband name="autopilot/yaw/ah-heading-error-trigger">
      <input>autopilot/yaw/ah-heading-error-deg</input>
      <width>20</width>
    </deadband>

    <pid name="autopilot/autohover/yaw-autotrim-raw">
      <input>autopilot/yaw/ah-heading-error-deg</input>
      <kp>/_tuning/pid/autohover-yaw-kp</kp>
      <ki>/_tuning/pid/autohover-yaw-ki</ki>
      <kd>/_tuning/pid/autohover-yaw-kd</kd>
      <trigger>autopilot/yaw/ah-heading-error-trigger</trigger>
      <clipto>
        <min>/_tuning/trim/autohover-yaw-min</min>
        <max>/_tuning/trim/autohover-yaw-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/autohover/yaw-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/autohover/yaw-autotrim-raw">
        autopilot/autohover/triggered == 1
        autopilot/lock/autopilot-lock-heading == 1
      </test>
    </switch>

  </channel>


  <!-- SAS -->
  <channel name="SAS">
    <!-- If SAS is active, auto-pitch, auto-roll and auto-yaw are respectively active. If autopilot, autohover or autolanding are engaged, priority is given to them depending on configuration. -->

    <!-- auto-pitch -->
    <switch name="autopilot/lock/autopitch-engaged">
      <default value="1"/>
      <test logic="AND" value="1">
        autopilot/lock/autopilot-engaged == 1
        autopilot/lock/sas-engaged == 1
        autopilot/lock/autohover-engaged == 0
        autopilot/lock/autospeed-engaged == 0
        <test logic="OR" value="1">
          autopilot/lock/autopilot-lock-altitude == 1
          autopilot/lock/radar-lock-altitude == 1
        </test>
      </test>
      <test logic="OR" value="0">
        autopilot/lock/autohover-engaged == 1
        autopilot/lock/autospeed-engaged == 1
        autopilot/lock/sas-engaged == 0
        gear/wow == 1
      </test>
    </switch>

    <switch name="autopilot/sas/pitch-ctrl">
      <default value="0.0"/>
      <test logic="AND" value="/orientation/pitch-rate-degps">
        autopilot/lock/sas-engaged == 1
        autopilot/lock/autopitch-engaged == 1
      </test>
    </switch>

    <pid name="autopilot/sas/pitch-autotrim-raw">
      <input>autopilot/sas/pitch-ctrl</input>
      <kp>/_tuning/pid/sas-pitch-kp</kp>
      <ki>/_tuning/pid/sas-pitch-ki</ki>
      <kd>/_tuning/pid/sas-pitch-kd</kd>
      <clipto>
        <min>/_tuning/trim/sas-pitch-min</min>
        <max>/_tuning/trim/sas-pitch-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/sas/pitch-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/sas/pitch-autotrim-raw">
        autopilot/lock/autopitch-engaged == 1
      </test>
    </switch>

    <!-- pitch-limit -->
    <fcs_function name="autopilot/sas/pitch-limit-autotrim">
      <function>
        <description>Sas-pitch-limit</description>
        <table>
          <independentVar>/orientation/pitch-deg</independentVar>
          <tableData>
            -20   -1
            -15   -.3
            -2    -.1
            0     0
            2     .1
            15    .3
            20    1
          </tableData>
        </table>
      </function>
      <clipto>
        <min>/_tuning/trim/sas-pitch-limit-min</min>
        <max>/_tuning/trim/sas-pitch-limit-max</max>
      </clipto>
    </fcs_function>

    <!-- auto-roll -->
    <switch name="autopilot/lock/autoroll-engaged">
      <default value="1"/>
      <test logic="AND" value="0">
        autopilot/lock/autopilot-engaged == 1
        autopilot/roll/channel-active == 1
      </test>
      <test logic="OR" value="0">
        autopilot/lock/autohover-engaged == 1
        autopilot/lock/sas-engaged == 0
      </test>
    </switch>

    <switch name="autopilot/sas/roll-ctrl">
      <default value="0.0"/>
      <test logic="AND" value="/orientation/roll-rate-degps">
        autopilot/lock/autoroll-engaged == 1
      </test>
    </switch>

    <pid name="autopilot/sas/roll-autotrim-raw">
      <input>autopilot/sas/roll-ctrl</input>
      <kp>/_tuning/pid/sas-roll-kp</kp>
      <ki>/_tuning/pid/sas-roll-ki</ki>
      <kd>/_tuning/pid/sas-roll-kd</kd>
      <clipto>
        <min>/_tuning/trim/sas-roll-min</min>
        <max>/_tuning/trim/sas-roll-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/sas/roll-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/sas/roll-autotrim-raw">
        autopilot/lock/autoroll-engaged == 1
      </test>
    </switch>

    <!-- auto-yaw -->
    <switch name="autopilot/lock/autoyaw-engaged">
      <default value="1"/>
      <test logic="AND" value="0">
        autopilot/lock/autopilot-engaged == 1
        autopilot/roll/channel-active == 1
      </test>
      <test logic="OR" value="0">
        autopilot/lock/autopilot-lock-heading == 1
        autopilot/lock/sas-engaged == 0
      </test>
    </switch>

    <switch name="autopilot/sas/yaw-ctrl">
      <default value="0.0"/>
      <test logic="AND" value="/orientation/yaw-rate-degps">
        autopilot/lock/autoyaw-engaged == 1
      </test>
    </switch>

    <pid name="autopilot/sas/yaw-autotrim-raw">
      <input>autopilot/sas/yaw-ctrl</input>
      <kp>/_tuning/pid/sas-yaw-kp</kp>
      <ki>/_tuning/pid/sas-yaw-ki</ki>
      <kd>/_tuning/pid/sas-yaw-kd</kd>
      <clipto>
        <min>/_tuning/trim/sas-yaw-min</min>
        <max>/_tuning/trim/sas-yaw-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/sas/yaw-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/sas/yaw-autotrim-raw">
        autopilot/lock/autoyaw-engaged == 1
      </test>
    </switch>

  </channel>

  <!-- autospeed -->
  <channel name="Autospeed">

    <switch name="autopilot/settings/target-speed-kmh">
      <default value="0"/>
      <test logic="AND" value="/instrumentation/speed-indicator/speed-bug-kmh">
        autopilot/lock/autopilot-engaged == 1
        autopilot/lock/autospeed-engaged == 1
      </test>
    </switch>

    <lag_filter name="autopilot/settings/speed-target-filtered-kmh">
      <input>autopilot/settings/target-speed-kmh</input>
      <c1>0.2</c1><!-- 0.9 -->
    </lag_filter>

    <fcs_function>
      <function name="autopilot/autospeed/error-raw-kmh">
        <difference>
          <property>autopilot/settings/speed-target-filtered-kmh</property>
          <property>/velocities/uBody-kmh</property>
        </difference>
      </function>
    </fcs_function>

    <switch name="autopilot/autospeed/error-kmh">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/autospeed/error-raw-kmh">
        autopilot/lock/autopilot-engaged == 1
        autopilot/lock/autospeed-engaged == 1
      </test>
    </switch>

    <pid name="autopilot/autospeed/pitch-autotrim-raw">
      <input>autopilot/autospeed/error-kmh</input>
      <kp>/_tuning/pid/autopilot-speed-pitch-kp</kp>
      <ki>/_tuning/pid/autopilot-speed-pitch-ki</ki>
      <kd>/_tuning/pid/autopilot-speed-pitch-kd</kd>
      <clipto>
        <min>/_tuning/trim/autopilot-pitch-min</min>
        <max>/_tuning/trim/autopilot-pitch-max</max>
      </clipto>
    </pid>

    <switch name="autopilot/autospeed/pitch-autotrim">
      <default value="0.0"/>
      <test logic="AND" value="autopilot/autospeed/pitch-autotrim-raw">
        autopilot/lock/autopilot-engaged == 1
        autopilot/lock/autospeed-engaged == 1
      </test>
    </switch>

  </channel>

</system>
