diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 1f949e7078..93aa3c637e 100644 --- a/src/drivers/rpm_capture/RPMCapture.cpp +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -59,9 +59,6 @@ bool RPMCapture::init() { bool success = false; - _min_pulse_period_us = static_cast(60.f * 1e6f / static_cast(RPM_MAX_VALUE * - _param_rpm_puls_per_rev.get())); - 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); @@ -127,28 +124,26 @@ void RPMCapture::Run() } ScheduleDelayed(RPM_PULSE_TIMEOUT); // Schule a new timeout + float rpm_raw{0.f}; - if (_period > _min_pulse_period_us) { - // Only update if the period is above the min pulse period threshold - float rpm_raw{0.f}; - - if (_period < RPM_PULSE_TIMEOUT) { - // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute - rpm_raw = 60.f * 1e6f / static_cast(_param_rpm_puls_per_rev.get() * _period); - } + if (_period < RPM_PULSE_TIMEOUT) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + rpm_raw = 60.f * 1e6f / static_cast(_param_rpm_puls_per_rev.get() * _period); + } + if (rpm_raw < RPM_MAX_VALUE) { + // Don't update RPM filter with outliers 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_median_filter.apply(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 index 0744c3951f..aec1b126e4 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -71,13 +71,12 @@ public: private: static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s; - static constexpr uint32_t RPM_MAX_VALUE = 15000; + static constexpr float RPM_MAX_VALUE = 50e3f; void Run() override; int _channel{-1}; uint32_t _rpm_capture_gpio{0}; - uint32_t _min_pulse_period_us{1}; ///< [us] minimum pulse period uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; uORB::PublicationMulti _rpm_pub{ORB_ID(rpm)};