jumpReadData.m

Go to the documentation of this file.

% =========================================================================== %
%> @brief reads binary files of JUMP Sensors
%>
%> this function reads the binary file provided by the JUMP Sensor and returns
%> a struct containing the data.
%>
%> @param filePath - full path to the binary file
%> @param options.time_correction - boolean to enable time correction (default: true)
%> @param options.plot_time_correction - boolean to plot the time correction (default: false)
%> @param options.time_jump_correction - boolean to correct time jumps (default: true)
%> @param options.plot_time_jump_correction - boolean to plot the time jump correction (default: false)
%> @param options.header_only - boolean to only read the header of the file (default: false)
%>
%> @retval data - struct containing the data. the fields are defined in
%> @ref jumpCheckData
%>
%> @note
%> - This implementation is inspired by the implementation of Tobias Voegeli
%> (ETHZ) from 2016.
%> - This function also undoes timing errors made in the firmware. See @ref
%> jumpCorrectData for more information.
%>
%> @code{.m}
%> % example usage:
%> data = jumpReadData("jump_1.BIN");
%> @endcode
%>
%> @file jumpReadData.m
%>
%> @copyright see the file @ref LICENSE in the root directory of the repository
% =========================================================================== %
 
function data = jumpReadData(filePath, options)
 
arguments
    filePath {mustBeFile}
    options.time_correction (1,1) logical = true
    options.plot_time_correction (1,1) logical = false
    options.time_jump_correction (1,1) logical = true
    options.plot_time_jump_correction (1,1) logical = false
    options.header_only (1,1) logical = false
end
 
FILE_HEADER = 512;
SAMPLE_SIZE = 59;
 
fileID = fopen(filePath);
if options.header_only
    data = uint8(fread(fileID, FILE_HEADER, 'uint8'));
else
    data = uint8(fread(fileID, inf, 'uint8'));
end
fclose(fileID);
 
%% check file consistency
% check for correct length (and correct if necessary)
len = (numel(data)-FILE_HEADER)/SAMPLE_SIZE;
if(rem(len,1)~=0)
    len                                     = floor(len);
    data(SAMPLE_SIZE*len+1+FILE_HEADER:end) = [];  % erase incomplete last block
end
 
% check if newline is at correct location
cr = data(FILE_HEADER+58:SAMPLE_SIZE:end);
lf = data(FILE_HEADER+59:SAMPLE_SIZE:end);
 
err = [find(lf~=10) find(cr~=13)];
if(~isempty(err))
    nerr = numel(unique(err));
    warning(['Error parsing file. Inconsistent block format. ' num2str(nerr) ' faulty block(s) found.'])
end
 
%% parse data block
% file header
ID          = sprintf(strcat(char(data(22:33))));
UID         = num2str(swapbytes(typecast(data(50:57),'uint64')));
name        = sprintf(strcat(char(data(58:73))));
sw_version  = strcat(num2str(typecast(data(74:77),'uint32')), '(', data(78),')');
hw_version  = strcat(num2str(typecast(data(80:83),'uint32')), '(', data(79),')');
acc_range   = double(data(84));
gyro_range  = double(uint32(data(85)) * 10);
mpu_cal     = double(data(86));
act_thrs    = double(data(87));
inact_dur   = double(data(88));
mpu_rate    = double(data(89));
adxl_rate   = double(data(90));
pres_rate   = double(data(91));
magn_en     = double(data(92));
sync        = double(data(93));
ble_address = dec2hex(uint64(typecast(data(96:99),'uint32'))*2^16 + uint64(data(95))*2^8 + uint64(data(94)));
ble_address = dec2hex(swapbytes(uint64(hex2dec(ble_address))));
ble_address = ble_address(1:end-4);
 
