Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: I2CPeripheral.cpp
- Revision:
- 1:c279bc3af90c
- Parent:
- 0:943820483318
- Child:
- 5:b9f2f62a8f90
--- a/I2CPeripheral.cpp Tue Jan 13 11:23:01 2015 +0000 +++ b/I2CPeripheral.cpp Wed Jan 14 15:44:34 2015 +0000 @@ -4,7 +4,7 @@ #include "Logger.h" I2CPeripheral::I2CPeripheral(I2C &i2c, const uint8_t address) : mBus(&i2c), mAddress(address) { - INFO("Initialised with ADDR=0x%02X", mAddress); + LOG("Initialised with ADDR=0x%02X", mAddress); } void I2CPeripheral::write_reg(const uint8_t reg, const uint8_t val) @@ -13,20 +13,36 @@ data[0] = reg; data[1] = val; if (mBus->write(mAddress, data, 2)) { - ERR("Write failed, addr=0x%02x, reg=%02Xh, data=%02Xh", reg, val); + ERR("Write failed, addr=0x%02x, reg=%02Xh, data=%02Xh", mAddress, reg, val); } } uint8_t I2CPeripheral::read_reg(const uint8_t reg) { - uint8_t byte; - if (mBus->write(mAddress, (const char*)®, 1, true)) { - ERR("Can't write reg=0x%02x to read from", reg); + + mBus->start(); + + if (!mBus->write(mAddress | 0x00)) { + WARN("No ACK after writing addr 0x%02x", mAddress); mBus->stop(); return 0; } - if (mBus->read(mAddress, (char*)&byte, 1)) { - ERR("Can't read reg=0x%02x", reg); + if (!mBus->write(reg)) { + WARN("No ACK after writing reg 0x%02x to addr 0x%02x", reg, mAddress); + mBus->stop(); + return 0; } - return byte; + + // Generated repeated start + mBus->start(); + if (!mBus->write(mAddress | 0x01)) { + WARN("No ACK after writing addr 0x%02x after Sr", mAddress); + mBus->stop(); + return 0; + } + + const uint8_t byte = mBus->read(0); // don't acknowledge, only need 1 byte + mBus->stop(); + + return byte; } \ No newline at end of file