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.
47 Commit
3 Rami (Branch)
138 MiB
 
 
 
 
 
 
Albero (Tree): 47dabdb226
Rami (Branch) Tag
${ item.name }
Crea branch ${ searchTerm }
da '47dabdb226'
${ noResults }
Modbus-rtu/PLSR/Src/plsr_planner.c

1048 righe
36 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,uint32_t fromHz, uint32_t toHz)

  44.                                     

  45.                                    

  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. //C(x)函数

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

  219. static uint64_t PlsrPlannerCurveIntegralQ32(uint64_t progressQ32,

  220.                                             uint16_t curveMode)

  221. {

  222.     const uint32_t *table;

  223.     uint64_t scaled;

  224.     uint32_t index;

  225.     uint32_t fraction;

  226.     uint64_t first;

  227.     uint64_t second;

  228. 

  229.     if (progressQ32 >= PLSR_PLANNER_Q32_ONE)

  230.     {

  231.         return PLSR_PLANNER_Q32_ONE / 2ULL;

  232.     }

  233.     if (curveMode == 0U)

  234.     {

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

  236.     }

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

  238.                               : PlsrPlannerSineIntegralQ24;

  239.     scaled = progressQ32 * 64ULL;

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

  241.     fraction = (uint32_t)scaled;

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

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

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

  245. }

  246. 

  247. static uint64_t PlsrPlannerRampAreaQ32(const PLSR_PLANNER_CONTEXT *context,

  248.                                        uint32_t fromHz,

  249.                                        uint32_t toHz,

  250.                                        uint64_t progressQ32)

  251. {

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

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

  254.                    + delta * (int64_t)PlsrPlannerCurveIntegralQ32(

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

  256. 

  257.     return (uint64_t)area;

  258. }

  259. 

  260. static uint64_t PlsrPlannerExactBoundaryQ32(

  261.     const PLSR_PLANNER_CONTEXT *context,

  262.     uint64_t previousBoundaryQ32,

  263.     uint64_t targetAreaQ32)

  264. {

  265.     uint64_t lowerQ32 = previousBoundaryQ32;

  266.     uint64_t upperQ32 = PLSR_PLANNER_Q32_ONE;

  267.     uint64_t middleQ32;

  268.     uint32_t iteration;

  269. 

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

  271.     {

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

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

  274.                                    context->rampToHz, middleQ32)

  275.             < targetAreaQ32)

  276.         {

  277.             lowerQ32 = middleQ32;

  278.         }

  279.         else

  280.         {

  281.             upperQ32 = middleQ32;

  282.         }

  283.     }

  284.     return upperQ32;

  285. }

  286. 

  287. static uint32_t PlsrPlannerInstantFrequency(

  288.     const PLSR_PLANNER_CONTEXT *context,

  289.     uint64_t progressQ32)

  290. {

  291.     const uint32_t *table;

  292.     uint64_t scaled;

  293.     uint64_t curveProgressQ32;

  294.     uint32_t index;

  295.     uint32_t gap;

  296. 

  297.     if (progressQ32 >= PLSR_PLANNER_Q32_ONE)

  298.     {

  299.         return context->rampToHz;

  300.     }

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

  302.     {

  303.         curveProgressQ32 = progressQ32;

  304.     }

  305.     else

  306.     {

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

  308.                     ? PlsrPlannerSmoothIntegralQ24

  309.                     : PlsrPlannerSineIntegralQ24;

  310.         scaled = progressQ32 * 64ULL;

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

  312.         curveProgressQ32 =

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

  314.     }

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

  316.     {

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

  318.         return context->rampFromHz

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

  320.     }

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

  322.     return context->rampFromHz

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

  324. }

  325. 

  326. static uint64_t PlsrPlannerPredictedBoundaryQ32(

  327.     const PLSR_PLANNER_CONTEXT *context,

  328.     uint64_t targetAreaQ32)

  329. {

  330.     uint64_t previousQ32 = context->rampBoundaryQ32;

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

  332.                                   ? 0ULL

  333.                                   : PlsrPlannerRampAreaQ32(

  334.                                         context, context->rampFromHz,

  335.                                         context->rampToHz, previousQ32);

  336.     uint64_t candidateQ32;

  337.     uint64_t candidateAreaQ32;

  338.     uint64_t differenceQ32;

  339.     uint64_t correctionQ32;

  340.     uint32_t derivativeHz;

  341. 

  342.     if (context->rampLastPhaseStepQ32

  343.         >= PLSR_PLANNER_Q32_ONE - previousQ32)

  344.     {

  345.         candidateQ32 = PLSR_PLANNER_Q32_ONE;

  346.     }

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

  348.     {

  349.         candidateQ32 = previousQ32 + context->rampLastPhaseStepQ32;

  350.     }

  351.     else

  352.     {

  353.         derivativeHz = PlsrPlannerInstantFrequency(context, previousQ32);

  354.         if (derivativeHz == 0UL)

  355.         {

  356.             derivativeHz = 1UL;

  357.         }

  358.         differenceQ32 = targetAreaQ32 - currentAreaQ32;

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

  360.                         / derivativeHz;

  361.         candidateQ32 = (correctionQ32

  362.                         >= PLSR_PLANNER_Q32_ONE - previousQ32)

  363.                            ? PLSR_PLANNER_Q32_ONE

  364.                            : previousQ32 + correctionQ32;

  365.     }

  366. 

  367.     candidateAreaQ32 = PlsrPlannerRampAreaQ32(

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

  369.     derivativeHz = PlsrPlannerInstantFrequency(context, candidateQ32);

  370.     if (derivativeHz == 0UL)

  371.     {

  372.         derivativeHz = 1UL;

  373.     }

  374.     if (candidateAreaQ32 < targetAreaQ32)

  375.     {

  376.         differenceQ32 = targetAreaQ32 - candidateAreaQ32;

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

  378.                         / derivativeHz;

  379.         candidateQ32 = (correctionQ32

  380.                         >= PLSR_PLANNER_Q32_ONE - candidateQ32)

  381.                            ? PLSR_PLANNER_Q32_ONE

  382.                            : candidateQ32 + correctionQ32;

  383.     }

  384.     else if (candidateAreaQ32 > targetAreaQ32)

  385.     {

  386.         differenceQ32 = candidateAreaQ32 - targetAreaQ32;

  387.         correctionQ32 = differenceQ32 / derivativeHz;

  388.         if (correctionQ32 == 0ULL)

  389.         {

  390.             correctionQ32 = 1ULL;

  391.         }

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

  393.                            ? previousQ32 + 1ULL

  394.                            : candidateQ32 - correctionQ32;

  395.     }

  396.     return candidateQ32;

  397. }

  398. 

  399. static void PlsrPlannerStartRamp(PLSR_PLANNER_CONTEXT *context,

  400.                                  uint8_t rampKind,

  401.                                  uint32_t fromHz,

  402.                                  uint32_t toHz,

  403.                                  uint32_t pulseCount)

  404. {

  405.     context->rampKind = rampKind;

  406.     context->rampRelativePulse = 0UL;

  407.     context->rampPulseCount = pulseCount;

  408.     context->rampFromHz = fromHz;

  409.     context->rampToHz = toHz;

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

  411.                                                  

  412.     context->rampTotalAreaQ32 = PlsrPlannerRampAreaQ32(

  413.         context, fromHz, toHz, PLSR_PLANNER_Q32_ONE);

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

  415.     context->rampAreaRemainder =

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

  417.     context->rampRemainderAccumulator = 0UL;

  418.     context->rampTargetAreaQ32 = 0ULL;

  419.     context->rampBoundaryQ32 = 0ULL;

  420.     context->rampActualTimeQ32 = 0ULL;

  421.     context->rampLastPhaseStepQ32 = 0ULL;

  422.     context->lastRampHz = 0UL;

  423.     context->rampFirstBoundaryQ32 = PlsrPlannerExactBoundaryQ32(

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

  425.     if (pulseCount > 1UL)

  426.     {

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

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

  429.         context->rampSecondBoundaryQ32 = PlsrPlannerExactBoundaryQ32(

  430.             context, context->rampFirstBoundaryQ32,

  431.             secondTargetAreaQ32);

  432.     }

  433.     else

  434.     {

  435.         context->rampSecondBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  436.     }

  437. }

  438. 

  439. static uint8_t PlsrPlannerSameSetting(

  440.     const PLSR_PLATFORM_TIMER_SETTING *first,

  441.     const PLSR_PLATFORM_TIMER_SETTING *second)

  442. {

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

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

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

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

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

  448. }

  449. 

  450. static uint8_t PlsrPlannerBuildStep(PLSR_PLANNER_CONTEXT *context,

  451.                                     uint32_t requestedHz,

  452.                                     PLSR_PLATFORM_TIMER_SETTING *setting,

  453.                                     uint32_t *normalizedRequestedHz)

  454. {

  455.     if (requestedHz == 0UL)

  456.     {

  457.         requestedHz = 1UL;

  458.     }

  459.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  460.     {

  461.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  462.     }

  463.     *normalizedRequestedHz = requestedHz;

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

  465.                                          PLSR_OUTPUT_PULSE_DIR,

  466.                                          requestedHz, setting);

  467. }

  468. 

  469. static uint8_t PlsrPlannerTakeRampStep(

  470.     PLSR_PLANNER_CONTEXT *context,

  471.     PLSR_PLATFORM_TIMER_SETTING *setting,

  472.     uint32_t *requestedFrequencyHz)

  473. {

  474.     uint64_t nextBoundaryQ32;

  475.     uint64_t desiredDeltaQ32;

  476.     uint64_t denominator;

  477.     uint64_t requestedHz;

  478.     uint64_t actualDeltaQ32;

  479. 

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

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

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

  483.     {

  484.         context->rampTargetAreaQ32++;

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

  486.     }

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

  488.     {

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

  490.         nextBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  491.     }

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

  493.     {

  494.         nextBoundaryQ32 = context->rampFirstBoundaryQ32;

  495.     }

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

  497.     {

  498.         nextBoundaryQ32 = context->rampSecondBoundaryQ32;

  499.     }

  500.     else

  501.     {

  502.         nextBoundaryQ32 = PlsrPlannerPredictedBoundaryQ32(

  503.             context, context->rampTargetAreaQ32);

  504.     }

  505. 

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

  507.                           ? (nextBoundaryQ32 - context->rampActualTimeQ32)

  508.                           : 1ULL;

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

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

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

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

  513.     requestedHz = (denominator == 0ULL)

  514.                       ? context->rampToHz

  515.                       : (context->rampTotalAreaQ32

  516.                          + denominator / 2ULL) / denominator;

  517.     if (requestedHz == 0ULL)

  518.     {

  519.         requestedHz = 1ULL;

  520.     }

  521.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  522.     {

  523.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  524.     }

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

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

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

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

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

  530.     {

  531.         requestedHz = context->lastRampHz;

  532.     }

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

  534.                              requestedFrequencyHz) == 0U)

  535.     {

  536.         return 0U;

  537.     }

  538. 

  539.     denominator = (uint64_t)context->rampPulseCount

  540.                   * setting->actualFrequencyHz;

  541.     actualDeltaQ32 = (denominator == 0ULL)

  542.                          ? desiredDeltaQ32

  543.                          : (context->rampTotalAreaQ32

  544.                             + denominator / 2ULL) / denominator;

  545.     context->rampActualTimeQ32 += actualDeltaQ32;

  546.     context->rampLastPhaseStepQ32 =

  547.         nextBoundaryQ32 - context->rampBoundaryQ32;

  548.     context->rampBoundaryQ32 = nextBoundaryQ32;

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

  550.     context->rampRelativePulse++;

  551.     return 1U;

  552. }

  553. 

  554. static uint8_t PlsrPlannerTakeStep(PLSR_PLANNER_CONTEXT *context,

  555.                                    PLSR_PLATFORM_TIMER_SETTING *setting,

  556.                                    uint32_t *requestedFrequencyHz,

  557.                                    uint32_t *repeatCount)

  558. {

  559.     uint32_t entryEnd = context->entryPulses;

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

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

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

  563.     *repeatCount = 1UL;

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

  565.     {

  566.         return 0U;

  567.     }

  568.     if (context->generatedPulses < entryEnd)

  569.     { //加速段 

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

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

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

  573.         }

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

  575.                                        

  576.     }

  577.     if (context->generatedPulses < steadyEnd)

  578.     {

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

  580.         context->rampKind = 0U;

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

  582.                                     requestedFrequencyHz);

  583.     }

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

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

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

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

  588.     }

  589.     return PlsrPlannerTakeRampStep(context, setting,

  590.                                    requestedFrequencyHz);

  591. }

  592. 

  593. PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context,

  594.                                      const PLSR_MOTION_BLOCK *block,

  595.                                      uint32_t appliedHz,

  596.                                      uint64_t phasePulses)

  597. {

  598.     uint64_t directRequired;

  599.     uint64_t entryRequired;

  600.     uint64_t exitRequired;

  601.     uint64_t entryWeight;

  602.     uint64_t exitWeight;

  603.     uint64_t totalWeight;

  604.     uint64_t scaledEntry;

  605. 

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

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

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

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

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

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

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

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

  614.         || (appliedHz > PLSR_FREQUENCY_MAX_HZ)

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

  616.     {

  617.         return PLSR_PLANNER_INVALID;

  618.     }

  619. 

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

  621.     context->block = *block;

  622.     context->phasePulses = phasePulses;

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

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

  625.     {

  626.         context->startHz = 1UL;

  627.     }

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

  629. 

  630.     directRequired = PlsrPlannerRequiredPulses(

  631.         block, PlsrPlannerRampWeight(

  632.                    context->startHz, context->endHz,

  633.                    PlsrPlannerBaseRampTime(block, context->startHz,

  634.                                            context->endHz)));

  635.     if (directRequired > block->pulseBudget)

  636.     {

  637.         context->peakHz = PlsrPlannerReachableFrequency(

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

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

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

  641.         context->clipped = 1U;

  642.         context->active = 1U;

  643.         return PLSR_PLANNER_CLIPPED;

  644.     }

  645. 

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

  647.                                       context->endHz);

  648.     entryWeight = PlsrPlannerRampWeight(

  649.         context->startHz, context->peakHz,

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

  651.     exitWeight = PlsrPlannerRampWeight(

  652.         context->peakHz, context->endHz,

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

  654.     entryRequired = PlsrPlannerRequiredPulses(block, entryWeight);

  655.     exitRequired = PlsrPlannerRequiredPulses(block, exitWeight);

  656. 

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

  658.     {

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

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

  661.         context->steadyPulses = block->pulseBudget

  662.                                 - context->entryPulses

  663.                                 - context->exitPulses;

  664.     }

  665.     else if (entryRequired == 0ULL)

  666.     {

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

  668.         context->clipped = 1U;

  669.     }

  670.     else if (exitRequired == 0ULL)

  671.     {

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

  673.         context->clipped = 1U;

  674.     }

  675.     else

  676.     {

  677.         totalWeight = entryWeight + exitWeight;

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

  679.                        + totalWeight / 2ULL) / totalWeight;

  680.         if (scaledEntry == 0ULL)

  681.         {

  682.             scaledEntry = 1ULL;

  683.         }

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

  685.         {

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

  687.         }

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

  689.         context->exitPulses = block->pulseBudget

  690.                               - context->entryPulses;

  691.         context->clipped = 1U;

  692.     }

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

  694.     {

  695.         context->clipped = 1U;

  696.     }

  697.     context->active = 1U;

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

  699.                                     : PLSR_PLANNER_OK;

  700. }

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

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

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

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

  705. {

  706.     PLSR_PLATFORM_TIMER_SETTING setting;

  707.     uint32_t requestedFrequencyHz;

  708.     uint32_t repeatCount;

  709.     uint16_t produced = 0U;

  710. 

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

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

  713.     {

  714.         return 0U;

  715.     }

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

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

  718. 

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

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

  721.         {

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

  723.             break;

  724.         }

  725.         if ((produced != 0U)

  726.             && (PlsrPlannerSameSetting(&output[produced - 1U].setting,&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. 

  735.         }

  736.         else

  737.         {

  738.             output[produced].setting = setting;

  739.             output[produced].requestedFrequencyHz = requestedFrequencyHz;

  740.             output[produced].repeatCount = repeatCount;

  741.             produced++;

  742.         }

  743.         context->generatedPulses += repeatCount;

  744.     }

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

  746.     {

  747.         context->active = 0U;

  748.     }

  749.     return produced;

  750. }

  751. 

  752. static uint64_t PlsrPlannerRampDurationUs(uint32_t pulseCount,

  753.                                            uint32_t fromHz,

  754.                                            uint32_t toHz)

  755. {

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

  757. 

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

  759.     {

  760.         return 0ULL;

  761.     }

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

  763.             + frequencySum - 1ULL) / frequencySum;

  764. }

  765. 

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

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

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

  769. static uint32_t PlsrPlannerRatioQ32(uint64_t numerator,

  770.                                     uint64_t denominator)

  771. {

  772.     uint64_t remainder;

  773.     uint32_t ratio = 0UL;

  774.     uint8_t bit;

  775. 

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

  777.     {

  778.         return 0UL;

  779.     }

  780.     if (numerator >= denominator)

  781.     {

  782.         return 0xFFFFFFFFUL;

  783.     }

  784.     remainder = numerator;

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

  786.     {

  787.         ratio <<= 1U;

  788.         remainder <<= 1U;

  789.         if (remainder >= denominator)

  790.         {

  791.             remainder -= denominator;

  792.             ratio |= 1UL;

  793.         }

  794.     }

  795.     return ratio;

  796. }

  797. 

  798. static uint32_t PlsrPlannerRampPulsesAtTime(

  799.     const PLSR_PLANNER_CONTEXT *context,

  800.     uint32_t pulseCount,

  801.     uint32_t fromHz,

  802.     uint32_t toHz,

  803.     uint64_t elapsedUs,

  804.     uint64_t durationUs,

  805.     uint64_t *progressQ32)

  806. {

  807.     PLSR_PLANNER_CONTEXT ramp = *context;

  808.     uint64_t partialAreaQ32;

  809.     uint64_t totalAreaQ32;

  810.     uint64_t product;

  811.     uint32_t areaRatioQ32;

  812.     uint32_t result;

  813. 

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

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

  816.     {

  817.         *progressQ32 = 0ULL;

  818.         return 0UL;

  819.     }

  820.     if (elapsedUs >= durationUs)

  821.     {

  822.         *progressQ32 = PLSR_PLANNER_Q32_ONE;

  823.         return pulseCount;

  824.     }

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

  826.     ramp.rampFromHz = fromHz;

  827.     ramp.rampToHz = toHz;

  828.     partialAreaQ32 = PlsrPlannerRampAreaQ32(

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

  830.     totalAreaQ32 = PlsrPlannerRampAreaQ32(

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

  832.     areaRatioQ32 = PlsrPlannerRatioQ32(partialAreaQ32,

  833.                                        totalAreaQ32);

  834.     product = (uint64_t)pulseCount * areaRatioQ32;

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

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

  837.     {

  838.         result++;

  839.     }

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

  841. }

  842. 

  843. static uint8_t PlsrPlannerSetPredictedFrequency(

  844.     const PLSR_PLANNER_CONTEXT *context,

  845.     uint32_t fromHz,

  846.     uint32_t toHz,

  847.     uint64_t progressQ32,

  848.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  849. {

  850.     PLSR_PLANNER_CONTEXT ramp = *context;

  851.     PLSR_PLATFORM_TIMER_SETTING setting;

  852.     uint32_t requestedHz;

  853. 

  854.     ramp.rampFromHz = fromHz;

  855.     ramp.rampToHz = toHz;

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

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

  858.                                       PLSR_OUTPUT_PULSE_DIR,

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

  860.     {

  861.         return 0U;

  862.     }

  863.     prediction->actualFrequencyHz = setting.actualFrequencyHz;

  864.     return 1U;

  865. }

  866. 

  867. static uint64_t PlsrPlannerRampTargetAreaQ32(uint64_t totalAreaQ32,

  868.                                              uint32_t pulseCount,

  869.                                              uint32_t pulseIndex)

  870. {

  871.     uint64_t step = totalAreaQ32 / pulseCount;

  872.     uint64_t remainder = totalAreaQ32 % pulseCount;

  873. 

  874.     return step * pulseIndex

  875.            + (remainder * pulseIndex) / pulseCount;

  876. }

  877. 

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

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

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

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

  882. static uint8_t PlsrPlannerSetPredictedRampRunFrequency(

  883.     const PLSR_PLANNER_CONTEXT *context,

  884.     uint32_t pulseCount,

  885.     uint32_t fromHz,

  886.     uint32_t toHz,

  887.     uint32_t pulseIndex,

  888.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  889. {

  890.     PLSR_PLANNER_CONTEXT ramp = *context;

  891.     PLSR_PLATFORM_TIMER_SETTING setting;

  892.     uint64_t totalAreaQ32;

  893.     uint64_t previousTargetAreaQ32;

  894.     uint64_t targetAreaQ32;

  895.     uint64_t previousBoundaryQ32;

  896.     uint64_t boundaryQ32;

  897.     uint64_t denominator;

  898.     uint64_t requestedHz;

  899. 

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

  901.     {

  902.         return PlsrPlannerSetPredictedFrequency(

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

  904.     }

  905.     if (pulseIndex > pulseCount)

  906.     {

  907.         pulseIndex = pulseCount;

  908.     }

  909.     ramp.rampFromHz = fromHz;

  910.     ramp.rampToHz = toHz;

  911.     totalAreaQ32 = PlsrPlannerRampAreaQ32(

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

  913.     previousTargetAreaQ32 = PlsrPlannerRampTargetAreaQ32(

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

  915.     targetAreaQ32 = PlsrPlannerRampTargetAreaQ32(

  916.         totalAreaQ32, pulseCount, pulseIndex);

  917.     previousBoundaryQ32 = (pulseIndex == 1UL)

  918.                               ? 0ULL

  919.                               : PlsrPlannerExactBoundaryQ32(

  920.                                     &ramp, 0ULL,

  921.                                     previousTargetAreaQ32);

  922.     boundaryQ32 = (pulseIndex == pulseCount)

  923.                       ? PLSR_PLANNER_Q32_ONE

  924.                       : PlsrPlannerExactBoundaryQ32(

  925.                             &ramp, previousBoundaryQ32,

  926.                             targetAreaQ32);

  927.     if (boundaryQ32 <= previousBoundaryQ32)

  928.     {

  929.         return 0U;

  930.     }

  931.     denominator = (uint64_t)pulseCount

  932.                   * (boundaryQ32 - previousBoundaryQ32);

  933.     requestedHz = (denominator == 0ULL)

  934.                       ? toHz

  935.                       : (totalAreaQ32 + denominator / 2ULL)

  936.                             / denominator;

  937.     if (requestedHz == 0ULL)

  938.     {

  939.         requestedHz = 1ULL;

  940.     }

  941.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  942.     {

  943.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  944.     }

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

  946.                                       PLSR_OUTPUT_PULSE_DIR,

  947.                                       (uint32_t)requestedHz,

  948.                                       &setting) == 0U)

  949.     {

  950.         return 0U;

  951.     }

  952.     prediction->actualFrequencyHz = setting.actualFrequencyHz;

  953.     return 1U;

  954. }

  955. 

  956. uint8_t PlsrPlannerPredictTime(

  957.     const PLSR_PLANNER_CONTEXT *context,

  958.     uint32_t elapsedUs,

  959.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  960. {

  961.     PLSR_PLATFORM_TIMER_SETTING steadySetting;

  962.     uint64_t remainingUs = elapsedUs;

  963.     uint64_t durationUs;

  964.     uint64_t progressQ32;

  965.     uint64_t partialPulses;

  966. 

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

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

  969.     {

  970.         return 0U;

  971.     }

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

  973. 

  974.     durationUs = PlsrPlannerRampDurationUs(

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

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

  977.     {

  978.         prediction->pulseCount = PlsrPlannerRampPulsesAtTime(

  979.             context, context->entryPulses, context->startHz,

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

  981.         prediction->phase = PLSR_PLANNER_PHASE_ENTRY;

  982.         prediction->deadlineInProfile = 1U;

  983.         return PlsrPlannerSetPredictedRampRunFrequency(

  984.             context, context->entryPulses, context->startHz,

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

  986.     }

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

  988.     {

  989.         remainingUs -= durationUs;

  990.     }

  991. 

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

  993.                                       PLSR_OUTPUT_PULSE_DIR,

  994.                                       context->peakHz,

  995.                                       &steadySetting) == 0U)

  996.     {

  997.         return 0U;

  998.     }

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

  1000.                      ? 0ULL

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

  1002.                         + steadySetting.actualFrequencyHz - 1UL)

  1003.                            / steadySetting.actualFrequencyHz;

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

  1005.     {

  1006.         partialPulses = (remainingUs * steadySetting.actualFrequencyHz

  1007.                          + 999999ULL) / 1000000ULL;

  1008.         if (partialPulses > context->steadyPulses)

  1009.         {

  1010.             partialPulses = context->steadyPulses;

  1011.         }

  1012.         prediction->pulseCount = context->entryPulses

  1013.                                  + (uint32_t)partialPulses;

  1014.         prediction->actualFrequencyHz = steadySetting.actualFrequencyHz;

  1015.         prediction->phase = PLSR_PLANNER_PHASE_STEADY;

  1016.         prediction->deadlineInProfile = 1U;

  1017.         return 1U;

  1018.     }

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

  1020.     {

  1021.         remainingUs -= durationUs;

  1022.     }

  1023. 

  1024.     durationUs = PlsrPlannerRampDurationUs(

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

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

  1027.     {

  1028.         partialPulses = PlsrPlannerRampPulsesAtTime(

  1029.             context, context->exitPulses, context->peakHz,

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

  1031.         prediction->pulseCount = context->entryPulses

  1032.                                  + context->steadyPulses

  1033.                                  + (uint32_t)partialPulses;

  1034.         prediction->phase = PLSR_PLANNER_PHASE_EXIT;

  1035.         prediction->deadlineInProfile = 1U;

  1036.         return PlsrPlannerSetPredictedRampRunFrequency(

  1037.             context, context->exitPulses, context->peakHz,

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

  1039.     }

  1040. 

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

  1042.     prediction->phase = PLSR_PLANNER_PHASE_COMPLETE;

  1043.     prediction->deadlineInProfile = 0U;

  1044.     return PlsrPlannerSetPredictedFrequency(

  1045.         context, context->endHz, context->endHz,

  1046.         PLSR_PLANNER_Q32_ONE, prediction);

  1047. }