% Extract and plot data from rate gyro and accelerometers placed onboard a % 1.25 wingspan RC flying wing. Data are logged with a high sampling rate of 1kHz. % Remote control instructions are also recorded and plotted at a lower % sampling rate. % Script also detect the launch time, and the landing (i.e. crash time). % from inertial sensors and mark theses time with wide black vertical line. % Time where bluetooth transmission lost errors occurs are marked with thin blue vertical % line. % Script written by Lubin Kerhuel -- January 2009 % see website : http://www.kerhuel.eu for more details about this script fclose all; % close all files fid = -1; % flag %% Uncomment one of the Fid open line and run the script or run it as is %% Morning flight (Poor wind condition) %fid = fopen('Flight003.txt'); %46 seconds flight with end of flight %fid = fopen('Flight004.txt'); %47 seconds flight %fid = fopen('Flight005.txt'); %35 seconds flight %fid = fopen('Flight006.txt'); %12.65 seconds flight with end of flight %% Launch time flight (huge weight added on the airplane noze) %fid = fopen('Flight010.txt'); %159 seconds flight (start at 56s) %fid = fopen('Flight011.txt'); %66 seconds flight with end of flight %fid = fopen('Flight012.txt'); %12.35 second flight with end of flight, more than one minute long log %fid = fopen('Flight013.txt'); %268.442 seconds flight - 11 errors %fid = fopen('Flight015.txt'); %272.59 seconds flight - 7 errors if (fid == -1) % open user dialog to select a .txt file with flight's data d = dir('*.txt'); str = {d.name}; [s,v] = listdlg('PromptString','Select a flight datafile to plot:',... 'SelectionMode','single',... 'ListString',str); fid = fopen(str{s}); end %% Read and close file Rn = fread(fid); % read txt file fclose(fid); % close file Rr = uint16(Rn); %% check the signature value every 11 sample, count transmission errors idxError = 1; Rout = []; IndexError = []; while ~isempty(idxError) idx = find(Rr==55); Rr = Rr(idx(1):end); sizeR = floor(length(Rr)/11)*11; Rr = Rr(1:sizeR); Rlocal = reshape(Rr,11,sizeR/11)'; idxError = find(Rlocal(:,1) ~= 55); if isempty(idxError) Rout = [Rout ; Rlocal ]; else Rlocal = Rlocal(1:(idxError(1)-2),:); Rr = Rr((idxError(1)*11):end); Rout = [Rout ; Rlocal ]; IndexError(end+1) = length(Rout)+1;%idxError(1)-1; % store place where error appears end end fprintf('%d Reception errors',length(IndexError)); %% reconstruct int16 values from binary field data Rr = Rout; V1 = Rr(:,2)+bitand(Rr(:,3),15)*256; V2 = Rr(:,4)*16 + bitand(Rr(:,3),240) ./ 16; V3 = Rr(:,5)+bitand(Rr(:,6),15)*256; V4 = Rr(:,7)*16 + bitand(Rr(:,6),240) ./ 16; V5 = Rr(:,8)+bitand(Rr(:,9),15)*256; V6 = Rr(:,10)*16 + bitand(Rr(:,9),240) ./ 16; %% Generate time L = length(V6); t = 0:.001:L*.001-.001; TimeReceptionError = t(IndexError); %ForSimulink = [t;double(V1');double(V2');double(V3');double(V4');double(V5');double(V6')]; %save ForSimulink ForSimulink; %save CalibrationGyroZ Rr; %save Static Rr; %% convert data into double GyroY = double(V1); GyroX = double(V4); GyroZ = double(V6); AccelY = double(V2); AccelX = double(V5); AccelZ = double(V3); %% unbias and scald gyro GyroY = (GyroY - (2048+50) ) * 80/2048*4.93; GyroX = (GyroX - (2048-141) ) * 80/2048*2.49; GyroZ = (GyroZ - (2048-130) ) * 80/2048*2.5; %% unbias accelerometers AccelX = (AccelX + (-704-615) ); AccelY = -(AccelY + (-783) ); AccelZ = (AccelZ + (-764) ); %Accel X : vertical (Positive = acceleration toward down) %Accel Y : horizontal forward (positive = acceleration forward) (change sign) %Accel Z : Horizontal lateral (positive = acceleration toward left) %% rename angular rate variables : p = -GyroY; % p : roll (positive = roll right) (change sign) q = -GyroX; % q : pitch (negatif = pich down) (change sign) r = -GyroZ; % r : yaw (positive = yaw toward right) (change sign) %% Plot 3 rate gyro, 3 accelerometers figure('Name','Rate gyro and accelerometers data remotely transmitted from an RC flying wing'); subplot(3,1,1) plot(t,p,'b') ; hold on; plot(t,q,'r'); plot(t,r,'g'); hold off; axis tight; ylabel({'rate gyro','deg/s'}); legend('p','q','r','Location','Best'); subplot(3,1,2) plot(t,AccelX./630,'r') ; hold on; plot(t,AccelY./630,'b'); plot(t,AccelZ./630,'g'); hold off;axis tight; ylabel({'Accelerometers','(g)'}); %% Reconstruct remote control and servo command data % check the signature value (170) every 17 sample, reconstruct valide % sequence V7 = Rr(:,11); V7 = double(V7); idx2 = find(V7 == 170); % index of all possible signature value V7Vector = V7; diffidx2 = diff(idx2); tmp = 0; idx2OK=[]; for(i=1:length(diffidx2)); if((diffidx2(i)+tmp) < 17) % if less than 17 bytes between each signature i = i+1; % do not take into account tmp = tmp + diffidx2(i); else tmp = 0; idx2OK(end+1) = idx2(i+1); end end idx2OK = [idx2(1) idx2OK(1:end-1) ]; % Only valide index separated by 17 or more bytes t2 = t(idx2OK); % Time index for theses datas %% Reconstruct data from remote control and servo control command Check = V7Vector((idx2OK-1)+1); % column with signature value : 170 VertLeft = V7Vector((idx2OK-1)+2) + V7Vector((idx2OK-1)+3)*256; HorzLeft = V7Vector((idx2OK-1)+4) + V7Vector((idx2OK-1)+5)*256; VertRight = V7Vector((idx2OK-1)+6) + V7Vector((idx2OK-1)+7)*256; HorzRight = V7Vector((idx2OK-1)+8) + V7Vector((idx2OK-1)+9)*256; Pot1 = V7Vector((idx2OK-1)+10) + V7Vector((idx2OK-1)+11)*256; Pot2 = V7Vector((idx2OK-1)+12) + V7Vector((idx2OK-1)+13)*256; ServoLeft = V7Vector((idx2OK-1)+14) + V7Vector((idx2OK-1)+15)*256; ServoRight = V7Vector((idx2OK-1)+16) + V7Vector((idx2OK-1)+17)*256; verif = find(Check ~= 170); if isempty(verif) disp('- data processing OK'); else disp('- data processing Not OK'); end %% take into account sign of data idx = find(VertLeft > 65535 / 2); VertLeft(idx)= VertLeft(idx)-65536; idx = find(HorzRight > 65535 / 2); HorzRight(idx)= HorzRight(idx)-65536; %% Plot remote control data subplot(3,1,3) plot(t2(1:length(VertLeft)),VertLeft,t2(1:length(HorzRight)),HorzRight); legend('Up/Down stick','roll stick','Location','Best'); axis tight; ylim([-6000 6000]) ylabel('RC command') xlabel('Time (s)') %% %% Search and Plot Tlaunch % find the end of the first saturation on the Y accel axis (longitudinal) Tmp = find(AccelY == 783); % Saturation value Tlaunch = t(Tmp(1)); Tmp2 = find(diff(Tmp) >20 ); if ~isempty(Tmp2) Tlaunch = t(Tmp(Tmp2(1))+1); end figure(1); subplot(3,1,1); hold on; plot([Tlaunch Tlaunch+eps],[-40000 40000],'k','linewidth',2); hold off; subplot(3,1,2); hold on; plot([Tlaunch Tlaunch+eps],[-40000 40000],'k','linewidth',2); hold off; subplot(3,1,3); hold on; plot([Tlaunch Tlaunch+eps],[-40000 40000],'k','linewidth',2); hold off; %% Search and plot TCrash % find the first deceleration on the Y accel axis that occurs after Tlaunch (longitudinal axis) Tmp = find(AccelY < -500); if isempty(Tmp) Tmp = length(AccelY); end Tmp2 = find(t(Tmp) > (Tlaunch+1)); % take 1 second marging if ~isempty(Tmp2) Tcrash = t(Tmp(Tmp2(1))); else Tcrash = t(end); end figure(1); subplot(3,1,1); hold on; plot([Tcrash Tcrash+eps],[-40000 40000],'k','linewidth',2);hold off; subplot(3,1,2); hold on; plot([Tcrash Tcrash+eps],[-40000 40000],'k','linewidth',2);hold off; subplot(3,1,3); hold on; plot([Tcrash Tcrash+eps],[-40000 40000],'k','linewidth',2);hold off; subplot(3,1,1); hold on; plot([TimeReceptionError ; TimeReceptionError+eps],[-40000 40000]'*ones(1,length(TimeReceptionError)),'--c');hold off; subplot(3,1,2); hold on; plot([TimeReceptionError ; TimeReceptionError+eps],[-40000 40000]'*ones(1,length(TimeReceptionError)),'--c');hold off; subplot(3,1,3); hold on; plot([TimeReceptionError ; TimeReceptionError+eps],[-40000 40000]'*ones(1,length(TimeReceptionError)),'--c');hold off; disp(sprintf('Flight time is %3.2f seconds',Tcrash-Tlaunch)); subplot(3,1,1) ;title(sprintf('Flight of %3.2f seconds : from %3.2f s until %3.2f s',Tcrash-Tlaunch,Tlaunch,Tcrash)); text(Tlaunch,-200,'\leftarrow Take off'); text(Tcrash,-200,'\leftarrow Crash'); text(Tlaunch/2+Tcrash/2,200,'Flight'); %% end of raw data plot break; %% Interpolation VertLeft = interp1q(t2',VertLeft,t'); HorzLeft = interp1q(t2',HorzLeft,t'); VertRight = interp1q(t2',VertRight,t'); HorzRight = interp1q(t2',HorzRight,t'); Pot1 = interp1q(t2',Pot1,t'); Pot2 = interp1q(t2',Pot2,t'); ServoLeft = interp1q(t2',ServoLeft,t'); ServoRight = interp1q(t2',ServoRight,t'); VertLeft(isnan(VertLeft)) = 0; HorzLeft(isnan(HorzLeft)) = 0; VertRight(isnan(VertRight)) = 0; HorzRight(isnan(HorzRight)) = 0; Pot1(isnan(Pot1)) = 0; Pot2(isnan(Pot2)) = 0; ServoLeft(isnan(ServoLeft)) = 0; ServoRight(isnan(ServoRight)) = 0; % few more test %plot(sqrt(AccelX.^2+AccelY.^2+AccelZ.^2)) %plot(cumsum(sqrt(AccelX.^2+AccelY.^2+AccelZ.^2)-605)) % figure(2); % plot(t,VertLeft); hold on; % plot(t,GyroX*10,'r'); hold on; % % figure(3); % plot(t,HorzRight); hold on; % plot(t,GyroY*10,'r'); hold on; % % figure(4); % VertLeft2 = VertLeft * 90/2000; % plot(diff(VertLeft2)*1000); %% export graph %print('-depsc2','-r600','graph'); %Lauhch close up for i=1:3 subplot(3,1,i); xlim([Tlaunch-.6 Tlaunch+.4]); end for i=1:3 subplot(3,1,i); xlim([Tcrash-.2 Tcrash+.6]); end