Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: I2CPeripheral.cpp
- Revision:
- 0:943820483318
- Child:
- 1:c279bc3af90c
diff -r 000000000000 -r 943820483318 I2CPeripheral.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/I2CPeripheral.cpp Tue Jan 13 11:23:01 2015 +0000 @@ -0,0 +1,32 @@ +#include "I2CPeripheral.h" +#include "mbed.h" +#define DEBUG "I2CPeripheral" +#include "Logger.h" + +I2CPeripheral::I2CPeripheral(I2C &i2c, const uint8_t address) : mBus(&i2c), mAddress(address) { + INFO("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", 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->stop(); + return 0; + } + + if (mBus->read(mAddress, (char*)&byte, 1)) { + ERR("Can't read reg=0x%02x", reg); + } + return byte; +} \ No newline at end of file