yuwenhao
/
Modbus-rtu
1
0
Förgrening 0
Kod Ärenden 0 Pull-förfrågningar 0 Släpp 0 Wiki Aktiviteter
Du kan inte välja fler än 25 ämnen Ämnen måste starta med en bokstav eller siffra, kan innehålla bindestreck ('-') och vara max 35 tecken långa.
30 Incheckningar
3 Grenar
138 MiB
 
 
 
 
 
 
Träd: 8d678f99d9
Grenar Taggar
${ item.name }
Skapa branchen ${ searchTerm }
från '8d678f99d9'
${ noResults }
Modbus-rtu/PLSR/Src/plsr_planner.c

736 rader
24 KiB
Rå Blame Historik 

  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. {

  453.     if (requestedHz == 0UL)

  454.     {

  455.         requestedHz = 1UL;

  456.     }

  457.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  458.     {

  459.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  460.     }

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

  462.                                          PLSR_OUTPUT_PULSE_DIR,

  463.                                          requestedHz, setting);

  464. }

  465. 

  466. static uint8_t PlsrPlannerTakeRampStep(

  467.     PLSR_PLANNER_CONTEXT *context,

  468.     PLSR_PLATFORM_TIMER_SETTING *setting)

  469. {

  470.     uint64_t nextBoundaryQ32;

  471.     uint64_t desiredDeltaQ32;

  472.     uint64_t denominator;

  473.     uint64_t requestedHz;

  474.     uint64_t actualDeltaQ32;

  475. 

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

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

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

  479.     {

  480.         context->rampTargetAreaQ32++;

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

  482.     }

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

  484.     {

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

  486.         nextBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  487.     }

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

  489.     {

  490.         nextBoundaryQ32 = context->rampFirstBoundaryQ32;

  491.     }

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

  493.     {

  494.         nextBoundaryQ32 = context->rampSecondBoundaryQ32;

  495.     }

  496.     else

  497.     {

  498.         nextBoundaryQ32 = PlsrPlannerPredictedBoundaryQ32(

  499.             context, context->rampTargetAreaQ32);

  500.     }

  501. 

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

  503.                           ? (nextBoundaryQ32 - context->rampActualTimeQ32)

  504.                           : 1ULL;

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

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

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

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

  509.     requestedHz = (denominator == 0ULL)

  510.                       ? context->rampToHz

  511.                       : (context->rampTotalAreaQ32

  512.                          + denominator / 2ULL) / denominator;

  513.     if (requestedHz == 0ULL)

  514.     {

  515.         requestedHz = 1ULL;

  516.     }

  517.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  518.     {

  519.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  520.     }

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

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

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

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

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

  526.     {

  527.         requestedHz = context->lastRampHz;

  528.     }

  529.     if (PlsrPlannerBuildStep(context, (uint32_t)requestedHz, setting) == 0U)

  530.     {

  531.         return 0U;

  532.     }

  533. 

  534.     denominator = (uint64_t)context->rampPulseCount

  535.                   * setting->actualFrequencyHz;

  536.     actualDeltaQ32 = (denominator == 0ULL)

  537.                          ? desiredDeltaQ32

  538.                          : (context->rampTotalAreaQ32

  539.                             + denominator / 2ULL) / denominator;

  540.     context->rampActualTimeQ32 += actualDeltaQ32;

  541.     context->rampLastPhaseStepQ32 =

  542.         nextBoundaryQ32 - context->rampBoundaryQ32;

  543.     context->rampBoundaryQ32 = nextBoundaryQ32;

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

  545.     context->rampRelativePulse++;

  546.     return 1U;

  547. }

  548. 

  549. static uint8_t PlsrPlannerTakeStep(PLSR_PLANNER_CONTEXT *context,

  550.                                    PLSR_PLATFORM_TIMER_SETTING *setting,

  551.                                    uint32_t *repeatCount)

  552. {

  553.     uint32_t entryEnd = context->entryPulses;

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

  555. 

  556.     *repeatCount = 1UL;

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

  558.     {

  559.         return 0U;

  560.     }

  561.     if (context->generatedPulses < entryEnd)

  562.     {

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

  564.         {

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

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

  567.         }

  568.         return PlsrPlannerTakeRampStep(context, setting);

  569.     }

  570.     if (context->generatedPulses < steadyEnd)

  571.     {

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

  573.         context->rampKind = 0U;

  574.         return PlsrPlannerBuildStep(context, context->peakHz, setting);

  575.     }

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

  577.     {

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

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

  580.     }

  581.     return PlsrPlannerTakeRampStep(context, setting);

  582. }

  583. 

  584. PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context,

  585.                                      const PLSR_MOTION_BLOCK *block,

  586.                                      uint32_t appliedHz,

  587.                                      uint64_t phasePulses)

  588. {

  589.     uint64_t directRequired;

  590.     uint64_t entryRequired;

  591.     uint64_t exitRequired;

  592.     uint64_t entryWeight;

  593.     uint64_t exitWeight;

  594.     uint64_t totalWeight;

  595.     uint64_t scaledEntry;

  596. 

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

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

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

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

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

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

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

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

  605.         || (appliedHz > PLSR_FREQUENCY_MAX_HZ)

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

  607.     {

  608.         return PLSR_PLANNER_INVALID;

  609.     }

  610. 

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

  612.     context->block = *block;

  613.     context->phasePulses = phasePulses;

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

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

  616.     {

  617.         context->startHz = 1UL;

  618.     }

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

  620. 

  621.     directRequired = PlsrPlannerRequiredPulses(

  622.         block, PlsrPlannerRampWeight(

  623.                    context->startHz, context->endHz,

  624.                    PlsrPlannerBaseRampTime(block, context->startHz,

  625.                                            context->endHz)));

  626.     if (directRequired > block->pulseBudget)

  627.     {

  628.         context->peakHz = PlsrPlannerReachableFrequency(

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

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

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

  632.         context->clipped = 1U;

  633.         context->active = 1U;

  634.         return PLSR_PLANNER_CLIPPED;

  635.     }

  636. 

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

  638.                                       context->endHz);

  639.     entryWeight = PlsrPlannerRampWeight(

  640.         context->startHz, context->peakHz,

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

  642.     exitWeight = PlsrPlannerRampWeight(

  643.         context->peakHz, context->endHz,

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

  645.     entryRequired = PlsrPlannerRequiredPulses(block, entryWeight);

  646.     exitRequired = PlsrPlannerRequiredPulses(block, exitWeight);

  647. 

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

  649.     {

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

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

  652.         context->steadyPulses = block->pulseBudget

  653.                                 - context->entryPulses

  654.                                 - context->exitPulses;

  655.     }

  656.     else if (entryRequired == 0ULL)

  657.     {

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

  659.         context->clipped = 1U;

  660.     }

  661.     else if (exitRequired == 0ULL)

  662.     {

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

  664.         context->clipped = 1U;

  665.     }

  666.     else

  667.     {

  668.         totalWeight = entryWeight + exitWeight;

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

  670.                        + totalWeight / 2ULL) / totalWeight;

  671.         if (scaledEntry == 0ULL)

  672.         {

  673.             scaledEntry = 1ULL;

  674.         }

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

  676.         {

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

  678.         }

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

  680.         context->exitPulses = block->pulseBudget

  681.                               - context->entryPulses;

  682.         context->clipped = 1U;

  683.     }

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

  685.     {

  686.         context->clipped = 1U;

  687.     }

  688.     context->active = 1U;

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

  690.                                     : PLSR_PLANNER_OK;

  691. }

  692. 

  693. uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context,

  694.                              PLSR_STREAM_ITEM *output,

  695.                              uint16_t capacity)

  696. {

  697.     PLSR_PLATFORM_TIMER_SETTING setting;

  698.     uint32_t repeatCount;

  699.     uint16_t produced = 0U;

  700. 

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

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

  703.     {

  704.         return 0U;

  705.     }

  706.     while ((produced < capacity)

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

  708.     {

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

  710.         {

  711.             context->active = 0U;

  712.             break;

  713.         }

  714.         if ((produced != 0U)

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

  716.                                        &setting) != 0U)

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

  718.                 <= 0xFFFFFFFFUL - repeatCount))

  719.         {

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

  721.         }

  722.         else

  723.         {

  724.             output[produced].setting = setting;

  725.             output[produced].repeatCount = repeatCount;

  726.             produced++;

  727.         }

  728.         context->generatedPulses += repeatCount;

  729.     }

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

  731.     {

  732.         context->active = 0U;

  733.     }

  734.     return produced;

  735. }