From 1b105c937d9e32cec0c704966e5562da9925e58d Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 29 Nov 2024 09:30:13 +0100
Subject: [PATCH] RpmControl: remove raw RPM spikes using median filter

---
 src/drivers/rpm_capture/RPMCapture.cpp | 2 +-
 src/drivers/rpm_capture/RPMCapture.hpp | 2 ++
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp
index 4783e5b7ea..432bb21d4b 100644
--- a/src/drivers/rpm_capture/RPMCapture.cpp
+++ b/src/drivers/rpm_capture/RPMCapture.cpp
@@ -138,7 +138,7 @@ void RPMCapture::Run()
 	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_filter.update(_rpm_median_filter.apply(rpm_raw));
 
 	rpm_s rpm{};
 	rpm.timestamp = now;
diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp
index a2156437a4..a986fc9ceb 100644
--- a/src/drivers/rpm_capture/RPMCapture.hpp
+++ b/src/drivers/rpm_capture/RPMCapture.hpp
@@ -35,6 +35,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
 #include <px4_arch/micro_hal.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -85,4 +86,5 @@ private:
 
 	hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt
 	AlphaFilter<float> _rpm_filter;
+	MedianFilter<float, 5> _rpm_median_filter;
 };