AP_HAL_PX4: use new I2CDevice interface

libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h

@@ -12,7 +12,8 @@ namespace PX4 {
         class PX4GPIO;
         class PX4DigitalSource;
         class NSHShellStream;
-        class PX4I2CDriver;
+        class I2CDevice;
+        class I2CDeviceManager;
         class PX4_I2C;
         class Semaphore;
 }

libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

@@ -15,7 +15,7 @@
 #include "AnalogIn.h"
 #include "Util.h"
 #include "GPIO.h"
-#include "I2CDriver.h"
+#include "I2CDevice.h"
 
 #include <AP_HAL_Empty/AP_HAL_Empty.h>
 #include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
@@ -31,7 +31,6 @@
 
 using namespace PX4;
 
-static PX4I2CDriver i2cDriver;
 static Empty::SPIDeviceManager spiDeviceManager;
 //static Empty::GPIO gpioDriver;
 
@@ -43,7 +42,7 @@ static PX4AnalogIn analogIn;
 static PX4Util utilInstance;
 static PX4GPIO gpioDriver;
 
-static Empty::I2CDeviceManager i2c_mgr_instance;
+static I2CDeviceManager i2c_mgr_instance;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
@@ -85,9 +84,9 @@ HAL_PX4::HAL_PX4() :
         &uartEDriver,  /* uartE */
         &uartFDriver,  /* uartF */
         &i2c_mgr_instance,
-        &i2cDriver, /* i2c */
-        NULL,   /* only one i2c */
-        NULL,   /* only one i2c */
+        NULL,   /* use i2c_mgr_instance */
+        NULL,   /* use i2c_mgr_instance */
+        NULL,   /* use i2c_mgr_instance */
         &spiDeviceManager, /* spi */
         &analogIn, /* analogin */
         &storageDriver, /* storage */

libraries/AP_HAL_PX4/I2CDevice.cpp

@@ -0,0 +1,73 @@
+#include "I2CDevice.h"
+
+#include <AP_HAL/AP_HAL.h>
+
+namespace PX4 {
+
+class PX4_I2C : public device::I2C {
+public:
+    PX4_I2C(uint8_t bus);
+    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len,
+                     uint8_t *recv, uint32_t recv_len);
+};
+
+PX4_I2C::PX4_I2C(uint8_t bus) :
+    I2C("AP_I2C", "/dev/api2c", bus, 0, 400000UL)
+{
+    init();
+}
+
+bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len,
+                          uint8_t *recv, uint32_t recv_len)
+{
+    set_address(address);
+    return transfer(send, send_len, recv, recv_len) == OK;
+}
+
+bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
+                         uint8_t *recv, uint32_t recv_len)
+{
+    return _device.do_transfer(_address, send, send_len, recv, recv_len);
+}
+
+bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
+                                        uint32_t recv_len, uint8_t times)
+{
+    while (times > 0) {
+        if (!transfer(nullptr, 0, recv, recv_len)) {
+            return false;
+        }
+
+        recv += recv_len;
+        times--;
+    }
+    return true;
+}
+
+int I2CDevice::get_fd()
+{
+    return -1;
+}
+
+AP_HAL::Semaphore *I2CDevice::get_semaphore()
+{
+    return &semaphore;
+}
+
+I2CDevice::~I2CDevice()
+{
+}
+
+I2CDeviceManager::I2CDeviceManager()
+{
+}
+
+AP_HAL::OwnPtr<AP_HAL::I2CDevice>
+I2CDeviceManager::get_device(uint8_t bus, uint8_t address)
+{
+    AP_HAL::OwnPtr<PX4_I2C> i2c { new PX4_I2C(bus) };
+    auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(*i2c, address));
+
+    return dev;
+}
+}

libraries/AP_HAL_PX4/I2CDevice.h

@@ -0,0 +1,87 @@
+#pragma once
+
+#include <vector>
+
+#include <AP_HAL/utility/OwnPtr.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
+#include <drivers/device/i2c.h>
+#include <arch/board/board.h>
+#include "AP_HAL_PX4.h"
+#include "board_config.h"
+
+namespace PX4 {
+
+class I2CDevice : public AP_HAL::I2CDevice {
+public:
+    static I2CDevice *from(AP_HAL::I2CDevice *dev)
+    {
+        return static_cast<I2CDevice*>(dev);
+    }
+
+    /* AP_HAL::I2CDevice implementation */
+    I2CDevice(PX4_I2C &device, uint8_t address)
+        : _device(device)
+        , _address(address)
+    {
+    }
+
+    ~I2CDevice();
+
+    /* See AP_HAL::I2CDevice::set_address() */
+    void set_address(uint8_t address) override { _address = address; }
+
+    /* See AP_HAL::I2CDevice::set_retries() */
+    void set_retries(uint8_t retries) override { _retries = retries; }
+
+    /* AP_HAL::Device implementation */
+
+    /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */
+    bool set_speed(enum Device::Speed speed) override { return true; }
+
+    /* See AP_HAL::Device::transfer() */
+    bool transfer(const uint8_t *send, uint32_t send_len,
+                  uint8_t *recv, uint32_t recv_len) override;
+
+    bool read_registers_multiple(uint8_t first_reg, uint8_t *recv,
+                                 uint32_t recv_len, uint8_t times) override;
+
+    /* See AP_HAL::Device::get_semaphore() */
+    AP_HAL::Semaphore *get_semaphore() override;
+
+    /* See AP_HAL::Device::register_periodic_callback() */
+    AP_HAL::Device::PeriodicHandle *register_periodic_callback(uint32_t period_usec, AP_HAL::MemberProc) override
+    {
+        /* Not implemented yet */
+        return nullptr;
+    };
+
+    /* See AP_HAL::Device::get_fd() */
+    int get_fd() override;
+
+protected:
+    PX4_I2C &_device;
+    uint8_t _address;
+    uint8_t _retries = 0;
+
+private:
+    // we use an empty semaphore as the underlying I2C class already has a semaphore
+    Empty::Semaphore semaphore;
+};
+
+class I2CDeviceManager : public AP_HAL::I2CDeviceManager {
+public:
+    friend class I2CDevice;
+
+    static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr)
+    {
+        return static_cast<I2CDeviceManager*>(i2c_mgr);
+    }
+
+    I2CDeviceManager();
+
+    /* AP_HAL::I2CDeviceManager implementation */
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> get_device(uint8_t bus, uint8_t address) override;
+};
+}

