1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/matlab/revo/sensor_log.m
2012-01-28 10:11:34 -06:00

155 lines
4.3 KiB
Matlab

fn = '~/Documents/Programming/serial_logger/bma180_l3gd20_desk_20120228.dat';
fn = '~/Documents/Programming/serial_logger/mpu6000_desk_20120228.dat';
fn = '~/Documents/Programming/serial_logger/mpu6000_desk2_20110228.dat';
fn = '~/Documents/Programming/serial_logger/output.dat';
fid = fopen(fn);
dat = uint8(fread(fid,'uint8'));
fclose(fid);
accel = zeros(4096,1);
accel_idx = 0;
gyro = zeros(4096,1);
gyro_idx = 0;
mag = zeros(4096,1);
mag_idx = 0;
latitude = zeros(4096,1);
longitude = zeros(4096,1);
altitude = zeros(4096,1);
heading = zeros(4096,1);
groundspeed = zeros(4096,1);
gps_satellites = zeros(4096,1);
gps_time = zeros(4096,1);
gps_idx = 0;
baro = zeros(4096,1);
baro_idx = 0;
total = length(dat);
count = 0;
head = 0;
last_time = 0;
good_samples = 0;
bad_samples = 0;
while head < (length(dat) - 200)
last_head = head;
count = count + 1;
if count >= 5000
disp(sprintf('Processed: %0.3g%% Bad: %0.3g%%',(head/total) * 100,bad_samples * 100 / (bad_samples + good_samples)));
count = 0;
end
idx = find(dat(head+1:head+100)==255,1,'first');
if isempty(idx)
head = head + 100;
continue;
end
head = head + idx;
% Get the time
time = double(dat(head+1))* 256 + double(dat(head+2));%typecast(flipud(dat(head+(1:2))),'uint16');
if min([(time - last_time) (last_time - time)]) > 2
disp(['Err' num2str(time-last_time)]);
last_time = time;
bad_samples = bad_samples + (head - last_head);
continue
end
last_time = time;
head = head + 2;
% Get the accels
accel_idx = accel_idx + 1;
if accel_idx > size(accel,1)
accel(accel_idx * 2,:) = 0;
end
accel(accel_idx,1:3) = typecast(dat(head+(1:12)),'single');
head = head + 12;
accel(accel_idx,4) = time;
% Get the gyros
gyro_idx = gyro_idx + 1;
if gyro_idx > size(gyro,1);
gyro(gyro_idx * 2,:) = 0;
end
gyro(gyro_idx,1:4) = typecast(dat(head+(1:16)),'single');
head = head + 16;
gyro(gyro_idx,5) = time;
if dat(head+1) == 1 % Process the mag data
head = head+1;
mag_idx = mag_idx + 1;
if mag_idx > size(mag,1)
mag(mag_idx * 2, :) = 0;
end
mag(mag_idx,1:3) = typecast(dat(head + (1:12)),'single');
head = head+12;
mag(mag_idx,4) = time;
end
if dat(head+1) == 2 % Process the GPS data
% typedef struct {
% int32_t Latitude;
% int32_t Longitude;
% float Altitude;
% float GeoidSeparation;
% float Heading;
% float Groundspeed;
% float PDOP;
% float HDOP;
% float VDOP;
% uint8_t Status;
% int8_t Satellites;
% } __attribute__((packed)) GPSPositionData;
head = head+1;
gps_idx = gps_idx + 1;
if gps_idx > length(latitude)
latitude(gps_idx * 2) = 0;
longitude(gps_idx * 2) = 0;
altitude(gps_idx * 2) = 0;
heading(gps_idx * 2) = 0;
groundspeed(gps_idx * 2) = 0;
gps_satellites(gps_idx * 2) = 0;
gps_time(gps_idx * 2) = 0;
end
latitude(gps_idx) = double(typecast(dat(head+(1:4)),'int32')) / 1e7;
longitude(gps_idx) = double(typecast(dat(head+(5:8)),'int32')) / 1e7;
altitude(gps_idx) = typecast(dat(head+(9:12)),'single');
heading(gps_idx) = typecast(dat(head+(17:20)),'single');
groundspeed(gps_idx) = typecast(dat(head+(21:24)),'single');
gps_satelites(gps_idx) = dat(head+38);
gps_time(gps_idx) = time;
head = head + 9 * 4 + 2;
end
if dat(head+1) == 3 % Process the baro data
head = head + 1;
baro_idx = baro_idx + 1;
if baro_idx > size(baro,1)
baro(baro_idx * 2,:) = 0;
end
baro(baro_idx,1) = typecast(dat(head+(1:4)),'single');
baro(baro_idx,2) = time;
end
good_samples = good_samples + (head - last_head);
end
accel(accel_idx+1:end,:) = [];
gyro(gyro_idx+1:end,:) = [];
mag(mag_idx+1:end) = [];
latitude(gps_idx+1:end) = [];
longitude(gps_idx+1:end) = [];
altitude(gps_idx+1:end) = [];
groundspeed(gps_idx+1:end) = [];
gps_satellites(gps_idx+1:end) = [];
gps_time(gps_idx+1:end) = [];
baro(baro_idx+1:end,:) = [];