Split OSD element rendering into draw and display states (#13813)

src/main/osd/osd.c

@@ -60,6 +60,8 @@
 #include "drivers/sdcard.h"
 #include "drivers/time.h"
 
+#include "drivers/pinio.h"
+
 #include "fc/core.h"
 #include "fc/gps_lap_timer.h"
 #include "fc/rc_controls.h"
@@ -203,10 +205,10 @@ const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = {
 };
 
 #define OSD_TASK_MARGIN                 1
-#define OSD_ELEMENT_MARGIN              1
+#define OSD_ELEMENT_MARGIN              4
 
 // Decay the estimated max task duration by 1/(1 << OSD_EXEC_TIME_SHIFT) on every invocation
-#define OSD_EXEC_TIME_SHIFT             8
+#define OSD_EXEC_TIME_SHIFT             5
 
 // Format a float to the specified number of decimal places with optional rounding.
 // OSD symbols can optionally be placed before and after the formatted number (use SYM_NONE for no symbol).
@@ -1334,7 +1336,9 @@ typedef enum {
     OSD_STATE_UPDATE_ALARMS,
     OSD_STATE_REFRESH_PREARM,
     OSD_STATE_UPDATE_CANVAS,
-    OSD_STATE_UPDATE_ELEMENTS,
+    // Elements are handled in two steps, drawing into a buffer, and then sending to the display
+    OSD_STATE_DRAW_ELEMENT,
+    OSD_STATE_DISPLAY_ELEMENT,
     OSD_STATE_UPDATE_HEARTBEAT,
     OSD_STATE_COMMIT,
     OSD_STATE_TRANSFER,
@@ -1358,7 +1362,10 @@ bool osdUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
 
             // Determine time of next update
             if (osdUpdateDueUs) {
-                osdUpdateDueUs += OSD_UPDATE_INTERVAL_US;
+                // Ensure there's not a flurry of updates to catch up
+                while (cmpTimeUs(osdUpdateDueUs, currentTimeUs) < 0) {
+                    osdUpdateDueUs += OSD_UPDATE_INTERVAL_US;
+                }
             } else {
                 osdUpdateDueUs = currentTimeUs + OSD_UPDATE_INTERVAL_US;
             }
@@ -1373,6 +1380,8 @@ void osdUpdate(timeUs_t currentTimeUs)
 {
     static uint16_t osdStateDurationFractionUs[OSD_STATE_COUNT] = { 0 };
     static uint32_t osdElementDurationFractionUs[OSD_ITEM_COUNT] = { 0 };
+    static bool moreElementsToDraw;
+
     timeUs_t executeTimeUs;
     osdState_e osdCurrentState = osdState;
 
@@ -1493,21 +1502,19 @@ void osdUpdate(timeUs_t currentTimeUs)
         }
 #endif // USE_GPS
 
-        osdSyncBlink();
+        osdSyncBlink(currentTimeUs);
 
-        osdState = OSD_STATE_UPDATE_ELEMENTS;
+        osdState = OSD_STATE_DRAW_ELEMENT;
 
         break;
 
-    case OSD_STATE_UPDATE_ELEMENTS:
+    case OSD_STATE_DRAW_ELEMENT:
         {
-            bool moreElements = true;
-
             uint8_t osdElement = osdGetActiveElement();
 
             timeUs_t startElementTime = micros();
 
-            moreElements = osdDrawNextActiveElement(osdDisplayPort, startElementTime);
+            moreElementsToDraw = osdDrawNextActiveElement(osdDisplayPort);
 
             executeTimeUs = micros() - startElementTime;
 
@@ -1518,7 +1525,14 @@ void osdUpdate(timeUs_t currentTimeUs)
                 osdElementDurationFractionUs[osdElement]--;
             }
 
-            if (moreElements) {
+            if (osdIsRenderPending()) {
+                osdState = OSD_STATE_DISPLAY_ELEMENT;
+
+                // Render the element just drawn
+                break;
+            }
+
+            if (moreElementsToDraw) {
                 // There are more elements to draw
                 break;
             }
@@ -1534,6 +1548,29 @@ void osdUpdate(timeUs_t currentTimeUs)
         }
         break;
 
+    case OSD_STATE_DISPLAY_ELEMENT:
+        {
+            const bool pendingDisplay = osdDisplayActiveElement();
+
+            if (!pendingDisplay) {
+                if (moreElementsToDraw) {
+                    // There is no more to draw so advance to the next element
+                    osdState = OSD_STATE_DRAW_ELEMENT;
+                } else {
+                    // Displaying the current element is complete and there are no futher elements to draw so advance
+#ifdef USE_SPEC_PREARM_SCREEN
+                    if (!ARMING_FLAG(ARMED) && osdConfig()->osd_show_spec_prearm) {
+                        osdState = OSD_STATE_REFRESH_PREARM;
+                    } else
+#endif // USE_SPEC_PREARM_SCREEN
+                    {
+                        osdState = OSD_STATE_COMMIT;
+                    }
+                }
+            }
+        }
+        break;
+
 #ifdef USE_SPEC_PREARM_SCREEN
     case OSD_STATE_REFRESH_PREARM:
         if (osdDrawSpec(osdDisplayPort)) {
@@ -1588,7 +1625,7 @@ void osdUpdate(timeUs_t currentTimeUs)
 
     if (osdState == OSD_STATE_IDLE) {
         schedulerSetNextStateTime((osdStateDurationFractionUs[OSD_STATE_CHECK] >> OSD_EXEC_TIME_SHIFT));
-    } else if (osdState == OSD_STATE_UPDATE_ELEMENTS) {
+    } else if (osdState == OSD_STATE_DRAW_ELEMENT) {
         schedulerSetNextStateTime((osdElementDurationFractionUs[osdGetActiveElement()] >> OSD_EXEC_TIME_SHIFT) + OSD_ELEMENT_MARGIN);
     } else {
         schedulerSetNextStateTime((osdStateDurationFractionUs[osdState] >> OSD_EXEC_TIME_SHIFT) + OSD_TASK_MARGIN);

src/main/osd/osd_elements.c

@@ -138,6 +138,8 @@
 #include "drivers/time.h"
 #include "drivers/vtx_common.h"
 
+#include "drivers/pinio.h"
+
 #include "fc/controlrate_profile.h"
 #include "fc/core.h"
 #include "fc/gps_lap_timer.h"
@@ -237,6 +239,12 @@ static uint32_t blinkBits[(OSD_ITEM_COUNT + 31) / 32];
 #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
 #define BLINK(item) (IS_BLINK(item) && blinkState)
 
+// Current element and render status
+static osdElementParms_t activeElement;
+static bool displayPendingForeground;
+static bool displayPendingBackground;
+static char elementBuff[OSD_ELEMENT_BUFFER_LENGTH];
+
 // Return whether element is a SYS element and needs special handling
 #define IS_SYS_OSD_ELEMENT(item) (item >= OSD_SYS_GOGGLE_VOLTAGE) && (item <= OSD_SYS_FAN_SPEED)
 
@@ -287,20 +295,16 @@ static int getEscRpmFreq(int i)
 static void renderOsdEscRpmOrFreq(getEscRpmOrFreqFnPtr escFnPtr, osdElementParms_t *element)
 {
     static uint8_t motor = 0;
-    int x = element->elemPosX;
-    int y = element->elemPosY;
-    char rpmStr[6];
     const int rpm = MIN((*escFnPtr)(motor),99999);
-    tfp_sprintf(rpmStr, "%d", rpm);
+
-    osdDisplayWrite(element, x, y + motor, DISPLAYPORT_SEVERITY_NORMAL, rpmStr);
+    tfp_sprintf(element->buff, "%d", rpm);
+    element->elemOffsetY = motor;
 
     if (++motor == getMotorCount()) {
         motor = 0;
     } else {
         element->rendered = false;
     }
-
-    element->drawElement = false;
 }
 #endif
 
@@ -736,7 +740,12 @@ static void osdElementArtificialHorizon(osdElementParms_t *element)
 
     const int y = ((-rollAngle * x) / 64) - pitchAngle;
     if (y >= 0 && y <= 81) {
-        osdDisplayWriteChar(element, element->elemPosX + x, element->elemPosY + (y / AH_SYMBOL_COUNT), DISPLAYPORT_SEVERITY_NORMAL, (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
+        element->elemOffsetX = x;
+        element->elemOffsetY = y / AH_SYMBOL_COUNT;
+
+        tfp_sprintf(element->buff, "%c", (SYM_AH_BAR9_0 + (y % AH_SYMBOL_COUNT)));
+    } else {
+        element->drawElement = false;  // element does not need to be rendered
     }
 
     if (x == 4) {
@@ -747,8 +756,6 @@ static void osdElementArtificialHorizon(osdElementParms_t *element)
         element->rendered = false;
         x++;
     }
-
-    element->drawElement = false;  // element already drawn
 }
 
 static void osdElementUpDownReference(osdElementParms_t *element)
@@ -771,12 +778,11 @@ static void osdElementUpDownReference(osdElementParms_t *element)
             psiB = earthUpinBodyFrame[1]; // calculate the yaw w/re to zenith (use small angle approx for sine)
             direction = UP;
         }
-        int posX = element->elemPosX + lrintf(scaleRangef(psiB, -M_PIf / 4, M_PIf / 4, -14, 14));
+        element->elemOffsetX = lrintf(scaleRangef(psiB, -M_PIf / 4, M_PIf / 4, -14, 14));
-        int posY = element->elemPosY + lrintf(scaleRangef(thetaB, -M_PIf / 4, M_PIf / 4, -8, 8));
+        element->elemOffsetY = lrintf(scaleRangef(thetaB, -M_PIf / 4, M_PIf / 4, -8, 8));
 
-        osdDisplayWrite(element, posX, posY, DISPLAYPORT_SEVERITY_NORMAL, symbol[direction]);
+        tfp_sprintf(element->buff, "%c", symbol[direction]);
     }
-    element->drawElement = false;  // element already drawn
 }
 #endif // USE_ACC
 
@@ -831,6 +837,8 @@ static void osdBackgroundCameraFrame(osdElementParms_t *element)
         osdDisplayWriteChar(element, xpos, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
         osdDisplayWriteChar(element, xpos + width - 1, ypos + i, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
 
+        element->drawElement = false;  // element already drawn
+
         if (++i == height) {
             i = 1;
             renderPhase = BOTTOM;
@@ -844,15 +852,12 @@ static void osdBackgroundCameraFrame(osdElementParms_t *element)
         element->buff[width] = 0;  // string terminator
 
         if (renderPhase == TOP) {
-            osdDisplayWrite(element, xpos, ypos, DISPLAYPORT_SEVERITY_NORMAL, element->buff);
             renderPhase = MIDDLE;
         } else {
-            osdDisplayWrite(element, xpos, ypos + height - 1, DISPLAYPORT_SEVERITY_NORMAL, element->buff);
+            element->elemOffsetY = height - 1;
             renderPhase = TOP;
         }
     }
-
-    element->drawElement = false;  // element already drawn
 }
 
 static void toUpperCase(char* dest, const char* src, unsigned int maxSrcLength)
@@ -1504,17 +1509,14 @@ static void osdElementPower(osdElementParms_t *element)
 static void osdElementRcChannels(osdElementParms_t *element)
 {
     static uint8_t channel = 0;
-    const uint8_t xpos = element->elemPosX;
-    const uint8_t ypos = element->elemPosY;
 
     if (osdConfig()->rcChannels[channel] >= 0) {
         // Translate (1000, 2000) to (-1000, 1000)
         int data = scaleRange(rcData[osdConfig()->rcChannels[channel]], PWM_RANGE_MIN, PWM_RANGE_MAX, -1000, 1000);
         // Opt for the simplest formatting for now.
         // Decimal notation can be added when tfp_sprintf supports float among fancy options.
-        char fmtbuf[6];
+        tfp_sprintf(element->buff, "%5d", data);
-        tfp_sprintf(fmtbuf, "%5d", data);
+        element->elemOffsetY = channel;
-        osdDisplayWrite(element, xpos, ypos + channel, DISPLAYPORT_SEVERITY_NORMAL, fmtbuf);
     }
 
     if (++channel == OSD_RCCHANNELS_COUNT) {
@@ -1522,8 +1524,6 @@ static void osdElementRcChannels(osdElementParms_t *element)
     } else {
         element->rendered = false;
     }
-
-    element->drawElement = false;  // element already drawn
 }
 
 static void osdElementRemainingTimeEstimate(osdElementParms_t *element)
@@ -1596,12 +1596,12 @@ static void osdElementRsnr(osdElementParms_t *element)
 static void osdBackgroundStickOverlay(osdElementParms_t *element)
 {
     static enum {VERT, HORZ} renderPhase = VERT;
-    const uint8_t xpos = element->elemPosX;
-    const uint8_t ypos = element->elemPosY;
 
     if (renderPhase == VERT) {
         static uint8_t y = 0;
-        osdDisplayWriteChar(element, xpos + ((OSD_STICK_OVERLAY_WIDTH - 1) / 2), ypos + y, DISPLAYPORT_SEVERITY_NORMAL, SYM_STICK_OVERLAY_VERTICAL);
+        tfp_sprintf(element->buff, "%c", SYM_STICK_OVERLAY_VERTICAL);
+        element->elemOffsetX = ((OSD_STICK_OVERLAY_WIDTH - 1) / 2);
+        element->elemOffsetY = y;
 
         y++;
 
@@ -1623,19 +1623,14 @@ static void osdBackgroundStickOverlay(osdElementParms_t *element)
         element->buff[((OSD_STICK_OVERLAY_WIDTH - 1) / 2)] = SYM_STICK_OVERLAY_CENTER;
         element->buff[OSD_STICK_OVERLAY_WIDTH] = 0;  // string terminator
 
-        osdDisplayWrite(element, xpos, ypos + ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2), DISPLAYPORT_SEVERITY_NORMAL, element->buff);
+        element->elemOffsetY = ((OSD_STICK_OVERLAY_HEIGHT - 1) / 2);
 
         renderPhase = VERT;
     }
-
-    element->drawElement = false;  // element already drawn
 }
 
 static void osdElementStickOverlay(osdElementParms_t *element)
 {
-    const uint8_t xpos = element->elemPosX;
-    const uint8_t ypos = element->elemPosY;
-
     // Now draw the cursor
     rc_alias_e vertical_channel, horizontal_channel;
 
@@ -1651,9 +1646,9 @@ static void osdElementStickOverlay(osdElementParms_t *element)
     const uint8_t cursorY = OSD_STICK_OVERLAY_VERTICAL_POSITIONS - 1 - scaleRange(constrain(rcData[vertical_channel], PWM_RANGE_MIN, PWM_RANGE_MAX - 1), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, OSD_STICK_OVERLAY_VERTICAL_POSITIONS);
     const char cursor = SYM_STICK_OVERLAY_SPRITE_HIGH + (cursorY % OSD_STICK_OVERLAY_SPRITE_HEIGHT);
 
-    osdDisplayWriteChar(element, xpos + cursorX, ypos + cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT, DISPLAYPORT_SEVERITY_NORMAL, cursor);
+    tfp_sprintf(element->buff, "%c", cursor);
-
+    element->elemOffsetX = cursorX;
-    element->drawElement = false;  // element already drawn
+    element->elemOffsetY = cursorY / OSD_STICK_OVERLAY_SPRITE_HEIGHT;
 }
 #endif // USE_OSD_STICK_OVERLAY
 
@@ -2110,30 +2105,30 @@ static bool osdDrawSingleElement(displayPort_t *osdDisplayPort, uint8_t item)
 
     uint8_t elemPosX = OSD_X(osdElementConfig()->item_pos[item]);
     uint8_t elemPosY = OSD_Y(osdElementConfig()->item_pos[item]);
-    char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
 
-    osdElementParms_t element;
+    activeElement.item = item;
-    element.item = item;
+    activeElement.elemPosX = elemPosX;
-    element.elemPosX = elemPosX;
+    activeElement.elemPosY = elemPosY;
-    element.elemPosY = elemPosY;
+    activeElement.elemOffsetX = 0;
-    element.type = OSD_TYPE(osdElementConfig()->item_pos[item]);
+    activeElement.elemOffsetY = 0;
-    element.buff = (char *)&buff;
+    activeElement.type = OSD_TYPE(osdElementConfig()->item_pos[item]);
-    element.osdDisplayPort = osdDisplayPort;
+    activeElement.buff = elementBuff;
-    element.drawElement = true;
+    activeElement.osdDisplayPort = osdDisplayPort;
-    element.rendered = true;
+    activeElement.drawElement = true;
-    element.attr = DISPLAYPORT_SEVERITY_NORMAL;
+    activeElement.rendered = true;
+    activeElement.attr = DISPLAYPORT_SEVERITY_NORMAL;
 
     // Call the element drawing function
     if (IS_SYS_OSD_ELEMENT(item)) {
         displaySys(osdDisplayPort, elemPosX, elemPosY, (displayPortSystemElement_e)(item - OSD_SYS_GOGGLE_VOLTAGE + DISPLAYPORT_SYS_GOGGLE_VOLTAGE));
     } else {
-        osdElementDrawFunction[item](&element);
+        osdElementDrawFunction[item](&activeElement);
-        if (element.drawElement) {
+        if (activeElement.drawElement) {
-            osdDisplayWrite(&element, elemPosX, elemPosY, element.attr, buff);
+            displayPendingForeground = true;
         }
     }
 
-    return element.rendered;
+    return activeElement.rendered;
 }
 
 static bool osdDrawSingleElementBackground(displayPort_t *osdDisplayPort, uint8_t item)
@@ -2145,32 +2140,37 @@ static bool osdDrawSingleElementBackground(displayPort_t *osdDisplayPort, uint8_
 
     uint8_t elemPosX = OSD_X(osdElementConfig()->item_pos[item]);
     uint8_t elemPosY = OSD_Y(osdElementConfig()->item_pos[item]);
-    char buff[OSD_ELEMENT_BUFFER_LENGTH] = "";
 
-    osdElementParms_t element;
+    activeElement.item = item;
-    element.item = item;
+    activeElement.elemPosX = elemPosX;
-    element.elemPosX = elemPosX;
+    activeElement.elemPosY = elemPosY;
-    element.elemPosY = elemPosY;
+    activeElement.elemOffsetX = 0;
-    element.type = OSD_TYPE(osdElementConfig()->item_pos[item]);
+    activeElement.elemOffsetY = 0;
-    element.buff = (char *)&buff;
+    activeElement.type = OSD_TYPE(osdElementConfig()->item_pos[item]);
-    element.osdDisplayPort = osdDisplayPort;
+    activeElement.buff = elementBuff;
-    element.rendered = true;
+    activeElement.osdDisplayPort = osdDisplayPort;
-    element.drawElement = true;
+    activeElement.rendered = true;
+    activeElement.drawElement = true;
 
     // Call the element background drawing function
-    osdElementBackgroundFunction[item](&element);
+    osdElementBackgroundFunction[item](&activeElement);
-    if (element.drawElement) {
+    if (activeElement.drawElement) {
-        osdDisplayWrite(&element, elemPosX, elemPosY, DISPLAYPORT_SEVERITY_NORMAL, buff);
+        displayPendingBackground = true;
     }
 
-    return element.rendered;
+    return activeElement.rendered;
 }
 
-static uint8_t activeElement = 0;
+static uint8_t activeElementNumber = 0;
+
+bool osdIsRenderPending(void)
+{
+    return displayPendingForeground | displayPendingBackground;
+}
 
 uint8_t osdGetActiveElement(void)
 {
-    return activeElement;
+    return activeElementNumber;
 }
 
 uint8_t osdGetActiveElementCount(void)
@@ -2178,36 +2178,78 @@ uint8_t osdGetActiveElementCount(void)
     return activeOsdElementCount;
 }
 
-// Return true if there are more elements to draw
+// Return true if there is more to display
-bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs)
+bool osdDisplayActiveElement(void)
 {
-    UNUSED(currentTimeUs);
+    if (activeElementNumber >= activeOsdElementCount) {
-    static bool backgroundRendered = false;
-    bool retval = true;
-
-    if (activeElement >= activeOsdElementCount) {
         return false;
     }
 
-    if (!backgroundLayerSupported && !backgroundRendered) {
+    // If there's a previously drawn background string to be displayed, do that
+    if (displayPendingBackground) {
+        osdDisplayWrite(&activeElement,
+                        activeElement.elemPosX + activeElement.elemOffsetX,
+                        activeElement.elemPosY + activeElement.elemOffsetY,
+                        activeElement.attr, activeElement.buff);
+
+        activeElement.buff[0] = '\0';
+
+        displayPendingBackground = false;
+
+        return displayPendingForeground;
+    }
+
+    // If there's a previously drawn foreground string to be displayed, do that
+    if (displayPendingForeground) {
+        osdDisplayWrite(&activeElement,
+                        activeElement.elemPosX + activeElement.elemOffsetX,
+                        activeElement.elemPosY + activeElement.elemOffsetY,
+                        activeElement.attr, activeElement.buff);
+
+        activeElement.buff[0] = '\0';
+
+        displayPendingForeground = false;
+    }
+
+    return false;
+}
+
+// Return true if there are more elements to draw
+bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort)
+{
+    static bool backgroundRendered = false;
+
+    if (activeElementNumber >= activeOsdElementCount) {
+        activeElementNumber = 0;
+        return false;
+    }
+
+    uint8_t item = activeOsdElementArray[activeElementNumber];
+
+    if (!backgroundLayerSupported && osdElementBackgroundFunction[item] && !backgroundRendered) {
         // If the background layer isn't supported then we
         // have to draw the element's static layer as well.
-        backgroundRendered = osdDrawSingleElementBackground(osdDisplayPort, activeOsdElementArray[activeElement]);
+        backgroundRendered = osdDrawSingleElementBackground(osdDisplayPort, item);
 
-        return retval;
+        // After the background always come back to check for foreground
+        return true;
     }
 
     // Only advance to the next element if rendering is complete
-    if (osdDrawSingleElement(osdDisplayPort, activeOsdElementArray[activeElement])) {
+    if (osdDrawSingleElement(osdDisplayPort, item)) {
-        // Prepare to render the background of the next element
+        // If rendering is complete then advance to the next element
-        backgroundRendered = false;
+        if (activeElement.rendered) {
-        if (++activeElement >= activeOsdElementCount) {
+            // Prepare to render the background of the next element
-            activeElement = 0;
+            backgroundRendered = false;
-            retval = false;
+
+            if (++activeElementNumber >= activeOsdElementCount) {
+                activeElementNumber = 0;
+                return false;
+            }
         }
     }
 
-    return retval;
+    return true;
 }
 
 #ifdef USE_SPEC_PREARM_SCREEN
@@ -2319,16 +2361,11 @@ void osdElementsInit(bool backgroundLayerFlag)
     pt1FilterInit(&batteryEfficiencyFilt, pt1FilterGain(EFFICIENCY_CUTOFF_HZ, 1.0f / osdConfig()->framerate_hz));
 }
 
-void osdSyncBlink(void)
+void osdSyncBlink(timeUs_t currentTimeUs)
 {
-    static int blinkCount = 0;
+    const int period = 1000000/OSD_BLINK_FREQUENCY_HZ;
 
-    // If the OSD blink is due a transition, do so
+    blinkState = ((currentTimeUs % period) < (period >> 1));
-    // Task runs at osdConfig()->framerate_hz Hz, so this will cycle at 2Hz
-    if (++blinkCount == ((osdConfig()->framerate_hz / OSD_BLINK_FREQUENCY_HZ) / 2)) {
-        blinkCount = 0;
-        blinkState = !blinkState;
-    }
 }
 
 void osdResetAlarms(void)

src/main/osd/osd_elements.h

@@ -35,6 +35,8 @@ typedef struct osdElementParms_s {
     uint8_t item;
     uint8_t elemPosX;
     uint8_t elemPosY;
+    uint8_t elemOffsetX;
+    uint8_t elemOffsetY;
     osdElementType_e type;
     char *buff;
     displayPort_t *osdDisplayPort;
@@ -56,12 +58,14 @@ int32_t osdGetSpeedToSelectedUnit(int32_t value);
 char osdGetSpeedToSelectedUnitSymbol(void);
 char osdGetTemperatureSymbolForSelectedUnit(void);
 void osdAddActiveElements(void);
-uint8_t osdGetActiveElement();
+bool osdIsRenderPending(void);
-uint8_t osdGetActiveElementCount();
+uint8_t osdGetActiveElement(void);
-bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs);
+uint8_t osdGetActiveElementCount(void);
+bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort);
+bool osdDisplayActiveElement(void);
 void osdDrawActiveElementsBackground(displayPort_t *osdDisplayPort);
 void osdElementsInit(bool backgroundLayerFlag);
-void osdSyncBlink();
+void osdSyncBlink(timeUs_t currentTimeUs);
 void osdResetAlarms(void);
 void osdUpdateAlarms(void);
 bool osdElementsNeedAccelerometer(void);

src/main/scheduler/scheduler.c

@@ -493,6 +493,7 @@ FAST_CODE void scheduler(void)
     static uint64_t gyroCyclesTotal = 0;
     static float devSquared = 0.0f;
 #endif
+
 #if !defined(UNIT_TEST)
     const timeUs_t schedulerStartTimeUs = micros();
 #endif
@@ -503,6 +504,7 @@ FAST_CODE void scheduler(void)
     uint16_t selectedTaskDynamicPriority = 0;
     uint32_t nextTargetCycles = 0;
     int32_t schedLoopRemainingCycles;
+    bool firstSchedulingOpportunity = false;
 
 #if defined(UNIT_TEST)
     if (nextTargetCycles == 0) {
@@ -571,6 +573,9 @@ FAST_CODE void scheduler(void)
                 lastFailsafeCheckMs = millis();
             }
 
+            // This is the first scheduling opportunity after the realtime tasks have run
+            firstSchedulingOpportunity = true;
+
 #if defined(USE_LATE_TASK_STATISTICS)
             gyroCyclesNow = cmpTimeCycles(nowCycles, lastTargetCycles);
             gyroCyclesTotal += gyroCyclesNow;
@@ -765,7 +770,7 @@ FAST_CODE void scheduler(void)
             // Allow a little extra time
             taskRequiredTimeCycles += taskGuardCycles;
 
-            if (!gyroEnabled || (taskRequiredTimeCycles < schedLoopRemainingCycles)) {
+            if (!gyroEnabled || firstSchedulingOpportunity || (taskRequiredTimeCycles < schedLoopRemainingCycles)) {
                 uint32_t antipatedEndCycles = nowCycles + taskRequiredTimeCycles;
                 taskExecutionTimeUs += schedulerExecuteTask(selectedTask, currentTimeUs);
                 nowCycles = getCycleCounter();

src/test/unit/link_quality_unittest.cc

@@ -426,7 +426,7 @@ TEST(LQTest, TestLQAlarm)
     // elements showing values in alarm range should flash
     simulationTime += 1000000;
     simulationTime -= simulationTime % 1000000;
-    startTime = simulationTime;
+    startTime = simulationTime + 0.25e6;
     for (int i = 0; i < 15; i++) {
         // Blinking should happen at 2Hz
         simulationTime = startTime + i*0.25e6;
@@ -438,6 +438,7 @@ TEST(LQTest, TestLQAlarm)
 #ifdef DEBUG_OSD
         displayPortTestPrint();
 #endif
+
         if (i % 2 == 0) {
             displayPortTestBufferSubstring(8,  1, "%c5", SYM_LINK_QUALITY);
         } else {

src/test/unit/osd_unittest.cc

@@ -510,7 +510,7 @@ TEST_F(OsdTest, TestStatsTiming)
     // statistics screen should display the following
     int row = 7;
     displayPortTestBufferSubstring(2, row++, "2017-11-19 10:12:");
-    displayPortTestBufferSubstring(2, row++, "TOTAL ARM         : 00:13.61");
+    displayPortTestBufferSubstring(2, row++, "TOTAL ARM         : 00:13.60");
     displayPortTestBufferSubstring(2, row++, "LAST ARM          : 00:01");
 }