Browse Source

AP_RangeFinder: fixed lightware serial with LW20 lidar

it needs a longer serial write to force it to serial mode from i2c
mission-4.1.18
Andrew Tridgell 6 years ago
parent
commit
ccfaaca82b
  1. 14
      libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp
  2. 1
      libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h

14
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp

@ -74,8 +74,18 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm) @@ -74,8 +74,18 @@ bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
}
}
// we need to write a byte to prompt another reading
uart->write('d');
uint32_t now = AP_HAL::millis();
if (last_init_ms == 0 ||
(now - last_init_ms > 1000 &&
now - state.last_reading_ms > 1000)) {
// send enough serial transitions to trigger LW20 into serial
// mode. It starts in dual I2C/serial mode, and wants to see
// enough transitions to switch into serial mode.
uart->write("www\r\n");
last_init_ms = now;
} else {
uart->write('d');
}
if (count == 0) {
return false;

1
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h

@ -32,4 +32,5 @@ private: @@ -32,4 +32,5 @@ private:
AP_HAL::UARTDriver *uart = nullptr;
char linebuf[10];
uint8_t linebuf_len = 0;
uint32_t last_init_ms;
};

Loading…
Cancel
Save