Break LED_STRIP update into 20us chunks (betaflight#13218)

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

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);

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)

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,

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) {}
 }

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) {}