<?xml version="1.0"?>
<!--
        Description: F14 CADC (AFCS, MANEUVERING FLAPS/SLATS)  
        Author: Richard Harrison (rjh@zaretto.com)
        References: F14-AAD-1, NASA TT F15,406 Improvement of manuevrability at low speed, W. Staudacher, 1972
-->


<system name="CADC">

    <property value="0.006"> systems/afcs/altitude-hold-pid-kp </property>
    <property value="0.0000028"> systems/afcs/altitude-hold-pid-ki </property>
    <property value="0.0025"> systems/afcs/altitude-hold-pid-kd </property>
    <property value="1.2"> systems/afcs/altitude-hold-elevator-gain </property>

    <property value="2"> systems/afcs/altitude-hold-lag-constant </property>
    <property value="0">systems/afcs/target-altitude-ft</property>
    <property value="0">systems/afcs/altitude-hold-active</property>
    <property value="0">systems/cadc/manuv-slat-extension-manual</property>
    <property value="0.9">systems/cadc/manuv-flap-extension-calc-lag</property>
    <property value="1.3">systems/cadc/manuv-flap-extension-washout-lag</property>

    <property value="0">systems/afcs/heading-hold-active</property>
    <property value="0">systems/afcs/SAS-roll-demand</property>

    <channel name="MANOUEVRE">

        <fcs_function name="systems/cadc/manuv-flap-fully-extended-max-mn">
            <function>
                <description>Manoeuver slat/flap ful extension max alt/mn</description>
                <!-- NAVAIR 01-F14AAD-1 Figures 2-51,2-52: p. 2-93  -->
                <sum>
                    <table>
                        <independentVar lookup="row">position/h-sl-ft</independentVar>
                        <tableData>
                            0	0.5105
                            5000	0.5610
                            10000	0.6206
                            15000	0.6788
                            20000	0.7491
                            23000	0.8011
                            28500	0.8516
                            35000	0.8595
                            40000	0.8643
                        </tableData>
                    </table>
                </sum>
            </function>
        </fcs_function>

        <fcs_function name="systems/cadc/manuv-flap-partially-extended-max-mn">
            <function>
                <description>Manoeuver flap partial extension max alt/mn</description>
                <!-- NAVAIR 01-F14AAD-1 Figures 2-51,2-52: p. 2-93  -->
                <sum>
                    <table>
                        <independentVar lookup="row">position/h-sl-ft</independentVar>
                        <tableData>
                            0	0.5806
                            5000	0.6296
                            10000	0.6984
                            15000	0.7703
                            20000	0.8299
                            23000	0.8591
                            28500	0.8654
                            35000	0.8702
                            40000	0.8765
                        </tableData>
                    </table>
                </sum>
            </function>
        </fcs_function>

        <fcs_function name="systems/cadc/manuv-flapslat-extension-aoa">
            <function>
                <description>Manoeuver flapslat extension schedule, NAVAIR 01-F14AAD-1 Figures 2-51,2-52: p. 2-93 </description>
                <sum>
                    <table>
                        <independentVar lookup="row">velocities/mach</independentVar>
                        <tableData>
                            0.0000	10.45
                            0.3952	10.45
                            0.3962	8.12
                            0.4521	7.91
                            0.4987	7.63
                            0.5576	7.15
                            0.6042	6.69
                            0.6537	6.08
                            0.7034	5.65
                            0.7593	5.44
                            0.8060	5.48
                            0.8558	5.49
                        </tableData>
                    </table>
                </sum>
            </function>
        </fcs_function>

        <fcs_function name="systems/cadc/manuv-flapslat-retraction-aoa">
            <function>
                <description>Manoeuver flapslat retraction schedule</description>
                <!-- NAVAIR 01-F14AAD-1 Figures 2-52: p. 2-93  -->
                <sum>
                    <table>
                        <independentVar lookup="row">velocities/mach</independentVar>
                        <tableData>
                            0.0000	7.62
                            0.38	7.62
                            0.3982	5.30
                            0.4541	5.12
                            0.5007	4.81
                            0.5534	4.27
                            0.6061	3.71
                            0.6557	3.13
                            0.7053	2.74
                            0.7550	2.47
                            0.8079	2.37
                            0.8672	2.73
                        </tableData>
                    </table>
                </sum>
            </function>
        </fcs_function>


        <fcs_function name="systems/cadc/manuv-flap-extension-calc">
            <function>
                <description>NAVAIR 01-F14AAD-1 S 2.21.2.4 p. 2-92 </description>
                <product>
                    <table>
                        <independentVar lookup="row">aero/alpha-deg</independentVar>
                        <independentVar lookup="column">velocities/mach</independentVar>
                        <tableData>
                                0.00000	0.50000	0.60000	0.70000	0.80000	0.90000	1.00000
                            0	0.00000	0.00000	0.00000	0.00000	0.00000	0.00000	0.00000
                            5	0.00000	0.00000	0.00000	0.00000	0.00000	0.00000	0.00000
                            6	0.07000	0.07000	0.07000	0.07000	0.07000	0.00000	0.00000
                            20	0.28571	0.28571	0.18571	0.18571	0.18571	0.00000	0.00000
                            35	0.28571	0.28571	0.18571	0.18571	0.18571	0.00000	0.00000
                            40	0.28571	0.28571	0.18571	0.18571	0.18571	0.00000	0.00000
                            45	0.28571	0.28571	0.18571	0.18571	0.18571	0.00000	0.00000
                            50	0.28571	0.28571	0.18571	0.18571	0.18571	0.00000	0.00000
                            55	0.28571	0.28571	0.18571	0.18571	0.18571	0.00000	0.00000
                        </tableData>
                    </table>
                </product>
            </function>
        </fcs_function>

        <fcs_function name="systems/cadc/manuv-slat-extension-calc">
            <function>
                <description>NAVAIR 01-F14AAD-1 S 2.21.2.4 p. 2-92 </description>
                <product>
                    <table>
                        <independentVar lookup="row">aero/alpha-deg</independentVar>
                        <independentVar lookup="column">velocities/mach</independentVar>
                        <tableData>
                                0.000000	0.45         0.500000	0.800000	0.900000	1.000000
                            0	0.000000	0.000000    0.000000	0.000000	0.000000	0.000000
                            8	0.000000   	0.000000    0.076471	0.076471	0.000000	0.000000
                            10	0.105294	0.105294    0.135294	0.135294	0.000000	0.000000
                            20	0.429412	0.429412    0.429412	0.429412	0.000000	0.000000
                            25	0.429412	0.429412    0.429412	0.429412	0.000000	0.000000
                            55	0.429412	0.429412    0.429412	0.429412	0.000000	0.000000
                        </tableData>
                    </table>
                </product>
            </function>
        </fcs_function>

        <!-- ensure within operating envelope -->

        <switch name="systems/cadc/manuv-active">
            <default value="0.0"/>

            <test value="0" logic="OR">
                    fcs/flap-cmd-norm gt 0.3714285714285714
                    aero/alpha-deg lt systems/cadc/manuv-flapslat-retraction-aoa
                    velocities/mach gt systems/cadc/manuv-flap-partially-extended-max-mn
                    gear/gear-cmd-norm ne 0
            </test>


            <test value="1" logic="OR">
                    <and>
                        fcs/flap-cmd-norm le 0.3714285714285714
                        aero/alpha-deg gt systems/cadc/manuv-flapslat-extension-aoa
                        velocities/mach le systems/cadc/manuv-flap-fully-extended-max-mn
                        gear/gear-cmd-norm eq 0
                        fcs/wing-sweep-cmd le 0.7352941176470588
                    </and>
                    systems/cadc/manuv-active ne 0
            </test>

            <!--
            <test value="0.6">
                <or>
                    <and>
                    aero/alpha-deg gt systems/cadc/manuv-flapslat-extension-aoa
                    velocities/mach le systems/cadc/manuv-flap-partially-extended-max-mn
                    </and>
                    systems/cadc/manuv-active ne 0
                </or>
                aero/alpha-deg gt systems/cadc/manuv-flapslat-extension-aoa
                systems/cadc/manuv-active ne 0
            </test>
