From 3049b9af015d4ea58315d2d9f3eac14c92d50634 Mon Sep 17 00:00:00 2001 From: Hidenori Date: Mon, 18 Jul 2016 14:57:46 -0400 Subject: [PATCH] Navio2: add support for GPIO and RGBLED --- cmake/configs/posix_rpi_common.cmake | 2 + src/drivers/navio_gpio/CMakeLists.txt | 45 ++++ src/drivers/navio_gpio/navio_gpio.cpp | 203 ++++++++++++++++ src/drivers/navio_gpio/navio_gpio.h | 119 ++++++++++ src/drivers/navio_gpio/test/CMakeLists.txt | 12 + src/drivers/navio_gpio/test/main.cpp | 6 + src/drivers/navio_gpio/test/test.cpp | 58 +++++ src/drivers/navio_rgbled/CMakeLists.txt | 46 ++++ src/drivers/navio_rgbled/navio_rgbled.cpp | 264 +++++++++++++++++++++ src/modules/commander/commander_helper.cpp | 4 + 10 files changed, 759 insertions(+) create mode 100644 src/drivers/navio_gpio/CMakeLists.txt create mode 100644 src/drivers/navio_gpio/navio_gpio.cpp create mode 100644 src/drivers/navio_gpio/navio_gpio.h create mode 100644 src/drivers/navio_gpio/test/CMakeLists.txt create mode 100644 src/drivers/navio_gpio/test/main.cpp create mode 100644 src/drivers/navio_gpio/test/test.cpp create mode 100644 src/drivers/navio_rgbled/CMakeLists.txt create mode 100644 src/drivers/navio_rgbled/navio_rgbled.cpp diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index b7f1dd0aa5..ad28fddc81 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -75,6 +75,8 @@ set(config_module_list drivers/gps drivers/navio_sysfs_rc_in drivers/navio_sysfs_pwm_out + drivers/navio_gpio + drivers/navio_rgbled # # Libraries diff --git a/src/drivers/navio_gpio/CMakeLists.txt b/src/drivers/navio_gpio/CMakeLists.txt new file mode 100644 index 0000000000..79b33fb737 --- /dev/null +++ b/src/drivers/navio_gpio/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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_module( + MODULE drivers__navio_gpio + MAIN navio_gpio + STACK_MAIN 1200 + COMPILE_FLAGS -Os + SRCS + navio_gpio.cpp + DEPENDS + platforms__common + ) + +#add_subdirectory(test) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/navio_gpio/navio_gpio.cpp b/src/drivers/navio_gpio/navio_gpio.cpp new file mode 100644 index 0000000000..5158a39d98 --- /dev/null +++ b/src/drivers/navio_gpio/navio_gpio.cpp @@ -0,0 +1,203 @@ +#include +#include +#include +#include +#include + +#include "navio_gpio.h" + +namespace navio_gpio +{ +extern "C" __EXPORT int navio_gpio_main(int argc, char *argv[]); + +int Gpio::start(void) +{ + int mem_fd; + + if ((mem_fd = open("/dev/gpiomem", O_RDWR | O_SYNC)) < 0) { + PX4_WARN("failed to open gpiomem"); + return -1; + } + + if ((_gpio_map = mmap(NULL, + GPIO_BLOCK_SIZE, + PROT_READ | PROT_WRITE, + MAP_SHARED, + mem_fd, + GPIO_PHYS_ADDR)) == MAP_FAILED) { + PX4_WARN("failed to mmap GPIO region"); + close(mem_fd); + return -1; + } + + close(mem_fd); + + return 0; +} + +int Gpio::stop(void) +{ + if (munmap(_gpio_map, GPIO_BLOCK_SIZE) < 0) { + return -1; + } + + return 0; +} + +void Gpio::atomic_modify(uint32_t addr, + unsigned int shift, + unsigned int mask, + unsigned int value) +{ + uint32_t tmp; + + m_lock.lock(); + + tmp = *(volatile uint32_t *)addr; + tmp = (tmp & ~(mask << shift)) | (value << shift); + *(volatile uint32_t *)addr = tmp; + + m_lock.unlock(); +} + + +int Gpio::configgpio(uint32_t pinset) +{ + unsigned int pin; + unsigned int cnf; + + uint32_t addr; + unsigned int shift; + + pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; + cnf = (pinset & GPIO_CNF_MASK) >> GPIO_CNF_SHIFT; + + addr = (uint32_t)_gpio_map + GPIO_GPFSEL0_OFFSET + pin / 10; + shift = (pin % 10) * 3; + + atomic_modify(addr, shift, GPIO_CNF_MASK, cnf); + + return 0; +} + + +int Gpio::unconfiggpio(uint32_t pinset) +{ + return Gpio::configgpio(pinset); +} + + +bool Gpio::gpioread(uint32_t pinset) +{ + unsigned int pin; + + uint32_t addr; + unsigned int shift; + + pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; + + addr = (uint32_t)_gpio_map + GPIO_GPLEV0_OFFSET + pin / 32; + shift = pin % 32; + + return (*(volatile uint32_t *)addr >> shift) & 0x1u; +} + + +void Gpio::gpiowrite(uint32_t pinset, bool value) +{ + unsigned int pin; + + uint32_t addr; + unsigned int shift; + + pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; + + if (value == 0) { + addr = (uint32_t)_gpio_map + GPIO_GPSET0_OFFSET + pin / 32; + + } else { + addr = (uint32_t)_gpio_map + GPIO_GPCLR0_OFFSET + pin / 32; + } + + shift = pin % 32; + + *(volatile uint32_t *)addr = 0x1u << shift; +} + +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s", reason); + } + + PX4_INFO("usage: navio_gpio {start|stop|status}"); +} +static Gpio *gpio = nullptr; + +int navio_gpio_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage("missing command"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (gpio != nullptr && gpio->isMapped()) { + PX4_WARN("already mapped"); + /* this is not an error */ + return 0; + } + + gpio = new Gpio(); + + if (gpio == nullptr) { + PX4_ERR("alloc failed"); + return -1; + } + + int ret = gpio->start(); + + if (ret != 0) { + PX4_ERR("start failed"); + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + + if (gpio == nullptr || gpio->isMapped()) { + PX4_WARN("not mapped"); + /* this is not an error */ + return 0; + } + + gpio->stop(); + + delete gpio; + gpio = nullptr; + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (gpio != nullptr && gpio->isMapped()) { + PX4_INFO("mapped"); + + } else { + PX4_INFO("not mapped\n"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; + +} + +} // navio_gpio diff --git a/src/drivers/navio_gpio/navio_gpio.h b/src/drivers/navio_gpio/navio_gpio.h new file mode 100644 index 0000000000..76163190eb --- /dev/null +++ b/src/drivers/navio_gpio/navio_gpio.h @@ -0,0 +1,119 @@ +#include "SyncObj.hpp" +#include + +#define GPIO_BLOCK_SIZE (4096) +#define GPIO_PHYS_ADDR (0x3F200000) +#define GPIO_GPFSEL0_OFFSET (0x0) +#define GPIO_GPLEV0_OFFSET (0x34) +#define GPIO_GPSET0_OFFSET (0x1C) +#define GPIO_GPCLR0_OFFSET (0x28) + + +#define GPIO_CNF_SHIFT 6 +#define GPIO_CNF_MASK (3 << GPIO_CNF_SHIFT) +#define GPIO_CNF_INPUT (0 << GPIO_CNF_SHIFT) +#define GPIO_CNF_OUTPUT (1 << GPIO_CNF_SHIFT) +#define GPIO_CNF_AF0 (4 << GPIO_CNF_SHIFT) +#define GPIO_CNF_AF1 (5 << GPIO_CNF_SHIFT) +#define GPIO_CNF_AF2 (7 << GPIO_CNF_SHIFT) +#define GPIO_CNF_AF3 (7 << GPIO_CNF_SHIFT) +#define GPIO_CNF_AF4 (3 << GPIO_CNF_SHIFT) +#define GPIO_CNF_AF5 (2 << GPIO_CNF_SHIFT) + +#define GPIO_PIN_SHIFT 0 +#define GPIO_PIN_MASK (63 << GPIO_PIN_SHIFT) +#define GPIO_PIN0 (0 << GPIO_PIN_SHIFT) +#define GPIO_PIN1 (1 << GPIO_PIN_SHIFT) +#define GPIO_PIN2 (2 << GPIO_PIN_SHIFT) +#define GPIO_PIN3 (3 << GPIO_PIN_SHIFT) +#define GPIO_PIN4 (4 << GPIO_PIN_SHIFT) +#define GPIO_PIN5 (5 << GPIO_PIN_SHIFT) +#define GPIO_PIN6 (6 << GPIO_PIN_SHIFT) +#define GPIO_PIN7 (7 << GPIO_PIN_SHIFT) +#define GPIO_PIN8 (8 << GPIO_PIN_SHIFT) +#define GPIO_PIN9 (9 << GPIO_PIN_SHIFT) +#define GPIO_PIN10 (10 << GPIO_PIN_SHIFT) +#define GPIO_PIN11 (11 << GPIO_PIN_SHIFT) +#define GPIO_PIN12 (12 << GPIO_PIN_SHIFT) +#define GPIO_PIN13 (13 << GPIO_PIN_SHIFT) +#define GPIO_PIN14 (14 << GPIO_PIN_SHIFT) +#define GPIO_PIN15 (15 << GPIO_PIN_SHIFT) +#define GPIO_PIN16 (16 << GPIO_PIN_SHIFT) +#define GPIO_PIN17 (17 << GPIO_PIN_SHIFT) +#define GPIO_PIN18 (18 << GPIO_PIN_SHIFT) +#define GPIO_PIN19 (19 << GPIO_PIN_SHIFT) +#define GPIO_PIN20 (20 << GPIO_PIN_SHIFT) +#define GPIO_PIN21 (21 << GPIO_PIN_SHIFT) +#define GPIO_PIN22 (22 << GPIO_PIN_SHIFT) +#define GPIO_PIN23 (23 << GPIO_PIN_SHIFT) +#define GPIO_PIN24 (24 << GPIO_PIN_SHIFT) +#define GPIO_PIN25 (25 << GPIO_PIN_SHIFT) +#define GPIO_PIN26 (26 << GPIO_PIN_SHIFT) +#define GPIO_PIN27 (27 << GPIO_PIN_SHIFT) +#define GPIO_PIN28 (28 << GPIO_PIN_SHIFT) +#define GPIO_PIN29 (29 << GPIO_PIN_SHIFT) +#define GPIO_PIN30 (30 << GPIO_PIN_SHIFT) +#define GPIO_PIN31 (31 << GPIO_PIN_SHIFT) +#define GPIO_PIN32 (32 << GPIO_PIN_SHIFT) +#define GPIO_PIN33 (33 << GPIO_PIN_SHIFT) +#define GPIO_PIN34 (34 << GPIO_PIN_SHIFT) +#define GPIO_PIN35 (35 << GPIO_PIN_SHIFT) +#define GPIO_PIN36 (36 << GPIO_PIN_SHIFT) +#define GPIO_PIN37 (37 << GPIO_PIN_SHIFT) +#define GPIO_PIN38 (38 << GPIO_PIN_SHIFT) +#define GPIO_PIN39 (39 << GPIO_PIN_SHIFT) +#define GPIO_PIN40 (40 << GPIO_PIN_SHIFT) +#define GPIO_PIN41 (41 << GPIO_PIN_SHIFT) +#define GPIO_PIN42 (42 << GPIO_PIN_SHIFT) +#define GPIO_PIN43 (43 << GPIO_PIN_SHIFT) +#define GPIO_PIN44 (44 << GPIO_PIN_SHIFT) +#define GPIO_PIN45 (45 << GPIO_PIN_SHIFT) +#define GPIO_PIN46 (46 << GPIO_PIN_SHIFT) +#define GPIO_PIN47 (47 << GPIO_PIN_SHIFT) +#define GPIO_PIN48 (48 << GPIO_PIN_SHIFT) +#define GPIO_PIN49 (49 << GPIO_PIN_SHIFT) +#define GPIO_PIN50 (50 << GPIO_PIN_SHIFT) +#define GPIO_PIN51 (51 << GPIO_PIN_SHIFT) +#define GPIO_PIN52 (52 << GPIO_PIN_SHIFT) +#define GPIO_PIN53 (53 << GPIO_PIN_SHIFT) +#define GPIO_PIN54 (54 << GPIO_PIN_SHIFT) + +using namespace DriverFramework; + +namespace navio_gpio +{ + +class Gpio +{ +public: + Gpio() : + _isMapped(false) + { + } + ~Gpio() + { + } + + int start(); + int stop(); + + int configgpio(uint32_t pinset); + int unconfiggpio(uint32_t pinset); + bool gpioread(uint32_t pinset); + void gpiowrite(uint32_t pinset, bool value); + + bool isMapped() { return _isMapped; } + +private: + void atomic_modify(uint32_t addr, + unsigned int shift, + unsigned int mask, + unsigned int value); + + void *_gpio_map; + bool _isMapped; + + SyncObj m_lock; +}; + +} // namespace navio_gpio diff --git a/src/drivers/navio_gpio/test/CMakeLists.txt b/src/drivers/navio_gpio/test/CMakeLists.txt new file mode 100644 index 0000000000..7922f79da7 --- /dev/null +++ b/src/drivers/navio_gpio/test/CMakeLists.txt @@ -0,0 +1,12 @@ +include_directories(..) + +add_executable(navio_gpio_test + main.cpp + test.cpp + ) + +target_link_libraries(navio_gpio_test + drivers__navio_gpio + platforms__posix__px4_layer + df_driver_framework + ) diff --git a/src/drivers/navio_gpio/test/main.cpp b/src/drivers/navio_gpio/test/main.cpp new file mode 100644 index 0000000000..394e637db4 --- /dev/null +++ b/src/drivers/navio_gpio/test/main.cpp @@ -0,0 +1,6 @@ +extern int do_test(); + +int main() +{ + return do_test(); +} diff --git a/src/drivers/navio_gpio/test/test.cpp b/src/drivers/navio_gpio/test/test.cpp new file mode 100644 index 0000000000..1e57b693a5 --- /dev/null +++ b/src/drivers/navio_gpio/test/test.cpp @@ -0,0 +1,58 @@ +#include "navio_gpio.h" + +#include +#include + +#define LED_CNF (GPIO_CNF_OUTPUT) +#define LED_pinR GPIO_PIN4 +#define LED_pinG GPIO_PIN27 +#define LED_pinB GPIO_PIN6 + +using namespace navio_gpio; + +int do_test(); + +int do_test() +{ + Gpio gpio; + + if (gpio.start() < 0) { + return -1; + } + + gpio.configgpio(LED_CNF | LED_pinR); + gpio.configgpio(LED_CNF | LED_pinG); + gpio.configgpio(LED_CNF | LED_pinB); + + gpio.gpiowrite(LED_pinR, 0); + gpio.gpiowrite(LED_pinG, 0); + gpio.gpiowrite(LED_pinB, 0); + printf("off\n"); + sleep(2); + + gpio.gpiowrite(LED_pinR, 1); + gpio.gpiowrite(LED_pinG, 0); + gpio.gpiowrite(LED_pinB, 0); + printf("red\n"); + sleep(2); + + gpio.gpiowrite(LED_pinR, 0); + gpio.gpiowrite(LED_pinG, 1); + gpio.gpiowrite(LED_pinB, 0); + printf("green\n"); + sleep(2); + + gpio.gpiowrite(LED_pinR, 0); + gpio.gpiowrite(LED_pinG, 0); + gpio.gpiowrite(LED_pinB, 1); + printf("blue\n"); + sleep(2); + + gpio.gpiowrite(LED_pinR, 0); + gpio.gpiowrite(LED_pinG, 0); + gpio.gpiowrite(LED_pinB, 0); + printf("off\n"); + gpio.stop(); + + return 0; +} diff --git a/src/drivers/navio_rgbled/CMakeLists.txt b/src/drivers/navio_rgbled/CMakeLists.txt new file mode 100644 index 0000000000..fb411106b1 --- /dev/null +++ b/src/drivers/navio_rgbled/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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(../navio_gpio) + +px4_add_module( + MODULE drivers__navio_rgbled + MAIN navio_rgbled + COMPILE_FLAGS -Os + SRCS + navio_rgbled.cpp + DEPENDS + platforms__common + drivers__navio_gpio + ) + +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/navio_rgbled/navio_rgbled.cpp b/src/drivers/navio_rgbled/navio_rgbled.cpp new file mode 100644 index 0000000000..88a190bf30 --- /dev/null +++ b/src/drivers/navio_rgbled/navio_rgbled.cpp @@ -0,0 +1,264 @@ +#include "navio_gpio.h" +#include "DevObj.hpp" + +#include +#include + +#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" +#define RGBLED_DEVICE_PATH "/dev/rgbled0" + +#define GPIO_LED_CNF (GPIO_CNF_OUTPUT) +#define GPIO_LED_R (GPIO_PIN4) +#define GPIO_LED_G (GPIO_PIN27) +#define GPIO_LED_B (GPIO_PIN6) + +using namespace DriverFramework; +using namespace navio_gpio; + +class RGBLED : public DevObj +{ +public: + RGBLED(const char *name) : + DevObj(name, + RGBLED_DEVICE_PATH, + RGBLED_BASE_DEVICE_PATH, + DeviceBusType_UNKNOWN, + 0), + _rgbsets{ + {0, 0, 0}, /* OFF */ + {1, 0, 0}, /* red */ + {1, 1, 0}, /* yellow */ + {1, 0, 1}, /* purple */ + {0, 1, 0}, /* green */ + {0, 0, 1}, /* blue */ + {1, 1, 1}, /* white */ + }, + _max_color(7), + _rgb{0, 0, 0}, + _turn(true) + { }; + virtual ~RGBLED() + { }; + + int start(); + int stop(); + int devIOCTL(unsigned long request, unsigned long arg); + +protected: + void _measure(); + +private: + Gpio _gpio; + const rgbled_rgbset_t _rgbsets[7]; + const int _max_color; + rgbled_rgbset_t _rgb; + bool _turn; +}; + +int RGBLED::start() +{ + int res; + + res = DevObj::init(); + + if (res != 0) { + DF_LOG_ERR("error: could not init DevObj"); + return res; + } + + _gpio.start(); + + _gpio.configgpio(GPIO_LED_CNF | GPIO_LED_R); + _gpio.configgpio(GPIO_LED_CNF | GPIO_LED_G); + _gpio.configgpio(GPIO_LED_CNF | GPIO_LED_B); + + return 0; +} + +int RGBLED::stop() +{ + int res; + + _gpio.stop(); + + res = DevObj::stop(); + + if (res < 0) { + DF_LOG_ERR("error: could not stop DevObj"); + //this may not be an error for this device + return res; + } + + return 0; +} + +int RGBLED::devIOCTL(unsigned long request, unsigned long arg) +{ + int ret = ENOTTY; + rgbled_rgbset_t *rgb; + + switch (request) { + case RGBLED_SET_RGB: + ret = 0; + rgb = (rgbled_rgbset_t *)arg; + _rgb.red = (rgb->red != 0) ? 1 : 0; + _rgb.green = (rgb->green != 0) ? 1 : 0; + _rgb.blue = (rgb->blue != 0) ? 1 : 0; + _gpio.gpiowrite(GPIO_LED_R, _rgb.red); + _gpio.gpiowrite(GPIO_LED_G, _rgb.green); + _gpio.gpiowrite(GPIO_LED_B, _rgb.blue); + break; + + case RGBLED_SET_COLOR: + if (arg > _max_color) { + ret = ENOTSUP; + + } else { + _rgb = _rgbsets[arg]; + _gpio.gpiowrite(GPIO_LED_R, _rgb.red); + _gpio.gpiowrite(GPIO_LED_G, _rgb.green); + _gpio.gpiowrite(GPIO_LED_B, _rgb.blue); + ret = 0; + } + + break; + + case RGBLED_SET_MODE: + ret = 0; + + switch (arg) { + case RGBLED_MODE_ON: + DevObj::setSampleInterval(0); + break; + + case RGBLED_MODE_BLINK_SLOW: + DevObj::setSampleInterval(20000 * 1000); + break; + + case RGBLED_MODE_BLINK_NORMAL: + DevObj::setSampleInterval(5000 * 1000); + break; + + case RGBLED_MODE_BLINK_FAST: + DevObj::setSampleInterval(1000 * 1000); + break; + + case RGBLED_MODE_BREATHE: + DevObj::setSampleInterval(15000 * 1000); + break; + + default: + ret = ENOTSUP; + } + + if (!m_work_handle.isValid()) { + // this can fail + DevObj::start(); + } + + break; + + case RGBLED_PLAY_SCRIPT_NAMED: + case RGBLED_PLAY_SCRIPT: + case RGBLED_SET_USER_SCRIPT: + case RGBLED_SET_PATTERN: + ret = ENOTSUP; + break; + + default: + ret = DevObj::devIOCTL(request, arg); + break; + } + + return ret; +} + +void RGBLED::_measure() +{ + if (_turn) { + _gpio.gpiowrite(GPIO_LED_R, 0); + _gpio.gpiowrite(GPIO_LED_G, 0); + _gpio.gpiowrite(GPIO_LED_B, 0); + _turn = false; + + } else { + _gpio.gpiowrite(GPIO_LED_R, _rgb.red); + _gpio.gpiowrite(GPIO_LED_G, _rgb.green); + _gpio.gpiowrite(GPIO_LED_B, _rgb.blue); + _turn = true; + } +} + +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_WARN("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/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 8bd5cc3324..aaa8bf7e8a 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -267,6 +267,7 @@ int led_init() { blink_msg_end = 0; +#ifndef CONFIG_ARCH_BOARD_NAVIO2 /* first open normal LEDs */ DevMgr::getHandle(LED0_DEVICE_PATH, h_leds); @@ -289,6 +290,7 @@ int led_init() /* switch amber off */ led_off(LED_AMBER); +#endif /* then try RGB LEDs, this can fail on FMUv1*/ DevHandle h; @@ -303,7 +305,9 @@ int led_init() void led_deinit() { +#ifndef CONFIG_ARCH_BOARD_NAVIO2 DevMgr::releaseHandle(h_leds); +#endif DevMgr::releaseHandle(h_rgbleds); }