Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
I2CPeripheral.cpp@5:b9f2f62a8f90, 2015-02-12 (annotated)
- Committer:
- pvaibhav
- Date:
- Thu Feb 12 17:17:35 2015 +0000
- Revision:
- 5:b9f2f62a8f90
- Parent:
- 1:c279bc3af90c
- Child:
- 7:604a8369b801
Gyro data can now be read fine. 100 Hz.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pvaibhav | 0:943820483318 | 1 | #include "I2CPeripheral.h" |
pvaibhav | 0:943820483318 | 2 | #include "mbed.h" |
pvaibhav | 0:943820483318 | 3 | #define DEBUG "I2CPeripheral" |
pvaibhav | 0:943820483318 | 4 | #include "Logger.h" |
pvaibhav | 0:943820483318 | 5 | |
pvaibhav | 0:943820483318 | 6 | I2CPeripheral::I2CPeripheral(I2C &i2c, const uint8_t address) : mBus(&i2c), mAddress(address) { |
pvaibhav | 1:c279bc3af90c | 7 | LOG("Initialised with ADDR=0x%02X", mAddress); |
pvaibhav | 0:943820483318 | 8 | } |
pvaibhav | 0:943820483318 | 9 | |
pvaibhav | 0:943820483318 | 10 | void I2CPeripheral::write_reg(const uint8_t reg, const uint8_t val) |
pvaibhav | 0:943820483318 | 11 | { |
pvaibhav | 0:943820483318 | 12 | char data[2]; |
pvaibhav | 0:943820483318 | 13 | data[0] = reg; |
pvaibhav | 0:943820483318 | 14 | data[1] = val; |
pvaibhav | 0:943820483318 | 15 | if (mBus->write(mAddress, data, 2)) { |
pvaibhav | 1:c279bc3af90c | 16 | ERR("Write failed, addr=0x%02x, reg=%02Xh, data=%02Xh", mAddress, reg, val); |
pvaibhav | 0:943820483318 | 17 | } |
pvaibhav | 0:943820483318 | 18 | } |
pvaibhav | 0:943820483318 | 19 | |
pvaibhav | 0:943820483318 | 20 | uint8_t I2CPeripheral::read_reg(const uint8_t reg) { |
pvaibhav | 5:b9f2f62a8f90 | 21 | uint8_t byte; |
pvaibhav | 5:b9f2f62a8f90 | 22 | read_reg(reg, &byte, 1); |
pvaibhav | 5:b9f2f62a8f90 | 23 | return byte; |
pvaibhav | 5:b9f2f62a8f90 | 24 | } |
pvaibhav | 5:b9f2f62a8f90 | 25 | |
pvaibhav | 5:b9f2f62a8f90 | 26 | void I2CPeripheral::read_reg(const uint8_t reg, uint8_t* destination, const size_t nBytes) { |
pvaibhav | 1:c279bc3af90c | 27 | mBus->start(); |
pvaibhav | 1:c279bc3af90c | 28 | |
pvaibhav | 1:c279bc3af90c | 29 | if (!mBus->write(mAddress | 0x00)) { |
pvaibhav | 1:c279bc3af90c | 30 | WARN("No ACK after writing addr 0x%02x", mAddress); |
pvaibhav | 0:943820483318 | 31 | mBus->stop(); |
pvaibhav | 5:b9f2f62a8f90 | 32 | return; |
pvaibhav | 0:943820483318 | 33 | } |
pvaibhav | 0:943820483318 | 34 | |
pvaibhav | 1:c279bc3af90c | 35 | if (!mBus->write(reg)) { |
pvaibhav | 1:c279bc3af90c | 36 | WARN("No ACK after writing reg 0x%02x to addr 0x%02x", reg, mAddress); |
pvaibhav | 1:c279bc3af90c | 37 | mBus->stop(); |
pvaibhav | 5:b9f2f62a8f90 | 38 | return; |
pvaibhav | 0:943820483318 | 39 | } |
pvaibhav | 1:c279bc3af90c | 40 | |
pvaibhav | 1:c279bc3af90c | 41 | // Generated repeated start |
pvaibhav | 1:c279bc3af90c | 42 | mBus->start(); |
pvaibhav | 1:c279bc3af90c | 43 | if (!mBus->write(mAddress | 0x01)) { |
pvaibhav | 1:c279bc3af90c | 44 | WARN("No ACK after writing addr 0x%02x after Sr", mAddress); |
pvaibhav | 1:c279bc3af90c | 45 | mBus->stop(); |
pvaibhav | 5:b9f2f62a8f90 | 46 | return; |
pvaibhav | 1:c279bc3af90c | 47 | } |
pvaibhav | 1:c279bc3af90c | 48 | |
pvaibhav | 5:b9f2f62a8f90 | 49 | for (size_t i = 0; i < nBytes; i++) { |
pvaibhav | 5:b9f2f62a8f90 | 50 | destination[i] = mBus->read(i < nBytes ? 1 : 0); // 0 signals to the chip that this is the last byte we're reading |
pvaibhav | 5:b9f2f62a8f90 | 51 | //LOG("byte[%d] = %#x", i, destination[i]); |
pvaibhav | 5:b9f2f62a8f90 | 52 | } |
pvaibhav | 1:c279bc3af90c | 53 | |
pvaibhav | 5:b9f2f62a8f90 | 54 | mBus->stop(); |
pvaibhav | 0:943820483318 | 55 | } |