Chris Pepper / MPU9150_DMP

The MPU9150s biggest selling point was its internal Motion Processor to offload the sensor fusion from the host processor, this library uploads the firmware to generate quaternions from the sensors. The API needs work but as other libraries don't support the DMP supplying as is.

Dependents:   MPU9150_Example CANSAT_COMBINED MPU9150_Example mbed_rifletool

MPU9150.cpp@2:e523a92390b6, 2014-09-01 (annotated)

Committer:
p3p
Date:
Mon Sep 01 14:25:06 2014 +0000
Revision:
2:e523a92390b6
Parent:
1:8ff0beb54dd4 
Fix Copyright

Who changed what in which revision?

UserRevisionLine numberNew contents of line
p3p 0:74f0ae286b03 1 #include "MPU9150.h"
p3p 0:74f0ae286b03 2
p3p 2:e523a92390b6 3 // Copyright (c) 2014 Chris Pepper
p3p 2:e523a92390b6 4
p3p 2:e523a92390b6 5 //The Firmware upload code was borrowed/modified from the sparkfun repository, which included this copyright
p3p 2:e523a92390b6 6 /* ============================================
p3p 2:e523a92390b6 7 I2Cdev device library code is placed under the MIT license
p3p 2:e523a92390b6 8 Copyright (c) 2012 Jeff Rowberg
p3p 2:e523a92390b6 9
p3p 2:e523a92390b6 10 Permission is hereby granted, free of charge, to any person obtaining a copy
p3p 2:e523a92390b6 11 of this software and associated documentation files (the "Software"), to deal
p3p 2:e523a92390b6 12 in the Software without restriction, including without limitation the rights
p3p 2:e523a92390b6 13 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
p3p 2:e523a92390b6 14 copies of the Software, and to permit persons to whom the Software is
p3p 2:e523a92390b6 15 furnished to do so, subject to the following conditions:
p3p 2:e523a92390b6 16
p3p 2:e523a92390b6 17 The above copyright notice and this permission notice shall be included in
p3p 2:e523a92390b6 18 all copies or substantial portions of the Software.
p3p 2:e523a92390b6 19
p3p 2:e523a92390b6 20 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
p3p 2:e523a92390b6 21 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
p3p 2:e523a92390b6 22 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
p3p 2:e523a92390b6 23 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
p3p 2:e523a92390b6 24 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
p3p 2:e523a92390b6 25 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
p3p 2:e523a92390b6 26 THE SOFTWARE.
p3p 2:e523a92390b6 27 ===============================================
p3p 2:e523a92390b6 28 */
p3p 2:e523a92390b6 29
p3p 2:e523a92390b6 30
p3p 0:74f0ae286b03 31 uint8_t MPU9150::getDeviceID(){
p3p 0:74f0ae286b03 32 uint8_t ret = 0;
p3p 0:74f0ae286b03 33 readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &ret);
p3p 0:74f0ae286b03 34 return ret;
p3p 0:74f0ae286b03 35 }
p3p 0:74f0ae286b03 36
p3p 0:74f0ae286b03 37 bool MPU9150::isReady(){
p3p 0:74f0ae286b03 38 return (getDeviceID() == (device_address >> 1));
p3p 0:74f0ae286b03 39 }
p3p 0:74f0ae286b03 40
p3p 0:74f0ae286b03 41 void MPU9150::initialise(){
p3p 0:74f0ae286b03 42 reset();
p3p 0:74f0ae286b03 43 wait_ms(20);//wait for reset
p3p 0:74f0ae286b03 44
p3p 0:74f0ae286b03 45 sleep(false);
p3p 0:74f0ae286b03 46 clockSelect(MPU6050_CLOCK_PLL_XGYRO); //use the gyro clock as its more reliable
p3p 0:74f0ae286b03 47 setGyroFullScaleRange(MPU6050_GYRO_FS_250);
p3p 0:74f0ae286b03 48 setAccelFullScaleRange(MPU6050_ACCEL_FS_2);
p3p 0:74f0ae286b03 49 setStandbyAccX(true);
p3p 0:74f0ae286b03 50 setI2CMasterClock(MPU6050_CLOCK_DIV_400);
p3p 0:74f0ae286b03 51 setDigitalLowPassFilter(MPU6050_DLPF_BW_42);
p3p 0:74f0ae286b03 52 setSampleRateDivider(4);
p3p 0:74f0ae286b03 53
p3p 0:74f0ae286b03 54 initialiseMagnetometer();
p3p 0:74f0ae286b03 55
p3p 0:74f0ae286b03 56 setFifoReset(true);
p3p 0:74f0ae286b03 57
p3p 0:74f0ae286b03 58 setTemperatureFifo(true);
p3p 0:74f0ae286b03 59 setAccelFifo(true);
p3p 0:74f0ae286b03 60 setGyroFifo(true);
p3p 0:74f0ae286b03 61 setSlave0Fifo(true);
p3p 0:74f0ae286b03 62
p3p 0:74f0ae286b03 63 setInterruptDataReadyEnable(true);
p3p 0:74f0ae286b03 64 setEnableFifo(true);
p3p 0:74f0ae286b03 65 }
p3p 0:74f0ae286b03 66
p3p 0:74f0ae286b03 67 void MPU9150::initialiseMagnetometer(){
p3p 0:74f0ae286b03 68 //set up slave 0 to read the magnetometor data
p3p 0:74f0ae286b03 69 setWaitForExternalSensor(true);
p3p 0:74f0ae286b03 70 //read data
p3p 0:74f0ae286b03 71 setI2cSlaveRW(0, true);
p3p 0:74f0ae286b03 72 setI2cSlaveAddress(0, 0x0C);
p3p 0:74f0ae286b03 73 setI2cSlaveRegister(0, 3);
p3p 0:74f0ae286b03 74 setI2cSlaveEnable(0, true);
p3p 0:74f0ae286b03 75 setI2cSlaveTransactionLength(0, 6);
p3p 0:74f0ae286b03 76
p3p 0:74f0ae286b03 77
p3p 0:74f0ae286b03 78 //set up slave 1 to request a new magnetometor reading by writing 0x01 to 0xA
p3p 0:74f0ae286b03 79 setI2cSlaveAddress(1, 0x0C);
p3p 0:74f0ae286b03 80 setI2cSlaveRegister(1, 0x0A);
p3p 0:74f0ae286b03 81 setI2cSlaveTransactionLength(1, 1);
p3p 0:74f0ae286b03 82 setI2cSlaveEnable(1, true);
p3p 0:74f0ae286b03 83 setI2cSlaveDataOut(1, 1);
p3p 0:74f0ae286b03 84
p3p 0:74f0ae286b03 85 //configure update rates
p3p 0:74f0ae286b03 86 setI2cMasterDelay(4);
p3p 0:74f0ae286b03 87 setI2cSlaveDelay(0, true);
p3p 0:74f0ae286b03 88 setI2cSlaveDelay(1, true);
p3p 0:74f0ae286b03 89
p3p 0:74f0ae286b03 90 //Enable the aux i2c bus with MPU9150 as master
p3p 0:74f0ae286b03 91 setI2cMasterEnable(true);
p3p 0:74f0ae286b03 92 }
p3p 0:74f0ae286b03 93
p3p 0:74f0ae286b03 94 void MPU9150::initialiseDMP(){
p3p 0:74f0ae286b03 95 reset();
p3p 0:74f0ae286b03 96 wait_ms(20);
p3p 0:74f0ae286b03 97 sleep(false);
p3p 0:74f0ae286b03 98
p3p 0:74f0ae286b03 99 setMemoryBank(0x10, true, true);
p3p 0:74f0ae286b03 100 setMemoryStartAddress(0x06);
p3p 1:8ff0beb54dd4 101 // debug.printf("Hardware Version: %d\r\n", readMemoryByte());
p3p 0:74f0ae286b03 102
p3p 0:74f0ae286b03 103 setMemoryBank(0);
p3p 0:74f0ae286b03 104 // check OTP bank valid
p3p 0:74f0ae286b03 105 uint8_t otpValid = getOTPBankValid();
p3p 1:8ff0beb54dd4 106 // debug.printf("optValid: %d\r\n", otpValid);
p3p 0:74f0ae286b03 107
p3p 0:74f0ae286b03 108 //Enabling interrupt latch, clear on any read, AUX bypass enabled
p3p 0:74f0ae286b03 109 write(MPU6050_RA_INT_PIN_CFG, 0x32);
p3p 0:74f0ae286b03 110
p3p 0:74f0ae286b03 111 if (writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0 ,0, true)) {
p3p 1:8ff0beb54dd4 112 // debug.printf("Success! DMP code written and verified.\r\n");
p3p 0:74f0ae286b03 113 if (writeDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
p3p 1:8ff0beb54dd4 114 // debug.printf("Success! DMP configuration written and verified.\r\n");
p3p 0:74f0ae286b03 115 setIntDMPEnabled(true);
p3p 0:74f0ae286b03 116 setInterruptFifoOverflowEnable(true);
p3p 0:74f0ae286b03 117 setSampleRateDivider(4);
p3p 0:74f0ae286b03 118 clockSelect(MPU6050_CLOCK_PLL_XGYRO);
p3p 0:74f0ae286b03 119 setDigitalLowPassFilter(MPU6050_DLPF_BW_42);
p3p 0:74f0ae286b03 120 setGyroFullScaleRange(MPU6050_GYRO_FS_2000);
p3p 0:74f0ae286b03 121
p3p 0:74f0ae286b03 122 setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
p3p 0:74f0ae286b03 123 setDMPConfig1(0x03);
p3p 0:74f0ae286b03 124 setDMPConfig2(0x00);
p3p 0:74f0ae286b03 125
p3p 0:74f0ae286b03 126 unsigned char *update_ptr = (unsigned char*)dmpUpdates;
p3p 0:74f0ae286b03 127 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 128 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 129 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 130
p3p 0:74f0ae286b03 131 setFifoReset(true);
p3p 0:74f0ae286b03 132
p3p 0:74f0ae286b03 133 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 134 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 135 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 136 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 137
p3p 0:74f0ae286b03 138 write(MPU6050_RA_PWR_MGMT_2, 0x00);
p3p 0:74f0ae286b03 139 setInterruptAnyReadClear(true);
p3p 0:74f0ae286b03 140 setInterruptLatch(true);
p3p 0:74f0ae286b03 141
p3p 0:74f0ae286b03 142 setI2cSlaveRW(0, true);
p3p 0:74f0ae286b03 143 setI2cSlaveAddress(0, 0x0C);
p3p 0:74f0ae286b03 144 setI2cSlaveRegister(0, 1);
p3p 0:74f0ae286b03 145 setI2cSlaveEnable(0, true);
p3p 0:74f0ae286b03 146 setI2cSlaveTransactionLength(0, 10);
p3p 0:74f0ae286b03 147
p3p 0:74f0ae286b03 148 //set up slave 1 to request a new magnetometor reading by writing 0x01 to 0xA
p3p 0:74f0ae286b03 149 setI2cSlaveAddress(2, 0x0C);
p3p 0:74f0ae286b03 150 setI2cSlaveRegister(2, 0x0A);
p3p 0:74f0ae286b03 151 setI2cSlaveTransactionLength(2, 1);
p3p 0:74f0ae286b03 152 setI2cSlaveEnable(2, true);
p3p 0:74f0ae286b03 153 setI2cSlaveDataOut(2, 1);
p3p 0:74f0ae286b03 154
p3p 0:74f0ae286b03 155 //configure update rates
p3p 0:74f0ae286b03 156 setI2cMasterDelay(4);
p3p 0:74f0ae286b03 157 setI2cSlaveDelay(0, true);
p3p 0:74f0ae286b03 158 setI2cSlaveDelay(2, true);
p3p 0:74f0ae286b03 159
p3p 0:74f0ae286b03 160 //Enable the aux i2c bus with MPU9150 as master
p3p 0:74f0ae286b03 161 setI2cMasterEnable(true);
p3p 0:74f0ae286b03 162
p3p 0:74f0ae286b03 163 write(MPU6050_RA_INT_PIN_CFG, 0x00);
p3p 0:74f0ae286b03 164
p3p 0:74f0ae286b03 165 // enable I2C master mode and reset DMP/FIFO
p3p 0:74f0ae286b03 166 //DEBUG_PRINTLN(F("Enabling I2C master mode..."));
p3p 0:74f0ae286b03 167 write( MPU6050_RA_USER_CTRL, 0x20);
p3p 0:74f0ae286b03 168 //DEBUG_PRINTLN(F("Resetting FIFO..."));
p3p 0:74f0ae286b03 169 write(MPU6050_RA_USER_CTRL, 0x24);
p3p 0:74f0ae286b03 170 //DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know"));
p3p 0:74f0ae286b03 171 write(MPU6050_RA_USER_CTRL, 0x20);
p3p 0:74f0ae286b03 172 //DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO..."));
p3p 0:74f0ae286b03 173 write(MPU6050_RA_USER_CTRL, 0xE8);
p3p 0:74f0ae286b03 174
p3p 0:74f0ae286b03 175 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 176 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 177 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 178 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 179 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 180 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 181 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 182 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 183 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 184 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 185 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 186 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 187 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 188 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 189
p3p 0:74f0ae286b03 190 //read?
p3p 0:74f0ae286b03 191 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 192 //stalls?
p3p 0:74f0ae286b03 193 //readMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1]);
p3p 0:74f0ae286b03 194
p3p 0:74f0ae286b03 195
p3p 0:74f0ae286b03 196 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 197 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 198 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 199 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 200 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 201 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 202 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 203 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 204 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 205 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 206
p3p 0:74f0ae286b03 207 int fifoCount = 0;
p3p 0:74f0ae286b03 208 while ((fifoCount = getFifoCount()) < 46);
p3p 0:74f0ae286b03 209 uint8_t buffer[128];
p3p 0:74f0ae286b03 210 getFifoBuffer((char *)buffer, fifoCount);
p3p 0:74f0ae286b03 211 getInterruptStatus();
p3p 0:74f0ae286b03 212
p3p 0:74f0ae286b03 213 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 214 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 215
p3p 0:74f0ae286b03 216 fifoCount = 0;
p3p 0:74f0ae286b03 217 while ((fifoCount = getFifoCount()) < 48);
p3p 0:74f0ae286b03 218 getFifoBuffer((char *)buffer, fifoCount);
p3p 0:74f0ae286b03 219 getInterruptStatus();
p3p 0:74f0ae286b03 220 fifoCount = 0;
p3p 0:74f0ae286b03 221 while ((fifoCount = getFifoCount()) < 48);
p3p 0:74f0ae286b03 222 getFifoBuffer((char *)buffer, fifoCount);
p3p 0:74f0ae286b03 223 getInterruptStatus();
p3p 0:74f0ae286b03 224
p3p 0:74f0ae286b03 225 update_ptr += update_ptr[2] + 3;
p3p 0:74f0ae286b03 226 writeMemoryBlock(update_ptr + 3, update_ptr[2], update_ptr[0], update_ptr[1], true);
p3p 0:74f0ae286b03 227
p3p 0:74f0ae286b03 228 setDMPEnabled(false);
p3p 0:74f0ae286b03 229
p3p 1:8ff0beb54dd4 230 // debug.printf("finished\r\n");
p3p 0:74f0ae286b03 231
p3p 0:74f0ae286b03 232 }
p3p 0:74f0ae286b03 233 }
p3p 0:74f0ae286b03 234
p3p 0:74f0ae286b03 235
p3p 0:74f0ae286b03 236 }
p3p 0:74f0ae286b03 237
p3p 0:74f0ae286b03 238 //PWR_MGMT_1 Control Register
p3p 0:74f0ae286b03 239 //*****************************/
p3p 0:74f0ae286b03 240 void MPU9150::reset(){
p3p 0:74f0ae286b03 241 writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
p3p 0:74f0ae286b03 242 }
p3p 0:74f0ae286b03 243
p3p 0:74f0ae286b03 244 void MPU9150::sleep(bool state){
p3p 0:74f0ae286b03 245 writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, state);
p3p 0:74f0ae286b03 246 }
p3p 0:74f0ae286b03 247
p3p 0:74f0ae286b03 248 /*
p3p 0:74f0ae286b03 249 cycle between sleep mode and waking up to take a single sample of data from
p3p 0:74f0ae286b03 250 active sensors at a rate determined by LP_WAKE_CTRL (register 108).
p3p 0:74f0ae286b03 251 */
p3p 0:74f0ae286b03 252 void MPU9150::cycleMode(bool state){
p3p 0:74f0ae286b03 253 writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, state);
p3p 0:74f0ae286b03 254 }
p3p 0:74f0ae286b03 255 void MPU9150::disableTemperatureSensor(bool state){
p3p 0:74f0ae286b03 256 writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, state);
p3p 0:74f0ae286b03 257 }
p3p 0:74f0ae286b03 258 void MPU9150::clockSelect(uint8_t clk){
p3p 0:74f0ae286b03 259 writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, clk);
p3p 0:74f0ae286b03 260 }
p3p 0:74f0ae286b03 261
p3p 0:74f0ae286b03 262 //PWR_MGMT_2 Control Register
p3p 0:74f0ae286b03 263 //*****************************/
p3p 0:74f0ae286b03 264 void MPU9150::setCycleWakeFrequency(uint8_t freq){
p3p 0:74f0ae286b03 265 writeBits(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, freq);
p3p 0:74f0ae286b03 266 }
p3p 0:74f0ae286b03 267 void MPU9150::setStandbyAccX(bool value){
p3p 0:74f0ae286b03 268 writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, value);
p3p 0:74f0ae286b03 269 }
p3p 0:74f0ae286b03 270 void MPU9150::setStandbyAccY(bool value){
p3p 0:74f0ae286b03 271 writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, value);
p3p 0:74f0ae286b03 272 }
p3p 0:74f0ae286b03 273 void MPU9150::setStandbyAccZ(bool value){
p3p 0:74f0ae286b03 274 writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, value);
p3p 0:74f0ae286b03 275 }
p3p 0:74f0ae286b03 276 void MPU9150::setStandbyGyroX( bool value){
p3p 0:74f0ae286b03 277 writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, value);
p3p 0:74f0ae286b03 278 }
p3p 0:74f0ae286b03 279 void MPU9150::setStandbyGyroY( bool value){
p3p 0:74f0ae286b03 280 writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, value);
p3p 0:74f0ae286b03 281 }
p3p 0:74f0ae286b03 282 void MPU9150::setStandbyGyroZ( bool value){
p3p 0:74f0ae286b03 283 writeBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, value);
p3p 0:74f0ae286b03 284 }
p3p 0:74f0ae286b03 285
p3p 0:74f0ae286b03 286 //SMPRT_DIV Sample Rate Divider
p3p 0:74f0ae286b03 287 //*****************************/
p3p 0:74f0ae286b03 288 void MPU9150::setSampleRateDivider(uint8_t value){
p3p 0:74f0ae286b03 289 write(MPU6050_RA_SMPLRT_DIV, value);
p3p 0:74f0ae286b03 290 }
p3p 0:74f0ae286b03 291
p3p 0:74f0ae286b03 292 //CONFIG
p3p 0:74f0ae286b03 293 void MPU9150::setExternalFrameSync(uint8_t value){
p3p 0:74f0ae286b03 294 writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, value);
p3p 0:74f0ae286b03 295 }
p3p 0:74f0ae286b03 296 void MPU9150::setDigitalLowPassFilter(uint8_t value){
p3p 0:74f0ae286b03 297 writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, value);
p3p 0:74f0ae286b03 298 }
p3p 0:74f0ae286b03 299
p3p 0:74f0ae286b03 300 //GYRO_CONFIG
p3p 0:74f0ae286b03 301 void MPU9150::setGyroSelfTest(bool value){
p3p 0:74f0ae286b03 302 writeBit(MPU6050_RA_GYRO_CONFIG, 7, value); //X
p3p 0:74f0ae286b03 303 writeBit(MPU6050_RA_GYRO_CONFIG, 6, value); //Y
p3p 0:74f0ae286b03 304 writeBit(MPU6050_RA_GYRO_CONFIG, 5, value); //Z
p3p 0:74f0ae286b03 305 }
p3p 0:74f0ae286b03 306
p3p 0:74f0ae286b03 307 void MPU9150::setGyroFullScaleRange(uint8_t value){
p3p 0:74f0ae286b03 308 writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, value);
p3p 0:74f0ae286b03 309 }
p3p 0:74f0ae286b03 310
p3p 0:74f0ae286b03 311 //ACCEL_CONFIG
p3p 0:74f0ae286b03 312 void MPU9150::setAccelSelfTest(bool value){
p3p 0:74f0ae286b03 313 writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, value);
p3p 0:74f0ae286b03 314 writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, value);
p3p 0:74f0ae286b03 315 writeBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, value);
p3p 0:74f0ae286b03 316 }
p3p 0:74f0ae286b03 317 void MPU9150::setAccelFullScaleRange(uint8_t value){
p3p 0:74f0ae286b03 318 writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT , MPU6050_ACONFIG_AFS_SEL_LENGTH, value);
p3p 0:74f0ae286b03 319 }
p3p 0:74f0ae286b03 320
p3p 0:74f0ae286b03 321 //FIFO_EN
p3p 0:74f0ae286b03 322 void MPU9150::setTemperatureFifo(bool value){
p3p 0:74f0ae286b03 323 writeBit(MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 324 }
p3p 0:74f0ae286b03 325 void MPU9150::setGyroFifo(bool value){
p3p 0:74f0ae286b03 326 writeBit(MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 327 writeBit(MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 328 writeBit(MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 329 }
p3p 0:74f0ae286b03 330 void MPU9150::setAccelFifo(bool value){
p3p 0:74f0ae286b03 331 writeBit(MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 332 }
p3p 0:74f0ae286b03 333 void MPU9150::setSlave2Fifo(bool value){
p3p 0:74f0ae286b03 334 writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 335 }
p3p 0:74f0ae286b03 336 void MPU9150::setSlave1Fifo(bool value){
p3p 0:74f0ae286b03 337 writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 338 }
p3p 0:74f0ae286b03 339 void MPU9150::setSlave0Fifo(bool value){
p3p 0:74f0ae286b03 340 writeBit(MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 341 }
p3p 0:74f0ae286b03 342
p3p 0:74f0ae286b03 343 //I2C_MST_CTRL
p3p 0:74f0ae286b03 344 void MPU9150::setMultiMaster(bool value){
p3p 0:74f0ae286b03 345 writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, value);
p3p 0:74f0ae286b03 346 }
p3p 0:74f0ae286b03 347 void MPU9150::setWaitForExternalSensor(bool value){
p3p 0:74f0ae286b03 348 writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, value);
p3p 0:74f0ae286b03 349 }
p3p 0:74f0ae286b03 350 void MPU9150::setSlave3Fifo(bool value){
p3p 0:74f0ae286b03 351 writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 352 }
p3p 0:74f0ae286b03 353 void MPU9150::setMasterStartStop(bool value){
p3p 0:74f0ae286b03 354 writeBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, value);
p3p 0:74f0ae286b03 355 }
p3p 0:74f0ae286b03 356 void MPU9150::setI2CMasterClock(uint8_t value){
p3p 0:74f0ae286b03 357 writeBits(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, value);
p3p 0:74f0ae286b03 358 }
p3p 0:74f0ae286b03 359
p3p 0:74f0ae286b03 360 //I2C slaves 0 to 3
p3p 0:74f0ae286b03 361 //I2C_SLV0_ADDR
p3p 0:74f0ae286b03 362 void MPU9150::setI2cSlaveRW(uint8_t slave_id, bool value){
p3p 0:74f0ae286b03 363 if(slave_id > 3)return;
p3p 0:74f0ae286b03 364 writeBit(MPU6050_RA_I2C_SLV0_ADDR + (slave_id * 3), MPU6050_I2C_SLV_RW_BIT, value);
p3p 0:74f0ae286b03 365 }
p3p 0:74f0ae286b03 366 void MPU9150::setI2cSlaveAddress(uint8_t slave_id, uint8_t value){
p3p 0:74f0ae286b03 367 if(slave_id > 3)return;
p3p 0:74f0ae286b03 368 writeBits(MPU6050_RA_I2C_SLV0_ADDR + (slave_id * 3), MPU6050_I2C_SLV_ADDR_BIT, MPU6050_I2C_SLV_ADDR_LENGTH, value);
p3p 0:74f0ae286b03 369 }
p3p 0:74f0ae286b03 370 //I2C_SLV0_REG,
p3p 0:74f0ae286b03 371 void MPU9150::setI2cSlaveRegister(uint8_t slave_id, uint8_t value){
p3p 0:74f0ae286b03 372 if(slave_id > 3)return;
p3p 0:74f0ae286b03 373 write(MPU6050_RA_I2C_SLV0_REG + (slave_id * 3), value);
p3p 0:74f0ae286b03 374 }
p3p 0:74f0ae286b03 375 //I2C_SLV0_CTRL
p3p 0:74f0ae286b03 376 void MPU9150::setI2cSlaveEnable(uint8_t slave_id, bool value){
p3p 0:74f0ae286b03 377 if(slave_id > 3)return;
p3p 0:74f0ae286b03 378 writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_EN_BIT, value);
p3p 0:74f0ae286b03 379 }
p3p 0:74f0ae286b03 380 void MPU9150::setI2cSlaveByteSwap(uint8_t slave_id, bool value){
p3p 0:74f0ae286b03 381 if(slave_id > 3)return;
p3p 0:74f0ae286b03 382 writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_BYTE_SW_BIT, value);
p3p 0:74f0ae286b03 383 }
p3p 0:74f0ae286b03 384 void MPU9150::setI2cSlaveRegDisable(uint8_t slave_id, bool value){
p3p 0:74f0ae286b03 385 if(slave_id > 3)return;
p3p 0:74f0ae286b03 386 writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_REG_DIS_BIT, value);
p3p 0:74f0ae286b03 387 }
p3p 0:74f0ae286b03 388 void MPU9150::setI2cSlaveByteGrouping(uint8_t slave_id, bool value){
p3p 0:74f0ae286b03 389 if(slave_id > 3)return;
p3p 0:74f0ae286b03 390 writeBit(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_GRP_BIT, value);
p3p 0:74f0ae286b03 391 }
p3p 0:74f0ae286b03 392 void MPU9150::setI2cSlaveTransactionLength(uint8_t slave_id, uint8_t value){
p3p 0:74f0ae286b03 393 if(slave_id > 3)return;
p3p 0:74f0ae286b03 394 writeBits(MPU6050_RA_I2C_SLV0_CTRL + (slave_id * 3), MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, value);
p3p 0:74f0ae286b03 395 }
p3p 0:74f0ae286b03 396 //I2C_SLV0_DO
p3p 0:74f0ae286b03 397 void MPU9150::setI2cSlaveDataOut(uint8_t slave_id, uint8_t value){
p3p 0:74f0ae286b03 398 if(slave_id > 3)return;
p3p 0:74f0ae286b03 399 write(MPU6050_RA_I2C_SLV0_DO + slave_id, value);
p3p 0:74f0ae286b03 400 }
p3p 0:74f0ae286b03 401 //I2C_MST_DELAY_CTRL
p3p 0:74f0ae286b03 402 void MPU9150::setI2cSlaveDelay(uint8_t slave_id, uint8_t value){
p3p 0:74f0ae286b03 403 writeBit(MPU6050_RA_I2C_MST_DELAY_CTRL, slave_id, value);
p3p 0:74f0ae286b03 404 }
p3p 0:74f0ae286b03 405 void MPU9150::setI2cSlaveShadowDelay(uint8_t value){
p3p 0:74f0ae286b03 406 writeBit(MPU6050_RA_I2C_MST_DELAY_CTRL, 7, value);
p3p 0:74f0ae286b03 407 }
p3p 0:74f0ae286b03 408
p3p 0:74f0ae286b03 409 //I2C slave4
p3p 0:74f0ae286b03 410 //I2C_SLV4_ADDR
p3p 0:74f0ae286b03 411 void MPU9150::setI2cSlave4RW( bool value){
p3p 0:74f0ae286b03 412 writeBit(MPU6050_RA_I2C_SLV4_ADDR, MPU6050_I2C_SLV4_RW_BIT, value);
p3p 0:74f0ae286b03 413 }
p3p 0:74f0ae286b03 414 void MPU9150::setI2cSlave4Address( uint8_t value){
p3p 0:74f0ae286b03 415 writeBits(MPU6050_RA_I2C_SLV4_ADDR, MPU6050_I2C_SLV4_ADDR_BIT, MPU6050_I2C_SLV4_ADDR_LENGTH, value);
p3p 0:74f0ae286b03 416 }
p3p 0:74f0ae286b03 417 //I2C_SLV4_REG,
p3p 0:74f0ae286b03 418 void MPU9150::setI2cSlave4Register(uint8_t value){
p3p 0:74f0ae286b03 419 write(MPU6050_RA_I2C_SLV4_REG, value);
p3p 0:74f0ae286b03 420 }
p3p 0:74f0ae286b03 421 //I2C_SLV4_DO
p3p 0:74f0ae286b03 422 void MPU9150::setI2cSlave4DataOut(uint8_t value){
p3p 0:74f0ae286b03 423 write(MPU6050_RA_I2C_SLV4_DO, value);
p3p 0:74f0ae286b03 424 }
p3p 0:74f0ae286b03 425
p3p 0:74f0ae286b03 426 //I2C_SLV4_CTRL
p3p 0:74f0ae286b03 427 void MPU9150::setI2cSlave4Enable(bool value){
p3p 0:74f0ae286b03 428 writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, value);
p3p 0:74f0ae286b03 429 }
p3p 0:74f0ae286b03 430
p3p 0:74f0ae286b03 431 void MPU9150::setI2cSlave4IntEnable(bool value){
p3p 0:74f0ae286b03 432 writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, value);
p3p 0:74f0ae286b03 433 }
p3p 0:74f0ae286b03 434
p3p 0:74f0ae286b03 435 void MPU9150::setI2cSlave4RegDisable(bool value){
p3p 0:74f0ae286b03 436 writeBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, value);
p3p 0:74f0ae286b03 437 }
p3p 0:74f0ae286b03 438
p3p 0:74f0ae286b03 439 void MPU9150::setI2cMasterDelay(uint8_t value){
p3p 0:74f0ae286b03 440 writeBits(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, value);
p3p 0:74f0ae286b03 441 }
p3p 0:74f0ae286b03 442
p3p 0:74f0ae286b03 443 uint8_t MPU9150::getI2cSlave4Di(){
p3p 0:74f0ae286b03 444 return get8(MPU6050_RA_I2C_SLV4_DI);
p3p 0:74f0ae286b03 445 }
p3p 0:74f0ae286b03 446
p3p 0:74f0ae286b03 447 //I2C_MST_STATUS
p3p 0:74f0ae286b03 448 bool MPU9150::setI2cPassthrough(){
p3p 0:74f0ae286b03 449 return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT);
p3p 0:74f0ae286b03 450 }
p3p 0:74f0ae286b03 451 bool MPU9150::setI2cSlave4Done(){
p3p 0:74f0ae286b03 452 return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT);
p3p 0:74f0ae286b03 453 }
p3p 0:74f0ae286b03 454 bool MPU9150::setI2cLostArbitration(){
p3p 0:74f0ae286b03 455 return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT);
p3p 0:74f0ae286b03 456 }
p3p 0:74f0ae286b03 457 bool MPU9150::setI2cSlave0Nack(){
p3p 0:74f0ae286b03 458 return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT);
p3p 0:74f0ae286b03 459 }
p3p 0:74f0ae286b03 460 bool MPU9150::setI2cSlave1Nack(){
p3p 0:74f0ae286b03 461 return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT);
p3p 0:74f0ae286b03 462 }
p3p 0:74f0ae286b03 463 bool MPU9150::setI2cSlave2Nack(){
p3p 0:74f0ae286b03 464 return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT);
p3p 0:74f0ae286b03 465 }
p3p 0:74f0ae286b03 466 bool MPU9150::setI2cSlave3Nack(){
p3p 0:74f0ae286b03 467 return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT);
p3p 0:74f0ae286b03 468 }
p3p 0:74f0ae286b03 469 bool MPU9150::setI2cSlave4Nack(){
p3p 0:74f0ae286b03 470 return getBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT);
p3p 0:74f0ae286b03 471 }
p3p 0:74f0ae286b03 472
p3p 0:74f0ae286b03 473 //INT_PIN_CFG
p3p 0:74f0ae286b03 474 void MPU9150::setInterruptActiveLow(bool value){
p3p 0:74f0ae286b03 475 writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, value);
p3p 0:74f0ae286b03 476 }
p3p 0:74f0ae286b03 477 void MPU9150::setInterruptOpenDrain(bool value){
p3p 0:74f0ae286b03 478 writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, value);
p3p 0:74f0ae286b03 479 }
p3p 0:74f0ae286b03 480 void MPU9150::setInterruptLatch(bool value){
p3p 0:74f0ae286b03 481 writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, value);
p3p 0:74f0ae286b03 482 }
p3p 0:74f0ae286b03 483 void MPU9150::setInterruptAnyReadClear(bool value){
p3p 0:74f0ae286b03 484 writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, value);
p3p 0:74f0ae286b03 485 }
p3p 0:74f0ae286b03 486 void MPU9150::setFsyncInterruptActiveLow(bool value){
p3p 0:74f0ae286b03 487 writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, value);
p3p 0:74f0ae286b03 488 }
p3p 0:74f0ae286b03 489 void MPU9150::setFsyncInterruptEnable(bool value){
p3p 0:74f0ae286b03 490 writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, value);
p3p 0:74f0ae286b03 491 }
p3p 0:74f0ae286b03 492 void MPU9150::setI2cAuxBypassEnable(bool value){
p3p 0:74f0ae286b03 493 writeBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, value);
p3p 0:74f0ae286b03 494 }
p3p 0:74f0ae286b03 495
p3p 0:74f0ae286b03 496 //INT_ENABLE
p3p 0:74f0ae286b03 497 void MPU9150::setInterruptFifoOverflowEnable(bool value){
p3p 0:74f0ae286b03 498 writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, value);
p3p 0:74f0ae286b03 499 }
p3p 0:74f0ae286b03 500 void MPU9150::setInterruptMasterEnable(bool value){
p3p 0:74f0ae286b03 501 writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, value);
p3p 0:74f0ae286b03 502 }
p3p 0:74f0ae286b03 503 void MPU9150::setInterruptDataReadyEnable(bool value){
p3p 0:74f0ae286b03 504 writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, value);
p3p 0:74f0ae286b03 505 }
p3p 0:74f0ae286b03 506
p3p 0:74f0ae286b03 507 //INT_STATUS
p3p 0:74f0ae286b03 508 bool MPU9150::getInterruptFifoOverflow(){
p3p 0:74f0ae286b03 509 return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
p3p 0:74f0ae286b03 510 }
p3p 0:74f0ae286b03 511 bool MPU9150::getInterruptMaster(){
p3p 0:74f0ae286b03 512 return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
p3p 0:74f0ae286b03 513 }
p3p 0:74f0ae286b03 514 bool MPU9150::getInterruptDataReady(){
p3p 0:74f0ae286b03 515 return getBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT);
p3p 0:74f0ae286b03 516 }
p3p 0:74f0ae286b03 517
p3p 0:74f0ae286b03 518 uint8_t MPU9150::getInterruptStatus(){
p3p 0:74f0ae286b03 519 return get8(MPU6050_RA_INT_STATUS);
p3p 0:74f0ae286b03 520 }
p3p 0:74f0ae286b03 521
p3p 0:74f0ae286b03 522 //SIGNAL_PATH_RESET
p3p 0:74f0ae286b03 523 void MPU9150::resetGyroSignalPath(){
p3p 0:74f0ae286b03 524 writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
p3p 0:74f0ae286b03 525 }
p3p 0:74f0ae286b03 526 void MPU9150::resetAccelSignalPath(){
p3p 0:74f0ae286b03 527 writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);
p3p 0:74f0ae286b03 528 }
p3p 0:74f0ae286b03 529 void MPU9150::resetTempSignalPath(){
p3p 0:74f0ae286b03 530 writeBit(MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);
p3p 0:74f0ae286b03 531 }
p3p 0:74f0ae286b03 532
p3p 0:74f0ae286b03 533 //USER_CTRL
p3p 0:74f0ae286b03 534 void MPU9150::setEnableFifo(bool value){
p3p 0:74f0ae286b03 535 writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, value);
p3p 0:74f0ae286b03 536 }
p3p 0:74f0ae286b03 537 void MPU9150::setI2cMasterEnable(bool value){
p3p 0:74f0ae286b03 538 writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, value);
p3p 0:74f0ae286b03 539 }
p3p 0:74f0ae286b03 540 void MPU9150::setFifoReset(bool value){
p3p 0:74f0ae286b03 541 writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, value);
p3p 0:74f0ae286b03 542 }
p3p 0:74f0ae286b03 543 void MPU9150::setI2cMasterReset(bool value){
p3p 0:74f0ae286b03 544 writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, value);
p3p 0:74f0ae286b03 545 }
p3p 0:74f0ae286b03 546 void MPU9150::setFullSensorReset(bool value){
p3p 0:74f0ae286b03 547 writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, value);
p3p 0:74f0ae286b03 548 }
p3p 0:74f0ae286b03 549
p3p 0:74f0ae286b03 550 //FIFO_COUNT_H and FIFO_COUNT_L
p3p 0:74f0ae286b03 551 int16_t MPU9150::getFifoCount(){
p3p 0:74f0ae286b03 552 return get16(MPU6050_RA_FIFO_COUNTH);
p3p 0:74f0ae286b03 553 }
p3p 0:74f0ae286b03 554
p3p 0:74f0ae286b03 555 //FIFO_R_W
p3p 0:74f0ae286b03 556 bool MPU9150::getFifoBuffer(char* buffer, int16_t length){
p3p 0:74f0ae286b03 557 return read(MPU6050_RA_FIFO_R_W, buffer, length);
p3p 0:74f0ae286b03 558 }
p3p 0:74f0ae286b03 559
p3p 0:74f0ae286b03 560 //UNDOCUMENTED (again reimplemention from sparkfun github) can't find any origional documentation
p3p 0:74f0ae286b03 561 // XG_OFFS_TC
p3p 0:74f0ae286b03 562 uint8_t MPU9150::getOTPBankValid() {
p3p 0:74f0ae286b03 563 return getBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT);
p3p 0:74f0ae286b03 564 }
p3p 0:74f0ae286b03 565
p3p 0:74f0ae286b03 566 //INT_ENABLE
p3p 0:74f0ae286b03 567 void MPU9150::setIntPLLReadyEnabled(bool value) {
p3p 0:74f0ae286b03 568 writeBit( MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, value);
p3p 0:74f0ae286b03 569 }
p3p 0:74f0ae286b03 570 void MPU9150::setIntDMPEnabled(bool value) {
p3p 0:74f0ae286b03 571 writeBit( MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, value);
p3p 0:74f0ae286b03 572 }
p3p 0:74f0ae286b03 573
p3p 0:74f0ae286b03 574 // INT_STATUS
p3p 0:74f0ae286b03 575 bool MPU9150::getIntPLLReadyStatus() {
p3p 0:74f0ae286b03 576 return getBit( MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
p3p 0:74f0ae286b03 577 }
p3p 0:74f0ae286b03 578 bool MPU9150::getIntDMPStatus() {
p3p 0:74f0ae286b03 579 return getBit( MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT);
p3p 0:74f0ae286b03 580 }
p3p 0:74f0ae286b03 581
p3p 0:74f0ae286b03 582 // USER_CTRL
p3p 0:74f0ae286b03 583 bool MPU9150::getDMPEnabled() {
p3p 0:74f0ae286b03 584 return getBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT);
p3p 0:74f0ae286b03 585 }
p3p 0:74f0ae286b03 586 void MPU9150::setDMPEnabled(bool value) {
p3p 0:74f0ae286b03 587 writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, value);
p3p 0:74f0ae286b03 588 }
p3p 0:74f0ae286b03 589 void MPU9150::resetDMP() {
p3p 0:74f0ae286b03 590 writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
p3p 0:74f0ae286b03 591 }
p3p 0:74f0ae286b03 592
p3p 0:74f0ae286b03 593 // BANK_SEL
p3p 0:74f0ae286b03 594 void MPU9150::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
p3p 0:74f0ae286b03 595 bank &= 0x1F;
p3p 0:74f0ae286b03 596 if (userBank){
p3p 0:74f0ae286b03 597 bank |= 0x20;
p3p 0:74f0ae286b03 598 }
p3p 0:74f0ae286b03 599 if (prefetchEnabled){
p3p 0:74f0ae286b03 600 bank |= 0x40;
p3p 0:74f0ae286b03 601 }
p3p 0:74f0ae286b03 602 write( MPU6050_RA_BANK_SEL, bank);
p3p 0:74f0ae286b03 603 }
p3p 0:74f0ae286b03 604
p3p 0:74f0ae286b03 605 // MEM_START_ADDR
p3p 0:74f0ae286b03 606 void MPU9150::setMemoryStartAddress(uint8_t address) {
p3p 0:74f0ae286b03 607 write(MPU6050_RA_MEM_START_ADDR, address);
p3p 0:74f0ae286b03 608 }
p3p 0:74f0ae286b03 609
p3p 0:74f0ae286b03 610 // MEM_R_W
p3p 0:74f0ae286b03 611 uint8_t MPU9150::readMemoryByte() {
p3p 0:74f0ae286b03 612 return get8(MPU6050_RA_MEM_R_W);
p3p 0:74f0ae286b03 613 }
p3p 0:74f0ae286b03 614 void MPU9150::writeMemoryByte(uint8_t value) {
p3p 0:74f0ae286b03 615 write(MPU6050_RA_MEM_R_W, value);
p3p 0:74f0ae286b03 616 }
p3p 0:74f0ae286b03 617 void MPU9150::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
p3p 0:74f0ae286b03 618 setMemoryBank(bank);
p3p 0:74f0ae286b03 619 setMemoryStartAddress(address);
p3p 0:74f0ae286b03 620
p3p 0:74f0ae286b03 621 uint8_t chunkSize;
p3p 0:74f0ae286b03 622 for (uint16_t i = 0; i < dataSize;) {
p3p 0:74f0ae286b03 623 // determine correct chunk size according to bank position and data size
p3p 0:74f0ae286b03 624 chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
p3p 0:74f0ae286b03 625
p3p 0:74f0ae286b03 626 // make sure we don't go past the data size
p3p 0:74f0ae286b03 627 if (i + chunkSize > dataSize) chunkSize = dataSize - i;
p3p 0:74f0ae286b03 628
p3p 0:74f0ae286b03 629 // make sure this chunk doesn't go past the bank boundary (256 bytes)
p3p 0:74f0ae286b03 630 if (chunkSize > 256 - address) chunkSize = 256 - address;
p3p 1:8ff0beb54dd4 631 //debug.printf("reading %d", chunkSize);
p3p 0:74f0ae286b03 632 // read the chunk of data as specified
p3p 0:74f0ae286b03 633 read(MPU6050_RA_MEM_R_W, (char*)(data+i), chunkSize);
p3p 1:8ff0beb54dd4 634 //debug.printf("read");
p3p 0:74f0ae286b03 635 // increase byte index by [chunkSize]
p3p 0:74f0ae286b03 636 i += chunkSize;
p3p 0:74f0ae286b03 637
p3p 0:74f0ae286b03 638 // uint8_t automatically wraps to 0 at 256
p3p 0:74f0ae286b03 639 address += chunkSize;
p3p 0:74f0ae286b03 640
p3p 0:74f0ae286b03 641 // if we aren't done, update bank (if necessary) and address
p3p 0:74f0ae286b03 642 if (i < dataSize) {
p3p 0:74f0ae286b03 643 if (address == 0) bank++;
p3p 0:74f0ae286b03 644 setMemoryBank(bank);
p3p 0:74f0ae286b03 645 setMemoryStartAddress(address);
p3p 0:74f0ae286b03 646 }
p3p 0:74f0ae286b03 647 }
p3p 0:74f0ae286b03 648 }
p3p 0:74f0ae286b03 649 bool MPU9150::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
p3p 0:74f0ae286b03 650 setMemoryBank(bank);
p3p 0:74f0ae286b03 651 setMemoryStartAddress(address);
p3p 0:74f0ae286b03 652 uint8_t chunkSize;
p3p 0:74f0ae286b03 653 uint8_t *verifyBuffer = 0;
p3p 0:74f0ae286b03 654 uint8_t *progBuffer = 0;
p3p 0:74f0ae286b03 655 uint16_t i;
p3p 0:74f0ae286b03 656
p3p 0:74f0ae286b03 657 if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
p3p 0:74f0ae286b03 658 for (i = 0; i < dataSize;) {
p3p 0:74f0ae286b03 659 // determine correct chunk size according to bank position and data size
p3p 0:74f0ae286b03 660 chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
p3p 0:74f0ae286b03 661
p3p 0:74f0ae286b03 662 // make sure we don't go past the data size
p3p 0:74f0ae286b03 663 if (i + chunkSize > dataSize) chunkSize = dataSize - i;
p3p 0:74f0ae286b03 664
p3p 0:74f0ae286b03 665 // make sure this chunk doesn't go past the bank boundary (256 bytes)
p3p 0:74f0ae286b03 666 if (chunkSize > 256 - address) chunkSize = 256 - address;
p3p 0:74f0ae286b03 667
p3p 0:74f0ae286b03 668 progBuffer = (uint8_t *)data + i;
p3p 0:74f0ae286b03 669
p3p 0:74f0ae286b03 670 write(MPU6050_RA_MEM_R_W, (char*)progBuffer, chunkSize);
p3p 0:74f0ae286b03 671
p3p 0:74f0ae286b03 672
p3p 0:74f0ae286b03 673 // verify data if needed
p3p 0:74f0ae286b03 674 if (verify && verifyBuffer) {
p3p 0:74f0ae286b03 675 setMemoryBank(bank);
p3p 0:74f0ae286b03 676 setMemoryStartAddress(address);
p3p 0:74f0ae286b03 677 read(MPU6050_RA_MEM_R_W, (char*)verifyBuffer, chunkSize);
p3p 0:74f0ae286b03 678 if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
p3p 0:74f0ae286b03 679 free(verifyBuffer);
p3p 1:8ff0beb54dd4 680 //debug.printf("invalid(%d, %d)\r\n", bank, read_errors, write_errors);
p3p 0:74f0ae286b03 681 return false; // uh oh.
p3p 0:74f0ae286b03 682 }
p3p 0:74f0ae286b03 683 }
p3p 0:74f0ae286b03 684
p3p 0:74f0ae286b03 685 // increase byte index by [chunkSize]
p3p 0:74f0ae286b03 686 i += chunkSize;
p3p 0:74f0ae286b03 687
p3p 0:74f0ae286b03 688 // uint8_t automatically wraps to 0 at 256
p3p 0:74f0ae286b03 689 address += chunkSize;
p3p 0:74f0ae286b03 690
p3p 0:74f0ae286b03 691 // if we aren't done, update bank (if necessary) and address
p3p 0:74f0ae286b03 692 if (i < dataSize) {
p3p 0:74f0ae286b03 693 if (address == 0) bank++;
p3p 0:74f0ae286b03 694 setMemoryBank(bank);
p3p 0:74f0ae286b03 695 setMemoryStartAddress(address);
p3p 0:74f0ae286b03 696 }
p3p 0:74f0ae286b03 697 }
p3p 0:74f0ae286b03 698 if (verify) free(verifyBuffer);
p3p 0:74f0ae286b03 699 return true;
p3p 0:74f0ae286b03 700 }
p3p 0:74f0ae286b03 701 bool MPU9150::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
p3p 0:74f0ae286b03 702 uint8_t *progBuffer;
p3p 0:74f0ae286b03 703 uint8_t success, special;
p3p 0:74f0ae286b03 704 uint16_t i;
p3p 0:74f0ae286b03 705
p3p 0:74f0ae286b03 706 // config set data is a long string of blocks with the following structure:
p3p 0:74f0ae286b03 707 // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
p3p 0:74f0ae286b03 708 uint8_t bank, offset, length;
p3p 0:74f0ae286b03 709 for (i = 0; i < dataSize;) {
p3p 0:74f0ae286b03 710 bank = data[i++];
p3p 0:74f0ae286b03 711 offset = data[i++];
p3p 0:74f0ae286b03 712 length = data[i++];
p3p 0:74f0ae286b03 713
p3p 0:74f0ae286b03 714 // write data or perform special action
p3p 0:74f0ae286b03 715 if (length > 0) {
p3p 0:74f0ae286b03 716 progBuffer = (uint8_t *)data + i;
p3p 0:74f0ae286b03 717 success = writeMemoryBlock(progBuffer, length, bank, offset, true);
p3p 0:74f0ae286b03 718 i += length;
p3p 0:74f0ae286b03 719 } else {
p3p 0:74f0ae286b03 720 // special instruction
p3p 0:74f0ae286b03 721 // NOTE: this kind of behavior (what and when to do certain things)
p3p 0:74f0ae286b03 722 // is totally undocumented. This code is in here based on observed
p3p 0:74f0ae286b03 723 // behavior only, and exactly why (or even whether) it has to be here
p3p 0:74f0ae286b03 724 // is anybody's guess for now.
p3p 0:74f0ae286b03 725 special = data[i++];
p3p 0:74f0ae286b03 726
p3p 0:74f0ae286b03 727 if (special == 0x01) {
p3p 0:74f0ae286b03 728 // enable DMP-related interrupts
p3p 0:74f0ae286b03 729 //setIntZeroMotionEnabled(true);
p3p 0:74f0ae286b03 730 //setIntFIFOBufferOverflowEnabled(true);
p3p 0:74f0ae286b03 731 //setIntDMPEnabled(true);
p3p 0:74f0ae286b03 732 write(MPU6050_RA_INT_ENABLE, 0x32); // single operation
p3p 0:74f0ae286b03 733 success = true;
p3p 0:74f0ae286b03 734 } else {
p3p 0:74f0ae286b03 735 // unknown special command
p3p 0:74f0ae286b03 736 success = false;
p3p 0:74f0ae286b03 737 }
p3p 0:74f0ae286b03 738 }
p3p 0:74f0ae286b03 739
p3p 0:74f0ae286b03 740 if (!success) {
p3p 0:74f0ae286b03 741 return false;
p3p 0:74f0ae286b03 742 }
p3p 0:74f0ae286b03 743 }
p3p 0:74f0ae286b03 744 return true;
p3p 0:74f0ae286b03 745 }
p3p 0:74f0ae286b03 746 // DMP_CFG_1
p3p 0:74f0ae286b03 747 uint8_t MPU9150::getDMPConfig1() {
p3p 0:74f0ae286b03 748 return get8(MPU6050_RA_DMP_CFG_1);
p3p 0:74f0ae286b03 749
p3p 0:74f0ae286b03 750 }
p3p 0:74f0ae286b03 751 void MPU9150::setDMPConfig1(uint8_t config) {
p3p 0:74f0ae286b03 752 write(MPU6050_RA_DMP_CFG_1, config);
p3p 0:74f0ae286b03 753 }
p3p 0:74f0ae286b03 754
p3p 0:74f0ae286b03 755 // DMP_CFG_2
p3p 0:74f0ae286b03 756 uint8_t MPU9150::getDMPConfig2() {
p3p 0:74f0ae286b03 757 return get8(MPU6050_RA_DMP_CFG_2);
p3p 0:74f0ae286b03 758
p3p 0:74f0ae286b03 759 }
p3p 0:74f0ae286b03 760 void MPU9150::setDMPConfig2(uint8_t config) {
p3p 0:74f0ae286b03 761 write(MPU6050_RA_DMP_CFG_2, config);
p3p 0:74f0ae286b03 762 }
p3p 0:74f0ae286b03 763
p3p 0:74f0ae286b03 764 //Utility Functions
p3p 0:74f0ae286b03 765 bool MPU9150::getBit(char reg_addr, uint8_t bit){
p3p 0:74f0ae286b03 766 uint8_t data = 0;
p3p 0:74f0ae286b03 767 readBit(reg_addr, bit, &data);
p3p 0:74f0ae286b03 768 return (bool)data;
p3p 0:74f0ae286b03 769 }
p3p 0:74f0ae286b03 770
p3p 0:74f0ae286b03 771 int8_t MPU9150::get8(char reg_addr){
p3p 0:74f0ae286b03 772 char data;
p3p 0:74f0ae286b03 773 read(reg_addr, &data);
p3p 0:74f0ae286b03 774 return data;
p3p 0:74f0ae286b03 775 }
p3p 0:74f0ae286b03 776
p3p 0:74f0ae286b03 777 int16_t MPU9150::get16(char reg_addr){
p3p 0:74f0ae286b03 778 char data[2];
p3p 1:8ff0beb54dd4 779 read(reg_addr, data, 2);
p3p 0:74f0ae286b03 780 return (data[0]<<8) + data[1];
p3p 0:74f0ae286b03 781 }
p3p 0:74f0ae286b03 782
p3p 0:74f0ae286b03 783 int16_t MPU9150::get16L(char reg_addr){
p3p 0:74f0ae286b03 784 char data[2];
p3p 0:74f0ae286b03 785 read(reg_addr, data, 2);
p3p 0:74f0ae286b03 786 return (data[1]<<8) + data[0];
p3p 0:74f0ae286b03 787 }
p3p 0:74f0ae286b03 788
p3p 0:74f0ae286b03 789 bool MPU9150::write(char reg_addr, char data){
p3p 0:74f0ae286b03 790 return write(reg_addr, &data, 1);
p3p 0:74f0ae286b03 791 }
p3p 0:74f0ae286b03 792
p3p 0:74f0ae286b03 793 bool MPU9150::write(char reg_addr, char* data, int length){
p3p 0:74f0ae286b03 794 i2c.start();
p3p 0:74f0ae286b03 795 i2c.write(device_address << 1);
p3p 0:74f0ae286b03 796 i2c.write(reg_addr);
p3p 0:74f0ae286b03 797 for(int i = 0; i < length; i++) {
p3p 0:74f0ae286b03 798 if(!i2c.write(data[i])){
p3p 0:74f0ae286b03 799 write_errors++;
p3p 1:8ff0beb54dd4 800 //debug.printf("Write Error %d\r\n", reg_addr);
p3p 0:74f0ae286b03 801 return false;
p3p 0:74f0ae286b03 802 }
p3p 0:74f0ae286b03 803 }
p3p 0:74f0ae286b03 804 i2c.stop();
p3p 0:74f0ae286b03 805 return true;
p3p 0:74f0ae286b03 806 }
p3p 0:74f0ae286b03 807
p3p 0:74f0ae286b03 808 bool MPU9150::writeBit(char reg_addr, uint8_t bit, bool value){
p3p 0:74f0ae286b03 809 return writeBits(reg_addr, bit, 1, (uint8_t)value);
p3p 0:74f0ae286b03 810 }
p3p 0:74f0ae286b03 811
p3p 0:74f0ae286b03 812 bool MPU9150::writeBits(char reg_addr, uint8_t bit_start, uint8_t length, uint8_t data){
p3p 0:74f0ae286b03 813 char ret;
p3p 0:74f0ae286b03 814
p3p 0:74f0ae286b03 815 if(!read(reg_addr, &ret)){
p3p 0:74f0ae286b03 816 return false;
p3p 0:74f0ae286b03 817 }
p3p 0:74f0ae286b03 818
p3p 0:74f0ae286b03 819 uint8_t mask = ((1 << length) - 1) << (bit_start - length + 1);
p3p 0:74f0ae286b03 820 data <<= (bit_start - length + 1);
p3p 0:74f0ae286b03 821
p3p 0:74f0ae286b03 822 data &= mask;
p3p 0:74f0ae286b03 823 ret &= ~(mask);
p3p 0:74f0ae286b03 824 ret |= data;
p3p 0:74f0ae286b03 825
p3p 0:74f0ae286b03 826 return write(reg_addr, ret);
p3p 0:74f0ae286b03 827 }
p3p 0:74f0ae286b03 828
p3p 0:74f0ae286b03 829 bool MPU9150::read(char reg_addr, char* data){
p3p 0:74f0ae286b03 830 return read(reg_addr, data, 1);
p3p 0:74f0ae286b03 831 }
p3p 0:74f0ae286b03 832
p3p 0:74f0ae286b03 833 bool MPU9150::read(char reg_addr, char* data, int length){
p3p 0:74f0ae286b03 834 if(i2c.write(device_address << 1, &reg_addr, 1, true)){
p3p 0:74f0ae286b03 835 read_errors ++;
p3p 1:8ff0beb54dd4 836 //debug.printf("Read: Address Write Error %d\r\n", reg_addr);
p3p 0:74f0ae286b03 837 return false;
p3p 0:74f0ae286b03 838 }
p3p 0:74f0ae286b03 839 if(i2c.read(device_address << 1, data, length)){
p3p 0:74f0ae286b03 840 read_errors ++;
p3p 1:8ff0beb54dd4 841 //debug.printf("Read: Error %d\r\n", reg_addr);
p3p 0:74f0ae286b03 842 return false;
p3p 0:74f0ae286b03 843 }
p3p 0:74f0ae286b03 844 return true;
p3p 0:74f0ae286b03 845 }
p3p 0:74f0ae286b03 846
p3p 0:74f0ae286b03 847
p3p 0:74f0ae286b03 848 bool MPU9150::readBit(char reg_addr, uint8_t bit_start, uint8_t *data){
p3p 0:74f0ae286b03 849 return readBits(reg_addr, bit_start, 1, data);
p3p 0:74f0ae286b03 850 }
p3p 0:74f0ae286b03 851
p3p 0:74f0ae286b03 852 bool MPU9150::readBits(char reg_addr, uint8_t bit_start, uint8_t length, uint8_t *data){
p3p 0:74f0ae286b03 853 char ret;
p3p 0:74f0ae286b03 854
p3p 0:74f0ae286b03 855 if(!read(reg_addr, &ret)){
p3p 0:74f0ae286b03 856 return false;
p3p 0:74f0ae286b03 857 }
p3p 0:74f0ae286b03 858
p3p 0:74f0ae286b03 859 uint8_t mask = ((1 << length) - 1) << (bit_start - length + 1);
p3p 0:74f0ae286b03 860 ret &= mask;
p3p 0:74f0ae286b03 861 ret >>= (bit_start - length + 1);
p3p 0:74f0ae286b03 862 *data = ret;
p3p 0:74f0ae286b03 863
p3p 0:74f0ae286b03 864 return true;
p3p 0:74f0ae286b03 865 }