% Calibration routine by Andrea and Pasquale Cirillo www.aepwebmasters.it
% Adapted to FreeIMU library by Fabio Varesano www.varesano.net
% Released under GPL v3 - See See http://www.gnu.org/copyleft/gpl.html

function SpherePlot(X, Y, Z, graph_title, filename)
  figure;
  subplot(2,2,1); plot3(X,Y,Z);
  title(graph_title);
  subplot(2,2,2); plot(X, Y);
  title('Vector projection on XY');
  subplot(2,2,3); plot(Y,Z);
  title('Vector projection on YZ');
  subplot(2,2,4); plot(Z,X);
  title('Vector projection on ZX');
  print(filename) % .pdf graphs generated
endfunction


function [OSx, OSy, OSz, SCx, SCy, SCz] = Ellipsoid_to_Sphere(data, graph_filename)
  
  SpherePlot(data(:,1), data(:,2), data(:,3), 'Uncalibrated data', strcat(graph_filename, '_uncalibrated.pdf'));

  %X=pinv([data(:,1) data(:,2) data(:,3) -data(:,2).^2 -data(:,3).^2 ones(size(data,1),1)])*(data(:,1).^2);
  %[data(:,2) -data(:,2).^2 -(data(:,2).^2)]
  H = [data(:,1) data(:,2) data(:, 3) -data(:,2).^2 -data(:,3).^2 ones(size(data,1),1)];
  w = data(:,1).^2;
  X = H \ w;
  OSx=X(1)/2;
  OSy=X(2)/(2*X(4));
  OSz=X(3)/(2*X(5));
  A=X(6)+OSx^2+X(4)*OSy^2+X(5)*OSz^2;
  B=A/X(4);
  C=A/X(5);
  SCx=sqrt(A);
  SCy=sqrt(B);
  SCz=sqrt(C);

  xx=data(:,1)-OSx;
  yy=data(:,2)-OSy;
  zz=data(:,3)-OSz;

  xxx=xx./SCx;
  yyy=yy./SCy;
  zzz=zz./SCz;

  SpherePlot(xxx, yyy, zzz, 'Calibrated data', strcat(graph_filename, '_calibrated.pdf'));
  
endfunction




data = dlmread('acc.txt', ' ');
[A_OSx, A_OSy, A_OSz, A_SCx, A_SCy, A_SCz] = Ellipsoid_to_Sphere(data, 'acc');

data = dlmread('magn.txt', ' ');
[M_OSx, M_OSy, M_OSz, M_SCx, M_SCy, M_SCz] = Ellipsoid_to_Sphere(data, 'magn');


%storing values to calibration header
calfile = fopen('calibration.h', 'w');

fprintf(calfile, '/**\n * FreeIMU calibration header. Automatically generated by octave AccMagnCalib.m.\n * Do not edit manually unless you know what you are doing.\n*/\n\n');

fprintf(calfile, 'const int acc_off_x = %d;\n', A_OSx);
fprintf(calfile, 'const int acc_off_y = %d;\n', A_OSy);
fprintf(calfile, 'const int acc_off_z = %d;\n', A_OSz);
fprintf(calfile, 'const float acc_scale_x = %f;\n', A_SCx);
fprintf(calfile, 'const float acc_scale_y = %f;\n', A_SCy);
fprintf(calfile, 'const float acc_scale_z = %f;\n', A_SCz);

fprintf(calfile, '\n\n');

fprintf(calfile, 'const int magn_off_x = %d;\n', M_OSx);
fprintf(calfile, 'const int magn_off_y = %d;\n', M_OSy);
fprintf(calfile, 'const int magn_off_z = %d;\n', M_OSz);
fprintf(calfile, 'const float magn_scale_x = %f;\n', M_SCx);
fprintf(calfile, 'const float magn_scale_y = %f;\n', M_SCy);
fprintf(calfile, 'const float magn_scale_z = %f;\n', M_SCz);

fclose(calfile);

%pause();