From e112685c6095f8bc6069101f4983a10bd49e9007 Mon Sep 17 00:00:00 2001 From: Aymeric Moizard Date: Thu, 2 Jan 2020 18:44:59 +0100 Subject: [PATCH 1/2] TFmini driver: fix fov for TFmini distance sensor and remove obsolete command Add support various TFmini models and basic autosetup for TFMini-plus Add "tfmini command -c XXXXXX" for device configuration Improve parser to support receiving confirmation from the device after a command has been written. Add SENS_EN_TFMINI param Improve autoconfigure mode for TFMini-plus upon starting. --- .../distance_sensor/tfmini/CMakeLists.txt | 1 - src/drivers/distance_sensor/tfmini/TFMINI.cpp | 379 +++++++++++++++++- src/drivers/distance_sensor/tfmini/TFMINI.hpp | 64 ++- .../distance_sensor/tfmini/parameters.c | 45 +++ .../distance_sensor/tfmini/tfmini_main.cpp | 86 +++- .../distance_sensor/tfmini/tfmini_parser.cpp | 134 ++++++- .../distance_sensor/tfmini/tfmini_parser.h | 65 ++- 7 files changed, 721 insertions(+), 53 deletions(-) create mode 100644 src/drivers/distance_sensor/tfmini/parameters.c diff --git a/src/drivers/distance_sensor/tfmini/CMakeLists.txt b/src/drivers/distance_sensor/tfmini/CMakeLists.txt index 37d965611d36..2869169dd368 100644 --- a/src/drivers/distance_sensor/tfmini/CMakeLists.txt +++ b/src/drivers/distance_sensor/tfmini/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MAIN tfmini SRCS TFMINI.cpp - TFMINI.hpp tfmini_main.cpp tfmini_parser.cpp MODULE_CONFIG diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index 150d1c486ddd..2a7e5a1ad3d7 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2020 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 @@ -72,23 +72,37 @@ TFMINI::~TFMINI() int TFMINI::init() { - int32_t hw_model = 1; // only one model so far... + int hw_model = 0; + param_get(param_find("SENS_EN_TFMINI"), &hw_model); - switch (hw_model) { - case 1: // TFMINI (12m, 100 Hz) - // Note: - // Sensor specification shows 0.3m as minimum, but in practice - // 0.3 is too close to minimum so chattering of invalid sensor decision + _hw_model = static_cast(hw_model); + + switch (_hw_model) { + case TFMINI_MODEL::MODEL_UNKNOWN: // Other TFMINI models (12m, 100 Hz, FOV 2.3) + // Note: Sensor specification shows 0.3m as minimum, but in practice + // 0.3m is too close to minimum so chattering of invalid sensor decision // is happening sometimes. this cause EKF to believe inconsistent range readings. - // So we set 0.4 as valid minimum. + // So we set 0.4m as valid minimum. + _px4_rangefinder.set_min_distance(0.4f); _px4_rangefinder.set_min_distance(0.4f); _px4_rangefinder.set_max_distance(12.0f); - _px4_rangefinder.set_fov(math::radians(1.15f)); + _px4_rangefinder.set_fov(math::radians(2.3f)); + break; + case TFMINI_MODEL::MODEL_TFMINI: // TFMINI (12m, 100 Hz, FOV 2.3) + _px4_rangefinder.set_min_distance(0.4f); + _px4_rangefinder.set_max_distance(12.0f); + _px4_rangefinder.set_fov(math::radians(2.3f)); + break; + + case TFMINI_MODEL::MODEL_TFMINIPLUS: // TFMINI-PLUS (12m, 100 Hz, FOV 3.6) + _px4_rangefinder.set_min_distance(0.1f); + _px4_rangefinder.set_max_distance(12.0f); + _px4_rangefinder.set_fov(math::radians(3.6f)); break; default: - PX4_ERR("invalid HW model %" PRId32 ".", hw_model); + PX4_ERR("invalid HW model %u.", (uint8_t)_hw_model); return -1; } @@ -168,6 +182,272 @@ TFMINI::init() return ret; } +void +TFMINI::autosetup_tfmini_process() +{ + if (_hw_model != TFMINI_MODEL::MODEL_TFMINI) { + return; + } + + if (_tfmini_setup._setup_step == TFMINI_SETUP_STEP::STEP4_SAVE_CONFIRMED) { + return; + } + + _tfmini_setup._counter = 0; + + // informations about commands and responses to/from TFMini has been found in SJ-PM-TFmini-T-01_A05 Product Mannual_EN.pdf + switch (_tfmini_setup._setup_step) { + case TFMINI_SETUP_STEP::STEP0_UNCONFIGURED: + + // we have written _tfmini_setup._com_enter and we expect reading 42 57 02 01 00 00 01 02 + if (_command_response[3] == 0x01 && _command_response[6] == 0x01 && _command_response[7] == 0x02) { + _tfmini_setup._setup_step = TFMINI_SETUP_STEP::STEP1_ENTER_CONFIRMED; + } + + break; + + case TFMINI_SETUP_STEP::STEP1_ENTER_CONFIRMED: + + // we have written _tfmini_setup._com_mode and we expect reading 42 57 02 01 00 00 01 06 + if (_command_response[3] == 0x01 && _command_response[6] == 0x01 && _command_response[7] == 0x06) { + _tfmini_setup._setup_step = TFMINI_SETUP_STEP::STEP2_MODE_CONFIRMED; + } + + break; + + case TFMINI_SETUP_STEP::STEP2_MODE_CONFIRMED: + + // we have written _tfmini_setup._com_unit and we expect reading 42 57 02 01 00 00 01 1A + if (_command_response[3] == 0x01 && _command_response[6] == 0x01 && _command_response[7] == 0x1A) { + _tfmini_setup._setup_step = TFMINI_SETUP_STEP::STEP3_UNIT_CONFIRMED; + } + + break; + + case TFMINI_SETUP_STEP::STEP3_UNIT_CONFIRMED: + + // we have written _tfmini_setup._com_save and we expect reading 42 57 02 01 00 00 00 02 + if (_command_response[3] == 0x01 && _command_response[6] == 0x00 && _command_response[7] == 0x02) { + _tfmini_setup._setup_step = TFMINI_SETUP_STEP::STEP4_SAVE_CONFIRMED; + } + + break; + + default: + break; + } +} + +void +TFMINI::autosetup_tfmini() +{ + if (_hw_model != TFMINI_MODEL::MODEL_TFMINI) { + return; + } + + if (_tfmini_setup._setup_step == TFMINI_SETUP_STEP::STEP4_SAVE_CONFIRMED) { + return; + } + + if (_fd < 0) { + return; + } + + _tfmini_setup._counter++; + + if (_tfmini_setup._counter != 1) { + return; + } + + int ret = 0; + + // informations about commands and responses to/from TFMini- has been found in SJ-PM-TFmini-T-01_A05 Product Mannual_EN.pdf + switch (_tfmini_setup._setup_step) { + case TFMINI_SETUP_STEP::STEP0_UNCONFIGURED: + ret = ::write(_fd, _tfmini_setup._com_enter, sizeof(_tfmini_setup._com_enter)); + break; + + case TFMINI_SETUP_STEP::STEP1_ENTER_CONFIRMED: + ret = ::write(_fd, _tfmini_setup._com_mode, sizeof(_tfmini_setup._com_mode)); + break; + + case TFMINI_SETUP_STEP::STEP2_MODE_CONFIRMED: + ret = ::write(_fd, _tfmini_setup._com_unit, sizeof(_tfmini_setup._com_unit)); + break; + + case TFMINI_SETUP_STEP::STEP3_UNIT_CONFIRMED: + ret = ::write(_fd, _tfmini_setup._com_save, sizeof(_tfmini_setup._com_save)); + break; + + default: + break; + } + + if (ret < 0) { + perf_count(_comms_errors); + } +} + +void +TFMINI::autosetup_tfminiplus_process() +{ + if (_hw_model != TFMINI_MODEL::MODEL_TFMINIPLUS) { + return; + } + + if (_tfminiplus_setup._setup_step == TFMINIPLUS_SETUP_STEP::STEP4_SAVE_CONFIRMED) { + return; + } + + _tfminiplus_setup._counter = 0; + + // informations about commands and responses to/from TFMini-plus has been found in TFmini-Plus A04-Product Mannual_EN.pdf + switch (_tfminiplus_setup._setup_step) { + case TFMINIPLUS_SETUP_STEP::STEP0_UNCONFIGURED: + + // we have written _tfminiplus_setup._com_version and we expect reading 5A 07 01 V1 V2 V3 SU + if (_command_response[1] == 0x07 && _command_response[2] == 0x01) { + _tfminiplus_setup._version[0] = _command_response[3]; + _tfminiplus_setup._version[1] = _command_response[4]; + _tfminiplus_setup._version[2] = _command_response[5]; + _tfminiplus_setup._setup_step = TFMINIPLUS_SETUP_STEP::STEP1_VERSION_CONFIRMED; + } + + break; + + case TFMINIPLUS_SETUP_STEP::STEP1_VERSION_CONFIRMED: + + // we have written _tfminiplus_setup._com_mode and we expect reading 5A 05 05 01 65 + if (_command_response[1] == 0x05 && _command_response[2] == 0x05 && _command_response[3] == 0x01) { + _tfminiplus_setup._setup_step = TFMINIPLUS_SETUP_STEP::STEP2_MODE_CONFIRMED; + } + + break; + + case TFMINIPLUS_SETUP_STEP::STEP2_MODE_CONFIRMED: + + // we have written _tfminiplus_setup._com_enable and we expect reading 5A 05 07 01 67 + if (_command_response[1] == 0x05 && _command_response[2] == 0x07 && _command_response[3] == 0x01) { + _tfminiplus_setup._setup_step = TFMINIPLUS_SETUP_STEP::STEP3_ENABLE_CONFIRMED; + } + + break; + + case TFMINIPLUS_SETUP_STEP::STEP3_ENABLE_CONFIRMED: + + // we have written _tfminiplus_setup._com_save and we expect reading 5A 05 11 00 6F + if (_command_response[1] == 0x05 && _command_response[2] == 0x11 && _command_response[3] == 0x00) { + _tfminiplus_setup._setup_step = TFMINIPLUS_SETUP_STEP::STEP4_SAVE_CONFIRMED; + } + + break; + + default: + break; + } +} + +void +TFMINI::autosetup_tfminiplus() +{ + if (_hw_model != TFMINI_MODEL::MODEL_TFMINIPLUS) { + return; + } + + if (_tfminiplus_setup._setup_step == TFMINIPLUS_SETUP_STEP::STEP4_SAVE_CONFIRMED) { + return; + } + + if (_fd < 0) { + return; + } + + _tfminiplus_setup._counter++; + + if (_tfminiplus_setup._counter != 1) { + return; + } + + int ret = 0; + + // informations about commands and responses to/from TFMini-plus has been found in TFmini-Plus A04-Product Mannual_EN.pdf + switch (_tfminiplus_setup._setup_step) { + case TFMINIPLUS_SETUP_STEP::STEP0_UNCONFIGURED: + ret = ::write(_fd, _tfminiplus_setup._com_version, sizeof(_tfminiplus_setup._com_version)); + break; + + case TFMINIPLUS_SETUP_STEP::STEP1_VERSION_CONFIRMED: + ret = ::write(_fd, _tfminiplus_setup._com_mode, sizeof(_tfminiplus_setup._com_mode)); + break; + + case TFMINIPLUS_SETUP_STEP::STEP2_MODE_CONFIRMED: + ret = ::write(_fd, _tfminiplus_setup._com_enable, sizeof(_tfminiplus_setup._com_enable)); + break; + + case TFMINIPLUS_SETUP_STEP::STEP3_ENABLE_CONFIRMED: + ret = ::write(_fd, _tfminiplus_setup._com_save, sizeof(_tfminiplus_setup._com_save)); + break; + + default: + break; + } + + if (ret < 0) { + perf_count(_comms_errors); + } +} + +int +TFMINI::write_command(uint8_t *command, uint8_t framelen) +{ + _command_result = false; + + if (_fd < 0) { + PX4_ERR("tfmini fd is closed"); + return PX4_ERROR; + } + + if (_hw_model != TFMINI_MODEL::MODEL_TFMINI && _hw_model != TFMINI_MODEL::MODEL_TFMINIPLUS) { + PX4_ERR("tfmini model not defined"); + return PX4_ERROR; + } + + if (framelen > 8) { + PX4_ERR("invalid command - too long"); + return PX4_ERROR; + } + + if (_hw_model == TFMINI_MODEL::MODEL_TFMINI && (command[0] != TFMINI_CMD_HEADER1 || command[1] != TFMINI_CMD_HEADER2 + || framelen != TFMINI_CMD_SIZE)) { + PX4_ERR("invalid command - not for tfmini model"); + return PX4_ERROR; + } + + if (_hw_model == TFMINI_MODEL::MODEL_TFMINIPLUS && (command[0] != TFMINIPLUS_CMD_HEADER1 || command[1] != framelen)) { + PX4_ERR("invalid command - not for tfmini-plus model"); + return PX4_ERROR; + } + + memcpy(_command_buf, command, framelen); + _command_size = framelen; + _command_retry = 3; + + return PX4_OK; +} + +bool +TFMINI::get_command_result() +{ + return _command_result; +} + +uint8_t * +TFMINI::get_command_response(uint8_t *response_size) +{ + *response_size = _command_response_size; + return _command_response; +} + int TFMINI::collect() { @@ -175,13 +455,28 @@ TFMINI::collect() // clear buffer if last read was too long ago int64_t read_elapsed = hrt_elapsed_time(&_last_read); + int ret = 0; + + autosetup_tfmini(); + autosetup_tfminiplus(); + + if (_command_result == false && _command_retry > 0) { + _command_retry--; + ret = ::write(_fd, _command_buf, _command_size); + + if (ret < 0) { + PX4_ERR("write err: %d", ret); + perf_count(_comms_errors); + } + } // the buffer for read chars is buflen minus null termination char readbuf[sizeof(_linebuf)] {}; unsigned readlen = sizeof(readbuf) - 1; - int ret = 0; float distance_m = -1.0f; + uint16_t signal_value = 0; + float temperature = -273.0f; // Check the number of bytes available in the buffer int bytes_available = 0; @@ -219,7 +514,19 @@ TFMINI::collect() // parse buffer for (int i = 0; i < ret; i++) { - tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m); + tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, _hw_model, &distance_m, &signal_value, &temperature, + _command_response, &_command_response_size); + + if (_parse_state == TFMINI_PARSE_STATE::STATE8_GOT_RESPONSE_CHECKSUM) { + _command_result = true; + _command_retry = 0; + autosetup_tfmini_process(); + autosetup_tfminiplus_process(); + } + + if (_parse_state == TFMINI_PARSE_STATE::STATE0_UNSYNC) { + perf_count(_comms_errors); + } } // bytes left to parse @@ -233,8 +540,32 @@ TFMINI::collect() return -EAGAIN; } + int8_t signal_quality = -1; + + switch (_hw_model) { + case TFMINI_MODEL::MODEL_TFMINI: // TFMINI (0 to 3000) + // to be implemented + break; + + case TFMINI_MODEL::MODEL_TFMINIPLUS: // TFMINI-PLUS (0 to 0xFFFF) + if (signal_value < 100 || signal_value == TFMINIPLUS_INVALID_MEASURE) { + signal_quality = 0; + break; + } + + //according to the spec, signal is within [0-0xFFFF] + //according to my test, signal is within [0-20000] + signal_value = math::min(signal_value, static_cast(20000)); + signal_quality = static_cast(static_cast(signal_value) / 200); //in percent, ie: x*100/20000 or x/200 + break; + + case TFMINI_MODEL::MODEL_UNKNOWN: // Other TFMINI models (unknown values) + default: + break; + } + // publish most recent valid measurement from buffer - _px4_rangefinder.update(timestamp_sample, distance_m); + _px4_rangefinder.update(timestamp_sample, distance_m, signal_quality); perf_end(_sample_perf); @@ -276,6 +607,28 @@ void TFMINI::print_info() { printf("Using port '%s'\n", _port); + + if (_hw_model == TFMINI_MODEL::MODEL_TFMINIPLUS + && _tfminiplus_setup._setup_step == TFMINIPLUS_SETUP_STEP::STEP4_SAVE_CONFIRMED) { + PX4_INFO("TFMINI-Plus is configured - version = %u.%u.%u\n", _tfminiplus_setup._version[0], + _tfminiplus_setup._version[1], + _tfminiplus_setup._version[2]); + + } else if (_hw_model == TFMINI_MODEL::MODEL_TFMINIPLUS) { + PX4_INFO("TFMINI-Plus is being configured(state = %u c = %u) - version = %u.%u.%u\n", + (uint8_t)_tfminiplus_setup._setup_step, + _tfminiplus_setup._counter, _tfminiplus_setup._version[0], _tfminiplus_setup._version[1], + _tfminiplus_setup._version[2]); + + } else if (_hw_model == TFMINI_MODEL::MODEL_TFMINI + && _tfmini_setup._setup_step == TFMINI_SETUP_STEP::STEP4_SAVE_CONFIRMED) { + PX4_INFO("TFMINI is configured - version = unknown\n"); + + } else if (_hw_model == TFMINI_MODEL::MODEL_TFMINI) { + PX4_INFO("TFMINI is being configured(state = %u c = %u) - version = unknown\n", (uint8_t)_tfmini_setup._setup_step, + _tfmini_setup._counter); + } + perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); } diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.hpp b/src/drivers/distance_sensor/tfmini/TFMINI.hpp index 027d73fa5248..2a5bf4dbac19 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.hpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2020 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 @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -60,6 +61,45 @@ using namespace time_literals; +enum class TFMINI_SETUP_STEP { + STEP0_UNCONFIGURED = 0, + STEP1_ENTER_CONFIRMED = 1, + STEP2_MODE_CONFIRMED = 2, + STEP3_UNIT_CONFIRMED = 3, + STEP4_SAVE_CONFIRMED = 4 +}; + +class TFMINI_SETUP +{ +public: + uint8_t _counter{0}; + TFMINI_SETUP_STEP _setup_step{TFMINI_SETUP_STEP::STEP0_UNCONFIGURED}; + uint8_t _com_enter[8] {0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x02}; //Enter configuration mode + uint8_t _com_mode[8] {0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06}; //Configure "Standard format, as show in in Table 6" + uint8_t _com_unit[8] {0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x1A}; //Configure "Output unit of distance data is cm" + uint8_t _com_save[8] {0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x00, 0x02}; //Exit configuration mode +}; + +enum class TFMINIPLUS_SETUP_STEP { + STEP0_UNCONFIGURED = 0, + STEP1_VERSION_CONFIRMED = 1, + STEP2_MODE_CONFIRMED = 2, + STEP3_ENABLE_CONFIRMED = 3, + STEP4_SAVE_CONFIRMED = 4 +}; + +class TFMINIPLUS_SETUP +{ +public: + uint8_t _counter{0}; + TFMINIPLUS_SETUP_STEP _setup_step{TFMINIPLUS_SETUP_STEP::STEP0_UNCONFIGURED}; + uint8_t _com_version[4] {0x5A, 0x04, 0x01, 0x5F}; + uint8_t _version[3] {0, 0, 0}; + uint8_t _com_mode[5] {0x5A, 0x05, 0x05, 0x01, 0x65}; + uint8_t _com_enable[5] {0x5A, 0x05, 0x07, 0x01, 0x67}; + uint8_t _com_save[4] {0X5A, 0x04, 0x11, 0x6F}; +}; + class TFMINI : public px4::ScheduledWorkItem { public: @@ -68,6 +108,10 @@ class TFMINI : public px4::ScheduledWorkItem int init(); + int write_command(uint8_t *command, uint8_t framelen); + bool get_command_result(); + uint8_t *get_command_response(uint8_t *response_size); + void print_info(); private: @@ -78,14 +122,28 @@ class TFMINI : public px4::ScheduledWorkItem void start(); void stop(); + void autosetup_tfmini(); + void autosetup_tfmini_process(); + void autosetup_tfminiplus(); + void autosetup_tfminiplus_process(); PX4Rangefinder _px4_rangefinder; + TFMINI_MODEL _hw_model {TFMINI_MODEL::MODEL_UNKNOWN}; TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE::STATE0_UNSYNC}; - - char _linebuf[10] {}; + uint8_t _linebuf[10] {}; char _port[20] {}; + bool _command_result{false}; + uint8_t _command_buf[10] {}; + uint8_t _command_size{0}; + uint8_t _command_response[10] {}; + uint8_t _command_response_size{0}; + uint8_t _command_retry{0}; + + TFMINI_SETUP _tfmini_setup; + TFMINIPLUS_SETUP _tfminiplus_setup; + static constexpr int kCONVERSIONINTERVAL{9_ms}; int _fd{-1}; diff --git a/src/drivers/distance_sensor/tfmini/parameters.c b/src/drivers/distance_sensor/tfmini/parameters.c new file mode 100644 index 000000000000..8eaaaf960bdb --- /dev/null +++ b/src/drivers/distance_sensor/tfmini/parameters.c @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 laser rangefinder + * + * @reboot_required true + * + * @boolean + * @group Sensors + * @value 0 undefined - prevent any non wanted commands to be executed + * @value 1 TFMINI + * @value 2 TFMINI-plus + */ +PARAM_DEFINE_INT32(SENS_EN_TFMINI, 0); diff --git a/src/drivers/distance_sensor/tfmini/tfmini_main.cpp b/src/drivers/distance_sensor/tfmini/tfmini_main.cpp index 4b3a9f51ff02..d86f5f6f93ed 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini_main.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2020 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 @@ -46,6 +46,7 @@ TFMINI *g_dev{nullptr}; int start(const char *port, uint8_t rotation); int status(); int stop(); +int command(uint8_t *command, uint8_t framelen); int usage(); int @@ -88,6 +89,46 @@ status() return 0; } +/** + * Perform some commands the driver; + * This can help to configure device. + * Refer to your TFMINI documentation for the available commands. + */ +int +command(uint8_t *command, uint8_t framelen) +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + g_dev->write_command(command, framelen); + + int counter = 0; + + do { + if (g_dev->get_command_result()) { + uint8_t responselen; + uint8_t *cresponse = g_dev->get_command_response(&responselen); + char response[10]; + uint8_t idx; + + for (idx = 0; idx < responselen; idx++) { + sprintf(response + idx * 2, "%02X", cresponse[idx]); + } + + PX4_INFO("command confirmed [%ims] [%s]", 20 * counter, response); + return PX4_OK; + } + + px4_usleep(20000); + counter++; + } while (counter < 10); // wait 200ms for a command response - should be enough + + PX4_ERR("command not confirmed"); + return PX4_ERROR; +} + int stop() { if (g_dev != nullptr) { @@ -123,6 +164,14 @@ Attempt to start driver on a specified serial device. $ tfmini start -d /dev/ttyS1 Stop driver $ tfmini stop +Retreive the version on a TFMINI-Plus: (0x5A 0x04 0x01 0x5F) +$ tfmini command -c 5A04015F +Configure TFMINI-Plus in Standard 9 bytes (cm) mode: (0x5A 0x05 0x05 0x01 0x65) +$ tfmini command -c 5A05050165 +Configure TFMINI-Plus Frame Rate: (0x5A 0x06 0x03 0xLL 0xHH 0xSU) - Example for 100Hz +$ tfmini command -c 5A06036400C7 +Apply and Save Settings for TFMINI-Plus: (0x5A 0x04 0x11 0x6F) +$ tfmini command -c 5A04116F )DESCR_STR"); PRINT_MODULE_USAGE_NAME("tfmini", "driver"); @@ -132,7 +181,8 @@ Stop driver PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); PRINT_MODULE_USAGE_COMMAND_DESCR("status","Driver status"); PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("command","Configure driver (require -c hexacommand as in -c 5A04015F)"); + PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, nullptr, "hexacommand", false); PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status"); return PX4_OK; } @@ -146,8 +196,9 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) const char *device_path = TFMINI_DEFAULT_PORT; int myoptind = 1; const char *myoptarg = nullptr; + const char *hexacommand = nullptr; - while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "R:d:c:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': rotation = (uint8_t)atoi(myoptarg); @@ -157,6 +208,10 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) device_path = myoptarg; break; + case 'c': + hexacommand = myoptarg; + break; + default: PX4_WARN("Unknown option!"); return PX4_ERROR; @@ -180,6 +235,31 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) } else if (!strcmp(argv[myoptind], "stop")) { return tfmini::stop(); + } else if (!strcmp(argv[myoptind], "command")) { + uint8_t mycommandlen=0; + uint8_t mycommand[8]; + + if (hexacommand == nullptr) + return tfmini::usage(); + + uint8_t len = strlen(hexacommand); + + if (len>16) { + PX4_ERR("invalid command - too long"); + return PX4_ERROR; + } + + //convert hexacommand to a table of of uint8_t + while (mycommandlen -#include // #define TFMINI_DEBUG @@ -52,29 +51,45 @@ const char *parser_state[] = { "0_UNSYNC", "1_SYNC_1", - "2_SYNC_2", - "3_GOT_DIST_L", - "4_GOT_DIST_H", - "5_GOT_STRENGTH_L", - "6_GOT_STRENGTH_H", - "7_GOT_PRESERVED", - "8_GOT_QUALITY" - "9_GOT_CHECKSUM" + "1_SYNC_2", + "2_GOT_DIST_L", + "2_GOT_DIST_H", + "3_GOT_STRENGTH_L", + "3_GOT_STRENGTH_H", + "4_GOT_PRESERVED", + "5_GOT_QUALITY", + "6_GOT_CHECKSUM", + "7_GOT_COMMAND_RESPONSE", + "8_GOT_RESPONSE_CHECKSUM" }; #endif -int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARSE_STATE *state, float *dist) +int tfmini_parse(uint8_t c, uint8_t *parserbuf, unsigned *parserbuf_index, TFMINI_PARSE_STATE *state, + TFMINI_MODEL hw_model, float *dist, uint16_t *strength, float *temperature, uint8_t *commandresponse, + uint8_t *commandresponse_size) { int ret = -1; //char *end; + unsigned char cksm = 0; switch (*state) { case TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM: - if (c == 'Y') { + case TFMINI_PARSE_STATE::STATE8_GOT_RESPONSE_CHECKSUM: + if (c == TFMINI_DATA_HEADER) { *state = TFMINI_PARSE_STATE::STATE1_SYNC_1; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; + } else if (hw_model == TFMINI_MODEL::MODEL_TFMINI && c == TFMINI_CMD_HEADER1) { + *state = TFMINI_PARSE_STATE::STATE7_GOT_COMMAND_RESPONSE; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else if (hw_model == TFMINI_MODEL::MODEL_TFMINIPLUS && c == TFMINIPLUS_CMD_HEADER1) { + *state = TFMINI_PARSE_STATE::STATE7_GOT_COMMAND_RESPONSE; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { *state = TFMINI_PARSE_STATE::STATE0_UNSYNC; } @@ -82,20 +97,40 @@ int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARS break; case TFMINI_PARSE_STATE::STATE0_UNSYNC: - if (c == 'Y') { + if (c == TFMINI_DATA_HEADER) { *state = TFMINI_PARSE_STATE::STATE1_SYNC_1; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; + + } else if (hw_model == TFMINI_MODEL::MODEL_TFMINI && c == TFMINI_CMD_HEADER1) { + *state = TFMINI_PARSE_STATE::STATE7_GOT_COMMAND_RESPONSE; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else if (hw_model == TFMINI_MODEL::MODEL_TFMINIPLUS && c == TFMINIPLUS_CMD_HEADER1) { + *state = TFMINI_PARSE_STATE::STATE7_GOT_COMMAND_RESPONSE; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; } break; case TFMINI_PARSE_STATE::STATE1_SYNC_1: - if (c == 'Y') { + if (c == TFMINI_DATA_HEADER) { *state = TFMINI_PARSE_STATE::STATE1_SYNC_2; parserbuf[*parserbuf_index] = c; (*parserbuf_index)++; + } else if (hw_model == TFMINI_MODEL::MODEL_TFMINI && c == TFMINI_CMD_HEADER1) { + *state = TFMINI_PARSE_STATE::STATE7_GOT_COMMAND_RESPONSE; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else if (hw_model == TFMINI_MODEL::MODEL_TFMINIPLUS && c == TFMINIPLUS_CMD_HEADER1) { + *state = TFMINI_PARSE_STATE::STATE7_GOT_COMMAND_RESPONSE; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { *state = TFMINI_PARSE_STATE::STATE0_UNSYNC; *parserbuf_index = 0; @@ -147,7 +182,7 @@ int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARS case TFMINI_PARSE_STATE::STATE5_GOT_QUALITY: // Find the checksum - unsigned char cksm = 0; + cksm = 0; for (int i = 0; i < 8; i++) { cksm += parserbuf[i]; @@ -155,11 +190,9 @@ int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARS if (c == cksm) { parserbuf[*parserbuf_index] = '\0'; - unsigned int t1 = parserbuf[2]; - unsigned int t2 = parserbuf[3]; - t2 <<= 8; - t2 += t1; - *dist = ((float)t2) / 100; + *dist = static_cast(parserbuf[2] + (parserbuf[3] << 8)) / 100.f; + *strength = parserbuf[4] + (parserbuf[5] << 8); + *temperature = static_cast(parserbuf[6] + (parserbuf[7] << 8)) / 8.f - 256.f; *state = TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM; *parserbuf_index = 0; ret = 0; @@ -170,6 +203,67 @@ int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARS } break; + + case TFMINI_PARSE_STATE::STATE7_GOT_COMMAND_RESPONSE: + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + // verify command line answers for tfmini + if (hw_model == TFMINI_MODEL::MODEL_TFMINI) { + if (parserbuf[1] != TFMINI_CMD_HEADER2) { + // discard first 2 bytes + *state = TFMINI_PARSE_STATE::STATE0_UNSYNC; + *parserbuf_index = 0; + break; + } + + if ((*parserbuf_index) < TFMINI_CMD_SIZE) { + break; + } + + *commandresponse_size = *parserbuf_index; + memcpy(commandresponse, parserbuf, *parserbuf_index); + + *state = TFMINI_PARSE_STATE::STATE8_GOT_RESPONSE_CHECKSUM; + *parserbuf_index = 0; + break; + } + + // verify command line answers for tfmini-plus + if (hw_model == TFMINI_MODEL::MODEL_TFMINIPLUS) { + if (*parserbuf_index < parserbuf[1]) { + break; + } + + if (parserbuf[1] > 8) { + *state = TFMINI_PARSE_STATE::STATE0_UNSYNC; + *parserbuf_index = 0; + break; + } + + // Find the checksum + cksm = 0; + + for (uint8_t i = 0; i < (*parserbuf_index) - 1; i++) { + cksm += parserbuf[i]; + } + + if (c == cksm) { + *commandresponse_size = *parserbuf_index; + memcpy(commandresponse, parserbuf, *parserbuf_index); + + *state = TFMINI_PARSE_STATE::STATE8_GOT_RESPONSE_CHECKSUM; + *parserbuf_index = 0; + break; + } + + *state = TFMINI_PARSE_STATE::STATE0_UNSYNC; + *parserbuf_index = 0; + break; + } + + break; + } #ifdef TFMINI_DEBUG diff --git a/src/drivers/distance_sensor/tfmini/tfmini_parser.h b/src/drivers/distance_sensor/tfmini/tfmini_parser.h index dbf40a5f5d85..5bec50204600 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini_parser.h +++ b/src/drivers/distance_sensor/tfmini/tfmini_parser.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2020 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 @@ -42,19 +42,54 @@ #pragma once -// Data Format for Benewake TFmini -// =============================== +#include +#include + +// Data Format for Benewake TFmini models +// ====================================== // 9 bytes total per message: +// 0) 0x59 // 1) 0x59 -// 2) 0x59 -// 3) Dist_L (low 8bit) -// 4) Dist_H (high 8bit) -// 5) Strength_L (low 8bit) -// 6) Strength_H (high 8bit) -// 7) Reserved bytes -// 8) Original signal quality degree -// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though +// 2) Dist_L (low 8bit) +// 3) Dist_H (high 8bit) +// 4) Strength_L (low 8bit) +// 5) Strength_H (high 8bit) +// 6) Reserved bytes +// 7) Original signal quality degree +// 8) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though + +// Command -and answers- Frame definitions for TFmini model +// ============================================================= +// 8 bytes total per message: +// 0) 0x42 +// 1) 0x57 +// 2) 0x02 by default +// 3) 0x00 on sending - 0x01 or 0xFF or 0x0f on receiving +// 4;5) EE FF double byte parameter, EE is low 8 bit; FF is high 8 bits +// 6) GG single byte parameter +// 7) HH instruction code + +// Command -and answers- Frame definitions for TFmini-plus model +// ============================================================= +// 0) 0x5A +// 1) Len: the total length of the frame（include Head and Checksum，unit: byte) +// 2) ID: identifier code of command +// 3;N-2) Data: data segment. Little endian format +// N-1) Checksum: sum of all bytes from Head to payload. Lower 8 bits +#define TFMINI_DATA_HEADER 0x59 +#define TFMINI_CMD_HEADER1 0x42 +#define TFMINI_CMD_HEADER2 0x57 +#define TFMINI_CMD_SIZE 8 + +#define TFMINIPLUS_CMD_HEADER1 0x5A +#define TFMINIPLUS_INVALID_MEASURE 0xFFFF + +enum class TFMINI_MODEL { + MODEL_UNKNOWN = 0, + MODEL_TFMINI, + MODEL_TFMINIPLUS +}; enum class TFMINI_PARSE_STATE { STATE0_UNSYNC = 0, @@ -66,7 +101,11 @@ enum class TFMINI_PARSE_STATE { STATE3_GOT_STRENGTH_H, STATE4_GOT_RESERVED, STATE5_GOT_QUALITY, - STATE6_GOT_CHECKSUM + STATE6_GOT_CHECKSUM, + STATE7_GOT_COMMAND_RESPONSE, + STATE8_GOT_RESPONSE_CHECKSUM }; -int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARSE_STATE *state, float *dist); +int tfmini_parse(uint8_t c, uint8_t *parserbuf, unsigned *parserbuf_index, TFMINI_PARSE_STATE *state, + TFMINI_MODEL hw_model, float *dist, uint16_t *strength, float *temperature, uint8_t *commandresponse, + uint8_t *commandresponse_size); From 2c60d673aff9b89544236dd3188ff5544763b3bc Mon Sep 17 00:00:00 2001 From: mcsauder Date: Sat, 2 Oct 2021 10:41:43 -0600 Subject: [PATCH 2/2] Refactor do-while into while loop. --- src/drivers/distance_sensor/tfmini/tfmini_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/distance_sensor/tfmini/tfmini_main.cpp b/src/drivers/distance_sensor/tfmini/tfmini_main.cpp index d86f5f6f93ed..e0d74df8cf4e 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini_main.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini_main.cpp @@ -106,7 +106,7 @@ command(uint8_t *command, uint8_t framelen) int counter = 0; - do { + while (counter < 10) { // wait 200ms for a command response - should be enough if (g_dev->get_command_result()) { uint8_t responselen; uint8_t *cresponse = g_dev->get_command_response(&responselen); @@ -123,7 +123,7 @@ command(uint8_t *command, uint8_t framelen) px4_usleep(20000); counter++; - } while (counter < 10); // wait 200ms for a command response - should be enough + } PX4_ERR("command not confirmed"); return PX4_ERROR;