AP_HAL_Linux: initial I2C driver implementation

this works sufficiently for the HMC5883

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;

libraries/AP_HAL_Linux/I2CDriver.cpp

@@ -2,27 +2,146 @@
 #include <AP_HAL.h>
 #include "I2CDriver.h"
 
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <linux/i2c-dev.h>
+
 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, &reg, 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;
+}

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__

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;
 }

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__