CALIBRATION.cpp

00001 #include <stdint.h>
00002 #include <stdio.h>
00003 #include <math.h>
00004 
00005 #include "mbed.h"
00006 #include "CALIBRATION.h"
00007 
00008 
00009 #define PI 3.14159265
00010 
00011 CALIBRATION::calib_data_t CALIBRATION::convert_2p_all(float angle[1], float meas[1][2]){
00012             // angle = | angle1 angle2| in RAD - 1/sqrt(3)*g 
00013             // meas =  | ax1    ay1     az1 |
00014             //         | ax2    ay2     az2 |
00015     calib_data_t  res;
00016     angle[1]=sin(angle[1]);
00017     angle[0]=sin(angle[0]);
00018     for(int i=0; i<3; i++){ 
00019         res.S[i]= (angle[1]-angle[0])/(meas[i][1]-meas[i][0]);
00020         res.B[i]= (angle[0]*meas[i][1]-angle[1]*meas[i][0])/(meas[i][1]-meas[i][0]);
00021     }
00022     return res;
00023 }
00024 
00025 void CALIBRATION::convert_4p(float meas[3], int axis){
00026             // https://www.eetimes.com/document.asp?doc_id=1274067&page_number=3
00027             // for each axis
00028             // angle = | anglex1-0° anglex2-90° angley3-180° angley4-270°|
00029             // meas =  | x1     x2      x3      x4 |
00030     float m, b;
00031     m = (meas[1]-meas[3])/2;
00032     b = (meas[0]-meas[3])/2;
00033     adxl355_sensitivity.S[axis] = adxl355_sensitivity.S[axis]/m;
00034     adxl355_sensitivity.B[axis] = adxl355_sensitivity.B[axis]-b;
00035 }  
00036          
00037 void CALIBRATION::convert_2p(float meas[1], int axis){
00038             // angle = | anglex1-0° anglex2-180°| = | anglex1@1g anglex2@-1g| in DEGREE
00039             // meas =  | x1     x2|
00040     float m, b;
00041     m = 2/(meas[0]-meas[1]);
00042     b = -(meas[0]+meas[1])/(meas[0]-meas[1]);
00043     adxl355_sensitivity.S[axis] = adxl355_sensitivity.S[axis]/m;
00044     adxl355_sensitivity.B[axis] = adxl355_sensitivity.B[axis]-b;
00045 }
00046 
00047 //CALIBRATION::calib_data_t CALIBRATION::convert_12p(float angle[11][2], float meas[11][2], int size){}
00048 
00049 
00050 /*For calculating Determinant of the Matrix */
00051 
00052 void CALIBRATION::matrix_reset(){
00053     // default values 
00054     adxl355_sensitivity.S[0] = 3.9e-6;
00055     adxl355_sensitivity.S[1] = 3.9e-6;
00056     adxl355_sensitivity.S[2] = 3.9e-6;
00057     adxl355_sensitivity.B[0] = 0;
00058     adxl355_sensitivity.B[1] = 0;
00059     adxl355_sensitivity.B[2] = 0;
00060     adxl355_sensitivity.St[0] = 0.02e-6;
00061     adxl355_sensitivity.St[1] = 0.02e-6;
00062     adxl355_sensitivity.St[2] = 0.02e-6;
00063     }
00064 
00065 
00066 
00067 //https://www.sanfoundry.com/c-program-find-inverse-matrix/
00068 
00069 //http://www.ccodechamp.com/c-program-to-find-inverse-of-matrix/