% Part 1: calculating the offset and sensitivity of all sensors

format none % Easier to read output without e-notation

mV_per_ADCUnit = 3000 / 2^16;

% This file contains the accelerometer sensitivity measurements
% Row 4: X-axis, Row 5: Y-axis, Row 6: Z-axis
% Columns 100:1100 with Z-axis pointing up
% Columns 1600:2600 with Z-axis pointing down
load sensitivity.mat;
disp('Accelerometer sensitivity');

% For getting the Z-channel offset, it is important that the
% we have equal number of measurements from up and down orientations.
z_up = Data(:,100:1100);
z_down = Data(:,1600:2600);
both = [z_up z_down];

% Averages = offsets
X_offset = mean(both(4, :))
Y_offset = mean(both(5, :))
Z_offset = mean(both(6, :))

X_offset_mV = X_offset * mV_per_ADCUnit
Y_offset_mV = Y_offset * mV_per_ADCUnit
Z_offset_mV = Z_offset * mV_per_ADCUnit

% Sensitivity we get from the Z-channel
% The difference of the up and down orientations equals 2 g acceleration
up_avg = mean(z_up(6, :))
down_avg = mean(z_down(6, :))
Z_sensitivity_ADCUnit_per_g = (up_avg - down_avg) / 2
Z_sensitivity_mV_per_g = Z_sensitivity_ADCUnit_per_g * mV_per_ADCUnit

% This file is taken when the sensor is still, so we can get the gyro offsets here.
% Rows 7,8,9: Z, X, Y gyros
% Columns 10000:30000: stable period
load noise.mat;
disp('');
disp('Gyroscope offsets');

Z_gyro_offset = mean(Data(7, 10000:30000))
X_gyro_offset = mean(Data(8, 10000:30000))
Y_gyro_offset = mean(Data(9, 10000:30000))

Z_gyro_offset_mV = Z_gyro_offset * mV_per_ADCUnit
X_gyro_offset_mV = X_gyro_offset * mV_per_ADCUnit
Y_gyro_offset_mV = Y_gyro_offset * mV_per_ADCUnit

% This file contains the the gyroscope sensitivity measurement from rotating
% clockwise.
% Row 4: X-axis gyro data
% Columns 3000:5000: sensor stationary before measurement
% Columns 18000:25000: sensor rotating clockwise
% Columns 41000:43000: sensor stationary after measurement
load gyro_sensitivity.mat;
disp('');
disp('Gyroscope sensitivity, clockwise');

gyro_offset_before = mean(Data(4, 3000:5000))
gyro_during = mean(Data(4, 18000:25000))
gyro_offset_after = mean(Data(4, 41000:43000))

% The time of 60 rotations was measured as 49.5s and 49.7s (two measurements)
rot_speed_rounds_per_sec = ((60 / 49.5) + (60 / 49.7)) / 2;
rot_speed_degs_per_sec = rot_speed_rounds_per_sec * 360
gyro_sensitivity_ADCUnit_per_degsec = (gyro_during - gyro_offset_after) / rot_speed_degs_per_sec
gyro_sensitivity_mV_per_degsec = gyro_sensitivity_ADCUnit_per_degsec * mV_per_ADCUnit

% Similarly, for counterclockwise rotation
% Row 4: X-axis gyro data
% Columns 1000:2000: sensor stationary before measurement
% Columns 40000:60000: sensor rotating counterclockwise
% Columns 100000:102000: sensor stationary after measurement
load gyro_sensitivity_counterclockwise;
disp('');
disp('Gyroscope sensitivity, counterclockwise');

gyro2_offset_before = mean(Data(4, 1000:2000))
gyro2_during = mean(Data(4, 40000:60000))
gyro2_offset_after = mean(Data(4, 100000:102000))

% The time of 60 rotations was measured as 44.4s and 44.8s (two measurements)
rot2_speed_rounds_per_sec = -((60 / 44.4) + (60 / 44.8)) / 2;
rot2_speed_degs_per_sec = rot2_speed_rounds_per_sec * 360
gyro2_sensitivity_ADCUnit_per_degsec = (gyro2_during - gyro2_offset_after) / rot2_speed_degs_per_sec
gyro2_sensitivity_mV_per_degsec = gyro2_sensitivity_ADCUnit_per_degsec * mV_per_ADCUnit


