Revision 14:cb369746225d, committed 2015-04-15

Comitter:

smigielski

Date:

Wed Apr 15 20:01:16 2015 +0000

Parent:

13:ef0ce8fa871f

Commit message:

ADXL362 self test working

Changed in this revision

diff -r ef0ce8fa871f -r cb369746225d ADXL362Sensor.cpp
--- a/ADXL362Sensor.cpp	Sun Apr 05 09:54:04 2015 +0000
+++ b/ADXL362Sensor.cpp	Wed Apr 15 20:01:16 2015 +0000
@@ -4,6 +4,9 @@
 
 ADXL362Sensor::ADXL362Sensor(SPI& spi_,DigitalOut& cs_,void (*debug_)(const char* format, ...)) : BaseSensor(debug_), spi(spi_), cs(cs_)  {    
     cs = UP;
+    writeRegister(ADXL362_SOFT_RESET, 0x52);  
+    wait_ms(1);
+    writeRegister(ADXL362_POWER_CTL,0x02);
 }
 
  
@@ -13,7 +16,7 @@
 
 
 uint32_t ADXL362Sensor::verifyIntegrity(uint32_t* errorResult) {
-    LOG("Start verfication of ADXL362 Sensor");
+    LOG("Start verfication of ADXL362 Sensor\r\n");
     uint32_t errors = 0;
     //Device id is 0xAD
     //Device mems id is 0x1D
@@ -22,7 +25,7 @@
     
     if (sensorId >> 8 !=0xAD1DF2){
         errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
-        LOG("Wrong sensorId: %X",sensorId);
+        LOG("Wrong sensorId: %X\r\n",sensorId);
     }
     
     //check status registry
@@ -30,13 +33,13 @@
     
     //indicate that SEU error was detetcted 
     if (status & (1 << 7)){
-        errorResult[errors++] = ERROR_SEU_ERROR_DETECT;
-        LOG("SEU error detected: %X",status);        
+        errorResult[errors++] = ERROR_WRONG_DEVICE_STATE;
+        LOG("SEU error detected: %X\r\n",status);        
     }
     //check that chip is in awaken state  
     if (!(status & (1 << 6))){
         errorResult[errors++] = ERROR_DEVICE_SLEEPING;
-        LOG("Chip not awaken: %X",status);
+        LOG("Chip not awaken: %X\r\n",status);
     }    
     
     //perform self test
@@ -56,29 +59,28 @@
     int16_t test_x12,test_y12,test_z12;
 // 1. Read acceleration data for the x-, y-, and z-axes.    
     refreshAcceleration12(&x12, &y12, &z12); 
-    LOG("Acc1: %d %d %d\r\n",x12,y12,z12);   
 // 2. Assert self test by setting the ST bit in the SELF_TEST register, Address 0x2E.
     writeRegister(ADXL362_SELF_TEST,0x01);
 // 3. Wait 1/ODR for the output to settle to its new value.
-    wait(1/ADXL362_ODR);
+    wait(4.0/ADXL362_ODR);
 // 4. Read acceleration data for the x-, y-, and z-axes.
     refreshAcceleration12(&test_x12, &test_y12, &test_z12);
-    LOG("Acc2: %d %d %d\r\n",test_x12,test_y12,test_z12);   
 // 5. Compare to the values from Step 1, and convert the difference from LSB to mg by multiplying by the sensitivity. If the observed difference falls within the self test output change specification listed in Table 1, then the device passes self test and is deemed operational.
-    float reactionX = (x12-test_x12)* ADXL362_MG2G_MULTIPLIER * SENSORS_GRAVITY_STANDARD / ADXL362_SELF_TEST_SCALE_FACTOR;
-    float reactionY = (y12-test_y12)* ADXL362_MG2G_MULTIPLIER * SENSORS_GRAVITY_STANDARD / ADXL362_SELF_TEST_SCALE_FACTOR;
-    float reactionZ = (z12-test_z12)* ADXL362_MG2G_MULTIPLIER * SENSORS_GRAVITY_STANDARD / ADXL362_SELF_TEST_SCALE_FACTOR;
+    float reactionX = (test_x12-x12) * 0.250f;
+    float reactionY = (test_y12-y12) * 0.250f;
+    float reactionZ = (test_z12-z12) * 0.250f;
+        
     
-    if (reactionX<0.450F || reactionX>0.710F ||
-        reactionY<-0.710F || reactionY>-0.450F ||
-        reactionZ<0.350F || reactionZ>0.650F){
-        errorResult[errors++] = ERROR_SELF_TEST_FAILED;
+    if (reactionX<230.0F || reactionX>870.0F ||
+        reactionY<-870.0F || reactionY>-230.0F ||
+        reactionZ<270.0F || reactionZ>800.0F){
+        errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
         LOG("Reaction: %+5.3f %+5.3f %+5.3f\r\n",reactionX,reactionY,reactionZ);
     }
     
 // 6. Deassert self test by clearing the ST bit in the SELF_TEST register, Address 0x2E.
     writeRegister(ADXL362_SELF_TEST,0x00);
-    
+    wait(4.0/ADXL362_ODR);
     return errors;
 }
 

diff -r ef0ce8fa871f -r cb369746225d ADXL362Sensor.h
--- a/ADXL362Sensor.h	Sun Apr 05 09:54:04 2015 +0000
+++ b/ADXL362Sensor.h	Wed Apr 15 20:01:16 2015 +0000
@@ -48,9 +48,9 @@
 #define ADXL362_READ_REGISTER 0x0b
 #define ADXL362_READ_FIFO 0x0d
 
-#define ADXL362_SELF_TEST_SCALE_FACTOR 2.4
+#define ADXL362_SELF_TEST_SCALE_FACTOR 2.4f
 #define ADXL362_MG2G_MULTIPLIER 0.002f
-#define ADXL362_ODR 100
+#define ADXL362_ODR 100.0f
 
  
 class ADXL362Sensor : public BaseSensor {

diff -r ef0ce8fa871f -r cb369746225d BaseSensor.h
--- a/BaseSensor.h	Sun Apr 05 09:54:04 2015 +0000
+++ b/BaseSensor.h	Wed Apr 15 20:01:16 2015 +0000
@@ -22,9 +22,11 @@
 
 /* Errors */
 #define ERROR_WRONG_DEVICE_ID 1
-#define ERROR_SEU_ERROR_DETECT 2
+#define ERROR_WRONG_DEVICE_STATE 2
 #define ERROR_DEVICE_SLEEPING 3
-#define ERROR_SELF_TEST_FAILED 4
+#define ERROR_ACCE_SELF_TEST_FAILED 4
+#define ERROR_GYRO_SELF_TEST_FAILED 5
+#define ERROR_MAGN_SELF_TEST_FAILED 6
 
 /** Sensor types */
 typedef enum

diff -r ef0ce8fa871f -r cb369746225d MPU9250Sensor.cpp
--- a/MPU9250Sensor.cpp	Sun Apr 05 09:54:04 2015 +0000
+++ b/MPU9250Sensor.cpp	Wed Apr 15 20:01:16 2015 +0000
@@ -5,13 +5,30 @@
 #define LOG(...) do printf(__VA_ARGS__); while (0)
 #endif
 
+#define SELF_TEST_COUNT 200
+
 MPU9250Sensor::MPU9250Sensor(SPI& spi_,DigitalOut& cs_,void (*debug_)(const char* format, ...)) : BaseSensor(debug_), spi(spi_), cs(cs_) {
    cs = UP;
     //To prevent switching into I2C mode when using SPI, the I2C interface should be disabled by setting the I2C_IF_DIS 
     //configuration bit. Setting this bit should be performed immediately after waiting for the time specified by the 
     //“Start-Up Time for Register Read/Write” in Section 6.3.
     
+    writeRegister(MPU9250_USER_CTRL,0x01,0x01,4);
     
+    //read Sensitivity Adjustment values
+    initMagnetometr();    
+}
+
+void MPU9250Sensor::initMagnetometr(){
+    uint8_t calibration[3];
+    //3. Set the compas into fuse mode
+    writeRegisterI2C(AK8963_CNTL, 0x0F);
+    
+    readRegistersI2C(AK8963_ASAX,3,calibration);
+    for (int i =0;i<3;i++){        
+        magnetometerCalibration[i]=(((float)calibration[i]-128.0)/256.0+1);
+        LOG("Magnet calib. %d: %+5.3f\r\n",i,magnetometerCalibration[i]);
+    }
 }
 
 char* MPU9250Sensor::getSimpleName() {
@@ -20,29 +37,23 @@
 
 
 uint32_t MPU9250Sensor::verifyIntegrity(uint32_t* errorResult) {
-    LOG("Start verfication of MPU9250 Sensor");
+    LOG("Start verfication of MPU9250 Sensor\r\n");
     uint32_t errors = 0;
     //who am I register value is 0x71
     uint8_t sensorId = readRegister(MPU9250_WHOAMI);
     
     if (sensorId !=0x71){
         errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
-        LOG("Wrong sensorId: %X",sensorId);
+        LOG("Wrong sensorId: %X\r\n",sensorId);
     }
     
-    //check status registry
-    uint8_t status  = readRegister(MPU9250_I2C_MST_STATUS);
-    
-    //indicate that SEU error was detetcted 
-    if (status & (1 << 7)){
-        errorResult[errors++] = ERROR_SEU_ERROR_DETECT;
-        LOG("SEU error detected: %X",status);        
+
+    //check magnetometer chip id  0x48
+    uint8_t magSensorId = readRegisterI2C(AK8963_WIA);
+    if (magSensorId !=0x48){
+        errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
+        LOG("Wrong magnetometr sensorId: %X\r\n",magSensorId);
     }
-    //check that chip is in awaken state  
-    if (!(status & (1 << 6))){
-        errorResult[errors++] = ERROR_DEVICE_SLEEPING;
-        LOG("Chip not awaken: %X",status);
-    }    
     
     //perform self test
     errors+=selfTest(&errorResult[errors]); 
@@ -52,14 +63,291 @@
 
 uint32_t MPU9250Sensor::selfTest(uint32_t* errorResult){
     uint32_t errors = 0;
+    // gyroscope Self-Test
+    errors+=gyroscopeSelfTest(&errorResult[errors]); 
+    // accelerometer’s Self-Test
+    errors+=accelerometerSelfTest(&errorResult[errors]); 
+    // compass Self-Test
+    errors+=magnetometerSelfTest(&errorResult[errors]); 
+     
+    return errors;
+}
+
+
+uint32_t MPU9250Sensor::gyroscopeSelfTest(uint32_t* errorResult){
+    uint32_t errors = 0;
+    int16_t rotation_speed[3],rotation_speed_self_test[3];
+    uint8_t selfTest[3];
+    float self_test_otp[3],self_test_response[3],gyro_offset[3];
+
+    //1) The routine starts by measuring the digital output of all three gyroscopes. 
+    //In order to do this, the following registers are modified:
     
-     return errors;
+    // Change the digital low pass filter (DLPF) code to 2 (Register Address: 26 (1Ah)
+    //Bit [2:0] – USR). The following table details the configuration of the component when the
+    //DLPF is configured to 2.
+    writeRegister(MPU9250_CONFIG,0x02,0x03);
+ 
+    // Store the existing full scale range select code (Register Address: 27 (1Bh) Bit
+    //[4:3] – USR) as Old_FS, then select a full scale range of 250dps by setting the
+    //ACCEL_FS_SEL bits to 00.
+    writeRegister(MPU9250_GYRO_CONFIG,MPU9250_GFS_250DPS,0x03,3);
+    
+    //2) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
+    //The averaged values will be the LSB of GX_OS, GY_OS, GZ_OS, AX_OS, AY_OS and
+    //AZ_OS in the software.
+    averageData(MPU9250_GYRO_XOUT_H,rotation_speed);
+    
+    //3) Set USR_Reg: (1Bh) Gyro_Config, gdrive_axisCTST [0-2] to b111 to enable Self-Test.
+    writeRegister(MPU9250_GYRO_CONFIG, 0x07,0x07,5); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
+    
+    //4) Wait 20ms for oscillations to stabilize 
+    wait_us(20);
+    
+    //5) Read the gyroscope  output at a 1kHz rate and average 200 readings.
+    //The averaged values will be the LSB of GX_ST_OS, GY_ST_OS, GZ_ST_OS in the software.
+    averageData(MPU9250_GYRO_XOUT_H,rotation_speed_self_test);
+    
+    //6) Calculate the Self-Test response as follows:
+    
+    //1) Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software:
+    //X-gyro: selftest1 (00): xg_st_data [0-7]
+    //Y-gyro: selftest1 (01): yg_st_data [0-7]
+    //Z-gyro: selftest1 (02): zg_st_data [0-7]
+    readRegisters(MPU9250_SELF_TEST_X_GYRO, 3, &selfTest[0]);
+    
+    //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
+    for (int i =0; i < 3; i++) { 
+        self_test_response[i] = (float)rotation_speed_self_test[i]-(float)rotation_speed[i];
+        self_test_otp[i] = (float)(2620.0f)*(pow( 1.01 , ((float)selfTest[i] - 1.0) ));
+    }
+    
+    //3) Determine passing or failing Self-Test
+
+    
+    for (int i =0; i < 3; i++) {
+        if (self_test_otp[i]!=0){
+            //a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response
+            //(GXST, GYST, GZST, AXST, AYST and AZST) to the factory Self-Test values
+            //(ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled
+            if (self_test_response[i]/self_test_otp[i]<0.5F){
+                errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
+                LOG("3.a Gyro resp %d: %+5.3f\r\n",i,self_test_response[i]);
+            }
+        } else {
+            //b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response
+            //(GXST, GYST, GZST, AXST, AYST and AZST) to the ST absolute limits (ST_AL)
+            //and report Self-Test is passing if all the following criteria are fulfilled. 
+            if (abs((float)MPU9250_GFS_250DPS_MULTIPLIER*self_test_response[i])<60){
+                errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
+                LOG("3.b Gyro resp %d: %+5.3f\r\n",i,self_test_response[i]);
+            } 
+        }
+    }
+    
+    //read offset
+    readGyroOffset(MPU9250_XG_OFFSET_H, &gyro_offset[0]);
+    
+    //c. If the Self-Test passes criteria (a) and (b), it’s necessary to check gyro offset values.
+    //Report passing Self-Test if the following criteria fulfilled.
+    for (int i =0; i < 3; i++) {
+        if (abs(gyro_offset[i]) < 20.0){
+                errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
+                LOG("3.c Gyro offset %d: %+5.3f\r\n",i,gyro_offset[i]);
+        }
+    }
+    
+    //3.1. External Configuration Cleanup
+    writeRegister(MPU9250_GYRO_CONFIG, 0x00,0x07,5);
+    wait_us(20);
+    
+    return errors;
+}
+
+
+uint32_t MPU9250Sensor::accelerometerSelfTest(uint32_t* errorResult){
+    uint32_t errors = 0;
+    int16_t acceleration[3],acceleration_self_test[3];
+    uint8_t selfTest[3];
+    float self_test_otp[3],self_test_response[3];
+    
+     //1) The routine starts by measuring the digital output of all three gyroscopes. 
+    //In order to do this, the following registers are modified:
+    
+    //Change the DLPF Code to 2 (Register Address: 29 (1Dh) Bit [2:0] – USR).
+    //The following table details the configuration of the component when the DLPF is configured
+    //to 2.
+
+    writeRegister(MPU9250_ACCEL_CONFIG_2,0x02,0x03 );
+    
+    //Store the existing full scale range select code (Register Address: 28 (1Ch)
+    //Bit [4:3] – USR) as Old_FS, then select a full scale range of ±2g by setting the
+    //ACCEL_FS_SEL bits to 00.
+    writeRegister(MPU9250_ACCEL_CONFIG,MPU9250_AFS_2G,0x03,3);
+    
+    //2) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
+    //The averaged values will be the LSB of GX_OS, GY_OS, GZ_OS, AX_OS, AY_OS and
+    //AZ_OS in the software.
+    averageData(MPU9250_ACCEL_XOUT_H,acceleration);
+    
+    //3) Set USR_Reg: (1Ch) Accel_Config, AX/Y/Z_ST_EN [0-2] to b111 to enable Self-Test.
+    writeRegister(MPU9250_ACCEL_CONFIG, 0x07,0x07,5); 
+    
+     //4) Wait 20ms for oscillations to stabilize 
+    wait_us(20);
+    
+    //5) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
+    //The averaged values will be the LSB of AX_ST_OS, AY_ST_OS and AZ_ST_OS in the software.
+    averageData(MPU9250_ACCEL_XOUT_H,acceleration_self_test);
+    
+    //6) Calculate the Self-Test response as follows:
+    
+       //1) Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software:
+    //X-gyro: selftest1 (00): xg_st_data [0-7]
+    //Y-gyro: selftest1 (01): yg_st_data [0-7]
+    //Z-gyro: selftest1 (02): zg_st_data [0-7]
+    readRegisters(MPU9250_SELF_TEST_X_ACCEL, 3, &selfTest[0]);
+    
+    //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
+    for (int i =0; i < 3; i++) { 
+        self_test_response[i] = (float)acceleration_self_test[i]-(float)acceleration[i];
+        self_test_otp[i] = (float)(2620.0)*(pow( 1.01 , ((float)selfTest[i] - 1.0) ));
+    }
+    
+    
+    //3) Determine passing or failing Self-Test
+    for (int i =0; i < 3; i++) {
+        if (self_test_otp[i]!=0){
+            //a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response
+            //(GXST, GYST, GZST, AXST, AYST and AZST) to the factory Self-Test values
+            //(ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled
+            float ratio = self_test_response[i]/self_test_otp[i];
+            if (ratio<0.5F || ratio > 1.5f){
+                errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
+                LOG("3.a Acc resp %d: %+5.3f\r\n",i,self_test_response[i]);
+            }
+        } else {
+            //b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response
+            //(GXST, GYST, GZST, AXST, AYST and AZST) to the ST absolute limits (ST_AL)
+            //and report Self-Test is passing if all the following criteria are fulfilled. 
+            float response=MPU9250_AFS_2G_MULTIPLIER*self_test_response[i];
+            if (abs(response)<0.225f || abs(response)>0.675f ){
+                errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
+                LOG("3.b Acc resp %d: %+5.3f\r\n",i,self_test_response[i]);
+            } 
+        }
+    }
+    
+    
+    //3.1. External Configuration Cleanup
+    writeRegister(MPU9250_ACCEL_CONFIG, 0x00,0x07,5);
+    wait_us(20);
+    
+    return errors;
+}
+
+uint32_t MPU9250Sensor::magnetometerSelfTest(uint32_t* errorResult){    
+    uint32_t errors = 0;
+    uint8_t rawData[7];
+    int16_t magnetometer[3];
+    float self_test_response[3];
+    //1. Set the compass into power-down mode.
+    writeRegisterI2C(AK8963_CNTL, 0x00);
+    
+    //2. Write “1” to the SELF bit of the ASTC register. Other bits in this register should be set to zero.
+    writeRegisterI2C(AK8963_ASTC, 0x01,0x01,6);
+    
+    //3. Set the self test mode in the “Mode” register.
+    writeRegisterI2C(AK8963_CNTL, 0x08);
+    
+    //4. Check if data is ready or not by polling the DRDY bit of the ST1 register. When the data is ready, proceed to step 5.
+     while (!(readRegisterI2C(AK8963_ST1) & 0x01)){
+          wait_us(1);
+     }
+          
+     //5. Read the measurement data in the compass measurement data registers.
+     readRegistersI2C(MPU9250_EXT_SENS_DATA_00, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array
+    uint8_t c = rawData[6]; // End data read by reading ST2 register
+    if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
+        magnetometer[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
+        magnetometer[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ;  // Data stored as little Endian
+        magnetometer[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 
+    } else {
+        magnetometer[0] = 0xffff;
+        magnetometer[1] = 0xffff;
+        magnetometer[2] = 0xffff;
+    }
+    
+    //6. Write “0” to SELF bit of the ASTC register.
+    writeRegisterI2C(AK8963_ASTC, 0x00,0x01,6);
+              
+    //7. Set the compass to power-down mode.
+    writeRegisterI2C(AK8963_CNTL, 0x00);
+    
+        //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
+    for (int i =0; i < 3; i++) { 
+        self_test_response[i] = (float)magnetometer[i]*magnetometerCalibration[i]*MPU9250M_4800uT;
+    }
+    
+    if(!(c & 0x10)) {
+        //Set Pass/Fail Criteria 16bit
+        if (self_test_response[0]<-200.0 || self_test_response[0] > 200.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[0]);
+        }
+        if (self_test_response[1]<-200.0 || self_test_response[1] > 200.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",1,self_test_response[1]);
+        }
+        if (self_test_response[2]<-3200.0 || self_test_response[2] > 800.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[2]);
+        }
+    } else {
+        //Set Pass/Fail Criteria 14bit
+        if (self_test_response[0]<-50.0 || self_test_response[0] > 50.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[0]);
+        }
+        if (self_test_response[1]<-50.0 || self_test_response[1] > 50.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",1,self_test_response[1]);
+        }
+        if (self_test_response[2]<-800.0 || self_test_response[2] > 200.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[2]);
+        }
+    }
+                     
+    return errors;
 }
 
 void MPU9250Sensor::getSensorDetails(sensor_t* sensorDetails) {
 
 }
 
+void MPU9250Sensor::readGyroOffset(uint8_t reg, float* data){
+    uint8_t rawData[6];
+    readRegisters(reg, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+    data[0] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+    data[1] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+    data[2] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+}
+
+void MPU9250Sensor::averageData(uint8_t reg,int16_t* data){
+    uint8_t rawData[6];
+    for( int i = 0; i < SELF_TEST_COUNT; i++) { // get average current values of gyro
+        readRegisters(reg, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+        data[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+        data[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+        data[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+    }
+    
+    for (int i =0; i < 3; i++) { // Get average of 200 values and store as average current readings
+        data[i] /= SELF_TEST_COUNT;
+    }
+}
+
 uint8_t MPU9250Sensor::readRegister( uint8_t reg){
     cs = DOWN;
     spi.write(reg| MPU9250_READ_FLAG);
@@ -67,10 +355,68 @@
     cs = UP;
     return val;
 }
-//
-//void MPU9250Sensor::writeRegister( uint8_t reg, uint8_t cmd ){
-//    cs->write(DOWN);
-//    spi->write(reg);
-//    spi->write(cmd);
-//    cs->write(UP);
-//}
+
+uint8_t MPU9250Sensor::readRegisterI2C(uint8_t reg){
+    cs = DOWN;
+    spi.write(MPU9250_I2C_SLV0_ADDR);
+    spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
+    spi.write(MPU9250_I2C_SLV0_REG);
+    spi.write(reg);
+    spi.write(MPU9250_I2C_SLV0_CTRL);
+    spi.write(0x81);
+    spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
+    uint8_t val = spi.write(0x00);
+    cs = UP;
+    return val;
+}
+
+void MPU9250Sensor::readRegisters(uint8_t reg, uint8_t count, uint8_t * dest){
+    cs = DOWN;
+    spi.write(reg| MPU9250_READ_FLAG);
+    for(int i = 0; i < count; i++) {
+        dest[i] = spi.write(0x00);
+    }
+    cs = UP;
+}
+
+void MPU9250Sensor::readRegistersI2C(uint8_t reg, uint8_t count, uint8_t * dest){
+    cs = DOWN;
+    spi.write(MPU9250_I2C_SLV0_ADDR);
+    spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
+    spi.write(MPU9250_I2C_SLV0_REG);
+    spi.write(reg);
+    spi.write(MPU9250_I2C_SLV0_CTRL);
+    spi.write(0x80| count);
+    spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
+    for(int i = 0; i < count; i++) {
+        dest[i] = spi.write(0x00);
+    }
+    cs = UP;
+}
+
+void MPU9250Sensor::writeRegister( uint8_t reg,  uint8_t data, uint8_t mask,uint8_t pos){
+    cs = DOWN;
+    spi.write(reg| MPU9250_READ_FLAG);
+    uint8_t val = spi.write(0x00);
+    spi.write(reg);
+    spi.write(val & ~(mask<<pos) | (data<<pos));
+    cs = UP;
+}
+
+void MPU9250Sensor::writeRegisterI2C( uint8_t reg,  uint8_t data, uint8_t mask,uint8_t pos){
+    uint8_t val;
+    cs = DOWN;
+    spi.write(MPU9250_I2C_SLV0_ADDR);
+    spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);    
+    spi.write(MPU9250_I2C_SLV0_REG);
+    spi.write(reg);
+    spi.write(MPU9250_I2C_SLV0_CTRL);
+    spi.write(0x81);
+    spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
+    val = spi.write(0x00);
+    spi.write(MPU9250_I2C_SLV0_ADDR);
+    spi.write(AK8963_I2C_ADDR);
+    spi.write(MPU9250_I2C_SLV0_DO);
+    val = spi.write(val & ~(mask<<pos) | (data<<pos));
+    cs = UP;
+}
\ No newline at end of file

diff -r ef0ce8fa871f -r cb369746225d MPU9250Sensor.h
--- a/MPU9250Sensor.h	Sun Apr 05 09:54:04 2015 +0000
+++ b/MPU9250Sensor.h	Wed Apr 15 20:01:16 2015 +0000
@@ -6,29 +6,21 @@
 #include "BaseSensor.h"
 
 // mpu9250 registers
-#define MPU9250_XG_OFFS_TC 0x00
-#define MPU9250_YG_OFFS_TC 0x01
-#define MPU9250_ZG_OFFS_TC 0x02
-#define MPU9250_X_FINE_GAIN 0x03
-#define MPU9250_Y_FINE_GAIN 0x04
-#define MPU9250_Z_FINE_GAIN 0x05
-#define MPU9250_XA_OFFS_H 0x06
-#define MPU9250_XA_OFFS_L 0x07
-#define MPU9250_YA_OFFS_H 0x08
-#define MPU9250_YA_OFFS_L 0x09
-#define MPU9250_ZA_OFFS_H 0x0A
-#define MPU9250_ZA_OFFS_L 0x0B
-#define MPU9250_PRODUCT_ID 0x0C
-#define MPU9250_SELF_TEST_X 0x0D
-#define MPU9250_SELF_TEST_Y 0x0E
-#define MPU9250_SELF_TEST_Z 0x0F
-#define MPU9250_SELF_TEST_A 0x10
-#define MPU9250_XG_OFFS_USRH 0x13
-#define MPU9250_XG_OFFS_USRL 0x14
-#define MPU9250_YG_OFFS_USRH 0x15
-#define MPU9250_YG_OFFS_USRL 0x16
-#define MPU9250_ZG_OFFS_USRH 0x17
-#define MPU9250_ZG_OFFS_USRL 0x18
+#define MPU9250_SELF_TEST_X_GYRO 0x00
+#define MPU9250_SELF_TEST_Y_GYRO 0x01
+#define MPU9250_SELF_TEST_Z_GYRO 0x02
+
+#define MPU9250_SELF_TEST_X_ACCEL 0x0D
+#define MPU9250_SELF_TEST_Y_ACCEL 0x0E
+#define MPU9250_SELF_TEST_Z_ACCEL 0x0F
+
+
+#define MPU9250_XG_OFFSET_H 0x13
+#define MPU9250_XG_OFFSET_L 0x14
+#define MPU9250_YG_OFFSET_H 0x15
+#define MPU9250_YG_OFFSET_L 0x16
+#define MPU9250_ZG_OFFSET_H 0x17
+#define MPU9250_ZG_OFFSET_L 0x18
 #define MPU9250_SMPLRT_DIV 0x19
 #define MPU9250_CONFIG 0x1A
 #define MPU9250_GYRO_CONFIG 0x1B
@@ -137,8 +129,8 @@
 #define AK8963_HZH                  0x08
 #define AK8963_ST2                  0x09
 // Write/Read Reg
-#define AK8963_CNTL1                0x0A
-#define AK8963_CNTL2                0x0B
+#define AK8963_CNTL                 0x0A
+#define AK8963_RSV                  0x0B
 #define AK8963_ASTC                 0x0C
 #define AK8963_TS1                  0x0D
 #define AK8963_TS2                  0x0E
@@ -149,6 +141,46 @@
 #define AK8963_ASAZ                 0x12
 
 #define MPU9250_READ_FLAG 0x80
+
+#define MPU9250_AFS_2G 0
+#define MPU9250_AFS_4G 1
+#define MPU9250_AFS_8G 2
+#define MPU9250_AFS_16G 3
+ 
+#define MPU9250_GFS_250DPS 0
+#define MPU9250_GFS_500DPS 1
+#define MPU9250_GFS_1000DPS 2
+#define MPU9250_GFS_2000DPS 3
+
+
+#define MPU9250_AFS_2G_MULTIPLIER 2.0/32768.0
+#define MPU9250_AFS_4G_MULTIPLIER 4.0/32768.0
+#define MPU9250_AFS_8G_MULTIPLIER 8.0/32768.0
+#define MPU9250_AFS_16G_MULTIPLIER 16.0/32768.0
+ 
+
+#define MPU9250_GFS_250DPS_MULTIPLIER 250.0/32768.0
+#define MPU9250_GFS_500DPS_MULTIPLIER 500.0/32768.0
+#define MPU9250_GFS_1000DPS_MULTIPLIER 1000.0/32768.0
+#define MPU9250_GFS_2000DPS_MULTIPLIER 2000.0/32768.0
+
+
+/* ---- Sensitivity --------------------------------------------------------- */
+ 
+#define MPU9250A_2g       ((float)0.000061035156f) // 0.000061035156 g/LSB
+#define MPU9250A_4g       ((float)0.000122070312f) // 0.000122070312 g/LSB
+#define MPU9250A_8g       ((float)0.000244140625f) // 0.000244140625 g/LSB
+#define MPU9250A_16g      ((float)0.000488281250f) // 0.000488281250 g/LSB
+ 
+#define MPU9250G_250dps   ((float)0.007633587786f) // 0.007633587786 dps/LSB
+#define MPU9250G_500dps   ((float)0.015267175572f) // 0.015267175572 dps/LSB
+#define MPU9250G_1000dps  ((float)0.030487804878f) // 0.030487804878 dps/LSB
+#define MPU9250G_2000dps  ((float)0.060975609756f) // 0.060975609756 dps/LSB
+ 
+#define MPU9250M_4800uT   ((float)0.15f)            // 0.15 uT/LSB
+ 
+#define MPU9250T_85degC   ((float)0.002995177763f) // 0.002995177763 degC/LSB
+
  
 class MPU9250Sensor : public BaseSensor {
 public:
@@ -163,8 +195,20 @@
     SPI& spi; 
     DigitalOut& cs;
     
+    float magnetometerCalibration[3];
+    void initMagnetometr();
     uint32_t selfTest(uint32_t* errorResult);
+    uint32_t gyroscopeSelfTest(uint32_t* errorResult);
+    uint32_t accelerometerSelfTest(uint32_t* errorResult);
+    uint32_t magnetometerSelfTest(uint32_t* errorResult);
+    void readGyroOffset(uint8_t reg, float* data);
+    void averageData(uint8_t reg,int16_t* data);
     uint8_t readRegister( uint8_t reg);
+    uint8_t readRegisterI2C( uint8_t reg);
+    void readRegisters(uint8_t reg, uint8_t count, uint8_t * dest);
+    void readRegistersI2C(uint8_t reg, uint8_t count, uint8_t * dest);
+    void writeRegister( uint8_t reg,  uint8_t data, uint8_t mask = 0xff,uint8_t pos = 0);
+    void writeRegisterI2C( uint8_t reg,  uint8_t data, uint8_t mask = 0xff,uint8_t pos = 0);
 };
  
 #endif
\ No newline at end of file

diff -r ef0ce8fa871f -r cb369746225d main.cpp
--- a/main.cpp	Sun Apr 05 09:54:04 2015 +0000
+++ b/main.cpp	Wed Apr 15 20:01:16 2015 +0000
@@ -17,8 +17,6 @@
 #include <string.h>
 #include <stdarg.h>
 #include "mbed.h"
-#include "BLEDevice.h"
-#include "UARTService.h"
 
 
 #define DEBUG 1 /* Set this if you need debug messages on the console;
@@ -28,8 +26,10 @@
 #include "ADXL362Sensor.h"
 #include "MPU9250Sensor.h"
 
-BLEDevice  ble;
-UARTService *uart;
+
+
+#define BAUD_RATE 115200
+Serial s(P0_5, P0_6);
 
 #if DEBUG
 #define LOG_BUFFER 100
@@ -42,16 +42,17 @@
 void debug ( const char* format, ...) {
     va_list argptr;
     va_start(argptr, format);
-    vsnprintf(debugBuffer, LOG_BUFFER ,format, argptr);
+//    vsnprintf(debugBuffer, LOG_BUFFER ,format, argptr);
+    vprintf(format, argptr);
     va_end(argptr);  
-    if (uart){
-        uart->write(debugBuffer, strlen(debugBuffer));
-    }
+//    if (uart){
+//        uart->write(debugBuffer, strlen(debugBuffer));
+//    }
 }
 
 
 
-SPI spi1(P0_28, p24, P0_29); // mosi, miso, sclk
+SPI spi1(P0_28, P0_24, P0_29); // mosi, miso, sclk
 DigitalOut adxl_cs(P0_23);
 DigitalOut mpu_cs(P0_18);
 
@@ -61,18 +62,14 @@
 DigitalIn button(P0_16,PullDown);
 
 
+
 uint32_t sensorErrors[10];
 
 
 ADXL362Sensor adxl362(spi1, adxl_cs, debug);
 MPU9250Sensor mpu9250(spi1, mpu_cs, debug);
 
-void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
-{
-    LOG("Disconnected!\n\r");
-    LOG("Restarting the advertising process\n\r");
-    ble.startAdvertising();
-}
+
 
 
 int testSensors(BaseSensor& sensor)
@@ -88,39 +85,18 @@
     return errorCount;   
 }
 
-void periodicCallback(void)
-{
-    led=!led;
-    LOG("ping\r\n");
-    testSensors(adxl362);
-    testSensors(mpu9250);
-}
+
 
 int main(void)
 {
-
-    led=1;
-    Ticker ticker;
-    ticker.attach(periodicCallback, 1);
+    s.baud(BAUD_RATE);
+    
 
     LOG("Initialising the nRF51822\n\r");
-    ble.init();
-    ble.onDisconnection(disconnectionCallback);
-    
-    uart = new UARTService(ble);
-
-    /* setup advertising */
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
-    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
-                                     (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
-                                     (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
-
-    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
-    ble.startAdvertising();
 
     while (true) {
-        ble.waitForEvent();
+        testSensors(adxl362);
+//        testSensors(mpu9250);
+        wait(10.0f);
     }
 }
\ No newline at end of file