## Contents

- Initialize Flags, Controls, Vectors
- Implementation of a Simple Ground Collision Avoidance System
- Implementation of a large-scale yaw to heading maneuver
- Time-Based Commands
- Basic "Smart" Controls
- Emforce G-limits and set maneuver states as needed.

    function [u_ref,t_maneuver] = getAutopilotCommands(t, x_f16, xequil,...
        uequil, flightLimits, ctrlLimits, autopilot, resetTrue)

    % Given the time, f16 state, and trim condition, returns desired Nz, ps,
    % Ny+r, and throttle settings.
    %
    %   Function Call:
    %       [u_ref] = getAutopilotCommands(t, x_f16, xequil, uequil, ...
    %           flightLimits, ctrlLimits, autopilot, resetTrue)
    %
    %   Inputs:
    %       t       - time of step (sec)
    %       x_f16   - State vector (f16 state) (16x1)
    %       xequil  - trim conditions (13x1)
    %       uequil  - equilibrium control (4x1)
    %       flightLimits - struct of flight limits
    %       ctrlLimits - struct of control limits
    %       autopilot - struct of autopilot settings:
    %           simpleGCAS              - "Roll & Pull" to level flight
    %           basicSpeedControl       - Proportional control on airspeed
    %           steadyLevelFlightHold   - PD control on pitch & roll
    %           levelTurnControl        - Pulls g's to stay level when banked
    %           turnToHeading          - Bank, G, unbank to heading
    %           timeTriggeredControl    - Program your own maneuvers f(t)
    %       resetTrue  - OPTIONAL: If 1, reset persistent variables
    %
    %   Outputs:
    %       u_ref(1)    = Down force, Nz_ref                    (g's)
    %       u_ref(2)    = Stability roll rate, ps_ref           (rad/sec)
    %       u_ref(3)    = Side force, Ny_r_ref                  (g's)
    %       u_ref(4)    = Throttle Setting                      (ft/sec)
    %       t_maneuver(1) =     Maneuver start time             (sec)
    %       t_maneuver(2) =     Maneuver end time               (sec)
    %       t_maneuver(3:n) =   Checkpoint time                 (sec)
    %
    %   x_f16 States:
    %       x_f16(1)  = air speed, VT                            (ft/s)
    %       x_f16(2)  = angle of attack, alpha                   (rad)
    %       x_f16(3)  = angle of sideslip, beta                  (rad)
    %       x_f16(4)  = roll angle, phi                          (rad)
    %       x_f16(5)  = pitch angle, theta                       (rad)
    %       x_f16(6)  = yaw angle, psi                           (rad)
    %       x_f16(7)  = roll rate, P                             (rad/s)
    %       x_f16(8)  = pitch rate, Q                            (rad/s)
    %       x_f16(9)  = yaw rate, R                              (rad/s)
    %       x_f16(10) = northward horizontal displacement, pn    (ft)
    %       x_f16(11) = eastward horizontal displacement, pe     (ft)
    %       x_f16(12) = altitude, h                              (ft)
    %       x_f16(13) = engine thrust dynamics lag state, pow    (lbs)
    %       ----------------------------------------------------------
    %       x_f16(14) = Integral of Nz error, Nz_e_i             (g's)
    %       x_f16(15) = Integral of Ps_error, Ps_e_i             (rad/sec)
    %       x_f16(16) = Integral of Ny_error, Ny_e_i             (g's)
    %
    %   Notes:
    %       This function is where you program the autopilot. Commanded
    %       Nz, ps, Ny+r, and throttle settings can be hard coded, time
    %       triggered, or state triggered.
    %
    %       Persistent variables are used to track any states that are not
    %       included in the F-16 state vector. These are needed for
    %       integration, tracking time flags, maneuver states, etc. If you
    %       modify this function to add your own, make sure to declare the
    %       persistent variables and included them in the "reset" block.
    %
    % <a href="https://github.com/pheidlauf/AeroBenchVV">AeroBenchVV</a>
    % Copyright: GNU General Public License 2017
    %
    % See also: GETDEFAULTSETTINGS, CONTROLLEDF16, PERSISTENT

## Initialize Flags, Controls, Vectors

Run top level function if called without arguments

    if(nargin==0)
        Main;
        return;
    end

    % If "resetTrue" not designated, set to false
    if(nargin < 8)
        resetTrue = 0;
    end

    % Declare persistent flags and variables
    persistent man_start man_complete man_check_1 man_check_2;
    persistent turnDirection;

    % Reset persistent flags if (re)starting simulation
    if(isempty(man_start) || t <= 0 || resetTrue)
        % Reset maneuver time flags if first func call or needed.
        man_start = -1;
        man_check_1 = -1;
        man_check_2 = -1;
        man_complete = -1;
        turnDirection = 0;
    end

    % Zero default commands
    Nz = 0;
    ps = 0;
    Ny_r = 0;
    throttle = 0;

    % Initialize t_maneuver vector to zeros
    t_maneuver = zeros(1,2);

    flightLimits =

          altitudeMin: 0
          altitudeMax: 10000
         maneuverTime: 15
                NzMax: 9
                NzMin: -2
        psMaxAccelDeg: 500
                 vMin: 300
                 vMax: 900
          alphaMinDeg: -10
          alphaMaxDeg: 45
           betaMaxDeg: 30

    ctrlLimits =

           ThrottleMax: 1
           ThrottleMin: 0
        ElevatorMaxDeg: 25
        ElevatorMinDeg: -25
         AileronMaxDeg: 21.500000000000000
         AileronMinDeg: -21.500000000000000
          RudderMaxDeg: 30
          RudderMinDeg: -30
            MaxBankDeg: 60
                 NzMax: 6
                 NzMin: -1

    autopilot =

                        title: 'Default Simulation'
            basicSpeedControl: 1
        steadyLevelFlightHold: 0
             levelTurnControl: 0
                   simpleGCAS: 0
                turnToHeading: 0
         timeTriggeredControl: 0

    ------------------------------------------------------------
    F-16 Decoupled LQR Controller for Nz, P_s, and Ny+r tracking
    ------------------------------------------------------------

    Operator Inputs =
                            Vt            h        gamma       psidot     thetadot
                     540.00000   3500.00000            0            0            0

    Trim Orientation Selected:   Steady Pull Up

    Inititial Conditions =
                            Vt        alpha         beta          phi        theta
                     540.00000      0.03703            0      0.78540     -1.25664

                           psi            p            q            r           pn
                      -0.78540            0            0            0            0

                            pe          alt          pow
                             0   3500.00000      9.00000

    Control Guess =
                      throttle     elevator      aileron       rudder
                       0.20000            0            0            0

    ------------------------------------------------------------
    Equilibrium / Trim Conditions

    State Equilibrium =
                            Vt        alpha         beta          phi        theta
                     540.00000      0.03441            0            0      0.03441

                           psi            p            q            r           pn
                             0            0            0            0            0

                            pe          alt          pow
                             0   3500.00000     10.35403

    Control Equilibrium =
                      throttle     elevator      aileron       rudder
                       0.15944     -0.77048            0            0

    ------------------------------------------------------------
    Running jacobFun.m
    Linearized F-16 SS Model

    lin_f16 =

      A =
                      Vt       alpha        beta         phi       theta
       Vt       -0.01658        10.6  -8.377e-06  -2.766e-06      -32.17
       alpha  -0.0002195     -0.9855   3.055e-06  -1.488e-06  -1.025e-05
       beta            0           0     -0.3124     0.05954           0
       phi             0           0           0           0           0
       theta           0           0           0           0           0
       psi             0           0           0           0           0
       p               0           0       -31.6           0           0
       q       6.662e-13       0.858           0           0           0
       r               0           0       8.904           0           0
       pn              1  -6.607e-06           0  -1.592e-06  -6.607e-06
       pe              0           0         540      -18.58           0
       alt    -6.579e-16        -540           0    4.64e-06         540
       pow             0           0           0           0           0

                     psi           p           q           r          pn
       Vt              0           0     -0.4842           0           0
       alpha           0           0      0.9146           0           0
       beta            0     0.03387           0     -0.9925           0
       phi             0           1           0     0.03443           0
       theta           0           0           1           0           0
       psi             0           0           0       1.001           0
       p               0      -3.574   0.0002627      0.6331           0
       q               0  -8.795e-06      -1.045   -0.002858           0
       r               0    -0.02249    0.002539     -0.4621           0
       pn              0           0           0           0           0
       pe            540           0           0           0           0
       alt             0           0           0           0           0
       pow             0           0           0           0           0

                      pe         alt         pow
       Vt              0    3.81e-05      0.3717
       alpha           0   1.774e-06   -2.37e-05
       beta            0           0           0
       phi             0           0           0
       theta           0           0           0
       psi             0           0           0
       p               0           0           0
       q               0  -5.338e-15           0
       r               0           0           0
       pn              0           0           0
       pe              0           0           0
       alt             0           0           0
       pow             0           0          -1

      B =
               Throttle   Elevator    Aileron     Rudder
       Vt             0     0.1848          0          0
       alpha          0  -0.002085          0          0
       beta           0          0  0.0002863  0.0007816
       phi            0          0          0          0
       theta          0          0          0          0
       psi            0          0          0          0
       p              0          0     -0.765     0.1376
       q              0    -0.1833          0          0
       r              0          0   -0.03332   -0.06472
       pn             0          0          0          0
       pe             0          0          0          0
       alt            0          0          0          0
       pow        64.94          0          0          0

      C =
                      Vt       alpha        beta         phi       theta
       Az       0.003698        16.9  -5.296e-08           0           0
       q               0           0           0           0           0
       alpha           0           1           0           0           0
       theta           0           0           0           0           1
       Vt              1           0           0           0           0
       Ay              0           0       -5.24           0           0
       p               0           0           0           0           0
       r               0           0           0           0           0
       beta            0           0           1           0           0
       phi             0           0           0           1           0

                     psi           p           q           r          pn
       Az              0    -4.1e-08      0.9454   -0.001336           0
       q               0           0           1           0           0
       alpha           0           0           0           0           0
       theta           0           0           0           0           0
       Vt              0           0           0           0           0
       Ay              0   -0.008953           0      0.1154           0
       p               0           1           0           0           0
       r               0           0           0           1           0
       beta            0           0           0           0           0
       phi             0           0           0           0           0

                      pe         alt         pow
       Az              0  -2.979e-05           0
       q               0           0           0
       alpha           0           0           0
       theta           0           0           0
       Vt              0           0           0
       Ay              0           0           0
       p               0           0           0
       r               0           0           0
       beta            0           0           0
       phi             0           0           0

      D =
              Throttle  Elevator   Aileron    Rudder
       Az            0  -0.05062         0         0
       q             0         0         0         0
       alpha         0         0         0         0
       theta         0         0         0         0
       Vt            0         0         0         0
       Ay            0         0  0.004801   0.01311
       p             0         0         0         0
       r             0         0         0         0
       beta          0         0         0         0
       phi           0         0         0         0

    Name: Linearized F-16 SS Model
    Continuous-time state-space model.

    Decoupled LQR Controller Gains =
                         alpha            q         Nz_i         beta            p
         elevator   -156.88015    -31.03701    -38.72983            0            0
          aileron            0            0            0     38.02751     -5.65497
           rudder            0            0            0     17.56400      1.58391

                             r         ps_i       Ny_r_i
         elevator            0            0            0
          aileron    -14.08804    -34.06416     -9.95406
           rudder    -41.43509      6.29550    -53.86016

    ------------------------------------------------------------
    Running Nonlinear Simulation in ODE45

## Implementation of a Simple Ground Collision Avoidance System

Concept: Roll until wings level (in the shortest direction) When abs(roll rate) < threshold, pull X g's until pitch angle > X deg

    if(autopilot.simpleGCAS)
        % Choose threshold values:
        eps_phi = deg2rad(5);   % Max roll angle magnitude before pulling g's
        eps_p = deg2rad(1);     % Max roll rate magnitude before pulling g's
        path_goal = deg2rad(0); % Final desired path angle
        Nz_des = 5;             % Desired maneuver g's

        % Pull out important variables for ease of use
        phi = x_f16(4);             % Roll angle    (rad)
        p = x_f16(7);               % Roll rate     (rad/sec)
        theta = x_f16(5);           % Pitch angle   (rad)
        alpha = x_f16(2);           % AoA           (rad)
        % Note: pathAngle = theta - alpha

        % Set Proportional-Derivative Control Gains
    	K_prop = 4;
        K_der = K_prop*0.3;

        % Maneuver starts at t == 0 (boring)
        man_start = 2;

        if(t<man_start)
            % Do nothing
        elseif(man_check_1 < 0)
            % Determine which angle is "level" (0, 180, 360, 720, etc)
            radsFromWingsLevel = round(phi/pi);
            % Until wings are "level" & roll rate is small
            if(abs(phi-pi*radsFromWingsLevel) < eps_phi && abs(p) < eps_p)
                man_check_1 = t;
            else
                % PD Control until phi == pi*radsFromWingsLevel
                ps = -(phi - pi*radsFromWingsLevel)*K_prop - p*K_der;
            end
        elseif(man_complete < 0)
            radsFromNoseLevel = round((theta-alpha)/(2*pi));
            if((theta-alpha) - 2*pi*radsFromNoseLevel > path_goal)
                man_complete = t;
            else
                Nz = Nz_des;
            end
        else
            % What to do once recovery is complete
            Nz = 0;
            ps = 0;
            autopilot.steadyLevelFlightHold = true;
        end

        % Set t_maneuver states
        t_maneuver(1) = man_start;
        t_maneuver(2) = man_complete;
        t_maneuver(3) = man_check_1;
    end

## Implementation of a large-scale yaw to heading maneuver

Note: This is not a heading tracker.

    if(autopilot.turnToHeading)
        % Enable speed control
        autopilot.basicSpeedControl       = true;
        % Choose threshold values:
        eps_psi = deg2rad(5);       % Max yaw angle at completion
        eps_r = deg2rad(0.1);       % Max yaw rate magnitude at completion
        eps_phi = deg2rad(1);       % Max roll angle at completion
        eps_p = deg2rad(0.1);       % Max roll rate at completion

        psi_goal = deg2rad(0);      % Final desired heading

        % Pull out important variables for ease of use
        alpha = x_f16(2);           % AoA           (rad)
        phi = x_f16(4);             % Roll angle    (rad)
        p = x_f16(7);               % Roll rate     (rad/sec)
        psi = x_f16(6);             % Heading angle (rad) zero is north
        r = x_f16(9);               % Heading rate  (rad/s)

        % Maneuver starts at t == 0 (boring)
        man_start = 2;
        maxBankAngle = deg2rad(ctrlLimits.MaxBankDeg);
        autopilot.levelTurnControl = true;

        % Set a flag to determine the direction of the turn
        if(turnDirection==0)
            turnDirection = sign(psi_goal - psi);
            % Negative =
        end

        if(t<man_start)
            % Do nothing
            autopilot.steadyLevelFlightHold = true;
        elseif(man_check_1 < 0)
            autopilot.steadyLevelFlightHold = false;
            % phi_goal has not been passed yet
            if(sign(psi_goal - psi) == turnDirection )
                % Set desired turn bank angle
                phi_goal = maxBankAngle*turnDirection;
                % Set Proportional-Derivative Control Gains
                K_prop = 4;
                K_der = K_prop*0.5;
                % PD control for roll rate
                ps = (phi_goal-phi)*K_prop - p*K_der;
            else
                man_check_1 = t;
            end
        elseif(man_complete < 0)
            % Wings are not level yet
            if(abs(phi) > eps_phi || abs(p) > eps_p)
                K_prop = 4;
                K_der = K_prop*0.5;
                % PD control for roll rate
                ps = -phi*K_prop - p*K_der;
            else
                man_complete = t;
            end
        else
            % What to do once recovery is complete
            autopilot.steadyLevelFlightHold = true;
        end

        t_maneuver(1) = man_start;
        t_maneuver(2) = man_complete;
        t_maneuver(3) = man_check_1;
    end

## Time-Based Commands

Note: This is just a sample of time-scheduled commands

    if(autopilot.timeTriggeredControl)
        maxBank = 60; % degrees
        if(t<5)
            ps = deg2rad(maxBank/5);
        elseif(t<10)
            ps = deg2rad(-maxBank/5);
        end
    end

## Basic "Smart" Controls

    % Proportional control on airspeed
    if(autopilot.basicSpeedControl)
        K_vt = 0.25;
        throttle = -K_vt*(x_f16(1) - xequil(1));
    end

    % Parallel PD control on pitch and roll modes to stay level
    if(autopilot.steadyLevelFlightHold)
        % NOTE: This is not a recovery maneuver

        % Pull out important variables for ease of use
        phi = x_f16(4);             % Roll angle    (rad)
        p = x_f16(7);               % Roll rate     (rad/sec)
        theta = x_f16(5);           % Pitch angle   (rad)
        q = x_f16(8);               % Pitch rate    (rad/sec)
        alpha = x_f16(2);           % AoA           (rad)

        % Set Proportional-Derivative control gains for roll
    	K_prop = 1;
        K_der = K_prop*0.3;

        % Determine which angle is "level" (0, 180, 360, 720, etc)
        radsFromWingsLevel = round(phi/pi);
        % PD Control on phi using roll rate
        ps = -(phi-pi*radsFromWingsLevel)*K_prop - p*K_der;

        % Set Proportional-Derivative control gains for pitch
    	K_prop2 = 2;
        K_der2 = K_prop*0.3;

        % Determine "which" angle is level (0, 360, 720, etc)
        radsFromNoseLevel = round((theta-alpha)/pi);
        % PD Control on theta using Nz
        Nz = -(theta - alpha - pi*radsFromNoseLevel)*K_prop2 - p*K_der2;

        % Note: Could implement PD control on psi using Ny_r (for small errors)
        % Set Proportional-Derivative control gains for pitch
        % 	K_prop3 = 5;
        %     K_der3 = K_prop*0.3;
        % PD Control on psi using Ny_r
        % Ny_r = (psi_goal-psi)*K_prop3 - r*K_der3;
    end

    % Pull g's to maintain altitude during bank based on geometry/trig
    if(autopilot.levelTurnControl)
        % Calculate theta
        phi = x_f16(4);

        % Note: This calculation only works for {-pi/2 < phi < pi/2}
        if(phi <= pi/2 && phi > -pi/2)
            Nz = 1/cos(phi) - 1; % Keeps plane at altitude
        else
            Nz = ctrlLimits.NzMax;
            fprintf('At time = %d, phi = %d deg\n',t,rad2deg(phi));
            warning('Singularity at pi/2 < theta < -pi/2. Nz_ref = 0');
        end
    end

## Emforce G-limits and set maneuver states as needed.

    if(Nz > ctrlLimits.NzMax)
        fprintf('Time = %d\n',t);
        fprintf('Nz   = %d g''s\n',Nz);
        fprintf('phi  = %d deg\n',rad2deg(x_f16(4)));
        Nz = ctrlLimits.NzMax;
        warning('Nz_ref exceeds NzMax. Constraining...');
        fprintf('Nz_trimmed   = %d g''s\n',Nz);
        disp('Note: ODE45 may re-calculate this time step');
    end

    if(Nz < ctrlLimits.NzMin)
        fprintf('Time = %d\n',t);
        fprintf('Nz   = %d g''s\n',Nz);
        fprintf('phi  = %d deg\n',rad2deg(x_f16(4)));
        Nz = ctrlLimits.NzMin;
        warning('Nz_ref below NzMin. Constraining...');
        fprintf('Nz_trimmed   = %d g''s\n',Nz);
        disp('Note: ODE45 may re-calculate this time step');
    end

    % Set t_maneuver states
    t_maneuver(1) = man_start;
    t_maneuver(2) = man_complete;

    % Create reference vector
    u_ref = [Nz; ps; Ny_r; throttle];

    end

    Simulation Complete
    Time to simulate (MM:SS.ms):
    00:35.807

    Begin back-calculation of controls
    Back-Calculation of Controls Complete
    Time to calculate (MM:SS.ms):
    01:23.684

    Begin struct generation & pass/fail analysis
    Pass/Fail Analysis Complete
    Time to calculate (MM:SS.ms):
    00:00.037

    ----------------------------------------------------------
    RESULTS:
                  Min & Max Values:
    Min Altitude:             96.178   ft
    Altitude Lost:          3403.822   ft
    Min Airspeed:            540.000   ft/sec
    Max Airspeed:            639.104   ft/sec
    Max Down Force:            5.222   g's
    Min Down Force:           -0.426   g's
    Max Side Force:            0.061   g's
    Max Roll Accel:          233.522   deg/sec^2

                  Maneuver Event Times:
    Maneuver Start:            2.000   sec
    Checkpoint  1:             3.395   sec
    Maneuver Complete:         8.000   sec
    Final Time:               15.000   sec
    (if Checkpoint 2 < Checkpoint 1, it is due to ODE45
    stepping backwards in time)

                  F-16 Attitude at Start:
    Roll   (phi):             45.000   deg
    Pitch  (theta):          -72.000   deg
    Yaw    (psi):            -45.000   deg

                  F-16 Attitude at End:
    Roll   (phi):             -0.000   deg
    Pitch  (theta):            2.249   deg
    Yaw    (psi):            -33.514   deg

                  Pass Fail Conditions:
               stable: 1
             airspeed: 1
                alpha: 1
                 beta: 1
                   Nz: 1
        psMaxAccelDeg: 1
             altitude: 1
         maneuverTime: 1

    ----------------------------------------------------------
    Begin plotting
    Plot Generation Complete
    Time to calculate (MM:SS.ms):
    00:01.872