Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
I2CPeripheral.cpp
- Committer:
- pvaibhav
- Date:
- 2015-02-12
- Revision:
- 5:b9f2f62a8f90
- Parent:
- 1:c279bc3af90c
- Child:
- 7:604a8369b801
File content as of revision 5:b9f2f62a8f90:
#include "I2CPeripheral.h" #include "mbed.h" #define DEBUG "I2CPeripheral" #include "Logger.h" I2CPeripheral::I2CPeripheral(I2C &i2c, const uint8_t address) : mBus(&i2c), mAddress(address) { LOG("Initialised with ADDR=0x%02X", mAddress); } void I2CPeripheral::write_reg(const uint8_t reg, const uint8_t val) { char data[2]; data[0] = reg; data[1] = val; if (mBus->write(mAddress, data, 2)) { 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; 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]); } mBus->stop(); }