if ~options.header_only
    
    
    % time
    sec    = double(typecast( reshape([data(FILE_HEADER+1:SAMPLE_SIZE:end), data(FILE_HEADER+2:SAMPLE_SIZE:end), data(FILE_HEADER+3:SAMPLE_SIZE:end), data(FILE_HEADER+4:SAMPLE_SIZE:end)]',1,[]), 'uint32'));
    subsec = double(typecast( reshape([data(FILE_HEADER+5:SAMPLE_SIZE:end), data(FILE_HEADER+6:SAMPLE_SIZE:end)]',1,[]), 'uint16'));
    t      = (sec+subsec/2^15)';
    t = datetime(1970,1,1,0,0,0) + seconds(t);
    
    % acceleration (from Ivensense MPU-9250)
    acc      = zeros(len,3);
    acc(:,1) = typecast( reshape([data((FILE_HEADER+7):SAMPLE_SIZE:end), data((FILE_HEADER+8):SAMPLE_SIZE:end)]',1,[]), 'int16');
    acc(:,2) = typecast( reshape([data((FILE_HEADER+9):SAMPLE_SIZE:end), data((FILE_HEADER+10):SAMPLE_SIZE:end)]',1,[]), 'int16');
    acc(:,3) = typecast( reshape([data((FILE_HEADER+11):SAMPLE_SIZE:end), data((FILE_HEADER+12):SAMPLE_SIZE:end)]',1,[]), 'int16');
    acc      = double(acc) / double(int32(0x10000) / (2 * int32(acc_range))); % rescale to unit 'g'
    acc      = acc * 9.80665; % standard gravity (BIPM, 1901)
    
    % angular rate (from Ivensense MPU-9250)
    gyro      = zeros(len,3);
    gyro(:,1) = typecast( reshape([data(FILE_HEADER+13:SAMPLE_SIZE:end), data(FILE_HEADER+14:SAMPLE_SIZE:end)]',1,[]), 'int16');
    gyro(:,2) = typecast( reshape([data(FILE_HEADER+15:SAMPLE_SIZE:end), data(FILE_HEADER+16:SAMPLE_SIZE:end)]',1,[]), 'int16');
    gyro(:,3) = typecast( reshape([data(FILE_HEADER+17:SAMPLE_SIZE:end), data(FILE_HEADER+18:SAMPLE_SIZE:end)]',1,[]), 'int16');
    gyro      = double(gyro) / (double(int32(0x10000)) / double(2 * int32(gyro_range))); % rescale to unit '°/s'
    
    % high G acceleration (from Analog Devices ADXL375)
    accHiG      = zeros(len,3);
    accHiG(:,1) = double(typecast( reshape([data(FILE_HEADER+35:SAMPLE_SIZE:end), data(FILE_HEADER+36:SAMPLE_SIZE:end)]',1,[]), 'int16'));
    accHiG(:,2) = double(typecast( reshape([data(FILE_HEADER+37:SAMPLE_SIZE:end), data(FILE_HEADER+38:SAMPLE_SIZE:end)]',1,[]), 'int16'));
    accHiG(:,3) = double(typecast( reshape([data(FILE_HEADER+39:SAMPLE_SIZE:end), data(FILE_HEADER+40:SAMPLE_SIZE:end)]',1,[]), 'int16'));
    accHiG      = accHiG / 20.5; % rescale to unit 'g'
    accHiG      = accHiG * 9.80665; % standard gravity (BIPM, 1901)
    
    % Magnetometer (from Ivensense MPU-9250)
    magn      = zeros(len,3);
    magn(:,1) = double(typecast( reshape([data(FILE_HEADER+41:SAMPLE_SIZE:end), data(FILE_HEADER+42:SAMPLE_SIZE:end)]',1,[]), 'int16'));
    magn(:,2) = double(typecast( reshape([data(FILE_HEADER+43:SAMPLE_SIZE:end), data(FILE_HEADER+44:SAMPLE_SIZE:end)]',1,[]), 'int16'));
    magn(:,3) = double(typecast( reshape([data(FILE_HEADER+45:SAMPLE_SIZE:end), data(FILE_HEADER+46:SAMPLE_SIZE:end)]',1,[]), 'int16'));
    magn      = magn / 0.6; % rescale to unit 'uT'
    
    % pressure (from TE Connectivity MS5611-01BA03)
    pressure = double(typecast( reshape([data(FILE_HEADER+47:SAMPLE_SIZE:end), data(FILE_HEADER+48:SAMPLE_SIZE:end), data(FILE_HEADER+49:SAMPLE_SIZE:end), data(FILE_HEADER+50:SAMPLE_SIZE:end)]',1,[]), 'int32'))';
    pressure = pressure / 100; % rescale to unit 'mbar'
    
    % temp (from TE Connectivity MS5611-01BA03)
    temp = double(typecast( reshape([data(FILE_HEADER+51:SAMPLE_SIZE:end), data(FILE_HEADER+52:SAMPLE_SIZE:end), data(FILE_HEADER+53:SAMPLE_SIZE:end), data(FILE_HEADER+54:SAMPLE_SIZE:end)]',1,[]), 'int32'))';
    temp = temp / 100; % rescale to unit '°C'
    
end
 
%% create data struct
data                             = struct;
data.ID                          = ID;
data.UID                         = UID;
data.name                        = name;
data.sw_version                  = sw_version;
data.hw_version                  = hw_version;
data.acc_range                   = acc_range;
data.gyro_range                  = gyro_range;
data.mpu_cal                     = mpu_cal;
data.act_thrs                    = act_thrs;
data.inact_dur                   = inact_dur;
data.mpu_rate                    = mpu_rate;
data.adxl_rate                   = adxl_rate;
data.pres_rate                   = pres_rate;
data.magn_en                     = magn_en;
data.sync                        = sync;
data.ble_address                 = ble_address;
 
if options.header_only
    return
end
 
data.tt                          = timetable(t,acc,gyro,accHiG,magn,pressure,temp);
data.tt.Properties.VariableNames = {'acc','gyr','hig','mag','prs','tmp'};
data.tt.Properties.VariableUnits = {'m/s^2','°/s','m/s^2','uT','mbar','°C'};
data.tt.t.Format                 = "dd.MM.uuuu HH:mm:ss.SSS";
data.t_start                     = data.tt.t(1);
 
% correct time jumps in the data
% there are binary files that have jumps in the full second timestamps. The
% subsecond timestamps, however, are correct. therefore this section corrects
% the full second timestamps to be continuous.
 
% time derivative
t_diff = diff(data.tt.t);
 
% check for jumps (a jump is defined as a difference of more than 1 second)
if any(abs(t_diff) > seconds(1))
    assert(options.time_jump_correction, 'Time jumps detected. Cannot correct. Set option time_jump_correction to true.')
    
    t_diff_old = t_diff;
    
    % split the difference into full seconds and subseconds
    t_diff_sec = round(seconds(t_diff));
    t_diff_subsec = seconds(t_diff) - t_diff_sec;
    
    % check iff there are jumps in the subseconds (less that 3 ms or more than 7 ms)
    assert(all(abs(t_diff_subsec) > 3e-3) && all(abs(t_diff_subsec) < 7e-3), 'Subsecond jumps detected. Cannot correct.')
    
    % check if the seconds difference sums up to zero
    assert(sum(t_diff_sec) == 0, 'Seconds difference does not sum up to zero. Cannot correct.')
    
    % create the correction time vector
    t_corr  = -cumsum(t_diff_sec);
    
    % correct the time
    data.tt.t(2:end) = data.tt.t(2:end) + seconds(t_corr);
    
    % check if the correction was successful
    assert(all(abs(diff(data.tt.t)) < seconds(1)), 'Time correction failed.')
    
    % plot the correction
    if options.plot_time_jump_correction
        figure
        yyaxis left
        plot(diff(data.tt.t), '.', DisplayName="Corrected time difference [s]")
        ylabel('Corrected time difference [s]')
        yyaxis right
        stem(t_diff_old, '.', MarkerSize=15, DisplayName="Original time difference [s]")
        ylabel('Time difference [s]')
        grid on;
        title('Time jump correction')
        legend(Location="best")
    end
    
end
 
% bring the timetable to regular sampling rate
if options.time_correction
    % correct the time
    data.tt = jumpCorrectData(data.tt, plot=options.plot_time_correction);
else
    % resample the data
    data.tt = retime(data.tt, 'regular', 'linear', 'TimeStep', seconds(1/data.mpu_rate));
    
    if options.plot_time_correction
        warning('Time correction is disabled. Cannot plot time correction.')
    end
end
 
% check output
[check, ~, data] = jumpCheckData(data);
 
if ~check
    error('Data check failed. Check the data struct.')
end
 
end