Akash Vibhute / MPU9150_DMP_Nucleo

Modified I2C write / read methods for ST nucleo platforms. Library and code now works with Nucleo F411. Should also work with Nucleo F401.

Dependents:   MPU9150_nucleo_noni2cdev MPU9150_nucleo_noni2cdev_F401 JPEGCamera_SIM808_MPU9150_STM32F401RE

Fork of MPU9150_DMP by Chris Pepper

MPU9150.cpp@5:20152826993d, 2015-03-29 (annotated)

Committer:
akashvibhute
Date:
Sun Mar 29 08:38:25 2015 +0000
Revision:
5:20152826993d
Parent:
4:a4f794629c31 
Inserted update comment

Who changed what in which revision?

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