diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 6043fd54125..12ea26e7e71 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -42,6 +42,9 @@ #include "drivers/dma.h" #include "drivers/io.h" +#include "drivers/time.h" + +#include "io/ledstrip.h" #include "light_ws2811strip.h" @@ -136,10 +139,12 @@ void ws2811LedStripEnable(void) const hsvColor_t hsv_black = { 0, 0, 0 }; setStripColor(&hsv_black); - // RGB or GRB ordering doesn't matter for black, use 4-channel LED configuraton to make sure all channels are zero - ws2811UpdateStrip(LED_GRBW, 100); ws2811Initialised = true; + + // 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)); } } @@ -185,15 +190,16 @@ STATIC_UNIT_TESTED void updateLEDDMABuffer(ledStripFormatRGB_e ledFormat, rgbCol * 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. */ -void ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness) +bool ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness) { + static uint8_t ledIndex = 0; + timeUs_t startTime = micros(); // don't wait - risk of infinite block, just get an update next time round if (!ws2811Initialised || ws2811LedDataTransferInProgress) { schedulerIgnoreTaskStateTime(); - return; + return false; } - unsigned ledIndex = 0; // reset led index // fill transmit buffer with correct compare values to achieve // correct pulse widths according to color values @@ -207,7 +213,12 @@ void ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness) rgbColor24bpp_t *rgb24 = hsvToRgb24(&scaledLed); updateLEDDMABuffer(ledFormat, rgb24, ledIndex++); + + if (cmpTimeUs(micros(), startTime) > LED_TARGET_UPDATE_US) { + return false; + } } + ledIndex = 0; needsFullRefresh = false; #ifdef USE_LED_STRIP_CACHE_MGMT @@ -216,6 +227,8 @@ void ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness) ws2811LedDataTransferInProgress = true; ws2811LedStripDMAEnable(); + + return true; } #endif diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index f4227c6e110..a3ead66d417 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -70,7 +70,7 @@ void ws2811LedStripEnable(void); bool ws2811LedStripHardwareInit(ioTag_t ioTag); void ws2811LedStripDMAEnable(void); -void ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness); +bool ws2811UpdateStrip(ledStripFormatRGB_e ledFormat, uint8_t brightness); void setLedHsv(uint16_t index, const hsvColor_t *color); void getLedHsv(uint16_t index, hsvColor_t *color); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 2db40796a3c..7c2f64a8b0e 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -89,6 +89,12 @@ static uint8_t previousProfileColorIndex = COLOR_UNDEFINED; #define MAX_TIMER_DELAY (5 * 1000 * 1000) +#define TASK_LEDSTRIP_RATE_FAST_HZ 4000 + +#define LED_TASK_MARGIN 1 +// Decay the estimated max task duration by 1/(1 << LED_EXEC_TIME_SHIFT) on every invocation +#define LED_EXEC_TIME_SHIFT 5 + #define PROFILE_COLOR_UPDATE_INTERVAL_US 1e6 // normally updates when color changes but this is a 1 second forced update #define VISUAL_BEEPER_COLOR COLOR_WHITE @@ -150,6 +156,12 @@ void pgResetFn_ledStripConfig(ledStripConfig_t *ledStripConfig) # error "Led strip length must match driver" #endif +typedef enum { + LED_PROFILE_SLOW, + LED_PROFILE_FAST, + LED_PROFILE_ADVANCE +} ledProfileSequence_t; + const hsvColor_t *colors; const modeColorIndexes_t *modeColors; specialColorIndexes_t specialColors; @@ -1090,39 +1102,56 @@ void updateRequiredOverlay(void) disabledTimerMask |= !isOverlayTypeUsed(LED_OVERLAY_INDICATOR) << timIndicator; } -static void applyStatusProfile(timeUs_t now) +static ledProfileSequence_t applyStatusProfile(timeUs_t now) { + static timId_e timId = 0; + static uint32_t timActive = 0; + static bool fixedLayersApplied = false; + timeUs_t startTime = micros(); - // apply all layers; triggered timed functions has to update timers - // test all led timers, setting corresponding bits - uint32_t timActive = 0; - for (timId_e timId = 0; timId < timTimerCount; timId++) { - if (!(disabledTimerMask & (1 << timId))) { - // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. - const timeDelta_t delta = cmpTimeUs(now, timerVal[timId]); - // max delay is limited to 5s - if (delta < 0 && delta > -MAX_TIMER_DELAY) - continue; // not ready yet - timActive |= 1 << timId; - if (delta >= 100 * 1000 || delta < 0) { - timerVal[timId] = now; + if (!timActive) { + // apply all layers; triggered timed functions has to update timers + // test all led timers, setting corresponding bits + for (timId_e timId = 0; timId < timTimerCount; timId++) { + if (!(disabledTimerMask & (1 << timId))) { + // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. + const timeDelta_t delta = cmpTimeUs(now, timerVal[timId]); + // max delay is limited to 5s + if (delta < 0 && delta > -MAX_TIMER_DELAY) + continue; // not ready yet + timActive |= 1 << timId; + if (delta >= 100 * 1000 || delta < 0) { + timerVal[timId] = now; + } } } + + if (!timActive) { + return LED_PROFILE_SLOW; // no change this update, keep old state + } } - if (!timActive) { - // Call schedulerIgnoreTaskExecTime() unless data is being processed - schedulerIgnoreTaskExecTime(); - return; // no change this update, keep old state + if (!fixedLayersApplied) { + applyLedFixedLayers(); + fixedLayersApplied = true; } - applyLedFixedLayers(); - for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) { + for (; timId < ARRAYLEN(layerTable); timId++) { uint32_t *timer = &timerVal[timId]; bool updateNow = timActive & (1 << timId); (*layerTable[timId])(updateNow, timer); + if (cmpTimeUs(micros(), startTime) > LED_TARGET_UPDATE_US) { + // Come back and complete this quickly + return LED_PROFILE_FAST; + } } - ws2811UpdateStrip((ledStripFormatRGB_e) ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness); + + // Reset state for next iteration + timActive = 0; + fixedLayersApplied = false; + timId = 0; + + return LED_PROFILE_ADVANCE; } bool parseColor(int index, const char *colorConfig) @@ -1207,11 +1236,15 @@ void ledStripEnable(void) void ledStripDisable(void) { - ledStripEnabled = false; - previousProfileColorIndex = COLOR_UNDEFINED; + if (ledStripEnabled) { + ledStripEnabled = false; + previousProfileColorIndex = COLOR_UNDEFINED; - setStripColor(&HSV(BLACK)); - ws2811UpdateStrip((ledStripFormatRGB_e)ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness); + 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)); + } } void ledStripInit(void) @@ -1236,7 +1269,7 @@ static uint8_t selectVisualBeeperColor(uint8_t colorIndex) } } -static void applySimpleProfile(timeUs_t currentTimeUs) +static ledProfileSequence_t applySimpleProfile(timeUs_t currentTimeUs) { static timeUs_t colorUpdateTimeUs = 0; uint8_t colorIndex = COLOR_BLACK; @@ -1303,15 +1336,27 @@ static void applySimpleProfile(timeUs_t currentTimeUs) if ((colorIndex != previousProfileColorIndex) || (currentTimeUs >= colorUpdateTimeUs)) { setStripColor(&ledStripStatusModeConfig()->colors[colorIndex]); - ws2811UpdateStrip((ledStripFormatRGB_e)ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness); previousProfileColorIndex = colorIndex; colorUpdateTimeUs = currentTimeUs + PROFILE_COLOR_UPDATE_INTERVAL_US; + return LED_PROFILE_ADVANCE; } + + return LED_PROFILE_SLOW; } +timeUs_t executeTimeUs; void ledStripUpdate(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); + static uint16_t ledStateDurationFractionUs[2] = { 0 }; + static bool applyProfile = true; + static ledProfileSequence_t ledProfileSequence = LED_PROFILE_SLOW; + static uint8_t updateIterations = 0; + bool ledCurrentState = applyProfile; + + if (ledProfileSequence != LED_PROFILE_SLOW) { + updateIterations++; + schedulerIgnoreTaskExecRate(); + } if (!isWS2811LedStripReady()) { // Call schedulerIgnoreTaskExecTime() unless data is being processed @@ -1326,26 +1371,55 @@ void ledStripUpdate(timeUs_t currentTimeUs) } if (ledStripEnabled) { - switch (ledStripConfig()->ledstrip_profile) { + if (applyProfile) { + + switch (ledStripConfig()->ledstrip_profile) { #ifdef USE_LED_STRIP_STATUS_MODE - case LED_PROFILE_STATUS: { - applyStatusProfile(currentTimeUs); - break; - } + case LED_PROFILE_STATUS: { + ledProfileSequence = applyStatusProfile(currentTimeUs); + break; + } #endif - case LED_PROFILE_RACE: - case LED_PROFILE_BEACON: { - applySimpleProfile(currentTimeUs); - break; + case LED_PROFILE_RACE: + case LED_PROFILE_BEACON: { + ledProfileSequence = applySimpleProfile(currentTimeUs); + break; + } + + default: + break; } - default: - break; + if (ledProfileSequence != LED_PROFILE_SLOW) { + if (ledProfileSequence == LED_PROFILE_ADVANCE) { + applyProfile = false; + } + // Reschedule the fast update + rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_LEDSTRIP_RATE_FAST_HZ)); + } + } else { + // Profile is applied, so now update the LEDs + if (ws2811UpdateStrip((ledStripFormatRGB_e) ledStripConfig()->ledstrip_grb_rgb, ledStripConfig()->ledstrip_brightness)) { + applyProfile = true; + // Restore the default LED task rate + ledProfileSequence = LED_PROFILE_SLOW; + rescheduleTask(TASK_SELF, TASK_PERIOD_HZ(TASK_LEDSTRIP_RATE_HZ) - updateIterations * TASK_PERIOD_HZ(TASK_LEDSTRIP_RATE_FAST_HZ)); + updateIterations = 0; + } } - } else { - // Call schedulerIgnoreTaskExecTime() unless data is being processed - schedulerIgnoreTaskExecTime(); } + + if (!schedulerGetIgnoreTaskExecTime()) { + executeTimeUs = micros() - currentTimeUs; + if (executeTimeUs > (ledStateDurationFractionUs[ledCurrentState] >> LED_EXEC_TIME_SHIFT)) { + ledStateDurationFractionUs[ledCurrentState] = executeTimeUs << LED_EXEC_TIME_SHIFT; + } else if (ledStateDurationFractionUs[ledCurrentState] > 0) { + // Slowly decay the max time + ledStateDurationFractionUs[ledCurrentState]--; + } + } + + schedulerSetNextStateTime((ledStateDurationFractionUs[applyProfile] >> LED_EXEC_TIME_SHIFT) + LED_TASK_MARGIN); } uint8_t getLedProfile(void) diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index e21e9c65787..3052c0941e7 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -71,6 +71,8 @@ #define LED_XY_MASK 0x0F #define CALCULATE_LED_XY(x, y) ((((x) & LED_XY_MASK) << LED_X_BIT_OFFSET) | (((y) & LED_XY_MASK) << LED_Y_BIT_OFFSET)) +#define LED_TARGET_UPDATE_US 20 + typedef enum { COLOR_BLACK = 0, COLOR_WHITE, diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 5618e8fe003..71901c934bb 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -50,6 +50,8 @@ extern "C" { #include "sensors/battery.h" + #include "scheduler/scheduler.h" + #include "target.h" } @@ -314,7 +316,7 @@ void ws2811LedStripInit(ioTag_t ioTag) UNUSED(ioTag); } -void ws2811UpdateStrip(ledStripFormatRGB_e, uint8_t) {} +bool ws2811UpdateStrip(ledStripFormatRGB_e, uint8_t) {return true;} void setLedValue(uint16_t index, const uint8_t value) { @@ -406,7 +408,9 @@ void ws2811LedStripEnable(void) { } void setUsedLedCount(unsigned) { } void pinioBoxTaskControl(void) {} +void rescheduleTask(taskId_e, timeDelta_t){} void schedulerIgnoreTaskExecTime(void) {} +void schedulerIgnoreTaskExecRate(void) {} bool schedulerGetIgnoreTaskExecTime() { return false; } void schedulerSetNextStateTime(timeDelta_t) {} } diff --git a/src/test/unit/ws2811_unittest.cc b/src/test/unit/ws2811_unittest.cc index f0cb233edf6..15408a6d6d2 100644 --- a/src/test/unit/ws2811_unittest.cc +++ b/src/test/unit/ws2811_unittest.cc @@ -31,6 +31,8 @@ extern "C" { #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) {}