diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 0c92602a25..458b9a7878 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -113,6 +113,12 @@ then vl53l1x start -X fi +# tf02 pro i2c distance sensor +if param compare -s SENS_EN_TF02PRO 1 +then + tf02pro start -X +fi + # ADIS16448 spi external IMU if param compare -s SENS_EN_ADIS164X 1 then diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index e7e3b632f6..dedcc94486 100644 --- a/src/drivers/distance_sensor/CMakeLists.txt +++ b/src/drivers/distance_sensor/CMakeLists.txt @@ -48,3 +48,4 @@ add_subdirectory(ulanding_radar) add_subdirectory(vl53l0x) add_subdirectory(vl53l1x) add_subdirectory(gy_us42) +add_subdirectory(tf02pro) diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig index 3bbc73ee57..ee98afed7e 100644 --- a/src/drivers/distance_sensor/Kconfig +++ b/src/drivers/distance_sensor/Kconfig @@ -18,6 +18,7 @@ menu "Distance sensors" select DRIVERS_DISTANCE_SENSOR_VL53L0X select DRIVERS_DISTANCE_SENSOR_VL53L1X select DRIVERS_DISTANCE_SENSOR_GY_US42 + select DRIVERS_DISTANCE_SENSOR_TF02PRO ---help--- Enable default set of distance sensor drivers diff --git a/src/drivers/distance_sensor/tf02pro/CMakeLists.txt b/src/drivers/distance_sensor/tf02pro/CMakeLists.txt new file mode 100644 index 0000000000..c09f78f820 --- /dev/null +++ b/src/drivers/distance_sensor/tf02pro/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2023 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__tf02pro + MAIN tf02pro + SRCS + TF02PRO.cpp + TF02PRO.hpp + tf02pro_main.cpp + DEPENDS + drivers_rangefinder + px4_work_queue + ) diff --git a/src/drivers/distance_sensor/tf02pro/Kconfig b/src/drivers/distance_sensor/tf02pro/Kconfig new file mode 100644 index 0000000000..0bf67f9bbd --- /dev/null +++ b/src/drivers/distance_sensor/tf02pro/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_TF02PRO + bool "tf02pro" + default n + ---help--- + Enable support for tf02pro diff --git a/src/drivers/distance_sensor/tf02pro/TF02PRO.cpp b/src/drivers/distance_sensor/tf02pro/TF02PRO.cpp new file mode 100644 index 0000000000..9cca1ec5ff --- /dev/null +++ b/src/drivers/distance_sensor/tf02pro/TF02PRO.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "TF02PRO.hpp" + +/** + * @brief Construct a new TF02PRO::TF02PRO object + * + * @param config + */ +TF02PRO::TF02PRO(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_rangefinder(get_device_id(), config.rotation) +{ + _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_TF02PRO); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); + _px4_rangefinder.set_max_distance(TF02PRO_MAX_DISTANCE); + _px4_rangefinder.set_min_distance(TF02PRO_MIN_DISTANCE); + _px4_rangefinder.set_fov(math::radians(3.0f)); +} + +/** + * @brief Destroy the TF02PRO::TF02PRO object + * + */ +TF02PRO::~TF02PRO() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +/** + * @brief + * + */ +void TF02PRO::start() +{ + _collect_phase = false; + + ScheduleDelayed(5); // 5 us +} + +/** + * @brief + * + * @return int + */ +int TF02PRO::init() +{ + if (I2C::init() != OK) { + return PX4_ERROR; + } + + px4_usleep(100000); + + int ret = measure(); + + if (ret == PX4_OK) { + start(); + } + + return ret; +} + +/** + * @brief + * + * @return int + * @Note + * Receive Frame + * Byte0: 0x59, frame header, same for each frame + * Byte1: 0x59, frame header, same for each frame + * Byte2: Dist_L distance value low 8 bits + * Byte3: Dist_H distance value high 8 bits + * Byte4: Strength_L low 8 bits + * Byte5: Strength_H high 8 bits + * Byte6: Temp_L low 8 bits + * Byte7: Temp_H high 8 bits + * Byte8: Checksum is the lower 8 bits of the cumulative sum of the number of the first 8 bytes + * + */ +int TF02PRO::collect() +{ + uint8_t recv_data[9] {}; + perf_begin(_sample_perf); + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = transfer(nullptr, 0, recv_data, sizeof(recv_data)); + + if (ret < 0) { + PX4_DEBUG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t strength = recv_data[5] << 8 | recv_data[4]; + uint16_t distance_mm = recv_data[3] << 8 | recv_data[2]; + + if (strength >= 60u && distance_mm < 45000u) { + float distance_m = float(distance_mm) * 1e-3f; + + _px4_rangefinder.update(timestamp_sample, distance_m); + } + + perf_end(_sample_perf); + return PX4_OK; +} + +/** + * @brief + * + * @return int + */ +int TF02PRO::measure() +{ + uint8_t obtain_Data_mm[5] = {0x5A, 0x05, 0x00, 0x06, 0x65}; + + int ret = transfer(obtain_Data_mm, sizeof(obtain_Data_mm), nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + return PX4_OK; +} + +/** + * @brief + * + */ +void TF02PRO::RunImpl() +{ + if (_collect_phase) { + if (OK != collect()) { + PX4_DEBUG("collection error"); + start(); + return; + } + + _collect_phase = false; + } + + if (OK != measure()) { + PX4_DEBUG("measure error I2C adress"); + } + + _collect_phase = true; + + ScheduleDelayed(_interval); +} + +/** + * @brief + * + */ +void TF02PRO::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/distance_sensor/tf02pro/TF02PRO.hpp b/src/drivers/distance_sensor/tf02pro/TF02PRO.hpp new file mode 100644 index 0000000000..e53a351075 --- /dev/null +++ b/src/drivers/distance_sensor/tf02pro/TF02PRO.hpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 TF02PRO.cpp + * + * Driver for the SRF02 sonar range finder adapted from the Maxbotix sonar range finder driver (srf02). + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +/* Configuration Constants */ +#define TF02PRO_BASEADDR 0x10 // 7-bit address. 8-bit address is 0x20. + +/* Device limits */ +#define TF02PRO_MIN_DISTANCE (0.10f) +#define TF02PRO_MAX_DISTANCE (35.00f) + +#define TF02PRO_CONVERSION_INTERVAL 100000 // 100 ms for one sensor. + +class TF02PRO : public device::I2C, public I2CSPIDriver +{ +public: + TF02PRO(const I2CSPIDriverConfig &config); + ~TF02PRO() override; + + static void print_usage(); + + int init() override; + void print_status() override; + + void RunImpl(); + +private: + + void start(); + 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. + */ + int probe_address(uint8_t address); + + PX4Rangefinder _px4_rangefinder; + + int _interval{TF02PRO_CONVERSION_INTERVAL}; + + bool _collect_phase{false}; + + 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/tf02pro/parameters.c b/src/drivers/distance_sensor/tf02pro/parameters.c new file mode 100644 index 0000000000..e37acc064b --- /dev/null +++ b/src/drivers/distance_sensor/tf02pro/parameters.c @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +/** + * TF02 Pro Distance Sensor (i2c) + * + * @reboot_required true + * + * @boolean + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_EN_TF02PRO, 0); diff --git a/src/drivers/distance_sensor/tf02pro/tf02pro_main.cpp b/src/drivers/distance_sensor/tf02pro/tf02pro_main.cpp new file mode 100644 index 0000000000..183b55fd29 --- /dev/null +++ b/src/drivers/distance_sensor/tf02pro/tf02pro_main.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 "TF02PRO.hpp" + +#include +#include + +void +TF02PRO::print_usage() +{ + PRINT_MODULE_USAGE_NAME("tf02pro", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x10); + PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" __EXPORT int tf02pro_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = TF02PRO; + BusCLIArguments cli{true, false}; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.default_i2c_frequency = 400000; // Fast speed (400Khz) + cli.i2c_address = TF02PRO_BASEADDR; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_TF02PRO); + + 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 6fb2d5f62d..a0d109229d 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -222,6 +222,8 @@ #define DRV_POWER_DEVTYPE_INA220 0xD3 #define DRV_POWER_DEVTYPE_INA238 0xD4 +#define DRV_DIST_DEVTYPE_TF02PRO 0xE0 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */