diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 1bf0da2add..a69cb94392 100644 --- a/src/main/osd/osd.c +++ b/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); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index fef1f63c53..349b7397dd 100644 --- a/src/main/osd/osd_elements.c +++ b/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)); - int posY = element->elemPosY + lrintf(scaleRangef(thetaB, -M_PIf / 4, M_PIf / 4, -8, 8)); + element->elemOffsetX = lrintf(scaleRangef(psiB, -M_PIf / 4, M_PIf / 4, -14, 14)); + 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(fmtbuf, "%5d", data); - osdDisplayWrite(element, xpos, ypos + channel, DISPLAYPORT_SEVERITY_NORMAL, fmtbuf); + tfp_sprintf(element->buff, "%5d", data); + element->elemOffsetY = channel; } 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); - - element->drawElement = false; // element already drawn + tfp_sprintf(element->buff, "%c", cursor); + element->elemOffsetX = cursorX; + 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; - element.item = item; - element.elemPosX = elemPosX; - element.elemPosY = elemPosY; - element.type = OSD_TYPE(osdElementConfig()->item_pos[item]); - element.buff = (char *)&buff; - element.osdDisplayPort = osdDisplayPort; - element.drawElement = true; - element.rendered = true; - element.attr = DISPLAYPORT_SEVERITY_NORMAL; + activeElement.item = item; + activeElement.elemPosX = elemPosX; + activeElement.elemPosY = elemPosY; + activeElement.elemOffsetX = 0; + activeElement.elemOffsetY = 0; + activeElement.type = OSD_TYPE(osdElementConfig()->item_pos[item]); + activeElement.buff = elementBuff; + activeElement.osdDisplayPort = osdDisplayPort; + activeElement.drawElement = true; + 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); - if (element.drawElement) { - osdDisplayWrite(&element, elemPosX, elemPosY, element.attr, buff); + osdElementDrawFunction[item](&activeElement); + if (activeElement.drawElement) { + 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; - element.item = item; - element.elemPosX = elemPosX; - element.elemPosY = elemPosY; - element.type = OSD_TYPE(osdElementConfig()->item_pos[item]); - element.buff = (char *)&buff; - element.osdDisplayPort = osdDisplayPort; - element.rendered = true; - element.drawElement = true; + activeElement.item = item; + activeElement.elemPosX = elemPosX; + activeElement.elemPosY = elemPosY; + activeElement.elemOffsetX = 0; + activeElement.elemOffsetY = 0; + activeElement.type = OSD_TYPE(osdElementConfig()->item_pos[item]); + activeElement.buff = elementBuff; + activeElement.osdDisplayPort = osdDisplayPort; + activeElement.rendered = true; + activeElement.drawElement = true; // Call the element background drawing function - osdElementBackgroundFunction[item](&element); - if (element.drawElement) { - osdDisplayWrite(&element, elemPosX, elemPosY, DISPLAYPORT_SEVERITY_NORMAL, buff); + osdElementBackgroundFunction[item](&activeElement); + if (activeElement.drawElement) { + 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 -bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs) +// Return true if there is more to display +bool osdDisplayActiveElement(void) { - UNUSED(currentTimeUs); - static bool backgroundRendered = false; - bool retval = true; - - if (activeElement >= activeOsdElementCount) { + if (activeElementNumber >= 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])) { - // Prepare to render the background of the next element - backgroundRendered = false; - if (++activeElement >= activeOsdElementCount) { - activeElement = 0; - retval = false; + if (osdDrawSingleElement(osdDisplayPort, item)) { + // If rendering is complete then advance to the next element + if (activeElement.rendered) { + // Prepare to render the background of the next element + backgroundRendered = 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 - // 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; - } + blinkState = ((currentTimeUs % period) < (period >> 1)); } void osdResetAlarms(void) diff --git a/src/main/osd/osd_elements.h b/src/main/osd/osd_elements.h index 358d1661f8..9f08c5ee33 100644 --- a/src/main/osd/osd_elements.h +++ b/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(); -uint8_t osdGetActiveElementCount(); -bool osdDrawNextActiveElement(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs); +bool osdIsRenderPending(void); +uint8_t osdGetActiveElement(void); +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); diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index be4ccf3058..ffac17ec45 100644 --- a/src/main/scheduler/scheduler.c +++ b/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(); diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index b61a29caec..05d4b90eaa 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/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 { diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 7336ea3829..13f688afd6 100644 --- a/src/test/unit/osd_unittest.cc +++ b/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"); }