tfmini_i2c: add support for Benewake driver on I2C

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -58,6 +58,11 @@ then
 	pga460 start
 fi
 
+if param greater -s SENS_EN_TFI2C 0
+then
+	tfmini_i2c start -X
+fi
+
 # Lightware i2c lidar sensor
 if param greater -s SENS_EN_SF1XX 0
 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(tfmini_i2c)

src/drivers/distance_sensor/tfmini_i2c/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2017-2019 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__tfmini_i2c
+	MAIN tfmini_i2c
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		tfmini_i2c.cpp
+		tfmini_i2c.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		drivers_rangefinder
+	)

src/drivers/distance_sensor/tfmini_i2c/module.yaml

@@ -0,0 +1,73 @@
+__max_tf_driver: &max_tf_driver 6
+
+module_name: Benewake i2c Driver
+
+parameters:
+    - group: Sensors
+      definitions:
+        SENS_EN_TFI2C:
+            description:
+                short: Enable Benewake i2c Driver
+                long: |
+                    Benewake TF Rangefinder i2c Driver
+            type: int32
+            min: 0
+            max: 6
+            default: 0
+            reboot_required: true
+
+        SENS_TF_${i}_ADDR:
+            description:
+                short: Benewake TF Sensor ${i} Address
+                long: |
+                    This parameter defines the address of the TFi2c sensor.
+            type: int32
+            min: 1
+            max: 254
+            num_instances: *max_tf_driver
+            instance_start: 0
+            default: 16
+
+        SENS_TF_${i}_ROT:
+            description:
+                short: Benewake TF Sensor ${i} Rotation
+                long: |
+                    This parameter defines the rotation of the sensor relative to the platform.
+            type: enum
+            values:
+                25: ROTATION_DOWNWARD_FACING
+                24: ROTATION_UPWARD_FACING
+                12: ROTATION_BACKWARD_FACING
+                0: ROTATION_FORWARD_FACING
+                6: ROTATION_LEFT_FACING
+                2: ROTATION_RIGHT_FACING
+            reboot_required: true
+            min: 0
+            max: 25
+            num_instances: *max_tf_driver
+            instance_start: 0
+            default: 0
+
+        SENS_TF_${i}_MAXD:
+            description:
+                short: Benewake TF Sensor ${i} Max Distance
+                long: |
+                    This parameter defines maximum measuring distance.
+            type: int32
+            min: 5
+            max: 20000
+            num_instances: *max_tf_driver
+            instance_start: 0
+            default: 1200
+
+        SENS_TF_${i}_MIND:
+            description:
+                short: Benewake TF Sensor ${i} Min Distance
+                long: |
+                    This parameter defines min measuring distance.
+            type: int32
+            min: 5
+            max: 20000
+            num_instances: *max_tf_driver
+            instance_start: 0
+            default: 10

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -0,0 +1,429 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 tfmini_i2c.cpp
+ *
+ * Driver for the Benewake range finders Sensor connected via I2C.
+ */
+#include "tfmini_i2c.hpp"
+
+using namespace time_literals;
+
+#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
+
+/* Configuration Constants */
+#define TFMINI_I2C_BUS_SPEED                        100000 // 100kHz bus speed.
+#define TFMINI_I2C_BASE_ADDR                        0x10   // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224.
+
+
+#define TFMINI_I2C_MEASURE_INTERVAL                 30_ms // 60ms minimum for one sonar.
+#define TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES 20_ms // 30ms minimum between each sonar measurement (watch out for interference!).
+
+
+
+TFMINI_I2C::TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) :
+	I2C(DRV_DIST_DEVTYPE_TFMINI_I2C, MODULE_NAME, bus, address, bus_frequency),
+	ModuleParams(nullptr),
+	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
+{
+	set_device_type(DRV_DIST_DEVTYPE_TFMINI_I2C);
+	_measure_interval = TFMINI_I2C_MEASURE_INTERVAL;
+}
+
+TFMINI_I2C::~TFMINI_I2C()
+{
+	// Unadvertise the distance sensor topic.
+	if (_distance_sensor_topic != nullptr) {
+		orb_unadvertise(_distance_sensor_topic);
+	}
+
+	// Free perf counters.
+	perf_free(_comms_error);
+	perf_free(_sample_perf);
+}
+
+int32_t
+TFMINI_I2C::getTFi2cParam(uint8_t index,uint8_t type)
+{
+	const char *pname_format_tfi2c[4] = {"SENS_TF_%d_ADDR","SENS_TF_%d_ROT","SENS_TF_%d_MAXD","SENS_TF_%d_MIND"};
+	char p_name[16];
+	int32_t value;
+	sprintf(p_name, pname_format_tfi2c[type], index);
+	param_t param_h = param_find(p_name);
+
+	if (param_h != PARAM_INVALID) {
+		param_get(param_h, &value);
+	} else {
+		PX4_DEBUG("PARAM_INVALID: %s", p_name);
+	}
+	return value;
+}
+
+int
+TFMINI_I2C::init()
+{
+	if (_p_sensor_enabled.get() == 0) {
+		PX4_WARN("disabled");
+		return PX4_ERROR;
+	}
+
+	// Initialize the I2C device
+	if (I2C::init() != OK) {
+		return PX4_ERROR;
+	}
+	// Check for connected rangefinders on each i2c port by decrementing from the base address,
+	uint8_t address;
+	for (uint8_t index = 0; index < RANGE_FINDER_MAX_SENSORS ; index++) {
+		// address = param_total_addr[index];
+		address = (uint8_t)getTFi2cParam(index,TF_PARAM_ADDR);
+		set_device_address(address);
+		px4_usleep(_measure_interval);
+		if (measure() == PX4_OK) {
+			px4_usleep(_measure_interval);
+			// Store I2C address、rotation、range
+			sensor_param_value[_sensor_count].addresses = address;
+			sensor_param_value[_sensor_count].rotations = (uint8_t)getTFi2cParam(index,TF_PARAM_ROT);
+			sensor_param_value[_sensor_count].max_range = getTFi2cParam(index,TF_PARAM_MAXD);
+			sensor_param_value[_sensor_count].min_range = getTFi2cParam(index,TF_PARAM_MIND);
+            		int set_ret = set_param();
+			printf("tf[%i]: address 0x%02x ,rototion %d, max:%d, min:%d, set param:%d\n", _sensor_count, \
+												get_device_address(), \
+												sensor_param_value[_sensor_count].rotations, \
+												sensor_param_value[_sensor_count].max_range, \
+												sensor_param_value[_sensor_count].min_range, \
+												set_ret);
+
+			_sensor_count++;
+			if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) {
+				break;
+			}
+		}
+		px4_usleep(_measure_interval);
+	}
+	get_a_data = 0;
+	// Return an error if no sensors were detected.
+	if (_sensor_count == 0) {
+		PX4_ERR("no sensors discovered");
+		return PX4_ERROR;
+	}
+
+	// If more than one sonar is detected, adjust the meaure interval to avoid sensor interference.
+	if (_sensor_count > 1) {
+		_measure_interval = TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES;
+	}
+
+	PX4_INFO("Total sensors connected: %i", _sensor_count);
+	return PX4_OK;
+}
+
+int
+TFMINI_I2C::set_param()
+{
+	int ret;
+	const uint8_t CMD_SYSTEM_RESET[] =       { 0x5A, 0x04, 0x04, 0x62 };
+	const uint8_t CMD_OUTPUT_FORMAT_CM[] =   { 0x5A, 0x05, 0x05, 0x01, 0x65 };
+	const uint8_t CMD_ENABLE_DATA_OUTPUT[] = { 0x5A, 0x05, 0x07, 0x01, 0x67 };
+	const uint8_t CMD_FRAME_RATE_100HZ[] =   { 0x5A, 0x06, 0x03, 0x64, 0x00, 0xC7 };
+	const uint8_t CMD_SAVE_SETTINGS[] =      { 0x5A, 0x04, 0x11, 0x6F };
+	const uint8_t *cmds[] = {
+		CMD_OUTPUT_FORMAT_CM,
+		CMD_FRAME_RATE_100HZ,
+		CMD_ENABLE_DATA_OUTPUT,
+		CMD_SAVE_SETTINGS,
+	};
+	for (uint8_t i = 0; i < ARRAY_SIZE(cmds); i++) {
+		ret = tfi2c_transfer(cmds[i], cmds[i][1], nullptr, 0);
+		if (!ret) {
+		PX4_INFO(": Unable to set configuration register %u\n",cmds[i][2]);
+		return ret;
+		}
+		px4_usleep(100_ms);
+	}
+	tfi2c_transfer(CMD_SYSTEM_RESET, sizeof(CMD_SYSTEM_RESET), nullptr, 0);
+
+	return PX4_OK;
+}
+int
+TFMINI_I2C::measure()
+{
+	// Send the command to take a measurement.
+	const uint8_t CMD_READ_MEASUREMENT[] = {0x5A,0x05,0x00,0x01,0x60 };
+	int ret_val = transfer(CMD_READ_MEASUREMENT, 5, nullptr, 0);
+	return ret_val;
+}
+
+int
+TFMINI_I2C::collect()
+{
+	uint16_t distance_cm = 0;
+	static uint32_t err_cnt;
+	const uint8_t Data_Len = 9;
+	static  uint8_t raw_data[Data_Len];
+	perf_begin(_sample_perf);
+
+	// Increment i2c adress to next sensor.
+	_sensor_index++;
+	_sensor_index %= _sensor_count;
+
+	// Set the sensor i2c adress for the active cycle.
+	set_device_address(sensor_param_value[_sensor_index].addresses );
+
+	// Transfer data from the bus.
+	int ret_val;
+
+	uint8_t CMD_READ_MEASUREMENT[] = { 0x5A,0x05,0x00,0x01,0x60 };
+	ret_val = tfi2c_transfer(CMD_READ_MEASUREMENT, 5, raw_data, Data_Len);
+
+	if (ret_val < 0) {
+		perf_count(_comms_error);
+		perf_end(_sample_perf);
+		return ret_val;
+	}
+	/**  header1, header2,distanceL,distanceH,timestampL,timestampH,checksum; **/
+	if(raw_data[0] == 0x59 && raw_data[1] == 0x59)
+	{
+		if(check_checksum(raw_data,Data_Len)){
+			distance_cm = ((uint16_t)raw_data[3] << 8) | raw_data[2];
+		}
+
+	}else{
+		err_cnt += 1;
+		return -EAGAIN;
+	}
+	if (distance_cm > sensor_param_value[_sensor_index].max_range || distance_cm < sensor_param_value[_sensor_index].min_range )	// 超出距离范围
+	{
+		distance_cm = sensor_param_value[_sensor_index].max_range ;
+	}
+
+	float distance_m = static_cast<float>(distance_cm) * 1e-2f;
+
+	distance_sensor_s report;
+	report.current_distance = distance_m;
+	report.device_id        = get_device_id();
+	report.max_distance     = sensor_param_value[_sensor_index].max_range / 100.0f;
+	report.min_distance     = sensor_param_value[_sensor_index].min_range / 100.0f;
+	report.orientation      = sensor_param_value[_sensor_index].rotations;
+	report.signal_quality   = -1;
+	report.timestamp        = hrt_absolute_time();
+	report.type             = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.variance         = 0.0f;
+	report.h_fov = math::radians(1.15f);
+	report.v_fov = math::radians(1.15f);
+
+	int instance_id;
+	orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id);
+	if (!get_a_data)
+	{
+		get_a_data = 1;
+	}
+	/** for test **/
+	// static uint64_t last_us ;
+	// static double dist_arr[6]={0.01,0.01,0.01,0.01,0.01,0.01};
+	// dist_arr[_sensor_index] = (double)distance_m;
+        // if(hrt_absolute_time() - last_us > 100 * 1_ms){
+        //     printf("dist:%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f\n",dist_arr[0],dist_arr[1],dist_arr[2],dist_arr[3],dist_arr[4],dist_arr[5]);
+        //     last_us = hrt_absolute_time();
+        // }
+	perf_end(_sample_perf);
+	return PX4_OK;
+}
+
+
+int
+TFMINI_I2C::check_checksum(uint8_t *arr, int pkt_len)
+{
+	uint8_t checksum = 0;
+	int i;
+
+	/* sum them all except the last (the checksum) */
+	for (i = 0; i < pkt_len - 1; i++) {
+	checksum += arr[i];
+	}
+	crc_clc = checksum;
+	return checksum == arr[pkt_len - 1];
+}
+
+int
+TFMINI_I2C::tfi2c_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
+{
+	bool send_and_receive = false;
+	if (send != nullptr && send_len > 0) {
+
+		int ret = transfer(send, send_len, nullptr, 0);
+		send_and_receive = true;
+		if (ret != PX4_OK) {
+			return ret;
+		}
+	}
+
+	if (recv != nullptr && recv_len > 0) {
+		if(send_and_receive){
+			px4_usleep(500_us);
+		}
+		return transfer(nullptr, 0, recv, recv_len);
+	}
+
+	return PX4_ERROR;
+}
+
+
+void
+TFMINI_I2C::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_error);
+	PX4_INFO("poll interval:  %ums", _measure_interval / 1000);
+
+	for (int i = 0; i < _sensor_count; i++) {
+		PX4_INFO("sensor: %i, address %u", i, sensor_param_value[i].addresses );
+	}
+}
+
+void
+TFMINI_I2C::RunImpl()
+{
+	// Collect the sensor data.
+	if (collect() != PX4_OK) {
+		PX4_INFO("collection error");
+		px4_usleep(1_s);
+
+	}
+}
+
+int
+TFMINI_I2C::set_address(const uint8_t address)
+{
+	if (_sensor_count > 1) {
+		PX4_INFO("multiple sensors are connected");
+		return PX4_ERROR;
+	}
+
+	PX4_INFO("requested address: %u", address);
+
+	uint8_t shifted_address = address ;
+	uint8_t cmd[5] = {0x5A, 0x05, 0x0B, shifted_address,0};
+    	cmd[4] = 0x5A + 0x05 + 0x0B + shifted_address;
+	if (transfer(cmd, sizeof(cmd), nullptr, 0) != PX4_OK) {
+		PX4_INFO("could not set the address");
+	}
+
+	set_device_address(address);
+	PX4_INFO("device address: %u", get_device_address());
+	return PX4_OK;
+}
+
+void
+TFMINI_I2C::start()
+{
+	// Fetch parameter values.
+	ModuleParams::updateParams();
+	// Schedule the driver cycle at regular intervals.
+	ScheduleOnInterval(_measure_interval);
+}
+
+void
+TFMINI_I2C::custom_method(const BusCLIArguments &cli)
+{
+	set_address(cli.i2c_address);
+}
+
+I2CSPIDriverBase *TFMINI_I2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				      int runtime_instance)
+{
+	TFMINI_I2C *instance = new TFMINI_I2C(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (instance->init() != PX4_OK) {
+		delete instance;
+		return nullptr;
+	}
+
+	instance->start();
+	return instance;
+}
+
+
+void
+TFMINI_I2C::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("tfmini_i2c", "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(0x16);
+	PRINT_MODULE_USAGE_COMMAND("set_address");
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x16);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" __EXPORT int tfmini_i2c_main(int argc, char *argv[])
+{
+	using ThisDriver = TFMINI_I2C;
+	BusCLIArguments cli{true, false};
+	cli.i2c_address = TFMINI_I2C_BASE_ADDR;
+	cli.default_i2c_frequency = TFMINI_I2C_BUS_SPEED;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_TFMINI_I2C);
+
+	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);
+	}
+
+	if (!strcmp(verb, "set_address")) {
+		return ThisDriver::module_custom_method(cli, iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.hpp

@@ -0,0 +1,170 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2019, 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 tfmini_i2c.cpp
+ *
+ * Driver for the Benewake TFmini laser rangefinder series
+ */
+
+#pragma once
+
+#include <fcntl.h>
+#include <math.h>
+#include <poll.h>
+#include <semaphore.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <board_config.h>
+#include <containers/Array.hpp>
+#include <drivers/device/device.h>
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <systemlib/mavlink_log.h>
+
+
+
+class TFMINI_I2C : public device::I2C, public ModuleParams, public I2CSPIDriver<TFMINI_I2C>
+{
+public:
+	TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address);
+	virtual ~TFMINI_I2C();
+
+	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+					     int runtime_instance);
+	static void print_usage();
+
+	virtual int init() override;
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void print_status() override;
+
+	/**
+	 * Sets a new device address.
+	 * @param address The new sensor address to be set: 1-254 even addresses only.
+	 * @return Returns PX4_OK iff successful, PX4_ERROR otherwise.
+	 */
+	int set_address(const uint8_t address);
+
+	/**
+	 * Initialise the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
+	void start();
+
+	/**
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 */
+	void RunImpl();
+
+protected:
+	void custom_method(const BusCLIArguments &cli) override;
+	int32_t getTFi2cParam(uint8_t index,uint8_t type);
+
+private:
+
+	/**
+	 * Collects the most recent sensor measurement data from the i2c bus.
+	 */
+	int collect();
+
+	/**
+	 * Sends an i2c measure command to start the next sonar ping.
+	 */
+	int measure();
+	int tfi2c_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
+	int check_checksum(uint8_t *arr, int pkt_len);
+
+	int set_param();
+
+	enum tf_param_type {
+		TF_PARAM_ADDR = 0,
+		TF_PARAM_ROT,
+		TF_PARAM_MAXD,
+		TF_PARAM_MIND
+	};
+
+	static constexpr uint8_t RANGE_FINDER_MAX_SENSORS = 6;
+	// px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_addresses {};
+	// px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_rotations {};
+
+	int _measure_interval{0};	// Initialize the measure interval for a single sensor.
+
+	int _sensor_index{0};	// Initialize counter for cycling i2c adresses to zero.
+
+	int _sensor_count{0};
+
+	orb_advert_t _distance_sensor_topic{nullptr};
+
+	perf_counter_t _comms_error{perf_alloc(PC_ELAPSED, "tfmini_i2c_comms_error")};
+	perf_counter_t _sample_perf{perf_alloc(PC_COUNT, "tfmini_i2c_sample_perf")};
+
+    	uint8_t crc_clc;
+	struct sensor_param_s
+	{
+		int32_t max_range;
+		int32_t min_range;
+		uint8_t addresses;
+		uint8_t rotations;
+	};
+	sensor_param_s sensor_param_value[RANGE_FINDER_MAX_SENSORS];
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_EN_TFI2C>)   _p_sensor_enabled
+
+	);
+    orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
+    float avoid_distance;
+    int entra_avoid_area{0};
+    uint8_t get_a_data;
+};

src/drivers/drv_sensor.h

@@ -207,6 +207,7 @@
 #define DRV_GPS_DEVTYPE_SIM 0xAF
 
 #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1
+#define DRV_DIST_DEVTYPE_TFMINI_I2C       0xB2
 
 #define DRV_DEVTYPE_UNUSED		0xff