sdp3x issue I2C general call reset if alone

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;

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; }
 

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();

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();
 

src/drivers/differential_pressure/sdp3x/SDP3X.hpp

@@ -44,6 +44,7 @@
 #include <drivers/airspeed/airspeed.h>
 #include <math.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/perf/perf_counter.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/i2c_spi_buses.h>
@@ -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")};
 };

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()
 {

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