From df8c9b4efbb4ea71f85f566671f3d7b34b23544c Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 20 Sep 2021 11:03:29 -0400
Subject: [PATCH] [DO NOT MERGE] run sdp3x at lower rate (20 Hz sample rate)

---
 .../differential_pressure/sdp3x/SDP3X.cpp     | 12 ++++++++----
 .../differential_pressure/sdp3x/SDP3X.hpp     | 19 ++++++++++++-------
 2 files changed, 20 insertions(+), 11 deletions(-)

diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp
index f85d95b801..ce337cd159 100644
--- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp
+++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp
@@ -77,7 +77,7 @@ SDP3X::configure()
 	int ret = write_command(SDP3X_CONT_MODE_STOP);
 
 	if (ret != PX4_OK) {
-		perf_count(_comms_errors);
+		perf_count(_configure_failed_perf);
 		DEVICE_DEBUG("stopping continous mode failed %d", ret);
 
 		if (ScheduledWorkItem::alone()) {
@@ -159,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);
@@ -178,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)) {
@@ -186,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();
 
@@ -202,6 +204,8 @@ SDP3X::collect()
 void
 SDP3X::RunImpl()
 {
+	perf_count(_interval_perf);
+
 	switch (_state) {
 	case State::RequireConfig:
 		if (configure() == PX4_OK) {
diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp
index 5536127bb8..88bd11b89d 100644
--- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp
+++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp
@@ -63,9 +63,7 @@
 #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>
@@ -76,11 +74,17 @@ public:
 		I2CSPIDriver(config),
 		_keep_retrying{config.keep_running}
 	{
-		_debug_enabled = true;
+		//_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();
 
@@ -101,8 +105,6 @@ private:
 	int	configure();
 	int	read_scale();
 
-	math::LowPassFilter2p<float> _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
-
 	bool init_sdp3x();
 
 	/**
@@ -120,4 +122,7 @@ private:
 	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")};
 };