From 53595bac0ebbac72d74eea23dd571b6b20c491ef Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 16 Nov 2017 02:42:32 -0500 Subject: [PATCH] board support add px4_i2c_bus_external/px4_spi_bus_external --- cmake/common/px4_base.cmake | 1 + src/drivers/batt_smbus/batt_smbus.cpp | 8 +-- src/drivers/bmi055/bmi055.hpp | 22 --------- src/drivers/bmi055/bmi055_accel.cpp | 2 +- src/drivers/bmi055/bmi055_gyro.cpp | 2 +- src/drivers/bmi160/bmi160.cpp | 4 +- src/drivers/bmi160/bmi160.hpp | 14 ------ src/drivers/bmm150/bmm150.cpp | 23 +++------ src/drivers/bmm150/bmm150.hpp | 4 +- src/drivers/boards/common/board_common.h | 40 +++++++++++++++ src/drivers/boards/px4fmu-v2/board_config.h | 1 - src/drivers/boards/px4fmu-v3/CMakeLists.txt | 1 + src/drivers/boards/px4fmu-v3/px4fmu_i2c.c | 55 +++++++++++++++++++++ src/drivers/boards/px4fmu-v3/px4fmu_spi.c | 15 ++++++ src/drivers/boards/px4fmu-v4/board_config.h | 1 - src/drivers/boards/sitl/board_config.h | 1 + src/drivers/bst/bst.cpp | 2 +- src/drivers/device/Device.hpp | 12 +++-- src/drivers/device/nuttx/cdev_platform.hpp | 3 +- src/drivers/device/nuttx/i2c_nuttx.cpp | 23 ++++----- src/drivers/device/nuttx/i2c_nuttx.h | 28 +---------- src/drivers/device/nuttx/spi.cpp | 7 ++- src/drivers/device/posix/cdev_platform.cpp | 2 +- src/drivers/device/posix/i2c_posix.cpp | 13 +++-- src/drivers/device/posix/i2c_posix.h | 28 +---------- src/drivers/device/spi.h | 4 +- src/drivers/fxas21002c/fxas21002c.cpp | 15 +----- src/drivers/fxos8701cq/fxos8701cq.cpp | 17 +------ src/drivers/hmc5883/hmc5883_i2c.cpp | 13 +---- src/drivers/l3gd20/l3gd20.cpp | 15 +----- src/drivers/lis3mdl/lis3mdl_i2c.cpp | 7 +-- src/drivers/ll40ls/LidarLiteI2C.cpp | 3 +- src/drivers/ll40ls/LidarLiteI2C.h | 3 -- src/drivers/lsm303d/lsm303d.cpp | 17 +------ src/drivers/mb12xx/mb12xx.cpp | 10 ++-- src/drivers/md25/md25.cpp | 12 ++--- src/drivers/md25/md25_main.cpp | 2 +- src/drivers/mkblctrl/mkblctrl.cpp | 8 +-- src/drivers/mpl3115a2/mpl3115a2.cpp | 9 +--- src/drivers/mpl3115a2/mpl3115a2_i2c.cpp | 2 +- src/drivers/mpu6000/mpu6000_i2c.cpp | 2 +- src/drivers/mpu6000/mpu6000_spi.cpp | 6 +-- src/drivers/mpu9250/mag_i2c.cpp | 2 +- src/drivers/mpu9250/mpu9250_i2c.cpp | 2 +- src/drivers/mpu9250/mpu9250_spi.cpp | 6 +-- src/drivers/ms5611/ms5611.cpp | 9 +--- src/drivers/ms5611/ms5611_i2c.cpp | 2 +- src/drivers/oreoled/oreoled.cpp | 28 +++++------ src/drivers/sdp3x_airspeed/SDP3X.cpp | 6 +-- src/drivers/sf1xx/sf1xx.cpp | 2 +- src/drivers/srf02/srf02.cpp | 10 ++-- src/drivers/srf02_i2c/srf02_i2c.cpp | 10 ++-- src/drivers/teraranger/teraranger.cpp | 8 +-- src/platforms/qurt/include/i2c.h | 2 + 54 files changed, 232 insertions(+), 312 deletions(-) create mode 100644 src/drivers/boards/px4fmu-v3/px4fmu_i2c.c diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 5c939446e9..430a9c385a 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -382,6 +382,7 @@ function(px4_add_common_flags) set(cxx_warnings -Wno-missing-field-initializers + -Wno-overloaded-virtual # TODO: fix and remove -Wreorder ) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index f9a07a75d4..c90e0ddc1c 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -440,11 +440,11 @@ BATT_SMBUS::search() { bool found_slave = false; uint16_t tmp; - int16_t orig_addr = get_address(); + int16_t orig_addr = get_device_address(); // search through all valid SMBus addresses for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { - set_address(i); + set_device_address(i); if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { warnx("battery found at 0x%x", (int)i); @@ -456,7 +456,7 @@ BATT_SMBUS::search() } // restore original i2c address - set_address(orig_addr); + set_device_address(orig_addr); // display completion message if (found_slave) { @@ -869,7 +869,7 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len } uint8_t tmp_buff[tmp_buff_len]; - tmp_buff[0] = (uint8_t)get_address() << 1; + tmp_buff[0] = (uint8_t)get_device_address() << 1; tmp_buff[1] = cmd; if (reading) { diff --git a/src/drivers/bmi055/bmi055.hpp b/src/drivers/bmi055/bmi055.hpp index a09a673f97..999957748c 100644 --- a/src/drivers/bmi055/bmi055.hpp +++ b/src/drivers/bmi055/bmi055.hpp @@ -251,12 +251,6 @@ /* Mask definitions for Gyro bandwidth */ #define BMI055_GYRO_BW_MASK 0x0F -#ifdef PX4_SPI_BUS_EXT -#define EXTERNAL_BUS PX4_SPI_BUS_EXT -#else -#define EXTERNAL_BUS 0 -#endif - class BMI055 : public device::SPI { @@ -441,14 +435,6 @@ private: */ int set_accel_range(unsigned max_g); - - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_bus == EXTERNAL_BUS); } - /** * Measurement self test * @@ -602,14 +588,6 @@ private: */ int set_gyro_range(unsigned max_dps); - - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_bus == EXTERNAL_BUS); } - /** * Measurement self test * diff --git a/src/drivers/bmi055/bmi055_accel.cpp b/src/drivers/bmi055/bmi055_accel.cpp index 7b6c086151..68d04d7b62 100644 --- a/src/drivers/bmi055/bmi055_accel.cpp +++ b/src/drivers/bmi055/bmi055_accel.cpp @@ -113,7 +113,7 @@ BMI055_accel::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + &_accel_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); diff --git a/src/drivers/bmi055/bmi055_gyro.cpp b/src/drivers/bmi055/bmi055_gyro.cpp index c6f3605a48..affe921278 100644 --- a/src/drivers/bmi055/bmi055_gyro.cpp +++ b/src/drivers/bmi055/bmi055_gyro.cpp @@ -118,7 +118,7 @@ BMI055_gyro::init() _gyro_reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + &_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_gyro_topic == nullptr) { warnx("ADVERT FAIL"); diff --git a/src/drivers/bmi160/bmi160.cpp b/src/drivers/bmi160/bmi160.cpp index ae2066984c..ac5a6f1534 100644 --- a/src/drivers/bmi160/bmi160.cpp +++ b/src/drivers/bmi160/bmi160.cpp @@ -190,7 +190,7 @@ BMI160::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + &_accel_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -202,7 +202,7 @@ BMI160::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + &_gyro->_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); diff --git a/src/drivers/bmi160/bmi160.hpp b/src/drivers/bmi160/bmi160.hpp index 0003864a88..b63529e6d6 100644 --- a/src/drivers/bmi160/bmi160.hpp +++ b/src/drivers/bmi160/bmi160.hpp @@ -244,13 +244,6 @@ #define BMI160_TIMER_REDUCTION 200 -#ifdef PX4_SPI_BUS_EXT -#define EXTERNAL_BUS PX4_SPI_BUS_EXT -#else -#define EXTERNAL_BUS 0 -#endif - - class BMI160_gyro; class BMI160 : public device::SPI @@ -433,13 +426,6 @@ private: */ uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_bus == EXTERNAL_BUS); } - /** * Measurement self test * diff --git a/src/drivers/bmm150/bmm150.cpp b/src/drivers/bmm150/bmm150.cpp index 5ea8b12426..142190de64 100644 --- a/src/drivers/bmm150/bmm150.cpp +++ b/src/drivers/bmm150/bmm150.cpp @@ -77,8 +77,8 @@ void start(bool external_bus, enum Rotation rotation) /* create the driver */ if (external_bus) { -#if defined(PX4_I2C_BUS_BMM150) - *g_dev_ptr = new BMM150(PX4_I2C_BUS_BMM150, path, external_bus, rotation); +#if defined(PX4_I2C_BUS_EXPANSION) + *g_dev_ptr = new BMM150(PX4_I2C_BUS_EXPANSION, path, rotation); #else PX4_ERR("External I2C not available"); exit(0); @@ -242,13 +242,11 @@ usage() } - -} +} // namespace bmm150 -BMM150 :: BMM150(int bus, const char *path, bool external, enum Rotation rotation) : +BMM150 :: BMM150(int bus, const char *path, enum Rotation rotation) : I2C("BMM150", path, bus, BMM150_SLAVE_ADDRESS, BMM150_BUS_SPEED), - _external(false), _running(false), _call_interval(0), _reports(nullptr), @@ -283,7 +281,6 @@ BMM150 :: BMM150(int bus, const char *path, bool external, enum Rotation rotatio { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_BMM150; - _external = external; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -294,10 +291,8 @@ BMM150 :: BMM150(int bus, const char *path, bool external, enum Rotation rotatio _scale.y_scale = 1.0f; _scale.z_offset = 0; _scale.z_scale = 1.0f; - } - BMM150 :: ~BMM150() { /* make sure we are truly inactive */ @@ -383,7 +378,7 @@ int BMM150::init() /* measurement will have generated a report, publish */ _topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrb, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + &_orb_class_instance, (external()) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_topic == nullptr) { PX4_WARN("ADVERT FAIL"); @@ -682,7 +677,7 @@ BMM150::collect() mrb.timestamp = hrt_absolute_time(); - mrb.is_external = is_external(); + mrb.is_external = external(); // report the error count as the number of bad transfers. // This allows the higher level code to decide if it @@ -877,12 +872,6 @@ BMM150::self_test() return (perf_event_count(_sample_perf) > 0) ? 0 : 1; } -bool BMM150::is_external() -{ - return _external; -} - - uint8_t BMM150::read_reg(uint8_t reg) { diff --git a/src/drivers/bmm150/bmm150.hpp b/src/drivers/bmm150/bmm150.hpp index 0ab1295f1a..14cdb9da46 100644 --- a/src/drivers/bmm150/bmm150.hpp +++ b/src/drivers/bmm150/bmm150.hpp @@ -195,10 +195,9 @@ struct bmm150_data { class BMM150 : public device::I2C { public: - BMM150(int bus, const char *path, bool external, enum Rotation rotation); + BMM150(int bus, const char *path, enum Rotation rotation); virtual ~BMM150(); - bool is_external(); virtual int init(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); @@ -220,7 +219,6 @@ protected: private: work_s _work{}; - bool _external; bool _running; diff --git a/src/drivers/boards/common/board_common.h b/src/drivers/boards/common/board_common.h index dd10c1b50f..822b2b11b1 100644 --- a/src/drivers/boards/common/board_common.h +++ b/src/drivers/boards/common/board_common.h @@ -869,4 +869,44 @@ static inline int board_register_power_state_notification_cb(power_button_state_ static inline int board_shutdown(void) { return -EINVAL; } #endif +/************************************************************************************ + * Name: px4_i2c_bus_external + * + ************************************************************************************/ + +#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) + +__EXPORT bool px4_i2c_bus_external(int bus); + +#else + +#ifdef PX4_I2C_BUS_ONBOARD +#define px4_i2c_bus_external(bus) (bus != PX4_I2C_BUS_ONBOARD) +#else +#define px4_i2c_bus_external(bus) true +#endif /* PX4_I2C_BUS_ONBOARD */ + +#endif /* BOARD_HAS_SIMPLE_HW_VERSIONING */ + + +/************************************************************************************ + * Name: px4_spi_bus_external + * + ************************************************************************************/ + +#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) + +__EXPORT bool px4_spi_bus_external(int bus); + +#else + +#ifdef PX4_SPI_BUS_EXT +#define px4_spi_bus_external(bus) (bus == PX4_SPI_BUS_EXT) +#else +#define px4_spi_bus_external(bus) false +#endif /* PX4_SPI_BUS_EXT */ + +#endif /* BOARD_HAS_SIMPLE_HW_VERSIONING */ + + #include "board_internal_common.h" diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 1c9bde1b5f..e7b8e29746 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -424,7 +424,6 @@ extern void board_peripheral_reset(int ms); extern void stm32_usbinitialize(void); - #include "../common/board_common.h" #endif /* __ASSEMBLY__ */ diff --git a/src/drivers/boards/px4fmu-v3/CMakeLists.txt b/src/drivers/boards/px4fmu-v3/CMakeLists.txt index f86b1be728..5eb7676e15 100644 --- a/src/drivers/boards/px4fmu-v3/CMakeLists.txt +++ b/src/drivers/boards/px4fmu-v3/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_module( MODULE drivers__boards__px4fmu-v3 SRCS px4fmu_can.c + px4fmu_i2c.c px4fmu_init.c px4fmu_led.c px4fmu_spi.c diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_i2c.c b/src/drivers/boards/px4fmu-v3/px4fmu_i2c.c new file mode 100644 index 0000000000..55d559976e --- /dev/null +++ b/src/drivers/boards/px4fmu-v3/px4fmu_i2c.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_i2c.c + * + * Board-specific I2C functions. + */ + +#include "board_config.h" + +__EXPORT bool px4_i2c_bus_external(int bus) +{ + if (HW_VER_FMUV3 == board_get_hw_version()) { + /* All FMUV3 2.1 i2c buses are external */ + return true; + + } else { + if (bus != PX4_I2C_BUS_ONBOARD) { + return true; + } + } + + return false; +} diff --git a/src/drivers/boards/px4fmu-v3/px4fmu_spi.c b/src/drivers/boards/px4fmu-v3/px4fmu_spi.c index fde6f586bf..237642a94d 100644 --- a/src/drivers/boards/px4fmu-v3/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v3/px4fmu_spi.c @@ -451,3 +451,18 @@ __EXPORT void board_spi_reset(int ms) stm32_spi1_initialize(); } + +__EXPORT bool px4_spi_bus_external(int bus) +{ + if (HW_VER_FMUV3 == board_get_hw_version()) { + /* all FMUv3 2.1 spi buses are internal */ + return false; + + } else { + if (bus == PX4_SPI_BUS_EXT) { + return true; + } + } + + return false; +} diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index f8cecf77e4..89904ddaa1 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -157,7 +157,6 @@ /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 #define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION -#define PX4_I2C_BUS_BMM150 PX4_I2C_BUS_EXPANSION /* Devices on the external bus. * diff --git a/src/drivers/boards/sitl/board_config.h b/src/drivers/boards/sitl/board_config.h index 11cfd68d54..f7e74a882d 100644 --- a/src/drivers/boards/sitl/board_config.h +++ b/src/drivers/boards/sitl/board_config.h @@ -51,6 +51,7 @@ #define CONFIG_ARCH_BOARD_SITL 1 #define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_ONBOARD 2 #define PX4_NUMBER_I2C_BUSES 1 #include diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp index 3956b058b5..aeca4574df 100644 --- a/src/drivers/bst/bst.cpp +++ b/src/drivers/bst/bst.cpp @@ -273,7 +273,7 @@ void BST::start() _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _battery_sub = orb_subscribe(ORB_ID(battery_status)); - set_address(0x00); // General call address + set_device_address(0x00); // General call address work_queue(LPWORK, &_work, BST::cycle_trampoline, this, 0); } diff --git a/src/drivers/device/Device.hpp b/src/drivers/device/Device.hpp index e4203ed521..9d5e106481 100644 --- a/src/drivers/device/Device.hpp +++ b/src/drivers/device/Device.hpp @@ -137,30 +137,32 @@ public: * * @return The bus ID */ - virtual uint8_t get_device_bus() { return _device_id.devid_s.bus; } + uint8_t get_device_bus() { return _device_id.devid_s.bus; } /** * Return the bus type the device is connected to. * * @return The bus type */ - virtual DeviceBusType get_device_bus_type() { return _device_id.devid_s.bus_type; } + DeviceBusType get_device_bus_type() { return _device_id.devid_s.bus_type; } /** * Return the bus address of the device. * * @return The bus address */ - virtual uint8_t get_device_address() { return _device_id.devid_s.address; } + uint8_t get_device_address() { return _device_id.devid_s.address; } + + void set_device_address(int address) { _device_id.devid_s.address = address; } /** * Set the device type * * @return The device type */ - virtual void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; } + void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; } - //virtual bool external() = 0; + virtual bool external() { return false; } /* broken out device elements. The bitfields are used to keep diff --git a/src/drivers/device/nuttx/cdev_platform.hpp b/src/drivers/device/nuttx/cdev_platform.hpp index c8f191f99b..11dfc950a9 100644 --- a/src/drivers/device/nuttx/cdev_platform.hpp +++ b/src/drivers/device/nuttx/cdev_platform.hpp @@ -3,9 +3,10 @@ #include -#include #include +#include + #define ATOMIC_ENTER irqstate_t flags = px4_enter_critical_section() #define ATOMIC_LEAVE px4_leave_critical_section(flags) diff --git a/src/drivers/device/nuttx/i2c_nuttx.cpp b/src/drivers/device/nuttx/i2c_nuttx.cpp index e55109fa69..7fcd8e2ded 100644 --- a/src/drivers/device/nuttx/i2c_nuttx.cpp +++ b/src/drivers/device/nuttx/i2c_nuttx.cpp @@ -61,9 +61,6 @@ I2C::I2C(const char *name, // public // protected _retries(0), - // private - _bus(bus), - _address(address), _frequency(frequency), _dev(nullptr) { @@ -108,7 +105,7 @@ I2C::init() unsigned bus_index; // attach to the i2c bus - _dev = px4_i2cbus_initialize(_bus); + _dev = px4_i2cbus_initialize(get_device_bus()); if (_dev == nullptr) { DEVICE_DEBUG("failed to init I2C"); @@ -118,7 +115,7 @@ I2C::init() // the above call fails for a non-existing bus index, // so the index math here is safe. - bus_index = _bus - 1; + bus_index = get_device_bus() - 1; // abort if the max frequency we allow (the frequency we ask) // is smaller than the bus frequency @@ -126,7 +123,7 @@ I2C::init() (void)px4_i2cbus_uninitialize(_dev); _dev = nullptr; DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", - _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); + get_device_bus(), _bus_clocks[bus_index] / 1000, _frequency / 1000); ret = -EINVAL; goto out; } @@ -165,7 +162,7 @@ I2C::init() // tell the world where we are DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", - _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); + get_device_bus(), get_device_address(), _bus_clocks[bus_index] / 1000, _frequency / 1000); out: @@ -198,8 +195,8 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re msgs = 0; if (send_len > 0) { - msgv[msgs].frequency = _bus_clocks[_bus - 1]; - msgv[msgs].addr = _address; + msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1]; + msgv[msgs].addr = get_device_address(); msgv[msgs].flags = 0; msgv[msgs].buffer = const_cast(send); msgv[msgs].length = send_len; @@ -207,8 +204,8 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } if (recv_len > 0) { - msgv[msgs].frequency = _bus_clocks[_bus - 1]; - msgv[msgs].addr = _address; + msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1]; + msgv[msgs].addr = get_device_address(); msgv[msgs].flags = I2C_M_READ; msgv[msgs].buffer = recv; msgv[msgs].length = recv_len; @@ -245,8 +242,8 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) /* force the device address and Frequency into the message vector */ for (unsigned i = 0; i < msgs; i++) { - msgv[i].frequency = _bus_clocks[_bus - 1]; - msgv[i].addr = _address; + msgv[i].frequency = _bus_clocks[get_device_address() - 1]; + msgv[i].addr = get_device_address(); } diff --git a/src/drivers/device/nuttx/i2c_nuttx.h b/src/drivers/device/nuttx/i2c_nuttx.h index 9a2a2042a7..745cab30f4 100644 --- a/src/drivers/device/nuttx/i2c_nuttx.h +++ b/src/drivers/device/nuttx/i2c_nuttx.h @@ -55,11 +55,6 @@ class __EXPORT I2C : public CDev public: - /** - * Get the address - */ - int16_t get_address() const { return _address; } - static int set_bus_clock(unsigned bus, unsigned clock_hz); static unsigned int _bus_clocks[BOARD_NUMBER_I2C_BUSES]; @@ -71,11 +66,6 @@ protected: */ unsigned _retries; - /** - * The I2C bus number the device is attached to. - */ - int _bus; - /** * @ Constructor * @@ -111,8 +101,7 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); /** * Perform a multi-part I2C transaction to the device. @@ -124,22 +113,9 @@ protected: */ int transfer(px4_i2c_msg_t *msgv, unsigned msgs); - /** - * Change the bus address. - * - * Most often useful during probe() when the driver is testing - * several possible bus addresses. - * - * @param address The new bus address to set. - */ - void set_address(uint16_t address) - { - _address = address; - _device_id.devid_s.address = _address; - } + bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); } private: - uint16_t _address; uint32_t _frequency; px4_i2c_dev_t *_dev; diff --git a/src/drivers/device/nuttx/spi.cpp b/src/drivers/device/nuttx/spi.cpp index 945e928f3c..f0ba376fe7 100644 --- a/src/drivers/device/nuttx/spi.cpp +++ b/src/drivers/device/nuttx/spi.cpp @@ -71,8 +71,7 @@ SPI::SPI(const char *name, _device(device), _mode(mode), _frequency(frequency), - _dev(nullptr), - _bus(bus) + _dev(nullptr) { // fill in _device_id fields for a SPI device _device_id.devid_s.bus_type = DeviceBusType_SPI; @@ -94,7 +93,7 @@ SPI::init() /* attach to the spi bus */ if (_dev == nullptr) { - _dev = px4_spibus_initialize(_bus); + _dev = px4_spibus_initialize(get_device_bus()); } if (_dev == nullptr) { @@ -123,7 +122,7 @@ SPI::init() } /* tell the workd where we are */ - DEVICE_LOG("on SPI bus %d at %d (%u KHz)", _bus, PX4_SPI_DEV_ID(_device), _frequency / 1000); + DEVICE_LOG("on SPI bus %d at %d (%u KHz)", get_device_bus(), PX4_SPI_DEV_ID(_device), _frequency / 1000); out: return ret; diff --git a/src/drivers/device/posix/cdev_platform.cpp b/src/drivers/device/posix/cdev_platform.cpp index fe2c99703b..38b7e5bfe8 100644 --- a/src/drivers/device/posix/cdev_platform.cpp +++ b/src/drivers/device/posix/cdev_platform.cpp @@ -106,7 +106,7 @@ extern "C" { return dev; } - int register_driver(const char *name, const device::px4_file_operations_t *fops, mode_t mode, void *data) + int register_driver(const char *name, const device::px4_file_operations_t *fops, device::mode_t mode, void *data) { PX4_DEBUG("CDev::register_driver %s", name); int ret = 0; diff --git a/src/drivers/device/posix/i2c_posix.cpp b/src/drivers/device/posix/i2c_posix.cpp index 668b5fbd22..0bb53d9c9d 100644 --- a/src/drivers/device/posix/i2c_posix.cpp +++ b/src/drivers/device/posix/i2c_posix.cpp @@ -40,7 +40,8 @@ * that is supplied. Should we just depend on the bus knowing? */ -#include "i2c.h" +#include "i2c_posix.h" + #ifdef __PX4_LINUX #include #include @@ -72,8 +73,6 @@ I2C::I2C(const char *name, // protected _retries(0), // private - _bus(bus), - _address(address), _fd(-1) { DEVICE_DEBUG("I2C::I2C name = %s devname = %s", name, devname); @@ -119,7 +118,7 @@ I2C::init() // Open the actual I2C device char dev_path[16]; - snprintf(dev_path, sizeof(dev_path), "/dev/i2c-%i", _bus); + snprintf(dev_path, sizeof(dev_path), "/dev/i2c-%i", get_device_bus()); _fd = ::open(dev_path, O_RDWR); if (_fd < 0) { @@ -156,7 +155,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re msgs = 0; if (send_len > 0) { - msgv[msgs].addr = _address; + msgv[msgs].addr = get_device_address(); msgv[msgs].flags = 0; msgv[msgs].buf = const_cast(send); msgv[msgs].len = send_len; @@ -164,7 +163,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } if (recv_len > 0) { - msgv[msgs].addr = _address; + msgv[msgs].addr = get_device_address(); msgv[msgs].flags = I2C_M_READ; msgv[msgs].buf = recv; msgv[msgs].len = recv_len; @@ -217,7 +216,7 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) /* force the device address into the message vector */ for (unsigned i = 0; i < msgs; i++) { - msgv[i].addr = _address; + msgv[i].addr = get_device_address(); } do { diff --git a/src/drivers/device/posix/i2c_posix.h b/src/drivers/device/posix/i2c_posix.h index fa77a8505e..517dd9f438 100644 --- a/src/drivers/device/posix/i2c_posix.h +++ b/src/drivers/device/posix/i2c_posix.h @@ -60,11 +60,6 @@ class __EXPORT I2C : public CDev public: - /** - * Get the address - */ - int16_t get_address() const { return _address; } - protected: /** * The number of times a read or write operation will be retried on @@ -72,11 +67,6 @@ protected: */ unsigned _retries; - /** - * The I2C bus number the device is attached to. - */ - int _bus; - /** * @ Constructor * @@ -110,8 +100,7 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); /** * Perform a multi-part I2C transaction to the device. @@ -123,22 +112,9 @@ protected: */ int transfer(struct i2c_msg *msgv, unsigned msgs); - /** - * Change the bus address. - * - * Most often useful during probe() when the driver is testing - * several possible bus addresses. - * - * @param address The new bus address to set. - */ - void set_address(uint16_t address) - { - _address = address; - _device_id.devid_s.address = _address; - } + bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); } private: - uint16_t _address; int _fd; std::string _dname; diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index f7337d0396..1da4401b04 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -142,10 +142,10 @@ private: SPI operator=(const SPI &); protected: - int _bus; - int _transfer(uint8_t *send, uint8_t *recv, unsigned len); + bool external() { return px4_spi_bus_external(get_device_bus()); } + }; } // namespace device diff --git a/src/drivers/fxas21002c/fxas21002c.cpp b/src/drivers/fxas21002c/fxas21002c.cpp index 851745d5de..4bd0f54b98 100644 --- a/src/drivers/fxas21002c/fxas21002c.cpp +++ b/src/drivers/fxas21002c/fxas21002c.cpp @@ -205,12 +205,6 @@ #define FXAS21002C_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */ -#ifdef PX4_SPI_BUS_EXT -#define EXTERNAL_BUS PX4_SPI_BUS_EXT -#else -#define EXTERNAL_BUS 0 -#endif - /* we set the timer interrupt to run a bit faster than the desired sample rate and then throw away duplicates using the data ready bit. @@ -319,13 +313,6 @@ private: */ void disable_i2c(); - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_bus == EXTERNAL_BUS); } - /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -534,7 +521,7 @@ FXAS21002C::init() /* measurement will have generated a report, publish */ _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { PX4_ERR("ADVERT ERR"); diff --git a/src/drivers/fxos8701cq/fxos8701cq.cpp b/src/drivers/fxos8701cq/fxos8701cq.cpp index 94dec63abf..3c23f74718 100644 --- a/src/drivers/fxos8701cq/fxos8701cq.cpp +++ b/src/drivers/fxos8701cq/fxos8701cq.cpp @@ -138,12 +138,6 @@ #define FXOS8701C_MAG_DEFAULT_RATE 100 #define FXOS8701C_ONE_G 9.80665f -#ifdef PX4_SPI_BUS_EXT -#define EXTERNAL_BUS PX4_SPI_BUS_EXT -#else -#define EXTERNAL_BUS 0 -#endif - /* we set the timer interrupt to run a bit faster than the desired sample rate and then throw away duplicates using the data ready bit. @@ -278,13 +272,6 @@ private: */ void disable_i2c(); - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_bus == EXTERNAL_BUS); } - /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -633,7 +620,7 @@ FXOS8701CQ::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { PX4_ERR("ADVERT ERR"); @@ -1551,7 +1538,7 @@ FXOS8701CQ::mag_measure() */ mag_report.timestamp = hrt_absolute_time(); - mag_report.is_external = is_external(); + mag_report.is_external = external(); mag_report.x_raw = _last_raw_mag_x; mag_report.y_raw = _last_raw_mag_y; diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 339115c2fd..81051d4bb0 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -113,18 +113,7 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) switch (operation) { case MAGIOCGEXTERNAL: -#ifdef PX4_I2C_BUS_ONBOARD - if (_bus == PX4_I2C_BUS_ONBOARD) { - return 0; - - } else { - return 1; - } - -#else - /* assume external for all boards that don't define PX4_I2C_BUS_ONBOARD */ - return 1; -#endif + return external(); case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index d26216ec0a..371affa2a9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -178,12 +178,6 @@ #define L3GD20_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */ -#ifdef PX4_SPI_BUS_EXT -#define EXTERNAL_BUS PX4_SPI_BUS_EXT -#else -#define EXTERNAL_BUS 0 -#endif - #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG #endif @@ -288,13 +282,6 @@ private: */ void disable_i2c(); - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_bus == EXTERNAL_BUS); } - /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -493,7 +480,7 @@ L3GD20::init() _reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { DEVICE_DEBUG("failed to create sensor_gyro publication"); diff --git a/src/drivers/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/lis3mdl/lis3mdl_i2c.cpp index 5ef08d7159..388bd5666f 100644 --- a/src/drivers/lis3mdl/lis3mdl_i2c.cpp +++ b/src/drivers/lis3mdl/lis3mdl_i2c.cpp @@ -112,12 +112,7 @@ LIS3MDL_I2C::ioctl(unsigned operation, unsigned &arg) switch (operation) { case MAGIOCGEXTERNAL: - if (_bus == PX4_I2C_BUS_EXPANSION) { - return 1; - - } else { - return 0; - } + external(); case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index b50d80dd49..9760809342 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -68,8 +68,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr _acquire_time_usec(0), _pause_measurements(false), _hw_version(0), - _sw_version(0), - _bus(bus) + _sw_version(0) { // up the retries since the device misses the first measure attempts _retries = 3; diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index c57fec0065..b1446e7c36 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -123,9 +123,6 @@ private: uint8_t _hw_version; uint8_t _sw_version; - /**< the bus the device is connected to */ - int _bus; - /** * LidarLite specific transfer function. This is needed * to avoid a stop transition with SCL high diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 5f277d4486..705b4ff6ce 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -208,12 +208,6 @@ #define LSM303D_ONE_G 9.80665f -#ifdef PX4_SPI_BUS_EXT -#define EXTERNAL_BUS PX4_SPI_BUS_EXT -#else -#define EXTERNAL_BUS 0 -#endif - /* we set the timer interrupt to run a bit faster than the desired sample rate and then throw away duplicates using the data ready bit. @@ -345,13 +339,6 @@ private: */ void disable_i2c(); - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_bus == EXTERNAL_BUS); } - /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -691,7 +678,7 @@ LSM303D::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { warnx("ADVERT ERR"); @@ -1673,7 +1660,7 @@ LSM303D::mag_measure() */ mag_report.timestamp = hrt_absolute_time(); - mag_report.is_external = is_external(); + mag_report.is_external = external(); mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 601bf0d333..26da536ccf 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -256,7 +256,7 @@ MB12XX::init() _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ - set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { return ret; @@ -282,7 +282,7 @@ MB12XX::init() So second iteration it uses i2c address 111, third iteration 110 and so on*/ for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ - set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ int ret2 = measure(); if (ret2 == 0) { /* sonar is present -> store address_index in array */ @@ -293,7 +293,7 @@ MB12XX::init() } _index_counter = MB12XX_BASEADDR; - set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */ /* if only one sonar detected, no special timing is required between firing, so use default */ if (addr_ind.size() == 1) { @@ -647,7 +647,7 @@ MB12XX::cycle() { if (_collect_phase) { _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ - set_address(_index_counter); + set_device_address(_index_counter); /* perform collection */ if (OK != collect()) { @@ -686,7 +686,7 @@ MB12XX::cycle() /* ensure sonar i2c adress is still correct */ _index_counter = addr_ind[_cycle_counter]; - set_address(_index_counter); + set_device_address(_index_counter); /* Perform measurement */ if (OK != measure()) { diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 4a6401f4f2..9bd591607d 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -357,7 +357,7 @@ int MD25::probe() //printf("searching for MD25 address\n"); while (true) { - set_address(testAddress); + set_device_address(testAddress); ret = readData(); if (ret == OK && !found) { @@ -376,11 +376,11 @@ int MD25::probe() } if (found) { - set_address(goodAddress); + set_device_address(goodAddress); return OK; } else { - set_address(0); + set_device_address(0); return ret; } } @@ -395,7 +395,7 @@ int MD25::search() //printf("searching for MD25 address\n"); while (true) { - set_address(testAddress); + set_device_address(testAddress); ret = readData(); if (ret == OK && !found) { @@ -415,11 +415,11 @@ int MD25::search() } if (found) { - set_address(goodAddress); + set_device_address(goodAddress); return OK; } else { - set_address(0); + set_device_address(0); return ret; } } diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index 5b889bf03a..a560458fc4 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -177,7 +177,7 @@ int md25_main(int argc, char *argv[]) int ret = md25.probe(); if (ret == OK) { - printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_address()); + printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_device_address()); } else { printf("MD25 not found on bus %d\n", bus); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 275bd1f13f..b2bb08849b 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -701,7 +701,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) result[1] = 0; result[2] = 0; - set_address(BLCTRL_BASE_ADDR + i); + set_device_address(BLCTRL_BASE_ADDR + i); if (OK == transfer(&msg, 1, &result[0], 3)) { Motor[i].Current = result[0]; @@ -765,7 +765,7 @@ MK::mk_servo_set(unsigned int chan, short val) } //if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) { - set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + set_device_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); if (Motor[chan].Version == BLCTRL_OLD) { /* @@ -902,7 +902,7 @@ MK::mk_servo_test(unsigned int chan) msg[1] = Motor[chan].SetPointLowerBits; } - set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + set_device_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); if (Motor[chan].Version == BLCTRL_OLD) { ret = transfer(&msg[0], 1, nullptr, 0); @@ -928,7 +928,7 @@ MK::mk_servo_locate() if (hrt_absolute_time() - last_timestamp > MOTOR_LOCATE_DELAY) { last_timestamp = hrt_absolute_time(); - set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + set_device_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); chan++; if (chan <= _num_outputs) { diff --git a/src/drivers/mpl3115a2/mpl3115a2.cpp b/src/drivers/mpl3115a2/mpl3115a2.cpp index 654b692bf0..ed64cae7fe 100644 --- a/src/drivers/mpl3115a2/mpl3115a2.cpp +++ b/src/drivers/mpl3115a2/mpl3115a2.cpp @@ -174,13 +174,6 @@ protected: */ void cycle(); - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } - /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. @@ -324,7 +317,7 @@ MPL3115A2::init() ret = OK; _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == nullptr) { warnx("failed to create sensor_baro publication"); diff --git a/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp b/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp index 664635f457..aa24ec328f 100644 --- a/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp +++ b/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp @@ -107,7 +107,7 @@ int MPL3115A2_I2C::init() { /* this will call probe() */ - set_address(MPL3115A2_ADDRESS); + set_device_address(MPL3115A2_ADDRESS); return I2C::init(); } diff --git a/src/drivers/mpu6000/mpu6000_i2c.cpp b/src/drivers/mpu6000/mpu6000_i2c.cpp index 884e521296..6693b939ac 100644 --- a/src/drivers/mpu6000/mpu6000_i2c.cpp +++ b/src/drivers/mpu6000/mpu6000_i2c.cpp @@ -116,7 +116,7 @@ MPU6000_I2C::ioctl(unsigned operation, unsigned &arg) switch (operation) { case ACCELIOCGEXTERNAL: - return _bus == PX4_I2C_BUS_EXPANSION ? 1 : 0; + return external(); case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); diff --git a/src/drivers/mpu6000/mpu6000_spi.cpp b/src/drivers/mpu6000/mpu6000_spi.cpp index d61560fe5d..99a70a2d8f 100644 --- a/src/drivers/mpu6000/mpu6000_spi.cpp +++ b/src/drivers/mpu6000/mpu6000_spi.cpp @@ -240,11 +240,7 @@ MPU6000_SPI::ioctl(unsigned operation, unsigned &arg) switch (operation) { case ACCELIOCGEXTERNAL: -#if defined(PX4_SPI_BUS_EXT) - return _bus == PX4_SPI_BUS_EXT ? 1 : 0; -#else - return 0; -#endif + external(); case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); diff --git a/src/drivers/mpu9250/mag_i2c.cpp b/src/drivers/mpu9250/mag_i2c.cpp index 39a08aea4a..15fbdbfb10 100644 --- a/src/drivers/mpu9250/mag_i2c.cpp +++ b/src/drivers/mpu9250/mag_i2c.cpp @@ -114,7 +114,7 @@ AK8963_I2C::ioctl(unsigned operation, unsigned &arg) switch (operation) { case ACCELIOCGEXTERNAL: - return _bus == PX4_I2C_BUS_EXPANSION ? 1 : 0; + return external(); case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); diff --git a/src/drivers/mpu9250/mpu9250_i2c.cpp b/src/drivers/mpu9250/mpu9250_i2c.cpp index ef808155da..c76a588733 100644 --- a/src/drivers/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/mpu9250/mpu9250_i2c.cpp @@ -112,7 +112,7 @@ MPU9250_I2C::ioctl(unsigned operation, unsigned &arg) switch (operation) { case ACCELIOCGEXTERNAL: - return _bus == PX4_I2C_BUS_EXPANSION ? 1 : 0; + return external(); case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); diff --git a/src/drivers/mpu9250/mpu9250_spi.cpp b/src/drivers/mpu9250/mpu9250_spi.cpp index 758dfd01ce..b8c7b6e2c5 100644 --- a/src/drivers/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/mpu9250/mpu9250_spi.cpp @@ -168,11 +168,7 @@ MPU9250_SPI::ioctl(unsigned operation, unsigned &arg) switch (operation) { case ACCELIOCGEXTERNAL: -#if defined(PX4_SPI_BUS_EXT) - return _bus == PX4_SPI_BUS_EXT ? 1 : 0; -#else - return 0; -#endif + external(); case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index bcd4ba146f..a7543b96e8 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -204,13 +204,6 @@ protected: */ void cycle(); - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } - /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. @@ -398,7 +391,7 @@ MS5611::init() ret = OK; _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, _interface->external() ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == nullptr) { warnx("failed to create sensor_baro publication"); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 21f986b4c9..8747d5e4a2 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -194,7 +194,7 @@ int MS5611_I2C::_probe_address(uint8_t address) { /* select the address we are going to try */ - set_address(address); + set_device_address(address); /* send reset command */ if (PX4_OK != _reset()) { diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index bbf96b98f4..26b615e041 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -321,7 +321,7 @@ OREOLED::cycle() perf_begin(_probe_perf); /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR + i); + set_device_address(OREOLED_BASE_I2C_ADDR + i); /* Calculate XOR CRC and append to the i2c write data */ msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i; @@ -500,7 +500,7 @@ OREOLED::cycle() perf_begin(_call_perf); /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); /* Calculate XOR CRC and append to the i2c write data */ uint8_t next_cmd_xor = OREOLED_BASE_I2C_ADDR + next_cmd.led_num; @@ -553,7 +553,7 @@ OREOLED::bootloader_app_reset(int led_num) int ret = -1; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); /* send a reset */ boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; @@ -602,7 +602,7 @@ OREOLED::bootloader_app_ping(int led_num) int ret = -1; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); /* send a pattern off command */ boot_cmd.buff[0] = 0xAA; @@ -642,7 +642,7 @@ OREOLED::bootloader_inapp_checksum(int led_num) uint16_t ret = 0x0000; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM; @@ -702,7 +702,7 @@ OREOLED::bootloader_ping(int led_num) int ret = -1; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; @@ -759,7 +759,7 @@ OREOLED::bootloader_version(int led_num) uint8_t ret = 0x00; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; @@ -815,7 +815,7 @@ OREOLED::bootloader_app_version(int led_num) uint16_t ret = 0x0000; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; @@ -874,7 +874,7 @@ OREOLED::bootloader_app_checksum(int led_num) uint16_t ret = 0x0000; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CRC; boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; @@ -933,7 +933,7 @@ OREOLED::bootloader_set_colour(int led_num, uint8_t red, uint8_t green) int ret = -1; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); boot_cmd.buff[0] = OREOLED_BOOT_CMD_SET_COLOUR; boot_cmd.buff[1] = red; @@ -1036,7 +1036,7 @@ OREOLED::bootloader_flash(int led_num) uint8_t flash_pages = ((fw_length + 64 - 1) / 64); /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; @@ -1201,7 +1201,7 @@ OREOLED::bootloader_boot(int led_num) int ret = -1; /* Set the current address */ - set_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); boot_cmd.buff[0] = OREOLED_BOOT_CMD_BOOT_APP; boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE; @@ -1554,7 +1554,7 @@ OREOLED::send_general_call() int ret = -ENODEV; /* set I2C address to zero */ - set_address(0); + set_device_address(0); /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/ uint8_t msg[] = {0x01, 0x00}; @@ -1579,7 +1579,7 @@ OREOLED::send_cmd(oreoled_cmd_t new_cmd) /* sanity check led number, health and cmd length */ if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) { /* set I2C address */ - set_address(OREOLED_BASE_I2C_ADDR + new_cmd.led_num); + set_device_address(OREOLED_BASE_I2C_ADDR + new_cmd.led_num); /* add to queue */ _cmd_queue->force(&new_cmd); diff --git a/src/drivers/sdp3x_airspeed/SDP3X.cpp b/src/drivers/sdp3x_airspeed/SDP3X.cpp index 0fa7c9bd0a..0715922c61 100644 --- a/src/drivers/sdp3x_airspeed/SDP3X.cpp +++ b/src/drivers/sdp3x_airspeed/SDP3X.cpp @@ -58,11 +58,11 @@ bool SDP3X::init_sdp3x() { // step 1 - reset on broadcast - uint16_t prev_addr = get_address(); - set_address(SDP3X_RESET_ADDR); + uint16_t prev_addr = get_device_address(); + set_device_address(SDP3X_RESET_ADDR); uint8_t reset_cmd = SDP3X_RESET_CMD; int ret = transfer(&reset_cmd, 1, nullptr, 0); - set_address(prev_addr); + set_device_address(prev_addr); if (ret != PX4_OK) { perf_count(_comms_errors); diff --git a/src/drivers/sf1xx/sf1xx.cpp b/src/drivers/sf1xx/sf1xx.cpp index 2f4d1be6af..f6795260dc 100644 --- a/src/drivers/sf1xx/sf1xx.cpp +++ b/src/drivers/sf1xx/sf1xx.cpp @@ -284,7 +284,7 @@ SF1XX::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); - set_address(SF1XX_BASEADDR); + set_device_address(SF1XX_BASEADDR); if (_reports == nullptr) { return ret; diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index ced30e4b23..d2d4281504 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -257,7 +257,7 @@ SRF02::init() _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = SRF02_BASEADDR; /* set temp sonar i2c address to base adress */ - set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { return ret; @@ -283,7 +283,7 @@ SRF02::init() So second iteration it uses i2c address 111, third iteration 110 and so on*/ for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { _index_counter = SRF02_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ - set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ int ret2 = measure(); if (ret2 == 0) { /* sonar is present -> store address_index in array */ @@ -294,7 +294,7 @@ SRF02::init() } _index_counter = SRF02_BASEADDR; - set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */ /* if only one sonar detected, no special timing is required between firing, so use default */ if (addr_ind.size() == 1) { @@ -651,7 +651,7 @@ SRF02::cycle() { if (_collect_phase) { _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ - set_address(_index_counter); + set_device_address(_index_counter); /* perform collection */ if (OK != collect()) { @@ -690,7 +690,7 @@ SRF02::cycle() /* ensure sonar i2c adress is still correct */ _index_counter = addr_ind[_cycle_counter]; - set_address(_index_counter); + set_device_address(_index_counter); /* Perform measurement */ if (OK != measure()) { diff --git a/src/drivers/srf02_i2c/srf02_i2c.cpp b/src/drivers/srf02_i2c/srf02_i2c.cpp index e071eed8c4..d4e41044a8 100644 --- a/src/drivers/srf02_i2c/srf02_i2c.cpp +++ b/src/drivers/srf02_i2c/srf02_i2c.cpp @@ -258,7 +258,7 @@ SRF02_I2C::init() _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = SRF02_I2C_BASEADDR; /* set temp sonar i2c address to base adress */ - set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { return ret; @@ -284,7 +284,7 @@ SRF02_I2C::init() So second iteration it uses i2c address 111, third iteration 110 and so on*/ for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { _index_counter = SRF02_I2C_BASEADDR + counter * 2; /* set temp sonar i2c address to base adress - counter */ - set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ int ret2 = measure(); if (ret2 == 0) { /* sonar is present -> store address_index in array */ @@ -295,7 +295,7 @@ SRF02_I2C::init() } _index_counter = SRF02_I2C_BASEADDR; - set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */ /* if only one sonar detected, no special timing is required between firing, so use default */ if (addr_ind.size() == 1) { @@ -651,7 +651,7 @@ SRF02_I2C::cycle() { if (_collect_phase) { _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ - set_address(_index_counter); + set_device_address(_index_counter); /* perform collection */ if (OK != collect()) { @@ -690,7 +690,7 @@ SRF02_I2C::cycle() /* ensure sonar i2c adress is still correct */ _index_counter = addr_ind[_cycle_counter]; - set_address(_index_counter); + set_device_address(_index_counter); /* Perform measurement */ if (OK != measure()) { diff --git a/src/drivers/teraranger/teraranger.cpp b/src/drivers/teraranger/teraranger.cpp index 412118c5d5..610fbfc8a2 100644 --- a/src/drivers/teraranger/teraranger.cpp +++ b/src/drivers/teraranger/teraranger.cpp @@ -289,10 +289,10 @@ TERARANGER::init() case 1: /* Autodetect */ /* Assume TROne */ - set_address(TRONE_BASEADDR); + set_device_address(TRONE_BASEADDR); if (I2C::init() != OK) { - set_address(TREVO_BASEADDR); + set_device_address(TREVO_BASEADDR); if (I2C::init() != OK) { goto out; @@ -310,7 +310,7 @@ TERARANGER::init() break; case 2: /* TROne */ - set_address(TRONE_BASEADDR); + set_device_address(TRONE_BASEADDR); if (I2C::init() != OK) { goto out; @@ -321,7 +321,7 @@ TERARANGER::init() break; case 3: /* TREvo */ - set_address(TREVO_BASEADDR); + set_device_address(TREVO_BASEADDR); /* do I2C init (and probe) first */ if (I2C::init() != OK) { diff --git a/src/platforms/qurt/include/i2c.h b/src/platforms/qurt/include/i2c.h index 5021cf1814..0e567d29a1 100644 --- a/src/platforms/qurt/include/i2c.h +++ b/src/platforms/qurt/include/i2c.h @@ -34,6 +34,8 @@ ****************************************************************************/ #pragma once +#include + struct i2c_msg_s { uint16_t addr; /* Slave address */ uint16_t flags; /* See I2C_M_* definitions */