[DO NOT MERGE] run sdp3x at lower rate (20 Hz sample rate)

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(_configure_failed_perf);
+		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();
 
@@ -140,10 +159,12 @@ int
 SDP3X::collect()
 {
 	perf_begin(_sample_perf);
+	perf_begin(_transfer_perf);
 
 	// read 6 bytes from the sensor
 	uint8_t val[6];
 	int ret = transfer(nullptr, 0, &val[0], sizeof(val));
+	perf_end(_transfer_perf);
 
 	if (ret != PX4_OK) {
 		perf_count(_comms_errors);
@@ -159,7 +180,7 @@ SDP3X::collect()
 	int16_t P = (((int16_t)val[0]) << 8) | val[1];
 	int16_t temp = (((int16_t)val[3]) << 8) | val[4];
 
-	float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale);
+	float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale) - _diff_pres_offset;
 	float temperature_c = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
 
 	if (PX4_ISFINITE(diff_press_pa_raw)) {
@@ -167,8 +188,8 @@ SDP3X::collect()
 
 		report.error_count = perf_event_count(_comms_errors);
 		report.temperature = temperature_c;
-		report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
-		report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
+		report.differential_pressure_filtered_pa = diff_press_pa_raw;
+		report.differential_pressure_raw_pa = diff_press_pa_raw;
 		report.device_id = _device_id.devid;
 		report.timestamp = hrt_absolute_time();
 
@@ -183,6 +204,8 @@ SDP3X::collect()
 void
 SDP3X::RunImpl()
 {
+	perf_count(_interval_perf);
+
 	switch (_state) {
 	case State::RequireConfig:
 		if (configure() == PX4_OK) {

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,14 +57,13 @@
 #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
 #define SDP3X_SCALE_PRESSURE_SDP33	20
 
-// Measurement rate is 20Hz
-#define SPD3X_MEAS_RATE 100
-#define SDP3X_MEAS_DRIVER_FILTER_FREQ 3.0f
+#define SPD3X_MEAS_RATE 20
 #define CONVERSION_INTERVAL	(1000000 / SPD3X_MEAS_RATE)	/* microseconds */
 
 class SDP3X : public Airspeed, public I2CSPIDriver<SDP3X>
@@ -74,9 +74,17 @@ public:
 		I2CSPIDriver(config),
 		_keep_retrying{config.keep_running}
 	{
+		//_debug_enabled = true;
+		_retries = 2;
 	}
 
-	virtual ~SDP3X() = default;
+	virtual ~SDP3X()
+	{
+		perf_free(_configure_perf);
+		perf_free(_interval_perf);
+		perf_free(_transfer_perf);
+		perf_free(_configure_failed_perf);
+	};
 
 	static void print_usage();
 
@@ -97,8 +105,6 @@ private:
 	int	configure();
 	int	read_scale();
 
-	math::LowPassFilter2p<float> _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
-
 	bool init_sdp3x();
 
 	/**
@@ -114,4 +120,9 @@ 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")};
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": configure")};
+	perf_counter_t _configure_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME": configure failed")};
 };

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