diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp index 3cd40aa298..e7776e0f95 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp @@ -61,10 +61,9 @@ bool AP_RangeFinder_BLPing::get_reading(float &reading_m) } averageStruct; // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - const int16_t b = uart->read(); - if (b < 0) { + for (auto i=0; i<8192; i++) { + uint8_t b; + if (!uart->read(b)) { break; } if (protocol.parse_byte(b) == PingProtocol::MessageId::DISTANCE_SIMPLE) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index ceea2fd168..f04056588d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -59,13 +59,11 @@ bool AP_RangeFinder_Benewake::get_reading(float &reading_m) uint16_t count_out_of_range = 0; // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - int16_t r = uart->read(); - if (r < 0) { - continue; + for (auto j=0; j<8192; j++) { + uint8_t c; + if (!uart->read(c)) { + break; } - uint8_t c = (uint8_t)r; // if buffer is empty and this byte is 0x59, add to buffer if (linebuf_len == 0) { if (c == BENEWAKE_FRAME_HEADER) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp index d59b030e50..16c37db38f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp @@ -46,10 +46,9 @@ bool AP_RangeFinder_Lanbao::get_reading(float &reading_m) // format is: [ 0xA5 | 0x5A | distance-MSB-mm | distance-LSB-mm | crc16 ] // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - int16_t b = uart->read(); - if (b == -1) { + for (auto i=0; i<8192; i++) { + uint8_t b; + if (!uart->read(b)) { break; } if (buf_len == 0 && b != 0xA5) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp index 04e38624e0..3d90877d83 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp @@ -51,13 +51,12 @@ bool AP_RangeFinder_LeddarVu8::get_reading(float &reading_m) bool latest_dist_valid = false; // read any available characters from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - int16_t r = uart->read(); - if (r < 0) { - continue; + for (auto i=0; i<8192; i++) { + uint8_t b; + if (!uart->read(b)) { + break; } - 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) { sum_cm += latest_dist_cm; count++; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index 9636b47bf2..61c2ed9c2d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -40,10 +40,11 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m) const int16_t distance_cm_max = max_distance_cm(); // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - char c = uart->read(); - + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } // use legacy protocol if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::LEGACY) { if (c == '\r') { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index 7f0173947c..50abcc6bce 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp @@ -42,11 +42,13 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(float &reading_m) } int32_t sum = 0; - int16_t nbytes = uart->available(); uint16_t count = 0; - while (nbytes-- > 0) { - char c = uart->read(); + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if (c == '\r') { linebuf[linebuf_len] = 0; sum += (int)atoi(linebuf); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index 61945ca7c3..bc6a3cf391 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -32,9 +32,11 @@ bool AP_RangeFinder_NMEA::get_reading(float &reading_m) // read any available lines from the lidar float sum = 0.0f; uint16_t count = 0; - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - char c = uart->read(); + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if (decode(c)) { sum += _distance_m; count++; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp index e9a502de79..088472b6b8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp @@ -54,13 +54,11 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m) uint16_t bad_read = 0; // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - int16_t r = uart->read(); - if (r < 0) { - continue; + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; } - uint8_t c = (uint8_t)r; // if buffer is empty and this byte is 0x57, add to buffer if (linebuf_len == 0) { if (c == FRAME_HEADER) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp index 04d8c06329..26328657d1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp @@ -39,11 +39,11 @@ bool AP_RangeFinder_USD1_Serial::detect_version(void) uint8_t count = 0; // read any available data from USD1_Serial - int16_t nbytes = uart->available(); - - while (nbytes-- > 0) { - uint8_t c = uart->read(); - + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if (((c == USD1_HDR_V0) || (c == USD1_HDR)) && !hdr_found) { byte1 = c; hdr_found = true; @@ -121,11 +121,11 @@ bool AP_RangeFinder_USD1_Serial::get_reading(float &reading_m) uint16_t count = 0; bool hdr_found = false; - int16_t nbytes = uart->available(); - - while (nbytes-- > 0) { - uint8_t c = uart->read(); - + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if ((c == _header) && !hdr_found) { // located header byte _linebuf_len = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp index cda423c79b..5885b2af14 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp @@ -86,9 +86,11 @@ bool AP_RangeFinder_Wasp::get_reading(float &reading_m) { // 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(); + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if (c == '\n') { linebuf[linebuf_len] = 0; linebuf_len = 0;