From a5a9b1011a6ea70529ba8a1db4fa70d427520f60 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 15 Sep 2022 15:50:28 -0400 Subject: [PATCH] [DO NOT MERGE] px4_i2c_device_external hacks --- boards/px4/fmu-v6c/src/board_config.h | 2 + boards/px4/fmu-v6c/src/i2c.cpp | 39 +++++++++++++++++++ platforms/common/i2c.cpp | 12 +++++- .../common/include/px4_platform_common/i2c.h | 5 +++ src/lib/drivers/device/nuttx/I2C.hpp | 2 +- src/lib/drivers/device/posix/I2C.hpp | 2 +- src/lib/sensor_calibration/Utilities.cpp | 2 +- 7 files changed, 60 insertions(+), 4 deletions(-) diff --git a/boards/px4/fmu-v6c/src/board_config.h b/boards/px4/fmu-v6c/src/board_config.h index d92fa394a0..dec040aca4 100644 --- a/boards/px4/fmu-v6c/src/board_config.h +++ b/boards/px4/fmu-v6c/src/board_config.h @@ -271,6 +271,8 @@ #define PX4_I2C_BUS_MTD 4,5 +#define BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL + #define BOARD_NUM_IO_TIMERS 5 diff --git a/boards/px4/fmu-v6c/src/i2c.cpp b/boards/px4/fmu-v6c/src/i2c.cpp index c774c0f404..ab0a2bbe55 100644 --- a/boards/px4/fmu-v6c/src/i2c.cpp +++ b/boards/px4/fmu-v6c/src/i2c.cpp @@ -33,8 +33,47 @@ #include +#include +#include + constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusExternal(1), initI2CBusExternal(2), initI2CBusExternal(4), }; + +bool px4_i2c_device_external(const uint32_t device_id) +{ + { + // The internal baro and mag on Pixhawk 6C are on an external + // bus. On rev 0, the bus is actually exposed externally, on + // rev 1+, it is properly internal, however, still marked as + // external for compatibility. + + // device_id: 4028193 + device::Device::DeviceId device_id_baro{}; + device_id_baro.devid_s.bus_type = device::Device::DeviceBusType_I2C; + device_id_baro.devid_s.bus = 4; + device_id_baro.devid_s.address = 0x77; + device_id_baro.devid_s.devtype = DRV_BARO_DEVTYPE_MS5611; + + if (device_id_baro.devid == device_id) { + return false; + } + + // device_id: 396321 + device::Device::DeviceId device_id_mag{}; + device_id_mag.devid_s.bus_type = device::Device::DeviceBusType_I2C; + device_id_mag.devid_s.bus = 4; + device_id_mag.devid_s.address = 0xc; + device_id_mag.devid_s.devtype = DRV_MAG_DEVTYPE_IST8310; + + if (device_id_mag.devid == device_id) { + return false; + } + } + + device::Device::DeviceId dev_id{}; + dev_id.devid = device_id; + return px4_i2c_bus_external(dev_id.devid_s.bus); +} diff --git a/platforms/common/i2c.cpp b/platforms/common/i2c.cpp index 5d5d7e71e7..467a202d86 100644 --- a/platforms/common/i2c.cpp +++ b/platforms/common/i2c.cpp @@ -42,7 +42,17 @@ bool px4_i2c_bus_external(const px4_i2c_bus_t &bus) { return bus.is_external; } -#endif +#endif // BOARD_OVERRIDE_I2C_BUS_EXTERNAL + +#ifndef BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL +#include +bool px4_i2c_device_external(const uint32_t device_id) +{ + device::Device::DeviceId dev_id{}; + dev_id.devid = device_id; + return px4_i2c_bus_external(dev_id.devid_s.bus); +} +#endif // BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL bool I2CBusIterator::next() { diff --git a/platforms/common/include/px4_platform_common/i2c.h b/platforms/common/include/px4_platform_common/i2c.h index 16c274860f..c2ec0e9d6f 100644 --- a/platforms/common/include/px4_platform_common/i2c.h +++ b/platforms/common/include/px4_platform_common/i2c.h @@ -66,6 +66,11 @@ static inline bool px4_i2c_bus_external(int bus) return true; } +/** + * runtime-check if a board has a specific device as external. + * This can be overridden by a board to add run-time checks. + */ +__EXPORT bool px4_i2c_device_external(const uint32_t device_id); /** * @class I2CBusIterator diff --git a/src/lib/drivers/device/nuttx/I2C.hpp b/src/lib/drivers/device/nuttx/I2C.hpp index e443db6113..7a9a5c08bd 100644 --- a/src/lib/drivers/device/nuttx/I2C.hpp +++ b/src/lib/drivers/device/nuttx/I2C.hpp @@ -108,7 +108,7 @@ protected: */ int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len); - bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); } + bool external() const override { return px4_i2c_device_external(_device_id.devid); } private: static unsigned int _bus_clocks[PX4_NUMBER_I2C_BUSES]; diff --git a/src/lib/drivers/device/posix/I2C.hpp b/src/lib/drivers/device/posix/I2C.hpp index e1fdbe9da1..7637cfc06d 100644 --- a/src/lib/drivers/device/posix/I2C.hpp +++ b/src/lib/drivers/device/posix/I2C.hpp @@ -104,7 +104,7 @@ protected: */ int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len); - virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); } + virtual bool external() const override { return px4_i2c_device_external(_device_id.devid); } private: int _fd{-1}; diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp index 8cdc760e1e..dd3f2d8cb2 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -250,7 +250,7 @@ bool DeviceExternal(uint32_t device_id) switch (bus_type) { case device::Device::DeviceBusType_I2C: #if defined(CONFIG_I2C) - external = px4_i2c_bus_external(id.devid_s.bus); + external = px4_i2c_device_external(device_id); #endif // CONFIG_I2C break;