yuwenhao
/
Modbus-rtu
1
0
Forka 0
Codice Problemi 0 Pull Requests 0 Rilasci 0 Wiki Attività
Non puoi selezionare più di 25 argomenti Gli argomenti devono iniziare con una lettera o un numero, possono includere trattini ('-') e possono essere lunghi fino a 35 caratteri.
33 Commit
3 Rami (Branch)
138 MiB
 
 
 
 
 
 
Albero (Tree): 4c3bee684a
Rami (Branch) Tag
${ item.name }
Crea branch ${ searchTerm }
da '4c3bee684a'
${ noResults }
Modbus-rtu/PLSR/Src/plsr_planner.c

749 righe
25 KiB
Originale Blame Cronologia 

  1. #include "plsr_planner.h"

  2. 

  3. #include <stddef.h>

  4. #include <string.h>

  5. 

  6. #define PLSR_PLANNER_Q32_ONE (4294967296ULL)

  7. 

  8. static const uint32_t PlsrPlannerSmoothIntegralQ24[65] =

  9. {

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

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

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

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

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

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

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

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

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

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

  20.     8126528UL, 8388608UL

  21. };

  22. 

  23. static const uint32_t PlsrPlannerSineIntegralQ24[65] =

  24. {

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

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

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

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

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

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

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

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

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

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

  35.     8126517UL, 8388608UL

  36. };

  37. 

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

  39. {

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

  41. }

  42. 

  43. static uint32_t PlsrPlannerRampTime(const PLSR_MOTION_BLOCK *block,

  44.                                     uint32_t fromHz,

  45.                                     uint32_t toHz)

  46. {

  47.     uint32_t baseTimeMs;

  48.     uint64_t durationMs;

  49. 

  50.     if (fromHz == toHz)

  51.     {

  52.         return 0UL;

  53.     }

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

  55.                                  : block->decelerationTimeMs;

  56.     if (baseTimeMs == 0UL)

  57.     {

  58.         return 0UL;

  59.     }

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

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

  62.                  / block->referenceSpeedHz;

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

  64.                                         : (uint32_t)durationMs;

  65. }

  66. 

  67. static uint16_t PlsrPlannerBaseRampTime(const PLSR_MOTION_BLOCK *block,

  68.                                         uint32_t fromHz,

  69.                                         uint32_t toHz)

  70. {

  71.     if (toHz > fromHz)

  72.     {

  73.         return block->accelerationTimeMs;

  74.     }

  75.     if (toHz < fromHz)

  76.     {

  77.         return block->decelerationTimeMs;

  78.     }

  79.     return 0U;

  80. }

  81. 

  82. static uint64_t PlsrPlannerRampWeight(uint32_t fromHz,

  83.                                       uint32_t toHz,

  84.                                       uint16_t baseTimeMs)

  85. {

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

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

  88.     uint64_t difference = (fromSquared > toSquared)

  89.                               ? (fromSquared - toSquared)

  90.                               : (toSquared - fromSquared);

  91. 

  92.     return difference * baseTimeMs;

  93. }

  94. 

  95. static uint64_t PlsrPlannerRequiredPulses(const PLSR_MOTION_BLOCK *block,

  96.                                           uint64_t rampWeight)

  97. {

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

  99. 

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

  101.                                 : (rampWeight + denominator - 1ULL)

  102.                                       / denominator;

  103. }

  104. 

  105. static uint32_t PlsrPlannerIntegerSquareRoot(uint64_t value)

  106. {

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

  108.     uint64_t root = 0ULL;

  109. 

  110.     while (bit > value)

  111.     {

  112.         bit >>= 2U;

  113.     }

  114.     while (bit != 0ULL)

  115.     {

  116.         if (value >= root + bit)

  117.         {

  118.             value -= root + bit;

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

  120.         }

  121.         else

  122.         {

  123.             root >>= 1U;

  124.         }

  125.         bit >>= 2U;

  126.     }

  127.     return (uint32_t)root;

  128. }

  129. 

  130. static uint32_t PlsrPlannerReachableFrequency(

  131.     const PLSR_MOTION_BLOCK *block,

  132.     uint32_t fromHz,

  133.     uint32_t towardHz,

  134.     uint32_t pulseCount)

  135. {

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

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

  138.     uint64_t changeSquared;

  139.     uint32_t reachableHz;

  140. 

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

  142.     {

  143.         return towardHz;

  144.     }

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

  146.                     * 1000UL / baseTimeMs;

  147.     if (towardHz > fromHz)

  148.     {

  149.         reachableHz = PlsrPlannerIntegerSquareRoot(

  150.             frequencySquared + changeSquared);

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

  152.     }

  153.     frequencySquared = (changeSquared >= frequencySquared)

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

  155.     reachableHz = PlsrPlannerIntegerSquareRoot(frequencySquared);

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

  157.     {

  158.         reachableHz++;

  159.     }

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

  161. }

  162. 

  163. static uint32_t PlsrPlannerPeak(const PLSR_MOTION_BLOCK *block,

  164.                                 uint32_t startHz,

  165.                                 uint32_t endHz)

  166. {

  167.     uint32_t targetHz = block->cruiseHz;

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

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

  170.     uint16_t entryTimeMs;

  171.     uint16_t exitTimeMs;

  172.     uint32_t timeSumMs;

  173.     uint64_t weightedEndpoints;

  174.     uint64_t availableArea;

  175.     uint64_t peakSquared;

  176.     uint32_t peakHz;

  177. 

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

  179.     {

  180.         return targetHz;

  181.     }

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

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

  184.     timeSumMs = (uint32_t)entryTimeMs + exitTimeMs;

  185.     if (timeSumMs == 0UL)

  186.     {

  187.         return targetHz;

  188.     }

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

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

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

  192.                     * block->referenceSpeedHz * 1000UL;

  193. 

  194.     if (targetHz > upperEndpoint)

  195.     {

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

  197.         peakHz = PlsrPlannerIntegerSquareRoot(peakSquared);

  198.         if (peakHz < upperEndpoint)

  199.         {

  200.             peakHz = upperEndpoint;

  201.         }

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

  203.     }

  204. 

  205.     if (availableArea >= weightedEndpoints)

  206.     {

  207.         return targetHz;

  208.     }

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

  210.     peakHz = PlsrPlannerIntegerSquareRoot(peakSquared);

  211.     if (peakHz < targetHz)

  212.     {

  213.         peakHz = targetHz;

  214.     }

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

  216. }

  217. 

  218. static uint64_t PlsrPlannerCurveIntegralQ32(uint64_t progressQ32,

  219.                                             uint16_t curveMode)

  220. {

  221.     const uint32_t *table;

  222.     uint64_t scaled;

  223.     uint32_t index;

  224.     uint32_t fraction;

  225.     uint64_t first;

  226.     uint64_t second;

  227. 

  228.     if (progressQ32 >= PLSR_PLANNER_Q32_ONE)

  229.     {

  230.         return PLSR_PLANNER_Q32_ONE / 2ULL;

  231.     }

  232.     if (curveMode == 0U)

  233.     {

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

  235.     }

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

  237.                               : PlsrPlannerSineIntegralQ24;

  238.     scaled = progressQ32 * 64ULL;

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

  240.     fraction = (uint32_t)scaled;

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

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

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

  244. }

  245. 

  246. static uint64_t PlsrPlannerRampAreaQ32(const PLSR_PLANNER_CONTEXT *context,

  247.                                        uint32_t fromHz,

  248.                                        uint32_t toHz,

  249.                                        uint64_t progressQ32)

  250. {

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

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

  253.                    + delta * (int64_t)PlsrPlannerCurveIntegralQ32(

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

  255. 

  256.     return (uint64_t)area;

  257. }

  258. 

  259. static uint64_t PlsrPlannerExactBoundaryQ32(

  260.     const PLSR_PLANNER_CONTEXT *context,

  261.     uint64_t previousBoundaryQ32,

  262.     uint64_t targetAreaQ32)

  263. {

  264.     uint64_t lowerQ32 = previousBoundaryQ32;

  265.     uint64_t upperQ32 = PLSR_PLANNER_Q32_ONE;

  266.     uint64_t middleQ32;

  267.     uint32_t iteration;

  268. 

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

  270.     {

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

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

  273.                                    context->rampToHz, middleQ32)

  274.             < targetAreaQ32)

  275.         {

  276.             lowerQ32 = middleQ32;

  277.         }

  278.         else

  279.         {

  280.             upperQ32 = middleQ32;

  281.         }

  282.     }

  283.     return upperQ32;

  284. }

  285. 

  286. static uint32_t PlsrPlannerInstantFrequency(

  287.     const PLSR_PLANNER_CONTEXT *context,

  288.     uint64_t progressQ32)

  289. {

  290.     const uint32_t *table;

  291.     uint64_t scaled;

  292.     uint64_t curveProgressQ32;

  293.     uint32_t index;

  294.     uint32_t gap;

  295. 

  296.     if (progressQ32 >= PLSR_PLANNER_Q32_ONE)

  297.     {

  298.         return context->rampToHz;

  299.     }

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

  301.     {

  302.         curveProgressQ32 = progressQ32;

  303.     }

  304.     else

  305.     {

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

  307.                     ? PlsrPlannerSmoothIntegralQ24

  308.                     : PlsrPlannerSineIntegralQ24;

  309.         scaled = progressQ32 * 64ULL;

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

  311.         curveProgressQ32 =

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

  313.     }

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

  315.     {

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

  317.         return context->rampFromHz

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

  319.     }

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

  321.     return context->rampFromHz

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

  323. }

  324. 

  325. static uint64_t PlsrPlannerPredictedBoundaryQ32(

  326.     const PLSR_PLANNER_CONTEXT *context,

  327.     uint64_t targetAreaQ32)

  328. {

  329.     uint64_t previousQ32 = context->rampBoundaryQ32;

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

  331.                                   ? 0ULL

  332.                                   : PlsrPlannerRampAreaQ32(

  333.                                         context, context->rampFromHz,

  334.                                         context->rampToHz, previousQ32);

  335.     uint64_t candidateQ32;

  336.     uint64_t candidateAreaQ32;

  337.     uint64_t differenceQ32;

  338.     uint64_t correctionQ32;

  339.     uint32_t derivativeHz;

  340. 

  341.     if (context->rampLastPhaseStepQ32

  342.         >= PLSR_PLANNER_Q32_ONE - previousQ32)

  343.     {

  344.         candidateQ32 = PLSR_PLANNER_Q32_ONE;

  345.     }

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

  347.     {

  348.         candidateQ32 = previousQ32 + context->rampLastPhaseStepQ32;

  349.     }

  350.     else

  351.     {

  352.         derivativeHz = PlsrPlannerInstantFrequency(context, previousQ32);

  353.         if (derivativeHz == 0UL)

  354.         {

  355.             derivativeHz = 1UL;

  356.         }

  357.         differenceQ32 = targetAreaQ32 - currentAreaQ32;

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

  359.                         / derivativeHz;

  360.         candidateQ32 = (correctionQ32

  361.                         >= PLSR_PLANNER_Q32_ONE - previousQ32)

  362.                            ? PLSR_PLANNER_Q32_ONE

  363.                            : previousQ32 + correctionQ32;

  364.     }

  365. 

  366.     candidateAreaQ32 = PlsrPlannerRampAreaQ32(

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

  368.     derivativeHz = PlsrPlannerInstantFrequency(context, candidateQ32);

  369.     if (derivativeHz == 0UL)

  370.     {

  371.         derivativeHz = 1UL;

  372.     }

  373.     if (candidateAreaQ32 < targetAreaQ32)

  374.     {

  375.         differenceQ32 = targetAreaQ32 - candidateAreaQ32;

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

  377.                         / derivativeHz;

  378.         candidateQ32 = (correctionQ32

  379.                         >= PLSR_PLANNER_Q32_ONE - candidateQ32)

  380.                            ? PLSR_PLANNER_Q32_ONE

  381.                            : candidateQ32 + correctionQ32;

  382.     }

  383.     else if (candidateAreaQ32 > targetAreaQ32)

  384.     {

  385.         differenceQ32 = candidateAreaQ32 - targetAreaQ32;

  386.         correctionQ32 = differenceQ32 / derivativeHz;

  387.         if (correctionQ32 == 0ULL)

  388.         {

  389.             correctionQ32 = 1ULL;

  390.         }

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

  392.                            ? previousQ32 + 1ULL

  393.                            : candidateQ32 - correctionQ32;

  394.     }

  395.     return candidateQ32;

  396. }

  397. 

  398. static void PlsrPlannerStartRamp(PLSR_PLANNER_CONTEXT *context,

  399.                                  uint8_t rampKind,

  400.                                  uint32_t fromHz,

  401.                                  uint32_t toHz,

  402.                                  uint32_t pulseCount)

  403. {

  404.     context->rampKind = rampKind;

  405.     context->rampRelativePulse = 0UL;

  406.     context->rampPulseCount = pulseCount;

  407.     context->rampFromHz = fromHz;

  408.     context->rampToHz = toHz;

  409.     context->rampDurationMs = PlsrPlannerRampTime(&context->block,

  410.                                                   fromHz, toHz);

  411.     context->rampTotalAreaQ32 = PlsrPlannerRampAreaQ32(

  412.         context, fromHz, toHz, PLSR_PLANNER_Q32_ONE);

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

  414.     context->rampAreaRemainder =

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

  416.     context->rampRemainderAccumulator = 0UL;

  417.     context->rampTargetAreaQ32 = 0ULL;

  418.     context->rampBoundaryQ32 = 0ULL;

  419.     context->rampActualTimeQ32 = 0ULL;

  420.     context->rampLastPhaseStepQ32 = 0ULL;

  421.     context->lastRampHz = 0UL;

  422.     context->rampFirstBoundaryQ32 = PlsrPlannerExactBoundaryQ32(

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

  424.     if (pulseCount > 1UL)

  425.     {

  426.         uint64_t secondTargetAreaQ32 = context->rampAreaStepQ32 * 2ULL

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

  428.         context->rampSecondBoundaryQ32 = PlsrPlannerExactBoundaryQ32(

  429.             context, context->rampFirstBoundaryQ32,

  430.             secondTargetAreaQ32);

  431.     }

  432.     else

  433.     {

  434.         context->rampSecondBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  435.     }

  436. }

  437. 

  438. static uint8_t PlsrPlannerSameSetting(

  439.     const PLSR_PLATFORM_TIMER_SETTING *first,

  440.     const PLSR_PLATFORM_TIMER_SETTING *second)

  441. {

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

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

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

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

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

  447. }

  448. 

  449. static uint8_t PlsrPlannerBuildStep(PLSR_PLANNER_CONTEXT *context,

  450.                                     uint32_t requestedHz,

  451.                                     PLSR_PLATFORM_TIMER_SETTING *setting,

  452.                                     uint32_t *normalizedRequestedHz)

  453. {

  454.     if (requestedHz == 0UL)

  455.     {

  456.         requestedHz = 1UL;

  457.     }

  458.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  459.     {

  460.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  461.     }

  462.     *normalizedRequestedHz = requestedHz;

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

  464.                                          PLSR_OUTPUT_PULSE_DIR,

  465.                                          requestedHz, setting);

  466. }

  467. 

  468. static uint8_t PlsrPlannerTakeRampStep(

  469.     PLSR_PLANNER_CONTEXT *context,

  470.     PLSR_PLATFORM_TIMER_SETTING *setting,

  471.     uint32_t *requestedFrequencyHz)

  472. {

  473.     uint64_t nextBoundaryQ32;

  474.     uint64_t desiredDeltaQ32;

  475.     uint64_t denominator;

  476.     uint64_t requestedHz;

  477.     uint64_t actualDeltaQ32;

  478. 

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

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

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

  482.     {

  483.         context->rampTargetAreaQ32++;

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

  485.     }

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

  487.     {

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

  489.         nextBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  490.     }

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

  492.     {

  493.         nextBoundaryQ32 = context->rampFirstBoundaryQ32;

  494.     }

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

  496.     {

  497.         nextBoundaryQ32 = context->rampSecondBoundaryQ32;

  498.     }

  499.     else

  500.     {

  501.         nextBoundaryQ32 = PlsrPlannerPredictedBoundaryQ32(

  502.             context, context->rampTargetAreaQ32);

  503.     }

  504. 

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

  506.                           ? (nextBoundaryQ32 - context->rampActualTimeQ32)

  507.                           : 1ULL;

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

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

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

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

  512.     requestedHz = (denominator == 0ULL)

  513.                       ? context->rampToHz

  514.                       : (context->rampTotalAreaQ32

  515.                          + denominator / 2ULL) / denominator;

  516.     if (requestedHz == 0ULL)

  517.     {

  518.         requestedHz = 1ULL;

  519.     }

  520.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  521.     {

  522.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  523.     }

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

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

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

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

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

  529.     {

  530.         requestedHz = context->lastRampHz;

  531.     }

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

  533.                              requestedFrequencyHz) == 0U)

  534.     {

  535.         return 0U;

  536.     }

  537. 

  538.     denominator = (uint64_t)context->rampPulseCount

  539.                   * setting->actualFrequencyHz;

  540.     actualDeltaQ32 = (denominator == 0ULL)

  541.                          ? desiredDeltaQ32

  542.                          : (context->rampTotalAreaQ32

  543.                             + denominator / 2ULL) / denominator;

  544.     context->rampActualTimeQ32 += actualDeltaQ32;

  545.     context->rampLastPhaseStepQ32 =

  546.         nextBoundaryQ32 - context->rampBoundaryQ32;

  547.     context->rampBoundaryQ32 = nextBoundaryQ32;

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

  549.     context->rampRelativePulse++;

  550.     return 1U;

  551. }

  552. 

  553. static uint8_t PlsrPlannerTakeStep(PLSR_PLANNER_CONTEXT *context,

  554.                                    PLSR_PLATFORM_TIMER_SETTING *setting,

  555.                                    uint32_t *requestedFrequencyHz,

  556.                                    uint32_t *repeatCount)

  557. {

  558.     uint32_t entryEnd = context->entryPulses;

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

  560. 

  561.     *repeatCount = 1UL;

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

  563.     {

  564.         return 0U;

  565.     }

  566.     if (context->generatedPulses < entryEnd)

  567.     {

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

  569.         {

  570.             PlsrPlannerStartRamp(context, 1U, context->startHz,

  571.                                  context->peakHz, context->entryPulses);

  572.         }

  573.         return PlsrPlannerTakeRampStep(context, setting,

  574.                                        requestedFrequencyHz);

  575.     }

  576.     if (context->generatedPulses < steadyEnd)

  577.     {

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

  579.         context->rampKind = 0U;

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

  581.                                     requestedFrequencyHz);

  582.     }

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

  584.     {

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

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

  587.     }

  588.     return PlsrPlannerTakeRampStep(context, setting,

  589.                                    requestedFrequencyHz);

  590. }

  591. 

  592. PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context,

  593.                                      const PLSR_MOTION_BLOCK *block,

  594.                                      uint32_t appliedHz,

  595.                                      uint64_t phasePulses)

  596. {

  597.     uint64_t directRequired;

  598.     uint64_t entryRequired;

  599.     uint64_t exitRequired;

  600.     uint64_t entryWeight;

  601.     uint64_t exitWeight;

  602.     uint64_t totalWeight;

  603.     uint64_t scaledEntry;

  604. 

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

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

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

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

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

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

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

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

  613.         || (appliedHz > PLSR_FREQUENCY_MAX_HZ)

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

  615.     {

  616.         return PLSR_PLANNER_INVALID;

  617.     }

  618. 

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

  620.     context->block = *block;

  621.     context->phasePulses = phasePulses;

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

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

  624.     {

  625.         context->startHz = 1UL;

  626.     }

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

  628. 

  629.     directRequired = PlsrPlannerRequiredPulses(

  630.         block, PlsrPlannerRampWeight(

  631.                    context->startHz, context->endHz,

  632.                    PlsrPlannerBaseRampTime(block, context->startHz,

  633.                                            context->endHz)));

  634.     if (directRequired > block->pulseBudget)

  635.     {

  636.         context->peakHz = PlsrPlannerReachableFrequency(

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

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

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

  640.         context->clipped = 1U;

  641.         context->active = 1U;

  642.         return PLSR_PLANNER_CLIPPED;

  643.     }

  644. 

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

  646.                                       context->endHz);

  647.     entryWeight = PlsrPlannerRampWeight(

  648.         context->startHz, context->peakHz,

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

  650.     exitWeight = PlsrPlannerRampWeight(

  651.         context->peakHz, context->endHz,

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

  653.     entryRequired = PlsrPlannerRequiredPulses(block, entryWeight);

  654.     exitRequired = PlsrPlannerRequiredPulses(block, exitWeight);

  655. 

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

  657.     {

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

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

  660.         context->steadyPulses = block->pulseBudget

  661.                                 - context->entryPulses

  662.                                 - context->exitPulses;

  663.     }

  664.     else if (entryRequired == 0ULL)

  665.     {

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

  667.         context->clipped = 1U;

  668.     }

  669.     else if (exitRequired == 0ULL)

  670.     {

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

  672.         context->clipped = 1U;

  673.     }

  674.     else

  675.     {

  676.         totalWeight = entryWeight + exitWeight;

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

  678.                        + totalWeight / 2ULL) / totalWeight;

  679.         if (scaledEntry == 0ULL)

  680.         {

  681.             scaledEntry = 1ULL;

  682.         }

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

  684.         {

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

  686.         }

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

  688.         context->exitPulses = block->pulseBudget

  689.                               - context->entryPulses;

  690.         context->clipped = 1U;

  691.     }

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

  693.     {

  694.         context->clipped = 1U;

  695.     }

  696.     context->active = 1U;

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

  698.                                     : PLSR_PLANNER_OK;

  699. }

  700. 

  701. uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context,

  702.                              PLSR_STREAM_ITEM *output,

  703.                              uint16_t capacity)

  704. {

  705.     PLSR_PLATFORM_TIMER_SETTING setting;

  706.     uint32_t requestedFrequencyHz;

  707.     uint32_t repeatCount;

  708.     uint16_t produced = 0U;

  709. 

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

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

  712.     {

  713.         return 0U;

  714.     }

  715.     while ((produced < capacity)

  716.            && (context->generatedPulses < context->block.pulseBudget))

  717.     {

  718.         if (PlsrPlannerTakeStep(context, &setting, &requestedFrequencyHz,

  719.                                 &repeatCount) == 0U)

  720.         {

  721.             context->active = 0U;

  722.             break;

  723.         }

  724.         if ((produced != 0U)

  725.             && (PlsrPlannerSameSetting(&output[produced - 1U].setting,

  726.                                        &setting) != 0U)

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

  728.                 == requestedFrequencyHz)

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

  730.                 <= 0xFFFFFFFFUL - repeatCount))

  731.         {

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

  733.         }

  734.         else

  735.         {

  736.             output[produced].setting = setting;

  737.             output[produced].requestedFrequencyHz = requestedFrequencyHz;

  738.             output[produced].repeatCount = repeatCount;

  739.             produced++;

  740.         }

  741.         context->generatedPulses += repeatCount;

  742.     }

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

  744.     {

  745.         context->active = 0U;

  746.     }

  747.     return produced;

  748. }