%
%    HEADING - Comprehensive plots of yaw and yaw control
%
%    MIT SeaGrant
%
% Copyright (C) 1995-1997, Massachussetts Institute of Technology
% Sea Grant Underwater Vehicles Laboratory
% $Revision: 1.2 $, $Date: 2000/01/14 03:37:15 $

if exist( 'h_yaw' ) ~= 1
    h_yaw = figure;
    winpos( position );
else 
    figure( h_yaw ); 
end

set( h_yaw, 'Name', 'DorOdyssey Yaw Control', 'DeleteFcn', 'clear h_yaw' );

time = dataDC(:,1);

subplot( 2, 1, 1 )
cla

[xx,yy] = stairs( time, mod360(dataDC(:,cPsi)*180/pi) );

%ydat = 180/pi*dataNav(:,mPsi);
%ind = find( abs(diff(dataDC(:,mPsi))) > pi );
%if ~isempty(ind), ydat( ind ) = NaN; end

plot( dataNav(:,1), mod360(dataNav(:,mPsi)*180/pi), xx, yy );

%ydat = 180/pi*dataDC(:,tPsi);
%ind = find( abs(diff(data(:,tPsi))) > pi );
%if ~isempty(ind), ydat( ind ) = NaN; end

line( dataDC(:,1), mod360(dataDC(:,tPsi)*180/pi), 'Color', 'r' );

title( ['Commanded and Measured Heading:  ' run_name] );
xlabel( 'elapsed time (s)' );
ylabel( 'heading (deg)' );
legend( 'Meas.', 'Cmded', 'Smoothed',3);
zoom on
grid on

subplot( 2, 1, 2 )
cla
hctrl
