Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: I2CPeripheral.cpp
- Revision:
- 7:604a8369b801
- Parent:
- 5:b9f2f62a8f90
--- a/I2CPeripheral.cpp Thu Feb 12 19:00:28 2015 +0000 +++ b/I2CPeripheral.cpp Tue Feb 17 16:53:50 2015 +0000 @@ -3,7 +3,8 @@ #define DEBUG "I2CPeripheral" #include "Logger.h" -I2CPeripheral::I2CPeripheral(I2C &i2c, const uint8_t address) : mBus(&i2c), mAddress(address) { +I2CPeripheral::I2CPeripheral(I2C &i2c, const uint8_t address) : mBus(&i2c), mAddress(address) +{ LOG("Initialised with ADDR=0x%02X", mAddress); } @@ -17,39 +18,19 @@ } } -uint8_t I2CPeripheral::read_reg(const uint8_t reg) { +uint8_t I2CPeripheral::read_reg(const uint8_t reg) +{ uint8_t byte; read_reg(reg, &byte, 1); return byte; } -void I2CPeripheral::read_reg(const uint8_t reg, uint8_t* destination, const size_t nBytes) { - mBus->start(); - - if (!mBus->write(mAddress | 0x00)) { - WARN("No ACK after writing addr 0x%02x", mAddress); - mBus->stop(); - return; - } - - if (!mBus->write(reg)) { - WARN("No ACK after writing reg 0x%02x to addr 0x%02x", reg, mAddress); - mBus->stop(); - return; - } - - // Generated repeated start - mBus->start(); - if (!mBus->write(mAddress | 0x01)) { - WARN("No ACK after writing addr 0x%02x after Sr", mAddress); - mBus->stop(); - return; - } - - for (size_t i = 0; i < nBytes; i++) { - destination[i] = mBus->read(i < nBytes ? 1 : 0); // 0 signals to the chip that this is the last byte we're reading - //LOG("byte[%d] = %#x", i, destination[i]); - } - +void I2CPeripheral::read_reg(const uint8_t reg, uint8_t* destination, const size_t nBytes) +{ + // Note about error checking - this function runs in a tight inner loop at 200 Hz or higher. + // Therefore, checking for success and showing error message was removed. + mBus->write(mAddress, (const char*)®, 1, true); + // For reasons not known to me, the read() function also seems to require repeated start 'true' + mBus->read(mAddress, (char*)destination, nBytes, true); mBus->stop(); } \ No newline at end of file