yuwenhao
/
Modbus-rtu
1
0
Форкнуть 0
Код Задачи 0 Pull Request'ы 0 Релизы 0 Вики Активность
Вы не можете выбрать более 25 тем Темы должны начинаться с буквы или цифры, могут содержать дефисы(-) и должны содержать не более 35 символов.
39 коммитов
3 Ветки
138 MiB
 
 
 
 
 
 
Дерево: c776438bcd
Ветки Теги
${ item.name }
Создать ветку ${ searchTerm }
из 'c776438bcd'
${ noResults }
Modbus-rtu/PLSR/Src/plsr_profile.c

724 строки
23 KiB
Исходник Вина История 

  1. #include "plsr_profile.h"

  2. #include <math.h>

  3. #include <string.h>

  4. 

  5. /* 正弦表：256 项，sin(pi*i/256) 的 Q16 表示。 */

  6. static const uint16_t PlsrProfileSineTable[256] =

  7. {

  8.     0, 804, 1608, 2412, 3216, 4019, 4821, 5623,

  9.     6424, 7224, 8022, 8820, 9616, 10411, 11204, 11996,

  10.     12785, 13573, 14359, 15143, 15924, 16703, 17479, 18253,

  11.     19024, 19792, 20557, 21320, 22078, 22834, 23586, 24335,

  12.     25080, 25821, 26558, 27291, 28020, 28745, 29466, 30182,

  13.     30893, 31600, 32303, 33000, 33692, 34380, 35062, 35738,

  14.     36410, 37076, 37736, 38391, 39040, 39683, 40320, 40951,

  15.     41576, 42194, 42806, 43412, 44011, 44604, 45190, 45769,

  16.     46341, 46906, 47464, 48015, 48559, 49095, 49624, 50146,

  17.     50660, 51166, 51665, 52156, 52639, 53114, 53581, 54040,

  18.     54491, 54934, 55368, 55794, 56212, 56621, 57022, 57414,

  19.     57798, 58172, 58538, 58896, 59244, 59583, 59914, 60235,

  20.     60547, 60851, 61145, 61429, 61705, 61971, 62228, 62476,

  21.     62714, 62943, 63162, 63372, 63572, 63763, 63944, 64115,

  22.     64277, 64429, 64571, 64704, 64827, 64940, 65043, 65137,

  23.     65220, 65294, 65358, 65413, 65457, 65492, 65516, 65531,

  24.     65535, 65531, 65516, 65492, 65457, 65413, 65358, 65294,

  25.     65220, 65137, 65043, 64940, 64827, 64704, 64571, 64429,

  26.     64277, 64115, 63944, 63763, 63572, 63372, 63162, 62943,

  27.     62714, 62476, 62228, 61971, 61705, 61429, 61145, 60851,

  28.     60547, 60235, 59914, 59583, 59244, 58896, 58538, 58172,

  29.     57798, 57414, 57022, 56621, 56212, 55794, 55368, 54934,

  30.     54491, 54040, 53581, 53114, 52639, 52156, 51665, 51166,

  31.     50660, 50146, 49624, 49095, 48559, 48015, 47464, 46906,

  32.     46341, 45769, 45190, 44604, 44011, 43412, 42806, 42194,

  33.     41576, 40951, 40320, 39683, 39040, 38391, 37736, 37076,

  34.     36410, 35738, 35062, 34380, 33692, 33000, 32303, 31600,

  35.     30893, 30182, 29466, 28745, 28020, 27291, 26558, 25821,

  36.     25080, 24335, 23586, 22834, 22078, 21320, 20557, 19792,

  37.     19024, 18253, 17479, 16703, 15924, 15143, 14359, 13573,

  38.     12785, 11996, 11204, 10411, 9616, 8820, 8022, 7224,

  39.     6424, 5623, 4821, 4019, 3216, 2412, 1608, 804,

  40. };

  41. static uint64_t PlsrProfileHzToQ32(uint32_t hz)

  42. {

  43.     return (uint64_t)hz << 32U;

  44. }

  45. 

  46. static uint32_t PlsrProfileSqrtU64(uint64_t value)

  47. {

  48.     uint64_t bit = UINT64_C(1) << 62U;

  49.     uint64_t root = 0UL;

  50. 

  51.     while (bit > value)

  52.     {

  53.         bit >>= 2U;

  54.     }

  55.     while (bit != 0UL)

  56.     {

  57.         if (value >= root + bit)

  58.         {

  59.             value -= root + bit;

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

  61.         }

  62.         else

  63.         {

  64.             root >>= 1U;

  65.         }

  66.         bit >>= 2U;

  67.     }

  68.     return (uint32_t)root;

  69. }

  70. 

  71. /* 每刷新步的斜率增量（Q32.32），支持 0.1ms 刷新（1000/refreshHz 可能 < 1）。 */

  72. static uint64_t PlsrProfileSlopePerStepQ32(uint32_t slopeHzPerMs,

  73.                                            uint32_t refreshHz)

  74. {

  75.     return PlsrProfileHzToQ32(slopeHzPerMs) * UINT64_C(1000)

  76.            / (uint64_t)refreshHz;

  77. }

  78. 

  79. /* 从当前频率减速到目标频率所需的脉冲数（Q32.32 精度）。 */

  80. static uint64_t PlsrProfileDecelPulsesQ32(uint32_t frequencyHz,

  81.                                           uint32_t targetHz,

  82.                                           uint32_t slopeHzPerMs)

  83. {

  84.     uint64_t numerator;

  85.     uint64_t denominator;

  86. 

  87.     if (frequencyHz <= targetHz)

  88.     {

  89.         return 0UL;

  90.     }

  91.     numerator = (uint64_t)frequencyHz * (uint64_t)frequencyHz

  92.                 - (uint64_t)targetHz * (uint64_t)targetHz;

  93.     denominator = 2000UL * (uint64_t)slopeHzPerMs;

  94.     if (denominator == 0UL)

  95.     {

  96.         return 0UL;

  97.     }

  98.     return (numerator / denominator) * PLSR_PROFILE_Q32_ONE

  99.            + ((numerator % denominator) * PLSR_PROFILE_Q32_ONE)

  100.                  / denominator;

  101. }

  102. 

  103. /* 直线增量初始化。 */

  104. static void PlsrProfileInitLinearDelta(PLSR_PROFILE_STATE *state,

  105.                                        uint32_t slopeHzPerMs,

  106.                                        uint32_t refreshHz)

  107. {

  108.     state->deltaPerStepQ32 = PlsrProfileSlopePerStepQ32(slopeHzPerMs,

  109.                                                         refreshHz);

  110.     state->jerkPerStepQ32 = 0UL;

  111.     state->curveStep = 0U;

  112.     state->curveStepLimit = 0U;

  113.     state->sPhase = 0U;

  114. }

  115. 

  116. /* S 形/正弦增量初始化：增量先升后降（S形）或正弦半波。 */

  117. static void PlsrProfileInitCurveDelta(PLSR_PROFILE_STATE *state,

  118.                                       uint32_t frequencyGapHz,

  119.                                       uint32_t slopeHzPerMs,

  120.                                       uint32_t refreshHz,

  121.                                       uint8_t curveMode)

  122. {

  123.     uint64_t linearPerStep = PlsrProfileSlopePerStepQ32(slopeHzPerMs,

  124.                                                         refreshHz);

  125.     uint64_t integerPerStepHz;

  126.     uint64_t rampSteps;

  127.     uint64_t jerkQ32;

  128. 

  129.     integerPerStepHz = ((uint64_t)slopeHzPerMs * UINT64_C(1000))

  130.                        / (uint64_t)refreshHz;

  131.     if (integerPerStepHz == 0UL)

  132.     {

  133.         integerPerStepHz = 1UL;

  134.     }

  135.     rampSteps = ((uint64_t)frequencyGapHz + integerPerStepHz - 1UL)

  136.                 / integerPerStepHz;

  137. 

  138.     if (curveMode == PLSR_PROFILE_CURVE_SINE)

  139.     {

  140.         /* 正弦：时间同直线，峰值增量 = 直线增量 * pi/2（面积匹配）。 */

  141.         state->deltaPerStepQ32 = 0UL;

  142.         state->jerkPerStepQ32 = 0UL;

  143.         state->curveStep = 0U;

  144.         state->curveStepLimit = (uint32_t)rampSteps;

  145.         state->sPhase = 0U;

  146.         return;

  147.     }

  148. 

  149.     /* S 形：增量 0→峰值→0，总时间 2 倍直线时间。 */

  150.     jerkQ32 = linearPerStep / rampSteps;

  151.     state->deltaPerStepQ32 = 0UL;

  152.     state->jerkPerStepQ32 = jerkQ32;

  153.     state->curveStep = 0U;

  154.     state->curveStepLimit = (uint32_t)(rampSteps * 2UL);

  155.     state->sPhase = 0U;

  156. }

  157. 

  158. /* 进入加速段（直线或曲线）。 */

  159. static void PlsrProfileBeginAccel(PLSR_PROFILE_STATE *state)

  160. {

  161.     uint32_t gapHz;

  162. 

  163.     if (state->targetFrequencyHz <= state->frequencyQ32 >> 32U)

  164.     {

  165.         state->phase = PLSR_PROFILE_PHASE_CRUISE;

  166.         return;

  167.     }

  168.     if (state->curveMode == PLSR_PROFILE_CURVE_LINEAR)

  169.     {

  170.         PlsrProfileInitLinearDelta(state,

  171.                                    state->accelSlopeHzPerMs,

  172.                                    state->refreshHz);

  173.     }

  174.     else

  175.     {

  176.         gapHz = state->targetFrequencyHz

  177.                 - (uint32_t)(state->frequencyQ32 >> 32U);

  178.         PlsrProfileInitCurveDelta(state,

  179.                                   gapHz,

  180.                                   state->accelSlopeHzPerMs,

  181.                                   state->refreshHz,

  182.                                   state->curveMode);

  183.     }

  184.     state->phase = PLSR_PROFILE_PHASE_ACCEL;

  185. }

  186. 

  187. /* 进入减速段：减速到 decelTargetHz（默认=终止速度，降频时=新目标）。 */

  188. static void PlsrProfileBeginDecel(PLSR_PROFILE_STATE *state)

  189. {

  190.     uint32_t gapHz;

  191. 

  192.     if (state->frequencyQ32 >> 32U <= state->decelTargetHz)

  193.     {

  194.         state->frequencyQ32 = PlsrProfileHzToQ32(state->decelTargetHz);

  195.         if (state->decelTargetHz == state->stopFrequencyHz)

  196.         {

  197.             state->phase = PLSR_PROFILE_PHASE_DONE;

  198.         }

  199.         else

  200.         {

  201.             /* 降频到新目标：转匀速继续。 */

  202.             state->targetFrequencyHz = state->decelTargetHz;

  203.             state->decelTargetHz = state->stopFrequencyHz;

  204.             state->phase = PLSR_PROFILE_PHASE_CRUISE;

  205.         }

  206.         return;

  207.     }

  208.     if (state->curveMode == PLSR_PROFILE_CURVE_LINEAR)

  209.     {

  210.         PlsrProfileInitLinearDelta(state,

  211.                                    state->decelSlopeHzPerMs,

  212.                                    state->refreshHz);

  213.     }

  214.     else

  215.     {

  216.         gapHz = (uint32_t)(state->frequencyQ32 >> 32U)

  217.                 - state->decelTargetHz;

  218.         PlsrProfileInitCurveDelta(state,

  219.                                   gapHz,

  220.                                   state->decelSlopeHzPerMs,

  221.                                   state->refreshHz,

  222.                                   state->curveMode);

  223.     }

  224.     state->phase = PLSR_PROFILE_PHASE_DECEL;

  225. }

  226. 

  227. /* 曲线模式下推进增量（S形三角增量 / 正弦查表）。 */

  228. static uint64_t PlsrProfileAdvanceDelta(PLSR_PROFILE_STATE *state,

  229.                                         uint64_t linearPerStepQ32)

  230. {

  231.     uint64_t delta;

  232. 

  233.     if (state->curveMode == PLSR_PROFILE_CURVE_SINE)

  234.     {

  235.         uint32_t index = ((uint32_t)state->curveStep * 256U)

  236.                          / (uint32_t)state->curveStepLimit;

  237. 

  238.         if (index > 255U)

  239.         {

  240.             index = 255U;

  241.         }

  242.         delta = ((uint64_t)PlsrProfileSineTable[index]

  243.                  * linearPerStepQ32) / 65536UL;

  244.     }

  245.     else

  246.     {

  247.         if (state->sPhase == 0U)

  248.         {

  249.             state->deltaPerStepQ32 += state->jerkPerStepQ32;

  250.             if ((state->curveStepLimit != 0U)

  251.                 && (state->curveStep >= state->curveStepLimit / 2U))

  252.             {

  253.                 state->sPhase = 1U;

  254.             }

  255.         }

  256.         else

  257.         {

  258.             if (state->deltaPerStepQ32 > state->jerkPerStepQ32)

  259.             {

  260.                 state->deltaPerStepQ32 -= state->jerkPerStepQ32;

  261.             }

  262.             else

  263.             {

  264.                 state->deltaPerStepQ32 = 0UL;

  265.             }

  266.         }

  267.         delta = state->deltaPerStepQ32;

  268.     }

  269.     state->curveStep++;

  270.     return delta;

  271. }

  272. 

  273. PLSR_RESULT PlsrProfileStart(PLSR_PROFILE_STATE *state,

  274.                              const PLSR_PROFILE_REQUEST *request,

  275.                              int64_t pulses,

  276.                              uint32_t refreshHz)

  277. {

  278.     uint32_t startHz;

  279. 

  280.     if ((state == NULL) || (request == NULL))

  281.     {

  282.         return PLSR_RESULT_INVALID_ARGUMENT;

  283.     }

  284.     if ((pulses <= 0) || (refreshHz == 0UL)

  285.         || (request->targetFrequencyHz == 0UL))

  286.     {

  287.         return PLSR_RESULT_INVALID_ARGUMENT;

  288.     }

  289. 

  290.     state->refreshHz = refreshHz;

  291.     state->totalPulses = pulses;

  292.     state->targetFrequencyHz = request->targetFrequencyHz;

  293.     if (request->targetFrequencyHz > request->maxFrequencyHz)

  294.     {

  295.         state->targetFrequencyHz = request->maxFrequencyHz;

  296.     }

  297.     state->startFrequencyHz = request->startFrequencyHz;

  298.     state->stopFrequencyHz = request->stopFrequencyHz;

  299.     state->decelTargetHz = request->stopFrequencyHz;

  300.     state->accelSlopeHzPerMs = request->accelSlopeHzPerMs;

  301.     state->decelSlopeHzPerMs = request->decelSlopeHzPerMs;

  302.     state->curveMode = request->curveMode;

  303.     startHz = request->startFrequencyHz;

  304.     if (startHz > state->targetFrequencyHz)

  305.     {

  306.         startHz = state->targetFrequencyHz;

  307.     }

  308.     state->frequencyQ32 = PlsrProfileHzToQ32(startHz);

  309.     state->emittedPulsesQ32 = 0UL;

  310.     state->phase = PLSR_PROFILE_PHASE_ACCEL;

  311.     state->started = 1U;

  312. 

  313.     if (startHz >= state->targetFrequencyHz)

  314.     {

  315.         state->phase = PLSR_PROFILE_PHASE_CRUISE;

  316.     }

  317.     else if (request->accelSlopeHzPerMs == 0UL)

  318.     {

  319.         state->frequencyQ32 = PlsrProfileHzToQ32(state->targetFrequencyHz);

  320.         state->phase = PLSR_PROFILE_PHASE_CRUISE;

  321.     }

  322.     else

  323.     {

  324.         PlsrProfileBeginAccel(state);

  325.     }

  326.     return PLSR_RESULT_OK;

  327. }

  328. 

  329. PLSR_RESULT PlsrProfileStep(PLSR_PROFILE_STATE *state,

  330.                             uint32_t *frequencyHz,

  331.                             uint8_t *completed)

  332. {

  333.     uint64_t emittedThisStep;

  334.     uint64_t remainingQ32;

  335.     uint64_t decelNeeded;

  336.     uint64_t linearPerStepQ32;

  337.     uint64_t deltaQ32;

  338.     uint32_t currentHz;

  339. 

  340.     if ((state == NULL) || (frequencyHz == NULL) || (completed == NULL))

  341.     {

  342.         return PLSR_RESULT_INVALID_ARGUMENT;

  343.     }

  344.     *completed = 0U;

  345.     if (state->phase == PLSR_PROFILE_PHASE_DONE)

  346.     {

  347.         *frequencyHz = 0U;

  348.         *completed = 1U;

  349.         return PLSR_RESULT_OK;

  350.     }

  351. 

  352.     linearPerStepQ32 = PlsrProfileSlopePerStepQ32(

  353.         (state->phase == PLSR_PROFILE_PHASE_ACCEL)

  354.             ? state->accelSlopeHzPerMs

  355.             : state->decelSlopeHzPerMs,

  356.         state->refreshHz);

  357. 

  358.     switch (state->phase)

  359.     {

  360.         case PLSR_PROFILE_PHASE_ACCEL:

  361.             deltaQ32 = PlsrProfileAdvanceDelta(state, linearPerStepQ32);

  362.             state->frequencyQ32 += deltaQ32;

  363.             if ((state->frequencyQ32

  364.                  >= PlsrProfileHzToQ32(state->targetFrequencyHz))

  365.                 || ((state->curveMode != PLSR_PROFILE_CURVE_LINEAR)

  366.                     && (state->curveStep >= state->curveStepLimit)))

  367.             {

  368.                 state->frequencyQ32 =

  369.                     PlsrProfileHzToQ32(state->targetFrequencyHz);

  370.                 state->phase = PLSR_PROFILE_PHASE_CRUISE;

  371.             }

  372.             break;

  373. 

  374.         case PLSR_PROFILE_PHASE_CRUISE:

  375.             /* 剩余脉冲不足以按当前频率完成减速时开始减速（Q32 精度）。 */

  376.             remainingQ32 = PlsrProfileHzToQ32(

  377.                                (uint32_t)state->totalPulses)

  378.                            - state->emittedPulsesQ32;

  379.             decelNeeded = PlsrProfileDecelPulsesQ32(

  380.                 (uint32_t)(state->frequencyQ32 >> 32U),

  381.                 state->stopFrequencyHz,

  382.                 state->decelSlopeHzPerMs);

  383.             if (remainingQ32 <= decelNeeded)

  384.             {

  385.                 PlsrProfileBeginDecel(state);

  386.             }

  387.             break;

  388. 

  389.         case PLSR_PROFILE_PHASE_DECEL:

  390.             deltaQ32 = PlsrProfileAdvanceDelta(state, linearPerStepQ32);

  391.             state->frequencyQ32 -= deltaQ32;

  392.             if (state->frequencyQ32

  393.                 <= PlsrProfileHzToQ32(state->decelTargetHz))

  394.             {

  395.                 state->frequencyQ32 =

  396.                     PlsrProfileHzToQ32(state->decelTargetHz);

  397.                 if (state->decelTargetHz == state->stopFrequencyHz)

  398.                 {

  399.                     state->phase = PLSR_PROFILE_PHASE_DONE;

  400.                 }

  401.                 else

  402.                 {

  403.                     /* 降频到新目标：转匀速继续。 */

  404.                     state->targetFrequencyHz = state->decelTargetHz;

  405.                     state->decelTargetHz = state->stopFrequencyHz;

  406.                     state->phase = PLSR_PROFILE_PHASE_CRUISE;

  407.                 }

  408.             }

  409.             break;

  410. 

  411.         default:

  412.             break;

  413.     }

  414. 

  415.     /* 发射本步脉冲：频率(Hz) / 刷新率。 */

  416.     currentHz = (uint32_t)(state->frequencyQ32 >> 32U);

  417.     emittedThisStep = state->frequencyQ32 / (uint64_t)state->refreshHz;

  418.     if ((state->emittedPulsesQ32 + emittedThisStep)

  419.         >= PlsrProfileHzToQ32((uint32_t)state->totalPulses))

  420.     {

  421.         state->emittedPulsesQ32 =

  422.             PlsrProfileHzToQ32((uint32_t)state->totalPulses);

  423.         state->phase = PLSR_PROFILE_PHASE_DONE;

  424.     }

  425.     else

  426.     {

  427.         state->emittedPulsesQ32 += emittedThisStep;

  428.     }

  429. 

  430.     *frequencyHz = currentHz;

  431.     if (state->phase == PLSR_PROFILE_PHASE_DONE)

  432.     {

  433.         *completed = 1U;

  434.         *frequencyHz = 0U;

  435.     }

  436.     return PLSR_RESULT_OK;

  437. }

  438. 

  439. PLSR_RESULT PlsrProfileRetarget(PLSR_PROFILE_STATE *state,

  440.                                 uint32_t newTargetFrequencyHz)

  441. {

  442.     uint32_t currentHz;

  443. 

  444.     if (state == NULL)

  445.     {

  446.         return PLSR_RESULT_INVALID_ARGUMENT;

  447.     }

  448.     if (newTargetFrequencyHz == 0UL)

  449.     {

  450.         return PLSR_RESULT_INVALID_ARGUMENT;

  451.     }

  452.     currentHz = (uint32_t)(state->frequencyQ32 >> 32U);

  453.     if (newTargetFrequencyHz == state->targetFrequencyHz)

  454.     {

  455.         return PLSR_RESULT_OK;

  456.     }

  457.     state->targetFrequencyHz = newTargetFrequencyHz;

  458. 

  459.     if (newTargetFrequencyHz > currentHz)

  460.     {

  461.         /* 升频：加速/匀速 → 加速；减速中 → 取消减速转加速。 */

  462.         if ((state->phase == PLSR_PROFILE_PHASE_DECEL)

  463.             || (state->phase == PLSR_PROFILE_PHASE_CRUISE)

  464.             || (state->phase == PLSR_PROFILE_PHASE_DONE))

  465.         {

  466.             PlsrProfileBeginAccel(state);

  467.         }

  468.     }

  469.     else

  470.     {

  471.         /* 降频：先减速到新目标，再转匀速继续（最终仍减速到终止速度）。 */

  472.         if ((state->phase == PLSR_PROFILE_PHASE_ACCEL)

  473.             || (state->phase == PLSR_PROFILE_PHASE_CRUISE))

  474.         {

  475.             state->decelTargetHz = newTargetFrequencyHz;

  476.             PlsrProfileBeginDecel(state);

  477.         }

  478.     }

  479.     return PLSR_RESULT_OK;

  480. }

  481. 

  482. PLSR_RESULT PlsrProfileRequestStop(PLSR_PROFILE_STATE *state)

  483. {

  484.     if ((state == NULL) || (state->started == 0U))

  485.     {

  486.         return PLSR_RESULT_INVALID_ARGUMENT;

  487.     }

  488.     state->stopFrequencyHz = 0U;

  489.     state->decelTargetHz = 0U;

  490.     if (state->decelSlopeHzPerMs == 0UL)

  491.     {

  492.         state->frequencyQ32 = 0UL;

  493.         state->phase = PLSR_PROFILE_PHASE_DONE;

  494.     }

  495.     else

  496.     {

  497.         PlsrProfileBeginDecel(state);

  498.     }

  499.     return PLSR_RESULT_OK;

  500. }

  501. 

  502. PLSR_RESULT PlsrProfilePlan(const PLSR_PROFILE_REQUEST *request,

  503.                             int64_t pulses,

  504.                             PLSR_PROFILE_PLAN *plan)

  505. {

  506.     double target;

  507.     double start;

  508.     double stop;

  509.     double accelSlope;

  510.     double decelSlope;

  511.     double accelTime;

  512.     double decelTime;

  513.     double accelPulses;

  514.     double decelPulses;

  515.     double cruisePulses;

  516.     double cruiseTime;

  517.     double peak;

  518. 

  519.     if ((request == NULL) || (plan == NULL) || (pulses <= 0))

  520.     {

  521.         return PLSR_RESULT_INVALID_ARGUMENT;

  522.     }

  523.     (void)memset(plan, 0, sizeof(*plan));

  524. 

  525.     target = (double)request->targetFrequencyHz;

  526.     if (target > (double)request->maxFrequencyHz)

  527.     {

  528.         target = (double)request->maxFrequencyHz;

  529.     }

  530.     start = (double)request->startFrequencyHz;

  531.     if (start > target)

  532.     {

  533.         start = target;

  534.     }

  535.     stop = (double)request->stopFrequencyHz;

  536.     accelSlope = (double)request->accelSlopeHzPerMs;

  537.     decelSlope = (double)request->decelSlopeHzPerMs;

  538. 

  539.     accelTime = (target > start) ? (target - start) / accelSlope : 0.0;

  540.     decelTime = (target > stop) ? (target - stop) / decelSlope : 0.0;

  541.     accelPulses = (start + target) * accelTime / 2000.0;

  542.     decelPulses = (target + stop) * decelTime / 2000.0;

  543. 

  544.     if ((accelPulses + decelPulses) >= (double)pulses)

  545.     {

  546.         /* 三角曲线：求峰值频率。 */

  547.         double sum = (double)pulses * 1000.0

  548.                      + start * start / (2.0 * accelSlope)

  549.                      + stop * stop / (2.0 * decelSlope);

  550.         double denom = 1.0 / (2.0 * accelSlope)

  551.                        + 1.0 / (2.0 * decelSlope);

  552. 

  553.         peak = (denom > 0.0) ? sqrt(sum / denom) : target;

  554.         accelTime = (peak > start) ? (peak - start) / accelSlope : 0.0;

  555.         decelTime = (peak > stop) ? (peak - stop) / decelSlope : 0.0;

  556.         accelPulses = (start + peak) * accelTime / 2000.0;

  557.         decelPulses = (peak + stop) * decelTime / 2000.0;

  558.         plan->peakFrequencyHz = (uint32_t)peak;

  559.         plan->triangular = 1U;

  560.     }

  561.     else

  562.     {

  563.         cruisePulses = (double)pulses - accelPulses - decelPulses;

  564.         cruiseTime = (target > 0.0) ? cruisePulses * 1000.0 / target : 0.0;

  565.         plan->cruiseTimeMs = (uint32_t)cruiseTime;

  566.         plan->peakFrequencyHz = (uint32_t)target;

  567.         plan->triangular = 0U;

  568.     }

  569. 

  570.     plan->accelTimeMs = (uint32_t)accelTime;

  571.     plan->decelTimeMs = (uint32_t)decelTime;

  572.     plan->totalTimeMs = plan->accelTimeMs + plan->cruiseTimeMs

  573.                         + plan->decelTimeMs;

  574.     plan->totalPulses = pulses;

  575.     return PLSR_RESULT_OK;

  576. }

  577. 

  578. /* 虚拟发射计数校准到硬件实际计数。

  579.  * ARPE 预装载使硬件频率切换滞后于 profile 理想频率（约一个周期），

  580.  * 不校准会导致虚拟计数提前到达 totalPulses：profile DONE 时硬件仍欠发，

  581.  * PWM 以 DONE 瞬间的冻结频率补发剩余脉冲（段尾低频平台/宽脉冲）。

  582.  * 每刷新周期由 core 在 Step 前调用，偏差不跨周期累积。 */

  583. void PlsrProfileSyncPulses(PLSR_PROFILE_STATE *state, uint64_t hwPulses)

  584. {

  585.     if (state == NULL)

  586.     {

  587.         return;

  588.     }

  589.     state->emittedPulsesQ32 = hwPulses << 32U;

  590. }

  591. 

  592. uint32_t PlsrProfileGetInitialOutputFrequency(

  593.     const PLSR_PROFILE_STATE *state)

  594. {

  595.     uint64_t accelerationHzPerSecond;

  596.     uint64_t pulseDiscriminant;

  597.     uint64_t targetSquare;

  598.     uint64_t currentSquare;

  599.     uint64_t denominator;

  600.     uint32_t currentHz;

  601.     uint32_t endHz;

  602.     uint32_t result;

  603. 

  604.     if (state == NULL)

  605.     {

  606.         return 0UL;

  607.     }

  608.     currentHz = (uint32_t)(state->frequencyQ32 >> 32U);

  609.     if ((state->phase != PLSR_PROFILE_PHASE_ACCEL)

  610.         || (state->curveMode != PLSR_PROFILE_CURVE_LINEAR)

  611.         || (state->accelSlopeHzPerMs == 0UL)

  612.         || (currentHz >= state->targetFrequencyHz))

  613.     {

  614.         return currentHz;

  615.     }

  616. 

  617.     accelerationHzPerSecond =

  618.         (uint64_t)state->accelSlopeHzPerMs * UINT64_C(1000);

  619.     currentSquare = (uint64_t)currentHz * currentHz;

  620.     targetSquare = (uint64_t)state->targetFrequencyHz

  621.                    * state->targetFrequencyHz;

  622.     pulseDiscriminant = currentSquare

  623.                         + UINT64_C(2) * accelerationHzPerSecond;

  624. 

  625.     if (pulseDiscriminant <= targetSquare)

  626.     {

  627.         /* 整个首脉冲都处于线性加速段：f1^2=f0^2+2a。 */

  628.         endHz = PlsrProfileSqrtU64(pulseDiscriminant);

  629.         result = (currentHz + endHz) / 2UL;

  630.     }

  631.     else

  632.     {

  633.         /* 不到一个脉冲就到达目标频率：剩余相位按目标频率运行。 */

  634.         uint64_t gap = (uint64_t)state->targetFrequencyHz - currentHz;

  635. 

  636.         denominator = gap * gap + UINT64_C(2) * accelerationHzPerSecond;

  637.         result = (uint32_t)((UINT64_C(2) * accelerationHzPerSecond

  638.                              * state->targetFrequencyHz)

  639.                             / denominator);

  640.     }

  641.     if (result == 0UL)

  642.     {

  643.         result = 1UL;

  644.     }

  645.     if (result > state->targetFrequencyHz)

  646.     {

  647.         result = state->targetFrequencyHz;

  648.     }

  649.     return result;

  650. }

  651. 

  652. uint32_t PlsrProfileGetBrakingOutputFrequency(

  653.     const PLSR_PROFILE_STATE *state,

  654.     uint64_t pulsesRemaining)

  655. {

  656.     uint64_t decelerationHzPerSecond;

  657.     uint64_t twiceDeceleration;

  658.     uint64_t stopSquare;

  659.     uint64_t targetSquare;

  660.     uint64_t brakingSquare;

  661.     uint64_t brakingPulses;

  662.     uint64_t startSquare;

  663.     uint64_t endSquare;

  664.     uint32_t startHz;

  665.     uint32_t endHz;

  666.     uint32_t result;

  667. 

  668.     if ((state == NULL) || (pulsesRemaining == 0UL))

  669.     {

  670.         return 0UL;

  671.     }

  672.     if ((state->curveMode != PLSR_PROFILE_CURVE_LINEAR)

  673.         || (state->decelSlopeHzPerMs == 0UL))

  674.     {

  675.         return state->targetFrequencyHz;

  676.     }

  677. 

  678.     decelerationHzPerSecond =

  679.         (uint64_t)state->decelSlopeHzPerMs * UINT64_C(1000);

  680.     twiceDeceleration = UINT64_C(2) * decelerationHzPerSecond;

  681.     stopSquare = (uint64_t)state->stopFrequencyHz

  682.                  * state->stopFrequencyHz;

  683.     targetSquare = (uint64_t)state->targetFrequencyHz

  684.                    * state->targetFrequencyHz;

  685.     if (targetSquare <= stopSquare)

  686.     {

  687.         return state->targetFrequencyHz;

  688.     }

  689. 

  690.     /* 先比较制动距离，再做乘法。这样超长路径直接保持目标频率，

  691.      * 同时避免 2*a*n 在极端脉冲数下发生 uint64_t 溢出。 */

  692.     brakingSquare = targetSquare - stopSquare;

  693.     brakingPulses = brakingSquare / twiceDeceleration;

  694.     if ((brakingSquare % twiceDeceleration) != 0UL)

  695.     {

  696.         brakingPulses++;

  697.     }

  698.     if (pulsesRemaining > brakingPulses)

  699.     {

  700.         return state->targetFrequencyHz;

  701.     }

  702.     startSquare = stopSquare

  703.                   + twiceDeceleration * pulsesRemaining;

  704.     endSquare = stopSquare

  705.                 + twiceDeceleration * (pulsesRemaining - 1UL);

  706.     startHz = PlsrProfileSqrtU64(startSquare);

  707.     endHz = PlsrProfileSqrtU64(endSquare);

  708.     result = (startHz + endHz) / 2UL;

  709. 

  710.     if (result < state->stopFrequencyHz)

  711.     {

  712.         result = state->stopFrequencyHz;

  713.     }

  714.     if (result > state->targetFrequencyHz)

  715.     {

  716.         result = state->targetFrequencyHz;

  717.     }

  718.     if (result == 0UL)

  719.     {

  720.         result = 1UL;

  721.     }

  722.     return result;

  723. }