#include "plsr_planner.h" #include #include #define PLSR_PLANNER_Q32_ONE (4294967296ULL) #if !defined(PLSR_HOST_TEST) #define PLSR_PLANNER_BENCHMARK_DEMCR_ADDRESS (0xE000EDFCUL) #define PLSR_PLANNER_BENCHMARK_DWT_CTRL (0xE0001000UL) #define PLSR_PLANNER_BENCHMARK_DWT_CYCCNT (0xE0001004UL) #define PLSR_PLANNER_BENCHMARK_TRCENA (1UL << 24U) #define PLSR_PLANNER_BENCHMARK_CYCCNTENA (1UL << 0U) #define PLSR_PLANNER_BENCHMARK_PULSES (100000UL) #define PLSR_PLANNER_BENCHMARK_BATCH_CALLS (128UL) #define PLSR_PLANNER_BENCHMARK_TARGET_HZ (100000UL) volatile uint32_t PlsrPlannerBenchmarkRequest; volatile uint32_t PlsrPlannerBenchmarkRunning; volatile uint32_t PlsrPlannerBenchmarkRunCount; volatile uint32_t PlsrPlannerBenchmarkCoreClockHz = 168000000UL; volatile PLSR_PLANNER_BENCHMARK_RESULT PlsrPlannerBenchmarkResults[3]; #endif #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) #define PLSR_PLANNER_CYCCNT_ADDRESS (0xE0001004UL) volatile PLSR_PLANNER_TIMING PlsrPlannerTiming; static uint32_t PlsrPlannerTimingNow(void) { return *((volatile uint32_t *)PLSR_PLANNER_CYCCNT_ADDRESS); } static void PlsrPlannerTimingRecord( volatile PLSR_PLANNER_TIMING_SAMPLE *sample, uint32_t startedAt) { uint32_t elapsed = PlsrPlannerTimingNow() - startedAt; uint32_t total = sample->totalCycles; sample->lastCycles = elapsed; if ((sample->callCount == 0UL) || (elapsed < sample->minCycles)) { sample->minCycles = elapsed; } if (sample->callCount != 0xFFFFFFFFUL) { sample->callCount++; } sample->totalCycles = (elapsed > (0xFFFFFFFFUL - total)) ? 0xFFFFFFFFUL : total + elapsed; if (elapsed > sample->maxCycles) { sample->maxCycles = elapsed; } } void PlsrPlannerTimingReset(void) { (void)memset((void *)&PlsrPlannerTiming, 0, sizeof(PlsrPlannerTiming)); } #endif #define PLSR_PLANNER_CURVE_TABLE_BITS (9U) #define PLSR_PLANNER_CURVE_TABLE_INTERVALS (1UL << PLSR_PLANNER_CURVE_TABLE_BITS) #define PLSR_PLANNER_CURVE_TABLE_SIZE (PLSR_PLANNER_CURVE_TABLE_INTERVALS + 1UL) #define PLSR_PLANNER_CURVE_DERIVATIVE_SHIFT (8U + PLSR_PLANNER_CURVE_TABLE_BITS) /* curveMode 1: fixed seven-section jerk profile. Each entry/exit ramp uses a 1:2:1 constant-jerk/constant-acceleration/constant-jerk ratio. The table stores the integral of the normalized frequency blend. */ static const uint32_t PlsrPlannerSmoothIntegralQ24[PLSR_PLANNER_CURVE_TABLE_SIZE] = { 0UL, 0UL, 1UL, 3UL, 7UL, 14UL, 24UL, 38UL, 57UL, 81UL, 111UL, 148UL, 192UL, 244UL, 305UL, 375UL, 455UL, 546UL, 648UL, 762UL, 889UL, 1029UL, 1183UL, 1352UL, 1536UL, 1736UL, 1953UL, 2187UL, 2439UL, 2710UL, 3000UL, 3310UL, 3641UL, 3993UL, 4367UL, 4764UL, 5184UL, 5628UL, 6097UL, 6591UL, 7111UL, 7658UL, 8232UL, 8834UL, 9465UL, 10125UL, 10815UL, 11536UL, 12288UL, 13072UL, 13889UL, 14739UL, 15623UL, 16542UL, 17496UL, 18486UL, 19513UL, 20577UL, 21679UL, 22820UL, 24000UL, 25220UL, 26481UL, 27783UL, 29127UL, 30514UL, 31944UL, 33418UL, 34937UL, 36501UL, 38111UL, 39768UL, 41472UL, 43224UL, 45025UL, 46875UL, 48775UL, 50726UL, 52728UL, 54782UL, 56889UL, 59049UL, 61263UL, 63532UL, 65856UL, 68236UL, 70673UL, 73167UL, 75719UL, 78330UL, 81000UL, 83730UL, 86521UL, 89373UL, 92287UL, 95264UL, 98304UL, 101408UL, 104577UL, 107811UL, 111111UL, 114478UL, 117912UL, 121414UL, 124985UL, 128625UL, 132335UL, 136116UL, 139968UL, 143892UL, 147889UL, 151959UL, 156103UL, 160322UL, 164616UL, 168986UL, 173433UL, 177957UL, 182559UL, 187240UL, 192000UL, 196840UL, 201761UL, 206763UL, 211847UL, 217014UL, 222264UL, 227598UL, 233017UL, 238521UL, 244110UL, 249785UL, 255545UL, 261390UL, 267321UL, 273337UL, 279438UL, 285625UL, 291897UL, 298254UL, 304697UL, 311225UL, 317838UL, 324537UL, 331321UL, 338190UL, 345145UL, 352185UL, 359310UL, 366521UL, 373817UL, 381198UL, 388665UL, 396217UL, 403854UL, 411577UL, 419385UL, 427278UL, 435257UL, 443321UL, 451470UL, 459705UL, 468025UL, 476430UL, 484921UL, 493497UL, 502158UL, 510905UL, 519737UL, 528654UL, 537657UL, 546745UL, 555918UL, 565177UL, 574521UL, 583950UL, 593465UL, 603065UL, 612750UL, 622521UL, 632377UL, 642318UL, 652345UL, 662457UL, 672654UL, 682937UL, 693305UL, 703758UL, 714297UL, 724921UL, 735630UL, 746425UL, 757305UL, 768270UL, 779321UL, 790457UL, 801678UL, 812985UL, 824377UL, 835854UL, 847417UL, 859065UL, 870798UL, 882617UL, 894521UL, 906510UL, 918585UL, 930745UL, 942990UL, 955321UL, 967737UL, 980238UL, 992825UL, 1005497UL, 1018254UL, 1031097UL, 1044025UL, 1057038UL, 1070137UL, 1083321UL, 1096590UL, 1109945UL, 1123385UL, 1136910UL, 1150521UL, 1164217UL, 1177998UL, 1191865UL, 1205817UL, 1219854UL, 1233977UL, 1248185UL, 1262478UL, 1276857UL, 1291321UL, 1305870UL, 1320505UL, 1335225UL, 1350030UL, 1364921UL, 1379897UL, 1394958UL, 1410105UL, 1425337UL, 1440654UL, 1456057UL, 1471545UL, 1487118UL, 1502777UL, 1518521UL, 1534350UL, 1550265UL, 1566265UL, 1582350UL, 1598521UL, 1614777UL, 1631118UL, 1647545UL, 1664057UL, 1680654UL, 1697337UL, 1714105UL, 1730958UL, 1747897UL, 1764921UL, 1782030UL, 1799225UL, 1816505UL, 1833870UL, 1851321UL, 1868857UL, 1886478UL, 1904185UL, 1921977UL, 1939854UL, 1957817UL, 1975865UL, 1993998UL, 2012217UL, 2030521UL, 2048910UL, 2067385UL, 2085945UL, 2104590UL, 2123321UL, 2142137UL, 2161038UL, 2180025UL, 2199097UL, 2218254UL, 2237497UL, 2256825UL, 2276238UL, 2295737UL, 2315321UL, 2334990UL, 2354745UL, 2374585UL, 2394510UL, 2414521UL, 2434617UL, 2454798UL, 2475065UL, 2495417UL, 2515854UL, 2536377UL, 2556985UL, 2577678UL, 2598457UL, 2619321UL, 2640270UL, 2661305UL, 2682425UL, 2703630UL, 2724921UL, 2746297UL, 2767758UL, 2789305UL, 2810937UL, 2832654UL, 2854457UL, 2876345UL, 2898318UL, 2920377UL, 2942521UL, 2964750UL, 2987065UL, 3009465UL, 3031950UL, 3054521UL, 3077177UL, 3099918UL, 3122745UL, 3145657UL, 3168654UL, 3191737UL, 3214905UL, 3238158UL, 3261497UL, 3284921UL, 3308430UL, 3332025UL, 3355705UL, 3379470UL, 3403321UL, 3427257UL, 3451278UL, 3475385UL, 3499577UL, 3523854UL, 3548217UL, 3572665UL, 3597198UL, 3621817UL, 3646521UL, 3671310UL, 3696185UL, 3721145UL, 3746190UL, 3771321UL, 3796537UL, 3821838UL, 3847225UL, 3872697UL, 3898254UL, 3923897UL, 3949625UL, 3975438UL, 4001337UL, 4027321UL, 4053390UL, 4079545UL, 4105785UL, 4132110UL, 4158521UL, 4185017UL, 4211598UL, 4238265UL, 4265017UL, 4291854UL, 4318777UL, 4345785UL, 4372878UL, 4400057UL, 4427321UL, 4454670UL, 4482104UL, 4509622UL, 4537223UL, 4564907UL, 4592673UL, 4620520UL, 4648448UL, 4676456UL, 4704543UL, 4732709UL, 4760953UL, 4789274UL, 4817672UL, 4846146UL, 4874695UL, 4903319UL, 4932017UL, 4960788UL, 4989632UL, 5018548UL, 5047535UL, 5076593UL, 5105721UL, 5134918UL, 5164184UL, 5193518UL, 5222919UL, 5252387UL, 5281921UL, 5311520UL, 5341184UL, 5370912UL, 5400703UL, 5430557UL, 5460473UL, 5490450UL, 5520488UL, 5550586UL, 5580743UL, 5610959UL, 5641233UL, 5671564UL, 5701952UL, 5732396UL, 5762895UL, 5793449UL, 5824057UL, 5854718UL, 5885432UL, 5916198UL, 5947015UL, 5977883UL, 6008801UL, 6039768UL, 6070784UL, 6101848UL, 6132959UL, 6164117UL, 6195321UL, 6226570UL, 6257864UL, 6289202UL, 6320583UL, 6352007UL, 6383473UL, 6414980UL, 6446528UL, 6478116UL, 6509743UL, 6541409UL, 6573113UL, 6604854UL, 6636632UL, 6668446UL, 6700295UL, 6732179UL, 6764097UL, 6796048UL, 6828032UL, 6860048UL, 6892095UL, 6924173UL, 6956281UL, 6988418UL, 7020584UL, 7052778UL, 7084999UL, 7117247UL, 7149521UL, 7181820UL, 7214144UL, 7246492UL, 7278863UL, 7311257UL, 7343673UL, 7376110UL, 7408568UL, 7441046UL, 7473543UL, 7506059UL, 7538593UL, 7571144UL, 7603712UL, 7636296UL, 7668895UL, 7701509UL, 7734137UL, 7766778UL, 7799432UL, 7832098UL, 7864775UL, 7897463UL, 7930161UL, 7962868UL, 7995584UL, 8028308UL, 8061039UL, 8093777UL, 8126521UL, 8159270UL, 8192024UL, 8224782UL, 8257543UL, 8290307UL, 8323073UL, 8355840UL, 8388608UL }; static const uint32_t PlsrPlannerSineIntegralQ24[PLSR_PLANNER_CURVE_TABLE_SIZE] = { 0UL, 0UL, 1UL, 3UL, 7UL, 13UL, 22UL, 35UL, 53UL, 75UL, 103UL, 137UL, 178UL, 226UL, 282UL, 347UL, 421UL, 505UL, 599UL, 705UL, 822UL, 951UL, 1094UL, 1250UL, 1420UL, 1604UL, 1805UL, 2021UL, 2254UL, 2503UL, 2771UL, 3057UL, 3362UL, 3687UL, 4032UL, 4398UL, 4785UL, 5194UL, 5626UL, 6081UL, 6560UL, 7063UL, 7592UL, 8146UL, 8726UL, 9333UL, 9967UL, 10630UL, 11321UL, 12041UL, 12791UL, 13571UL, 14382UL, 15225UL, 16100UL, 17008UL, 17949UL, 18923UL, 19933UL, 20977UL, 22057UL, 23173UL, 24325UL, 25516UL, 26744UL, 28010UL, 29316UL, 30661UL, 32046UL, 33472UL, 34939UL, 36449UL, 38000UL, 39595UL, 41233UL, 42915UL, 44642UL, 46414UL, 48232UL, 50096UL, 52007UL, 53966UL, 55972UL, 58027UL, 60131UL, 62284UL, 64487UL, 66742UL, 69047UL, 71404UL, 73813UL, 76275UL, 78790UL, 81359UL, 83982UL, 86660UL, 89393UL, 92182UL, 95028UL, 97930UL, 100890UL, 103908UL, 106984UL, 110119UL, 113314UL, 116568UL, 119882UL, 123258UL, 126695UL, 130194UL, 133755UL, 137379UL, 141066UL, 144817UL, 148632UL, 152512UL, 156457UL, 160468UL, 164545UL, 168688UL, 172899UL, 177177UL, 181523UL, 185937UL, 190421UL, 194973UL, 199596UL, 204289UL, 209052UL, 213886UL, 218792UL, 223770UL, 228820UL, 233943UL, 239140UL, 244409UL, 249753UL, 255172UL, 260665UL, 266234UL, 271878UL, 277599UL, 283396UL, 289270UL, 295221UL, 301250UL, 307358UL, 313543UL, 319808UL, 326151UL, 332575UL, 339078UL, 345662UL, 352326UL, 359072UL, 365899UL, 372808UL, 379799UL, 386873UL, 394029UL, 401269UL, 408592UL, 416000UL, 423491UL, 431068UL, 438729UL, 446475UL, 454307UL, 462225UL, 470229UL, 478320UL, 486497UL, 494762UL, 503114UL, 511554UL, 520082UL, 528698UL, 537403UL, 546197UL, 555080UL, 564053UL, 573116UL, 582268UL, 591511UL, 600845UL, 610269UL, 619785UL, 629392UL, 639090UL, 648881UL, 658763UL, 668739UL, 678806UL, 688967UL, 699221UL, 709568UL, 720008UL, 730543UL, 741171UL, 751894UL, 762711UL, 773623UL, 784629UL, 795731UL, 806928UL, 818220UL, 829608UL, 841092UL, 852672UL, 864348UL, 876121UL, 887990UL, 899955UL, 912018UL, 924178UL, 936435UL, 948789UL, 961241UL, 973790UL, 986438UL, 999183UL, 1012026UL, 1024968UL, 1038007UL, 1051146UL, 1064383UL, 1077718UL, 1091153UL, 1104686UL, 1118319UL, 1132051UL, 1145882UL, 1159812UL, 1173841UL, 1187971UL, 1202200UL, 1216528UL, 1230956UL, 1245485UL, 1260113UL, 1274841UL, 1289669UL, 1304597UL, 1319626UL, 1334754UL, 1349983UL, 1365312UL, 1380742UL, 1396271UL, 1411902UL, 1427632UL, 1443464UL, 1459395UL, 1475428UL, 1491560UL, 1507793UL, 1524127UL, 1540561UL, 1557096UL, 1573732UL, 1590467UL, 1607304UL, 1624240UL, 1641278UL, 1658415UL, 1675654UL, 1692992UL, 1710431UL, 1727970UL, 1745610UL, 1763349UL, 1781189UL, 1799129UL, 1817169UL, 1835309UL, 1853548UL, 1871888UL, 1890328UL, 1908867UL, 1927505UL, 1946244UL, 1965082UL, 1984019UL, 2003055UL, 2022190UL, 2041425UL, 2060758UL, 2080191UL, 2099722UL, 2119351UL, 2139080UL, 2158906UL, 2178831UL, 2198854UL, 2218974UL, 2239193UL, 2259509UL, 2279923UL, 2300434UL, 2321042UL, 2341747UL, 2362550UL, 2383449UL, 2404444UL, 2425536UL, 2446724UL, 2468008UL, 2489388UL, 2510864UL, 2532435UL, 2554101UL, 2575863UL, 2597719UL, 2619670UL, 2641715UL, 2663855UL, 2686088UL, 2708416UL, 2730837UL, 2753351UL, 2775958UL, 2798659UL, 2821451UL, 2844337UL, 2867314UL, 2890384UL, 2913545UL, 2936797UL, 2960141UL, 2983575UL, 3007100UL, 3030716UL, 3054421UL, 3078216UL, 3102101UL, 3126075UL, 3150138UL, 3174290UL, 3198530UL, 3222858UL, 3247274UL, 3271777UL, 3296368UL, 3321045UL, 3345809UL, 3370659UL, 3395595UL, 3420617UL, 3445724UL, 3470915UL, 3496192UL, 3521552UL, 3546997UL, 3572525UL, 3598137UL, 3623831UL, 3649608UL, 3675467UL, 3701408UL, 3727430UL, 3753534UL, 3779718UL, 3805983UL, 3832327UL, 3858752UL, 3885255UL, 3911838UL, 3938498UL, 3965237UL, 3992054UL, 4018948UL, 4045919UL, 4072966UL, 4100090UL, 4127289UL, 4154564UL, 4181913UL, 4209337UL, 4236836UL, 4264407UL, 4292052UL, 4319770UL, 4347560UL, 4375422UL, 4403356UL, 4431361UL, 4459436UL, 4487581UL, 4515797UL, 4544081UL, 4572435UL, 4600857UL, 4629347UL, 4657904UL, 4686529UL, 4715220UL, 4743977UL, 4772800UL, 4801688UL, 4830641UL, 4859658UL, 4888739UL, 4917883UL, 4947090UL, 4976359UL, 5005690UL, 5035082UL, 5064536UL, 5094050UL, 5123623UL, 5153256UL, 5182948UL, 5212698UL, 5242506UL, 5272372UL, 5302294UL, 5332273UL, 5362308UL, 5392398UL, 5422543UL, 5452742UL, 5482995UL, 5513301UL, 5543660UL, 5574071UL, 5604534UL, 5635047UL, 5665612UL, 5696227UL, 5726891UL, 5757604UL, 5788366UL, 5819175UL, 5850032UL, 5880936UL, 5911886UL, 5942882UL, 5973923UL, 6005009UL, 6036139UL, 6067312UL, 6098529UL, 6129787UL, 6161088UL, 6192430UL, 6223813UL, 6255236UL, 6286698UL, 6318200UL, 6349740UL, 6381317UL, 6412933UL, 6444585UL, 6476273UL, 6507997UL, 6539755UL, 6571549UL, 6603376UL, 6635236UL, 6667129UL, 6699054UL, 6731011UL, 6762999UL, 6795017UL, 6827065UL, 6859142UL, 6891247UL, 6923381UL, 6955542UL, 6987730UL, 7019944UL, 7052183UL, 7084448UL, 7116737UL, 7149050UL, 7181386UL, 7213745UL, 7246126UL, 7278528UL, 7310951UL, 7343394UL, 7375857UL, 7408339UL, 7440839UL, 7473358UL, 7505893UL, 7538445UL, 7571012UL, 7603596UL, 7636194UL, 7668806UL, 7701431UL, 7734070UL, 7766721UL, 7799383UL, 7832057UL, 7864741UL, 7897435UL, 7930138UL, 7962850UL, 7995570UL, 8028297UL, 8061031UL, 8093771UL, 8126517UL, 8159267UL, 8192022UL, 8224781UL, 8257543UL, 8290307UL, 8323073UL, 8355840UL, 8388608UL }; static uint32_t PlsrPlannerAbsDifference(uint32_t first, uint32_t second) { return (first > second) ? (first - second) : (second - first); } static uint32_t PlsrPlannerRampTime(const PLSR_MOTION_BLOCK *block,uint32_t fromHz, uint32_t toHz) { uint32_t baseTimeMs; uint64_t durationMs; if (fromHz == toHz) { return 0UL; } baseTimeMs = (toHz > fromHz) ? block->accelerationTimeMs : block->decelerationTimeMs; if (baseTimeMs == 0UL) { return 0UL; } durationMs = ((uint64_t)PlsrPlannerAbsDifference(fromHz, toHz) * baseTimeMs + block->referenceSpeedHz - 1UL) / block->referenceSpeedHz; return (durationMs > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL : (uint32_t)durationMs; } static uint16_t PlsrPlannerBaseRampTime(const PLSR_MOTION_BLOCK *block, uint32_t fromHz, uint32_t toHz) { if (toHz > fromHz) { return block->accelerationTimeMs; } if (toHz < fromHz) { return block->decelerationTimeMs; } return 0U; } static uint64_t PlsrPlannerRampWeight(uint32_t fromHz, uint32_t toHz, uint16_t baseTimeMs) { uint64_t fromSquared = (uint64_t)fromHz * fromHz; uint64_t toSquared = (uint64_t)toHz * toHz; uint64_t difference = (fromSquared > toSquared) ? (fromSquared - toSquared) : (toSquared - fromSquared); return difference * baseTimeMs; } static uint64_t PlsrPlannerRequiredPulses(const PLSR_MOTION_BLOCK *block, uint64_t rampWeight) { uint64_t denominator = (uint64_t)2U * block->referenceSpeedHz * 1000UL; return (rampWeight == 0ULL) ? 0ULL : (rampWeight + denominator - 1ULL) / denominator; } static uint32_t PlsrPlannerIntegerSquareRoot(uint64_t value) { uint64_t bit = (uint64_t)1U << 62U; uint64_t root = 0ULL; while (bit > value) { bit >>= 2U; } while (bit != 0ULL) { if (value >= root + bit) { value -= root + bit; root = (root >> 1U) + bit; } else { root >>= 1U; } bit >>= 2U; } return (uint32_t)root; } static uint32_t PlsrPlannerReachableFrequency( const PLSR_MOTION_BLOCK *block, uint32_t fromHz, uint32_t towardHz, uint32_t pulseCount) { uint16_t baseTimeMs = PlsrPlannerBaseRampTime(block, fromHz, towardHz); uint64_t frequencySquared = (uint64_t)fromHz * fromHz; uint64_t changeSquared; uint32_t reachableHz; if ((baseTimeMs == 0U) || (fromHz == towardHz)) { return towardHz; } changeSquared = (uint64_t)2U * pulseCount * block->referenceSpeedHz * 1000UL / baseTimeMs; if (towardHz > fromHz) { reachableHz = PlsrPlannerIntegerSquareRoot( frequencySquared + changeSquared); return (reachableHz > towardHz) ? towardHz : reachableHz; } frequencySquared = (changeSquared >= frequencySquared) ? 0ULL : (frequencySquared - changeSquared); reachableHz = PlsrPlannerIntegerSquareRoot(frequencySquared); if ((uint64_t)reachableHz * reachableHz < frequencySquared) { reachableHz++; } return (reachableHz < towardHz) ? towardHz : reachableHz; } static uint32_t PlsrPlannerPeak(const PLSR_MOTION_BLOCK *block, uint32_t startHz, uint32_t endHz) { uint32_t targetHz = block->cruiseHz; uint32_t upperEndpoint = (startHz > endHz) ? startHz : endHz; uint32_t lowerEndpoint = (startHz < endHz) ? startHz : endHz; uint16_t entryTimeMs; uint16_t exitTimeMs; uint32_t timeSumMs; uint64_t weightedEndpoints; uint64_t availableArea; uint64_t peakSquared; uint32_t peakHz; if ((targetHz <= upperEndpoint) && (targetHz >= lowerEndpoint)) { return targetHz; } entryTimeMs = PlsrPlannerBaseRampTime(block, startHz, targetHz); exitTimeMs = PlsrPlannerBaseRampTime(block, targetHz, endHz); timeSumMs = (uint32_t)entryTimeMs + exitTimeMs; if (timeSumMs == 0UL) { return targetHz; } weightedEndpoints = (uint64_t)startHz * startHz * entryTimeMs + (uint64_t)endHz * endHz * exitTimeMs; availableArea = (uint64_t)2U * block->pulseBudget * block->referenceSpeedHz * 1000UL; if (targetHz > upperEndpoint) { peakSquared = (availableArea + weightedEndpoints) / timeSumMs; peakHz = PlsrPlannerIntegerSquareRoot(peakSquared); if (peakHz < upperEndpoint) { peakHz = upperEndpoint; } return (peakHz > targetHz) ? targetHz : peakHz; } if (availableArea >= weightedEndpoints) { return targetHz; } peakSquared = (weightedEndpoints - availableArea) / timeSumMs; peakHz = PlsrPlannerIntegerSquareRoot(peakSquared); if (peakHz < targetHz) { peakHz = targetHz; } return (peakHz > lowerEndpoint) ? lowerEndpoint : peakHz; } //C(x)函数 //瞬时速度=delta*C(x) static uint64_t PlsrPlannerCurveIntegralQ32(uint64_t progressQ32, uint16_t curveMode) { const uint32_t *table; uint64_t scaled; uint32_t index; uint32_t fraction; uint64_t first; uint64_t second; if (progressQ32 >= PLSR_PLANNER_Q32_ONE) { return PLSR_PLANNER_Q32_ONE / 2ULL; } if (curveMode == 0U) { return (progressQ32 * progressQ32) >> 33U; } table = (curveMode == 1U) ? PlsrPlannerSmoothIntegralQ24 : PlsrPlannerSineIntegralQ24; scaled = progressQ32 * PLSR_PLANNER_CURVE_TABLE_INTERVALS; index = (uint32_t)(scaled >> 32U); fraction = (uint32_t)scaled; first = (uint64_t)table[index] << 8U; second = (uint64_t)table[index + 1UL] << 8U; return first + (((second - first) * fraction) >> 32U); } static uint64_t PlsrPlannerRampAreaQ32(const PLSR_PLANNER_CONTEXT *context, uint32_t fromHz, uint32_t toHz, uint64_t progressQ32) { int64_t delta = (int64_t)toHz - (int64_t)fromHz; int64_t area = (int64_t)((uint64_t)fromHz * progressQ32) + delta * (int64_t)PlsrPlannerCurveIntegralQ32( progressQ32, context->block.curveMode); return (uint64_t)area; } static uint64_t PlsrPlannerExactBoundaryQ32( const PLSR_PLANNER_CONTEXT *context, uint64_t previousBoundaryQ32, uint64_t targetAreaQ32) { uint64_t lowerQ32 = previousBoundaryQ32; uint64_t upperQ32 = PLSR_PLANNER_Q32_ONE; uint64_t middleQ32; uint32_t iteration; for (iteration = 0UL; iteration < 32UL; iteration++) { middleQ32 = lowerQ32 + ((upperQ32 - lowerQ32) >> 1U); if (PlsrPlannerRampAreaQ32(context, context->rampFromHz, context->rampToHz, middleQ32) < targetAreaQ32) { lowerQ32 = middleQ32; } else { upperQ32 = middleQ32; } } return upperQ32; } static uint32_t PlsrPlannerInstantFrequency( const PLSR_PLANNER_CONTEXT *context, uint64_t progressQ32) { const uint32_t *table; uint64_t scaled; uint64_t curveProgressQ32; uint32_t index; uint32_t gap; if (progressQ32 >= PLSR_PLANNER_Q32_ONE) { return context->rampToHz; } if (context->block.curveMode == 0U) { curveProgressQ32 = progressQ32; } else { table = (context->block.curveMode == 1U) ? PlsrPlannerSmoothIntegralQ24 : PlsrPlannerSineIntegralQ24; scaled = progressQ32 * PLSR_PLANNER_CURVE_TABLE_INTERVALS; index = (uint32_t)(scaled >> 32U); curveProgressQ32 = (uint64_t)(table[index + 1UL] - table[index]) << PLSR_PLANNER_CURVE_DERIVATIVE_SHIFT; } if (context->rampToHz >= context->rampFromHz) { gap = context->rampToHz - context->rampFromHz; return context->rampFromHz + (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U); } gap = context->rampFromHz - context->rampToHz; return context->rampFromHz - (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U); } static uint64_t PlsrPlannerPredictedBoundaryQ32( const PLSR_PLANNER_CONTEXT *context, uint64_t targetAreaQ32) { uint64_t previousQ32 = context->rampBoundaryQ32; uint64_t currentAreaQ32 = (previousQ32 == 0ULL) ? 0ULL : PlsrPlannerRampAreaQ32( context, context->rampFromHz, context->rampToHz, previousQ32); uint64_t candidateQ32; uint64_t candidateAreaQ32; uint64_t differenceQ32; uint64_t correctionQ32; uint32_t derivativeHz; if (context->rampLastPhaseStepQ32 >= PLSR_PLANNER_Q32_ONE - previousQ32) { candidateQ32 = PLSR_PLANNER_Q32_ONE; } else if (context->rampLastPhaseStepQ32 != 0ULL) { candidateQ32 = previousQ32 + context->rampLastPhaseStepQ32; } else { derivativeHz = PlsrPlannerInstantFrequency(context, previousQ32); if (derivativeHz == 0UL) { derivativeHz = 1UL; } differenceQ32 = targetAreaQ32 - currentAreaQ32; correctionQ32 = (differenceQ32 + derivativeHz - 1UL) / derivativeHz; candidateQ32 = (correctionQ32 >= PLSR_PLANNER_Q32_ONE - previousQ32) ? PLSR_PLANNER_Q32_ONE : previousQ32 + correctionQ32; } candidateAreaQ32 = PlsrPlannerRampAreaQ32( context, context->rampFromHz, context->rampToHz, candidateQ32); derivativeHz = PlsrPlannerInstantFrequency(context, candidateQ32); if (derivativeHz == 0UL) { derivativeHz = 1UL; } if (candidateAreaQ32 < targetAreaQ32) { differenceQ32 = targetAreaQ32 - candidateAreaQ32; correctionQ32 = (differenceQ32 + derivativeHz - 1UL) / derivativeHz; candidateQ32 = (correctionQ32 >= PLSR_PLANNER_Q32_ONE - candidateQ32) ? PLSR_PLANNER_Q32_ONE : candidateQ32 + correctionQ32; } else if (candidateAreaQ32 > targetAreaQ32) { differenceQ32 = candidateAreaQ32 - targetAreaQ32; correctionQ32 = differenceQ32 / derivativeHz; if (correctionQ32 == 0ULL) { correctionQ32 = 1ULL; } candidateQ32 = (correctionQ32 >= candidateQ32 - previousQ32) ? previousQ32 + 1ULL : candidateQ32 - correctionQ32; } return candidateQ32; } static void PlsrPlannerStartRamp(PLSR_PLANNER_CONTEXT *context, uint8_t rampKind, uint32_t fromHz, uint32_t toHz, uint32_t pulseCount) { #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) uint32_t totalStartedAt = PlsrPlannerTimingNow(); uint32_t phaseStartedAt; #endif context->rampKind = rampKind; context->rampRelativePulse = 0UL; context->rampPulseCount = pulseCount; context->rampFromHz = fromHz; context->rampToHz = toHz; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) phaseStartedAt = PlsrPlannerTimingNow(); #endif context->rampDurationMs = PlsrPlannerRampTime(&context->block, fromHz, toHz); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampTime, phaseStartedAt); phaseStartedAt = PlsrPlannerTimingNow(); #endif context->rampTotalAreaQ32 = PlsrPlannerRampAreaQ32( context, fromHz, toHz, PLSR_PLANNER_Q32_ONE); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampTotalArea, phaseStartedAt); phaseStartedAt = PlsrPlannerTimingNow(); #endif context->rampAreaStepQ32 = context->rampTotalAreaQ32 / pulseCount; context->rampAreaRemainder = (uint32_t)(context->rampTotalAreaQ32 % pulseCount); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampAreaSplit, phaseStartedAt); #endif context->rampRemainderAccumulator = 0UL; context->rampTargetAreaQ32 = 0ULL; context->rampBoundaryQ32 = 0ULL; context->rampActualTimeQ32 = 0ULL; context->rampLastPhaseStepQ32 = 0ULL; context->lastRampHz = 0UL; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) phaseStartedAt = PlsrPlannerTimingNow(); #endif context->rampFirstBoundaryQ32 = PlsrPlannerExactBoundaryQ32( context, 0ULL, context->rampAreaStepQ32); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampFirstBoundary, phaseStartedAt); #endif if (pulseCount > 1UL) { uint64_t secondTargetAreaQ32; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) phaseStartedAt = PlsrPlannerTimingNow(); #endif secondTargetAreaQ32 = context->rampAreaStepQ32 * 2ULL + ((uint64_t)context->rampAreaRemainder * 2ULL) / pulseCount; context->rampSecondBoundaryQ32 = PlsrPlannerExactBoundaryQ32( context, context->rampFirstBoundaryQ32, secondTargetAreaQ32); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampSecondBoundary, phaseStartedAt); #endif } else { context->rampSecondBoundaryQ32 = PLSR_PLANNER_Q32_ONE; } #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.startRamp, totalStartedAt); #endif } static uint8_t PlsrPlannerSameSetting( const PLSR_PLATFORM_TIMER_SETTING *first, const PLSR_PLATFORM_TIMER_SETTING *second) { return ((first->actualFrequencyHz == second->actualFrequencyHz) && (first->prescaler == second->prescaler) && (first->pairPrescaler == second->pairPrescaler) && (first->period == second->period) && (first->compare == second->compare)) ? 1U : 0U; } static uint8_t PlsrPlannerBuildStep(PLSR_PLANNER_CONTEXT *context, uint32_t requestedHz, PLSR_PLATFORM_TIMER_SETTING *setting, uint32_t *normalizedRequestedHz) { #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) uint32_t startedAt; uint8_t result; #endif if (requestedHz == 0UL) { requestedHz = 1UL; } if (requestedHz > PLSR_FREQUENCY_MAX_HZ) { requestedHz = PLSR_FREQUENCY_MAX_HZ; } *normalizedRequestedHz = requestedHz; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) startedAt = PlsrPlannerTimingNow(); result = PlsrPlatformBuildTimerSetting(context->block.pulseOutput, PLSR_OUTPUT_PULSE_DIR, requestedHz, setting); PlsrPlannerTimingRecord(&PlsrPlannerTiming.timerSetting, startedAt); return result; #else return PlsrPlatformBuildTimerSetting(context->block.pulseOutput, PLSR_OUTPUT_PULSE_DIR, requestedHz, setting); #endif } static uint8_t PlsrPlannerTakeRampStep( PLSR_PLANNER_CONTEXT *context, PLSR_PLATFORM_TIMER_SETTING *setting, uint32_t *requestedFrequencyHz) { uint64_t nextBoundaryQ32; uint64_t desiredDeltaQ32; uint64_t denominator; uint64_t requestedHz; uint64_t actualDeltaQ32; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) uint32_t totalStartedAt = PlsrPlannerTimingNow(); uint32_t phaseStartedAt; #endif context->rampTargetAreaQ32 += context->rampAreaStepQ32; context->rampRemainderAccumulator += context->rampAreaRemainder; if (context->rampRemainderAccumulator >= context->rampPulseCount) { context->rampTargetAreaQ32++; context->rampRemainderAccumulator -= context->rampPulseCount; } if (context->rampRelativePulse + 1UL >= context->rampPulseCount) { context->rampTargetAreaQ32 = context->rampTotalAreaQ32; nextBoundaryQ32 = PLSR_PLANNER_Q32_ONE; } else if (context->rampRelativePulse == 0UL) { nextBoundaryQ32 = context->rampFirstBoundaryQ32; } else if (context->rampRelativePulse == 1UL) { nextBoundaryQ32 = context->rampSecondBoundaryQ32; } else { #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) phaseStartedAt = PlsrPlannerTimingNow(); #endif nextBoundaryQ32 = PlsrPlannerPredictedBoundaryQ32( context, context->rampTargetAreaQ32); #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampPredictedBoundary, phaseStartedAt); #endif } desiredDeltaQ32 = (nextBoundaryQ32 > context->rampActualTimeQ32) ? (nextBoundaryQ32 - context->rampActualTimeQ32) : 1ULL; /* The allocated pulse count closes the ramp area exactly. Derive the physical duration from N/averageHz instead of rounding it to whole milliseconds; short clipped ramps can be well below 1 ms. */ #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) phaseStartedAt = PlsrPlannerTimingNow(); #endif denominator = (uint64_t)context->rampPulseCount * desiredDeltaQ32; requestedHz = (denominator == 0ULL) ? context->rampToHz : (context->rampTotalAreaQ32 + denominator / 2ULL) / denominator; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampRequestedDivide, phaseStartedAt); #endif if (requestedHz == 0ULL) { requestedHz = 1ULL; } if (requestedHz > PLSR_FREQUENCY_MAX_HZ) { requestedHz = PLSR_FREQUENCY_MAX_HZ; } if ((context->lastRampHz != 0UL) && (((context->rampToHz > context->rampFromHz) && (requestedHz < context->lastRampHz)) || ((context->rampToHz < context->rampFromHz) && (requestedHz > context->lastRampHz)))) { requestedHz = context->lastRampHz; } if (PlsrPlannerBuildStep(context, (uint32_t)requestedHz, setting, requestedFrequencyHz) == 0U) { #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampStep, totalStartedAt); #endif return 0U; } #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) phaseStartedAt = PlsrPlannerTimingNow(); #endif denominator = (uint64_t)context->rampPulseCount * setting->actualFrequencyHz; actualDeltaQ32 = (denominator == 0ULL) ? desiredDeltaQ32 : (context->rampTotalAreaQ32 + denominator / 2ULL) / denominator; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampActualDivide, phaseStartedAt); #endif context->rampActualTimeQ32 += actualDeltaQ32; context->rampLastPhaseStepQ32 = nextBoundaryQ32 - context->rampBoundaryQ32; context->rampBoundaryQ32 = nextBoundaryQ32; context->lastRampHz = setting->actualFrequencyHz; context->rampRelativePulse++; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampStep, totalStartedAt); #endif return 1U; } static uint8_t PlsrPlannerTakeStep(PLSR_PLANNER_CONTEXT *context, PLSR_PLATFORM_TIMER_SETTING *setting, uint32_t *requestedFrequencyHz, uint32_t *repeatCount) { uint32_t entryEnd = context->entryPulses; //加速段结束门槛。已经吐出的脉冲 < entryEnd 还在加速。例如加速 1000，这里就是 1000 uint32_t steadyEnd = entryEnd + context->steadyPulses; //匀速段结束门槛。< steadyEnd 且 ≥ entryEnd 就是匀速。例如再加 5000 匀速，这里就是 6000。再往后是减速 *repeatCount = 1UL; if (context->generatedPulses >= context->block.pulseBudget) { return 0U; } if (context->generatedPulses < entryEnd) { //加速段 if (context->rampKind != 1U) { // 第一次走进加速 PlsrPlannerStartRamp(context, 1U, context->startHz,context->peakHz, context->entryPulses); } return PlsrPlannerTakeRampStep(context, setting,requestedFrequencyHz); } if (context->generatedPulses < steadyEnd) { *repeatCount = steadyEnd - context->generatedPulses; context->rampKind = 0U; return PlsrPlannerBuildStep(context, context->peakHz, setting, requestedFrequencyHz); } if (context->rampKind != 2U) { // 第一次走进减速 PlsrPlannerStartRamp(context, 2U, context->peakHz, context->endHz, context->exitPulses); } return PlsrPlannerTakeRampStep(context, setting, requestedFrequencyHz); } PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context, const PLSR_MOTION_BLOCK *block, uint32_t appliedHz, uint64_t phasePulses) { uint64_t directRequired; uint64_t entryRequired; uint64_t exitRequired; uint64_t entryWeight; uint64_t exitWeight; uint64_t totalWeight; uint64_t scaledEntry; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) uint32_t startedAt = PlsrPlannerTimingNow(); #endif if ((context == NULL) || (block == NULL) || (block->pulseBudget == 0UL) || (block->referenceSpeedHz == 0UL) || (block->referenceSpeedHz > PLSR_FREQUENCY_MAX_HZ) || (block->entryHz > PLSR_FREQUENCY_MAX_HZ) || (block->cruiseHz == 0UL) || (block->cruiseHz > PLSR_FREQUENCY_MAX_HZ) || (block->exitHz > PLSR_FREQUENCY_MAX_HZ) || (appliedHz > PLSR_FREQUENCY_MAX_HZ) || (block->curveMode > 2U) || (block->pulseOutput > 3U)) { #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt); #endif return PLSR_PLANNER_INVALID; } (void)memset(context, 0, sizeof(*context)); context->block = *block; context->phasePulses = phasePulses; context->startHz = (appliedHz != 0UL) ? appliedHz : block->entryHz; if (context->startHz == 0UL) { context->startHz = 1UL; } context->endHz = (block->exitHz == 0UL) ? 1UL : block->exitHz; directRequired = PlsrPlannerRequiredPulses( block, PlsrPlannerRampWeight( context->startHz, context->endHz, PlsrPlannerBaseRampTime(block, context->startHz, context->endHz))); if (directRequired > block->pulseBudget) { context->peakHz = PlsrPlannerReachableFrequency( block, context->startHz, context->endHz, block->pulseBudget); context->endHz = context->peakHz; context->entryPulses = block->pulseBudget; context->clipped = 1U; context->active = 1U; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt); #endif return PLSR_PLANNER_CLIPPED; } context->peakHz = PlsrPlannerPeak(block, context->startHz, context->endHz); entryWeight = PlsrPlannerRampWeight( context->startHz, context->peakHz, PlsrPlannerBaseRampTime(block, context->startHz, context->peakHz)); exitWeight = PlsrPlannerRampWeight( context->peakHz, context->endHz, PlsrPlannerBaseRampTime(block, context->peakHz, context->endHz)); entryRequired = PlsrPlannerRequiredPulses(block, entryWeight); exitRequired = PlsrPlannerRequiredPulses(block, exitWeight); if ((entryRequired + exitRequired) <= block->pulseBudget) { context->entryPulses = (uint32_t)entryRequired; context->exitPulses = (uint32_t)exitRequired; context->steadyPulses = block->pulseBudget - context->entryPulses - context->exitPulses; } else if (entryRequired == 0ULL) { context->exitPulses = block->pulseBudget; context->clipped = 1U; } else if (exitRequired == 0ULL) { context->entryPulses = block->pulseBudget; context->clipped = 1U; } else { totalWeight = entryWeight + exitWeight; scaledEntry = ((uint64_t)block->pulseBudget * entryWeight + totalWeight / 2ULL) / totalWeight; if (scaledEntry == 0ULL) { scaledEntry = 1ULL; } if (scaledEntry >= block->pulseBudget) { scaledEntry = block->pulseBudget - 1UL; } context->entryPulses = (uint32_t)scaledEntry; context->exitPulses = block->pulseBudget - context->entryPulses; context->clipped = 1U; } if (context->peakHz != block->cruiseHz) { context->clipped = 1U; } context->active = 1U; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt); #endif return (context->clipped != 0U) ? PLSR_PLANNER_CLIPPED : PLSR_PLANNER_OK; } //返回值：实际写了几项（合并后的项数，不是脉冲数）。0 = 没吐出任何东西 uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context,//规划账本：三段脉冲、已经吐了多少、斜坡面积指针 PLSR_STREAM_ITEM *output,//输出数组，调用方准备好的格子 uint16_t capacity)//这一次最多往 output 里写几项 { PLSR_PLATFORM_TIMER_SETTING setting; uint32_t requestedFrequencyHz; uint32_t repeatCount; uint16_t produced = 0U; #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) uint32_t startedAt = PlsrPlannerTimingNow(); #endif if ((context == NULL) || (output == NULL) || (capacity == 0U) || (context->active == 0U)) { #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.generate, startedAt); #endif return 0U; } while ((produced < capacity)&& (context->generatedPulses < context->block.pulseBudget)) { //输出数组还没写满&&本段预算还没全部交给外面。generatedPulses 按脉冲个数计 if (PlsrPlannerTakeStep(context, &setting, &requestedFrequencyHz, &repeatCount) == 0U) //本圈 TakeStep 算出的量化后 PSC/ARR/actualHz，先放栈上 { context->active = 0U;//规划器关掉 break; } if ((produced != 0U) && (PlsrPlannerSameSetting(&output[produced - 1U].setting,&setting) != 0U) && (output[produced - 1U].requestedFrequencyHz == requestedFrequencyHz) && (output[produced - 1U].repeatCount <= 0xFFFFFFFFUL - repeatCount)) { output[produced - 1U].repeatCount += repeatCount; //不用开新格 } else { output[produced].setting = setting; output[produced].requestedFrequencyHz = requestedFrequencyHz; output[produced].repeatCount = repeatCount; produced++; } context->generatedPulses += repeatCount; } if (context->generatedPulses >= context->block.pulseBudget) { context->active = 0U; } #if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \ && !defined(PLSR_HOST_TEST) PlsrPlannerTimingRecord(&PlsrPlannerTiming.generate, startedAt); #endif return produced; } #if !defined(PLSR_HOST_TEST) static uint32_t PlsrPlannerBenchmarkCyclesNow(void) { return *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CYCCNT); } static void PlsrPlannerBenchmarkEnableCounter(void) { *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DEMCR_ADDRESS) |= PLSR_PLANNER_BENCHMARK_TRCENA; *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CYCCNT) = 0UL; *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CTRL) |= PLSR_PLANNER_BENCHMARK_CYCCNTENA; } static uint32_t PlsrPlannerBenchmarkCyclesQ16(uint32_t cycles, uint32_t pulses) { if (pulses == 0UL) { return 0UL; } return (uint32_t)((((uint64_t)cycles << 16U) + pulses / 2UL) / pulses); } static void PlsrPlannerBenchmarkMode(uint16_t curveMode) { PLSR_MOTION_BLOCK block; PLSR_PLANNER_CONTEXT context; PLSR_STREAM_ITEM item; PLSR_PLANNER_BENCHMARK_RESULT result; PLSR_PLANNER_STATUS status; uint64_t totalCycles = 0ULL; uint32_t minimumBlockQ16 = 0xFFFFFFFFUL; uint32_t maximumBlockQ16 = 0UL; uint32_t generateCalls = 0UL; uint8_t failed = 0U; (void)memset(&block, 0, sizeof(block)); (void)memset(&context, 0, sizeof(context)); (void)memset(&item, 0, sizeof(item)); (void)memset(&result, 0, sizeof(result)); block.entryHz = 1UL; block.cruiseHz = PLSR_PLANNER_BENCHMARK_TARGET_HZ; block.exitHz = 1UL; block.pulseBudget = PLSR_PLANNER_BENCHMARK_PULSES; block.referenceSpeedHz = PLSR_PLANNER_BENCHMARK_TARGET_HZ; block.accelerationTimeMs = 1000U; block.decelerationTimeMs = 1000U; block.curveMode = curveMode; block.pulseOutput = 0U; block.boundary = PLSR_BOUNDARY_STOP; status = PlsrPlannerBegin(&context, &block, 0UL, 0ULL); result.curveMode = curveMode; result.beginStatus = (uint32_t)status; if (status == PLSR_PLANNER_OK) { while (context.active != 0U) { uint32_t batchCalls = 0UL; uint32_t startedAt = PlsrPlannerBenchmarkCyclesNow(); uint32_t elapsed; uint32_t blockQ16; while ((batchCalls < PLSR_PLANNER_BENCHMARK_BATCH_CALLS) && (context.active != 0U)) { if (PlsrPlannerGenerate(&context, &item, 1U) == 0U) { failed = 1U; break; } batchCalls++; } elapsed = PlsrPlannerBenchmarkCyclesNow() - startedAt; if (batchCalls != 0UL) { blockQ16 = PlsrPlannerBenchmarkCyclesQ16(elapsed, batchCalls); totalCycles += elapsed; generateCalls += batchCalls; if (blockQ16 < minimumBlockQ16) { minimumBlockQ16 = blockQ16; } if (blockQ16 > maximumBlockQ16) { maximumBlockQ16 = blockQ16; } } if (failed != 0U) { break; } } } result.plannedPulses = context.generatedPulses; result.generateCalls = generateCalls; result.totalCycles = totalCycles; result.minimumBlockCyclesPerPulseQ16 = (minimumBlockQ16 == 0xFFFFFFFFUL) ? 0UL : minimumBlockQ16; result.maximumBlockCyclesPerPulseQ16 = maximumBlockQ16; if ((context.generatedPulses != 0UL) && (totalCycles != 0ULL)) { uint64_t averageQ16 = ((totalCycles << 16U) + context.generatedPulses / 2UL) / context.generatedPulses; uint64_t estimatedHz = ((uint64_t)PlsrPlannerBenchmarkCoreClockHz * context.generatedPulses + totalCycles / 2ULL) / totalCycles; uint64_t targetCyclesQ16 = ((uint64_t)PlsrPlannerBenchmarkCoreClockHz << 16U) / PLSR_PLANNER_BENCHMARK_TARGET_HZ; result.averageCyclesPerPulseQ16 = (averageQ16 > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL : (uint32_t)averageQ16; result.estimatedPulsesPerSecond = (estimatedHz > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL : (uint32_t)estimatedHz; result.passes100k = (averageQ16 <= targetCyclesQ16) ? 1UL : 0UL; } result.completed = ((failed == 0U) && (context.generatedPulses == PLSR_PLANNER_BENCHMARK_PULSES) && (context.active == 0U)) ? 1UL : 0UL; PlsrPlannerBenchmarkResults[curveMode] = result; } void PlsrPlannerBenchmarkService(void) { uint16_t curveMode; if ((PlsrPlannerBenchmarkRequest == 0UL) || (PlsrPlannerBenchmarkRunning != 0UL)) { return; } PlsrPlannerBenchmarkRequest = 0UL; PlsrPlannerBenchmarkRunning = 1UL; (void)memset((void *)PlsrPlannerBenchmarkResults, 0, sizeof(PlsrPlannerBenchmarkResults)); PlsrPlannerBenchmarkEnableCounter(); for (curveMode = 0U; curveMode < 3U; curveMode++) { PlsrPlannerBenchmarkMode(curveMode); } if (PlsrPlannerBenchmarkRunCount != 0xFFFFFFFFUL) { PlsrPlannerBenchmarkRunCount++; } PlsrPlannerBenchmarkRunning = 0UL; } #endif static uint64_t PlsrPlannerRampDurationUs(uint32_t pulseCount, uint32_t fromHz, uint32_t toHz) { uint64_t frequencySum = (uint64_t)fromHz + toHz; if ((pulseCount == 0UL) || (frequencySum == 0ULL)) { return 0ULL; } return ((uint64_t)2U * pulseCount * 1000000ULL + frequencySum - 1ULL) / frequencySum; } /* Return floor(numerator / denominator * 2^32) without requiring a 128-bit intermediate. Both operands are bounded by the planner's 100 kHz Q32 ramp area, so the normalized remainder can be doubled safely. */ static uint32_t PlsrPlannerRatioQ32(uint64_t numerator, uint64_t denominator) { uint64_t remainder; uint32_t ratio = 0UL; uint8_t bit; if ((numerator == 0ULL) || (denominator == 0ULL)) { return 0UL; } if (numerator >= denominator) { return 0xFFFFFFFFUL; } remainder = numerator; for (bit = 0U; bit < 32U; bit++) { ratio <<= 1U; remainder <<= 1U; if (remainder >= denominator) { remainder -= denominator; ratio |= 1UL; } } return ratio; } static uint32_t PlsrPlannerRampPulsesAtTime( const PLSR_PLANNER_CONTEXT *context, uint32_t pulseCount, uint32_t fromHz, uint32_t toHz, uint64_t elapsedUs, uint64_t durationUs, uint64_t *progressQ32) { PLSR_PLANNER_CONTEXT ramp = *context; uint64_t partialAreaQ32; uint64_t totalAreaQ32; uint64_t product; uint32_t areaRatioQ32; uint32_t result; if ((pulseCount == 0UL) || (elapsedUs == 0ULL) || (durationUs == 0ULL)) { *progressQ32 = 0ULL; return 0UL; } if (elapsedUs >= durationUs) { *progressQ32 = PLSR_PLANNER_Q32_ONE; return pulseCount; } *progressQ32 = (elapsedUs * PLSR_PLANNER_Q32_ONE) / durationUs; ramp.rampFromHz = fromHz; ramp.rampToHz = toHz; partialAreaQ32 = PlsrPlannerRampAreaQ32( &ramp, fromHz, toHz, *progressQ32); totalAreaQ32 = PlsrPlannerRampAreaQ32( &ramp, fromHz, toHz, PLSR_PLANNER_Q32_ONE); areaRatioQ32 = PlsrPlannerRatioQ32(partialAreaQ32, totalAreaQ32); product = (uint64_t)pulseCount * areaRatioQ32; result = (uint32_t)(product >> 32U); if ((uint32_t)product != 0UL) { result++; } return (result > pulseCount) ? pulseCount : result; } static uint8_t PlsrPlannerSetPredictedFrequency( const PLSR_PLANNER_CONTEXT *context, uint32_t fromHz, uint32_t toHz, uint64_t progressQ32, PLSR_PLANNER_TIME_PREDICTION *prediction) { PLSR_PLANNER_CONTEXT ramp = *context; PLSR_PLATFORM_TIMER_SETTING setting; uint32_t requestedHz; ramp.rampFromHz = fromHz; ramp.rampToHz = toHz; requestedHz = PlsrPlannerInstantFrequency(&ramp, progressQ32); if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput, PLSR_OUTPUT_PULSE_DIR, requestedHz, &setting) == 0U) { return 0U; } prediction->actualFrequencyHz = setting.actualFrequencyHz; return 1U; } static uint64_t PlsrPlannerRampTargetAreaQ32(uint64_t totalAreaQ32, uint32_t pulseCount, uint32_t pulseIndex) { uint64_t step = totalAreaQ32 / pulseCount; uint64_t remainder = totalAreaQ32 % pulseCount; return step * pulseIndex + (remainder * pulseIndex) / pulseCount; } /* Predict the timer setting of the last complete ramp pulse at the deadline. A ramp pulse represents the average frequency between two equal-area curve boundaries; carrying that run setting is closer to the hardware state than carrying the mathematical instantaneous frequency at the boundary. */ static uint8_t PlsrPlannerSetPredictedRampRunFrequency( const PLSR_PLANNER_CONTEXT *context, uint32_t pulseCount, uint32_t fromHz, uint32_t toHz, uint32_t pulseIndex, PLSR_PLANNER_TIME_PREDICTION *prediction) { PLSR_PLANNER_CONTEXT ramp = *context; PLSR_PLATFORM_TIMER_SETTING setting; uint64_t totalAreaQ32; uint64_t previousTargetAreaQ32; uint64_t targetAreaQ32; uint64_t previousBoundaryQ32; uint64_t boundaryQ32; uint64_t denominator; uint64_t requestedHz; if ((pulseCount == 0UL) || (pulseIndex == 0UL)) { return PlsrPlannerSetPredictedFrequency( context, fromHz, toHz, 0ULL, prediction); } if (pulseIndex > pulseCount) { pulseIndex = pulseCount; } ramp.rampFromHz = fromHz; ramp.rampToHz = toHz; totalAreaQ32 = PlsrPlannerRampAreaQ32( &ramp, fromHz, toHz, PLSR_PLANNER_Q32_ONE); previousTargetAreaQ32 = PlsrPlannerRampTargetAreaQ32( totalAreaQ32, pulseCount, pulseIndex - 1UL); targetAreaQ32 = PlsrPlannerRampTargetAreaQ32( totalAreaQ32, pulseCount, pulseIndex); previousBoundaryQ32 = (pulseIndex == 1UL) ? 0ULL : PlsrPlannerExactBoundaryQ32( &ramp, 0ULL, previousTargetAreaQ32); boundaryQ32 = (pulseIndex == pulseCount) ? PLSR_PLANNER_Q32_ONE : PlsrPlannerExactBoundaryQ32( &ramp, previousBoundaryQ32, targetAreaQ32); if (boundaryQ32 <= previousBoundaryQ32) { return 0U; } denominator = (uint64_t)pulseCount * (boundaryQ32 - previousBoundaryQ32); requestedHz = (denominator == 0ULL) ? toHz : (totalAreaQ32 + denominator / 2ULL) / denominator; if (requestedHz == 0ULL) { requestedHz = 1ULL; } if (requestedHz > PLSR_FREQUENCY_MAX_HZ) { requestedHz = PLSR_FREQUENCY_MAX_HZ; } if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput, PLSR_OUTPUT_PULSE_DIR, (uint32_t)requestedHz, &setting) == 0U) { return 0U; } prediction->actualFrequencyHz = setting.actualFrequencyHz; return 1U; } uint8_t PlsrPlannerPredictTime( const PLSR_PLANNER_CONTEXT *context, uint32_t elapsedUs, PLSR_PLANNER_TIME_PREDICTION *prediction) { PLSR_PLATFORM_TIMER_SETTING steadySetting; uint64_t remainingUs = elapsedUs; uint64_t durationUs; uint64_t progressQ32; uint64_t partialPulses; if ((context == NULL) || (prediction == NULL) || (context->block.pulseBudget == 0UL)) { return 0U; } (void)memset(prediction, 0, sizeof(*prediction)); durationUs = PlsrPlannerRampDurationUs( context->entryPulses, context->startHz, context->peakHz); if ((context->entryPulses != 0UL) && (remainingUs <= durationUs)) { prediction->pulseCount = PlsrPlannerRampPulsesAtTime( context, context->entryPulses, context->startHz, context->peakHz, remainingUs, durationUs, &progressQ32); prediction->phase = PLSR_PLANNER_PHASE_ENTRY; prediction->deadlineInProfile = 1U; return PlsrPlannerSetPredictedRampRunFrequency( context, context->entryPulses, context->startHz, context->peakHz, prediction->pulseCount, prediction); } if (context->entryPulses != 0UL) { remainingUs -= durationUs; } if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput, PLSR_OUTPUT_PULSE_DIR, context->peakHz, &steadySetting) == 0U) { return 0U; } durationUs = (context->steadyPulses == 0UL) ? 0ULL : ((uint64_t)context->steadyPulses * 1000000ULL + steadySetting.actualFrequencyHz - 1UL) / steadySetting.actualFrequencyHz; if ((context->steadyPulses != 0UL) && (remainingUs <= durationUs)) { partialPulses = (remainingUs * steadySetting.actualFrequencyHz + 999999ULL) / 1000000ULL; if (partialPulses > context->steadyPulses) { partialPulses = context->steadyPulses; } prediction->pulseCount = context->entryPulses + (uint32_t)partialPulses; prediction->actualFrequencyHz = steadySetting.actualFrequencyHz; prediction->phase = PLSR_PLANNER_PHASE_STEADY; prediction->deadlineInProfile = 1U; return 1U; } if (context->steadyPulses != 0UL) { remainingUs -= durationUs; } durationUs = PlsrPlannerRampDurationUs( context->exitPulses, context->peakHz, context->endHz); if ((context->exitPulses != 0UL) && (remainingUs <= durationUs)) { partialPulses = PlsrPlannerRampPulsesAtTime( context, context->exitPulses, context->peakHz, context->endHz, remainingUs, durationUs, &progressQ32); prediction->pulseCount = context->entryPulses + context->steadyPulses + (uint32_t)partialPulses; prediction->phase = PLSR_PLANNER_PHASE_EXIT; prediction->deadlineInProfile = 1U; return PlsrPlannerSetPredictedRampRunFrequency( context, context->exitPulses, context->peakHz, context->endHz, (uint32_t)partialPulses, prediction); } prediction->pulseCount = context->block.pulseBudget; prediction->phase = PLSR_PLANNER_PHASE_COMPLETE; prediction->deadlineInProfile = 0U; return PlsrPlannerSetPredictedFrequency( context, context->endHz, context->endHz, PLSR_PLANNER_Q32_ONE, prediction); }