diff --git a/src/drivers/distance_sensor/sf0x/CMakeLists.txt b/src/drivers/distance_sensor/sf0x/CMakeLists.txt index 3b4ab53a70..62fa50cf36 100644 --- a/src/drivers/distance_sensor/sf0x/CMakeLists.txt +++ b/src/drivers/distance_sensor/sf0x/CMakeLists.txt @@ -36,7 +36,9 @@ px4_add_module( COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS - sf0x.cpp + SF0X.cpp + SF0X.hpp + sf0x_main.cpp sf0x_parser.cpp MODULE_CONFIG module.yaml diff --git a/src/drivers/distance_sensor/sf0x/sf0x.cpp b/src/drivers/distance_sensor/sf0x/SF0X.cpp similarity index 62% rename from src/drivers/distance_sensor/sf0x/sf0x.cpp rename to src/drivers/distance_sensor/sf0x/SF0X.cpp index 0f9c94db26..587426b446 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x.cpp +++ b/src/drivers/distance_sensor/sf0x/SF0X.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-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 @@ -31,130 +31,7 @@ * ****************************************************************************/ -/** - * @file sf0x.cpp - * @author Lorenz Meier - * @author Greg Hulands - * - * Driver for the Lightware SF0x laser rangefinder series - */ - -#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 "sf0x_parser.h" - -/* Configuration Constants */ - -#define SF0X_TAKE_RANGE_REG 'd' - -// designated SERIAL4/5 on Pixhawk -#define SF0X_DEFAULT_PORT "/dev/ttyS6" - -class SF0X : public cdev::CDev, public px4::ScheduledWorkItem -{ -public: - SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - virtual ~SF0X(); - - virtual int init() override; - - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -private: - char _port[20]; - uint8_t _rotation; - float _min_distance; - float _max_distance; - int _conversion_interval; - ringbuffer::RingBuffer *_reports; - int _measure_interval; - bool _collect_phase; - int _fd; - char _linebuf[10]; - unsigned _linebuf_index; - enum SF0X_PARSE_STATE _parse_state; - hrt_abstime _last_read; - - int _class_instance; - int _orb_class_instance; - - orb_advert_t _distance_sensor_topic; - - unsigned _consecutive_fail_count; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - - /** - * 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(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE - * and SF0X_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; - int measure(); - int collect(); - -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); +#include "SF0X.hpp" SF0X::SF0X(const char *port, uint8_t rotation) : CDev(RANGE_FINDER0_DEVICE_PATH), @@ -663,263 +540,3 @@ SF0X::print_info() printf("poll interval: %d\n", _measure_interval); _reports->print_info("report queue"); } - -/** - * Local functions in support of the shell command. - */ -namespace sf0x -{ - -SF0X *g_dev; - -int start(const char *port, uint8_t rotation); -int stop(); -int test(); -int reset(); -int info(); - -/** - * Start the driver. - */ -int -start(const char *port, uint8_t rotation) -{ - int fd; - - if (g_dev != nullptr) { - PX4_WARN("already started"); - return -1; - } - - /* create the driver */ - g_dev = new SF0X(port, rotation); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER0_DEVICE_PATH, 0); - - if (fd < 0) { - PX4_ERR("device open fail (%i)", errno); - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - return 0; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - return -1; -} - -/** - * Stop the driver - */ -int stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - return -1; - } - - return 0; -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -int -test() -{ - struct distance_sensor_s report; - ssize_t sz; - - int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH); - return -1; - } - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("immediate read failed"); - return -1; - } - - print_message(report); - - /* start the sensor polling at 2 Hz rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - PX4_ERR("failed to set 2Hz poll rate"); - return -1; - } - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - int ret = poll(&fds, 1, 2000); - - if (ret != 1) { - PX4_ERR("timed out"); - break; - } - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report)); - break; - } - - print_message(report); - } - - /* reset the sensor polling to the default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - PX4_ERR("ioctl SENSORIOCSPOLLRATE failed"); - return -1; - } - - return 0; -} - -/** - * Reset the driver. - */ -int -reset() -{ - int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("open failed (%i)", errno); - return -1; - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return -1; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return -1; - } - - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return -1; - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - return 0; -} - -} // namespace - -int -sf0x_main(int argc, char *argv[]) -{ - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - const char *device_path = SF0X_DEFAULT_PORT; - int ch; - 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 -1; - } - } - - if (myoptind >= argc) { - goto out_error; - } - - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - return sf0x::start(device_path, rotation); - } - - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - return sf0x::stop(); - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[myoptind], "test")) { - return sf0x::test(); - } - - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - return sf0x::reset(); - } - - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - return sf0x::info(); - } - -out_error: - PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); - return -1; -} diff --git a/src/drivers/distance_sensor/sf0x/SF0X.hpp b/src/drivers/distance_sensor/sf0x/SF0X.hpp new file mode 100644 index 0000000000..41185dad0d --- /dev/null +++ b/src/drivers/distance_sensor/sf0x/SF0X.hpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 SF0X.hpp + * @author Lorenz Meier + * @author Greg Hulands + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "sf0x_parser.h" + +/* Configuration Constants */ + +#define SF0X_TAKE_RANGE_REG 'd' + +// designated SERIAL4/5 on Pixhawk +#define SF0X_DEFAULT_PORT "/dev/ttyS6" + +class SF0X : public cdev::CDev, public px4::ScheduledWorkItem +{ +public: + SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + virtual ~SF0X(); + + virtual int init() override; + + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + char _port[20]; + uint8_t _rotation; + float _min_distance; + float _max_distance; + int _conversion_interval; + ringbuffer::RingBuffer *_reports; + int _measure_interval; + bool _collect_phase; + int _fd; + char _linebuf[10]; + unsigned _linebuf_index; + enum SF0X_PARSE_STATE _parse_state; + hrt_abstime _last_read; + + int _class_instance; + int _orb_class_instance; + + orb_advert_t _distance_sensor_topic; + + unsigned _consecutive_fail_count; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + + /** + * 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(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE + * and SF0X_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void Run() override; + int measure(); + int collect(); + +}; diff --git a/src/drivers/distance_sensor/sf0x/sf0x_main.cpp b/src/drivers/distance_sensor/sf0x/sf0x_main.cpp new file mode 100644 index 0000000000..b6713bc613 --- /dev/null +++ b/src/drivers/distance_sensor/sf0x/sf0x_main.cpp @@ -0,0 +1,301 @@ +/**************************************************************************** + * + * Copyright (c) 2014-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 "SF0X.hpp" + +#include + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); + +/** + * Local functions in support of the shell command. + */ +namespace sf0x +{ + +SF0X *g_dev; + +int start(const char *port, uint8_t rotation); +int stop(); +int test(); +int reset(); +int info(); + +/** + * Start the driver. + */ +int +start(const char *port, uint8_t rotation) +{ + int fd; + + if (g_dev != nullptr) { + PX4_WARN("already started"); + return -1; + } + + /* create the driver */ + g_dev = new SF0X(port, rotation); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER0_DEVICE_PATH, 0); + + if (fd < 0) { + PX4_ERR("device open fail (%i)", errno); + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + return 0; + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + return -1; +} + +/** + * Stop the driver + */ +int stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + return -1; + } + + return 0; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +int +test() +{ + struct distance_sensor_s report; + ssize_t sz; + + int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + PX4_ERR("%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH); + return -1; + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("immediate read failed"); + return -1; + } + + print_message(report); + + /* start the sensor polling at 2 Hz rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_ERR("failed to set 2Hz poll rate"); + return -1; + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + int ret = poll(&fds, 1, 2000); + + if (ret != 1) { + PX4_ERR("timed out"); + break; + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report)); + break; + } + + print_message(report); + } + + /* reset the sensor polling to the default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_ERR("ioctl SENSORIOCSPOLLRATE failed"); + return -1; + } + + return 0; +} + +/** + * Reset the driver. + */ +int +reset() +{ + int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + PX4_ERR("open failed (%i)", errno); + return -1; + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + return -1; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); + return -1; + } + + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return -1; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; +} + +} // namespace + +int +sf0x_main(int argc, char *argv[]) +{ + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + const char *device_path = SF0X_DEFAULT_PORT; + int ch; + 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 -1; + } + } + + if (myoptind >= argc) { + goto out_error; + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[myoptind], "start")) { + return sf0x::start(device_path, rotation); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[myoptind], "stop")) { + return sf0x::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[myoptind], "test")) { + return sf0x::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[myoptind], "reset")) { + return sf0x::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { + return sf0x::info(); + } + +out_error: + PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); + return -1; +}