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@0:74f0ae286b03, 2014-08-31 (annotated)

Committer:
p3p
Date:
Sun Aug 31 12:52:29 2014 +0000
Revision:
0:74f0ae286b03
Child:
1:8ff0beb54dd4
MPU9150 api using its DMP for quaternions

Who changed what in which revision?

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