Revision 9:6c803986dbde, committed 2018-09-03

Comitter:

vtoffoli

Date:

Mon Sep 03 10:39:56 2018 +0000

Parent:

8:9e6ead2ee8d7

Child:

10:f5ba762b58b4

Commit message:

update 2018_09_03;

Changed in this revision

--- a/ADXRS290/ADXRS290.cpp	Tue Aug 21 13:25:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,73 +0,0 @@
-#include <stdint.h>
-#include "mbed.h"
-#include "ADXRS290.h"
-
-/** ----------------------------------- */
-/** SPI - MAX 5MHZ                      */
-/** ----------------------------------- */
-ADXRS290::ADXRS290(PinName cs_pin, PinName MOSI, PinName MISO, PinName SCK): adxrs290(MOSI, MISO, SCK), cs(cs_pin)
-{
-    cs = 1;
-    adxrs290.format(8,_SPI_MODE);
-    adxrs290.lock();
-    gyro_sens = 5e-3;
-    t_sens = 0.1;
-}
-void ADXRS290::frequency(int hz)
-{
-    adxrs290.frequency(hz);
-}
-
-/** ----------------------------------- */
-/** Writes the reg register with data   */
-/** ----------------------------------- */
-void ADXRS290::write_reg(ADXRS290_register_t reg, uint8_t data){
-    adxrs290.format(8, _SPI_MODE);
-    cs = false;
-    adxrs290.write(static_cast<uint8_t>(reg) | _WRITE_REG_CMD);
-    adxrs290.write(data);
-    cs = true;
-}
-/** ----------------------------------- */
-/** Reads the reg register              */
-/** ----------------------------------- */
-uint8_t ADXRS290::read_reg(ADXRS290_register_t reg){
-    uint8_t ret_val;
-    adxrs290.format(8, _SPI_MODE);
-    cs = false;
-    adxrs290.write(static_cast<uint8_t>(reg) | _READ_REG_CMD);
-    ret_val = adxrs290.write(_DUMMY_BYTE);
-    cs = true;
-    return ret_val;
-}
-uint16_t ADXRS290::read_reg_u16(ADXRS290_register_t reg){
-    uint16_t ret_val = 0;
-    adxrs290.format(8, _SPI_MODE);
-    cs = false;
-    adxrs290.write(static_cast<uint8_t>(reg) | _READ_REG_CMD);
-    ret_val = adxrs290.write(_DUMMY_BYTE);
-    ret_val = (ret_val) | (adxrs290.write(_DUMMY_BYTE)<<8);
-    cs = true;
-    return ret_val;
-}
-/** ----------------------------------- */
-/** Sets the CTL registers              */
-/** ----------------------------------- */    
-void ADXRS290::set_power_ctl_reg(uint8_t data){
-    write_reg(POWER_CTL, data);
-    wait(0.1);
-}
-void ADXRS290::set_filter_ctl_reg(ADXRS290_filter_ctl_t hpf, ADXRS290_filter_ctl_t lpf){
-    write_reg(FILTER, hpf|lpf);
-}
-void ADXRS290::set_sync(ADXRS290_dataready_ctl_t data){}
-    
-uint16_t ADXRS290::scanx(){    
-    return read_reg_u16(DATAX0);
-}
-uint16_t ADXRS290::scany(){
-    return read_reg_u16(DATAY0);
-}
-uint16_t ADXRS290::scant(){
-    return read_reg_u16(TEMP0);
-}    
\ No newline at end of file

