HAL_PX4: implement I2CDevice driver for PX4

libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

@@ -16,6 +16,7 @@
 #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>
@@ -43,7 +44,7 @@ static PX4AnalogIn analogIn;
 static PX4Util utilInstance;
 static PX4GPIO gpioDriver;
 
-static Empty::I2CDeviceManager i2c_mgr_instance;
+static PX4::I2CDeviceManager i2c_mgr_instance;
 
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"

libraries/AP_HAL_PX4/I2CDevice.cpp

@@ -0,0 +1,80 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "I2CDevice.h"
+
+#include <AP_HAL/AP_HAL.h>
+
+#include "Util.h"
+
+namespace PX4 {
+
+static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
+
+uint8_t PX4::PX4_I2C::instance;
+
+/*
+  implement wrapper for PX4 I2C driver
+ */
+bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len)
+{
+    if (!init_done) {
+        init_done = true;
+        // we do late init() so we can setup the device paths
+        snprintf(devname, sizeof(devname), "AP_I2C_%u", instance);
+        snprintf(devpath, sizeof(devpath), "/dev/api2c%u", instance);
+        init_ok = (init() == OK);
+        if (init_ok) {
+            instance++;
+        }
+    }
+    if (!init_ok) {
+        return false;
+    }
+    set_address(address);
+    bool ret = (transfer(send, send_len, recv, recv_len) == OK);
+    return ret;
+}
+    
+I2CDevice::I2CDevice(uint8_t bus, uint8_t address) :
+    _bus(bus),
+    _address(address)
+{
+}
+    
+I2CDevice::~I2CDevice()
+{
+}
+
+bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
+                         uint8_t *recv, uint32_t recv_len)
+{
+    return _bus.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)
+{
+    return false;
+}
+
+AP_HAL::OwnPtr<AP_HAL::I2CDevice>
+I2CDeviceManager::get_device(uint8_t bus, uint8_t address)
+{
+    auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(bus, address));
+    return dev;
+}
+
+}

libraries/AP_HAL_PX4/I2CDevice.h

@@ -0,0 +1,90 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * Copyright (C) 2015-2016  Intel Corporation. All rights reserved.
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <inttypes.h>
+
+#include <AP_HAL/HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_HAL/utility/OwnPtr.h>
+#include <AP_HAL_Empty/AP_HAL_Empty.h>
+#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
+#include "I2CWrapper.h"
+
+namespace PX4 {
+
+class I2CDevice : public AP_HAL::I2CDevice {
+public:
+    static I2CDevice *from(AP_HAL::I2CDevice *dev)
+    {
+        return static_cast<I2CDevice*>(dev);
+    }
+
+    I2CDevice(uint8_t bus, uint8_t 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; }
+
+    /* 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::register_periodic_callback() */
+    AP_HAL::Device::PeriodicHandle *register_periodic_callback(
+        uint32_t period_usec, AP_HAL::MemberProc) override
+    {
+        /* Not implemented yet */
+        return nullptr;
+    };
+
+    // this makes no sense on PX4 
+    int get_fd() override { return -1; }
+
+    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 _bus;
+    uint8_t _address;
+    uint8_t _retries = 0;
+};
+
+class I2CDeviceManager : public AP_HAL::I2CDeviceManager {
+public:
+    friend class I2CDevice;
+
+    static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr)
+    {
+        return static_cast<I2CDeviceManager*>(i2c_mgr);
+    }
+
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> get_device(uint8_t bus, uint8_t address) override;
+};
+
+}

libraries/AP_HAL_PX4/I2CDriver.cpp

@@ -3,42 +3,12 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 
 #include "I2CDriver.h"
-#include <drivers/device/i2c.h>
+#include "I2CWrapper.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)
 {

libraries/AP_HAL_PX4/I2CWrapper.h

@@ -0,0 +1,28 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+
+#include <arch/board/board.h>
+#include "board_config.h"
+#include <drivers/device/i2c.h>
+#include "AP_HAL_PX4.h"
+
+extern const AP_HAL::HAL& hal;
+/*
+  wrapper class for I2C to expose protected functions from PX4Firmware
+ */
+class PX4::PX4_I2C : public device::I2C {
+public:
+    PX4_I2C(uint8_t bus) : I2C(devname, devpath, bus, 0, 400000UL) { }
+    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len);
+
+private:
+    static uint8_t instance;
+    bool init_done;
+    bool init_ok;
+    char devname[10];
+    char devpath[14];
+};
+
+
+#endif // CONFIG_HAL_BOARD