Browse Source

board support add px4_i2c_bus_external/px4_spi_bus_external

sbg
Daniel Agar 7 years ago committed by Lorenz Meier
parent
commit
53595bac0e
  1. 1
      cmake/common/px4_base.cmake
  2. 8
      src/drivers/batt_smbus/batt_smbus.cpp
  3. 22
      src/drivers/bmi055/bmi055.hpp
  4. 2
      src/drivers/bmi055/bmi055_accel.cpp
  5. 2
      src/drivers/bmi055/bmi055_gyro.cpp
  6. 4
      src/drivers/bmi160/bmi160.cpp
  7. 14
      src/drivers/bmi160/bmi160.hpp
  8. 23
      src/drivers/bmm150/bmm150.cpp
  9. 4
      src/drivers/bmm150/bmm150.hpp
  10. 40
      src/drivers/boards/common/board_common.h
  11. 1
      src/drivers/boards/px4fmu-v2/board_config.h
  12. 1
      src/drivers/boards/px4fmu-v3/CMakeLists.txt
  13. 55
      src/drivers/boards/px4fmu-v3/px4fmu_i2c.c
  14. 15
      src/drivers/boards/px4fmu-v3/px4fmu_spi.c
  15. 1
      src/drivers/boards/px4fmu-v4/board_config.h
  16. 1
      src/drivers/boards/sitl/board_config.h
  17. 2
      src/drivers/bst/bst.cpp
  18. 12
      src/drivers/device/Device.hpp
  19. 3
      src/drivers/device/nuttx/cdev_platform.hpp
  20. 23
      src/drivers/device/nuttx/i2c_nuttx.cpp
  21. 28
      src/drivers/device/nuttx/i2c_nuttx.h
  22. 7
      src/drivers/device/nuttx/spi.cpp
  23. 2
      src/drivers/device/posix/cdev_platform.cpp
  24. 13
      src/drivers/device/posix/i2c_posix.cpp
  25. 28
      src/drivers/device/posix/i2c_posix.h
  26. 4
      src/drivers/device/spi.h
  27. 15
      src/drivers/fxas21002c/fxas21002c.cpp
  28. 17
      src/drivers/fxos8701cq/fxos8701cq.cpp
  29. 13
      src/drivers/hmc5883/hmc5883_i2c.cpp
  30. 15
      src/drivers/l3gd20/l3gd20.cpp
  31. 7
      src/drivers/lis3mdl/lis3mdl_i2c.cpp
  32. 3
      src/drivers/ll40ls/LidarLiteI2C.cpp
  33. 3
      src/drivers/ll40ls/LidarLiteI2C.h
  34. 17
      src/drivers/lsm303d/lsm303d.cpp
  35. 10
      src/drivers/mb12xx/mb12xx.cpp
  36. 12
      src/drivers/md25/md25.cpp
  37. 2
      src/drivers/md25/md25_main.cpp
  38. 8
      src/drivers/mkblctrl/mkblctrl.cpp
  39. 9
      src/drivers/mpl3115a2/mpl3115a2.cpp
  40. 2
      src/drivers/mpl3115a2/mpl3115a2_i2c.cpp
  41. 2
      src/drivers/mpu6000/mpu6000_i2c.cpp
  42. 6
      src/drivers/mpu6000/mpu6000_spi.cpp
  43. 2
      src/drivers/mpu9250/mag_i2c.cpp
  44. 2
      src/drivers/mpu9250/mpu9250_i2c.cpp
  45. 6
      src/drivers/mpu9250/mpu9250_spi.cpp
  46. 9
      src/drivers/ms5611/ms5611.cpp
  47. 2
      src/drivers/ms5611/ms5611_i2c.cpp
  48. 28
      src/drivers/oreoled/oreoled.cpp
  49. 6
      src/drivers/sdp3x_airspeed/SDP3X.cpp
  50. 2
      src/drivers/sf1xx/sf1xx.cpp
  51. 10
      src/drivers/srf02/srf02.cpp
  52. 10
      src/drivers/srf02_i2c/srf02_i2c.cpp
  53. 8
      src/drivers/teraranger/teraranger.cpp
  54. 2
      src/platforms/qurt/include/i2c.h

1
cmake/common/px4_base.cmake

@ -382,6 +382,7 @@ function(px4_add_common_flags) @@ -382,6 +382,7 @@ function(px4_add_common_flags)
set(cxx_warnings
-Wno-missing-field-initializers
-Wno-overloaded-virtual # TODO: fix and remove
-Wreorder
)

8
src/drivers/batt_smbus/batt_smbus.cpp

