1.updata mbed os 5. 2.restore I2C SDA lock low.
Fork of MPU9150_DMP_Nucleo by
MPU9150.cpp@6:9fc30fdfa3c6, 2017-06-23 (annotated)
- Committer:
- tgw
- Date:
- Fri Jun 23 01:18:33 2017 +0000
- Revision:
- 6:9fc30fdfa3c6
- Parent:
- 5:20152826993d
1.updata mbed os.; 2.restore I2C SDA lock low.
Who changed what in which revision?
User | Revision | Line number | New 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 | { |
tgw | 6:9fc30fdfa3c6 | 803 | /* |
akashvibhute | 3:b0eb5b37ba29 | 804 | char reg_addrs[1] = {reg_addr}; |
akashvibhute | 3:b0eb5b37ba29 | 805 | char data_w[length]; |
akashvibhute | 3:b0eb5b37ba29 | 806 | for(int i=0; i<length; i++) |
akashvibhute | 3:b0eb5b37ba29 | 807 | data_w[i] = data[i]; |
tgw | 6:9fc30fdfa3c6 | 808 | |
akashvibhute | 3:b0eb5b37ba29 | 809 | i2c.write(device_address<<1, reg_addrs, 1, true); |
akashvibhute | 3:b0eb5b37ba29 | 810 | for(int i = 0; i < length; i++) |
akashvibhute | 3:b0eb5b37ba29 | 811 | { |
akashvibhute | 3:b0eb5b37ba29 | 812 | if(!i2c.write(data_w[i])) |
akashvibhute | 3:b0eb5b37ba29 | 813 | { |
p3p | 0:74f0ae286b03 | 814 | write_errors++; |
p3p | 1:8ff0beb54dd4 | 815 | //debug.printf("Write Error %d\r\n", reg_addr); |
p3p | 0:74f0ae286b03 | 816 | return false; |
p3p | 0:74f0ae286b03 | 817 | } |
p3p | 0:74f0ae286b03 | 818 | } |
p3p | 0:74f0ae286b03 | 819 | i2c.stop(); |
p3p | 0:74f0ae286b03 | 820 | return true; |
tgw | 6:9fc30fdfa3c6 | 821 | */ |
tgw | 6:9fc30fdfa3c6 | 822 | //-------------------------------------tgw-------------------------------------------- |
tgw | 6:9fc30fdfa3c6 | 823 | i2c.start(); |
tgw | 6:9fc30fdfa3c6 | 824 | i2c.write(device_address<<1); |
tgw | 6:9fc30fdfa3c6 | 825 | i2c.write(reg_addr); |
tgw | 6:9fc30fdfa3c6 | 826 | for(int i = 0; i < length; i++) |
tgw | 6:9fc30fdfa3c6 | 827 | { |
tgw | 6:9fc30fdfa3c6 | 828 | i2c.write(data[i]); |
tgw | 6:9fc30fdfa3c6 | 829 | } |
tgw | 6:9fc30fdfa3c6 | 830 | i2c.stop(); |
tgw | 6:9fc30fdfa3c6 | 831 | return true; |
p3p | 0:74f0ae286b03 | 832 | } |
p3p | 0:74f0ae286b03 | 833 | |
p3p | 0:74f0ae286b03 | 834 | bool MPU9150::writeBit(char reg_addr, uint8_t bit, bool value){ |
p3p | 0:74f0ae286b03 | 835 | return writeBits(reg_addr, bit, 1, (uint8_t)value); |
p3p | 0:74f0ae286b03 | 836 | } |
p3p | 0:74f0ae286b03 | 837 | |
p3p | 0:74f0ae286b03 | 838 | bool MPU9150::writeBits(char reg_addr, uint8_t bit_start, uint8_t length, uint8_t data){ |
p3p | 0:74f0ae286b03 | 839 | char ret; |
p3p | 0:74f0ae286b03 | 840 | |
p3p | 0:74f0ae286b03 | 841 | if(!read(reg_addr, &ret)){ |
p3p | 0:74f0ae286b03 | 842 | return false; |
p3p | 0:74f0ae286b03 | 843 | } |
p3p | 0:74f0ae286b03 | 844 | |
p3p | 0:74f0ae286b03 | 845 | uint8_t mask = ((1 << length) - 1) << (bit_start - length + 1); |
p3p | 0:74f0ae286b03 | 846 | data <<= (bit_start - length + 1); |
p3p | 0:74f0ae286b03 | 847 | |
p3p | 0:74f0ae286b03 | 848 | data &= mask; |
p3p | 0:74f0ae286b03 | 849 | ret &= ~(mask); |
p3p | 0:74f0ae286b03 | 850 | ret |= data; |
p3p | 0:74f0ae286b03 | 851 | |
p3p | 0:74f0ae286b03 | 852 | return write(reg_addr, ret); |
p3p | 0:74f0ae286b03 | 853 | } |
p3p | 0:74f0ae286b03 | 854 | |
p3p | 0:74f0ae286b03 | 855 | bool MPU9150::read(char reg_addr, char* data){ |
p3p | 0:74f0ae286b03 | 856 | return read(reg_addr, data, 1); |
p3p | 0:74f0ae286b03 | 857 | } |
p3p | 0:74f0ae286b03 | 858 | |
akashvibhute | 3:b0eb5b37ba29 | 859 | bool MPU9150::read(char reg_addr, char* data, int length) |
akashvibhute | 3:b0eb5b37ba29 | 860 | { |
akashvibhute | 3:b0eb5b37ba29 | 861 | char command[1]; |
akashvibhute | 3:b0eb5b37ba29 | 862 | command[0] = reg_addr; |
akashvibhute | 3:b0eb5b37ba29 | 863 | char *redData = (char*)malloc(length); |
tgw | 6:9fc30fdfa3c6 | 864 | /* |
akashvibhute | 3:b0eb5b37ba29 | 865 | if(i2c.write(device_address<<1, command, 1, true)) |
akashvibhute | 3:b0eb5b37ba29 | 866 | { |
p3p | 0:74f0ae286b03 | 867 | read_errors ++; |
p3p | 1:8ff0beb54dd4 | 868 | //debug.printf("Read: Address Write Error %d\r\n", reg_addr); |
p3p | 0:74f0ae286b03 | 869 | return false; |
p3p | 0:74f0ae286b03 | 870 | } |
akashvibhute | 3:b0eb5b37ba29 | 871 | if(!i2c.read(device_address<<1, redData, length, false)) |
akashvibhute | 3:b0eb5b37ba29 | 872 | { |
akashvibhute | 3:b0eb5b37ba29 | 873 | for(int i =0; i < length; i++) |
akashvibhute | 3:b0eb5b37ba29 | 874 | { |
akashvibhute | 3:b0eb5b37ba29 | 875 | data[i] = redData[i]; |
akashvibhute | 3:b0eb5b37ba29 | 876 | } |
akashvibhute | 3:b0eb5b37ba29 | 877 | } |
akashvibhute | 3:b0eb5b37ba29 | 878 | else |
akashvibhute | 3:b0eb5b37ba29 | 879 | { |
p3p | 0:74f0ae286b03 | 880 | read_errors ++; |
p3p | 1:8ff0beb54dd4 | 881 | //debug.printf("Read: Error %d\r\n", reg_addr); |
p3p | 0:74f0ae286b03 | 882 | return false; |
p3p | 0:74f0ae286b03 | 883 | } |
akashvibhute | 3:b0eb5b37ba29 | 884 | free (redData); |
p3p | 0:74f0ae286b03 | 885 | return true; |
tgw | 6:9fc30fdfa3c6 | 886 | */ |
tgw | 6:9fc30fdfa3c6 | 887 | //---------------------------------------tgw-------------------------------------------- |
tgw | 6:9fc30fdfa3c6 | 888 | if(i2c.write(device_address<<1, command, 1, true)) |
tgw | 6:9fc30fdfa3c6 | 889 | { |
tgw | 6:9fc30fdfa3c6 | 890 | read_errors ++; |
tgw | 6:9fc30fdfa3c6 | 891 | //debug.printf("Read: Address Write Error %d\r\n", reg_addr); |
tgw | 6:9fc30fdfa3c6 | 892 | //return false; |
tgw | 6:9fc30fdfa3c6 | 893 | } |
tgw | 6:9fc30fdfa3c6 | 894 | if(!i2c.read(device_address<<1, redData, length, false)) |
tgw | 6:9fc30fdfa3c6 | 895 | { |
tgw | 6:9fc30fdfa3c6 | 896 | for(int i =0; i < length; i++) |
tgw | 6:9fc30fdfa3c6 | 897 | { |
tgw | 6:9fc30fdfa3c6 | 898 | data[i] = redData[i]; |
tgw | 6:9fc30fdfa3c6 | 899 | } |
tgw | 6:9fc30fdfa3c6 | 900 | } |
tgw | 6:9fc30fdfa3c6 | 901 | else |
tgw | 6:9fc30fdfa3c6 | 902 | { |
tgw | 6:9fc30fdfa3c6 | 903 | read_errors ++; |
tgw | 6:9fc30fdfa3c6 | 904 | //debug.printf("Read: Error %d\r\n", reg_addr); |
tgw | 6:9fc30fdfa3c6 | 905 | //return false; |
tgw | 6:9fc30fdfa3c6 | 906 | } |
tgw | 6:9fc30fdfa3c6 | 907 | free (redData); |
tgw | 6:9fc30fdfa3c6 | 908 | return true; |
p3p | 0:74f0ae286b03 | 909 | } |
p3p | 0:74f0ae286b03 | 910 | |
p3p | 0:74f0ae286b03 | 911 | |
p3p | 0:74f0ae286b03 | 912 | bool MPU9150::readBit(char reg_addr, uint8_t bit_start, uint8_t *data){ |
p3p | 0:74f0ae286b03 | 913 | return readBits(reg_addr, bit_start, 1, data); |
p3p | 0:74f0ae286b03 | 914 | } |
p3p | 0:74f0ae286b03 | 915 | |
p3p | 0:74f0ae286b03 | 916 | bool MPU9150::readBits(char reg_addr, uint8_t bit_start, uint8_t length, uint8_t *data){ |
p3p | 0:74f0ae286b03 | 917 | char ret; |
p3p | 0:74f0ae286b03 | 918 | |
p3p | 0:74f0ae286b03 | 919 | if(!read(reg_addr, &ret)){ |
p3p | 0:74f0ae286b03 | 920 | return false; |
p3p | 0:74f0ae286b03 | 921 | } |
p3p | 0:74f0ae286b03 | 922 | |
p3p | 0:74f0ae286b03 | 923 | uint8_t mask = ((1 << length) - 1) << (bit_start - length + 1); |
p3p | 0:74f0ae286b03 | 924 | ret &= mask; |
p3p | 0:74f0ae286b03 | 925 | ret >>= (bit_start - length + 1); |
p3p | 0:74f0ae286b03 | 926 | *data = ret; |
p3p | 0:74f0ae286b03 | 927 | |
p3p | 0:74f0ae286b03 | 928 | return true; |
p3p | 0:74f0ae286b03 | 929 | } |