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*)&reg, 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