diff --git a/src/drivers/navio_gpio/navio_gpio.cpp b/src/drivers/navio_gpio/navio_gpio.cpp index f39f9387ab..7396f800f6 100644 --- a/src/drivers/navio_gpio/navio_gpio.cpp +++ b/src/drivers/navio_gpio/navio_gpio.cpp @@ -113,10 +113,10 @@ void Gpio::gpiowrite(uint32_t pinset, bool value) pin = (pinset & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT; if (value == 0) { - addr = (uint32_t)_gpio_map + GPIO_GPSET0_OFFSET + pin / 32; + addr = (uint32_t)_gpio_map + GPIO_GPCLR0_OFFSET + pin / 32; } else { - addr = (uint32_t)_gpio_map + GPIO_GPCLR0_OFFSET + pin / 32; + addr = (uint32_t)_gpio_map + GPIO_GPSET0_OFFSET + pin / 32; } shift = pin % 32; diff --git a/src/drivers/navio_gpio/test/test.cpp b/src/drivers/navio_gpio/test/test.cpp index 1e57b693a5..584fe6de11 100644 --- a/src/drivers/navio_gpio/test/test.cpp +++ b/src/drivers/navio_gpio/test/test.cpp @@ -8,6 +8,9 @@ #define LED_pinG GPIO_PIN27 #define LED_pinB GPIO_PIN6 +#define LED_OFF 1 +#define LED_ON 0 + using namespace navio_gpio; int do_test(); @@ -24,33 +27,34 @@ int do_test() 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); + + gpio.gpiowrite(LED_pinR, LED_OFF); + gpio.gpiowrite(LED_pinG, LED_OFF); + gpio.gpiowrite(LED_pinB, LED_OFF); printf("off\n"); sleep(2); - gpio.gpiowrite(LED_pinR, 1); - gpio.gpiowrite(LED_pinG, 0); - gpio.gpiowrite(LED_pinB, 0); + gpio.gpiowrite(LED_pinR, LED_ON); + gpio.gpiowrite(LED_pinG, LED_OFF); + gpio.gpiowrite(LED_pinB, LED_OFF); printf("red\n"); sleep(2); - gpio.gpiowrite(LED_pinR, 0); - gpio.gpiowrite(LED_pinG, 1); - gpio.gpiowrite(LED_pinB, 0); + gpio.gpiowrite(LED_pinR, LED_OFF); + gpio.gpiowrite(LED_pinG, LED_ON); + gpio.gpiowrite(LED_pinB, LED_OFF); printf("green\n"); sleep(2); - gpio.gpiowrite(LED_pinR, 0); - gpio.gpiowrite(LED_pinG, 0); - gpio.gpiowrite(LED_pinB, 1); + gpio.gpiowrite(LED_pinR, LED_OFF); + gpio.gpiowrite(LED_pinG, LED_OFF); + gpio.gpiowrite(LED_pinB, LED_ON); printf("blue\n"); sleep(2); - gpio.gpiowrite(LED_pinR, 0); - gpio.gpiowrite(LED_pinG, 0); - gpio.gpiowrite(LED_pinB, 0); + gpio.gpiowrite(LED_pinR, LED_OFF); + gpio.gpiowrite(LED_pinG, LED_OFF); + gpio.gpiowrite(LED_pinB, LED_OFF); printf("off\n"); gpio.stop(); diff --git a/src/drivers/navio_rgbled/CMakeLists.txt b/src/drivers/navio_rgbled/CMakeLists.txt index fb411106b1..998a840e69 100644 --- a/src/drivers/navio_rgbled/CMakeLists.txt +++ b/src/drivers/navio_rgbled/CMakeLists.txt @@ -43,4 +43,5 @@ px4_add_module( drivers__navio_gpio ) +#add_subdirectory(test) # 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 index 88a190bf30..6ced23c5db 100644 --- a/src/drivers/navio_rgbled/navio_rgbled.cpp +++ b/src/drivers/navio_rgbled/navio_rgbled.cpp @@ -1,11 +1,8 @@ -#include "navio_gpio.h" -#include "DevObj.hpp" #include #include -#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" -#define RGBLED_DEVICE_PATH "/dev/rgbled0" +#include "navio_rgbled.h" #define GPIO_LED_CNF (GPIO_CNF_OUTPUT) #define GPIO_LED_R (GPIO_PIN4) @@ -13,47 +10,7 @@ #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() { @@ -101,9 +58,9 @@ int RGBLED::devIOCTL(unsigned long request, unsigned long arg) 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; + _rgb.red = (rgb->red != 0) ? LED_ON : LED_OFF; + _rgb.green = (rgb->green != 0) ? LED_ON : LED_OFF; + _rgb.blue = (rgb->blue != 0) ? LED_ON : LED_OFF; _gpio.gpiowrite(GPIO_LED_R, _rgb.red); _gpio.gpiowrite(GPIO_LED_G, _rgb.green); _gpio.gpiowrite(GPIO_LED_B, _rgb.blue); @@ -132,19 +89,19 @@ int RGBLED::devIOCTL(unsigned long request, unsigned long arg) break; case RGBLED_MODE_BLINK_SLOW: - DevObj::setSampleInterval(20000 * 1000); + DevObj::setSampleInterval(2000 * 1000); break; case RGBLED_MODE_BLINK_NORMAL: - DevObj::setSampleInterval(5000 * 1000); + DevObj::setSampleInterval(500 * 1000); break; case RGBLED_MODE_BLINK_FAST: - DevObj::setSampleInterval(1000 * 1000); + DevObj::setSampleInterval(100 * 1000); break; case RGBLED_MODE_BREATHE: - DevObj::setSampleInterval(15000 * 1000); + DevObj::setSampleInterval(1500 * 1000); break; default: @@ -176,9 +133,9 @@ int RGBLED::devIOCTL(unsigned long request, unsigned long arg) void RGBLED::_measure() { if (_turn) { - _gpio.gpiowrite(GPIO_LED_R, 0); - _gpio.gpiowrite(GPIO_LED_G, 0); - _gpio.gpiowrite(GPIO_LED_B, 0); + _gpio.gpiowrite(GPIO_LED_R, LED_OFF); + _gpio.gpiowrite(GPIO_LED_G, LED_OFF); + _gpio.gpiowrite(GPIO_LED_B, LED_OFF); _turn = false; } else { diff --git a/src/drivers/navio_rgbled/navio_rgbled.h b/src/drivers/navio_rgbled/navio_rgbled.h new file mode 100644 index 0000000000..389b02807e --- /dev/null +++ b/src/drivers/navio_rgbled/navio_rgbled.h @@ -0,0 +1,54 @@ + +#include "DevObj.hpp" + +#include "navio_gpio.h" + +#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled" +#define RGBLED_DEVICE_PATH "/dev/rgbled0" + +// inverted +#define LED_ON 0 +#define LED_OFF 1 + + +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{ + {LED_OFF, LED_OFF, LED_OFF}, /* OFF */ + {LED_ON, LED_OFF, LED_OFF}, /* red */ + {LED_ON, LED_ON, LED_OFF}, /* yellow */ + {LED_ON, LED_OFF, LED_ON}, /* purple */ + {LED_OFF, LED_ON, LED_OFF}, /* green */ + {LED_OFF, LED_OFF, LED_ON}, /* blue */ + {LED_ON, LED_ON, LED_ON}, /* white */ + }, + _max_color(7), + _rgb{LED_OFF, LED_OFF, LED_OFF}, + _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; +}; diff --git a/src/drivers/navio_rgbled/test/CMakeLists.txt b/src/drivers/navio_rgbled/test/CMakeLists.txt new file mode 100644 index 0000000000..d7c8abb776 --- /dev/null +++ b/src/drivers/navio_rgbled/test/CMakeLists.txt @@ -0,0 +1,13 @@ +include_directories(..) + +add_executable(navio_rgbled_test + main.cpp + test.cpp + ) + +target_link_libraries(navio_rgbled_test + drivers__navio_rgbled + drivers__navio_gpio + platforms__posix__px4_layer + df_driver_framework + ) diff --git a/src/drivers/navio_rgbled/test/main.cpp b/src/drivers/navio_rgbled/test/main.cpp new file mode 100644 index 0000000000..394e637db4 --- /dev/null +++ b/src/drivers/navio_rgbled/test/main.cpp @@ -0,0 +1,6 @@ +extern int do_test(); + +int main() +{ + return do_test(); +} diff --git a/src/drivers/navio_rgbled/test/test.cpp b/src/drivers/navio_rgbled/test/test.cpp new file mode 100644 index 0000000000..d4278f6713 --- /dev/null +++ b/src/drivers/navio_rgbled/test/test.cpp @@ -0,0 +1,79 @@ +#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; +}