% Script to set waypoints for MPX-5
figure(1)
clf
d=0
EWP=[0 0 24000+d 34000+d 60000+d 44000+d 0]   % Easterly coordinate of waypoints (ft)
NWP= [-1000 500 500 24000 0 -44000 0]  % Northerly coordinate of waypoints (ft)
HWP=[300 310 350 300 270 310 300]   % Altitude of waypoints (ft)
VWP= [60.0 64.0 72.0 65.0 60.0 66.6 63.5]  % Velocity of waypoints (ft/sec)
plot(EWP,NWP,'r+')
hold on
plot(EWP,NWP,'r--')
%axis([0 44000 -44000 44000])
axis square
hold off

%[sys,x0,str,ts] = aircraft7(t,x,u,flag,constant,xIC)
% flag=3 >>> sys=ACOutputs(t,x,u)
NA=xIC(10)     % North position of the aircraft, feet
EA=xIC(11)     % East position of the aircraft, feet
flag=3;yIC=aircraft8(t,xIC,uIC,flag,constant,xIC)
Vnorth=yIC(17)    % Northerly velocity in NED coordinate, ft/s
Veast=yIC(18)     % Easterly velocity in NED coordinate, ft/s
de=EWP(2)-EA;
dn=NWP(2)-NA;
num=de*Vnorth-dn*Veast;
den=dn*Vnorth+de*Veast;
ErrorPsiIC=atan2(num,den) % Required by guidance s-function initialization
%PsiC=atan2((EWP(2)-EA),(NWP(2)-NA));
%PsiGT=atan2(Veast,Vnorth);
%ErrorPsi2=PsiC-PsiGT
plot(EWP,NWP)
xlabel('East (ft)')
ylabel('North (ft)')
title('"MPX-5 desired trajectory')
axis square
figure
plot(EWP,VWP)
xlabel('East (ft)')
ylabel('Velocity(ft/s)')
title('"MPX-5 speed versus east position')
