%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot Altex/QNX navigation data.
% AUTHOR:  Initial; rsm
% DATE:    99/12/14
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Units and Fundamental Constants:
  units
  hardflag=0;

  time = 1;

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

  if exist( 'ahrs_dpt' ) ~= 1
      ahrs_dpt = figure;
  else 
      figure( ahrs_dpt ); 
  end
    
  set( ahrs_dpt, 'name', 'DorOdyssey Attitude & Heading Reference', ...
       'DeleteFcn', 'clear ahrs_dpt' );
  set(gcf,'position',position)
  clf

    subplot(3,1,1)
    [xx,yy] = stairs( dataNav(:,time), dataNav(:,mPhi)/d2r);
    plot(xx,yy);
    ylabel('Degrees')
    title(['Roll, Degrees',str]);
    grid on
    zoom on

    subplot(3,1,2)
    [xx,yy] = stairs( dataNav(:,time), dataNav(:,mTheta)/d2r);
    plot(xx,yy);
    ylabel('Degrees')
    title(['Pitch, Degrees',str]);
    grid on

    subplot(3,1,3)
    [xx,yy] = stairs( dataNav(:,time), dataNav(:,mPsi)/d2r);
    plot(xx,yy);
    ylabel('Degrees')
    title(['Yaw, Degrees',str]);
    grid on


  if exist( 'ahrsR_dpt' ) ~= 1
      ahrsR_dpt = figure;
  else 
      figure( ahrsR_dpt ); 
  end
    
  set( ahrsR_dpt, 'name', 'DorOdyssey Body Rates', ...
       'DeleteFcn', 'clear ahrsR_dpt' );
  set(gcf,'position',position)
  clf
    subplot(3,1,1)
    [xx,yy] = stairs( dataNav(:,time), dataNav(:,mOmega_x)/d2r);
    plot(xx,yy);
    ylabel('Degrees/Sec')
    title(['X Axis Body Rate (Omega B x), Degrees/Sec',str]);
    grid on

    subplot(3,1,2)
    [xx,yy] = stairs( dataNav(:,time), dataNav(:,mOmega_y)/d2r);
    plot(xx,yy);
    ylabel('Degrees/Sec')
    title(['Y Axis Body Rate (Omega B y), Degrees/Sec',str]);
    grid on

    subplot(3,1,3)
    [xx,yy] = stairs( dataNav(:,time), dataNav(:,mOmega_z)/d2r);
    plot(xx,yy);
    ylabel('Degrees/Sec')
    title(['Z Axis Body Rate (Omega B z), Degrees/Sec',str]);
    grid on


  return






  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(3,1,1)
  plot(dataNav(:,tIx), dataNav(:,[pitchIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Pitch, Degrees',str]);
  grid on
  subplot(3,1,2)
  plot(dataNav(:,tIx), [dataNav(:,qIx)]/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate q (Omega_N_B_y)',str]);
  xlabel('Seconds');
  grid on
  subplot(3,1,3)
  plot(dataNav(:,tIx), [dataNav(:,deltaEIx)]*180/pi);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Elevator Angle',str]);
  xlabel('Seconds');
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(3,1,1)
  plot(dataNav(:,tIx), dataNav(:,[yawIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Heading, Degrees',str]);
  grid on
  subplot(3,1,2)
  plot(dataNav(:,tIx), dataNav(:,[rIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate r (Omega_N_B_z)',str]);
  grid on
  subplot(3,1,3)
  plot(dataNav(:,tIx), [dataNav(:,deltaRIx)]*180/pi);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Rudder Angle',str]);
  xlabel('Seconds');
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(2,1,1)
  plot(dataNav(:,tIx), dataNav(:,[rollIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Roll, Degrees',str]);
  grid on
  subplot(2,1,2)
  plot(dataNav(:,tIx), dataNav(:,[pIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate p (Omega_N_B_x)',str]);
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  plot(dataNav(:,yPosIx), dataNav(:,[xPosIx]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('North, Meters')
  xlabel('East, Meters')
  title(['Vehicle Location in Earth Fixed Coordinates',str]);
  grid on

