From 514d4fd57b72ce97d6bed27c11ffffaaf04a1ca3 Mon Sep 17 00:00:00 2001 From: SalimTerryLi Date: Mon, 3 May 2021 00:09:35 +0800 Subject: [PATCH] drivers/distance_sensor: GY-SR04 sonar range finder driver --- src/drivers/distance_sensor/CMakeLists.txt | 1 + .../distance_sensor/gy_us42/CMakeLists.txt | 43 ++++++ .../distance_sensor/gy_us42/GY_US42.cpp | 133 ++++++++++++++++++ .../distance_sensor/gy_us42/GY_US42.hpp | 103 ++++++++++++++ .../distance_sensor/gy_us42/GY_US42_main.cpp | 107 ++++++++++++++ src/drivers/drv_sensor.h | 1 + 6 files changed, 388 insertions(+) create mode 100644 src/drivers/distance_sensor/gy_us42/CMakeLists.txt create mode 100644 src/drivers/distance_sensor/gy_us42/GY_US42.cpp create mode 100644 src/drivers/distance_sensor/gy_us42/GY_US42.hpp create mode 100644 src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index fa20cf8fbd..1c9e783799 100644 --- a/src/drivers/distance_sensor/CMakeLists.txt +++ b/src/drivers/distance_sensor/CMakeLists.txt @@ -46,3 +46,4 @@ add_subdirectory(tfmini) add_subdirectory(ulanding_radar) add_subdirectory(vl53l0x) add_subdirectory(vl53l1x) +add_subdirectory(gy_us42) diff --git a/src/drivers/distance_sensor/gy_us42/CMakeLists.txt b/src/drivers/distance_sensor/gy_us42/CMakeLists.txt new file mode 100644 index 0000000000..7d59f37cb4 --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__distance_sensor__gy_us42 + MAIN gy_us42 + SRCS + GY_US42.cpp + GY_US42.hpp + GY_US42_main.cpp + DEPENDS + drivers_rangefinder + px4_work_queue + ) diff --git a/src/drivers/distance_sensor/gy_us42/GY_US42.cpp b/src/drivers/distance_sensor/gy_us42/GY_US42.cpp new file mode 100644 index 0000000000..81643d8791 --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/GY_US42.cpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "GY_US42.hpp" + +GY_US42::GY_US42(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) : + I2C(DRV_DIST_DEVTYPE_GY_US42, MODULE_NAME, bus, address, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _px4_rangefinder(get_device_id(), rotation) +{ + _px4_rangefinder.set_max_distance(GY_US42_MAX_DISTANCE); + _px4_rangefinder.set_min_distance(GY_US42_MIN_DISTANCE); +} + +GY_US42::~GY_US42() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int GY_US42::init() +{ + // I2C init (and probe) first. + if (I2C::init() != OK) { + return PX4_ERROR; + } + + _state = STATE::POWERON_WAIT; + ScheduleDelayed(2000); + return OK; +} + +int GY_US42::collect() +{ + // Read from the sensor. + uint8_t val[2] = {}; + perf_begin(_sample_perf); + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = transfer(nullptr, 0, val, 2); + + if (ret < 0) { + PX4_DEBUG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + + _px4_rangefinder.update(timestamp_sample, distance_m); + + perf_end(_sample_perf); + return PX4_OK; +} + +int GY_US42::measure() +{ + uint8_t cmd[1] = {GY_US42_TAKE_RANGE_REG}; + + // Send the command to begin a measurement. + int ret = transfer(cmd, 1, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + return PX4_OK; +} + +void GY_US42::RunImpl() +{ + switch (_state) { + case STATE::INIT: + // do nothing + break; + + case STATE::POWERON_WAIT: + measure(); + _state = STATE::MEASURE_WAIT; + ScheduleOnInterval(GY_US42_CONVERSION_INTERVAL, GY_US42_CONVERSION_INTERVAL); + break; + + case STATE::MEASURE_WAIT: + collect(); + measure(); + // forever loop + break; + + case STATE::MODIFYADDR_WAIT: + break; + } +} + +void GY_US42::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/distance_sensor/gy_us42/GY_US42.hpp b/src/drivers/distance_sensor/gy_us42/GY_US42.hpp new file mode 100644 index 0000000000..f051a5f974 --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/GY_US42.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file GY_US42.cpp + * + * Driver for the GY-US42 sonar range finder on I2C. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +/* Configuration Constants */ +#define GY_US42_BASEADDR 0x70 // 7-bit address. 8-bit address is 0xE0. + +/* GY_US42 Registers addresses */ +#define GY_US42_TAKE_RANGE_REG 0x51 // Measure range Register. +#define GY_US42_SET_ADDRESS_CMD1 0xAA // Change address 1 cmd. +#define GY_US42_SET_ADDRESS_CMD2 0xA5 // Change address 2 cmd. + +/* Device limits */ +#define GY_US42_MIN_DISTANCE (0.20f) +#define GY_US42_MAX_DISTANCE (7.2f) + +#define GY_US42_CONVERSION_INTERVAL 50000 // 50ms for one sonar. + +class GY_US42 : public device::I2C, public I2CSPIDriver +{ +public: + GY_US42(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, + int address = GY_US42_BASEADDR); + ~GY_US42() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + int init() override; + void print_status() override; + + void RunImpl(); + +private: + + enum class STATE : uint8_t { + INIT, + POWERON_WAIT, + MEASURE_WAIT, + MODIFYADDR_WAIT + }; + STATE _state{STATE::INIT}; + + int collect(); + int measure(); + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + + PX4Rangefinder _px4_rangefinder; + + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; +}; diff --git a/src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp b/src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp new file mode 100644 index 0000000000..8392912846 --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "GY_US42.hpp" + +#include +#include + +void +GY_US42::print_usage() +{ + PRINT_MODULE_USAGE_NAME("gy_us42", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +I2CSPIDriverBase *GY_US42::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + GY_US42 *instance = new GY_US42(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + return instance; +} + +extern "C" __EXPORT int gy_us42_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = GY_US42; + BusCLIArguments cli{true, false}; + cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.default_i2c_frequency = 100000; + + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.orientation = atoi(cli.optarg()); + break; + } + } + + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_GY_US42); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index eb589d656a..b004834fb9 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -176,6 +176,7 @@ #define DRV_DIST_DEVTYPE_SIM 0x9a #define DRV_DIST_DEVTYPE_SRF05 0x9b +#define DRV_DIST_DEVTYPE_GY_US42 0x9c #define DRV_GPS_DEVTYPE_ASHTECH 0xA0