Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

drivers/MPU6050-DMP-Ian/I2C/I2Cdev.cpp@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:964eb6a2ef00 1 // ported from arduino library: https://github.com/jrowberg/i2cdevlib
majik 0:964eb6a2ef00 2 // written by szymon gaertig (email: szymon@gaertig.com.pl, website: szymongaertig.pl)
majik 0:964eb6a2ef00 3 // Changelog:
majik 0:964eb6a2ef00 4 // 2013-01-08 - first release
majik 0:964eb6a2ef00 5
majik 0:964eb6a2ef00 6 #include "I2Cdev.h"
majik 0:964eb6a2ef00 7
majik 0:964eb6a2ef00 8 //#define useDebugSerial
majik 0:964eb6a2ef00 9
majik 0:964eb6a2ef00 10 I2Cdev::I2Cdev(): i2c(I2C_SDA,I2C_SCL)//, debugSerial(USBTX, USBRX)
majik 0:964eb6a2ef00 11 {
majik 0:964eb6a2ef00 12
majik 0:964eb6a2ef00 13 }
majik 0:964eb6a2ef00 14
majik 0:964eb6a2ef00 15 I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): i2c(i2cSda,i2cScl)//, debugSerial(USBTX, USBRX)
majik 0:964eb6a2ef00 16 {
majik 0:964eb6a2ef00 17
majik 0:964eb6a2ef00 18 }
majik 0:964eb6a2ef00 19
majik 0:964eb6a2ef00 20 /** Read a single bit from an 8-bit device register.
majik 0:964eb6a2ef00 21 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 22 * @param regAddr Register regAddr to read from
majik 0:964eb6a2ef00 23 * @param bitNum Bit position to read (0-7)
majik 0:964eb6a2ef00 24 * @param data Container for single bit value
majik 0:964eb6a2ef00 25 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
majik 0:964eb6a2ef00 26 * @return Status of read operation (true = success)
majik 0:964eb6a2ef00 27 */
majik 0:964eb6a2ef00 28 int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
majik 0:964eb6a2ef00 29 uint8_t b;
majik 0:964eb6a2ef00 30 uint8_t count = readByte(devAddr, regAddr, &b, timeout);
majik 0:964eb6a2ef00 31 *data = b & (1 << bitNum);
majik 0:964eb6a2ef00 32 return count;
majik 0:964eb6a2ef00 33 }
majik 0:964eb6a2ef00 34
majik 0:964eb6a2ef00 35 /** Read a single bit from a 16-bit device register.
majik 0:964eb6a2ef00 36 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 37 * @param regAddr Register regAddr to read from
majik 0:964eb6a2ef00 38 * @param bitNum Bit position to read (0-15)
majik 0:964eb6a2ef00 39 * @param data Container for single bit value
majik 0:964eb6a2ef00 40 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
majik 0:964eb6a2ef00 41 * @return Status of read operation (true = success)
majik 0:964eb6a2ef00 42 */
majik 0:964eb6a2ef00 43 int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) {
majik 0:964eb6a2ef00 44 uint16_t b;
majik 0:964eb6a2ef00 45 uint8_t count = readWord(devAddr, regAddr, &b, timeout);
majik 0:964eb6a2ef00 46 *data = b & (1 << bitNum);
majik 0:964eb6a2ef00 47 return count;
majik 0:964eb6a2ef00 48 }
majik 0:964eb6a2ef00 49
majik 0:964eb6a2ef00 50 /** Read multiple bits from an 8-bit device register.
majik 0:964eb6a2ef00 51 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 52 * @param regAddr Register regAddr to read from
majik 0:964eb6a2ef00 53 * @param bitStart First bit position to read (0-7)
majik 0:964eb6a2ef00 54 * @param length Number of bits to read (not more than 8)
majik 0:964eb6a2ef00 55 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
majik 0:964eb6a2ef00 56 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
majik 0:964eb6a2ef00 57 * @return Status of read operation (true = success)
majik 0:964eb6a2ef00 58 */
majik 0:964eb6a2ef00 59 int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
majik 0:964eb6a2ef00 60 // 01101001 read byte
majik 0:964eb6a2ef00 61 // 76543210 bit numbers
majik 0:964eb6a2ef00 62 // xxx args: bitStart=4, length=3
majik 0:964eb6a2ef00 63 // 010 masked
majik 0:964eb6a2ef00 64 // -> 010 shifted
majik 0:964eb6a2ef00 65 uint8_t count, b;
majik 0:964eb6a2ef00 66 if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
majik 0:964eb6a2ef00 67 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
majik 0:964eb6a2ef00 68 b &= mask;
majik 0:964eb6a2ef00 69 b >>= (bitStart - length + 1);
majik 0:964eb6a2ef00 70 *data = b;
majik 0:964eb6a2ef00 71 }
majik 0:964eb6a2ef00 72 return count;
majik 0:964eb6a2ef00 73 }
majik 0:964eb6a2ef00 74
majik 0:964eb6a2ef00 75 /** Read multiple bits from a 16-bit device register.
majik 0:964eb6a2ef00 76 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 77 * @param regAddr Register regAddr to read from
majik 0:964eb6a2ef00 78 * @param bitStart First bit position to read (0-15)
majik 0:964eb6a2ef00 79 * @param length Number of bits to read (not more than 16)
majik 0:964eb6a2ef00 80 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
majik 0:964eb6a2ef00 81 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
majik 0:964eb6a2ef00 82 * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
majik 0:964eb6a2ef00 83 */
majik 0:964eb6a2ef00 84 int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) {
majik 0:964eb6a2ef00 85 // 1101011001101001 read byte
majik 0:964eb6a2ef00 86 // fedcba9876543210 bit numbers
majik 0:964eb6a2ef00 87 // xxx args: bitStart=12, length=3
majik 0:964eb6a2ef00 88 // 010 masked
majik 0:964eb6a2ef00 89 // -> 010 shifted
majik 0:964eb6a2ef00 90 uint8_t count;
majik 0:964eb6a2ef00 91 uint16_t w;
majik 0:964eb6a2ef00 92 if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
majik 0:964eb6a2ef00 93 uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
majik 0:964eb6a2ef00 94 w &= mask;
majik 0:964eb6a2ef00 95 w >>= (bitStart - length + 1);
majik 0:964eb6a2ef00 96 *data = w;
majik 0:964eb6a2ef00 97 }
majik 0:964eb6a2ef00 98 return count;
majik 0:964eb6a2ef00 99 }
majik 0:964eb6a2ef00 100 /** Read single byte from an 8-bit device register.
majik 0:964eb6a2ef00 101 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 102 * @param regAddr Register regAddr to read from
majik 0:964eb6a2ef00 103 * @param data Container for byte value read from device
majik 0:964eb6a2ef00 104 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
majik 0:964eb6a2ef00 105 * @return Status of read operation (true = success)
majik 0:964eb6a2ef00 106 */
majik 0:964eb6a2ef00 107 int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
majik 0:964eb6a2ef00 108 return readBytes(devAddr, regAddr, 1, data, timeout);
majik 0:964eb6a2ef00 109 }
majik 0:964eb6a2ef00 110
majik 0:964eb6a2ef00 111 /** Read single word from a 16-bit device register.
majik 0:964eb6a2ef00 112 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 113 * @param regAddr Register regAddr to read from
majik 0:964eb6a2ef00 114 * @param data Container for word value read from device
majik 0:964eb6a2ef00 115 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
majik 0:964eb6a2ef00 116 * @return Status of read operation (true = success)
majik 0:964eb6a2ef00 117 */
majik 0:964eb6a2ef00 118 int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) {
majik 0:964eb6a2ef00 119 return readWords(devAddr, regAddr, 1, data, timeout);
majik 0:964eb6a2ef00 120 }
majik 0:964eb6a2ef00 121
majik 0:964eb6a2ef00 122 /** Read multiple bytes from an 8-bit device register.
majik 0:964eb6a2ef00 123 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 124 * @param regAddr First register regAddr to read from
majik 0:964eb6a2ef00 125 * @param length Number of bytes to read
majik 0:964eb6a2ef00 126 * @param data Buffer to store read data in
majik 0:964eb6a2ef00 127 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
majik 0:964eb6a2ef00 128 * @return Number of bytes read (-1 indicates failure)
majik 0:964eb6a2ef00 129 */
majik 0:964eb6a2ef00 130 int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
majik 0:964eb6a2ef00 131 {
majik 0:964eb6a2ef00 132 char command[1];
majik 0:964eb6a2ef00 133 command[0] = regAddr;
majik 0:964eb6a2ef00 134 char *redData = (char*)malloc(length);
majik 0:964eb6a2ef00 135 i2c.write(devAddr<<1, command, 1, true);
majik 0:964eb6a2ef00 136 i2c.read(devAddr<<1, redData, length);
majik 0:964eb6a2ef00 137 for(int i =0; i < length; i++) {
majik 0:964eb6a2ef00 138 data[i] = redData[i];
majik 0:964eb6a2ef00 139 }
majik 0:964eb6a2ef00 140 free (redData);
majik 0:964eb6a2ef00 141 return length;
majik 0:964eb6a2ef00 142 }
majik 0:964eb6a2ef00 143
majik 0:964eb6a2ef00 144 int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout)
majik 0:964eb6a2ef00 145 {
majik 0:964eb6a2ef00 146 return 0;
majik 0:964eb6a2ef00 147 }
majik 0:964eb6a2ef00 148
majik 0:964eb6a2ef00 149 /** write a single bit in an 8-bit device register.
majik 0:964eb6a2ef00 150 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 151 * @param regAddr Register regAddr to write to
majik 0:964eb6a2ef00 152 * @param bitNum Bit position to write (0-7)
majik 0:964eb6a2ef00 153 * @param value New bit value to write
majik 0:964eb6a2ef00 154 * @return Status of operation (true = success)
majik 0:964eb6a2ef00 155 */
majik 0:964eb6a2ef00 156 bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
majik 0:964eb6a2ef00 157 uint8_t b;
majik 0:964eb6a2ef00 158 readByte(devAddr, regAddr, &b);
majik 0:964eb6a2ef00 159 b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
majik 0:964eb6a2ef00 160 return writeByte(devAddr, regAddr, b);
majik 0:964eb6a2ef00 161 }
majik 0:964eb6a2ef00 162
majik 0:964eb6a2ef00 163 /** write a single bit in a 16-bit device register.
majik 0:964eb6a2ef00 164 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 165 * @param regAddr Register regAddr to write to
majik 0:964eb6a2ef00 166 * @param bitNum Bit position to write (0-15)
majik 0:964eb6a2ef00 167 * @param value New bit value to write
majik 0:964eb6a2ef00 168 * @return Status of operation (true = success)
majik 0:964eb6a2ef00 169 */
majik 0:964eb6a2ef00 170 bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
majik 0:964eb6a2ef00 171 uint16_t w;
majik 0:964eb6a2ef00 172 readWord(devAddr, regAddr, &w);
majik 0:964eb6a2ef00 173 w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
majik 0:964eb6a2ef00 174 return writeWord(devAddr, regAddr, w);
majik 0:964eb6a2ef00 175 }
majik 0:964eb6a2ef00 176
majik 0:964eb6a2ef00 177 /** Write multiple bits in an 8-bit device register.
majik 0:964eb6a2ef00 178 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 179 * @param regAddr Register regAddr to write to
majik 0:964eb6a2ef00 180 * @param bitStart First bit position to write (0-7)
majik 0:964eb6a2ef00 181 * @param length Number of bits to write (not more than 8)
majik 0:964eb6a2ef00 182 * @param data Right-aligned value to write
majik 0:964eb6a2ef00 183 * @return Status of operation (true = success)
majik 0:964eb6a2ef00 184 */
majik 0:964eb6a2ef00 185 bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
majik 0:964eb6a2ef00 186 // 010 value to write
majik 0:964eb6a2ef00 187 // 76543210 bit numbers
majik 0:964eb6a2ef00 188 // xxx args: bitStart=4, length=3
majik 0:964eb6a2ef00 189 // 00011100 mask byte
majik 0:964eb6a2ef00 190 // 10101111 original value (sample)
majik 0:964eb6a2ef00 191 // 10100011 original & ~mask
majik 0:964eb6a2ef00 192 // 10101011 masked | value
majik 0:964eb6a2ef00 193 uint8_t b;
majik 0:964eb6a2ef00 194 if (readByte(devAddr, regAddr, &b) != 0) {
majik 0:964eb6a2ef00 195 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
majik 0:964eb6a2ef00 196 data <<= (bitStart - length + 1); // shift data into correct position
majik 0:964eb6a2ef00 197 data &= mask; // zero all non-important bits in data
majik 0:964eb6a2ef00 198 b &= ~(mask); // zero all important bits in existing byte
majik 0:964eb6a2ef00 199 b |= data; // combine data with existing byte
majik 0:964eb6a2ef00 200 return writeByte(devAddr, regAddr, b);
majik 0:964eb6a2ef00 201 } else {
majik 0:964eb6a2ef00 202 return false;
majik 0:964eb6a2ef00 203 }
majik 0:964eb6a2ef00 204 }
majik 0:964eb6a2ef00 205
majik 0:964eb6a2ef00 206 /** Write multiple bits in a 16-bit device register.
majik 0:964eb6a2ef00 207 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 208 * @param regAddr Register regAddr to write to
majik 0:964eb6a2ef00 209 * @param bitStart First bit position to write (0-15)
majik 0:964eb6a2ef00 210 * @param length Number of bits to write (not more than 16)
majik 0:964eb6a2ef00 211 * @param data Right-aligned value to write
majik 0:964eb6a2ef00 212 * @return Status of operation (true = success)
majik 0:964eb6a2ef00 213 */
majik 0:964eb6a2ef00 214 bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
majik 0:964eb6a2ef00 215 // 010 value to write
majik 0:964eb6a2ef00 216 // fedcba9876543210 bit numbers
majik 0:964eb6a2ef00 217 // xxx args: bitStart=12, length=3
majik 0:964eb6a2ef00 218 // 0001110000000000 mask byte
majik 0:964eb6a2ef00 219 // 1010111110010110 original value (sample)
majik 0:964eb6a2ef00 220 // 1010001110010110 original & ~mask
majik 0:964eb6a2ef00 221 // 1010101110010110 masked | value
majik 0:964eb6a2ef00 222 uint16_t w;
majik 0:964eb6a2ef00 223 if (readWord(devAddr, regAddr, &w) != 0) {
majik 0:964eb6a2ef00 224 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
majik 0:964eb6a2ef00 225 data <<= (bitStart - length + 1); // shift data into correct position
majik 0:964eb6a2ef00 226 data &= mask; // zero all non-important bits in data
majik 0:964eb6a2ef00 227 w &= ~(mask); // zero all important bits in existing word
majik 0:964eb6a2ef00 228 w |= data; // combine data with existing word
majik 0:964eb6a2ef00 229 return writeWord(devAddr, regAddr, w);
majik 0:964eb6a2ef00 230 } else {
majik 0:964eb6a2ef00 231 return false;
majik 0:964eb6a2ef00 232 }
majik 0:964eb6a2ef00 233 }
majik 0:964eb6a2ef00 234
majik 0:964eb6a2ef00 235 /** Write single byte to an 8-bit device register.
majik 0:964eb6a2ef00 236 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 237 * @param regAddr Register address to write to
majik 0:964eb6a2ef00 238 * @param data New byte value to write
majik 0:964eb6a2ef00 239 * @return Status of operation (true = success)
majik 0:964eb6a2ef00 240 */
majik 0:964eb6a2ef00 241 bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
majik 0:964eb6a2ef00 242 return writeBytes(devAddr, regAddr, 1, &data);
majik 0:964eb6a2ef00 243 }
majik 0:964eb6a2ef00 244
majik 0:964eb6a2ef00 245 /** Write single word to a 16-bit device register.
majik 0:964eb6a2ef00 246 * @param devAddr I2C slave device address
majik 0:964eb6a2ef00 247 * @param regAddr Register address to write to
majik 0:964eb6a2ef00 248 * @param data New word value to write
majik 0:964eb6a2ef00 249 * @return Status of operation (true = success)
majik 0:964eb6a2ef00 250 */
majik 0:964eb6a2ef00 251 bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
majik 0:964eb6a2ef00 252 return writeWords(devAddr, regAddr, 1, &data);
majik 0:964eb6a2ef00 253 }
majik 0:964eb6a2ef00 254
majik 0:964eb6a2ef00 255 bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
majik 0:964eb6a2ef00 256 {
majik 0:964eb6a2ef00 257 i2c.start();
majik 0:964eb6a2ef00 258 i2c.write(devAddr<<1);
majik 0:964eb6a2ef00 259 i2c.write(regAddr);
majik 0:964eb6a2ef00 260 for(int i = 0; i < length; i++) {
majik 0:964eb6a2ef00 261 i2c.write(data[i]);
majik 0:964eb6a2ef00 262 }
majik 0:964eb6a2ef00 263 i2c.stop();
majik 0:964eb6a2ef00 264 return true;
majik 0:964eb6a2ef00 265 }
majik 0:964eb6a2ef00 266
majik 0:964eb6a2ef00 267 bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data)
majik 0:964eb6a2ef00 268 {
majik 0:964eb6a2ef00 269 return true;
majik 0:964eb6a2ef00 270 }
majik 0:964eb6a2ef00 271
majik 0:964eb6a2ef00 272 uint16_t I2Cdev::readTimeout(void)
majik 0:964eb6a2ef00 273 {
majik 0:964eb6a2ef00 274 return 0;
majik 0:964eb6a2ef00 275 }