%
%    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.8 $, $Date: 2001/11/05 13:19:04 $

global titleStr;
global vehicleName

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

set( h_yaw, 'Name', [vehicleName,' Yaw Control'], 'DeleteFcn', 'clear h_yaw' );
set(gcf,'position',position)

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 );
%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 (s)' )
ylabel( 'Rudder Control Activity (deg)' )