--- a/ADXRS290/ADXRS290.h	Tue Aug 21 13:25:37 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,106 +0,0 @@
-
-#ifndef ADXRS290_H_
-#define ADXRS290_H_
-
-class ADXRS290
-{
-public: 
-    float gyro_sens;  
-    float t_sens;  
-    // -------------------------- //
-    // REGISTERS                  // 
-    // -------------------------- //
-    typedef enum {
-        DEVID_AD = 0x00,
-        DEVID_MST = 0x01,
-        PARTID = 0x02,
-        REVID = 0x03,
-        SN0 = 0x04,
-        SN1 = 0x05,
-        SN2 = 0x06,
-        SN3 = 0x07,
-        DATAX0 = 0x08,
-        DATAX1 = 0x09,
-        DATAY0 = 0x0A,
-        DATAY1 = 0x0B,
-        TEMP0 = 0x0C,
-        TEMP1 = 0x0D,
-        POWER_CTL = 0x10,
-        FILTER = 0x11,
-        DATA_READY = 0x12
-    } ADXRS290_register_t;
-    // -------------------------- //
-    // REGISTERS - DEFAULT VALUES //
-    // -------------------------- //
-    // Modes - POWER_CTL  
-    typedef enum {
-        TEMP_ON = 0x00,
-        TEMP_OFF = 0x01,
-        STANDBY = 0x00,
-        MEASUREMENT = 0x02
-    } ADXL355_modes_t;    
-    // High-Pass and Low-Pass Filter - FILTER 
-    typedef enum {
-        LPF480 = 0x00,
-        LPF320 = 0x01,
-        LPF160 = 0x02,
-        LPF80 = 0x03,
-        LPF56 = 0x04,
-        LPF40 = 0x05,
-        LPF28 = 0x06,
-        LPF20 = 0x07,
-        HPFOFF = 0x00,
-        HPF001 = 0x10,
-        HPF002 = 0x20,
-        HPF004 = 0x30,
-        HPF008 = 0x40,
-        HPF017 = 0x50,
-        HPF035 = 0x60,
-        HPF070 = 0x70,
-        HPF140 = 0x80,
-        HPF280 = 0x90,
-        HPF1130 = 0xA0
-    } ADXRS290_filter_ctl_t;
-    // External timing register - INT_MAP 
-    typedef enum {
-        OVR_EN = 0x04,
-        FULL_EN = 0x02,
-        RDY_EN = 0x01
-    } ADXRS290_intmap_ctl_t;
-    // External timing register - SYNC 
-    typedef enum {
-        ANAL_SYNC = 0x00,
-        DIGI_SYNC = 0x01
-    } ADXRS290_dataready_ctl_t;
-    
-    // -------------------------- //
-    // FUNCTIONS                  //  
-    // -------------------------- //
-    // SPI configuration & constructor 
-    ADXRS290(PinName cs_pin , PinName MOSI , PinName MISO , PinName SCK );
-    void frequency(int hz);
-    // SPI configuration & constructor 
-    void write_reg(ADXRS290_register_t reg, uint8_t data);
-    uint8_t read_reg(ADXRS290_register_t reg);
-    uint16_t read_reg_u16(ADXRS290_register_t reg);
-    // ADXRS general register R/W methods 
-    void set_power_ctl_reg(uint8_t data);
-    void set_filter_ctl_reg(ADXRS290_filter_ctl_t hpf, ADXRS290_filter_ctl_t odr);
-    void set_sync(ADXRS290_dataready_ctl_t data);
-    // ADXRS X/Y/T scanning methods   
-    uint16_t scanx();
-    uint16_t scany();
-    uint16_t scant();
-    // ADXRS tilt methods and calibration
-    // TBD
-private:
-    // SPI adxl355;                 ///< SPI instance of the ADXL
-    SPI adxrs290; DigitalOut cs;
-    const static uint8_t _DEVICE_AD = 0x92;     // contect of DEVID_AD (only-read) register 
-    const static uint8_t _DUMMY_BYTE = 0xAA;    // 10101010
-    const static uint8_t _WRITE_REG_CMD = 0x00; // write register
-    const static uint8_t _READ_REG_CMD = 0x80;  // read register
-    const static uint8_t _SPI_MODE = 4;         // timing scheme
-};
- 
-#endif
\ No newline at end of file

--- 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

--- a/Calibration/CALIBRATION.h	Tue Aug 21 13:25:37 2018 +0000
+++ b/Calibration/CALIBRATION.h	Mon Sep 03 10:39:56 2018 +0000
@@ -9,34 +9,33 @@
     // CONST AND VARIABLES        // 
     // -------------------------- //
     typedef struct {
-        // sensitivity
-        float S[2][2];
-        float St;
+        // sensitivity - we do not consider cross sensitivity
+        float S[3]; 
+        // temperature sensitivity 
+        float St[3];
         // 0g offset
-        float B[2][0];
+        float B[3];
     } 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];     
+    calib_data_t  adxl355_sensitivity;
+    calib_data_t  adxl357_sensitivity;
     // -------------------------- //
     // 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]);
+    calib_data_t convert_2p_all(float angle[1], float meas[1][2]);
+            // angle = | angle1 angle2| in RAD
+            // meas =  | ax1    ay1     az1 |
+            //         | ax2    ay2     az2 |
+    void convert_2p(float meas[1], int axis);
+            // angle = | angle1 angle2| in RAD
+            // meas =  | x1     x2 |
+    void convert_4p(float meas[3], int axis);
+            // for each axis
+            // angle = | anglex1 anglex2 angley3 angley4|
+            // meas =  | x1     x2      x3      x4 |
     
-    float matrix_determinant(float a[12][12], float k);
-    float matrix_cofactor(float a[12][2], float k);
-    
+    float matrix_determinant(float a[12][12], float k);    
     void matrix_reset();
-    void matrix_g(float angle[4][12]);    // angle expressed in degree!
-    void matrix_x(float meas[3][12]);
 private:
 };
  

--- a/main.cpp	Tue Aug 21 13:25:37 2018 +0000
+++ b/main.cpp	Mon Sep 03 10:39:56 2018 +0000
@@ -2,18 +2,17 @@
 #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;
+#define pi 3.14159265;
 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},
+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(){
@@ -34,32 +33,137 @@
     accl.set_power_ctl_reg(accl.MEASUREMENT);
     d=accl.read_reg(accl.POWER_CTL);
     pc.printf("power control on measurement mode = %x \r\n",d);
-    float x, y,z;
+    float x, y, z, w, k, h;
     float t;
     // save data info a file
+    pc.printf("x \t y \t z \t t\r\n");
     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);}    // 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");
