From be61d93d649404b8c803a93a4fa951062bffc364 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Sep 2019 16:37:00 -0400 Subject: [PATCH] tfmini split out header and main --- .../distance_sensor/tfmini/CMakeLists.txt | 8 +- .../tfmini/{tfmini.cpp => TFMINI.cpp} | 219 +----------------- src/drivers/distance_sensor/tfmini/TFMINI.hpp | 100 ++++++++ .../distance_sensor/tfmini/tfmini_main.cpp | 188 +++++++++++++++ 4 files changed, 294 insertions(+), 221 deletions(-) rename src/drivers/distance_sensor/tfmini/{tfmini.cpp => TFMINI.cpp} (58%) create mode 100644 src/drivers/distance_sensor/tfmini/TFMINI.hpp create mode 100644 src/drivers/distance_sensor/tfmini/tfmini_main.cpp diff --git a/src/drivers/distance_sensor/tfmini/CMakeLists.txt b/src/drivers/distance_sensor/tfmini/CMakeLists.txt index d5ac0bac6b..51ad9c5f4b 100644 --- a/src/drivers/distance_sensor/tfmini/CMakeLists.txt +++ b/src/drivers/distance_sensor/tfmini/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# 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 @@ -33,10 +33,10 @@ px4_add_module( MODULE drivers__tfmini MAIN tfmini - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS - tfmini.cpp + TFMINI.cpp + TFMINI.hpp + tfmini_main.cpp tfmini_parser.cpp MODULE_CONFIG module.yaml diff --git a/src/drivers/distance_sensor/tfmini/tfmini.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp similarity index 58% rename from src/drivers/distance_sensor/tfmini/tfmini.cpp rename to src/drivers/distance_sensor/tfmini/TFMINI.cpp index 50efb06b56..8418544f14 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -31,72 +31,7 @@ * ****************************************************************************/ -/** - * @file tfmini.cpp - * @author Lorenz Meier - * @author Greg Hulands - * @author Ayush Gaud - * @author Christoph Tobler - * @author Mohammed Kabir - * - * Driver for the Benewake TFmini laser rangefinder series - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "tfmini_parser.h" - -#define TFMINI_DEFAULT_PORT "/dev/ttyS3" - -using namespace time_literals; - -class TFMINI : public px4::ScheduledWorkItem -{ -public: - TFMINI(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - virtual ~TFMINI(); - - int init(); - - void print_info(); - -private: - - int collect(); - - void Run() override; - - void start(); - void stop(); - - PX4Rangefinder _px4_rangefinder; - - TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE::STATE0_UNSYNC}; - - char _linebuf[10] {}; - char _port[20] {}; - - static constexpr int kCONVERSIONINTERVAL{9_ms}; - - int _fd{-1}; - - unsigned int _linebuf_index{0}; - - hrt_abstime _last_read{0}; - - perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; - perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; - -}; +#include "TFMINI.hpp" TFMINI::TFMINI(const char *port, uint8_t rotation) : ScheduledWorkItem(px4::wq_configurations::hp_default), @@ -127,7 +62,7 @@ TFMINI::init() case 1: // TFMINI (12m, 100 Hz) _px4_rangefinder.set_min_distance(0.3f); _px4_rangefinder.set_max_distance(12.0f); - _px4_rangefinder.set_fov(math::radians(1.15)); + _px4_rangefinder.set_fov(math::radians(1.15f)); break; @@ -325,153 +260,3 @@ TFMINI::print_info() _px4_rangefinder.print_status(); } - -/** - * Local functions in support of the shell command. - */ -namespace tfmini -{ - -TFMINI *g_dev{nullptr}; - -int start(const char *port, uint8_t rotation); -int status(); -int stop(); -int usage(); - -int -start(const char *port, uint8_t rotation) -{ - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_OK; - } - - // Instantiate the driver. - g_dev = new TFMINI(port, rotation); - - if (g_dev == nullptr) { - PX4_ERR("driver start failed"); - return PX4_ERROR; - } - - if (OK != g_dev->init()) { - PX4_ERR("driver start failed"); - delete g_dev; - g_dev = nullptr; - return PX4_ERROR; - } - - return PX4_OK; -} - -int -status() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - return 0; -} - -int stop() -{ - if (g_dev != nullptr) { - PX4_INFO("stopping driver"); - delete g_dev; - g_dev = nullptr; - PX4_INFO("driver stopped"); - - } else { - PX4_ERR("driver not running"); - return 1; - } - - return PX4_OK; -} - -int -usage() -{ - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description - -Serial bus driver for the Benewake TFmini LiDAR. - -Most boards are configured to enable/start the driver on a specified UART using the SENS_TFMINI_CFG parameter. - -Setup/usage information: https://docs.px4.io/en/sensor/tfmini.html - -### Examples - -Attempt to start driver on a specified serial device. -$ tfmini start -d /dev/ttyS1 -Stop driver -$ tfmini stop -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("tfmini", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); - PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); - PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 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("status","Print driver status"); - return PX4_OK; -} - -} // namespace - -extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) -{ - int ch = 0; - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - const char *device_path = TFMINI_DEFAULT_PORT; - int myoptind = 1; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - break; - - case 'd': - device_path = myoptarg; - break; - - default: - PX4_WARN("Unknown option!"); - return PX4_ERROR; - } - } - - if (myoptind >= argc) { - PX4_ERR("unrecognized command"); - return tfmini::usage(); - } - - if (!strcmp(argv[myoptind], "start")) { - if (strcmp(device_path, "") != 0) { - return tfmini::start(device_path, rotation); - - } else { - PX4_WARN("Please specify device path!"); - return tfmini::usage(); - } - } else if (!strcmp(argv[myoptind], "stop")) { - return tfmini::stop(); - } else if (!strcmp(argv[myoptind], "status")) { - return tfmini::status(); - } - - return tfmini::usage(); -} diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.hpp b/src/drivers/distance_sensor/tfmini/TFMINI.hpp new file mode 100644 index 0000000000..c7199dea96 --- /dev/null +++ b/src/drivers/distance_sensor/tfmini/TFMINI.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file tfmini.cpp + * @author Lorenz Meier + * @author Greg Hulands + * @author Ayush Gaud + * @author Christoph Tobler + * @author Mohammed Kabir + * + * Driver for the Benewake TFmini laser rangefinder series + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "tfmini_parser.h" + +#define TFMINI_DEFAULT_PORT "/dev/ttyS3" + +using namespace time_literals; + +class TFMINI : public px4::ScheduledWorkItem +{ +public: + TFMINI(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + virtual ~TFMINI(); + + int init(); + + void print_info(); + +private: + + int collect(); + + void Run() override; + + void start(); + void stop(); + + PX4Rangefinder _px4_rangefinder; + + TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE::STATE0_UNSYNC}; + + char _linebuf[10] {}; + char _port[20] {}; + + static constexpr int kCONVERSIONINTERVAL{9_ms}; + + int _fd{-1}; + + unsigned int _linebuf_index{0}; + + hrt_abstime _last_read{0}; + + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + +}; diff --git a/src/drivers/distance_sensor/tfmini/tfmini_main.cpp b/src/drivers/distance_sensor/tfmini/tfmini_main.cpp new file mode 100644 index 0000000000..a48149e5d8 --- /dev/null +++ b/src/drivers/distance_sensor/tfmini/tfmini_main.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "TFMINI.hpp" + +#include + +/** + * Local functions in support of the shell command. + */ +namespace tfmini +{ + +TFMINI *g_dev{nullptr}; + +int start(const char *port, uint8_t rotation); +int status(); +int stop(); +int usage(); + +int +start(const char *port, uint8_t rotation) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_OK; + } + + // Instantiate the driver. + g_dev = new TFMINI(port, rotation); + + if (g_dev == nullptr) { + PX4_ERR("driver start failed"); + return PX4_ERROR; + } + + if (OK != g_dev->init()) { + PX4_ERR("driver start failed"); + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; + } + + return PX4_OK; +} + +int +status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; +} + +int stop() +{ + if (g_dev != nullptr) { + PX4_INFO("stopping driver"); + delete g_dev; + g_dev = nullptr; + PX4_INFO("driver stopped"); + + } else { + PX4_ERR("driver not running"); + return 1; + } + + return PX4_OK; +} + +int +usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Serial bus driver for the Benewake TFmini LiDAR. + +Most boards are configured to enable/start the driver on a specified UART using the SENS_TFMINI_CFG parameter. + +Setup/usage information: https://docs.px4.io/en/sensor/tfmini.html + +### Examples + +Attempt to start driver on a specified serial device. +$ tfmini start -d /dev/ttyS1 +Stop driver +$ tfmini stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("tfmini", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); + PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 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("status","Print driver status"); + return PX4_OK; +} + +} // namespace + +extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) +{ + int ch = 0; + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + const char *device_path = TFMINI_DEFAULT_PORT; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (uint8_t)atoi(myoptarg); + break; + + case 'd': + device_path = myoptarg; + break; + + default: + PX4_WARN("Unknown option!"); + return PX4_ERROR; + } + } + + if (myoptind >= argc) { + PX4_ERR("unrecognized command"); + return tfmini::usage(); + } + + if (!strcmp(argv[myoptind], "start")) { + if (strcmp(device_path, "") != 0) { + return tfmini::start(device_path, rotation); + + } else { + PX4_WARN("Please specify device path!"); + return tfmini::usage(); + } + + } else if (!strcmp(argv[myoptind], "stop")) { + return tfmini::stop(); + + } else if (!strcmp(argv[myoptind], "status")) { + return tfmini::status(); + } + + return tfmini::usage(); +}