fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

RPMCapture: publish outliers in rpm message but don't update the filters and estimate

Browse Source
This commit is contained in:
Matthias Grob 2024-12-10 19:50:35 +01:00
parent 9ca06be4f5
commit d027f8bf39
2 changed files with 13 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/drivers/rpm_capture/RPMCapture.cpp
View File

@ -59,9 +59,6 @@ bool RPMCapture::init()
{
bool success = false;
_min_pulse_period_us = static_cast<uint32_t>(60.f * 1e6f / static_cast<float>(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<float>(_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<float>(_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)

3
src/drivers/rpm_capture/RPMCapture.hpp
View File

@ -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_s> _pwm_input_pub{ORB_ID(pwm_input)};
uORB::PublicationMulti<rpm_s> _rpm_pub{ORB_ID(rpm)};