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@1:8ff0beb54dd4, 2014-09-01 (annotated)

Committer:
p3p
Date:
Mon Sep 01 13:35:07 2014 +0000
Revision:
1:8ff0beb54dd4
Parent:
0:74f0ae286b03
Child:
2:e523a92390b6
commented out debug messages

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