matlab/revo/sensor_log.m

   1 fn = '~/Documents/Programming/serial_logger/bma180_l3gd20_desk_20120228.dat';
   2 fn = '~/Documents/Programming/serial_logger/mpu6000_desk_20120228.dat';
   3 fn = '~/Documents/Programming/serial_logger/mpu6000_desk2_20110228.dat';
   4 fn = '~/Documents/Programming/serial_logger/output.dat';
   5 fid = fopen(fn);
   6 dat = uint8(fread(fid,'uint8'));
   7 fclose(fid);
   8
   9 accel = zeros(4096,1);
  10 accel_idx = 0;
  11
  12 gyro = zeros(4096,1);
  13 gyro_idx = 0;
  14
  15 mag = zeros(4096,1);
  16 mag_idx = 0;
  17
  18 latitude = zeros(4096,1);
  19 longitude = zeros(4096,1);
  20 altitude = zeros(4096,1);
  21 heading = zeros(4096,1);
  22 groundspeed = zeros(4096,1);
  23 gps_satellites = zeros(4096,1);
  24 gps_time = zeros(4096,1);
  25 gps_idx = 0;
  26
  27 baro = zeros(4096,1);
  28 baro_idx = 0;
  29
  30 total = length(dat);
  31 count = 0;
  32 head = 0;
  33 last_time = 0;
  34 good_samples = 0;
  35 bad_samples = 0;
  36 while head < (length(dat) - 200)
  37
  38     last_head = head;
  39
  40     count = count + 1;
  41     if count >= 5000
  42         disp(sprintf('Processed: %0.3g%% Bad: %0.3g%%',(head/total) * 100,bad_samples * 100 / (bad_samples + good_samples)));
  43         count = 0;
  44     end
  45     idx = find(dat(head+1:head+100)==255,1,'first');
  46     if isempty(idx)
  47         head = head + 100;
  48         continue;
  49     end
  50     head = head + idx;
  51
  52
  53     % Get the time
  54     time = double(dat(head+1))* 256 + double(dat(head+2));%typecast(flipud(dat(head+(1:2))),'uint16');
  55     if min([(time - last_time) (last_time - time)]) > 2
  56         disp(['Err' num2str(time-last_time)]);
  57         last_time = time;
  58         bad_samples = bad_samples + (head - last_head);
  59         continue
  60     end
  61     last_time = time;
  62
  63     head = head + 2;
  64
  65     % Get the accels
  66     accel_idx = accel_idx + 1;
  67     if accel_idx > size(accel,1)
  68         accel(accel_idx * 2,:) = 0;
  69     end
  70     accel(accel_idx,1:3) = typecast(dat(head+(1:12)),'single');
  71     head = head + 12;
  72     accel(accel_idx,4) = time;
  73
  74     % Get the gyros
  75     gyro_idx = gyro_idx + 1;
  76     if gyro_idx > size(gyro,1);
  77         gyro(gyro_idx * 2,:) = 0;
  78     end
  79     gyro(gyro_idx,1:4) = typecast(dat(head+(1:16)),'single');
  80     head = head + 16;
  81     gyro(gyro_idx,5) = time;
  82
  83     if dat(head+1) == 1 % Process the mag data
  84         head = head+1;
  85         mag_idx = mag_idx + 1;
  86         if mag_idx > size(mag,1)
  87             mag(mag_idx * 2, :) = 0;
  88         end
  89         mag(mag_idx,1:3) = typecast(dat(head + (1:12)),'single');
  90         head = head+12;
  91         mag(mag_idx,4) = time;
  92     end
  93
  94     if dat(head+1) == 2 % Process the GPS data
  95         % typedef struct {
  96         %     int32_t Latitude;
  97         %     int32_t Longitude;
  98         %     float Altitude;
  99         %     float GeoidSeparation;
 100         %     float Heading;
 101         %     float Groundspeed;
 102         %     float PDOP;
 103         %     float HDOP;
 104         %     float VDOP;
 105         %     uint8_t Status;
 106         %     int8_t Satellites;
 107         % } __attribute__((packed)) GPSPositionData;
 108
 109         head = head+1;
 110
 111         gps_idx = gps_idx + 1;
 112         if gps_idx > length(latitude)
 113             latitude(gps_idx * 2) = 0;
 114             longitude(gps_idx * 2) = 0;
 115             altitude(gps_idx * 2) = 0;
 116             heading(gps_idx * 2) = 0;
 117             groundspeed(gps_idx * 2) = 0;
 118             gps_satellites(gps_idx * 2) = 0;
 119             gps_time(gps_idx * 2) = 0;
 120         end
 121
 122         latitude(gps_idx) = double(typecast(dat(head+(1:4)),'int32')) / 1e7;
 123         longitude(gps_idx) = double(typecast(dat(head+(5:8)),'int32')) / 1e7;
 124         altitude(gps_idx) = typecast(dat(head+(9:12)),'single');
 125         heading(gps_idx) = typecast(dat(head+(17:20)),'single');
 126         groundspeed(gps_idx) = typecast(dat(head+(21:24)),'single');
 127         gps_satelites(gps_idx) = dat(head+38);
 128         gps_time(gps_idx) = time;
 129         head = head + 9 * 4 + 2;
 130     end
 131
 132     if dat(head+1) == 3 % Process the baro data
 133         head = head + 1;
 134         baro_idx = baro_idx + 1;
 135         if baro_idx  > size(baro,1)
 136             baro(baro_idx * 2,:) = 0;
 137         end
 138         baro(baro_idx,1) = typecast(dat(head+(1:4)),'single');
 139         baro(baro_idx,2) = time;
 140     end
 141
 142     good_samples = good_samples + (head - last_head);
 143 end
 144
 145 accel(accel_idx+1:end,:) = [];
 146 gyro(gyro_idx+1:end,:) = [];
 147 mag(mag_idx+1:end) = [];
 148 latitude(gps_idx+1:end) = [];
 149 longitude(gps_idx+1:end) = [];
 150 altitude(gps_idx+1:end) = [];
 151 groundspeed(gps_idx+1:end) = [];
 152 gps_satellites(gps_idx+1:end) = [];
 153 gps_time(gps_idx+1:end) = [];
 154 baro(baro_idx+1:end,:) = [];