diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp index 432bb21d4b..c9c831b6db 100644 --- 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(60.f * 1e6f / static_cast(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(_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(_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_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); } - 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); } 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 a986fc9ceb..872ab06a5b 100644 --- a/src/drivers/rpm_capture/RPMCapture.hpp +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -70,11 +70,14 @@ public: 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_pub{ORB_ID(pwm_input)}; uORB::PublicationMulti _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 index 7397177370..08502f6982 100644 --- 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);