aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RangeFinder: LightwareSerial driver handles invalid distances

Browse Source 
reports the longer of 100m or RNGFNDx_MAX_CM+1m
This commit is contained in:
Randy Mackay 2019-07-27 15:05:42 +09:00
parent fc65e04ef2
commit 480a3ebb03
1 changed files with 28 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
View File

@ -20,6 +20,9 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define LIGHTWARE_DIST_MAX_CM 10000
#define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100
/* /*
The constructor also initialises the rangefinder. Note that this The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we constructor is not called until detect() returns true, so we
@ -54,18 +57,25 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
return false; return false;
} }
float sum = 0; // sum of all readings taken
uint16_t valid_count = 0; // number of valid readings
uint16_t invalid_count = 0; // number of invalid readings
// read any available lines from the lidar // read any available lines from the lidar
float sum = 0;
uint16_t count = 0;
int16_t nbytes = uart->available(); int16_t nbytes = uart->available();
while (nbytes-- > 0) { while (nbytes-- > 0) {
char c = uart->read(); char c = uart->read();
if (c == '\r') { if (c == '\r') {
linebuf[linebuf_len] = 0; linebuf[linebuf_len] = 0;
sum += (float)atof(linebuf); const float dist = (float)atof(linebuf);
count++; if (!is_negative(dist)) {
sum += dist;
valid_count++;
} else {
invalid_count++;
}
linebuf_len = 0; linebuf_len = 0;
} else if (isdigit(c) || c == '.') { } else if (isdigit(c) || c == '.' || c == '-') {
linebuf[linebuf_len++] = c; linebuf[linebuf_len++] = c;
if (linebuf_len == sizeof(linebuf)) { if (linebuf_len == sizeof(linebuf)) {
// too long, discard the line // too long, discard the line
@ -87,13 +97,22 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
uart->write('d'); uart->write('d');
} }
if (count == 0) { // return average of all valid readings
return false; if (valid_count > 0) {
} reading_cm = 100 * sum / valid_count;
reading_cm = 100 * sum / count;
return true; return true;
} }
// all readings were invalid so return out-of-range-high value
if (invalid_count > 0) {
reading_cm = MIN(MAX(LIGHTWARE_DIST_MAX_CM, max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX);
return true;
}
// no readings so return false
return false;
}
/* /*
update the state of the sensor update the state of the sensor
*/ */