V=100       %  constant trim speed, ft per second
g=32.2      %  accel of gravity
xIC=[1000 1000 -4*pi/4] %  intitial condition
firstwp=2   % next must always start out at a value greater then 1 (so that next-1 is a valid waypoint)
%xwp=[0 100 200 300 1100]; % North waypoints (ft)
%ywp=[0 100 200 300 1100]; % East wapyoints (ft)
xwp=[1100 900 800 700 -1000    -2500]
ywp=[1100 900 800 700 1500 -2500]
R=100; % Radius for determining how close you are to a waypoint
Kphi=-1/g    % Gain on Ccdot-Cdot
Kcdot=1/5    % Gain on Cc-C