From 71c92a8de2fa9ca5c0fa8b29b80f3580315758ff Mon Sep 17 00:00:00 2001 From: ywh <2227158009@qq.com> Date: Sat, 15 Aug 2026 20:17:37 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A4=8D=E6=AE=B5=E9=97=B4=E5=9B=BA=E5=AE=9A?= =?UTF-8?q?=E5=A4=9A=E5=87=BA=207=EF=BD=9E8=20ms=20=E7=9A=84=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PLSR/Src/plsr.c | 603 +++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 572 insertions(+), 31 deletions(-) diff --git a/PLSR/Src/plsr.c b/PLSR/Src/plsr.c index 638ab49..df9456e 100644 --- a/PLSR/Src/plsr.c +++ b/PLSR/Src/plsr.c @@ -62,6 +62,8 @@ #define PLSR_PROFILE_STARTUP_BUDGET (400U) #define PLSR_PROFILE_REFILL_TARGET (800U) #define PLSR_PROFILE_REFILL_BUDGET (200U) +#define PLSR_TIMED_START_BUILD_BUDGET (32U) +#define PLSR_TIMED_START_READY_ITEMS PLSR_PROFILE_STARTUP_BUDGET #define PLSR_PROFILE_LOW_WATER (100U) #define PLSR_PROFILE_LOW_WATER_CLEAR (200U) #define PLSR_UNDERRUN_HOLD_TIME_US (2000UL) @@ -141,6 +143,28 @@ typedef struct volatile uint8_t generatorComplete; } PLSR_PROFILE_QUEUE; +/* A WAIT_TIME boundary is a cold start, but its planner work is predictable + while the source segment is still running. Build that next stream in the + inactive queue bank and publish it only after the startup horizon is ready. + The boundary path then contains no planner loop. */ +typedef struct +{ + PLSR_PROFILE_ENTRY firstRun; + uint64_t magnitude; + uint32_t sourceEpoch; + uint32_t queueGeneration; + uint16_t generatedItems; + uint8_t sourceSegment; + uint8_t nextSegment; + uint8_t positive; + uint8_t directionLevel; + uint8_t directionChanged; + uint8_t queueBank; + volatile uint8_t building; + volatile uint8_t valid; + volatile uint8_t pendingActivation; +} PLSR_TIMED_START; + typedef struct { uint64_t magnitude; @@ -260,6 +284,7 @@ static PLSR_SHORT_PROFILE PlsrShortProfile; static PLSR_PROFILE_QUEUE PlsrProfileQueues[2]; static volatile uint8_t PlsrProfileQueueBank; #define PlsrProfileQueue (PlsrProfileQueues[PlsrProfileQueueBank]) +static PLSR_TIMED_START PlsrTimedStart; static PLSR_HANDOFF_PLAN PlsrHandoffPlan; static volatile uint64_t PlsrCountedObservedPublished; static volatile uint8_t PlsrCountedHandoffStaged; @@ -359,7 +384,8 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, uint8_t segmentNumber, uint32_t startFrequencyHz, uint32_t targetFrequencyHz, - uint64_t pulseCount); + uint64_t pulseCount, + uint8_t currentPositive); static uint8_t PlsrShortProfileTakeRun(PLSR_SHORT_PROFILE *profile, PLSR_PROFILE_ENTRY *entry); static uint8_t PlsrShortProfileTakeRunLimited( @@ -372,6 +398,12 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( uint32_t targetHz, uint32_t totalPulses); static void PlsrProfileQueueReset(void); +static void PlsrProfileQueueResetBankLocked(uint8_t bank); +static void PlsrInvalidateTimedStartLocked(void); +static void PlsrServiceTimedStartPreparation(void); +static PLSR_PLATFORM_SERVICE_RESULT PlsrTryStartPreparedTimedSegment( + uint8_t segmentNumber); +static uint8_t PlsrStartPreparedTimedOutput(void); static void PlsrProfileQueueInvalidateGeneration(void); static uint8_t PlsrProfileQueueBegin( const PLSR_SHORT_PROFILE *producerProfile, @@ -1702,13 +1734,13 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, uint8_t segmentNumber, uint32_t startFrequencyHz, uint32_t targetFrequencyHz, - uint64_t pulseCount) + uint64_t pulseCount, + uint8_t currentPositive) { PLSR_MOTION_BLOCK block; PLSR_PLANNER_STATUS status; const PLSR_SEGMENT_CONFIG *segment; uint8_t nextSegment = 0U; - uint8_t currentPositive; uint8_t nextPositive; (void)memset(profile, 0, sizeof(*profile)); @@ -1741,7 +1773,6 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, } if (nextSegment != 0U) { - currentPositive = PlsrCountPositive; if (PlsrActiveConfig.positionMode == PLSR_POSITION_RELATIVE) { currentPositive = (segment->pulses >= 0L) ? 1U : 0U; @@ -1749,11 +1780,12 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, (PlsrActiveConfig.segments[nextSegment - 1U].pulses >= 0L) ? 1U : 0U; } - else if ((segmentNumber == PlsrCurrentSegment) - && (PlsrPredictNextDirection(nextSegment, - &nextPositive) != 0U)) + else if ((int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses + != (int64_t)segment->pulses) { - /* direction resolved from the live absolute position */ + nextPositive = + ((int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses + > (int64_t)segment->pulses) ? 1U : 0U; } else { @@ -1915,6 +1947,8 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( PlsrPlatformExitCritical(criticalState); return PLSR_PLATFORM_SERVICE_DEFERRED; } + /* The replacement stream owns the inactive bank from this point. */ + PlsrInvalidateTimedStartLocked(); PlsrPlatformExitCritical(criticalState); if (totalPulses < committedPulses) @@ -1993,7 +2027,8 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( if ((tailFrequencyHz == 0UL) || (PlsrPrepareShortProfile(&replacement, segmentNumber, tailFrequencyHz, targetHz, - replacementPulses) == 0U)) + replacementPulses, + PlsrCountPositive) == 0U)) { return PLSR_PLATFORM_SERVICE_FAILED; } @@ -2145,25 +2180,40 @@ static void PlsrProfileRecordPlannerStatus( } } +static void PlsrInvalidateTimedStartLocked(void) +{ + PlsrTimedStart.building = 0U; + PlsrTimedStart.valid = 0U; + PlsrTimedStart.pendingActivation = 0U; +} + +static void PlsrProfileQueueResetBankLocked(uint8_t bank) +{ + PLSR_PROFILE_QUEUE *queue = &PlsrProfileQueues[bank & 1U]; + + queue->generation++; + queue->readIndex = 0U; + queue->writeIndex = 0U; + queue->active = 0U; + queue->generatorComplete = 0U; + queue->producerEpoch = 0UL; + queue->repeatRemaining = 0UL; + queue->underrunDebtPulses = 0UL; + queue->producerSegment = 0U; + queue->preparedHandoffBank = 0U; + queue->haveLastSetting = 0U; + queue->producerProfile.active = 0U; +} + static void PlsrProfileQueueReset(void) { uint32_t criticalState = PlsrPlatformEnterCritical(); uint8_t bank; + PlsrInvalidateTimedStartLocked(); for (bank = 0U; bank < 2U; bank++) { - PlsrProfileQueues[bank].generation++; - PlsrProfileQueues[bank].readIndex = 0U; - PlsrProfileQueues[bank].writeIndex = 0U; - PlsrProfileQueues[bank].active = 0U; - PlsrProfileQueues[bank].generatorComplete = 0U; - PlsrProfileQueues[bank].producerEpoch = 0UL; - PlsrProfileQueues[bank].repeatRemaining = 0UL; - PlsrProfileQueues[bank].underrunDebtPulses = 0UL; - PlsrProfileQueues[bank].producerSegment = 0U; - PlsrProfileQueues[bank].preparedHandoffBank = 0U; - PlsrProfileQueues[bank].haveLastSetting = 0U; - PlsrProfileQueues[bank].producerProfile.active = 0U; + PlsrProfileQueueResetBankLocked(bank); } PlsrProfileQueueBank = 0U; PlsrProfileQueueLowWaterLatched = 0U; @@ -2273,6 +2323,251 @@ static uint8_t PlsrProfileQueueAppendLocked( return 1U; } +static void PlsrServiceTimedStartPreparation(void) +{ + PLSR_PROFILE_QUEUE *queue; + PLSR_SHORT_PROFILE profile; + PLSR_PROFILE_ENTRY entry; + PLSR_HANDOFF_PLAN *handoff; + const PLSR_SEGMENT_CONFIG *source; + uint32_t criticalState; + uint32_t sourceEpoch; + uint32_t queueGeneration; + uint32_t targetFrequencyHz; + uint32_t startFrequencyHz; + uint64_t remaining; + int64_t predictedPosition; + int64_t displacement; + uint64_t magnitude; + int32_t position; + uint16_t budget; + uint8_t sourceSegment; + uint8_t nextSegment; + uint8_t positive; + uint8_t directionLevel; + uint8_t directionChanged; + uint8_t queueBank; + + if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) + || (PlsrStopRequested != 0U) + || (PlsrTimedStart.pendingActivation != 0U)) + { + return; + } + + criticalState = PlsrPlatformEnterCritical(); + sourceEpoch = PlsrSegmentEpoch; + sourceSegment = PlsrCurrentSegment; + if ((sourceSegment == 0U) + || (sourceSegment > PlsrActiveConfig.segmentCount) + || (PlsrActiveConfig.segments[sourceSegment - 1U].waitType + != PLSR_WAIT_TIME)) + { + PlsrInvalidateTimedStartLocked(); + PlsrPlatformExitCritical(criticalState); + return; + } + if ((PlsrTimedStart.building != 0U) + || (PlsrTimedStart.valid != 0U)) + { + if ((PlsrTimedStart.sourceEpoch != sourceEpoch) + || (PlsrTimedStart.sourceSegment != sourceSegment) + || (PlsrTimedStart.queueBank == PlsrProfileQueueBank) + || (PlsrProfileQueues[PlsrTimedStart.queueBank].generation + != PlsrTimedStart.queueGeneration)) + { + PlsrInvalidateTimedStartLocked(); + } + else + { + PlsrPlatformExitCritical(criticalState); + if (PlsrTimedStart.valid != 0U) + { + return; + } + goto fill_timed_start; + } + } + position = PlsrPosition; + remaining = PlsrRemainingPulses; + positive = PlsrCountPositive; + PlsrPlatformExitCritical(criticalState); + + if (PlsrGetNextSegment(&nextSegment) == 0U) + { + return; + } + source = &PlsrActiveConfig.segments[sourceSegment - 1U]; + if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE) + { + predictedPosition = source->pulses; + } + else if (positive != 0U) + { + predictedPosition = (int64_t)position + (int64_t)remaining; + } + else + { + predictedPosition = (int64_t)position - (int64_t)remaining; + } + if ((predictedPosition > (int64_t)INT32_MAX) + || (predictedPosition < (int64_t)INT32_MIN)) + { + return; + } + displacement = PlsrSegmentDisplacement( + nextSegment, (int32_t)predictedPosition); + if (displacement == 0) + { + return; + } + positive = (displacement > 0) ? 1U : 0U; + magnitude = (displacement < 0) ? (uint64_t)(-displacement) + : (uint64_t)displacement; + if (magnitude > 0xFFFFFFFFULL) + { + return; + } + directionLevel = positive; + if (PlsrActiveConfig.directionNegativeLogic != 0U) + { + directionLevel ^= 1U; + } + directionChanged = ((PlsrLastDirectionValid == 0U) + || (PlsrLastDirectionOutput + != (uint8_t)PlsrActiveConfig.directionOutput) + || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U; + targetFrequencyHz = + PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz; + startFrequencyHz = PlsrEffectiveStartFrequency( + targetFrequencyHz, 0U, directionChanged, 0UL); + if ((PlsrPrepareShortProfile(&profile, nextSegment, + startFrequencyHz, targetFrequencyHz, + magnitude, positive) == 0U) + || (PlsrShortProfileTakeRun(&profile, &entry) == 0U)) + { + return; + } + + /* Keep the seamless plan for this future segment consistent with its + possibly clipped terminal frequency. */ + handoff = &PlsrPreparedHandoffPlans[PlsrPreparedHandoffBank] + [nextSegment - 1U]; + (void)PlsrBuildHandoffPlan(nextSegment, &PlsrActiveConfig, + profile.endHz, handoff); + + criticalState = PlsrPlatformEnterCritical(); + if ((PlsrSegmentEpoch != sourceEpoch) + || (PlsrCurrentSegment != sourceSegment)) + { + PlsrPlatformExitCritical(criticalState); + return; + } + queueBank = (uint8_t)(PlsrProfileQueueBank ^ 1U); + PlsrProfileQueueResetBankLocked(queueBank); + queue = &PlsrProfileQueues[queueBank]; + queue->producerEpoch = sourceEpoch + 1UL; + queue->producerSegment = nextSegment; + queue->preparedHandoffBank = PlsrPreparedHandoffBank; + queue->repeatRemaining = 0UL; + queue->underrunDebtPulses = 0UL; + queue->haveLastSetting = 0U; + PlsrCopyShortProfile(&queue->producerProfile, &profile); + queue->active = 1U; + queue->generatorComplete = (profile.active == 0U) ? 1U : 0U; + + PlsrTimedStart.firstRun = entry; + PlsrTimedStart.magnitude = magnitude; + PlsrTimedStart.sourceEpoch = sourceEpoch; + PlsrTimedStart.queueGeneration = queue->generation; + PlsrTimedStart.generatedItems = 0U; + PlsrTimedStart.sourceSegment = sourceSegment; + PlsrTimedStart.nextSegment = nextSegment; + PlsrTimedStart.positive = positive; + PlsrTimedStart.directionLevel = directionLevel; + PlsrTimedStart.directionChanged = directionChanged; + PlsrTimedStart.queueBank = queueBank; + PlsrTimedStart.valid = (queue->generatorComplete != 0U) ? 1U : 0U; + PlsrTimedStart.building = (PlsrTimedStart.valid == 0U) ? 1U : 0U; + PlsrTimedStart.pendingActivation = 0U; + PlsrPlatformExitCritical(criticalState); + PlsrProfileRecordPlannerStatus(&profile); + +fill_timed_start: + budget = PLSR_TIMED_START_BUILD_BUDGET; + while (budget != 0U) + { + criticalState = PlsrPlatformEnterCritical(); + queueBank = PlsrTimedStart.queueBank; + queue = &PlsrProfileQueues[queueBank]; + queueGeneration = PlsrTimedStart.queueGeneration; + if ((PlsrTimedStart.building == 0U) + || (PlsrTimedStart.sourceEpoch != PlsrSegmentEpoch) + || (PlsrTimedStart.sourceSegment != PlsrCurrentSegment) + || (queueBank == PlsrProfileQueueBank) + || (queue->generation != queueGeneration)) + { + PlsrInvalidateTimedStartLocked(); + PlsrPlatformExitCritical(criticalState); + return; + } + PlsrCopyShortProfile(&profile, &queue->producerProfile); + PlsrPlatformExitCritical(criticalState); + + if (PlsrShortProfileTakeRun(&profile, &entry) == 0U) + { + criticalState = PlsrPlatformEnterCritical(); + if ((PlsrTimedStart.building != 0U) + && (queue->generation == queueGeneration)) + { + queue->generatorComplete = 1U; + PlsrTimedStart.building = 0U; + PlsrTimedStart.valid = 1U; + } + PlsrPlatformExitCritical(criticalState); + return; + } + + criticalState = PlsrPlatformEnterCritical(); + if ((PlsrTimedStart.building == 0U) + || (PlsrTimedStart.sourceEpoch != PlsrSegmentEpoch) + || (queueBank == PlsrProfileQueueBank) + || (queue->generation != queueGeneration)) + { + PlsrInvalidateTimedStartLocked(); + PlsrPlatformExitCritical(criticalState); + return; + } + if (PlsrProfileQueueAppendLocked(queue, &entry) == 0U) + { + PlsrInvalidateTimedStartLocked(); + PlsrPlatformExitCritical(criticalState); + return; + } + PlsrCopyShortProfile(&queue->producerProfile, &profile); + if (PlsrTimedStart.generatedItems != 0xFFFFU) + { + PlsrTimedStart.generatedItems++; + } + if ((profile.active == 0U) + || (profile.nextPeriod >= profile.pulseCount)) + { + queue->generatorComplete = 1U; + } + if ((PlsrTimedStart.generatedItems + >= PLSR_TIMED_START_READY_ITEMS) + || (queue->generatorComplete != 0U)) + { + PlsrTimedStart.building = 0U; + PlsrTimedStart.valid = 1U; + PlsrPlatformExitCritical(criticalState); + return; + } + PlsrPlatformExitCritical(criticalState); + budget--; + } +} + #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) static uint32_t PlsrProfileTimingNow(void) @@ -2646,6 +2941,195 @@ static void PlsrMaybePlanBoundaryRamp(uint32_t expectedEpoch) } } +static uint8_t PlsrStartPreparedTimedOutput(void) +{ + PLSR_PROFILE_QUEUE *queue; + PLSR_PROFILE_ENTRY firstRun; + uint32_t criticalState; + uint32_t firstFrequencyHz; + uint32_t secondFrequencyHz; + uint32_t actualFrequencyHz; + uint32_t queueReadIndex; + uint32_t queueWriteIndex; + uint8_t queueBank; + + criticalState = PlsrPlatformEnterCritical(); + queueBank = PlsrTimedStart.queueBank; + queue = &PlsrProfileQueues[queueBank]; + if ((PlsrTimedStart.pendingActivation == 0U) + || (PlsrTimedStart.valid == 0U) + || (PlsrCurrentSegment != PlsrTimedStart.nextSegment) + || (PlsrSegmentEpoch != PlsrTimedStart.sourceEpoch + 1UL) + || (queueBank == PlsrProfileQueueBank) + || (queue->generation != PlsrTimedStart.queueGeneration) + || (queue->producerEpoch != PlsrSegmentEpoch) + || (queue->producerSegment != PlsrCurrentSegment)) + { + PlsrPlatformExitCritical(criticalState); + return 0U; + } + firstRun = PlsrTimedStart.firstRun; + PlsrProfileQueueResetBankLocked(PlsrProfileQueueBank); + PlsrProfileQueueBank = queueBank; + PlsrCopyShortProfile(&PlsrShortProfile, &queue->producerProfile); + PlsrTimedStart.pendingActivation = 0U; + PlsrTimedStart.valid = 0U; + PlsrTimedStart.building = 0U; + PlsrPlatformExitCritical(criticalState); + + PlsrSegmentClockStarted = 1U; + PlsrSegmentElapsedMs = 0UL; + PlsrRamp.active = 0U; + PlsrCountedHandoffStaged = 0U; + if (PlsrStageCountedHandoff() == 0U) + { + return 0U; + } + + firstFrequencyHz = firstRun.setting.actualFrequencyHz; + secondFrequencyHz = firstFrequencyHz; + criticalState = PlsrPlatformEnterCritical(); + queueReadIndex = queue->readIndex; + queueWriteIndex = queue->writeIndex; + if (queueReadIndex != queueWriteIndex) + { + secondFrequencyHz = queue->entries[ + queueReadIndex & PLSR_PROFILE_QUEUE_MASK] + .setting.actualFrequencyHz; + } + queue->lastSetting = firstRun.setting; + queue->haveLastSetting = 1U; + PlsrPlatformExitCritical(criticalState); + + if (secondFrequencyHz > firstFrequencyHz) + { + PlsrRunStatus = PLSR_STATUS_ACCELERATING; + } + else if (secondFrequencyHz < firstFrequencyHz) + { + PlsrRunStatus = PLSR_STATUS_DECELERATING; + } + else + { + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + PlsrCurrentFrequencyHz = firstFrequencyHz; + PlsrQueuedFrequencyHz = secondFrequencyHz; + PlsrDiagnosticQueuedExpectedHz = firstRun.setting.actualFrequencyHz; + PlsrCountedObservedPublished = PlsrPlatformObservedPulses( + (uint8_t)PlsrActiveConfig.pulseOutput); + PlsrProfileQueueMinimumDepth = PlsrProfileQueueCount(); + PlsrProfileQueueLowWaterLatched = 0U; + PlsrPulseActive = 1U; + PlsrExecutor.generation++; + PlsrExecutor.mode = (queue->generatorComplete != 0U) + ? PLSR_EXEC_STEP_TABLE + : PLSR_EXEC_STREAM; + if (PlsrPlatformStartCountedStreamPrepared( + (uint8_t)PlsrActiveConfig.pulseOutput, + &firstRun.setting, firstRun.repeatCount, + &actualFrequencyHz) == 0U) + { + PlsrPulseActive = 0U; + PlsrExecutor.mode = PLSR_EXEC_IDLE; + PlsrProfileQueueReset(); + PlsrShortProfile.active = 0U; + return 0U; + } + PlsrCurrentFrequencyHz = actualFrequencyHz; + PlsrDiagnosticRecordFrequency(firstRun.requestedFrequencyHz, + firstRun.setting.actualFrequencyHz, + actualFrequencyHz); + return 1U; +} + +static PLSR_PLATFORM_SERVICE_RESULT PlsrTryStartPreparedTimedSegment( + uint8_t segmentNumber) +{ + PLSR_PROFILE_QUEUE *queue; + uint32_t criticalState; + uint64_t magnitude; + uint8_t positive; + uint8_t directionLevel; + uint8_t directionChanged; + + criticalState = PlsrPlatformEnterCritical(); + queue = &PlsrProfileQueues[PlsrTimedStart.queueBank]; + if ((PlsrTimedStart.valid == 0U) + || (PlsrTimedStart.pendingActivation != 0U) + || (PlsrTimedStart.sourceEpoch != PlsrSegmentEpoch) + || (PlsrTimedStart.sourceSegment != PlsrCurrentSegment) + || (PlsrTimedStart.nextSegment != segmentNumber) + || (PlsrTimedStart.queueBank == PlsrProfileQueueBank) + || (queue->generation != PlsrTimedStart.queueGeneration)) + { + PlsrPlatformExitCritical(criticalState); + return PLSR_PLATFORM_SERVICE_DEFERRED; + } + magnitude = PlsrTimedStart.magnitude; + positive = PlsrTimedStart.positive; + directionLevel = PlsrTimedStart.directionLevel; + directionChanged = PlsrTimedStart.directionChanged; + + PlsrSegmentEpoch++; + PlsrCurrentSegment = segmentNumber; + PlsrSegmentClockStarted = 0U; + PlsrSegmentElapsedMs = 0UL; + PlsrWaitElapsedMs = 0UL; + PlsrBoundaryRampStarted = 0U; + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrCutRequested = 0U; + PlsrStopPulsesRemaining = 0U; + PlsrAbStopArmed = 0U; + PlsrFrequencyUpdatePending = 0U; + PlsrDeferredFrequencyPending = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrHandoffPlan.valid = 0U; + PlsrShortProfile.active = 0U; + PlsrShortProfile.nextPeriod = 0U; + PlsrDirectionDelayActive = 0U; + PlsrDirectionDelayRemainingMs = 0U; + PlsrExtEdgePending = 0U; + PlsrRemainingPulses = magnitude; + PlsrCountPositive = positive; + PlsrTimedStart.pendingActivation = 1U; + PlsrPlatformExitCritical(criticalState); + + PlsrExtPreviousLevel = + PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); + if (PlsrPlatformPrepare((uint8_t)PlsrActiveConfig.pulseOutput, + (uint8_t)PlsrActiveConfig.directionOutput, + directionLevel, + (uint8_t)PlsrActiveConfig.outputMode, + positive) == 0U) + { + criticalState = PlsrPlatformEnterCritical(); + PlsrInvalidateTimedStartLocked(); + PlsrPlatformExitCritical(criticalState); + return PLSR_PLATFORM_SERVICE_FAILED; + } + PlsrDiagnosticBeginSegment(segmentNumber, magnitude, positive); + PlsrLastDirectionValid = 1U; + PlsrLastDirectionOutput = (uint8_t)PlsrActiveConfig.directionOutput; + PlsrLastDirectionLevel = directionLevel; + + if ((directionChanged != 0U) + && (PlsrActiveConfig.directionDelayMs != 0U)) + { + PlsrDirectionDelayActive = 1U; + PlsrDirectionDelayRemainingMs = PlsrActiveConfig.directionDelayMs; + PlsrRunStatus = PLSR_STATUS_ACCELERATING; + return PLSR_PLATFORM_SERVICE_READY; + } + if (PlsrStartPreparedTimedOutput() == 0U) + { + return PLSR_PLATFORM_SERVICE_FAILED; + } + return PLSR_PLATFORM_SERVICE_READY; +} + static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz) { @@ -2655,17 +3139,23 @@ static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz) uint32_t secondFrequencyHz; uint32_t actualFrequencyHz; uint32_t profileEpoch; + uint64_t remainingPulses; uint8_t profileSegment; uint8_t profileHandoffBank; + uint8_t countPositive; uint16_t fillBudget = PLSR_PROFILE_STARTUP_BUDGET; PLSR_PROFILE_ENTRY firstRun; PlsrSegmentClockStarted = 1U; PlsrSegmentElapsedMs = 0UL; + profileSegment = PlsrCurrentSegment; + countPositive = PlsrCountPositive; + remainingPulses = PlsrRemainingSnapshot(); if (PlsrPrepareShortProfile(&PlsrShortProfile, - PlsrCurrentSegment, startFrequencyHz, + profileSegment, startFrequencyHz, targetFrequencyHz, - PlsrRemainingSnapshot()) != 0U) + remainingPulses, + countPositive) != 0U) { PlsrRamp.active = 0U; PlsrRefreshCurrentHandoffPlan(PlsrShortProfile.endHz); @@ -2676,7 +3166,6 @@ static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz) firstFrequencyHz = firstRun.setting.actualFrequencyHz; secondFrequencyHz = firstFrequencyHz; profileEpoch = PlsrSegmentEpoch; - profileSegment = PlsrCurrentSegment; profileHandoffBank = PlsrPreparedHandoffBank; (void)PlsrProfileQueueBegin(&PlsrShortProfile, profileEpoch, @@ -3071,6 +3560,7 @@ static void PlsrTransitionToNext(uint8_t allowCarry) { uint8_t nextSegment; uint32_t carryFrequencyHz = PlsrBoundaryFrequencyHz; + PLSR_PLATFORM_SERVICE_RESULT preparedResult; if (PlsrGetNextSegment(&nextSegment) == 0U) { @@ -3078,6 +3568,20 @@ static void PlsrTransitionToNext(uint8_t allowCarry) return; } + if (allowCarry == 0U) + { + preparedResult = PlsrTryStartPreparedTimedSegment(nextSegment); + if (preparedResult == PLSR_PLATFORM_SERVICE_READY) + { + return; + } + if (preparedResult == PLSR_PLATFORM_SERVICE_FAILED) + { + PlsrEnterError(PLSR_ERROR_INVALID_RESOURCE); + return; + } + } + if (PlsrStartSegment(nextSegment, allowCarry, carryFrequencyHz) == 0U) { PlsrEnterError(PLSR_ERROR_INVALID_RESOURCE); @@ -3147,7 +3651,7 @@ static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, PlsrResolvedSegmentFrequency(frequencyConfig, nextSegment - 1U); if (PlsrPrepareShortProfile(&plan->profile, nextSegment, carryFrequencyHz, nextFrequencyHz, - plan->magnitude) == 0U) + plan->magnitude, positive) == 0U) { return 0U; } @@ -3960,7 +4464,9 @@ static PLSR_PLATFORM_QUEUE_RESULT PlsrTryContinuousHandoff(void) static void PlsrHandleBoundary(uint8_t extEdge) { const PLSR_SEGMENT_CONFIG *segment; + uint32_t criticalState; uint8_t wasCut = PlsrBoundaryWasCut; + uint8_t preserveTimedStart; PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); PlsrBoundaryPending = 0U; @@ -3969,7 +4475,23 @@ static void PlsrHandleBoundary(uint8_t extEdge) PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrShortProfile.active = 0U; - PlsrProfileQueueReset(); + criticalState = PlsrPlatformEnterCritical(); + preserveTimedStart = + (((PlsrTimedStart.valid != 0U) + || (PlsrTimedStart.building != 0U)) + && (PlsrTimedStart.sourceEpoch == PlsrSegmentEpoch) + && (PlsrTimedStart.sourceSegment == PlsrCurrentSegment) + && (PlsrTimedStart.queueBank != PlsrProfileQueueBank)) ? 1U : 0U; + if (preserveTimedStart != 0U) + { + PlsrProfileQueueResetBankLocked(PlsrProfileQueueBank); + PlsrProfileQueueLowWaterLatched = 0U; + } + PlsrPlatformExitCritical(criticalState); + if (preserveTimedStart == 0U) + { + PlsrProfileQueueReset(); + } PlsrHandoffPlan.valid = 0U; PlsrDeferredFrequencyPending = 0U; PlsrAbStopArmed = 0U; @@ -4017,7 +4539,14 @@ static void PlsrHandleBoundary(uint8_t extEdge) { case PLSR_WAIT_TIME: PlsrWaitElapsedMs = 0UL; - PlsrRunStatus = PLSR_STATUS_WAITING; + if (segment->waitTimeMs == 0UL) + { + PlsrTransitionToNext(0U); + } + else + { + PlsrRunStatus = PLSR_STATUS_WAITING; + } break; case PLSR_WAIT_SIGNAL: @@ -4690,6 +5219,10 @@ void PlsrPoll1ms(void) return; } + /* This work is bounded and targets only the inactive bank. It is done + after servicing the live queue so preparation cannot starve output. */ + PlsrServiceTimedStartPreparation(); + criticalState = PlsrPlatformEnterCritical(); pollEpoch = PlsrSegmentEpoch; extLevel = PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); @@ -4734,10 +5267,17 @@ void PlsrPoll1ms(void) if (PlsrDirectionDelayRemainingMs == 0U) { PlsrDirectionDelayActive = 0U; - if (PlsrBeginSegmentOutput(PlsrEffectiveStartFrequency( - PlsrActiveConfig.segments[PlsrCurrentSegment - 1U] - .frequencyHz, - 0U, 1U, 0UL)) == 0U) + if (PlsrTimedStart.pendingActivation != 0U) + { + if (PlsrStartPreparedTimedOutput() == 0U) + { + PlsrEnterError(PLSR_ERROR_TIMER); + } + } + else if (PlsrBeginSegmentOutput(PlsrEffectiveStartFrequency( + PlsrActiveConfig.segments[PlsrCurrentSegment - 1U] + .frequencyHz, + 0U, 1U, 0UL)) == 0U) { PlsrEnterError(PLSR_ERROR_TIMER); } @@ -5408,6 +5948,7 @@ PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, PlsrShadowConfig = PlsrCandidateConfig; if (updateActiveFrequencies != 0U) { + PlsrInvalidateTimedStartLocked(); for (index = 0U; index < PlsrActiveConfig.segmentCount; index++) { uint8_t frequencyChanged =