diff --git a/msg/Rpm.msg b/msg/Rpm.msg index baab7c6a67..7a6b179b84 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -2,3 +2,6 @@ uint64 timestamp # time since system start (microseconds) float32 indicated_frequency_rpm # indicated rotor Frequency in Revolution per minute float32 estimated_accurancy_rpm # estimated accuracy in Revolution per minute + +float32 rpm_raw # measured rpm +float32 rpm_estimate # filtered rpm diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 397f6060c9..3df4b0f3ea 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -99,13 +99,34 @@ void RPMCapture::Run() return; } + hrt_abstime now = hrt_absolute_time(); + const hrt_abstime period = _hrt_timestamp - _hrt_timestamp_prev; + pwm_input_s pwm_input{}; - pwm_input.timestamp = hrt_absolute_time(); - pwm_input.period = _hrt_timestamp - _hrt_timestamp_prev; + 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); + + float rpm_raw{0.f}; + + if ((1 < period) && (period < 1_s)) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + rpm_raw = 60.f * 1e6f / (static_cast(period) * 1.f); + } + + 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); } 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 index b386c46ac7..309f29a03b 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -34,12 +34,14 @@ #pragma once #include +#include #include #include #include #include #include #include +#include using namespace time_literals; @@ -71,9 +73,13 @@ private: int _channel{-1}; uint32_t _rpm_capture_gpio{0}; uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; + uORB::Publication _rpm_pub{ORB_ID(rpm)}; hrt_abstime _hrt_timestamp{0}; hrt_abstime _hrt_timestamp_prev{0}; uint32_t _error_count{0}; px4::atomic _value_processed{true}; + + hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt + AlphaFilter _rpm_filter; };