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; +};