From f767918e0e1424b99293dff6e708e61f3e66ab89 Mon Sep 17 00:00:00 2001 From: Luiz Ywata Date: Wed, 4 May 2016 16:51:24 -0300 Subject: [PATCH] AP_HAL_PX4: use new I2CDevice interface --- libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h | 3 +- libraries/AP_HAL_PX4/HAL_PX4_Class.cpp | 11 +-- libraries/AP_HAL_PX4/I2CDevice.cpp | 73 ++++++++++++++ libraries/AP_HAL_PX4/I2CDevice.h | 87 +++++++++++++++++ libraries/AP_HAL_PX4/I2CDriver.cpp | 100 -------------------- libraries/AP_HAL_PX4/I2CDriver.h | 47 --------- 6 files changed, 167 insertions(+), 154 deletions(-) create mode 100644 libraries/AP_HAL_PX4/I2CDevice.cpp create mode 100644 libraries/AP_HAL_PX4/I2CDevice.h delete mode 100644 libraries/AP_HAL_PX4/I2CDriver.cpp delete mode 100644 libraries/AP_HAL_PX4/I2CDriver.h diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h index 1642b466a6..8f7ab6c685 100644 --- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h +++ b/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; } diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp index 5fa1a669da..931d2dd4da 100644 --- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp +++ b/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 #include @@ -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 */ diff --git a/libraries/AP_HAL_PX4/I2CDevice.cpp b/libraries/AP_HAL_PX4/I2CDevice.cpp new file mode 100644 index 0000000000..636b9fe66f --- /dev/null +++ b/libraries/AP_HAL_PX4/I2CDevice.cpp @@ -0,0 +1,73 @@ +#include "I2CDevice.h" + +#include + +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 +I2CDeviceManager::get_device(uint8_t bus, uint8_t address) +{ + AP_HAL::OwnPtr i2c { new PX4_I2C(bus) }; + auto dev = AP_HAL::OwnPtr(new I2CDevice(*i2c, address)); + + return dev; +} +} diff --git a/libraries/AP_HAL_PX4/I2CDevice.h b/libraries/AP_HAL_PX4/I2CDevice.h new file mode 100644 index 0000000000..9bad240e68 --- /dev/null +++ b/libraries/AP_HAL_PX4/I2CDevice.h @@ -0,0 +1,87 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#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(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(i2c_mgr); + } + + I2CDeviceManager(); + + /* AP_HAL::I2CDeviceManager implementation */ + AP_HAL::OwnPtr get_device(uint8_t bus, uint8_t address) override; +}; +} diff --git a/libraries/AP_HAL_PX4/I2CDriver.cpp b/libraries/AP_HAL_PX4/I2CDriver.cpp deleted file mode 100644 index 6813620a9c..0000000000 --- a/libraries/AP_HAL_PX4/I2CDriver.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - -#include "I2CDriver.h" -#include -#include -#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, ®) == 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, ®, 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, ®, 1, data, len) ? 0:1; -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_PX4/I2CDriver.h b/libraries/AP_HAL_PX4/I2CDriver.h deleted file mode 100644 index f7039e5434..0000000000 --- a/libraries/AP_HAL_PX4/I2CDriver.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include "AP_HAL_PX4.h" -#include -#include - -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; -};