fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 16:33:33 +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-19 07:30:25 +01:00
parent 9ca06be4f5
commit d027f8bf39
2 changed files with 13 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/rpm_capture/RPMCapture.cpp
+12 -17
View File
@@ -59,9 +59,6 @@ bool RPMCapture::init()
{ {
bool success = false; 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) { for (unsigned i = 0; i < 16; ++i) {
char param_name[17]; char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); 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 ScheduleDelayed(RPM_PULSE_TIMEOUT); // Schule a new timeout
float rpm_raw{0.f};
if (_period > _min_pulse_period_us) { if (_period < RPM_PULSE_TIMEOUT) {
// Only update if the period is above the min pulse period threshold // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute
float rpm_raw{0.f}; 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); const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f);
_timestamp_last_update = now; _timestamp_last_update = now;
_rpm_filter.setParameters(dt, 0.5f); _rpm_filter.setParameters(dt, 0.5f);
_rpm_filter.update(_rpm_median_filter.apply(rpm_raw)); _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) int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg)
src/drivers/rpm_capture/RPMCapture.hpp
+1 -2
View File
@@ -71,13 +71,12 @@ public:
private: private:
static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s; 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; void Run() override;
int _channel{-1}; int _channel{-1};
uint32_t _rpm_capture_gpio{0}; 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::Publication<pwm_input_s> _pwm_input_pub{ORB_ID(pwm_input)};
uORB::PublicationMulti<rpm_s> _rpm_pub{ORB_ID(rpm)}; uORB::PublicationMulti<rpm_s> _rpm_pub{ORB_ID(rpm)};