ground/gcs/src/experimental/SerialLogger/analyzeINSGPS.m

   1 function [q gyro accel rpy time] = analyzeINSGPS(fn)
   2 % Analyzes data collected from SerialLogger while DUMP_FRIENDLY
   3 % enabled in AHRS
   4 %
   5 % [q gyro accel time] = analyzeINSGPS(fn)
   6
   7 fid = fopen(fn);
   8
   9 i = 1;
  10 data(i).block = -1;
  11 tline = fgetl(fid);
  12 while ischar(tline) && ~isempty(tline)
  13     switch(tline(1))
  14         case 'q'
  15             c = textscan(tline,'q: %f %f %f %f');
  16             data(i).q = [c{:}] / 1000;
  17         case 'b'
  18             c = textscan(tline,'b: %f');
  19             i = i+1;
  20             data(i).block = c{1};
  21         case 'm'
  22             c = textscan(tline,'m: %f %f %f');
  23             data(i).mag = [c{:}];
  24         case 'a'
  25             c = textscan(tline,'a: %f %f %f %f');
  26             data(i).accel = [c{:}] / 1000;
  27         case 'g'
  28             c = textscan(tline,'g: %f %f %f %f');
  29             data(i).gyro = [c{:}] / 1000;
  30     end
  31     tline = fgetl(fid);
  32 end
  33
  34 fclose(fid);
  35
  36 b = [data.block]; % get block counts
  37 gaps = find(diff(b) ~= 1);
  38 if(gaps) % get biggest contiguous chunk
  39     gaps = [1 gaps length(b)];
  40     lengths = diff(gaps);
  41     [foo idx] = max(lengths);
  42     idx = gaps(idx):gaps(idx+1);
  43     data = data(idx);
  44 end
  45
  46 data(end) = []; % delete in case partial update
  47
  48 q = cat(1,data.q);
  49 accel = cat(1,data.accel);
  50 gyro = cat(1,data.gyro);
  51 time = (1:size(q,1)) / 50;
  52
  53 rpy = Quaternion2RPY(q);
  54
  55 h(1) = subplot(411);
  56 plot(time,q)
  57 ylabel('Quaternion');
  58 h(2) = subplot(412);
  59 plot(time,rpy);
  60 ylabel('RPY'); legend('Roll','Pitch','Yaw');
  61 h(3) = subplot(413);
  62 plot(time,accel);
  63 ylabel('m/s')
  64 h(4) = subplot(414);
  65 plot(time,gyro);
  66 ylabel('rad/sec');
  67 xlabel('Time (s)')
  68
  69 linkaxes(h,'x');
  70
  71 function rpy = Quaternion2RPY(q)
  72
  73 RAD2DEG = 180/pi;
  74
  75 q = bsxfun(@rdivide,q,sqrt(sum(q.^2,2)));
  76 qs = q .* q;
  77
  78 R13 = 2*(q(:,2).*q(:,4) -q(:,1).*q(:,3));     %2*(q[1]*q[3]-q[0]*q[2]);
  79 R11 = qs(:,1) + qs(:,2) - qs(:,3) - qs(:,4);  %q0s+q1s-q2s-q3s;
  80 R12 = 2*(q(:,2).*q(:,3) + q(:,1).*q(:,4));    %2*(q[1]*q[2]+q[0]*q[3]);
  81 R23 = 2*(q(:,3).*q(:,4) + q(:,1) .* q(:,2));  %2*(q[2]*q[3]+q[0]*q[1]);
  82 R33 = qs(:,1)-qs(:,2)-qs(:,3)+qs(:,4);        %q0s-q1s-q2s+q3s;
  83
  84 rpy(:,2)=RAD2DEG*asin(-R13);   % pitch always between -pi/2 to pi/2
  85 rpy(:,3)=RAD2DEG*atan2(R12,R11);
  86 rpy(:,1)=RAD2DEG*atan2(R23,R33);
  87