#include "plsr_planner.h" #include #include #define PLSR_PLANNER_Q32_ONE (4294967296ULL) static const uint32_t PlsrPlannerSmoothIntegralQ24[65] = { 0UL, 64UL, 504UL, 1688UL, 3968UL, 7688UL, 13176UL, 20752UL, 30720UL, 43376UL, 59000UL, 77864UL, 100224UL, 126328UL, 156408UL, 190688UL, 229376UL, 272672UL, 320760UL, 373816UL, 432000UL, 495464UL, 564344UL, 638768UL, 718848UL, 804688UL, 896376UL, 993992UL, 1097600UL, 1207256UL, 1323000UL, 1444864UL, 1572864UL, 1707008UL, 1847288UL, 1993688UL, 2146176UL, 2304712UL, 2469240UL, 2639696UL, 2816000UL, 2998064UL, 3185784UL, 3379048UL, 3577728UL, 3781688UL, 3990776UL, 4204832UL, 4423680UL, 4647136UL, 4875000UL, 5107064UL, 5343104UL, 5582888UL, 5826168UL, 6072688UL, 6322176UL, 6574352UL, 6828920UL, 7085576UL, 7344000UL, 7603864UL, 7864824UL, 8126528UL, 8388608UL }; static const uint32_t PlsrPlannerSineIntegralQ24[65] = { 0UL, 53UL, 421UL, 1420UL, 3362UL, 6560UL, 11321UL, 17949UL, 26744UL, 38000UL, 52007UL, 69047UL, 89393UL, 113314UL, 141066UL, 172899UL, 209052UL, 249753UL, 295221UL, 345662UL, 401269UL, 462225UL, 528698UL, 600845UL, 678806UL, 762711UL, 852672UL, 948789UL, 1051146UL, 1159812UL, 1274841UL, 1396271UL, 1524127UL, 1658415UL, 1799129UL, 1946244UL, 2099722UL, 2259509UL, 2425536UL, 2597719UL, 2775958UL, 2960141UL, 3150138UL, 3345809UL, 3546997UL, 3753534UL, 3965237UL, 4181913UL, 4403356UL, 4629347UL, 4859658UL, 5094050UL, 5332273UL, 5574071UL, 5819175UL, 6067312UL, 6318200UL, 6571549UL, 6827065UL, 7084448UL, 7343394UL, 7603596UL, 7864741UL, 8126517UL, 8388608UL }; static uint32_t PlsrPlannerAbsDifference(uint32_t first, uint32_t second) { return (first > second) ? (first - second) : (second - first); } static uint32_t PlsrPlannerRampTime(const PLSR_MOTION_BLOCK *block, uint32_t fromHz, uint32_t toHz) { uint32_t baseTimeMs; uint64_t durationMs; if (fromHz == toHz) { return 0UL; } baseTimeMs = (toHz > fromHz) ? block->accelerationTimeMs : block->decelerationTimeMs; if (baseTimeMs == 0UL) { return 0UL; } durationMs = ((uint64_t)PlsrPlannerAbsDifference(fromHz, toHz) * baseTimeMs + block->referenceSpeedHz - 1UL) / block->referenceSpeedHz; return (durationMs > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL : (uint32_t)durationMs; } static uint16_t PlsrPlannerBaseRampTime(const PLSR_MOTION_BLOCK *block, uint32_t fromHz, uint32_t toHz) { if (toHz > fromHz) { return block->accelerationTimeMs; } if (toHz < fromHz) { return block->decelerationTimeMs; } return 0U; } static uint64_t PlsrPlannerRampWeight(uint32_t fromHz, uint32_t toHz, uint16_t baseTimeMs) { uint64_t fromSquared = (uint64_t)fromHz * fromHz; uint64_t toSquared = (uint64_t)toHz * toHz; uint64_t difference = (fromSquared > toSquared) ? (fromSquared - toSquared) : (toSquared - fromSquared); return difference * baseTimeMs; } static uint64_t PlsrPlannerRequiredPulses(const PLSR_MOTION_BLOCK *block, uint64_t rampWeight) { uint64_t denominator = (uint64_t)2U * block->referenceSpeedHz * 1000UL; return (rampWeight == 0ULL) ? 0ULL : (rampWeight + denominator - 1ULL) / denominator; } static uint32_t PlsrPlannerIntegerSquareRoot(uint64_t value) { uint64_t bit = (uint64_t)1U << 62U; uint64_t root = 0ULL; while (bit > value) { bit >>= 2U; } while (bit != 0ULL) { if (value >= root + bit) { value -= root + bit; root = (root >> 1U) + bit; } else { root >>= 1U; } bit >>= 2U; } return (uint32_t)root; } static uint32_t PlsrPlannerReachableFrequency( const PLSR_MOTION_BLOCK *block, uint32_t fromHz, uint32_t towardHz, uint32_t pulseCount) { uint16_t baseTimeMs = PlsrPlannerBaseRampTime(block, fromHz, towardHz); uint64_t frequencySquared = (uint64_t)fromHz * fromHz; uint64_t changeSquared; uint32_t reachableHz; if ((baseTimeMs == 0U) || (fromHz == towardHz)) { return towardHz; } changeSquared = (uint64_t)2U * pulseCount * block->referenceSpeedHz * 1000UL / baseTimeMs; if (towardHz > fromHz) { reachableHz = PlsrPlannerIntegerSquareRoot( frequencySquared + changeSquared); return (reachableHz > towardHz) ? towardHz : reachableHz; } frequencySquared = (changeSquared >= frequencySquared) ? 0ULL : (frequencySquared - changeSquared); reachableHz = PlsrPlannerIntegerSquareRoot(frequencySquared); if ((uint64_t)reachableHz * reachableHz < frequencySquared) { reachableHz++; } return (reachableHz < towardHz) ? towardHz : reachableHz; } static uint32_t PlsrPlannerPeak(const PLSR_MOTION_BLOCK *block, uint32_t startHz, uint32_t endHz) { uint32_t targetHz = block->cruiseHz; uint32_t upperEndpoint = (startHz > endHz) ? startHz : endHz; uint32_t lowerEndpoint = (startHz < endHz) ? startHz : endHz; uint16_t entryTimeMs; uint16_t exitTimeMs; uint32_t timeSumMs; uint64_t weightedEndpoints; uint64_t availableArea; uint64_t peakSquared; uint32_t peakHz; if ((targetHz <= upperEndpoint) && (targetHz >= lowerEndpoint)) { return targetHz; } entryTimeMs = PlsrPlannerBaseRampTime(block, startHz, targetHz); exitTimeMs = PlsrPlannerBaseRampTime(block, targetHz, endHz); timeSumMs = (uint32_t)entryTimeMs + exitTimeMs; if (timeSumMs == 0UL) { return targetHz; } weightedEndpoints = (uint64_t)startHz * startHz * entryTimeMs + (uint64_t)endHz * endHz * exitTimeMs; availableArea = (uint64_t)2U * block->pulseBudget * block->referenceSpeedHz * 1000UL; if (targetHz > upperEndpoint) { peakSquared = (availableArea + weightedEndpoints) / timeSumMs; peakHz = PlsrPlannerIntegerSquareRoot(peakSquared); if (peakHz < upperEndpoint) { peakHz = upperEndpoint; } return (peakHz > targetHz) ? targetHz : peakHz; } if (availableArea >= weightedEndpoints) { return targetHz; } peakSquared = (weightedEndpoints - availableArea) / timeSumMs; peakHz = PlsrPlannerIntegerSquareRoot(peakSquared); if (peakHz < targetHz) { peakHz = targetHz; } return (peakHz > lowerEndpoint) ? lowerEndpoint : peakHz; } static uint64_t PlsrPlannerCurveIntegralQ32(uint64_t progressQ32, uint16_t curveMode) { const uint32_t *table; uint64_t scaled; uint32_t index; uint32_t fraction; uint64_t first; uint64_t second; if (progressQ32 >= PLSR_PLANNER_Q32_ONE) { return PLSR_PLANNER_Q32_ONE / 2ULL; } if (curveMode == 0U) { return (progressQ32 * progressQ32) >> 33U; } table = (curveMode == 1U) ? PlsrPlannerSmoothIntegralQ24 : PlsrPlannerSineIntegralQ24; scaled = progressQ32 * 64ULL; index = (uint32_t)(scaled >> 32U); fraction = (uint32_t)scaled; first = (uint64_t)table[index] << 8U; second = (uint64_t)table[index + 1UL] << 8U; return first + (((second - first) * fraction) >> 32U); } static uint64_t PlsrPlannerRampAreaQ32(const PLSR_PLANNER_CONTEXT *context, uint32_t fromHz, uint32_t toHz, uint64_t progressQ32) { int64_t delta = (int64_t)toHz - (int64_t)fromHz; int64_t area = (int64_t)((uint64_t)fromHz * progressQ32) + delta * (int64_t)PlsrPlannerCurveIntegralQ32( progressQ32, context->block.curveMode); return (uint64_t)area; } static uint64_t PlsrPlannerExactBoundaryQ32( const PLSR_PLANNER_CONTEXT *context, uint64_t previousBoundaryQ32, uint64_t targetAreaQ32) { uint64_t lowerQ32 = previousBoundaryQ32; uint64_t upperQ32 = PLSR_PLANNER_Q32_ONE; uint64_t middleQ32; uint32_t iteration; for (iteration = 0UL; iteration < 32UL; iteration++) { middleQ32 = lowerQ32 + ((upperQ32 - lowerQ32) >> 1U); if (PlsrPlannerRampAreaQ32(context, context->rampFromHz, context->rampToHz, middleQ32) < targetAreaQ32) { lowerQ32 = middleQ32; } else { upperQ32 = middleQ32; } } return upperQ32; } static uint32_t PlsrPlannerInstantFrequency( const PLSR_PLANNER_CONTEXT *context, uint64_t progressQ32) { const uint32_t *table; uint64_t scaled; uint64_t curveProgressQ32; uint32_t index; uint32_t gap; if (progressQ32 >= PLSR_PLANNER_Q32_ONE) { return context->rampToHz; } if (context->block.curveMode == 0U) { curveProgressQ32 = progressQ32; } else { table = (context->block.curveMode == 1U) ? PlsrPlannerSmoothIntegralQ24 : PlsrPlannerSineIntegralQ24; scaled = progressQ32 * 64ULL; index = (uint32_t)(scaled >> 32U); curveProgressQ32 = (uint64_t)(table[index + 1UL] - table[index]) << 14U; } if (context->rampToHz >= context->rampFromHz) { gap = context->rampToHz - context->rampFromHz; return context->rampFromHz + (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U); } gap = context->rampFromHz - context->rampToHz; return context->rampFromHz - (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U); } static uint64_t PlsrPlannerPredictedBoundaryQ32( const PLSR_PLANNER_CONTEXT *context, uint64_t targetAreaQ32) { uint64_t previousQ32 = context->rampBoundaryQ32; uint64_t currentAreaQ32 = (previousQ32 == 0ULL) ? 0ULL : PlsrPlannerRampAreaQ32( context, context->rampFromHz, context->rampToHz, previousQ32); uint64_t candidateQ32; uint64_t candidateAreaQ32; uint64_t differenceQ32; uint64_t correctionQ32; uint32_t derivativeHz; if (context->rampLastPhaseStepQ32 >= PLSR_PLANNER_Q32_ONE - previousQ32) { candidateQ32 = PLSR_PLANNER_Q32_ONE; } else if (context->rampLastPhaseStepQ32 != 0ULL) { candidateQ32 = previousQ32 + context->rampLastPhaseStepQ32; } else { derivativeHz = PlsrPlannerInstantFrequency(context, previousQ32); if (derivativeHz == 0UL) { derivativeHz = 1UL; } differenceQ32 = targetAreaQ32 - currentAreaQ32; correctionQ32 = (differenceQ32 + derivativeHz - 1UL) / derivativeHz; candidateQ32 = (correctionQ32 >= PLSR_PLANNER_Q32_ONE - previousQ32) ? PLSR_PLANNER_Q32_ONE : previousQ32 + correctionQ32; } candidateAreaQ32 = PlsrPlannerRampAreaQ32( context, context->rampFromHz, context->rampToHz, candidateQ32); derivativeHz = PlsrPlannerInstantFrequency(context, candidateQ32); if (derivativeHz == 0UL) { derivativeHz = 1UL; } if (candidateAreaQ32 < targetAreaQ32) { differenceQ32 = targetAreaQ32 - candidateAreaQ32; correctionQ32 = (differenceQ32 + derivativeHz - 1UL) / derivativeHz; candidateQ32 = (correctionQ32 >= PLSR_PLANNER_Q32_ONE - candidateQ32) ? PLSR_PLANNER_Q32_ONE : candidateQ32 + correctionQ32; } else if (candidateAreaQ32 > targetAreaQ32) { differenceQ32 = candidateAreaQ32 - targetAreaQ32; correctionQ32 = differenceQ32 / derivativeHz; if (correctionQ32 == 0ULL) { correctionQ32 = 1ULL; } candidateQ32 = (correctionQ32 >= candidateQ32 - previousQ32) ? previousQ32 + 1ULL : candidateQ32 - correctionQ32; } return candidateQ32; } static void PlsrPlannerStartRamp(PLSR_PLANNER_CONTEXT *context, uint8_t rampKind, uint32_t fromHz, uint32_t toHz, uint32_t pulseCount) { context->rampKind = rampKind; context->rampRelativePulse = 0UL; context->rampPulseCount = pulseCount; context->rampFromHz = fromHz; context->rampToHz = toHz; context->rampDurationMs = PlsrPlannerRampTime(&context->block, fromHz, toHz); context->rampTotalAreaQ32 = PlsrPlannerRampAreaQ32( context, fromHz, toHz, PLSR_PLANNER_Q32_ONE); context->rampAreaStepQ32 = context->rampTotalAreaQ32 / pulseCount; context->rampAreaRemainder = (uint32_t)(context->rampTotalAreaQ32 % pulseCount); context->rampRemainderAccumulator = 0UL; context->rampTargetAreaQ32 = 0ULL; context->rampBoundaryQ32 = 0ULL; context->rampActualTimeQ32 = 0ULL; context->rampLastPhaseStepQ32 = 0ULL; context->lastRampHz = 0UL; context->rampFirstBoundaryQ32 = PlsrPlannerExactBoundaryQ32( context, 0ULL, context->rampAreaStepQ32); if (pulseCount > 1UL) { uint64_t secondTargetAreaQ32 = context->rampAreaStepQ32 * 2ULL + ((uint64_t)context->rampAreaRemainder * 2ULL) / pulseCount; context->rampSecondBoundaryQ32 = PlsrPlannerExactBoundaryQ32( context, context->rampFirstBoundaryQ32, secondTargetAreaQ32); } else { context->rampSecondBoundaryQ32 = PLSR_PLANNER_Q32_ONE; } } static uint8_t PlsrPlannerSameSetting( const PLSR_PLATFORM_TIMER_SETTING *first, const PLSR_PLATFORM_TIMER_SETTING *second) { return ((first->actualFrequencyHz == second->actualFrequencyHz) && (first->prescaler == second->prescaler) && (first->pairPrescaler == second->pairPrescaler) && (first->period == second->period) && (first->compare == second->compare)) ? 1U : 0U; } static uint8_t PlsrPlannerBuildStep(PLSR_PLANNER_CONTEXT *context, uint32_t requestedHz, PLSR_PLATFORM_TIMER_SETTING *setting, uint32_t *normalizedRequestedHz) { if (requestedHz == 0UL) { requestedHz = 1UL; } if (requestedHz > PLSR_FREQUENCY_MAX_HZ) { requestedHz = PLSR_FREQUENCY_MAX_HZ; } *normalizedRequestedHz = requestedHz; return PlsrPlatformBuildTimerSetting(context->block.pulseOutput, PLSR_OUTPUT_PULSE_DIR, requestedHz, setting); } static uint8_t PlsrPlannerTakeRampStep( PLSR_PLANNER_CONTEXT *context, PLSR_PLATFORM_TIMER_SETTING *setting, uint32_t *requestedFrequencyHz) { uint64_t nextBoundaryQ32; uint64_t desiredDeltaQ32; uint64_t denominator; uint64_t requestedHz; uint64_t actualDeltaQ32; context->rampTargetAreaQ32 += context->rampAreaStepQ32; context->rampRemainderAccumulator += context->rampAreaRemainder; if (context->rampRemainderAccumulator >= context->rampPulseCount) { context->rampTargetAreaQ32++; context->rampRemainderAccumulator -= context->rampPulseCount; } if (context->rampRelativePulse + 1UL >= context->rampPulseCount) { context->rampTargetAreaQ32 = context->rampTotalAreaQ32; nextBoundaryQ32 = PLSR_PLANNER_Q32_ONE; } else if (context->rampRelativePulse == 0UL) { nextBoundaryQ32 = context->rampFirstBoundaryQ32; } else if (context->rampRelativePulse == 1UL) { nextBoundaryQ32 = context->rampSecondBoundaryQ32; } else { nextBoundaryQ32 = PlsrPlannerPredictedBoundaryQ32( context, context->rampTargetAreaQ32); } desiredDeltaQ32 = (nextBoundaryQ32 > context->rampActualTimeQ32) ? (nextBoundaryQ32 - context->rampActualTimeQ32) : 1ULL; /* The allocated pulse count closes the ramp area exactly. Derive the physical duration from N/averageHz instead of rounding it to whole milliseconds; short clipped ramps can be well below 1 ms. */ denominator = (uint64_t)context->rampPulseCount * desiredDeltaQ32; requestedHz = (denominator == 0ULL) ? context->rampToHz : (context->rampTotalAreaQ32 + denominator / 2ULL) / denominator; if (requestedHz == 0ULL) { requestedHz = 1ULL; } if (requestedHz > PLSR_FREQUENCY_MAX_HZ) { requestedHz = PLSR_FREQUENCY_MAX_HZ; } if ((context->lastRampHz != 0UL) && (((context->rampToHz > context->rampFromHz) && (requestedHz < context->lastRampHz)) || ((context->rampToHz < context->rampFromHz) && (requestedHz > context->lastRampHz)))) { requestedHz = context->lastRampHz; } if (PlsrPlannerBuildStep(context, (uint32_t)requestedHz, setting, requestedFrequencyHz) == 0U) { return 0U; } denominator = (uint64_t)context->rampPulseCount * setting->actualFrequencyHz; actualDeltaQ32 = (denominator == 0ULL) ? desiredDeltaQ32 : (context->rampTotalAreaQ32 + denominator / 2ULL) / denominator; context->rampActualTimeQ32 += actualDeltaQ32; context->rampLastPhaseStepQ32 = nextBoundaryQ32 - context->rampBoundaryQ32; context->rampBoundaryQ32 = nextBoundaryQ32; context->lastRampHz = setting->actualFrequencyHz; context->rampRelativePulse++; return 1U; } static uint8_t PlsrPlannerTakeStep(PLSR_PLANNER_CONTEXT *context, PLSR_PLATFORM_TIMER_SETTING *setting, uint32_t *requestedFrequencyHz, uint32_t *repeatCount) { uint32_t entryEnd = context->entryPulses; uint32_t steadyEnd = entryEnd + context->steadyPulses; *repeatCount = 1UL; if (context->generatedPulses >= context->block.pulseBudget) { return 0U; } if (context->generatedPulses < entryEnd) { if (context->rampKind != 1U) { PlsrPlannerStartRamp(context, 1U, context->startHz, context->peakHz, context->entryPulses); } return PlsrPlannerTakeRampStep(context, setting, requestedFrequencyHz); } if (context->generatedPulses < steadyEnd) { *repeatCount = steadyEnd - context->generatedPulses; context->rampKind = 0U; return PlsrPlannerBuildStep(context, context->peakHz, setting, requestedFrequencyHz); } if (context->rampKind != 2U) { PlsrPlannerStartRamp(context, 2U, context->peakHz, context->endHz, context->exitPulses); } return PlsrPlannerTakeRampStep(context, setting, requestedFrequencyHz); } PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context, const PLSR_MOTION_BLOCK *block, uint32_t appliedHz, uint64_t phasePulses) { uint64_t directRequired; uint64_t entryRequired; uint64_t exitRequired; uint64_t entryWeight; uint64_t exitWeight; uint64_t totalWeight; uint64_t scaledEntry; if ((context == NULL) || (block == NULL) || (block->pulseBudget == 0UL) || (block->referenceSpeedHz == 0UL) || (block->referenceSpeedHz > PLSR_FREQUENCY_MAX_HZ) || (block->entryHz > PLSR_FREQUENCY_MAX_HZ) || (block->cruiseHz == 0UL) || (block->cruiseHz > PLSR_FREQUENCY_MAX_HZ) || (block->exitHz > PLSR_FREQUENCY_MAX_HZ) || (appliedHz > PLSR_FREQUENCY_MAX_HZ) || (block->curveMode > 2U) || (block->pulseOutput > 3U)) { return PLSR_PLANNER_INVALID; } (void)memset(context, 0, sizeof(*context)); context->block = *block; context->phasePulses = phasePulses; context->startHz = (appliedHz != 0UL) ? appliedHz : block->entryHz; if (context->startHz == 0UL) { context->startHz = 1UL; } context->endHz = (block->exitHz == 0UL) ? 1UL : block->exitHz; directRequired = PlsrPlannerRequiredPulses( block, PlsrPlannerRampWeight( context->startHz, context->endHz, PlsrPlannerBaseRampTime(block, context->startHz, context->endHz))); if (directRequired > block->pulseBudget) { context->peakHz = PlsrPlannerReachableFrequency( block, context->startHz, context->endHz, block->pulseBudget); context->endHz = context->peakHz; context->entryPulses = block->pulseBudget; context->clipped = 1U; context->active = 1U; return PLSR_PLANNER_CLIPPED; } context->peakHz = PlsrPlannerPeak(block, context->startHz, context->endHz); entryWeight = PlsrPlannerRampWeight( context->startHz, context->peakHz, PlsrPlannerBaseRampTime(block, context->startHz, context->peakHz)); exitWeight = PlsrPlannerRampWeight( context->peakHz, context->endHz, PlsrPlannerBaseRampTime(block, context->peakHz, context->endHz)); entryRequired = PlsrPlannerRequiredPulses(block, entryWeight); exitRequired = PlsrPlannerRequiredPulses(block, exitWeight); if ((entryRequired + exitRequired) <= block->pulseBudget) { context->entryPulses = (uint32_t)entryRequired; context->exitPulses = (uint32_t)exitRequired; context->steadyPulses = block->pulseBudget - context->entryPulses - context->exitPulses; } else if (entryRequired == 0ULL) { context->exitPulses = block->pulseBudget; context->clipped = 1U; } else if (exitRequired == 0ULL) { context->entryPulses = block->pulseBudget; context->clipped = 1U; } else { totalWeight = entryWeight + exitWeight; scaledEntry = ((uint64_t)block->pulseBudget * entryWeight + totalWeight / 2ULL) / totalWeight; if (scaledEntry == 0ULL) { scaledEntry = 1ULL; } if (scaledEntry >= block->pulseBudget) { scaledEntry = block->pulseBudget - 1UL; } context->entryPulses = (uint32_t)scaledEntry; context->exitPulses = block->pulseBudget - context->entryPulses; context->clipped = 1U; } if (context->peakHz != block->cruiseHz) { context->clipped = 1U; } context->active = 1U; return (context->clipped != 0U) ? PLSR_PLANNER_CLIPPED : PLSR_PLANNER_OK; } uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context, PLSR_STREAM_ITEM *output, uint16_t capacity) { PLSR_PLATFORM_TIMER_SETTING setting; uint32_t requestedFrequencyHz; uint32_t repeatCount; uint16_t produced = 0U; if ((context == NULL) || (output == NULL) || (capacity == 0U) || (context->active == 0U)) { return 0U; } while ((produced < capacity) && (context->generatedPulses < context->block.pulseBudget)) { if (PlsrPlannerTakeStep(context, &setting, &requestedFrequencyHz, &repeatCount) == 0U) { context->active = 0U; break; } if ((produced != 0U) && (PlsrPlannerSameSetting(&output[produced - 1U].setting, &setting) != 0U) && (output[produced - 1U].requestedFrequencyHz == requestedFrequencyHz) && (output[produced - 1U].repeatCount <= 0xFFFFFFFFUL - repeatCount)) { output[produced - 1U].repeatCount += repeatCount; } else { output[produced].setting = setting; output[produced].requestedFrequencyHz = requestedFrequencyHz; output[produced].repeatCount = repeatCount; produced++; } context->generatedPulses += repeatCount; } if (context->generatedPulses >= context->block.pulseBudget) { context->active = 0U; } 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); }