yuwenhao
/
Modbus-rtu
1
0
Forkuj 0
Kod Zgłoszenia 0 Oczekujące zmiany 0 Wydania 0 Wiki Aktywność
Nie możesz wybrać więcej, niż 25 tematów Tematy muszą się zaczynać od litery lub cyfry, mogą zawierać myślniki ('-') i mogą mieć do 35 znaków.
53 Commity
3 Gałęzie
138 MiB
 
 
 
 
 
 
Drzewo: dd4a42e3b9
Gałęzie Tagi
${ item.name }
Utwórz gałąź ${ searchTerm }
z 'dd4a42e3b9'
${ noResults }
Modbus-rtu/PLSR/Src/plsr_planner.c

1516 wiersze
58 KiB
Czysty Wina Historia 

  1. #include "plsr_planner.h"

  2. 

  3. #include <stddef.h>

  4. #include <string.h>

  5. 

  6. #define PLSR_PLANNER_Q32_ONE (4294967296ULL)

  7. 

  8. #if !defined(PLSR_HOST_TEST)

  9. #define PLSR_PLANNER_BENCHMARK_DEMCR_ADDRESS  (0xE000EDFCUL)

  10. #define PLSR_PLANNER_BENCHMARK_DWT_CTRL       (0xE0001000UL)

  11. #define PLSR_PLANNER_BENCHMARK_DWT_CYCCNT     (0xE0001004UL)

  12. #define PLSR_PLANNER_BENCHMARK_TRCENA         (1UL << 24U)

  13. #define PLSR_PLANNER_BENCHMARK_CYCCNTENA      (1UL << 0U)

  14. #define PLSR_PLANNER_BENCHMARK_PULSES         (100000UL)

  15. #define PLSR_PLANNER_BENCHMARK_BATCH_CALLS    (128UL)

  16. #define PLSR_PLANNER_BENCHMARK_TARGET_HZ      (100000UL)

  17. 

  18. volatile uint32_t PlsrPlannerBenchmarkRequest;

  19. volatile uint32_t PlsrPlannerBenchmarkRunning;

  20. volatile uint32_t PlsrPlannerBenchmarkRunCount;

  21. volatile uint32_t PlsrPlannerBenchmarkCoreClockHz = 168000000UL;

  22. volatile PLSR_PLANNER_BENCHMARK_RESULT PlsrPlannerBenchmarkResults[3];

  23. #endif

  24. 

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

  26.     && !defined(PLSR_HOST_TEST)

  27. #define PLSR_PLANNER_CYCCNT_ADDRESS (0xE0001004UL)

  28. 

  29. volatile PLSR_PLANNER_TIMING PlsrPlannerTiming;

  30. 

  31. static uint32_t PlsrPlannerTimingNow(void)

  32. {

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

  34. }

  35. 

  36. static void PlsrPlannerTimingRecord(

  37.     volatile PLSR_PLANNER_TIMING_SAMPLE *sample,

  38.     uint32_t startedAt)

  39. {

  40.     uint32_t elapsed = PlsrPlannerTimingNow() - startedAt;

  41.     uint32_t total = sample->totalCycles;

  42. 

  43.     sample->lastCycles = elapsed;

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

  45.     {

  46.         sample->minCycles = elapsed;

  47.     }

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

  49.     {

  50.         sample->callCount++;

  51.     }

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

  53.                               ? 0xFFFFFFFFUL : total + elapsed;

  54.     if (elapsed > sample->maxCycles)

  55.     {

  56.         sample->maxCycles = elapsed;

  57.     }

  58. }

  59. 

  60. void PlsrPlannerTimingReset(void)

  61. {

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

  63. }

  64. #endif

  65. 

  66. #define PLSR_PLANNER_CURVE_TABLE_BITS       (9U)

  67. #define PLSR_PLANNER_CURVE_TABLE_INTERVALS  (1UL << PLSR_PLANNER_CURVE_TABLE_BITS)

  68. #define PLSR_PLANNER_CURVE_TABLE_SIZE       (PLSR_PLANNER_CURVE_TABLE_INTERVALS + 1UL)

  69. #define PLSR_PLANNER_CURVE_DERIVATIVE_SHIFT (8U + PLSR_PLANNER_CURVE_TABLE_BITS)

  70. 

  71. /* curveMode 1: fixed seven-section jerk profile.  Each entry/exit ramp

  72.    uses a 1:2:1 constant-jerk/constant-acceleration/constant-jerk ratio.

  73.    The table stores the integral of the normalized frequency blend. */

  74. static const uint32_t PlsrPlannerSmoothIntegralQ24[PLSR_PLANNER_CURVE_TABLE_SIZE] =

  75. {

  76.     0UL, 0UL, 1UL, 3UL, 7UL, 14UL, 24UL, 38UL,

  77.     57UL, 81UL, 111UL, 148UL, 192UL, 244UL, 305UL, 375UL,

  78.     455UL, 546UL, 648UL, 762UL, 889UL, 1029UL, 1183UL, 1352UL,

  79.     1536UL, 1736UL, 1953UL, 2187UL, 2439UL, 2710UL, 3000UL, 3310UL,

  80.     3641UL, 3993UL, 4367UL, 4764UL, 5184UL, 5628UL, 6097UL, 6591UL,

  81.     7111UL, 7658UL, 8232UL, 8834UL, 9465UL, 10125UL, 10815UL, 11536UL,

  82.     12288UL, 13072UL, 13889UL, 14739UL, 15623UL, 16542UL, 17496UL, 18486UL,

  83.     19513UL, 20577UL, 21679UL, 22820UL, 24000UL, 25220UL, 26481UL, 27783UL,

  84.     29127UL, 30514UL, 31944UL, 33418UL, 34937UL, 36501UL, 38111UL, 39768UL,

  85.     41472UL, 43224UL, 45025UL, 46875UL, 48775UL, 50726UL, 52728UL, 54782UL,

  86.     56889UL, 59049UL, 61263UL, 63532UL, 65856UL, 68236UL, 70673UL, 73167UL,

  87.     75719UL, 78330UL, 81000UL, 83730UL, 86521UL, 89373UL, 92287UL, 95264UL,

  88.     98304UL, 101408UL, 104577UL, 107811UL, 111111UL, 114478UL, 117912UL, 121414UL,

  89.     124985UL, 128625UL, 132335UL, 136116UL, 139968UL, 143892UL, 147889UL, 151959UL,

  90.     156103UL, 160322UL, 164616UL, 168986UL, 173433UL, 177957UL, 182559UL, 187240UL,

  91.     192000UL, 196840UL, 201761UL, 206763UL, 211847UL, 217014UL, 222264UL, 227598UL,

  92.     233017UL, 238521UL, 244110UL, 249785UL, 255545UL, 261390UL, 267321UL, 273337UL,

  93.     279438UL, 285625UL, 291897UL, 298254UL, 304697UL, 311225UL, 317838UL, 324537UL,

  94.     331321UL, 338190UL, 345145UL, 352185UL, 359310UL, 366521UL, 373817UL, 381198UL,

  95.     388665UL, 396217UL, 403854UL, 411577UL, 419385UL, 427278UL, 435257UL, 443321UL,

  96.     451470UL, 459705UL, 468025UL, 476430UL, 484921UL, 493497UL, 502158UL, 510905UL,

  97.     519737UL, 528654UL, 537657UL, 546745UL, 555918UL, 565177UL, 574521UL, 583950UL,

  98.     593465UL, 603065UL, 612750UL, 622521UL, 632377UL, 642318UL, 652345UL, 662457UL,

  99.     672654UL, 682937UL, 693305UL, 703758UL, 714297UL, 724921UL, 735630UL, 746425UL,

  100.     757305UL, 768270UL, 779321UL, 790457UL, 801678UL, 812985UL, 824377UL, 835854UL,

  101.     847417UL, 859065UL, 870798UL, 882617UL, 894521UL, 906510UL, 918585UL, 930745UL,

  102.     942990UL, 955321UL, 967737UL, 980238UL, 992825UL, 1005497UL, 1018254UL, 1031097UL,

  103.     1044025UL, 1057038UL, 1070137UL, 1083321UL, 1096590UL, 1109945UL, 1123385UL, 1136910UL,

  104.     1150521UL, 1164217UL, 1177998UL, 1191865UL, 1205817UL, 1219854UL, 1233977UL, 1248185UL,

  105.     1262478UL, 1276857UL, 1291321UL, 1305870UL, 1320505UL, 1335225UL, 1350030UL, 1364921UL,

  106.     1379897UL, 1394958UL, 1410105UL, 1425337UL, 1440654UL, 1456057UL, 1471545UL, 1487118UL,

  107.     1502777UL, 1518521UL, 1534350UL, 1550265UL, 1566265UL, 1582350UL, 1598521UL, 1614777UL,

  108.     1631118UL, 1647545UL, 1664057UL, 1680654UL, 1697337UL, 1714105UL, 1730958UL, 1747897UL,

  109.     1764921UL, 1782030UL, 1799225UL, 1816505UL, 1833870UL, 1851321UL, 1868857UL, 1886478UL,

  110.     1904185UL, 1921977UL, 1939854UL, 1957817UL, 1975865UL, 1993998UL, 2012217UL, 2030521UL,

  111.     2048910UL, 2067385UL, 2085945UL, 2104590UL, 2123321UL, 2142137UL, 2161038UL, 2180025UL,

  112.     2199097UL, 2218254UL, 2237497UL, 2256825UL, 2276238UL, 2295737UL, 2315321UL, 2334990UL,

  113.     2354745UL, 2374585UL, 2394510UL, 2414521UL, 2434617UL, 2454798UL, 2475065UL, 2495417UL,

  114.     2515854UL, 2536377UL, 2556985UL, 2577678UL, 2598457UL, 2619321UL, 2640270UL, 2661305UL,

  115.     2682425UL, 2703630UL, 2724921UL, 2746297UL, 2767758UL, 2789305UL, 2810937UL, 2832654UL,

  116.     2854457UL, 2876345UL, 2898318UL, 2920377UL, 2942521UL, 2964750UL, 2987065UL, 3009465UL,

  117.     3031950UL, 3054521UL, 3077177UL, 3099918UL, 3122745UL, 3145657UL, 3168654UL, 3191737UL,

  118.     3214905UL, 3238158UL, 3261497UL, 3284921UL, 3308430UL, 3332025UL, 3355705UL, 3379470UL,

  119.     3403321UL, 3427257UL, 3451278UL, 3475385UL, 3499577UL, 3523854UL, 3548217UL, 3572665UL,

  120.     3597198UL, 3621817UL, 3646521UL, 3671310UL, 3696185UL, 3721145UL, 3746190UL, 3771321UL,

  121.     3796537UL, 3821838UL, 3847225UL, 3872697UL, 3898254UL, 3923897UL, 3949625UL, 3975438UL,

  122.     4001337UL, 4027321UL, 4053390UL, 4079545UL, 4105785UL, 4132110UL, 4158521UL, 4185017UL,

  123.     4211598UL, 4238265UL, 4265017UL, 4291854UL, 4318777UL, 4345785UL, 4372878UL, 4400057UL,

  124.     4427321UL, 4454670UL, 4482104UL, 4509622UL, 4537223UL, 4564907UL, 4592673UL, 4620520UL,

  125.     4648448UL, 4676456UL, 4704543UL, 4732709UL, 4760953UL, 4789274UL, 4817672UL, 4846146UL,

  126.     4874695UL, 4903319UL, 4932017UL, 4960788UL, 4989632UL, 5018548UL, 5047535UL, 5076593UL,

  127.     5105721UL, 5134918UL, 5164184UL, 5193518UL, 5222919UL, 5252387UL, 5281921UL, 5311520UL,

  128.     5341184UL, 5370912UL, 5400703UL, 5430557UL, 5460473UL, 5490450UL, 5520488UL, 5550586UL,

  129.     5580743UL, 5610959UL, 5641233UL, 5671564UL, 5701952UL, 5732396UL, 5762895UL, 5793449UL,

  130.     5824057UL, 5854718UL, 5885432UL, 5916198UL, 5947015UL, 5977883UL, 6008801UL, 6039768UL,

  131.     6070784UL, 6101848UL, 6132959UL, 6164117UL, 6195321UL, 6226570UL, 6257864UL, 6289202UL,

  132.     6320583UL, 6352007UL, 6383473UL, 6414980UL, 6446528UL, 6478116UL, 6509743UL, 6541409UL,

  133.     6573113UL, 6604854UL, 6636632UL, 6668446UL, 6700295UL, 6732179UL, 6764097UL, 6796048UL,

  134.     6828032UL, 6860048UL, 6892095UL, 6924173UL, 6956281UL, 6988418UL, 7020584UL, 7052778UL,

  135.     7084999UL, 7117247UL, 7149521UL, 7181820UL, 7214144UL, 7246492UL, 7278863UL, 7311257UL,

  136.     7343673UL, 7376110UL, 7408568UL, 7441046UL, 7473543UL, 7506059UL, 7538593UL, 7571144UL,

  137.     7603712UL, 7636296UL, 7668895UL, 7701509UL, 7734137UL, 7766778UL, 7799432UL, 7832098UL,

  138.     7864775UL, 7897463UL, 7930161UL, 7962868UL, 7995584UL, 8028308UL, 8061039UL, 8093777UL,

  139.     8126521UL, 8159270UL, 8192024UL, 8224782UL, 8257543UL, 8290307UL, 8323073UL, 8355840UL,

  140.     8388608UL

  141. };

  142. 

  143. static const uint32_t PlsrPlannerSineIntegralQ24[PLSR_PLANNER_CURVE_TABLE_SIZE] =

  144. {

  145.     0UL, 0UL, 1UL, 3UL, 7UL, 13UL, 22UL, 35UL,

  146.     53UL, 75UL, 103UL, 137UL, 178UL, 226UL, 282UL, 347UL,

  147.     421UL, 505UL, 599UL, 705UL, 822UL, 951UL, 1094UL, 1250UL,

  148.     1420UL, 1604UL, 1805UL, 2021UL, 2254UL, 2503UL, 2771UL, 3057UL,

  149.     3362UL, 3687UL, 4032UL, 4398UL, 4785UL, 5194UL, 5626UL, 6081UL,

  150.     6560UL, 7063UL, 7592UL, 8146UL, 8726UL, 9333UL, 9967UL, 10630UL,

  151.     11321UL, 12041UL, 12791UL, 13571UL, 14382UL, 15225UL, 16100UL, 17008UL,

  152.     17949UL, 18923UL, 19933UL, 20977UL, 22057UL, 23173UL, 24325UL, 25516UL,

  153.     26744UL, 28010UL, 29316UL, 30661UL, 32046UL, 33472UL, 34939UL, 36449UL,

  154.     38000UL, 39595UL, 41233UL, 42915UL, 44642UL, 46414UL, 48232UL, 50096UL,

  155.     52007UL, 53966UL, 55972UL, 58027UL, 60131UL, 62284UL, 64487UL, 66742UL,

  156.     69047UL, 71404UL, 73813UL, 76275UL, 78790UL, 81359UL, 83982UL, 86660UL,

  157.     89393UL, 92182UL, 95028UL, 97930UL, 100890UL, 103908UL, 106984UL, 110119UL,

  158.     113314UL, 116568UL, 119882UL, 123258UL, 126695UL, 130194UL, 133755UL, 137379UL,

  159.     141066UL, 144817UL, 148632UL, 152512UL, 156457UL, 160468UL, 164545UL, 168688UL,

  160.     172899UL, 177177UL, 181523UL, 185937UL, 190421UL, 194973UL, 199596UL, 204289UL,

  161.     209052UL, 213886UL, 218792UL, 223770UL, 228820UL, 233943UL, 239140UL, 244409UL,

  162.     249753UL, 255172UL, 260665UL, 266234UL, 271878UL, 277599UL, 283396UL, 289270UL,

  163.     295221UL, 301250UL, 307358UL, 313543UL, 319808UL, 326151UL, 332575UL, 339078UL,

  164.     345662UL, 352326UL, 359072UL, 365899UL, 372808UL, 379799UL, 386873UL, 394029UL,

  165.     401269UL, 408592UL, 416000UL, 423491UL, 431068UL, 438729UL, 446475UL, 454307UL,

  166.     462225UL, 470229UL, 478320UL, 486497UL, 494762UL, 503114UL, 511554UL, 520082UL,

  167.     528698UL, 537403UL, 546197UL, 555080UL, 564053UL, 573116UL, 582268UL, 591511UL,

  168.     600845UL, 610269UL, 619785UL, 629392UL, 639090UL, 648881UL, 658763UL, 668739UL,

  169.     678806UL, 688967UL, 699221UL, 709568UL, 720008UL, 730543UL, 741171UL, 751894UL,

  170.     762711UL, 773623UL, 784629UL, 795731UL, 806928UL, 818220UL, 829608UL, 841092UL,

  171.     852672UL, 864348UL, 876121UL, 887990UL, 899955UL, 912018UL, 924178UL, 936435UL,

  172.     948789UL, 961241UL, 973790UL, 986438UL, 999183UL, 1012026UL, 1024968UL, 1038007UL,

  173.     1051146UL, 1064383UL, 1077718UL, 1091153UL, 1104686UL, 1118319UL, 1132051UL, 1145882UL,

  174.     1159812UL, 1173841UL, 1187971UL, 1202200UL, 1216528UL, 1230956UL, 1245485UL, 1260113UL,

  175.     1274841UL, 1289669UL, 1304597UL, 1319626UL, 1334754UL, 1349983UL, 1365312UL, 1380742UL,

  176.     1396271UL, 1411902UL, 1427632UL, 1443464UL, 1459395UL, 1475428UL, 1491560UL, 1507793UL,

  177.     1524127UL, 1540561UL, 1557096UL, 1573732UL, 1590467UL, 1607304UL, 1624240UL, 1641278UL,

  178.     1658415UL, 1675654UL, 1692992UL, 1710431UL, 1727970UL, 1745610UL, 1763349UL, 1781189UL,

  179.     1799129UL, 1817169UL, 1835309UL, 1853548UL, 1871888UL, 1890328UL, 1908867UL, 1927505UL,

  180.     1946244UL, 1965082UL, 1984019UL, 2003055UL, 2022190UL, 2041425UL, 2060758UL, 2080191UL,

  181.     2099722UL, 2119351UL, 2139080UL, 2158906UL, 2178831UL, 2198854UL, 2218974UL, 2239193UL,

  182.     2259509UL, 2279923UL, 2300434UL, 2321042UL, 2341747UL, 2362550UL, 2383449UL, 2404444UL,

  183.     2425536UL, 2446724UL, 2468008UL, 2489388UL, 2510864UL, 2532435UL, 2554101UL, 2575863UL,

  184.     2597719UL, 2619670UL, 2641715UL, 2663855UL, 2686088UL, 2708416UL, 2730837UL, 2753351UL,

  185.     2775958UL, 2798659UL, 2821451UL, 2844337UL, 2867314UL, 2890384UL, 2913545UL, 2936797UL,

  186.     2960141UL, 2983575UL, 3007100UL, 3030716UL, 3054421UL, 3078216UL, 3102101UL, 3126075UL,

  187.     3150138UL, 3174290UL, 3198530UL, 3222858UL, 3247274UL, 3271777UL, 3296368UL, 3321045UL,

  188.     3345809UL, 3370659UL, 3395595UL, 3420617UL, 3445724UL, 3470915UL, 3496192UL, 3521552UL,

  189.     3546997UL, 3572525UL, 3598137UL, 3623831UL, 3649608UL, 3675467UL, 3701408UL, 3727430UL,

  190.     3753534UL, 3779718UL, 3805983UL, 3832327UL, 3858752UL, 3885255UL, 3911838UL, 3938498UL,

  191.     3965237UL, 3992054UL, 4018948UL, 4045919UL, 4072966UL, 4100090UL, 4127289UL, 4154564UL,

  192.     4181913UL, 4209337UL, 4236836UL, 4264407UL, 4292052UL, 4319770UL, 4347560UL, 4375422UL,

  193.     4403356UL, 4431361UL, 4459436UL, 4487581UL, 4515797UL, 4544081UL, 4572435UL, 4600857UL,

  194.     4629347UL, 4657904UL, 4686529UL, 4715220UL, 4743977UL, 4772800UL, 4801688UL, 4830641UL,

  195.     4859658UL, 4888739UL, 4917883UL, 4947090UL, 4976359UL, 5005690UL, 5035082UL, 5064536UL,

  196.     5094050UL, 5123623UL, 5153256UL, 5182948UL, 5212698UL, 5242506UL, 5272372UL, 5302294UL,

  197.     5332273UL, 5362308UL, 5392398UL, 5422543UL, 5452742UL, 5482995UL, 5513301UL, 5543660UL,

  198.     5574071UL, 5604534UL, 5635047UL, 5665612UL, 5696227UL, 5726891UL, 5757604UL, 5788366UL,

  199.     5819175UL, 5850032UL, 5880936UL, 5911886UL, 5942882UL, 5973923UL, 6005009UL, 6036139UL,

  200.     6067312UL, 6098529UL, 6129787UL, 6161088UL, 6192430UL, 6223813UL, 6255236UL, 6286698UL,

  201.     6318200UL, 6349740UL, 6381317UL, 6412933UL, 6444585UL, 6476273UL, 6507997UL, 6539755UL,

  202.     6571549UL, 6603376UL, 6635236UL, 6667129UL, 6699054UL, 6731011UL, 6762999UL, 6795017UL,

  203.     6827065UL, 6859142UL, 6891247UL, 6923381UL, 6955542UL, 6987730UL, 7019944UL, 7052183UL,

  204.     7084448UL, 7116737UL, 7149050UL, 7181386UL, 7213745UL, 7246126UL, 7278528UL, 7310951UL,

  205.     7343394UL, 7375857UL, 7408339UL, 7440839UL, 7473358UL, 7505893UL, 7538445UL, 7571012UL,

  206.     7603596UL, 7636194UL, 7668806UL, 7701431UL, 7734070UL, 7766721UL, 7799383UL, 7832057UL,

  207.     7864741UL, 7897435UL, 7930138UL, 7962850UL, 7995570UL, 8028297UL, 8061031UL, 8093771UL,

  208.     8126517UL, 8159267UL, 8192022UL, 8224781UL, 8257543UL, 8290307UL, 8323073UL, 8355840UL,

  209.     8388608UL

  210. };

  211. 

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

  213. {

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

  215. }

  216. 

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

  218.                                     

  219.                                    

  220. {

  221.     uint32_t baseTimeMs;

  222.     uint64_t durationMs;

  223. 

  224.     if (fromHz == toHz)

  225.     {

  226.         return 0UL;

  227.     }

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

  229.                                  : block->decelerationTimeMs;

  230.     if (baseTimeMs == 0UL)

  231.     {

  232.         return 0UL;

  233.     }

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

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

  236.                  / block->referenceSpeedHz;

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

  238.                                         : (uint32_t)durationMs;

  239. }

  240. 

  241. static uint16_t PlsrPlannerBaseRampTime(const PLSR_MOTION_BLOCK *block,

  242.                                         uint32_t fromHz,

  243.                                         uint32_t toHz)

  244. {

  245.     if (toHz > fromHz)

  246.     {

  247.         return block->accelerationTimeMs;

  248.     }

  249.     if (toHz < fromHz)

  250.     {

  251.         return block->decelerationTimeMs;

  252.     }

  253.     return 0U;

  254. }

  255. 

  256. static uint64_t PlsrPlannerRampWeight(uint32_t fromHz,

  257.                                       uint32_t toHz,

  258.                                       uint16_t baseTimeMs)

  259. {

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

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

  262.     uint64_t difference = (fromSquared > toSquared)

  263.                               ? (fromSquared - toSquared)

  264.                               : (toSquared - fromSquared);

  265. 

  266.     return difference * baseTimeMs;

  267. }

  268. 

  269. static uint64_t PlsrPlannerRequiredPulses(const PLSR_MOTION_BLOCK *block,

  270.                                           uint64_t rampWeight)

  271. {

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

  273. 

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

  275.                                 : (rampWeight + denominator - 1ULL)

  276.                                       / denominator;

  277. }

  278. 

  279. static uint32_t PlsrPlannerIntegerSquareRoot(uint64_t value)

  280. {

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

  282.     uint64_t root = 0ULL;

  283. 

  284.     while (bit > value)

  285.     {

  286.         bit >>= 2U;

  287.     }

  288.     while (bit != 0ULL)

  289.     {

  290.         if (value >= root + bit)

  291.         {

  292.             value -= root + bit;

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

  294.         }

  295.         else

  296.         {

  297.             root >>= 1U;

  298.         }

  299.         bit >>= 2U;

  300.     }

  301.     return (uint32_t)root;

  302. }

  303. 

  304. static uint32_t PlsrPlannerReachableFrequency(

  305.     const PLSR_MOTION_BLOCK *block,

  306.     uint32_t fromHz,

  307.     uint32_t towardHz,

  308.     uint32_t pulseCount)

  309. {

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

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

  312.     uint64_t changeSquared;

  313.     uint32_t reachableHz;

  314. 

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

  316.     {

  317.         return towardHz;

  318.     }

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

  320.                     * 1000UL / baseTimeMs;

  321.     if (towardHz > fromHz)

  322.     {

  323.         reachableHz = PlsrPlannerIntegerSquareRoot(

  324.             frequencySquared + changeSquared);

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

  326.     }

  327.     frequencySquared = (changeSquared >= frequencySquared)

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

  329.     reachableHz = PlsrPlannerIntegerSquareRoot(frequencySquared);

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

  331.     {

  332.         reachableHz++;

  333.     }

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

  335. }

  336. 

  337. static uint32_t PlsrPlannerPeak(const PLSR_MOTION_BLOCK *block,

  338.                                 uint32_t startHz,

  339.                                 uint32_t endHz)

  340. {

  341.     uint32_t targetHz = block->cruiseHz;

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

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

  344.     uint16_t entryTimeMs;

  345.     uint16_t exitTimeMs;

  346.     uint32_t timeSumMs;

  347.     uint64_t weightedEndpoints;

  348.     uint64_t availableArea;

  349.     uint64_t peakSquared;

  350.     uint32_t peakHz;

  351. 

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

  353.     {

  354.         return targetHz;

  355.     }

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

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

  358.     timeSumMs = (uint32_t)entryTimeMs + exitTimeMs;

  359.     if (timeSumMs == 0UL)

  360.     {

  361.         return targetHz;

  362.     }

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

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

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

  366.                     * block->referenceSpeedHz * 1000UL;

  367. 

  368.     if (targetHz > upperEndpoint)

  369.     {

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

  371.         peakHz = PlsrPlannerIntegerSquareRoot(peakSquared);

  372.         if (peakHz < upperEndpoint)

  373.         {

  374.             peakHz = upperEndpoint;

  375.         }

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

  377.     }

  378. 

  379.     if (availableArea >= weightedEndpoints)

  380.     {

  381.         return targetHz;

  382.     }

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

  384.     peakHz = PlsrPlannerIntegerSquareRoot(peakSquared);

  385.     if (peakHz < targetHz)

  386.     {

  387.         peakHz = targetHz;

  388.     }

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

  390. }

  391. //C(x)函数

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

  393. static uint64_t PlsrPlannerCurveIntegralQ32(uint64_t progressQ32,

  394.                                             uint16_t curveMode)

  395. {

  396.     const uint32_t *table;

  397.     uint64_t scaled;

  398.     uint32_t index;

  399.     uint32_t fraction;

  400.     uint64_t first;

  401.     uint64_t second;

  402. 

  403.     if (progressQ32 >= PLSR_PLANNER_Q32_ONE)

  404.     {

  405.         return PLSR_PLANNER_Q32_ONE / 2ULL;

  406.     }

  407.     if (curveMode == 0U)

  408.     {

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

  410.     }

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

  412.                               : PlsrPlannerSineIntegralQ24;

  413.     scaled = progressQ32 * PLSR_PLANNER_CURVE_TABLE_INTERVALS;

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

  415.     fraction = (uint32_t)scaled;

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

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

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

  419. }

  420. 

  421. static uint64_t PlsrPlannerRampAreaQ32(const PLSR_PLANNER_CONTEXT *context,

  422.                                        uint32_t fromHz,

  423.                                        uint32_t toHz,

  424.                                        uint64_t progressQ32)

  425. {

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

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

  428.                    + delta * (int64_t)PlsrPlannerCurveIntegralQ32(

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

  430. 

  431.     return (uint64_t)area;

  432. }

  433. 

  434. static uint64_t PlsrPlannerExactBoundaryQ32(

  435.     const PLSR_PLANNER_CONTEXT *context,

  436.     uint64_t previousBoundaryQ32,

  437.     uint64_t targetAreaQ32)

  438. {

  439.     uint64_t lowerQ32 = previousBoundaryQ32;

  440.     uint64_t upperQ32 = PLSR_PLANNER_Q32_ONE;

  441.     uint64_t middleQ32;

  442.     uint32_t iteration;

  443. 

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

  445.     {

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

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

  448.                                    context->rampToHz, middleQ32)

  449.             < targetAreaQ32)

  450.         {

  451.             lowerQ32 = middleQ32;

  452.         }

  453.         else

  454.         {

  455.             upperQ32 = middleQ32;

  456.         }

  457.     }

  458.     return upperQ32;

  459. }

  460. 

  461. static uint32_t PlsrPlannerInstantFrequency(

  462.     const PLSR_PLANNER_CONTEXT *context,

  463.     uint64_t progressQ32)

  464. {

  465.     const uint32_t *table;

  466.     uint64_t scaled;

  467.     uint64_t curveProgressQ32;

  468.     uint32_t index;

  469.     uint32_t gap;

  470. 

  471.     if (progressQ32 >= PLSR_PLANNER_Q32_ONE)

  472.     {

  473.         return context->rampToHz;

  474.     }

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

  476.     {

  477.         curveProgressQ32 = progressQ32;

  478.     }

  479.     else

  480.     {

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

  482.                     ? PlsrPlannerSmoothIntegralQ24

  483.                     : PlsrPlannerSineIntegralQ24;

  484.         scaled = progressQ32 * PLSR_PLANNER_CURVE_TABLE_INTERVALS;

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

  486.         curveProgressQ32 =

  487.             (uint64_t)(table[index + 1UL] - table[index])

  488.             << PLSR_PLANNER_CURVE_DERIVATIVE_SHIFT;

  489.     }

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

  491.     {

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

  493.         return context->rampFromHz

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

  495.     }

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

  497.     return context->rampFromHz

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

  499. }

  500. 

  501. static uint64_t PlsrPlannerPredictedBoundaryQ32(

  502.     const PLSR_PLANNER_CONTEXT *context,

  503.     uint64_t targetAreaQ32)

  504. {

  505.     uint64_t previousQ32 = context->rampBoundaryQ32;

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

  507.                                   ? 0ULL

  508.                                   : PlsrPlannerRampAreaQ32(

  509.                                         context, context->rampFromHz,

  510.                                         context->rampToHz, previousQ32);

  511.     uint64_t candidateQ32;

  512.     uint64_t candidateAreaQ32;

  513.     uint64_t differenceQ32;

  514.     uint64_t correctionQ32;

  515.     uint32_t derivativeHz;

  516. 

  517.     if (context->rampLastPhaseStepQ32

  518.         >= PLSR_PLANNER_Q32_ONE - previousQ32)

  519.     {

  520.         candidateQ32 = PLSR_PLANNER_Q32_ONE;

  521.     }

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

  523.     {

  524.         candidateQ32 = previousQ32 + context->rampLastPhaseStepQ32;

  525.     }

  526.     else

  527.     {

  528.         derivativeHz = PlsrPlannerInstantFrequency(context, previousQ32);

  529.         if (derivativeHz == 0UL)

  530.         {

  531.             derivativeHz = 1UL;

  532.         }

  533.         differenceQ32 = targetAreaQ32 - currentAreaQ32;

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

  535.                         / derivativeHz;

  536.         candidateQ32 = (correctionQ32

  537.                         >= PLSR_PLANNER_Q32_ONE - previousQ32)

  538.                            ? PLSR_PLANNER_Q32_ONE

  539.                            : previousQ32 + correctionQ32;

  540.     }

  541. 

  542.     candidateAreaQ32 = PlsrPlannerRampAreaQ32(

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

  544.     derivativeHz = PlsrPlannerInstantFrequency(context, candidateQ32);

  545.     if (derivativeHz == 0UL)

  546.     {

  547.         derivativeHz = 1UL;

  548.     }

  549.     if (candidateAreaQ32 < targetAreaQ32)

  550.     {

  551.         differenceQ32 = targetAreaQ32 - candidateAreaQ32;

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

  553.                         / derivativeHz;

  554.         candidateQ32 = (correctionQ32

  555.                         >= PLSR_PLANNER_Q32_ONE - candidateQ32)

  556.                            ? PLSR_PLANNER_Q32_ONE

  557.                            : candidateQ32 + correctionQ32;

  558.     }

  559.     else if (candidateAreaQ32 > targetAreaQ32)

  560.     {

  561.         differenceQ32 = candidateAreaQ32 - targetAreaQ32;

  562.         correctionQ32 = differenceQ32 / derivativeHz;

  563.         if (correctionQ32 == 0ULL)

  564.         {

  565.             correctionQ32 = 1ULL;

  566.         }

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

  568.                            ? previousQ32 + 1ULL

  569.                            : candidateQ32 - correctionQ32;

  570.     }

  571.     return candidateQ32;

  572. }

  573. 

  574. static void PlsrPlannerStartRamp(PLSR_PLANNER_CONTEXT *context,

  575.                                  uint8_t rampKind,

  576.                                  uint32_t fromHz,

  577.                                  uint32_t toHz,

  578.                                  uint32_t pulseCount)

  579. {

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

  581.     && !defined(PLSR_HOST_TEST)

  582.     uint32_t totalStartedAt = PlsrPlannerTimingNow();

  583.     uint32_t phaseStartedAt;

  584. #endif

  585. 

  586.     context->rampKind = rampKind;

  587.     context->rampRelativePulse = 0UL;

  588.     context->rampPulseCount = pulseCount;

  589.     context->rampFromHz = fromHz;

  590.     context->rampToHz = toHz;

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

  592.     && !defined(PLSR_HOST_TEST)

  593.     phaseStartedAt = PlsrPlannerTimingNow();

  594. #endif

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

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

  597.     && !defined(PLSR_HOST_TEST)

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

  599.     phaseStartedAt = PlsrPlannerTimingNow();

  600. #endif

  601.     context->rampTotalAreaQ32 = PlsrPlannerRampAreaQ32(

  602.         context, fromHz, toHz, PLSR_PLANNER_Q32_ONE);

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

  604.     && !defined(PLSR_HOST_TEST)

  605.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampTotalArea,

  606.                             phaseStartedAt);

  607.     phaseStartedAt = PlsrPlannerTimingNow();

  608. #endif

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

  610.     context->rampAreaRemainder =

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

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

  613.     && !defined(PLSR_HOST_TEST)

  614.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampAreaSplit,

  615.                             phaseStartedAt);

  616. #endif

  617.     context->rampRemainderAccumulator = 0UL;

  618.     context->rampTargetAreaQ32 = 0ULL;

  619.     context->rampBoundaryQ32 = 0ULL;

  620.     context->rampActualTimeQ32 = 0ULL;

  621.     context->rampLastPhaseStepQ32 = 0ULL;

  622.     context->lastRampHz = 0UL;

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

  624.     && !defined(PLSR_HOST_TEST)

  625.     phaseStartedAt = PlsrPlannerTimingNow();

  626. #endif

  627.     context->rampFirstBoundaryQ32 = PlsrPlannerExactBoundaryQ32(

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

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

  630.     && !defined(PLSR_HOST_TEST)

  631.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampFirstBoundary,

  632.                             phaseStartedAt);

  633. #endif

  634.     if (pulseCount > 1UL)

  635.     {

  636.         uint64_t secondTargetAreaQ32;

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

  638.     && !defined(PLSR_HOST_TEST)

  639.         phaseStartedAt = PlsrPlannerTimingNow();

  640. #endif

  641.         secondTargetAreaQ32 = context->rampAreaStepQ32 * 2ULL

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

  643.         context->rampSecondBoundaryQ32 = PlsrPlannerExactBoundaryQ32(

  644.             context, context->rampFirstBoundaryQ32,

  645.             secondTargetAreaQ32);

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

  647.     && !defined(PLSR_HOST_TEST)

  648.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampSecondBoundary,

  649.                                 phaseStartedAt);

  650. #endif

  651.     }

  652.     else

  653.     {

  654.         context->rampSecondBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  655.     }

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

  657.     && !defined(PLSR_HOST_TEST)

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

  659. #endif

  660. }

  661. 

  662. static uint8_t PlsrPlannerSameSetting(

  663.     const PLSR_PLATFORM_TIMER_SETTING *first,

  664.     const PLSR_PLATFORM_TIMER_SETTING *second)

  665. {

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

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

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

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

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

  671. }

  672. 

  673. static uint8_t PlsrPlannerBuildStep(PLSR_PLANNER_CONTEXT *context,

  674.                                     uint32_t requestedHz,

  675.                                     PLSR_PLATFORM_TIMER_SETTING *setting,

  676.                                     uint32_t *normalizedRequestedHz)

  677. {

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

  679.     && !defined(PLSR_HOST_TEST)

  680.     uint32_t startedAt;

  681.     uint8_t result;

  682. #endif

  683. 

  684.     if (requestedHz == 0UL)

  685.     {

  686.         requestedHz = 1UL;

  687.     }

  688.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  689.     {

  690.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  691.     }

  692.     *normalizedRequestedHz = requestedHz;

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

  694.     && !defined(PLSR_HOST_TEST)

  695.     startedAt = PlsrPlannerTimingNow();

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

  697.                                            PLSR_OUTPUT_PULSE_DIR,

  698.                                            requestedHz, setting);

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

  700.     return result;

  701. #else

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

  703.                                          PLSR_OUTPUT_PULSE_DIR,

  704.                                          requestedHz, setting);

  705. #endif

  706. }

  707. 

  708. static uint8_t PlsrPlannerTakeRampStep(

  709.     PLSR_PLANNER_CONTEXT *context,

  710.     PLSR_PLATFORM_TIMER_SETTING *setting,

  711.     uint32_t *requestedFrequencyHz)

  712. {

  713.     uint64_t nextBoundaryQ32;

  714.     uint64_t desiredDeltaQ32;

  715.     uint64_t denominator;

  716.     uint64_t requestedHz;

  717.     uint64_t actualDeltaQ32;

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

  719.     && !defined(PLSR_HOST_TEST)

  720.     uint32_t totalStartedAt = PlsrPlannerTimingNow();

  721.     uint32_t phaseStartedAt;

  722. #endif

  723. 

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

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

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

  727.     {

  728.         context->rampTargetAreaQ32++;

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

  730.     }

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

  732.     {

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

  734.         nextBoundaryQ32 = PLSR_PLANNER_Q32_ONE;

  735.     }

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

  737.     {

  738.         nextBoundaryQ32 = context->rampFirstBoundaryQ32;

  739.     }

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

  741.     {

  742.         nextBoundaryQ32 = context->rampSecondBoundaryQ32;

  743.     }

  744.     else

  745.     {

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

  747.     && !defined(PLSR_HOST_TEST)

  748.         phaseStartedAt = PlsrPlannerTimingNow();

  749. #endif

  750.         nextBoundaryQ32 = PlsrPlannerPredictedBoundaryQ32(

  751.             context, context->rampTargetAreaQ32);

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

  753.     && !defined(PLSR_HOST_TEST)

  754.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampPredictedBoundary,

  755.                                 phaseStartedAt);

  756. #endif

  757.     }

  758. 

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

  760.                           ? (nextBoundaryQ32 - context->rampActualTimeQ32)

  761.                           : 1ULL;

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

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

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

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

  766.     && !defined(PLSR_HOST_TEST)

  767.     phaseStartedAt = PlsrPlannerTimingNow();

  768. #endif

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

  770.     requestedHz = (denominator == 0ULL)

  771.                       ? context->rampToHz

  772.                       : (context->rampTotalAreaQ32

  773.                          + denominator / 2ULL) / denominator;

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

  775.     && !defined(PLSR_HOST_TEST)

  776.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampRequestedDivide,

  777.                             phaseStartedAt);

  778. #endif

  779.     if (requestedHz == 0ULL)

  780.     {

  781.         requestedHz = 1ULL;

  782.     }

  783.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  784.     {

  785.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  786.     }

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

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

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

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

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

  792.     {

  793.         requestedHz = context->lastRampHz;

  794.     }

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

  796.                              requestedFrequencyHz) == 0U)

  797.     {

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

  799.     && !defined(PLSR_HOST_TEST)

  800.         PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampStep,

  801.                                 totalStartedAt);

  802. #endif

  803.         return 0U;

  804.     }

  805. 

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

  807.     && !defined(PLSR_HOST_TEST)

  808.     phaseStartedAt = PlsrPlannerTimingNow();

  809. #endif

  810.     denominator = (uint64_t)context->rampPulseCount

  811.                   * setting->actualFrequencyHz;

  812.     actualDeltaQ32 = (denominator == 0ULL)

  813.                          ? desiredDeltaQ32

  814.                          : (context->rampTotalAreaQ32

  815.                             + denominator / 2ULL) / denominator;

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

  817.     && !defined(PLSR_HOST_TEST)

  818.     PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampActualDivide,

  819.                             phaseStartedAt);

  820. #endif

  821.     context->rampActualTimeQ32 += actualDeltaQ32;

  822.     context->rampLastPhaseStepQ32 =

  823.         nextBoundaryQ32 - context->rampBoundaryQ32;

  824.     context->rampBoundaryQ32 = nextBoundaryQ32;

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

  826.     context->rampRelativePulse++;

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

  828.     && !defined(PLSR_HOST_TEST)

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

  830. #endif

  831.     return 1U;

  832. }

  833. 

  834. static uint8_t PlsrPlannerTakeStep(PLSR_PLANNER_CONTEXT *context,

  835.                                    PLSR_PLATFORM_TIMER_SETTING *setting,

  836.                                    uint32_t *requestedFrequencyHz,

  837.                                    uint32_t *repeatCount)

  838. {

  839.     uint32_t entryEnd = context->entryPulses;

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

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

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

  843.     *repeatCount = 1UL;

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

  845.     {

  846.         return 0U;

  847.     }

  848.     if (context->generatedPulses < entryEnd)

  849.     { //加速段 

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

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

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

  853.         }

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

  855.                                        

  856.     }

  857.     if (context->generatedPulses < steadyEnd)

  858.     {

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

  860.         context->rampKind = 0U;

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

  862.                                     requestedFrequencyHz);

  863.     }

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

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

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

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

  868.     }

  869.     return PlsrPlannerTakeRampStep(context, setting,

  870.                                    requestedFrequencyHz);

  871. }

  872. 

  873. PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context,

  874.                                      const PLSR_MOTION_BLOCK *block,

  875.                                      uint32_t appliedHz,

  876.                                      uint64_t phasePulses)

  877. {

  878.     uint64_t directRequired;

  879.     uint64_t entryRequired;

  880.     uint64_t exitRequired;

  881.     uint64_t entryWeight;

  882.     uint64_t exitWeight;

  883.     uint64_t totalWeight;

  884.     uint64_t scaledEntry;

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

  886.     && !defined(PLSR_HOST_TEST)

  887.     uint32_t startedAt = PlsrPlannerTimingNow();

  888. #endif

  889. 

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

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

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

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

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

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

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

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

  898.         || (appliedHz > PLSR_FREQUENCY_MAX_HZ)

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

  900.     {

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

  902.     && !defined(PLSR_HOST_TEST)

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

  904. #endif

  905.         return PLSR_PLANNER_INVALID;

  906.     }

  907. 

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

  909.     context->block = *block;

  910.     context->phasePulses = phasePulses;

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

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

  913.     {

  914.         context->startHz = 1UL;

  915.     }

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

  917. 

  918.     directRequired = PlsrPlannerRequiredPulses(

  919.         block, PlsrPlannerRampWeight(

  920.                    context->startHz, context->endHz,

  921.                    PlsrPlannerBaseRampTime(block, context->startHz,

  922.                                            context->endHz)));

  923.     if (directRequired > block->pulseBudget)

  924.     {

  925.         context->peakHz = PlsrPlannerReachableFrequency(

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

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

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

  929.         context->clipped = 1U;

  930.         context->active = 1U;

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

  932.     && !defined(PLSR_HOST_TEST)

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

  934. #endif

  935.         return PLSR_PLANNER_CLIPPED;

  936.     }

  937. 

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

  939.                                       context->endHz);

  940.     entryWeight = PlsrPlannerRampWeight(

  941.         context->startHz, context->peakHz,

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

  943.     exitWeight = PlsrPlannerRampWeight(

  944.         context->peakHz, context->endHz,

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

  946.     entryRequired = PlsrPlannerRequiredPulses(block, entryWeight);

  947.     exitRequired = PlsrPlannerRequiredPulses(block, exitWeight);

  948. 

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

  950.     {

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

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

  953.         context->steadyPulses = block->pulseBudget

  954.                                 - context->entryPulses

  955.                                 - context->exitPulses;

  956.     }

  957.     else if (entryRequired == 0ULL)

  958.     {

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

  960.         context->clipped = 1U;

  961.     }

  962.     else if (exitRequired == 0ULL)

  963.     {

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

  965.         context->clipped = 1U;

  966.     }

  967.     else

  968.     {

  969.         totalWeight = entryWeight + exitWeight;

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

  971.                        + totalWeight / 2ULL) / totalWeight;

  972.         if (scaledEntry == 0ULL)

  973.         {

  974.             scaledEntry = 1ULL;

  975.         }

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

  977.         {

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

  979.         }

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

  981.         context->exitPulses = block->pulseBudget

  982.                               - context->entryPulses;

  983.         context->clipped = 1U;

  984.     }

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

  986.     {

  987.         context->clipped = 1U;

  988.     }

  989.     context->active = 1U;

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

  991.     && !defined(PLSR_HOST_TEST)

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

  993. #endif

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

  995.                                     : PLSR_PLANNER_OK;

  996. }

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

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

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

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

  1001. {

  1002.     PLSR_PLATFORM_TIMER_SETTING setting;

  1003.     uint32_t requestedFrequencyHz;

  1004.     uint32_t repeatCount;

  1005.     uint16_t produced = 0U;

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

  1007.     && !defined(PLSR_HOST_TEST)

  1008.     uint32_t startedAt = PlsrPlannerTimingNow();

  1009. #endif

  1010. 

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

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

  1013.     {

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

  1015.     && !defined(PLSR_HOST_TEST)

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

  1017. #endif

  1018.         return 0U;

  1019.     }

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

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

  1022. 

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

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

  1025.         {

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

  1027.             break;

  1028.         }

  1029.         if ((produced != 0U)

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

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

  1032.                 == requestedFrequencyHz)

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

  1034.                 <= 0xFFFFFFFFUL - repeatCount))

  1035.         {

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

  1037.             //不用开新格

  1038. 

  1039.         }

  1040.         else

  1041.         {

  1042.             output[produced].setting = setting;

  1043.             output[produced].requestedFrequencyHz = requestedFrequencyHz;

  1044.             output[produced].repeatCount = repeatCount;

  1045.             produced++;

  1046.         }

  1047.         context->generatedPulses += repeatCount;

  1048.     }

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

  1050.     {

  1051.         context->active = 0U;

  1052.     }

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

  1054.     && !defined(PLSR_HOST_TEST)

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

  1056. #endif

  1057.     return produced;

  1058. }

  1059. 

  1060. #if !defined(PLSR_HOST_TEST)

  1061. static uint32_t PlsrPlannerBenchmarkCyclesNow(void)

  1062. {

  1063.     return *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CYCCNT);

  1064. }

  1065. 

  1066. static void PlsrPlannerBenchmarkEnableCounter(void)

  1067. {

  1068.     *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DEMCR_ADDRESS) |=

  1069.         PLSR_PLANNER_BENCHMARK_TRCENA;

  1070.     *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CYCCNT) = 0UL;

  1071.     *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CTRL) |=

  1072.         PLSR_PLANNER_BENCHMARK_CYCCNTENA;

  1073. }

  1074. 

  1075. static uint32_t PlsrPlannerBenchmarkCyclesQ16(uint32_t cycles,

  1076.                                                uint32_t pulses)

  1077. {

  1078.     if (pulses == 0UL)

  1079.     {

  1080.         return 0UL;

  1081.     }

  1082.     return (uint32_t)((((uint64_t)cycles << 16U) + pulses / 2UL)

  1083.                       / pulses);

  1084. }

  1085. 

  1086. static void PlsrPlannerBenchmarkMode(uint16_t curveMode)

  1087. {

  1088.     PLSR_MOTION_BLOCK block;

  1089.     PLSR_PLANNER_CONTEXT context;

  1090.     PLSR_STREAM_ITEM item;

  1091.     PLSR_PLANNER_BENCHMARK_RESULT result;

  1092.     PLSR_PLANNER_STATUS status;

  1093.     uint64_t totalCycles = 0ULL;

  1094.     uint32_t minimumBlockQ16 = 0xFFFFFFFFUL;

  1095.     uint32_t maximumBlockQ16 = 0UL;

  1096.     uint32_t generateCalls = 0UL;

  1097.     uint8_t failed = 0U;

  1098. 

  1099.     (void)memset(&block, 0, sizeof(block));

  1100.     (void)memset(&context, 0, sizeof(context));

  1101.     (void)memset(&item, 0, sizeof(item));

  1102.     (void)memset(&result, 0, sizeof(result));

  1103. 

  1104.     block.entryHz = 1UL;

  1105.     block.cruiseHz = PLSR_PLANNER_BENCHMARK_TARGET_HZ;

  1106.     block.exitHz = 1UL;

  1107.     block.pulseBudget = PLSR_PLANNER_BENCHMARK_PULSES;

  1108.     block.referenceSpeedHz = PLSR_PLANNER_BENCHMARK_TARGET_HZ;

  1109.     block.accelerationTimeMs = 1000U;

  1110.     block.decelerationTimeMs = 1000U;

  1111.     block.curveMode = curveMode;

  1112.     block.pulseOutput = 0U;

  1113.     block.boundary = PLSR_BOUNDARY_STOP;

  1114. 

  1115.     status = PlsrPlannerBegin(&context, &block, 0UL, 0ULL);

  1116.     result.curveMode = curveMode;

  1117.     result.beginStatus = (uint32_t)status;

  1118.     if (status == PLSR_PLANNER_OK)

  1119.     {

  1120.         while (context.active != 0U)

  1121.         {

  1122.             uint32_t batchCalls = 0UL;

  1123.             uint32_t startedAt = PlsrPlannerBenchmarkCyclesNow();

  1124.             uint32_t elapsed;

  1125.             uint32_t blockQ16;

  1126. 

  1127.             while ((batchCalls < PLSR_PLANNER_BENCHMARK_BATCH_CALLS)

  1128.                    && (context.active != 0U))

  1129.             {

  1130.                 if (PlsrPlannerGenerate(&context, &item, 1U) == 0U)

  1131.                 {

  1132.                     failed = 1U;

  1133.                     break;

  1134.                 }

  1135.                 batchCalls++;

  1136.             }

  1137.             elapsed = PlsrPlannerBenchmarkCyclesNow() - startedAt;

  1138.             if (batchCalls != 0UL)

  1139.             {

  1140.                 blockQ16 = PlsrPlannerBenchmarkCyclesQ16(elapsed,

  1141.                                                          batchCalls);

  1142.                 totalCycles += elapsed;

  1143.                 generateCalls += batchCalls;

  1144.                 if (blockQ16 < minimumBlockQ16)

  1145.                 {

  1146.                     minimumBlockQ16 = blockQ16;

  1147.                 }

  1148.                 if (blockQ16 > maximumBlockQ16)

  1149.                 {

  1150.                     maximumBlockQ16 = blockQ16;

  1151.                 }

  1152.             }

  1153.             if (failed != 0U)

  1154.             {

  1155.                 break;

  1156.             }

  1157.         }

  1158.     }

  1159. 

  1160.     result.plannedPulses = context.generatedPulses;

  1161.     result.generateCalls = generateCalls;

  1162.     result.totalCycles = totalCycles;

  1163.     result.minimumBlockCyclesPerPulseQ16 =

  1164.         (minimumBlockQ16 == 0xFFFFFFFFUL) ? 0UL : minimumBlockQ16;

  1165.     result.maximumBlockCyclesPerPulseQ16 = maximumBlockQ16;

  1166.     if ((context.generatedPulses != 0UL) && (totalCycles != 0ULL))

  1167.     {

  1168.         uint64_t averageQ16 = ((totalCycles << 16U)

  1169.                                + context.generatedPulses / 2UL)

  1170.                               / context.generatedPulses;

  1171.         uint64_t estimatedHz =

  1172.             ((uint64_t)PlsrPlannerBenchmarkCoreClockHz

  1173.              * context.generatedPulses + totalCycles / 2ULL)

  1174.             / totalCycles;

  1175.         uint64_t targetCyclesQ16 =

  1176.             ((uint64_t)PlsrPlannerBenchmarkCoreClockHz << 16U)

  1177.             / PLSR_PLANNER_BENCHMARK_TARGET_HZ;

  1178. 

  1179.         result.averageCyclesPerPulseQ16 =

  1180.             (averageQ16 > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL

  1181.                                          : (uint32_t)averageQ16;

  1182.         result.estimatedPulsesPerSecond =

  1183.             (estimatedHz > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL

  1184.                                           : (uint32_t)estimatedHz;

  1185.         result.passes100k = (averageQ16 <= targetCyclesQ16) ? 1UL : 0UL;

  1186.     }

  1187.     result.completed = ((failed == 0U)

  1188.                         && (context.generatedPulses

  1189.                             == PLSR_PLANNER_BENCHMARK_PULSES)

  1190.                         && (context.active == 0U)) ? 1UL : 0UL;

  1191.     PlsrPlannerBenchmarkResults[curveMode] = result;

  1192. }

  1193. 

  1194. void PlsrPlannerBenchmarkService(void)

  1195. {

  1196.     uint16_t curveMode;

  1197. 

  1198.     if ((PlsrPlannerBenchmarkRequest == 0UL)

  1199.         || (PlsrPlannerBenchmarkRunning != 0UL))

  1200.     {

  1201.         return;

  1202.     }

  1203.     PlsrPlannerBenchmarkRequest = 0UL;

  1204.     PlsrPlannerBenchmarkRunning = 1UL;

  1205.     (void)memset((void *)PlsrPlannerBenchmarkResults, 0,

  1206.                  sizeof(PlsrPlannerBenchmarkResults));

  1207.     PlsrPlannerBenchmarkEnableCounter();

  1208.     for (curveMode = 0U; curveMode < 3U; curveMode++)

  1209.     {

  1210.         PlsrPlannerBenchmarkMode(curveMode);

  1211.     }

  1212.     if (PlsrPlannerBenchmarkRunCount != 0xFFFFFFFFUL)

  1213.     {

  1214.         PlsrPlannerBenchmarkRunCount++;

  1215.     }

  1216.     PlsrPlannerBenchmarkRunning = 0UL;

  1217. }

  1218. #endif

  1219. 

  1220. static uint64_t PlsrPlannerRampDurationUs(uint32_t pulseCount,

  1221.                                            uint32_t fromHz,

  1222.                                            uint32_t toHz)

  1223. {

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

  1225. 

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

  1227.     {

  1228.         return 0ULL;

  1229.     }

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

  1231.             + frequencySum - 1ULL) / frequencySum;

  1232. }

  1233. 

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

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

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

  1237. static uint32_t PlsrPlannerRatioQ32(uint64_t numerator,

  1238.                                     uint64_t denominator)

  1239. {

  1240.     uint64_t remainder;

  1241.     uint32_t ratio = 0UL;

  1242.     uint8_t bit;

  1243. 

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

  1245.     {

  1246.         return 0UL;

  1247.     }

  1248.     if (numerator >= denominator)

  1249.     {

  1250.         return 0xFFFFFFFFUL;

  1251.     }

  1252.     remainder = numerator;

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

  1254.     {

  1255.         ratio <<= 1U;

  1256.         remainder <<= 1U;

  1257.         if (remainder >= denominator)

  1258.         {

  1259.             remainder -= denominator;

  1260.             ratio |= 1UL;

  1261.         }

  1262.     }

  1263.     return ratio;

  1264. }

  1265. 

  1266. static uint32_t PlsrPlannerRampPulsesAtTime(

  1267.     const PLSR_PLANNER_CONTEXT *context,

  1268.     uint32_t pulseCount,

  1269.     uint32_t fromHz,

  1270.     uint32_t toHz,

  1271.     uint64_t elapsedUs,

  1272.     uint64_t durationUs,

  1273.     uint64_t *progressQ32)

  1274. {

  1275.     PLSR_PLANNER_CONTEXT ramp = *context;

  1276.     uint64_t partialAreaQ32;

  1277.     uint64_t totalAreaQ32;

  1278.     uint64_t product;

  1279.     uint32_t areaRatioQ32;

  1280.     uint32_t result;

  1281. 

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

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

  1284.     {

  1285.         *progressQ32 = 0ULL;

  1286.         return 0UL;

  1287.     }

  1288.     if (elapsedUs >= durationUs)

  1289.     {

  1290.         *progressQ32 = PLSR_PLANNER_Q32_ONE;

  1291.         return pulseCount;

  1292.     }

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

  1294.     ramp.rampFromHz = fromHz;

  1295.     ramp.rampToHz = toHz;

  1296.     partialAreaQ32 = PlsrPlannerRampAreaQ32(

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

  1298.     totalAreaQ32 = PlsrPlannerRampAreaQ32(

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

  1300.     areaRatioQ32 = PlsrPlannerRatioQ32(partialAreaQ32,

  1301.                                        totalAreaQ32);

  1302.     product = (uint64_t)pulseCount * areaRatioQ32;

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

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

  1305.     {

  1306.         result++;

  1307.     }

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

  1309. }

  1310. 

  1311. static uint8_t PlsrPlannerSetPredictedFrequency(

  1312.     const PLSR_PLANNER_CONTEXT *context,

  1313.     uint32_t fromHz,

  1314.     uint32_t toHz,

  1315.     uint64_t progressQ32,

  1316.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  1317. {

  1318.     PLSR_PLANNER_CONTEXT ramp = *context;

  1319.     PLSR_PLATFORM_TIMER_SETTING setting;

  1320.     uint32_t requestedHz;

  1321. 

  1322.     ramp.rampFromHz = fromHz;

  1323.     ramp.rampToHz = toHz;

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

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

  1326.                                       PLSR_OUTPUT_PULSE_DIR,

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

  1328.     {

  1329.         return 0U;

  1330.     }

  1331.     prediction->actualFrequencyHz = setting.actualFrequencyHz;

  1332.     return 1U;

  1333. }

  1334. 

  1335. static uint64_t PlsrPlannerRampTargetAreaQ32(uint64_t totalAreaQ32,

  1336.                                              uint32_t pulseCount,

  1337.                                              uint32_t pulseIndex)

  1338. {

  1339.     uint64_t step = totalAreaQ32 / pulseCount;

  1340.     uint64_t remainder = totalAreaQ32 % pulseCount;

  1341. 

  1342.     return step * pulseIndex

  1343.            + (remainder * pulseIndex) / pulseCount;

  1344. }

  1345. 

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

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

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

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

  1350. static uint8_t PlsrPlannerSetPredictedRampRunFrequency(

  1351.     const PLSR_PLANNER_CONTEXT *context,

  1352.     uint32_t pulseCount,

  1353.     uint32_t fromHz,

  1354.     uint32_t toHz,

  1355.     uint32_t pulseIndex,

  1356.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  1357. {

  1358.     PLSR_PLANNER_CONTEXT ramp = *context;

  1359.     PLSR_PLATFORM_TIMER_SETTING setting;

  1360.     uint64_t totalAreaQ32;

  1361.     uint64_t previousTargetAreaQ32;

  1362.     uint64_t targetAreaQ32;

  1363.     uint64_t previousBoundaryQ32;

  1364.     uint64_t boundaryQ32;

  1365.     uint64_t denominator;

  1366.     uint64_t requestedHz;

  1367. 

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

  1369.     {

  1370.         return PlsrPlannerSetPredictedFrequency(

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

  1372.     }

  1373.     if (pulseIndex > pulseCount)

  1374.     {

  1375.         pulseIndex = pulseCount;

  1376.     }

  1377.     ramp.rampFromHz = fromHz;

  1378.     ramp.rampToHz = toHz;

  1379.     totalAreaQ32 = PlsrPlannerRampAreaQ32(

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

  1381.     previousTargetAreaQ32 = PlsrPlannerRampTargetAreaQ32(

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

  1383.     targetAreaQ32 = PlsrPlannerRampTargetAreaQ32(

  1384.         totalAreaQ32, pulseCount, pulseIndex);

  1385.     previousBoundaryQ32 = (pulseIndex == 1UL)

  1386.                               ? 0ULL

  1387.                               : PlsrPlannerExactBoundaryQ32(

  1388.                                     &ramp, 0ULL,

  1389.                                     previousTargetAreaQ32);

  1390.     boundaryQ32 = (pulseIndex == pulseCount)

  1391.                       ? PLSR_PLANNER_Q32_ONE

  1392.                       : PlsrPlannerExactBoundaryQ32(

  1393.                             &ramp, previousBoundaryQ32,

  1394.                             targetAreaQ32);

  1395.     if (boundaryQ32 <= previousBoundaryQ32)

  1396.     {

  1397.         return 0U;

  1398.     }

  1399.     denominator = (uint64_t)pulseCount

  1400.                   * (boundaryQ32 - previousBoundaryQ32);

  1401.     requestedHz = (denominator == 0ULL)

  1402.                       ? toHz

  1403.                       : (totalAreaQ32 + denominator / 2ULL)

  1404.                             / denominator;

  1405.     if (requestedHz == 0ULL)

  1406.     {

  1407.         requestedHz = 1ULL;

  1408.     }

  1409.     if (requestedHz > PLSR_FREQUENCY_MAX_HZ)

  1410.     {

  1411.         requestedHz = PLSR_FREQUENCY_MAX_HZ;

  1412.     }

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

  1414.                                       PLSR_OUTPUT_PULSE_DIR,

  1415.                                       (uint32_t)requestedHz,

  1416.                                       &setting) == 0U)

  1417.     {

  1418.         return 0U;

  1419.     }

  1420.     prediction->actualFrequencyHz = setting.actualFrequencyHz;

  1421.     return 1U;

  1422. }

  1423. 

  1424. uint8_t PlsrPlannerPredictTime(

  1425.     const PLSR_PLANNER_CONTEXT *context,

  1426.     uint32_t elapsedUs,

  1427.     PLSR_PLANNER_TIME_PREDICTION *prediction)

  1428. {

  1429.     PLSR_PLATFORM_TIMER_SETTING steadySetting;

  1430.     uint64_t remainingUs = elapsedUs;

  1431.     uint64_t durationUs;

  1432.     uint64_t progressQ32;

  1433.     uint64_t partialPulses;

  1434. 

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

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

  1437.     {

  1438.         return 0U;

  1439.     }

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

  1441. 

  1442.     durationUs = PlsrPlannerRampDurationUs(

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

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

  1445.     {

  1446.         prediction->pulseCount = PlsrPlannerRampPulsesAtTime(

  1447.             context, context->entryPulses, context->startHz,

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

  1449.         prediction->phase = PLSR_PLANNER_PHASE_ENTRY;

  1450.         prediction->deadlineInProfile = 1U;

  1451.         return PlsrPlannerSetPredictedRampRunFrequency(

  1452.             context, context->entryPulses, context->startHz,

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

  1454.     }

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

  1456.     {

  1457.         remainingUs -= durationUs;

  1458.     }

  1459. 

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

  1461.                                       PLSR_OUTPUT_PULSE_DIR,

  1462.                                       context->peakHz,

  1463.                                       &steadySetting) == 0U)

  1464.     {

  1465.         return 0U;

  1466.     }

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

  1468.                      ? 0ULL

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

  1470.                         + steadySetting.actualFrequencyHz - 1UL)

  1471.                            / steadySetting.actualFrequencyHz;

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

  1473.     {

  1474.         partialPulses = (remainingUs * steadySetting.actualFrequencyHz

  1475.                          + 999999ULL) / 1000000ULL;

  1476.         if (partialPulses > context->steadyPulses)

  1477.         {

  1478.             partialPulses = context->steadyPulses;

  1479.         }

  1480.         prediction->pulseCount = context->entryPulses

  1481.                                  + (uint32_t)partialPulses;

  1482.         prediction->actualFrequencyHz = steadySetting.actualFrequencyHz;

  1483.         prediction->phase = PLSR_PLANNER_PHASE_STEADY;

  1484.         prediction->deadlineInProfile = 1U;

  1485.         return 1U;

  1486.     }

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

  1488.     {

  1489.         remainingUs -= durationUs;

  1490.     }

  1491. 

  1492.     durationUs = PlsrPlannerRampDurationUs(

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

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

  1495.     {

  1496.         partialPulses = PlsrPlannerRampPulsesAtTime(

  1497.             context, context->exitPulses, context->peakHz,

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

  1499.         prediction->pulseCount = context->entryPulses

  1500.                                  + context->steadyPulses

  1501.                                  + (uint32_t)partialPulses;

  1502.         prediction->phase = PLSR_PLANNER_PHASE_EXIT;

  1503.         prediction->deadlineInProfile = 1U;

  1504.         return PlsrPlannerSetPredictedRampRunFrequency(

  1505.             context, context->exitPulses, context->peakHz,

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

  1507.     }

  1508. 

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

  1510.     prediction->phase = PLSR_PLANNER_PHASE_COMPLETE;

  1511.     prediction->deadlineInProfile = 0U;

  1512.     return PlsrPlannerSetPredictedFrequency(

  1513.         context, context->endHz, context->endHz,

  1514.         PLSR_PLANNER_Q32_ONE, prediction);

  1515. }