From 58a31318eea07fed2d270617a7d90d70df6b0a29 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 28 Sep 2013 10:00:49 +1000 Subject: [PATCH] AP_HAL_Linux: initial I2C driver implementation this works sufficiently for the HMC5883 --- libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 3 +- libraries/AP_HAL_Linux/I2CDriver.cpp | 151 ++++++++++++++++++--- libraries/AP_HAL_Linux/I2CDriver.h | 7 +- libraries/AP_HAL_Linux/Semaphores.cpp | 5 +- libraries/AP_HAL_Linux/Semaphores.h | 2 +- 5 files changed, 146 insertions(+), 22 deletions(-) diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 0e4200bab6..c5217c287f 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -20,7 +20,7 @@ static LinuxUARTDriver uartBDriver; static Empty::EmptyUARTDriver uartCDriver; static LinuxSemaphore i2cSemaphore; -static LinuxI2CDriver i2cDriver(&i2cSemaphore); +static LinuxI2CDriver i2cDriver(&i2cSemaphore, "/dev/i2c-1"); static Empty::EmptySPIDeviceManager spiDeviceManager; static LinuxAnalogIn analogIn; static LinuxStorage storageDriver; @@ -76,6 +76,7 @@ void HAL_Linux::init(int argc,char* const argv[]) const * Scheduler should likely come first. */ scheduler->init(NULL); uartA->begin(115200); + i2c->begin(); } const HAL_Linux AP_HAL_Linux; diff --git a/libraries/AP_HAL_Linux/I2CDriver.cpp b/libraries/AP_HAL_Linux/I2CDriver.cpp index f2822a1c7b..6cc6c02868 100644 --- a/libraries/AP_HAL_Linux/I2CDriver.cpp +++ b/libraries/AP_HAL_Linux/I2CDriver.cpp @@ -2,27 +2,146 @@ #include #include "I2CDriver.h" +#include +#include +#include +#include +#include +#include +#include + using namespace Linux; -void LinuxI2CDriver::begin() {} -void LinuxI2CDriver::end() {} -void LinuxI2CDriver::setTimeout(uint16_t ms) {} -void LinuxI2CDriver::setHighSpeed(bool active) {} +/* + constructor + */ +LinuxI2CDriver::LinuxI2CDriver(AP_HAL::Semaphore* semaphore, const char *device) : + _semaphore(semaphore), + _fd(-1), + _device(device) +{ +} + +/* + called from HAL class init() + */ +void LinuxI2CDriver::begin() +{ + if (_fd != -1) { + close(_fd); + } + _fd = open(_device, O_RDWR); +} + +void LinuxI2CDriver::end() +{ + if (_fd != -1) { + ::close(_fd); + _fd = -1; + } +} + +/* + tell the I2C library what device we want to talk to + */ +bool LinuxI2CDriver::set_address(uint8_t addr) +{ + if (_fd == -1) { + return false; + } + if (_addr != addr) { + ioctl(_fd, I2C_SLAVE, addr); + _addr = addr; + } + return true; +} + +void LinuxI2CDriver::setTimeout(uint16_t ms) +{ + // unimplemented +} + +void LinuxI2CDriver::setHighSpeed(bool active) +{ + // unimplemented +} uint8_t LinuxI2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data) -{return 0;} -uint8_t LinuxI2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val) -{return 0;} +{ + if (!set_address(addr)) { + return 1; + } + if (::write(_fd, data, len) != len) { + return 1; + } + return 0; // success +} + + uint8_t LinuxI2CDriver::writeRegisters(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t* data) -{return 0;} + uint8_t len, uint8_t* data) +{ + uint8_t buf[len+1]; + buf[0] = reg; + if (len != 0) { + memcpy(&buf[1], data, len); + } + return write(addr, len+1, buf); +} + +uint8_t LinuxI2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val) +{ + if (!set_address(addr)) { + return 1; + } + return i2c_smbus_write_byte_data(_fd, reg, val); +} uint8_t LinuxI2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data) -{return 0;} -uint8_t LinuxI2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data) -{return 0;} -uint8_t LinuxI2CDriver::readRegisters(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t* data) -{return 0;} +{ + if (!set_address(addr)) { + return 1; + } + if (write(addr, 0, NULL) != 0) { + return 1; + } + if (::read(_fd, data, len) != len) { + return 1; + } + return 0; +} -uint8_t LinuxI2CDriver::lockup_count() {return 0;} +uint8_t LinuxI2CDriver::readRegisters(uint8_t addr, uint8_t reg, + uint8_t len, uint8_t* data) +{ + if (!set_address(addr)) { + return 1; + } + // send the address to read from + if (::write(_fd, ®, 1) != 1) { + return 1; + } + if (::read(_fd, data, len) != len) { + return 1; + } + return 0; +} + + +uint8_t LinuxI2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data) +{ + if (!set_address(addr)) { + return 1; + } + int32_t v = i2c_smbus_read_byte_data(_fd, reg); + if (v == -1) { + return 1; + } + *data = v & 0xFF; + return 0; +} + +uint8_t LinuxI2CDriver::lockup_count() +{ + return 0; +} diff --git a/libraries/AP_HAL_Linux/I2CDriver.h b/libraries/AP_HAL_Linux/I2CDriver.h index 2d13668afe..ee92f87ed4 100644 --- a/libraries/AP_HAL_Linux/I2CDriver.h +++ b/libraries/AP_HAL_Linux/I2CDriver.h @@ -6,7 +6,8 @@ class Linux::LinuxI2CDriver : public AP_HAL::I2CDriver { public: - LinuxI2CDriver(AP_HAL::Semaphore* semaphore) : _semaphore(semaphore) {} + LinuxI2CDriver(AP_HAL::Semaphore* semaphore, const char *device); + void begin(); void end(); void setTimeout(uint16_t ms); @@ -36,6 +37,10 @@ public: private: AP_HAL::Semaphore* _semaphore; + bool set_address(uint8_t addr); + int _fd; + uint8_t _addr; + const char *_device; }; #endif // __AP_HAL_LINUX_I2CDRIVER_H__ diff --git a/libraries/AP_HAL_Linux/Semaphores.cpp b/libraries/AP_HAL_Linux/Semaphores.cpp index 9d2abb8fd1..ad7db08294 100644 --- a/libraries/AP_HAL_Linux/Semaphores.cpp +++ b/libraries/AP_HAL_Linux/Semaphores.cpp @@ -17,11 +17,10 @@ bool LinuxSemaphore::take(uint32_t timeout_ms) { } bool LinuxSemaphore::take_nonblocking() { - /* No syncronisation primitives to garuntee this is correct */ + /* we need pthread semaphores here */ if (!_taken) { _taken = true; return true; - } else { - return false; } + return false; } diff --git a/libraries/AP_HAL_Linux/Semaphores.h b/libraries/AP_HAL_Linux/Semaphores.h index bf1c3a2e88..b8b0e56b35 100644 --- a/libraries/AP_HAL_Linux/Semaphores.h +++ b/libraries/AP_HAL_Linux/Semaphores.h @@ -11,7 +11,7 @@ public: bool take(uint32_t timeout_ms); bool take_nonblocking(); private: - bool _taken; + volatile bool _taken; }; #endif // __AP_HAL_LINUX_SEMAPHORE_H__