From 64531c4521491de68c2412269eead3d04e073b8f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 19 Jan 2021 05:58:38 -0800 Subject: [PATCH] Add SK6812 Serial PWM LED Driver --- platforms/nuttx/src/px4/common/CMakeLists.txt | 1 + .../src/px4/common/srgbled/CMakeLists.txt | 36 +++++ .../nuttx/src/px4/common/srgbled/srgbled.cpp | 128 ++++++++++++++++++ src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp | 2 +- 4 files changed, 166 insertions(+), 1 deletion(-) create mode 100644 platforms/nuttx/src/px4/common/srgbled/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/common/srgbled/srgbled.cpp diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index 313b8150b6..5bb17b5877 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -64,3 +64,4 @@ endif() add_dependencies(px4_layer prebuild_targets) add_subdirectory(gpio) +add_subdirectory(srgbled) diff --git a/platforms/nuttx/src/px4/common/srgbled/CMakeLists.txt b/platforms/nuttx/src/px4/common/srgbled/CMakeLists.txt new file mode 100644 index 0000000000..c46678a08a --- /dev/null +++ b/platforms/nuttx/src/px4/common/srgbled/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 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(platform_srgbled + srgbled.cpp +) diff --git a/platforms/nuttx/src/px4/common/srgbled/srgbled.cpp b/platforms/nuttx/src/px4/common/srgbled/srgbled.cpp new file mode 100644 index 0000000000..936340daf2 --- /dev/null +++ b/platforms/nuttx/src/px4/common/srgbled/srgbled.cpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 srgbled.cpp +* Author: David.Sidrane@Nscdg.com* +* This is a driver for the the neopixel class of serial RGB LEDs. +* +* It is arch dependent on ARM for the DWT and the GPIO is done in arch +* independent manner. +* +* To use this driver the board must define: + +* BOARD_HAS_N_S_RGB_LED - the number of LEDs +* BOARD_SRGBLED_BIT - the bit number it is connected to. 0-n (not a mask) + and +* BOARD_SRGBLED_PORT - The address of the port's "data out" register +* the LED is connected to. +* OR +* BOARD_SRGBLED_SET_PORT - for an arch with set/clear GPIO. The address of the +* port's "SET Bit" register +* BOARD_SRGBLED_CLEAR - for an arch with set/clear GPIO. The address of the +* port's "Clear Bit" register +* +* N.B. This version is small but will disable interrupts for 30 us per LED! +* With an 8 LED's this is 240uS and not tolerable for systems with sensors. +* It is however useful in bootloaders. +* +* The DMA version of this driver should be used for systems that can not have +* interrupts for long times. +* +*/ + +#include +#include +#include +#include +#include + +#define REG(_addr) (*(volatile uint32_t *)(_addr)) +#define rDEMCR REG(NVIC_DEMCR) +#define rDWT_CTRL REG(DWT_CTRL) +#define rDWT_CNT REG(DWT_CYCCNT) + +#if defined(BOARD_SRGBLED_PORT) && defined(BOARD_SRGBLED_BIT) +# define PORT REG(BOARD_SRGBLED_PORT) +# define D0 ((PORT) &= ~(1 << BOARD_SRGBLED_BIT)); +# define D1 ((PORT) |= (1 << BOARD_SRGBLED_BIT)); +#elif defined(BOARD_SRGBLED_SET_PORT) && defined(BOARD_SRGBLED_CLEAR_PORT) && defined(BOARD_SRGBLED_BIT) +# define PORT REG(BOARD_SRGBLED_PORT) +# define D0 ((BOARD_SRGBLED_CLEAR_PORT) |= (1 << BOARD_SRGBLED_BIT)); +# define D1 ((BOARD_SRGBLED_SET_PORT) |= (1 << BOARD_SRGBLED_BIT)); +#else +# error BOARD_SRGBLED_[]{SET|CLEAR}_]PORT and BOARD_SRGBLED_BIT needs to be defined. +#endif + +#define DWT_DEADLINE(t) rDWT_CNT + (t) +#define DWT_WAIT(v, D) while((rDWT_CNT - (v)) < (D)){} + +#define T0H (STM32_SYSCLK_FREQUENCY/3333333) +#define T1H (STM32_SYSCLK_FREQUENCY/1666666) +#define TW (STM32_SYSCLK_FREQUENCY/850000) + +#define COLOR_PER_LED 3 // There is a R G B in each package. +#define BITS_PER_COLOR 8 // Each LED has 8 bits of luminosity +#define BITS_PER_PACKAGE (BITS_PER_COLOR * COLOR_PER_LED) + +#if defined(BOARD_HAS_N_S_RGB_LED) && !defined(S_RGB_LED_DMA) + + +int neopixel_write_no_dma(uint8_t r, uint8_t g, uint8_t b, uint8_t led_count) +{ + register neopixel::NeoLEDData::led_data_t data; + data.grb[2] = g; + data.grb[1] = r; + data.grb[0] = b; + rDEMCR |= NVIC_DEMCR_TRCENA; + rDWT_CTRL |= DWT_CTRL_CYCCNTENA_MASK; + irqstate_t state = px4_enter_critical_section(); + + while (led_count--) { + uint32_t deadline = DWT_DEADLINE(TW); + + for (uint32_t mask = 1 << (BITS_PER_PACKAGE - 1); mask != 0; mask >>= 1) { + DWT_WAIT(deadline, TW); + deadline = rDWT_CNT; + D1; + DWT_WAIT(deadline, data.l & mask ? T1H : T0H); + D0; + } + + DWT_WAIT(deadline, TW); + } + + px4_leave_critical_section(state); + return 0; +} +#endif // BOARD_HAS_SRGBLED diff --git a/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp index 4a24c3bdba..3e33ee0e13 100644 --- a/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp +++ b/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp @@ -72,6 +72,7 @@ private: }; extern int led_pwm_servo_set(unsigned channel, uint8_t value); +extern int led_pwm_servo_set_ex(uint8_t r, uint8_t g, uint8_t b, uint8_t led_count); extern unsigned led_pwm_servo_get(unsigned channel); extern int led_pwm_servo_init(void); extern void led_pwm_servo_deinit(void); @@ -208,7 +209,6 @@ RGBLED_PWM::send_led_rgb() led_pwm_servo_set(4, _g); led_pwm_servo_set(5, _b); #endif - return (OK); }