diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 3937f8d493..387eda1af6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -54,7 +54,7 @@ if param compare -s SENS_EN_LL40LS 1 then if pwm_input start then - ll40ls start pwm + ll40ls_pwm start fi fi diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index 9790b1c030..bb6607272c 100644 --- a/src/drivers/distance_sensor/CMakeLists.txt +++ b/src/drivers/distance_sensor/CMakeLists.txt @@ -34,6 +34,7 @@ add_subdirectory(cm8jl65) add_subdirectory(leddar_one) add_subdirectory(ll40ls) +add_subdirectory(ll40ls_pwm) add_subdirectory(mappydot) add_subdirectory(mb12xx) add_subdirectory(pga460) diff --git a/src/drivers/distance_sensor/ll40ls/CMakeLists.txt b/src/drivers/distance_sensor/ll40ls/CMakeLists.txt index 31d36bf056..659fdb8b20 100644 --- a/src/drivers/distance_sensor/ll40ls/CMakeLists.txt +++ b/src/drivers/distance_sensor/ll40ls/CMakeLists.txt @@ -37,10 +37,7 @@ px4_add_module( -Wno-cast-align # TODO: fix and enable SRCS ll40ls.cpp - LidarLite.cpp LidarLiteI2C.cpp - LidarLitePWM.cpp DEPENDS drivers_rangefinder - #arch_io_pins # LidarLitePWM ) diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp deleted file mode 100644 index 19eb40da0f..0000000000 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * 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 LidarLite.h - * @author Johan Jansen - * - * Generic interface driver for the PulsedLight Lidar-Lite range finders. - */ - -#include "LidarLite.h" - -LidarLite::LidarLite(const uint8_t rotation) : - _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation) -{ - _px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE); - _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE); - _px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian -} - -LidarLite::~LidarLite() -{ - perf_free(_sample_perf); - perf_free(_comms_errors); - perf_free(_sensor_resets); - perf_free(_sensor_zero_resets); -}; - -void -LidarLite::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_sensor_resets); - perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u \n", get_measure_interval()); -} diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index 7e79ac593c..0e1f38095e 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -42,10 +42,13 @@ #include "LidarLiteI2C.h" LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int address) : - LidarLite(rotation), I2C("LL40LS", nullptr, bus, address, 100000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())) + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), + _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation) { + _px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE); + _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE); + _px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian // up the retries since the device misses the first measure attempts _retries = 3; } @@ -53,6 +56,10 @@ LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int addr LidarLiteI2C::~LidarLiteI2C() { stop(); + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_sensor_resets); + perf_free(_sensor_zero_resets); } int @@ -66,6 +73,16 @@ LidarLiteI2C::init() return PX4_OK; } +void +LidarLiteI2C::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_sensor_resets); + perf_print_counter(_sensor_zero_resets); + printf("poll interval: %u \n", get_measure_interval()); +} + int LidarLiteI2C::read_reg(const uint8_t reg, uint8_t &val) { diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index f4ac7041e5..16c3acca46 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -40,14 +40,16 @@ #pragma once -#include "LidarLite.h" - #include #include #include #include #include +#include +#include +#include +using namespace time_literals; /* Configuration Constants */ static constexpr uint8_t LL40LS_BASEADDR = 0x62; /* 7-bit address */ @@ -77,20 +79,30 @@ static constexpr int LL40LS_SIGNAL_STRENGTH_LOW = 24; /* Minimum signal s static constexpr int LL40LS_PEAK_STRENGTH_LOW = 135; /* Minimum peak strength for accepting a measurement */ static constexpr int LL40LS_PEAK_STRENGTH_HIGH = 234; /* Max peak strength raw value */ +static constexpr float LL40LS_MIN_DISTANCE{0.05f}; +static constexpr float LL40LS_MAX_DISTANCE{25.00f}; +static constexpr float LL40LS_MAX_DISTANCE_V2{35.00f}; -class LidarLiteI2C : public LidarLite, public device::I2C, public px4::ScheduledWorkItem +// Normal conversion wait time. +static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{50_ms}; + +// Maximum time to wait for a conversion to complete. +static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms}; + + +class LidarLiteI2C : public device::I2C, public px4::ScheduledWorkItem { public: LidarLiteI2C(const int bus, const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, const int address = LL40LS_BASEADDR); virtual ~LidarLiteI2C(); - int init() override; + int init(); /** * Print sensor registers to console for debugging. */ - void print_registers() override; + void print_registers(); /** * Initialise the automatic measurement state machine and start it. @@ -98,21 +110,24 @@ public: * @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() override; + void start(); + + void print_info(); /** * Stop the automatic measurement state machine. */ - void stop() override; + void stop(); protected: + uint32_t get_measure_interval() const { return LL40LS_CONVERSION_INTERVAL; }; - int measure() override; + int measure(); /** * Reset the sensor to power on defaults plus additional configurations. */ - int reset_sensor() override; + int reset_sensor(); int probe() override; @@ -122,7 +137,7 @@ protected: private: - int collect() override; + int collect(); /** * LidarLite specific transfer function. This is needed @@ -163,4 +178,11 @@ private: uint16_t _zero_counter{0}; uint64_t _acquire_time_usec{0}; + + PX4Rangefinder _px4_rangefinder; + + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "ll40ls: comms errors")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "ll40ls: read")}; + perf_counter_t _sensor_resets{perf_alloc(PC_COUNT, "ll40ls: resets")}; + perf_counter_t _sensor_zero_resets{perf_alloc(PC_COUNT, "ll40ls: zero resets")}; }; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h deleted file mode 100644 index 8bf4fa3d8e..0000000000 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h +++ /dev/null @@ -1,82 +0,0 @@ -/**************************************************************************** - * - * 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 LidarLitePWM.h - * @author Johan Jansen - * @author Ban Siesta - * - * Driver for the PulsedLight Lidar-Lite range finders connected via PWM. - * - * This driver accesses the pwm_input published by the pwm_input driver. - */ -#pragma once - -#include "LidarLite.h" - -#include - -#include -#include -#include - -#if DIRECT_PWM_OUTPUT_CHANNELS >= 6 -#define GPIO_VDD_RANGEFINDER_EN_CHAN 5 // use pin 6 -#define LIDAR_LITE_PWM_SUPPORTED - -class LidarLitePWM : public LidarLite, public px4::ScheduledWorkItem -{ -public: - LidarLitePWM(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - virtual ~LidarLitePWM(); - - int init() override; - void start() override; - void stop() override; - -protected: - - int collect() override; - int measure() override; - -private: - - void Run() override; - - uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)}; - - pwm_input_s _pwm{}; -}; - -#endif diff --git a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp index 6ecabe591a..278dbc6eb1 100644 --- a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp +++ b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp @@ -53,7 +53,6 @@ #include #include "LidarLiteI2C.h" -#include "LidarLitePWM.h" /** @@ -62,13 +61,12 @@ namespace ll40ls { -LidarLite *instance = nullptr; +LidarLiteI2C *instance = nullptr; int print_regs(); int start(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); int start_bus(const int bus = PX4_I2C_BUS_EXPANSION, const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); -int start_pwm(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); int status(); int stop(); int usage(); @@ -150,48 +148,6 @@ start_bus(const int bus, const uint8_t rotation) return PX4_OK; } -/** - * Start the pwm driver. - * - * This function only returns if the sensor is up and running - * or could not be detected successfully. - */ -int -start_pwm(const uint8_t rotation) -{ - if (instance != nullptr) { - PX4_ERR("already started"); - return PX4_OK; - } - -#ifdef LIDAR_LITE_PWM_SUPPORTED - instance = new LidarLitePWM(rotation); -#else - instance = nullptr; - PX4_ERR("PWM input not supported."); -#endif - - if (instance == nullptr) { - PX4_ERR("Failed to instantiate the driver"); - delete instance; - return PX4_ERROR; - } - - // Initialize the sensor. - if (instance->init() != PX4_OK) { - PX4_ERR("Failed to initialize LidarLite pwm."); - delete instance; - instance = nullptr; - return PX4_ERROR; - } - - // Start the driver. - instance->start(); - - PX4_INFO("driver started"); - return PX4_OK; -} - /** * @brief Prints status info about the driver. */ @@ -252,8 +208,6 @@ $ ll40ls stop PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND_DESCR("print_regs","Print the register values"); PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "PWM device"); - PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "I2C device"); PRINT_MODULE_USAGE_PARAM_FLAG('a', "Attempt to start driver on all I2C buses (first one found)", true); PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true); PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true); @@ -279,7 +233,6 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]) uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; bool start_i2c_all = false; - bool start_pwm = false; while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { @@ -301,23 +254,6 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]) } } - // Determine protocol. - if (argc > myoptind + 1) { - const char *protocol = argv[myoptind + 1]; - - if (!strcmp(protocol, "i2c")) { - PX4_INFO("protocol %s", protocol); - - } else if (!strcmp(protocol, "pwm")) { - PX4_INFO("protocol %s", protocol); - start_pwm = true; - - } else { - PX4_INFO("unknown protocol, choose pwm or i2c"); - return ll40ls::usage(); - } - } - if (myoptind >= argc) { return ll40ls::usage(); } @@ -333,10 +269,6 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]) PX4_INFO("starting all i2c busses"); return ll40ls::start(rotation); - } else if (start_pwm) { - PX4_INFO("starting pwm"); - return ll40ls::start_pwm(rotation); - } else { return ll40ls::start_bus(bus, rotation); } diff --git a/src/drivers/distance_sensor/ll40ls_pwm/CMakeLists.txt b/src/drivers/distance_sensor/ll40ls_pwm/CMakeLists.txt new file mode 100644 index 0000000000..5f9ec4bfe7 --- /dev/null +++ b/src/drivers/distance_sensor/ll40ls_pwm/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ +px4_add_module( + MODULE drivers__ll40ls_pwm + MAIN ll40ls_pwm + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + ll40ls.cpp + LidarLitePWM.cpp + DEPENDS + drivers_rangefinder + #arch_io_pins # LidarLitePWM + ) diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp similarity index 84% rename from src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp rename to src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp index 756d99a2c7..82b3b52687 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp @@ -49,15 +49,22 @@ #include LidarLitePWM::LidarLitePWM(const uint8_t rotation) : - LidarLite(rotation), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation) { + _px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE); + _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE); + _px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian px4_arch_configgpio(io_timer_channel_get_gpio_output(GPIO_VDD_RANGEFINDER_EN_CHAN)); } LidarLitePWM::~LidarLitePWM() { stop(); + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_sensor_resets); + perf_free(_sensor_zero_resets); } int @@ -106,7 +113,6 @@ LidarLitePWM::measure() if (current_distance <= 0.0f) { perf_count(_sensor_zero_resets); perf_end(_sample_perf); - return reset_sensor(); } _px4_rangefinder.update(timestamp_sample, current_distance); @@ -130,4 +136,14 @@ LidarLitePWM::collect() return EAGAIN; } +void +LidarLitePWM::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_sensor_resets); + perf_print_counter(_sensor_zero_resets); + printf("poll interval: %u \n", get_measure_interval()); +} + #endif diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.h b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.h similarity index 74% rename from src/drivers/distance_sensor/ll40ls/LidarLite.h rename to src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.h index 266c762ef8..1da621f8e9 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.h +++ b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.h @@ -33,13 +33,21 @@ /** - * @file LidarLite.h + * @file LidarLitePWM.h * @author Johan Jansen + * @author Ban Siesta * - * Generic interface driver for the PulsedLight Lidar-Lite range finders. + * Driver for the PulsedLight Lidar-Lite range finders connected via PWM. + * + * This driver accesses the pwm_input published by the pwm_input driver. */ #pragma once +#include + +#include +#include +#include #include #include #include @@ -57,35 +65,36 @@ static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{50_ms}; // Maximum time to wait for a conversion to complete. static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms}; -class LidarLite +#if DIRECT_PWM_OUTPUT_CHANNELS >= 6 +#define GPIO_VDD_RANGEFINDER_EN_CHAN 5 // use pin 6 +#define LIDAR_LITE_PWM_SUPPORTED + +class LidarLitePWM : public px4::ScheduledWorkItem { public: - LidarLite(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - virtual ~LidarLite(); + LidarLitePWM(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + virtual ~LidarLitePWM(); - virtual int init() = 0; - virtual void start() = 0; - virtual void stop() = 0; + int init(); + void start(); + void stop(); - /** - * @brief Diagnostics - print some basic information about the driver. - */ void print_info(); - /** - * @brief print registers to console. - */ - virtual void print_registers() {}; - protected: - uint32_t get_measure_interval() const { return _measure_interval; }; + int collect(); + int measure(); - virtual int collect() = 0; + void Run() override; - virtual int measure() = 0; +private: + uint32_t get_measure_interval() const { return LL40LS_CONVERSION_INTERVAL; }; - virtual int reset_sensor() { return PX4_ERROR; }; + + uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)}; + + pwm_input_s _pwm{}; PX4Rangefinder _px4_rangefinder; @@ -93,8 +102,6 @@ protected: perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "ll40ls: read")}; perf_counter_t _sensor_resets{perf_alloc(PC_COUNT, "ll40ls: resets")}; perf_counter_t _sensor_zero_resets{perf_alloc(PC_COUNT, "ll40ls: zero resets")}; - -private: - - uint32_t _measure_interval{LL40LS_CONVERSION_INTERVAL}; }; + +#endif diff --git a/src/drivers/distance_sensor/ll40ls_pwm/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls_pwm/ll40ls.cpp new file mode 100644 index 0000000000..cd793cab52 --- /dev/null +++ b/src/drivers/distance_sensor/ll40ls_pwm/ll40ls.cpp @@ -0,0 +1,218 @@ +/**************************************************************************** + * + * 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 ll40ls.cpp + * @author Allyson Kreft + * @author Johan Jansen + * @author Ban Siesta + * @author James Goppert + * + * Interface for the PulsedLight Lidar-Lite range finders. + */ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "LidarLitePWM.h" + +#ifdef LIDAR_LITE_PWM_SUPPORTED + +/** + * @brief Local functions in support of the shell command. + */ +namespace ll40ls_pwm +{ + +LidarLitePWM *instance = nullptr; + +int start_pwm(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); +int status(); +int stop(); +int usage(); + +/** + * Start the pwm driver. + * + * This function only returns if the sensor is up and running + * or could not be detected successfully. + */ +int +start_pwm(const uint8_t rotation) +{ + if (instance != nullptr) { + PX4_ERR("already started"); + return PX4_OK; + } + + instance = new LidarLitePWM(rotation); + + if (instance == nullptr) { + PX4_ERR("Failed to instantiate the driver"); + return PX4_ERROR; + } + + // Initialize the sensor. + if (instance->init() != PX4_OK) { + PX4_ERR("Failed to initialize LidarLite pwm."); + delete instance; + instance = nullptr; + return PX4_ERROR; + } + + // Start the driver. + instance->start(); + + PX4_INFO("driver started"); + return PX4_OK; +} + +/** + * @brief Prints status info about the driver. + */ +int +status() +{ + if (instance == nullptr) { + PX4_ERR("driver not running"); + return PX4_ERROR; + } + + instance->print_info(); + return PX4_OK; +} + +/** + * @brief Stops the driver + */ +int +stop() +{ + if (instance != nullptr) { + delete instance; + instance = nullptr; + } + + return PX4_OK; +} + +/** + * @brief Displays driver usage at the console. + */ +int +usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +PWM driver for LidarLite rangefinders. + +The sensor/driver must be enabled using the parameter SENS_EN_LL40LS. + +Setup/usage information: https://docs.px4.io/en/sensor/lidar_lite.html +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ll40ls", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); + PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status information"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); + return PX4_OK; +} + +} // namespace ll40ls + + +/** + * @brief Driver 'main' command. + */ +extern "C" __EXPORT int ll40ls_pwm_main(int argc, char *argv[]) +{ + const char *myoptarg = nullptr; + + int ch = 0; + int myoptind = 1; + + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (uint8_t)atoi(myoptarg); + PX4_INFO("Setting Lidar orientation to %d", (int)rotation); + break; + + default: + return ll40ls_pwm::usage(); + } + } + + if (myoptind >= argc) { + return ll40ls_pwm::usage(); + } + + // Start the driver. + if (!strcmp(argv[myoptind], "start")) { + return ll40ls_pwm::start_pwm(rotation); + } + + // Print the driver status. + if (!strcmp(argv[myoptind], "status")) { + return ll40ls_pwm::status(); + } + + // Stop the driver + if (!strcmp(argv[myoptind], "stop")) { + return ll40ls_pwm::stop(); + } + + // Print driver usage information. + return ll40ls_pwm::usage(); +} + +#else +extern "C" __EXPORT int ll40ls_pwm_main(int argc, char *argv[]) +{ + PX4_ERR("PWM input not supported."); + return -1; +} +#endif