distance_sensor: Add TF02 Pro I2C

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

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)

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
 

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
+	)

src/drivers/distance_sensor/tf02pro/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_DISTANCE_SENSOR_TF02PRO
+	bool "tf02pro"
+	default n
+	---help---
+		Enable support for tf02pro

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

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 <px4_platform_common/px4_config.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+
+/* 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<TF02PRO>
+{
+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")};
+};

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);

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 <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+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;
+}

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 */