Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: reduce change of false rangefinder readings triggering a flare 

This doesn't count repeated readings towards the rangefinder count,
and resets the counter if the change is more than 20% of full
range. This greatly reduces the impact of poor sonar sensors for
landing
This commit is contained in:
Andrew Tridgell 2016-10-23 15:53:12 +11:00
parent 68f0777dac
commit 5ac62c27e2
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -230,6 +230,7 @@ private:
uint32_t last_correction_time_ms; uint32_t last_correction_time_ms;
uint8_t in_range_count; uint8_t in_range_count;
float height_estimate; float height_estimate;
float last_distance;
} rangefinder_state; } rangefinder_state;
#endif #endif

9
ArduPlane/altitude.cpp
View File

@ -601,6 +601,7 @@ float Plane::rangefinder_correction(void)
void Plane::rangefinder_height_update(void) void Plane::rangefinder_height_update(void)
{ {
float distance = rangefinder.distance_cm()*0.01f; float distance = rangefinder.distance_cm()*0.01f;
if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) { if ((rangefinder.status() == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) {
if (!rangefinder_state.have_initial_reading) { if (!rangefinder_state.have_initial_reading) {
rangefinder_state.have_initial_reading = true; rangefinder_state.have_initial_reading = true;
@ -616,9 +617,14 @@ void Plane::rangefinder_height_update(void)
// catch Lidars that are giving a constant range, either due // catch Lidars that are giving a constant range, either due
// to misconfiguration or a faulty sensor // to misconfiguration or a faulty sensor
if (rangefinder_state.in_range_count < 10) { if (rangefinder_state.in_range_count < 10) {
if (fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm()*0.01f) { if (!is_equal(distance, rangefinder_state.last_distance) &&
fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm()*0.01f) {
rangefinder_state.in_range_count++; rangefinder_state.in_range_count++;
} }
if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm()*0.01*0.2) {
// changes by more than 20% of full range will reset counter
rangefinder_state.in_range_count = 0;
}
} else { } else {
rangefinder_state.in_range = true; rangefinder_state.in_range = true;
if (!rangefinder_state.in_use && if (!rangefinder_state.in_use &&
@ -633,6 +639,7 @@ void Plane::rangefinder_height_update(void)
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
} }
} }
rangefinder_state.last_distance = distance;
} else { } else {
rangefinder_state.in_range_count = 0; rangefinder_state.in_range_count = 0;
rangefinder_state.in_range = false; rangefinder_state.in_range = false;