%    USBANGLE - Plot the USBL goal heading and vehicle actual heading in top
%    window and roll corrected azimuth and elevation angles in lower window
%
%    B. A. Moran, 09-Jan-1998

%    Copyright (C) 1998
%    Massachussetts Institute of Technology
%    Sea Grant AUV Laboratory
%    $Id: usbangle.m,v 1.3 1998/05/15 15:01:41 bamoran Exp $	


if exist( 'h_usba' ) ~= 1
    h_usba = figure;
    if strcmp( computer, 'SGI' )
	winpos( [100+100*rand 100+100*rand 1024 768] );
    else
	winpos( position(2,:) );
    end
else 
    h_usba = figure( h_usba ); 
end
    
set( h_usba, 'name', 'USBL Azimuth & Range Rate', 'DeleteFcn', 'clear h_usba');

ind = find( finite(data(:,m_usbl_time)) );
R2D = 180/pi;

subplot( 2, 1, 2 ); cla

plot( data(ind,m_present_time), data(ind,m_inertial_el)*R2D, '+', ...
    data(ind,m_present_time), data(ind,m_inertial_az)*R2D, 'ro');
zoom on
title( 'Corrected Azimuth (o) and Elevation (+)' );
xlabel( 'elapsed time (s)' )
ylabel( 'corrected angle (deg)' )

xtick = get(gca, 'XTick');

subplot( 2, 1, 1 ); cla

plot( time, [(data(:,m_yaw)*R2D) data(:,t_hdg)*R2D], ...
    time(ind), (data(ind,m_inertial_az) + data(ind,m_yaw))*R2D, 'r+' )

zoom on
title( ['USBL Goal (r+) and Heading Actual: ' run_name] )
ylabel( 'heading (deg)' )

xwin([xtick(1) max(xtick)]);