RPMCapture: added pulse per rev parameter. Do not publish if pulses a…

…re to fast
PX4 · Dec 10, 2024 · 81de3d3 · 81de3d3
1 parent 0e1187f
commit 81de3d3
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 16 deletions.
diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp
@@ -58,6 +58,9 @@ bool RPMCapture::init()
 {
 	bool success = false;
 
+	param_get(param_find("RPM_PULSES_PER_REV"), &_pulses_per_revolution);
+	_min_pulse_period_us = static_cast<uint32_t>(60.f * 1e6f / static_cast<float>(RPM_MAX_VALUE * _pulses_per_revolution));
+
 	for (unsigned i = 0; i < 16; ++i) {
 		char param_name[17];
 		snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
@@ -125,26 +128,28 @@ void RPMCapture::Run()
 		ScheduleDelayed(RPM_PULSE_TIMEOUT);
 	}
 
-	float rpm_raw{0.f};
 
-	if ((1 < _period) && (_period < RPM_PULSE_TIMEOUT)) {
-		// 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute
-		rpm_raw = 60.f * 1e6f / (static_cast<float>(_period) * 1.f);
+	if (_period > _min_pulse_period_us) {
+		// Only update if the period is above the min pulse period threshold
+		float rpm_raw{0.f};
 
-	} else {
-		_rpm_filter.reset(rpm_raw);
-	}
+		if (_period < RPM_PULSE_TIMEOUT) {
+			// 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute
+			rpm_raw = 60.f * 1e6f / static_cast<float>(_pulses_per_revolution * _period);
+		}
+
+		const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f);
+		_timestamp_last_update = now;
+		_rpm_filter.setParameters(dt, 0.5f);
+		_rpm_filter.update(rpm_raw);
 
-	const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f);
-	_timestamp_last_update = now;
-	_rpm_filter.setParameters(dt, 0.5f);
-	_rpm_filter.update(rpm_raw);
+		rpm_s rpm{};
+		rpm.timestamp = now;
+		rpm.rpm_raw = rpm_raw;
+		rpm.rpm_estimate = _rpm_filter.getState();
+		_rpm_pub.publish(rpm);
+	}
 
-	rpm_s rpm{};
-	rpm.timestamp = now;
-	rpm.rpm_raw = rpm_raw;
-	rpm.rpm_estimate = _rpm_filter.getState();
-	_rpm_pub.publish(rpm);
 }
 
 int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg)

diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp
@@ -69,11 +69,14 @@ class RPMCapture : public ModuleBase<RPMCapture>, public px4::ScheduledWorkItem
 
 private:
 	static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s;
+	static constexpr uint32_t RPM_MAX_VALUE = 15000;
 
 	void Run() override;
 
 	int _channel{-1};
 	uint32_t _rpm_capture_gpio{0};
+	uint32_t _pulses_per_revolution{1};
+	uint32_t _min_pulse_period_us{1};	///< [us] minimum pulse period
 	uORB::Publication<pwm_input_s> _pwm_input_pub{ORB_ID(pwm_input)};
 	uORB::PublicationMulti<rpm_s> _rpm_pub{ORB_ID(rpm)};
 

diff --git a/src/drivers/rpm_capture/rpm_capture_params.c b/src/drivers/rpm_capture/rpm_capture_params.c
@@ -41,3 +41,15 @@
  * @reboot_required true
  */
 PARAM_DEFINE_INT32(RPM_CAP_ENABLE, 0);
+
+/**
+ * RPM Pulses per Revolution
+ *
+ * Number of pulses per revolution for the RPM sensor.
+ *
+ * @group System
+ * @min 1
+ * @max 50
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(RPM_PULSES_PER_REV, 1);