mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 07:44:03 -04:00
AP_RangeFinder: HC-SR04: Add glitch filter
This commit is contained in:
parent
fc1ea612b1
commit
f9b02a6814
@ -157,6 +157,24 @@ void AP_RangeFinder_HC_SR04::update(void)
|
|||||||
// gcs().send_text(MAV_SEVERITY_WARNING, "Pong!");
|
// gcs().send_text(MAV_SEVERITY_WARNING, "Pong!");
|
||||||
// a new reading - convert time to distance
|
// a new reading - convert time to distance
|
||||||
state.distance_cm = value_us * (1.0/58.0f); // 58 is from datasheet, mult for performance
|
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;
|
last_reading_ms = now;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -32,6 +32,8 @@ private:
|
|||||||
int8_t echo_pin;
|
int8_t echo_pin;
|
||||||
int8_t trigger_pin;
|
int8_t trigger_pin;
|
||||||
uint32_t last_reading_ms; // system time of last read (used for health reporting)
|
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:
|
// follow are modified by the IRQ handler:
|
||||||
uint32_t pulse_start_us; // system time of start of timing pulse
|
uint32_t pulse_start_us; // system time of start of timing pulse
|
||||||
|
Loading…
Reference in New Issue
Block a user