% Displacement.m
%
% This script... 
% Date          Author          Notes
% 20/11/2011    B. Kieft        Attempting to extract linear displacement. 


%% Start of script
addpath('quaternion_library');      % include quaternion library
%close all;                          % close all figures
%clear;                              % clear all variables
clc;                                % clear the command terminal

%% Import and plot sensor data



if( exist('position') ~= 1 ) position = [3 59 1213 1070]; end


if( exist('Kp') ~=1) Kp = .002;end
if( exist('Ki') ~=1) Ki = 0;end

if( exist('fileName') )
  fullFileName = strcat(fileName,'.csv');
  if( exist(fullFileName) == 2 )

    AHRSData = mfcsvread(fullFileName);
    Ts = .01;   %Sample Period; 100 Hz sample rate. 

    Euler(:,1) = AHRSData.euler_y;
    Euler(:,2) = AHRSData.euler_x;
    Euler(:,3) = AHRSData.euler_z;
    
    time = AHRSData.sample*Ts;
    
    xImu = 0;
  end
else
  fprintf('Running zero-motion case with g and b hardcoded constants.\n');
  xImu = 0;
  Ts = .01;                             %Sample Period; 100 Hz sample rate. 
  npts = 1000;
  time               =  [1:npts]'*Ts;
end

%% Plot algorithm output as Euler angles

figure('Name', 'Euler Angles');
set(gcf,'position',position)
hold on;
%plot(time, rad2deg(Euler));
plot(time, Euler(:,1), 'r');
plot(time, Euler(:,2), 'g');
plot(time, Euler(:,3), 'b');
str = sprintf( 'Euler Angles; Kp = %5.2e, Ki = %5.2e',Kp, Ki);
title(str);
xlabel('Time (s)');
ylabel('Angle (deg)');
legend('phi', 'theta', 'psi');
hold off;
grid on


%% End of script
