diff --git a/PLSR/Src/plsr.c b/PLSR/Src/plsr.c index 9db6c3c..4978efc 100644 --- a/PLSR/Src/plsr.c +++ b/PLSR/Src/plsr.c @@ -160,6 +160,7 @@ typedef struct uint8_t directionLevel; uint8_t directionChanged; uint8_t queueBank; + uint8_t nextActTimedCut; volatile uint8_t building; volatile uint8_t valid; volatile uint8_t pendingActivation; @@ -181,6 +182,7 @@ typedef struct uint8_t warmupCount; uint8_t nextSegment; uint8_t positive; + uint8_t nextActTimedCut; volatile uint8_t valid; } PLSR_HANDOFF_PLAN; @@ -191,6 +193,7 @@ typedef struct uint32_t completedPulses; uint8_t nextSegment; uint8_t positive; + uint8_t nextActTimedCut; } PLSR_COUNTED_BOUNDARY_EVENT; typedef enum @@ -259,6 +262,7 @@ static volatile int32_t PlsrPosition; static volatile uint64_t PlsrRemainingPulses; static volatile uint8_t PlsrPulseActive; static volatile uint8_t PlsrCutRequested; +static volatile uint8_t PlsrActTimedCutPlanned; static volatile uint8_t PlsrBoundaryPending; static volatile uint8_t PlsrBoundaryWasCut; static volatile uint8_t PlsrCountPositive; @@ -397,6 +401,10 @@ static uint8_t PlsrShortProfileTakeRunLimited( PLSR_SHORT_PROFILE *profile, PLSR_PROFILE_ENTRY *entry, uint32_t maximumRepeats); +static uint8_t PlsrLimitProfileToActTime( + PLSR_SHORT_PROFILE *profile, + uint16_t actTimeMs, + uint8_t *timedCutPlanned); static void PlsrProfileRecordPlannerStatus( const PLSR_SHORT_PROFILE *profile); static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( @@ -1615,72 +1623,133 @@ static uint8_t PlsrRampAdvance(uint32_t expectedEpoch) return 1U; } -static uint8_t PlsrGetNextSegment(uint8_t *nextSegment) +static uint32_t PlsrResolvedSegmentFrequency(const PLSR_CONFIG *config, + uint8_t segmentIndex) { - const PLSR_SEGMENT_CONFIG *segment = - &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; + uint32_t frequencyHz = config->segments[segmentIndex].frequencyHz; + + return (frequencyHz == 0UL) ? config->defaultSpeedHz : frequencyHz; +} + +static int64_t PlsrSegmentDisplacement(uint8_t segmentNumber, + int32_t referencePosition) +{ + int32_t configured = + PlsrActiveConfig.segments[segmentNumber - 1U].pulses; + + if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE) + { + return (int64_t)configured - (int64_t)referencePosition; + } + return configured; +} + +static uint8_t PlsrSegmentSuccessor(uint8_t segmentNumber, + uint8_t *nextSegment) +{ + const PLSR_SEGMENT_CONFIG *segment; + if ((segmentNumber == 0U) || (nextSegment == NULL) + || (segmentNumber > PlsrActiveConfig.segmentCount)) + { + return 0U; + } + segment = &PlsrActiveConfig.segments[segmentNumber - 1U]; if (segment->jumpSegment != 0U) { *nextSegment = (uint8_t)segment->jumpSegment; return 1U; } - if (PlsrCurrentSegment < PlsrActiveConfig.segmentCount) + if (segmentNumber < PlsrActiveConfig.segmentCount) { - *nextSegment = (uint8_t)(PlsrCurrentSegment + 1U); + *nextSegment = (uint8_t)(segmentNumber + 1U); return 1U; } return 0U; } -static uint32_t PlsrResolvedSegmentFrequency(const PLSR_CONFIG *config, - uint8_t segmentIndex) +/* Absolute targets are converted here, before any planner/queue code sees the + segment. Zero relative displacement is a true no-op: follow its successor + (including jump rules) until a motion segment is found. The bounded walk + also turns an all-zero or zero-only jump loop into end-of-motion. */ +static uint8_t PlsrResolveMotionSegment(uint8_t candidateSegment, + int32_t referencePosition, + uint8_t *motionSegment, + int64_t *displacement) { - uint32_t frequencyHz = config->segments[segmentIndex].frequencyHz; + uint8_t inspected; - return (frequencyHz == 0UL) ? config->defaultSpeedHz : frequencyHz; + if ((motionSegment == NULL) || (displacement == NULL)) + { + return 0U; + } + for (inspected = 0U; + inspected < PlsrActiveConfig.segmentCount; + inspected++) + { + if ((candidateSegment == 0U) + || (candidateSegment > PlsrActiveConfig.segmentCount)) + { + return 0U; + } + *displacement = PlsrSegmentDisplacement(candidateSegment, + referencePosition); + if (*displacement != 0) + { + *motionSegment = candidateSegment; + return 1U; + } + if (PlsrSegmentSuccessor(candidateSegment, + &candidateSegment) == 0U) + { + return 0U; + } + } + return 0U; } -static int64_t PlsrSegmentDisplacement(uint8_t segmentNumber, - int32_t referencePosition) +static uint8_t PlsrResolveNextMotionSegment(uint8_t sourceSegment, + int32_t referencePosition, + uint8_t *motionSegment, + int64_t *displacement) { - int32_t configured = - PlsrActiveConfig.segments[segmentNumber - 1U].pulses; + uint8_t candidateSegment; - if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE) + if (PlsrSegmentSuccessor(sourceSegment, &candidateSegment) == 0U) { - return (int64_t)configured - (int64_t)referencePosition; + return 0U; } - return configured; + return PlsrResolveMotionSegment(candidateSegment, referencePosition, + motionSegment, displacement); } -static uint8_t PlsrPredictNextDirection(uint8_t nextSegment, - uint8_t *positive) +static uint8_t PlsrPredictNextMotion(uint8_t *nextSegment, + uint8_t *positive) { uint32_t criticalState; uint64_t remaining; int32_t position; - uint32_t predictedBits; - int32_t predictedPosition; + int64_t predictedPosition; int64_t displacement; + uint8_t countPositive; criticalState = PlsrPlatformEnterCritical(); remaining = PlsrRemainingPulses; position = PlsrPosition; + countPositive = PlsrCountPositive; PlsrPlatformExitCritical(criticalState); - predictedBits = (uint32_t)position; - if (PlsrCountPositive != 0U) - { - predictedBits += (uint32_t)remaining; - } - else + predictedPosition = (countPositive != 0U) + ? (int64_t)position + (int64_t)remaining + : (int64_t)position - (int64_t)remaining; + if ((predictedPosition > (int64_t)INT32_MAX) + || (predictedPosition < (int64_t)INT32_MIN)) { - predictedBits -= (uint32_t)remaining; + return 0U; } - predictedPosition = (int32_t)predictedBits; - displacement = PlsrSegmentDisplacement(nextSegment, predictedPosition); - if (displacement == 0) + if (PlsrResolveNextMotionSegment( + PlsrCurrentSegment, (int32_t)predictedPosition, + nextSegment, &displacement) == 0U) { return 0U; } @@ -1745,6 +1814,8 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, PLSR_MOTION_BLOCK block; PLSR_PLANNER_STATUS status; const PLSR_SEGMENT_CONFIG *segment; + int64_t nextDisplacement; + int32_t boundaryPosition; uint8_t nextSegment = 0U; uint8_t nextPositive; @@ -1771,31 +1842,19 @@ static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, segment = &PlsrActiveConfig.segments[segmentNumber - 1U]; if (segment->waitType == PLSR_EXT_OR_COMPLETE) { - nextSegment = (segment->jumpSegment != 0U) - ? (uint8_t)segment->jumpSegment - : ((segmentNumber < PlsrActiveConfig.segmentCount) - ? (uint8_t)(segmentNumber + 1U) : 0U); + boundaryPosition = (PlsrActiveConfig.positionMode + == PLSR_POSITION_ABSOLUTE) + ? segment->pulses : 0L; + if (PlsrResolveNextMotionSegment( + segmentNumber, boundaryPosition, + &nextSegment, &nextDisplacement) == 0U) + { + nextSegment = 0U; + } } if (nextSegment != 0U) { - if (PlsrActiveConfig.positionMode == PLSR_POSITION_RELATIVE) - { - currentPositive = (segment->pulses >= 0L) ? 1U : 0U; - nextPositive = - (PlsrActiveConfig.segments[nextSegment - 1U].pulses >= 0L) - ? 1U : 0U; - } - else if ((int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses - != (int64_t)segment->pulses) - { - nextPositive = - ((int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses - > (int64_t)segment->pulses) ? 1U : 0U; - } - else - { - nextPositive = (uint8_t)(currentPositive ^ 1U); - } + nextPositive = (nextDisplacement > 0) ? 1U : 0U; if (nextPositive == currentPositive) { block.exitHz = targetFrequencyHz; @@ -1878,6 +1937,48 @@ static uint8_t PlsrShortProfileTakeRunLimited( return 1U; } +/* ACT_TIME never replans the source block into a different trajectory. The + full configured block is prepared first; this function locates the ACT + deadline in that block and limits generation to the corresponding prefix. + The planner prediction uses the same curve integrals and timer quantization + as the stream generator, so its terminal frequency is safe to carry into + the next block. */ +static uint8_t PlsrLimitProfileToActTime( + PLSR_SHORT_PROFILE *profile, + uint16_t actTimeMs, + uint8_t *timedCutPlanned) +{ + PLSR_PLANNER_TIME_PREDICTION prediction; + + if ((profile == NULL) || (timedCutPlanned == NULL)) + { + return 0U; + } + *timedCutPlanned = 0U; + if (PlsrPlannerPredictTime(&profile->planner, + (uint32_t)actTimeMs * 1000UL, + &prediction) == 0U) + { + return 0U; + } + if (prediction.deadlineInProfile == 0U) + { + return 1U; + } + if (prediction.pulseCount > profile->pulseCount) + { + return 0U; + } + profile->pulseCount = prediction.pulseCount; + profile->endHz = prediction.actualFrequencyHz; + if (profile->pulseCount == 0UL) + { + profile->active = 0U; + } + *timedCutPlanned = 1U; + return 1U; +} + /* Build the replacement stream in the inactive queue bank while the IRQ keeps consuming the published bank. A bounded prefix from the old queue bridges the construction interval; the final bank flip is the only critical part. */ @@ -2104,6 +2205,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrReplanPulseDir( PlsrRamp.active = 0U; PlsrHandoffPlan.valid = 0U; PlsrCountedHandoffStaged = 0U; + PlsrActTimedCutPlanned = 0U; PlsrProfileQueueBank = destinationBank; PlsrCopyShortProfile(&PlsrShortProfile, &destination->producerProfile); @@ -2190,6 +2292,7 @@ static void PlsrInvalidateTimedStartLocked(void) PlsrTimedStart.building = 0U; PlsrTimedStart.valid = 0U; PlsrTimedStart.pendingActivation = 0U; + PlsrTimedStart.nextActTimedCut = 0U; } static void PlsrProfileQueueResetBankLocked(uint8_t bank) @@ -2334,7 +2437,6 @@ static void PlsrServiceTimedStartPreparation(void) 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; @@ -2352,6 +2454,7 @@ static void PlsrServiceTimedStartPreparation(void) uint8_t directionLevel; uint8_t directionChanged; uint8_t queueBank; + uint8_t nextActTimedCut = 0U; if ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) || (PlsrStopRequested != 0U) @@ -2368,7 +2471,10 @@ static void PlsrServiceTimedStartPreparation(void) || ((PlsrActiveConfig.segments[sourceSegment - 1U].waitType != PLSR_WAIT_TIME) && (PlsrActiveConfig.segments[sourceSegment - 1U].waitType - != PLSR_WAIT_SIGNAL))) + != PLSR_WAIT_SIGNAL) + && ((PlsrActiveConfig.segments[sourceSegment - 1U].waitType + != PLSR_ACT_TIME) + || (PlsrRunStatus != PLSR_STATUS_WAITING)))) { PlsrInvalidateTimedStartLocked(); PlsrPlatformExitCritical(criticalState); @@ -2400,16 +2506,7 @@ static void PlsrServiceTimedStartPreparation(void) 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) + if (positive != 0U) { predictedPosition = (int64_t)position + (int64_t)remaining; } @@ -2422,9 +2519,9 @@ static void PlsrServiceTimedStartPreparation(void) { return; } - displacement = PlsrSegmentDisplacement( - nextSegment, (int32_t)predictedPosition); - if (displacement == 0) + if (PlsrResolveNextMotionSegment( + sourceSegment, (int32_t)predictedPosition, + &nextSegment, &displacement) == 0U) { return; } @@ -2448,10 +2545,24 @@ static void PlsrServiceTimedStartPreparation(void) PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz; startFrequencyHz = PlsrEffectiveStartFrequency( targetFrequencyHz, 0U, directionChanged, 0UL); - if ((PlsrPrepareShortProfile(&profile, nextSegment, - startFrequencyHz, targetFrequencyHz, - magnitude, positive) == 0U) - || (PlsrShortProfileTakeRun(&profile, &entry) == 0U)) + if (PlsrPrepareShortProfile(&profile, nextSegment, + startFrequencyHz, targetFrequencyHz, + magnitude, positive) == 0U) + { + return; + } + if ((PlsrActiveConfig.segments[nextSegment - 1U].waitType + == PLSR_ACT_TIME) + && ((PlsrLimitProfileToActTime( + &profile, + PlsrActiveConfig.segments[nextSegment - 1U].actTimeMs, + &nextActTimedCut) == 0U) + || (profile.pulseCount == 0UL))) + { + return; + } + magnitude = profile.pulseCount; + if (PlsrShortProfileTakeRun(&profile, &entry) == 0U) { return; } @@ -2494,6 +2605,7 @@ static void PlsrServiceTimedStartPreparation(void) PlsrTimedStart.directionLevel = directionLevel; PlsrTimedStart.directionChanged = directionChanged; PlsrTimedStart.queueBank = queueBank; + PlsrTimedStart.nextActTimedCut = nextActTimedCut; PlsrTimedStart.valid = (queue->generatorComplete != 0U) ? 1U : 0U; PlsrTimedStart.building = (PlsrTimedStart.valid == 0U) ? 1U : 0U; PlsrTimedStart.pendingActivation = 0U; @@ -2857,14 +2969,13 @@ static void PlsrMaybePlanBoundaryRamp(uint32_t expectedEpoch) } segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; - hasNext = PlsrGetNextSegment(&nextSegment); + hasNext = PlsrPredictNextMotion(&nextSegment, &nextPositive); targetHz = PlsrEffectiveStopFrequency(segment->frequencyHz); if ((hasNext != 0U) && (segment->waitType == PLSR_EXT_OR_COMPLETE)) { - if ((PlsrPredictNextDirection(nextSegment, &nextPositive) != 0U) - && (nextPositive == PlsrCountPositive)) + if (nextPositive == PlsrCountPositive) { targetHz = PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz; } @@ -2988,6 +3099,7 @@ static uint8_t PlsrStartPreparedTimedOutput(void) PlsrSegmentElapsedMs = 0UL; PlsrRamp.active = 0U; PlsrCountedHandoffStaged = 0U; + PlsrRefreshCurrentHandoffPlan(PlsrShortProfile.endHz); if (PlsrStageCountedHandoff() == 0U) { return 0U; @@ -3101,6 +3213,7 @@ static PLSR_PLATFORM_SERVICE_RESULT PlsrTryStartPreparedTimedSegment( PlsrExtEdgePending = 0U; PlsrRemainingPulses = magnitude; PlsrCountPositive = positive; + PlsrActTimedCutPlanned = PlsrTimedStart.nextActTimedCut; PlsrTimedStart.pendingActivation = 1U; PlsrPlatformExitCritical(criticalState); @@ -3150,6 +3263,7 @@ static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz) uint8_t profileSegment; uint8_t profileHandoffBank; uint8_t countPositive; + uint8_t timedCutPlanned = 0U; uint16_t fillBudget = PLSR_PROFILE_STARTUP_BUDGET; PLSR_PROFILE_ENTRY firstRun; @@ -3165,6 +3279,31 @@ static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz) countPositive) != 0U) { PlsrRamp.active = 0U; + if ((PlsrActiveConfig.segments[profileSegment - 1U].waitType + == PLSR_ACT_TIME) + && (PlsrLimitProfileToActTime( + &PlsrShortProfile, + PlsrActiveConfig.segments[profileSegment - 1U].actTimeMs, + &timedCutPlanned) == 0U)) + { + return 0U; + } + PlsrActTimedCutPlanned = timedCutPlanned; + if (timedCutPlanned != 0U) + { + uint32_t criticalState = PlsrPlatformEnterCritical(); + + PlsrRemainingPulses = PlsrShortProfile.pulseCount; + PlsrPlatformExitCritical(criticalState); + PlsrDiagnosticBeginSegment(profileSegment, + PlsrShortProfile.pulseCount, + countPositive); + if (PlsrShortProfile.pulseCount == 0UL) + { + PlsrRunStatus = PLSR_STATUS_WAITING; + return 1U; + } + } PlsrRefreshCurrentHandoffPlan(PlsrShortProfile.endHz); if (PlsrShortProfileTakeRun(&PlsrShortProfile, &firstRun) == 0U) { @@ -3305,7 +3444,12 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber, criticalState = PlsrPlatformEnterCritical(); position = PlsrPosition; PlsrPlatformExitCritical(criticalState); - displacement = PlsrSegmentDisplacement(segmentNumber, position); + if (PlsrResolveMotionSegment(segmentNumber, position, + &segmentNumber, &displacement) == 0U) + { + PlsrFinishCompleted(); + return 1U; + } positive = (displacement >= 0) ? 1U : 0U; magnitude = (displacement < 0) ? (uint64_t)(-displacement) : (uint64_t)displacement; @@ -3319,6 +3463,7 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber, PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCutRequested = 0U; + PlsrActTimedCutPlanned = 0U; PlsrStopPulsesRemaining = 0U; PlsrAbStopArmed = 0U; PlsrFrequencyUpdatePending = 0U; @@ -3341,6 +3486,20 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber, PlsrExtPreviousLevel = PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); + directionLevel = positive; + if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) + && (PlsrActiveConfig.directionNegativeLogic != 0U)) + { + directionLevel ^= 1U; + } + directionChanged = ((PlsrLastDirectionValid == 0U) + || (PlsrLastDirectionOutput + != (uint8_t)PlsrActiveConfig.directionOutput) + || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U; + startFrequencyHz = PlsrEffectiveStartFrequency( + PlsrActiveConfig.segments[segmentNumber - 1U].frequencyHz, + allowCarry, directionChanged, carryFrequencyHz); + criticalState = PlsrPlatformEnterCritical(); PlsrRemainingPulses = magnitude; PlsrCountPositive = positive; @@ -3360,28 +3519,6 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber, return 1U; } - if (magnitude == 0UL) - { - PlsrSegmentClockStarted = 1U; - PlsrRunStatus = PLSR_STATUS_RUNNING; - PlsrBoundaryFrequencyHz = 0UL; - PlsrBoundaryPending = 1U; - PlsrDiagnosticBeginSegment(segmentNumber, 0UL, positive); - PlsrDiagnosticFinishSegment(1U); - return 1U; - } - - directionLevel = positive; - if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) - && (PlsrActiveConfig.directionNegativeLogic != 0U)) - { - directionLevel ^= 1U; - } - directionChanged = ((PlsrLastDirectionValid == 0U) - || (PlsrLastDirectionOutput - != (uint8_t)PlsrActiveConfig.directionOutput) - || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U; - if (PlsrPlatformPrepare((uint8_t)PlsrActiveConfig.pulseOutput, (uint8_t)PlsrActiveConfig.directionOutput, directionLevel, @@ -3395,10 +3532,6 @@ static uint8_t PlsrStartSegment(uint8_t segmentNumber, PlsrLastDirectionOutput = (uint8_t)PlsrActiveConfig.directionOutput; PlsrLastDirectionLevel = directionLevel; - startFrequencyHz = PlsrEffectiveStartFrequency( - PlsrActiveConfig.segments[segmentNumber - 1U].frequencyHz, - allowCarry, directionChanged, carryFrequencyHz); - if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) && (directionChanged != 0U) && (PlsrActiveConfig.directionDelayMs != 0U)) @@ -3458,6 +3591,7 @@ static void PlsrFinishCompleted(void) PlsrRemainingPulses = 0UL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; + PlsrActTimedCutPlanned = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCurrentFrequencyHz = 0UL; @@ -3497,6 +3631,7 @@ static void PlsrFinishStopped(void) PlsrRemainingPulses = 0UL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; + PlsrActTimedCutPlanned = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCurrentFrequencyHz = 0UL; @@ -3536,6 +3671,7 @@ static void PlsrEnterError(PLSR_ERROR error) PlsrRemainingPulses = 0UL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; + PlsrActTimedCutPlanned = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCurrentFrequencyHz = 0UL; @@ -3570,10 +3706,18 @@ static void PlsrEnterError(PLSR_ERROR error) static void PlsrTransitionToNext(uint8_t allowCarry) { uint8_t nextSegment; + int32_t position; + int64_t displacement; + uint32_t criticalState; uint32_t carryFrequencyHz = PlsrBoundaryFrequencyHz; PLSR_PLATFORM_SERVICE_RESULT preparedResult; - if (PlsrGetNextSegment(&nextSegment) == 0U) + criticalState = PlsrPlatformEnterCritical(); + position = PlsrPosition; + PlsrPlatformExitCritical(criticalState); + if (PlsrResolveNextMotionSegment( + PlsrCurrentSegment, position, + &nextSegment, &displacement) == 0U) { PlsrFinishCompleted(); return; @@ -3607,9 +3751,16 @@ static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, const PLSR_SEGMENT_CONFIG *segment; uint8_t nextSegment; int64_t displacement; + int64_t sourceEndpoint; + uint64_t sourceRemaining; + int32_t sourcePosition; uint32_t nextFrequencyHz; + uint32_t criticalState; uint8_t positive; uint8_t warmupIndex; + uint8_t sourceActTimed; + uint8_t nextActTimedCut = 0U; + uint8_t sourcePositive; plan->valid = 0U; if ((sourceSegment == 0U) @@ -3619,33 +3770,37 @@ static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, } segment = &PlsrActiveConfig.segments[sourceSegment - 1U]; - if (segment->waitType != PLSR_EXT_OR_COMPLETE) + sourceActTimed = ((segment->waitType == PLSR_ACT_TIME) + && (sourceSegment == PlsrCurrentSegment) + && (PlsrActTimedCutPlanned != 0U)) ? 1U : 0U; + if ((segment->waitType != PLSR_EXT_OR_COMPLETE) + && (sourceActTimed == 0U)) { return 0U; } - if (segment->jumpSegment != 0U) - { - nextSegment = (uint8_t)segment->jumpSegment; - } - else if (sourceSegment < PlsrActiveConfig.segmentCount) - { - nextSegment = (uint8_t)(sourceSegment + 1U); - } - else - { - return 0U; - } - + sourceEndpoint = 0; if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE) { - displacement = (int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses - - (int64_t)segment->pulses; - } - else - { - displacement = PlsrActiveConfig.segments[nextSegment - 1U].pulses; + sourceEndpoint = segment->pulses; + if (sourceActTimed != 0U) + { + criticalState = PlsrPlatformEnterCritical(); + sourcePosition = PlsrPosition; + sourceRemaining = PlsrRemainingPulses; + sourcePositive = PlsrCountPositive; + PlsrPlatformExitCritical(criticalState); + sourceEndpoint = (sourcePositive != 0U) + ? (int64_t)sourcePosition + + (int64_t)sourceRemaining + : (int64_t)sourcePosition + - (int64_t)sourceRemaining; + } } - if (displacement == 0) + if ((sourceEndpoint > (int64_t)INT32_MAX) + || (sourceEndpoint < (int64_t)INT32_MIN) + || (PlsrResolveNextMotionSegment( + sourceSegment, (int32_t)sourceEndpoint, + &nextSegment, &displacement) == 0U)) { return 0U; } @@ -3666,6 +3821,20 @@ static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, { return 0U; } + if (PlsrActiveConfig.segments[nextSegment - 1U].waitType + == PLSR_ACT_TIME) + { + if ((PlsrActiveConfig.segments[nextSegment - 1U].actTimeMs == 0U) + || (PlsrLimitProfileToActTime( + &plan->profile, + PlsrActiveConfig.segments[nextSegment - 1U].actTimeMs, + &nextActTimedCut) == 0U) + || (plan->profile.pulseCount == 0UL)) + { + return 0U; + } + plan->magnitude = plan->profile.pulseCount; + } { PLSR_PROFILE_ENTRY firstEntry; PLSR_PROFILE_ENTRY secondEntry; @@ -3718,6 +3887,7 @@ static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, plan->secondFrequencyHz = plan->secondSetting.actualFrequencyHz; plan->nextSegment = nextSegment; plan->positive = positive; + plan->nextActTimedCut = nextActTimedCut; plan->valid = 1U; return 1U; } @@ -4080,6 +4250,7 @@ void PlsrExecCountedSegmentBoundaryIrq(uint8_t pulseOutput, event->completedPulses = completedPulses; event->nextSegment = PlsrHandoffPlan.nextSegment; event->positive = PlsrHandoffPlan.positive; + event->nextActTimedCut = PlsrHandoffPlan.nextActTimedCut; #if defined(__ICCARM__) __DMB(); #endif @@ -4196,6 +4367,7 @@ static void PlsrExecServiceCountedBoundaryEvent(void) PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCutRequested = 0U; + PlsrActTimedCutPlanned = event.nextActTimedCut; if (PlsrFrequencyUpdatePending != 0U) { /* An online frequency transaction that met a latched segment marker @@ -4211,6 +4383,7 @@ static void PlsrExecServiceCountedBoundaryEvent(void) PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); PlsrCopyShortProfile(&PlsrShortProfile, &PlsrProfileQueue.producerProfile); + PlsrRefreshCurrentHandoffPlan(PlsrShortProfile.endHz); PlsrDiagnosticBeginSegment(nextSegment, event.magnitude, event.positive); PlsrSeamlessHandoffPending = 1U; PlsrCountedHandoffStaged = 0U; @@ -4423,6 +4596,7 @@ static PLSR_PLATFORM_QUEUE_RESULT PlsrTryContinuousHandoff(void) PlsrCurrentSegment = nextSegment; PlsrRemainingPulses = magnitude; PlsrCountPositive = positive; + PlsrActTimedCutPlanned = PlsrHandoffPlan.nextActTimedCut; PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz; PlsrSegmentClockStarted = 1U; PlsrSegmentElapsedMs = 0UL; @@ -4618,7 +4792,9 @@ static void PlsrHandleBoundary(uint8_t extEdge) if (wasCut != 0U) { PlsrTransitionToNext( - (PlsrActiveConfig.sendMode == PLSR_SEND_SUBSEQUENT) ? 1U : 0U); + ((segment->waitType == PLSR_ACT_TIME) + || (PlsrActiveConfig.sendMode == PLSR_SEND_SUBSEQUENT)) + ? 1U : 0U); return; } @@ -4694,7 +4870,25 @@ static void PlsrRequestCut(uint32_t expectedEpoch) else if (PlsrPulseActive != 0U) { PlsrCutRequested = 1U; - (void)PlsrArmFinalAbBoundaryLocked(); + if ((PlsrActiveConfig.outputMode == PLSR_OUTPUT_PULSE_DIR) + && ((PlsrExecutor.mode == PLSR_EXEC_STEP_TABLE) + || (PlsrExecutor.mode == PLSR_EXEC_STREAM) + || (PlsrExecutor.mode == PLSR_EXEC_STOPPING))) + { + /* Freeze both producer-side handoff state and the platform's + latched next run. Hardware stops at the next safe falling + edge; normal counted completion performs boundary bookkeeping + in the task context. */ + PlsrHandoffPlan.valid = 0U; + PlsrCountedHandoffStaged = 0U; + PlsrExecutor.mode = PLSR_EXEC_STOPPING; + (void)PlsrPlatformRequestFiniteCut( + (uint8_t)PlsrActiveConfig.pulseOutput); + } + else + { + (void)PlsrArmFinalAbBoundaryLocked(); + } } else { @@ -4844,6 +5038,7 @@ uint8_t PlsrInit(void) PlsrRemainingPulses = 0UL; PlsrPulseActive = 0U; PlsrCutRequested = 0U; + PlsrActTimedCutPlanned = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrCountOverflowPending = 0U; @@ -5098,6 +5293,7 @@ static void PlsrExecuteClear(void) PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; PlsrCutRequested = 0U; + PlsrActTimedCutPlanned = 0U; PlsrBoundaryPending = 0U; PlsrBoundaryWasCut = 0U; PlsrDirectionDelayActive = 0U; @@ -5225,8 +5421,10 @@ static uint8_t PlsrServiceCountedExecutor(void) PlsrHandoffPlan.valid = 0U; PlsrCountedHandoffStaged = 0U; PlsrBoundaryFrequencyHz = completedFrequencyHz; - PlsrBoundaryWasCut = (PlsrCutRequested != 0U) ? 1U : 0U; + PlsrBoundaryWasCut = ((PlsrCutRequested != 0U) + || (PlsrActTimedCutPlanned != 0U)) ? 1U : 0U; PlsrCutRequested = 0U; + PlsrActTimedCutPlanned = 0U; PlsrPulseActive = 0U; PlsrCurrentFrequencyHz = 0UL; PlsrQueuedFrequencyHz = 0UL; @@ -5540,7 +5738,9 @@ void PlsrPoll1ms(void) PlsrPlatformExitCritical(criticalState); if ((activeSegment->waitType == PLSR_ACT_TIME) - && (PlsrSegmentElapsedMs >= activeSegment->actTimeMs)) + && (PlsrSegmentElapsedMs >= activeSegment->actTimeMs) + && ((PlsrActiveConfig.outputMode != PLSR_OUTPUT_PULSE_DIR) + || (PlsrActTimedCutPlanned == 0U))) { PlsrRequestCut(pollEpoch); return; diff --git a/PLSR/Src/plsr_planner.c b/PLSR/Src/plsr_planner.c index f2e7cfa..8f60348 100644 --- a/PLSR/Src/plsr_planner.c +++ b/PLSR/Src/plsr_planner.c @@ -746,3 +746,300 @@ uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context, } return produced; } + +static uint64_t PlsrPlannerRampDurationUs(uint32_t pulseCount, + uint32_t fromHz, + uint32_t toHz) +{ + uint64_t frequencySum = (uint64_t)fromHz + toHz; + + if ((pulseCount == 0UL) || (frequencySum == 0ULL)) + { + return 0ULL; + } + return ((uint64_t)2U * pulseCount * 1000000ULL + + frequencySum - 1ULL) / frequencySum; +} + +/* Return floor(numerator / denominator * 2^32) without requiring a + 128-bit intermediate. Both operands are bounded by the planner's + 100 kHz Q32 ramp area, so the normalized remainder can be doubled safely. */ +static uint32_t PlsrPlannerRatioQ32(uint64_t numerator, + uint64_t denominator) +{ + uint64_t remainder; + uint32_t ratio = 0UL; + uint8_t bit; + + if ((numerator == 0ULL) || (denominator == 0ULL)) + { + return 0UL; + } + if (numerator >= denominator) + { + return 0xFFFFFFFFUL; + } + remainder = numerator; + for (bit = 0U; bit < 32U; bit++) + { + ratio <<= 1U; + remainder <<= 1U; + if (remainder >= denominator) + { + remainder -= denominator; + ratio |= 1UL; + } + } + return ratio; +} + +static uint32_t PlsrPlannerRampPulsesAtTime( + const PLSR_PLANNER_CONTEXT *context, + uint32_t pulseCount, + uint32_t fromHz, + uint32_t toHz, + uint64_t elapsedUs, + uint64_t durationUs, + uint64_t *progressQ32) +{ + PLSR_PLANNER_CONTEXT ramp = *context; + uint64_t partialAreaQ32; + uint64_t totalAreaQ32; + uint64_t product; + uint32_t areaRatioQ32; + uint32_t result; + + if ((pulseCount == 0UL) || (elapsedUs == 0ULL) + || (durationUs == 0ULL)) + { + *progressQ32 = 0ULL; + return 0UL; + } + if (elapsedUs >= durationUs) + { + *progressQ32 = PLSR_PLANNER_Q32_ONE; + return pulseCount; + } + *progressQ32 = (elapsedUs * PLSR_PLANNER_Q32_ONE) / durationUs; + ramp.rampFromHz = fromHz; + ramp.rampToHz = toHz; + partialAreaQ32 = PlsrPlannerRampAreaQ32( + &ramp, fromHz, toHz, *progressQ32); + totalAreaQ32 = PlsrPlannerRampAreaQ32( + &ramp, fromHz, toHz, PLSR_PLANNER_Q32_ONE); + areaRatioQ32 = PlsrPlannerRatioQ32(partialAreaQ32, + totalAreaQ32); + product = (uint64_t)pulseCount * areaRatioQ32; + result = (uint32_t)(product >> 32U); + if ((uint32_t)product != 0UL) + { + result++; + } + return (result > pulseCount) ? pulseCount : result; +} + +static uint8_t PlsrPlannerSetPredictedFrequency( + const PLSR_PLANNER_CONTEXT *context, + uint32_t fromHz, + uint32_t toHz, + uint64_t progressQ32, + PLSR_PLANNER_TIME_PREDICTION *prediction) +{ + PLSR_PLANNER_CONTEXT ramp = *context; + PLSR_PLATFORM_TIMER_SETTING setting; + uint32_t requestedHz; + + ramp.rampFromHz = fromHz; + ramp.rampToHz = toHz; + requestedHz = PlsrPlannerInstantFrequency(&ramp, progressQ32); + if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput, + PLSR_OUTPUT_PULSE_DIR, + requestedHz, &setting) == 0U) + { + return 0U; + } + prediction->actualFrequencyHz = setting.actualFrequencyHz; + return 1U; +} + +static uint64_t PlsrPlannerRampTargetAreaQ32(uint64_t totalAreaQ32, + uint32_t pulseCount, + uint32_t pulseIndex) +{ + uint64_t step = totalAreaQ32 / pulseCount; + uint64_t remainder = totalAreaQ32 % pulseCount; + + return step * pulseIndex + + (remainder * pulseIndex) / pulseCount; +} + +/* Predict the timer setting of the last complete ramp pulse at the deadline. + A ramp pulse represents the average frequency between two equal-area curve + boundaries; carrying that run setting is closer to the hardware state than + carrying the mathematical instantaneous frequency at the boundary. */ +static uint8_t PlsrPlannerSetPredictedRampRunFrequency( + const PLSR_PLANNER_CONTEXT *context, + uint32_t pulseCount, + uint32_t fromHz, + uint32_t toHz, + uint32_t pulseIndex, + PLSR_PLANNER_TIME_PREDICTION *prediction) +{ + PLSR_PLANNER_CONTEXT ramp = *context; + PLSR_PLATFORM_TIMER_SETTING setting; + uint64_t totalAreaQ32; + uint64_t previousTargetAreaQ32; + uint64_t targetAreaQ32; + uint64_t previousBoundaryQ32; + uint64_t boundaryQ32; + uint64_t denominator; + uint64_t requestedHz; + + if ((pulseCount == 0UL) || (pulseIndex == 0UL)) + { + return PlsrPlannerSetPredictedFrequency( + context, fromHz, toHz, 0ULL, prediction); + } + if (pulseIndex > pulseCount) + { + pulseIndex = pulseCount; + } + ramp.rampFromHz = fromHz; + ramp.rampToHz = toHz; + totalAreaQ32 = PlsrPlannerRampAreaQ32( + &ramp, fromHz, toHz, PLSR_PLANNER_Q32_ONE); + previousTargetAreaQ32 = PlsrPlannerRampTargetAreaQ32( + totalAreaQ32, pulseCount, pulseIndex - 1UL); + targetAreaQ32 = PlsrPlannerRampTargetAreaQ32( + totalAreaQ32, pulseCount, pulseIndex); + previousBoundaryQ32 = (pulseIndex == 1UL) + ? 0ULL + : PlsrPlannerExactBoundaryQ32( + &ramp, 0ULL, + previousTargetAreaQ32); + boundaryQ32 = (pulseIndex == pulseCount) + ? PLSR_PLANNER_Q32_ONE + : PlsrPlannerExactBoundaryQ32( + &ramp, previousBoundaryQ32, + targetAreaQ32); + if (boundaryQ32 <= previousBoundaryQ32) + { + return 0U; + } + denominator = (uint64_t)pulseCount + * (boundaryQ32 - previousBoundaryQ32); + requestedHz = (denominator == 0ULL) + ? toHz + : (totalAreaQ32 + denominator / 2ULL) + / denominator; + if (requestedHz == 0ULL) + { + requestedHz = 1ULL; + } + if (requestedHz > PLSR_FREQUENCY_MAX_HZ) + { + requestedHz = PLSR_FREQUENCY_MAX_HZ; + } + if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput, + PLSR_OUTPUT_PULSE_DIR, + (uint32_t)requestedHz, + &setting) == 0U) + { + return 0U; + } + prediction->actualFrequencyHz = setting.actualFrequencyHz; + return 1U; +} + +uint8_t PlsrPlannerPredictTime( + const PLSR_PLANNER_CONTEXT *context, + uint32_t elapsedUs, + PLSR_PLANNER_TIME_PREDICTION *prediction) +{ + PLSR_PLATFORM_TIMER_SETTING steadySetting; + uint64_t remainingUs = elapsedUs; + uint64_t durationUs; + uint64_t progressQ32; + uint64_t partialPulses; + + if ((context == NULL) || (prediction == NULL) + || (context->block.pulseBudget == 0UL)) + { + return 0U; + } + (void)memset(prediction, 0, sizeof(*prediction)); + + durationUs = PlsrPlannerRampDurationUs( + context->entryPulses, context->startHz, context->peakHz); + if ((context->entryPulses != 0UL) && (remainingUs <= durationUs)) + { + prediction->pulseCount = PlsrPlannerRampPulsesAtTime( + context, context->entryPulses, context->startHz, + context->peakHz, remainingUs, durationUs, &progressQ32); + prediction->phase = PLSR_PLANNER_PHASE_ENTRY; + prediction->deadlineInProfile = 1U; + return PlsrPlannerSetPredictedRampRunFrequency( + context, context->entryPulses, context->startHz, + context->peakHz, prediction->pulseCount, prediction); + } + if (context->entryPulses != 0UL) + { + remainingUs -= durationUs; + } + + if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput, + PLSR_OUTPUT_PULSE_DIR, + context->peakHz, + &steadySetting) == 0U) + { + return 0U; + } + durationUs = (context->steadyPulses == 0UL) + ? 0ULL + : ((uint64_t)context->steadyPulses * 1000000ULL + + steadySetting.actualFrequencyHz - 1UL) + / steadySetting.actualFrequencyHz; + if ((context->steadyPulses != 0UL) && (remainingUs <= durationUs)) + { + partialPulses = (remainingUs * steadySetting.actualFrequencyHz + + 999999ULL) / 1000000ULL; + if (partialPulses > context->steadyPulses) + { + partialPulses = context->steadyPulses; + } + prediction->pulseCount = context->entryPulses + + (uint32_t)partialPulses; + prediction->actualFrequencyHz = steadySetting.actualFrequencyHz; + prediction->phase = PLSR_PLANNER_PHASE_STEADY; + prediction->deadlineInProfile = 1U; + return 1U; + } + if (context->steadyPulses != 0UL) + { + remainingUs -= durationUs; + } + + durationUs = PlsrPlannerRampDurationUs( + context->exitPulses, context->peakHz, context->endHz); + if ((context->exitPulses != 0UL) && (remainingUs <= durationUs)) + { + partialPulses = PlsrPlannerRampPulsesAtTime( + context, context->exitPulses, context->peakHz, + context->endHz, remainingUs, durationUs, &progressQ32); + prediction->pulseCount = context->entryPulses + + context->steadyPulses + + (uint32_t)partialPulses; + prediction->phase = PLSR_PLANNER_PHASE_EXIT; + prediction->deadlineInProfile = 1U; + return PlsrPlannerSetPredictedRampRunFrequency( + context, context->exitPulses, context->peakHz, + context->endHz, (uint32_t)partialPulses, prediction); + } + + prediction->pulseCount = context->block.pulseBudget; + prediction->phase = PLSR_PLANNER_PHASE_COMPLETE; + prediction->deadlineInProfile = 0U; + return PlsrPlannerSetPredictedFrequency( + context, context->endHz, context->endHz, + PLSR_PLANNER_Q32_ONE, prediction); +} diff --git a/PLSR/Src/plsr_planner.h b/PLSR/Src/plsr_planner.h index cc55d4a..e521e71 100644 --- a/PLSR/Src/plsr_planner.h +++ b/PLSR/Src/plsr_planner.h @@ -47,6 +47,22 @@ typedef enum PLSR_PLANNER_INVALID } PLSR_PLANNER_STATUS; +typedef enum +{ + PLSR_PLANNER_PHASE_ENTRY = 0, + PLSR_PLANNER_PHASE_STEADY, + PLSR_PLANNER_PHASE_EXIT, + PLSR_PLANNER_PHASE_COMPLETE +} PLSR_PLANNER_PHASE; + +typedef struct +{ + uint32_t pulseCount; + uint32_t actualFrequencyHz; + PLSR_PLANNER_PHASE phase; + uint8_t deadlineInProfile; +} PLSR_PLANNER_TIME_PREDICTION; + typedef struct { PLSR_MOTION_BLOCK block; @@ -86,5 +102,9 @@ PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context, uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context, PLSR_STREAM_ITEM *output, uint16_t capacity); +uint8_t PlsrPlannerPredictTime( + const PLSR_PLANNER_CONTEXT *context, + uint32_t elapsedUs, + PLSR_PLANNER_TIME_PREDICTION *prediction); #endif /* PLSR_PLANNER_H */ diff --git a/PLSR/Src/plsr_platform.h b/PLSR/Src/plsr_platform.h index c6b148e..85771ab 100644 --- a/PLSR/Src/plsr_platform.h +++ b/PLSR/Src/plsr_platform.h @@ -89,6 +89,7 @@ PLSR_PLATFORM_QUEUE_RESULT PlsrPlatformUpdateFinitePrepared( uint32_t *actualFrequencyHz); uint8_t PlsrPlatformRetargetFiniteStop(uint8_t pulseOutput, uint32_t drainPulses); +uint8_t PlsrPlatformRequestFiniteCut(uint8_t pulseOutput); uint8_t PlsrPlatformFiniteRetargetReady(uint8_t pulseOutput, uint32_t *activeFrequencyHz); uint8_t PlsrPlatformFinitePipelineSnapshot(uint8_t pulseOutput, diff --git a/PLSR/Src/plsr_platform_f407.c b/PLSR/Src/plsr_platform_f407.c index 2dabd05..ce90896 100644 --- a/PLSR/Src/plsr_platform_f407.c +++ b/PLSR/Src/plsr_platform_f407.c @@ -515,6 +515,29 @@ uint8_t PlsrPlatformRetargetFiniteStop(uint8_t pulseOutput, return 1U; } +uint8_t PlsrPlatformRequestFiniteCut(uint8_t pulseOutput) +{ + if ((pulseOutput > 3U) + || (PlsrHostFiniteActive[pulseOutput] == 0U) + || (PlsrHostCountedStreamActive[pulseOutput] == 0U)) + { + return 0U; + } + + /* The host model has no half-pulse phase. Complete the cut at the + current emitted boundary and publish the normal finite-completion + event expected by the counted executor. */ + PlsrHostFiniteTarget[pulseOutput] = + PlsrHostFiniteEmitted[pulseOutput]; + PlsrHostFiniteStepCount[pulseOutput] = 0U; + PlsrHostFiniteStepIndex[pulseOutput] = 0U; + PlsrHostFiniteBoundaryReadIndex[pulseOutput] = 0U; + PlsrHostFiniteCompletedStepCount[pulseOutput] = 0U; + PlsrHostFiniteActive[pulseOutput] = 0U; + PlsrHostFiniteComplete[pulseOutput] = 1U; + return 1U; +} + uint8_t PlsrPlatformFiniteRetargetReady(uint8_t pulseOutput, uint32_t *activeFrequencyHz) { @@ -1551,6 +1574,7 @@ static void PlsrFiniteArmNextStepPrepare(uint8_t pulseOutput, TIM_TypeDef *counter, uint32_t blockPulses); static void PlsrFiniteRetargetAtFallingEdge(uint8_t pulseOutput); +static void PlsrFiniteCutAtFallingEdge(uint8_t pulseOutput); static void PlsrFiniteStopAtFallingEdge(uint8_t pulseOutput); static uint8_t PlsrFinalArmJobOutput(uint8_t pulseOutput) @@ -3418,6 +3442,47 @@ uint8_t PlsrPlatformRetargetFiniteStop(uint8_t pulseOutput, return 1U; } +uint8_t PlsrPlatformRequestFiniteCut(uint8_t pulseOutput) +{ + TIM_TypeDef *timer; + uint32_t criticalState; + + if (pulseOutput > 3U) + { + return 0U; + } + + criticalState = PlsrPlatformEnterCritical(); + if ((PlsrFiniteActive[pulseOutput] == 0U) + || (PlsrFiniteStreamActive[pulseOutput] == 0U) + || (PlsrCounterIndexByOutput[pulseOutput] >= PLSR_COUNTER_COUNT) + || (PlsrFiniteRetargetPending[pulseOutput] != 0U)) + { + PlsrPlatformExitCritical(criticalState); + return 0U; + } + if (PlsrFiniteTailStopPending[pulseOutput] != 0U) + { + PlsrPlatformExitCritical(criticalState); + return 1U; + } + + /* Do not allow a run already staged by the producer to cross the cut. + CC1 reaches the physical pulse's falling edge, where the output is + idle and can be stopped without shortening the high or low width. */ + PlsrFiniteStreamNextValid[pulseOutput] = 0U; + PlsrFiniteStreamNextStartsSegment[pulseOutput] = 0U; + PlsrFiniteStreamSourceDone[pulseOutput] = 1U; + PlsrFiniteStreamSourceFault[pulseOutput] = 0U; + PlsrFiniteTailStopPending[pulseOutput] = 1U; + timer = PlsrTimerMap[pulseOutput].timer; + timer->SR = ~TIM_SR_CC1IF; + timer->DIER |= TIM_DIER_CC1IE; + __DMB(); + PlsrPlatformExitCritical(criticalState); + return 1U; +} + uint8_t PlsrPlatformFiniteRetargetReady(uint8_t pulseOutput, uint32_t *activeFrequencyHz) { @@ -4594,7 +4659,7 @@ static void PlsrHandleTimerIrq(uint8_t pulseOutput) && (PlsrFiniteTailStopPending[pulseOutput] != 0U)) { timer->SR = ~TIM_SR_CC1IF; - PlsrFiniteStopAtFallingEdge(pulseOutput); + PlsrFiniteCutAtFallingEdge(pulseOutput); goto irq_done; } #if PLSR_DEBUG_TIMING @@ -5170,6 +5235,50 @@ static void PlsrFiniteRetargetAtFallingEdge(uint8_t pulseOutput) PlsrCounterBegin(pulseOutput); } +static void PlsrFiniteCutAtFallingEdge(uint8_t pulseOutput) +{ + uint8_t counterIndex = PlsrCounterIndexByOutput[pulseOutput]; + TIM_TypeDef *counter = PlsrCounters[counterIndex]; + uint32_t blockCount; + uint32_t completed; + + /* Freeze the external counter at the same falling edge that makes the + pulse output idle, then convert the partial current run into the + terminal run consumed by PlsrPlatformTakeFiniteCompletion(). */ + PlsrCounterSuspend(pulseOutput); + blockCount = (uint16_t)counter->CNT; + if (blockCount >= PlsrFiniteCounterPreload[pulseOutput]) + { + blockCount -= PlsrFiniteCounterPreload[pulseOutput]; + } + else + { + blockCount = 0UL; + } + completed = PlsrFiniteTargetPulses[pulseOutput] + - PlsrFiniteRemainingPulses[pulseOutput] + blockCount; + if (completed > PlsrFiniteTargetPulses[pulseOutput]) + { + completed = PlsrFiniteTargetPulses[pulseOutput]; + } + + PlsrObservedPulseBase[pulseOutput] += completed; + PlsrObservedPulsePublished[pulseOutput] = + PlsrObservedPulseBase[pulseOutput]; + PlsrFiniteTargetPulses[pulseOutput] = completed; + PlsrFiniteRemainingPulses[pulseOutput] = 0UL; + PlsrFiniteStepCount[pulseOutput] = 0U; + PlsrFiniteStepIndex[pulseOutput] = 0U; + PlsrFiniteBoundaryReadIndex[pulseOutput] = 0U; + PlsrFiniteCompletedStepCount[pulseOutput] = 0U; + PlsrFiniteStreamNextValid[pulseOutput] = 0U; + PlsrFiniteStreamNextStartsSegment[pulseOutput] = 0U; + PlsrCounterOverflowPulses[counterIndex] = 0UL; + counter->CNT = 0UL; + counter->SR = 0UL; + PlsrFiniteStopAtFallingEdge(pulseOutput); +} + static void PlsrFiniteStopAtFallingEdge(uint8_t pulseOutput) { TIM_TypeDef *timer = PlsrTimerMap[pulseOutput].timer;