增加tfmini iic，数据读取成功

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -65,6 +65,11 @@ then
 	mb12xx start -X
 fi
 
+if param greater -s SENS_EN_TFI2C 0
+then
+	tfmini_i2c start -X
+fi
+
 # pga460 sonar sensor
 if param greater -s SENS_EN_PGA460 0
 then

fmuv3_build.sh

@@ -0,0 +1,8 @@
+date -R
+starttime=`date +'%Y-%m-%d %H:%M:%S'`
+make px4_fmu-v3_default
+endtime=`date +'%Y-%m-%d %H:%M:%S'`
+date -R
+start_seconds=$(date --date="$starttime" +%s);
+end_seconds=$(date --date="$endtime" +%s);
+echo "本次运行时间： "$((end_seconds-start_seconds))"s"

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,42 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__tfmini_i2c
+	MAIN tfmini_i2c
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		tfmini_i2c.cpp
+	DEPENDS
+		drivers_rangefinder
+	)

src/drivers/distance_sensor/tfmini_i2c/parameters.c

@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Benewake TFmini Rangefinder Sonar (tfmini i2c)
+ *
+ * @reboot_required true
+ *
+ * @boolean
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_EN_TFI2C, 0);
+
+/**
+ * Benewake TFmini Sensor 0 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_0_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 1 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_1_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 2 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_2_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 3 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_3_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 4 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_4_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 5 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 6 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_6_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 7 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_7_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 8 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_8_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 9 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_9_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 10 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_10_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor 12 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 7
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ */
+PARAM_DEFINE_INT32(SENS_TF_11_ROT, 0);

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -0,0 +1,560 @@
+/****************************************************************************
+ *
+ *   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
+ * @author Greg Hulands
+ * @author Jon Verbeke <jon.verbeke@kuleuven.be>
+ *
+ * Driver for the Maxbotix sonar range finders connected via I2C.
+ */
+
+#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>
+
+using namespace time_literals;
+
+/* Configuration Constants */
+#define TFMINI_I2C_BASE_ADDR                        0x10   // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224.
+#define TFMINI_I2C_MIN_ADDR                         0x0f   // 7-bit address is 0x5A = 90.  8-bit address is 0xB4 = 180.
+#define TFMINI_I2C_BUS_SPEED                        100000 // 100kHz bus speed.
+
+/* TF_I2Cxx Registers addresses */
+#define TFMINI_I2C_TAKE_RANGE_REG                   0x51 // Measure range Register.
+#define TFMINI_I2C_SET_ADDRESS_1                    0xAA // Change address 1 Register.
+#define TFMINI_I2C_SET_ADDRESS_2                    0xA5 // Change address 2 Register.
+
+/* Device limits */
+#define TFMINI_I2C_MIN_DISTANCE                     (0.10f)
+#define TFMINI_I2C_MAX_DISTANCE                     (8.00f)
+
+#define TFMINI_I2C_MEASURE_INTERVAL                 100_ms // 60ms minimum for one sonar.
+#define TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100_ms // 30ms minimum between each sonar measurement (watch out for interference!).
+
+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: 200-224 even addresses only.
+	 * @return Returns PX4_OK iff successful, PX4_ERROR otherwise.
+	 */
+	int set_address(const uint8_t address = TFMINI_I2C_BASE_ADDR);
+
+	/**
+	 * 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;
+
+private:
+
+	/**
+	 * Collects the most recent sensor measurement data from the i2c bus.
+	 */
+	int collect();
+
+	/**
+	 * Gets the current sensor rotation value.
+	 */
+	int get_sensor_rotation(const size_t index);
+
+	/**
+	 * 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);
+
+	static constexpr uint8_t RANGE_FINDER_MAX_SENSORS = 4;
+	px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_addresses {};
+	px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_rotations {};
+
+	int _measure_interval{TFMINI_I2C_MEASURE_INTERVAL};	// 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;
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_EN_TFI2C>)   _p_sensor_enabled,
+		(ParamInt<px4::params::SENS_TF_0_ROT>)  _p_sensor0_rot,
+		(ParamInt<px4::params::SENS_TF_1_ROT>)  _p_sensor1_rot,
+		(ParamInt<px4::params::SENS_TF_2_ROT>)  _p_sensor2_rot,
+		(ParamInt<px4::params::SENS_TF_3_ROT>)  _p_sensor3_rot,
+		(ParamInt<px4::params::SENS_TF_4_ROT>)  _p_sensor4_rot,
+		(ParamInt<px4::params::SENS_TF_5_ROT>)  _p_sensor5_rot,
+		(ParamInt<px4::params::SENS_TF_6_ROT>)  _p_sensor6_rot,
+		(ParamInt<px4::params::SENS_TF_7_ROT>)  _p_sensor7_rot,
+		(ParamInt<px4::params::SENS_TF_8_ROT>)  _p_sensor8_rot,
+		(ParamInt<px4::params::SENS_TF_9_ROT>)  _p_sensor9_rot,
+		(ParamInt<px4::params::SENS_TF_10_ROT>) _p_sensor10_rot,
+		(ParamInt<px4::params::SENS_TF_11_ROT>) _p_sensor11_rot
+	);
+};
+
+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);
+}
+
+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);
+}
+
+int
+TFMINI_I2C::collect()
+{
+
+    uint16_t distance = 0;
+    // uint16_t strength;
+    // uint32_t timestamp;
+    static uint32_t err_cnt;
+
+    static uint64_t last_us ;
+	perf_begin(_sample_perf);
+
+    uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
+    static  uint8_t raw_data[11];
+
+	// 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_addresses[_sensor_index]);
+
+	// Transfer data from the bus.
+	// int ret_val = transfer(CMD_READ_MEASUREMENT, 5, raw_data, 11);
+	int ret_val = tfi2c_transfer(CMD_READ_MEASUREMENT, 5, raw_data, 11);
+
+	if (ret_val < 0) {
+		PX4_ERR("sensor %i read failed, address: 0x%02X", _sensor_index, get_device_address());
+		perf_count(_comms_error);
+		perf_end(_sample_perf);
+		return ret_val;
+	}
+    /////////////////////
+
+    /**
+     *
+    uint8_t header1;
+    uint8_t header2;
+    le16_t distance;
+    le16_t strength;
+    le32_t timestamp;
+    uint8_t checksum;
+    */
+    if(raw_data[0] == 0x59 && raw_data[1] == 0x59)
+    {
+        if(check_checksum(raw_data,11)){
+            distance = ((uint16_t)raw_data[3] << 8) | raw_data[2];
+            // strength = ((uint16_t)raw_data[5] << 8) | raw_data[4];
+            // timestamp = ((uint32_t)raw_data[9] << 8) | ((uint32_t)raw_data[8] << 8) | ((uint32_t)raw_data[7] << 8) | raw_data[6];
+        }
+
+    }else{
+        err_cnt += 1;
+        if(hrt_absolute_time() - last_us > 2 * 1_s){
+            printf("{%d}err arr\n",__LINE__);
+            for (size_t i = 0; i < 11; i++)
+            {
+                printf("%02x ",raw_data[i]);
+            }
+            printf("\n");
+
+            printf("{%d}errcnt:%d, head %02x,%02x,crc: %02x ,clc:%02x\n",__LINE__,err_cnt,raw_data[0],raw_data[1],raw_data[10],crc_clc);
+            last_us = hrt_absolute_time();
+            err_cnt = 0;
+        }
+		// return -EAGAIN;
+    }
+///////////////////////
+	uint16_t distance_cm = distance;
+	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     = TFMINI_I2C_MAX_DISTANCE;
+	report.min_distance     = TFMINI_I2C_MIN_DISTANCE;
+	report.orientation      = _sensor_rotations[_sensor_index];
+	report.signal_quality   = -1;
+	report.timestamp        = hrt_absolute_time();
+	report.type             = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.variance         = 0.0f;
+
+	int instance_id;
+	orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id);
+
+	// Begin the next measurement.
+	if (measure() != PX4_OK) {
+		PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address());
+		perf_count(_comms_error);
+		perf_end(_sample_perf);
+		return ret_val;
+	}
+
+	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::get_sensor_rotation(const size_t index)
+{
+	switch (index) {
+	case 0: return _p_sensor0_rot.get();
+
+	case 1: return _p_sensor1_rot.get();
+
+	case 2: return _p_sensor2_rot.get();
+
+	case 3: return _p_sensor3_rot.get();
+
+	case 4: return _p_sensor4_rot.get();
+
+	case 5: return _p_sensor5_rot.get();
+
+	case 6: return _p_sensor6_rot.get();
+
+	case 7: return _p_sensor7_rot.get();
+
+	case 8: return _p_sensor8_rot.get();
+
+	case 9: return _p_sensor9_rot.get();
+
+	case 10: return _p_sensor10_rot.get();
+
+	case 11: return _p_sensor11_rot.get();
+
+	default: return PX4_ERROR;
+	}
+}
+
+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,
+	// (TFMINI_I2C_BASE_ADDR = 112, TFMINI_I2C_MIN_ADDR = 90).
+	for (uint8_t address = TFMINI_I2C_BASE_ADDR; address > TFMINI_I2C_MIN_ADDR ; address--) {
+		set_device_address(address);
+
+		if (measure() == PX4_OK) {
+			// Store I2C address
+			_sensor_addresses[_sensor_count] = address;
+			_sensor_rotations[_sensor_count] = get_sensor_rotation(_sensor_count);
+			_sensor_count++;
+
+			PX4_INFO("sensor %i at address 0x%02X added", _sensor_count, get_device_address());
+
+			if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) {
+				break;
+			}
+
+			px4_usleep(_measure_interval);
+		}
+	}
+
+	// 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::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;
+}
+
+int
+TFMINI_I2C::measure()
+{
+	// Send the command to take a measurement.
+    // const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
+    // const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
+    const uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
+
+	// uint8_t cmd = TFMINI_I2C_TAKE_RANGE_REG;
+	int ret_val = transfer(CMD_READ_MEASUREMENT, 5, nullptr, 0);
+
+	return ret_val;
+}
+
+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_addresses[i]);
+	}
+}
+
+void
+TFMINI_I2C::RunImpl()
+{
+	// Collect the sensor data.
+	if (collect() != PX4_OK) {
+		PX4_INFO("collection error");
+	}
+}
+
+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(0x70);
+	PRINT_MODULE_USAGE_COMMAND("set_address");
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x70);
+	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/drv_sensor.h

@@ -195,6 +195,7 @@
 
 #define DRV_GPS_DEVTYPE_SIM 0xAF
 
+#define DRV_DIST_DEVTYPE_TFMINI_I2C       0xB0
 
 #define DRV_DEVTYPE_UNUSED		0xff
 

v5_build.sh

@@ -0,0 +1,8 @@
+date -R
+starttime=`date +'%Y-%m-%d %H:%M:%S'`
+make px4_fmu-v5_default
+endtime=`date +'%Y-%m-%d %H:%M:%S'`
+date -R
+start_seconds=$(date --date="$starttime" +%s);
+end_seconds=$(date --date="$endtime" +%s);
+echo "本次运行时间： "$((end_seconds-start_seconds))"s"