libraries/AP_HAL_PX4/I2CDriver.cpp

@@ -1,100 +0,0 @@
-#include <AP_HAL/AP_HAL.h>
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-
-#include "I2CDriver.h"
-#include <drivers/device/i2c.h>
-#include <arch/board/board.h>
-#include "board_config.h"
-
-extern const AP_HAL::HAL& hal;
-
-using namespace PX4;
-
-/*
-  wrapper class for I2C to expose protected functions from PX4Firmware
- */
-class PX4::PX4_I2C : public device::I2C {
-public:
-    PX4_I2C(uint8_t bus);
-    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len);
-};
-
-// constructor
-PX4_I2C::PX4_I2C(uint8_t bus) : 
-    I2C("AP_I2C", "/dev/api2c", bus, 0, 400000UL)
-{
-    /*
-      attach to the bus. Return value can be ignored as we have no probe function
-    */
-    init();
-}
-
-/*
-  main transfer function
- */
-bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
-{
-    set_address(address);
-    return transfer(send, send_len, recv, recv_len) == OK;
-}
-
-// constructor for main i2c class
-PX4I2CDriver::PX4I2CDriver(void)
-{
-    px4_i2c = new PX4_I2C(PX4_I2C_BUS_EXPANSION);
-    if (px4_i2c == nullptr) {
-        AP_HAL::panic("Unable to allocate PX4 I2C driver");
-    }
-}
-
-/*
-  expose transfer function
- */
-bool PX4I2CDriver::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
-{
-    return px4_i2c->do_transfer(address, send, send_len, recv, recv_len);
-}
-
-/* write: for i2c devices which do not obey register conventions */
-uint8_t PX4I2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data)
-{
-    return do_transfer(addr, data, len, nullptr, 0) ? 0:1;
-}
-
-/* writeRegister: write a single 8-bit value to a register */
-uint8_t PX4I2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val)
-{
-    uint8_t d[2] = { reg, val };
-    return do_transfer(addr, d, sizeof(d), nullptr, 0) ? 0:1;
-}
-
-/* writeRegisters: write bytes to contigious registers */
-uint8_t PX4I2CDriver::writeRegisters(uint8_t addr, uint8_t reg,
-                                     uint8_t len, uint8_t* data)
-{
-    return write(addr, 1, &reg) == 0 && write(addr, len, data) == 0;
-}
-
-/* read: for i2c devices which do not obey register conventions */
-uint8_t PX4I2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
-{
-    return do_transfer(addr, nullptr, 0, data, len) ? 0:1;
-}
-    
-/* readRegister: read from a device register - writes the register,
- * then reads back an 8-bit value. */
-uint8_t PX4I2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data)
-{
-    return do_transfer(addr, &reg, 1, data, 1) ? 0:1;
-}
-
-/* readRegister: read contigious device registers - writes the first 
- * register, then reads back multiple bytes */
-uint8_t PX4I2CDriver::readRegisters(uint8_t addr, uint8_t reg,
-                                    uint8_t len, uint8_t* data)
-{
-    return do_transfer(addr, &reg, 1, data, len) ? 0:1;
-}
-
-#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_PX4/I2CDriver.h

@@ -1,47 +0,0 @@
-#pragma once
-
-#include "AP_HAL_PX4.h"
-#include <AP_HAL_Empty/AP_HAL_Empty.h>
-#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
-
-class PX4::PX4I2CDriver : public AP_HAL::I2CDriver {
-public:
-    PX4I2CDriver(void);
-
-    void begin() override {}
-    void end() override {}
-    void setTimeout(uint16_t ms) override {}
-    void setHighSpeed(bool active) override {}
-
-    // main transfer function
-    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) override;
-
-    /* write: for i2c devices which do not obey register conventions */
-    uint8_t write(uint8_t addr, uint8_t len, uint8_t* data) override;
-
-    /* writeRegister: write a single 8-bit value to a register */
-    uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val) override;
-
-    /* writeRegisters: write bytes to contigious registers */
-    uint8_t writeRegisters(uint8_t addr, uint8_t reg, uint8_t len, uint8_t* data) override;
-
-    /* read: for i2c devices which do not obey register conventions */
-    uint8_t read(uint8_t addr, uint8_t len, uint8_t* data) override;
-    
-    /* readRegister: read from a device register - writes the register,
-     * then reads back an 8-bit value. */
-    uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data) override;
-
-    /* readRegister: read contigious device registers - writes the first 
-     * register, then reads back multiple bytes */
-    uint8_t readRegisters(uint8_t addr, uint8_t reg, uint8_t len, uint8_t* data) override;
-    
-    uint8_t lockup_count() override { return 0; }
-
-    AP_HAL::Semaphore* get_semaphore() override { return &semaphore; }
-
-private:
-    // we use an empty semaphore as the underlying I2C class already has a semaphore
-    Empty::Semaphore semaphore;
-    PX4_I2C *px4_i2c = nullptr;
-};