Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder_LightWareSerial: check dist for lost signal flags

Browse Source 
consider messages valid only if they are non-negative AND if the value is not a known lost-signal reading
This commit is contained in:
Wynn, Jesse Stewart 2021-11-17 13:21:48 -08:00 committed by Peter Barker
parent 477528bd93
commit a7321cbae1
2 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -34,6 +34,9 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
uint16_t valid_count = 0; // number of valid readings
uint16_t invalid_count = 0; // number of invalid readings
// max distance the sensor can reliably measure - read from parameters
const int16_t distance_cm_max = max_distance_cm();
// read any available lines from the lidar
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
@ -44,7 +47,7 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
if (c == '\r') {
linebuf[linebuf_len] = 0;
const float dist = strtof(linebuf, nullptr);
if (!is_negative(dist)) {
if (!is_negative(dist) && !is_lost_signal_distance(dist * 100, distance_cm_max)) {
sum += dist;
valid_count++;
// if still determining protocol update legacy valid count
@ -74,8 +77,8 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
} else {
// received the low byte which should be second
if (high_byte_received) {
const float dist = (high_byte & 0x7f) << 7 | (c & 0x7f);
if (!is_negative(dist)) {
const int16_t dist = (high_byte & 0x7f) << 7 | (c & 0x7f);
if (dist >= 0 && !is_lost_signal_distance(dist, distance_cm_max)) {
sum += dist * 0.01f;
valid_count++;
// if still determining protocol update binary valid count
@ -121,10 +124,26 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
// all readings were invalid so return out-of-range-high value
if (invalid_count > 0) {
reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
reading_m = MIN(MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX) * 0.01f;
return true;
}
// no readings so return false
return false;
}
// check to see if distance returned by the LiDAR is a known lost-signal distance flag
bool AP_RangeFinder_LightWareSerial::is_lost_signal_distance(int16_t distance_cm, int16_t distance_cm_max)
{
if (distance_cm < distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM) {
// in-range
return false;
}
const int16_t bad_distances[] { 13000, 16000, 23000, 25000 };
for (const auto bad_distance_cm : bad_distances) {
if (distance_cm == bad_distance_cm) {
return true;
}
}
return false;
}

1
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
View File

@ -19,6 +19,7 @@ protected:
private:
// get a reading
bool get_reading(float &reading_m) override;
bool is_lost_signal_distance(int16_t distance_cm, int16_t distance_cm_max);
char linebuf[10]; // legacy protocol buffer
uint8_t linebuf_len; // legacy protocol buffer length