% To extract IMU data from a file
% viewdat --imu_diag -h -m YourData.T04 > IMU.txt
% the data will be in m/s^2
load('IMU.txt');

% User can define a title String which is added to each file name
if(exist('TitleStr','var') == 0)
  TitleStr = 'VibrationTest';
end

% Sample frequency
Fs = 200;

% Survey Operating Spec
Name{1} = 'Survey Operating';
Spec{1}(:,1) = [5    350  500  500];
Spec{1}(:,2) = [0.015  0.015 0.00735 0.0001];
% Survey Survival Spec
Name{2} = 'Survey Survival';
Spec{2}(:,1) = [10    300  1000  1000];
Spec{2}(:,2) = [0.04  0.004 0.002 0.0000001];
% Profile 2 - Cab
Name{3} = 'Profile 2 - Cab';
Spec{3}(:,1) = [24    60  102  300 2000];
Spec{3}(:,2) = [0.04 0.5 0.5 0.1 0.1];
% Profile 5 - ROPS (Safety Cage)
Name{4} = 'Profile 5 - ROPS (Safety Cage)';
Spec{4}(:,1) = [24 60 102 300 2000];
Spec{4}(:,2) = [0.04 0.04 0.04 0.025 0.025];
% Profile 10 - Blade
Name{5} = 'Profile 10 - Blade';
Spec{5}(:,1) = [24 96 150 300 2000];
Spec{5}(:,2) = [0.1 0.5 0.5 0.19 0.19];

% Colours for the spec lines
colorArray(1,1:3) = [0.5   0.5  1.0 ];
colorArray(2,1:3) = [1.0   0.0  0.0 ];
colorArray(3,1:3) = [1.0   0.75  0.0 ];
colorArray(4,1:3) = [0.0   1.0  0.0 ];
colorArray(5,1:3) = [0.0   1.0  1.0 ];


% Convert from m/s^2 to g
scale = 0.101972;

% We'll process X, then Y and finally X
TypeStr = ['X'; 'Y'; 'Z';];

% Data columns
dTime = 4;
dXAccel = 8;
dYAccel = 9;
dZAccel = 10;

% Plot the time series
figure
plot(IMU(:,dTime), IMU(:,dXAccel).*scale, 'b');
hold on
plot(IMU(:,dTime), IMU(:,dYAccel).*scale, 'r');
plot(IMU(:,dTime), IMU(:,dZAccel).*scale, 'g');
legend('X','Y','Z','location','best');
grid
xlabel('GPS Time [Secs]');
ylabel('Acceleration [g]');
print([TitleStr,'-IMU.png'],'-dpng');

% loop for each axis and plot the PSD
for i=1:3
  % convert to g
  Accel = IMU(:,((dXAccel-1)+i)) * scale;

  if(exist('pwelch') == 2)
    % Needs the signal processing tool box
    [pxx,f] = pwelch(Accel-mean(Accel),[],[],[],Fs);
  else
    N = 2^16;
    if(length(Accel) < N)
      N = length(Accel);
    end
    [pxx,f,rms_ret] = psdfft(Accel-mean(Accel), N, Fs, 2, round(N/8), 0);
  end

  figure
  colormap('default');
  semilogy(f,pxx);
  A = axis;
  A(3) = 1e-6;
  A(4) = 1;

  hold on;
  % Plot the various specs
  for j=1:length(Spec)
    semilogy(Spec{j}(:,1), Spec{j}(:,2), 'color',colorArray(j,:),'linewidth',3);
  end

  axis(A)
  grid
  xlabel('Frequency [Hz]');
  ylabel('PSD [g^2/Hz]');
  legend(['Accelerometer',Name],'location','best');
  title([TitleStr,' - ',TypeStr(i),' Axis']);
  print([TitleStr,'-',TypeStr(i),'Axis.png'],'-dpng');

  if(exist('spectrogram') == 2)
    figure
    L = round(length(Accel)/2048);
    spectrogram(Accel - mean(Accel),L,[],[],200,'psd')
    C = colorbar;
    % ToDo - check the units!
    C.Label.String = 'PSD [g^2/Hz dB]';
    xlabel('Frequency [Hz]');
    colormap('Jet');
    title([TitleStr,' - ',TypeStr(i),' Axis']);
    print([TitleStr,'-',TypeStr(i),'Spectrogram.png'],'-dpng');
  else
    disp('Spectrogram not available - ToDo implement without the signal processing toolbox');
  end
end


