AP_RangeFinder: make LightWare I2C native work with more hw versions

this allows the native i2c lightware driver to work with a wide range
of lidars from LightWare, removing the specific version check, and the
version specific config commands

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp

@@ -107,35 +107,25 @@ bool AP_RangeFinder_LightWareI2C::write_bytes(uint8_t *write_buf_u8, uint32_t le
 /**
  * Disables "address tagging" in the sf20 response packets.
  */
-bool AP_RangeFinder_LightWareI2C::sf20_disable_address_tagging()
+void AP_RangeFinder_LightWareI2C::sf20_disable_address_tagging()
 {
-    if (!sf20_send_and_expect("#CT,0\r\n", "ct:0")) {
+    sf20_send_and_expect("#CT,0\r\n", "ct:0");
-        return false;
-    }
-    return true;
-}
-
-bool AP_RangeFinder_LightWareI2C::sf20_product_name_check()
-{
-    if (!sf20_send_and_expect("?P\r\n", "p:LW20,")) {
-        return false;
-    }
-    return true;
 }
 
+/*
+  send a native command and check for an expected reply
+ */
 bool AP_RangeFinder_LightWareI2C::sf20_send_and_expect(const char* send_msg, const char* expected_reply)
 {
-    uint8_t rx_bytes[lx20_max_reply_len_bytes + 1];
+    const size_t expected_reply_len = strlen(expected_reply);
-    size_t expected_reply_len = strlen(expected_reply);
+    uint8_t rx_bytes[expected_reply_len + 1];
+    memset(rx_bytes, 0, sizeof(rx_bytes));
 
     if ((expected_reply_len > lx20_max_reply_len_bytes) ||
         (expected_reply_len < 2)) {
         return false;
     }
 
-    rx_bytes[expected_reply_len] = 0;
-    rx_bytes[2] = 0;
-
     if (!write_bytes((uint8_t*)send_msg,
                      strlen(send_msg))) {
         return false;
@@ -150,13 +140,50 @@ bool AP_RangeFinder_LightWareI2C::sf20_send_and_expect(const char* send_msg, con
         return false;
     }
 
-    if (!_dev->read(rx_bytes, expected_reply_len)) {
+    for (uint8_t i=0; i<10; i++) {
-        return false;
+        if (_dev->read(rx_bytes, expected_reply_len)) {
+            break;
+        }
+        // give a bit of time for the remaining bytes to be available
+        hal.scheduler->delay(1);
     }
 
     return memcmp(rx_bytes, expected_reply, expected_reply_len) == 0;
 }
 
+/*
+  send a native command and fill a reply into a buffer. Used for
+  version string
+ */
+void AP_RangeFinder_LightWareI2C::sf20_get_version(const char* send_msg, const char *reply_prefix, char reply[15])
+{
+    const size_t expected_reply_len = 16;
+    uint8_t rx_bytes[expected_reply_len + 1];
+    memset(rx_bytes, 0, sizeof(rx_bytes));
+
+    if (!write_bytes((uint8_t*)send_msg, strlen(send_msg))) {
+        return;
+    }
+
+    if (!sf20_wait_on_reply(rx_bytes)) {
+        return;
+    }
+
+    if ((rx_bytes[0] != uint8_t(reply_prefix[0])) ||
+        (rx_bytes[1] != uint8_t(reply_prefix[1])) ) {
+        return;
+    }
+
+    for (uint8_t i=0; i<10; i++) {
+        if (_dev->read(rx_bytes, expected_reply_len)) {
+            break;
+        }
+        // give a bit of time for the remaining bytes to be available
+        hal.scheduler->delay(1);
+    }
+    memcpy(reply, &rx_bytes[2], 14);
+}
+
 /* Driver first attempts to initialize the sf20.
  * If for any reason this fails, the driver attempts to initialize the legacy LightWare LiDAR.
  * If this fails, the driver returns false indicating no LightWare LiDAR is present.
@@ -212,22 +239,22 @@ bool AP_RangeFinder_LightWareI2C::legacy_init()
  */
 bool AP_RangeFinder_LightWareI2C::sf20_init()
 {
-    // Makes sure that "address tagging" is turned off.
+    // version strings for reporting
-    // Address tagging starts every response with "0x66".
+    char version[15] {};
-    // Turns off Address Tagging just in case it was previously left on in the non-volatile configuration.
-    if (!sf20_disable_address_tagging()) {
-        return false;
-    }
 
-    if (!sf20_product_name_check()) {
+    sf20_get_version("?P\r\n", "p:", version);
-        return false;
-    }
 
-    // Disconnect the servo.
+    if (version[0]) {
-    if (!sf20_send_and_expect("#SC,0\r\n", "sc:0")) {
+        hal.console->printf("SF20 Lidar version %s\n", version);
-        return false;
     }
 
+    // Makes sure that "address tagging" is turned off.
+    // Address tagging starts every response with "0x66".
+    // Turns off Address Tagging just in case it was previously left on in the non-volatile configuration.
+    sf20_disable_address_tagging();
+    // Disconnect the servo (if applicable)
+    sf20_send_and_expect("#SC,0\r\n", "sc:0");
+
     // Change the power consumption:
     // 0 = power off
     // 1 = power on
@@ -254,13 +281,15 @@ bool AP_RangeFinder_LightWareI2C::sf20_init()
     }
 #endif
 
-    /* Sets the Laser Encoding to a fixed pattern to assess how well it improves operation with interference from
+#if 0
-     * the Precision Landing infrared beacon.
+    /*
+      this can be used to change the laser encoding pattern
+      Changes the laser encoding pattern: 3 (Random A) [0..4].
     */
-    // Changes the laser encoding pattern: 3 (Random A) [0..4].
     if (!sf20_send_and_expect("#LE,3\r\n", "le:3")) {
         return false;
     }
+#endif
 
     // Sets datum offset [-10.00 ... 10.00].
     if (!sf20_send_and_expect("#LO,0.00\r\n", "lo:0.00")) {

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.h

@@ -36,10 +36,10 @@ private:
                                 AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
 
     bool write_bytes(uint8_t *write_buf_u8, uint32_t len_u8);
-    bool sf20_disable_address_tagging();
+    void sf20_disable_address_tagging();
-    bool sf20_product_name_check();
     bool sf20_send_and_expect(const char* send, const char* expected_reply);
     bool sf20_set_lost_signal_confirmations();
+    void sf20_get_version(const char* send_msg, const char *reply_prefix, char reply[5]);
     bool sf20_wait_on_reply(uint8_t *rx_two_bytes);
     bool init();
     bool legacy_init();