diff --git a/src/drivers/blinkm/module.mk b/src/drivers/blinkm/module.mk index c906733175..712f266317 100644 --- a/src/drivers/blinkm/module.mk +++ b/src/drivers/blinkm/module.mk @@ -37,6 +37,10 @@ MODULE_COMMAND = blinkm -SRCS = blinkm.cpp +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = blinkm_nuttx.cpp +else +SRCS = blinkm_linux.cpp +endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp index 52f336b3a5..1a3492c8f2 100644 --- a/src/drivers/device/i2c_linux.cpp +++ b/src/drivers/device/i2c_linux.cpp @@ -41,17 +41,19 @@ */ #include "i2c.h" +#include +#include +#include +#include +#include namespace device { -unsigned int I2C::_bus_clocks[3] = { 100000, 100000, 100000 }; - I2C::I2C(const char *name, const char *devname, int bus, - uint16_t address, - uint32_t frequency) : + uint16_t address) : // base class CDev(name, devname), // public @@ -60,8 +62,8 @@ I2C::I2C(const char *name, // private _bus(bus), _address(address), - _frequency(frequency), - _dev(nullptr) + _fd(-1), + _dname(devname) { // fill in _device_id fields for a I2C device _device_id.devid_s.bus_type = DeviceBusType_I2C; @@ -73,162 +75,98 @@ I2C::I2C(const char *name, I2C::~I2C() { - if (_dev) { - px4_i2cuninitialize(_dev); - _dev = nullptr; - } -} - -int -I2C::set_bus_clock(unsigned bus, unsigned clock_hz) -{ - int index = bus - 1; - - if (index < 0 || index >= static_cast(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) { - return -EINVAL; - } - - if (_bus_clocks[index] > 0) { - // debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); + if (_fd >= 0) { + ::close(_fd); + _fd = -1; } - _bus_clocks[index] = clock_hz; - - return PX4_OK; } int I2C::init() { int ret = PX4_OK; - unsigned bus_index; - - // attach to the i2c bus - _dev = px4_i2cinitialize(_bus); - - if (_dev == nullptr) { - debug("failed to init I2C"); - ret = -ENOENT; - goto out; - } - - // the above call fails for a non-existing bus index, - // so the index math here is safe. - bus_index = _bus - 1; - - // abort if the max frequency we allow (the frequency we ask) - // is smaller than the bus frequency - if (_bus_clocks[bus_index] > _frequency) { - (void)px4_i2cuninitialize(_dev); - _dev = nullptr; - log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", - _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); - ret = -EINVAL; - goto out; - } - // set the bus frequency on the first access if it has - // not been set yet - if (_bus_clocks[bus_index] == 0) { - _bus_clocks[bus_index] = _frequency; - } - - // set frequency for this instance once to the bus speed - // the bus speed is the maximum supported by all devices on the bus, - // as we have to prioritize performance over compatibility. - // If a new device requires a lower clock speed, this has to be - // manually set via "fmu i2c " before starting any - // drivers. - // This is necessary as automatically lowering the bus speed - // for maximum compatibility could induce timing issues on - // critical sensors the adopter might be unaware of. - I2C_SETFREQUENCY(_dev, _bus_clocks[bus_index]); - - // call the probe function to check whether the device is present - ret = probe(); - - if (ret != PX4_OK) { - debug("probe failed"); - goto out; - } + // 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(); if (ret != PX4_OK) { debug("cdev init failed"); - goto out; + return ret; } - // tell the world where we are - log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", - _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); + _fd = ::open(_dname.c_str(), O_RDWR); + if (_fd < 0) { + warnx("could not open %s", _dname.c_str()); + return -errno; + } -out: - if ((ret != PX4_OK) && (_dev != nullptr)) { - px4_i2cuninitialize(_dev); - _dev = nullptr; - } return ret; } -int -I2C::probe() -{ - // Assume the device is too stupid to be discoverable. - return PX4_OK; -} - int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { - px4_i2c_msg_t msgv[2]; + struct i2c_msg msgv[2]; unsigned msgs; + struct i2c_rdwr_ioctl_data packets; int ret; unsigned retry_count = 0; do { // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); - msgs = 0; if (send_len > 0) { msgv[msgs].addr = _address; msgv[msgs].flags = 0; - msgv[msgs].buffer = const_cast(send); - msgv[msgs].length = send_len; + msgv[msgs].buf = const_cast(send); + msgv[msgs].len = send_len; msgs++; } if (recv_len > 0) { msgv[msgs].addr = _address; msgv[msgs].flags = I2C_M_READ; - msgv[msgs].buffer = recv; - msgv[msgs].length = recv_len; + msgv[msgs].buf = recv; + msgv[msgs].len = recv_len; msgs++; } if (msgs == 0) return -EINVAL; - ret = I2C_TRANSFER(_dev, &msgv[0], msgs); + packets.msgs = msgv; + packets.nmsgs = msgs; + + ret = ::ioctl(_fd, I2C_RDWR, &packets); + if (ret < 0) { + warnx("I2C transfer failed"); + return 1; + } /* success */ if (ret == PX4_OK) break; +// No way to reset device from userspace +#if 0 /* if we have already retried once, or we are going to give up, then reset the bus */ if ((retry_count >= 1) || (retry_count >= _retries)) px4_i2creset(_dev); +#endif } while (retry_count++ < _retries); return ret; - } int -I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs) +I2C::transfer(struct i2c_msg *msgv, unsigned msgs) { + struct i2c_rdwr_ioctl_data packets; int ret; unsigned retry_count = 0; @@ -236,17 +174,26 @@ I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs) for (unsigned i = 0; i < msgs; i++) msgv[i].addr = _address; - do { - ret = I2C_TRANSFER(_dev, msgv, msgs); + packets.msgs = msgv; + packets.nmsgs = msgs; + + ret = ::ioctl(_fd, I2C_RDWR, &packets); + if (ret < 0) { + warnx("I2C transfer failed"); + return 1; + } /* success */ if (ret == PX4_OK) break; +// No way to reset device from userspace +#if 0 /* if we have already retried once, or we are going to give up, then reset the bus */ if ((retry_count >= 1) || (retry_count >= _retries)) px4_i2creset(_dev); +#endif } while (retry_count++ < _retries); diff --git a/src/drivers/device/i2c_linux.h b/src/drivers/device/i2c_linux.h index 3de6c96586..56eaea4b96 100644 --- a/src/drivers/device/i2c_linux.h +++ b/src/drivers/device/i2c_linux.h @@ -43,6 +43,9 @@ #include "device.h" #include +#include +#include +#include namespace device __EXPORT { @@ -60,10 +63,6 @@ public: */ int16_t get_address() const { return _address; } - static int set_bus_clock(unsigned bus, unsigned clock_hz); - - static unsigned int _bus_clocks[3]; - protected: /** * The number of times a read or write operation will be retried on @@ -89,16 +88,16 @@ protected: I2C(const char *name, const char *devname, int bus, - uint16_t address, - uint32_t frequency); + uint16_t address); virtual ~I2C(); virtual int init(); - +#if 0 /** * Check for the presence of the device on the bus. */ virtual int probe(); +#endif /** * Perform an I2C transaction to the device. @@ -123,8 +122,9 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - int transfer(px4_i2c_msg_t *msgv, unsigned msgs); + int transfer(struct i2c_msg *msgv, unsigned msgs); +#if 0 /** * Change the bus address. * @@ -137,11 +137,12 @@ protected: _address = address; _device_id.devid_s.address = _address; } +#endif private: uint16_t _address; - uint32_t _frequency; - px4_i2c_dev_t *_dev; + int _fd; + std::string _dname; I2C(const device::I2C&); I2C operator=(const device::I2C&); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 733fafbd87..dd67bc5d13 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -74,13 +74,17 @@ public: virtual int dev_read(unsigned offset, void *data, unsigned count); virtual int dev_ioctl(unsigned operation, unsigned &arg); +#ifdef __PX4_NUTTX protected: virtual int probe(); +#endif private: ms5611::prom_u &_prom; +#ifdef __PX4_NUTTX int _probe_address(uint8_t address); +#endif /** * Send a reset command to the MS5611. @@ -112,7 +116,11 @@ MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) } MS5611_I2C::MS5611_I2C(uint8_t bus, ms5611::prom_u &prom) : - I2C("MS5611_I2C", nullptr, bus, 0, 400000), + I2C("MS5611_I2C", nullptr, bus, 0 +#ifdef __PX4_NUTTX +, 400000 +#endif +), _prom(prom) { } @@ -172,6 +180,7 @@ MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg) return ret; } +#ifdef __PX4_NUTTX int MS5611_I2C::probe() { @@ -206,6 +215,7 @@ MS5611_I2C::_probe_address(uint8_t address) return PX4_OK; } +#endif int