%
% PURPOSE: Plot DVL data from Tiburon
% AUTHOR:  Rob
% DATE:    00/11/8
%
%  global filename

  units
  
  % titleStr = ['; Data Set: ', fileName];
  titleStr = 'DVL';

  str = [' ',titleStr,' Plotted on ',myDate];
  PaperPosn = [.5 .5 3.75 2.5];
  FigurePosn = [5 30 815 760];
  i = 0;

  if exist( 'tibdvlfig' ) ~= 1
    tibdvlfig = figure;
    set(gcf,'position',FigurePosn)
  else
    figure(tibdvlfig);
  end
  set( tibdvlfig, 'Name', 'DVL Data', 'DeleteFcn', 'clear tibdvlfig' );

  time1 = time-time(1);  
% time1 = time;
%
% Take optional start and stop time parameters to enlarge sections
% of the plot.
%
  if exist('t0') ~= 1
    n1 = 1;
  else
    n1 = find( time1==t0 );
    if isempty(n1)
      disp(' t0 is out of range.')
      n1 = 1;
    end
  end

  if exist('tf') ~= 1
    n2 = length(time1);
  else
    n2 = find( time1 == tf );
    if isempty(n2)
      disp(' tf is out of range.')
      n2 = length(time1);
    end
  end

  subplot(2,1,1)
  plot(time1(n1:n2), [dvl1(n1:n2) dvl2(n1:n2) dvl3(n1:n2)])
  speed = sqrt([dvl1(n1:n2) dvl2(n1:n2) dvl3(n1:n2)].* ...
			  [dvl1(n1:n2) dvl2(n1:n2) dvl3(n1:n2)]...
			  *[1 1 1]');
  line(time1(n1:n2), speed,'Color','c')
  legend('X','Y','Z','Speed')
  grid on
  zoom on
  title('Velocity Over the Bottom')
  ylabel('Meters/Sec')

  subplot(2,1,2)
  plot(time1(n1:n2), dvlValid(n1:n2))
  grid on
  zoom on
  title('Data Valid Flag')
  set(gca,'YLim',[-.1 1.1])

  if exist( 'dvlData' ) == 1
    if exist( 'tibdvlprocfig' ) ~= 1
      tibdvlprocfig = figure;
      set(gcf,'position',FigurePosn)
    else
      figure(tibdvlprocfig);
    end
    set( tibdvlprocfig, 'Name', 'Processed DVL Data', ...
		      'DeleteFcn', 'clear tibdvlprocfig' );

    speedProc = sqrt([dvlData(n1:n2,2) dvlData(n1:n2,3) dvlData(n1:n2,4)].* ...
		     [dvlData(n1:n2,2) dvlData(n1:n2,3) dvlData(n1:n2,4)]...
		     *[1 1 1]');
    subplot(2,1,1)
    plot(time1(n1:n2), [speed speedProc])
    legend('Speed','Processed')
    grid on
    zoom on
    title('Speed Over the Bottom')
    ylabel('Meters/Sec')

    subplot(2,1,2)
    plot(time1(n1:n2), [dvlValid(n1:n2) dvlData(n1:n2,5)])
    grid on
    zoom on
    title('Data Valid Flag')
    set(gca,'YLim',[-.1 1.1])
  end % exist dvlData

  if exist( 'tibusblfig' ) ~= 1
    tibusblfig = figure;
    set(gcf,'position',FigurePosn)
  else
    figure(tibusblfig);
  end
  set( tibusblfig, 'Name', 'USBL Data', 'DeleteFcn', 'clear tibusblfig' );

  kUTM = pi/180*rE;
  plot((positionLong(n1:n2)- positionLong(1))*kUTM ,...
       (positionLat(n1:n2) - positionLat(1) )*kUTM ,'.')
  set( gca, 'DataAspectRatio', [1 1 .25] );
  grid on
  zoom on
  ylabel('North')
  xlabel('East')
  title('Vehicle Position in APPROXIMATE Relative UTM Coords')


  if exist( 'tibahrsfig' ) ~= 1
    tibahrsfig = figure;
    set(gcf,'position',FigurePosn)
  else
    figure(tibahrsfig);
  end
  set( tibahrsfig, 'Name', 'AHRS Data', 'DeleteFcn', 'clear tibahrsfig' );

  subplot(3,1,1)
  plot(time1(n1:n2), unwrap([compass(n1:n2) usblHeading(n1:n2)]))
  grid on
  zoom on
  ylabel('Degrees')
  xlabel('Heading')

  subplot(3,1,2)
  plot(time1(n1:n2), [pitch(n1:n2) usblPitch(n1:n2)])
  grid on
  zoom on
  ylabel('Degrees')
  xlabel('Pitch, usblPitch')

  subplot(3,1,3)
  plot(time1(n1:n2), [roll(n1:n2) usblRoll(n1:n2)])
  grid on
  zoom on
  ylabel('Degrees')
  xlabel('Roll, usblRoll')
  title('Seconds')


