mirror of https://github.com/ArduPilot/ardupilot
AP_RangeFinder: use boolean uartdriver read method
This commit is contained in:
parent
41f61da0d9
commit
9394a7b26b
|
@ -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) {
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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++;
|
||||||
|
|
|
@ -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') {
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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++;
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue