Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: HC-SR04: Add glitch filter

Browse Source
This commit is contained in:
Henry Wurzburg 2020-05-29 17:52:38 -05:00 committed by Andrew Tridgell
parent fc1ea612b1
commit f9b02a6814
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.cpp
View File

@ -157,6 +157,24 @@ void AP_RangeFinder_HC_SR04::update(void)
// gcs().send_text(MAV_SEVERITY_WARNING, "Pong!");
// a new reading - convert time to distance
state.distance_cm = value_us * (1.0/58.0f); // 58 is from datasheet, mult for performance
// glitch remover: measurement is greater than .5m from last.
// the SR-04 seeems to suffer from single-measurement glitches
// which can be removed by a simple filter.
if (labs(state.distance_cm - last_distance_cm) > 50) {
// if greater for 5 readings then pass it as new height,
// otherwise use last reading
if (glitch_count++ > 4) {
last_distance_cm = state.distance_cm;
} else {
state.distance_cm = last_distance_cm;
}
} else {
// is not greater 0.5m, pass on and reset glitch counter
last_distance_cm = state.distance_cm;
glitch_count = 0;
}
last_reading_ms = now;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_HC_SR04.h
View File

@ -32,6 +32,8 @@ private:
int8_t echo_pin;
int8_t trigger_pin;
uint32_t last_reading_ms; // system time of last read (used for health reporting)
uint32_t last_distance_cm; // last distance reported (used to prevent glitches in measurement)
uint8_t glitch_count; // glitch counter
// follow are modified by the IRQ handler:
uint32_t pulse_start_us; // system time of start of timing pulse