diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f718d02d75..daaa68d8dc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/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 diff --git a/fmuv3_build.sh b/fmuv3_build.sh new file mode 100755 index 0000000000..8dfc6f4ca7 --- /dev/null +++ b/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" diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index e7e3b632f6..ef2c207696 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(tfmini_i2c) diff --git a/src/drivers/distance_sensor/tfmini_i2c/CMakeLists.txt b/src/drivers/distance_sensor/tfmini_i2c/CMakeLists.txt new file mode 100644 index 0000000000..299b42168f --- /dev/null +++ b/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 + ) diff --git a/src/drivers/distance_sensor/tfmini_i2c/parameters.c b/src/drivers/distance_sensor/tfmini_i2c/parameters.c new file mode 100644 index 0000000000..a73bcf12e3 --- /dev/null +++ b/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); diff --git a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp new file mode 100644 index 0000000000..bd0387237d --- /dev/null +++ b/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 + * + * Driver for the Maxbotix sonar range finders connected via I2C. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 +{ +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 _sensor_addresses {}; + px4::Array _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) _p_sensor_enabled, + (ParamInt) _p_sensor0_rot, + (ParamInt) _p_sensor1_rot, + (ParamInt) _p_sensor2_rot, + (ParamInt) _p_sensor3_rot, + (ParamInt) _p_sensor4_rot, + (ParamInt) _p_sensor5_rot, + (ParamInt) _p_sensor6_rot, + (ParamInt) _p_sensor7_rot, + (ParamInt) _p_sensor8_rot, + (ParamInt) _p_sensor9_rot, + (ParamInt) _p_sensor10_rot, + (ParamInt) _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(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; +} diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index c4c9e739c8..664ab0866d 100644 --- a/src/drivers/drv_sensor.h +++ b/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 diff --git a/v5_build.sh b/v5_build.sh new file mode 100755 index 0000000000..5f4ac9a68d --- /dev/null +++ b/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"