-     */ 
-      
-   //}
-    
+        pc.printf("%f \t %f \t %f  \t %f   \r\n" , x,y,z,t);
+        wait(0.1);}    
+   // test the calibration procedure
+   test.matrix_reset();
+   pc.printf("Start calibration test (y/n)?\r\n");
+   char calib = pc.getc();
+   switch(calib){
+       case 'y': pc.printf("Select the calibration procedure: 2point or 4point? (2/4)\r\n");
+                char point = pc.getc();
+                if(point == '2'){
+                    pc.printf("Place the device verticaly to obtain x value close to -1, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; y = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        if (x<y) {y=x;}
+                        pc.printf("%f  \t %f \r\n" , x,y);
+                        wait(0.1);}
+                    pc.printf("Place the device verticaly to obtain x value close to +1, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; z = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        if (x>z) {z=x;}
+                        pc.printf("%f  \t %f \r\n" , x,z);
+                        wait(0.1);}     
+                    float meas[] = {z, y};
+                    test.convert_2p(meas, 0);
+                    pc.printf("The input for the calibration process are: %f  \t %f \r\n" , meas[0], meas[1]);
+                    pc.printf("The result (Sensibility and Bias) is: %f  \t %f \r\n" , test.adxl355_sensitivity.S[0], test.adxl355_sensitivity.B[0]);
+                    }
+                else if(point == '4'){
+                    pc.printf("Place the device verticaly to obtain x value close to 0, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; y = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        y=(y+x)/2;
+                        pc.printf("%f  \t %f \r\n" , x,y);
+                        wait(0.1);}
+                    pc.printf("Rotate the device to obtain x value close to -1, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; z = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        if (x<z) {z=x;}
+                        pc.printf("%f  \t %f \r\n" , x,z);
+                        wait(0.1);}
+                    pc.printf("Place the device verticaly to obtain x value close to 0, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; w = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        w=(w+x)/2;
+                        pc.printf("%f  \t %f \r\n" , x,w);
+                        wait(0.1);}
+                    pc.printf("Rotate the device verticaly to obtain x value close to +1, press any character to start the aquisition \r\n");
+                    pc.getc();
+                    x = accl.convert(accl.scanx())*accl.axis355_sens; k = x;
+                    for(int i=0; i<50; i++){
+                        x = accl.convert(accl.scanx())*accl.axis355_sens;
+                        if (x>k) {k=x;}
+                        pc.printf("%f  \t %f \r\n" , x,k);
+                        wait(0.1);}     
+                    float meas[] = {y, z, w, k};
+                    test.convert_4p(meas, 0);
+                    pc.printf("The input for the calibration process are: %f  \t %f \t %f \t %f \t \r\n" , y, z, w, k);
+                    pc.printf("The result (Sensibility and Bias) is: %f  \t %f \r\n" , test.adxl355_sensitivity.S[0], test.adxl355_sensitivity.B[0]);
+                    }
+                else {pc.printf("Input value not recognized \r\n"); pc.printf("%c \r\n",point);}
+                break;
+       case 'n': pc.printf("No calibration test \r\n"); 
+                break;
+       default:  pc.printf("Input value not recognized \r\n"); 
+                break;
+       }     
+    // test reading angle
+   pc.printf("Start reading angle after calibration (y/n)?\r\n");
+   calib = pc.getc();
+   switch(calib){
+       case 'y': pc.printf("Select the procedure for angle measurement: 1axis, 2axis or 3axis? (1/2/3)\r\n");
+                char point = pc.getc();
+                pc.printf("x \t y \t z \t pitch \t roll \t tau \r\n");
+                if(point == '1'){
+                    for(int i=0; i<50; 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;
+                        w = asin(x/9.81)*180/pi;
+                        k = asin(y/9.81)*180/pi;
+                        h = asin(z/9.81)*180/pi;
+                        pc.printf("%f \t %f \t %f \t %f \t %f \t %f  \r\n" , x,y,z,w,k,h);
+                        wait(0.1);}   
+                    }
+                else if(point == '2'){
+                    for(int i=0; i<50; 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;
+                        w = atan(x/z)*180/pi;;
+                        k = atan(y/z)*180/pi;;
+                        h = 0;
+                        pc.printf("%f \t %f \t %f \t %f \t %f \t %f  \r\n" , x,y,z,w,k,h);
+                        wait(0.1);}
+                    }
+                else if(point == '3'){
+                    for(int i=0; i<50; 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;
+                        w = atan2(x,sqrt(pow(y,2)+pow(z,2)))*180/pi; 
+                        k = atan2(y,sqrt(pow(x,2)+pow(z,2)))*180/pi;
+                        h = atan2(sqrt(pow(x,2)+pow(y,2)),z)*180/pi;
+                        pc.printf("%f \t %f \t %f \t %f \t %f \t %f  \r\n" , x,y,z,w,k,h);
+                        wait(0.1);} 
+                    }  
+                else {pc.printf("Input value not recognized \r\n"); pc.printf("%c \r\n",point);}
+                break;
+       case 'n': pc.printf("No calibration test \r\n"); 
+                break;
+       default:  pc.printf("Input value not recognized \r\n"); 
+                break;
+       }  
 }
  
\ No newline at end of file