Browse Source

AP_RangeFinder: use new UARTDriver discard_input method

zr-v5.1
Peter Barker 5 years ago committed by Andrew Tridgell
parent
commit
95f9769041
  1. 13
      libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp

13
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp

@ -32,16 +32,13 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm) @@ -32,16 +32,13 @@ bool AP_RangeFinder_LeddarOne::get_reading(uint16_t &reading_cm)
switch (modbus_status) {
case LEDDARONE_MODBUS_STATE_INIT: {
uint8_t index = 0;
// clear read buffer
uint32_t nbytes = uart->available();
while (nbytes-- > 0) {
uart->read();
if (++index > LEDDARONE_SERIAL_PORT_MAX) {
// LEDDARONE_STATE_ERR_SERIAL_PORT
return false;
}
if (nbytes > LEDDARONE_SERIAL_PORT_MAX) {
// LEDDARONE_STATE_ERR_SERIAL_PORT
return false;
}
uart->discard_input();
// clear buffer and buffer_len
memset(read_buffer, 0, sizeof(read_buffer));
read_len = 0;

Loading…
Cancel
Save