yuwenhao
/
Modbus-rtu
1
0
Rozštěpit 0
Zdrojový kód Úkoly 0 Požadavky na natažení 0 Vydání 0 Wiki Aktivita
Nelze vybrat více než 25 témat Téma musí začínat písmenem nebo číslem, může obsahovat pomlčky („-“) a může být dlouhé až 35 znaků.
48 Revize
3 Větve
138 MiB
 
 
 
 
 
 
Strom: a6c667d485
Větve Značky
${ item.name }
Vytvořit větev ${ searchTerm }
z „a6c667d485“
${ noResults }
Modbus-rtu/PLSR/Src/plsr_planner.c

1222 řádky
42 KiB
Surový Blame Historie 

  1. #include "plsr_planner.h"

  2. 

  3. #include <stddef.h>

  4. #include <string.h>

  5. 

  6. #define PLSR_PLANNER_Q32_ONE (4294967296ULL)

  7. 

  8. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  9.     && !defined(PLSR_HOST_TEST)

  10. #define PLSR_PLANNER_CYCCNT_ADDRESS (0xE0001004UL)

  11. 

  12. volatile PLSR_PLANNER_TIMING PlsrPlannerTiming;

  13. 

  14. static uint32_t PlsrPlannerTimingNow(void)

  15. {

  16.     return *((volatile uint32_t *)PLSR_PLANNER_CYCCNT_ADDRESS);

  17. }

  18. 

  19. static void PlsrPlannerTimingRecord(

  20.     volatile PLSR_PLANNER_TIMING_SAMPLE *sample,

  21.     uint32_t startedAt)

  22. {

  23.     uint32_t elapsed = PlsrPlannerTimingNow() - startedAt;

  24.     uint32_t total = sample->totalCycles;

  25. 

  26.     sample->lastCycles = elapsed;

  27.     if ((sample->callCount == 0UL) || (elapsed < sample->minCycles))

  28.     {

  29.         sample->minCycles = elapsed;

  30.     }

  31.     if (sample->callCount != 0xFFFFFFFFUL)

  32.     {

  33.         sample->callCount++;

  34.     }

  35.     sample->totalCycles = (elapsed > (0xFFFFFFFFUL - total))

  36.                               ? 0xFFFFFFFFUL : total + elapsed;

  37.     if (elapsed > sample->maxCycles)

  38.     {

  39.         sample->maxCycles = elapsed;

  40.     }

  41. }

  42. 

  43. void PlsrPlannerTimingReset(void)

  44. {

  45.     (void)memset((void *)&PlsrPlannerTiming, 0, sizeof(PlsrPlannerTiming));

  46. }

  47. #endif

  48. 

  49. static const uint32_t PlsrPlannerSmoothIntegralQ24[65] =

  50. {

  51.     0UL, 64UL, 504UL, 1688UL, 3968UL, 7688UL, 13176UL, 20752UL,

  52.     30720UL, 43376UL, 59000UL, 77864UL, 100224UL, 126328UL, 156408UL,

  53.     190688UL, 229376UL, 272672UL, 320760UL, 373816UL, 432000UL,

  54.     495464UL, 564344UL, 638768UL, 718848UL, 804688UL, 896376UL,

  55.     993992UL, 1097600UL, 1207256UL, 1323000UL, 1444864UL, 1572864UL,

  56.     1707008UL, 1847288UL, 1993688UL, 2146176UL, 2304712UL, 2469240UL,

  57.     2639696UL, 2816000UL, 2998064UL, 3185784UL, 3379048UL, 3577728UL,

  58.     3781688UL, 3990776UL, 4204832UL, 4423680UL, 4647136UL, 4875000UL,

  59.     5107064UL, 5343104UL, 5582888UL, 5826168UL, 6072688UL, 6322176UL,

  60.     6574352UL, 6828920UL, 7085576UL, 7344000UL, 7603864UL, 7864824UL,

  61.     8126528UL, 8388608UL

  62. };

  63. 

  64. static const uint32_t PlsrPlannerSineIntegralQ24[65] =

  65. {

  66.     0UL, 53UL, 421UL, 1420UL, 3362UL, 6560UL, 11321UL, 17949UL,

  67.     26744UL, 38000UL, 52007UL, 69047UL, 89393UL, 113314UL, 141066UL,

  68.     172899UL, 209052UL, 249753UL, 295221UL, 345662UL, 401269UL,

  69.     462225UL, 528698UL, 600845UL, 678806UL, 762711UL, 852672UL,

  70.     948789UL, 1051146UL, 1159812UL, 1274841UL, 1396271UL, 1524127UL,

  71.     1658415UL, 1799129UL, 1946244UL, 2099722UL, 2259509UL, 2425536UL,

  72.     2597719UL, 2775958UL, 2960141UL, 3150138UL, 3345809UL, 3546997UL,

  73.     3753534UL, 3965237UL, 4181913UL, 4403356UL, 4629347UL, 4859658UL,

  74.     5094050UL, 5332273UL, 5574071UL, 5819175UL, 6067312UL, 6318200UL,

  75.     6571549UL, 6827065UL, 7084448UL, 7343394UL, 7603596UL, 7864741UL,

  76.     8126517UL, 8388608UL

  77. };

  78. 

  79. static uint32_t PlsrPlannerAbsDifference(uint32_t first, uint32_t second)

  80. {

  81.     return (first > second) ? (first - second) : (second - first);

  82. }

  83. 

  84. static uint32_t PlsrPlannerRampTime(const PLSR_MOTION_BLOCK *block,uint32_t fromHz, uint32_t toHz)

  85.                                     

  86.                                    

  87. {

  88.     uint32_t baseTimeMs;

  89.     uint64_t durationMs;

  90. 

  91.     if (fromHz == toHz)

  92.     {

  93.         return 0UL;

  94.     }

  95.     baseTimeMs = (toHz > fromHz) ? block->accelerationTimeMs

  96.                                  : block->decelerationTimeMs;

  97.     if (baseTimeMs == 0UL)

  98.     {

  99.         return 0UL;

  100.     }

  101.     durationMs = ((uint64_t)PlsrPlannerAbsDifference(fromHz, toHz)

  102.                   * baseTimeMs + block->referenceSpeedHz - 1UL)

  103.                  / block->referenceSpeedHz;

  104.     return (durationMs > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL

  105.                                         : (uint32_t)durationMs;

  106. }

  107. 

  108. static uint16_t PlsrPlannerBaseRampTime(const PLSR_MOTION_BLOCK *block,

  109.                                         uint32_t fromHz,

  110.                                         uint32_t toHz)

  111. {

  112.     if (toHz > fromHz)

  113.     {

  114.         return block->accelerationTimeMs;

  115.     }

  116.     if (toHz < fromHz)

  117.     {

  118.         return block->decelerationTimeMs;

  119.     }

  120.     return 0U;

  121. }

  122. 

  123. static uint64_t PlsrPlannerRampWeight(uint32_t fromHz,

  124.                                       uint32_t toHz,

  125.                                       uint16_t baseTimeMs)

  126. {

  127.     uint64_t fromSquared = (uint64_t)fromHz * fromHz;

  128.     uint64_t toSquared = (uint64_t)toHz * toHz;

  129.     uint64_t difference = (fromSquared > toSquared)

  130.                               ? (fromSquared - toSquared)

  131.                               : (toSquared - fromSquared);

  132. 

  133.     return difference * baseTimeMs;

  134. }

  135. 

  136. static uint64_t PlsrPlannerRequiredPulses(const PLSR_MOTION_BLOCK *block,

  137.                                           uint64_t rampWeight)

  138. {

  139.     uint64_t denominator = (uint64_t)2U * block->referenceSpeedHz * 1000UL;

  140. 

  141.     return (rampWeight == 0ULL) ? 0ULL

  142.                                 : (rampWeight + denominator - 1ULL)

  143.                                       / denominator;

  144. }

  145. 

  146. static uint32_t PlsrPlannerIntegerSquareRoot(uint64_t value)

  147. {

  148.     uint64_t bit = (uint64_t)1U << 62U;

  149.     uint64_t root = 0ULL;

  150. 

  151.     while (bit > value)

  152.     {

  153.         bit >>= 2U;

  154.     }

  155.     while (bit != 0ULL)

  156.     {

  157.         if (value >= root + bit)

  158.         {

  159.             value -= root + bit;

  160.             root = (root >> 1U) + bit;

  161.         }

  162.         else

  163.         {

  164.             root >>= 1U;

  165.         }

  166.         bit >>= 2U;

  167.     }

  168.     return (uint32_t)root;

  169. }

  170. 

  171. static uint32_t PlsrPlannerReachableFrequency(

  172.     const PLSR_MOTION_BLOCK *block,

  173.     uint32_t fromHz,

  174.     uint32_t towardHz,

  175.     uint32_t pulseCount)

  176. {

  177.     uint16_t baseTimeMs = PlsrPlannerBaseRampTime(block, fromHz, towardHz);

  178.     uint64_t frequencySquared = (uint64_t)fromHz * fromHz;

  179.     uint64_t changeSquared;

  180.     uint32_t reachableHz;

  181. 

  182.     if ((baseTimeMs == 0U) || (fromHz == towardHz))

  183.     {

  184.         return towardHz;

  185.     }

  186.     changeSquared = (uint64_t)2U * pulseCount * block->referenceSpeedHz

  187.                     * 1000UL / baseTimeMs;

  188.     if (towardHz > fromHz)

  189.     {

  190.         reachableHz = PlsrPlannerIntegerSquareRoot(

  191.             frequencySquared + changeSquared);

  192.         return (reachableHz > towardHz) ? towardHz : reachableHz;

  193.     }

  194.     frequencySquared = (changeSquared >= frequencySquared)

  195.                            ? 0ULL : (frequencySquared - changeSquared);

  196.     reachableHz = PlsrPlannerIntegerSquareRoot(frequencySquared);

  197.     if ((uint64_t)reachableHz * reachableHz < frequencySquared)

  198.     {

  199.         reachableHz++;

  200.     }

  201.     return (reachableHz < towardHz) ? towardHz : reachableHz;

  202. }

  203. 

  204. static uint32_t PlsrPlannerPeak(const PLSR_MOTION_BLOCK *block,

  205.                                 uint32_t startHz,

  206.                                 uint32_t endHz)

  207. {

  208.     uint32_t targetHz = block->cruiseHz;

  209.     uint32_t upperEndpoint = (startHz > endHz) ? startHz : endHz;

  210.     uint32_t lowerEndpoint = (startHz < endHz) ? startHz : endHz;

  211.     uint16_t entryTimeMs;

  212.     uint16_t exitTimeMs;

  213.     uint32_t timeSumMs;

  214.     uint64_t weightedEndpoints;

  215.     uint64_t availableArea;

  216.     uint64_t peakSquared;

  217.     uint32_t peakHz;

  218. 

  219.     if ((targetHz <= upperEndpoint) && (targetHz >= lowerEndpoint))

  220.     {

  221.         return targetHz;

  222.     }

  223.     entryTimeMs = PlsrPlannerBaseRampTime(block, startHz, targetHz);

  224.     exitTimeMs = PlsrPlannerBaseRampTime(block, targetHz, endHz);

  225.     timeSumMs = (uint32_t)entryTimeMs + exitTimeMs;

  226.     if (timeSumMs == 0UL)

  227.     {

  228.         return targetHz;

  229.     }

  230.     weightedEndpoints = (uint64_t)startHz * startHz * entryTimeMs

  231.                         + (uint64_t)endHz * endHz * exitTimeMs;

  232.     availableArea = (uint64_t)2U * block->pulseBudget

  233.                     * block->referenceSpeedHz * 1000UL;

  234. 

  235.     if (targetHz > upperEndpoint)

  236.     {

  237.         peakSquared = (availableArea + weightedEndpoints) / timeSumMs;

  238.         peakHz = PlsrPlannerIntegerSquareRoot(peakSquared);

  239.         if (peakHz < upperEndpoint)

  240.         {

  241.             peakHz = upperEndpoint;

  242.         }

  243.         return (peakHz > targetHz) ? targetHz : peakHz;

  244.     }

  245. 

  246.     if (availableArea >= weightedEndpoints)

  247.     {

  248.         return targetHz;

  249.     }

  250.     peakSquared = (weightedEndpoints - availableArea) / timeSumMs;

  251.     peakHz = PlsrPlannerIntegerSquareRoot(peakSquared);

  252.     if (peakHz < targetHz)

  253.     {

  254.         peakHz = targetHz;

  255.     }

  256.     return (peakHz > lowerEndpoint) ? lowerEndpoint : peakHz;

  257. }

  258. //C(x)函数

  259. //瞬时速度=delta*C(x)

  260. static uint64_t PlsrPlannerCurveIntegralQ32(uint64_t progressQ32,

  261.                                             uint16_t curveMode)

  262. {

  263.     const uint32_t *table;

  264.     uint64_t scaled;

  265.     uint32_t index;

  266.     uint32_t fraction;

  267.     uint64_t first;

  268.     uint64_t second;

  269. 

  270.     if (progressQ32 >= PLSR_PLANNER_Q32_ONE)

  271.     {

  272.         return PLSR_PLANNER_Q32_ONE / 2ULL;

  273.     }

  274.     if (curveMode == 0U)

  275.     {

  276.         return (progressQ32 * progressQ32) >> 33U;

  277.     }

  278.     table = (curveMode == 1U) ? PlsrPlannerSmoothIntegralQ24

  279.                               : PlsrPlannerSineIntegralQ24;

  280.     scaled = progressQ32 * 64ULL;

  281.     index = (uint32_t)(scaled >> 32U);

  282.     fraction = (uint32_t)scaled;

  283.     first = (uint64_t)table[index] << 8U;

  284.     second = (uint64_t)table[index + 1UL] << 8U;

  285.     return first + (((second - first) * fraction) >> 32U);

  286. }

  287. 

  288. static uint64_t PlsrPlannerRampAreaQ32(const PLSR_PLANNER_CONTEXT *context,

  289.                                        uint32_t fromHz,

  290.                                        uint32_t toHz,

  291.                                        uint64_t progressQ32)

  292. {

  293.     int64_t delta = (int64_t)toHz - (int64_t)fromHz;

  294.     int64_t area = (int64_t)((uint64_t)fromHz * progressQ32)

  295.                    + delta * (int64_t)PlsrPlannerCurveIntegralQ32(

  296.                        progressQ32, context->block.curveMode);

  297. 

  298.     return (uint64_t)area;

  299. }

  300. 

  301. static uint64_t PlsrPlannerExactBoundaryQ32(

  302.     const PLSR_PLANNER_CONTEXT *context,

  303.     uint64_t previousBoundaryQ32,

  304.     uint64_t targetAreaQ32)

  305. {

  306.     uint64_t lowerQ32 = previousBoundaryQ32;

  307.     uint64_t upperQ32 = PLSR_PLANNER_Q32_ONE;

  308.     uint64_t middleQ32;

  309.     uint32_t iteration;

  310. 

  311.     for (iteration = 0UL; iteration < 32UL; iteration++)

  312.     {

  313.         middleQ32 = lowerQ32 + ((upperQ32 - lowerQ32) >> 1U);

  314.         if (PlsrPlannerRampAreaQ32(context, context->rampFromHz,

  315.                                    context->rampToHz, middleQ32)

  316.             < targetAreaQ32)

  317.         {

  318.             lowerQ32 = middleQ32;

  319.         }

  320.         else

  321.         {

  322.             upperQ32 = middleQ32;

  323.         }

  324.     }

  325.     return upperQ32;

  326. }

  327. 

  328. static uint32_t PlsrPlannerInstantFrequency(

  329.     const PLSR_PLANNER_CONTEXT *context,

  330.     uint64_t progressQ32)

  331. {

  332.     const uint32_t *table;

  333.     uint64_t scaled;

  334.     uint64_t curveProgressQ32;

  335.     uint32_t index;

  336.     uint32_t gap;

  337. 

  338.     if (progressQ32 >= PLSR_PLANNER_Q32_ONE)

  339.     {

  340.         return context->rampToHz;

  341.     }

  342.     if (context->block.curveMode == 0U)

  343.     {

  344.         curveProgressQ32 = progressQ32;

  345.     }

  346.     else

  347.     {

  348.         table = (context->block.curveMode == 1U)

  349.                     ? PlsrPlannerSmoothIntegralQ24

  350.                     : PlsrPlannerSineIntegralQ24;

  351.         scaled = progressQ32 * 64ULL;

  352.         index = (uint32_t)(scaled >> 32U);

  353.         curveProgressQ32 =

  354.             (uint64_t)(table[index + 1UL] - table[index]) << 14U;

  355.     }

  356.     if (context->rampToHz >= context->rampFromHz)

  357.     {

  358.         gap = context->rampToHz - context->rampFromHz;

  359.         return context->rampFromHz

  360.                + (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U);

  361.     }

  362.     gap = context->rampFromHz - context->rampToHz;

  363.     return context->rampFromHz

  364.            - (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U);

  365. }

  366. 

  367. static uint64_t PlsrPlannerPredictedBoundaryQ32(

  368.     const PLSR_PLANNER_CONTEXT *context,

  369.     uint64_t targetAreaQ32)

  370. {

  371.     uint64_t previousQ32 = context->rampBoundaryQ32;

  372.     uint64_t currentAreaQ32 = (previousQ32 == 0ULL)

  373.                                   ? 0ULL

  374.                                   : PlsrPlannerRampAreaQ32(

  375.                                         context, context->rampFromHz,

  376.                                         context->rampToHz, previousQ32);

  377.     uint64_t candidateQ32;

  378.     uint64_t candidateAreaQ32;

  379.     uint64_t differenceQ32;

  380.     uint64_t correctionQ32;

  381.     uint32_t derivativeHz;

  382. 

  383.     if (context->rampLastPhaseStepQ32

  384.         >= PLSR_PLANNER_Q32_ONE - previousQ32)

  385.     {

  386.         candidateQ32 = PLSR_PLANNER_Q32_ONE;

  387.     }

  388.     else if (context->rampLastPhaseStepQ32 != 0ULL)

  389.     {

  390.         candidateQ32 = previousQ32 + context->rampLastPhaseStepQ32;

  391.     }

  392.     else

  393.     {

  394.         derivativeHz = PlsrPlannerInstantFrequency(context, previousQ32);

  395.         if (derivativeHz == 0UL)

  396.         {

  397.             derivativeHz = 1UL;

  398.         }

  399.         differenceQ32 = targetAreaQ32 - currentAreaQ32;

  400.         correctionQ32 = (differenceQ32 + derivativeHz - 1UL)

  401.                         / derivativeHz;

  402.         candidateQ32 = (correctionQ32

  403.                         >= PLSR_PLANNER_Q32_ONE - previousQ32)

  404.                            ? PLSR_PLANNER_Q32_ONE

  405.                            : previousQ32 + correctionQ32;

  406.     }

  407. 

  408.     candidateAreaQ32 = PlsrPlannerRampAreaQ32(

  409.         context, context->rampFromHz, context->rampToHz, candidateQ32);

  410.     derivativeHz = PlsrPlannerInstantFrequency(context, candidateQ32);

  411.     if (derivativeHz == 0UL)

  412.     {

  413.         derivativeHz = 1UL;

  414.     }

  415.     if (candidateAreaQ32 < targetAreaQ32)

  416.     {

  417.         differenceQ32 = targetAreaQ32 - candidateAreaQ32;

  418.         correctionQ32 = (differenceQ32 + derivativeHz - 1UL)

  419.                         / derivativeHz;

  420.         candidateQ32 = (correctionQ32

  421.                         >= PLSR_PLANNER_Q32_ONE - candidateQ32)

  422.                            ? PLSR_PLANNER_Q32_ONE

  423.                            : candidateQ32 + correctionQ32;

  424.     }

  425.     else if (candidateAreaQ32 > targetAreaQ32)

  426.     {

  427.         differenceQ32 = candidateAreaQ32 - targetAreaQ32;

  428.         correctionQ32 = differenceQ32 / derivativeHz;

  429.         if (correctionQ32 == 0ULL)

  430.         {

  431.             correctionQ32 = 1ULL;

  432.         }

  433.         candidateQ32 = (correctionQ32 >= candidateQ32 - previousQ32)

  434.                            ? previousQ32 + 1ULL

  435.                            : candidateQ32 - correctionQ32;

  436.     }

  437.     return candidateQ32;

  438. }

  439. 

  440. static void PlsrPlannerStartRamp(PLSR_PLANNER_CONTEXT *context,

  441.                                  uint8_t rampKind,

  442.                                  uint32_t fromHz,

  443.                                  uint32_t toHz,

  444.                                  uint32_t pulseCount)

  445. {

  446. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  447.     && !defined(PLSR_HOST_TEST)

  448.     uint32_t totalStartedAt = PlsrPlannerTimingNow();

  449.     uint32_t phaseStartedAt;

  450. #endif

  451. 

  452.     context->rampKind = rampKind;

  453.     context->rampRelativePulse = 0UL;

  454.     context->rampPulseCount = pulseCount;

  455.     context->rampFromHz = fromHz;

  456.     context->rampToHz = toHz;

  457. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  458.     && !defined(PLSR_HOST_TEST)

  459.     phaseStartedAt = PlsrPlannerTimingNow();

  460. #endif

  461.     context->rampDurationMs = PlsrPlannerRampTime(&context->block, fromHz, toHz);

  462. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  463.     && !defined(PLSR_HOST_TEST)

  464.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampTime, phaseStartedAt);

  465.     phaseStartedAt = PlsrPlannerTimingNow();

  466. #endif

  467.     context->rampTotalAreaQ32 = PlsrPlannerRampAreaQ32(

  468.         context, fromHz, toHz, PLSR_PLANNER_Q32_ONE);

  469. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  470.     && !defined(PLSR_HOST_TEST)

  471.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampTotalArea,

  472.                             phaseStartedAt);

  473.     phaseStartedAt = PlsrPlannerTimingNow();

  474. #endif

  475.     context->rampAreaStepQ32 = context->rampTotalAreaQ32 / pulseCount;

  476.     context->rampAreaRemainder =

  477.         (uint32_t)(context->rampTotalAreaQ32 % pulseCount);

  478. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  479.     && !defined(PLSR_HOST_TEST)

  480.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampAreaSplit,

  481.                             phaseStartedAt);

  482. #endif

  483.     context->rampRemainderAccumulator = 0UL;

  484.     context->rampTargetAreaQ32 = 0ULL;

  485.     context->rampBoundaryQ32 = 0ULL;

  486.     context->rampActualTimeQ32 = 0ULL;

  487.     context->rampLastPhaseStepQ32 = 0ULL;

  488.     context->lastRampHz = 0UL;

  489. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  490.     && !defined(PLSR_HOST_TEST)

  491.     phaseStartedAt = PlsrPlannerTimingNow();

  492. #endif

  493.     context->rampFirstBoundaryQ32 = PlsrPlannerExactBoundaryQ32(

  494.         context, 0ULL, context->rampAreaStepQ32);

  495. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  496.     && !defined(PLSR_HOST_TEST)

  497.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampFirstBoundary,

  498.                             phaseStartedAt);

  499. #endif

  500.     if (pulseCount > 1UL)

  501.     {

  502.         uint64_t secondTargetAreaQ32;

  503. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  504.     && !defined(PLSR_HOST_TEST)

  505.         phaseStartedAt = PlsrPlannerTimingNow();

  506. #endif

  507.         secondTargetAreaQ32 = context->rampAreaStepQ32 * 2ULL

  508.             + ((uint64_t)context->rampAreaRemainder * 2ULL) / pulseCount;

  509.         context->rampSecondBoundaryQ32 = PlsrPlannerExactBoundaryQ32(

  510.             context, context->rampFirstBoundaryQ32,

  511.             secondTargetAreaQ32);

  512. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  513.     && !defined(PLSR_HOST_TEST)

  514.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampSecondBoundary,

  515.                                 phaseStartedAt);

  516. #endif

  517.     }

  518.     else

  519.     {

  520.         context->rampSecondBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  521.     }

  522. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  523.     && !defined(PLSR_HOST_TEST)

  524.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.startRamp, totalStartedAt);

  525. #endif

  526. }

  527. 

  528. static uint8_t PlsrPlannerSameSetting(

  529.     const PLSR_PLATFORM_TIMER_SETTING *first,

  530.     const PLSR_PLATFORM_TIMER_SETTING *second)

  531. {

  532.     return ((first->actualFrequencyHz == second->actualFrequencyHz)

  533.             && (first->prescaler == second->prescaler)

  534.             && (first->pairPrescaler == second->pairPrescaler)

  535.             && (first->period == second->period)

  536.             && (first->compare == second->compare)) ? 1U : 0U;

  537. }

  538. 

  539. static uint8_t PlsrPlannerBuildStep(PLSR_PLANNER_CONTEXT *context,

  540.                                     uint32_t requestedHz,

  541.                                     PLSR_PLATFORM_TIMER_SETTING *setting,

  542.                                     uint32_t *normalizedRequestedHz)

  543. {

  544. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  545.     && !defined(PLSR_HOST_TEST)

  546.     uint32_t startedAt;

  547.     uint8_t result;

  548. #endif

  549. 

  550.     if (requestedHz == 0UL)

  551.     {

  552.         requestedHz = 1UL;

  553.     }

  554.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  555.     {

  556.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  557.     }

  558.     *normalizedRequestedHz = requestedHz;

  559. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  560.     && !defined(PLSR_HOST_TEST)

  561.     startedAt = PlsrPlannerTimingNow();

  562.     result = PlsrPlatformBuildTimerSetting(context->block.pulseOutput,

  563.                                            PLSR_OUTPUT_PULSE_DIR,

  564.                                            requestedHz, setting);

  565.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.timerSetting, startedAt);

  566.     return result;

  567. #else

  568.     return PlsrPlatformBuildTimerSetting(context->block.pulseOutput,

  569.                                          PLSR_OUTPUT_PULSE_DIR,

  570.                                          requestedHz, setting);

  571. #endif

  572. }

  573. 

  574. static uint8_t PlsrPlannerTakeRampStep(

  575.     PLSR_PLANNER_CONTEXT *context,

  576.     PLSR_PLATFORM_TIMER_SETTING *setting,

  577.     uint32_t *requestedFrequencyHz)

  578. {

  579.     uint64_t nextBoundaryQ32;

  580.     uint64_t desiredDeltaQ32;

  581.     uint64_t denominator;

  582.     uint64_t requestedHz;

  583.     uint64_t actualDeltaQ32;

  584. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  585.     && !defined(PLSR_HOST_TEST)

  586.     uint32_t totalStartedAt = PlsrPlannerTimingNow();

  587.     uint32_t phaseStartedAt;

  588. #endif

  589. 

  590.     context->rampTargetAreaQ32 += context->rampAreaStepQ32;

  591.     context->rampRemainderAccumulator += context->rampAreaRemainder;

  592.     if (context->rampRemainderAccumulator >= context->rampPulseCount)

  593.     {

  594.         context->rampTargetAreaQ32++;

  595.         context->rampRemainderAccumulator -= context->rampPulseCount;

  596.     }

  597.     if (context->rampRelativePulse + 1UL >= context->rampPulseCount)

  598.     {

  599.         context->rampTargetAreaQ32 = context->rampTotalAreaQ32;

  600.         nextBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  601.     }

  602.     else if (context->rampRelativePulse == 0UL)

  603.     {

  604.         nextBoundaryQ32 = context->rampFirstBoundaryQ32;

  605.     }

  606.     else if (context->rampRelativePulse == 1UL)

  607.     {

  608.         nextBoundaryQ32 = context->rampSecondBoundaryQ32;

  609.     }

  610.     else

  611.     {

  612. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  613.     && !defined(PLSR_HOST_TEST)

  614.         phaseStartedAt = PlsrPlannerTimingNow();

  615. #endif

  616.         nextBoundaryQ32 = PlsrPlannerPredictedBoundaryQ32(

  617.             context, context->rampTargetAreaQ32);

  618. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  619.     && !defined(PLSR_HOST_TEST)

  620.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampPredictedBoundary,

  621.                                 phaseStartedAt);

  622. #endif

  623.     }

  624. 

  625.     desiredDeltaQ32 = (nextBoundaryQ32 > context->rampActualTimeQ32)

  626.                           ? (nextBoundaryQ32 - context->rampActualTimeQ32)

  627.                           : 1ULL;

  628.     /* The allocated pulse count closes the ramp area exactly.  Derive the

  629.        physical duration from N/averageHz instead of rounding it to whole

  630.        milliseconds; short clipped ramps can be well below 1 ms. */

  631. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  632.     && !defined(PLSR_HOST_TEST)

  633.     phaseStartedAt = PlsrPlannerTimingNow();

  634. #endif

  635.     denominator = (uint64_t)context->rampPulseCount * desiredDeltaQ32;

  636.     requestedHz = (denominator == 0ULL)

  637.                       ? context->rampToHz

  638.                       : (context->rampTotalAreaQ32

  639.                          + denominator / 2ULL) / denominator;

  640. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  641.     && !defined(PLSR_HOST_TEST)

  642.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampRequestedDivide,

  643.                             phaseStartedAt);

  644. #endif

  645.     if (requestedHz == 0ULL)

  646.     {

  647.         requestedHz = 1ULL;

  648.     }

  649.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  650.     {

  651.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  652.     }

  653.     if ((context->lastRampHz != 0UL)

  654.         && (((context->rampToHz > context->rampFromHz)

  655.              && (requestedHz < context->lastRampHz))

  656.             || ((context->rampToHz < context->rampFromHz)

  657.                 && (requestedHz > context->lastRampHz))))

  658.     {

  659.         requestedHz = context->lastRampHz;

  660.     }

  661.     if (PlsrPlannerBuildStep(context, (uint32_t)requestedHz, setting,

  662.                              requestedFrequencyHz) == 0U)

  663.     {

  664. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  665.     && !defined(PLSR_HOST_TEST)

  666.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampStep,

  667.                                 totalStartedAt);

  668. #endif

  669.         return 0U;

  670.     }

  671. 

  672. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  673.     && !defined(PLSR_HOST_TEST)

  674.     phaseStartedAt = PlsrPlannerTimingNow();

  675. #endif

  676.     denominator = (uint64_t)context->rampPulseCount

  677.                   * setting->actualFrequencyHz;

  678.     actualDeltaQ32 = (denominator == 0ULL)

  679.                          ? desiredDeltaQ32

  680.                          : (context->rampTotalAreaQ32

  681.                             + denominator / 2ULL) / denominator;

  682. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  683.     && !defined(PLSR_HOST_TEST)

  684.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampActualDivide,

  685.                             phaseStartedAt);

  686. #endif

  687.     context->rampActualTimeQ32 += actualDeltaQ32;

  688.     context->rampLastPhaseStepQ32 =

  689.         nextBoundaryQ32 - context->rampBoundaryQ32;

  690.     context->rampBoundaryQ32 = nextBoundaryQ32;

  691.     context->lastRampHz = setting->actualFrequencyHz;

  692.     context->rampRelativePulse++;

  693. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  694.     && !defined(PLSR_HOST_TEST)

  695.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampStep, totalStartedAt);

  696. #endif

  697.     return 1U;

  698. }

  699. 

  700. static uint8_t PlsrPlannerTakeStep(PLSR_PLANNER_CONTEXT *context,

  701.                                    PLSR_PLATFORM_TIMER_SETTING *setting,

  702.                                    uint32_t *requestedFrequencyHz,

  703.                                    uint32_t *repeatCount)

  704. {

  705.     uint32_t entryEnd = context->entryPulses;

  706.     //加速段结束门槛。已经吐出的脉冲 < entryEnd 还在加速。例如加速 1000，这里就是 1000

  707.     uint32_t steadyEnd = entryEnd + context->steadyPulses;

  708.     //匀速段结束门槛。< steadyEnd 且 ≥ entryEnd 就是匀速。例如再加 5000 匀速，这里就是 6000。再往后是减速

  709.     *repeatCount = 1UL;

  710.     if (context->generatedPulses >= context->block.pulseBudget)

  711.     {

  712.         return 0U;

  713.     }

  714.     if (context->generatedPulses < entryEnd)

  715.     { //加速段 

  716.         if (context->rampKind != 1U)

  717.         {   // 第一次走进加速

  718.             PlsrPlannerStartRamp(context, 1U, context->startHz,context->peakHz, context->entryPulses);                              

  719.         }

  720.         return PlsrPlannerTakeRampStep(context, setting,requestedFrequencyHz);

  721.                                        

  722.     }

  723.     if (context->generatedPulses < steadyEnd)

  724.     {

  725.         *repeatCount = steadyEnd - context->generatedPulses;

  726.         context->rampKind = 0U;

  727.         return PlsrPlannerBuildStep(context, context->peakHz, setting,

  728.                                     requestedFrequencyHz);

  729.     }

  730.     if (context->rampKind != 2U)

  731.     {   // 第一次走进减速

  732.         PlsrPlannerStartRamp(context, 2U, context->peakHz,

  733.                              context->endHz, context->exitPulses);

  734.     }

  735.     return PlsrPlannerTakeRampStep(context, setting,

  736.                                    requestedFrequencyHz);

  737. }

  738. 

  739. PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context,

  740.                                      const PLSR_MOTION_BLOCK *block,

  741.                                      uint32_t appliedHz,

  742.                                      uint64_t phasePulses)

  743. {

  744.     uint64_t directRequired;

  745.     uint64_t entryRequired;

  746.     uint64_t exitRequired;

  747.     uint64_t entryWeight;

  748.     uint64_t exitWeight;

  749.     uint64_t totalWeight;

  750.     uint64_t scaledEntry;

  751. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  752.     && !defined(PLSR_HOST_TEST)

  753.     uint32_t startedAt = PlsrPlannerTimingNow();

  754. #endif

  755. 

  756.     if ((context == NULL) || (block == NULL)

  757.         || (block->pulseBudget == 0UL)

  758.         || (block->referenceSpeedHz == 0UL)

  759.         || (block->referenceSpeedHz > PLSR_FREQUENCY_MAX_HZ)

  760.         || (block->entryHz > PLSR_FREQUENCY_MAX_HZ)

  761.         || (block->cruiseHz == 0UL)

  762.         || (block->cruiseHz > PLSR_FREQUENCY_MAX_HZ)

  763.         || (block->exitHz > PLSR_FREQUENCY_MAX_HZ)

  764.         || (appliedHz > PLSR_FREQUENCY_MAX_HZ)

  765.         || (block->curveMode > 2U) || (block->pulseOutput > 3U))

  766.     {

  767. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  768.     && !defined(PLSR_HOST_TEST)

  769.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt);

  770. #endif

  771.         return PLSR_PLANNER_INVALID;

  772.     }

  773. 

  774.     (void)memset(context, 0, sizeof(*context));

  775.     context->block = *block;

  776.     context->phasePulses = phasePulses;

  777.     context->startHz = (appliedHz != 0UL) ? appliedHz : block->entryHz;

  778.     if (context->startHz == 0UL)

  779.     {

  780.         context->startHz = 1UL;

  781.     }

  782.     context->endHz = (block->exitHz == 0UL) ? 1UL : block->exitHz;

  783. 

  784.     directRequired = PlsrPlannerRequiredPulses(

  785.         block, PlsrPlannerRampWeight(

  786.                    context->startHz, context->endHz,

  787.                    PlsrPlannerBaseRampTime(block, context->startHz,

  788.                                            context->endHz)));

  789.     if (directRequired > block->pulseBudget)

  790.     {

  791.         context->peakHz = PlsrPlannerReachableFrequency(

  792.             block, context->startHz, context->endHz, block->pulseBudget);

  793.         context->endHz = context->peakHz;

  794.         context->entryPulses = block->pulseBudget;

  795.         context->clipped = 1U;

  796.         context->active = 1U;

  797. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  798.     && !defined(PLSR_HOST_TEST)

  799.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt);

  800. #endif

  801.         return PLSR_PLANNER_CLIPPED;

  802.     }

  803. 

  804.     context->peakHz = PlsrPlannerPeak(block, context->startHz,

  805.                                       context->endHz);

  806.     entryWeight = PlsrPlannerRampWeight(

  807.         context->startHz, context->peakHz,

  808.         PlsrPlannerBaseRampTime(block, context->startHz, context->peakHz));

  809.     exitWeight = PlsrPlannerRampWeight(

  810.         context->peakHz, context->endHz,

  811.         PlsrPlannerBaseRampTime(block, context->peakHz, context->endHz));

  812.     entryRequired = PlsrPlannerRequiredPulses(block, entryWeight);

  813.     exitRequired = PlsrPlannerRequiredPulses(block, exitWeight);

  814. 

  815.     if ((entryRequired + exitRequired) <= block->pulseBudget)

  816.     {

  817.         context->entryPulses = (uint32_t)entryRequired;

  818.         context->exitPulses = (uint32_t)exitRequired;

  819.         context->steadyPulses = block->pulseBudget

  820.                                 - context->entryPulses

  821.                                 - context->exitPulses;

  822.     }

  823.     else if (entryRequired == 0ULL)

  824.     {

  825.         context->exitPulses = block->pulseBudget;

  826.         context->clipped = 1U;

  827.     }

  828.     else if (exitRequired == 0ULL)

  829.     {

  830.         context->entryPulses = block->pulseBudget;

  831.         context->clipped = 1U;

  832.     }

  833.     else

  834.     {

  835.         totalWeight = entryWeight + exitWeight;

  836.         scaledEntry = ((uint64_t)block->pulseBudget * entryWeight

  837.                        + totalWeight / 2ULL) / totalWeight;

  838.         if (scaledEntry == 0ULL)

  839.         {

  840.             scaledEntry = 1ULL;

  841.         }

  842.         if (scaledEntry >= block->pulseBudget)

  843.         {

  844.             scaledEntry = block->pulseBudget - 1UL;

  845.         }

  846.         context->entryPulses = (uint32_t)scaledEntry;

  847.         context->exitPulses = block->pulseBudget

  848.                               - context->entryPulses;

  849.         context->clipped = 1U;

  850.     }

  851.     if (context->peakHz != block->cruiseHz)

  852.     {

  853.         context->clipped = 1U;

  854.     }

  855.     context->active = 1U;

  856. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  857.     && !defined(PLSR_HOST_TEST)

  858.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt);

  859. #endif

  860.     return (context->clipped != 0U) ? PLSR_PLANNER_CLIPPED

  861.                                     : PLSR_PLANNER_OK;

  862. }

  863. //返回值：实际写了几项（合并后的项数，不是脉冲数）。0 = 没吐出任何东西

  864. uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context,//规划账本：三段脉冲、已经吐了多少、斜坡面积指针

  865.                              PLSR_STREAM_ITEM *output,//输出数组，调用方准备好的格子

  866.                              uint16_t capacity)//这一次最多往 output 里写几项

  867. {

  868.     PLSR_PLATFORM_TIMER_SETTING setting;

  869.     uint32_t requestedFrequencyHz;

  870.     uint32_t repeatCount;

  871.     uint16_t produced = 0U;

  872. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  873.     && !defined(PLSR_HOST_TEST)

  874.     uint32_t startedAt = PlsrPlannerTimingNow();

  875. #endif

  876. 

  877.     if ((context == NULL) || (output == NULL) || (capacity == 0U)

  878.         || (context->active == 0U))

  879.     {

  880. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  881.     && !defined(PLSR_HOST_TEST)

  882.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.generate, startedAt);

  883. #endif

  884.         return 0U;

  885.     }

  886.     while ((produced < capacity)&& (context->generatedPulses < context->block.pulseBudget))

  887.     {  //输出数组还没写满&&本段预算还没全部交给外面。generatedPulses 按脉冲个数计

  888. 

  889.         if (PlsrPlannerTakeStep(context, &setting, &requestedFrequencyHz, &repeatCount) == 0U)

  890.         //本圈 TakeStep 算出的量化后 PSC/ARR/actualHz，先放栈上                       

  891.         {

  892.             context->active = 0U;//规划器关掉

  893.             break;

  894.         }

  895.         if ((produced != 0U)

  896.             && (PlsrPlannerSameSetting(&output[produced - 1U].setting,&setting) != 0U)                              

  897.             && (output[produced - 1U].requestedFrequencyHz

  898.                 == requestedFrequencyHz)

  899.             && (output[produced - 1U].repeatCount

  900.                 <= 0xFFFFFFFFUL - repeatCount))

  901.         {

  902.             output[produced - 1U].repeatCount += repeatCount;

  903.             //不用开新格

  904. 

  905.         }

  906.         else

  907.         {

  908.             output[produced].setting = setting;

  909.             output[produced].requestedFrequencyHz = requestedFrequencyHz;

  910.             output[produced].repeatCount = repeatCount;

  911.             produced++;

  912.         }

  913.         context->generatedPulses += repeatCount;

  914.     }

  915.     if (context->generatedPulses >= context->block.pulseBudget)

  916.     {

  917.         context->active = 0U;

  918.     }

  919. #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \

  920.     && !defined(PLSR_HOST_TEST)

  921.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.generate, startedAt);

  922. #endif

  923.     return produced;

  924. }

  925. 

  926. static uint64_t PlsrPlannerRampDurationUs(uint32_t pulseCount,

  927.                                            uint32_t fromHz,

  928.                                            uint32_t toHz)

  929. {

  930.     uint64_t frequencySum = (uint64_t)fromHz + toHz;

  931. 

  932.     if ((pulseCount == 0UL) || (frequencySum == 0ULL))

  933.     {

  934.         return 0ULL;

  935.     }

  936.     return ((uint64_t)2U * pulseCount * 1000000ULL

  937.             + frequencySum - 1ULL) / frequencySum;

  938. }

  939. 

  940. /* Return floor(numerator / denominator * 2^32) without requiring a

  941.    128-bit intermediate.  Both operands are bounded by the planner's

  942.    100 kHz Q32 ramp area, so the normalized remainder can be doubled safely. */

  943. static uint32_t PlsrPlannerRatioQ32(uint64_t numerator,

  944.                                     uint64_t denominator)

  945. {

  946.     uint64_t remainder;

  947.     uint32_t ratio = 0UL;

  948.     uint8_t bit;

  949. 

  950.     if ((numerator == 0ULL) || (denominator == 0ULL))

  951.     {

  952.         return 0UL;

  953.     }

  954.     if (numerator >= denominator)

  955.     {

  956.         return 0xFFFFFFFFUL;

  957.     }

  958.     remainder = numerator;

  959.     for (bit = 0U; bit < 32U; bit++)

  960.     {

  961.         ratio <<= 1U;

  962.         remainder <<= 1U;

  963.         if (remainder >= denominator)

  964.         {

  965.             remainder -= denominator;

  966.             ratio |= 1UL;

  967.         }

  968.     }

  969.     return ratio;

  970. }

  971. 

  972. static uint32_t PlsrPlannerRampPulsesAtTime(

  973.     const PLSR_PLANNER_CONTEXT *context,

  974.     uint32_t pulseCount,

  975.     uint32_t fromHz,

  976.     uint32_t toHz,

  977.     uint64_t elapsedUs,

  978.     uint64_t durationUs,

  979.     uint64_t *progressQ32)

  980. {

  981.     PLSR_PLANNER_CONTEXT ramp = *context;

  982.     uint64_t partialAreaQ32;

  983.     uint64_t totalAreaQ32;

  984.     uint64_t product;

  985.     uint32_t areaRatioQ32;

  986.     uint32_t result;

  987. 

  988.     if ((pulseCount == 0UL) || (elapsedUs == 0ULL)

  989.         || (durationUs == 0ULL))

  990.     {

  991.         *progressQ32 = 0ULL;

  992.         return 0UL;

  993.     }

  994.     if (elapsedUs >= durationUs)

  995.     {

  996.         *progressQ32 = PLSR_PLANNER_Q32_ONE;

  997.         return pulseCount;

  998.     }

  999.     *progressQ32 = (elapsedUs * PLSR_PLANNER_Q32_ONE) / durationUs;

  1000.     ramp.rampFromHz = fromHz;

  1001.     ramp.rampToHz = toHz;

  1002.     partialAreaQ32 = PlsrPlannerRampAreaQ32(

  1003.         &ramp, fromHz, toHz, *progressQ32);

  1004.     totalAreaQ32 = PlsrPlannerRampAreaQ32(

  1005.         &ramp, fromHz, toHz, PLSR_PLANNER_Q32_ONE);

  1006.     areaRatioQ32 = PlsrPlannerRatioQ32(partialAreaQ32,

  1007.                                        totalAreaQ32);

  1008.     product = (uint64_t)pulseCount * areaRatioQ32;

  1009.     result = (uint32_t)(product >> 32U);

  1010.     if ((uint32_t)product != 0UL)

  1011.     {

  1012.         result++;

  1013.     }

  1014.     return (result > pulseCount) ? pulseCount : result;

  1015. }

  1016. 

  1017. static uint8_t PlsrPlannerSetPredictedFrequency(

  1018.     const PLSR_PLANNER_CONTEXT *context,

  1019.     uint32_t fromHz,

  1020.     uint32_t toHz,

  1021.     uint64_t progressQ32,

  1022.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  1023. {

  1024.     PLSR_PLANNER_CONTEXT ramp = *context;

  1025.     PLSR_PLATFORM_TIMER_SETTING setting;

  1026.     uint32_t requestedHz;

  1027. 

  1028.     ramp.rampFromHz = fromHz;

  1029.     ramp.rampToHz = toHz;

  1030.     requestedHz = PlsrPlannerInstantFrequency(&ramp, progressQ32);

  1031.     if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput,

  1032.                                       PLSR_OUTPUT_PULSE_DIR,

  1033.                                       requestedHz, &setting) == 0U)

  1034.     {

  1035.         return 0U;

  1036.     }

  1037.     prediction->actualFrequencyHz = setting.actualFrequencyHz;

  1038.     return 1U;

  1039. }

  1040. 

  1041. static uint64_t PlsrPlannerRampTargetAreaQ32(uint64_t totalAreaQ32,

  1042.                                              uint32_t pulseCount,

  1043.                                              uint32_t pulseIndex)

  1044. {

  1045.     uint64_t step = totalAreaQ32 / pulseCount;

  1046.     uint64_t remainder = totalAreaQ32 % pulseCount;

  1047. 

  1048.     return step * pulseIndex

  1049.            + (remainder * pulseIndex) / pulseCount;

  1050. }

  1051. 

  1052. /* Predict the timer setting of the last complete ramp pulse at the deadline.

  1053.    A ramp pulse represents the average frequency between two equal-area curve

  1054.    boundaries; carrying that run setting is closer to the hardware state than

  1055.    carrying the mathematical instantaneous frequency at the boundary. */

  1056. static uint8_t PlsrPlannerSetPredictedRampRunFrequency(

  1057.     const PLSR_PLANNER_CONTEXT *context,

  1058.     uint32_t pulseCount,

  1059.     uint32_t fromHz,

  1060.     uint32_t toHz,

  1061.     uint32_t pulseIndex,

  1062.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  1063. {

  1064.     PLSR_PLANNER_CONTEXT ramp = *context;

  1065.     PLSR_PLATFORM_TIMER_SETTING setting;

  1066.     uint64_t totalAreaQ32;

  1067.     uint64_t previousTargetAreaQ32;

  1068.     uint64_t targetAreaQ32;

  1069.     uint64_t previousBoundaryQ32;

  1070.     uint64_t boundaryQ32;

  1071.     uint64_t denominator;

  1072.     uint64_t requestedHz;

  1073. 

  1074.     if ((pulseCount == 0UL) || (pulseIndex == 0UL))

  1075.     {

  1076.         return PlsrPlannerSetPredictedFrequency(

  1077.             context, fromHz, toHz, 0ULL, prediction);

  1078.     }

  1079.     if (pulseIndex > pulseCount)

  1080.     {

  1081.         pulseIndex = pulseCount;

  1082.     }

  1083.     ramp.rampFromHz = fromHz;

  1084.     ramp.rampToHz = toHz;

  1085.     totalAreaQ32 = PlsrPlannerRampAreaQ32(

  1086.         &ramp, fromHz, toHz, PLSR_PLANNER_Q32_ONE);

  1087.     previousTargetAreaQ32 = PlsrPlannerRampTargetAreaQ32(

  1088.         totalAreaQ32, pulseCount, pulseIndex - 1UL);

  1089.     targetAreaQ32 = PlsrPlannerRampTargetAreaQ32(

  1090.         totalAreaQ32, pulseCount, pulseIndex);

  1091.     previousBoundaryQ32 = (pulseIndex == 1UL)

  1092.                               ? 0ULL

  1093.                               : PlsrPlannerExactBoundaryQ32(

  1094.                                     &ramp, 0ULL,

  1095.                                     previousTargetAreaQ32);

  1096.     boundaryQ32 = (pulseIndex == pulseCount)

  1097.                       ? PLSR_PLANNER_Q32_ONE

  1098.                       : PlsrPlannerExactBoundaryQ32(

  1099.                             &ramp, previousBoundaryQ32,

  1100.                             targetAreaQ32);

  1101.     if (boundaryQ32 <= previousBoundaryQ32)

  1102.     {

  1103.         return 0U;

  1104.     }

  1105.     denominator = (uint64_t)pulseCount

  1106.                   * (boundaryQ32 - previousBoundaryQ32);

  1107.     requestedHz = (denominator == 0ULL)

  1108.                       ? toHz

  1109.                       : (totalAreaQ32 + denominator / 2ULL)

  1110.                             / denominator;

  1111.     if (requestedHz == 0ULL)

  1112.     {

  1113.         requestedHz = 1ULL;

  1114.     }

  1115.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  1116.     {

  1117.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  1118.     }

  1119.     if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput,

  1120.                                       PLSR_OUTPUT_PULSE_DIR,

  1121.                                       (uint32_t)requestedHz,

  1122.                                       &setting) == 0U)

  1123.     {

  1124.         return 0U;

  1125.     }

  1126.     prediction->actualFrequencyHz = setting.actualFrequencyHz;

  1127.     return 1U;

  1128. }

  1129. 

  1130. uint8_t PlsrPlannerPredictTime(

  1131.     const PLSR_PLANNER_CONTEXT *context,

  1132.     uint32_t elapsedUs,

  1133.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  1134. {

  1135.     PLSR_PLATFORM_TIMER_SETTING steadySetting;

  1136.     uint64_t remainingUs = elapsedUs;

  1137.     uint64_t durationUs;

  1138.     uint64_t progressQ32;

  1139.     uint64_t partialPulses;

  1140. 

  1141.     if ((context == NULL) || (prediction == NULL)

  1142.         || (context->block.pulseBudget == 0UL))

  1143.     {

  1144.         return 0U;

  1145.     }

  1146.     (void)memset(prediction, 0, sizeof(*prediction));

  1147. 

  1148.     durationUs = PlsrPlannerRampDurationUs(

  1149.         context->entryPulses, context->startHz, context->peakHz);

  1150.     if ((context->entryPulses != 0UL) && (remainingUs <= durationUs))

  1151.     {

  1152.         prediction->pulseCount = PlsrPlannerRampPulsesAtTime(

  1153.             context, context->entryPulses, context->startHz,

  1154.             context->peakHz, remainingUs, durationUs, &progressQ32);

  1155.         prediction->phase = PLSR_PLANNER_PHASE_ENTRY;

  1156.         prediction->deadlineInProfile = 1U;

  1157.         return PlsrPlannerSetPredictedRampRunFrequency(

  1158.             context, context->entryPulses, context->startHz,

  1159.             context->peakHz, prediction->pulseCount, prediction);

  1160.     }

  1161.     if (context->entryPulses != 0UL)

  1162.     {

  1163.         remainingUs -= durationUs;

  1164.     }

  1165. 

  1166.     if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput,

  1167.                                       PLSR_OUTPUT_PULSE_DIR,

  1168.                                       context->peakHz,

  1169.                                       &steadySetting) == 0U)

  1170.     {

  1171.         return 0U;

  1172.     }

  1173.     durationUs = (context->steadyPulses == 0UL)

  1174.                      ? 0ULL

  1175.                      : ((uint64_t)context->steadyPulses * 1000000ULL

  1176.                         + steadySetting.actualFrequencyHz - 1UL)

  1177.                            / steadySetting.actualFrequencyHz;

  1178.     if ((context->steadyPulses != 0UL) && (remainingUs <= durationUs))

  1179.     {

  1180.         partialPulses = (remainingUs * steadySetting.actualFrequencyHz

  1181.                          + 999999ULL) / 1000000ULL;

  1182.         if (partialPulses > context->steadyPulses)

  1183.         {

  1184.             partialPulses = context->steadyPulses;

  1185.         }

  1186.         prediction->pulseCount = context->entryPulses

  1187.                                  + (uint32_t)partialPulses;

  1188.         prediction->actualFrequencyHz = steadySetting.actualFrequencyHz;

  1189.         prediction->phase = PLSR_PLANNER_PHASE_STEADY;

  1190.         prediction->deadlineInProfile = 1U;

  1191.         return 1U;

  1192.     }

  1193.     if (context->steadyPulses != 0UL)

  1194.     {

  1195.         remainingUs -= durationUs;

  1196.     }

  1197. 

  1198.     durationUs = PlsrPlannerRampDurationUs(

  1199.         context->exitPulses, context->peakHz, context->endHz);

  1200.     if ((context->exitPulses != 0UL) && (remainingUs <= durationUs))

  1201.     {

  1202.         partialPulses = PlsrPlannerRampPulsesAtTime(

  1203.             context, context->exitPulses, context->peakHz,

  1204.             context->endHz, remainingUs, durationUs, &progressQ32);

  1205.         prediction->pulseCount = context->entryPulses

  1206.                                  + context->steadyPulses

  1207.                                  + (uint32_t)partialPulses;

  1208.         prediction->phase = PLSR_PLANNER_PHASE_EXIT;

  1209.         prediction->deadlineInProfile = 1U;

  1210.         return PlsrPlannerSetPredictedRampRunFrequency(

  1211.             context, context->exitPulses, context->peakHz,

  1212.             context->endHz, (uint32_t)partialPulses, prediction);

  1213.     }

  1214. 

  1215.     prediction->pulseCount = context->block.pulseBudget;

  1216.     prediction->phase = PLSR_PLANNER_PHASE_COMPLETE;

  1217.     prediction->deadlineInProfile = 0U;

  1218.     return PlsrPlannerSetPredictedFrequency(

  1219.         context, context->endHz, context->endHz,

  1220.         PLSR_PLANNER_Q32_ONE, prediction);

  1221. }