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

Beagle Pup Autopilot (Sperry SP3)

Copyright (c) 2015 Richard Senior

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

**************************************************************************
-->

<PropertyList>

  <!-- =============================================================== -->
  <!-- Servos                                                          -->
  <!-- =============================================================== -->

  <filter>
    <name>Aileron Servo</name>
    <feedback-if-disabled>true</feedback-if-disabled>
    <enable>
      <condition>
        <property>autopilot/locks/active</property>
      </condition>
    </enable>
    <input>autopilot/internal/servos/aileron</input>
    <output>controls/flight/aileron-trim</output>
    <type>noise-spike</type>
    <max-rate-of-change>0.2</max-rate-of-change>
    <min>-1.0</min>
    <max>1.0</max>
  </filter>

  <filter>
    <name>Elevator Servo</name>
    <feedback-if-disabled>true</feedback-if-disabled>
    <enable>
      <condition>
        <property>autopilot/locks/active</property>
      </condition>
    </enable>
    <input>autopilot/internal/servos/elevator</input>
    <output>controls/flight/elevator-trim</output>
    <type>noise-spike</type>
    <max-rate-of-change>0.2</max-rate-of-change>
    <min>-1.0</min>
    <max>1.0</max>
  </filter>

  <filter>
    <name>Rudder Servo</name>
    <feedback-if-disabled>true</feedback-if-disabled>
    <enable>
      <condition>
        <property>autopilot/locks/active</property>
      </condition>
    </enable>
    <input>autopilot/internal/servos/rudder</input>
    <output>controls/flight/rudder-trim</output>
    <type>noise-spike</type>
    <max-rate-of-change>0.2</max-rate-of-change>
    <min>-1.0</min>
    <max>1.0</max>
  </filter>

  <!-- =============================================================== -->
  <!-- Roll Axis                                                       -->
  <!-- =============================================================== -->

  <filter>
    <type>exponential</type>
    <filter-time>1.0</filter-time>
    <input>autopilot/sperry/turn-select</input>
    <output>autopilot/sperry/turn-select-filtered</output>
  </filter>

  <filter>
    <name>Turn Select</name>
    <type>gain</type>
    <enable>
      <condition>
        <not>
          <property>autopilot/locks/heading-hold</property>
        </not>
      </condition>
    </enable>
    <input>
      <expression>
        <sum>
          <property>autopilot/sperry/turn-select-filtered</property>
          <property>autopilot/sperry/roll-trim</property>
        </sum>
      </expression>
    </input>
    <output>autopilot/internal/target-roll-deg</output>
  </filter>

  <filter>
    <name>Target Heading Error</name>
    <type>gain</type>
    <gain>1.0</gain>
    <input>
      <property>autopilot/internal/target-heading-deg</property>
    </input>
    <reference>
      <property>instrumentation/heading-indicator/indicated-heading-deg</property>
    </reference>
    <output>autopilot/internal/target-heading-error-deg</output>
    <period>
      <min>-180</min>
      <max>180</max>
    </period>
  </filter>

  <filter>
    <name>Heading Hold</name>
    <enable>
      <condition>
        <property>autopilot/locks/heading-hold</property>
      </condition>
    </enable>
    <type>gain</type>
    <gain>2.0</gain>
    <input>autopilot/internal/target-heading-error-deg</input>
    <output>autopilot/internal/target-roll-deg</output>
  </filter>

  <filter>
    <type>exponential</type>
    <filter-time>1.0</filter-time>
    <input>autopilot/internal/target-roll-deg</input>
    <output>autopilot/internal/target-roll-deg-filtered</output>
  </filter>

  <pid-controller>
    <name>Roll Axis</name>
    <enable>
      <condition>
        <property>autopilot/locks/active</property>
      </condition>
    </enable>
    <input>instrumentation/attitude-indicator/internal-roll-deg</input>
    <reference>autopilot/internal/target-roll-deg-filtered</reference>
    <output>autopilot/internal/servos/aileron</output>
    <config>
      <Kp>0.02</Kp>
      <Ti>30.0</Ti>
      <Td>0.0001</Td>
      <u_min>-1.0</u_min>
      <u_max>1.0</u_max>
    </config>
  </pid-controller>

  <!-- =============================================================== -->
  <!-- Pitch Axis                                                      -->
  <!-- =============================================================== -->

  <filter>
    <name>Pitch Select</name>
    <type>gain</type>
    <gain>1.0</gain>
    <enable>
      <condition>
        <and>
          <property>autopilot/locks/active</property>
          <not>
            <property>autopilot/locks/altitude-hold</property>
          </not>
        </and>
      </condition>
    </enable>
    <input>
      <expression>
        <sum>
          <property>autopilot/sperry/pitch-select</property>
          <property>autopilot/sperry/elev-trim</property>
        </sum>
      </expression>
    </input>
    <output>autopilot/internal/target-pitch-deg</output>
  </filter>

  <pid-controller>
    <name>Altitude Hold</name>
    <enable>
      <condition>
        <and>
          <property>autopilot/locks/active</property>
          <property>autopilot/locks/altitude-hold</property>
        </and>
      </condition>
    </enable>
    <input>autopilot/internal/target-altitude-ft</input>
    <reference>instrumentation/altimeter/pressure-alt-ft</reference>
    <output>autopilot/internal/target-pitch-deg</output>
    <config>
      <Kp>-0.07</Kp>
      <Ti>20.0</Ti>
      <Td>0.0001</Td>
      <u_min>-6.0</u_min>
      <u_max>6.0</u_max>
    </config>
  </pid-controller>

  <filter>
    <type>exponential</type>
    <filter-time>2.0</filter-time>
    <input>autopilot/internal/target-pitch-deg</input>
    <output>autopilot/internal/target-pitch-deg-filtered</output>
  </filter>

  <pid-controller>
    <name>Pitch Axis</name>
    <enable>
      <condition>
        <property>autopilot/locks/active</property>
      </condition>
    </enable>
    <input>autopilot/internal/target-pitch-deg-filtered</input>
    <reference>instrumentation/attitude-indicator/indicated-pitch-deg</reference>
    <output>autopilot/internal/servos/elevator</output>
    <config>
      <Kp>0.025</Kp>
      <Ti>20.0</Ti>
      <Td>0.0002</Td>
      <u_min>-1.0</u_min>
      <u_max>1.0</u_max>
    </config>
  </pid-controller>

  <!-- =============================================================== -->
  <!-- Yaw Axis                                                        -->
  <!-- =============================================================== -->

  <pid-controller>
    <name>Yaw Axis</name>
    <enable>
      <condition>
        <and>
          <property>autopilot/locks/active</property>
          <not>
            <property>controls/flight/auto-coordination</property>
          </not>
        </and>
      </condition>
    </enable>
    <input>instrumentation/slip-skid-ball/indicated-slip-skid</input>
    <output>autopilot/internal/servos/rudder</output>
    <config>
      <Kp>0.12</Kp>
      <Ti>20.0</Ti>
      <Td>0.0001</Td>
      <u_min>-0.3</u_min>
      <u_max>0.3</u_max>
    </config>
  </pid-controller>

</PropertyList>
