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

AP_RangeFinder: LightwareSerial driver handles invalid distances 

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 committed by Andrew Tridgell
parent d515b40966
commit 4b47e6bf91
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;
#define LIGHTWARE_DIST_MAX_CM 10000U
#define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100U
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
@ -53,18 +56,25 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
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
float sum = 0;
uint16_t count = 0;
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
char c = uart->read();
if (c == '\r') {
linebuf[linebuf_len] = 0;
sum += (float)atof(linebuf);
count++;
const float dist = (float)atof(linebuf);
if (!is_negative(dist)) {
sum += dist;
valid_count++;
} else {
invalid_count++;
}
linebuf_len = 0;
} else if (isdigit(c) || c == '.') {
} else if (isdigit(c) || c == '.' || c == '-') {
linebuf[linebuf_len++] = c;
if (linebuf_len == sizeof(linebuf)) {
// too long, discard the line
@ -86,11 +96,20 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
uart->write('d');
}
if (count == 0) {
return false;
// return average of all valid readings
if (valid_count > 0) {
reading_cm = 100 * sum / valid_count;
return true;
}
reading_cm = 100 * sum / count;
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, unsigned(max_distance_cm()) + LIGHTWARE_OUT_OF_RANGE_ADD_CM), unsigned(UINT16_MAX));
return true;
}
// no readings so return false
return false;
}
/*