@ -440,11 +440,11 @@ BATT_SMBUS::search() @@ -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() @@ -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 @@ -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) {

22
src/drivers/bmi055/bmi055.hpp

@ -251,12 +251,6 @@ @@ -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: @@ -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: @@ -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
*

2
src/drivers/bmi055/bmi055_accel.cpp

@ -113,7 +113,7 @@ BMI055_accel::init() @@ -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");

2
src/drivers/bmi055/bmi055_gyro.cpp

@ -118,7 +118,7 @@ BMI055_gyro::init() @@ -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");

4
src/drivers/bmi160/bmi160.cpp

@ -190,7 +190,7 @@ BMI160::init() @@ -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() @@ -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");

14
src/drivers/bmi160/bmi160.hpp

@ -244,13 +244,6 @@ @@ -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: @@ -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
*

23
src/drivers/bmm150/bmm150.cpp

@ -77,8 +77,8 @@ void start(bool external_bus, enum Rotation rotation) @@ -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() @@ -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 @@ -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 @@ -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() @@ -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() @@ -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() @@ -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)
{

4
src/drivers/bmm150/bmm150.hpp

@ -195,10 +195,9 @@ struct bmm150_data { @@ -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: @@ -220,7 +219,6 @@ protected:
private:
work_s _work{};
bool _external;
bool _running;

40
src/drivers/boards/common/board_common.h

@ -869,4 +869,44 @@ static inline int board_register_power_state_notification_cb(power_button_state_ @@ -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"

1
src/drivers/boards/px4fmu-v2/board_config.h

@ -424,7 +424,6 @@ extern void board_peripheral_reset(int ms); @@ -424,7 +424,6 @@ extern void board_peripheral_reset(int ms);
extern void stm32_usbinitialize(void);
#include "../common/board_common.h"
#endif /* __ASSEMBLY__ */

1
src/drivers/boards/px4fmu-v3/CMakeLists.txt

@ -34,6 +34,7 @@ px4_add_module( @@ -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

55
src/drivers/boards/px4fmu-v3/px4fmu_i2c.c

@ -0,0 +1,55 @@ @@ -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;
}

15
src/drivers/boards/px4fmu-v3/px4fmu_spi.c

@ -451,3 +451,18 @@ __EXPORT void board_spi_reset(int ms) @@ -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;
}

1
src/drivers/boards/px4fmu-v4/board_config.h

@ -157,7 +157,6 @@ @@ -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.
*

1
src/drivers/boards/sitl/board_config.h

@ -51,6 +51,7 @@ @@ -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 <system_config.h>

2
src/drivers/bst/bst.cpp

@ -273,7 +273,7 @@ void BST::start() @@ -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);
}

12
src/drivers/device/Device.hpp

@ -137,30 +137,32 @@ public: @@ -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

3
src/drivers/device/nuttx/cdev_platform.hpp

@ -3,9 +3,10 @@ @@ -3,9 +3,10 @@
#include <cinttypes>
#include <px4_micro_hal.h>
#include <nuttx/arch.h>
#include <px4_micro_hal.h>
#define ATOMIC_ENTER irqstate_t flags = px4_enter_critical_section()
#define ATOMIC_LEAVE px4_leave_critical_section(flags)

23
src/drivers/device/nuttx/i2c_nuttx.cpp

@ -61,9 +61,6 @@ I2C::I2C(const char *name, @@ -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() @@ -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() @@ -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() @@ -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() @@ -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 @@ -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<uint8_t *>(send);
msgv[msgs].length = send_len;
@ -207,8 +204,8 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re @@ -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) @@ -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();
}

28
src/drivers/device/nuttx/i2c_nuttx.h

