Diff: MPU9250Sensor.cpp

Revision:

13:ef0ce8fa871f

Parent:

11:70359785c2a7

Child:

14:cb369746225d

--- a/MPU9250Sensor.cpp	Tue Mar 03 20:00:21 2015 +0000
+++ b/MPU9250Sensor.cpp	Sun Apr 05 09:54:04 2015 +0000
@@ -5,10 +5,8 @@
 #define LOG(...) do printf(__VA_ARGS__); while (0)
 #endif
 
-MPU9250Sensor::MPU9250Sensor(SPI* spi_,DigitalOut* cs_) : BaseSensor() {
-    this->spi=spi_;
-    this->cs=cs_;
-    *cs = UP;
+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.
@@ -25,44 +23,50 @@
     LOG("Start verfication of MPU9250 Sensor");
     uint32_t errors = 0;
     //who am I register value is 0x71
-//    uint8_t sensorId = readRegister(MPU9250_WHOAMI);
+    uint8_t sensorId = readRegister(MPU9250_WHOAMI);
     
-//    if (sensorId !=0x71){
-//        errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
-//        LOG("Wrong sensorId: %X",sensorId);
-//    }
+    if (sensorId !=0x71){
+        errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
+        LOG("Wrong sensorId: %X",sensorId);
+    }
     
     //check status registry
-//    uint8_t status  = readRegister(STATUS);
+    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);        
-//    }
+    if (status & (1 << 7)){
+        errorResult[errors++] = ERROR_SEU_ERROR_DETECT;
+        LOG("SEU error detected: %X",status);        
+    }
     //check that chip is in awaken state  
-    //if (!(status & (1 << 6))){
-//        errorResult[errors++] = ERROR_DEVICE_SLEEPING;
-//        LOG("Chip not awaken: %X",status);
-//    }    
+    if (!(status & (1 << 6))){
+        errorResult[errors++] = ERROR_DEVICE_SLEEPING;
+        LOG("Chip not awaken: %X",status);
+    }    
     
     //perform self test
-//    errors+=selfTest(&errorResult[errors]); 
+    errors+=selfTest(&errorResult[errors]); 
     
     return errors;
 }
 
+uint32_t MPU9250Sensor::selfTest(uint32_t* errorResult){
+    uint32_t errors = 0;
+    
+     return errors;
+}
+
 void MPU9250Sensor::getSensorDetails(sensor_t* sensorDetails) {
 
 }
 
-//uint8_t MPU9250Sensor::readRegister( uint8_t reg){
-//    cs->write(DOWN);
-//    spi->write(reg| MPU9250_READ_FLAG);
-//    uint8_t val = spi->write(0x00);
-//    cs->write(UP);
-//    return (val);
-//}
+uint8_t MPU9250Sensor::readRegister( uint8_t reg){
+    cs = DOWN;
+    spi.write(reg| MPU9250_READ_FLAG);
+    uint8_t val = spi.write(0x00);
+    cs = UP;
+    return val;
+}
 //
 //void MPU9250Sensor::writeRegister( uint8_t reg, uint8_t cmd ){
 //    cs->write(DOWN);