<?xml version="1.0"?>
<!-- 
  POH 7-4:
  Oil pressure sends propeller towards HIGH RPM or unfeather position,
  nitrogen pressure sends propeller towards low RPM or feather position.
  ...
  Feathering takes place in approximately six seconds
  ...
  Feathering lock engages at 800 RPM

  POH 8-9: Nitrogen pressure 22psi at 70-100degF

  Propeller Owner's Manual 115N pg 6-36:
  one degree of blade pitch change results in incr/decr of approx. 
  140-150 RPM
  
-->
<system name="PropGovernor">

  <channel name="Left">

    <!-- compute the feather-lock -->
    <switch name="propulsion/engine[0]/prop-governor/max-blade-angle">
      <!-- default: don't change max-blade-angle -->
      <default value="propulsion/engine[0]/prop-governor/max-blade-angle"/>

      <!-- above 800rpm and rpm-lever in feather-position on not yet feathered -->
      <test logic="AND" value="80">
        propulsion/engine[0]/prop-governor/max-blade-angle le 50 
        fcs/advance-cmd-norm[0] lt 0.1
        propulsion/engine[0]/propeller-rpm ge 800
      </test>

      <!-- feathered and rpm-lever not in feathered position -->
      <test logic="AND" value="30">
        propulsion/engine[0]/prop-governor/max-blade-angle gt 50
        fcs/advance-cmd-norm[0] ge 0.1
      </test>

      <!-- provide an initial value for system start,
           otherwise max-blade-angle would be zero  -->
      <test logic="AND" value="30">
        propulsion/engine[0]/prop-governor/max-blade-angle lt 30 
      </test>
    </switch>

    <!-- compute the propeller underspeed (simulate the flyweights) -->
    <fcs_function name="propulsion/engine[0]/prop-governor/underspeed-rpm">
      <function>
        <difference>
          <sum>
            <product>
              <property>fcs/advance-cmd-norm[0]</property>
              <value>1875</value> <!-- max.rpm minus min.rpm -->
            </product>
            <value>700</value> <!-- min.rpm -->
          </sum>
          <property>propulsion/engine[0]/propeller-rpm</property>
        </difference>
      </function>
      <clipto>
        <min>0</min>
        <max>10</max>
      </clipto>
    </fcs_function>

    <!-- compute the oil/nitrogen pressure -->
    <fcs_function name="propulsion/engine[0]/prop-governor/pressure">
      <function>
        <difference>
          <product>
            <property>propulsion/engine[0]/oil-pressure-psi</property>
            <property>propulsion/engine[0]/prop-governor/underspeed-rpm</property>
            <value>1.0</value> <!-- 1/max-underspeed-rpm -->
          </product>
          <value>22.0</value> <!-- nitrogen pressure -->
        </difference>
      </function>
    </fcs_function>

    <!-- check if the blade-angle is at it's limiting peg to
         stop the integrator -->
    <switch name="propulsion/engine[0]/prop-governor/peg">
      <default value="0"/>

      <!-- negative (nitrogen) pressure drives to low RPM (high angle) -->
      <test logic="AND" value="1">
        propulsion/engine[0]/prop-governor/pressure le 0
        propulsion/engine[0]/blade-angle ge propulsion/engine[0]/prop-governor/max-blade-angle
      </test>

      <!-- positive (oil) pressure drives to high RPM (low angle) -->
      <test logic="AND" value="1">
        propulsion/engine[0]/prop-governor/pressure ge 0
        propulsion/engine[0]/blade-angle le 11 
      </test>
    </switch>

    <pid name="propulsion/engine[0]/prop-governor/target">
      <input>-propulsion/engine[0]/prop-governor/pressure</input>
      <!-- 6 seconds for 69deg travel at 22psi: ki=69/22/6 -->
      <ki>0.523</ki> 
      <trigger>propulsion/engine[0]/prop-governor/peg</trigger>
      <output>propulsion/engine[0]/blade-angle</output>
    </pid>

  </channel>

  <channel name="Right">

    <!-- compute the feather-lock -->
    <switch name="propulsion/engine[1]/prop-governor/max-blade-angle">
      <!-- default: don't change max-blade-angle -->
      <default value="propulsion/engine[1]/prop-governor/max-blade-angle"/>

      <!-- above 800rpm and rpm-lever in feather-position on not yet feathered -->
      <test logic="AND" value="80">
        propulsion/engine[1]/prop-governor/max-blade-angle le 50 
        fcs/advance-cmd-norm[1] lt 0.1
        propulsion/engine[1]/propeller-rpm ge 800
      </test>

      <!-- feathered and rpm-lever not in feathered position -->
      <test logic="AND" value="30">
        propulsion/engine[1]/prop-governor/max-blade-angle gt 50
        fcs/advance-cmd-norm[1] ge 0.1
      </test>

      <!-- provide an initial value for system start,
           otherwise max-blade-angle would be zero  -->
      <test logic="AND" value="30">
        propulsion/engine[1]/prop-governor/max-blade-angle lt 30 
      </test>
    </switch>

    <!-- compute the propeller underspeed (simulate the flyweights) -->
    <fcs_function name="propulsion/engine[1]/prop-governor/underspeed-rpm">
      <function>
        <difference>
          <sum>
            <product>
              <property>fcs/advance-cmd-norm[1]</property>
              <value>1875</value> <!-- max.rpm minus min.rpm -->
            </product>
            <value>700</value> <!-- min.rpm -->
          </sum>
          <property>propulsion/engine[1]/propeller-rpm</property>
        </difference>
      </function>
      <clipto>
        <min>0</min>
        <max>10</max>
      </clipto>
    </fcs_function>

    <!-- compute the oil/nitrogen pressure -->
    <fcs_function name="propulsion/engine[1]/prop-governor/pressure">
      <function>
        <difference>
          <product>
            <property>propulsion/engine[1]/oil-pressure-psi</property>
            <property>propulsion/engine[1]/prop-governor/underspeed-rpm</property>
            <value>0.1</value> <!-- 1/max-underspeed-rpm -->
          </product>
          <value>22.0</value> <!-- nitrogen pressure -->
        </difference>
      </function>
    </fcs_function>

    <!-- check if the blade-angle is at it's limiting peg to
         stop the integrator -->
    <switch name="propulsion/engine[1]/prop-governor/peg">
      <default value="0"/>

      <!-- negative (nitrogen) pressure drives to low RPM (high angle) -->
      <test logic="AND" value="1">
        propulsion/engine[1]/prop-governor/pressure le 0
        propulsion/engine[1]/blade-angle ge propulsion/engine[1]/prop-governor/max-blade-angle
      </test>

      <!-- positive (oil) pressure drives to high RPM (low angle) -->
      <test logic="AND" value="1">
        propulsion/engine[1]/prop-governor/pressure ge 0
        propulsion/engine[1]/blade-angle le 11 
      </test>
    </switch>

    <pid name="propulsion/engine[1]/prop-governor/target">
      <input>-propulsion/engine[1]/prop-governor/pressure</input>
      <!-- 6 seconds for 69deg travel at 22psi: ki=69/22/6 -->
      <ki>0.523</ki> 
      <trigger>propulsion/engine[1]/prop-governor/peg</trigger>
      <output>propulsion/engine[1]/blade-angle</output>
    </pid>

  </channel>
</system>
