AP_RangeFinder: add serial support for GY-US42v2 rangefinder

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -39,6 +39,7 @@
 #include "AP_RangeFinder_Benewake_TFMini.h"
 #include "AP_RangeFinder_Benewake_TFMiniPlus.h"
 #include "AP_RangeFinder_PWM.h"
+#include "AP_RangeFinder_GYUS42v2.h"
 #include "AP_RangeFinder_HC_SR04.h"
 #include "AP_RangeFinder_BLPing.h"
 #include "AP_RangeFinder_UAVCAN.h"
@@ -527,6 +528,12 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         break;
 #endif
 
+    case Type::GYUS42v2:
+        if (AP_RangeFinder_GYUS42v2::detect(serial_instance)) {
+            drivers[instance] = new AP_RangeFinder_GYUS42v2(state[instance], params[instance], serial_instance++);
+        }
+        break;
+
     default:
         break;
     }

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -80,6 +80,7 @@ public:
         VL53L1X_Short = 28,
         LeddarVu8_Serial = 29,
         HC_SR04 = 30,
+        GYUS42v2 = 31,
     };
 
     enum class Function {

libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp

@@ -0,0 +1,72 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_RangeFinder_GYUS42v2.h"
+#include <ctype.h>
+
+extern const AP_HAL::HAL& hal;
+
+bool AP_RangeFinder_GYUS42v2::find_signature_in_buffer(uint8_t start)
+{
+    for (uint8_t i=start; i<buffer_used; i++) {
+        if (buffer[i] == 0x5A) {
+            memmove(&buffer[0], &buffer[i], buffer_used-i);
+            buffer_used -= i;
+            return true;
+        }
+    }
+    // header byte not in buffer
+    buffer_used = 0;
+    return false;
+}
+
+// get_reading - read a value from the sensor
+bool AP_RangeFinder_GYUS42v2::get_reading(uint16_t &reading_cm)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    const uint8_t num_read = uart->read(&buffer[buffer_used], ARRAY_SIZE(buffer)-buffer_used);
+    buffer_used += num_read;
+
+    if (buffer_used == 0) {
+        return false;
+    }
+
+    if (buffer[0] != 0x5A &&
+        !find_signature_in_buffer(1)) {
+        return false;
+    }
+
+    if (buffer_used < ARRAY_SIZE(buffer)) {
+        return false;
+    }
+
+    uint8_t sum = 0;
+    for (uint8_t i=0; i<6; i++) {
+        sum += buffer[i];
+    }
+    if (buffer[6] != sum) {
+        find_signature_in_buffer(1);
+        return false;
+    }
+
+    reading_cm = buffer[4] << 8 | buffer[5];
+    buffer_used = 0;
+
+    return true;
+}

libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
+class AP_RangeFinder_GYUS42v2 : public AP_RangeFinder_Backend_Serial
+{
+
+public:
+
+    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
+
+protected:
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_ULTRASOUND;
+    }
+
+    uint32_t initial_baudrate(uint8_t serial_instance) const override {
+        return 9600;
+    }
+
+private:
+
+    // get a reading
+    bool get_reading(uint16_t &reading_cm) override;
+
+    // find signature byte in buffer starting at start, moving that
+    // byte and following bytes to start of buffer.
+    bool find_signature_in_buffer(uint8_t start);
+
+    uint8_t buffer[7];
+    uint8_t buffer_used;
+};