Revision 8:9e6ead2ee8d7, committed 2018-08-21

Comitter:

vtoffoli

Date:

Tue Aug 21 13:25:37 2018 +0000

Parent:

7:5aaa09c40283

Child:

9:6c803986dbde

Commit message:

COG+ADXL355

Changed in this revision

--- a/ADXL35x/ADXL355.cpp	Tue Aug 14 11:33:30 2018 +0000
+++ b/ADXL35x/ADXL355.cpp	Tue Aug 21 13:25:37 2018 +0000
@@ -16,9 +16,6 @@
     adxl355.lock();
     axis355_sens = 3.9e-6;
     axis357_sens = 19.5e-6;
-    calib_data.Sxx = 3.9e-6;
-    calib_data.Syy = 3.9e-6;
-    calib_data.Szz = 3.9e-6;
 }
 void ADXL355::frequency(int hz)
 {
@@ -223,8 +220,7 @@
     ret_val |= static_cast<uint64_t>(y) << 24;
     ret_val |= static_cast<uint64_t>(z) ;
     return ret_val;
-    }
-
+}
 /** ----------------------------------- */
 /** CALIBRATION AND CONVERSION          */
 /** ----------------------------------- */    
@@ -237,4 +233,4 @@
     //uint32_t rawValue = data<<(32-nbit);
     // Otherwise perform the 2's complement math on the value
     return float((~(data - 0x01)) & 0xfffff) * -1;
-    }
\ No newline at end of file
+}
\ No newline at end of file

--- a/ADXL35x/ADXL355.h	Tue Aug 14 11:33:30 2018 +0000
+++ b/ADXL35x/ADXL355.h	Tue Aug 21 13:25:37 2018 +0000
@@ -7,30 +7,10 @@
 public: 
     // -------------------------- //
     // CONST AND VARIABLES        // 
-    // -------------------------- //
-    typedef struct {
-        // sensitivity
-        float Sxx;  
-        float Sxy;
-        float Sxz;
-        float Syx;
-        float Syy;
-        float Syz;
-        float Szx;
-        float Szy;
-        float Szz;
-        float St;
-        // 0g offset
-        float Bx;
-        float By;
-        float Bz;
-        float Bt;
-    } ADXL355_calib_t;   
     const static float t_sens = -9.05;    
     const static float t_bias = 1852;    
     float axis355_sens;
-    float axis357_sens;
-    ADXL355_calib_t calib_data;      
+    float axis357_sens;     
     // -------------------------- //
     // REGISTERS                  // 
     // -------------------------- //
@@ -181,11 +161,8 @@
     void fifo_setup(uint8_t nr_of_entries);
     uint32_t fifo_read_u32();
     uint64_t fifo_scan();
-    // ADXL calibration
+    // ADXL conversion
     float convert(uint32_t data);