@ -55,11 +55,6 @@ class __EXPORT I2C : public CDev @@ -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: @@ -71,11 +66,6 @@ protected:
*/
unsigned _retries;
/**
* The I2C bus number the device is attached to.
*/
int _bus;
/**
* @ Constructor
*
@ -111,8 +101,7 @@ protected: @@ -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: @@ -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;

7
src/drivers/device/nuttx/spi.cpp

@ -71,8 +71,7 @@ SPI::SPI(const char *name, @@ -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() @@ -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() @@ -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;

2
src/drivers/device/posix/cdev_platform.cpp

@ -106,7 +106,7 @@ extern "C" { @@ -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;

13
src/drivers/device/posix/i2c_posix.cpp

@ -40,7 +40,8 @@ @@ -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 <linux/i2c.h>
#include <linux/i2c-dev.h>
@ -72,8 +73,6 @@ I2C::I2C(const char *name, @@ -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() @@ -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 @@ -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<uint8_t *>(send);
msgv[msgs].len = send_len;
@ -164,7 +163,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re @@ -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) @@ -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 {

28
src/drivers/device/posix/i2c_posix.h

@ -60,11 +60,6 @@ class __EXPORT I2C : public CDev @@ -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: @@ -72,11 +67,6 @@ protected:
*/
unsigned _retries;
/**
* The I2C bus number the device is attached to.
*/
int _bus;
/**
* @ Constructor
*
@ -110,8 +100,7 @@ protected: @@ -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: @@ -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;

4
src/drivers/device/spi.h

@ -142,10 +142,10 @@ private: @@ -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

15
src/drivers/fxas21002c/fxas21002c.cpp

@ -205,12 +205,6 @@ @@ -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: @@ -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() @@ -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");

17
src/drivers/fxos8701cq/fxos8701cq.cpp

@ -138,12 +138,6 @@ @@ -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: @@ -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() @@ -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() @@ -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;

13
src/drivers/hmc5883/hmc5883_i2c.cpp

@ -113,18 +113,7 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) @@ -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);

15
src/drivers/l3gd20/l3gd20.cpp

@ -178,12 +178,6 @@ @@ -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: @@ -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() @@ -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");

7
src/drivers/lis3mdl/lis3mdl_i2c.cpp

@ -112,12 +112,7 @@ LIS3MDL_I2C::ioctl(unsigned operation, unsigned &arg) @@ -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);

3
src/drivers/ll40ls/LidarLiteI2C.cpp

@ -68,8 +68,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr @@ -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;

3
src/drivers/ll40ls/LidarLiteI2C.h

@ -123,9 +123,6 @@ private: @@ -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

17
src/drivers/lsm303d/lsm303d.cpp

@ -208,12 +208,6 @@ @@ -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: @@ -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() @@ -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() @@ -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;

10
src/drivers/mb12xx/mb12xx.cpp

@ -256,7 +256,7 @@ MB12XX::init() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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()) {

12
src/drivers/md25/md25.cpp

@ -357,7 +357,7 @@ int MD25::probe() @@ -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() @@ -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() @@ -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() @@ -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;
}
}

2
src/drivers/md25/md25_main.cpp

@ -177,7 +177,7 @@ int md25_main(int argc, char *argv[]) @@ -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);

8
src/drivers/mkblctrl/mkblctrl.cpp

@ -701,7 +701,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) @@ -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) @@ -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) @@ -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() @@ -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) {

9
src/drivers/mpl3115a2/mpl3115a2.cpp

@ -174,13 +174,6 @@ protected: @@ -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() @@ -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");

2
src/drivers/mpl3115a2/mpl3115a2_i2c.cpp

@ -107,7 +107,7 @@ int @@ -107,7 +107,7 @@ int
MPL3115A2_I2C::init()
{
/* this will call probe() */
set_address(MPL3115A2_ADDRESS);
set_device_address(MPL3115A2_ADDRESS);
return I2C::init();
}

2
src/drivers/mpu6000/mpu6000_i2c.cpp

@ -116,7 +116,7 @@ MPU6000_I2C::ioctl(unsigned operation, unsigned &arg) @@ -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);

6
src/drivers/mpu6000/mpu6000_spi.cpp

@ -240,11 +240,7 @@ MPU6000_SPI::ioctl(unsigned operation, unsigned &arg) @@ -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);

2
src/drivers/mpu9250/mag_i2c.cpp

@ -114,7 +114,7 @@ AK8963_I2C::ioctl(unsigned operation, unsigned &arg) @@ -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);

2
src/drivers/mpu9250/mpu9250_i2c.cpp

@ -112,7 +112,7 @@ MPU9250_I2C::ioctl(unsigned operation, unsigned &arg) @@ -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);

6
src/drivers/mpu9250/mpu9250_spi.cpp

@ -168,11 +168,7 @@ MPU9250_SPI::ioctl(unsigned operation, unsigned &arg) @@ -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);

9
src/drivers/ms5611/ms5611.cpp

@ -204,13 +204,6 @@ protected: @@ -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() @@ -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");

2
src/drivers/ms5611/ms5611_i2c.cpp

@ -194,7 +194,7 @@ int @@ -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()) {

28
src/drivers/oreoled/oreoled.cpp

@ -321,7 +321,7 @@ OREOLED::cycle() @@ -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() @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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) @@ -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);

6
src/drivers/sdp3x_airspeed/SDP3X.cpp

@ -58,11 +58,11 @@ bool @@ -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);

2
src/drivers/sf1xx/sf1xx.cpp

@ -284,7 +284,7 @@ SF1XX::init() @@ -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;

10
src/drivers/srf02/srf02.cpp

@ -257,7 +257,7 @@ SRF02::init() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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()) {

10
src/drivers/srf02_i2c/srf02_i2c.cpp

@ -258,7 +258,7 @@ SRF02_I2C::init() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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()) {

8
src/drivers/teraranger/teraranger.cpp

@ -289,10 +289,10 @@ TERARANGER::init() @@ -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() @@ -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() @@ -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) {

2
src/platforms/qurt/include/i2c.h

@ -34,6 +34,8 @@ @@ -34,6 +34,8 @@
****************************************************************************/
#pragma once
#include <stdint.h>
struct i2c_msg_s {
uint16_t addr; /* Slave address */
uint16_t flags; /* See I2C_M_* definitions */

Loading…
Cancel
Save