MPU6050.cpp

00001 //ported from arduino library: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
00002 //written by szymon gaertig (email: szymon@gaertig.com.pl)
00003 //
00004 //Changelog:
00005 //2013-01-08 - first beta release
00006 
00007 // I2Cdev library collection - MPU6050 I2C device class
00008 // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
00009 // 8/24/2011 by Jeff Rowberg <jeff@rowberg.net>
00010 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
00011 //
00012 // Changelog:
00013 //     ... - ongoing debug release
00014 
00015 // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
00016 // DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF
00017 // YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING.
00018 
00019 /* ============================================
00020 I2Cdev device library code is placed under the MIT license
00021 Copyright (c) 2012 Jeff Rowberg
00022 
00023 Permission is hereby granted, free of charge, to any person obtaining a copy
00024 of this software and associated documentation files (the "Software"), to deal
00025 in the Software without restriction, including without limitation the rights
00026 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00027 copies of the Software, and to permit persons to whom the Software is
00028 furnished to do so, subject to the following conditions:
00029 
00030 The above copyright notice and this permission notice shall be included in
00031 all copies or substantial portions of the Software.
00032 
00033 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00034 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00035 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00036 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00037 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00038 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00039 THE SOFTWARE.
00040 ===============================================
00041 */
00042 
00043 #include "mbed.h"
00044 #include "MPU6050.h"
00045 #include "rtos.h"
00046 
00047 //#define useDebugSerial
00048 
00049 //instead of using pgmspace.h
00050 typedef const unsigned char prog_uchar;
00051 #define pgm_read_byte_near(x) (*(prog_uchar*)x)
00052 #define pgm_read_byte(x) (*(prog_uchar*)x)
00053 
00054 /** Default constructor, uses default I2C address.
00055  * @see MPU6050_DEFAULT_ADDRESS
00056  */
00057 MPU6050::MPU6050() : debugSerial(USBTX, USBRX), sampling(false)
00058 {
00059     devAddr = MPU6050_DEFAULT_ADDRESS << 1;
00060     debugSerial.baud(115200);
00061     
00062     tX[0] = 0; tX[1] = 1; tX[2] = 0;
00063     tY[0] = 1; tY[1] = 0; tY[2] = 0;
00064     tZ[0] = 0; tZ[1] = 0; tZ[2] = -1;
00065 }
00066 
00067 /** Specific address constructor.
00068  * @param address I2C address
00069  * @see MPU6050_DEFAULT_ADDRESS
00070  * @see MPU6050_ADDRESS_AD0_LOW
00071  * @see MPU6050_ADDRESS_AD0_HIGH
00072  */
00073 MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX), sampling(false)
00074 {
00075     devAddr = address << 1;
00076     debugSerial.baud(115200);
00077     
00078     tX[0] = 0; tX[1] = 1; tX[2] = 0;
00079     tY[0] = 1; tY[1] = 0; tY[2] = 0;
00080     tZ[0] = 0; tZ[1] = 0; tZ[2] = -1;
00081 }
00082 
00083 /** Power on and prepare for general usage.
00084  * This will activate the device and take it out of sleep mode (which must be done
00085  * after start-up). This function also sets both the accelerometer and the gyroscope
00086  * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
00087  * the clock source to use the X Gyro for reference, which is slightly better than
00088  * the default internal clock source.
00089  */
00090 void MPU6050::initialize()
00091 {
00092 #ifdef useDebugSerial
00093     debugSerial.printf("MPU6050::initialize start\n");
00094 #endif
00095     setClockSource(MPU6050_CLOCK_PLL_XGYRO);
00096     setFullScaleGyroRange(MPU6050_GYRO_FS_250);
00097     setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00098     setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
00099 
00100 #ifdef useDebugSerial
00101     debugSerial.printf("MPU6050::initialize end\n");
00102 #endif
00103 }
00104 
00105 /** Verify the I2C connection.
00106  * Make sure the device is connected and responds as expected.
00107  * @return True if connection is valid, false otherwise
00108  */
00109 bool MPU6050::testConnection()
00110 {
00111 #ifdef useDebugSerial
00112     debugSerial.printf("MPU6050::testConnection start\n");
00113 #endif
00114     uint8_t deviceId = getDeviceID();
00115 #ifdef useDebugSerial
00116     debugSerial.printf("DeviceId = %d\n",deviceId);
00117 #endif
00118     return deviceId == 0x34;
00119 }
00120 
00121 // AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)
00122 
00123 /** Get the auxiliary I2C supply voltage level.
00124  * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
00125  * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
00126  * the MPU-6000, which does not have a VLOGIC pin.
00127  * @return I2C supply voltage level (0=VLOGIC, 1=VDD)
00128  */
00129 uint8_t MPU6050::getAuxVDDIOLevel()
00130 {
00131     i2Cdev.readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer);
00132     return buffer[0];
00133 }
00134 /** Set the auxiliary I2C supply voltage level.
00135  * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
00136  * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
00137  * the MPU-6000, which does not have a VLOGIC pin.
00138  * @param level I2C supply voltage level (0=VLOGIC, 1=VDD)
00139  */
00140 void MPU6050::setAuxVDDIOLevel(uint8_t level)
00141 {
00142     i2Cdev.writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level);
00143 }
00144 
00145 // SMPLRT_DIV register
00146 
00147 /** Get gyroscope output rate divider.
00148  * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero
00149  * Motion detection, and Free Fall detection are all based on the Sample Rate.
00150  * The Sample Rate is generated by dividing the gyroscope output rate by
00151  * SMPLRT_DIV:
00152  *
00153  * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
00154  *
00155  * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or
00156  * 7), and 1kHz when the DLPF is enabled (see Register 26).
00157  *
00158  * Note: The accelerometer output rate is 1kHz. This means that for a Sample
00159  * Rate greater than 1kHz, the same accelerometer sample may be output to the
00160  * FIFO, DMP, and sensor registers more than once.
00161  *
00162  * For a diagram of the gyroscope and accelerometer signal paths, see Section 8
00163  * of the MPU-6000/MPU-6050 Product Specification document.
00164  *
00165  * @return Current sample rate
00166  * @see MPU6050_RA_SMPLRT_DIV
00167  */
00168 uint8_t MPU6050::getRate()
00169 {
00170     i2Cdev.readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer);
00171     return buffer[0];
00172 }
00173 /** Set gyroscope sample rate divider.
00174  * @param rate New sample rate divider
00175  * @see getRate()
00176  * @see MPU6050_RA_SMPLRT_DIV
00177  */
00178 void MPU6050::setRate(uint8_t rate)
00179 {
00180     i2Cdev.writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
00181 }
00182 
00183 
00184 // CONFIG register
00185 
00186 /** Get external FSYNC configuration.
00187  * Configures the external Frame Synchronization (FSYNC) pin sampling. An
00188  * external signal connected to the FSYNC pin can be sampled by configuring
00189  * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short
00190  * strobes may be captured. The latched FSYNC signal will be sampled at the
00191  * Sampling Rate, as defined in register 25. After sampling, the latch will
00192  * reset to the current FSYNC signal state.
00193  *
00194  * The sampled value will be reported in place of the least significant bit in
00195  * a sensor data register determined by the value of EXT_SYNC_SET according to
00196  * the following table.
00197  *
00198  * <pre>
00199  * EXT_SYNC_SET | FSYNC Bit Location
00200  * -------------+-------------------
00201  * 0            | Input disabled
00202  * 1            | TEMP_OUT_L[0]
00203  * 2            | GYRO_XOUT_L[0]
00204  * 3            | GYRO_YOUT_L[0]
00205  * 4            | GYRO_ZOUT_L[0]
00206  * 5            | ACCEL_XOUT_L[0]
00207  * 6            | ACCEL_YOUT_L[0]
00208  * 7            | ACCEL_ZOUT_L[0]
00209  * </pre>
00210  *
00211  * @return FSYNC configuration value
00212  */
00213 uint8_t MPU6050::getExternalFrameSync()
00214 {
00215     i2Cdev.readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer);
00216     return buffer[0];
00217 }
00218 /** Set external FSYNC configuration.
00219  * @see getExternalFrameSync()
00220  * @see MPU6050_RA_CONFIG
00221  * @param sync New FSYNC configuration value
00222  */
00223 void MPU6050::setExternalFrameSync(uint8_t sync)
00224 {
00225     i2Cdev.writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
00226 }
00227 /** Get digital low-pass filter configuration.
00228  * The DLPF_CFG parameter sets the digital low pass filter configuration. It
00229  * also determines the internal sampling rate used by the device as shown in
00230  * the table below.
00231  *
00232  * Note: The accelerometer output rate is 1kHz. This means that for a Sample
00233  * Rate greater than 1kHz, the same accelerometer sample may be output to the
00234  * FIFO, DMP, and sensor registers more than once.
00235  *
00236  * <pre>
00237  *          |   ACCELEROMETER    |           GYROSCOPE
00238  * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
00239  * ---------+-----------+--------+-----------+--------+-------------
00240  * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
00241  * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
00242  * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
00243  * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
00244  * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
00245  * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
00246  * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
00247  * 7        |   -- Reserved --   |   -- Reserved --   | Reserved
00248  * </pre>
00249  *
00250  * @return DLFP configuration
00251  * @see MPU6050_RA_CONFIG
00252  * @see MPU6050_CFG_DLPF_CFG_BIT
00253  * @see MPU6050_CFG_DLPF_CFG_LENGTH
00254  */
00255 uint8_t MPU6050::getDLPFMode()
00256 {
00257     i2Cdev.readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer);
00258     return buffer[0];
00259 }
00260 /** Set digital low-pass filter configuration.
00261  * @param mode New DLFP configuration setting
00262  * @see getDLPFBandwidth()
00263  * @see MPU6050_DLPF_BW_256
00264  * @see MPU6050_RA_CONFIG
00265  * @see MPU6050_CFG_DLPF_CFG_BIT
00266  * @see MPU6050_CFG_DLPF_CFG_LENGTH
00267  */
00268 void MPU6050::setDLPFMode(uint8_t mode)
00269 {
00270     i2Cdev.writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
00271 }
00272 
00273 // GYRO_CONFIG register
00274 
00275 /** Get full-scale gyroscope range.
00276  * The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
00277  * as described in the table below.
00278  *
00279  * <pre>
00280  * 0 = +/- 250 degrees/sec
00281  * 1 = +/- 500 degrees/sec
00282  * 2 = +/- 1000 degrees/sec
00283  * 3 = +/- 2000 degrees/sec
00284  * </pre>
00285  *
00286  * @return Current full-scale gyroscope range setting
00287  * @see MPU6050_GYRO_FS_250
00288  * @see MPU6050_RA_GYRO_CONFIG
00289  * @see MPU6050_GCONFIG_FS_SEL_BIT
00290  * @see MPU6050_GCONFIG_FS_SEL_LENGTH
00291  */
00292 uint8_t MPU6050::getFullScaleGyroRange()
00293 {
00294     i2Cdev.readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer);
00295     return buffer[0];
00296 }
00297 /** Set full-scale gyroscope range.
00298  * @param range New full-scale gyroscope range value
00299  * @see getFullScaleRange()
00300  * @see MPU6050_GYRO_FS_250
00301  * @see MPU6050_RA_GYRO_CONFIG
00302  * @see MPU6050_GCONFIG_FS_SEL_BIT
00303  * @see MPU6050_GCONFIG_FS_SEL_LENGTH
00304  */
00305 void MPU6050::setFullScaleGyroRange(uint8_t range)
00306 {
00307     i2Cdev.writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
00308 }
00309 
00310 // ACCEL_CONFIG register
00311 
00312 /** Get self-test enabled setting for accelerometer X axis.
00313  * @return Self-test enabled value
00314  * @see MPU6050_RA_ACCEL_CONFIG
00315  */
00316 bool MPU6050::getAccelXSelfTest()
00317 {
00318     i2Cdev.readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer);
00319     return buffer[0];
00320 }
00321 /** Get self-test enabled setting for accelerometer X axis.
00322  * @param enabled Self-test enabled value
00323  * @see MPU6050_RA_ACCEL_CONFIG
00324  */
00325 void MPU6050::setAccelXSelfTest(bool enabled)
00326 {
00327     i2Cdev.writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled);
00328 }
00329 /** Get self-test enabled value for accelerometer Y axis.
00330  * @return Self-test enabled value
00331  * @see MPU6050_RA_ACCEL_CONFIG
00332  */
00333 bool MPU6050::getAccelYSelfTest()
00334 {
00335     i2Cdev.readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer);
00336     return buffer[0];
00337 }
00338 /** Get self-test enabled value for accelerometer Y axis.
00339  * @param enabled Self-test enabled value
00340  * @see MPU6050_RA_ACCEL_CONFIG
00341  */
00342 void MPU6050::setAccelYSelfTest(bool enabled)
00343 {
00344     i2Cdev.writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled);
00345 }
00346 /** Get self-test enabled value for accelerometer Z axis.
00347  * @return Self-test enabled value
00348  * @see MPU6050_RA_ACCEL_CONFIG
00349  */
00350 bool MPU6050::getAccelZSelfTest()
00351 {
00352     i2Cdev.readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer);
00353     return buffer[0];
00354 }
00355 /** Set self-test enabled value for accelerometer Z axis.
00356  * @param enabled Self-test enabled value
00357  * @see MPU6050_RA_ACCEL_CONFIG
00358  */
00359 void MPU6050::setAccelZSelfTest(bool enabled)
00360 {
00361     i2Cdev.writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled);
00362 }
00363 /** Get full-scale accelerometer range.
00364  * The FS_SEL parameter allows setting the full-scale range of the accelerometer
00365  * sensors, as described in the table below.
00366  *
00367  * <pre>
00368  * 0 = +/- 2g
00369  * 1 = +/- 4g
00370  * 2 = +/- 8g
00371  * 3 = +/- 16g
00372  * </pre>
00373  *
00374  * @return Current full-scale accelerometer range setting
00375  * @see MPU6050_ACCEL_FS_2
00376  * @see MPU6050_RA_ACCEL_CONFIG
00377  * @see MPU6050_ACONFIG_AFS_SEL_BIT
00378  * @see MPU6050_ACONFIG_AFS_SEL_LENGTH
00379  */
00380 uint8_t MPU6050::getFullScaleAccelRange()
00381 {
00382     i2Cdev.readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer);
00383     return buffer[0];
00384 }
00385 /** Set full-scale accelerometer range.
00386  * @param range New full-scale accelerometer range setting
00387  * @see getFullScaleAccelRange()
00388  */
00389 void MPU6050::setFullScaleAccelRange(uint8_t range)
00390 {
00391     i2Cdev.writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
00392 }
00393 /** Get the high-pass filter configuration.
00394  * The DHPF is a filter module in the path leading to motion detectors (Free
00395  * Fall, Motion threshold, and Zero Motion). The high pass filter output is not
00396  * available to the data registers (see Figure in Section 8 of the MPU-6000/
00397  * MPU-6050 Product Specification document).
00398  *
00399  * The high pass filter has three modes:
00400  *
00401  * <pre>
00402  *    Reset: The filter output settles to zero within one sample. This
00403  *           effectively disables the high pass filter. This mode may be toggled
00404  *           to quickly settle the filter.
00405  *
00406  *    On:    The high pass filter will pass signals above the cut off frequency.
00407  *
00408  *    Hold:  When triggered, the filter holds the present sample. The filter
00409  *           output will be the difference between the input sample and the held
00410  *           sample.
00411  * </pre>
00412  *
00413  * <pre>
00414  * ACCEL_HPF | Filter Mode | Cut-off Frequency
00415  * ----------+-------------+------------------
00416  * 0         | Reset       | None
00417  * 1         | On          | 5Hz
00418  * 2         | On          | 2.5Hz
00419  * 3         | On          | 1.25Hz
00420  * 4         | On          | 0.63Hz
00421  * 7         | Hold        | None
00422  * </pre>
00423  *
00424  * @return Current high-pass filter configuration
00425  * @see MPU6050_DHPF_RESET
00426  * @see MPU6050_RA_ACCEL_CONFIG
00427  */
00428 uint8_t MPU6050::getDHPFMode()
00429 {
00430     i2Cdev.readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer);
00431     return buffer[0];
00432 }
00433 /** Set the high-pass filter configuration.
00434  * @param bandwidth New high-pass filter configuration
00435  * @see setDHPFMode()
00436  * @see MPU6050_DHPF_RESET
00437  * @see MPU6050_RA_ACCEL_CONFIG
00438  */
00439 void MPU6050::setDHPFMode(uint8_t bandwidth)
00440 {
00441     i2Cdev.writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
00442 }
00443 
00444 // FF_THR register
00445 
00446 /** Get free-fall event acceleration threshold.
00447  * This register configures the detection threshold for Free Fall event
00448  * detection. The unit of FF_THR is 1LSB = 2mg. Free Fall is detected when the
00449  * absolute value of the accelerometer measurements for the three axes are each
00450  * less than the detection threshold. This condition increments the Free Fall
00451  * duration counter (Register 30). The Free Fall interrupt is triggered when the
00452  * Free Fall duration counter reaches the time specified in FF_DUR.
00453  *
00454  * For more details on the Free Fall detection interrupt, see Section 8.2 of the
00455  * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and
00456  * 58 of this document.
00457  *
00458  * @return Current free-fall acceleration threshold value (LSB = 2mg)
00459  * @see MPU6050_RA_FF_THR
00460  */
00461 uint8_t MPU6050::getFreefallDetectionThreshold()
00462 {
00463     i2Cdev.readByte(devAddr, MPU6050_RA_FF_THR, buffer);
00464     return buffer[0];
00465 }
00466 /** Get free-fall event acceleration threshold.
00467  * @param threshold New free-fall acceleration threshold value (LSB = 2mg)
00468  * @see getFreefallDetectionThreshold()
00469  * @see MPU6050_RA_FF_THR
00470  */
00471 void MPU6050::setFreefallDetectionThreshold(uint8_t threshold)
00472 {
00473     i2Cdev.writeByte(devAddr, MPU6050_RA_FF_THR, threshold);
00474 }
00475 
00476 // FF_DUR register
00477 
00478 /** Get free-fall event duration threshold.
00479  * This register configures the duration counter threshold for Free Fall event
00480  * detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit
00481  * of 1 LSB = 1 ms.
00482  *
00483  * The Free Fall duration counter increments while the absolute value of the
00484  * accelerometer measurements are each less than the detection threshold
00485  * (Register 29). The Free Fall interrupt is triggered when the Free Fall
00486  * duration counter reaches the time specified in this register.
00487  *
00488  * For more details on the Free Fall detection interrupt, see Section 8.2 of
00489  * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56
00490  * and 58 of this document.
00491  *
00492  * @return Current free-fall duration threshold value (LSB = 1ms)
00493  * @see MPU6050_RA_FF_DUR
00494  */
00495 uint8_t MPU6050::getFreefallDetectionDuration()
00496 {
00497     i2Cdev.readByte(devAddr, MPU6050_RA_FF_DUR, buffer);
00498     return buffer[0];
00499 }
00500 /** Get free-fall event duration threshold.
00501  * @param duration New free-fall duration threshold value (LSB = 1ms)
00502  * @see getFreefallDetectionDuration()
00503  * @see MPU6050_RA_FF_DUR
00504  */
00505 void MPU6050::setFreefallDetectionDuration(uint8_t duration)
00506 {
00507     i2Cdev.writeByte(devAddr, MPU6050_RA_FF_DUR, duration);
00508 }
00509 
00510 // MOT_THR register
00511 
00512 /** Get motion detection event acceleration threshold.
00513  * This register configures the detection threshold for Motion interrupt
00514  * generation. The unit of MOT_THR is 1LSB = 2mg. Motion is detected when the
00515  * absolute value of any of the accelerometer measurements exceeds this Motion
00516  * detection threshold. This condition increments the Motion detection duration
00517  * counter (Register 32). The Motion detection interrupt is triggered when the
00518  * Motion Detection counter reaches the time count specified in MOT_DUR
00519  * (Register 32).
00520  *
00521  * The Motion interrupt will indicate the axis and polarity of detected motion
00522  * in MOT_DETECT_STATUS (Register 97).
00523  *
00524  * For more details on the Motion detection interrupt, see Section 8.3 of the
00525  * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and
00526  * 58 of this document.
00527  *
00528  * @return Current motion detection acceleration threshold value (LSB = 2mg)
00529  * @see MPU6050_RA_MOT_THR
00530  */
00531 uint8_t MPU6050::getMotionDetectionThreshold()
00532 {
00533     i2Cdev.readByte(devAddr, MPU6050_RA_MOT_THR, buffer);
00534     return buffer[0];
00535 }
00536 /** Set free-fall event acceleration threshold.
00537  * @param threshold New motion detection acceleration threshold value (LSB = 2mg)
00538  * @see getMotionDetectionThreshold()
00539  * @see MPU6050_RA_MOT_THR
00540  */
00541 void MPU6050::setMotionDetectionThreshold(uint8_t threshold)
00542 {
00543     i2Cdev.writeByte(devAddr, MPU6050_RA_MOT_THR, threshold);
00544 }
00545 
00546 // MOT_DUR register
00547 
00548 /** Get motion detection event duration threshold.
00549  * This register configures the duration counter threshold for Motion interrupt
00550  * generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit
00551  * of 1LSB = 1ms. The Motion detection duration counter increments when the
00552  * absolute value of any of the accelerometer measurements exceeds the Motion
00553  * detection threshold (Register 31). The Motion detection interrupt is
00554  * triggered when the Motion detection counter reaches the time count specified
00555  * in this register.
00556  *
00557  * For more details on the Motion detection interrupt, see Section 8.3 of the
00558  * MPU-6000/MPU-6050 Product Specification document.
00559  *
00560  * @return Current motion detection duration threshold value (LSB = 1ms)
00561  * @see MPU6050_RA_MOT_DUR
00562  */
00563 uint8_t MPU6050::getMotionDetectionDuration()
00564 {
00565     i2Cdev.readByte(devAddr, MPU6050_RA_MOT_DUR, buffer);
00566     return buffer[0];
00567 }
00568 /** Set motion detection event duration threshold.
00569  * @param duration New motion detection duration threshold value (LSB = 1ms)
00570  * @see getMotionDetectionDuration()
00571  * @see MPU6050_RA_MOT_DUR
00572  */
00573 void MPU6050::setMotionDetectionDuration(uint8_t duration)
00574 {
00575     i2Cdev.writeByte(devAddr, MPU6050_RA_MOT_DUR, duration);
00576 }
00577 
00578 // ZRMOT_THR register
00579 
00580 /** Get zero motion detection event acceleration threshold.
00581  * This register configures the detection threshold for Zero Motion interrupt
00582  * generation. The unit of ZRMOT_THR is 1LSB = 2mg. Zero Motion is detected when
00583  * the absolute value of the accelerometer measurements for the 3 axes are each
00584  * less than the detection threshold. This condition increments the Zero Motion
00585  * duration counter (Register 34). The Zero Motion interrupt is triggered when
00586  * the Zero Motion duration counter reaches the time count specified in
00587  * ZRMOT_DUR (Register 34).
00588  *
00589  * Unlike Free Fall or Motion detection, Zero Motion detection triggers an
00590  * interrupt both when Zero Motion is first detected and when Zero Motion is no
00591  * longer detected.
00592  *
00593  * When a zero motion event is detected, a Zero Motion Status will be indicated
00594  * in the MOT_DETECT_STATUS register (Register 97). When a motion-to-zero-motion
00595  * condition is detected, the status bit is set to 1. When a zero-motion-to-
00596  * motion condition is detected, the status bit is set to 0.
00597  *
00598  * For more details on the Zero Motion detection interrupt, see Section 8.4 of
00599  * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56
00600  * and 58 of this document.
00601  *
00602  * @return Current zero motion detection acceleration threshold value (LSB = 2mg)
00603  * @see MPU6050_RA_ZRMOT_THR
00604  */
00605 uint8_t MPU6050::getZeroMotionDetectionThreshold()
00606 {
00607     i2Cdev.readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer);
00608     return buffer[0];
00609 }
00610 /** Set zero motion detection event acceleration threshold.
00611  * @param threshold New zero motion detection acceleration threshold value (LSB = 2mg)
00612  * @see getZeroMotionDetectionThreshold()
00613  * @see MPU6050_RA_ZRMOT_THR
00614  */
00615 void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold)
00616 {
00617     i2Cdev.writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold);
00618 }
00619 
00620 // ZRMOT_DUR register
00621 
00622 /** Get zero motion detection event duration threshold.
00623  * This register configures the duration counter threshold for Zero Motion
00624  * interrupt generation. The duration counter ticks at 16 Hz, therefore
00625  * ZRMOT_DUR has a unit of 1 LSB = 64 ms. The Zero Motion duration counter
00626  * increments while the absolute value of the accelerometer measurements are
00627  * each less than the detection threshold (Register 33). The Zero Motion
00628  * interrupt is triggered when the Zero Motion duration counter reaches the time
00629  * count specified in this register.
00630  *
00631  * For more details on the Zero Motion detection interrupt, see Section 8.4 of
00632  * the MPU-6000/MPU-6050 Product Specification document, as well as Registers 56
00633  * and 58 of this document.
00634  *
00635  * @return Current zero motion detection duration threshold value (LSB = 64ms)
00636  * @see MPU6050_RA_ZRMOT_DUR
00637  */
00638 uint8_t MPU6050::getZeroMotionDetectionDuration()
00639 {
00640     i2Cdev.readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer);
00641     return buffer[0];
00642 }
00643 /** Set zero motion detection event duration threshold.
00644  * @param duration New zero motion detection duration threshold value (LSB = 1ms)
00645  * @see getZeroMotionDetectionDuration()
00646  * @see MPU6050_RA_ZRMOT_DUR
00647  */
00648 void MPU6050::setZeroMotionDetectionDuration(uint8_t duration)
00649 {
00650     i2Cdev.writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration);
00651 }
00652 
00653 // FIFO_EN register
00654 
00655 /** Get temperature FIFO enabled value.
00656  * When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and
00657  * 66) to be written into the FIFO buffer.
00658  * @return Current temperature FIFO enabled value
00659  * @see MPU6050_RA_FIFO_EN
00660  */
00661 bool MPU6050::getTempFIFOEnabled()
00662 {
00663     i2Cdev.readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer);
00664     return buffer[0];
00665 }
00666 /** Set temperature FIFO enabled value.
00667  * @param enabled New temperature FIFO enabled value
00668  * @see getTempFIFOEnabled()
00669  * @see MPU6050_RA_FIFO_EN
00670  */
00671 void MPU6050::setTempFIFOEnabled(bool enabled)
00672 {
00673     i2Cdev.writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
00674 }
00675 /** Get gyroscope X-axis FIFO enabled value.
00676  * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
00677  * 68) to be written into the FIFO buffer.
00678  * @return Current gyroscope X-axis FIFO enabled value
00679  * @see MPU6050_RA_FIFO_EN
00680  */
00681 bool MPU6050::getXGyroFIFOEnabled()
00682 {
00683     i2Cdev.readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer);
00684     return buffer[0];
00685 }
00686 /** Set gyroscope X-axis FIFO enabled value.
00687  * @param enabled New gyroscope X-axis FIFO enabled value
00688  * @see getXGyroFIFOEnabled()
00689  * @see MPU6050_RA_FIFO_EN
00690  */
00691 void MPU6050::setXGyroFIFOEnabled(bool enabled)
00692 {
00693     i2Cdev.writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled);
00694 }
00695 /** Get gyroscope Y-axis FIFO enabled value.
00696  * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
00697  * 70) to be written into the FIFO buffer.
00698  * @return Current gyroscope Y-axis FIFO enabled value
00699  * @see MPU6050_RA_FIFO_EN
00700  */
00701 bool MPU6050::getYGyroFIFOEnabled()
00702 {
00703     i2Cdev.readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer);
00704     return buffer[0];
00705 }
00706 /** Set gyroscope Y-axis FIFO enabled value.
00707  * @param enabled New gyroscope Y-axis FIFO enabled value
00708  * @see getYGyroFIFOEnabled()
00709  * @see MPU6050_RA_FIFO_EN
00710  */
00711 void MPU6050::setYGyroFIFOEnabled(bool enabled)
00712 {
00713     i2Cdev.writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled);
00714 }
00715 /** Get gyroscope Z-axis FIFO enabled value.
00716  * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
00717  * 72) to be written into the FIFO buffer.
00718  * @return Current gyroscope Z-axis FIFO enabled value
00719  * @see MPU6050_RA_FIFO_EN
00720  */
00721 bool MPU6050::getZGyroFIFOEnabled()
00722 {
00723     i2Cdev.readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer);
00724     return buffer[0];
00725 }
00726 /** Set gyroscope Z-axis FIFO enabled value.
00727  * @param enabled New gyroscope Z-axis FIFO enabled value
00728  * @see getZGyroFIFOEnabled()
00729  * @see MPU6050_RA_FIFO_EN
00730  */
00731 void MPU6050::setZGyroFIFOEnabled(bool enabled)
00732 {
00733     i2Cdev.writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled);
00734 }
00735 /** Get accelerometer FIFO enabled value.
00736  * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
00737  * ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be
00738  * written into the FIFO buffer.
00739  * @return Current accelerometer FIFO enabled value
00740  * @see MPU6050_RA_FIFO_EN
00741  */
00742 bool MPU6050::getAccelFIFOEnabled()
00743 {
00744     i2Cdev.readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer);
00745     return buffer[0];
00746 }
00747 /** Set accelerometer FIFO enabled value.
00748  * @param enabled New accelerometer FIFO enabled value
00749  * @see getAccelFIFOEnabled()
00750  * @see MPU6050_RA_FIFO_EN
00751  */
00752 void MPU6050::setAccelFIFOEnabled(bool enabled)
00753 {
00754     i2Cdev.writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled);
00755 }
00756 /** Get Slave 2 FIFO enabled value.
00757  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
00758  * associated with Slave 2 to be written into the FIFO buffer.
00759  * @return Current Slave 2 FIFO enabled value
00760  * @see MPU6050_RA_FIFO_EN
00761  */
00762 bool MPU6050::getSlave2FIFOEnabled()
00763 {
00764     i2Cdev.readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer);
00765     return buffer[0];
00766 }
00767 /** Set Slave 2 FIFO enabled value.
00768  * @param enabled New Slave 2 FIFO enabled value
00769  * @see getSlave2FIFOEnabled()
00770  * @see MPU6050_RA_FIFO_EN
00771  */
00772 void MPU6050::setSlave2FIFOEnabled(bool enabled)
00773 {
00774     i2Cdev.writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled);
00775 }
00776 /** Get Slave 1 FIFO enabled value.
00777  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
00778  * associated with Slave 1 to be written into the FIFO buffer.
00779  * @return Current Slave 1 FIFO enabled value
00780  * @see MPU6050_RA_FIFO_EN
00781  */
00782 bool MPU6050::getSlave1FIFOEnabled()
00783 {
00784     i2Cdev.readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer);
00785     return buffer[0];
00786 }
00787 /** Set Slave 1 FIFO enabled value.
00788  * @param enabled New Slave 1 FIFO enabled value
00789  * @see getSlave1FIFOEnabled()
00790  * @see MPU6050_RA_FIFO_EN
00791  */
00792 void MPU6050::setSlave1FIFOEnabled(bool enabled)
00793 {
00794     i2Cdev.writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled);
00795 }
00796 /** Get Slave 0 FIFO enabled value.
00797  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
00798  * associated with Slave 0 to be written into the FIFO buffer.
00799  * @return Current Slave 0 FIFO enabled value
00800  * @see MPU6050_RA_FIFO_EN
00801  */
00802 bool MPU6050::getSlave0FIFOEnabled()
00803 {
00804     i2Cdev.readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer);
00805     return buffer[0];
00806 }
00807 /** Set Slave 0 FIFO enabled value.
00808  * @param enabled New Slave 0 FIFO enabled value
00809  * @see getSlave0FIFOEnabled()
00810  * @see MPU6050_RA_FIFO_EN
00811  */
00812 void MPU6050::setSlave0FIFOEnabled(bool enabled)
00813 {
00814     i2Cdev.writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled);
00815 }
00816 
00817 // I2C_MST_CTRL register
00818 
00819 /** Get multi-master enabled value.
00820  * Multi-master capability allows multiple I2C masters to operate on the same
00821  * bus. In circuits where multi-master capability is required, set MULT_MST_EN
00822  * to 1. This will increase current drawn by approximately 30uA.
00823  *
00824  * In circuits where multi-master capability is required, the state of the I2C
00825  * bus must always be monitored by each separate I2C Master. Before an I2C
00826  * Master can assume arbitration of the bus, it must first confirm that no other
00827  * I2C Master has arbitration of the bus. When MULT_MST_EN is set to 1, the
00828  * MPU-60X0's bus arbitration detection logic is turned on, enabling it to
00829  * detect when the bus is available.
00830  *
00831  * @return Current multi-master enabled value
00832  * @see MPU6050_RA_I2C_MST_CTRL
00833  */
00834 bool MPU6050::getMultiMasterEnabled()
00835 {
00836     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer);
00837     return buffer[0];
00838 }
00839 /** Set multi-master enabled value.
00840  * @param enabled New multi-master enabled value
00841  * @see getMultiMasterEnabled()
00842  * @see MPU6050_RA_I2C_MST_CTRL
00843  */
00844 void MPU6050::setMultiMasterEnabled(bool enabled)
00845 {
00846     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
00847 }
00848 /** Get wait-for-external-sensor-data enabled value.
00849  * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
00850  * delayed until External Sensor data from the Slave Devices are loaded into the
00851  * EXT_SENS_DATA registers. This is used to ensure that both the internal sensor
00852  * data (i.e. from gyro and accel) and external sensor data have been loaded to
00853  * their respective data registers (i.e. the data is synced) when the Data Ready
00854  * interrupt is triggered.
00855  *
00856  * @return Current wait-for-external-sensor-data enabled value
00857  * @see MPU6050_RA_I2C_MST_CTRL
00858  */
00859 bool MPU6050::getWaitForExternalSensorEnabled()
00860 {
00861     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer);
00862     return buffer[0];
00863 }
00864 /** Set wait-for-external-sensor-data enabled value.
00865  * @param enabled New wait-for-external-sensor-data enabled value
00866  * @see getWaitForExternalSensorEnabled()
00867  * @see MPU6050_RA_I2C_MST_CTRL
00868  */
00869 void MPU6050::setWaitForExternalSensorEnabled(bool enabled)
00870 {
00871     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
00872 }
00873 /** Get Slave 3 FIFO enabled value.
00874  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
00875  * associated with Slave 3 to be written into the FIFO buffer.
00876  * @return Current Slave 3 FIFO enabled value
00877  * @see MPU6050_RA_MST_CTRL
00878  */
00879 bool MPU6050::getSlave3FIFOEnabled()
00880 {
00881     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer);
00882     return buffer[0];
00883 }
00884 /** Set Slave 3 FIFO enabled value.
00885  * @param enabled New Slave 3 FIFO enabled value
00886  * @see getSlave3FIFOEnabled()
00887  * @see MPU6050_RA_MST_CTRL
00888  */
00889 void MPU6050::setSlave3FIFOEnabled(bool enabled)
00890 {
00891     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled);
00892 }
00893 /** Get slave read/write transition enabled value.
00894  * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
00895  * read to the next slave read. If the bit equals 0, there will be a restart
00896  * between reads. If the bit equals 1, there will be a stop followed by a start
00897  * of the following read. When a write transaction follows a read transaction,
00898  * the stop followed by a start of the successive write will be always used.
00899  *
00900  * @return Current slave read/write transition enabled value
00901  * @see MPU6050_RA_I2C_MST_CTRL
00902  */
00903 bool MPU6050::getSlaveReadWriteTransitionEnabled()
00904 {
00905     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer);
00906     return buffer[0];
00907 }
00908 /** Set slave read/write transition enabled value.
00909  * @param enabled New slave read/write transition enabled value
00910  * @see getSlaveReadWriteTransitionEnabled()
00911  * @see MPU6050_RA_I2C_MST_CTRL
00912  */
00913 void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled)
00914 {
00915     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled);
00916 }
00917 /** Get I2C master clock speed.
00918  * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the
00919  * MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to
00920  * the following table:
00921  *
00922  * <pre>
00923  * I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider
00924  * ------------+------------------------+-------------------
00925  * 0           | 348kHz                 | 23
00926  * 1           | 333kHz                 | 24
00927  * 2           | 320kHz                 | 25
00928  * 3           | 308kHz                 | 26
00929  * 4           | 296kHz                 | 27
00930  * 5           | 286kHz                 | 28
00931  * 6           | 276kHz                 | 29
00932  * 7           | 267kHz                 | 30
00933  * 8           | 258kHz                 | 31
00934  * 9           | 500kHz                 | 16
00935  * 10          | 471kHz                 | 17
00936  * 11          | 444kHz                 | 18
00937  * 12          | 421kHz                 | 19
00938  * 13          | 400kHz                 | 20
00939  * 14          | 381kHz                 | 21
00940  * 15          | 364kHz                 | 22
00941  * </pre>
00942  *
00943  * @return Current I2C master clock speed
00944  * @see MPU6050_RA_I2C_MST_CTRL
00945  */
00946 uint8_t MPU6050::getMasterClockSpeed()
00947 {
00948     i2Cdev.readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer);
00949     return buffer[0];
00950 }
00951 /** Set I2C master clock speed.
00952  * @reparam speed Current I2C master clock speed
00953  * @see MPU6050_RA_I2C_MST_CTRL
00954  */
00955 void MPU6050::setMasterClockSpeed(uint8_t speed)
00956 {
00957     i2Cdev.writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed);
00958 }
00959 
00960 // I2C_SLV* registers (Slave 0-3)
00961 
00962 /** Get the I2C address of the specified slave (0-3).
00963  * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
00964  * operation, and if it is cleared, then it's a write operation. The remaining
00965  * bits (6-0) are the 7-bit device address of the slave device.
00966  *
00967  * In read mode, the result of the read is placed in the lowest available
00968  * EXT_SENS_DATA register. For further information regarding the allocation of
00969  * read results, please refer to the EXT_SENS_DATA register description
00970  * (Registers 73 - 96).
00971  *
00972  * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
00973  * characteristics, and so it has its own functions (getSlave4* and setSlave4*).
00974  *
00975  * I2C data transactions are performed at the Sample Rate, as defined in
00976  * Register 25. The user is responsible for ensuring that I2C data transactions
00977  * to and from each enabled Slave can be completed within a single period of the
00978  * Sample Rate.
00979  *
00980  * The I2C slave access rate can be reduced relative to the Sample Rate. This
00981  * reduced access rate is determined by I2C_MST_DLY (Register 52). Whether a
00982  * slave's access rate is reduced relative to the Sample Rate is determined by
00983  * I2C_MST_DELAY_CTRL (Register 103).
00984  *
00985  * The processing order for the slaves is fixed. The sequence followed for
00986  * processing the slaves is Slave 0, Slave 1, Slave 2, Slave 3 and Slave 4. If a
00987  * particular Slave is disabled it will be skipped.
00988  *
00989  * Each slave can either be accessed at the sample rate or at a reduced sample
00990  * rate. In a case where some slaves are accessed at the Sample Rate and some
00991  * slaves are accessed at the reduced rate, the sequence of accessing the slaves
00992  * (Slave 0 to Slave 4) is still followed. However, the reduced rate slaves will
00993  * be skipped if their access rate dictates that they should not be accessed
00994  * during that particular cycle. For further information regarding the reduced
00995  * access rate, please refer to Register 52. Whether a slave is accessed at the
00996  * Sample Rate or at the reduced rate is determined by the Delay Enable bits in
00997  * Register 103.
00998  *
00999  * @param num Slave number (0-3)
01000  * @return Current address for specified slave
01001  * @see MPU6050_RA_I2C_SLV0_ADDR
01002  */
01003 uint8_t MPU6050::getSlaveAddress(uint8_t num)
01004 {
01005     if (num > 3) return 0;
01006     i2Cdev.readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer);
01007     return buffer[0];
01008 }
01009 /** Set the I2C address of the specified slave (0-3).
01010  * @param num Slave number (0-3)
01011  * @param address New address for specified slave
01012  * @see getSlaveAddress()
01013  * @see MPU6050_RA_I2C_SLV0_ADDR
01014  */
01015 void MPU6050::setSlaveAddress(uint8_t num, uint8_t address)
01016 {
01017     if (num > 3) return;
01018     i2Cdev.writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address);
01019 }
01020 /** Get the active internal register for the specified slave (0-3).
01021  * Read/write operations for this slave will be done to whatever internal
01022  * register address is stored in this MPU register.
01023  *
01024  * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
01025  * characteristics, and so it has its own functions.
01026  *
01027  * @param num Slave number (0-3)
01028  * @return Current active register for specified slave
01029  * @see MPU6050_RA_I2C_SLV0_REG
01030  */
01031 uint8_t MPU6050::getSlaveRegister(uint8_t num)
01032 {
01033     if (num > 3) return 0;
01034     i2Cdev.readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer);
01035     return buffer[0];
01036 }
01037 /** Set the active internal register for the specified slave (0-3).
01038  * @param num Slave number (0-3)
01039  * @param reg New active register for specified slave
01040  * @see getSlaveRegister()
01041  * @see MPU6050_RA_I2C_SLV0_REG
01042  */
01043 void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg)
01044 {
01045     if (num > 3) return;
01046     i2Cdev.writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, reg);
01047 }
01048 /** Get the enabled value for the specified slave (0-3).
01049  * When set to 1, this bit enables Slave 0 for data transfer operations. When
01050  * cleared to 0, this bit disables Slave 0 from data transfer operations.
01051  * @param num Slave number (0-3)
01052  * @return Current enabled value for specified slave
01053  * @see MPU6050_RA_I2C_SLV0_CTRL
01054  */
01055 bool MPU6050::getSlaveEnabled(uint8_t num)
01056 {
01057     if (num > 3) return 0;
01058     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer);
01059     return buffer[0];
01060 }
01061 /** Set the enabled value for the specified slave (0-3).
01062  * @param num Slave number (0-3)
01063  * @param enabled New enabled value for specified slave 
01064  * @see getSlaveEnabled()
01065  * @see MPU6050_RA_I2C_SLV0_CTRL
01066  */
01067 void MPU6050::setSlaveEnabled(uint8_t num, bool enabled)
01068 {
01069     if (num > 3) return;
01070     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled);
01071 }
01072 /** Get word pair byte-swapping enabled for the specified slave (0-3).
01073  * When set to 1, this bit enables byte swapping. When byte swapping is enabled,
01074  * the high and low bytes of a word pair are swapped. Please refer to
01075  * I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0,
01076  * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA
01077  * registers in the order they were transferred.
01078  *
01079  * @param num Slave number (0-3)
01080  * @return Current word pair byte-swapping enabled value for specified slave
01081  * @see MPU6050_RA_I2C_SLV0_CTRL
01082  */
01083 bool MPU6050::getSlaveWordByteSwap(uint8_t num)
01084 {
01085     if (num > 3) return 0;
01086     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer);
01087     return buffer[0];
01088 }
01089 /** Set word pair byte-swapping enabled for the specified slave (0-3).
01090  * @param num Slave number (0-3)
01091  * @param enabled New word pair byte-swapping enabled value for specified slave
01092  * @see getSlaveWordByteSwap()
01093  * @see MPU6050_RA_I2C_SLV0_CTRL
01094  */
01095 void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
01096 {
01097     if (num > 3) return;
01098     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
01099 }
01100 /** Get write mode for the specified slave (0-3).
01101  * When set to 1, the transaction will read or write data only. When cleared to
01102  * 0, the transaction will write a register address prior to reading or writing
01103  * data. This should equal 0 when specifying the register address within the
01104  * Slave device to/from which the ensuing data transaction will take place.
01105  *
01106  * @param num Slave number (0-3)
01107  * @return Current write mode for specified slave (0 = register address + data, 1 = data only)
01108  * @see MPU6050_RA_I2C_SLV0_CTRL
01109  */
01110 bool MPU6050::getSlaveWriteMode(uint8_t num)
01111 {
01112     if (num > 3) return 0;
01113     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer);
01114     return buffer[0];
01115 }
01116 /** Set write mode for the specified slave (0-3).
01117  * @param num Slave number (0-3)
01118  * @param mode New write mode for specified slave (0 = register address + data, 1 = data only)
01119  * @see getSlaveWriteMode()
01120  * @see MPU6050_RA_I2C_SLV0_CTRL
01121  */
01122 void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
01123 {
01124     if (num > 3) return;
01125     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
01126 }
01127 /** Get word pair grouping order offset for the specified slave (0-3).
01128  * This sets specifies the grouping order of word pairs received from registers.
01129  * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even,
01130  * then odd register addresses) are paired to form a word. When set to 1, bytes
01131  * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
01132  * register addresses) are paired to form a word.
01133  *
01134  * @param num Slave number (0-3)
01135  * @return Current word pair grouping order offset for specified slave
01136  * @see MPU6050_RA_I2C_SLV0_CTRL
01137  */
01138 bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
01139 {
01140     if (num > 3) return 0;
01141     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer);
01142     return buffer[0];
01143 }
01144 /** Set word pair grouping order offset for the specified slave (0-3).
01145  * @param num Slave number (0-3)
01146  * @param enabled New word pair grouping order offset for specified slave
01147  * @see getSlaveWordGroupOffset()
01148  * @see MPU6050_RA_I2C_SLV0_CTRL
01149  */
01150 void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
01151 {
01152     if (num > 3) return;
01153     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled);
01154 }
01155 /** Get number of bytes to read for the specified slave (0-3).
01156  * Specifies the number of bytes transferred to and from Slave 0. Clearing this
01157  * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
01158  * @param num Slave number (0-3)
01159  * @return Number of bytes to read for specified slave
01160  * @see MPU6050_RA_I2C_SLV0_CTRL
01161  */
01162 uint8_t MPU6050::getSlaveDataLength(uint8_t num)
01163 {
01164     if (num > 3) return 0;
01165     i2Cdev.readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer);
01166     return buffer[0];
01167 }
01168 /** Set number of bytes to read for the specified slave (0-3).
01169  * @param num Slave number (0-3)
01170  * @param length Number of bytes to read for specified slave
01171  * @see getSlaveDataLength()
01172  * @see MPU6050_RA_I2C_SLV0_CTRL
01173  */
01174 void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length)
01175 {
01176     if (num > 3) return;
01177     i2Cdev.writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length);
01178 }
01179 
01180 // I2C_SLV* registers (Slave 4)
01181 
01182 /** Get the I2C address of Slave 4.
01183  * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
01184  * operation, and if it is cleared, then it's a write operation. The remaining
01185  * bits (6-0) are the 7-bit device address of the slave device.
01186  *
01187  * @return Current address for Slave 4
01188  * @see getSlaveAddress()
01189  * @see MPU6050_RA_I2C_SLV4_ADDR
01190  */
01191 uint8_t MPU6050::getSlave4Address()
01192 {
01193     i2Cdev.readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer);
01194     return buffer[0];
01195 }
01196 /** Set the I2C address of Slave 4.
01197  * @param address New address for Slave 4
01198  * @see getSlave4Address()
01199  * @see MPU6050_RA_I2C_SLV4_ADDR
01200  */
01201 void MPU6050::setSlave4Address(uint8_t address)
01202 {
01203     i2Cdev.writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address);
01204 }
01205 /** Get the active internal register for the Slave 4.
01206  * Read/write operations for this slave will be done to whatever internal
01207  * register address is stored in this MPU register.
01208  *
01209  * @return Current active register for Slave 4
01210  * @see MPU6050_RA_I2C_SLV4_REG
01211  */
01212 uint8_t MPU6050::getSlave4Register()
01213 {
01214     i2Cdev.readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer);
01215     return buffer[0];
01216 }
01217 /** Set the active internal register for Slave 4.
01218  * @param reg New active register for Slave 4
01219  * @see getSlave4Register()
01220  * @see MPU6050_RA_I2C_SLV4_REG
01221  */
01222 void MPU6050::setSlave4Register(uint8_t reg)
01223 {
01224     i2Cdev.writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg);
01225 }
01226 /** Set new byte to write to Slave 4.
01227  * This register stores the data to be written into the Slave 4. If I2C_SLV4_RW
01228  * is set 1 (set to read), this register has no effect.
01229  * @param data New byte to write to Slave 4
01230  * @see MPU6050_RA_I2C_SLV4_DO
01231  */
01232 void MPU6050::setSlave4OutputByte(uint8_t data)
01233 {
01234     i2Cdev.writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data);
01235 }
01236 /** Get the enabled value for the Slave 4.
01237  * When set to 1, this bit enables Slave 4 for data transfer operations. When
01238  * cleared to 0, this bit disables Slave 4 from data transfer operations.
01239  * @return Current enabled value for Slave 4
01240  * @see MPU6050_RA_I2C_SLV4_CTRL
01241  */
01242 bool MPU6050::getSlave4Enabled()
01243 {
01244     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer);
01245     return buffer[0];
01246 }
01247 /** Set the enabled value for Slave 4.
01248  * @param enabled New enabled value for Slave 4
01249  * @see getSlave4Enabled()
01250  * @see MPU6050_RA_I2C_SLV4_CTRL
01251  */
01252 void MPU6050::setSlave4Enabled(bool enabled)
01253 {
01254     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled);
01255 }
01256 /** Get the enabled value for Slave 4 transaction interrupts.
01257  * When set to 1, this bit enables the generation of an interrupt signal upon
01258  * completion of a Slave 4 transaction. When cleared to 0, this bit disables the
01259  * generation of an interrupt signal upon completion of a Slave 4 transaction.
01260  * The interrupt status can be observed in Register 54.
01261  *
01262  * @return Current enabled value for Slave 4 transaction interrupts.
01263  * @see MPU6050_RA_I2C_SLV4_CTRL
01264  */
01265 bool MPU6050::getSlave4InterruptEnabled()
01266 {
01267     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer);
01268     return buffer[0];
01269 }
01270 /** Set the enabled value for Slave 4 transaction interrupts.
01271  * @param enabled New enabled value for Slave 4 transaction interrupts.
01272  * @see getSlave4InterruptEnabled()
01273  * @see MPU6050_RA_I2C_SLV4_CTRL
01274  */
01275 void MPU6050::setSlave4InterruptEnabled(bool enabled)
01276 {
01277     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
01278 }
01279 /** Get write mode for Slave 4.
01280  * When set to 1, the transaction will read or write data only. When cleared to
01281  * 0, the transaction will write a register address prior to reading or writing
01282  * data. This should equal 0 when specifying the register address within the
01283  * Slave device to/from which the ensuing data transaction will take place.
01284  *
01285  * @return Current write mode for Slave 4 (0 = register address + data, 1 = data only)
01286  * @see MPU6050_RA_I2C_SLV4_CTRL
01287  */
01288 bool MPU6050::getSlave4WriteMode()
01289 {
01290     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer);
01291     return buffer[0];
01292 }
01293 /** Set write mode for the Slave 4.
01294  * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data only)
01295  * @see getSlave4WriteMode()
01296  * @see MPU6050_RA_I2C_SLV4_CTRL
01297  */
01298 void MPU6050::setSlave4WriteMode(bool mode)
01299 {
01300     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode);
01301 }
01302 /** Get Slave 4 master delay value.
01303  * This configures the reduced access rate of I2C slaves relative to the Sample
01304  * Rate. When a slave's access rate is decreased relative to the Sample Rate,
01305  * the slave is accessed every:
01306  *
01307  *     1 / (1 + I2C_MST_DLY) samples
01308  *
01309  * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and
01310  * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to
01311  * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For
01312  * further information regarding the Sample Rate, please refer to register 25.
01313  *
01314  * @return Current Slave 4 master delay value
01315  * @see MPU6050_RA_I2C_SLV4_CTRL
01316  */
01317 uint8_t MPU6050::getSlave4MasterDelay()
01318 {
01319     i2Cdev.readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer);
01320     return buffer[0];
01321 }
01322 /** Set Slave 4 master delay value.
01323  * @param delay New Slave 4 master delay value
01324  * @see getSlave4MasterDelay()
01325  * @see MPU6050_RA_I2C_SLV4_CTRL
01326  */
01327 void MPU6050::setSlave4MasterDelay(uint8_t delay)
01328 {
01329     i2Cdev.writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
01330 }
01331 /** Get last available byte read from Slave 4.
01332  * This register stores the data read from Slave 4. This field is populated
01333  * after a read transaction.
01334  * @return Last available byte read from to Slave 4
01335  * @see MPU6050_RA_I2C_SLV4_DI
01336  */
01337 uint8_t MPU6050::getSlate4InputByte()
01338 {
01339     i2Cdev.readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer);
01340     return buffer[0];
01341 }
01342 
01343 // I2C_MST_STATUS register
01344 
01345 /** Get FSYNC interrupt status.
01346  * This bit reflects the status of the FSYNC interrupt from an external device
01347  * into the MPU-60X0. This is used as a way to pass an external interrupt
01348  * through the MPU-60X0 to the host application processor. When set to 1, this
01349  * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG
01350  * (Register 55).
01351  * @return FSYNC interrupt status
01352  * @see MPU6050_RA_I2C_MST_STATUS
01353  */
01354 bool MPU6050::getPassthroughStatus()
01355 {
01356     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer);
01357     return buffer[0];
01358 }
01359 /** Get Slave 4 transaction done status.
01360  * Automatically sets to 1 when a Slave 4 transaction has completed. This
01361  * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register
01362  * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
01363  * I2C_SLV4_CTRL register (Register 52).
01364  * @return Slave 4 transaction done status
01365  * @see MPU6050_RA_I2C_MST_STATUS
01366  */
01367 bool MPU6050::getSlave4IsDone()
01368 {
01369     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer);
01370     return buffer[0];
01371 }
01372 /** Get master arbitration lost status.
01373  * This bit automatically sets to 1 when the I2C Master has lost arbitration of
01374  * the auxiliary I2C bus (an error condition). This triggers an interrupt if the
01375  * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
01376  * @return Master arbitration lost status
01377  * @see MPU6050_RA_I2C_MST_STATUS
01378  */
01379 bool MPU6050::getLostArbitration()
01380 {
01381     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer);
01382     return buffer[0];
01383 }
01384 /** Get Slave 4 NACK status.
01385  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
01386  * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
01387  * bit in the INT_ENABLE register (Register 56) is asserted.
01388  * @return Slave 4 NACK interrupt status
01389  * @see MPU6050_RA_I2C_MST_STATUS
01390  */
01391 bool MPU6050::getSlave4Nack()
01392 {
01393     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer);
01394     return buffer[0];
01395 }
01396 /** Get Slave 3 NACK status.
01397  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
01398  * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN
01399  * bit in the INT_ENABLE register (Register 56) is asserted.
01400  * @return Slave 3 NACK interrupt status
01401  * @see MPU6050_RA_I2C_MST_STATUS
01402  */
01403 bool MPU6050::getSlave3Nack()
01404 {
01405     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer);
01406     return buffer[0];
01407 }
01408 /** Get Slave 2 NACK status.
01409  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
01410  * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN
01411  * bit in the INT_ENABLE register (Register 56) is asserted.
01412  * @return Slave 2 NACK interrupt status
01413  * @see MPU6050_RA_I2C_MST_STATUS
01414  */
01415 bool MPU6050::getSlave2Nack()
01416 {
01417     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer);
01418     return buffer[0];
01419 }
01420 /** Get Slave 1 NACK status.
01421  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
01422  * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN
01423  * bit in the INT_ENABLE register (Register 56) is asserted.
01424  * @return Slave 1 NACK interrupt status
01425  * @see MPU6050_RA_I2C_MST_STATUS
01426  */
01427 bool MPU6050::getSlave1Nack()
01428 {
01429     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer);
01430     return buffer[0];
01431 }
01432 /** Get Slave 0 NACK status.
01433  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
01434  * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN
01435  * bit in the INT_ENABLE register (Register 56) is asserted.
01436  * @return Slave 0 NACK interrupt status
01437  * @see MPU6050_RA_I2C_MST_STATUS
01438  */
01439 bool MPU6050::getSlave0Nack()
01440 {
01441     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer);
01442     return buffer[0];
01443 }
01444 
01445 // INT_PIN_CFG register
01446 
01447 /** Get interrupt logic level mode.
01448  * Will be set 0 for active-high, 1 for active-low.
01449  * @return Current interrupt mode (0=active-high, 1=active-low)
01450  * @see MPU6050_RA_INT_PIN_CFG
01451  * @see MPU6050_INTCFG_INT_LEVEL_BIT
01452  */
01453 bool MPU6050::getInterruptMode()
01454 {
01455     i2Cdev.readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer);
01456     return buffer[0];
01457 }
01458 /** Set interrupt logic level mode.
01459  * @param mode New interrupt mode (0=active-high, 1=active-low)
01460  * @see getInterruptMode()
01461  * @see MPU6050_RA_INT_PIN_CFG
01462  * @see MPU6050_INTCFG_INT_LEVEL_BIT
01463  */
01464 void MPU6050::setInterruptMode(bool mode)
01465 {
01466     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode);
01467 }
01468 /** Get interrupt drive mode.
01469  * Will be set 0 for push-pull, 1 for open-drain.
01470  * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
01471  * @see MPU6050_RA_INT_PIN_CFG
01472  * @see MPU6050_INTCFG_INT_OPEN_BIT
01473  */
01474 bool MPU6050::getInterruptDrive()
01475 {
01476     i2Cdev.readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer);
01477     return buffer[0];
01478 }
01479 /** Set interrupt drive mode.
01480  * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
01481  * @see getInterruptDrive()
01482  * @see MPU6050_RA_INT_PIN_CFG
01483  * @see MPU6050_INTCFG_INT_OPEN_BIT
01484  */
01485 void MPU6050::setInterruptDrive(bool drive)
01486 {
01487     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive);
01488 }
01489 /** Get interrupt latch mode.
01490  * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
01491  * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
01492  * @see MPU6050_RA_INT_PIN_CFG
01493  * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
01494  */
01495 bool MPU6050::getInterruptLatch()
01496 {
01497     i2Cdev.readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer);
01498     return buffer[0];
01499 }
01500 /** Set interrupt latch mode.
01501  * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
01502  * @see getInterruptLatch()
01503  * @see MPU6050_RA_INT_PIN_CFG
01504  * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
01505  */
01506 void MPU6050::setInterruptLatch(bool latch)
01507 {
01508     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch);
01509 }
01510 /** Get interrupt latch clear mode.
01511  * Will be set 0 for status-read-only, 1 for any-register-read.
01512  * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
01513  * @see MPU6050_RA_INT_PIN_CFG
01514  * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
01515  */
01516 bool MPU6050::getInterruptLatchClear()
01517 {
01518     i2Cdev.readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer);
01519     return buffer[0];
01520 }
01521 /** Set interrupt latch clear mode.
01522  * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
01523  * @see getInterruptLatchClear()
01524  * @see MPU6050_RA_INT_PIN_CFG
01525  * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
01526  */
01527 void MPU6050::setInterruptLatchClear(bool clear)
01528 {
01529     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
01530 }
01531 /** Get FSYNC interrupt logic level mode.
01532  * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
01533  * @see getFSyncInterruptMode()
01534  * @see MPU6050_RA_INT_PIN_CFG
01535  * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
01536  */
01537 bool MPU6050::getFSyncInterruptLevel()
01538 {
01539     i2Cdev.readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
01540     return buffer[0];
01541 }
01542 /** Set FSYNC interrupt logic level mode.
01543  * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
01544  * @see getFSyncInterruptMode()
01545  * @see MPU6050_RA_INT_PIN_CFG
01546  * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
01547  */
01548 void MPU6050::setFSyncInterruptLevel(bool level)
01549 {
01550     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
01551 }
01552 /** Get FSYNC pin interrupt enabled setting.
01553  * Will be set 0 for disabled, 1 for enabled.
01554  * @return Current interrupt enabled setting
01555  * @see MPU6050_RA_INT_PIN_CFG
01556  * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
01557  */
01558 bool MPU6050::getFSyncInterruptEnabled()
01559 {
01560     i2Cdev.readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer);
01561     return buffer[0];
01562 }
01563 /** Set FSYNC pin interrupt enabled setting.
01564  * @param enabled New FSYNC pin interrupt enabled setting
01565  * @see getFSyncInterruptEnabled()
01566  * @see MPU6050_RA_INT_PIN_CFG
01567  * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
01568  */
01569 void MPU6050::setFSyncInterruptEnabled(bool enabled)
01570 {
01571     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
01572 }
01573 /** Get I2C bypass enabled status.
01574  * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
01575  * 0, the host application processor will be able to directly access the
01576  * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
01577  * application processor will not be able to directly access the auxiliary I2C
01578  * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
01579  * bit[5]).
01580  * @return Current I2C bypass enabled status
01581  * @see MPU6050_RA_INT_PIN_CFG
01582  * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
01583  */
01584 bool MPU6050::getI2CBypassEnabled()
01585 {
01586     i2Cdev.readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer);
01587     return buffer[0];
01588 }
01589 /** Set I2C bypass enabled status.
01590  * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
01591  * 0, the host application processor will be able to directly access the
01592  * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
01593  * application processor will not be able to directly access the auxiliary I2C
01594  * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
01595  * bit[5]).
01596  * @param enabled New I2C bypass enabled status
01597  * @see MPU6050_RA_INT_PIN_CFG
01598  * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
01599  */
01600 void MPU6050::setI2CBypassEnabled(bool enabled)
01601 {
01602     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
01603 }
01604 /** Get reference clock output enabled status.
01605  * When this bit is equal to 1, a reference clock output is provided at the
01606  * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
01607  * further information regarding CLKOUT, please refer to the MPU-60X0 Product
01608  * Specification document.
01609  * @return Current reference clock output enabled status
01610  * @see MPU6050_RA_INT_PIN_CFG
01611  * @see MPU6050_INTCFG_CLKOUT_EN_BIT
01612  */
01613 bool MPU6050::getClockOutputEnabled()
01614 {
01615     i2Cdev.readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer);
01616     return buffer[0];
01617 }
01618 /** Set reference clock output enabled status.
01619  * When this bit is equal to 1, a reference clock output is provided at the
01620  * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
01621  * further information regarding CLKOUT, please refer to the MPU-60X0 Product
01622  * Specification document.
01623  * @param enabled New reference clock output enabled status
01624  * @see MPU6050_RA_INT_PIN_CFG
01625  * @see MPU6050_INTCFG_CLKOUT_EN_BIT
01626  */
01627 void MPU6050::setClockOutputEnabled(bool enabled)
01628 {
01629     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
01630 }
01631 
01632 // INT_ENABLE register
01633 
01634 /** Get full interrupt enabled status.
01635  * Full register byte for all interrupts, for quick reading. Each bit will be
01636  * set 0 for disabled, 1 for enabled.
01637  * @return Current interrupt enabled status
01638  * @see MPU6050_RA_INT_ENABLE
01639  * @see MPU6050_INTERRUPT_FF_BIT
01640  **/
01641 uint8_t MPU6050::getIntEnabled()
01642 {
01643     i2Cdev.readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer);
01644     return buffer[0];
01645 }
01646 /** Set full interrupt enabled status.
01647  * Full register byte for all interrupts, for quick reading. Each bit should be
01648  * set 0 for disabled, 1 for enabled.
01649  * @param enabled New interrupt enabled status
01650  * @see getIntFreefallEnabled()
01651  * @see MPU6050_RA_INT_ENABLE
01652  * @see MPU6050_INTERRUPT_FF_BIT
01653  **/
01654 void MPU6050::setIntEnabled(uint8_t enabled)
01655 {
01656     i2Cdev.writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled);
01657 }
01658 /** Get Free Fall interrupt enabled status.
01659  * Will be set 0 for disabled, 1 for enabled.
01660  * @return Current interrupt enabled status
01661  * @see MPU6050_RA_INT_ENABLE
01662  * @see MPU6050_INTERRUPT_FF_BIT
01663  **/
01664 bool MPU6050::getIntFreefallEnabled()
01665 {
01666     i2Cdev.readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer);
01667     return buffer[0];
01668 }
01669 /** Set Free Fall interrupt enabled status.
01670  * @param enabled New interrupt enabled status
01671  * @see getIntFreefallEnabled()
01672  * @see MPU6050_RA_INT_ENABLE
01673  * @see MPU6050_INTERRUPT_FF_BIT
01674  **/
01675 void MPU6050::setIntFreefallEnabled(bool enabled)
01676 {
01677     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled);
01678 }
01679 /** Get Motion Detection interrupt enabled status.
01680  * Will be set 0 for disabled, 1 for enabled.
01681  * @return Current interrupt enabled status
01682  * @see MPU6050_RA_INT_ENABLE
01683  * @see MPU6050_INTERRUPT_MOT_BIT
01684  **/
01685 bool MPU6050::getIntMotionEnabled()
01686 {
01687     i2Cdev.readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer);
01688     return buffer[0];
01689 }
01690 /** Set Motion Detection interrupt enabled status.
01691  * @param enabled New interrupt enabled status
01692  * @see getIntMotionEnabled()
01693  * @see MPU6050_RA_INT_ENABLE
01694  * @see MPU6050_INTERRUPT_MOT_BIT
01695  **/
01696 void MPU6050::setIntMotionEnabled(bool enabled)
01697 {
01698     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
01699 }
01700 /** Get Zero Motion Detection interrupt enabled status.
01701  * Will be set 0 for disabled, 1 for enabled.
01702  * @return Current interrupt enabled status
01703  * @see MPU6050_RA_INT_ENABLE
01704  * @see MPU6050_INTERRUPT_ZMOT_BIT
01705  **/
01706 bool MPU6050::getIntZeroMotionEnabled()
01707 {
01708     i2Cdev.readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
01709     return buffer[0];
01710 }
01711 /** Set Zero Motion Detection interrupt enabled status.
01712  * @param enabled New interrupt enabled status
01713  * @see getIntZeroMotionEnabled()
01714  * @see MPU6050_RA_INT_ENABLE
01715  * @see MPU6050_INTERRUPT_ZMOT_BIT
01716  **/
01717 void MPU6050::setIntZeroMotionEnabled(bool enabled)
01718 {
01719     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
01720 }
01721 /** Get FIFO Buffer Overflow interrupt enabled status.
01722  * Will be set 0 for disabled, 1 for enabled.
01723  * @return Current interrupt enabled status
01724  * @see MPU6050_RA_INT_ENABLE
01725  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
01726  **/
01727 bool MPU6050::getIntFIFOBufferOverflowEnabled()
01728 {
01729     i2Cdev.readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
01730     return buffer[0];
01731 }
01732 /** Set FIFO Buffer Overflow interrupt enabled status.
01733  * @param enabled New interrupt enabled status
01734  * @see getIntFIFOBufferOverflowEnabled()
01735  * @see MPU6050_RA_INT_ENABLE
01736  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
01737  **/
01738 void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled)
01739 {
01740     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
01741 }
01742 /** Get I2C Master interrupt enabled status.
01743  * This enables any of the I2C Master interrupt sources to generate an
01744  * interrupt. Will be set 0 for disabled, 1 for enabled.
01745  * @return Current interrupt enabled status
01746  * @see MPU6050_RA_INT_ENABLE
01747  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
01748  **/
01749 bool MPU6050::getIntI2CMasterEnabled()
01750 {
01751     i2Cdev.readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
01752     return buffer[0];
01753 }
01754 /** Set I2C Master interrupt enabled status.
01755  * @param enabled New interrupt enabled status
01756  * @see getIntI2CMasterEnabled()
01757  * @see MPU6050_RA_INT_ENABLE
01758  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
01759  **/
01760 void MPU6050::setIntI2CMasterEnabled(bool enabled)
01761 {
01762     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled);
01763 }
01764 /** Get Data Ready interrupt enabled setting.
01765  * This event occurs each time a write operation to all of the sensor registers
01766  * has been completed. Will be set 0 for disabled, 1 for enabled.
01767  * @return Current interrupt enabled status
01768  * @see MPU6050_RA_INT_ENABLE
01769  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
01770  */
01771 bool MPU6050::getIntDataReadyEnabled()
01772 {
01773     i2Cdev.readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
01774     return buffer[0];
01775 }
01776 /** Set Data Ready interrupt enabled status.
01777  * @param enabled New interrupt enabled status
01778  * @see getIntDataReadyEnabled()
01779  * @see MPU6050_RA_INT_CFG
01780  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
01781  */
01782 void MPU6050::setIntDataReadyEnabled(bool enabled)
01783 {
01784     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
01785 }
01786 
01787 // INT_STATUS register
01788 
01789 /** Get full set of interrupt status bits.
01790  * These bits clear to 0 after the register has been read. Very useful
01791  * for getting multiple INT statuses, since each single bit read clears
01792  * all of them because it has to read the whole byte.
01793  * @return Current interrupt status
01794  * @see MPU6050_RA_INT_STATUS
01795  */
01796 uint8_t MPU6050::getIntStatus()
01797 {
01798     i2Cdev.readByte(devAddr, MPU6050_RA_INT_STATUS, buffer);
01799     return buffer[0];
01800 }
01801 /** Get Free Fall interrupt status.
01802  * This bit automatically sets to 1 when a Free Fall interrupt has been
01803  * generated. The bit clears to 0 after the register has been read.
01804  * @return Current interrupt status
01805  * @see MPU6050_RA_INT_STATUS
01806  * @see MPU6050_INTERRUPT_FF_BIT
01807  */
01808 bool MPU6050::getIntFreefallStatus()
01809 {
01810     i2Cdev.readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer);
01811     return buffer[0];
01812 }
01813 /** Get Motion Detection interrupt status.
01814  * This bit automatically sets to 1 when a Motion Detection interrupt has been
01815  * generated. The bit clears to 0 after the register has been read.
01816  * @return Current interrupt status
01817  * @see MPU6050_RA_INT_STATUS
01818  * @see MPU6050_INTERRUPT_MOT_BIT
01819  */
01820 bool MPU6050::getIntMotionStatus()
01821 {
01822     i2Cdev.readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer);
01823     return buffer[0];
01824 }
01825 /** Get Zero Motion Detection interrupt status.
01826  * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
01827  * been generated. The bit clears to 0 after the register has been read.
01828  * @return Current interrupt status
01829  * @see MPU6050_RA_INT_STATUS
01830  * @see MPU6050_INTERRUPT_ZMOT_BIT
01831  */
01832 bool MPU6050::getIntZeroMotionStatus()
01833 {
01834     i2Cdev.readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
01835     return buffer[0];
01836 }
01837 /** Get FIFO Buffer Overflow interrupt status.
01838  * This bit automatically sets to 1 when a Free Fall interrupt has been
01839  * generated. The bit clears to 0 after the register has been read.
01840  * @return Current interrupt status
01841  * @see MPU6050_RA_INT_STATUS
01842  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
01843  */
01844 bool MPU6050::getIntFIFOBufferOverflowStatus()
01845 {
01846     i2Cdev.readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
01847     return buffer[0];
01848 }
01849 /** Get I2C Master interrupt status.
01850  * This bit automatically sets to 1 when an I2C Master interrupt has been
01851  * generated. For a list of I2C Master interrupts, please refer to Register 54.
01852  * The bit clears to 0 after the register has been read.
01853  * @return Current interrupt status
01854  * @see MPU6050_RA_INT_STATUS
01855  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
01856  */
01857 bool MPU6050::getIntI2CMasterStatus()
01858 {
01859     i2Cdev.readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
01860     return buffer[0];
01861 }
01862 /** Get Data Ready interrupt status.
01863  * This bit automatically sets to 1 when a Data Ready interrupt has been
01864  * generated. The bit clears to 0 after the register has been read.
01865  * @return Current interrupt status
01866  * @see MPU6050_RA_INT_STATUS
01867  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
01868  */
01869 bool MPU6050::getIntDataReadyStatus()
01870 {
01871     i2Cdev.readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
01872     return buffer[0];
01873 }
01874 
01875 // ACCEL_*OUT_* registers
01876 
01877 /** Get raw 9-axis motion sensor readings (accel/gyro/compass).
01878  * FUNCTION NOT FULLY IMPLEMENTED YET.
01879  * @param ax 16-bit signed integer container for accelerometer X-axis value
01880  * @param ay 16-bit signed integer container for accelerometer Y-axis value
01881  * @param az 16-bit signed integer container for accelerometer Z-axis value
01882  * @param gx 16-bit signed integer container for gyroscope X-axis value
01883  * @param gy 16-bit signed integer container for gyroscope Y-axis value
01884  * @param gz 16-bit signed integer container for gyroscope Z-axis value
01885  * @param mx 16-bit signed integer container for magnetometer X-axis value
01886  * @param my 16-bit signed integer container for magnetometer Y-axis value
01887  * @param mz 16-bit signed integer container for magnetometer Z-axis value
01888  * @see getMotion6()
01889  * @see getAcceleration()
01890  * @see getRotation()
01891  * @see MPU6050_RA_ACCEL_XOUT_H
01892  */
01893 void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz)
01894 {
01895 #ifdef MPU9250
01896     get9250Motion9Counts(ax, ay, az, gx, gy, gz, mx, my, mz);
01897 #else
01898     getMotion6(ax, ay, az, gx, gy, gz);
01899     // TODO: magnetometer integration
01900 #endif
01901 }
01902 
01903 /** Get raw 6-axis motion sensor readings (accel/gyro).
01904  * Retrieves all currently available motion sensor values.
01905  * @param ax 16-bit signed integer container for accelerometer X-axis value
01906  * @param ay 16-bit signed integer container for accelerometer Y-axis value
01907  * @param az 16-bit signed integer container for accelerometer Z-axis value
01908  * @param gx 16-bit signed integer container for gyroscope X-axis value
01909  * @param gy 16-bit signed integer container for gyroscope Y-axis value
01910  * @param gz 16-bit signed integer container for gyroscope Z-axis value
01911  * @see getAcceleration()
01912  * @see getRotation()
01913  * @see MPU6050_RA_ACCEL_XOUT_H
01914  */
01915 void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
01916 {
01917     *ax = ax_cache;
01918     *ay = ay_cache;
01919     *az = az_cache;
01920     *gx = gx_cache;
01921     *gy = gy_cache;
01922     *gz = gz_cache;
01923 }
01924 
01925 /*
01926  * Populate readings cache when finished reading from I2C
01927  */
01928 uint32_t mpureadfin(uint32_t param){
01929     MPU6050* ins = (MPU6050*)param;
01930 #ifdef MPU9250
01931     ins->ax_cache = (((int16_t)ins->mpu_buffer[0]) << 8) | ins->mpu_buffer[1];
01932     ins->ay_cache = (((int16_t)ins->mpu_buffer[2]) << 8) | ins->mpu_buffer[3];
01933     ins->az_cache = (((int16_t)ins->mpu_buffer[4]) << 8) | ins->mpu_buffer[5];
01934     ins->gx_cache = (((int16_t)ins->mpu_buffer[8]) << 8) | ins->mpu_buffer[9];
01935     ins->gy_cache = (((int16_t)ins->mpu_buffer[10]) << 8) | ins->mpu_buffer[11];
01936     ins->gz_cache = (((int16_t)ins->mpu_buffer[12]) << 8) | ins->mpu_buffer[13];
01937     ins->hx_cache = (((int16_t)ins->mpu_buffer[15]) << 8) | ins->mpu_buffer[14];
01938     ins->hy_cache = (((int16_t)ins->mpu_buffer[17]) << 8) | ins->mpu_buffer[16];
01939     ins->hz_cache = (((int16_t)ins->mpu_buffer[19]) << 8) | ins->mpu_buffer[18];
01940 #else
01941     ins->ax_cache = (((int16_t)ins->mpu_buffer[0]) << 8) | ins->mpu_buffer[1];
01942     ins->ay_cache = (((int16_t)ins->mpu_buffer[2]) << 8) | ins->mpu_buffer[3];
01943     ins->az_cache = (((int16_t)ins->mpu_buffer[4]) << 8) | ins->mpu_buffer[5];
01944     ins->gx_cache = (((int16_t)ins->mpu_buffer[8]) << 8) | ins->mpu_buffer[9];
01945     ins->gy_cache = (((int16_t)ins->mpu_buffer[10]) << 8) | ins->mpu_buffer[11];
01946     ins->gz_cache = (((int16_t)ins->mpu_buffer[12]) << 8) | ins->mpu_buffer[13];
01947 #endif
01948     return 0;
01949 }
01950 
01951 void MPU6050::sample(bool on){
01952    if (sampling && !on)
01953       mpu_sampling.detach();
01954    if (!sampling && on)
01955       start_sampling();
01956    sampling = on;
01957 }
01958 
01959 #ifdef MPU9250
01960 volatile bool magn_valid_9250 = false;
01961 #endif
01962 
01963 void MPU6050::mpu_sample_func(){ 
01964 #ifdef MPU9250
01965     i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 21, (uint8_t*)mpu_buffer, &mpureadfin, this);
01966     static uint32_t counter = 0;
01967     counter += 1;
01968     if (counter % 5 == 0) {
01969         magn_valid_9250 = true;
01970     }
01971 #else
01972     i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 14, (uint8_t*)mpu_buffer, &mpureadfin, this);
01973 #endif
01974 }
01975 
01976 void MPU6050::start_sampling(){
01977     sampling = true;
01978     mpu_cmd = MPU6050_RA_ACCEL_XOUT_H;
01979     // sample at 500hz
01980     mpu_sampling.attach_us(this, &MPU6050::mpu_sample_func, 2000);
01981 }
01982 
01983 /** Get 3-axis accelerometer readings.
01984  * These registers store the most recent accelerometer measurements.
01985  * Accelerometer measurements are written to these registers at the Sample Rate
01986  * as defined in Register 25.
01987  *
01988  * The accelerometer measurement registers, along with the temperature
01989  * measurement registers, gyroscope measurement registers, and external sensor
01990  * data registers, are composed of two sets of registers: an internal register
01991  * set and a user-facing read register set.
01992  *
01993  * The data within the accelerometer sensors' internal register set is always
01994  * updated at the Sample Rate. Meanwhile, the user-facing read register set
01995  * duplicates the internal register set's data values whenever the serial
01996  * interface is idle. This guarantees that a burst read of sensor registers will
01997  * read measurements from the same sampling instant. Note that if burst reads
01998  * are not used, the user is responsible for ensuring a set of single byte reads
01999  * correspond to a single sampling instant by checking the Data Ready interrupt.
02000  *
02001  * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
02002  * (Register 28). For each full scale setting, the accelerometers' sensitivity
02003  * per LSB in ACCEL_xOUT is shown in the table below:
02004  *
02005  * <pre>
02006  * AFS_SEL | Full Scale Range | LSB Sensitivity
02007  * --------+------------------+----------------
02008  * 0       | +/- 2g           | 8192 LSB/mg
02009  * 1       | +/- 4g           | 4096 LSB/mg
02010  * 2       | +/- 8g           | 2048 LSB/mg
02011  * 3       | +/- 16g          | 1024 LSB/mg
02012  * </pre>
02013  *
02014  * @param x 16-bit signed integer container for X-axis acceleration
02015  * @param y 16-bit signed integer container for Y-axis acceleration
02016  * @param z 16-bit signed integer container for Z-axis acceleration
02017  * @see MPU6050_RA_GYRO_XOUT_H
02018  */
02019 void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z)
02020 {
02021     i2Cdev.readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);
02022     *x = (((int16_t)buffer[0]) << 8) | buffer[1];
02023     *y = (((int16_t)buffer[2]) << 8) | buffer[3];
02024     *z = (((int16_t)buffer[4]) << 8) | buffer[5];
02025 }
02026 /** Get X-axis accelerometer reading.
02027  * @return X-axis acceleration measurement in 16-bit 2's complement format
02028  * @see getMotion6()
02029  * @see MPU6050_RA_ACCEL_XOUT_H
02030  */
02031 int16_t MPU6050::getAccelerationX()
02032 {
02033     i2Cdev.readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer);
02034     return (((int16_t)buffer[0]) << 8) | buffer[1];
02035 }
02036 /** Get Y-axis accelerometer reading.
02037  * @return Y-axis acceleration measurement in 16-bit 2's complement format
02038  * @see getMotion6()
02039  * @see MPU6050_RA_ACCEL_YOUT_H
02040  */
02041 int16_t MPU6050::getAccelerationY()
02042 {
02043     i2Cdev.readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer);
02044     return (((int16_t)buffer[0]) << 8) | buffer[1];
02045 }
02046 /** Get Z-axis accelerometer reading.
02047  * @return Z-axis acceleration measurement in 16-bit 2's complement format
02048  * @see getMotion6()
02049  * @see MPU6050_RA_ACCEL_ZOUT_H
02050  */
02051 int16_t MPU6050::getAccelerationZ()
02052 {
02053     i2Cdev.readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);
02054     return (((int16_t)buffer[0]) << 8) | buffer[1];
02055 }
02056 
02057 // TEMP_OUT_* registers
02058 
02059 /** Get current internal temperature.
02060  * @return Temperature reading in 16-bit 2's complement format
02061  * @see MPU6050_RA_TEMP_OUT_H
02062  */
02063 int16_t MPU6050::getTemperature()
02064 {
02065     i2Cdev.readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer);
02066     return (((int16_t)buffer[0]) << 8) | buffer[1];
02067 }
02068 
02069 // GYRO_*OUT_* registers
02070 
02071 /** Get 3-axis gyroscope readings.
02072  * These gyroscope measurement registers, along with the accelerometer
02073  * measurement registers, temperature measurement registers, and external sensor
02074  * data registers, are composed of two sets of registers: an internal register
02075  * set and a user-facing read register set.
02076  * The data within the gyroscope sensors' internal register set is always
02077  * updated at the Sample Rate. Meanwhile, the user-facing read register set
02078  * duplicates the internal register set's data values whenever the serial
02079  * interface is idle. This guarantees that a burst read of sensor registers will
02080  * read measurements from the same sampling instant. Note that if burst reads
02081  * are not used, the user is responsible for ensuring a set of single byte reads
02082  * correspond to a single sampling instant by checking the Data Ready interrupt.
02083  *
02084  * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
02085  * (Register 27). For each full scale setting, the gyroscopes' sensitivity per
02086  * LSB in GYRO_xOUT is shown in the table below:
02087  *
02088  * <pre>
02089  * FS_SEL | Full Scale Range   | LSB Sensitivity
02090  * -------+--------------------+----------------
02091  * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
02092  * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
02093  * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
02094  * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
02095  * </pre>
02096  *
02097  * @param x 16-bit signed integer container for X-axis rotation
02098  * @param y 16-bit signed integer container for Y-axis rotation
02099  * @param z 16-bit signed integer container for Z-axis rotation
02100  * @see getMotion6()
02101  * @see MPU6050_RA_GYRO_XOUT_H
02102  */
02103 void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z)
02104 {
02105     i2Cdev.readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
02106     *x = (((int16_t)buffer[0]) << 8) | buffer[1];
02107     *y = (((int16_t)buffer[2]) << 8) | buffer[3];
02108     *z = (((int16_t)buffer[4]) << 8) | buffer[5];
02109 }
02110 /** Get X-axis gyroscope reading.
02111  * @return X-axis rotation measurement in 16-bit 2's complement format
02112  * @see getMotion6()
02113  * @see MPU6050_RA_GYRO_XOUT_H
02114  */
02115 int16_t MPU6050::getRotationX()
02116 {
02117     i2Cdev.readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer);
02118     return (((int16_t)buffer[0]) << 8) | buffer[1];
02119 }
02120 /** Get Y-axis gyroscope reading.
02121  * @return Y-axis rotation measurement in 16-bit 2's complement format
02122  * @see getMotion6()
02123  * @see MPU6050_RA_GYRO_YOUT_H
02124  */
02125 int16_t MPU6050::getRotationY()
02126 {
02127     i2Cdev.readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer);
02128     return (((int16_t)buffer[0]) << 8) | buffer[1];
02129 }
02130 /** Get Z-axis gyroscope reading.
02131  * @return Z-axis rotation measurement in 16-bit 2's complement format
02132  * @see getMotion6()
02133  * @see MPU6050_RA_GYRO_ZOUT_H
02134  */
02135 int16_t MPU6050::getRotationZ()
02136 {
02137     i2Cdev.readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
02138     return (((int16_t)buffer[0]) << 8) | buffer[1];
02139 }
02140 
02141 // EXT_SENS_DATA_* registers
02142 
02143 /** Read single byte from external sensor data register.
02144  * These registers store data read from external sensors by the Slave 0, 1, 2,
02145  * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in
02146  * I2C_SLV4_DI (Register 53).
02147  *
02148  * External sensor data is written to these registers at the Sample Rate as
02149  * defined in Register 25. This access rate can be reduced by using the Slave
02150  * Delay Enable registers (Register 103).
02151  *
02152  * External sensor data registers, along with the gyroscope measurement
02153  * registers, accelerometer measurement registers, and temperature measurement
02154  * registers, are composed of two sets of registers: an internal register set
02155  * and a user-facing read register set.
02156  *
02157  * The data within the external sensors' internal register set is always updated
02158  * at the Sample Rate (or the reduced access rate) whenever the serial interface
02159  * is idle. This guarantees that a burst read of sensor registers will read
02160  * measurements from the same sampling instant. Note that if burst reads are not
02161  * used, the user is responsible for ensuring a set of single byte reads
02162  * correspond to a single sampling instant by checking the Data Ready interrupt.
02163  *
02164  * Data is placed in these external sensor data registers according to
02165  * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39,
02166  * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from
02167  * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as
02168  * defined in Register 25) or delayed rate (if specified in Register 52 and
02169  * 103). During each Sample cycle, slave reads are performed in order of Slave
02170  * number. If all slaves are enabled with more than zero bytes to be read, the
02171  * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3.
02172  *
02173  * Each enabled slave will have EXT_SENS_DATA registers associated with it by
02174  * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from
02175  * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may
02176  * change the higher numbered slaves' associated registers. Furthermore, if
02177  * fewer total bytes are being read from the external sensors as a result of
02178  * such a change, then the data remaining in the registers which no longer have
02179  * an associated slave device (i.e. high numbered registers) will remain in
02180  * these previously allocated registers unless reset.
02181  *
02182  * If the sum of the read lengths of all SLVx transactions exceed the number of
02183  * available EXT_SENS_DATA registers, the excess bytes will be dropped. There
02184  * are 24 EXT_SENS_DATA registers and hence the total read lengths between all
02185  * the slaves cannot be greater than 24 or some bytes will be lost.
02186  *
02187  * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further
02188  * information regarding the characteristics of Slave 4, please refer to
02189  * Registers 49 to 53.
02190  *
02191  * EXAMPLE:
02192  * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and
02193  * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that
02194  * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00
02195  * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05
02196  * will be associated with Slave 1. If Slave 2 is enabled as well, registers
02197  * starting from EXT_SENS_DATA_06 will be allocated to Slave 2.
02198  *
02199  * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then
02200  * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3
02201  * instead.
02202  *
02203  * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE:
02204  * If a slave is disabled at any time, the space initially allocated to the
02205  * slave in the EXT_SENS_DATA register, will remain associated with that slave.
02206  * This is to avoid dynamic adjustment of the register allocation.
02207  *
02208  * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all
02209  * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106).
02210  *
02211  * This above is also true if one of the slaves gets NACKed and stops
02212  * functioning.
02213  *
02214  * @param position Starting position (0-23)
02215  * @return Byte read from register
02216  */
02217 uint8_t MPU6050::getExternalSensorByte(int position)
02218 {
02219     i2Cdev.readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer);
02220     return buffer[0];
02221 }
02222 /** Read word (2 bytes) from external sensor data registers.
02223  * @param position Starting position (0-21)
02224  * @return Word read from register
02225  * @see getExternalSensorByte()
02226  */
02227 uint16_t MPU6050::getExternalSensorWord(int position)
02228 {
02229     i2Cdev.readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer);
02230     return (((uint16_t)buffer[0]) << 8) | buffer[1];
02231 }
02232 /** Read double word (4 bytes) from external sensor data registers.
02233  * @param position Starting position (0-20)
02234  * @return Double word read from registers
02235  * @see getExternalSensorByte()
02236  */
02237 uint32_t MPU6050::getExternalSensorDWord(int position)
02238 {
02239     i2Cdev.readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer);
02240     return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3];
02241 }
02242 
02243 // MOT_DETECT_STATUS register
02244 
02245 /** Get X-axis negative motion detection interrupt status.
02246  * @return Motion detection status
02247  * @see MPU6050_RA_MOT_DETECT_STATUS
02248  * @see MPU6050_MOTION_MOT_XNEG_BIT
02249  */
02250 bool MPU6050::getXNegMotionDetected()
02251 {
02252     i2Cdev.readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer);
02253     return buffer[0];
02254 }
02255 /** Get X-axis positive motion detection interrupt status.
02256  * @return Motion detection status
02257  * @see MPU6050_RA_MOT_DETECT_STATUS
02258  * @see MPU6050_MOTION_MOT_XPOS_BIT
02259  */
02260 bool MPU6050::getXPosMotionDetected()
02261 {
02262     i2Cdev.readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer);
02263     return buffer[0];
02264 }
02265 /** Get Y-axis negative motion detection interrupt status.
02266  * @return Motion detection status
02267  * @see MPU6050_RA_MOT_DETECT_STATUS
02268  * @see MPU6050_MOTION_MOT_YNEG_BIT
02269  */
02270 bool MPU6050::getYNegMotionDetected()
02271 {
02272     i2Cdev.readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer);
02273     return buffer[0];
02274 }
02275 /** Get Y-axis positive motion detection interrupt status.
02276  * @return Motion detection status
02277  * @see MPU6050_RA_MOT_DETECT_STATUS
02278  * @see MPU6050_MOTION_MOT_YPOS_BIT
02279  */
02280 bool MPU6050::getYPosMotionDetected()
02281 {
02282     i2Cdev.readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer);
02283     return buffer[0];
02284 }
02285 /** Get Z-axis negative motion detection interrupt status.
02286  * @return Motion detection status
02287  * @see MPU6050_RA_MOT_DETECT_STATUS
02288  * @see MPU6050_MOTION_MOT_ZNEG_BIT
02289  */
02290 bool MPU6050::getZNegMotionDetected()
02291 {
02292     i2Cdev.readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer);
02293     return buffer[0];
02294 }
02295 /** Get Z-axis positive motion detection interrupt status.
02296  * @return Motion detection status
02297  * @see MPU6050_RA_MOT_DETECT_STATUS
02298  * @see MPU6050_MOTION_MOT_ZPOS_BIT
02299  */
02300 bool MPU6050::getZPosMotionDetected()
02301 {
02302     i2Cdev.readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer);
02303     return buffer[0];
02304 }
02305 /** Get zero motion detection interrupt status.
02306  * @return Motion detection status
02307  * @see MPU6050_RA_MOT_DETECT_STATUS
02308  * @see MPU6050_MOTION_MOT_ZRMOT_BIT
02309  */
02310 bool MPU6050::getZeroMotionDetected()
02311 {
02312     i2Cdev.readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer);
02313     return buffer[0];
02314 }
02315 
02316 // I2C_SLV*_DO register
02317 
02318 /** Write byte to Data Output container for specified slave.
02319  * This register holds the output data written into Slave when Slave is set to
02320  * write mode. For further information regarding Slave control, please
02321  * refer to Registers 37 to 39 and immediately following.
02322  * @param num Slave number (0-3)
02323  * @param data Byte to write
02324  * @see MPU6050_RA_I2C_SLV0_DO
02325  */
02326 void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data)
02327 {
02328     if (num > 3) return;
02329     i2Cdev.writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
02330 }
02331 
02332 // I2C_MST_DELAY_CTRL register
02333 
02334 /** Get external data shadow delay enabled status.
02335  * This register is used to specify the timing of external sensor data
02336  * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external
02337  * sensor data is delayed until all data has been received.
02338  * @return Current external data shadow delay enabled status.
02339  * @see MPU6050_RA_I2C_MST_DELAY_CTRL
02340  * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
02341  */
02342 bool MPU6050::getExternalShadowDelayEnabled()
02343 {
02344     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer);
02345     return buffer[0];
02346 }
02347 /** Set external data shadow delay enabled status.
02348  * @param enabled New external data shadow delay enabled status.
02349  * @see getExternalShadowDelayEnabled()
02350  * @see MPU6050_RA_I2C_MST_DELAY_CTRL
02351  * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
02352  */
02353 void MPU6050::setExternalShadowDelayEnabled(bool enabled)
02354 {
02355     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
02356 }
02357 /** Get slave delay enabled status.
02358  * When a particular slave delay is enabled, the rate of access for the that
02359  * slave device is reduced. When a slave's access rate is decreased relative to
02360  * the Sample Rate, the slave is accessed every:
02361  *
02362  *     1 / (1 + I2C_MST_DLY) Samples
02363  *
02364  * This base Sample Rate in turn is determined by SMPLRT_DIV (register  * 25)
02365  * and DLPF_CFG (register 26).
02366  *
02367  * For further information regarding I2C_MST_DLY, please refer to register 52.
02368  * For further information regarding the Sample Rate, please refer to register 25.
02369  *
02370  * @param num Slave number (0-4)
02371  * @return Current slave delay enabled status.
02372  * @see MPU6050_RA_I2C_MST_DELAY_CTRL
02373  * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
02374  */
02375 bool MPU6050::getSlaveDelayEnabled(uint8_t num)
02376 {
02377     // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
02378     if (num > 4) return 0;
02379     i2Cdev.readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer);
02380     return buffer[0];
02381 }
02382 /** Set slave delay enabled status.
02383  * @param num Slave number (0-4)
02384  * @param enabled New slave delay enabled status.
02385  * @see MPU6050_RA_I2C_MST_DELAY_CTRL
02386  * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
02387  */
02388 void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled)
02389 {
02390     i2Cdev.writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
02391 }
02392 
02393 // SIGNAL_PATH_RESET register
02394 
02395 /** Reset gyroscope signal path.
02396  * The reset will revert the signal path analog to digital converters and
02397  * filters to their power up configurations.
02398  * @see MPU6050_RA_SIGNAL_PATH_RESET
02399  * @see MPU6050_PATHRESET_GYRO_RESET_BIT
02400  */
02401 void MPU6050::resetGyroscopePath()
02402 {
02403     i2Cdev.writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
02404 }
02405 /** Reset accelerometer signal path.
02406  * The reset will revert the signal path analog to digital converters and
02407  * filters to their power up configurations.
02408  * @see MPU6050_RA_SIGNAL_PATH_RESET
02409  * @see MPU6050_PATHRESET_ACCEL_RESET_BIT
02410  */
02411 void MPU6050::resetAccelerometerPath()
02412 {
02413     i2Cdev.writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);
02414 }
02415 /** Reset temperature sensor signal path.
02416  * The reset will revert the signal path analog to digital converters and
02417  * filters to their power up configurations.
02418  * @see MPU6050_RA_SIGNAL_PATH_RESET
02419  * @see MPU6050_PATHRESET_TEMP_RESET_BIT
02420  */
02421 void MPU6050::resetTemperaturePath()
02422 {
02423     i2Cdev.writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);
02424 }
02425 
02426 // MOT_DETECT_CTRL register
02427 
02428 /** Get accelerometer power-on delay.
02429  * The accelerometer data path provides samples to the sensor registers, Motion
02430  * detection, Zero Motion detection, and Free Fall detection modules. The
02431  * signal path contains filters which must be flushed on wake-up with new
02432  * samples before the detection modules begin operations. The default wake-up
02433  * delay, of 4ms can be lengthened by up to 3ms. This additional delay is
02434  * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select
02435  * any value above zero unless instructed otherwise by InvenSense. Please refer
02436  * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for
02437  * further information regarding the detection modules.
02438  * @return Current accelerometer power-on delay
02439  * @see MPU6050_RA_MOT_DETECT_CTRL
02440  * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
02441  */
02442 uint8_t MPU6050::getAccelerometerPowerOnDelay()
02443 {
02444     i2Cdev.readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
02445     return buffer[0];
02446 }
02447 /** Set accelerometer power-on delay.
02448  * @param delay New accelerometer power-on delay (0-3)
02449  * @see getAccelerometerPowerOnDelay()
02450  * @see MPU6050_RA_MOT_DETECT_CTRL
02451  * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
02452  */
02453 void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay)
02454 {
02455     i2Cdev.writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
02456 }
02457 /** Get Free Fall detection counter decrement configuration.
02458  * Detection is registered by the Free Fall detection module after accelerometer
02459  * measurements meet their respective threshold conditions over a specified
02460  * number of samples. When the threshold conditions are met, the corresponding
02461  * detection counter increments by 1. The user may control the rate at which the
02462  * detection counter decrements when the threshold condition is not met by
02463  * configuring FF_COUNT. The decrement rate can be set according to the
02464  * following table:
02465  *
02466  * <pre>
02467  * FF_COUNT | Counter Decrement
02468  * ---------+------------------
02469  * 0        | Reset
02470  * 1        | 1
02471  * 2        | 2
02472  * 3        | 4
02473  * </pre>
02474  *
02475  * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will
02476  * reset the counter to 0. For further information on Free Fall detection,
02477  * please refer to Registers 29 to 32.
02478  *
02479  * @return Current decrement configuration
02480  * @see MPU6050_RA_MOT_DETECT_CTRL
02481  * @see MPU6050_DETECT_FF_COUNT_BIT
02482  */
02483 uint8_t MPU6050::getFreefallDetectionCounterDecrement()
02484 {
02485     i2Cdev.readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer);
02486     return buffer[0];
02487 }
02488 /** Set Free Fall detection counter decrement configuration.
02489  * @param decrement New decrement configuration value
02490  * @see getFreefallDetectionCounterDecrement()
02491  * @see MPU6050_RA_MOT_DETECT_CTRL
02492  * @see MPU6050_DETECT_FF_COUNT_BIT
02493  */
02494 void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement)
02495 {
02496     i2Cdev.writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, decrement);
02497 }
02498 /** Get Motion detection counter decrement configuration.
02499  * Detection is registered by the Motion detection module after accelerometer
02500  * measurements meet their respective threshold conditions over a specified
02501  * number of samples. When the threshold conditions are met, the corresponding
02502  * detection counter increments by 1. The user may control the rate at which the
02503  * detection counter decrements when the threshold condition is not met by
02504  * configuring MOT_COUNT. The decrement rate can be set according to the
02505  * following table:
02506  *
02507  * <pre>
02508  * MOT_COUNT | Counter Decrement
02509  * ----------+------------------
02510  * 0         | Reset
02511  * 1         | 1
02512  * 2         | 2
02513  * 3         | 4
02514  * </pre>
02515  *
02516  * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will
02517  * reset the counter to 0. For further information on Motion detection,
02518  * please refer to Registers 29 to 32.
02519  *
02520  */
02521 uint8_t MPU6050::getMotionDetectionCounterDecrement()
02522 {
02523     i2Cdev.readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer);
02524     return buffer[0];
02525 }
02526 /** Set Motion detection counter decrement configuration.
02527  * @param decrement New decrement configuration value
02528  * @see getMotionDetectionCounterDecrement()
02529  * @see MPU6050_RA_MOT_DETECT_CTRL
02530  * @see MPU6050_DETECT_MOT_COUNT_BIT
02531  */
02532 void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement)
02533 {
02534     i2Cdev.writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, decrement);
02535 }
02536 
02537 // USER_CTRL register
02538 
02539 /** Get FIFO enabled status.
02540  * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
02541  * cannot be written to or read from while disabled. The FIFO buffer's state
02542  * does not change unless the MPU-60X0 is power cycled.
02543  * @return Current FIFO enabled status
02544  * @see MPU6050_RA_USER_CTRL
02545  * @see MPU6050_USERCTRL_FIFO_EN_BIT
02546  */
02547 bool MPU6050::getFIFOEnabled()
02548 {
02549     i2Cdev.readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer);
02550     return buffer[0];
02551 }
02552 /** Set FIFO enabled status.
02553  * @param enabled New FIFO enabled status
02554  * @see getFIFOEnabled()
02555  * @see MPU6050_RA_USER_CTRL
02556  * @see MPU6050_USERCTRL_FIFO_EN_BIT
02557  */
02558 void MPU6050::setFIFOEnabled(bool enabled)
02559 {
02560     i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
02561 }
02562 /** Get I2C Master Mode enabled status.
02563  * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
02564  * external sensor slave devices on the auxiliary I2C bus. When this bit is
02565  * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically
02566  * driven by the primary I2C bus (SDA and SCL). This is a precondition to
02567  * enabling Bypass Mode. For further information regarding Bypass Mode, please
02568  * refer to Register 55.
02569  * @return Current I2C Master Mode enabled status
02570  * @see MPU6050_RA_USER_CTRL
02571  * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
02572  */
02573 bool MPU6050::getI2CMasterModeEnabled()
02574 {
02575     i2Cdev.readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer);
02576     return buffer[0];
02577 }
02578 /** Set I2C Master Mode enabled status.
02579  * @param enabled New I2C Master Mode enabled status
02580  * @see getI2CMasterModeEnabled()
02581  * @see MPU6050_RA_USER_CTRL
02582  * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
02583  */
02584 void MPU6050::setI2CMasterModeEnabled(bool enabled)
02585 {
02586     i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
02587 }
02588 /** Switch from I2C to SPI mode (MPU-6000 only)
02589  * If this is set, the primary SPI interface will be enabled in place of the
02590  * disabled primary I2C interface.
02591  */
02592 void MPU6050::switchSPIEnabled(bool enabled)
02593 {
02594     i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
02595 }
02596 /** Reset the FIFO.
02597  * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
02598  * bit automatically clears to 0 after the reset has been triggered.
02599  * @see MPU6050_RA_USER_CTRL
02600  * @see MPU6050_USERCTRL_FIFO_RESET_BIT
02601  */
02602 void MPU6050::resetFIFO()
02603 {
02604     i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true);
02605 }
02606 /** Reset the I2C Master.
02607  * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
02608  * This bit automatically clears to 0 after the reset has been triggered.
02609  * @see MPU6050_RA_USER_CTRL
02610  * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT
02611  */
02612 void MPU6050::resetI2CMaster()
02613 {
02614     i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true);
02615 }
02616 /** Reset all sensor registers and signal paths.
02617  * When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
02618  * accelerometers, and temperature sensor). This operation will also clear the
02619  * sensor registers. This bit automatically clears to 0 after the reset has been
02620  * triggered.
02621  *
02622  * When resetting only the signal path (and not the sensor registers), please
02623  * use Register 104, SIGNAL_PATH_RESET.
02624  *
02625  * @see MPU6050_RA_USER_CTRL
02626  * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT
02627  */
02628 void MPU6050::resetSensors()
02629 {
02630     i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true);
02631 }
02632 
02633 // PWR_MGMT_1 register
02634 
02635 /** Trigger a full device reset.
02636  * A small delay of ~50ms may be desirable after triggering a reset.
02637  * @see MPU6050_RA_PWR_MGMT_1
02638  * @see MPU6050_PWR1_DEVICE_RESET_BIT
02639  */
02640 void MPU6050::reset()
02641 {
02642     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
02643 }
02644 /** Get sleep mode status.
02645  * Setting the SLEEP bit in the register puts the device into very low power
02646  * sleep mode. In this mode, only the serial interface and internal registers
02647  * remain active, allowing for a very low standby current. Clearing this bit
02648  * puts the device back into normal mode. To save power, the individual standby
02649  * selections for each of the gyros should be used if any gyro axis is not used
02650  * by the application.
02651  * @return Current sleep mode enabled status
02652  * @see MPU6050_RA_PWR_MGMT_1
02653  * @see MPU6050_PWR1_SLEEP_BIT
02654  */
02655 bool MPU6050::getSleepEnabled()
02656 {
02657     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer);
02658     return buffer[0];
02659 }
02660 /** Set sleep mode status.
02661  * @param enabled New sleep mode enabled status
02662  * @see getSleepEnabled()
02663  * @see MPU6050_RA_PWR_MGMT_1
02664  * @see MPU6050_PWR1_SLEEP_BIT
02665  */
02666 void MPU6050::setSleepEnabled(bool enabled)
02667 {
02668     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
02669 }
02670 /** Get wake cycle enabled status.
02671  * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
02672  * between sleep mode and waking up to take a single sample of data from active
02673  * sensors at a rate determined by LP_WAKE_CTRL (register 108).
02674  * @return Current sleep mode enabled status
02675  * @see MPU6050_RA_PWR_MGMT_1
02676  * @see MPU6050_PWR1_CYCLE_BIT
02677  */
02678 bool MPU6050::getWakeCycleEnabled()
02679 {
02680     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer);
02681     return buffer[0];
02682 }
02683 /** Set wake cycle enabled status.
02684  * @param enabled New sleep mode enabled status
02685  * @see getWakeCycleEnabled()
02686  * @see MPU6050_RA_PWR_MGMT_1
02687  * @see MPU6050_PWR1_CYCLE_BIT
02688  */
02689 void MPU6050::setWakeCycleEnabled(bool enabled)
02690 {
02691     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
02692 }
02693 /** Get temperature sensor enabled status.
02694  * Control the usage of the internal temperature sensor.
02695  *
02696  * Note: this register stores the *disabled* value, but for consistency with the
02697  * rest of the code, the function is named and used with standard true/false
02698  * values to indicate whether the sensor is enabled or disabled, respectively.
02699  *
02700  * @return Current temperature sensor enabled status
02701  * @see MPU6050_RA_PWR_MGMT_1
02702  * @see MPU6050_PWR1_TEMP_DIS_BIT
02703  */
02704 bool MPU6050::getTempSensorEnabled()
02705 {
02706     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer);
02707     return buffer[0] == 0; // 1 is actually disabled here
02708 }
02709 /** Set temperature sensor enabled status.
02710  * Note: this register stores the *disabled* value, but for consistency with the
02711  * rest of the code, the function is named and used with standard true/false
02712  * values to indicate whether the sensor is enabled or disabled, respectively.
02713  *
02714  * @param enabled New temperature sensor enabled status
02715  * @see getTempSensorEnabled()
02716  * @see MPU6050_RA_PWR_MGMT_1
02717  * @see MPU6050_PWR1_TEMP_DIS_BIT
02718  */
02719 void MPU6050::setTempSensorEnabled(bool enabled)
02720 {
02721     // 1 is actually disabled here
02722     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
02723 }
02724 /** Get clock source setting.
02725  * @return Current clock source setting
02726  * @see MPU6050_RA_PWR_MGMT_1
02727  * @see MPU6050_PWR1_CLKSEL_BIT
02728  * @see MPU6050_PWR1_CLKSEL_LENGTH
02729  */
02730 uint8_t MPU6050::getClockSource()
02731 {
02732     i2Cdev.readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer);
02733     return buffer[0];
02734 }
02735 /** Set clock source setting.
02736  * An internal 8MHz oscillator, gyroscope based clock, or external sources can
02737  * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
02738  * or an external source is chosen as the clock source, the MPU-60X0 can operate
02739  * in low power modes with the gyroscopes disabled.
02740  *
02741  * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
02742  * However, it is highly recommended that the device be configured to use one of
02743  * the gyroscopes (or an external clock source) as the clock reference for
02744  * improved stability. The clock source can be selected according to the following table:
02745  *
02746  * <pre>
02747  * CLK_SEL | Clock Source
02748  * --------+--------------------------------------
02749  * 0       | Internal oscillator
02750  * 1       | PLL with X Gyro reference
02751  * 2       | PLL with Y Gyro reference
02752  * 3       | PLL with Z Gyro reference
02753  * 4       | PLL with external 32.768kHz reference
02754  * 5       | PLL with external 19.2MHz reference
02755  * 6       | Reserved
02756  * 7       | Stops the clock and keeps the timing generator in reset
02757  * </pre>
02758  *
02759  * @param source New clock source setting
02760  * @see getClockSource()
02761  * @see MPU6050_RA_PWR_MGMT_1
02762  * @see MPU6050_PWR1_CLKSEL_BIT
02763  * @see MPU6050_PWR1_CLKSEL_LENGTH
02764  */
02765 void MPU6050::setClockSource(uint8_t source)
02766 {
02767     i2Cdev.writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
02768 }
02769 
02770 // PWR_MGMT_2 register
02771 
02772 /** Get wake frequency in Accel-Only Low Power Mode.
02773  * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting
02774  * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode,
02775  * the device will power off all devices except for the primary I2C interface,
02776  * waking only the accelerometer at fixed intervals to take a single
02777  * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL
02778  * as shown below:
02779  *
02780  * <pre>
02781  * LP_WAKE_CTRL | Wake-up Frequency
02782  * -------------+------------------
02783  * 0            | 1.25 Hz
02784  * 1            | 2.5 Hz
02785  * 2            | 5 Hz
02786  * 3            | 10 Hz
02787  * <pre>
02788  *
02789  * For further information regarding the MPU-60X0's power modes, please refer to
02790  * Register 107.
02791  *
02792  * @return Current wake frequency
02793  * @see MPU6050_RA_PWR_MGMT_2
02794  */
02795 uint8_t MPU6050::getWakeFrequency()
02796 {
02797     i2Cdev.readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
02798     return buffer[0];
02799 }
02800 /** Set wake frequency in Accel-Only Low Power Mode.
02801  * @param frequency New wake frequency
02802  * @see MPU6050_RA_PWR_MGMT_2
02803  */
02804 void MPU6050::setWakeFrequency(uint8_t frequency)
02805 {
02806     i2Cdev.writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
02807 }
02808 
02809 /** Get X-axis accelerometer standby enabled status.
02810  * If enabled, the X-axis will not gather or report data (or use power).
02811  * @return Current X-axis standby enabled status
02812  * @see MPU6050_RA_PWR_MGMT_2
02813  * @see MPU6050_PWR2_STBY_XA_BIT
02814  */
02815 bool MPU6050::getStandbyXAccelEnabled()
02816 {
02817     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer);
02818     return buffer[0];
02819 }
02820 /** Set X-axis accelerometer standby enabled status.
02821  * @param New X-axis standby enabled status
02822  * @see getStandbyXAccelEnabled()
02823  * @see MPU6050_RA_PWR_MGMT_2
02824  * @see MPU6050_PWR2_STBY_XA_BIT
02825  */
02826 void MPU6050::setStandbyXAccelEnabled(bool enabled)
02827 {
02828     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled);
02829 }
02830 /** Get Y-axis accelerometer standby enabled status.
02831  * If enabled, the Y-axis will not gather or report data (or use power).
02832  * @return Current Y-axis standby enabled status
02833  * @see MPU6050_RA_PWR_MGMT_2
02834  * @see MPU6050_PWR2_STBY_YA_BIT
02835  */
02836 bool MPU6050::getStandbyYAccelEnabled()
02837 {
02838     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer);
02839     return buffer[0];
02840 }
02841 /** Set Y-axis accelerometer standby enabled status.
02842  * @param New Y-axis standby enabled status
02843  * @see getStandbyYAccelEnabled()
02844  * @see MPU6050_RA_PWR_MGMT_2
02845  * @see MPU6050_PWR2_STBY_YA_BIT
02846  */
02847 void MPU6050::setStandbyYAccelEnabled(bool enabled)
02848 {
02849     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled);
02850 }
02851 /** Get Z-axis accelerometer standby enabled status.
02852  * If enabled, the Z-axis will not gather or report data (or use power).
02853  * @return Current Z-axis standby enabled status
02854  * @see MPU6050_RA_PWR_MGMT_2
02855  * @see MPU6050_PWR2_STBY_ZA_BIT
02856  */
02857 bool MPU6050::getStandbyZAccelEnabled()
02858 {
02859     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer);
02860     return buffer[0];
02861 }
02862 /** Set Z-axis accelerometer standby enabled status.
02863  * @param New Z-axis standby enabled status
02864  * @see getStandbyZAccelEnabled()
02865  * @see MPU6050_RA_PWR_MGMT_2
02866  * @see MPU6050_PWR2_STBY_ZA_BIT
02867  */
02868 void MPU6050::setStandbyZAccelEnabled(bool enabled)
02869 {
02870     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled);
02871 }
02872 /** Get X-axis gyroscope standby enabled status.
02873  * If enabled, the X-axis will not gather or report data (or use power).
02874  * @return Current X-axis standby enabled status
02875  * @see MPU6050_RA_PWR_MGMT_2
02876  * @see MPU6050_PWR2_STBY_XG_BIT
02877  */
02878 bool MPU6050::getStandbyXGyroEnabled()
02879 {
02880     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer);
02881     return buffer[0];
02882 }
02883 /** Set X-axis gyroscope standby enabled status.
02884  * @param New X-axis standby enabled status
02885  * @see getStandbyXGyroEnabled()
02886  * @see MPU6050_RA_PWR_MGMT_2
02887  * @see MPU6050_PWR2_STBY_XG_BIT
02888  */
02889 void MPU6050::setStandbyXGyroEnabled(bool enabled)
02890 {
02891     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled);
02892 }
02893 /** Get Y-axis gyroscope standby enabled status.
02894  * If enabled, the Y-axis will not gather or report data (or use power).
02895  * @return Current Y-axis standby enabled status
02896  * @see MPU6050_RA_PWR_MGMT_2
02897  * @see MPU6050_PWR2_STBY_YG_BIT
02898  */
02899 bool MPU6050::getStandbyYGyroEnabled()
02900 {
02901     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer);
02902     return buffer[0];
02903 }
02904 /** Set Y-axis gyroscope standby enabled status.
02905  * @param New Y-axis standby enabled status
02906  * @see getStandbyYGyroEnabled()
02907  * @see MPU6050_RA_PWR_MGMT_2
02908  * @see MPU6050_PWR2_STBY_YG_BIT
02909  */
02910 void MPU6050::setStandbyYGyroEnabled(bool enabled)
02911 {
02912     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled);
02913 }
02914 /** Get Z-axis gyroscope standby enabled status.
02915  * If enabled, the Z-axis will not gather or report data (or use power).
02916  * @return Current Z-axis standby enabled status
02917  * @see MPU6050_RA_PWR_MGMT_2
02918  * @see MPU6050_PWR2_STBY_ZG_BIT
02919  */
02920 bool MPU6050::getStandbyZGyroEnabled()
02921 {
02922     i2Cdev.readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer);
02923     return buffer[0];
02924 }
02925 /** Set Z-axis gyroscope standby enabled status.
02926  * @param New Z-axis standby enabled status
02927  * @see getStandbyZGyroEnabled()
02928  * @see MPU6050_RA_PWR_MGMT_2
02929  * @see MPU6050_PWR2_STBY_ZG_BIT
02930  */
02931 void MPU6050::setStandbyZGyroEnabled(bool enabled)
02932 {
02933     i2Cdev.writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
02934 }
02935 
02936 // FIFO_COUNT* registers
02937 
02938 /** Get current FIFO buffer size.
02939  * This value indicates the number of bytes stored in the FIFO buffer. This
02940  * number is in turn the number of bytes that can be read from the FIFO buffer
02941  * and it is directly proportional to the number of samples available given the
02942  * set of sensor data bound to be stored in the FIFO (register 35 and 36).
02943  * @return Current FIFO buffer size
02944  */
02945 uint16_t MPU6050::getFIFOCount()
02946 {
02947     i2Cdev.readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
02948     return (((uint16_t)buffer[0]) << 8) | buffer[1];
02949 }
02950 
02951 // FIFO_R_W register
02952 
02953 /** Get byte from FIFO buffer.
02954  * This register is used to read and write data from the FIFO buffer. Data is
02955  * written to the FIFO in order of register number (from lowest to highest). If
02956  * all the FIFO enable flags (see below) are enabled and all External Sensor
02957  * Data registers (Registers 73 to 96) are associated with a Slave device, the
02958  * contents of registers 59 through 96 will be written in order at the Sample
02959  * Rate.
02960  *
02961  * The contents of the sensor data registers (Registers 59 to 96) are written
02962  * into the FIFO buffer when their corresponding FIFO enable flags are set to 1
02963  * in FIFO_EN (Register 35). An additional flag for the sensor data registers
02964  * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36).
02965  *
02966  * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is
02967  * automatically set to 1. This bit is located in INT_STATUS (Register 58).
02968  * When the FIFO buffer has overflowed, the oldest data will be lost and new
02969  * data will be written to the FIFO.
02970  *
02971  * If the FIFO buffer is empty, reading this register will return the last byte
02972  * that was previously read from the FIFO until new data is available. The user
02973  * should check FIFO_COUNT to ensure that the FIFO buffer is not read when
02974  * empty.
02975  *
02976  * @return Byte from FIFO buffer
02977  */
02978 uint8_t MPU6050::getFIFOByte()
02979 {
02980     i2Cdev.readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer);
02981     return buffer[0];
02982 }
02983 void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length)
02984 {
02985     i2Cdev.readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
02986 }
02987 /** Write byte to FIFO buffer.
02988  * @see getFIFOByte()
02989  * @see MPU6050_RA_FIFO_R_W
02990  */
02991 void MPU6050::setFIFOByte(uint8_t data)
02992 {
02993     i2Cdev.writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
02994 }
02995 
02996 // WHO_AM_I register
02997 
02998 /** Get Device ID.
02999  * This register is used to verify the identity of the device (0b110100, 0x34).
03000  * @return Device ID (6 bits only! should be 0x34)
03001  * @see MPU6050_RA_WHO_AM_I
03002  * @see MPU6050_WHO_AM_I_BIT
03003  * @see MPU6050_WHO_AM_I_LENGTH
03004  */
03005 uint8_t MPU6050::getDeviceID()
03006 {
03007     i2Cdev.readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
03008     return buffer[0];
03009 }
03010 /** Set Device ID.
03011  * Write a new ID into the WHO_AM_I register (no idea why this should ever be
03012  * necessary though).
03013  * @param id New device ID to set.
03014  * @see getDeviceID()
03015  * @see MPU6050_RA_WHO_AM_I
03016  * @see MPU6050_WHO_AM_I_BIT
03017  * @see MPU6050_WHO_AM_I_LENGTH
03018  */
03019 void MPU6050::setDeviceID(uint8_t id)
03020 {
03021     i2Cdev.writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id);
03022 }
03023 
03024 // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
03025 
03026 // XG_OFFS_TC register
03027 
03028 uint8_t MPU6050::getOTPBankValid()
03029 {
03030     i2Cdev.readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
03031     return buffer[0];
03032 }
03033 void MPU6050::setOTPBankValid(bool enabled)
03034 {
03035     i2Cdev.writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
03036 }
03037 int8_t MPU6050::getXGyroOffset()
03038 {
03039     i2Cdev.readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
03040     return buffer[0];
03041 }
03042 void MPU6050::setXGyroOffset(int8_t offset)
03043 {
03044     i2Cdev.writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
03045 }
03046 
03047 // YG_OFFS_TC register
03048 
03049 int8_t MPU6050::getYGyroOffset()
03050 {
03051     i2Cdev.readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
03052     return buffer[0];
03053 }
03054 void MPU6050::setYGyroOffset(int8_t offset)
03055 {
03056     i2Cdev.writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
03057 }
03058 
03059 // ZG_OFFS_TC register
03060 
03061 int8_t MPU6050::getZGyroOffset()
03062 {
03063     i2Cdev.readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
03064     return buffer[0];
03065 }
03066 void MPU6050::setZGyroOffset(int8_t offset)
03067 {
03068     i2Cdev.writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
03069 }
03070 
03071 // X_FINE_GAIN register
03072 
03073 int8_t MPU6050::getXFineGain()
03074 {
03075     i2Cdev.readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer);
03076     return buffer[0];
03077 }
03078 void MPU6050::setXFineGain(int8_t gain)
03079 {
03080     i2Cdev.writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain);
03081 }
03082 
03083 // Y_FINE_GAIN register
03084 
03085 int8_t MPU6050::getYFineGain()
03086 {
03087     i2Cdev.readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer);
03088     return buffer[0];
03089 }
03090 void MPU6050::setYFineGain(int8_t gain)
03091 {
03092     i2Cdev.writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
03093 }
03094 
03095 // Z_FINE_GAIN register
03096 
03097 int8_t MPU6050::getZFineGain()
03098 {
03099     i2Cdev.readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer);
03100     return buffer[0];
03101 }
03102 void MPU6050::setZFineGain(int8_t gain)
03103 {
03104     i2Cdev.writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
03105 }
03106 
03107 // XA_OFFS_* registers
03108 
03109 int16_t MPU6050::getXAccelOffset()
03110 {
03111     i2Cdev.readBytes(devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
03112     return (((int16_t)buffer[0]) << 8) | buffer[1];
03113 }
03114 void MPU6050::setXAccelOffset(int16_t offset)
03115 {
03116     i2Cdev.writeWord(devAddr, MPU6050_RA_XA_OFFS_H, offset);
03117 }
03118 
03119 // YA_OFFS_* register
03120 
03121 int16_t MPU6050::getYAccelOffset()
03122 {
03123     i2Cdev.readBytes(devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
03124     return (((int16_t)buffer[0]) << 8) | buffer[1];
03125 }
03126 void MPU6050::setYAccelOffset(int16_t offset)
03127 {
03128     i2Cdev.writeWord(devAddr, MPU6050_RA_YA_OFFS_H, offset);
03129 }
03130 
03131 // ZA_OFFS_* register
03132 
03133 int16_t MPU6050::getZAccelOffset()
03134 {
03135     i2Cdev.readBytes(devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
03136     return (((int16_t)buffer[0]) << 8) | buffer[1];
03137 }
03138 void MPU6050::setZAccelOffset(int16_t offset)
03139 {
03140     i2Cdev.writeWord(devAddr, MPU6050_RA_ZA_OFFS_H, offset);
03141 }
03142 
03143 // XG_OFFS_USR* registers
03144 
03145 int16_t MPU6050::getXGyroOffsetUser()
03146 {
03147     i2Cdev.readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer);
03148     return (((int16_t)buffer[0]) << 8) | buffer[1];
03149 }
03150 void MPU6050::setXGyroOffsetUser(int16_t offset)
03151 {
03152     i2Cdev.writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
03153 }
03154 
03155 // YG_OFFS_USR* register
03156 
03157 int16_t MPU6050::getYGyroOffsetUser()
03158 {
03159     i2Cdev.readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
03160     return (((int16_t)buffer[0]) << 8) | buffer[1];
03161 }
03162 void MPU6050::setYGyroOffsetUser(int16_t offset)
03163 {
03164     i2Cdev.writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
03165 }
03166 
03167 // ZG_OFFS_USR* register
03168 
03169 int16_t MPU6050::getZGyroOffsetUser()
03170 {
03171     i2Cdev.readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
03172     return (((int16_t)buffer[0]) << 8) | buffer[1];
03173 }
03174 void MPU6050::setZGyroOffsetUser(int16_t offset)
03175 {
03176     i2Cdev.writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
03177 }
03178 
03179 // INT_ENABLE register (DMP functions)
03180 
03181 bool MPU6050::getIntPLLReadyEnabled()
03182 {
03183     i2Cdev.readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
03184     return buffer[0];
03185 }
03186 void MPU6050::setIntPLLReadyEnabled(bool enabled)
03187 {
03188     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
03189 }
03190 bool MPU6050::getIntDMPEnabled()
03191 {
03192     i2Cdev.readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
03193     return buffer[0];
03194 }
03195 void MPU6050::setIntDMPEnabled(bool enabled)
03196 {
03197     i2Cdev.writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
03198 }
03199 
03200 // DMP_INT_STATUS
03201 
03202 bool MPU6050::getDMPInt5Status()
03203 {
03204     i2Cdev.readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer);
03205     return buffer[0];
03206 }
03207 bool MPU6050::getDMPInt4Status()
03208 {
03209     i2Cdev.readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer);
03210     return buffer[0];
03211 }
03212 bool MPU6050::getDMPInt3Status()
03213 {
03214     i2Cdev.readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer);
03215     return buffer[0];
03216 }
03217 bool MPU6050::getDMPInt2Status()
03218 {
03219     i2Cdev.readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer);
03220     return buffer[0];
03221 }
03222 bool MPU6050::getDMPInt1Status()
03223 {
03224     i2Cdev.readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer);
03225     return buffer[0];
03226 }
03227 bool MPU6050::getDMPInt0Status()
03228 {
03229     i2Cdev.readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer);
03230     return buffer[0];
03231 }
03232 
03233 // INT_STATUS register (DMP functions)
03234 
03235 bool MPU6050::getIntPLLReadyStatus()
03236 {
03237     i2Cdev.readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
03238     return buffer[0];
03239 }
03240 bool MPU6050::getIntDMPStatus()
03241 {
03242     i2Cdev.readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
03243     return buffer[0];
03244 }
03245 
03246 // USER_CTRL register (DMP functions)
03247 
03248 bool MPU6050::getDMPEnabled()
03249 {
03250     i2Cdev.readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer);
03251     return buffer[0];
03252 }
03253 void MPU6050::setDMPEnabled(bool enabled)
03254 {
03255     i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
03256 }
03257 void MPU6050::resetDMP()
03258 {
03259     i2Cdev.writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
03260 }
03261 
03262 // BANK_SEL register
03263 
03264 void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank)
03265 {
03266     bank &= 0x1F;
03267     if (userBank) bank |= 0x20;
03268     if (prefetchEnabled) bank |= 0x40;
03269     i2Cdev.writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
03270 }
03271 
03272 // MEM_START_ADDR register
03273 
03274 void MPU6050::setMemoryStartAddress(uint8_t address)
03275 {
03276     i2Cdev.writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
03277 }
03278 
03279 // MEM_R_W register
03280 
03281 uint8_t MPU6050::readMemoryByte()
03282 {
03283     i2Cdev.readByte(devAddr, MPU6050_RA_MEM_R_W, buffer);
03284     return buffer[0];
03285 }
03286 void MPU6050::writeMemoryByte(uint8_t data)
03287 {
03288     i2Cdev.writeByte(devAddr, MPU6050_RA_MEM_R_W, data);
03289 }
03290 void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address)
03291 {
03292     setMemoryBank(bank);
03293     setMemoryStartAddress(address);
03294     uint8_t chunkSize;
03295     for (uint16_t i = 0; i < dataSize;) {
03296         // determine correct chunk size according to bank position and data size
03297         chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
03298 
03299         // make sure we don't go past the data size
03300         if (i + chunkSize > dataSize) chunkSize = dataSize - i;
03301 
03302         // make sure this chunk doesn't go past the bank boundary (256 bytes)
03303         if (chunkSize > 256 - address) chunkSize = 256 - address;
03304 
03305         // read the chunk of data as specified
03306         i2Cdev.readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
03307 
03308         // increase byte index by [chunkSize]
03309         i += chunkSize;
03310 
03311         // uint8_t automatically wraps to 0 at 256
03312         address += chunkSize;
03313 
03314         // if we aren't done, update bank (if necessary) and address
03315         if (i < dataSize) {
03316             if (address == 0) bank++;
03317             setMemoryBank(bank);
03318             setMemoryStartAddress(address);
03319         }
03320     }
03321 }
03322 bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem)
03323 {
03324     setMemoryBank(bank);
03325     setMemoryStartAddress(address);
03326     uint8_t chunkSize;
03327     uint8_t *verifyBuffer;
03328     uint8_t *progBuffer;
03329     uint16_t i;
03330     uint8_t j;
03331     if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
03332     if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
03333     for (i = 0; i < dataSize;) {
03334         // determine correct chunk size according to bank position and data size
03335         chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
03336 
03337         // make sure we don't go past the data size
03338         if (i + chunkSize > dataSize) chunkSize = dataSize - i;
03339 
03340         // make sure this chunk doesn't go past the bank boundary (256 bytes)
03341         if (chunkSize > 256 - address) chunkSize = 256 - address;
03342 
03343         if (useProgMem) {
03344             // write the chunk of data as specified
03345             for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
03346         } else {
03347             // write the chunk of data as specified
03348             progBuffer = (uint8_t *)data + i;
03349         }
03350 
03351         i2Cdev.writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
03352 
03353         // verify data if needed
03354         if (verify && verifyBuffer) {
03355             setMemoryBank(bank);
03356             setMemoryStartAddress(address);
03357             i2Cdev.readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
03358             if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
03359                 /*Serial.print("Block write verification error, bank ");
03360                 Serial.print(bank, DEC);
03361                 Serial.print(", address ");
03362                 Serial.print(address, DEC);
03363                 Serial.print("!\nExpected:");
03364                 for (j = 0; j < chunkSize; j++) {
03365                     Serial.print(" 0x");
03366                     if (progBuffer[j] < 16) Serial.print("0");
03367                     Serial.print(progBuffer[j], HEX);
03368                 }
03369                 Serial.print("\nReceived:");
03370                 for (uint8_t j = 0; j < chunkSize; j++) {
03371                     Serial.print(" 0x");
03372                     if (verifyBuffer[i + j] < 16) Serial.print("0");
03373                     Serial.print(verifyBuffer[i + j], HEX);
03374                 }
03375                 Serial.print("\n");*/
03376                 free(verifyBuffer);
03377                 if (useProgMem) free(progBuffer);
03378                 return false; // uh oh.
03379             }
03380         }
03381 
03382         // increase byte index by [chunkSize]
03383         i += chunkSize;
03384 
03385         // uint8_t automatically wraps to 0 at 256
03386         address += chunkSize;
03387 
03388         // if we aren't done, update bank (if necessary) and address
03389         if (i < dataSize) {
03390             if (address == 0) bank++;
03391             setMemoryBank(bank);
03392             setMemoryStartAddress(address);
03393         }
03394     }
03395     if (verify) free(verifyBuffer);
03396     if (useProgMem) free(progBuffer);
03397     return true;
03398 }
03399 bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify)
03400 {
03401     return writeMemoryBlock(data, dataSize, bank, address, verify, true);
03402 }
03403 bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem)
03404 {
03405     uint8_t *progBuffer, success, special;
03406     uint16_t i, j;
03407     if (useProgMem) {
03408         progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary
03409     }
03410 
03411     // config set data is a long string of blocks with the following structure:
03412     // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
03413     uint8_t bank, offset, length;
03414     for (i = 0; i < dataSize;) {
03415         if (useProgMem) {
03416             bank = pgm_read_byte(data + i++);
03417             offset = pgm_read_byte(data + i++);
03418             length = pgm_read_byte(data + i++);
03419         } else {
03420             bank = data[i++];
03421             offset = data[i++];
03422             length = data[i++];
03423         }
03424 
03425         // write data or perform special action
03426         if (length > 0) {
03427             // regular block of data to write
03428             /*Serial.print("Writing config block to bank ");
03429             Serial.print(bank);
03430             Serial.print(", offset ");
03431             Serial.print(offset);
03432             Serial.print(", length=");
03433             Serial.println(length);*/
03434             if (useProgMem) {
03435                 if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length);
03436                 for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j);
03437             } else {
03438                 progBuffer = (uint8_t *)data + i;
03439             }
03440             success = writeMemoryBlock(progBuffer, length, bank, offset, true);
03441             i += length;
03442         } else {
03443             // special instruction
03444             // NOTE: this kind of behavior (what and when to do certain things)
03445             // is totally undocumented. This code is in here based on observed
03446             // behavior only, and exactly why (or even whether) it has to be here
03447             // is anybody's guess for now.
03448             if (useProgMem) {
03449                 special = pgm_read_byte(data + i++);
03450             } else {
03451                 special = data[i++];
03452             }
03453             /*Serial.print("Special command code ");
03454             Serial.print(special, HEX);
03455             Serial.println(" found...");*/
03456             if (special == 0x01) {
03457                 // enable DMP-related interrupts
03458 
03459                 //setIntZeroMotionEnabled(true);
03460                 //setIntFIFOBufferOverflowEnabled(true);
03461                 //setIntDMPEnabled(true);
03462                 i2Cdev.writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32);  // single operation
03463 
03464                 success = true;
03465             } else {
03466                 // unknown special command
03467                 success = false;
03468             }
03469         }
03470 
03471         if (!success) {
03472             if (useProgMem) free(progBuffer);
03473             return false; // uh oh
03474         }
03475     }
03476     if (useProgMem) free(progBuffer);
03477     return true;
03478 }
03479 bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize)
03480 {
03481     return writeDMPConfigurationSet(data, dataSize, false);
03482 }
03483 
03484 // DMP_CFG_1 register
03485 
03486 uint8_t MPU6050::getDMPConfig1()
03487 {
03488     i2Cdev.readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer);
03489     return buffer[0];
03490 }
03491 void MPU6050::setDMPConfig1(uint8_t config)
03492 {
03493     i2Cdev.writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
03494 }
03495 
03496 // DMP_CFG_2 register
03497 
03498 uint8_t MPU6050::getDMPConfig2()
03499 {
03500     i2Cdev.readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer);
03501     return buffer[0];
03502 }
03503 void MPU6050::setDMPConfig2(uint8_t config)
03504 {
03505     i2Cdev.writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
03506 }
03507 
03508 // =============================================================================
03509 // =============================================================================
03510 // =============================================================================
03511 // =============================================================================
03512 // =============================================================================
03513 // =============================================================================
03514 // ============================== MPU 9250 parts ===============================
03515 // =============================================================================
03516 // =============================================================================
03517 // =============================================================================
03518 // =============================================================================
03519 // =============================================================================
03520 
03521 #ifdef MPU9250
03522 
03523 /** initialize 9250.
03524  * 
03525  */
03526 void MPU6050::initialize9250() {
03527     reset();
03528     Thread::wait(50);
03529     setStandbyDisable();
03530     setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
03531     setClockSource(MPU60X0_CLOCK_PLL_XGYRO);
03532     setFullScaleGyroRange(MPU60X0_GYRO_FS_2000);
03533     setFullScaleAccelRange(MPU60X0_ACCEL_FS_2);
03534 
03535     //data = 1000 / rate - 1;
03536     //setRate(data);    
03537 }
03538 
03539 /** Disable MPU-6050 standby mode. Really only used for MPU-9250
03540  * @param 
03541  * @see 
03542  * @see MPU60X0_RA_PWR_MGMT_2
03543  * @see 
03544  */
03545 void MPU6050::setStandbyDisable() {
03546     i2Cdev.writeByte(devAddr, MPU60X0_RA_PWR_MGMT_2, 0x00);
03547 }
03548 
03549 void MPU6050::initialize9250MasterMode() {
03550     
03551     uint8_t buff[3];
03552     uint8_t data[7];
03553     float _magScaleX, _magScaleY, _magScaleZ;   
03554     
03555     //set dev address for magnetometer
03556     magDevAddr = AK8963_DEFAULT_ADDRESS;
03557     
03558     Thread::wait(50);
03559     reset();
03560 
03561     setStandbyDisable();
03562     setSleepEnabled(false);
03563     
03564     // select clock source to gyro
03565     if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
03566         debugSerial.printf("Clock Source Not Set\n");
03567     }
03568 
03569     // enable I2C master mode
03570     if( !writeRegister(MPU60X0_RA_USER_CTRL,I2C_MST_EN) ){
03571         debugSerial.printf("Master Mode Not Set\n");
03572     }
03573 
03574     // set the I2C bus speed to 400 kHz
03575     if( !writeRegister(MPU60X0_RA_I2C_MST_CTRL,I2C_MST_CLK) ){
03576         debugSerial.printf("I2C Bus Speed Not Set\n");
03577     }
03578 
03579     // set AK8963 to Power Down
03580     if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
03581         debugSerial.printf("AK Not Powered Down\n");
03582     }
03583 
03584     // reset the MPU9250
03585     writeRegister(MPU60X0_RA_PWR_MGMT_1, PWR_RESET);
03586 
03587     // wait for MPU-9250 to come back up
03588     Thread::wait(2);
03589 
03590     // reset the AK8963
03591     writeAKRegister(AK8963_RA_CNTL2, PWR_RESET);
03592 
03593     // select clock source to gyro
03594     if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
03595         debugSerial.printf("Clock Source Not Set\n");
03596     }
03597 
03598     // check the WHO AM I byte, expected value is 0x71 (decimal 113)
03599     readRegister(MPU60X0_RA_WHO_AM_I, 1,&buff[0]);
03600     if( buff[0] != 113 ){
03601         debugSerial.printf("9250 Not Recognized\n");
03602     }
03603 
03604     // enable accelerometer and gyro
03605     if( !writeRegister(MPU60X0_RA_PWR_MGMT_2, SEN_ENABLE) ){
03606         debugSerial.printf("Accel and Gyro not Enabled\n");
03607     }
03608 
03609     /* setup the accel and gyro ranges */
03610     setFullScaleGyroRange(MPU60X0_GYRO_FS_2000);    // set gyro range to +/- 250 deg/second
03611     setFullScaleAccelRange(MPU60X0_ACCEL_FS_2);     // set accel range to +- 2g
03612     //setFilt9250(DLPF_BANDWIDTH_92HZ, 4); 
03613     
03614     // enable I2C master mode
03615     if( !writeRegister(MPU60X0_RA_USER_CTRL,I2C_MST_EN) ){
03616         debugSerial.printf("Master Mode Set\n");
03617     }
03618 
03619     // set the I2C bus speed to 400 kHz
03620     if( !writeRegister(MPU60X0_RA_I2C_MST_CTRL,MPU60X0_CLOCK_PLL_XGYRO) ){
03621         debugSerial.printf("I2C Bus Set\n");
03622     }
03623 
03624     // check AK8963 WHO AM I register, expected value is 0x48 (decimal 72)
03625     readAKRegisters(AK8963_RA_WIA, sizeof(buff), &buff[0]);
03626     if(  buff[0] != 72 ){
03627         debugSerial.printf("%d", buff[0]);
03628         debugSerial.printf(", ");
03629         debugSerial.printf("AK does not match\n");
03630     }
03631 
03632     /* get the magnetometer calibration */
03633 
03634     // set AK8963 to Power Down
03635     if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
03636         debugSerial.printf("AK Not Powered Down\n");
03637     }
03638     Thread::wait(100); // long wait between AK8963 mode changes
03639 
03640     // set AK8963 to FUSE ROM access
03641     if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_FUSEROM)){
03642         debugSerial.printf("FUSE ROM Access Not set\n");
03643     }
03644 
03645     Thread::wait(100); // long wait between AK8963 mode changes
03646     
03647     // read the AK8963 ASA registers and compute magnetometer scale factors
03648     readAKRegisters(AK8963_RA_ASAX, sizeof(buff), &buff[0]);
03649     //_magScaleX = ((((float)buff[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
03650     //_magScaleY = ((((float)buff[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
03651     //_magScaleZ = ((((float)buff[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 
03652     _magScaleX = buff[0];
03653     _magScaleY = buff[1];
03654     _magScaleZ = buff[2];
03655     //Serial.print(_magScaleX); Serial.print(", "); Serial.print(_magScaleY); 
03656     //Serial.print(", "); Serial.println(_magScaleZ);
03657     
03658     // set AK8963 to Power Down
03659     if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
03660         debugSerial.printf("AK Not Powered Down\n");
03661     }
03662     Thread::wait(100); // long wait between AK8963 mode changes  
03663 
03664     // set AK8963 to 16 bit resolution, 100 Hz update rate
03665     if( !writeAKRegister(AK8963_RA_CNTL1, 0x16) ){
03666         debugSerial.printf("Res Not Set\n");
03667     }
03668     Thread::wait(100); // long wait between AK8963 mode changes
03669 
03670     // select clock source to gyro
03671     if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
03672         debugSerial.printf("Clock Source Not Set\n");
03673     }      
03674 
03675     // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
03676     readAKRegisters(AK8963_RA_HXL,sizeof(data),&data[0]);
03677     //Serial.println((((int16_t)data[1]) << 8) | data[0]);  
03678     //Serial.println((((int16_t)data[3]) << 8) | data[2]);
03679     //Serial.println((((int16_t)data[5]) << 8) | data[4]);
03680 
03681     // successful init, return 0
03682     // debugSerial.printf("FINISHED\n");
03683 }
03684 
03685 
03686 /* sets the DLPF and interrupt settings */
03687 int MPU6050::setFilt9250(mpu9250_dlpf_bandwidth bandwidth, uint8_t SRD){
03688     uint8_t data[7];
03689 
03690     switch(bandwidth) {
03691         case DLPF_BANDWIDTH_184HZ:
03692             if( !writeRegister(MPU9250_RA_ACCEL_CONFIG2, MPU60X0_DLPF_BW_184) ){ // setting accel bandwidth to 184Hz
03693                 return -1;
03694             } 
03695             if( !writeRegister(MPU60X0_RA_GYRO_CONFIG, MPU60X0_DLPF_BW_184) ){ // setting gyro bandwidth to 184Hz
03696                 return -1;
03697             }
03698             break;
03699 
03700         case DLPF_BANDWIDTH_92HZ:
03701             if( !writeRegister(MPU9250_RA_ACCEL_CONFIG2, MPU60X0_DLPF_BW_98) ){ // setting accel bandwidth to 92Hz
03702                 return -1;
03703             } 
03704             if( !writeRegister(MPU60X0_RA_GYRO_CONFIG, MPU60X0_DLPF_BW_98) ){ // setting gyro bandwidth to 92Hz
03705                 return -1;
03706             }
03707             break; 
03708 
03709         case DLPF_BANDWIDTH_41HZ:
03710             if( !writeRegister(MPU9250_RA_ACCEL_CONFIG2, MPU60X0_DLPF_BW_42) ){ // setting accel bandwidth to 41Hz
03711                 return -1;
03712             } 
03713             if( !writeRegister(MPU60X0_RA_GYRO_CONFIG, MPU60X0_DLPF_BW_42) ){ // setting gyro bandwidth to 41Hz
03714                 return -1;
03715             } 
03716             break;
03717 
03718         case DLPF_BANDWIDTH_20HZ:
03719             if( !writeRegister(MPU9250_RA_ACCEL_CONFIG2, MPU60X0_DLPF_BW_20) ){ // setting accel bandwidth to 20Hz
03720                 return -1;
03721             } 
03722             if( !writeRegister(MPU60X0_RA_GYRO_CONFIG, MPU60X0_DLPF_BW_20) ){ // setting gyro bandwidth to 20Hz
03723                 return -1;
03724             }
03725             break;
03726 
03727         case DLPF_BANDWIDTH_10HZ:
03728             if( !writeRegister(MPU9250_RA_ACCEL_CONFIG2,MPU60X0_DLPF_BW_10) ){ // setting accel bandwidth to 10Hz
03729                 return -1;
03730             } 
03731             if( !writeRegister(MPU60X0_RA_GYRO_CONFIG,MPU60X0_DLPF_BW_10) ){ // setting gyro bandwidth to 10Hz
03732                 return -1;
03733             }
03734             break;
03735 
03736         case DLPF_BANDWIDTH_5HZ:
03737             if( !writeRegister(MPU9250_RA_ACCEL_CONFIG2,MPU60X0_DLPF_BW_5) ){ // setting accel bandwidth to 5Hz
03738                 return -1;
03739             } 
03740             if( !writeRegister(MPU60X0_RA_GYRO_CONFIG,MPU60X0_DLPF_BW_5) ){ // setting gyro bandwidth to 5Hz
03741                 return -1;
03742             }
03743             break; 
03744     }
03745 
03746     /* setting the sample rate divider */
03747     if( !writeRegister(MPU60X0_RA_SMPLRT_DIV,SRD) ){ // setting the sample rate divider
03748         return -1;
03749     } 
03750 
03751     if(SRD > 9){
03752 
03753         // set AK8963 to Power Down
03754         if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
03755             return -1;
03756         }
03757         Thread::wait(100); // long wait between AK8963 mode changes  
03758 
03759         // set AK8963 to 16 bit resolution, 8 Hz update rate
03760         if( !writeAKRegister(AK8963_RA_CNTL1, 0x12) ){
03761             return -1;
03762         }
03763         Thread::wait(100); // long wait between AK8963 mode changes     
03764 
03765         // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
03766         readAKRegisters(AK8963_RA_HXL,sizeof(data),&data[0]);
03767     }
03768 
03769     /* setting the interrupt */
03770     if( !writeRegister(MPU60X0_RA_INT_PIN_CFG,INT_PULSE_50US) ){ // setup interrupt, 50 us pulse
03771         return -1;
03772     }  
03773     if( !writeRegister(MPU60X0_RA_INT_ENABLE,INT_RAW_RDY_EN) ){ // set to data ready
03774         return -1;
03775     }  
03776 
03777     // successful filter setup, return 0
03778     return 0; 
03779 }
03780 
03781 /* enables and disables the interrupt */
03782 int MPU6050::enableInt9250(bool enable){
03783 
03784     if(enable){
03785         /* setting the interrupt */
03786         if( !writeRegister(MPU60X0_RA_INT_PIN_CFG,INT_PULSE_50US) ){ // setup interrupt, 50 us pulse
03787             return -1;
03788         }  
03789         if( !writeRegister(MPU60X0_RA_INT_ENABLE,INT_RAW_RDY_EN) ){ // set to data ready
03790             return -1;
03791         }  
03792     }
03793     else{
03794         if( !writeRegister(MPU60X0_RA_INT_ENABLE,INT_DISABLE) ){ // disable interrupt
03795             return -1;
03796         }  
03797     }
03798 
03799     // successful interrupt setup, return 0
03800     return 0; 
03801 }
03802 
03803 
03804 
03805 
03806 /* get accelerometer data given pointers to store the three values, return data as counts */
03807 void MPU6050::get9250AccelCounts(int16_t* ax, int16_t* ay, int16_t* az){
03808     uint8_t buff[6];
03809     int16_t axx, ayy, azz;
03810 
03811     readRegister(MPU60X0_RA_ACCEL_XOUT_H, sizeof(buff), &buff[0]); // grab the data from the MPU9250
03812 
03813     axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
03814     ayy = (((int16_t)buff[2]) << 8) | buff[3];
03815     azz = (((int16_t)buff[4]) << 8) | buff[5];
03816 
03817     *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
03818     *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
03819     *az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;
03820 }
03821 
03822 
03823 /* get gyro data given pointers to store the three values, return data as counts */
03824 void MPU6050::get9250GyroCounts(int16_t* gx, int16_t* gy, int16_t* gz){
03825     uint8_t buff[6];
03826     int16_t gxx, gyy, gzz;
03827 
03828     readRegister(MPU60X0_RA_GYRO_XOUT_H, sizeof(buff), &buff[0]); // grab the data from the MPU9250
03829 
03830     gxx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
03831     gyy = (((int16_t)buff[2]) << 8) | buff[3];
03832     gzz = (((int16_t)buff[4]) << 8) | buff[5];
03833 
03834     *gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz; // transform axes
03835     *gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
03836     *gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
03837 }
03838 
03839 /* get magnetometer data given pointers to store the three values, return data as counts */
03840 void MPU6050::get9250MagCounts(int16_t* hx, int16_t* hy, int16_t* hz){
03841     uint8_t buff[7];
03842     // read the magnetometer data off the external sensor buffer
03843     readRegister(MPU60X0_RA_EXT_SENS_DATA_00,sizeof(buff),&buff[0]);
03844 
03845     if( buff[6] == 0x10 ) { // check for overflow
03846         *hx = (((int16_t)buff[1]) << 8) | buff[0];  // combine into 16 bit values
03847         *hy = (((int16_t)buff[3]) << 8) | buff[2];
03848         *hz = (((int16_t)buff[5]) << 8) | buff[4];
03849     }
03850     else{
03851         *hx = 0;  
03852         *hy = 0;
03853         *hz = 0;
03854     }
03855 }
03856 
03857 /* get temperature data given pointer to store the value, return data as counts */
03858 void MPU6050::get9250TempCounts(int16_t* t){
03859     uint8_t buff[2];
03860 
03861     readRegister(MPU60X0_RA_TEMP_OUT_H, sizeof(buff), &buff[0]); // grab the data from the MPU9250
03862 
03863     *t = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit value and return
03864 }
03865 
03866 void MPU6050::get9250Motion9Counts(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* hx, int16_t* hy, int16_t* hz)
03867 {
03868     int16_t axx, ayy, azz, gxx, gyy, gzz;
03869 
03870     axx = ax_cache;
03871     ayy = ay_cache;
03872     azz = az_cache;
03873     gxx = gx_cache;
03874     gyy = gy_cache;
03875     gzz = gz_cache;
03876     *hx = hx_cache;
03877     *hy = hy_cache;
03878     *hz = hz_cache;
03879 
03880     *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
03881     *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
03882     *az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;
03883 
03884     *gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz;
03885     *gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
03886     *gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
03887 }
03888 
03889 /* get accelerometer, gyro, and magnetometer data given pointers to store values */
03890 void MPU6050::get9250Motion9(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz){
03891     int16_t accel[3];
03892     int16_t gyro[3];
03893     int16_t mag[3];
03894 
03895     get9250Motion9Counts(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2], &mag[0], &mag[1], &mag[2]);
03896 
03897     *ax = ((float) accel[0]); // typecast and scale to values
03898     *ay = ((float) accel[1]);
03899     *az = ((float) accel[2]);
03900 
03901     *gx = ((float) gyro[0]);
03902     *gy = ((float) gyro[1]);
03903     *gz = ((float) gyro[2]);
03904 
03905     *hx = ((float) mag[0]);
03906     *hy = ((float) mag[1]);
03907     *hz = ((float) mag[2]);
03908 
03909 }
03910 
03911 void MPU6050::readRegister(uint8_t subAddress, uint8_t count, uint8_t* dest) {
03912     i2Cdev.readBytes(devAddr, subAddress, count, dest);
03913 }
03914 
03915 /* writes a register to the AK8963 given a register address and data */
03916 bool MPU6050::writeAKRegister(uint8_t subAddress, uint8_t data) {
03917     uint8_t count = 1;
03918     uint8_t buff[1];
03919     
03920     writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, magDevAddr); // set slave 0 to the AK8963 and set for write
03921     writeRegister(MPU60X0_RA_I2C_SLV0_REG,subAddress); // set the register to the desired AK8963 sub address
03922     writeRegister(MPU60X0_RA_I2C_SLV0_DO, data); // store the data for write
03923     writeRegister(MPU60X0_RA_I2C_SLV0_CTRL, I2C_SLV0_EN | count); // enable I2C and send 1 byte
03924 
03925     // read the register and confirm
03926     readAKRegisters(subAddress, sizeof(buff), &buff[0]);
03927 
03928     if (buff[0] == data) {
03929         return true;
03930     } else {
03931         return false;
03932     }
03933 }
03934 
03935 
03936 /* reads registers from the AK8963 */
03937 void MPU6050::readAKRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest) {
03938     
03939     writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, magDevAddr | I2C_READ_FLAG); // set slave 0 to the AK8963 and set for read
03940     writeRegister(MPU60X0_RA_I2C_SLV0_REG,  subAddress); // set the register to the desired AK8963 sub address
03941     writeRegister(MPU60X0_RA_I2C_SLV0_CTRL, I2C_SLV0_EN | count); // enable I2C and request the bytes
03942     Thread::wait(1); // takes some time for these registers to fill
03943     readRegister(MPU60X0_RA_EXT_SENS_DATA_00, count, dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers
03944 
03945 }
03946 
03947 bool MPU6050::writeRegister(uint8_t subAddress, uint8_t data) {
03948     uint8_t buff[1];
03949 
03950     i2Cdev.writeByte(devAddr, subAddress, data);
03951    
03952     Thread::wait(10); // need to slow down how fast I write to MPU9250
03953     
03954     /* read back the register */
03955     readRegister(subAddress,sizeof(buff),&buff[0]);
03956 
03957     /* check the read back register against the written register */
03958     if (buff[0] == data) {
03959         return true;
03960     } else {
03961         return false;
03962     }
03963     
03964 }
03965 
03966 #endif // MPU9250
03967 
03968 
03969 
03970