-    ADXL355_calib_t convert_2p();
-    ADXL355_calib_t convert_3to8p();
-    ADXL355_calib_t convert_12p();
     
 private:
     // SPI adxl355;                 ///< SPI instance of the ADXL

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/CALIBRATION.cpp	Tue Aug 21 13:25:37 2018 +0000
@@ -0,0 +1,129 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "mbed.h"
+#include "CALIBRATION.h"
+
+
+#define PI 3.14159265
+
+CALIBRATION::calib_data_t CALIBRATION::convert_2p(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;
+}
+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;
+}
+//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);
+    }
+
+
+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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/CALIBRATION.h	Tue Aug 21 13:25:37 2018 +0000
@@ -0,0 +1,43 @@
+
+#ifndef CALIBRATION_H_
+#define CALIBRATION_H_
+
+class CALIBRATION
+{
+public: 
+    // -------------------------- //
+    // CONST AND VARIABLES        // 
+    // -------------------------- //
+    typedef struct {
+        // sensitivity
+        float S[2][2];
+        float St;
+        // 0g offset
+        float B[2][0];
+    } calib_data_t;
+    typedef struct {
+        float x_matrix[12][4];
+        float x_transpose[4][12];
+        float xtx_product[12][12];
+    } matrix_data_t;
+    float g_matrix[3][12]; 
+    matrix_data_t coeff_matrix;  
+    float w_matrix[4][3];     
+    // -------------------------- //
+    // FUNCTIONS                  //  
+    // -------------------------- //
+    
+    calib_data_t convert_2p(float angle[11][2], float meas[11][2]);
+    calib_data_t convert_6p(float angle[11][2], float meas[11][2]);
+    calib_data_t convert_12p(float angle[11][2], float meas[11][2]);
+    
+    float matrix_determinant(float a[12][12], float k);
+    float matrix_cofactor(float a[12][2], float k);
+    
+    void matrix_reset();
+    void matrix_g(float angle[4][12]);    // angle expressed in degree!
+    void matrix_x(float meas[3][12]);
+private:
+};
+ 
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/READMEcalibration.md	Tue Aug 21 13:25:37 2018 +0000
@@ -0,0 +1,27 @@
+/* -----------------------------------------------------------------------------
+/* ADXL calibration equation
+/* -----------------------------------------------------------------------------
+
+|acx|   |pxx pxy pxz| |ax| |bx|
+|acy| = |pyx pyy pyz|*|ay|+|by|
+|acz|   |pzx pzy pzz| |az| |bz|
+
+Rotation matrix
+z axis pointing down -  downwards pointing gravity vector of magnitude 1 g
+The yaw rotation is about the positive z axis, the pitch rotation is about the
+positive y axis and the roll rotation is about the positive x axis
+
+/* -----------------------------------------------------------------------------
+/* ADXL calibration procedures
+/* -----------------------------------------------------------------------------
+
+2 MEASUREMENT ORIENTATIONS: the orientations are selected to give an equal 
+posit5ive gravitational filed of g/sqrt(3) in each axis followed by -g/sqrt(3) 
+in each axis
+We assumed that pij=0 when i!=j
+
+
+3to8 MEASUREMENT ORIENTATIONS
+
+
+12 MEASUREMENT ORIENTATIONS
\ No newline at end of file

--- a/READMEcalibration.md	Tue Aug 14 11:33:30 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-/* -----------------------------------------------------------------------------
-/* ADXL calibration equation
-/* -----------------------------------------------------------------------------
-
-|acx|   |pxx pxy pxz| |ax| |bx|
-|acy| = |pyx pyy pyz|*|ay|+|by|
-|acz|   |pzx pzy pzz| |az| |bz|
-
-/* -----------------------------------------------------------------------------
-/* ADXL calibration procedures
-/* -----------------------------------------------------------------------------
-
-2 MEASUREMENT ORIENTATIONS: the orientations are selected to give an equal 
-posit5ive gravitational filed of g/sqrt(3) in each axis followed by -g/sqrt(3) 
-in each axis
-We assumed that pij=0 when i!=j
-
-
-3to8 MEASUREMENT ORIENTATIONS
-
-
-12 MEASUREMENT ORIENTATIONS
\ No newline at end of file

--- a/main.cpp	Tue Aug 14 11:33:30 2018 +0000
+++ b/main.cpp	Tue Aug 21 13:25:37 2018 +0000
@@ -1,14 +1,21 @@
 #include "mbed.h"
+#include <math.h>
 #include <inttypes.h>
 #include "ADXL355.h"
 #include "ADXRS290.h"
+#include "CALIBRATION.h"
  
 Serial pc(USBTX, USBRX);
  
 ADXL355 accl(SPI1_CS0, SPI1_MOSI, SPI1_MISO, SPI1_SCLK);    // PMOD port
 ADXRS290 gyro(SPI0_CS2, SPI0_MOSI, SPI0_MISO, SPI0_SCLK);   // PMOD port
- 
-    
+CALIBRATION test;
+float angle[3][12] = {  {-55, -125, -147, 33, -128, 52, 0, 0, 0, 0, 0, 0},
+                        {6, -6, 20, -20, -69, 69, 0, 0, 0, 0, 0, 0 }, 
+                        {1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0}}; 
+    float meas[3][12] =  {  {-0.09, 0.16,  -0.31,  0.37,  0.91,  -0.88},
+                            {-0.78, 0.81,  -0.46,  0.49,  -0.28,  0.32},
+                            {0.54,  -0.55, -0.78,  0.76,  -0.20,  0.20}};   
 int main(){
     pc.baud(9600);
     pc.printf("SPI ADXL355 and ADXL357 Demo\n");
@@ -30,13 +37,29 @@
     float x, y,z;
     float t;
     // save data info a file
-    while(1000) {
+    for(int i=0; i<100; i++) {
         x = accl.convert(accl.scanx())*accl.axis355_sens;
         y = accl.convert(accl.scany())*accl.axis355_sens;
         z = accl.convert(accl.scanz())*accl.axis355_sens;
         t = 25+float(accl.scant()-1852)/(-9.05);
         pc.printf("%f \t %f \t %f  \t %f \r\n" , x,y,z,t);
-        wait(0.1);
-  }
+        wait(0.1);}    // test the calibration procedure
+   // test.matrix_reset();
+   // test.matrix_g(angle);
+   // test.matrix_x(meas);
+   // int i, j;
+ 
+   /* output each array element's value */
+   /*
+    pc.printf("power control on measurement mode = %x \r\n",d);
+   for ( i = 0; i < 3; i++ ) {
+         for ( j = 0; j < 12; j++ ) {
+             pc.printf( "%f\n", i,j, test.g_matrix[i][j] );
+      }
+    pc.printf("\r\n");
+     */ 
+      
+   //}
+    
 }
  
\ No newline at end of file