diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
index 5976aaec6f..b0ca9a4f5c 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp
+++ b/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;
}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h
index ddc70f3517..b52e6030f9 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.h
+++ b/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 {
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.cpp
new file mode 100644
index 0000000000..c67584c064
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include "AP_RangeFinder_GYUS42v2.h"
+#include
+
+extern const AP_HAL::HAL& hal;
+
+bool AP_RangeFinder_GYUS42v2::find_signature_in_buffer(uint8_t start)
+{
+ for (uint8_t i=start; iread(&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;
+}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h b/libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h
new file mode 100644
index 0000000000..4a7da24130
--- /dev/null
+++ b/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;
+};