%
%    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.6 $, $Date: 2000/09/01 22:25:21 $

global titleStr;
global vehicleName

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

set( h_yaw, 'Name', [vehicleName,' 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


if exist('heading_bias') == 1
  line(dataNav(:,1),...
       mod360((dataNav(:,heading_bias) + dataNav(:,mPsi))*180/pi),'color','c')
end

%plotptr = plot( dataNav(:,1), mod360(dataNav(:,mPsi)*180/pi), xx, yy );
plotptr = plot( dataNav(:,1), [unwrap(dataNav(:,mPsi))+2*pi]*180/pi, xx, yy );
%lxmax   = get(plotptr(1), xmax);
%set(plotptr, 'xlim', [0 lxmax]);

%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( ['Heading: Commanded-Green; Smoothed Command-Red; Measured-Blue: ' ...
	titleStr] );
xlabel( 'Elapsed time (s)' );
ylabel( 'Heading (deg)' );
%legend( 'Meas.', 'Cmded', 'Smoothed',3);
zoom on
grid on

subplot( 2, 1, 2 )
cla
plot( dataDC(:,1), 180/pi*dataDC(:,psiProp), 'Color', 'r' );
line( dataDC(:,1), 180/pi*dataDC(:,psiInt),  'Color', [0 .75 .75] );
line( dataDC(:,1), 180/pi*dataDC(:,psiRate), 'Color', [0 .5 0] );
line( dataDC(:,1), 180/pi*dataDC(:,cDeltaR), 'Color', 'b' );
zoom on
grid on
title( ['Rudder PID; Prop-Red; Deriv-Green; Rudder-Blue)'] );
%legend( 'Prop.', 'Int.', 'Deriv', 'Cmd. Rudder');
xlabel( 'Elapsed Time Sec.' )
ylabel( 'Rudder Control Activity Deg.' )