-->

        </switch>

        <switch name="systems/cadc/manuv-flap-extension-cmd">
            <default value="0"/>
            <test value="systems/cadc/manuv-flap-extension-calc">
                systems/cadc/manuv-active ne 0
                systems/cadc/manuv-flap-extension-calc ge systems/cadc/manuv-flap-extension-cmd
            </test>
            <test value="systems/cadc/manuv-flap-extension-cmd">
                systems/cadc/manuv-active ne 0
            </test>

        </switch>

        <switch name="systems/cadc/manuv-slat-extension-cmd">
            <default value="0"/>
            <test value="systems/cadc/manuv-slat-extension-calc">
                systems/cadc/manuv-active ne 0
                fcs/wing-sweep-cmd le 0.7352941176470588
            </test>
            <test value="systems/cadc/manuv-slat-extension-manual">
                systems/cadc/manuv-active eq 0
                fcs/wing-sweep-cmd le 0.7352941176470588
            </test>
        </switch>


    </channel>

    <channel name="AFCS">
        <!-- ensure within operating envelope -->

        <switch name="systems/afcs/altitude-hold-trigger">
            <default value="0.0"/>
            <test value="1.0">
                <and>
                    propulsion/engine[0]/set-running == 0
                    propulsion/engine[1]/set-running == 0
                    systems/afcs/altitude-hold-active ne 0
                </and>
            </test>
        </switch>

        <summer name="systems/afcs/altitude-hold-divergence">
            <input> systems/afcs/target-altitude-ft </input>
            <input> -position/h-sl-ft </input>
        </summer>


        <switch name="systems/afcs/altitude-hold-alt-diff-delta">
            <default value="0.0"/>

            <test value="0">
                systems/afcs/altitude-hold-divergence lt 20
                systems/afcs/altitude-hold-divergence gt -20
            </test>

            <test value="-systems/afcs/altitude-hold-lag-constant">
                systems/afcs/altitude-hold-divergence gt 10
                systems/afcs/altitude-hold-divergence lt systems/afcs/altitude-hold-alt-diff-feet
            </test>

            <test value="systems/afcs/altitude-hold-lag-constant">
                systems/afcs/altitude-hold-divergence lt -10
                systems/afcs/altitude-hold-divergence gt systems/afcs/altitude-hold-alt-diff-feet
            </test>

            <test value="-systems/afcs/altitude-hold-lag-constant">
                systems/afcs/altitude-hold-divergence lt -4
                systems/afcs/altitude-hold-alt-diff-feet ge -2000
            </test>

            <test value="systems/afcs/altitude-hold-lag-constant">
                systems/afcs/altitude-hold-divergence gt 4
                systems/afcs/altitude-hold-alt-diff-feet le 2000
            </test>


        </switch>

        <summer name="systems/afcs/altitude-hold-alt-diff-feet1">
            <input>systems/afcs/altitude-hold-alt-diff-feet</input>
            <input>systems/afcs/altitude-hold-alt-diff-delta</input>
            <output>systems/afcs/altitude-hold-alt-diff-feet</output>
        </summer>

        <pure_gain name="systems/afcs/altitude-hold-vsi-dmd-fps">
            <input> systems/afcs/altitude-hold-alt-diff-feet </input>
            <gain>0.0166666666666667</gain>
            <!-- from fpm to fps -->
        </pure_gain>

        <summer name="systems/afcs/altitude-hold-vsi">
            <input> systems/afcs/altitude-hold-vsi-dmd-fps </input>
            <input> -velocities/h-dot-fps  </input>
        </summer>

        <switch name="systems/afcs/altitude-hold-dmd-vsi">
            <default value="0.0"/>
            <test value="systems/afcs/altitude-hold-vsi">
                systems/afcs/altitude-hold-active == 1
            </test>
        </switch>

        <pid name="systems/afcs/altitude-hold-divergence-pid">
            <input>systems/afcs/altitude-hold-dmd-vsi</input>
            <kp> systems/afcs/altitude-hold-pid-kp </kp>
            <ki> systems/afcs/altitude-hold-pid-ki </ki>
            <kd> systems/afcs/altitude-hold-pid-kd </kd>
            <trigger> systems/afcs/altitude-hold-trigger </trigger>
        </pid>

<!--
        <pure_gain name="systems/afcs/altitude-hold-divergence-pid-contents">
            <input> systems/afcs/altitude-hold-divergence-pid </input>
            <gain> 1.0 </gain>
            <output>systems/afcs/altitude-hold-divergence-pid-contents</output>
        </pure_gain>
-->
        <pure_gain name="systems/afcs/elevator-cmd-out">
            <input> systems/afcs/altitude-hold-divergence-pid </input>
            <gain> -systems/afcs/altitude-hold-elevator-gain </gain>
            <clipto>
                <min> -0.4</min>
                <max> 0.4</max>
            </clipto>
            <output>systems/afcs/elevator-cmd-delta</output>
        </pure_gain>

    </channel>
</system>