Icarus Sensors / Mbed 2 deprecated SelfTestBoot

Self test boot program for testing icarus sensors

Dependencies:   BLE_API mbed nRF51822

Fork of BLE_UARTConsole by Bluetooth Low Energy

MPU9250Sensor.cpp@14:cb369746225d, 2015-04-15 (annotated)

Committer:
smigielski
Date:
Wed Apr 15 20:01:16 2015 +0000
Revision:
14:cb369746225d
Parent:
13:ef0ce8fa871f 
ADXL362 self test working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smigielski 10:3a24c970db40 1 #include "MPU9250Sensor.h"
smigielski 10:3a24c970db40 2 #include "mbed.h"
smigielski 10:3a24c970db40 3
smigielski 10:3a24c970db40 4 #ifndef LOG
smigielski 10:3a24c970db40 5 #define LOG(...) do printf(__VA_ARGS__); while (0)
smigielski 10:3a24c970db40 6 #endif
smigielski 10:3a24c970db40 7
smigielski 14:cb369746225d 8 #define SELF_TEST_COUNT 200
smigielski 14:cb369746225d 9
smigielski 13:ef0ce8fa871f 10 MPU9250Sensor::MPU9250Sensor(SPI& spi_,DigitalOut& cs_,void (*debug_)(const char* format, ...)) : BaseSensor(debug_), spi(spi_), cs(cs_) {
smigielski 13:ef0ce8fa871f 11 cs = UP;
smigielski 10:3a24c970db40 12 //To prevent switching into I2C mode when using SPI, the I2C interface should be disabled by setting the I2C_IF_DIS
smigielski 10:3a24c970db40 13 //configuration bit. Setting this bit should be performed immediately after waiting for the time specified by the
smigielski 10:3a24c970db40 14 //“Start-Up Time for Register Read/Write” in Section 6.3.
smigielski 10:3a24c970db40 15
smigielski 14:cb369746225d 16 writeRegister(MPU9250_USER_CTRL,0x01,0x01,4);
smigielski 10:3a24c970db40 17
smigielski 14:cb369746225d 18 //read Sensitivity Adjustment values
smigielski 14:cb369746225d 19 initMagnetometr();
smigielski 14:cb369746225d 20 }
smigielski 14:cb369746225d 21
smigielski 14:cb369746225d 22 void MPU9250Sensor::initMagnetometr(){
smigielski 14:cb369746225d 23 uint8_t calibration[3];
smigielski 14:cb369746225d 24 //3. Set the compas into fuse mode
smigielski 14:cb369746225d 25 writeRegisterI2C(AK8963_CNTL, 0x0F);
smigielski 14:cb369746225d 26
smigielski 14:cb369746225d 27 readRegistersI2C(AK8963_ASAX,3,calibration);
smigielski 14:cb369746225d 28 for (int i =0;i<3;i++){
smigielski 14:cb369746225d 29 magnetometerCalibration[i]=(((float)calibration[i]-128.0)/256.0+1);
smigielski 14:cb369746225d 30 LOG("Magnet calib. %d: %+5.3f\r\n",i,magnetometerCalibration[i]);
smigielski 14:cb369746225d 31 }
smigielski 10:3a24c970db40 32 }
smigielski 10:3a24c970db40 33
smigielski 10:3a24c970db40 34 char* MPU9250Sensor::getSimpleName() {
smigielski 10:3a24c970db40 35 return "MPU9250";
smigielski 10:3a24c970db40 36 }
smigielski 10:3a24c970db40 37
smigielski 10:3a24c970db40 38
smigielski 10:3a24c970db40 39 uint32_t MPU9250Sensor::verifyIntegrity(uint32_t* errorResult) {
smigielski 14:cb369746225d 40 LOG("Start verfication of MPU9250 Sensor\r\n");
smigielski 10:3a24c970db40 41 uint32_t errors = 0;
smigielski 11:70359785c2a7 42 //who am I register value is 0x71
smigielski 13:ef0ce8fa871f 43 uint8_t sensorId = readRegister(MPU9250_WHOAMI);
smigielski 10:3a24c970db40 44
smigielski 13:ef0ce8fa871f 45 if (sensorId !=0x71){
smigielski 13:ef0ce8fa871f 46 errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
smigielski 14:cb369746225d 47 LOG("Wrong sensorId: %X\r\n",sensorId);
smigielski 13:ef0ce8fa871f 48 }
smigielski 10:3a24c970db40 49
smigielski 14:cb369746225d 50
smigielski 14:cb369746225d 51 //check magnetometer chip id 0x48
smigielski 14:cb369746225d 52 uint8_t magSensorId = readRegisterI2C(AK8963_WIA);
smigielski 14:cb369746225d 53 if (magSensorId !=0x48){
smigielski 14:cb369746225d 54 errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
smigielski 14:cb369746225d 55 LOG("Wrong magnetometr sensorId: %X\r\n",magSensorId);
smigielski 13:ef0ce8fa871f 56 }
smigielski 10:3a24c970db40 57
smigielski 10:3a24c970db40 58 //perform self test
smigielski 13:ef0ce8fa871f 59 errors+=selfTest(&errorResult[errors]);
smigielski 10:3a24c970db40 60
smigielski 10:3a24c970db40 61 return errors;
smigielski 10:3a24c970db40 62 }
smigielski 10:3a24c970db40 63
smigielski 13:ef0ce8fa871f 64 uint32_t MPU9250Sensor::selfTest(uint32_t* errorResult){
smigielski 13:ef0ce8fa871f 65 uint32_t errors = 0;
smigielski 14:cb369746225d 66 // gyroscope Self-Test
smigielski 14:cb369746225d 67 errors+=gyroscopeSelfTest(&errorResult[errors]);
smigielski 14:cb369746225d 68 // accelerometer’s Self-Test
smigielski 14:cb369746225d 69 errors+=accelerometerSelfTest(&errorResult[errors]);
smigielski 14:cb369746225d 70 // compass Self-Test
smigielski 14:cb369746225d 71 errors+=magnetometerSelfTest(&errorResult[errors]);
smigielski 14:cb369746225d 72
smigielski 14:cb369746225d 73 return errors;
smigielski 14:cb369746225d 74 }
smigielski 14:cb369746225d 75
smigielski 14:cb369746225d 76
smigielski 14:cb369746225d 77 uint32_t MPU9250Sensor::gyroscopeSelfTest(uint32_t* errorResult){
smigielski 14:cb369746225d 78 uint32_t errors = 0;
smigielski 14:cb369746225d 79 int16_t rotation_speed[3],rotation_speed_self_test[3];
smigielski 14:cb369746225d 80 uint8_t selfTest[3];
smigielski 14:cb369746225d 81 float self_test_otp[3],self_test_response[3],gyro_offset[3];
smigielski 14:cb369746225d 82
smigielski 14:cb369746225d 83 //1) The routine starts by measuring the digital output of all three gyroscopes.
smigielski 14:cb369746225d 84 //In order to do this, the following registers are modified:
smigielski 13:ef0ce8fa871f 85
smigielski 14:cb369746225d 86 // Change the digital low pass filter (DLPF) code to 2 (Register Address: 26 (1Ah)
smigielski 14:cb369746225d 87 //Bit [2:0] – USR). The following table details the configuration of the component when the
smigielski 14:cb369746225d 88 //DLPF is configured to 2.
smigielski 14:cb369746225d 89 writeRegister(MPU9250_CONFIG,0x02,0x03);
smigielski 14:cb369746225d 90
smigielski 14:cb369746225d 91 // Store the existing full scale range select code (Register Address: 27 (1Bh) Bit
smigielski 14:cb369746225d 92 //[4:3] – USR) as Old_FS, then select a full scale range of 250dps by setting the
smigielski 14:cb369746225d 93 //ACCEL_FS_SEL bits to 00.
smigielski 14:cb369746225d 94 writeRegister(MPU9250_GYRO_CONFIG,MPU9250_GFS_250DPS,0x03,3);
smigielski 14:cb369746225d 95
smigielski 14:cb369746225d 96 //2) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
smigielski 14:cb369746225d 97 //The averaged values will be the LSB of GX_OS, GY_OS, GZ_OS, AX_OS, AY_OS and
smigielski 14:cb369746225d 98 //AZ_OS in the software.
smigielski 14:cb369746225d 99 averageData(MPU9250_GYRO_XOUT_H,rotation_speed);
smigielski 14:cb369746225d 100
smigielski 14:cb369746225d 101 //3) Set USR_Reg: (1Bh) Gyro_Config, gdrive_axisCTST [0-2] to b111 to enable Self-Test.
smigielski 14:cb369746225d 102 writeRegister(MPU9250_GYRO_CONFIG, 0x07,0x07,5); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
smigielski 14:cb369746225d 103
smigielski 14:cb369746225d 104 //4) Wait 20ms for oscillations to stabilize
smigielski 14:cb369746225d 105 wait_us(20);
smigielski 14:cb369746225d 106
smigielski 14:cb369746225d 107 //5) Read the gyroscope output at a 1kHz rate and average 200 readings.
smigielski 14:cb369746225d 108 //The averaged values will be the LSB of GX_ST_OS, GY_ST_OS, GZ_ST_OS in the software.
smigielski 14:cb369746225d 109 averageData(MPU9250_GYRO_XOUT_H,rotation_speed_self_test);
smigielski 14:cb369746225d 110
smigielski 14:cb369746225d 111 //6) Calculate the Self-Test response as follows:
smigielski 14:cb369746225d 112
smigielski 14:cb369746225d 113 //1) Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software:
smigielski 14:cb369746225d 114 //X-gyro: selftest1 (00): xg_st_data [0-7]
smigielski 14:cb369746225d 115 //Y-gyro: selftest1 (01): yg_st_data [0-7]
smigielski 14:cb369746225d 116 //Z-gyro: selftest1 (02): zg_st_data [0-7]
smigielski 14:cb369746225d 117 readRegisters(MPU9250_SELF_TEST_X_GYRO, 3, &selfTest[0]);
smigielski 14:cb369746225d 118
smigielski 14:cb369746225d 119 //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:
smigielski 14:cb369746225d 120 for (int i =0; i < 3; i++) {
smigielski 14:cb369746225d 121 self_test_response[i] = (float)rotation_speed_self_test[i]-(float)rotation_speed[i];
smigielski 14:cb369746225d 122 self_test_otp[i] = (float)(2620.0f)*(pow( 1.01 , ((float)selfTest[i] - 1.0) ));
smigielski 14:cb369746225d 123 }
smigielski 14:cb369746225d 124
smigielski 14:cb369746225d 125 //3) Determine passing or failing Self-Test
smigielski 14:cb369746225d 126
smigielski 14:cb369746225d 127
smigielski 14:cb369746225d 128 for (int i =0; i < 3; i++) {
smigielski 14:cb369746225d 129 if (self_test_otp[i]!=0){
smigielski 14:cb369746225d 130 //a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response
smigielski 14:cb369746225d 131 //(GXST, GYST, GZST, AXST, AYST and AZST) to the factory Self-Test values
smigielski 14:cb369746225d 132 //(ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled
smigielski 14:cb369746225d 133 if (self_test_response[i]/self_test_otp[i]<0.5F){
smigielski 14:cb369746225d 134 errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
smigielski 14:cb369746225d 135 LOG("3.a Gyro resp %d: %+5.3f\r\n",i,self_test_response[i]);
smigielski 14:cb369746225d 136 }
smigielski 14:cb369746225d 137 } else {
smigielski 14:cb369746225d 138 //b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response
smigielski 14:cb369746225d 139 //(GXST, GYST, GZST, AXST, AYST and AZST) to the ST absolute limits (ST_AL)
smigielski 14:cb369746225d 140 //and report Self-Test is passing if all the following criteria are fulfilled.
smigielski 14:cb369746225d 141 if (abs((float)MPU9250_GFS_250DPS_MULTIPLIER*self_test_response[i])<60){
smigielski 14:cb369746225d 142 errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
smigielski 14:cb369746225d 143 LOG("3.b Gyro resp %d: %+5.3f\r\n",i,self_test_response[i]);
smigielski 14:cb369746225d 144 }
smigielski 14:cb369746225d 145 }
smigielski 14:cb369746225d 146 }
smigielski 14:cb369746225d 147
smigielski 14:cb369746225d 148 //read offset
smigielski 14:cb369746225d 149 readGyroOffset(MPU9250_XG_OFFSET_H, &gyro_offset[0]);
smigielski 14:cb369746225d 150
smigielski 14:cb369746225d 151 //c. If the Self-Test passes criteria (a) and (b), it’s necessary to check gyro offset values.
smigielski 14:cb369746225d 152 //Report passing Self-Test if the following criteria fulfilled.
smigielski 14:cb369746225d 153 for (int i =0; i < 3; i++) {
smigielski 14:cb369746225d 154 if (abs(gyro_offset[i]) < 20.0){
smigielski 14:cb369746225d 155 errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
smigielski 14:cb369746225d 156 LOG("3.c Gyro offset %d: %+5.3f\r\n",i,gyro_offset[i]);
smigielski 14:cb369746225d 157 }
smigielski 14:cb369746225d 158 }
smigielski 14:cb369746225d 159
smigielski 14:cb369746225d 160 //3.1. External Configuration Cleanup
smigielski 14:cb369746225d 161 writeRegister(MPU9250_GYRO_CONFIG, 0x00,0x07,5);
smigielski 14:cb369746225d 162 wait_us(20);
smigielski 14:cb369746225d 163
smigielski 14:cb369746225d 164 return errors;
smigielski 14:cb369746225d 165 }
smigielski 14:cb369746225d 166
smigielski 14:cb369746225d 167
smigielski 14:cb369746225d 168 uint32_t MPU9250Sensor::accelerometerSelfTest(uint32_t* errorResult){
smigielski 14:cb369746225d 169 uint32_t errors = 0;
smigielski 14:cb369746225d 170 int16_t acceleration[3],acceleration_self_test[3];
smigielski 14:cb369746225d 171 uint8_t selfTest[3];
smigielski 14:cb369746225d 172 float self_test_otp[3],self_test_response[3];
smigielski 14:cb369746225d 173
smigielski 14:cb369746225d 174 //1) The routine starts by measuring the digital output of all three gyroscopes.
smigielski 14:cb369746225d 175 //In order to do this, the following registers are modified:
smigielski 14:cb369746225d 176
smigielski 14:cb369746225d 177 //Change the DLPF Code to 2 (Register Address: 29 (1Dh) Bit [2:0] – USR).
smigielski 14:cb369746225d 178 //The following table details the configuration of the component when the DLPF is configured
smigielski 14:cb369746225d 179 //to 2.
smigielski 14:cb369746225d 180
smigielski 14:cb369746225d 181 writeRegister(MPU9250_ACCEL_CONFIG_2,0x02,0x03 );
smigielski 14:cb369746225d 182
smigielski 14:cb369746225d 183 //Store the existing full scale range select code (Register Address: 28 (1Ch)
smigielski 14:cb369746225d 184 //Bit [4:3] – USR) as Old_FS, then select a full scale range of ±2g by setting the
smigielski 14:cb369746225d 185 //ACCEL_FS_SEL bits to 00.
smigielski 14:cb369746225d 186 writeRegister(MPU9250_ACCEL_CONFIG,MPU9250_AFS_2G,0x03,3);
smigielski 14:cb369746225d 187
smigielski 14:cb369746225d 188 //2) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
smigielski 14:cb369746225d 189 //The averaged values will be the LSB of GX_OS, GY_OS, GZ_OS, AX_OS, AY_OS and
smigielski 14:cb369746225d 190 //AZ_OS in the software.
smigielski 14:cb369746225d 191 averageData(MPU9250_ACCEL_XOUT_H,acceleration);
smigielski 14:cb369746225d 192
smigielski 14:cb369746225d 193 //3) Set USR_Reg: (1Ch) Accel_Config, AX/Y/Z_ST_EN [0-2] to b111 to enable Self-Test.
smigielski 14:cb369746225d 194 writeRegister(MPU9250_ACCEL_CONFIG, 0x07,0x07,5);
smigielski 14:cb369746225d 195
smigielski 14:cb369746225d 196 //4) Wait 20ms for oscillations to stabilize
smigielski 14:cb369746225d 197 wait_us(20);
smigielski 14:cb369746225d 198
smigielski 14:cb369746225d 199 //5) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
smigielski 14:cb369746225d 200 //The averaged values will be the LSB of AX_ST_OS, AY_ST_OS and AZ_ST_OS in the software.
smigielski 14:cb369746225d 201 averageData(MPU9250_ACCEL_XOUT_H,acceleration_self_test);
smigielski 14:cb369746225d 202
smigielski 14:cb369746225d 203 //6) Calculate the Self-Test response as follows:
smigielski 14:cb369746225d 204
smigielski 14:cb369746225d 205 //1) Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software:
smigielski 14:cb369746225d 206 //X-gyro: selftest1 (00): xg_st_data [0-7]
smigielski 14:cb369746225d 207 //Y-gyro: selftest1 (01): yg_st_data [0-7]
smigielski 14:cb369746225d 208 //Z-gyro: selftest1 (02): zg_st_data [0-7]
smigielski 14:cb369746225d 209 readRegisters(MPU9250_SELF_TEST_X_ACCEL, 3, &selfTest[0]);
smigielski 14:cb369746225d 210
smigielski 14:cb369746225d 211 //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:
smigielski 14:cb369746225d 212 for (int i =0; i < 3; i++) {
smigielski 14:cb369746225d 213 self_test_response[i] = (float)acceleration_self_test[i]-(float)acceleration[i];
smigielski 14:cb369746225d 214 self_test_otp[i] = (float)(2620.0)*(pow( 1.01 , ((float)selfTest[i] - 1.0) ));
smigielski 14:cb369746225d 215 }
smigielski 14:cb369746225d 216
smigielski 14:cb369746225d 217
smigielski 14:cb369746225d 218 //3) Determine passing or failing Self-Test
smigielski 14:cb369746225d 219 for (int i =0; i < 3; i++) {
smigielski 14:cb369746225d 220 if (self_test_otp[i]!=0){
smigielski 14:cb369746225d 221 //a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response
smigielski 14:cb369746225d 222 //(GXST, GYST, GZST, AXST, AYST and AZST) to the factory Self-Test values
smigielski 14:cb369746225d 223 //(ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled
smigielski 14:cb369746225d 224 float ratio = self_test_response[i]/self_test_otp[i];
smigielski 14:cb369746225d 225 if (ratio<0.5F || ratio > 1.5f){
smigielski 14:cb369746225d 226 errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
smigielski 14:cb369746225d 227 LOG("3.a Acc resp %d: %+5.3f\r\n",i,self_test_response[i]);
smigielski 14:cb369746225d 228 }
smigielski 14:cb369746225d 229 } else {
smigielski 14:cb369746225d 230 //b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response
smigielski 14:cb369746225d 231 //(GXST, GYST, GZST, AXST, AYST and AZST) to the ST absolute limits (ST_AL)
smigielski 14:cb369746225d 232 //and report Self-Test is passing if all the following criteria are fulfilled.
smigielski 14:cb369746225d 233 float response=MPU9250_AFS_2G_MULTIPLIER*self_test_response[i];
smigielski 14:cb369746225d 234 if (abs(response)<0.225f || abs(response)>0.675f ){
smigielski 14:cb369746225d 235 errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
smigielski 14:cb369746225d 236 LOG("3.b Acc resp %d: %+5.3f\r\n",i,self_test_response[i]);
smigielski 14:cb369746225d 237 }
smigielski 14:cb369746225d 238 }
smigielski 14:cb369746225d 239 }
smigielski 14:cb369746225d 240
smigielski 14:cb369746225d 241
smigielski 14:cb369746225d 242 //3.1. External Configuration Cleanup
smigielski 14:cb369746225d 243 writeRegister(MPU9250_ACCEL_CONFIG, 0x00,0x07,5);
smigielski 14:cb369746225d 244 wait_us(20);
smigielski 14:cb369746225d 245
smigielski 14:cb369746225d 246 return errors;
smigielski 14:cb369746225d 247 }
smigielski 14:cb369746225d 248
smigielski 14:cb369746225d 249 uint32_t MPU9250Sensor::magnetometerSelfTest(uint32_t* errorResult){
smigielski 14:cb369746225d 250 uint32_t errors = 0;
smigielski 14:cb369746225d 251 uint8_t rawData[7];
smigielski 14:cb369746225d 252 int16_t magnetometer[3];
smigielski 14:cb369746225d 253 float self_test_response[3];
smigielski 14:cb369746225d 254 //1. Set the compass into power-down mode.
smigielski 14:cb369746225d 255 writeRegisterI2C(AK8963_CNTL, 0x00);
smigielski 14:cb369746225d 256
smigielski 14:cb369746225d 257 //2. Write “1” to the SELF bit of the ASTC register. Other bits in this register should be set to zero.
smigielski 14:cb369746225d 258 writeRegisterI2C(AK8963_ASTC, 0x01,0x01,6);
smigielski 14:cb369746225d 259
smigielski 14:cb369746225d 260 //3. Set the self test mode in the “Mode” register.
smigielski 14:cb369746225d 261 writeRegisterI2C(AK8963_CNTL, 0x08);
smigielski 14:cb369746225d 262
smigielski 14:cb369746225d 263 //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.
smigielski 14:cb369746225d 264 while (!(readRegisterI2C(AK8963_ST1) & 0x01)){
smigielski 14:cb369746225d 265 wait_us(1);
smigielski 14:cb369746225d 266 }
smigielski 14:cb369746225d 267
smigielski 14:cb369746225d 268 //5. Read the measurement data in the compass measurement data registers.
smigielski 14:cb369746225d 269 readRegistersI2C(MPU9250_EXT_SENS_DATA_00, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
smigielski 14:cb369746225d 270 uint8_t c = rawData[6]; // End data read by reading ST2 register
smigielski 14:cb369746225d 271 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
smigielski 14:cb369746225d 272 magnetometer[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
smigielski 14:cb369746225d 273 magnetometer[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
smigielski 14:cb369746225d 274 magnetometer[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
smigielski 14:cb369746225d 275 } else {
smigielski 14:cb369746225d 276 magnetometer[0] = 0xffff;
smigielski 14:cb369746225d 277 magnetometer[1] = 0xffff;
smigielski 14:cb369746225d 278 magnetometer[2] = 0xffff;
smigielski 14:cb369746225d 279 }
smigielski 14:cb369746225d 280
smigielski 14:cb369746225d 281 //6. Write “0” to SELF bit of the ASTC register.
smigielski 14:cb369746225d 282 writeRegisterI2C(AK8963_ASTC, 0x00,0x01,6);
smigielski 14:cb369746225d 283
smigielski 14:cb369746225d 284 //7. Set the compass to power-down mode.
smigielski 14:cb369746225d 285 writeRegisterI2C(AK8963_CNTL, 0x00);
smigielski 14:cb369746225d 286
smigielski 14:cb369746225d 287 //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:
smigielski 14:cb369746225d 288 for (int i =0; i < 3; i++) {
smigielski 14:cb369746225d 289 self_test_response[i] = (float)magnetometer[i]*magnetometerCalibration[i]*MPU9250M_4800uT;
smigielski 14:cb369746225d 290 }
smigielski 14:cb369746225d 291
smigielski 14:cb369746225d 292 if(!(c & 0x10)) {
smigielski 14:cb369746225d 293 //Set Pass/Fail Criteria 16bit
smigielski 14:cb369746225d 294 if (self_test_response[0]<-200.0 || self_test_response[0] > 200.0){
smigielski 14:cb369746225d 295 errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
smigielski 14:cb369746225d 296 LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[0]);
smigielski 14:cb369746225d 297 }
smigielski 14:cb369746225d 298 if (self_test_response[1]<-200.0 || self_test_response[1] > 200.0){
smigielski 14:cb369746225d 299 errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
smigielski 14:cb369746225d 300 LOG("Magn resp %d: %+5.3f\r\n",1,self_test_response[1]);
smigielski 14:cb369746225d 301 }
smigielski 14:cb369746225d 302 if (self_test_response[2]<-3200.0 || self_test_response[2] > 800.0){
smigielski 14:cb369746225d 303 errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
smigielski 14:cb369746225d 304 LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[2]);
smigielski 14:cb369746225d 305 }
smigielski 14:cb369746225d 306 } else {
smigielski 14:cb369746225d 307 //Set Pass/Fail Criteria 14bit
smigielski 14:cb369746225d 308 if (self_test_response[0]<-50.0 || self_test_response[0] > 50.0){
smigielski 14:cb369746225d 309 errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
smigielski 14:cb369746225d 310 LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[0]);
smigielski 14:cb369746225d 311 }
smigielski 14:cb369746225d 312 if (self_test_response[1]<-50.0 || self_test_response[1] > 50.0){
smigielski 14:cb369746225d 313 errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
smigielski 14:cb369746225d 314 LOG("Magn resp %d: %+5.3f\r\n",1,self_test_response[1]);
smigielski 14:cb369746225d 315 }
smigielski 14:cb369746225d 316 if (self_test_response[2]<-800.0 || self_test_response[2] > 200.0){
smigielski 14:cb369746225d 317 errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
smigielski 14:cb369746225d 318 LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[2]);
smigielski 14:cb369746225d 319 }
smigielski 14:cb369746225d 320 }
smigielski 14:cb369746225d 321
smigielski 14:cb369746225d 322 return errors;
smigielski 13:ef0ce8fa871f 323 }
smigielski 13:ef0ce8fa871f 324
smigielski 10:3a24c970db40 325 void MPU9250Sensor::getSensorDetails(sensor_t* sensorDetails) {
smigielski 10:3a24c970db40 326
smigielski 10:3a24c970db40 327 }
smigielski 11:70359785c2a7 328
smigielski 14:cb369746225d 329 void MPU9250Sensor::readGyroOffset(uint8_t reg, float* data){
smigielski 14:cb369746225d 330 uint8_t rawData[6];
smigielski 14:cb369746225d 331 readRegisters(reg, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
smigielski 14:cb369746225d 332 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
smigielski 14:cb369746225d 333 data[1] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
smigielski 14:cb369746225d 334 data[2] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
smigielski 14:cb369746225d 335 }
smigielski 14:cb369746225d 336
smigielski 14:cb369746225d 337 void MPU9250Sensor::averageData(uint8_t reg,int16_t* data){
smigielski 14:cb369746225d 338 uint8_t rawData[6];
smigielski 14:cb369746225d 339 for( int i = 0; i < SELF_TEST_COUNT; i++) { // get average current values of gyro
smigielski 14:cb369746225d 340 readRegisters(reg, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
smigielski 14:cb369746225d 341 data[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
smigielski 14:cb369746225d 342 data[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
smigielski 14:cb369746225d 343 data[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
smigielski 14:cb369746225d 344 }
smigielski 14:cb369746225d 345
smigielski 14:cb369746225d 346 for (int i =0; i < 3; i++) { // Get average of 200 values and store as average current readings
smigielski 14:cb369746225d 347 data[i] /= SELF_TEST_COUNT;
smigielski 14:cb369746225d 348 }
smigielski 14:cb369746225d 349 }
smigielski 14:cb369746225d 350
smigielski 13:ef0ce8fa871f 351 uint8_t MPU9250Sensor::readRegister( uint8_t reg){
smigielski 13:ef0ce8fa871f 352 cs = DOWN;
smigielski 13:ef0ce8fa871f 353 spi.write(reg| MPU9250_READ_FLAG);
smigielski 13:ef0ce8fa871f 354 uint8_t val = spi.write(0x00);
smigielski 13:ef0ce8fa871f 355 cs = UP;
smigielski 13:ef0ce8fa871f 356 return val;
smigielski 13:ef0ce8fa871f 357 }
smigielski 14:cb369746225d 358
smigielski 14:cb369746225d 359 uint8_t MPU9250Sensor::readRegisterI2C(uint8_t reg){
smigielski 14:cb369746225d 360 cs = DOWN;
smigielski 14:cb369746225d 361 spi.write(MPU9250_I2C_SLV0_ADDR);
smigielski 14:cb369746225d 362 spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
smigielski 14:cb369746225d 363 spi.write(MPU9250_I2C_SLV0_REG);
smigielski 14:cb369746225d 364 spi.write(reg);
smigielski 14:cb369746225d 365 spi.write(MPU9250_I2C_SLV0_CTRL);
smigielski 14:cb369746225d 366 spi.write(0x81);
smigielski 14:cb369746225d 367 spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
smigielski 14:cb369746225d 368 uint8_t val = spi.write(0x00);
smigielski 14:cb369746225d 369 cs = UP;
smigielski 14:cb369746225d 370 return val;
smigielski 14:cb369746225d 371 }
smigielski 14:cb369746225d 372
smigielski 14:cb369746225d 373 void MPU9250Sensor::readRegisters(uint8_t reg, uint8_t count, uint8_t * dest){
smigielski 14:cb369746225d 374 cs = DOWN;
smigielski 14:cb369746225d 375 spi.write(reg| MPU9250_READ_FLAG);
smigielski 14:cb369746225d 376 for(int i = 0; i < count; i++) {
smigielski 14:cb369746225d 377 dest[i] = spi.write(0x00);
smigielski 14:cb369746225d 378 }
smigielski 14:cb369746225d 379 cs = UP;
smigielski 14:cb369746225d 380 }
smigielski 14:cb369746225d 381
smigielski 14:cb369746225d 382 void MPU9250Sensor::readRegistersI2C(uint8_t reg, uint8_t count, uint8_t * dest){
smigielski 14:cb369746225d 383 cs = DOWN;
smigielski 14:cb369746225d 384 spi.write(MPU9250_I2C_SLV0_ADDR);
smigielski 14:cb369746225d 385 spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
smigielski 14:cb369746225d 386 spi.write(MPU9250_I2C_SLV0_REG);
smigielski 14:cb369746225d 387 spi.write(reg);
smigielski 14:cb369746225d 388 spi.write(MPU9250_I2C_SLV0_CTRL);
smigielski 14:cb369746225d 389 spi.write(0x80| count);
smigielski 14:cb369746225d 390 spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
smigielski 14:cb369746225d 391 for(int i = 0; i < count; i++) {
smigielski 14:cb369746225d 392 dest[i] = spi.write(0x00);
smigielski 14:cb369746225d 393 }
smigielski 14:cb369746225d 394 cs = UP;
smigielski 14:cb369746225d 395 }
smigielski 14:cb369746225d 396
smigielski 14:cb369746225d 397 void MPU9250Sensor::writeRegister( uint8_t reg, uint8_t data, uint8_t mask,uint8_t pos){
smigielski 14:cb369746225d 398 cs = DOWN;
smigielski 14:cb369746225d 399 spi.write(reg| MPU9250_READ_FLAG);
smigielski 14:cb369746225d 400 uint8_t val = spi.write(0x00);
smigielski 14:cb369746225d 401 spi.write(reg);
smigielski 14:cb369746225d 402 spi.write(val & ~(mask<<pos) | (data<<pos));
smigielski 14:cb369746225d 403 cs = UP;
smigielski 14:cb369746225d 404 }
smigielski 14:cb369746225d 405
smigielski 14:cb369746225d 406 void MPU9250Sensor::writeRegisterI2C( uint8_t reg, uint8_t data, uint8_t mask,uint8_t pos){
smigielski 14:cb369746225d 407 uint8_t val;
smigielski 14:cb369746225d 408 cs = DOWN;
smigielski 14:cb369746225d 409 spi.write(MPU9250_I2C_SLV0_ADDR);
smigielski 14:cb369746225d 410 spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
smigielski 14:cb369746225d 411 spi.write(MPU9250_I2C_SLV0_REG);
smigielski 14:cb369746225d 412 spi.write(reg);
smigielski 14:cb369746225d 413 spi.write(MPU9250_I2C_SLV0_CTRL);
smigielski 14:cb369746225d 414 spi.write(0x81);
smigielski 14:cb369746225d 415 spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
smigielski 14:cb369746225d 416 val = spi.write(0x00);
smigielski 14:cb369746225d 417 spi.write(MPU9250_I2C_SLV0_ADDR);
smigielski 14:cb369746225d 418 spi.write(AK8963_I2C_ADDR);
smigielski 14:cb369746225d 419 spi.write(MPU9250_I2C_SLV0_DO);
smigielski 14:cb369746225d 420 val = spi.write(val & ~(mask<<pos) | (data<<pos));
smigielski 14:cb369746225d 421 cs = UP;
smigielski 14:cb369746225d 422 }