close all
clear all


addpath('SpinCalc');
addpath('quaternions');

MatlabFilename = 'AllDataRaw.mat';
if(~exist('AllData'))
    AllData = load(MatlabFilename);
end

Turns(1) = 0;
Heading = AllData.RC_Heading(1);
for i = 2:length(AllData.RC_Heading)
    Turns(i) = Turns(i-1);
    if(abs(AllData.RC_Heading(i)) > 90)
      if((sign(AllData.RC_Heading(i-1)) == 1) && (sign(AllData.RC_Heading(i)) == -1))
        Turns(i) = Turns(i-1)+1;
      end
      if((sign(AllData.RC_Heading(i-1)) == -1) && (sign(AllData.RC_Heading(i)) == 1))
        Turns(i) = Turns(i-1)-1;
      end
    end
           
   Heading(i) = AllData.RC_Heading(i)+Turns(i)*360;
        
end

mode = bitand(cast(AllData.RC_Status,'uint16'),3);

figure('Position',[1000    73     1404     1425]);
ah(1) = subplot(3,1,1)
plot(1:length(AllData.RC_Heading),AllData.RC_Heading,'-x');
hold all
plot(1:length(AllData.RC_Heading),AllData.RC_TargetHeading);
plot(1:length(AllData.RC_Heading),mode*100,'-rx');
%datetick('x');
ah(2) = subplot(3,1,2)
plot(1:length(AllData.RC_Heading),Heading);
%datetick('x');
ah(3) = subplot(3,1,3)
plot(1:length(AllData.RC_Heading),Turns);
%datetick('x');

linkaxes(ah,'x');

