diff --git a/boards/emlid/navio2/navio_rgbled/CMakeLists.txt b/boards/emlid/navio2/navio_rgbled/CMakeLists.txt index a834d76c35..c97bceafee 100644 --- a/boards/emlid/navio2/navio_rgbled/CMakeLists.txt +++ b/boards/emlid/navio2/navio_rgbled/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# Copyright (c) 2016-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 @@ -32,13 +32,12 @@ ############################################################################ px4_add_module( - MODULE drivers__navio_rgbled + MODULE navio_rgbled MAIN navio_rgbled SRCS - navio_rgbled.cpp + NavioRGBLed.cpp + NavioRGBLed.hpp DEPENDS led - linux_gpio + px4_work_queue ) - -#add_subdirectory(test) diff --git a/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp new file mode 100644 index 0000000000..85dff24a25 --- /dev/null +++ b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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. + * + ****************************************************************************/ + +#include "NavioRGBLed.hpp" + +NavioRGBLed::NavioRGBLed() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +}; + +NavioRGBLed::~NavioRGBLed() +{ + ScheduleClear(); + + _ledR.off(); + _ledG.off(); + _ledB.off(); +} + +int NavioRGBLed::init() +{ + _ledR.off(); + _ledG.off(); + _ledB.off(); + + // kick off work queue + ScheduleNow(); + + return PX4_OK; +} + +void NavioRGBLed::Run() +{ + LedControlData led_control_data{}; + + if (_led_controller.update(led_control_data) == 1) { + switch (led_control_data.leds[0].color) { + case led_control_s::COLOR_RED: + _ledR.on(); + _ledG.off(); + _ledB.off(); + break; + + case led_control_s::COLOR_GREEN: + _ledR.off(); + _ledG.on(); + _ledB.off(); + break; + + case led_control_s::COLOR_BLUE: + _ledR.off(); + _ledG.off(); + _ledB.on(); + break; + + case led_control_s::COLOR_AMBER: // make it the same as yellow + case led_control_s::COLOR_YELLOW: + _ledR.on(); + _ledG.on(); + _ledB.off(); + break; + + case led_control_s::COLOR_PURPLE: + _ledR.on(); + _ledG.off(); + _ledB.on(); + break; + + case led_control_s::COLOR_CYAN: + _ledR.off(); + _ledG.on(); + _ledB.on(); + break; + + case led_control_s::COLOR_WHITE: + _ledR.on(); + _ledG.on(); + _ledB.on(); + break; + + default: // led_control_s::COLOR_OFF + _ledR.off(); + _ledG.off(); + _ledB.off(); + break; + } + } + + /* re-queue ourselves to run again later */ + ScheduleDelayed(_led_controller.maximum_update_interval()); +} + +int NavioRGBLed::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int NavioRGBLed::task_spawn(int argc, char *argv[]) +{ + NavioRGBLed *instance = new NavioRGBLed(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int NavioRGBLed::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Emlid Navio2 RGB LED driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("navio_rgbled", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int navio_rgbled_main(int argc, char *argv[]) +{ + return NavioRGBLed::main(argc, argv); +} diff --git a/boards/emlid/navio2/navio_rgbled/navio_rgbled.h b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.hpp similarity index 61% rename from boards/emlid/navio2/navio_rgbled/navio_rgbled.h rename to boards/emlid/navio2/navio_rgbled/NavioRGBLed.hpp index 55fc3f8fa8..01bcc26580 100644 --- a/boards/emlid/navio2/navio_rgbled/navio_rgbled.h +++ b/boards/emlid/navio2/navio_rgbled/NavioRGBLed.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -30,28 +30,54 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + #pragma once -#include -#include +#include +#include +#include +#include #include -class RGBLED : public DriverFramework::DevObj +class NavioRGBLed : public ModuleBase, public px4::ScheduledWorkItem { public: - RGBLED(const char *name); - virtual ~RGBLED() = default; + NavioRGBLed(); + ~NavioRGBLed() override; - int start(); - int stop(); + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); -protected: - void _measure(); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + int init(); private: + + void Run() override; + LedController _led_controller; - LinuxGPIO _gpioR; - LinuxGPIO _gpioG; - LinuxGPIO _gpioB; + + + class SysRGBLED + { + public: + explicit SysRGBLED(const char *path) : _fd(open(path, O_WRONLY)) {} + ~SysRGBLED() { close(_fd); } + + bool on() { return (write(_fd, "0", 1) > 0); } + bool off() { return (write(_fd, "1", 1) > 0); } + + private: + int _fd{-1}; + }; + + SysRGBLED _ledR{"/sys/class/leds/rgb_led0/brightness"}; + SysRGBLED _ledG{"/sys/class/leds/rgb_led1/brightness"}; + SysRGBLED _ledB{"/sys/class/leds/rgb_led2/brightness"}; }; diff --git a/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp b/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp deleted file mode 100644 index 1faeaca0d6..0000000000 --- a/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 - 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include - -#include - -#include "navio_rgbled.h" - -#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" - -using namespace DriverFramework; - - -RGBLED::RGBLED(const char *name) - : DevObj(name, - RGBLED0_DEVICE_PATH, - RGBLED_BASE_DEVICE_PATH, - DeviceBusType_UNKNOWN, - 0) - , _gpioR(4) - , _gpioG(27) - , _gpioB(6) -{ -}; - -int RGBLED::start() -{ - int res = DevObj::init(); - - if (res != 0) { - DF_LOG_ERR("could not init DevObj (%i)", res); - return res; - } - - res = _gpioR.exportPin(); - - if (res != 0) { - PX4_ERR("red led: failed to export"); - goto cleanup; - } - - res = _gpioR.setDirection(LinuxGPIO::Direction::OUT); - - if (res != 0) { - PX4_ERR("red led: failed to set direction"); - goto cleanup; - } - - res = _gpioG.exportPin(); - - if (res != 0) { - PX4_ERR("green led: failed to export"); - goto cleanup; - } - - res = _gpioG.setDirection(LinuxGPIO::Direction::OUT); - - if (res != 0) { - PX4_ERR("green led: failed to set direction"); - goto cleanup; - } - - res = _gpioB.exportPin(); - - if (res != 0) { - PX4_ERR("blue led: failed to export"); - goto cleanup; - } - - res = _gpioB.setDirection(LinuxGPIO::Direction::OUT); - - if (res != 0) { - PX4_ERR("blue led: failed to set direction"); - goto cleanup; - } - - // update at fixed interval - DevObj::setSampleInterval(_led_controller.maximum_update_interval()); - - res = DevObj::start(); - - if (res != 0) { - DF_LOG_ERR("could not start DevObj (%i)", res); - return res; - } - - return res; - -cleanup: - _gpioR.unexportPin(); - _gpioG.unexportPin(); - _gpioB.unexportPin(); - - return res; -} - -int -RGBLED::stop() -{ - _gpioR.unexportPin(); - _gpioG.unexportPin(); - _gpioB.unexportPin(); - - int res = DevObj::stop(); - - if (res < 0) { - DF_LOG_ERR("could not stop DevObj"); - //this may not be an error for this device - return res; - } - - return 0; -} - -void -RGBLED::_measure() -{ - LedControlData led_control_data; - - if (_led_controller.update(led_control_data) == 1) { - switch (led_control_data.leds[0].color) { - case led_control_s::COLOR_RED: - _gpioR.writeValue(LinuxGPIO::Value::LOW); - _gpioG.writeValue(LinuxGPIO::Value::HIGH); - _gpioB.writeValue(LinuxGPIO::Value::HIGH); - break; - - case led_control_s::COLOR_GREEN: - _gpioR.writeValue(LinuxGPIO::Value::HIGH); - _gpioG.writeValue(LinuxGPIO::Value::LOW); - _gpioB.writeValue(LinuxGPIO::Value::HIGH); - break; - - case led_control_s::COLOR_BLUE: - _gpioR.writeValue(LinuxGPIO::Value::HIGH); - _gpioG.writeValue(LinuxGPIO::Value::HIGH); - _gpioB.writeValue(LinuxGPIO::Value::LOW); - break; - - case led_control_s::COLOR_AMBER: //make it the same as yellow - case led_control_s::COLOR_YELLOW: - _gpioR.writeValue(LinuxGPIO::Value::LOW); - _gpioG.writeValue(LinuxGPIO::Value::LOW); - _gpioB.writeValue(LinuxGPIO::Value::HIGH); - break; - - case led_control_s::COLOR_PURPLE: - _gpioR.writeValue(LinuxGPIO::Value::LOW); - _gpioG.writeValue(LinuxGPIO::Value::HIGH); - _gpioB.writeValue(LinuxGPIO::Value::LOW); - break; - - case led_control_s::COLOR_CYAN: - _gpioR.writeValue(LinuxGPIO::Value::HIGH); - _gpioG.writeValue(LinuxGPIO::Value::LOW); - _gpioB.writeValue(LinuxGPIO::Value::LOW); - break; - - case led_control_s::COLOR_WHITE: - _gpioR.writeValue(LinuxGPIO::Value::LOW); - _gpioG.writeValue(LinuxGPIO::Value::LOW); - _gpioB.writeValue(LinuxGPIO::Value::LOW); - break; - - default: // led_control_s::COLOR_OFF - _gpioR.writeValue(LinuxGPIO::Value::HIGH); - _gpioG.writeValue(LinuxGPIO::Value::HIGH); - _gpioB.writeValue(LinuxGPIO::Value::HIGH); - break; - } - } -} - -extern "C" { __EXPORT int navio_rgbled_main(int argc, char *argv[]); } - -namespace navio_rgbled -{ -int start(); -int stop(); -void usage(); - -RGBLED *g_dev = nullptr; - -int start() -{ - g_dev = new RGBLED("navio_rgbled"); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating RGBLED"); - return -1; - } - - return g_dev->start(); -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("not running"); - return -1; - } - - g_dev->stop(); - - delete g_dev; - g_dev = nullptr; - - return 0; -} - -void usage() -{ - PX4_INFO("Usage: navio_rgbled 'start', 'stop'"); -} - -} //namespace navio_rgbled - -int navio_rgbled_main(int argc, char *argv[]) -{ - int ret = 0; - int myoptind = 1; - - if (argc <= 1) { - navio_rgbled::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - - if (!strcmp(verb, "start")) { - ret = navio_rgbled::start(); - } - - else if (!strcmp(verb, "stop")) { - ret = navio_rgbled::stop(); - } - - else { - navio_rgbled::usage(); - return 1; - } - - return ret; -} - diff --git a/boards/emlid/navio2/navio_rgbled/test/CMakeLists.txt b/boards/emlid/navio2/navio_rgbled/test/CMakeLists.txt deleted file mode 100644 index 6ec6b63116..0000000000 --- a/boards/emlid/navio2/navio_rgbled/test/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 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_directories(..) - -add_executable(navio_rgbled_test - main.cpp - test.cpp - ) - -target_link_libraries(navio_rgbled_test - drivers__navio_rgbled - drivers__navio_gpio - df_driver_framework - ) diff --git a/boards/emlid/navio2/navio_rgbled/test/main.cpp b/boards/emlid/navio2/navio_rgbled/test/main.cpp deleted file mode 100644 index 645404f2cf..0000000000 --- a/boards/emlid/navio2/navio_rgbled/test/main.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - -extern int do_test(); - -int main() -{ - return do_test(); -} diff --git a/boards/emlid/navio2/navio_rgbled/test/test.cpp b/boards/emlid/navio2/navio_rgbled/test/test.cpp deleted file mode 100644 index 39c93c8244..0000000000 --- a/boards/emlid/navio2/navio_rgbled/test/test.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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 - -#include -#include - -#include - -#include - -#include "navio_rgbled.h" - -using namespace DriverFramework; - -int do_test(); - -int do_test() -{ - DevHandle h; - RGBLED *g_dev = nullptr; - - if (Framework::initialize() < 0) { - printf("Framework init failed\n"); - return -1; - } - - g_dev = new RGBLED("navio_rgbled test"); - g_dev->start(); - - DevMgr::getHandle(RGBLED0_DEVICE_PATH, h); - - if (!h.isValid()) { - printf("No RGB LED at " RGBLED0_DEVICE_PATH); - return -1; - } - - printf("off\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_OFF); - sleep(2); - - printf("red\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_RED); - sleep(2); - - printf("yellow\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_YELLOW); - sleep(2); - - printf("purple\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_PURPLE); - sleep(2); - - printf("green\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_GREEN); - sleep(2); - - printf("blue\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_BLUE); - sleep(2); - - printf("blue blink slow\n"); - h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_SLOW); - sleep(10); - - printf("green blink normal\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_GREEN); - h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_NORMAL); - sleep(10); - - printf("red blink fast\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_RED); - h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_FAST); - sleep(10); - - printf("blue breathe (bogus)\n"); - h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_BLUE); - h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BREATHE); - sleep(10); - - return 0; -} diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index 3995d705bf..b74962c5b9 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -37,7 +37,6 @@ add_subdirectory(barometer) add_subdirectory(device) add_subdirectory(gyroscope) add_subdirectory(led) -add_subdirectory(linux_gpio) add_subdirectory(magnetometer) add_subdirectory(rangefinder) add_subdirectory(smbus) diff --git a/src/lib/drivers/linux_gpio/CMakeLists.txt b/src/lib/drivers/linux_gpio/CMakeLists.txt deleted file mode 100644 index 8f8a966718..0000000000 --- a/src/lib/drivers/linux_gpio/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 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_library(linux_gpio - linux_gpio.cpp - ) - -#add_subdirectory(test) diff --git a/src/lib/drivers/linux_gpio/linux_gpio.cpp b/src/lib/drivers/linux_gpio/linux_gpio.cpp deleted file mode 100644 index c6d4080bd5..0000000000 --- a/src/lib/drivers/linux_gpio/linux_gpio.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 - 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "linux_gpio.h" - -#include -#include -#include -#include -#include -#include - -#include - -#define PIN_INDEX_BUFFER_MAX (16) -#define PIN_DIRECTION_BUFFER_MAX (30 + PIN_INDEX_BUFFER_MAX) -#define PIN_VALUE_BUFFER_MAX (26 + PIN_INDEX_BUFFER_MAX) - -LinuxGPIO::LinuxGPIO(unsigned int pin) - : _pin(pin) - , _fd(-1) -{ -} - -LinuxGPIO::~LinuxGPIO() -{ - if (_fd != -1) { - close(_fd); - } -} - -int LinuxGPIO::exportPin() -{ - int ret; - char pinIndex[PIN_INDEX_BUFFER_MAX]; - char valuePath[PIN_VALUE_BUFFER_MAX]; - int fd = -1; - int bytes_to_write; - - struct stat statbuf; - - /* If GPIO was already opened, close it */ - if (_fd != -1) { - close(_fd); - _fd = -1; - } - - /* Check if GPIO is already exported */ - snprintf(valuePath, PIN_VALUE_BUFFER_MAX, "/sys/class/gpio/gpio%d/value", _pin); - ret = stat(valuePath, &statbuf); - - if (ret == -1) { - /* GPIO is not exported */ - fd = open("/sys/class/gpio/export", O_WRONLY); - - if (fd == -1) { - int err = errno; - PX4_ERR("export %u: open: %s (%d)", _pin, strerror(err), err); - return -1; - } - - bytes_to_write = snprintf(pinIndex, PIN_INDEX_BUFFER_MAX, "%u", _pin); - ret = write(fd, pinIndex, bytes_to_write); - - if (ret == -1) { - int err = errno; - PX4_ERR("export %u: write: %s (%d)", _pin, strerror(err), err); - goto cleanup; - - } else if (ret != bytes_to_write) { - PX4_ERR("export %u: write incomplete %d != %d", _pin, ret, bytes_to_write); - goto cleanup; - } - } - - _fd = open(valuePath, O_RDWR); - - if (_fd == -1) { - int err = errno; - ret = -1; - PX4_ERR("export %u: open: %s (%d)", _pin, strerror(err), err); - goto cleanup; - } - - ret = 0; - -cleanup: - - if (fd != -1) { - close(fd); - } - - return ret; -} - -int LinuxGPIO::unexportPin() -{ - char pinIndex[PIN_INDEX_BUFFER_MAX]; - int fd; - int ret; - int bytes_to_write; - - if (_fd != -1) { - close(_fd); - _fd = -1; - } - - fd = open("/sys/class/gpio/unexport", O_WRONLY); - - if (fd == -1) { - int err = errno; - PX4_ERR("unexport %u: open: %s (%d)", _pin, strerror(err), err); - return -1; - } - - bytes_to_write = snprintf(pinIndex, PIN_INDEX_BUFFER_MAX, "%u", _pin); - ret = write(fd, pinIndex, bytes_to_write); - - if (ret == -1) { - int err = errno; - PX4_ERR("unexport %u: write: %s (%d)", _pin, strerror(err), err); - goto cleanup; - - } else if (ret != bytes_to_write) { - PX4_ERR("unexport %u: write incomplete %d != %d", _pin, ret, bytes_to_write); - goto cleanup; - } - - ret = 0; - -cleanup: - close(fd); - - return ret; -} - -int LinuxGPIO::setDirection(LinuxGPIO::Direction dir) -{ - char path[PIN_DIRECTION_BUFFER_MAX]; - int fd; - int ret; - - snprintf(path, PIN_DIRECTION_BUFFER_MAX, "/sys/class/gpio/gpio%d/direction", _pin); - fd = open(path, O_WRONLY); - - if (fd == -1) { - int err = errno; - PX4_ERR("dir %u: open: %s (%d)", _pin, strerror(err), err); - return -1; - } - - if (dir == Direction::IN) { - ret = write(fd, "in", 2); - - } else { - ret = write(fd, "out", 3); - } - - if (ret == -1) { - int err = errno; - PX4_ERR("dir %u: write: %s (%d)", _pin, strerror(err), err); - goto cleanup; - } - - ret = 0; - -cleanup: - close(fd); - - return ret; -} - -int LinuxGPIO::readValue() -{ - char buf[2]; - int ret; - - ret = ::read(_fd, buf, sizeof(buf)); - - if (ret == -1) { - int err = errno; - PX4_ERR("readValue %u: read: %s (%d)", _pin, strerror(err), err); - return ret; - } - - ret = strtol(buf, nullptr, 10); - - return ret; -} - -int LinuxGPIO::writeValue(LinuxGPIO::Value value) -{ - char buf[2]; - int ret; - - if (value != Value::LOW && value != Value::HIGH) { - return -EINVAL; - } - - int buflen = snprintf(buf, sizeof(buf), "%u", (unsigned int)value); - - ret = ::write(_fd, buf, buflen); - - if (ret == -1) { - int err = errno; - PX4_ERR("writeValue %u: write: %s (%d)", _pin, strerror(err), err); - return ret; - } - - return 0; -} diff --git a/src/lib/drivers/linux_gpio/linux_gpio.h b/src/lib/drivers/linux_gpio/linux_gpio.h deleted file mode 100644 index d3e3df1d5b..0000000000 --- a/src/lib/drivers/linux_gpio/linux_gpio.h +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file linux_gpio.h - * - * Linux sysfs GPIO Interface - * - * This interface allows manipulation of sysfs GPIOs on Linux - * - * @author Nicolae Rosia - */ - -#pragma once - -class LinuxGPIO -{ -public: - LinuxGPIO(unsigned int pin); - ~LinuxGPIO(); - - enum class Direction { - IN = 0, - OUT = 1, - }; - - enum class Value { - LOW = 0, - HIGH = 1, - }; - - int exportPin(); - int unexportPin(); - int setDirection(LinuxGPIO::Direction dir); - int readValue(); - int writeValue(LinuxGPIO::Value value); - -private: - int _pin; - int _fd; -}; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 473141fb1a..1ea4014788 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -63,9 +63,6 @@ #include #include "commander_helper.h" -#include "DevMgr.hpp" - -using namespace DriverFramework; #define VEHICLE_TYPE_FIXED_WING 1 #define VEHICLE_TYPE_QUADROTOR 2 @@ -131,8 +128,8 @@ static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for rep static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end static unsigned int tune_durations[TONE_NUMBER_OF_TUNES] {}; -static DevHandle h_leds; -static DevHandle h_buzzer; +static int fd_leds{-1}; + static led_control_s led_control {}; static orb_advert_t led_control_pub = nullptr; static tune_control_s tune_control {}; @@ -296,25 +293,25 @@ int led_init() led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH); /* first open normal LEDs */ - DevMgr::getHandle(LED0_DEVICE_PATH, h_leds); + fd_leds = px4_open(LED0_DEVICE_PATH, O_RDWR); - if (!h_leds.isValid()) { - PX4_WARN("LED: getHandle fail\n"); + if (fd_leds < 0) { + PX4_ERR("LED: open %s failed", LED0_DEVICE_PATH); return PX4_ERROR; } /* the green LED is only available on FMUv5 */ - (void)h_leds.ioctl(LED_ON, LED_GREEN); + px4_ioctl(fd_leds, LED_ON, LED_GREEN); /* the blue LED is only available on AeroCore but not FMUv2 */ - (void)h_leds.ioctl(LED_ON, LED_BLUE); + px4_ioctl(fd_leds, LED_ON, LED_BLUE); /* switch blue off */ led_off(LED_BLUE); /* we consider the amber led mandatory */ - if (h_leds.ioctl(LED_ON, LED_AMBER)) { - PX4_WARN("Amber LED: ioctl fail\n"); + if (px4_ioctl(fd_leds, LED_ON, LED_AMBER)) { + PX4_WARN("Amber LED: ioctl fail"); return PX4_ERROR; } @@ -327,22 +324,22 @@ int led_init() void led_deinit() { orb_unadvertise(led_control_pub); - DevMgr::releaseHandle(h_leds); + px4_close(fd_leds); } int led_toggle(int led) { - return h_leds.ioctl(LED_TOGGLE, led); + return px4_ioctl(fd_leds, LED_TOGGLE, led); } int led_on(int led) { - return h_leds.ioctl(LED_ON, led); + return px4_ioctl(fd_leds, LED_ON, led); } int led_off(int led) { - return h_leds.ioctl(LED_OFF, led); + return px4_ioctl(fd_leds, LED_OFF, led); } void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint8_t prio)