MPU6050.cpp

00001 #include "MPU6050.h"
00002 
00003 /**
00004  * @brief   Creates an MPU6050 object
00005  * @note    Default address is 0x68
00006  *          Default acceleration range 2G
00007  *          Default gyroscope range is 250 dps (degrees per second)
00008  *          I2C_SDA and I2C_SCL pins are used for I2C connection by default
00009  *          Interrupt pin is not connected (NC) by default
00010  * @param
00011  * @retval
00012  */
00013 MPU6050::MPU6050
00014 (
00015     uint8_t     addr /*= 0x68*/,
00016     AccelScale  accelScale /*= AFS_2G*/,
00017     GyroScale   gyroScale /*= GFS_250DPS*/,
00018     PinName     sdaPin /*= I2C_SDA*/,
00019     PinName     sclPin /*= I2C_SCL*/,
00020     PinName     interruptInPin  /*= NC*/
00021 ) :
00022     _addr(addr << 1),
00023     _accelScale(accelScale),
00024     _gyroScale(gyroScale),
00025     _i2c(sdaPin, sclPin),
00026     _interruptIn(interruptInPin),
00027     _accelRes(0),
00028     _gyroRes(0),
00029     _beta(sqrt(3.0f / 4.0f) * _gyroError),
00030     _zeta(sqrt(3.0f / 4.0f) * _gyroDrift)
00031 {
00032     _i2c.frequency(400000); // 400 kHz
00033 }
00034 
00035 /**
00036  * @brief
00037  * @note
00038  * @param
00039  * @retval
00040  */
00041 void MPU6050::rise(Callback<void (void)> func)
00042 {
00043     _interruptIn.rise(func);
00044 }
00045 
00046 /**
00047  * @brief
00048  * @note
00049  * @param
00050  * @retval
00051  */
00052 template<typename T>
00053 void MPU6050::rise(T* tptr, void (T:: *mptr) (void))
00054 {
00055     if ((mptr != NULL) && (tptr != NULL))
00056         _interruptIn.rise(tptr, &T::mptr);
00057 }
00058 
00059 /**
00060  * @brief
00061  * @note
00062  * @param
00063  * @retval
00064  */
00065 void MPU6050::fall(Callback<void (void)> func)
00066 {
00067     _interruptIn.fall(func);
00068 }
00069 
00070 /**
00071  * @brief
00072  * @note
00073  * @param
00074  * @retval
00075  */
00076 template<typename T>
00077 void MPU6050::fall(T* tptr, void (T:: *mptr) (void))
00078 {
00079     if ((mptr != NULL) && (tptr != NULL))
00080         _interruptIn.fall(tptr, &T::mptr);
00081 }
00082 
00083 /**
00084  * @brief   Writes a byte to register
00085  * @note
00086  * @param
00087  * @retval
00088  */
00089 void MPU6050::writeReg(uint8_t reg, uint8_t byte)
00090 {
00091     char    data[2];
00092 
00093     data[0] = reg;
00094     data[1] = byte;
00095     _i2c.write(_addr, data, sizeof(data), false);
00096 }
00097 
00098 /**
00099  * @brief   Reads byte from a register
00100  * @note
00101  * @param
00102  * @retval
00103  */
00104 uint8_t MPU6050::readReg(uint8_t reg)
00105 {
00106     char    ret;                                    // `ret` will store the register data
00107 
00108     _i2c.write(_addr, (const char*) &reg, 1, 1);    // no stop
00109     _i2c.read(_addr, &ret, 1, 0);
00110     return ret;
00111 }
00112 
00113 /**
00114  * @brief   Read multiple bytes from a register
00115  * @note
00116  * @param
00117  * @retval
00118  */
00119 void MPU6050::readRegBytes(uint8_t reg, uint8_t len, uint8_t* dest)
00120 {
00121     _i2c.write(_addr, (const char*) &reg, 1, 1);    // no stop
00122     _i2c.read(_addr, (char*)dest, len, 0);
00123 }
00124 
00125 /**
00126  * @brief   Initializes the MPU6050
00127  * @note
00128  * @param
00129  * @retval
00130  */
00131 bool MPU6050::init()
00132 {
00133     uint8_t c;
00134 
00135     writeReg(PWR_MGMT_1, 0x00); // Select internal OSC as clock source, clear sleep mode bit (6), enable all sensors
00136 
00137     // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
00138 #if MBED_MAJOR_VERSION > 5
00139     ThisThread::sleep_for(100ms);
00140 #else
00141     wait_ms(100);
00142 #endif
00143     //
00144 
00145     // get stable time source
00146     writeReg(PWR_MGMT_1, 0x01); // Select PLL with x-axis gyroscope reference as clock source, bits 2:0 = 001
00147 
00148     // Configure Gyro and Accelerometer
00149     // DLPF_CFG = bits 2:0 = 0x03 (0b010)
00150     // Sets the sample rate to 1 kHz for Low Pass Filter (DLPF) for both the accelerometers and gyros
00151     // disables FSYNC and sets accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
00152     // Maximum delay is 4.9 ms (= 204.08 Hz) which is just over a 200 Hz maximum rate
00153     writeReg(CONFIG, 0x03);
00154 
00155     // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
00156     // Where gyroscope output rate = 1kHz when the DLPF is disabled (DLPF_CFG = 0 or 7)
00157     // Use a 200 Hz rate; the same rate set in CONFIG above: 200 = 1000/(1 + 4)
00158     writeReg(SMPLRT_DIV, 0x04);
00159 
00160     // Set gyroscope full scale range
00161     c = readReg(GYRO_CONFIG);   // Read the GYRO_CONFIG register
00162     c &= 0b11100000;            // Clear self-test bits [7:5]
00163 
00164     // Range selectors FS_SEL is 0 - 3, so left-shift 2-bit value into positions 4:3
00165     c |= (_gyroScale << 3);     // Set FS_SEL bits [4:3]
00166     writeReg(GYRO_CONFIG, c);   // Set the full scale range for the gyro
00167 
00168     // Update gyro scale according to the selected gyro full scale range.
00169     // Possible gyro scales (and their register bit settings) are:
00170     // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11).
00171     // Set DPS/(ADC tick) based on that 2-bit value:
00172     switch (_gyroScale) {
00173         case GFS_250DPS:
00174             _gyroRes = 250.0 / 32768.0;
00175             break;
00176 
00177         case GFS_500DPS:
00178             _gyroRes = 500.0 / 32768.0;
00179             break;
00180 
00181         case GFS_1000DPS:
00182             _gyroRes = 1000.0 / 32768.0;
00183             break;
00184 
00185         case GFS_2000DPS:
00186             _gyroRes = 2000.0 / 32768.0;
00187             break;
00188     }
00189 
00190     // Set accelerometer configuration
00191     c = readReg(ACCEL_CONFIG);  // Read the ACCEL_CONFIG register
00192     c &= 0b11100000;            // Clear self-test bits [7:5]
00193 
00194     // Range selectors AFS_SEL is 0 - 3, so left-shift 2-bit value into positions 4:3
00195     c |= (_accelScale << 3);
00196     writeReg(ACCEL_CONFIG, c);  // Set full scale range for the accelerometer
00197 
00198     // Update acceleration scle to the selected acceleration scale range.
00199     // Possible accelerometer scales (and their register bit settings) are:
00200     // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11).
00201     // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
00202     switch (_accelScale) {
00203         case AFS_2G:
00204             _accelRes = 2.0 / 32768.0;
00205             break;
00206 
00207         case AFS_4G:
00208             _accelRes = 4.0 / 32768.0;
00209             break;
00210 
00211         case AFS_8G:
00212             _accelRes = 8.0 / 32768.0;
00213             break;
00214 
00215         case AFS_16G:
00216             _accelRes = 16.0 / 32768.0;
00217             break;
00218     }
00219 
00220     // Configure Interrupts and Bypass Enable
00221     // When bit [7] (INT_LEVEL) is equal to 0, the logic level for the INT pin is active high.
00222     // When bit [6] (INT_OPEN) is equal to 0, the INT pin is configured as push-pull.
00223     // When bit [5] (LATCH_INT_EN) is equal to 1, the INT pin is held high until the interrupt is cleared.
00224     // When bit [4] (INT_RD_CLEAR) is equal to 0, interrupt status bits are cleared only by reading INT_STATUS
00225     // When bit [3] (FSYNC_INT_LEVEL) is equal to 0, the logic level for the FSYNC pin
00226     // (when used as an interrupt to the host processor) is active high.
00227     // When bit [2] (FSYNC_INT_EN) is equal to 0, this bit disables the FSYNC pin from causing
00228     // an interrupt to the host processor.
00229     // When bit [1] (I2C_BYPASS_EN) is equal to 1 and bit [5] (I2C_MST_EN) in the USER_CTRL register) is equal to 0,
00230     // the host application processor will be able to directly access the auxiliary I2C bus of the MPU-60X0.
00231     writeReg(INT_PIN_CFG, 0b00100010);
00232 
00233     // Clear bit [5] in the USER_CTRL register
00234     c = readReg(USER_CTRL);
00235     c &= ~(1 << I2C_MST_EN);
00236     writeReg(USER_CTRL, c);
00237 
00238     // Enable data ready interrupt (bit [0])
00239     writeReg(INT_ENABLE, 0x01);
00240 
00241     return true;
00242 }
00243 
00244 /**
00245  * @brief   Indicates whether data is available after an interrupt
00246  * @note    The DATA_RDY_INT bit clears to 0 after the INT_STATUS register has been read.
00247  * @param
00248  * @retval
00249  */
00250 bool MPU6050::dataReady()
00251 {
00252     return(readReg(INT_STATUS) & (1 << DATA_RDY_INT));
00253 }
00254 
00255 /**
00256  * @brief   Reads acceleration ADC (analog to digital converter)
00257  * @note
00258  * @param
00259  * @retval  Acceleration ADC array of three elements
00260  */
00261 int16_t* MPU6050::accelADC()
00262 {
00263     uint8_t rawData[6];                                     // x/y/z accel register data stored here
00264 
00265     readRegBytes(ACCEL_XOUT_H, 6, rawData);                 // Read the six raw data registers into data array
00266     _accelAdc[0] = int16_t(rawData[0]) << 8 | rawData[1];   // Turn the MSB and LSB into a signed 16-bit value
00267     _accelAdc[1] = int16_t(rawData[2]) << 8 | rawData[3];
00268     _accelAdc[2] = int16_t(rawData[4]) << 8 | rawData[5];
00269 
00270     return _accelAdc;
00271 }
00272 
00273 /**
00274  * @brief   Reads gyro ADC (analog to digital converter)
00275  * @note
00276  * @param
00277  * @retval  Gyro ADC array of three elements
00278  */
00279 int16_t* MPU6050::gyroADC()
00280 {
00281     uint8_t rawData[6];                                     // x/y/z gyro register data stored here
00282 
00283     readRegBytes(GYRO_XOUT_H, 6, rawData);                  // Read the six raw data registers sequentially into data array
00284     _gyroAdc[0] = int16_t(rawData[0]) << 8 | rawData[1];    // Turn the MSB and LSB into a signed 16-bit value
00285     _gyroAdc[1] = int16_t(rawData[2]) << 8 | rawData[3];
00286     _gyroAdc[2] = int16_t(rawData[4]) << 8 | rawData[5];
00287 
00288     return _gyroAdc;
00289 }
00290 
00291 /**
00292  * @brief   Reads temperature ADC (analog to digital converter)
00293  * @note
00294  * @param
00295  * @retval  Temperature ADC value
00296  */
00297 int16_t MPU6050::tempADC()
00298 {
00299     uint8_t rawData[2];                             // x/y/z gyro register data stored here
00300 
00301     readRegBytes(TEMP_OUT_H, 2, &rawData[0]);       // Read the two raw data registers sequentially into data array
00302     return int16_t(rawData[0]) << 8 | rawData[1];   // Turn the MSB and LSB into a 16-bit value
00303 }
00304 
00305 /**
00306  * @brief
00307  * @note
00308  * @param
00309  * @retval
00310  */
00311 float MPU6050::temp()
00312 {
00313     return(tempADC() / 340. + 36.53);   // Temperature in degrees Celsius
00314 }
00315 
00316 /**
00317  * @brief   Configures the motion detection control for low power accelerometer mode
00318  * @note    The sensor has a high-pass filter necessary to invoke to allow the sensor motion detection algorithms work properly
00319  *          Motion detection occurs on free-fall (acceleration below a threshold for some time for all axes), motion (acceleration
00320  *          above a threshold for some time on at least one axis), and zero-motion toggle (acceleration on each axis less than a
00321  *          threshold for some time sets this flag, motion above the threshold turns it off). The high-pass filter takes gravity out
00322  *          consideration for these threshold evaluations; otherwise, the flags would be set all the time!
00323  * @param
00324  * @retval
00325  */
00326 void MPU6050::lowPowerAccelOnly()
00327 {
00328     uint8_t c = readReg(PWR_MGMT_1);
00329     writeReg(PWR_MGMT_1, c &~0x30);     // Clear sleep and cycle bits [5:6]
00330     writeReg(PWR_MGMT_1, c | 0x30);     // Set sleep and cycle bits [5:6] to zero to make sure accelerometer is running
00331     c = readReg(PWR_MGMT_2);
00332     writeReg(PWR_MGMT_2, c &~0x38);     // Clear standby XA, YA, and ZA bits [3:5]
00333     writeReg(PWR_MGMT_2, c | 0x00);     // Set XA, YA, and ZA bits [3:5] to zero to make sure accelerometer is running
00334     c = readReg(ACCEL_CONFIG);
00335     writeReg(ACCEL_CONFIG, c &~0x07);   // Clear high-pass filter bits [2:0]
00336 
00337     // Set high-pass filter to 0) reset (disable), 1) 5 Hz, 2) 2.5 Hz, 3) 1.25 Hz, 4) 0.63 Hz, or 7) Hold
00338     writeReg(ACCEL_CONFIG, c | 0x00);   // Set ACCEL_HPF to 0; reset mode disbaling high-pass filter
00339     c = readReg(CONFIG);
00340     writeReg(CONFIG, c &~0x07);         // Clear low-pass filter bits [2:0]
00341     writeReg(CONFIG, c | 0x00);         // Set DLPD_CFG to 0; 260 Hz bandwidth, 1 kHz rate
00342     c = readReg(INT_ENABLE);
00343     writeReg(INT_ENABLE, c &~0xFF);     // Clear all interrupts
00344     writeReg(INT_ENABLE, 0x40);         // Enable motion threshold (bits 5) interrupt only
00345 
00346     // Motion detection interrupt requires the absolute value of any axis to lie above the detection threshold
00347     // for at least the counter duration
00348     writeReg(MOT_THR, 0x80);            // Set motion detection to 0.256 g; LSB = 2 mg
00349     writeReg(MOT_DUR, 0x01);            // Set motion detect duration to 1  ms; LSB is 1 ms @ 1 kHz rate
00350 
00351     // Add delay for accumulation of samples
00352 #if MBED_MAJOR_VERSION > 5
00353     ThisThread::sleep_for(100ms);
00354 #else
00355     wait_ms(100);
00356 #endif
00357     c = readReg(ACCEL_CONFIG);
00358     writeReg(ACCEL_CONFIG, c &~0x07);   // Clear high-pass filter bits [2:0]
00359     writeReg(ACCEL_CONFIG, c | 0x07);   // Set ACCEL_HPF to 7; hold the initial accleration value as a referance
00360     c = readReg(PWR_MGMT_2);
00361     writeReg(PWR_MGMT_2, c &~0xC7);     // Clear standby XA, YA, and ZA bits [3:5] and LP_WAKE_CTRL bits [6:7]
00362     writeReg(PWR_MGMT_2, c | 0x47);     // Set wakeup frequency to 5 Hz, and disable XG, YG, and ZG gyros (bits [0:2])
00363     c = readReg(PWR_MGMT_1);
00364     writeReg(PWR_MGMT_1, c &~0x20);     // Clear sleep and cycle bit 5
00365     writeReg(PWR_MGMT_1, c | 0x20);     // Set cycle bit 5 to begin low power accelerometer motion interrupts
00366 }
00367 
00368 /**
00369  * @brief   Resets the MPU6050
00370  * @note
00371  * @param
00372  * @retval
00373  */
00374 void MPU6050::reset()
00375 {
00376     // Reset the device
00377 
00378     writeReg(PWR_MGMT_1, 0b10000000);   // Write a one to bit 7 (reset bit);
00379 #if MBED_MAJOR_VERSION > 5
00380     ThisThread::sleep_for(100ms);
00381 #else
00382     wait_ms(100);
00383 #endif
00384 }
00385 
00386 /**
00387  * @brief   Calibrates the MPU6050
00388  * @note    Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
00389  *          of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
00390  * @param
00391  * @retval
00392  */
00393 void MPU6050::calibrate()
00394 {
00395     uint8_t     data[12];                       // data array to hold accelerometer and gyro x, y, z, data
00396     uint16_t    packetCount, fifoCount;
00397     int32_t     gyroBias[3] = { 0, 0, 0 };
00398     int32_t     accelBias[3] = { 0, 0, 0 };
00399 
00400     // Reset the device, reset all registers, clear gyro and accelerometer bias registers
00401 
00402     reset();
00403 
00404     // Get stable time source: set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
00405     writeReg(PWR_MGMT_1, 0x01);
00406     writeReg(PWR_MGMT_2, 0x00);
00407 #if MBED_MAJOR_VERSION > 5
00408     ThisThread::sleep_for(200ms);
00409 #else
00410     wait_ms(200);
00411 #endif
00412     //
00413 
00414     // Configure the device for bias calculation
00415     writeReg(INT_ENABLE, 0x00);                 // Disable all interrupts
00416     writeReg(FIFO_EN, 0x00);                    // Disable FIFO
00417     writeReg(PWR_MGMT_1, 0x00);                 // Turn on internal clock source
00418     writeReg(I2C_MST_CTRL, 0x00);               // Disable I2C master
00419     writeReg(USER_CTRL, 0x00);                  // Disable FIFO and I2C master modes
00420     writeReg(USER_CTRL, 0x0C);                  // Reset FIFO and DMP
00421 #if MBED_MAJOR_VERSION > 5
00422     ThisThread::sleep_for(200ms);
00423 #else
00424     wait_ms(200);
00425 #endif
00426     // Configure MPU6050 gyro and accelerometer for bias calculation
00427 
00428     writeReg(CONFIG, 0x01);                     // Set low-pass filter to 188 Hz
00429     writeReg(SMPLRT_DIV, 0x00);                 // Set sample rate to 1 kHz
00430     writeReg(GYRO_CONFIG, 0x00);                // Set gyro full-scale to 250 degrees per second, maximum sensitivity
00431     writeReg(ACCEL_CONFIG, 0x00);               // Set accelerometer full-scale to 2 g, maximum sensitivity
00432     uint16_t    gyroSensitivity = 131;          // = 131 LSB/degrees/sec
00433     uint16_t    accelSensitivity = 16384;       // = 16384 LSB/g
00434 
00435     // Configure FIFO to capture accelerometer and gyro data for bias calculation
00436     writeReg(USER_CTRL, 0x40);                  // Enable FIFO
00437     writeReg(FIFO_EN, 0x78);                    // Enable gyro and accelerometer sensors for FIFO  (max size 1024 bytes in MPU-6050)
00438 
00439     // accumulate 80 samples in 80 milliseconds = 960 bytes
00440 #if MBED_MAJOR_VERSION > 5
00441     ThisThread::sleep_for(80ms);
00442 #else
00443     wait_ms(80);
00444 #endif
00445     //
00446 
00447     // At end of sample accumulation, turn off FIFO sensor read
00448     writeReg(FIFO_EN, 0x00);                    // Disable gyro and accelerometer sensors for FIFO
00449     readRegBytes(FIFO_COUNTH, 2, &data[0]);     // read FIFO sample count
00450     fifoCount = ((uint16_t) data[0] << 8) | data[1];
00451     packetCount = fifoCount / 12;               // How many sets of full gyro and accelerometer data for averaging
00452     for (int i = 0; i < packetCount; i++) {
00453         int16_t accel_temp[3] = { 0, 0, 0 },
00454         gyro_temp[3] = { 0, 0, 0 };
00455         readRegBytes(FIFO_R_W, 12, &data[0]);   // read data for averaging
00456         accel_temp[0] = (int16_t) (((int16_t) data[0] << 8) | data[1]); // Form signed 16-bit integer for each sample in FIFO
00457         accel_temp[1] = (int16_t) (((int16_t) data[2] << 8) | data[3]);
00458         accel_temp[2] = (int16_t) (((int16_t) data[4] << 8) | data[5]);
00459         gyro_temp[0] = (int16_t) (((int16_t) data[6] << 8) | data[7]);
00460         gyro_temp[1] = (int16_t) (((int16_t) data[8] << 8) | data[9]);
00461         gyro_temp[2] = (int16_t) (((int16_t) data[10] << 8) | data[11]);
00462 
00463         accelBias[0] += (int32_t) accel_temp[0];                        // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
00464         accelBias[1] += (int32_t) accel_temp[1];
00465         accelBias[2] += (int32_t) accel_temp[2];
00466         gyroBias[0] += (int32_t) gyro_temp[0];
00467         gyroBias[1] += (int32_t) gyro_temp[1];
00468         gyroBias[2] += (int32_t) gyro_temp[2];
00469     }
00470 
00471     accelBias[0] /= (int32_t) packetCount;                              // Normalize sums to get average count biases
00472     accelBias[1] /= (int32_t) packetCount;
00473     accelBias[2] /= (int32_t) packetCount;
00474     gyroBias[0] /= (int32_t) packetCount;
00475     gyroBias[1] /= (int32_t) packetCount;
00476     gyroBias[2] /= (int32_t) packetCount;
00477 
00478     if (accelBias[2] > 0L) {
00479         accelBias[2] -= (int32_t) accelSensitivity;
00480     }                                           // Remove gravity from the z-axis accelerometer bias calculation
00481     else {
00482         accelBias[2] += (int32_t) accelSensitivity;
00483     }
00484 
00485     // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
00486     data[0] = (-gyroBias[0] / 4 >> 8) & 0xFF;   // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
00487     data[1] = (-gyroBias[0] / 4) & 0xFF;        // Biases are additive, so change sign on calculated average gyro biases
00488     data[2] = (-gyroBias[1] / 4 >> 8) & 0xFF;
00489     data[3] = (-gyroBias[1] / 4) & 0xFF;
00490     data[4] = (-gyroBias[2] / 4 >> 8) & 0xFF;
00491     data[5] = (-gyroBias[2] / 4) & 0xFF;
00492 
00493     // Push gyro biases to hardware registers
00494     writeReg(XG_OFFS_USRH, data[0]);
00495     writeReg(XG_OFFS_USRL, data[1]);
00496     writeReg(YG_OFFS_USRH, data[2]);
00497     writeReg(YG_OFFS_USRL, data[3]);
00498     writeReg(ZG_OFFS_USRH, data[4]);
00499     writeReg(ZG_OFFS_USRL, data[5]);
00500 
00501     _gyroBias[0] = (float)gyroBias[0] / (float)gyroSensitivity; // construct gyro bias in deg/s for later manual subtraction
00502     _gyroBias[1] = (float)gyroBias[1] / (float)gyroSensitivity;
00503     _gyroBias[2] = (float)gyroBias[2] / (float)gyroSensitivity;
00504 
00505     // Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
00506     // factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
00507     // non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
00508     // compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
00509     // the accelerometer biases calculated above must be divided by 8.
00510     int32_t accel_bias_reg[3] = { 0, 0, 0 };                    // A place to hold the factory accelerometer trim biases
00511 
00512     readRegBytes(XA_OFFSET_H, 2, &data[0]);                     // Read factory accelerometer trim values
00513     accel_bias_reg[0] = (int16_t) ((int16_t) data[0] << 8) | data[1];
00514     readRegBytes(YA_OFFSET_H, 2, &data[0]);
00515     accel_bias_reg[1] = (int16_t) ((int16_t) data[0] << 8) | data[1];
00516     readRegBytes(ZA_OFFSET_H, 2, &data[0]);
00517     accel_bias_reg[2] = (int16_t) ((int16_t) data[0] << 8) | data[1];
00518 
00519     uint32_t    mask = 1uL;                                     // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
00520     uint8_t     mask_bit[3] = { 0, 0, 0 };                      // Define array to hold mask bit for each accelerometer bias axis
00521 
00522     for (int i = 0; i < 3; i++) {
00523         if (accel_bias_reg[i] & mask)
00524             mask_bit[i] = 0x01;                                 // If temperature compensation bit is set, record that fact in mask_bit
00525     }
00526 
00527     // Construct total accelerometer bias, including calculated average accelerometer bias from above
00528     accel_bias_reg[0] -= (accelBias[0] / 8);                    // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
00529     accel_bias_reg[1] -= (accelBias[1] / 8);
00530     accel_bias_reg[2] -= (accelBias[2] / 8);
00531 
00532     data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
00533     data[1] = (accel_bias_reg[0]) & 0xFF;
00534     data[1] = data[1] | mask_bit[0];                            // preserve temperature compensation bit when writing back to accelerometer bias registers
00535     data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
00536     data[3] = (accel_bias_reg[1]) & 0xFF;
00537     data[3] = data[3] | mask_bit[1];                            // preserve temperature compensation bit when writing back to accelerometer bias registers
00538     data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
00539     data[5] = (accel_bias_reg[2]) & 0xFF;
00540     data[5] = data[5] | mask_bit[2];                            // preserve temperature compensation bit when writing back to accelerometer bias registers
00541 
00542     // Push accelerometer biases to hardware registers
00543     //  writeByte(_address, XA_OFFSET_H, data[0]);
00544     //  writeByte(_address, XA_OFFSET_L_TC, data[1]);
00545     //  writeByte(_address, YA_OFFSET_H, data[2]);
00546     //  writeByte(_address, YA_OFFSET_L_TC, data[3]);
00547     //  writeByte(_address, ZA_OFFSET_H, data[4]);
00548     //  writeByte(_address, ZA_OFFSET_L_TC, data[5]);
00549     // Output scaled accelerometer biases for manual subtraction in the main program
00550     _accelBias[0] = (float)accelBias[0] / (float)accelSensitivity;
00551     _accelBias[1] = (float)accelBias[1] / (float)accelSensitivity;
00552     _accelBias[2] = (float)accelBias[2] / (float)accelSensitivity;
00553 }
00554 
00555 /**
00556  * @brief   Performs accelerometer and gyroscope self test
00557  * @note    Checks calibration wrt factory settings.
00558  *          Should return percent deviation from factory trim values
00559  *          +/- 14 or less deviation is a pass
00560  * @param
00561  * @retval  true    if passed
00562  *          false   otherwise
00563  */
00564 bool MPU6050::selfTestOK()
00565 {
00566     uint8_t rawData[4] = { 0, 0, 0, 0 };
00567     uint8_t selfTest[6];
00568     float   factoryTrim[6];
00569     //
00570     // Configure the accelerometer for self-test
00571     writeReg(ACCEL_CONFIG, 0xF0);                               // Enable self test on all three axes and set accelerometer range to +/- 8 g
00572     writeReg(GYRO_CONFIG, 0xE0);                                // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
00573 #if MBED_MAJOR_VERSION > 5
00574     ThisThread::sleep_for(250ms);
00575 #else
00576     wait_ms(250);
00577 #endif
00578     rawData[0] = readReg(SELF_TEST_X);                          // X-axis self-test results
00579     rawData[1] = readReg(SELF_TEST_Y);                          // Y-axis self-test results
00580     rawData[2] = readReg(SELF_TEST_Z);                          // Z-axis self-test results
00581     rawData[3] = readReg(SELF_TEST_A);                          // Mixed-axis self-test results
00582 
00583     // Extract the acceleration test results first
00584     selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4; // XA_TEST result is a five-bit unsigned integer
00585     selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4; // YA_TEST result is a five-bit unsigned integer
00586     selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4; // ZA_TEST result is a five-bit unsigned integer
00587 
00588     // Extract the gyration test results first
00589     selfTest[3] = rawData[0] & 0x1F;                            // XG_TEST result is a five-bit unsigned integer
00590     selfTest[4] = rawData[1] & 0x1F;                            // YG_TEST result is a five-bit unsigned integer
00591     selfTest[5] = rawData[2] & 0x1F;                            // ZG_TEST result is a five-bit unsigned integer
00592 
00593     // Process results to allow final comparison with factory set values
00594     factoryTrim[0] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((selfTest[0] - 1.0f) / 30.0f)));    // FT[Xa] factory trim calculation
00595     factoryTrim[1] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((selfTest[1] - 1.0f) / 30.0f)));    // FT[Ya] factory trim calculation
00596     factoryTrim[2] = (4096.0f * 0.34f) * (pow((0.92f / 0.34f), ((selfTest[2] - 1.0f) / 30.0f)));    // FT[Za] factory trim calculation
00597     factoryTrim[3] = (25.0f * 131.0f) * (pow(1.046f, (selfTest[3] - 1.0f)));                        // FT[Xg] factory trim calculation
00598     factoryTrim[4] = (-25.0f * 131.0f) * (pow(1.046f, (selfTest[4] - 1.0f)));                       // FT[Yg] factory trim calculation
00599     factoryTrim[5] = (25.0f * 131.0f) * (pow(1.046f, (selfTest[5] - 1.0f)));                        // FT[Zg] factory trim calculation
00600 
00601     //  Output self-test results and factory trim calculation if desired
00602     //  Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]);
00603     //  Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]);
00604     //  Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]);
00605     //  Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]);
00606     // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
00607     // To get to percent, must multiply by 100 and subtract result from 100
00608     for (int i = 0; i < 6; i++) {
00609         _selfTest[i] = 100.0f + 100.0f * (selfTest[i] - factoryTrim[i]) / factoryTrim[i];           // Report percent differences
00610     }
00611 
00612     return
00613         (
00614             _selfTest[0] < 1.0f &&
00615             _selfTest[1] < 1.0f &&
00616             _selfTest[2] < 1.0f &&
00617             _selfTest[3] < 1.0f &&
00618             _selfTest[4] < 1.0f &&
00619             _selfTest[5] < 1.0f
00620         );
00621 }
00622 
00623 /**
00624  * @brief   Computes acceleration
00625  * @note
00626  * @param
00627  * @retval
00628  */
00629 float* MPU6050::accel()
00630 {
00631     accelADC();
00632 
00633     // Compute actual acceleration values, this depends on used scale
00634     accelX = (float)_accelAdc[0] * _accelRes - _accelBias[0];
00635     accelY = (float)_accelAdc[1] * _accelRes - _accelBias[1];
00636     accelZ = (float)_accelAdc[2] * _accelRes - _accelBias[2];
00637 
00638     return accelData;
00639 }
00640 
00641 /**
00642  * @brief   Computes gyro
00643  * @note
00644  * @param
00645  * @retval
00646  */
00647 float* MPU6050::gyro()
00648 {
00649     gyroADC();
00650 
00651     // Compute actual gyro values, this depends on used scale
00652     gyroX = (float)_gyroAdc[0] * _gyroRes - _gyroBias[0];
00653     gyroY = (float)_gyroAdc[1] * _gyroRes - _gyroBias[1];
00654     gyroZ = (float)_gyroAdc[2] * _gyroRes - _gyroBias[2];
00655     return gyroData;
00656 }
00657 
00658 /**
00659  * @brief   Adjusts filter gain after device is stabilized
00660  * @note
00661  * @param
00662  * @retval
00663  */
00664 void MPU6050::setGain(float beta, float zeta)
00665 {
00666     _beta = beta;  // set filter gain
00667     _zeta = zeta;  // set bias drift gain
00668 }
00669 
00670 /**
00671  * @brief   Madgwick filter
00672  * @note    Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
00673  *          (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
00674  *          which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative
00675  *          device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
00676  *          The performance of the orientation filter is almost as good as conventional Kalman-based filtering algorithms
00677  *          but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
00678  * @param   deltaT  Integration time in seconds.
00679  * @retval
00680  */
00681 void MPU6050::madgwickFilter(float deltaT)
00682 {
00683     // aliases for better readability
00684     float&  q1 = _q[0];
00685     float&  q2 = _q[1];
00686     float&  q3 = _q[2];
00687     float&  q4 = _q[3];
00688     float   norm;       // vector norm
00689     float   f1, f2, f3; // objective funcion elements
00690     float   j_11or24, j_12or23, j_13or22, j_14or21, j_32, j_33; // objective function Jacobian elements
00691     float   qDot1, qDot2, qDot3, qDot4;
00692     float   hatDot1, hatDot2, hatDot3, hatDot4;
00693     float   ax = accelX;
00694     float   ay = accelY;
00695     float   az = accelZ;
00696     float   gx = gyroX * M_PI / 180.0f;                         // convert to rad/s
00697     float   gy = gyroY * M_PI / 180.0f;                         // convert to rad/s
00698     float   gz = gyroZ * M_PI / 180.0f;                         // convert to rad/s
00699     float   gErrX, gErrY, gErrZ, gBiasX, gBiasY, gBiasZ;        // gyro bias errors
00700 
00701     // Auxiliary variables to avoid repeated arithmetic
00702     float   halfq1 = 0.5f * q1;
00703     float   halfq2 = 0.5f * q2;
00704     float   halfq3 = 0.5f * q3;
00705     float   halfq4 = 0.5f * q4;
00706     float   _2q1 = 2.0f * q1;
00707     float   _2q2 = 2.0f * q2;
00708     float   _2q3 = 2.0f * q3;
00709     float   _2q4 = 2.0f * q4;
00710     //float _2q1q3 = 2.0f * q1 * q3;
00711     //float _2q3q4 = 2.0f * q3 * q4;
00712     // Normalise accelerometer measurement
00713     norm = sqrt(ax * ax + ay * ay + az * az);
00714     if (norm == 0.0f)
00715         return;                                         // handle NaN
00716     norm = 1.0f / norm;
00717     ax *= norm;
00718     ay *= norm;
00719     az *= norm;
00720 
00721     // Compute the objective function and Jacobian
00722     f1 = _2q2 * q4 - _2q1 * q3 - ax;
00723     f2 = _2q1 * q2 + _2q3 * q4 - ay;
00724     f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;
00725     j_11or24 = _2q3;
00726     j_12or23 = _2q4;
00727     j_13or22 = _2q1;
00728     j_14or21 = _2q2;
00729     j_32 = 2.0f * j_14or21;
00730     j_33 = 2.0f * j_11or24;
00731 
00732     // Compute the gradient (matrix multiplication)
00733     hatDot1 = j_14or21 * f2 - j_11or24 * f1;
00734     hatDot2 = j_12or23 * f1 + j_13or22 * f2 - j_32 * f3;
00735     hatDot3 = j_12or23 * f2 - j_33 * f3 - j_13or22 * f1;
00736     hatDot4 = j_14or21 * f1 + j_11or24 * f2;
00737 
00738     // Normalize the gradient
00739     norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
00740     hatDot1 /= norm;
00741     hatDot2 /= norm;
00742     hatDot3 /= norm;
00743     hatDot4 /= norm;
00744 
00745     // Compute estimated gyroscope errors
00746     gErrX = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;
00747     gErrY = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;
00748     gErrZ = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;
00749 
00750     // Compute gyroscope biases
00751     gBiasX += gErrX * deltaT * _zeta;
00752     gBiasY += gErrY * deltaT * _zeta;
00753     gBiasZ += gErrZ * deltaT * _zeta;
00754 
00755     // Remove gyroscope biases
00756     gx -= gBiasX;
00757     gy -= gBiasY;
00758     gz -= gBiasZ;
00759 
00760     // Compute the quaternion derivative
00761     qDot1 = -halfq2 * gx - halfq3 * gy - halfq4 * gz;
00762     qDot2 = halfq1 * gx + halfq3 * gz - halfq4 * gy;
00763     qDot3 = halfq1 * gy - halfq2 * gz + halfq4 * gx;
00764     qDot4 = halfq1 * gz + halfq2 * gy - halfq3 * gx;
00765 
00766     // Compute the integrated estimated quaternion derivative
00767     q1 += (qDot1 - (_beta * hatDot1)) * deltaT;
00768     q2 += (qDot2 - (_beta * hatDot2)) * deltaT;
00769     q3 += (qDot3 - (_beta * hatDot3)) * deltaT;
00770     q4 += (qDot4 - (_beta * hatDot4)) * deltaT;
00771 
00772     // Normalize the quaternion
00773     norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
00774     norm = 1.0f / norm;
00775     _q[0] = q1 * norm;
00776     _q[1] = q2 * norm;
00777     _q[2] = q3 * norm;
00778     _q[3] = q4 * norm;
00779 }
00780 
00781 /**
00782  * @brief   Calculates yaw in degree
00783  * @note
00784  * @param
00785  * @retval
00786  */
00787 float MPU6050::yaw()
00788 {
00789     float   yawRad = atan2
00790         (
00791             2.0f * (_q[1] * _q[2] + _q[0] * _q[3]),
00792             _q[0] * _q[0] + _q[1] * _q[1] - _q[2] * _q[2] - _q[3] * _q[3]
00793         );
00794 
00795     return yawRad * 180.f / M_PI;
00796 }
00797 
00798 /**
00799  * @brief   Calculates pitch in degree
00800  * @note
00801  * @param
00802  * @retval
00803  */
00804 float MPU6050::pitch()
00805 {
00806     float   pitchRad = -asin(2.0f * (_q[1] * _q[3] - _q[0] * _q[2]));
00807 
00808     return pitchRad * 180.f / M_PI;
00809 }
00810 
00811 /**
00812  * @brief   Claculates roll in degree
00813  * @note
00814  * @param
00815  * @retval
00816  */
00817 float MPU6050::roll()
00818 {
00819     float   rollRad = atan2
00820         (
00821             2.0f * (_q[0] * _q[1] + _q[2] * _q[3]),
00822             _q[0] * _q[0] - _q[1] * _q[1] - _q[2] * _q[2] + _q[3] * _q[3]
00823         );
00824 
00825     return rollRad * 180.0f / M_PI;
00826 }