AP_RangeFinder: use boolean uartdriver read method

This commit is contained in:
Peter Barker 2023-11-29 09:16:22 +11:00 committed by Peter Barker
parent 41f61da0d9
commit 9394a7b26b
10 changed files with 49 additions and 49 deletions

View File

@ -61,10 +61,9 @@ bool AP_RangeFinder_BLPing::get_reading(float &reading_m)
} averageStruct; } averageStruct;
// read any available lines from the lidar // read any available lines from the lidar
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
while (nbytes-- > 0) { uint8_t b;
const int16_t b = uart->read(); if (!uart->read(b)) {
if (b < 0) {
break; break;
} }
if (protocol.parse_byte(b) == PingProtocol::MessageId::DISTANCE_SIMPLE) { if (protocol.parse_byte(b) == PingProtocol::MessageId::DISTANCE_SIMPLE) {

View File

@ -59,13 +59,11 @@ bool AP_RangeFinder_Benewake::get_reading(float &reading_m)
uint16_t count_out_of_range = 0; uint16_t count_out_of_range = 0;
// read any available lines from the lidar // read any available lines from the lidar
int16_t nbytes = uart->available(); for (auto j=0; j<8192; j++) {
while (nbytes-- > 0) { uint8_t c;
int16_t r = uart->read(); if (!uart->read(c)) {
if (r < 0) { break;
continue;
} }
uint8_t c = (uint8_t)r;
// if buffer is empty and this byte is 0x59, add to buffer // if buffer is empty and this byte is 0x59, add to buffer
if (linebuf_len == 0) { if (linebuf_len == 0) {
if (c == BENEWAKE_FRAME_HEADER) { if (c == BENEWAKE_FRAME_HEADER) {

View File

@ -46,10 +46,9 @@ bool AP_RangeFinder_Lanbao::get_reading(float &reading_m)
// format is: [ 0xA5 | 0x5A | distance-MSB-mm | distance-LSB-mm | crc16 ] // format is: [ 0xA5 | 0x5A | distance-MSB-mm | distance-LSB-mm | crc16 ]
// read any available lines from the lidar // read any available lines from the lidar
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
while (nbytes-- > 0) { uint8_t b;
int16_t b = uart->read(); if (!uart->read(b)) {
if (b == -1) {
break; break;
} }
if (buf_len == 0 && b != 0xA5) { if (buf_len == 0 && b != 0xA5) {

View File

@ -51,13 +51,12 @@ bool AP_RangeFinder_LeddarVu8::get_reading(float &reading_m)
bool latest_dist_valid = false; bool latest_dist_valid = false;
// read any available characters from the lidar // read any available characters from the lidar
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
while (nbytes-- > 0) { uint8_t b;
int16_t r = uart->read(); if (!uart->read(b)) {
if (r < 0) { break;
continue;
} }
if (parse_byte((uint8_t)r, latest_dist_valid, latest_dist_cm)) { if (parse_byte(b, latest_dist_valid, latest_dist_cm)) {
if (latest_dist_valid) { if (latest_dist_valid) {
sum_cm += latest_dist_cm; sum_cm += latest_dist_cm;
count++; count++;

View File

@ -40,10 +40,11 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
const int16_t distance_cm_max = max_distance_cm(); const int16_t distance_cm_max = max_distance_cm();
// read any available lines from the lidar // read any available lines from the lidar
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
while (nbytes-- > 0) { uint8_t c;
char c = uart->read(); if (!uart->read(c)) {
break;
}
// use legacy protocol // use legacy protocol
if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::LEGACY) { if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::LEGACY) {
if (c == '\r') { if (c == '\r') {

View File

@ -42,11 +42,13 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(float &reading_m)
} }
int32_t sum = 0; int32_t sum = 0;
int16_t nbytes = uart->available();
uint16_t count = 0; uint16_t count = 0;
while (nbytes-- > 0) { for (auto i=0; i<8192; i++) {
char c = uart->read(); uint8_t c;
if (!uart->read(c)) {
break;
}
if (c == '\r') { if (c == '\r') {
linebuf[linebuf_len] = 0; linebuf[linebuf_len] = 0;
sum += (int)atoi(linebuf); sum += (int)atoi(linebuf);

View File

@ -32,9 +32,11 @@ bool AP_RangeFinder_NMEA::get_reading(float &reading_m)
// read any available lines from the lidar // read any available lines from the lidar
float sum = 0.0f; float sum = 0.0f;
uint16_t count = 0; uint16_t count = 0;
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
while (nbytes-- > 0) { uint8_t c;
char c = uart->read(); if (!uart->read(c)) {
break;
}
if (decode(c)) { if (decode(c)) {
sum += _distance_m; sum += _distance_m;
count++; count++;

View File

@ -54,13 +54,11 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m)
uint16_t bad_read = 0; uint16_t bad_read = 0;
// read any available lines from the lidar // read any available lines from the lidar
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
while (nbytes-- > 0) { uint8_t c;
int16_t r = uart->read(); if (!uart->read(c)) {
if (r < 0) { break;
continue;
} }
uint8_t c = (uint8_t)r;
// if buffer is empty and this byte is 0x57, add to buffer // if buffer is empty and this byte is 0x57, add to buffer
if (linebuf_len == 0) { if (linebuf_len == 0) {
if (c == FRAME_HEADER) { if (c == FRAME_HEADER) {

View File

@ -39,11 +39,11 @@ bool AP_RangeFinder_USD1_Serial::detect_version(void)
uint8_t count = 0; uint8_t count = 0;
// read any available data from USD1_Serial // read any available data from USD1_Serial
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
uint8_t c;
while (nbytes-- > 0) { if (!uart->read(c)) {
uint8_t c = uart->read(); break;
}
if (((c == USD1_HDR_V0) || (c == USD1_HDR)) && !hdr_found) { if (((c == USD1_HDR_V0) || (c == USD1_HDR)) && !hdr_found) {
byte1 = c; byte1 = c;
hdr_found = true; hdr_found = true;
@ -121,11 +121,11 @@ bool AP_RangeFinder_USD1_Serial::get_reading(float &reading_m)
uint16_t count = 0; uint16_t count = 0;
bool hdr_found = false; bool hdr_found = false;
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
uint8_t c;
while (nbytes-- > 0) { if (!uart->read(c)) {
uint8_t c = uart->read(); break;
}
if ((c == _header) && !hdr_found) { if ((c == _header) && !hdr_found) {
// located header byte // located header byte
_linebuf_len = 0; _linebuf_len = 0;

View File

@ -86,9 +86,11 @@ bool AP_RangeFinder_Wasp::get_reading(float &reading_m) {
// read any available lines from the lidar // read any available lines from the lidar
float sum = 0; float sum = 0;
uint16_t count = 0; uint16_t count = 0;
int16_t nbytes = uart->available(); for (auto i=0; i<8192; i++) {
while (nbytes-- > 0) { uint8_t c;
char c = uart->read(); if (!uart->read(c)) {
break;
}
if (c == '\n') { if (c == '\n') {
linebuf[linebuf_len] = 0; linebuf[linebuf_len] = 0;
linebuf_len = 0; linebuf_len = 0;