%    HEADING - Comprehensive plots of yaw and yaw control
%
%    B. A. Moran, 1995-1997

% Copyright (C) 1995-1997, Massachussetts Institute of Technology
% Sea Grant Underwater Vehicles Laboratory
% $Revision: 1.2 $, $Date: 1998/05/15 15:01:41 $

if exist( 'h_yaw' ) ~= 1
    h_yaw = figure;
    if strcmp( computer, 'SGI' )
	winpos( [100 + 100*rand 100 + 100*rand 1024 768] );
    else
	winpos( position(2,:) );
    end
else 
    h_yaw = figure( h_yaw ); 
end

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

subplot( 4, 1, 3 )
cla
[xx,yy] = stairs( time, mod360(data(:,c_heading)*180/pi) );
ydat = 180/pi*data(:,m_yaw);
ind = find( abs(diff(data(:,m_yaw))) > pi );
if ~isempty(ind), ydat( ind ) = NaN; end
plot( time, ydat, xx, yy );
ydat = 180/pi*data(:,t_hdg);
ind = find( abs(diff(data(:,t_hdg))) > pi );
if ~isempty(ind), ydat( ind ) = NaN; end
line( time, ydat, 'Color', 'r' );

title( ['Commanded and Measured Heading:  ' run_name] );
xlabel( 'elapsed time (s)' );
ylabel( 'heading (deg)' );
zoom on

subplot( 4, 1, 4 )
cla
hctrl
