From 34ccd5c99f1e05842fa782585318e8c367be8e61 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 15 Jul 2021 11:15:41 -0400 Subject: [PATCH] sdp3x issue I2C general call reset if alone --- .../px4_work_queue/WorkItem.hpp | 2 ++ .../px4_work_queue/WorkQueue.hpp | 2 ++ platforms/common/px4_work_queue/WorkQueue.cpp | 5 ++++ .../differential_pressure/sdp3x/SDP3X.cpp | 25 +++++++++++++++-- .../differential_pressure/sdp3x/SDP3X.hpp | 6 ++++ src/lib/drivers/device/nuttx/I2C.cpp | 28 +++++++++++++++++++ src/lib/drivers/device/nuttx/I2C.hpp | 3 ++ 7 files changed, 68 insertions(+), 3 deletions(-) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp index 2f95b064c4..8cb717e8f2 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp @@ -122,6 +122,8 @@ protected: bool Init(const wq_config_t &config); void Deinit(); + bool alone() { return _wq && (_wq->count() <= 1); } + float elapsed_time() const; float average_rate() const; float average_interval() const; diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index f2a66bb898..4555b5e002 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -73,6 +73,8 @@ public: void print_status(bool last = false); + size_t count(); + // WorkQueues sorted numerically by relative priority (-1 to -255) bool operator<=(const WorkQueue &rhs) const { return _config.relative_priority >= rhs.get_config().relative_priority; } diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index fa32017912..c731f01e8b 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -204,6 +204,11 @@ void WorkQueue::Run() PX4_DEBUG("%s: exiting", _config.name); } +size_t WorkQueue::count() +{ + return _work_items.size(); +} + void WorkQueue::print_status(bool last) { const size_t num_items = _work_items.size(); diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index b5d4231d54..f85d95b801 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -72,7 +72,26 @@ SDP3X::init_sdp3x() int SDP3X::configure() { - int ret = write_command(SDP3X_CONT_MEAS_AVG_MODE); + perf_count(_configure_perf); + + int ret = write_command(SDP3X_CONT_MODE_STOP); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + DEVICE_DEBUG("stopping continous mode failed %d", ret); + + if (ScheduledWorkItem::alone()) { + DEVICE_DEBUG("alone on the bus, issuing general call reset"); + ResetBus(); + + } else { + // TODO: temporary + DEVICE_DEBUG("forcing bus reset"); + ResetBus(); + } + } + + ret = write_command(SDP3X_CONT_MEAS_AVG_MODE); if (ret != PX4_OK) { perf_count(_comms_errors); @@ -95,7 +114,7 @@ SDP3X::read_scale() if (ret != PX4_OK) { perf_count(_comms_errors); - PX4_ERR("get scale failed"); + DEVICE_DEBUG("get scale failed"); return ret; } @@ -124,7 +143,7 @@ SDP3X::read_scale() return PX4_OK; } -int SDP3X::init() +int SDP3X::init() { int ret = Airspeed::init(); diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index f740d7b3b1..5536127bb8 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -56,6 +57,7 @@ #define SDP3X_RESET_ADDR 0x00 #define SDP3X_RESET_CMD 0x06 #define SDP3X_CONT_MEAS_AVG_MODE 0x3615 +#define SDP3X_CONT_MODE_STOP 0x3FF9 #define SDP3X_SCALE_PRESSURE_SDP31 60 #define SDP3X_SCALE_PRESSURE_SDP32 240 @@ -74,6 +76,8 @@ public: I2CSPIDriver(config), _keep_retrying{config.keep_running} { + _debug_enabled = true; + _retries = 2; } virtual ~SDP3X() = default; @@ -114,4 +118,6 @@ private: uint16_t _scale{0}; const bool _keep_retrying; State _state{State::RequireConfig}; + + perf_counter_t _configure_perf{perf_alloc(PC_COUNT, MODULE_NAME": configure")}; }; diff --git a/src/lib/drivers/device/nuttx/I2C.cpp b/src/lib/drivers/device/nuttx/I2C.cpp index c2ffb06c04..5597a96148 100644 --- a/src/lib/drivers/device/nuttx/I2C.cpp +++ b/src/lib/drivers/device/nuttx/I2C.cpp @@ -96,6 +96,34 @@ I2C::set_bus_clock(unsigned bus, unsigned clock_hz) return OK; } +bool I2C::ResetBus() +{ + if (_dev) { + px4_i2cbus_uninitialize(_dev); + _dev = nullptr; + } + + _dev = px4_i2cbus_initialize(get_device_bus()); + +#if defined(CONFIG_I2C_RESET) + I2C_RESET(_dev); +#endif // CONFIG_I2C_RESET + + // send software reset to all + uint8_t buf[1] {}; + buf[0] = 0x06; // software reset + + i2c_msg_s msg{}; + msg.frequency = I2C_SPEED_STANDARD; + msg.addr = 0x00; // general call address + msg.buffer = &buf[0]; + msg.length = 1; + + int ret_transfer = I2C_TRANSFER(_dev, &msg, 1); + + return (_dev != nullptr) && (ret_transfer == 0); +} + int I2C::init() { diff --git a/src/lib/drivers/device/nuttx/I2C.hpp b/src/lib/drivers/device/nuttx/I2C.hpp index d37d809c8b..1d31142f3d 100644 --- a/src/lib/drivers/device/nuttx/I2C.hpp +++ b/src/lib/drivers/device/nuttx/I2C.hpp @@ -72,6 +72,9 @@ public: static int set_bus_clock(unsigned bus, unsigned clock_hz); + + bool ResetBus(); + protected: /** * The number of times a read or write operation will be retried on