From e847a6347d0fa230ff96576fe0b4498fda4c0275 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 20 Sep 2014 01:00:13 +0100 Subject: [PATCH] Improve speed of LED STRIP code. Shaved about 150us each strip update. This commit leaves in some timing trace code for the CC3D target, the results of which can be seen in the debug variables for the sections being timed. A later commit will remove/disable the timing code. --- src/main/build_config.h | 31 +++++++++ src/main/drivers/light_ws2811strip.c | 36 +++++++---- src/main/drivers/light_ws2811strip.h | 7 +- src/main/drivers/pwm_rx.c | 1 - src/main/io/ledstrip.c | 10 +++ src/main/main.c | 3 + src/main/target/CC3D/target.h | 2 + src/test/Makefile | 19 +++++- src/test/unit/ws2811_unittest.cc | 95 ++++++++++++++++++++++++++++ 9 files changed, 189 insertions(+), 15 deletions(-) create mode 100644 src/test/unit/ws2811_unittest.cc diff --git a/src/main/build_config.h b/src/main/build_config.h index ebf1bbc457..948662a4e7 100644 --- a/src/main/build_config.h +++ b/src/main/build_config.h @@ -20,9 +20,40 @@ #define UNUSED(x) (void)(x) #define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)])) +#ifdef UNIT_TEST +#define STATIC_UNIT_TESTED // make visible to unit test +#define UNIT_TESTED +#else +#define STATIC_UNIT_TESTED static +#define UNIT_TESTED +#endif + //#define SOFT_I2C // enable to test software i2c #ifndef __CC_ARM #define REQUIRE_CC_ARM_PRINTF_SUPPORT #define REQUIRE_PRINTF_LONG_SUPPORT #endif + +extern int16_t debug[4]; + +#ifdef DEBUG_SECTION_TIMES +extern uint32_t sectionTimes[2][4]; + +#define TIME_SECTION_BEGIN(index) { \ + extern uint32_t sectionTimes[2][4]; \ + sectionTimes[0][index] = micros(); \ +} + +#define TIME_SECTION_END(index) { \ + extern uint32_t sectionTimes[2][4]; \ + sectionTimes[1][index] = micros(); \ + debug[index] = sectionTimes[1][index] - sectionTimes[0][index]; \ +} +#else + +#define TIME_SECTION_BEGIN(index) {} +#define TIME_SECTION_END(index) {} + +#endif + diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 107bbeaa75..23977536d9 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -25,9 +25,12 @@ */ #include #include +#include #include "platform.h" +#include "build_config.h" + #include "common/color.h" #include "common/colorconversion.h" #include "drivers/light_ws2811strip.h" @@ -75,6 +78,7 @@ void setStripColors(const hsvColor_t *colors) void ws2811LedStripInit(void) { + memset(&ledStripDMABuffer, 0, WS2811_DMA_BUFFER_SIZE); ws2811LedStripHardwareInit(); ws2811UpdateStrip(); } @@ -84,10 +88,22 @@ bool isWS2811LedStripReady(void) return !ws2811LedDataTransferInProgress; } -static uint16_t dmaBufferOffset; +STATIC_UNIT_TESTED uint16_t dmaBufferOffset; static int16_t ledIndex; -void updateLEDDMABuffer(uint8_t componentValue) +#define USE_FAST_DMA_BUFFER_IMPL +#ifdef USE_FAST_DMA_BUFFER_IMPL + +STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color) +{ + uint32_t grb = (color->rgb.g << 16) | (color->rgb.r << 8) | (color->rgb.b); + + for (int8_t index = 23; index >= 0; index--) { + ledStripDMABuffer[dmaBufferOffset++] = (grb & (1 << index)) ? BIT_COMPARE_1 : BIT_COMPARE_0; + } +} +#else +STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue) { uint8_t bitIndex; @@ -95,15 +111,16 @@ void updateLEDDMABuffer(uint8_t componentValue) { if ((componentValue << bitIndex) & 0x80 ) // data sent MSB first, j = 0 is MSB j = 7 is LSB { - ledStripDMABuffer[dmaBufferOffset] = 17; // compare value for logical 1 + ledStripDMABuffer[dmaBufferOffset] = BIT_COMPARE_1; } else { - ledStripDMABuffer[dmaBufferOffset] = 9; // compare value for logical 0 + ledStripDMABuffer[dmaBufferOffset] = BIT_COMPARE_0; // compare value for logical 0 } dmaBufferOffset++; } } +#endif /* * This method is non-blocking unless an existing LED update is in progress. @@ -128,20 +145,17 @@ void ws2811UpdateStrip(void) { rgb24 = hsvToRgb24(&ledColorBuffer[ledIndex]); +#ifdef USE_FAST_DMA_BUFFER_IMPL + fastUpdateLEDDMABuffer(rgb24); +#else updateLEDDMABuffer(rgb24->rgb.g); updateLEDDMABuffer(rgb24->rgb.r); updateLEDDMABuffer(rgb24->rgb.b); +#endif ledIndex++; } - // add needed delay at end of byte cycle, pulsewidth = 0 - while(dmaBufferOffset < WS2811_DMA_BUFFER_SIZE) - { - ledStripDMABuffer[dmaBufferOffset] = 0; - dmaBufferOffset++; - } - ws2811LedDataTransferInProgress = 1; ws2811LedStripDMAEnable(); } diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 0efadbebeb..88282ca593 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -21,7 +21,12 @@ #define WS2811_BITS_PER_LED 24 #define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay -#define WS2811_DMA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH + WS2811_DELAY_BUFFER_LENGTH) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) +#define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH) + +#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) + +#define BIT_COMPARE_1 17 // timer compare value for logical 1 +#define BIT_COMPARE_0 9 // timer compare value for logical 0 void ws2811LedStripInit(void); diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index c992c1c21f..34af99d219 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -216,7 +216,6 @@ static void ppmEdgeCallback(uint8_t port, captureCompare_t capture) #define MAX_MISSED_PWM_EVENTS 10 -extern uint16_t debug[4]; static void pwmOverflowCallback(uint8_t port, captureCompare_t capture) { UNUSED(capture); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 3ee0082f82..42ae1fdb60 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -725,8 +725,11 @@ void updateLedStrip(void) static uint8_t warningState = 0; static uint8_t warningFlags; + TIME_SECTION_BEGIN(3); + // LAYER 1 + TIME_SECTION_BEGIN(0); applyLedModeLayer(); applyLedThrottleLayer(); @@ -781,7 +784,14 @@ void updateLedStrip(void) #ifdef USE_LED_ANIMATION applyLedAnimationLayer(); #endif + TIME_SECTION_END(0); + + TIME_SECTION_BEGIN(2); ws2811UpdateStrip(); + TIME_SECTION_END(2); + + TIME_SECTION_END(3); + } bool parseColor(uint8_t index, char *colorConfig) diff --git a/src/main/main.c b/src/main/main.c index 95a68a70d1..ef14d77a1e 100755 --- a/src/main/main.c +++ b/src/main/main.c @@ -68,6 +68,9 @@ #include "build_config.h" +#ifdef DEBUG_SECTION_TIMES +uint32_t sectionTimes[2][4]; +#endif extern uint32_t previousTime; #ifdef SOFTSERIAL_LOOPBACK diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 4163ceda8e..bffd52c806 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#define DEBUG_SECTION_TIMES + #define LED0_GPIO GPIOB #define LED0_PIN Pin_3 // PB3 (LED) #define LED0_PERIPHERAL RCC_APB2Periph_GPIOB diff --git a/src/test/Makefile b/src/test/Makefile index 4a21cbf859..4a3700ad9b 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -29,7 +29,7 @@ OBJECT_DIR = ../../obj/test CPPFLAGS += -isystem $(GTEST_DIR)/inc # Flags passed to the C++ compiler. -CXXFLAGS += -g -Wall -Wextra -pthread -ggdb -O0 +CXXFLAGS += -g -Wall -Wextra -pthread -ggdb -O0 -DUNIT_TEST # All tests produced by this Makefile. Remember to add new tests you # created to the list. @@ -39,7 +39,8 @@ TESTS = \ gps_conversion_unittest \ telemetry_hott_unittest \ rc_controls_unittest \ - ledstrip_unittest + ledstrip_unittest \ + ws2811_unittest # All Google Test headers. Usually you shouldn't change this # definition. @@ -172,3 +173,17 @@ ledstrip_unittest :$(OBJECT_DIR)/io/ledstrip.o $(OBJECT_DIR)/ledstrip_unittest.o $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + +$(OBJECT_DIR)/drivers/light_ws2811strip.o : $(USER_DIR)/drivers/light_ws2811strip.c $(USER_DIR)/drivers/light_ws2811strip.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/light_ws2811strip.c -o $@ + +$(OBJECT_DIR)/ws2811_unittest.o : $(TEST_DIR)/ws2811_unittest.cc \ + $(USER_DIR)/drivers/light_ws2811strip.h $(GTEST_HEADERS) + @mkdir -p $(dir $@) + $(CXX) $(CPPFLAGS) $(CXXFLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ws2811_unittest.cc -o $@ + +ws2811_unittest :$(OBJECT_DIR)/drivers/light_ws2811strip.o $(OBJECT_DIR)/ws2811_unittest.o $(OBJECT_DIR)/gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@ + + diff --git a/src/test/unit/ws2811_unittest.cc b/src/test/unit/ws2811_unittest.cc new file mode 100644 index 0000000000..5606155a24 --- /dev/null +++ b/src/test/unit/ws2811_unittest.cc @@ -0,0 +1,95 @@ +/* + * 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 + +#include "build_config.h" + +#include "common/color.h" + +#include "drivers/light_ws2811strip.h" + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +STATIC_UNIT_TESTED extern uint16_t dmaBufferOffset; + +STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color); +STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue); + +TEST(WS2812, updateDMABuffer) { + // given + rgbColor24bpp_t color1 = {0xFF,0xAA,0x55}; + + // and + dmaBufferOffset = 0; + + // when +#if 0 + updateLEDDMABuffer(color1.rgb.g); + updateLEDDMABuffer(color1.rgb.r); + updateLEDDMABuffer(color1.rgb.b); +#else + fastUpdateLEDDMABuffer(&color1); +#endif + + // then + EXPECT_EQ(24, dmaBufferOffset); + + // 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++; +} + +rgbColor24bpp_t* hsvToRgb24(const hsvColor_t *c) { + UNUSED(c); + return NULL; +} + +void ws2811LedStripHardwareInit(void) {} +void ws2811LedStripDMAEnable(void) {}