From 41b55724bc1c88c742bb8392228fb6649fb02f7c Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 27 Nov 2024 14:39:00 +0100
Subject: [PATCH] RPMCapture: Detect rpm sensing timeout

---
 src/drivers/rpm_capture/RPMCapture.cpp | 27 ++++++++++++++++----------
 src/drivers/rpm_capture/RPMCapture.hpp |  1 +
 2 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp
index 3df4b0f3ea..e58b82f8ac 100644
--- a/src/drivers/rpm_capture/RPMCapture.cpp
+++ b/src/drivers/rpm_capture/RPMCapture.cpp
@@ -43,6 +43,7 @@ RPMCapture::RPMCapture() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
 	_pwm_input_pub.advertise();
+	ScheduleOnInterval(1_s);
 }
 
 RPMCapture::~RPMCapture()
@@ -100,21 +101,27 @@ void RPMCapture::Run()
 	}
 
 	hrt_abstime now = hrt_absolute_time();
-	const hrt_abstime period = _hrt_timestamp - _hrt_timestamp_prev;
 
-	pwm_input_s pwm_input{};
-	pwm_input.timestamp = now;
-	pwm_input.period = period;
-	pwm_input.error_count = _error_count;
-	_hrt_timestamp_prev = _hrt_timestamp;
-	_value_processed.store(true);
-	_pwm_input_pub.publish(pwm_input);
+	if (!_value_processed.load()) {
+		_period = _hrt_timestamp - _hrt_timestamp_prev;
+		_hrt_timestamp_prev = _hrt_timestamp;
+		_value_processed.store(true);
+
+		pwm_input_s pwm_input{};
+		pwm_input.timestamp = now;
+		pwm_input.period = _period;
+		pwm_input.error_count = _error_count;
+		_pwm_input_pub.publish(pwm_input);
+
+	} else if (now > _hrt_timestamp_prev + 1_s) {
+		_period = 0;
+	}
 
 	float rpm_raw{0.f};
 
-	if ((1 < period) && (period < 1_s)) {
+	if ((1 < _period) && (_period < 1_s)) {
 		// 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute
-		rpm_raw = 60.f * 1e6f / (static_cast<float>(period) * 1.f);
+		rpm_raw = 60.f * 1e6f / (static_cast<float>(_period) * 1.f);
 	}
 
 	const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f);
diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp
index 309f29a03b..63103ae955 100644
--- a/src/drivers/rpm_capture/RPMCapture.hpp
+++ b/src/drivers/rpm_capture/RPMCapture.hpp
@@ -77,6 +77,7 @@ private:
 
 	hrt_abstime _hrt_timestamp{0};
 	hrt_abstime _hrt_timestamp_prev{0};
+	uint32_t _period{0};
 	uint32_t _error_count{0};
 	px4::atomic<bool> _value_processed{true};