Contents

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