diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index b69b87ca58..1d0e91a3d5 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -28,7 +28,6 @@ #include #include -#include #include "platform.h" #include "common/maths.h" @@ -50,28 +49,11 @@ #include "scheduler/scheduler.h" -#ifdef USE_LED_STRIP_CACHE_MGMT -// WS2811_DMA_BUFFER_SIZE is multiples of uint32_t -// Number of bytes required for buffer -#define WS2811_DMA_BUF_BYTES (WS2811_DMA_BUFFER_SIZE * sizeof(uint32_t)) -// Number of bytes required to cache align buffer -#define WS2811_DMA_BUF_CACHE_ALIGN_BYTES ((WS2811_DMA_BUF_BYTES + 0x20) & ~0x1f) -// Size of array to create a cache aligned buffer -#define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof(uint32_t)) -DMA_RW_AXI __attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH]; -#else -DMA_DATA_ZERO_INIT uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#endif - -static ioTag_t ledStripIoTag; static bool ws2811Initialised = false; volatile bool ws2811LedDataTransferInProgress = false; static unsigned usedLedCount = 0; static bool needsFullRefresh = true; -uint16_t BIT_COMPARE_1 = 0; -uint16_t BIT_COMPARE_0 = 0; - static hsvColor_t ledColorBuffer[WS2811_DATA_BUFFER_SIZE]; #if !defined(USE_WS2811_SINGLE_COLOUR) @@ -119,17 +101,10 @@ void setUsedLedCount(unsigned ledCount) needsFullRefresh = true; } -void ws2811LedStripInit(ioTag_t ioTag) -{ - memset(ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); - - ledStripIoTag = ioTag; -} - void ws2811LedStripEnable(void) { if (!ws2811Initialised) { - if (!ws2811LedStripHardwareInit(ledStripIoTag)) { + if (!ws2811LedStripHardwareInit()) { return; } @@ -140,7 +115,7 @@ void ws2811LedStripEnable(void) // RGB or GRB ordering doesn't matter for black, use 4-channel LED configuraton to make sure all channels are zero // Multiple calls may be required as normally broken into multiple parts - while (!ws2811UpdateStrip(LED_GRBW, 100)); + while (!ws2811UpdateStrip(100)); } } @@ -149,44 +124,11 @@ bool isWS2811LedStripReady(void) return ws2811Initialised && !ws2811LedDataTransferInProgress; } -STATIC_UNIT_TESTED void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbColor24bpp_t *color, unsigned ledIndex) -{ - uint32_t bits_per_led; - uint32_t packed_colour; - - switch (ledFormat) { - case LED_RGB: // WS2811 drivers use RGB format - packed_colour = (color->rgb.r << 16) | (color->rgb.g << 8) | (color->rgb.b); - bits_per_led = 24; - break; - - case LED_GRBW: // SK6812 drivers use this - { - /* reconstruct white channel from RGB, making the intensity a bit nonlinear, but thats fine for this use case */ - uint8_t white = MIN(MIN(color->rgb.r, color->rgb.g), color->rgb.b); - packed_colour = (color->rgb.g << 24) | (color->rgb.r << 16) | (color->rgb.b << 8) | (white); - bits_per_led = 32; - break; - } - - case LED_GRB: // WS2812 drivers use GRB format - default: - packed_colour = (color->rgb.g << 16) | (color->rgb.r << 8) | (color->rgb.b); - bits_per_led = 24; - break; - } - - unsigned dmaBufferOffset = 0; - for (int index = bits_per_led - 1; index >= 0; index--) { - ledStripDMABuffer[ledIndex * bits_per_led + dmaBufferOffset++] = (packed_colour & (1U << index)) ? BIT_COMPARE_1 : BIT_COMPARE_0; - } -} - /* * This method is non-blocking unless an existing LED update is in progress. * it does not wait until all the LEDs have been updated, that happens in the background. */ -bool ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness) +bool ws2811UpdateStrip(uint8_t brightness) { static uint8_t ledIndex = 0; timeUs_t startTime = micros(); @@ -207,7 +149,7 @@ bool ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness) rgbColor24bpp_t *rgb24 = hsvToRgb24(&scaledLed); - updateLEDDMABuffer(ledFormat, rgb24, ledIndex++); + ws2811LedStripUpdateTransferBuffer(rgb24, ledIndex++); if (cmpTimeUs(micros(), startTime) > LED_TARGET_UPDATE_US) { return false; @@ -216,12 +158,8 @@ bool ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness) ledIndex = 0; needsFullRefresh = false; -#ifdef USE_LED_STRIP_CACHE_MGMT - SCB_CleanDCache_by_Addr(ledStripDMABuffer, WS2811_DMA_BUF_CACHE_ALIGN_BYTES); -#endif - ws2811LedDataTransferInProgress = true; - ws2811LedStripDMAEnable(); + ws2811LedStripStartTransfer(); return true; } diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index a3ead66d41..e66dc1a87d 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -25,38 +25,14 @@ #include "drivers/io_types.h" #define WS2811_LED_STRIP_LENGTH LED_STRIP_MAX_LENGTH - -#define WS2811_BITS_PER_LED_MAX 32 +#define WS2811_CARRIER_HZ 800000 #if defined(USE_WS2811_SINGLE_COLOUR) #define WS2811_DATA_BUFFER_SIZE 1 -#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE * WS2811_BITS_PER_LED_MAX) -// Do 2 extra iterations of the DMA transfer with the output set to low to generate the > 50us delay. -#define WS2811_DELAY_ITERATIONS 2 #else #define WS2811_DATA_BUFFER_SIZE WS2811_LED_STRIP_LENGTH -// for 50us delay -#define WS2811_DELAY_BUFFER_LENGTH 42 -// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) -#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE * WS2811_BITS_PER_LED_MAX + WS2811_DELAY_BUFFER_LENGTH) #endif -#ifdef USE_LED_STRIP_CACHE_MGMT -// WS2811_DMA_BUFFER_SIZE is multiples of uint32_t -// Number of bytes required for buffer -#define WS2811_DMA_BUF_BYTES (WS2811_DMA_BUFFER_SIZE * sizeof(uint32_t)) -// Number of bytes required to cache align buffer -#define WS2811_DMA_BUF_CACHE_ALIGN_BYTES ((WS2811_DMA_BUF_BYTES + 0x20) & ~0x1f) -// Size of array to create a cache aligned buffer -#define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof(uint32_t)) -extern uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH]; -#else -extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; -#endif - -#define WS2811_TIMER_MHZ 48 -#define WS2811_CARRIER_HZ 800000 - // Enumeration to match the string options defined in lookupLedStripFormatRGB in settings.c typedef enum { LED_GRB, @@ -64,13 +40,16 @@ typedef enum { LED_GRBW } ledStripFormatRGB_e; -void ws2811LedStripInit(ioTag_t ioTag); +extern volatile bool ws2811LedDataTransferInProgress; + +void ws2811LedStripInit(ioTag_t ioTag, ledStripFormatRGB_e ledFormat); void ws2811LedStripEnable(void); -bool ws2811LedStripHardwareInit(ioTag_t ioTag); -void ws2811LedStripDMAEnable(void); +bool ws2811LedStripHardwareInit(void); +void ws2811LedStripStartTransfer(void); +void ws2811LedStripUpdateTransferBuffer(rgbColor24bpp_t *color, unsigned ledIndex); -bool ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness); +bool ws2811UpdateStrip(uint8_t brightness); void setLedHsv(uint16_t index, const hsvColor_t *color); void getLedHsv(uint16_t index, hsvColor_t *color); @@ -84,8 +63,3 @@ void setStripColors(const hsvColor_t *colors); void setUsedLedCount(unsigned ledCount); bool isWS2811LedStripReady(void); - -extern volatile bool ws2811LedDataTransferInProgress; - -extern uint16_t BIT_COMPARE_1; -extern uint16_t BIT_COMPARE_0; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 8c2e977205..5f8619af93 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -1361,7 +1361,7 @@ void ledStripDisable(void) setStripColor(&HSV(BLACK)); // Multiple calls may be required as normally broken into multiple parts - while (!ws2811UpdateStrip((ledStripFormatRGB_e)ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness)); + while (!ws2811UpdateStrip(ledStripConfig()->ledstrip_brightness)); } } @@ -1375,7 +1375,7 @@ void ledStripInit(void) reevaluateLedConfig(); #endif - ws2811LedStripInit(ledStripConfig()->ioTag); + ws2811LedStripInit(ledStripConfig()->ioTag, (ledStripFormatRGB_e)ledStripConfig()->ledstrip_grb_rgb); } static uint8_t selectVisualBeeperColor(uint8_t colorIndex, bool *colorIndexIsCustom) @@ -1543,7 +1543,7 @@ void ledStripUpdate(timeUs_t currentTimeUs) } else { static bool multipassUpdate = false; // Profile is applied, so now update the LEDs - if (ws2811UpdateStrip((ledStripFormatRGB_e) ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness)) { + if (ws2811UpdateStrip(ledStripConfig()->ledstrip_brightness)) { // Final pass updating the DMA buffer is always short if (multipassUpdate) { schedulerIgnoreTaskExecTime(); diff --git a/src/platform/APM32/light_ws2811strip_apm32.c b/src/platform/APM32/light_ws2811strip_apm32.c index c2cafd8ba7..b1c1206458 100644 --- a/src/platform/APM32/light_ws2811strip_apm32.c +++ b/src/platform/APM32/light_ws2811strip_apm32.c @@ -37,6 +37,7 @@ #include "drivers/timer.h" #include "drivers/light_ws2811strip.h" +#include "platform/light_ws2811strip_stm32.h" static IO_t ws2811IO = IO_NONE; @@ -50,13 +51,13 @@ static FAST_IRQ_HANDLER void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descr ws2811LedDataTransferInProgress = false; } -bool ws2811LedStripHardwareInit(ioTag_t ioTag) +bool ws2811LedStripHardwareInit(void) { - if (!ioTag) { + if (!ledStripIoTag) { return false; } - const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_LED_STRIP, 0); + const timerHardware_t *timerHardware = timerAllocate(ledStripIoTag, OWNER_LED_STRIP, 0); if (timerHardware == NULL) { return false; @@ -105,7 +106,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) static DMA_HandleTypeDef hdma_tim; - ws2811IO = IOGetByTag(ioTag); + ws2811IO = IOGetByTag(ledStripIoTag); IOInit(ws2811IO, OWNER_LED_STRIP, 0); IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction); @@ -172,7 +173,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) return true; } -void ws2811LedStripDMAEnable(void) +void ws2811LedStripStartTransfer(void) { if (DMA_SetCurrDataCounter(&TmrHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != DAL_OK) { /* DMA set error */ diff --git a/src/platform/APM32/mk/APM32F4.mk b/src/platform/APM32/mk/APM32F4.mk index 7affd8f0e5..216546a9fd 100644 --- a/src/platform/APM32/mk/APM32F4.mk +++ b/src/platform/APM32/mk/APM32F4.mk @@ -189,7 +189,8 @@ MCU_COMMON_SRC = \ common/stm32/serial_uart_pinconfig.c \ drivers/serial_escserial.c \ drivers/serial_pinconfig.c \ - APM32/system_apm32f4xx.c + APM32/system_apm32f4xx.c \ + common/stm32/ledstrip_ws2811_stm32.c VCP_SRC = \ APM32/usb/vcp/usbd_cdc_descriptor.c \ diff --git a/src/platform/AT32/light_ws2811strip_at32f43x.c b/src/platform/AT32/light_ws2811strip_at32f43x.c index e4afd9f202..c5aa6e9388 100644 --- a/src/platform/AT32/light_ws2811strip_at32f43x.c +++ b/src/platform/AT32/light_ws2811strip_at32f43x.c @@ -39,6 +39,7 @@ #include "drivers/timer.h" #include "drivers/light_ws2811strip.h" +#include "platform/light_ws2811strip_stm32.h" static IO_t ws2811IO = IO_NONE; @@ -71,13 +72,13 @@ static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) } } -bool ws2811LedStripHardwareInit(ioTag_t ioTag) +bool ws2811LedStripHardwareInit(void) { - if (!ioTag) { + if (!ledStripIoTag) { return false; } - const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_LED_STRIP, 0); + const timerHardware_t *timerHardware = timerAllocate(ledStripIoTag, OWNER_LED_STRIP, 0); if (timerHardware == NULL) { return false; @@ -102,7 +103,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) return false; } - ws2811IO = IOGetByTag(ioTag); + ws2811IO = IOGetByTag(ledStripIoTag); IOInit(ws2811IO, OWNER_LED_STRIP, 0); IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP, timerHardware->alternateFunction); @@ -192,12 +193,11 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) return true; } -void ws2811LedStripDMAEnable(void) +void ws2811LedStripStartTransfer(void) { xDMA_SetCurrDataCounter(dmaRef, WS2811_DMA_BUFFER_SIZE); tmr_counter_value_set(timer, 0); tmr_counter_enable(timer, TRUE); xDMA_Cmd(dmaRef, TRUE); - } #endif diff --git a/src/platform/AT32/mk/AT32F4.mk b/src/platform/AT32/mk/AT32F4.mk index e9ab808694..0fec317648 100644 --- a/src/platform/AT32/mk/AT32F4.mk +++ b/src/platform/AT32/mk/AT32F4.mk @@ -135,7 +135,8 @@ MCU_COMMON_SRC = \ msc/usbd_storage_emfat.c \ msc/emfat.c \ msc/emfat_file.c \ - msc/usbd_storage_sd_spi.c + msc/usbd_storage_sd_spi.c \ + common/stm32/ledstrip_ws2811_stm32.c SPEED_OPTIMISED_SRC += \ common/stm32/dshot_bitbang_shared.c \ diff --git a/src/platform/PICO/dma_pico.c b/src/platform/PICO/dma_pico.c index ad2eb640ca..3fa65227ac 100644 --- a/src/platform/PICO/dma_pico.c +++ b/src/platform/PICO/dma_pico.c @@ -135,7 +135,6 @@ void dmaSetHandler(dmaIdentifier_e identifier, dmaCallbackHandlerFuncPtr callbac dmaDescriptors[index].irqN = core ? DMA_IRQ_1_IRQn : DMA_IRQ_0_IRQn; irq_handler_t irq_handler = core ? dma_irq1_handler : dma_irq0_handler; - irq_set_exclusive_handler(dmaDescriptors[index].irqN, irq_handler); irq_set_enabled(dmaDescriptors[index].irqN, true); diff --git a/src/platform/PICO/light_ws2811strip_pico.c b/src/platform/PICO/light_ws2811strip_pico.c new file mode 100644 index 0000000000..4fe57f0a30 --- /dev/null +++ b/src/platform/PICO/light_ws2811strip_pico.c @@ -0,0 +1,228 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_LED_STRIP + +#include "common/color.h" +#include "common/maths.h" + +#include "drivers/dma.h" +#include "platform/dma.h" +#include "drivers/io.h" +#include "drivers/time.h" +#include "drivers/light_ws2811strip.h" +#include "hardware/clocks.h" +#include "hardware/pio.h" + +#define WS2812_WRAP_TARGET 0 +#define WS2812_WRAP 3 +#define WS2812_PIO_VERSION 0 + +#define WS2812_T1 3 +#define WS2812_T2 3 +#define WS2812_T3 4 + +#define WS2812_BYTES_PER_LED 4 // Number of bytes per WS2812 LED (RGB) +#define WS2811_LED_STRIP_BUFFER_SIZE (WS2811_LED_STRIP_LENGTH * WS2812_BYTES_PER_LED) + +static IO_t ledStripIO = IO_NONE; +static ioTag_t ledStripIoTag = IO_TAG_NONE; +static ledStripFormatRGB_e ledStripFormat = LED_GRB; // Default format is RGB +static uint8_t frameSize = 3; // Default format is RGB, so 3 is the size of the frame (RGB) + +// DMA channel +static uint8_t dma_chan; + +// Buffer to hold the LED color data +static uint8_t led_data[WS2811_LED_STRIP_BUFFER_SIZE]; +static uint8_t ledStripPendingTransferCount = 0; +static timeUs_t ledStripCompletedTime = 0; + +static const uint16_t ws2812_program_instructions[] = { + // .wrap_target + 0x6321, // 0: out x, 1 side 0 [3] + 0x1223, // 1: jmp !x, 3 side 1 [2] + 0x1200, // 2: jmp 0 side 1 [2] + 0xa242, // 3: nop side 0 [2] + // .wrap +}; + +static const struct pio_program ws2812_program = { + .instructions = ws2812_program_instructions, + .length = ARRAYLEN(ws2812_program_instructions), + .origin = -1, + .pio_version = WS2812_PIO_VERSION, + .used_gpio_ranges = 0x0 +}; + +static inline pio_sm_config ws2812_program_get_default_config(uint offset) +{ + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + WS2812_WRAP_TARGET, offset + WS2812_WRAP); + sm_config_set_sideset(&c, 1, false, false); + return c; +} + +static inline void ws2812_program_init(PIO pio, uint sm, uint offset, uint pin, float freq, bool rgbw) +{ + pio_gpio_init(pio, pin); + pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); + pio_sm_config c = ws2812_program_get_default_config(offset); + sm_config_set_sideset_pins(&c, pin); + sm_config_set_out_shift(&c, false, true, rgbw ? 32 : 24); // when should we pull from the fifo + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + int cycles_per_bit = WS2812_T1 + WS2812_T2 + WS2812_T3; + float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +static FAST_IRQ_HANDLER void ws2811LedStripDmaHandler(dmaChannelDescriptor_t* descriptor) +{ + UNUSED(descriptor); + ws2811LedDataTransferInProgress = false; + ledStripCompletedTime = micros(); +} + +void ws2811LedStripInit(ioTag_t ioTag, ledStripFormatRGB_e ledFormat) +{ + ledStripFormat = ledFormat; // Store the format globally + frameSize = (ledStripFormat == LED_GRBW) ? 4 : 3; // 4 bytes for GRBW, 3 bytes for RGB/GRB + memset(led_data, 0, sizeof(led_data)); + ledStripIoTag = ioTag; +} + +bool ws2811LedStripHardwareInit(void) +{ + if (!ledStripIoTag) { + return false; + } + + IO_t io = IOGetByTag(ledStripIoTag); + if (!IOIsFreeOrPreinit(io)) { + return false; + } + + // This will find a free pio and state machine for our program and load it for us + PIO pio; + uint sm; + uint offset; + + int pinIndex = DEFIO_TAG_PIN(ledStripIoTag); + bool success = pio_claim_free_sm_and_add_program_for_gpio_range(&ws2812_program, &pio, &sm, &offset, pinIndex, 1, true); + if (!success) { + return false; // No free PIO or state machine available + } + ws2812_program_init(pio, sm, offset, pinIndex, WS2811_CARRIER_HZ, ledStripFormat == LED_GRBW); + + // --- DMA Configuration --- + const dmaIdentifier_e dma_id = dmaGetFreeIdentifier(); + if (dma_id == DMA_NONE) { + return false; // No free DMA channel available + } + + dma_chan = DMA_IDENTIFIER_TO_CHANNEL(dma_id); + dma_channel_config c = dma_channel_get_default_config(dma_chan); + channel_config_set_transfer_data_size(&c, DMA_SIZE_8); + channel_config_set_read_increment(&c, true); + channel_config_set_write_increment(&c, false); + channel_config_set_dreq(&c, pio_get_dreq(pio, sm, true)); + + dma_channel_configure( + dma_chan, + &c, + &pio->txf[sm], // Write address (PIO TX FIFO) + NULL, // Read address (set later) + WS2811_LED_STRIP_BUFFER_SIZE, // Number of transfers + false // Don't start immediately + ); + + // --- Interrupt Configuration --- + dmaSetHandler(dma_id, ws2811LedStripDmaHandler, 0, 0); + + IOInit(io, OWNER_LED_STRIP, 0); + ledStripIO = io; + return true; +} + +void ws2811LedStripStartTransfer(void) +{ + if (!ledStripIO) { + ws2811LedDataTransferInProgress = false; + return; // Not initialized + } + + // guard to ensure we don't start a transfer before a reset period has elapsed. + if (ABS(cmpTimeUs(ledStripCompletedTime, micros())) < 50) { + ws2811LedDataTransferInProgress = false; + return; // Not initialized + } + + // Set the read address to the led_data buffer + dma_channel_set_read_addr(dma_chan, led_data, false); + // Start the DMA transfer + dma_channel_set_trans_count(dma_chan, ledStripPendingTransferCount, true); + ledStripPendingTransferCount = 0; +} + +void ws2811LedStripUpdateTransferBuffer(rgbColor24bpp_t *color, unsigned ledIndex) +{ + if (ledIndex >= WS2811_LED_STRIP_LENGTH) { + return; // Index out of bounds + } + + // FIFO buffer for PIO is 32 bits wide, but we use 24 bits for RGB or 32 bits for GRBW + // the PIO pulls data from the buffer bit by bit. + const uint8_t bufferIndex = ledIndex * frameSize; // RGB or GRBW format + switch(ledStripFormat) { + case LED_RGB: // WS2811 drivers use RGB format + led_data[bufferIndex] = color->rgb.r; + led_data[bufferIndex + 1] = color->rgb.g; + led_data[bufferIndex + 2] = color->rgb.b; + break; + + case LED_GRBW: // SK6812 drivers use GRBW format + // Reconstruct white channel from RGB, making the intensity a bit nonlinear, but that's fine for this use case + { + uint8_t white = MIN(MIN(color->rgb.r, color->rgb.g), color->rgb.b); + led_data[bufferIndex] = color->rgb.g; // Green + led_data[bufferIndex + 1] = color->rgb.r; // Red + led_data[bufferIndex + 2] = color->rgb.b; // Blue + led_data[bufferIndex + 3] = white; // White + } + break; + case LED_GRB: // WS2812 drivers use GRB format + default: + led_data[bufferIndex] = color->rgb.g; + led_data[bufferIndex + 1] = color->rgb.r; + led_data[bufferIndex + 2] = color->rgb.b; + break; + } + ledStripPendingTransferCount = bufferIndex + frameSize; // Update the count of bytes to transfer +} +#endif diff --git a/src/platform/PICO/mk/RP2350.mk b/src/platform/PICO/mk/RP2350.mk index b26625e412..7950f96084 100644 --- a/src/platform/PICO/mk/RP2350.mk +++ b/src/platform/PICO/mk/RP2350.mk @@ -408,7 +408,8 @@ MCU_COMMON_SRC = \ PICO/serial_usb_vcp_pico.c \ PICO/system.c \ PICO/usb/usb_cdc.c \ - PICO/multicore.c + PICO/multicore.c \ + PICO/light_ws2811strip_pico.c DEVICE_STDPERIPH_SRC := \ $(PICO_LIB_SRC) \ diff --git a/src/platform/PICO/target/RP2350A/target.h b/src/platform/PICO/target/RP2350A/target.h index e459539ad7..9f108a4597 100644 --- a/src/platform/PICO/target/RP2350A/target.h +++ b/src/platform/PICO/target/RP2350A/target.h @@ -141,7 +141,6 @@ #undef USE_RPM_LIMIT #undef USE_SERVOS -#undef USE_LED_STRIP #undef USE_OSD #undef USE_OSD_SD #undef USE_OSD_HD diff --git a/src/platform/PICO/target/RP2350B/target.h b/src/platform/PICO/target/RP2350B/target.h index fd48dc30fa..27735dfcb7 100644 --- a/src/platform/PICO/target/RP2350B/target.h +++ b/src/platform/PICO/target/RP2350B/target.h @@ -179,7 +179,6 @@ #undef USE_RPM_LIMIT #undef USE_SERVOS -#undef USE_LED_STRIP #undef USE_OSD #undef USE_OSD_SD #undef USE_OSD_HD diff --git a/src/platform/STM32/light_ws2811strip_hal.c b/src/platform/STM32/light_ws2811strip_hal.c index c5f885c1b2..38ee57ca3a 100644 --- a/src/platform/STM32/light_ws2811strip_hal.c +++ b/src/platform/STM32/light_ws2811strip_hal.c @@ -36,6 +36,7 @@ #include "drivers/timer.h" #include "drivers/light_ws2811strip.h" +#include "platform/light_ws2811strip_stm32.h" static IO_t ws2811IO = IO_NONE; @@ -49,13 +50,13 @@ static FAST_IRQ_HANDLER void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descr ws2811LedDataTransferInProgress = false; } -bool ws2811LedStripHardwareInit(ioTag_t ioTag) +bool ws2811LedStripHardwareInit(void) { - if (!ioTag) { + if (!ledStripIoTag) { return false; } - const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_LED_STRIP, 0); + const timerHardware_t *timerHardware = timerAllocate(ledStripIoTag, OWNER_LED_STRIP, 0); if (timerHardware == NULL) { return false; @@ -103,7 +104,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) static DMA_HandleTypeDef hdma_tim; - ws2811IO = IOGetByTag(ioTag); + ws2811IO = IOGetByTag(ledStripIoTag); IOInit(ws2811IO, OWNER_LED_STRIP, 0); IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction); @@ -176,8 +177,12 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) return true; } -void ws2811LedStripDMAEnable(void) +void ws2811LedStripStartTransfer(void) { +#ifdef USE_LED_STRIP_CACHE_MGMT + SCB_CleanDCache_by_Addr(ledStripDMABuffer, WS2811_DMA_BUF_CACHE_ALIGN_BYTES); +#endif + if (DMA_SetCurrDataCounter(&TimHandle, timerChannel, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) { /* DMA set error */ ws2811LedDataTransferInProgress = false; diff --git a/src/platform/STM32/light_ws2811strip_stdperiph.c b/src/platform/STM32/light_ws2811strip_stdperiph.c index 13723de3f5..e4ad635bbe 100644 --- a/src/platform/STM32/light_ws2811strip_stdperiph.c +++ b/src/platform/STM32/light_ws2811strip_stdperiph.c @@ -38,6 +38,7 @@ #include "drivers/timer.h" #include "drivers/light_ws2811strip.h" +#include "platform/light_ws2811strip_stm32.h" static IO_t ws2811IO = IO_NONE; #if defined(STM32F4) @@ -73,9 +74,9 @@ static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) } } -bool ws2811LedStripHardwareInit(ioTag_t ioTag) +bool ws2811LedStripHardwareInit(void) { - if (!ioTag) { + if (!ledStripIoTag) { return false; } @@ -83,7 +84,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) TIM_OCInitTypeDef TIM_OCInitStructure; DMA_InitTypeDef DMA_InitStructure; - const timerHardware_t *timerHardware = timerAllocate(ioTag, OWNER_LED_STRIP, 0); + const timerHardware_t *timerHardware = timerAllocate(ledStripIoTag, OWNER_LED_STRIP, 0); if (timerHardware == NULL) { return false; @@ -113,7 +114,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) return false; } - ws2811IO = IOGetByTag(ioTag); + ws2811IO = IOGetByTag(ledStripIoTag); IOInit(ws2811IO, OWNER_LED_STRIP, 0); IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction); @@ -203,7 +204,7 @@ bool ws2811LedStripHardwareInit(ioTag_t ioTag) return true; } -void ws2811LedStripDMAEnable(void) +void ws2811LedStripStartTransfer(void) { xDMA_SetCurrDataCounter(dmaRef, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred TIM_SetCounter(timer, 0); diff --git a/src/platform/STM32/mk/STM32_COMMON.mk b/src/platform/STM32/mk/STM32_COMMON.mk index da565fd38c..90a6187c50 100644 --- a/src/platform/STM32/mk/STM32_COMMON.mk +++ b/src/platform/STM32/mk/STM32_COMMON.mk @@ -17,7 +17,8 @@ MCU_COMMON_SRC += \ STM32/pwm_output_hw.c \ common/stm32/pwm_output_dshot_shared.c \ common/stm32/pwm_output_beeper.c \ - common/stm32/dshot_bitbang_shared.c + common/stm32/dshot_bitbang_shared.c \ + common/stm32/ledstrip_ws2811_stm32.c SIZE_OPTIMISED_SRC += \ drivers/bus_spi_config.c \ diff --git a/src/platform/common/stm32/ledstrip_ws2811_stm32.c b/src/platform/common/stm32/ledstrip_ws2811_stm32.c new file mode 100644 index 0000000000..0863e9b582 --- /dev/null +++ b/src/platform/common/stm32/ledstrip_ws2811_stm32.c @@ -0,0 +1,90 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" +#include "common/maths.h" + +#include "common/color.h" +#include "drivers/light_ws2811strip.h" +#include "platform/light_ws2811strip_stm32.h" + +uint16_t BIT_COMPARE_1 = 0; +uint16_t BIT_COMPARE_0 = 0; + +#ifdef USE_LED_STRIP_CACHE_MGMT +// WS2811_DMA_BUFFER_SIZE is multiples of uint32_t +// Number of bytes required for buffer +#define WS2811_DMA_BUF_BYTES (WS2811_DMA_BUFFER_SIZE * sizeof(uint32_t)) +// Number of bytes required to cache align buffer +#define WS2811_DMA_BUF_CACHE_ALIGN_BYTES ((WS2811_DMA_BUF_BYTES + 0x20) & ~0x1f) +// Size of array to create a cache aligned buffer +#define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof(uint32_t)) +DMA_RW_AXI __attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH]; +#else +DMA_DATA_ZERO_INIT uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#endif + +ioTag_t ledStripIoTag; +static ledStripFormatRGB_e ws2811LedFormat; + +void ws2811LedStripInit(ioTag_t ioTag, ledStripFormatRGB_e ledFormat) +{ + ws2811LedFormat = ledFormat; + memset(ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); + ledStripIoTag = ioTag; +} + +void ws2811LedStripUpdateTransferBuffer(rgbColor24bpp_t *color, unsigned ledIndex) +{ + uint32_t bits_per_led; + uint32_t packed_colour; + + switch (ws2811LedFormat) { + case LED_RGB: // WS2811 drivers use RGB format + packed_colour = (color->rgb.r << 16) | (color->rgb.g << 8) | (color->rgb.b); + bits_per_led = 24; + break; + + case LED_GRBW: // SK6812 drivers use this + { + /* reconstruct white channel from RGB, making the intensity a bit nonlinear, but thats fine for this use case */ + uint8_t white = MIN(MIN(color->rgb.r, color->rgb.g), color->rgb.b); + packed_colour = (color->rgb.g << 24) | (color->rgb.r << 16) | (color->rgb.b << 8) | (white); + bits_per_led = 32; + break; + } + + case LED_GRB: // WS2812 drivers use GRB format + default: + packed_colour = (color->rgb.g << 16) | (color->rgb.r << 8) | (color->rgb.b); + bits_per_led = 24; + break; + } + + unsigned dmaBufferOffset = 0; + for (int index = bits_per_led - 1; index >= 0; index--) { + ledStripDMABuffer[ledIndex * bits_per_led + dmaBufferOffset++] = (packed_colour & (1U << index)) ? BIT_COMPARE_1 : BIT_COMPARE_0; + } +} diff --git a/src/platform/common/stm32/platform/light_ws2811strip_stm32.h b/src/platform/common/stm32/platform/light_ws2811strip_stm32.h new file mode 100644 index 0000000000..667fd79907 --- /dev/null +++ b/src/platform/common/stm32/platform/light_ws2811strip_stm32.h @@ -0,0 +1,61 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + + +#pragma once + +#include "platform.h" + +#include "drivers/io_types.h" +#include "drivers/light_ws2811strip.h" + +#define WS2811_LED_STRIP_LENGTH LED_STRIP_MAX_LENGTH + +#define WS2811_BITS_PER_LED_MAX 32 +#define WS2811_TIMER_MHZ 48 + +#if defined(USE_WS2811_SINGLE_COLOUR) +#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE * WS2811_BITS_PER_LED_MAX) +// Do 2 extra iterations of the DMA transfer with the output set to low to generate the > 50us delay. +#define WS2811_DELAY_ITERATIONS 2 +#else +// for 50us delay +#define WS2811_DELAY_BUFFER_LENGTH 42 +// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) +#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE * WS2811_BITS_PER_LED_MAX + WS2811_DELAY_BUFFER_LENGTH) +#endif + +#ifdef USE_LED_STRIP_CACHE_MGMT +// WS2811_DMA_BUFFER_SIZE is multiples of uint32_t +// Number of bytes required for buffer +#define WS2811_DMA_BUF_BYTES (WS2811_DMA_BUFFER_SIZE * sizeof(uint32_t)) +// Number of bytes required to cache align buffer +#define WS2811_DMA_BUF_CACHE_ALIGN_BYTES ((WS2811_DMA_BUF_BYTES + 0x20) & ~0x1f) +// Size of array to create a cache aligned buffer +#define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof(uint32_t)) +extern uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH]; +#else +extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#endif + +extern uint16_t BIT_COMPARE_1; +extern uint16_t BIT_COMPARE_0; +extern ioTag_t ledStripIoTag; diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index da9fc21ef7..1a82ea8ffd 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -311,12 +311,13 @@ batteryState_e getBatteryState(void) return BATTERY_OK; } -void ws2811LedStripInit(ioTag_t ioTag) +void ws2811LedStripInit(ioTag_t ioTag, ledStripFormatRGB_e ledFormat) { UNUSED(ioTag); + UNUSED(ledFormat); } -bool ws2811UpdateStrip(ledStripFormatRGB_e, uint8_t) {return true;} +bool ws2811UpdateStrip(uint8_t) {return true;} void setLedValue(uint16_t index, const uint8_t value) { diff --git a/src/test/unit/ws2811_unittest.cc b/src/test/unit/ws2811_unittest.cc deleted file mode 100644 index 15408a6d6d..0000000000 --- a/src/test/unit/ws2811_unittest.cc +++ /dev/null @@ -1,99 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ -#include -#include - -#include - -extern "C" { - #include "build/build_config.h" - - #include "common/color.h" - - #include "drivers/light_ws2811strip.h" -} - -#include "unittest_macros.h" -#include "gtest/gtest.h" - -extern "C" { - uint32_t simulatedTime = 0; - uint32_t micros(void) { return simulatedTime; } - void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbColor24bpp_t *color, unsigned ledIndex); - void schedulerIgnoreTaskExecTime(void) {} - void schedulerIgnoreTaskStateTime(void) {} -} - -TEST(WS2812, updateDMABuffer) -{ - // given - rgbColor24bpp_t color1 = { .raw = {0xFF,0xAA,0x55} }; - - // when - updateLEDDMABuffer(LED_GRB, &color1, 0); - - // and - uint8_t byteIndex = 0; - - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 0]); - EXPECT_EQ(BIT_COMPARE_0, ledStripDMABuffer[(byteIndex * 8) + 1]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 2]); - EXPECT_EQ(BIT_COMPARE_0, ledStripDMABuffer[(byteIndex * 8) + 3]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 4]); - EXPECT_EQ(BIT_COMPARE_0, ledStripDMABuffer[(byteIndex * 8) + 5]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 6]); - EXPECT_EQ(BIT_COMPARE_0, ledStripDMABuffer[(byteIndex * 8) + 7]); - byteIndex++; - - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 0]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 1]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 2]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 3]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 4]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 5]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 6]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 7]); - byteIndex++; - - EXPECT_EQ(BIT_COMPARE_0, ledStripDMABuffer[(byteIndex * 8) + 0]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 1]); - EXPECT_EQ(BIT_COMPARE_0, ledStripDMABuffer[(byteIndex * 8) + 2]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 3]); - EXPECT_EQ(BIT_COMPARE_0, ledStripDMABuffer[(byteIndex * 8) + 4]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 5]); - EXPECT_EQ(BIT_COMPARE_0, ledStripDMABuffer[(byteIndex * 8) + 6]); - EXPECT_EQ(BIT_COMPARE_1, ledStripDMABuffer[(byteIndex * 8) + 7]); - byteIndex++; -} - -extern "C" { -rgbColor24bpp_t* hsvToRgb24(const hsvColor_t *c) -{ - UNUSED(c); - return NULL; -} - -bool ws2811LedStripHardwareInit(ioTag_t ioTag) -{ - UNUSED(ioTag); - - return true; -} - -void ws2811LedStripDMAEnable(void) {} -} -