From eb8a3de987fd17cd50eb46efe74adb61ba807a94 Mon Sep 17 00:00:00 2001 From: CUAVcaijie Date: Mon, 26 Nov 2018 02:40:51 +0800 Subject: [PATCH] add rgbled_ncp5623c driver (#10889) Co-Authored-By: CUAVcaijie --- ROMFS/px4fmu_common/init.d/rcS | 2 + cmake/configs/nuttx_px4fmu-v3_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + .../configs/nuttx_px4fmu-v4pro_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v5_default.cmake | 1 + src/drivers/drv_led.h | 1 + src/drivers/rgbled_ncp5623c/CMakeLists.txt | 43 ++ .../rgbled_ncp5623c/rgbled_ncp5623c.cpp | 427 ++++++++++++++++++ .../rgbled_ncp5623c/rgbled_ncp5623c_params.c | 56 +++ 9 files changed, 533 insertions(+) create mode 100755 src/drivers/rgbled_ncp5623c/CMakeLists.txt create mode 100755 src/drivers/rgbled_ncp5623c/rgbled_ncp5623c.cpp create mode 100755 src/drivers/rgbled_ncp5623c/rgbled_ncp5623c_params.c diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cd59fe865e..02595c3ee3 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -243,6 +243,8 @@ else fi fi + rgbled_ncp5623c start + # # FMUv5 may have both PWM I2C RGB LED support. # diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake index afbd4e3c6b..fadda78865 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -46,6 +46,7 @@ set(config_module_list drivers/px4fmu drivers/px4io drivers/rgbled + drivers/rgbled_ncp5623c drivers/stm32 drivers/stm32/adc drivers/stm32/tone_alarm diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index e618b027fd..56cdb9dfc7 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -34,6 +34,7 @@ set(config_module_list drivers/px4flow drivers/px4fmu drivers/rgbled + drivers/rgbled_ncp5623c drivers/rc_input drivers/stm32 drivers/stm32/adc diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake index 0407747ab9..02fd61d13e 100644 --- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake @@ -39,6 +39,7 @@ set(config_module_list drivers/px4fmu drivers/px4io drivers/rgbled + drivers/rgbled_ncp5623c drivers/stm32 drivers/stm32/adc drivers/stm32/tone_alarm diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake index 97cda95273..49c991808e 100644 --- a/cmake/configs/nuttx_px4fmu-v5_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake @@ -40,6 +40,7 @@ set(config_module_list drivers/px4io drivers/rc_input drivers/rgbled + drivers/rgbled_ncp5623c #drivers/rgbled_pwm # Enable to put the three leds into PWM RGB mode drivers/stm32 drivers/stm32/adc diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 4cec85f71e..e02bb4fa24 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -59,4 +59,5 @@ static const int LED_UORB_QUEUE_LENGTH = BOARD_MAX_LEDS; // Legacy paths - 2 are need to allow both pwm and i2c drviers to co-exist #define RGBLED0_DEVICE_PATH "/dev/rgbled0" // Primary RGB LED on i2c +#define RGBLED1_DEVICE_PATH "/dev/rgbled1" // Primary RGB LED(NCP5623C) on i2c #define RGBLED_PWM0_DEVICE_PATH "/dev/rgbled_pwm0" // Secondary RGB LED on PWM diff --git a/src/drivers/rgbled_ncp5623c/CMakeLists.txt b/src/drivers/rgbled_ncp5623c/CMakeLists.txt new file mode 100755 index 0000000000..76710a0b01 --- /dev/null +++ b/src/drivers/rgbled_ncp5623c/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2018 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__rgbled_ncp5623c + MAIN rgbled_ncp5623c + COMPILE_FLAGS + SRCS + rgbled_ncp5623c.cpp + DEPENDS + drivers__device + led + ) diff --git a/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c.cpp new file mode 100755 index 0000000000..0dec360493 --- /dev/null +++ b/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c.cpp @@ -0,0 +1,427 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 rgbled_ncp5623c.cpp + * + * Driver for the onboard RGB LED controller (NCP5623C) connected via I2C. + * + * @author CUAVcaijie + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include +#include + +#include "uORB/topics/parameter_update.h" + +#define ADDR 0x39 /**< I2C adress of NCP5623C */ + +#define NCP5623_LED_CURRENT 0x20 /**< Current register */ +#define NCP5623_LED_PWM0 0x40 /**< pwm0 register */ +#define NCP5623_LED_PWM1 0x60 /**< pwm1 register */ +#define NCP5623_LED_PWM2 0x80 /**< pwm2 register */ + +#define NCP5623_LED_BRIGHT 0x1f /**< full brightness */ +#define NCP5623_LED_OFF 0x00 /**< off */ + + +class RGBLED_NPC5623C : public device::I2C +{ +public: + RGBLED_NPC5623C(int bus, int rgbled); + virtual ~RGBLED_NPC5623C(); + + + virtual int init(); + virtual int probe(); +private: + work_s _work; + + float _brightness; + float _max_brightness; + + uint8_t _r; + uint8_t _g; + uint8_t _b; + volatile bool _running; + volatile bool _should_run; + bool _leds_enabled; + int _param_sub; + + LedController _led_controller; + + static void led_trampoline(void *arg); + void led(); + + int send_led_rgb(); + void update_params(); + + int write(uint8_t reg, uint8_t data); +}; + +/* for now, we only support one RGBLED */ +namespace +{ +RGBLED_NPC5623C *g_rgbled = nullptr; +} + +void rgbled_ncp5623c_usage(); + +extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]); + +RGBLED_NPC5623C::RGBLED_NPC5623C(int bus, int rgbled) : + I2C("rgbled1", RGBLED1_DEVICE_PATH, bus, rgbled +#ifdef __PX4_NUTTX + , 100000 /* maximum speed supported */ +#endif + ), + _work{}, + _brightness(1.0f), + _max_brightness(1.0f), + _r(0), + _g(0), + _b(0), + _running(false), + _should_run(true), + _leds_enabled(true), + _param_sub(-1) +{ +} + +RGBLED_NPC5623C::~RGBLED_NPC5623C() +{ + _should_run = false; + int counter = 0; + + while (_running && ++counter < 10) { + usleep(100000); + } +} + +int +RGBLED_NPC5623C::write(uint8_t reg, uint8_t data) +{ + uint8_t msg[1] = { 0x00 }; + msg[0] = ((reg & 0xe0) | (data & 0x1f)); + + int ret = transfer(&msg[0], 1, nullptr, 0); + + return ret; +} + +int +RGBLED_NPC5623C::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + update_params(); + + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLED_NPC5623C::led_trampoline, this, 0); + + return OK; +} + +int +RGBLED_NPC5623C::probe() +{ + _retries = 4; + + return write(NCP5623_LED_CURRENT, 0x00); +} + +void +RGBLED_NPC5623C::led_trampoline(void *arg) +{ + RGBLED_NPC5623C *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +RGBLED_NPC5623C::led() +{ + if (!_should_run) { + if (_param_sub >= 0) { + orb_unsubscribe(_param_sub); + } + + int led_control_sub = _led_controller.led_control_subscription(); + + if (led_control_sub >= 0) { + orb_unsubscribe(led_control_sub); + } + + _running = false; + return; + } + + if (_param_sub < 0) { + _param_sub = orb_subscribe(ORB_ID(parameter_update)); + } + + if (!_led_controller.is_init()) { + int led_control_sub = orb_subscribe(ORB_ID(led_control)); + _led_controller.init(led_control_sub); + } + + if (_param_sub >= 0) { + bool updated = false; + orb_check(_param_sub, &updated); + + if (updated) { + parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + update_params(); + // Immediately update to change brightness + send_led_rgb(); + } + } + + 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: + _r = NCP5623_LED_BRIGHT; _g = 0; _b = 0; + break; + + case led_control_s::COLOR_GREEN: + _r = 0; _g = NCP5623_LED_BRIGHT; _b = 0; + break; + + case led_control_s::COLOR_BLUE: + _r = 0; _g = 0; _b = NCP5623_LED_BRIGHT; + break; + + case led_control_s::COLOR_AMBER: //make it the same as yellow + case led_control_s::COLOR_YELLOW: + _r = NCP5623_LED_BRIGHT; _g = NCP5623_LED_BRIGHT; _b = 0; + break; + + case led_control_s::COLOR_PURPLE: + _r = NCP5623_LED_BRIGHT; _g = 0; _b = NCP5623_LED_BRIGHT; + break; + + case led_control_s::COLOR_CYAN: + _r = 0; _g = NCP5623_LED_BRIGHT; _b = NCP5623_LED_BRIGHT; + break; + + case led_control_s::COLOR_WHITE: + _r = NCP5623_LED_BRIGHT; _g = NCP5623_LED_BRIGHT; _b = NCP5623_LED_BRIGHT; + break; + + default: // led_control_s::COLOR_OFF + _r = 0; _g = 0; _b = 0; + break; + } + + _brightness = (float)led_control_data.leds[0].brightness / 255.f; + send_led_rgb(); + + } + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED_NPC5623C::led_trampoline, this, + USEC2TICK(_led_controller.maximum_update_interval())); +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLED_NPC5623C::send_led_rgb() +{ + + uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80}; + uint8_t brightness = 0x1f * _max_brightness; + + msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f); + msg[2] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f); + msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f); + msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f); + + return transfer(&msg[0], 7, nullptr, 0); +} + +void +RGBLED_NPC5623C::update_params() +{ + int32_t maxbrt = 31; + param_get(param_find("LED_RGB1_MAXBRT"), &maxbrt); + maxbrt = maxbrt > 31 ? 31 : maxbrt; + maxbrt = maxbrt < 0 ? 0 : maxbrt; + + if (maxbrt == 0) { + maxbrt = 1; + } + + _max_brightness = maxbrt / 31.0f; +} + +void +rgbled_ncp5623c_usage() +{ + PX4_INFO("missing command: try 'start', 'stop'"); + PX4_INFO("options:"); + PX4_INFO(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + PX4_INFO(" -a addr (0x%x)", ADDR); +} + +int +rgbled_ncp5623c_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int rgbledadr = ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'a': + rgbledadr = strtol(myoptarg, nullptr, 0); + break; + + case 'b': + i2cdevice = strtol(myoptarg, nullptr, 0); + break; + + default: + rgbled_ncp5623c_usage(); + return 1; + } + } + + if (myoptind >= argc) { + rgbled_ncp5623c_usage(); + return 1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + if (g_rgbled != nullptr) { + PX4_WARN("already started"); + return 1; + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLED_NPC5623C(PX4_I2C_BUS_EXPANSION, rgbledadr); + + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + + if (g_rgbled == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + PX4_WARN("no RGB led on bus #%d", i2cdevice); + return 1; + } + + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_rgbled == nullptr) { + g_rgbled = new RGBLED_NPC5623C(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) { + PX4_WARN("alloc failed"); + return 1; + } + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + PX4_WARN("no RGB led on bus #%d", i2cdevice); + return 1; + } + } + + return 0; + } + + /* need the driver past this point */ + if (g_rgbled == nullptr) { + PX4_WARN("not started"); + rgbled_ncp5623c_usage(); + return 1; + } + + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + return 0; + } + + rgbled_ncp5623c_usage(); + return 1; +} diff --git a/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c_params.c b/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c_params.c new file mode 100755 index 0000000000..d1eb461e4a --- /dev/null +++ b/src/drivers/rgbled_ncp5623c/rgbled_ncp5623c_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 rgbled_ncp5623c_params.c + * + * Parameters defined by the RBG led driver + * + * @author CUAVcaijie + */ + + +#include +#include + +/** + * RGB Led brightness limit + * + * Set to 0 to disable, 1 for minimum brightness up to 31 (max) + * + * @min 0 + * @max 31 + * @group System + */ +PARAM_DEFINE_INT32(LED_RGB1_MAXBRT, 31); +