diff --git a/src/drivers/device/nuttx/I2C.cpp b/src/drivers/device/nuttx/I2C.cpp index 28cd4136c8..cc944f131f 100644 --- a/src/drivers/device/nuttx/I2C.cpp +++ b/src/drivers/device/nuttx/I2C.cpp @@ -51,19 +51,11 @@ namespace device unsigned int I2C::_bus_clocks[BOARD_NUMBER_I2C_BUSES] = BOARD_I2C_BUS_CLOCK_INIT; -I2C::I2C(const char *name, - const char *devname, - int bus, - uint16_t address, - uint32_t frequency) : - // base class +I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency) : CDev(name, devname), - // public - // protected - _retries(0), - _frequency(frequency), - _dev(nullptr) + _frequency(frequency) { + DEVICE_DEBUG("I2C::I2C name = %s devname = %s", name, devname); // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; _device_id.devid_s.bus = bus; @@ -101,7 +93,7 @@ I2C::set_bus_clock(unsigned bus, unsigned clock_hz) int I2C::init() { - int ret = OK; + int ret = PX4_ERROR; unsigned bus_index; // attach to the i2c bus @@ -174,24 +166,21 @@ out: return ret; } -int -I2C::probe() -{ - // Assume the device is too stupid to be discoverable. - return OK; -} - int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { - struct i2c_msg_s msgv[2]; + px4_i2c_msg_t msgv[2]; unsigned msgs; - int ret; + int ret = PX4_ERROR; unsigned retry_count = 0; - do { - // DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + if (_dev == nullptr) { + PX4_ERR("I2C device not opened"); + return 1; + } + do { + DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; if (send_len > 0) { @@ -219,39 +208,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re ret = I2C_TRANSFER(_dev, &msgv[0], msgs); /* success */ - if (ret == OK) { - break; - } - - /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) { - I2C_RESET(_dev); - } - - } while (retry_count++ < _retries); - - return ret; - -} - -int -I2C::transfer(i2c_msg_s *msgv, unsigned msgs) -{ - int ret; - unsigned retry_count = 0; - - /* force the device address and Frequency into the message vector */ - for (unsigned i = 0; i < msgs; i++) { - msgv[i].frequency = _bus_clocks[get_device_address() - 1]; - msgv[i].addr = get_device_address(); - } - - - do { - ret = I2C_TRANSFER(_dev, msgv, msgs); - - /* success */ - if (ret == OK) { + if (ret == PX4_OK) { break; } diff --git a/src/drivers/device/nuttx/I2C.hpp b/src/drivers/device/nuttx/I2C.hpp index 745cab30f4..2c3bafb6b5 100644 --- a/src/drivers/device/nuttx/I2C.hpp +++ b/src/drivers/device/nuttx/I2C.hpp @@ -40,7 +40,7 @@ #ifndef _DEVICE_I2C_H #define _DEVICE_I2C_H -#include "../device.h" +#include "../CDev.hpp" #include @@ -64,7 +64,7 @@ protected: * The number of times a read or write operation will be retried on * error. */ - unsigned _retries; + uint8_t _retries{0}; /** * @ Constructor @@ -75,11 +75,7 @@ protected: * @param address I2C bus address, or zero if set_address will be used * @param frequency I2C bus frequency for the device (currently not used) */ - I2C(const char *name, - const char *devname, - int bus, - uint16_t address, - uint32_t frequency); + I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency); virtual ~I2C(); virtual int init(); @@ -87,7 +83,7 @@ protected: /** * Check for the presence of the device on the bus. */ - virtual int probe(); + virtual int probe() { return PX4_OK; } /** * Perform an I2C transaction to the device. @@ -103,21 +99,11 @@ protected: */ int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); - /** - * Perform a multi-part I2C transaction to the device. - * - * @param msgv An I2C message vector. - * @param msgs The number of entries in the message vector. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int transfer(px4_i2c_msg_t *msgv, unsigned msgs); - bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); } private: - uint32_t _frequency; - px4_i2c_dev_t *_dev; + uint32_t _frequency{0}; + px4_i2c_dev_t *_dev{nullptr}; I2C(const device::I2C &); I2C operator=(const device::I2C &); diff --git a/src/drivers/device/posix/I2C.cpp b/src/drivers/device/posix/I2C.cpp index d0f90a6b1f..2300f6265e 100644 --- a/src/drivers/device/posix/I2C.cpp +++ b/src/drivers/device/posix/I2C.cpp @@ -46,9 +46,6 @@ #include #include #endif -#include -#include -#include #ifdef __PX4_QURT #define PX4_SIMULATE_I2C 1 @@ -62,18 +59,8 @@ static constexpr const int simulate = PX4_SIMULATE_I2C; namespace device { -I2C::I2C(const char *name, - const char *devname, - int bus, - uint16_t address, - uint32_t frequency) : - // base class - CDev(name, devname), - // public - // protected - _retries(0), - // private - _fd(-1) +I2C::I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency) : + CDev(name, devname) { DEVICE_DEBUG("I2C::I2C name = %s devname = %s", name, devname); // fill in _device_id fields for a I2C device @@ -89,7 +76,7 @@ I2C::~I2C() if (_fd >= 0) { #ifndef __PX4_QURT ::close(_fd); -#endif +#endif /* !__PX4_QURT */ _fd = -1; } } @@ -97,13 +84,11 @@ I2C::~I2C() int I2C::init() { - int ret = PX4_OK; - // Assume the driver set the desired bus frequency. There is no standard // way to set it from user space. // do base class init, which will create device node, etc - ret = CDev::init(); + int ret = CDev::init(); if (ret != PX4_OK) { DEVICE_DEBUG("CDev::init failed"); @@ -127,7 +112,7 @@ I2C::init() return PX4_ERROR; } -#endif +#endif /* !__PX4_QURT */ } return ret; @@ -137,12 +122,11 @@ int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { #ifndef __PX4_LINUX - return 1; + return PX4_ERROR; #else struct i2c_msg msgv[2]; unsigned msgs; - struct i2c_rdwr_ioctl_data packets; - int ret; + int ret = PX4_ERROR; unsigned retry_count = 0; if (_fd < 0) { @@ -174,7 +158,10 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re return -EINVAL; } + struct i2c_rdwr_ioctl_data packets; + packets.msgs = msgv; + packets.nmsgs = msgs; if (simulate) { @@ -204,93 +191,4 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re #endif } -int -I2C::transfer(struct i2c_msg *msgv, unsigned msgs) -{ -#ifndef __PX4_LINUX - return 1; -#else - struct i2c_rdwr_ioctl_data packets; - int ret; - unsigned retry_count = 0; - - /* force the device address into the message vector */ - for (unsigned i = 0; i < msgs; i++) { - msgv[i].addr = get_device_address(); - } - - do { - packets.msgs = msgv; - packets.nmsgs = msgs; - - if (simulate) { - DEVICE_DEBUG("I2C SIM: transfer_2 on %s", get_devname()); - ret = PX4_OK; - - } else { - ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); - } - - if (ret < 0) { - DEVICE_DEBUG("I2C transfer failed"); - return 1; - } - - /* success */ - if (ret == PX4_OK) { - break; - } - - } while (retry_count++ < _retries); - - return ret; -#endif -} - -int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; - switch (cmd) { -#ifdef __PX4_LINUX - - case I2C_RDWR: - DEVICE_DEBUG("Use I2C::transfer, not ioctl"); - return 0; -#endif - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - -ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) -{ - if (simulate) { - // FIXME no idea what this should be - DEVICE_DEBUG("2C SIM I2C::read"); - return 0; - } - -#ifndef __PX4_QURT - return ::read(_fd, buffer, buflen); -#else - return 0; -#endif -} - -ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) -{ - if (simulate) { - DEVICE_DEBUG("2C SIM I2C::write"); - return buflen; - } - -#ifndef __PX4_QURT - return ::write(_fd, buffer, buflen); -#else - return buflen; -#endif -} - } // namespace device diff --git a/src/drivers/device/posix/I2C.hpp b/src/drivers/device/posix/I2C.hpp index 517dd9f438..17240fd55e 100644 --- a/src/drivers/device/posix/I2C.hpp +++ b/src/drivers/device/posix/I2C.hpp @@ -43,11 +43,11 @@ #include "../CDev.hpp" #include + #ifdef __PX4_LINUX #include #include #endif -#include namespace device __EXPORT { @@ -65,7 +65,7 @@ protected: * The number of times a read or write operation will be retried on * error. */ - unsigned _retries; + uint8_t _retries{0}; /** * @ Constructor @@ -74,19 +74,17 @@ protected: * @param devname Device node name * @param bus I2C bus on which the device lives * @param address I2C bus address, or zero if set_address will be used + * @param frequency I2C bus frequency for the device (currently not used) */ - I2C(const char *name, - const char *devname, - int bus, - uint16_t address, - uint32_t frequency = 0); + I2C(const char *name, const char *devname, int bus, uint16_t address, uint32_t frequency = 0); virtual ~I2C(); virtual int init(); - virtual ssize_t read(file_t *handlep, char *buffer, size_t buflen); - virtual ssize_t write(file_t *handlep, const char *buffer, size_t buflen); - virtual int ioctl(file_t *handlep, int cmd, unsigned long arg); + /** + * Check for the presence of the device on the bus. + */ + virtual int probe() { return PX4_OK; } /** * Perform an I2C transaction to the device. @@ -102,21 +100,10 @@ protected: */ int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); - /** - * Perform a multi-part I2C transaction to the device. - * - * @param msgv An I2C message vector. - * @param msgs The number of entries in the message vector. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int transfer(struct i2c_msg *msgv, unsigned msgs); - bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); } private: - int _fd; - std::string _dname; + int _fd{-1}; I2C(const device::I2C &); I2C operator=(const device::I2C &);