Diff: Calibration/CALIBRATION.cpp

Revision:

9:6c803986dbde

Parent:

8:9e6ead2ee8d7

--- a/Calibration/CALIBRATION.cpp	Tue Aug 21 13:25:37 2018 +0000
+++ b/Calibration/CALIBRATION.cpp	Mon Sep 03 10:39:56 2018 +0000
@@ -8,122 +8,62 @@
 
 #define PI 3.14159265
 
-CALIBRATION::calib_data_t CALIBRATION::convert_2p(float angle[11][2], float meas[11][2]){
+CALIBRATION::calib_data_t CALIBRATION::convert_2p_all(float angle[1], float meas[1][2]){
+            // angle = | angle1 angle2| in RAD - 1/sqrt(3)*g 
+            // meas =  | ax1    ay1     az1 |
+            //         | ax2    ay2     az2 |
     calib_data_t  res;
+    angle[1]=sin(angle[1]);
+    angle[0]=sin(angle[0]);
     for(int i=0; i<3; i++){ 
-        res.S[i][i]= (angle[i][1]-angle[i][0])/(meas[i][1]-meas[i][0]);
-        res.B[i][0]= (angle[i][0]*meas[i][1]-angle[i][1]*meas[i][0])/(meas[i][1]-meas[i][0]);
+        res.S[i]= (angle[1]-angle[0])/(meas[i][1]-meas[i][0]);
+        res.B[i]= (angle[0]*meas[i][1]-angle[1]*meas[i][0])/(meas[i][1]-meas[i][0]);
     }
     return res;
 }
-CALIBRATION::calib_data_t CALIBRATION::convert_6p(float angle[11][2], float meas[11][2]){
-    calib_data_t  res;
-    for(int i=0; i<3; i++){ 
-        res.S[i][i]= (angle[i][1]-angle[i][0])/(meas[i][1]-meas[i][0]);
-        res.B[i][0]= (angle[i][0]*meas[i][1]-angle[i][1]*meas[i][0])/(meas[i][1]-meas[i][0]);
-    }
-    return res;
+
+void CALIBRATION::convert_4p(float meas[3], int axis){
+            // https://www.eetimes.com/document.asp?doc_id=1274067&page_number=3
+            // for each axis
+            // angle = | anglex1-0° anglex2-90° angley3-180° angley4-270°|
+            // meas =  | x1     x2      x3      x4 |
+    float m, b;
+    m = (meas[1]-meas[3])/2;
+    b = (meas[0]-meas[3])/2;
+    adxl355_sensitivity.S[axis] = adxl355_sensitivity.S[axis]/m;
+    adxl355_sensitivity.B[axis] = adxl355_sensitivity.B[axis]-b;
+}  
+         
+void CALIBRATION::convert_2p(float meas[1], int axis){
+            // angle = | anglex1-0° anglex2-180°| = | anglex1@1g anglex2@-1g| in DEGREE
+            // meas =  | x1     x2|
+    float m, b;
+    m = 2/(meas[0]-meas[1]);
+    b = -(meas[0]+meas[1])/(meas[0]-meas[1]);
+    adxl355_sensitivity.S[axis] = adxl355_sensitivity.S[axis]/m;
+    adxl355_sensitivity.B[axis] = adxl355_sensitivity.B[axis]-b;
 }
+
 //CALIBRATION::calib_data_t CALIBRATION::convert_12p(float angle[11][2], float meas[11][2], int size){}
 
 
 /*For calculating Determinant of the Matrix */
 
 void CALIBRATION::matrix_reset(){
-    memset(g_matrix, 0, sizeof(g_matrix[0][0]) * 2 * 11);
-    memset(coeff_matrix.x_matrix, 0, sizeof(coeff_matrix.x_matrix[0][0]) * 11 * 3);  
-    memset(w_matrix, 0, sizeof(w_matrix[0][0]) * 3 * 2);
+    // default values 
+    adxl355_sensitivity.S[0] = 3.9e-6;
+    adxl355_sensitivity.S[1] = 3.9e-6;
+    adxl355_sensitivity.S[2] = 3.9e-6;
+    adxl355_sensitivity.B[0] = 0;
+    adxl355_sensitivity.B[1] = 0;
+    adxl355_sensitivity.B[2] = 0;
+    adxl355_sensitivity.St[0] = 0.02e-6;
+    adxl355_sensitivity.St[1] = 0.02e-6;
+    adxl355_sensitivity.St[2] = 0.02e-6;
     }
 
 
-void CALIBRATION::matrix_g(float angle[4][12]){
-    // angle expressed in degree: roll, pitch, yaw
-    double roll, pitch;
-    int i;
-    for ( i = 0; i < 12; ++i) {
-        roll = angle[0][i]*PI/180;
-        pitch = angle[1][i]*PI/180;
-        g_matrix[0][i] = -sin(pitch);
-        g_matrix[1][i] = cos(pitch)*sin(roll);
-        g_matrix[2][i] = cos(pitch)*cos(roll);
-        }
-   }
 
-void CALIBRATION::matrix_x(float meas[3][12]){
-    int i,j,k;
-    // define xt and x
-    for( i = 0; i < 12; i++){
-        coeff_matrix.x_matrix[i][0] = meas[0][i];
-        coeff_matrix.x_matrix[i][1] = meas[1][i];
-        coeff_matrix.x_matrix[i][2] = meas[2][i];
-        coeff_matrix.x_matrix[i][3] = 1;
-        coeff_matrix.x_transpose[0][i] = meas[0][i];
-        coeff_matrix.x_transpose[1][i] = meas[1][i];
-        coeff_matrix.x_transpose[2][i] = meas[2][i];
-        coeff_matrix.x_transpose[3][i] = 1;
-    }
-    // product xt * x
-    double sum = 0;
-    for ( i=0; i< 12; i++){
-        for( j=0; j<3; j++){
-            sum = sum + coeff_matrix.x_transpose[i][j]+coeff_matrix.x_matrix[i][j];
-            }
-        coeff_matrix.xtx_product[i][j] = sum;
-        sum = 0;
-        }
-    // determinte  of xt*t
-    float det;
-    det = matrix_determinant(coeff_matrix.xtx_product,12);
-    // cofactor of ct*x
-    float b[12][12],fac[12][12];
-    int p,q;
-    int m,n;
-    for (q=0;q<12;q++){
-        for (p=0;p<12;p++){
-            m=0; n=0;
-            for (i=0;i<12;i++){
-                for (j=0;j<12;j++){
-                    if (i != q && j != p){
-                        b[m][n]=coeff_matrix.xtx_product[i][j];
-                        if (n<(12-2))
-                            n++;
-                        else{
-                            n=0; m++;}
-                    }
-                }
-            }
-            //fac[q][p] = pow(-1,p+q)* matrix_determinant(b,12-1);
-        }
-    }
-}
-
-float CALIBRATION::matrix_determinant(float a[12][12], float k){
-  float s = 1, det = 0, b[12][12];
-  int i, j, m, n, c;
-  if (k == 1) { return (a[0][0]);}
-  else{
-     det = 0;
-     for (c = 0; c < k; c++){
-        m = 0; n = 0;
-        for (i = 0;i < k; i++){
-            for (j = 0 ;j < k; j++){
-                b[i][j] = 0;
-                if (i != 0 && j != c){
-                   b[m][n] = a[i][j];
-                   if (n < (k - 2))
-                    n++;
-                   else
-                    {n = 0; m++;}
-                   }
-               }
-             }
-          det = det + s * (a[0][c] * matrix_determinant(b, k - 1));
-          s = -1 * s;
-          }
-    }
-    return (det);
-}
- 
 //https://www.sanfoundry.com/c-program-find-inverse-matrix/
 
 //http://www.ccodechamp.com/c-program-to-find-inverse-of-matrix/
\ No newline at end of file