From 13e25111959789065bd49b91ad414fc770297378 Mon Sep 17 00:00:00 2001 From: hanyongwei <2043702190@qq.com> Date: Thu, 3 Sep 2026 13:50:16 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BB=88=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: hanyongwei <2043702190@qq.com> --- app/main.c | 2 +- iar/plsr.dep | 339 +++--- plsr/accel_curve/plsr_accel_curve.c | 1533 ++++++++++++------------- plsr/accel_curve/plsr_accel_curve.h | 55 +- plsr/command/plsr_command.c | 59 +- plsr/command/plsr_command.h | 42 +- plsr/param/plsr_param.c | 92 +- plsr/param/plsr_param.h | 104 +- plsr/plsr.c | 26 +- plsr/plsr.h | 7 +- plsr/pulse_driver/plsr_pulse_driver.c | 543 +++++---- plsr/pulse_driver/plsr_pulse_driver.h | 9 +- plsr/run_control/plsr_run_control.c | 1504 ++++++++++++------------ plsr/signal_io/plsr_signal_io.c | 50 +- 14 files changed, 2102 insertions(+), 2263 deletions(-) diff --git a/app/main.c b/app/main.c index 809ff50..17b4fde 100644 --- a/app/main.c +++ b/app/main.c @@ -138,7 +138,7 @@ void SystemClock_Config(void) RCC_OscInitStruct.PLL.PLLM = 12; /* HSE=12MHz → VCO in = 1MHz */ RCC_OscInitStruct.PLL.PLLN = 336; RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; - RCC_OscInitStruct.PLL.PLLQ = 4;/*不准改成7*/ + RCC_OscInitStruct.PLL.PLLQ = 4; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); diff --git a/iar/plsr.dep b/iar/plsr.dep index d60db83..0acb0c8 100644 --- a/iar/plsr.dep +++ b/iar/plsr.dep @@ -600,6 +600,7 @@ $PROJ_DIR$\Debug\Obj\startup_stm32f407xx.o $PROJ_DIR$\..\ucos\uC-LIB\lib_ascii.h $PROJ_DIR$\..\ucos\uC-LIB\lib_math.c + $PROJ_DIR$\..\ucos\uC-LIB\stm32f4xx_hal_conf.h $PROJ_DIR$\..\ucos\uC-LIB\lib_def.h $PROJ_DIR$\..\ucos\uCOS-II\Source\os_time.c $PROJ_DIR$\..\ucos\uCOS-II\Source\os_flag.c @@ -683,7 +684,7 @@ ILINK - 155 668 + 155 669 @@ -692,11 +693,11 @@ ICCARM - 656 + 657 BICOMP - 642 + 643 __cstat @@ -706,7 +707,7 @@ ICCARM - 12 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 446 1 590 552 549 127 333 550 567 583 580 566 437 548 578 + 12 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 446 1 590 552 549 127 333 550 567 583 580 566 437 548 578 @@ -715,7 +716,7 @@ ILINK - 668 + 669 OBJCOPY @@ -725,7 +726,7 @@ ILINK - 86 325 110 315 109 104 321 115 43 609 105 656 66 45 586 618 165 58 42 166 137 108 68 129 661 323 198 622 21 100 156 327 628 192 592 74 135 185 32 107 89 54 95 61 195 644 173 48 170 23 319 186 617 119 641 116 175 197 85 120 202 161 671 49 184 199 103 55 637 102 210 88 64 635 52 670 94 140 132 331 207 187 203 40 666 59 77 22 106 131 60 629 150 652 182 153 75 632 663 152 196 97 93 665 56 69 633 312 151 30 314 99 326 335 96 123 655 177 176 650 128 138 25 654 660 171 194 121 47 113 126 200 657 658 205 + 86 325 110 315 109 104 321 115 43 610 105 657 66 45 586 619 165 58 42 166 137 108 68 129 662 323 198 623 21 100 156 327 629 192 592 74 135 185 32 107 89 54 95 61 195 645 173 48 170 23 319 186 618 119 642 116 175 197 85 120 202 161 672 49 184 199 103 55 638 102 210 88 64 636 52 671 94 140 132 331 207 187 203 40 667 59 77 22 106 131 60 630 150 653 182 153 75 633 664 152 196 97 93 666 56 69 634 312 151 30 314 99 326 335 96 123 656 177 176 651 128 138 25 655 661 171 194 121 47 113 126 200 658 659 205 @@ -748,7 +749,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -757,11 +758,11 @@ ICCARM - 617 + 618 BICOMP - 625 + 626 __cstat @@ -771,7 +772,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -794,7 +795,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -807,7 +808,7 @@ BICOMP - 631 + 632 __cstat @@ -817,7 +818,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -826,7 +827,7 @@ ICCARM - 671 + 672 BICOMP @@ -840,7 +841,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -863,7 +864,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -886,7 +887,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -909,7 +910,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -932,7 +933,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -945,7 +946,7 @@ BICOMP - 611 + 612 __cstat @@ -955,7 +956,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -978,7 +979,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1001,7 +1002,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1024,7 +1025,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1047,7 +1048,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1070,7 +1071,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1093,7 +1094,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1106,7 +1107,7 @@ BICOMP - 659 + 660 __cstat @@ -1116,7 +1117,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1139,7 +1140,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1162,7 +1163,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1185,7 +1186,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1208,7 +1209,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1221,7 +1222,7 @@ BICOMP - 648 + 649 __cstat @@ -1231,7 +1232,7 @@ ICCARM - 1 12 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 1 12 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1244,7 +1245,7 @@ BICOMP - 630 + 631 __cstat @@ -1254,7 +1255,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1277,7 +1278,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1300,7 +1301,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1309,7 +1310,7 @@ ICCARM - 641 + 642 BICOMP @@ -1323,7 +1324,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1346,7 +1347,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1359,7 +1360,7 @@ BICOMP - 636 + 637 __cstat @@ -1369,7 +1370,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1378,7 +1379,7 @@ ICCARM - 644 + 645 BICOMP @@ -1392,7 +1393,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1415,7 +1416,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1428,7 +1429,7 @@ BICOMP - 643 + 644 __cstat @@ -1438,7 +1439,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1461,7 +1462,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1474,7 +1475,7 @@ BICOMP - 649 + 650 __cstat @@ -1484,7 +1485,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1507,7 +1508,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1530,7 +1531,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1553,7 +1554,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1562,11 +1563,11 @@ ICCARM - 635 + 636 BICOMP - 639 + 640 __cstat @@ -1576,7 +1577,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1589,7 +1590,7 @@ BICOMP - 616 + 617 __cstat @@ -1599,7 +1600,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1622,7 +1623,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1645,7 +1646,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1658,7 +1659,7 @@ BICOMP - 640 + 641 __cstat @@ -1668,7 +1669,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1677,11 +1678,11 @@ ICCARM - 666 + 667 BICOMP - 667 + 668 __cstat @@ -1691,7 +1692,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1714,7 +1715,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1727,7 +1728,7 @@ BICOMP - 662 + 663 __cstat @@ -1737,7 +1738,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1750,7 +1751,7 @@ BICOMP - 664 + 665 __cstat @@ -1760,7 +1761,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1773,7 +1774,7 @@ BICOMP - 646 + 647 __cstat @@ -1783,7 +1784,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1806,7 +1807,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1829,7 +1830,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1852,7 +1853,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1865,7 +1866,7 @@ BICOMP - 653 + 654 __cstat @@ -1875,7 +1876,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1884,7 +1885,7 @@ ICCARM - 629 + 630 BICOMP @@ -1898,7 +1899,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1907,7 +1908,7 @@ ICCARM - 670 + 671 BICOMP @@ -1921,7 +1922,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1944,7 +1945,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1967,7 +1968,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1990,7 +1991,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -1999,7 +2000,7 @@ ICCARM - 637 + 638 BICOMP @@ -2013,7 +2014,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2036,7 +2037,7 @@ ICCARM - 12 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 12 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2045,11 +2046,11 @@ ICCARM - 652 + 653 BICOMP - 610 + 611 __cstat @@ -2059,7 +2060,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2082,7 +2083,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2105,7 +2106,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2128,7 +2129,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2151,7 +2152,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2174,7 +2175,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2183,7 +2184,7 @@ ICCARM - 650 + 651 BICOMP @@ -2204,7 +2205,7 @@ BICOMP - 626 + 627 __cstat @@ -2214,7 +2215,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2223,7 +2224,7 @@ ICCARM - 632 + 633 BICOMP @@ -2237,7 +2238,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2294,7 +2295,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2334,7 +2335,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2343,7 +2344,7 @@ ICCARM - 660 + 661 BICOMP @@ -2391,7 +2392,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2438,7 +2439,7 @@ BICOMP - 647 + 648 __cstat @@ -2448,7 +2449,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2457,7 +2458,7 @@ ICCARM - 654 + 655 BICOMP @@ -2478,7 +2479,7 @@ BICOMP - 613 + 614 __cstat @@ -2491,7 +2492,7 @@ ICCARM - 663 + 664 BICOMP @@ -2505,7 +2506,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2528,7 +2529,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2558,7 +2559,7 @@ BICOMP - 669 + 670 __cstat @@ -2568,7 +2569,7 @@ ICCARM - 444 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 2 14 421 383 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 453 431 423 + 444 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 2 14 421 383 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 453 431 423 @@ -2591,7 +2592,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2604,7 +2605,7 @@ BICOMP - 645 + 646 __cstat @@ -2631,7 +2632,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2661,7 +2662,7 @@ BICOMP - 620 + 621 __cstat @@ -2674,7 +2675,7 @@ ICCARM - 633 + 634 BICOMP @@ -2705,7 +2706,7 @@ ICCARM - 12 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 425 590 552 549 127 333 550 567 583 580 573 + 12 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 425 590 552 549 127 333 550 567 583 580 573 @@ -2731,7 +2732,7 @@ ICCARM - 655 + 656 BICOMP @@ -2748,7 +2749,7 @@ ICCARM - 665 + 666 BICOMP @@ -2762,7 +2763,7 @@ ICCARM - 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2788,7 +2789,7 @@ AARM - 609 + 610 @@ -2811,7 +2812,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 2 14 421 383 15 6 18 114 34 16 8 5 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 2 14 421 383 15 6 18 114 34 16 8 5 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2834,7 +2835,7 @@ ICCARM - 566 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 590 552 549 127 333 550 567 583 580 118 638 548 578 557 1 12 560 570 + 566 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 590 552 549 127 333 550 567 583 580 118 639 548 578 557 1 12 560 570 @@ -2857,7 +2858,7 @@ ICCARM - 549 127 208 90 57 35 627 333 550 567 556 595 599 601 600 593 + 549 127 208 90 57 35 628 333 550 567 556 596 600 602 601 593 @@ -2866,11 +2867,11 @@ ICCARM - 628 + 629 BICOMP - 623 + 624 __cstat @@ -2880,7 +2881,7 @@ ICCARM - 569 114 34 90 57 35 627 548 578 577 557 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 560 573 590 552 549 127 333 550 567 583 580 + 569 114 34 90 57 35 628 548 578 577 557 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 560 573 590 552 549 127 333 550 567 583 580 @@ -2903,7 +2904,7 @@ ICCARM - 578 114 34 90 57 35 627 570 560 566 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 590 552 549 127 333 550 567 583 580 + 578 114 34 90 57 35 628 570 560 566 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 590 552 549 127 333 550 567 583 580 @@ -2916,7 +2917,7 @@ BICOMP - 615 + 616 __cstat @@ -2926,7 +2927,7 @@ ICCARM - 570 114 34 90 57 35 627 548 578 560 569 557 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 566 590 552 549 127 333 550 567 583 580 + 570 114 34 90 57 35 628 548 578 560 569 557 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 566 590 552 549 127 333 550 567 583 580 @@ -2939,7 +2940,7 @@ BICOMP - 614 + 615 __cstat @@ -2949,7 +2950,7 @@ ICCARM - 446 12 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 446 12 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2972,7 +2973,7 @@ ICCARM - 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 2 14 421 383 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 + 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 2 14 421 383 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 @@ -2985,7 +2986,7 @@ BICOMP - 619 + 620 __cstat @@ -2995,7 +2996,7 @@ ICCARM - 560 114 34 90 57 35 627 578 569 557 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 548 570 566 590 552 549 127 333 550 567 583 580 + 560 114 34 90 57 35 628 578 569 557 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 548 570 566 590 552 549 127 333 550 567 583 580 @@ -3013,7 +3014,7 @@ ICCARM - 622 + 623 BICOMP @@ -3027,7 +3028,7 @@ ICCARM - 577 114 34 90 57 35 627 578 + 577 114 34 90 57 35 628 578 @@ -3050,7 +3051,7 @@ ICCARM - 573 114 34 90 57 35 627 578 569 12 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 590 552 549 127 333 550 567 583 580 + 573 114 34 90 57 35 628 578 569 12 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 590 552 549 127 333 550 567 583 580 @@ -3063,7 +3064,7 @@ BICOMP - 621 + 622 __cstat @@ -3073,7 +3074,7 @@ ICCARM - 437 2 14 421 383 15 6 18 114 34 90 57 35 627 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 12 + 437 2 14 421 383 15 6 18 114 34 90 57 35 628 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 12 @@ -3105,7 +3106,7 @@ ICCARM - 548 114 34 90 57 35 627 578 557 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 569 570 560 573 590 552 549 127 333 550 567 583 580 + 548 114 34 90 57 35 628 578 557 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 569 570 560 573 590 552 549 127 333 550 567 583 580 @@ -3128,7 +3129,7 @@ ICCARM - 557 114 34 90 57 35 627 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 578 12 + 557 114 34 90 57 35 628 2 14 421 383 15 6 18 16 8 5 208 19 20 9 0 62 308 411 367 376 359 384 385 362 378 371 369 418 415 397 398 399 578 12 @@ -3151,7 +3152,7 @@ ICCARM - 556 549 127 208 90 57 35 627 333 550 567 595 599 601 600 593 + 556 549 127 208 90 57 35 628 333 550 567 596 600 602 601 593 @@ -3174,7 +3175,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3196,7 +3197,7 @@ BICOMP - 651 + 652 __cstat @@ -3206,7 +3207,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3229,7 +3230,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3242,7 +3243,7 @@ BICOMP - 634 + 635 __cstat @@ -3252,7 +3253,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3275,7 +3276,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3298,7 +3299,7 @@ ICCARM - 599 549 127 208 90 57 35 627 333 550 567 556 595 600 593 601 603 + 600 549 127 208 90 57 35 628 333 550 567 556 596 601 593 602 604 @@ -3311,7 +3312,7 @@ BICOMP - 624 + 625 __cstat @@ -3321,7 +3322,7 @@ ICCARM - 603 549 127 208 90 57 35 627 333 550 567 556 595 599 601 600 593 + 604 549 127 208 90 57 35 628 333 550 567 556 596 600 602 601 593 @@ -3330,7 +3331,7 @@ ICCARM - 661 + 662 BICOMP @@ -3344,7 +3345,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3367,7 +3368,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3390,7 +3391,7 @@ ICCARM - 593 549 127 208 90 57 35 627 333 550 567 595 + 593 549 127 208 90 57 35 628 333 550 567 596 @@ -3413,7 +3414,7 @@ ICCARM - 600 549 127 208 90 57 35 627 333 550 567 595 593 601 + 601 549 127 208 90 57 35 628 333 550 567 596 593 602 @@ -3422,7 +3423,7 @@ ICCARM - 618 + 619 BICOMP @@ -3436,7 +3437,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 595 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 596 @@ -3459,7 +3460,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3482,7 +3483,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3495,7 +3496,7 @@ BICOMP - 612 + 613 __cstat @@ -3505,7 +3506,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 @@ -3528,7 +3529,7 @@ ICCARM - 590 552 549 127 208 90 57 35 627 333 550 567 583 580 + 590 552 549 127 208 90 57 35 628 333 550 567 583 580 diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index b4354d1..7d6bead 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -5,28 +5,25 @@ #include "plsr_accel_curve.h" #include -PlsrAccelPlan_t g_plsr_accel_plan; /* 当前段三相规划结果 */ /* 整段规划全局一份 */ -PlsrAccelRuntime_t g_plsr_accel_runtime; /* 当前相 ISR 逐拍状态 */ /* 当前相运行态全局一份 */ - -/* 正弦 raised-cosine 形状表（千分比 0..1000） */ -static const uint16_t s_plsr_sine_shape_tab[33] = { /* 半个余弦形状的进度表，0到1000 */ - 0U, 2U, 10U, 22U, 38U, 59U, 84U, 113U, /* 前半段：进度小，形状值也小 */ - 146U, 183U, 222U, 264U, 309U, 355U, 402U, 451U, /* 中间往上爬 */ - 500U, 549U, 598U, 645U, 691U, 736U, 778U, 817U, /* 过半：500是中点 */ - 854U, 887U, 916U, 941U, 962U, 978U, 990U, 998U, /* 后半段往1000收 */ - 1000U /* 终点1000=形状走完 */ +PlsrAccelPlan_t g_plsr_accel_plan; +PlsrAccelRuntime_t g_plsr_accel_runtime; + +static const uint16_t s_plsr_sine_shape_tab[33] = { + 0U, 2U, 10U, 22U, 38U, 59U, 84U, 113U, + 146U, 183U, 222U, 264U, 309U, 355U, 402U, 451U, + 500U, 549U, 598U, 645U, 691U, 736U, 778U, 817U, + 854U, 887U, 916U, 941U, 962U, 978U, 990U, 998U, + 1000U }; -/* S/正弦频率预计算表：PlanSeg 时填好；ISR 只切换/查表。匀速不改频。 */ -#define PLSR_RAMP_TBL_MAX 4096U /* 表最长4096项，超了就抽样 */ -static uint32_t s_accel_freq_table[PLSR_RAMP_TBL_MAX]; /* 加速相频率表 */ /* 加速相每拍频率 */ -static uint32_t s_decel_freq_table[PLSR_RAMP_TBL_MAX]; /* 减速相频率表 */ /* 减速相每拍频率 */ -static uint32_t s_accel_table_length; /* 加速表有效项数 */ /* 加速表用了几项 */ -static uint32_t s_accel_table_stride; /* 加速表抽样步长 */ /* 加速表几个脉冲读一项 */ -static uint32_t s_decel_table_length; /* 减速表有效项数 */ /* 减速表用了几项 */ -static uint32_t s_decel_table_stride; /* 减速表抽样步长 */ /* 减速表几个脉冲读一项 */ +#define PLSR_RAMP_TBL_MAX 4096U +static uint32_t s_accel_freq_table[PLSR_RAMP_TBL_MAX]; +static uint32_t s_decel_freq_table[PLSR_RAMP_TBL_MAX]; +static uint32_t s_accel_table_length; +static uint32_t s_accel_table_stride; +static uint32_t s_decel_table_length; +static uint32_t s_decel_table_stride; -/* 静态辅助函数前置声明 */ static uint32_t PlsrAccelCurveClampFreqHz(uint32_t freq_hz); static uint32_t PlsrAccelCurveAbsDiff(uint32_t first_val, uint32_t second_val); static uint32_t PlsrAccelCurveIntSqrt(uint64_t squared_freq); @@ -112,30 +109,24 @@ static void PlsrAccelRuntimeSelectFreqTable(PlsrAccelRuntime_t *runtime, uint32_t table_length, uint32_t table_stride); -/*============================================================================*/ -/* 公共 API */ -/*============================================================================*/ - -/* 配置起速 → 本段实际入口频率 */ uint32_t PlsrAccelCurveResolveStartHz(uint32_t configured_freq_hz, uint32_t target_freq_hz, uint32_t default_speed_hz, uint32_t accel_time_ms, uint32_t decel_time_ms) { - /* resolving_start=1：用 accel_time_ms 算 a */ + return PlsrAccelCurveResolveBoundaryFreq(configured_freq_hz, target_freq_hz, default_speed_hz, accel_time_ms, decel_time_ms, 1U); } -/* 配置止速 → 本段实际出口频率 */ uint32_t PlsrAccelCurveResolveEndHz(uint32_t configured_freq_hz, uint32_t target_freq_hz, uint32_t default_speed_hz, uint32_t accel_time_ms, uint32_t decel_time_ms) { - /* resolving_start=0：用 decel_time_ms 算 a（止速侧） */ + return PlsrAccelCurveResolveBoundaryFreq(configured_freq_hz, target_freq_hz, default_speed_hz, accel_time_ms, decel_time_ms, 0U); } @@ -156,91 +147,84 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, uint32_t decel_time_ms, PlsrAccelMode_e curve_mode) { - uint32_t accel_pulses; /* 本段加速要几拍 */ - uint32_t decel_pulses; /* 本段减速要几拍 */ - uint32_t peak_freq_hz; /* 实际峰值（可能被砍） */ - uint32_t accel_rate_hz_per_s; /* 加速斜率 Hz/s */ - uint32_t decel_rate_hz_per_s; /* 减速斜率 Hz/s */ - PlsrAccelMode_e selected_mode; /* 曲线模式 */ + uint32_t accel_pulses; + uint32_t decel_pulses; + uint32_t peak_freq_hz; + uint32_t accel_rate_hz_per_s; + uint32_t decel_rate_hz_per_s; + PlsrAccelMode_e selected_mode; - if (plan == (PlsrAccelPlan_t *)0) /* 空指针直接走人 */ + if (plan == (PlsrAccelPlan_t *)0) { - return; /* 没plan别往下算 */ + return; } - /* 入口先钳到 100k，避免后续开方/乘溢出 */ - start_freq_hz = PlsrAccelCurveClampFreqHz(start_freq_hz); /* 起频钳住 */ - target_freq_hz = PlsrAccelCurveClampFreqHz(target_freq_hz); /* 目标峰钳住 */ - end_freq_hz = PlsrAccelCurveClampFreqHz(end_freq_hz); /* 止频钳住 */ + start_freq_hz = PlsrAccelCurveClampFreqHz(start_freq_hz); + target_freq_hz = PlsrAccelCurveClampFreqHz(target_freq_hz); + end_freq_hz = PlsrAccelCurveClampFreqHz(end_freq_hz); - /* 非法模式回退直线 */ - selected_mode = (curve_mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : curve_mode; /* 超范围就当直线 */ + selected_mode = (curve_mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : curve_mode; - /* 起速未配（<1）：按配置/起跳规则补入口 */ - if (start_freq_hz < 1U) /* 起速没配 */ + if (start_freq_hz < 1U) { - start_freq_hz = PlsrAccelCurveResolveStartHz(0U, target_freq_hz, default_speed_hz, /* 补实际入口频 */ + start_freq_hz = PlsrAccelCurveResolveStartHz(0U, target_freq_hz, default_speed_hz, accel_time_ms, decel_time_ms); } - /* 止速未配（<1）：按配置/起跳规则补出口 */ - if (end_freq_hz < 1U) /* 止速没配 */ + + if (end_freq_hz < 1U) { - end_freq_hz = PlsrAccelCurveResolveEndHz(0U, target_freq_hz, default_speed_hz, /* 补实际出口频 */ + end_freq_hz = PlsrAccelCurveResolveEndHz(0U, target_freq_hz, default_speed_hz, accel_time_ms, decel_time_ms); } - /* a = default*1000/t_ms；无默认速度时用本段频差 */ accel_rate_hz_per_s = PlsrAccelCurveCalcRate(default_speed_hz, start_freq_hz, target_freq_hz, - accel_time_ms); /* 加速斜率 Hz/s */ + accel_time_ms); decel_rate_hz_per_s = PlsrAccelCurveCalcRate(default_speed_hz, target_freq_hz, end_freq_hz, - decel_time_ms); /* 减速斜率 Hz/s */ + decel_time_ms); - /* 先按请求峰值写死不变字段；峰值可能随后被 FitPeak 下调 */ - peak_freq_hz = target_freq_hz; /* 先假定能到请求的峰 */ + peak_freq_hz = target_freq_hz; - plan->start_freq_hz = start_freq_hz; /* 写入口频 */ - plan->end_freq_hz = end_freq_hz; /* 写出口频 */ - plan->curve_mode = selected_mode; /* 写曲线模式 */ - plan->accel_rate_hz_per_s = accel_rate_hz_per_s; /* 写加速斜率 */ - plan->decel_rate_hz_per_s = decel_rate_hz_per_s; /* 写减速斜率 */ + plan->start_freq_hz = start_freq_hz; + plan->end_freq_hz = end_freq_hz; + plan->curve_mode = selected_mode; + plan->accel_rate_hz_per_s = accel_rate_hz_per_s; + plan->decel_rate_hz_per_s = decel_rate_hz_per_s; - /* 按当前峰估 acc/dec 脉冲（直线闭合式；S/正弦仿真） */ - PlsrAccelCurveCalcPhasePulses(start_freq_hz, /* 起 */ - peak_freq_hz, /* 峰 */ - end_freq_hz, /* 止 */ - accel_rate_hz_per_s, /* 加速斜率 */ - decel_rate_hz_per_s, /* 减速斜率 */ - selected_mode, /* 直线/S/正弦 */ - &accel_pulses, /* 写出加速拍数 */ - &decel_pulses); /* 写出减速拍数 */ + PlsrAccelCurveCalcPhasePulses(start_freq_hz, + peak_freq_hz, + end_freq_hz, + accel_rate_hz_per_s, + decel_rate_hz_per_s, + selected_mode, + &accel_pulses, + &decel_pulses); - /* --- 特例：本段 0 脉冲 --- */ - if (total_pulses == 0U) /* 本段0脉冲 */ + if (total_pulses == 0U) { - plan->const_pulses = 0U; /* 匀速拍数清零 */ - plan->target_freq_hz = peak_freq_hz; /* 峰还是请求值 */ - plan->accel_pulses = 0U; /* 加速0拍 */ - plan->decel_pulses = 0U; /* 减速0拍 */ - return; /* 整段没活干，收工 */ + plan->const_pulses = 0U; + plan->target_freq_hz = peak_freq_hz; + plan->accel_pulses = 0U; + plan->decel_pulses = 0U; + return; } /* * --- 特例：出口=目标 且 光加速就要用光 N 个脉冲 --- * 整段当纯加速，无 dec、无 const（短段只爬峰就停） */ - if ((end_freq_hz == target_freq_hz) && (accel_pulses > total_pulses)) /* 出口就是峰，且加速拍已超总脉冲 */ + if ((end_freq_hz == target_freq_hz) && (accel_pulses > total_pulses)) { - plan->const_pulses = 0U; /* 匀速不要了 */ - plan->target_freq_hz = peak_freq_hz; /* 峰仍按请求 */ - plan->accel_pulses = total_pulses; /* 所有脉冲都拿去加速 */ - plan->decel_pulses = 0U; /* 不减速 */ - return; /* 短段只爬峰就停 */ + plan->const_pulses = 0U; + plan->target_freq_hz = peak_freq_hz; + plan->accel_pulses = total_pulses; + plan->decel_pulses = 0U; + return; } /* @@ -248,16 +232,16 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, * 够：余量给匀速（梯形） * 不够：砍匀速并降峰，使 acc+dec<=N（三角；禁止只截断 dec） */ - if (accel_pulses + decel_pulses <= total_pulses) /* acc+dec装得下？ */ + if (accel_pulses + decel_pulses <= total_pulses) { - plan->const_pulses = total_pulses - accel_pulses - decel_pulses; /* 够装：剩下的给匀速（梯形） */ + plan->const_pulses = total_pulses - accel_pulses - decel_pulses; } else { - plan->const_pulses = 0U; /* 不够：砍匀速，三角波 */ - peak_freq_hz = PlsrAccelCurveFitTargetFreq(total_pulses, start_freq_hz, target_freq_hz, end_freq_hz, /* 二分找能塞下的最高峰 */ + plan->const_pulses = 0U; + peak_freq_hz = PlsrAccelCurveFitTargetFreq(total_pulses, start_freq_hz, target_freq_hz, end_freq_hz, accel_rate_hz_per_s, decel_rate_hz_per_s, selected_mode); - PlsrAccelCurveCalcPhasePulses(start_freq_hz, peak_freq_hz, end_freq_hz, /* 按降峰后的峰重算 acc/dec */ + PlsrAccelCurveCalcPhasePulses(start_freq_hz, peak_freq_hz, end_freq_hz, accel_rate_hz_per_s, decel_rate_hz_per_s, selected_mode, &accel_pulses, &decel_pulses); } @@ -266,26 +250,26 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, * --- 补丁：峰值 > 出口 但 dec 算成 0 --- * 强制留 1 脉冲收尾，从 acc 或 const 挪给 dec */ - if ((peak_freq_hz > end_freq_hz) && (total_pulses > 1U) && (decel_pulses == 0U)) /* 该减速却算出0拍 */ + if ((peak_freq_hz > end_freq_hz) && (total_pulses > 1U) && (decel_pulses == 0U)) { - decel_pulses = 1U; /* 硬塞1拍减速 */ - if (accel_pulses >= total_pulses) /* 加速占满了？ */ + decel_pulses = 1U; + if (accel_pulses >= total_pulses) { - accel_pulses = total_pulses - 1U; /* 从加速挪1拍给减速 */ + accel_pulses = total_pulses - 1U; } - if (plan->const_pulses > 0U) /* 匀速还有余量？ */ + if (plan->const_pulses > 0U) { - plan->const_pulses--; /* 匀速减1给减速 */ + plan->const_pulses--; } else { - plan->const_pulses = total_pulses - accel_pulses - decel_pulses; /* 没匀速就重算余量 */ + plan->const_pulses = total_pulses - accel_pulses - decel_pulses; } } - plan->target_freq_hz = peak_freq_hz; /* 可能低于请求目标 */ - plan->accel_pulses = accel_pulses; /* 写回加速拍数 */ - plan->decel_pulses = decel_pulses; /* 写回减速拍数 */ + plan->target_freq_hz = peak_freq_hz; + plan->accel_pulses = accel_pulses; + plan->decel_pulses = decel_pulses; } /** @@ -295,35 +279,34 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, uint32_t completed_seg_pulses) { - uint32_t const_phase_end; /* 匀速段结束边界 */ - uint32_t pulse_count; /* 减速相内第几拍 */ - uint32_t freq_hz; /* 算出来的频率 */ - uint8_t freq_rising; /* 1=往上爬频，0=往下掉 */ + uint32_t const_phase_end; + uint32_t pulse_count; + uint32_t freq_hz; + uint8_t freq_rising; - if (plan == (PlsrAccelPlan_t *)0) /* 空plan返回0 */ + if (plan == (PlsrAccelPlan_t *)0) { - return 0U; /* 没plan没法取频 */ + return 0U; } - const_phase_end = plan->accel_pulses + plan->const_pulses; /* 匀速段末边界 */ + const_phase_end = plan->accel_pulses + plan->const_pulses; - /* 加速相：completed < accel 且起峰不同频 */ - if ((plan->accel_pulses > 0U) && (completed_seg_pulses < plan->accel_pulses) && /* 还在加速相 */ - (plan->start_freq_hz != plan->target_freq_hz)) /* 起峰不同才爬 */ + if ((plan->accel_pulses > 0U) && (completed_seg_pulses < plan->accel_pulses) && + (plan->start_freq_hz != plan->target_freq_hz)) { - freq_rising = (plan->target_freq_hz >= plan->start_freq_hz) ? 1U : 0U; /* 看峰比起高还是低 */ - if (plan->curve_mode == PLSR_ACCEL_LINEAR) /* 直线走v^2公式 */ + freq_rising = (plan->target_freq_hz >= plan->start_freq_hz) ? 1U : 0U; + if (plan->curve_mode == PLSR_ACCEL_LINEAR) { - /* 升用 accel_rate，降用 decel_rate */ - uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; /* 挑斜率 */ + + uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; freq_hz = PlsrAccelCurveLinearFreqAtPulse(plan->start_freq_hz, selected_rate_hz_per_s, - completed_seg_pulses, freq_rising, plan->target_freq_hz); /* 按脉冲序号用平方关系取频 */ + completed_seg_pulses, freq_rising, plan->target_freq_hz); } else { - /* S/正弦：按脉冲序号积分时间再取频 */ - uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; /* 挑斜率 */ + + uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; freq_hz = PlsrAccelCurveTimedFreqAtPulse( plan->start_freq_hz, @@ -332,42 +315,40 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, plan->curve_mode, completed_seg_pulses); } - if ((freq_hz < 1U) && (plan->target_freq_hz >= 1U)) /* 算出0但目标有效？ */ + if ((freq_hz < 1U) && (plan->target_freq_hz >= 1U)) { - freq_hz = 1U; /* 避免 0Hz 写 ARR */ + freq_hz = 1U; } return PlsrAccelCurveClampFreqHz(freq_hz); } - /* 匀速：保持峰值 */ - if (completed_seg_pulses < const_phase_end) /* 还在匀速区间 */ + if (completed_seg_pulses < const_phase_end) { - return PlsrAccelCurveClampFreqHz(plan->target_freq_hz); /* 钉峰值 */ + return PlsrAccelCurveClampFreqHz(plan->target_freq_hz); } - /* 无减速或峰=出口：直接出口频 */ - if ((plan->decel_pulses == 0U) || (plan->target_freq_hz == plan->end_freq_hz)) /* 不用减速 */ + if ((plan->decel_pulses == 0U) || (plan->target_freq_hz == plan->end_freq_hz)) { - return PlsrAccelCurveClampFreqHz(plan->end_freq_hz); /* 直接出口 */ + return PlsrAccelCurveClampFreqHz(plan->end_freq_hz); } - pulse_count = completed_seg_pulses - const_phase_end; /* 减速相内序号 */ - if (pulse_count >= plan->decel_pulses) /* 减速拍用尽 */ + pulse_count = completed_seg_pulses - const_phase_end; + if (pulse_count >= plan->decel_pulses) { - return PlsrAccelCurveClampFreqHz(plan->end_freq_hz); /* 钉出口 */ + return PlsrAccelCurveClampFreqHz(plan->end_freq_hz); } - freq_rising = (plan->end_freq_hz >= plan->target_freq_hz) ? 1U : 0U; /* 减速相：看止比峰高还是低 */ - if (plan->curve_mode == PLSR_ACCEL_LINEAR) /* 直线 */ + freq_rising = (plan->end_freq_hz >= plan->target_freq_hz) ? 1U : 0U; + if (plan->curve_mode == PLSR_ACCEL_LINEAR) { - uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; /* 挑斜率 */ + uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; freq_hz = PlsrAccelCurveLinearFreqAtPulse(plan->target_freq_hz, selected_rate_hz_per_s, - pulse_count, freq_rising, plan->end_freq_hz); /* 从峰往止按直线取频 */ + pulse_count, freq_rising, plan->end_freq_hz); } - else /* S/正弦 */ + else { - uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; /* 挑斜率 */ + uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; freq_hz = PlsrAccelCurveTimedFreqAtPulse( plan->target_freq_hz, @@ -376,9 +357,9 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, plan->curve_mode, pulse_count); } - if (freq_hz < 1U) /* 兜底别出0Hz */ + if (freq_hz < 1U) { - freq_hz = 1U; /* 钉成1Hz */ + freq_hz = 1U; } return PlsrAccelCurveClampFreqHz(freq_hz); } @@ -389,47 +370,45 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, */ void PlsrAccelPrebuildFreqTables(const PlsrAccelPlan_t *plan) { - PlsrAccelRuntime_t temporary_runtime; /* 临时跑一相，用来填表 */ + PlsrAccelRuntime_t temporary_runtime; - s_accel_table_length = 0U; /* 清加速表长度 */ - s_decel_table_length = 0U; /* 清减速表长度 */ - s_accel_table_stride = 1U; /* 加速步长先按1 */ - s_decel_table_stride = 1U; /* 减速步长先按1 */ + s_accel_table_length = 0U; + s_decel_table_length = 0U; + s_accel_table_stride = 1U; + s_decel_table_stride = 1U; - if ((plan == (const PlsrAccelPlan_t *)0) || /* 没plan或 */ - (plan->curve_mode == PLSR_ACCEL_LINEAR)) /* 直线模式 */ + if ((plan == (const PlsrAccelPlan_t *)0) || + (plan->curve_mode == PLSR_ACCEL_LINEAR)) { - return; /* 直线 ISR 走 NextFreqLinear，无需表 */ + return; } - /* 加速表：起 → 峰 */ - if ((plan->accel_pulses > 0U) && (plan->start_freq_hz != plan->target_freq_hz)) /* 有加速且起峰不同 */ + if ((plan->accel_pulses > 0U) && (plan->start_freq_hz != plan->target_freq_hz)) { - PlsrAccelRuntimeBeginRamp(&temporary_runtime, /* 开临时加速相 */ - plan->start_freq_hz, /* 起→峰 */ - plan->target_freq_hz, /* 到峰 */ - plan->accel_pulses, /* 加速总拍数 */ - plan->accel_rate_hz_per_s, /* 加速斜率 */ - plan->decel_rate_hz_per_s, /* 减速斜率也带上（方向里头会选） */ - plan->curve_mode); /* 曲线模式 */ - s_accel_table_length = PlsrAccelRuntimeBuildFreqTable(&temporary_runtime, s_accel_freq_table, /* 仿真整相写进加速表 */ - PLSR_RAMP_TBL_MAX, /* 表容量上限 */ - &s_accel_table_stride); /* 抽样步长写回 */ + PlsrAccelRuntimeBeginRamp(&temporary_runtime, + plan->start_freq_hz, + plan->target_freq_hz, + plan->accel_pulses, + plan->accel_rate_hz_per_s, + plan->decel_rate_hz_per_s, + plan->curve_mode); + s_accel_table_length = PlsrAccelRuntimeBuildFreqTable(&temporary_runtime, s_accel_freq_table, + PLSR_RAMP_TBL_MAX, + &s_accel_table_stride); } - /* 减速表：峰 → 止 */ - if ((plan->decel_pulses > 0U) && (plan->target_freq_hz != plan->end_freq_hz)) /* 有减速且峰止不同 */ + if ((plan->decel_pulses > 0U) && (plan->target_freq_hz != plan->end_freq_hz)) { - PlsrAccelRuntimeBeginRamp(&temporary_runtime, /* 开临时减速相 */ - plan->target_freq_hz, /* 峰→止 */ - plan->end_freq_hz, /* 到止 */ - plan->decel_pulses, /* 减速总拍数 */ - plan->accel_rate_hz_per_s, /* 斜率 */ - plan->decel_rate_hz_per_s, /* 斜率 */ - plan->curve_mode); /* 模式 */ - s_decel_table_length = PlsrAccelRuntimeBuildFreqTable(&temporary_runtime, s_decel_freq_table, /* 仿真写进减速表 */ - PLSR_RAMP_TBL_MAX, /* 容量 */ - &s_decel_table_stride); /* 步长写回 */ + PlsrAccelRuntimeBeginRamp(&temporary_runtime, + plan->target_freq_hz, + plan->end_freq_hz, + plan->decel_pulses, + plan->accel_rate_hz_per_s, + plan->decel_rate_hz_per_s, + plan->curve_mode); + s_decel_table_length = PlsrAccelRuntimeBuildFreqTable(&temporary_runtime, s_decel_freq_table, + PLSR_RAMP_TBL_MAX, + &s_decel_table_stride); } } @@ -437,67 +416,67 @@ void PlsrAccelPrebuildFreqTables(const PlsrAccelPlan_t *plan) void PlsrAccelBeginAccel(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan) { - if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) /* 指针检查 */ - { - return; /* 缺谁都不开相 */ - } - /*计算加速或者减速的时间*/ - PlsrAccelRuntimeBeginRamp(runtime, /* 填运行态：起→峰 */ - plan->start_freq_hz, /* 入口频 */ - plan->target_freq_hz, /* 峰值 */ - plan->accel_pulses, /* 加速拍预算 */ - plan->accel_rate_hz_per_s, /* 加速斜率 */ - plan->decel_rate_hz_per_s, /* 减速斜率 */ - plan->curve_mode); /* 曲线模式 */ - /*选择频率表*/ - PlsrAccelRuntimeSelectFreqTable(runtime, 1U, s_accel_table_length, /* 绑加速预建表 id=1 */ - s_accel_table_stride); /* 步长一起绑 */ + if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + { + return; + } + + PlsrAccelRuntimeBeginRamp(runtime, + plan->start_freq_hz, + plan->target_freq_hz, + plan->accel_pulses, + plan->accel_rate_hz_per_s, + plan->decel_rate_hz_per_s, + plan->curve_mode); + + PlsrAccelRuntimeSelectFreqTable(runtime, 1U, s_accel_table_length, + s_accel_table_stride); } /** @brief 进入减速相 */ void PlsrAccelBeginDecel(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan) { - if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) /* 指针检查 */ - { - return; /* 缺谁都不开相 */ - } - PlsrAccelRuntimeBeginRamp(runtime, /* 填运行态：峰→止 */ - plan->target_freq_hz, /* 从峰 */ - plan->end_freq_hz, /* 到止 */ - plan->decel_pulses, /* 减速拍预算 */ - plan->accel_rate_hz_per_s, /* 斜率 */ - plan->decel_rate_hz_per_s, /* 斜率 */ - plan->curve_mode); /* 模式 */ - /* 表已在 PlanSeg/Prebuild 填好；ISR 内禁止再建表 */ - PlsrAccelRuntimeSelectFreqTable(runtime, 2U, s_decel_table_length, /* 绑减速预建表 id=2 */ - s_decel_table_stride); /* 步长一起绑 */ + if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + { + return; + } + PlsrAccelRuntimeBeginRamp(runtime, + plan->target_freq_hz, + plan->end_freq_hz, + plan->decel_pulses, + plan->accel_rate_hz_per_s, + plan->decel_rate_hz_per_s, + plan->curve_mode); + + PlsrAccelRuntimeSelectFreqTable(runtime, 2U, s_decel_table_length, + s_decel_table_stride); } /** @brief 进入匀速相 */ void PlsrAccelBeginConstSpeed(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan) { - if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) /* 指针检查 */ - { - return; /* 缺谁都不开相 */ - } - runtime->start_freq_hz = plan->target_freq_hz; /* 起点钉峰值 */ - runtime->end_freq_hz = plan->target_freq_hz; /* 终点也是峰值 */ - runtime->cur_freq_hz = plan->target_freq_hz; /* 当前频=峰 */ - runtime->accel_hz_per_s = 0U; /* 斜率清零：不改频 */ - runtime->completed_pulses = 0U; /* 匀速相计数清零 */ - runtime->total_pulses = plan->const_pulses; /* 匀速总拍数 */ - runtime->freq_rising = 1U; /* 方向随便填，反正不爬 */ - runtime->curve_mode = plan->curve_mode; /* 模式照抄 */ - runtime->is_active = 0U; /* 匀速：ISR 不改频 */ - runtime->freq_table_id = 0U; /* 不用频率表 */ - runtime->table_length = 0U; /* 表长0 */ - runtime->table_stride = 1U; /* 步长无关 */ - runtime->elapsed_time_us = 0U; /* 时间清零 */ - runtime->jerk_time_us = 0U; /* 加加速度阶段时间清零 */ - runtime->const_accel_time_us = 0U; /* 恒加速时间清零 */ - runtime->ramp_time_us = 0U; /* 斜坡总时间清零 */ + if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + { + return; + } + runtime->start_freq_hz = plan->target_freq_hz; + runtime->end_freq_hz = plan->target_freq_hz; + runtime->cur_freq_hz = plan->target_freq_hz; + runtime->accel_hz_per_s = 0U; + runtime->completed_pulses = 0U; + runtime->total_pulses = plan->const_pulses; + runtime->freq_rising = 1U; + runtime->curve_mode = plan->curve_mode; + runtime->is_active = 0U; + runtime->freq_table_id = 0U; + runtime->table_length = 0U; + runtime->table_stride = 1U; + runtime->elapsed_time_us = 0U; + runtime->jerk_time_us = 0U; + runtime->const_accel_time_us = 0U; + runtime->ramp_time_us = 0U; } /** @@ -506,184 +485,178 @@ void PlsrAccelBeginConstSpeed(PlsrAccelRuntime_t *runtime, */ uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime) { - const uint32_t *freq_table; /* 指向加速表或减速表 */ - uint32_t table_index; /* 表下标 */ + const uint32_t *freq_table; + uint32_t table_index; - if (runtime == (PlsrAccelRuntime_t *)0) /* 空指针 */ + if (runtime == (PlsrAccelRuntime_t *)0) { - return 0U; /* 没runtime返回0 */ + return 0U; } - if (runtime->is_active == 0U) /* 相已停 */ + if (runtime->is_active == 0U) { - return PlsrAccelCurveClampFreqHz(runtime->cur_freq_hz); /* 已停相：保持 */ + return PlsrAccelCurveClampFreqHz(runtime->cur_freq_hz); } - if (runtime->completed_pulses < 0xFFFFFFFFUL) /* 防溢出再加拍 */ + if (runtime->completed_pulses < 0xFFFFFFFFUL) { - runtime->completed_pulses++; /* 本相已完成拍数+1 */ + runtime->completed_pulses++; } - if (runtime->curve_mode == PLSR_ACCEL_LINEAR) /* 直线模式 */ + if (runtime->curve_mode == PLSR_ACCEL_LINEAR) { - if (runtime->accel_hz_per_s == 0U) /* 斜率为0 */ + if (runtime->accel_hz_per_s == 0U) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* a=0：阶跃到终点 */ + runtime->cur_freq_hz = runtime->end_freq_hz; } else { - runtime->cur_freq_hz = PlsrAccelNextFreqLinear(runtime); /* 用平方目标±1逼近下一频 */ + runtime->cur_freq_hz = PlsrAccelNextFreqLinear(runtime); } } - else if (runtime->freq_table_id != 0U) /* 有预建表 */ + else if (runtime->freq_table_id != 0U) { - /* ISR 里只查表，不做 S/正弦实时积分（算太慢） */ - if (runtime->table_length < 1U) /* 表空了？ */ + + if (runtime->table_length < 1U) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 直接钉终点 */ + runtime->cur_freq_hz = runtime->end_freq_hz; } else { - uint32_t table_stride = (runtime->table_stride < 1U) ? /* 步长至少1 */ - 1U : runtime->table_stride; /* 防0步长 */ + uint32_t table_stride = (runtime->table_stride < 1U) ? + 1U : runtime->table_stride; - table_index = (runtime->completed_pulses - 1U) / table_stride; /* completed从1起，除步长得下标 */ - if (table_index >= runtime->table_length) /* 越界？ */ + table_index = (runtime->completed_pulses - 1U) / table_stride; + if (table_index >= runtime->table_length) { - table_index = runtime->table_length - 1U; /* 夹到最后一项 */ + table_index = runtime->table_length - 1U; } - freq_table = (runtime->freq_table_id == 1U) ? /* 1=加速表，其它=减速表 */ - s_accel_freq_table : /* 加速频率表 */ - s_decel_freq_table; /* 减速频率表 */ - runtime->cur_freq_hz = freq_table[table_index]; /* 查表得本拍频率 */ + freq_table = (runtime->freq_table_id == 1U) ? + s_accel_freq_table : + s_decel_freq_table; + runtime->cur_freq_hz = freq_table[table_index]; } } else { - /* 建表失败兜底：仍走时间步进 */ - runtime->cur_freq_hz = PlsrAccelNextFreqTimed(runtime); /* 时间域取下一频 */ + + runtime->cur_freq_hz = PlsrAccelNextFreqTimed(runtime); } - /* 本相脉冲用尽：钉终点并停相 */ - if ((runtime->total_pulses > 0U) && (runtime->completed_pulses >= runtime->total_pulses)) /* 拍数用尽？ */ + if ((runtime->total_pulses > 0U) && (runtime->completed_pulses >= runtime->total_pulses)) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉到本相终点频 */ - runtime->is_active = 0U; /* 停相 */ + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; } - else if (runtime->curve_mode == PLSR_ACCEL_LINEAR) /* 直线未用尽 */ + else if (runtime->curve_mode == PLSR_ACCEL_LINEAR) { /* * 未到本相最后一拍：不要钳到 end。 * 否则减速在 n=N-1 就落到止速，ARPE 下会表现为末两拍同频。 */ - if (runtime->freq_rising == 0U) /* 往下掉频 */ + if (runtime->freq_rising == 0U) { - if ((runtime->cur_freq_hz <= runtime->end_freq_hz) && /* 已经碰到或越过终点 */ - (runtime->end_freq_hz < 100000U)) /* 终点还没顶到100k */ + if ((runtime->cur_freq_hz <= runtime->end_freq_hz) && + (runtime->end_freq_hz < 100000U)) { - runtime->cur_freq_hz = runtime->end_freq_hz + 1U; /* 故意高1Hz，末拍再落地 */ + runtime->cur_freq_hz = runtime->end_freq_hz + 1U; } } - else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && /* 往上爬且已到/过终点 */ - (runtime->end_freq_hz > 1U) && /* 终点不是1 */ - (runtime->total_pulses > 0U) && /* 还有总拍数 */ - (runtime->completed_pulses < runtime->total_pulses)) /* 还没到最后一拍 */ + else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && + (runtime->end_freq_hz > 1U) && + (runtime->total_pulses > 0U) && + (runtime->completed_pulses < runtime->total_pulses)) { - runtime->cur_freq_hz = runtime->end_freq_hz - 1U; /* 故意低1Hz，别提前钉死 */ + runtime->cur_freq_hz = runtime->end_freq_hz - 1U; } } - else if (runtime->freq_table_id != 0U) /* 查表路径同样处理 */ + else if (runtime->freq_table_id != 0U) { - /* 查表：最后一拍之前若已是 end，同样让出 1Hz */ - if ((runtime->total_pulses > 0U) && /* 还没跑完本相 */ - (runtime->completed_pulses < runtime->total_pulses)) /* 未到最后一拍 */ + + if ((runtime->total_pulses > 0U) && + (runtime->completed_pulses < runtime->total_pulses)) { - if (runtime->freq_rising == 0U) /* 往下掉 */ + if (runtime->freq_rising == 0U) { - if ((runtime->cur_freq_hz <= runtime->end_freq_hz) && /* 已经<=终点 */ - (runtime->end_freq_hz < 100000U)) /* 终点未顶格 */ + if ((runtime->cur_freq_hz <= runtime->end_freq_hz) && + (runtime->end_freq_hz < 100000U)) { - runtime->cur_freq_hz = runtime->end_freq_hz + 1U; /* 让出1Hz */ + runtime->cur_freq_hz = runtime->end_freq_hz + 1U; } } - else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && /* 往上爬已>=终点 */ - (runtime->end_freq_hz > 1U)) /* 终点有效 */ + else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && + (runtime->end_freq_hz > 1U)) { - runtime->cur_freq_hz = runtime->end_freq_hz - 1U; /* 让出1Hz */ + runtime->cur_freq_hz = runtime->end_freq_hz - 1U; } } } - else if (runtime->freq_table_id == 0U) /* 时间步进兜底 */ + else if (runtime->freq_table_id == 0U) { - /* 时间步进路径：到时或越过终点则停 */ - if ((runtime->ramp_time_us > 0U) && (runtime->elapsed_time_us >= runtime->ramp_time_us)) /* 时间到了斜坡总时间 */ + + if ((runtime->ramp_time_us > 0U) && (runtime->elapsed_time_us >= runtime->ramp_time_us)) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉终点 */ - runtime->is_active = 0U; /* 停相 */ + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; } - else if (runtime->freq_rising != 0U) /* 往上爬 */ + else if (runtime->freq_rising != 0U) { - if (runtime->cur_freq_hz >= runtime->end_freq_hz) /* 频已到终点 */ + if (runtime->cur_freq_hz >= runtime->end_freq_hz) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉终点 */ - runtime->is_active = 0U; /* 停相 */ + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; } } - else if (runtime->cur_freq_hz <= runtime->end_freq_hz) /* 往下掉且已到/过终点 */ + else if (runtime->cur_freq_hz <= runtime->end_freq_hz) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉终点 */ - runtime->is_active = 0U; /* 停相 */ + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; } } - return PlsrAccelCurveClampFreqHz(runtime->cur_freq_hz); /* 最后钳一次再返回 */ + return PlsrAccelCurveClampFreqHz(runtime->cur_freq_hz); } -/*============================================================================*/ -/* 工具 */ -/*============================================================================*/ - -/* 频率大于 100k 时暂时钳到 100k；启动门禁另报 0x04 */ static uint32_t PlsrAccelCurveClampFreqHz(uint32_t freq_hz) { - if (freq_hz > 100000U) /* 超100k就砍 */ + if (freq_hz > 100000U) { - freq_hz = 100000U; /* 钉100k */ + freq_hz = 100000U; } - return freq_hz; /* 返回钳后值 */ + return freq_hz; } /** @brief 两个无符号数之间的距离 */ static uint32_t PlsrAccelCurveAbsDiff(uint32_t first_val, uint32_t second_val) { - return (first_val >= second_val) ? /* 大减小，保证非负差 */ - (first_val - second_val) : (second_val - first_val); /* 谁大谁当被减数 */ + return (first_val >= second_val) ? + (first_val - second_val) : (second_val - first_val); } /** 64 位整数平方根（牛顿法），支持 freq_hz^2 + 2an */ static uint32_t PlsrAccelCurveIntSqrt(uint64_t squared_freq) { - uint64_t estimate; /* 当前估计 */ - uint64_t next_estimate; /* 下一步估计 */ - uint32_t loop_index; /* 迭代次数 */ + uint64_t estimate; + uint64_t next_estimate; + uint32_t loop_index; - if (squared_freq <= 1ULL) /* 0或1开方就是自己 */ + if (squared_freq <= 1ULL) { - return (uint32_t)squared_freq; /* 直接回 */ + return (uint32_t)squared_freq; } - estimate = squared_freq; /* 初值先用原数 */ - if (estimate > 100000ULL) /* 太大就从100k起迭代 */ + estimate = squared_freq; + if (estimate > 100000ULL) { - estimate = 100000ULL; /* 缩小初值 */ + estimate = 100000ULL; } - for (loop_index = 0U; loop_index < 40U; loop_index++) /* 牛顿迭代最多40轮 */ + for (loop_index = 0U; loop_index < 40U; loop_index++) { - next_estimate = (estimate + squared_freq / estimate) / 2ULL; /* estimate = (x + n/x)/2 */ - if (next_estimate >= estimate) /* 不再变小就停 */ + next_estimate = (estimate + squared_freq / estimate) / 2ULL; + if (next_estimate >= estimate) { - break; /* 收敛了 */ + break; } - estimate = next_estimate; /* 继续收紧 */ + estimate = next_estimate; } - return (uint32_t)estimate; /* 整数部分开方结果 */ + return (uint32_t)estimate; } /** @@ -692,15 +665,15 @@ static uint32_t PlsrAccelCurveIntSqrt(uint64_t squared_freq) */ static uint32_t PlsrAccelCurveRateFromDefaultSpeed(uint32_t default_speed_hz, uint32_t ramp_time_ms) { - if (ramp_time_ms == 0U) /* 时间为0：斜率0（阶跃） */ + if (ramp_time_ms == 0U) { - return 0U; /* a=0 */ + return 0U; } - if (default_speed_hz == 0U) /* 默认速度没配 */ + if (default_speed_hz == 0U) { - return 0U; /* 也当0 */ + return 0U; } - return (default_speed_hz * 1000UL) / ramp_time_ms; /* a=默认速度*1000/斜坡毫秒，单位Hz/s */ + return (default_speed_hz * 1000UL) / ramp_time_ms; } /** 按默认速度计算变化率；默认速度未配置时用本段频差计算。 */ @@ -709,22 +682,22 @@ static uint32_t PlsrAccelCurveCalcRate(uint32_t default_speed_hz, uint32_t end_freq_hz, uint32_t ramp_time_ms) { - uint32_t rate_hz_per_s; /* 算出来的斜率 */ - uint32_t freq_diff_hz; /* 本段频差 */ + uint32_t rate_hz_per_s; + uint32_t freq_diff_hz; - rate_hz_per_s = PlsrAccelCurveRateFromDefaultSpeed(default_speed_hz, ramp_time_ms); /* 优先用默认速度那套 */ - if ((rate_hz_per_s != 0U) || (ramp_time_ms == 0U)) /* 有斜率，或时间本来就是0 */ + rate_hz_per_s = PlsrAccelCurveRateFromDefaultSpeed(default_speed_hz, ramp_time_ms); + if ((rate_hz_per_s != 0U) || (ramp_time_ms == 0U)) { - return rate_hz_per_s; /* 直接用 */ + return rate_hz_per_s; } freq_diff_hz = - PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); /* 没默认速度：改用|起-止| */ - if (freq_diff_hz == 0U) /* 同频？ */ + PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); + if (freq_diff_hz == 0U) { - return 0U; /* 没差就0斜率 */ + return 0U; } - return (freq_diff_hz * 1000UL) / ramp_time_ms; /* a=频差*1000/毫秒 */ + return (freq_diff_hz * 1000UL) / ramp_time_ms; } /** @@ -733,20 +706,20 @@ static uint32_t PlsrAccelCurveCalcRate(uint32_t default_speed_hz, */ static uint32_t PlsrAccelCurveCalcJumpFreq(uint32_t starting_freq_hz, uint32_t accel_hz_per_s) { - uint64_t squared_freq; /* 频率平方累加量 */ - uint32_t freq_hz; /* 开方后的频率 */ + uint64_t squared_freq; + uint32_t freq_hz; - if (accel_hz_per_s == 0U) /* a=0：不起跳 */ + if (accel_hz_per_s == 0U) { - return (starting_freq_hz >= 1U) ? PlsrAccelCurveClampFreqHz(starting_freq_hz) : 1U; /* 有起点用起点，否则1Hz */ + return (starting_freq_hz >= 1U) ? PlsrAccelCurveClampFreqHz(starting_freq_hz) : 1U; } - squared_freq = (uint64_t)starting_freq_hz * (uint64_t)starting_freq_hz + (2ULL * (uint64_t)accel_hz_per_s); /* f^2 = f0^2 + 2a（脉冲域等价） */ - freq_hz = PlsrAccelCurveIntSqrt(squared_freq); /* 开方得起跳频 */ - if (freq_hz < 1U) /* 别出0 */ + squared_freq = (uint64_t)starting_freq_hz * (uint64_t)starting_freq_hz + (2ULL * (uint64_t)accel_hz_per_s); + freq_hz = PlsrAccelCurveIntSqrt(squared_freq); + if (freq_hz < 1U) { - freq_hz = 1U; /* 至少1Hz */ + freq_hz = 1U; } - return PlsrAccelCurveClampFreqHz(freq_hz); /* 再钳100k */ + return PlsrAccelCurveClampFreqHz(freq_hz); } /** @@ -766,45 +739,42 @@ static uint32_t PlsrAccelCurveResolveBoundaryFreq(uint32_t configured_freq_hz, uint32_t decel_time_ms, uint8_t resolving_start) { - uint32_t ramp_time_ms; /* 用哪侧斜坡时间 */ - uint32_t accel_hz_per_s; /* 峰值加速度对应的斜率 */ - uint32_t jump_freq_hz; /* 起跳频率 */ + uint32_t ramp_time_ms; + uint32_t accel_hz_per_s; + uint32_t jump_freq_hz; - /* 验证频率是否超过100k */ - configured_freq_hz = PlsrAccelCurveClampFreqHz(configured_freq_hz); /* 配置值钳住 */ - target_freq_hz = PlsrAccelCurveClampFreqHz(target_freq_hz); /* 目标钳住 */ - if (target_freq_hz < 1U) /* 目标无效 */ + configured_freq_hz = PlsrAccelCurveClampFreqHz(configured_freq_hz); + target_freq_hz = PlsrAccelCurveClampFreqHz(target_freq_hz); + if (target_freq_hz < 1U) { - return 1U; /* 至少给1Hz出口/入口 */ + return 1U; } - /* 配置起/止 > 目标：直接用配置（高位进/出） */ - if (configured_freq_hz > target_freq_hz) /* 配置比目标还高 */ + if (configured_freq_hz > target_freq_hz) { - return configured_freq_hz; /* 高位进/出，不用起跳 */ + return configured_freq_hz; } - ramp_time_ms = (resolving_start != 0U) ? accel_time_ms : decel_time_ms; /* 起用加速时间，止用减速时间 */ - /* a 与 Plan 同式，保证起跳与斜坡一致 */ + ramp_time_ms = (resolving_start != 0U) ? accel_time_ms : decel_time_ms; + accel_hz_per_s = - PlsrAccelCurveCalcRate(default_speed_hz, /* 按同Plan公式算a */ - configured_freq_hz, /* 从配置频 */ - target_freq_hz, /* 到目标 */ - ramp_time_ms); /* 斜坡毫秒 */ - if (accel_hz_per_s == 0U) /* a=0 */ + PlsrAccelCurveCalcRate(default_speed_hz, + configured_freq_hz, + target_freq_hz, + ramp_time_ms); + if (accel_hz_per_s == 0U) { - return (configured_freq_hz >= 1U) ? configured_freq_hz : target_freq_hz; /* 有配置用配置，否则用目标 */ + return (configured_freq_hz >= 1U) ? configured_freq_hz : target_freq_hz; } /* jump = sqrt(f0^2 + 2a)；配置为 0 时即 sqrt(2a) */ - jump_freq_hz = PlsrAccelCurveCalcJumpFreq(configured_freq_hz, accel_hz_per_s); /* 算起跳：sqrt(f0^2+2a) */ + jump_freq_hz = PlsrAccelCurveCalcJumpFreq(configured_freq_hz, accel_hz_per_s); - /* 目标 < 起跳 → 用目标；否则用起跳 */ - if (target_freq_hz < jump_freq_hz) /* 目标比起跳还低 */ + if (target_freq_hz < jump_freq_hz) { - return target_freq_hz; /* 没必要从起跳起，用目标 */ + return target_freq_hz; } - return jump_freq_hz; /* 否则用起跳频 */ + return jump_freq_hz; } /** 千分比线性插值：ratio=0→start，ratio≥1000→end */ @@ -812,22 +782,18 @@ static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, uint32_t end_val, uint32_t ratio_permille) { - if (ratio_permille >= 1000U) /* 进度>=1000 */ + if (ratio_permille >= 1000U) { - return end_val; /* 直接终点 */ + return end_val; } - if (end_val >= start_val) /* 终点>=起点：往上插 */ + if (end_val >= start_val) { - return start_val + /* start + 差*千分比 */ - ((end_val - start_val) * ratio_permille) / 1000UL; /* 按千分比摊 */ + return start_val + + ((end_val - start_val) * ratio_permille) / 1000UL; } - return start_val - /* 往下插 */ - ((start_val - end_val) * ratio_permille) / 1000UL; /* start - 差*千分比 */ -} /* 按千分比摊 */ - -/*============================================================================*/ -/* 直线 */ -/*============================================================================*/ + return start_val - + ((start_val - end_val) * ratio_permille) / 1000UL; +} /** * 脉冲闭合：N = ceil(|end² - start²| / (2a))。 @@ -836,36 +802,36 @@ static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, static uint32_t PlsrAccelCurveCalcLinearRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s) { - uint64_t start_freq_squared; /* 起点频率平方 */ - uint64_t end_freq_squared; /* 终点频率平方 */ - uint64_t squared_freq_diff; /* |f1^2-f0^2| */ - uint64_t twice_accel; /* 2a */ - uint64_t pulse_count; /* 要几拍 */ + uint64_t start_freq_squared; + uint64_t end_freq_squared; + uint64_t squared_freq_diff; + uint64_t twice_accel; + uint64_t pulse_count; - if (start_freq_hz == end_freq_hz) /* 同频：0拍 */ + if (start_freq_hz == end_freq_hz) { - return 0U; /* 不用爬 */ + return 0U; } - if (accel_hz_per_s == 0U) /* a=0 */ + if (accel_hz_per_s == 0U) { - return 0U; /* 阶跃 */ + return 0U; } - start_freq_squared = (uint64_t)start_freq_hz * (uint64_t)start_freq_hz; /* f0^2 */ - end_freq_squared = (uint64_t)end_freq_hz * (uint64_t)end_freq_hz; /* f1^2 */ - squared_freq_diff = (end_freq_squared > start_freq_squared) ? (end_freq_squared - start_freq_squared) : (start_freq_squared - end_freq_squared); /* 平方差取绝对值 */ - twice_accel = 2ULL * (uint64_t)accel_hz_per_s; /* 分母2a */ - /* ceil(Δ(f²) / (2a)) */ - pulse_count = (squared_freq_diff + twice_accel - 1ULL) / twice_accel; /* 向上取整：差多少平方要除2a */ - if (pulse_count == 0ULL) /* 算成0？ */ + start_freq_squared = (uint64_t)start_freq_hz * (uint64_t)start_freq_hz; + end_freq_squared = (uint64_t)end_freq_hz * (uint64_t)end_freq_hz; + squared_freq_diff = (end_freq_squared > start_freq_squared) ? (end_freq_squared - start_freq_squared) : (start_freq_squared - end_freq_squared); + twice_accel = 2ULL * (uint64_t)accel_hz_per_s; + + pulse_count = (squared_freq_diff + twice_accel - 1ULL) / twice_accel; + if (pulse_count == 0ULL) { - pulse_count = 1ULL; /* 至少1拍 */ - } /* 超32位？ */ + pulse_count = 1ULL; + } if (pulse_count > 0xFFFFFFFFULL) - { /* 钉满 */ + { return 0xFFFFFFFFUL; } - return (uint32_t)pulse_count; /* 回拍数 */ + return (uint32_t)pulse_count; } /** 直线：freq_hz = sqrt(start_freq_hz^2 + 2*a*pulse_count)，升到/降到不超过 target_limit_hz 方向 */ @@ -873,51 +839,51 @@ static uint32_t PlsrAccelCurveLinearFreqAtPulse(uint32_t start_freq_hz, uint32_t uint32_t pulse_count, uint8_t freq_rising, uint32_t target_limit_hz) { - uint64_t squared_freq; /* 当前频率平方 */ - uint32_t freq_hz; /* 算得频率 */ + uint64_t squared_freq; + uint32_t freq_hz; - if (pulse_count == 0U) /* 第0拍：还在起点 */ + if (pulse_count == 0U) { - freq_hz = start_freq_hz; /* 频率=起点 */ + freq_hz = start_freq_hz; } - else if (accel_hz_per_s == 0U) /* a=0：一步到位 */ + else if (accel_hz_per_s == 0U) { - freq_hz = target_limit_hz; /* 直接目标限 */ + freq_hz = target_limit_hz; } else { - squared_freq = (uint64_t)start_freq_hz * (uint64_t)start_freq_hz; /* 先放f0^2 */ - if (freq_rising != 0U) /* 往上爬 */ + squared_freq = (uint64_t)start_freq_hz * (uint64_t)start_freq_hz; + if (freq_rising != 0U) { - squared_freq += 2ULL * (uint64_t)accel_hz_per_s * (uint64_t)pulse_count; /* f^2 = f0^2 + 2*a*n */ - freq_hz = PlsrAccelCurveIntSqrt(squared_freq); /* 开方 */ - if (freq_hz > target_limit_hz) /* 别超过上限 */ + squared_freq += 2ULL * (uint64_t)accel_hz_per_s * (uint64_t)pulse_count; + freq_hz = PlsrAccelCurveIntSqrt(squared_freq); + if (freq_hz > target_limit_hz) { - freq_hz = target_limit_hz; /* 钳到目标限 */ + freq_hz = target_limit_hz; } } - else /* 往下掉 */ + else { - uint64_t freq_drop = 2ULL * (uint64_t)accel_hz_per_s * (uint64_t)pulse_count; /* 要减掉的平方量 2*a*n */ - if (freq_drop >= squared_freq) /* 减穿了？ */ + uint64_t freq_drop = 2ULL * (uint64_t)accel_hz_per_s * (uint64_t)pulse_count; + if (freq_drop >= squared_freq) { - freq_hz = target_limit_hz; /* 落到目标限 */ + freq_hz = target_limit_hz; } else { - freq_hz = PlsrAccelCurveIntSqrt(squared_freq - freq_drop); /* f = sqrt(f0^2 - 2an) */ - if (freq_hz < target_limit_hz) /* 别低于下限 */ + freq_hz = PlsrAccelCurveIntSqrt(squared_freq - freq_drop); + if (freq_hz < target_limit_hz) { - freq_hz = target_limit_hz; /* 钳到目标限 */ + freq_hz = target_limit_hz; } } } } - if (freq_hz < 1U) /* 别出0 */ + if (freq_hz < 1U) { - freq_hz = 1U; /* 至少1 */ + freq_hz = 1U; } - return PlsrAccelCurveClampFreqHz(freq_hz); /* 钳100k返回 */ + return PlsrAccelCurveClampFreqHz(freq_hz); } /** @@ -926,114 +892,107 @@ static uint32_t PlsrAccelCurveLinearFreqAtPulse(uint32_t start_freq_hz, uint32_t */ static uint32_t PlsrAccelNextFreqLinear(PlsrAccelRuntime_t *runtime) { - uint64_t target_freq_squared; /* 本拍目标f^2 */ - uint64_t cur_freq_squared; /* 试探频的平方 */ - uint64_t start_freq_squared; /* 起点平方 */ - uint32_t freq_hz; /* 试探频率 */ - uint32_t iteration_guard; /* 循环保险丝，防死转 */ + uint64_t target_freq_squared; + uint64_t cur_freq_squared; + uint64_t start_freq_squared; + uint32_t freq_hz; + uint32_t iteration_guard; - /* 本拍应逼近的频率平方 */ - start_freq_squared = (uint64_t)runtime->start_freq_hz * (uint64_t)runtime->start_freq_hz; /* 起点f^2 */ - if (runtime->freq_rising != 0U) /* 往上爬 */ + start_freq_squared = (uint64_t)runtime->start_freq_hz * (uint64_t)runtime->start_freq_hz; + if (runtime->freq_rising != 0U) { - target_freq_squared = start_freq_squared + (2ULL * (uint64_t)runtime->accel_hz_per_s * (uint64_t)runtime->completed_pulses); /* 目标f^2 = f0^2 + 2*a*n */ + target_freq_squared = start_freq_squared + (2ULL * (uint64_t)runtime->accel_hz_per_s * (uint64_t)runtime->completed_pulses); } - else /* 往下掉 */ + else { - uint64_t freq_drop = 2ULL * (uint64_t)runtime->accel_hz_per_s * (uint64_t)runtime->completed_pulses; /* 要减的平方量 */ - target_freq_squared = (freq_drop >= start_freq_squared) ? 0ULL : (start_freq_squared - freq_drop); /* 减穿就目标0，否则f0^2-2an */ + uint64_t freq_drop = 2ULL * (uint64_t)runtime->accel_hz_per_s * (uint64_t)runtime->completed_pulses; + target_freq_squared = (freq_drop >= start_freq_squared) ? 0ULL : (start_freq_squared - freq_drop); } - freq_hz = runtime->cur_freq_hz; /* 从上一拍频率起步 */ - if (freq_hz < 1U) /* 保护 */ + freq_hz = runtime->cur_freq_hz; + if (freq_hz < 1U) { - freq_hz = 1U; /* 至少1 */ + freq_hz = 1U; } - /* 从当前频逐步 ±1，直到最贴近 target_freq_squared */ - for (iteration_guard = 0U; iteration_guard < 2048U; iteration_guard++) /* 最多试2048次±1 */ + for (iteration_guard = 0U; iteration_guard < 2048U; iteration_guard++) { - cur_freq_squared = (uint64_t)freq_hz * (uint64_t)freq_hz; /* 当前试探的f^2 */ - if (runtime->freq_rising != 0U) /* 升频分支 */ + cur_freq_squared = (uint64_t)freq_hz * (uint64_t)freq_hz; + if (runtime->freq_rising != 0U) { - if ((freq_hz < runtime->end_freq_hz) && (((uint64_t)(freq_hz + 1U) * (uint64_t)(freq_hz + 1U)) <= target_freq_squared)) /* 还能往上且(f+1)^2还没超过目标 */ + if ((freq_hz < runtime->end_freq_hz) && (((uint64_t)(freq_hz + 1U) * (uint64_t)(freq_hz + 1U)) <= target_freq_squared)) { - freq_hz++; /* 频+1 */ - continue; /* 再试 */ + freq_hz++; + continue; } - if ((freq_hz > 1U) && (cur_freq_squared > target_freq_squared)) /* 偏高了往回退 */ + if ((freq_hz > 1U) && (cur_freq_squared > target_freq_squared)) { - freq_hz--; /* 频-1 */ - continue; /* 再试 */ + freq_hz--; + continue; } - break; /* 贴住了，收工 */ + break; } - else /* 降频分支 */ + else { - if ((freq_hz > runtime->end_freq_hz) && (cur_freq_squared > target_freq_squared)) /* 还能往下且当前平方还大于目标 */ + if ((freq_hz > runtime->end_freq_hz) && (cur_freq_squared > target_freq_squared)) { - freq_hz--; /* 频-1 */ - continue; /* 再试 */ + freq_hz--; + continue; } - if ((freq_hz < 100000U) && /* 偏低了往上补 */ - (((uint64_t)(freq_hz + 1U) * (uint64_t)(freq_hz + 1U)) < target_freq_squared)) /* (f+1)^2仍小于目标 */ + if ((freq_hz < 100000U) && + (((uint64_t)(freq_hz + 1U) * (uint64_t)(freq_hz + 1U)) < target_freq_squared)) { - freq_hz++; /* 频+1 */ - continue; /* 再试 */ + freq_hz++; + continue; } - break; /* 贴住了 */ + break; } } - /* 不超过本相终点 */ - if (runtime->freq_rising != 0U) /* 升频：别超过终点 */ + if (runtime->freq_rising != 0U) { - if (freq_hz > runtime->end_freq_hz) /* 超了 */ + if (freq_hz > runtime->end_freq_hz) { - freq_hz = runtime->end_freq_hz; /* 钉终点 */ + freq_hz = runtime->end_freq_hz; } } - else /* 降频：别低于终点 */ + else { - if (freq_hz < runtime->end_freq_hz) /* 低了 */ + if (freq_hz < runtime->end_freq_hz) { - freq_hz = runtime->end_freq_hz; /* 钉终点 */ + freq_hz = runtime->end_freq_hz; } } - if (freq_hz < 1U) /* 保护 */ + if (freq_hz < 1U) { - freq_hz = 1U; /* 至少1 */ + freq_hz = 1U; } - return PlsrAccelCurveClampFreqHz(freq_hz); /* 钳后返回 */ + return PlsrAccelCurveClampFreqHz(freq_hz); } -/*============================================================================*/ -/* S / 正弦 */ -/*============================================================================*/ - /** raised-cosine 形状：进度千分比 → 形状千分比（查 33 点表再插值） */ static uint32_t PlsrAccelCurveShapeSinePermille(uint32_t progress_permille) { - uint32_t table_index; /* 表下标 */ - uint32_t seg_progress; /* 段内进度 */ - uint32_t min_val; /* 段左端形状值 */ - uint32_t max_val; /* 段右端形状值 */ + uint32_t table_index; + uint32_t seg_progress; + uint32_t min_val; + uint32_t max_val; - if (progress_permille >= 1000U) /* 进度满了 */ + if (progress_permille >= 1000U) { - return 1000U; /* 形状也满1000 */ + return 1000U; } - /* progress/1000 → 表下标 0..32，相邻点线性插值 */ - table_index = (progress_permille * 32UL) / 1000UL; /* 映射到0..32格 */ - if (table_index >= 32U) /* 末格？ */ + + table_index = (progress_permille * 32UL) / 1000UL; + if (table_index >= 32U) { - return 1000U; /* 直接1000 */ + return 1000U; } - seg_progress = (progress_permille * 32UL) - (table_index * 1000UL); /* 0..999 对应段内进度 */ - min_val = (uint32_t)s_plsr_sine_shape_tab[table_index]; /* 左点 */ - max_val = (uint32_t)s_plsr_sine_shape_tab[table_index + 1U]; /* 右点 */ - return min_val + /* 两点之间按段内进度插 */ - (((max_val - min_val) * seg_progress) / 1000UL); /* 线性摊 */ + seg_progress = (progress_permille * 32UL) - (table_index * 1000UL); + min_val = (uint32_t)s_plsr_sine_shape_tab[table_index]; + max_val = (uint32_t)s_plsr_sine_shape_tab[table_index + 1U]; + return min_val + + (((max_val - min_val) * seg_progress) / 1000UL); } /** @@ -1046,83 +1005,83 @@ static void PlsrAccelCurveCalcSCurveTimes(uint32_t start_freq_hz, uint32_t end_f uint32_t *const_accel_time_us_out, uint32_t *ramp_time_us_out) { - uint32_t freq_diff_hz; /* 起止频差 */ - uint64_t numerator; /* 分子：频差*1e6 */ - uint64_t denominator; /* 分母：3*峰值加速度 */ - uint32_t calc_jerk_time_us; /* 加加速度阶段时间（微秒） */ + uint32_t freq_diff_hz; + uint64_t numerator; + uint64_t denominator; + uint32_t calc_jerk_time_us; - *jerk_time_us_out = 0U; /* 先清零输出 */ - *const_accel_time_us_out = 0U; /* 恒加速阶段清零 */ - *ramp_time_us_out = 0U; /* 斜坡总时间清零 */ + *jerk_time_us_out = 0U; + *const_accel_time_us_out = 0U; + *ramp_time_us_out = 0U; - freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); /* 算|Δf| */ - if ((freq_diff_hz < 1U) || (accel_hz_per_s < 1U)) /* 没差或没加速度 */ + freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); + if ((freq_diff_hz < 1U) || (accel_hz_per_s < 1U)) { - return; /* 时间全0返回 */ + return; } /* Tj_us = ceil(freq_diff_hz * 1e6 / (3A)) */ - numerator = (uint64_t)freq_diff_hz * 1000000ULL; /* 分子=频差*1e6（换成微秒） */ - denominator = 3ULL * (uint64_t)accel_hz_per_s; /* 分母=3*峰值加速度A */ - calc_jerk_time_us = /* Tj=频差/(3*峰值加速度)，向上取整，单位微秒 */ - (uint32_t)((numerator + denominator - 1ULL) / denominator); /* ceil除法 */ - if (calc_jerk_time_us < 1U) /* 太小？ */ + numerator = (uint64_t)freq_diff_hz * 1000000ULL; + denominator = 3ULL * (uint64_t)accel_hz_per_s; + calc_jerk_time_us = + (uint32_t)((numerator + denominator - 1ULL) / denominator); + if (calc_jerk_time_us < 1U) { - calc_jerk_time_us = 1U; /* 至少1微秒 */ + calc_jerk_time_us = 1U; } - *jerk_time_us_out = calc_jerk_time_us; /* 写出加加速度阶段时间Tj */ - *const_accel_time_us_out = calc_jerk_time_us * 2U; /* Ta=2*Tj，恒加速段 */ - *ramp_time_us_out = calc_jerk_time_us * 4U; /* 斜坡总时间T=4*Tj（1+2+1） */ + *jerk_time_us_out = calc_jerk_time_us; + *const_accel_time_us_out = calc_jerk_time_us * 2U; + *ramp_time_us_out = calc_jerk_time_us * 4U; } /** 正弦 raised-cosine：峰值加速度 = A ⇒ T = π·Δf / (2A) */ static void PlsrAccelCurveCalcSineCurveTime(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s, uint32_t *ramp_time_us) { - uint32_t freq_diff_hz; /* 起止频差 */ - uint64_t numerator; /* 分子 */ - uint64_t denominator; /* 分母 */ + uint32_t freq_diff_hz; + uint64_t numerator; + uint64_t denominator; - *ramp_time_us = 0U; /* 先清斜坡总时间 */ - freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); /* 算|Δf| */ - if ((freq_diff_hz < 1U) || (accel_hz_per_s < 1U)) /* 无效就返回 */ + *ramp_time_us = 0U; + freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); + if ((freq_diff_hz < 1U) || (accel_hz_per_s < 1U)) { - return; /* 不填时间 */ + return; } - /* T_us = ceil(π*freq_diff_hz*1e6 / (2A))，π≈355/113 */ - numerator = (uint64_t)freq_diff_hz * 1000000ULL * 355ULL; /* 分子=Δf*1e6*355（π≈355/113） */ - denominator = 2ULL * (uint64_t)accel_hz_per_s * 113ULL; /* 分母=2*A*113 */ - *ramp_time_us = (uint32_t)((numerator + denominator - 1ULL) / denominator); /* 斜坡总时间T=π*频差/(2*峰值加速度)，向上取整，单位微秒 */ - if (*ramp_time_us < 1U) /* 太小？ */ + + numerator = (uint64_t)freq_diff_hz * 1000000ULL * 355ULL; + denominator = 2ULL * (uint64_t)accel_hz_per_s * 113ULL; + *ramp_time_us = (uint32_t)((numerator + denominator - 1ULL) / denominator); + if (*ramp_time_us < 1U) { - *ramp_time_us = 1U; /* 至少1微秒 */ + *ramp_time_us = 1U; } } /** Δf = A * elapsed_time_us / 1e6 */ static uint32_t PlsrAccelCurveDeltaHzFromAt(uint32_t accel_hz_per_s, uint32_t elapsed_time_us) { - return (uint32_t)(((uint64_t)accel_hz_per_s * (uint64_t)elapsed_time_us) / 1000000ULL); /* Δf=A*t/1e6，匀加速走过的频差 */ + return (uint32_t)(((uint64_t)accel_hz_per_s * (uint64_t)elapsed_time_us) / 1000000ULL); } /** Δf = A * elapsed_time_us^2 / (2 * jerk_time_us * 1e6) — jerk 段 */ static uint32_t PlsrAccelCurveDeltaHzFromJerk(uint32_t accel_hz_per_s, uint32_t elapsed_time_us, uint32_t jerk_time_us) { - uint64_t calc; /* 中间累乘 */ + uint64_t calc; - if ((jerk_time_us < 1U) || (elapsed_time_us < 1U) || (accel_hz_per_s < 1U)) /* 缺参数 */ + if ((jerk_time_us < 1U) || (elapsed_time_us < 1U) || (accel_hz_per_s < 1U)) { - return 0U; /* 变不出频差 */ + return 0U; } /* ((A * t / jerk_time_us) * t) / 2e6 */ - calc = ((uint64_t)accel_hz_per_s * (uint64_t)elapsed_time_us) / (uint64_t)jerk_time_us; /* 先 A*t/Tj */ - calc = (calc * (uint64_t)elapsed_time_us) / 2000000ULL; /* 再 *t / 2e6 → ½*(A/Tj)*t² */ - if (calc > 0xFFFFFFFFULL) /* 超32位？ */ + calc = ((uint64_t)accel_hz_per_s * (uint64_t)elapsed_time_us) / (uint64_t)jerk_time_us; + calc = (calc * (uint64_t)elapsed_time_us) / 2000000ULL; + if (calc > 0xFFFFFFFFULL) { - return 0xFFFFFFFFUL; /* 钉满 */ + return 0xFFFFFFFFUL; } - return (uint32_t)calc; /* 这段加加速度爬出的频差 */ + return (uint32_t)calc; } /** @@ -1133,76 +1092,76 @@ static uint32_t PlsrAccelCurveSCurveFreqAtTime(uint32_t start_freq_hz, uint32_t uint32_t jerk_time_us, uint32_t const_accel_time_us, uint32_t ramp_time_us, uint32_t elapsed_time_us) { - uint32_t freq_diff_hz; /* 总频差 */ - uint32_t freq_change_hz; /* 此刻已变的频差 */ - uint32_t decel_start_time_us; /* 末段开始时刻 */ - uint32_t decel_elapsed_us; /* 末段里走了多久 */ - uint8_t freq_rising; /* 升还是降 */ + uint32_t freq_diff_hz; + uint32_t freq_change_hz; + uint32_t decel_start_time_us; + uint32_t decel_elapsed_us; + uint8_t freq_rising; - freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); /* 算总|Δf| */ - freq_rising = (end_freq_hz >= start_freq_hz) ? 1U : 0U; /* 看方向 */ + freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); + freq_rising = (end_freq_hz >= start_freq_hz) ? 1U : 0U; - if ((freq_diff_hz < 1U) || (ramp_time_us < 1U) || (accel_hz_per_s < 1U)) /* 参数不齐 */ + if ((freq_diff_hz < 1U) || (ramp_time_us < 1U) || (accel_hz_per_s < 1U)) { - return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 直接终点 */ + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - if (elapsed_time_us >= ramp_time_us) /* 时间到了 */ + if (elapsed_time_us >= ramp_time_us) { - return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 钉终点 */ + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - if ((jerk_time_us < 1U) || (elapsed_time_us <= jerk_time_us)) /* 还在第一段：加速度从0爬到最大 */ + if ((jerk_time_us < 1U) || (elapsed_time_us <= jerk_time_us)) { - /* jerk 升：Δf = 0.5*(A/Tj)*t^2 */ - freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, elapsed_time_us, (jerk_time_us < 1U) ? 1U : jerk_time_us); /* Δf=½*(A/Tj)*t² */ + + freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, elapsed_time_us, (jerk_time_us < 1U) ? 1U : jerk_time_us); } else { - decel_start_time_us = jerk_time_us + const_accel_time_us; /* 恒加速段结束= Tj+Ta */ - if (elapsed_time_us <= decel_start_time_us) /* 还在恒加速 */ + decel_start_time_us = jerk_time_us + const_accel_time_us; + if (elapsed_time_us <= decel_start_time_us) { - /* 恒加速：Δf = 0.5*A*Tj + A*(t-Tj) */ - freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, jerk_time_us, jerk_time_us); /* 先加上第一段贡献 ½*A*Tj */ - freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, elapsed_time_us - jerk_time_us); /* 再加上 A*(t-Tj) */ + + freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, jerk_time_us, jerk_time_us); + freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, elapsed_time_us - jerk_time_us); } else { /* jerk 降：Δf = 0.5*A*Tj + A*Ta + A*τ - 0.5*(A/Tj)*τ^2 */ - decel_elapsed_us = elapsed_time_us - decel_start_time_us; /* 末段相对时间τ */ - if (decel_elapsed_us > jerk_time_us) /* τ别超Tj */ + decel_elapsed_us = elapsed_time_us - decel_start_time_us; + if (decel_elapsed_us > jerk_time_us) { - decel_elapsed_us = jerk_time_us; /* 夹到Tj */ + decel_elapsed_us = jerk_time_us; } - freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, jerk_time_us, jerk_time_us); /* 前面两段累计 */ - freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, const_accel_time_us); /* 加上恒加速整段 */ - freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, decel_elapsed_us); /* 末段先当匀加速加上A*τ */ + freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, jerk_time_us, jerk_time_us); + freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, const_accel_time_us); + freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, decel_elapsed_us); { - uint32_t freq_drop = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, decel_elapsed_us, jerk_time_us); /* 再扣回½*(A/Tj)*τ²（加速度往0收） */ - if (freq_change_hz > freq_drop) /* 够扣？ */ + uint32_t freq_drop = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, decel_elapsed_us, jerk_time_us); + if (freq_change_hz > freq_drop) { - freq_change_hz -= freq_drop; /* 扣掉 */ + freq_change_hz -= freq_drop; } else { - freq_change_hz = 0U; /* 不够就清零 */ + freq_change_hz = 0U; } } } } if (freq_change_hz > freq_diff_hz) - { /* 别超过总频差 */ - freq_change_hz = freq_diff_hz; /* 钳到总频差 */ + { + freq_change_hz = freq_diff_hz; } - if (freq_rising != 0U) /* 升频：起点+变化 */ + if (freq_rising != 0U) { - return PlsrAccelCurveClampFreqHz(start_freq_hz + freq_change_hz); /* 钳后返回 */ + return PlsrAccelCurveClampFreqHz(start_freq_hz + freq_change_hz); } - if (start_freq_hz > freq_change_hz) /* 降频且扣得动 */ + if (start_freq_hz > freq_change_hz) { - return PlsrAccelCurveClampFreqHz(start_freq_hz - freq_change_hz); /* 起点-变化 */ + return PlsrAccelCurveClampFreqHz(start_freq_hz - freq_change_hz); } - return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 扣穿了就终点 */ + return PlsrAccelCurveClampFreqHz(end_freq_hz); } /** @@ -1212,30 +1171,30 @@ static uint32_t PlsrAccelCurveSCurveFreqAtTime(uint32_t start_freq_hz, uint32_t static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t ramp_time_us, uint32_t elapsed_time_us) { - uint32_t progress_permille; /* 时间进度千分比 */ - uint32_t ratio_permille; /* 形状千分比 */ - uint32_t freq_hz; /* 插值后频率 */ + uint32_t progress_permille; + uint32_t ratio_permille; + uint32_t freq_hz; - if ((ramp_time_us < 1U) || (start_freq_hz == end_freq_hz)) /* 没时间或同频 */ + if ((ramp_time_us < 1U) || (start_freq_hz == end_freq_hz)) { - return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 直接终点 */ + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - if (elapsed_time_us >= ramp_time_us) /* 时间走完 */ + if (elapsed_time_us >= ramp_time_us) { - return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 钉终点 */ + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - progress_permille = (uint32_t)(((uint64_t)elapsed_time_us * 1000ULL) / (uint64_t)ramp_time_us); /* p=t/T */ - if (progress_permille > 1000U) /* 进度超了？ */ + progress_permille = (uint32_t)(((uint64_t)elapsed_time_us * 1000ULL) / (uint64_t)ramp_time_us); + if (progress_permille > 1000U) { - progress_permille = 1000U; /* 夹到1000 */ + progress_permille = 1000U; } - ratio_permille = PlsrAccelCurveShapeSinePermille(progress_permille); /* 半个余弦形状：进度→形状 */ - freq_hz = PlsrAccelCurveInterpolate(start_freq_hz, end_freq_hz, ratio_permille); /* 按形状在起止间插频 */ - if (freq_hz < 1U) /* 保护 */ + ratio_permille = PlsrAccelCurveShapeSinePermille(progress_permille); + freq_hz = PlsrAccelCurveInterpolate(start_freq_hz, end_freq_hz, ratio_permille); + if (freq_hz < 1U) { - freq_hz = 1U; /* 至少1 */ + freq_hz = 1U; } - return PlsrAccelCurveClampFreqHz(freq_hz); /* 钳后返回 */ + return PlsrAccelCurveClampFreqHz(freq_hz); } /** 按 S/正弦时间曲线，从起点逐步积分到第 completed_pulses 拍的频率 */ @@ -1246,61 +1205,59 @@ static uint32_t PlsrAccelCurveTimedFreqAtPulse( PlsrAccelMode_e curve_mode, uint32_t completed_pulses) { - uint32_t jerk_time_us = 0U; /* 加加速度阶段时间 */ - uint32_t const_accel_time_us = 0U; /* 恒加速时间 */ - uint32_t ramp_time_us = 0U; /* 斜坡总时间 */ - uint32_t elapsed_time_us = 0U; /* 累计已过微秒 */ - uint32_t cur_freq_hz = start_freq_hz; /* 从起点频开始积 */ - uint32_t pulse_index; /* 拍循环下标 */ - - /* 先算整段斜坡时间参数 */ - if (curve_mode == PLSR_ACCEL_S) /* S曲线 */ - { - PlsrAccelCurveCalcSCurveTimes(start_freq_hz, /* 起点频率 */ - end_freq_hz, /* 终点频率 */ - rate_hz_per_s, /* 峰值加速度 A */ - &jerk_time_us, /* 出：加速度爬升阶段时间 Tj */ - &const_accel_time_us, /* 出：恒加速阶段时间 Ta */ - &ramp_time_us); /* 出：斜坡总时间 T=4*Tj */ + uint32_t jerk_time_us = 0U; + uint32_t const_accel_time_us = 0U; + uint32_t ramp_time_us = 0U; + uint32_t elapsed_time_us = 0U; + uint32_t cur_freq_hz = start_freq_hz; + uint32_t pulse_index; + + if (curve_mode == PLSR_ACCEL_S) + { + PlsrAccelCurveCalcSCurveTimes(start_freq_hz, + end_freq_hz, + rate_hz_per_s, + &jerk_time_us, + &const_accel_time_us, + &ramp_time_us); } else { - PlsrAccelCurveCalcSineCurveTime(start_freq_hz, /* 起点频率 */ - end_freq_hz, /* 终点频率 */ - rate_hz_per_s, /* 峰值加速度 A */ - &ramp_time_us); /* 出：斜坡总时间 T */ + PlsrAccelCurveCalcSineCurveTime(start_freq_hz, + end_freq_hz, + rate_hz_per_s, + &ramp_time_us); } - /* 每拍用上一拍周期推进时间，再按 t 取频 */ - for (pulse_index = 0U; pulse_index < completed_pulses; pulse_index++) /* 一拍一拍积到目标拍数 */ + for (pulse_index = 0U; pulse_index < completed_pulses; pulse_index++) { - if (cur_freq_hz < 1U) /* 保护当前频 */ + if (cur_freq_hz < 1U) { - cur_freq_hz = 1U; /* 至少1 */ + cur_freq_hz = 1U; } - elapsed_time_us += 1000000UL / cur_freq_hz; /* Δt = 1/f（µs） */ + elapsed_time_us += 1000000UL / cur_freq_hz; - if (curve_mode == PLSR_ACCEL_S) /* S：按累计时间取频 */ + if (curve_mode == PLSR_ACCEL_S) { cur_freq_hz = - PlsrAccelCurveSCurveFreqAtTime(start_freq_hz, /* 起点频率 */ - end_freq_hz, /* 终点频率 */ - rate_hz_per_s, /* 峰值加速度 A */ - jerk_time_us, /* Tj */ - const_accel_time_us, /* Ta */ - ramp_time_us, /* 斜坡总时间 T */ - elapsed_time_us); /* 已经过了多少微秒 */ + PlsrAccelCurveSCurveFreqAtTime(start_freq_hz, + end_freq_hz, + rate_hz_per_s, + jerk_time_us, + const_accel_time_us, + ramp_time_us, + elapsed_time_us); } else { cur_freq_hz = - PlsrAccelCurveSineFreqAtTime(start_freq_hz, /* 起点频率 */ - end_freq_hz, /* 终点频率 */ - ramp_time_us, /* 斜坡总时间 T */ - elapsed_time_us); /* 已经过了多少微秒 */ + PlsrAccelCurveSineFreqAtTime(start_freq_hz, + end_freq_hz, + ramp_time_us, + elapsed_time_us); } } - return cur_freq_hz; /* 积完返回该拍频率 */ + return cur_freq_hz; } /** @@ -1311,42 +1268,39 @@ static uint32_t PlsrAccelCurveSimulateRampPulses(uint32_t start_freq_hz, uint32_ uint32_t accel_hz_per_s, PlsrAccelMode_e curve_mode) { - PlsrAccelRuntime_t runtime; /* 临时运行态 */ - uint32_t iteration_guard; /* 循环保险丝 */ + PlsrAccelRuntime_t runtime; + uint32_t iteration_guard; - /* 同频或 a=0：无斜坡脉冲 */ - if (start_freq_hz == end_freq_hz || accel_hz_per_s == 0U) /* 同频或a=0 */ + if (start_freq_hz == end_freq_hz || accel_hz_per_s == 0U) { - return 0U; /* 0拍斜坡 */ + return 0U; } - /* 开相：total 填满，靠 is_active 自然停 */ - PlsrAccelRuntimeBeginRamp(&runtime, /* 开相仿真 */ - start_freq_hz, /* 起始频率 */ - end_freq_hz, /* 终止频率 */ - 0xFFFFFFFFUL, /* 不限脉冲上限 */ - accel_hz_per_s, /* 斜率 */ - accel_hz_per_s, /* 同斜率（单相） */ - curve_mode); /* 模式 */ - /* 斜坡时间无效时，至少算 1 脉冲 */ - if (runtime.ramp_time_us < 1U) + PlsrAccelRuntimeBeginRamp(&runtime, + start_freq_hz, + end_freq_hz, + 0xFFFFFFFFUL, + accel_hz_per_s, + accel_hz_per_s, + curve_mode); + + if (runtime.ramp_time_us < 1U) { - return 1U; /* 至少当1拍 */ + return 1U; } - iteration_guard = 0U; /* 计数清零 */ - while ((runtime.is_active != 0U) && (iteration_guard < 5000000U)) /* 还在跑且没到上限 */ + iteration_guard = 0U; + while ((runtime.is_active != 0U) && (iteration_guard < 5000000U)) { - iteration_guard++; /* 计一轮 */ - PlsrAccelRuntimeSimulateOnePulse(&runtime); /* 时间步进一拍 */ + iteration_guard++; + PlsrAccelRuntimeSimulateOnePulse(&runtime); } - /* 未走出任何脉冲时兜底为 1 */ - if (runtime.completed_pulses == 0U) /* 一拍都没走出？ */ + if (runtime.completed_pulses == 0U) { - return 1U; /* 兜底1 */ + return 1U; } - return runtime.completed_pulses; /* 仿真得到的拍数=加速从起点爬到终点要几拍 */ + return runtime.completed_pulses; } /** @@ -1354,64 +1308,58 @@ static uint32_t PlsrAccelCurveSimulateRampPulses(uint32_t start_freq_hz, uint32_ */ static uint32_t PlsrAccelNextFreqTimed(PlsrAccelRuntime_t *runtime) { - uint32_t previous_freq_hz; /* 上一拍频率 */ - uint32_t pulse_period_us; /* 上一拍周期微秒 */ - uint32_t freq_hz; /* 本拍算出的频 */ + uint32_t previous_freq_hz; + uint32_t pulse_period_us; + uint32_t freq_hz; - if ((runtime->ramp_time_us < 1U) || (runtime->accel_hz_per_s < 1U)) /* 没斜坡时间或没加速度 */ + if ((runtime->ramp_time_us < 1U) || (runtime->accel_hz_per_s < 1U)) { - return PlsrAccelCurveClampFreqHz(runtime->end_freq_hz); /* 直接终点 */ + return PlsrAccelCurveClampFreqHz(runtime->end_freq_hz); } - previous_freq_hz = runtime->cur_freq_hz; /* 记下上一频 */ - if (previous_freq_hz < 1U) + previous_freq_hz = runtime->cur_freq_hz; + if (previous_freq_hz < 1U) { - previous_freq_hz = 1U; /* 至少1Hz */ + previous_freq_hz = 1U; } - pulse_period_us = 1000000UL / previous_freq_hz; /* 上一拍周期 µs */ + pulse_period_us = 1000000UL / previous_freq_hz; - /* 累加时间，防溢出钉死上限 */ - if (runtime->elapsed_time_us < (0xFFFFFFFFUL - pulse_period_us)) /* 累加不会溢出？ */ + if (runtime->elapsed_time_us < (0xFFFFFFFFUL - pulse_period_us)) { - runtime->elapsed_time_us += pulse_period_us; /* 时间+=上一拍周期 */ + runtime->elapsed_time_us += pulse_period_us; } else { - runtime->elapsed_time_us = 0xFFFFFFFFUL; /* 溢了就钉死最大，最大4295秒 */ + runtime->elapsed_time_us = 0xFFFFFFFFUL; } - if (runtime->elapsed_time_us >= runtime->ramp_time_us) /* 到了斜坡总时间 */ + if (runtime->elapsed_time_us >= runtime->ramp_time_us) { - return PlsrAccelCurveClampFreqHz(runtime->end_freq_hz); /* 返回终点频 */ + return PlsrAccelCurveClampFreqHz(runtime->end_freq_hz); } - if (runtime->curve_mode == PLSR_ACCEL_S) /* S曲线 */ + if (runtime->curve_mode == PLSR_ACCEL_S) { - freq_hz = PlsrAccelCurveSCurveFreqAtTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, /* 按三段时间取频 */ - runtime->jerk_time_us, runtime->const_accel_time_us, runtime->ramp_time_us, /* 带上Tj/Ta/T和已过时间 */ - runtime->elapsed_time_us); /* 已过微秒 */ + freq_hz = PlsrAccelCurveSCurveFreqAtTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, + runtime->jerk_time_us, runtime->const_accel_time_us, runtime->ramp_time_us, + runtime->elapsed_time_us); } else { - freq_hz = PlsrAccelCurveSineFreqAtTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->ramp_time_us, runtime->elapsed_time_us); /* 正弦按时间取频 */ + freq_hz = PlsrAccelCurveSineFreqAtTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->ramp_time_us, runtime->elapsed_time_us); } - return freq_hz; /* 返回本拍频 */ + return freq_hz; } -/*============================================================================*/ -/* 规划 */ -/*============================================================================*/ - -/* 单段斜坡脉冲数：S/正弦仿真；直线闭合式 N=ceil(|f1²-f0²|/(2a)) */ static uint32_t PlsrAccelCurveCalcRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s, PlsrAccelMode_e curve_mode) { - if ((curve_mode == PLSR_ACCEL_SINE) || (curve_mode == PLSR_ACCEL_S)) /* S或正弦 */ + if ((curve_mode == PLSR_ACCEL_SINE) || (curve_mode == PLSR_ACCEL_S)) { - return PlsrAccelCurveSimulateRampPulses(start_freq_hz, end_freq_hz, accel_hz_per_s, curve_mode); /* 只能仿真出拍数 */ + return PlsrAccelCurveSimulateRampPulses(start_freq_hz, end_freq_hz, accel_hz_per_s, curve_mode); } - return PlsrAccelCurveCalcLinearRampPulses(start_freq_hz, end_freq_hz, accel_hz_per_s); /* 直线：用v^2闭合式直接算 */ + return PlsrAccelCurveCalcLinearRampPulses(start_freq_hz, end_freq_hz, accel_hz_per_s); } /** @@ -1426,78 +1374,76 @@ static uint32_t PlsrAccelCurveFitTargetFreq(uint32_t total_pulses, uint32_t decel_rate_hz_per_s, PlsrAccelMode_e curve_mode) { - uint32_t low; /* 二分左界：偏移量下界，从 0 起 */ - uint32_t high; /* 二分右界：偏移量上界 */ - uint32_t mid; /* 二分中点：当前试探的偏移量 */ - uint32_t best; /* 目前可行且最大的偏移量 */ - uint32_t peak_hz; /* 由 mid 算出的试探峰值 */ - uint32_t acc_n; /* 起→峰 要几拍 */ - uint32_t dec_n; /* 峰→止 要几拍 */ - uint32_t max_offset; /* 偏移上界 = |段表峰 - 起点| */ - uint8_t peak_up; /* 1=段表峰在起点之上，0=在之下 */ + uint32_t low; + uint32_t high; + uint32_t mid; + uint32_t best; + uint32_t peak_hz; + uint32_t acc_n; + uint32_t dec_n; + uint32_t max_offset; + uint8_t peak_up; - /* 段表峰相对起点是往上还是往下 */ - peak_up = (peak_tgt_hz >= start_freq_hz) ? 1U : 0U; /* 看段表峰比起高还是低 */ - if (peak_up != 0U) /* 往上爬 */ + peak_up = (peak_tgt_hz >= start_freq_hz) ? 1U : 0U; + if (peak_up != 0U) { - max_offset = peak_tgt_hz - start_freq_hz; /* 上界 = 段表峰 - 起点 */ + max_offset = peak_tgt_hz - start_freq_hz; } - else /* 往下掉 */ + else { - max_offset = start_freq_hz - peak_tgt_hz; /* 上界 = 起点 - 段表峰 */ + max_offset = start_freq_hz - peak_tgt_hz; } - low = 0U; /* 偏移从 0 开始试 */ - high = max_offset; /* 最多偏到段表峰 */ - best = 0U; /* 还没找到更好的 */ + low = 0U; + high = max_offset; + best = 0U; - while (low <= high) /* 标准二分 */ + while (low <= high) { - mid = low + ((high - low) / 2UL); /* 取中点偏移 */ + mid = low + ((high - low) / 2UL); - /* 起点 ± mid 得到本轮回试探的峰值 */ - if (peak_up != 0U) /* 往上：峰 = 起点 + 偏移 */ + if (peak_up != 0U) { - peak_hz = start_freq_hz + mid; /* 试探峰 */ - acc_n = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_hz, accel_rate_hz_per_s, curve_mode); /* 起→峰，用加速斜率 */ + peak_hz = start_freq_hz + mid; + acc_n = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_hz, accel_rate_hz_per_s, curve_mode); } - else /* 往下：峰 = 起点 - 偏移 */ + else { - peak_hz = start_freq_hz - mid; /* 试探峰 */ - acc_n = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_hz, decel_rate_hz_per_s, curve_mode); /* 起→峰往下，用减速斜率 */ + peak_hz = start_freq_hz - mid; + acc_n = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_hz, decel_rate_hz_per_s, curve_mode); } - /* 峰→止：看止速比峰高还是低，选对应斜率 */ + dec_n = PlsrAccelCurveCalcRampPulses(peak_hz, end_freq_hz, (end_freq_hz > peak_hz) ? accel_rate_hz_per_s : decel_rate_hz_per_s, curve_mode); - if ((acc_n + dec_n) <= total_pulses) /* 加减速拍数塞得进总脉冲？ */ + if ((acc_n + dec_n) <= total_pulses) { - best = mid; /* 这个偏移可行，先记下来 */ - if (mid == max_offset) /* 已经顶到段表峰了 */ + best = mid; + if (mid == max_offset) { - break; /* 没法再大了 */ + break; } - low = mid + 1UL; /* 往更大的偏移试 */ - if (low > high) /* 左右界碰头了 */ + low = mid + 1UL; + if (low > high) { - break; /* 搜完了 */ + break; } } - else /* 超预算了 */ + else { - if (mid == 0U) /* 偏移为 0 都塞不进 */ + if (mid == 0U) { - break; /* 没救，只能停在起点 */ + break; } - high = mid - 1UL; /* 缩小右界，试更小的峰 */ + high = mid - 1UL; } } - if (peak_up != 0U) /* 往上爬：返回起点 + 最好偏移 */ + if (peak_up != 0U) { - return start_freq_hz + best; /* 能到的最高峰 */ + return start_freq_hz + best; } - return start_freq_hz - best; /* 往下掉：返回起点 - 最好偏移 */ + return start_freq_hz - best; } /** @@ -1513,53 +1459,47 @@ static void PlsrAccelCurveCalcPhasePulses(uint32_t start_freq_hz, uint32_t *accel_pulses, uint32_t *decel_pulses) { - /* 起→峰 */ - if (start_freq_hz > peak_freq_hz) /* 起比峰高：这段其实在掉频 */ + + if (start_freq_hz > peak_freq_hz) { - *accel_pulses = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_freq_hz, decel_rate_hz_per_s, curve_mode); /* 用减速斜率算加速相拍数 */ + *accel_pulses = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_freq_hz, decel_rate_hz_per_s, curve_mode); } else { - *accel_pulses = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_freq_hz, accel_rate_hz_per_s, curve_mode); /* 正常往上爬：用加速斜率 */ + *accel_pulses = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_freq_hz, accel_rate_hz_per_s, curve_mode); } - /* 峰→止 */ - if (end_freq_hz > peak_freq_hz) /* 止比峰高：峰→止在往上 */ + + if (end_freq_hz > peak_freq_hz) { - *decel_pulses = PlsrAccelCurveCalcRampPulses(peak_freq_hz, end_freq_hz, accel_rate_hz_per_s, curve_mode); /* 用加速斜率 */ + *decel_pulses = PlsrAccelCurveCalcRampPulses(peak_freq_hz, end_freq_hz, accel_rate_hz_per_s, curve_mode); } else { - *decel_pulses = PlsrAccelCurveCalcRampPulses(peak_freq_hz, end_freq_hz, decel_rate_hz_per_s, curve_mode); /* 正常峰→止往下：用减速斜率 */ + *decel_pulses = PlsrAccelCurveCalcRampPulses(peak_freq_hz, end_freq_hz, decel_rate_hz_per_s, curve_mode); } } -/*============================================================================*/ -/* 运行态 */ -/*============================================================================*/ - -/* 按曲线模式预填 Tj/Ta/T（直线不填时间） */ static void PlsrAccelRuntimeCalcTimes(PlsrAccelRuntime_t *runtime) { - runtime->elapsed_time_us = 0U; /* 已过时间清零 */ - runtime->jerk_time_us = 0U; /* 加加速度阶段清零 */ - runtime->const_accel_time_us = 0U; /* 恒加速清零 */ - runtime->ramp_time_us = 0U; /* 斜坡总时间清零 */ + runtime->elapsed_time_us = 0U; + runtime->jerk_time_us = 0U; + runtime->const_accel_time_us = 0U; + runtime->ramp_time_us = 0U; - /* a=0 或同频：无斜坡时间 */ - if ((runtime->accel_hz_per_s < 1U) || (runtime->start_freq_hz == runtime->end_freq_hz)) /* 没斜率或同频 */ + if ((runtime->accel_hz_per_s < 1U) || (runtime->start_freq_hz == runtime->end_freq_hz)) { - return; /* 时间保持0 */ + return; } - if (runtime->curve_mode == PLSR_ACCEL_S) /* S模式 */ + if (runtime->curve_mode == PLSR_ACCEL_S) { - /* Tj:Ta:Tj=1:2:1 → T=4*Tj */ - PlsrAccelCurveCalcSCurveTimes(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, /* 填Tj/Ta/斜坡总时间 */ - &runtime->jerk_time_us, &runtime->const_accel_time_us, &runtime->ramp_time_us); /* 写进runtime */ + + PlsrAccelCurveCalcSCurveTimes(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, + &runtime->jerk_time_us, &runtime->const_accel_time_us, &runtime->ramp_time_us); } - else if (runtime->curve_mode == PLSR_ACCEL_SINE) /* 正弦模式 */ + else if (runtime->curve_mode == PLSR_ACCEL_SINE) { - /* T = π·Δf / (2A) */ - PlsrAccelCurveCalcSineCurveTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, &runtime->ramp_time_us); /* 只填斜坡总时间 */ + + PlsrAccelCurveCalcSineCurveTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, &runtime->ramp_time_us); } } @@ -1572,55 +1512,55 @@ static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, uint32_t decel_rate_hz_per_s, PlsrAccelMode_e curve_mode) { - runtime->start_freq_hz = start_freq_hz; /* 本相起点 */ - runtime->end_freq_hz = end_freq_hz; /* 本相终点 */ - runtime->cur_freq_hz = start_freq_hz; /* 当前从起点开跑 */ - runtime->completed_pulses = 0U; /* 拍计数清零 */ - runtime->total_pulses = total_pulses; /* 本相总拍数 */ + runtime->start_freq_hz = start_freq_hz; + runtime->end_freq_hz = end_freq_hz; + runtime->cur_freq_hz = start_freq_hz; + runtime->completed_pulses = 0U; + runtime->total_pulses = total_pulses; runtime->freq_rising = - (end_freq_hz >= start_freq_hz) ? 1U : 0U; /* 1=升频 */ - /* 升用加速斜率，降用减速斜率 */ - runtime->accel_hz_per_s = /* 本相实际用的斜率 */ + (end_freq_hz >= start_freq_hz) ? 1U : 0U; + + runtime->accel_hz_per_s = (runtime->freq_rising != 0U) ? - accel_rate_hz_per_s : decel_rate_hz_per_s; /* 升用accel，降用decel */ - runtime->curve_mode = curve_mode; /* 选好的斜率 */ - /* 有脉冲且起止不同才激活 */ + accel_rate_hz_per_s : decel_rate_hz_per_s; + runtime->curve_mode = curve_mode; + runtime->is_active = ((total_pulses > 0U) && - (start_freq_hz != end_freq_hz)) ? 1U : 0U; /* 有拍且起止不同才算活跃 */ - runtime->freq_table_id = 0U; /* 开相先不绑表；BeginAcc/Dec 再 Select */ - runtime->table_length = 0U; /* 表长清 */ - runtime->table_stride = 1U; /* 步长默认1 */ - PlsrAccelRuntimeCalcTimes(runtime); /* 按模式预填时间参数 */ + (start_freq_hz != end_freq_hz)) ? 1U : 0U; + runtime->freq_table_id = 0U; + runtime->table_length = 0U; + runtime->table_stride = 1U; + PlsrAccelRuntimeCalcTimes(runtime); } /** 离散仿真一步：与建表 / 脉冲预算共用（时间步进） */ static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime) { - if (runtime->completed_pulses < 0xFFFFFFFFUL) /* 防溢出 */ + if (runtime->completed_pulses < 0xFFFFFFFFUL) { - runtime->completed_pulses++; /* 仿真拍+1 */ + runtime->completed_pulses++; } - runtime->cur_freq_hz = PlsrAccelNextFreqTimed(runtime); /* 时间域推进一拍取频 */ - /* 到时或越过终点 → 钉终点并停相 */ - if ((runtime->ramp_time_us > 0U) && (runtime->elapsed_time_us >= runtime->ramp_time_us)) /* 时间到斜坡总时间？ */ + runtime->cur_freq_hz = PlsrAccelNextFreqTimed(runtime); + + if ((runtime->ramp_time_us > 0U) && (runtime->elapsed_time_us >= runtime->ramp_time_us)) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉终点 */ - runtime->is_active = 0U; /* 停 */ + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; } - else if (runtime->freq_rising != 0U) /* 升频 */ + else if (runtime->freq_rising != 0U) { - if (runtime->cur_freq_hz >= runtime->end_freq_hz) /* 已到/过终点 */ + if (runtime->cur_freq_hz >= runtime->end_freq_hz) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉终点 */ - runtime->is_active = 0U; /* 停 */ + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; } } else { - if (runtime->cur_freq_hz <= runtime->end_freq_hz) /* 降频已到/过终点 */ + if (runtime->cur_freq_hz <= runtime->end_freq_hz) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉终点 */ - runtime->is_active = 0U; /* 停 */ + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; } } } @@ -1634,81 +1574,78 @@ static uint32_t PlsrAccelRuntimeBuildFreqTable(PlsrAccelRuntime_t *runtime, uint32_t table_capacity, uint32_t *table_stride_out) { - PlsrAccelRuntime_t simulation; /* 独立仿真副本 */ - uint32_t loop_index; /* 脉冲循环 */ - uint32_t table_index; /* 写入下标 */ - uint32_t table_stride; /* 抽样步长 */ - uint32_t table_length; /* 有效表长 */ + PlsrAccelRuntime_t simulation; + uint32_t loop_index; + uint32_t table_index; + uint32_t table_stride; + uint32_t table_length; - *table_stride_out = 1U; /* 默认步长1 */ - if ((runtime->is_active == 0U) || (runtime->total_pulses < 1U) || /* 不活跃/无拍/直线/空表/无容量 */ - (runtime->curve_mode == PLSR_ACCEL_LINEAR) || (freq_table == (uint32_t *)0) || /* 直线不建表 */ - (table_capacity < 1U)) /* 条件不满足 */ + *table_stride_out = 1U; + if ((runtime->is_active == 0U) || (runtime->total_pulses < 1U) || + (runtime->curve_mode == PLSR_ACCEL_LINEAR) || (freq_table == (uint32_t *)0) || + (table_capacity < 1U)) { - return 0U; /* 直线或不活跃：不建表 */ + return 0U; } - /* 超容量则抽样，保证能塞进表 */ - table_stride = 1U; /* 先按1 */ - if (runtime->total_pulses > table_capacity) /* 脉冲比表容量多？ */ + table_stride = 1U; + if (runtime->total_pulses > table_capacity) { - table_stride = (runtime->total_pulses + table_capacity - 1U) / table_capacity; /* 步长=ceil(总拍/容量)，向上取整 */ + table_stride = (runtime->total_pulses + table_capacity - 1U) / table_capacity; } - *table_stride_out = table_stride; /* 告诉调用方步长 */ + *table_stride_out = table_stride; - /* 独立副本仿真，避免改坏调用方 runtime */ - simulation = *runtime; /* 拷一份runtime */ - simulation.completed_pulses = 0U; /* 仿真从0拍起 */ - simulation.cur_freq_hz = runtime->start_freq_hz; /* 频回到起点 */ - simulation.is_active = 1U; /* 强制活跃 */ - PlsrAccelRuntimeCalcTimes(&simulation); /* 重算时间参数 */ - if (simulation.ramp_time_us < 1U) /* 斜坡总时间无效 */ + simulation = *runtime; + simulation.completed_pulses = 0U; + simulation.cur_freq_hz = runtime->start_freq_hz; + simulation.is_active = 1U; + PlsrAccelRuntimeCalcTimes(&simulation); + if (simulation.ramp_time_us < 1U) { - freq_table[0] = runtime->end_freq_hz; /* 表里塞一个终点 */ - return 1U; /* 长度1 */ + freq_table[0] = runtime->end_freq_hz; + return 1U; } - table_length = 0U; /* 表长从0计 */ - for (loop_index = 0U; loop_index < runtime->total_pulses; loop_index++) /* 按总拍逐拍仿真 */ + table_length = 0U; + for (loop_index = 0U; loop_index < runtime->total_pulses; loop_index++) { - PlsrAccelRuntimeSimulateOnePulse(&simulation); /* 推一拍 */ - table_index = (simulation.completed_pulses - 1U) / table_stride; /* completed/stride → 下标 */ - if (table_index >= table_capacity) /* 越界夹住 */ + PlsrAccelRuntimeSimulateOnePulse(&simulation); + table_index = (simulation.completed_pulses - 1U) / table_stride; + if (table_index >= table_capacity) { - table_index = table_capacity - 1U; /* 夹到末格 */ + table_index = table_capacity - 1U; } - freq_table[table_index] = simulation.cur_freq_hz; /* 写入当前频 */ - if ((table_index + 1U) > table_length) /* 刷新有效长度 */ + freq_table[table_index] = simulation.cur_freq_hz; + if ((table_index + 1U) > table_length) { - table_length = table_index + 1U; /* 长度至少盖住该下标 */ + table_length = table_index + 1U; } - /* 提前到达终点：余下表项填 end */ - if (simulation.is_active == 0U) /* 提前停了？ */ + + if (simulation.is_active == 0U) { - while ((loop_index + 1U) < runtime->total_pulses) /* 把剩下拍都填成终点 */ + while ((loop_index + 1U) < runtime->total_pulses) { - loop_index++; /* 假推进loop */ - table_index = loop_index / table_stride; /* 按拍号算下标 */ - if (table_index >= table_capacity) /* 越界？ */ + loop_index++; + table_index = loop_index / table_stride; + if (table_index >= table_capacity) { - table_index = table_capacity - 1U; /* 夹住 */ + table_index = table_capacity - 1U; } - freq_table[table_index] = runtime->end_freq_hz; /* 填终点频 */ - if ((table_index + 1U) > table_length) /* 更新长度 */ + freq_table[table_index] = runtime->end_freq_hz; + if ((table_index + 1U) > table_length) { - table_length = table_index + 1U; /* 盖住 */ + table_length = table_index + 1U; } } - break; /* 跳出主循环 */ + break; } } - /* 末项强制终点频，避免抽样截断偏离 */ - if (table_length > 0U) /* 有表项？ */ + if (table_length > 0U) { - freq_table[table_length - 1U] = runtime->end_freq_hz; /* 最后一格强制终点频 */ + freq_table[table_length - 1U] = runtime->end_freq_hz; } - return table_length; /* 返回表长 */ + return table_length; } /** 绑加速表(id=1)或减速表(id=2)；长度为 0 则退回不用表 */ @@ -1717,8 +1654,8 @@ static void PlsrAccelRuntimeSelectFreqTable(PlsrAccelRuntime_t *runtime, uint32_t table_length, uint32_t table_stride) { - runtime->table_length = table_length; /* 记下表长 */ - runtime->table_stride = (table_stride < 1U) ? 1U : table_stride; /* 步长至少1 */ - runtime->freq_table_id = (table_length > 0U) ? freq_table_id : 0U; /* 有长度才绑id，否则退回0=不用表 */ + runtime->table_length = table_length; + runtime->table_stride = (table_stride < 1U) ? 1U : table_stride; + runtime->freq_table_id = (table_length > 0U) ? freq_table_id : 0U; } diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index cd1422f..5b5bf4d 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -16,15 +16,15 @@ * run_control 进入减速时使用局部规划副本，不会改写这里的整段规划预算。 */ typedef struct { - uint32_t accel_pulses; /* 入口到目标频率需要的脉冲数 */ - uint32_t const_pulses; /* 保持目标频率的脉冲数 */ - uint32_t decel_pulses; /* 目标到出口频率需要的脉冲数 */ - uint32_t start_freq_hz; /* 本段实际入口频率 */ - uint32_t target_freq_hz; /* 实际目标频率，短段可能被降低 */ - uint32_t end_freq_hz; /* 本段出口频率 */ - uint32_t accel_rate_hz_per_s; /* 升高频率时使用的变化率 */ - uint32_t decel_rate_hz_per_s; /* 降低频率时使用的变化率 */ - PlsrAccelMode_e curve_mode; /* 直线 / S / 正弦 */ + uint32_t accel_pulses; + uint32_t const_pulses; + uint32_t decel_pulses; + uint32_t start_freq_hz; + uint32_t target_freq_hz; + uint32_t end_freq_hz; + uint32_t accel_rate_hz_per_s; + uint32_t decel_rate_hz_per_s; + PlsrAccelMode_e curve_mode; } PlsrAccelPlan_t; /** 配置起速 → 规划入口：起速>目标用起速；否则目标与起跳比较取其一 */ @@ -65,27 +65,26 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, * S/正弦：查预计算频率表（开相 BeginAcc/BeginDec 时填好） */ typedef struct { - uint32_t cur_freq_hz; /* 当前输出频率 */ - uint32_t start_freq_hz; /* 当前相起点频率 */ - uint32_t end_freq_hz; /* 当前相终点频率 */ - uint32_t accel_hz_per_s; /* 当前相使用的变化率 */ - uint32_t completed_pulses; /* 当前相已经步进的脉冲数 */ - uint32_t total_pulses; /* 当前相计划步进的脉冲数 */ - uint32_t elapsed_time_us; /* 时间曲线已经经过的时间 */ - uint32_t jerk_time_us; /* S 曲线单个 jerk 阶段时间 */ - uint32_t const_accel_time_us; /* S 曲线恒加速阶段时间 */ - uint32_t ramp_time_us; /* 整个斜坡时间 */ - uint32_t table_stride; /* 每几个脉冲读取一个表项 */ - uint32_t table_length; /* 表中有效项数 */ - uint8_t freq_rising; /* 1=频率升高，0=降低 */ - uint8_t is_active; /* 1=当前相仍在运行 */ - uint8_t freq_table_id; /* 0=不用表，1=加速表，2=减速表 */ - PlsrAccelMode_e curve_mode; /* 直线 / S / 正弦 */ + uint32_t cur_freq_hz; + uint32_t start_freq_hz; + uint32_t end_freq_hz; + uint32_t accel_hz_per_s; + uint32_t completed_pulses; + uint32_t total_pulses; + uint32_t elapsed_time_us; + uint32_t jerk_time_us; + uint32_t const_accel_time_us; + uint32_t ramp_time_us; + uint32_t table_stride; + uint32_t table_length; + uint8_t freq_rising; + uint8_t is_active; + uint8_t freq_table_id; + PlsrAccelMode_e curve_mode; } PlsrAccelRuntime_t; -/* 当前段唯一的曲线规划和逐拍状态，run_control 直接使用，不再另存副本。 */ -extern PlsrAccelPlan_t g_plsr_accel_plan; /* 本段三相规划结果 */ -extern PlsrAccelRuntime_t g_plsr_accel_runtime; /* 当前相 ISR 逐拍状态 */ +extern PlsrAccelPlan_t g_plsr_accel_plan; +extern PlsrAccelRuntime_t g_plsr_accel_runtime; /** PlanSeg 后调用：预建 S/正弦加减速频率表（勿在 ISR） */ void PlsrAccelPrebuildFreqTables(const PlsrAccelPlan_t *plan); diff --git a/plsr/command/plsr_command.c b/plsr/command/plsr_command.c index 890a806..d641396 100644 --- a/plsr/command/plsr_command.c +++ b/plsr/command/plsr_command.c @@ -30,9 +30,9 @@ * s_live_freq_req — 运行中写频请求标志，Poll 消费后清 0 * s_live_freq_hz — 待应用的 new_tgt_hz */ -static uint16_t s_fault = PLSR_ERR_NONE; /* 故障码镜像，与 0x2006 同步 */ -static volatile uint8_t s_live_freq_req; /* 1=运行中改频待 Poll 消费 */ -static volatile uint32_t s_live_freq_hz; /* 待应用的新目标频率 Hz */ +static uint16_t s_fault = PLSR_ERR_NONE; +static volatile uint8_t s_live_freq_req; +static volatile uint32_t s_live_freq_hz; /** * @brief 写 32 位值到相邻两个modbus保持寄存器 @@ -57,13 +57,13 @@ static void PlsrCommandPublishMonitor(void) if (g_plsr_busy != 0U) { - PlsrCommandWriteDWord(PLSR_MON_FREQ_L, g_plsr_output_freq_hz); /* 0x2002/03 当前频 */ - WriteHoldReg(PLSR_MON_RUN_STATUS, 1U); /* 0x2004 运行中 */ - WriteHoldReg(PLSR_MON_CUR_SEG, (uint16_t)(g_plsr_cur_seg_idx + 1U)); /* 0x2005 1-based */ + PlsrCommandWriteDWord(PLSR_MON_FREQ_L, g_plsr_output_freq_hz); + WriteHoldReg(PLSR_MON_RUN_STATUS, 1U); + WriteHoldReg(PLSR_MON_CUR_SEG, (uint16_t)(g_plsr_cur_seg_idx + 1U)); } else { - /* 空闲/停止：运行、段、频率一律清零；累计与故障码单独维护 */ + PlsrCommandWriteDWord(PLSR_MON_FREQ_L, 0U); WriteHoldReg(PLSR_MON_RUN_STATUS, 0U); WriteHoldReg(PLSR_MON_CUR_SEG, 0U); @@ -107,7 +107,7 @@ void PlsrCommandInit(void) WriteHoldReg(PLSR_CTRL_REG, 0U); s_live_freq_req = 0U; s_live_freq_hz = 0U; - /* 不清故障：上电由 PersistRestoreMonitor 恢复；无镜像时 s_fault 默认为 0 */ + PlsrCommandPublishMonitor(); } @@ -118,18 +118,18 @@ void PlsrCommandInit(void) */ void PlsrCommandOnSegFreqHoldWrite(uint16_t start_addr, uint16_t quantity) { - uint16_t cur_seg; /* 当前运行段（1-based） */ - uint16_t seg; /* 本次写入对应的段下标（0-based） */ + uint16_t cur_seg; + uint16_t seg; uint16_t offset; uint32_t freqency; if (quantity != 2U) { - return; /* 须为频率双字 */ + return; } if (g_plsr_busy == 0U) { - return; /* 空闲改频走参数块，不走本路径 */ + return; } if (start_addr < PLSR_REG_SEG1_BASE) { @@ -139,7 +139,7 @@ void PlsrCommandOnSegFreqHoldWrite(uint16_t start_addr, uint16_t quantity) offset = (uint16_t)(start_addr - PLSR_REG_SEG1_BASE); if ((offset % PLSR_SEG_STRIDE) != PLSR_SEG_OFF_FREQ_L) { - return; /* 地址须对齐该段 FREQ_L */ + return; } seg = (uint16_t)(offset / PLSR_SEG_STRIDE); if (seg >= PLSR_SEG_MAX) @@ -150,22 +150,22 @@ void PlsrCommandOnSegFreqHoldWrite(uint16_t start_addr, uint16_t quantity) cur_seg = (uint16_t)(g_plsr_cur_seg_idx + 1U); if ((cur_seg < 1U) || ((uint16_t)(cur_seg - 1U) != seg)) { - return; /* 仅允许改当前运行段 */ + return; } freqency = (uint32_t)ReadHoldReg(start_addr) | ((uint32_t)ReadHoldReg((uint16_t)(start_addr + 1U)) << 16); if ((freqency < 1U) || (freqency > 100000U)) { - PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); /* 非法：不改段表、不请求 */ + PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); return; } - g_plsr_segs[seg].freq_hz = (int32_t)freqency; /* 更新运行段表 */ - PlsrPersistSaveSegFreqToBkp(seg, freqency); /* 仅该段频率落 BKP */ + g_plsr_segs[seg].freq_hz = (int32_t)freqency; + PlsrPersistSaveSegFreqToBkp(seg, freqency); s_live_freq_hz = freqency; - s_live_freq_req = 1U; /* 交 Poll → ChangeFreq */ + s_live_freq_req = 1U; } /** @@ -181,42 +181,41 @@ void PlsrCommandOnSegFreqHoldWrite(uint16_t start_addr, uint16_t quantity) void PlsrCommandPoll(void) { uint16_t ctrl = ReadHoldReg(PLSR_CTRL_REG); - uint8_t skip_monitor = 0U; /* 精确等待时跳过本拍监控刷新 */ + uint8_t skip_monitor = 0U; if (ctrl != 0U) { - WriteHoldReg(PLSR_CTRL_REG, 0U); /* 立即清，避免重复触发 */ + WriteHoldReg(PLSR_CTRL_REG, 0U); if ((ctrl & PLSR_CTRL_BIT_STOP) != 0U) { - PlsrStop(); /* 急停 */ + PlsrStop(); } if ((ctrl & PLSR_CTRL_BIT_CLR) != 0U) { - PlsrClearAccPulse(); /* 清累计 + 绝对原点 */ - PlsrCommandClearFault(); /* 同步清故障码 */ + PlsrClearAccPulse(); + PlsrCommandClearFault(); } - /* START：同拍有 STOP 则不启 */ if (((ctrl & PLSR_CTRL_BIT_START) != 0U) && ((ctrl & PLSR_CTRL_BIT_STOP) == 0U)) { if (g_plsr_busy != 0U) { - PlsrCommandSetFault(PLSR_ERR_DUP_START); /* 0x0C 重复启动 */ + PlsrCommandSetFault(PLSR_ERR_DUP_START); } else if (PlsrCommandIsStartBlocked() != 0U) { - PlsrStop(); /* 0x04~0x0B：拒启，仅停 */ + PlsrStop(); } else { - PlsrStop(); /* 先清残留再启 */ + PlsrStop(); (void)PlsrStart(PlsrParamGetStartSeg()); if (PlsrRunControlIsPreciseWait() != 0U) { - skip_monitor = 1U; /* 换向/WAIT 精确等 TIM5 */ + skip_monitor = 1U; } } } @@ -226,11 +225,11 @@ void PlsrCommandPoll(void) { uint32_t freq = s_live_freq_hz; s_live_freq_req = 0U; - (void)PlsrChangeFreq(freq); /* 重规划剩余脉冲 */ + (void)PlsrChangeFreq(freq); } if (skip_monitor == 0U) { - PlsrCommandPublishMonitor(); /* 回写 0x2000~0x2006 */ + PlsrCommandPublishMonitor(); } } diff --git a/plsr/command/plsr_command.h b/plsr/command/plsr_command.h index bf50896..3af2915 100644 --- a/plsr/command/plsr_command.h +++ b/plsr/command/plsr_command.h @@ -28,36 +28,36 @@ #include /* ---------- 监控保持寄存器（双字低地址存低 16 位，高地址存高 16 位） ---------- */ -#define PLSR_MON_ACC_PULSE_L 0x2000U /* 脉冲累计 SDW，有符号补码 */ +#define PLSR_MON_ACC_PULSE_L 0x2000U #define PLSR_MON_ACC_PULSE_H 0x2001U -#define PLSR_MON_FREQ_L 0x2002U /* 当前发送频率 Hz，UDW */ +#define PLSR_MON_FREQ_L 0x2002U #define PLSR_MON_FREQ_H 0x2003U -#define PLSR_MON_RUN_STATUS 0x2004U /* 0=空闲，1=运行中 */ -#define PLSR_MON_CUR_SEG 0x2005U /* 当前执行段号，1-based；空闲为 0 */ -#define PLSR_MON_ERR 0x2006U /* 故障码，见 PLSR_ERR_* */ +#define PLSR_MON_RUN_STATUS 0x2004U +#define PLSR_MON_CUR_SEG 0x2005U +#define PLSR_MON_ERR 0x2006U /* ---------- 控制寄存器（读/写；从站处理后写回 0） ---------- */ #define PLSR_CTRL_REG 0x3000U -#define PLSR_CTRL_BIT_START (1U << 0) /* 置 1：Apply 参数后启动 */ -#define PLSR_CTRL_BIT_STOP (1U << 1) /* 置 1：急停 */ -#define PLSR_CTRL_BIT_CLR (1U << 2) /* 置 1：清零累计脉冲与故障码 */ +#define PLSR_CTRL_BIT_START (1U << 0) +#define PLSR_CTRL_BIT_STOP (1U << 1) +#define PLSR_CTRL_BIT_CLR (1U << 2) /* * 故障码定义（写入 0x2006，须经 PlsrCommandSetFault，勿直接只写寄存器） */ -#define PLSR_ERR_NONE 0x00U //无故障 -#define PLSR_ERR_PULSE_CNT 0x01U //实际脉冲数与段目标不一致 -#define PLSR_ERR_FREQ_UNREACH 0x02U //规划峰值达不到设定目标（三角） -#define PLSR_ERR_CURVE_MISMATCH 0x03U //加减速相内脉冲与规划预算不符 -#define PLSR_ERR_FREQ_ILLEGAL 0x04U //段频率 ∉ [1, 100000] Hz（非 0） -#define PLSR_ERR_PULSE_RANGE 0x05U //脉冲个数 ∉ int32 合法范围 -#define PLSR_ERR_SEG_COUNT 0x06U //总段数 ∉ [1, 10] -#define PLSR_ERR_START_SEG 0x07U //起始段 ∉ [1, 10] -#define PLSR_ERR_START_END_SPD 0x08U //起/止速非0且 ∉ [1, 100000] Hz -#define PLSR_ERR_PULSE_Y 0x09U //脉冲端子 ∉ {Y0,Y1,Y2} -#define PLSR_ERR_DIR_Y 0x0AU //方向端子 ≠ Y3 -#define PLSR_ERR_DEFAULT_SPD 0x0BU //默认速度 ∉ [1, 100000] Hz -#define PLSR_ERR_DUP_START 0x0CU //运行中重复 START +#define PLSR_ERR_NONE 0x00U +#define PLSR_ERR_PULSE_CNT 0x01U +#define PLSR_ERR_FREQ_UNREACH 0x02U +#define PLSR_ERR_CURVE_MISMATCH 0x03U +#define PLSR_ERR_FREQ_ILLEGAL 0x04U +#define PLSR_ERR_PULSE_RANGE 0x05U +#define PLSR_ERR_SEG_COUNT 0x06U +#define PLSR_ERR_START_SEG 0x07U +#define PLSR_ERR_START_END_SPD 0x08U +#define PLSR_ERR_PULSE_Y 0x09U +#define PLSR_ERR_DIR_Y 0x0AU +#define PLSR_ERR_DEFAULT_SPD 0x0BU +#define PLSR_ERR_DUP_START 0x0CU /** 参数类门禁故障码下限 */ #define PLSR_ERR_PARAM_BLOCK_MIN PLSR_ERR_FREQ_ILLEGAL diff --git a/plsr/param/plsr_param.c b/plsr/param/plsr_param.c index 0ed229f..214c7af 100644 --- a/plsr/param/plsr_param.c +++ b/plsr/param/plsr_param.c @@ -17,24 +17,24 @@ #include "modbus_rtu.h" #include "ucos_ii.h" -PlsrCfg_t g_plsr_config; /* 公共参数运行映像 */ -PlsrSeg_t g_plsr_segs[PLSR_SEG_MAX]; /* 段表运行映像（最多 PLSR_SEG_MAX） */ +PlsrCfg_t g_plsr_config; +PlsrSeg_t g_plsr_segs[PLSR_SEG_MAX]; -static OS_EVENT *s_param_sem; /* 分帧收齐后唤醒 ParamBlockTask */ -static volatile uint16_t s_expect_frames; /* 本批期望总帧数 = 5 + 段数 */ -static volatile uint16_t s_recv_frames; /* 已收帧计数 */ -static volatile uint8_t s_xfer_active; /* 1=正在接收一批分帧 */ +static OS_EVENT *s_param_sem; +static volatile uint16_t s_expect_frames; +static volatile uint16_t s_recv_frames; +static volatile uint8_t s_xfer_active; /** @return 有效段数，软钳到 [0, PLSR_SEG_MAX] */ uint16_t PlsrParamGetSegCount(void) { if (g_plsr_config.seg_count < 1U) { - return 0U; /* 无有效段 */ + return 0U; } if (g_plsr_config.seg_count > PLSR_SEG_MAX) { - return (uint16_t)PLSR_SEG_MAX; /* 超上限钳住 */ + return (uint16_t)PLSR_SEG_MAX; } return g_plsr_config.seg_count; } @@ -48,7 +48,7 @@ uint16_t PlsrParamGetStartSeg(void) if (n < 1U) { - return 1U; /* 无段时仍返回 1 */ + return 1U; } if (g_plsr_config.start_seg < 1U) { @@ -56,7 +56,7 @@ uint16_t PlsrParamGetStartSeg(void) } if (g_plsr_config.start_seg > n) { - return n; /* 超出则钳到末段 */ + return n; } return g_plsr_config.start_seg; } @@ -114,7 +114,7 @@ static uint8_t PlsrParamIsSegFrameWrite(uint16_t start_addr, uint16_t quantity) } /*============================================================================*/ -/* 参数校验辅助 */ + /*============================================================================*/ #define PLSR_PARAM_SPD_MIN_HZ 1U @@ -156,7 +156,7 @@ static uint8_t PlsrParamIsSegFreqValid(int32_t freq_hz) /** 脉冲个数 SDW：须在 int32 可安全取绝对值的范围内（排除 INT_MIN） */ static uint8_t PlsrParamIsPulseCntValid(int32_t pulse_cnt) { - /* INT_MIN 无法安全取反成正的目标脉冲数，报 0x05 */ + if (pulse_cnt == (int32_t)(-2147483647L - 1L)) { return 0U; @@ -165,7 +165,7 @@ static uint8_t PlsrParamIsPulseCntValid(int32_t pulse_cnt) } /*============================================================================*/ -/* 分帧导入 / 导出 */ + /*============================================================================*/ /** @@ -182,41 +182,41 @@ uint8_t PlsrParamBlockApplyFromHold(void) uint16_t base; uint16_t fault = PLSR_ERR_NONE; - g_plsr_config.pulse_y = ReadHoldReg(PLSR_REG_PULSE_Y); /* 脉冲端子 Y0~Y2 */ + g_plsr_config.pulse_y = ReadHoldReg(PLSR_REG_PULSE_Y); if (g_plsr_config.pulse_y > 2U) { - fault = PLSR_ERR_PULSE_Y; /* 0x07：端子非法 */ + fault = PLSR_ERR_PULSE_Y; } - g_plsr_config.dir_y = ReadHoldReg(PLSR_REG_DIR_Y); /* 方向端子须为 Y3 */ + g_plsr_config.dir_y = ReadHoldReg(PLSR_REG_DIR_Y); if (g_plsr_config.dir_y != 3U) { - fault = PLSR_ERR_DIR_Y; /* 0x08 */ + fault = PLSR_ERR_DIR_Y; } - g_plsr_config.wait_x_sel = ReadHoldReg(PLSR_REG_WAIT_X) ? 1U : 0U; /* 0=X4，1=X5 */ - g_plsr_config.ext_x_sel = ReadHoldReg(PLSR_REG_EXT_X) ? 1U : 0U; /* 0=X4，1=X5 */ + g_plsr_config.wait_x_sel = ReadHoldReg(PLSR_REG_WAIT_X) ? 1U : 0U; + g_plsr_config.ext_x_sel = ReadHoldReg(PLSR_REG_EXT_X) ? 1U : 0U; g_plsr_config.send_mode = (PlsrSendMode_e)(ReadHoldReg(PLSR_REG_SEND_MODE) ? 1U : 0U); - g_plsr_config.dir_delay_ms = ReadHoldReg(PLSR_REG_DIR_DELAY); /* 换向延时 ms */ + g_plsr_config.dir_delay_ms = ReadHoldReg(PLSR_REG_DIR_DELAY); g_plsr_config.dir_logic = (ReadHoldReg(PLSR_REG_DIR_LOGIC) != 0U) ? - PLSR_DIR_LOGIC_NEG : PLSR_DIR_LOGIC_POS; /* 方向电平逻辑 */ + PLSR_DIR_LOGIC_NEG : PLSR_DIR_LOGIC_POS; g_plsr_config.accel_mode = (PlsrAccelMode_e)ReadHoldReg(PLSR_REG_ACCEL_MODE); g_plsr_config.run_mode = (ReadHoldReg(PLSR_REG_RUN_MODE) != 0U) ? - PLSR_POS_ABSOLUTE : PLSR_POS_RELATIVE; /* 绝对/相对 */ + PLSR_POS_ABSOLUTE : PLSR_POS_RELATIVE; n = ReadHoldReg(PLSR_REG_SEG_COUNT); g_plsr_config.seg_count = n; if ((n < 1U) || (n > PLSR_SEG_MAX)) { - fault = PLSR_ERR_SEG_COUNT; /* 0x09：段数越界 */ + fault = PLSR_ERR_SEG_COUNT; } - g_plsr_config.start_seg = ReadHoldReg(PLSR_REG_START_SEG); /* 1-based 启动段 */ + g_plsr_config.start_seg = ReadHoldReg(PLSR_REG_START_SEG); if ((n >= 1U) && (n <= PLSR_SEG_MAX)) { if ((g_plsr_config.start_seg < 1U) || (g_plsr_config.start_seg > n)) { - fault = PLSR_ERR_START_SEG; /* 0x0A：启动段越界 */ + fault = PLSR_ERR_START_SEG; } } else if (g_plsr_config.start_seg < 1U) @@ -227,23 +227,23 @@ uint8_t PlsrParamBlockApplyFromHold(void) g_plsr_config.default_speed = PlsrParamReadDWord(PLSR_REG_DEFAULT_SPD_L); if (PlsrParamIsSpeedHzValid(g_plsr_config.default_speed) == 0U) { - fault = PLSR_ERR_DEFAULT_SPD; /* 0x0B：默认速度非法 */ + fault = PLSR_ERR_DEFAULT_SPD; } - g_plsr_config.start_speed = PlsrParamReadDWord(PLSR_REG_START_SPD_L); /* 起跳频，0 合法 */ + g_plsr_config.start_speed = PlsrParamReadDWord(PLSR_REG_START_SPD_L); if (PlsrParamIsStartEndSpeedValid(g_plsr_config.start_speed) == 0U) { fault = PLSR_ERR_START_END_SPD; } - g_plsr_config.end_speed = PlsrParamReadDWord(PLSR_REG_END_SPD_L); /* 止速，0 合法 */ + g_plsr_config.end_speed = PlsrParamReadDWord(PLSR_REG_END_SPD_L); if (PlsrParamIsStartEndSpeedValid(g_plsr_config.end_speed) == 0U) { fault = PLSR_ERR_START_END_SPD; } - g_plsr_config.accel_ms = ReadHoldReg(PLSR_REG_ACCEL_MS); /* 加速参考时间 */ - g_plsr_config.decel_ms = ReadHoldReg(PLSR_REG_DECEL_MS); /* 减速参考时间 */ + g_plsr_config.accel_ms = ReadHoldReg(PLSR_REG_ACCEL_MS); + g_plsr_config.decel_ms = ReadHoldReg(PLSR_REG_DECEL_MS); if ((n >= 1U) && (n <= PLSR_SEG_MAX)) { @@ -254,32 +254,32 @@ uint8_t PlsrParamBlockApplyFromHold(void) (int32_t)PlsrParamReadDWord((uint16_t)(base + PLSR_SEG_OFF_FREQ_L)); if (PlsrParamIsSegFreqValid(g_plsr_segs[i].freq_hz) == 0U) { - fault = PLSR_ERR_FREQ_ILLEGAL; /* 0x04：段频率非法 */ + fault = PLSR_ERR_FREQ_ILLEGAL; } g_plsr_segs[i].pulse_cnt = (int32_t)PlsrParamReadDWord((uint16_t)(base + PLSR_SEG_OFF_PULSE_L)); if (PlsrParamIsPulseCntValid(g_plsr_segs[i].pulse_cnt) == 0U) { - fault = PLSR_ERR_PULSE_RANGE; /* 0x05：脉冲数 INT_MIN */ + fault = PLSR_ERR_PULSE_RANGE; } g_plsr_segs[i].wait_type = (PlsrWaitType_e)ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_WAIT)); g_plsr_segs[i].wait_ms = - ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_WAIT_MS)); /* 段后等待 ms */ + ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_WAIT_MS)); g_plsr_segs[i].act_ms = - ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_ACT_MS)); /* ACT 定时 ms */ + ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_ACT_MS)); g_plsr_segs[i].jump_seg = - ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_JUMP)); /* 跳转目标段 */ + ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_JUMP)); } } - PlsrCommandSetFault(fault); /* 写 0x2006 镜像 */ + PlsrCommandSetFault(fault); if (fault == PLSR_ERR_NONE) { - return 1U; /* 可落存 BKP */ + return 1U; } return 0U; } @@ -302,33 +302,33 @@ void PlsrParamBlockOnHoldWrite(uint16_t start_addr, uint16_t quantity) if ((start_addr == PLSR_REG_COMMON_F1_BASE) && (quantity >= PLSR_PARAM_COMMON_FRAME1_WORDS)) { - /* 公共帧 1：开新一批，期望 5+段数 */ + uint16_t seg_n = ReadHoldReg(PLSR_REG_SEG_COUNT); if (seg_n > PLSR_SEG_MAX) { - seg_n = (uint16_t)PLSR_SEG_MAX; /* 计数软钳，避免 expect 溢出 */ + seg_n = (uint16_t)PLSR_SEG_MAX; } s_expect_frames = (uint16_t)(PLSR_PARAM_COMMON_FRAMES + seg_n); - s_recv_frames = 1U; /* 帧 1 已计入 */ - s_xfer_active = 1U; /* 开始收批 */ + s_recv_frames = 1U; + s_xfer_active = 1U; } else if ((s_xfer_active != 0U) && ((PlsrParamIsCommonTailFrameWrite(start_addr, quantity) != 0U) || (PlsrParamIsSegFrameWrite(start_addr, quantity) != 0U))) { - s_recv_frames++; /* 公共帧 2~5 或段帧 +1 */ + s_recv_frames++; } else { - return; /* 非本批帧 / 未开批 → 忽略 */ + return; } if ((s_xfer_active != 0U) && (s_recv_frames >= s_expect_frames)) { - s_xfer_active = 0U; /* 收齐 */ + s_xfer_active = 0U; if (s_param_sem != (OS_EVENT *)0) { - (void)OSSemPost(s_param_sem); /* 唤醒 ParamBlockTask */ + (void)OSSemPost(s_param_sem); } } } @@ -364,7 +364,7 @@ void PlsrParamBlockTask(void *pArg) if (PlsrParamBlockApplyFromHold() != 0U) { - PlsrPersistSaveParamsFromHold(); /* 仅全部校验通过才存 BKP */ + PlsrPersistSaveParamsFromHold(); } } } diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h index 318a02e..21a5af6 100644 --- a/plsr/param/plsr_param.h +++ b/plsr/param/plsr_param.h @@ -41,56 +41,56 @@ #define PLSR_SEG_MAX 10U /* ---------- 公共参数保持寄存器（0x1000 起，共 20 字到 0x1013） ---------- */ -#define PLSR_REG_PULSE_Y 0x1000U /* 脉冲端子：0=Y0 1=Y1 2=Y2 */ -#define PLSR_REG_DIR_Y 0x1001U /* 方向端子：固件强制 3=Y3 */ -#define PLSR_REG_WAIT_X 0x1002U /* WAIT 输入选择：0=X4 1=X5 */ -#define PLSR_REG_EXT_X 0x1003U /* EXT 输入选择：0=X4 1=X5 */ -#define PLSR_REG_SEND_MODE 0x1004U /* 发送方式：0完成 1后续 */ -#define PLSR_REG_DIR_DELAY 0x1005U /* 方向建立延时 ms */ -#define PLSR_REG_DIR_LOGIC 0x1006U /* 方向逻辑：0正 1负 */ -#define PLSR_REG_ACCEL_MODE 0x1007U /* 曲线：0直线 1=S 2正弦 */ -#define PLSR_REG_RUN_MODE 0x1008U /* 定位：0相对 1绝对 */ -#define PLSR_REG_SEG_COUNT 0x1009U /* 脉冲总段数 N（决定 expect=5+N） */ -#define PLSR_REG_START_SEG 0x100AU /* 启动段号 1-based */ -#define PLSR_REG_DEFAULT_SPD_L 0x100BU /* 默认速度 Hz，双字低 */ +#define PLSR_REG_PULSE_Y 0x1000U +#define PLSR_REG_DIR_Y 0x1001U +#define PLSR_REG_WAIT_X 0x1002U +#define PLSR_REG_EXT_X 0x1003U +#define PLSR_REG_SEND_MODE 0x1004U +#define PLSR_REG_DIR_DELAY 0x1005U +#define PLSR_REG_DIR_LOGIC 0x1006U +#define PLSR_REG_ACCEL_MODE 0x1007U +#define PLSR_REG_RUN_MODE 0x1008U +#define PLSR_REG_SEG_COUNT 0x1009U +#define PLSR_REG_START_SEG 0x100AU +#define PLSR_REG_DEFAULT_SPD_L 0x100BU #define PLSR_REG_DEFAULT_SPD_H 0x100CU -#define PLSR_REG_START_SPD_L 0x100DU /* 起速 Hz */ +#define PLSR_REG_START_SPD_L 0x100DU #define PLSR_REG_START_SPD_H 0x100EU -/* 0x100F 保留，写帧填 0 */ -#define PLSR_REG_END_SPD_L 0x1010U /* 止速 Hz */ + +#define PLSR_REG_END_SPD_L 0x1010U #define PLSR_REG_END_SPD_H 0x1011U -#define PLSR_REG_ACCEL_MS 0x1012U /* 默认速度加速时间 ms */ -#define PLSR_REG_DECEL_MS 0x1013U /* 默认速度减速时间 ms */ +#define PLSR_REG_ACCEL_MS 0x1012U +#define PLSR_REG_DECEL_MS 0x1013U /* ---------- 公共参数分帧（5 帧 + N 段帧） ---------- */ #define PLSR_PARAM_COMMON_FRAMES 5U -#define PLSR_PARAM_COMMON_FRAME1_WORDS 11U /* 0x1000~0x100A */ -#define PLSR_PARAM_COMMON_DWORD_WORDS 2U /* 双字帧字数 */ +#define PLSR_PARAM_COMMON_FRAME1_WORDS 11U +#define PLSR_PARAM_COMMON_DWORD_WORDS 2U -#define PLSR_REG_COMMON_F1_BASE PLSR_REG_PULSE_Y /* 0x1000 */ -#define PLSR_REG_COMMON_F1_END PLSR_REG_START_SEG /* 0x100A */ -#define PLSR_REG_COMMON_F2_BASE PLSR_REG_DEFAULT_SPD_L /* 0x100B */ -#define PLSR_REG_COMMON_F3_BASE PLSR_REG_START_SPD_L /* 0x100D */ -#define PLSR_REG_COMMON_F4_BASE PLSR_REG_END_SPD_L /* 0x1010 */ -#define PLSR_REG_COMMON_F5_BASE PLSR_REG_ACCEL_MS /* 0x1012 */ +#define PLSR_REG_COMMON_F1_BASE PLSR_REG_PULSE_Y +#define PLSR_REG_COMMON_F1_END PLSR_REG_START_SEG +#define PLSR_REG_COMMON_F2_BASE PLSR_REG_DEFAULT_SPD_L +#define PLSR_REG_COMMON_F3_BASE PLSR_REG_START_SPD_L +#define PLSR_REG_COMMON_F4_BASE PLSR_REG_END_SPD_L +#define PLSR_REG_COMMON_F5_BASE PLSR_REG_ACCEL_MS /* ---------- 段参数：第 i 段基址 = 0x1100 + i*0x10，每段 8 字 ---------- */ #define PLSR_REG_SEG1_BASE 0x1100U #define PLSR_SEG_STRIDE 0x0010U -#define PLSR_SEG_OFF_FREQ_L 0U /* 目标频率 SDW；0=用 default_speed */ +#define PLSR_SEG_OFF_FREQ_L 0U #define PLSR_SEG_OFF_FREQ_H 1U -#define PLSR_SEG_OFF_PULSE_L 2U /* 脉冲数/绝对坐标 SDW，负=反转 */ +#define PLSR_SEG_OFF_PULSE_L 2U #define PLSR_SEG_OFF_PULSE_H 3U -#define PLSR_SEG_OFF_WAIT 4U /* PlsrWaitType_e */ -#define PLSR_SEG_OFF_WAIT_MS 5U /* WAIT 时间 ms */ -#define PLSR_SEG_OFF_ACT_MS 6U /* ACT 时间 ms */ -#define PLSR_SEG_OFF_JUMP 7U /* 0=顺序/末段停；1~N=跳转目标段号 */ +#define PLSR_SEG_OFF_WAIT 4U +#define PLSR_SEG_OFF_WAIT_MS 5U +#define PLSR_SEG_OFF_ACT_MS 6U +#define PLSR_SEG_OFF_JUMP 7U /** 方向逻辑：正转时方向脚电平 */ typedef enum { - PLSR_DIR_LOGIC_POS = 0, /* 正转→方向高，反转→低 */ - PLSR_DIR_LOGIC_NEG = 1 /* 相反 */ + PLSR_DIR_LOGIC_POS = 0, + PLSR_DIR_LOGIC_NEG = 1 } PlsrDirLogic_e; /** @@ -105,9 +105,9 @@ typedef enum { /** 加减速曲线形状 */ typedef enum { - PLSR_ACCEL_LINEAR = 0, /* f^2 = f0^2 ± 2an */ - PLSR_ACCEL_S = 1, /* 时间域七段 S：加速度梯形 1:2:1 */ - PLSR_ACCEL_SINE = 2 /* 时间域 raised-cosine */ + PLSR_ACCEL_LINEAR = 0, + PLSR_ACCEL_S = 1, + PLSR_ACCEL_SINE = 2 } PlsrAccelMode_e; /** @@ -127,43 +127,43 @@ typedef enum { * 与下一段反向时规划同其它等待（减速到止速，下一段从起速再爬） */ typedef enum { - PLSR_WAIT_TIME = 0, /* 发完后等 wait_ms（0 按 1ms），再跳转 */ - PLSR_WAIT_SIGNAL = 1, /* 发完后等 WAIT 口下降沿 */ - PLSR_WAIT_ACT = 2, /* 开跑后满 act_ms 从当前频切下一段；脉冲先完则等到点 */ - PLSR_WAIT_EXT = 3, /* 运行中 EXT 沿中途切段；发完后仍等 EXT */ - PLSR_WAIT_EXT_OR_DONE = 4 /* EXT 沿或脉冲发完，先到先切 */ + PLSR_WAIT_TIME = 0, + PLSR_WAIT_SIGNAL = 1, + PLSR_WAIT_ACT = 2, + PLSR_WAIT_EXT = 3, + PLSR_WAIT_EXT_OR_DONE = 4 } PlsrWaitType_e; /** * 公共运行参数映像 */ typedef struct { - uint16_t pulse_y; /* 0=Y0/TIM10 1=Y1/TIM13 2=Y2/TIM11 */ - uint16_t dir_y; /* 固定 3=Y3 */ - uint16_t wait_x_sel; /* 0=X4 1=X5 */ + uint16_t pulse_y; + uint16_t dir_y; + uint16_t wait_x_sel; uint16_t ext_x_sel; PlsrSendMode_e send_mode; uint16_t dir_delay_ms; PlsrDirLogic_e dir_logic; PlsrAccelMode_e accel_mode; PlsrPosMode_e run_mode; - uint16_t seg_count; /* 有效段数 0~PLSR_SEG_MAX */ - uint16_t start_seg; /* 1-based */ - uint32_t default_speed; /* Hz；段频为 0 时作段目标；并定斜率 */ - uint32_t start_speed; /* 起速；0=运行层 ResolveStartHz 起跳 */ - uint32_t end_speed; /* 止速；0=ResolveEndHz 落地后停表 */ + uint16_t seg_count; + uint16_t start_seg; + uint32_t default_speed; + uint32_t start_speed; + uint32_t end_speed; uint16_t accel_ms; uint16_t decel_ms; } PlsrCfg_t; /** 单段运行参数 */ typedef struct { - int32_t freq_hz; /* 0=用默认速度；解析后须在 1~100k，否则 0x04 */ - int32_t pulse_cnt; /* 可负=反向；0=本段不发 */ + int32_t freq_hz; + int32_t pulse_cnt; PlsrWaitType_e wait_type; uint16_t wait_ms; uint16_t act_ms; - uint16_t jump_seg; /* 0=顺序/末段结束；1~N=跳转 */ + uint16_t jump_seg; } PlsrSeg_t; /* ---------- 运行参数 ---------- */ diff --git a/plsr/plsr.c b/plsr/plsr.c index a45367b..55f7fb8 100644 --- a/plsr/plsr.c +++ b/plsr/plsr.c @@ -27,16 +27,16 @@ */ void PlsrInit(void) { - PlsrPersistInit(); /* 使能 BKPSRAM 写访问，否则存BKP无效 */ - PlsrPersistLoad(); /* BKPSRAM → g_hold_reg */ - (void)PlsrParamBlockApplyFromHold(); /* hold → 运行参数 + 校验 */ + PlsrPersistInit(); + PlsrPersistLoad(); + (void)PlsrParamBlockApplyFromHold(); PlsrPulseDriverInit(); PlsrSignalIoInit(); PlsrRunControlInit(); - PlsrPersistRestoreRuntime(); /* hold 0x2000 累计 → run_control */ + PlsrPersistRestoreRuntime(); PlsrCommandInit(); - PlsrPersistRestoreMonitor(); /* 恢复故障码；0x3000/运行态强制空闲 */ - PlsrPersistTickMonitor(); /* 监控区 hold+BKP 与运行态对齐 */ + PlsrPersistRestoreMonitor(); + PlsrPersistTickMonitor(); } /** @@ -50,11 +50,11 @@ void PlsrInit(void) */ void PlsrTask(void *pArg) { - static uint8_t s_booted = 0U; /* 1=已完成首次延迟对齐（等 Modbus 填 hold） */ + static uint8_t s_booted = 0U; (void)pArg; - PlsrRunControlWakeInit(); /* OS 已启动，创建 WAIT/ACT 到期唤醒信号量 */ + PlsrRunControlWakeInit(); while(1) { @@ -64,17 +64,17 @@ void PlsrTask(void *pArg) */ if (s_booted == 0U) { - OSTimeDly(3U); /* ~30ms @ 100Hz tick；让出 CPU 给 Modbus 完成初始化 */ + OSTimeDly(3U); PlsrCommandInit(); PlsrPersistRestoreMonitor(); PlsrPersistTickMonitor(); s_booted = 1U; } - PlsrRunControlTickMs(); /* 处理延时/外部信号输入，优先：WAIT/换向到期立刻开表 */ - PlsrSignalIoDebouncePoll(); /* OS 节拍消抖确认 */ - PlsrCommandPoll(); /* 收 START/STOP/CLR，回写 0x2000~0x2006 监控 ，决定是否开pwm输出*/ - PlsrPersistTickMonitor(); /* 监控区：运行态→hold→BKP（每 OS 节拍） */ + PlsrRunControlTickMs(); + PlsrSignalIoDebouncePoll(); + PlsrCommandPoll(); + PlsrPersistTickMonitor(); /* * 换向 / WAIT 时间：WakePend(0) 只等 TIM5 Post（墙钟≈设定 ms）。 diff --git a/plsr/plsr.h b/plsr/plsr.h index e0dca89..27ad1e2 100644 --- a/plsr/plsr.h +++ b/plsr/plsr.h @@ -24,10 +24,9 @@ #include -/* run_control 拥有的共享运行状态；段下标从 0 开始。 */ -extern volatile uint8_t g_plsr_busy; /* 运行忙标志（上位机监控） */ -extern volatile uint16_t g_plsr_cur_seg_idx; /* 当前段下标（0-based） */ -extern volatile int32_t g_plsr_accumulated_pulses; /* 全局累计脉冲（有符号） */ +extern volatile uint8_t g_plsr_busy; +extern volatile uint16_t g_plsr_cur_seg_idx; +extern volatile int32_t g_plsr_accumulated_pulses; #include "plsr_param.h" /** diff --git a/plsr/pulse_driver/plsr_pulse_driver.c b/plsr/pulse_driver/plsr_pulse_driver.c index 38c1433..d98ac7f 100644 --- a/plsr/pulse_driver/plsr_pulse_driver.c +++ b/plsr/pulse_driver/plsr_pulse_driver.c @@ -24,51 +24,47 @@ #include "plsr_param.h" #include "main.h" -TIM_HandleTypeDef htim10; /* TIM10：Y0 / PF6 / CH1 */ -TIM_HandleTypeDef htim11; /* TIM11：Y2 / PF7 / CH1 */ -TIM_HandleTypeDef htim13; /* TIM13：Y1 / PF8 / CH1（端子号与 TIM 交叉） */ +TIM_HandleTypeDef htim10; +TIM_HandleTypeDef htim11; +TIM_HandleTypeDef htim13; -#define PLSR_TIM_CLK_APB2_HZ 168000000UL /* TIM10 / TIM11 */ -#define PLSR_TIM_CLK_APB1_HZ 84000000UL /* TIM13 */ +#define PLSR_TIM_CLK_APB2_HZ 168000000UL +#define PLSR_TIM_CLK_APB1_HZ 84000000UL #define PLSR_Y0_PORT GPIOF -#define PLSR_Y0_PIN GPIO_PIN_6 /* TIM10 */ +#define PLSR_Y0_PIN GPIO_PIN_6 #define PLSR_Y1_PORT GPIOF -#define PLSR_Y1_PIN GPIO_PIN_8 /* TIM13：端子编号与 TIM 交叉 */ +#define PLSR_Y1_PIN GPIO_PIN_8 #define PLSR_Y2_PORT GPIOF -#define PLSR_Y2_PIN GPIO_PIN_7 /* TIM11 */ +#define PLSR_Y2_PIN GPIO_PIN_7 #define PLSR_Y3_PORT GPIOF -#define PLSR_Y3_PIN GPIO_PIN_9 /* 方向脚，纯 GPIO，无定时器 */ - -volatile uint32_t g_plsr_output_freq_hz; /* 当前输出频率 Hz（监控） */ -volatile uint8_t g_plsr_pwm_running; /* 1=PWM 正在跑 */ -uint8_t g_plsr_direction_forward; /* 1=正转方向 */ -uint8_t g_plsr_direction_valid; /* 1=方向脚已有效写出 */ -static uint32_t s_last_psc = 0xFFFFFFFFUL; /* 上次写入的 PSC（未装载=全 F） */ -static uint32_t s_last_arr = 0xFFFFFFFFUL; /* 上次 ARR */ -static uint32_t s_last_ccr; /* 上次 CCR（约 50% 占空） */ -static TIM_HandleTypeDef *s_active_htim; /* 当前选中的脉冲 TIM */ - -static uint8_t s_psc_locked; /* 1=本段已锁 PSC，只改 ARR */ -static uint32_t s_locked_psc; /* 锁定的 PSC 值 */ - -static volatile uint32_t s_req_freq; /* ISR 挂起改频目标 Hz */ -static volatile uint8_t s_req_pending; /* 1=有挂起改频，任务 ApplyPending */ +#define PLSR_Y3_PIN GPIO_PIN_9 + +volatile uint32_t g_plsr_output_freq_hz; +volatile uint8_t g_plsr_pwm_running; +uint8_t g_plsr_direction_forward; +uint8_t g_plsr_direction_valid; +static uint32_t s_last_psc = 0xFFFFFFFFUL; +static uint32_t s_last_arr = 0xFFFFFFFFUL; +static uint32_t s_last_ccr; +static TIM_HandleTypeDef *s_active_htim; + +static uint8_t s_psc_locked; +static uint32_t s_locked_psc; + +static volatile uint32_t s_req_freq; +static volatile uint8_t s_req_pending; /* 1=DIR_WAIT 已装载寄存器且 CEN 关：到期 Commit 前必须 CNT=0 再开 PWM */ static uint8_t s_regs_prepared; -static volatile uint8_t s_seg_end_fall_armed; /* 1=已武装末拍 CC1 下降沿拉低 */ +static volatile uint8_t s_seg_end_fall_armed; /* * 冷启动首拍保护：Commit 后第一次 SetFreqIsr 必须先钉住工作 ARR=起跳频， * 再把下一拍写入影子。否则 ARPE 未生效时 ISR 会直接改写工作寄存器， * 示波器上首周期频率变成「第 2 个脉冲」的频率（起跳越高越明显）。 */ -static volatile uint8_t s_protect_start_period; /* 1=首拍须钉住工作 ARR */ -static uint32_t s_protect_arr; /* 首拍保护用 ARR */ -static uint32_t s_protect_ccr; /* 首拍保护用 CCR */ - -/*============================================================================*/ -/* 路选择 / 时钟 / GPIO */ -/*============================================================================*/ +static volatile uint8_t s_protect_start_period; +static uint32_t s_protect_arr; +static uint32_t s_protect_ccr; /** * @brief 按 pulse_y 返回对应 HAL 句柄 @@ -78,23 +74,23 @@ static TIM_HandleTypeDef *PlsrPulseDriverHtimByY(uint16_t pulse_y) { if (pulse_y == 1U) { - return &htim13; /* Y1 → TIM13 CH1 → PF8（APB1 84MHz） */ + return &htim13; } if (pulse_y == 2U) { - return &htim11; /* Y2 → TIM11 CH1 → PF7（APB2 168MHz） */ + return &htim11; } - return &htim10; /* Y0（默认）→ TIM10 CH1 → PF6（APB2 168MHz） */ + return &htim10; } /** @brief 从公共参数取脉冲端子，越界钳到 Y0 */ static uint8_t PlsrPulseDriverSelectY(void) { - uint16_t y = g_plsr_config.pulse_y; /* 公共参数：0=Y0/TIM10，1=Y1/TIM13，2=Y2/TIM11 */ + uint16_t y = g_plsr_config.pulse_y; if (y > 2U) { - y = 0U; /* 非法端子 → 钳到 Y0（TIM10/PF6） */ + y = 0U; } return (uint8_t)y; } @@ -105,9 +101,9 @@ static uint32_t PlsrPulseDriverTimClkHz(const TIM_HandleTypeDef *htim) if ((htim != (const TIM_HandleTypeDef *)0) && (htim->Instance == TIM13)) { - return PLSR_TIM_CLK_APB1_HZ; /* TIM13（Y1/PF8）：APB1 定时器时钟 84MHz */ + return PLSR_TIM_CLK_APB1_HZ; } - return PLSR_TIM_CLK_APB2_HZ; /* TIM10（Y0）或 TIM11（Y2）：APB2 定时器时钟 168MHz */ + return PLSR_TIM_CLK_APB2_HZ; } /** @brief 初始化 Y3 方向脚为推挽输出低 */ @@ -115,14 +111,14 @@ static void PlsrPulseDriverDirGpioInit(void) { GPIO_InitTypeDef gpio = {0}; - __HAL_RCC_GPIOF_CLK_ENABLE(); /* 打开 GPIOF 时钟（Y0~Y3 都在 PF） */ + __HAL_RCC_GPIOF_CLK_ENABLE(); - gpio.Pin = PLSR_Y3_PIN; /* PF9 = Y3 方向，不是定时器 */ - gpio.Mode = GPIO_MODE_OUTPUT_PP; /* 推挽输出 */ + gpio.Pin = PLSR_Y3_PIN; + gpio.Mode = GPIO_MODE_OUTPUT_PP; gpio.Pull = GPIO_NOPULL; gpio.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(PLSR_Y3_PORT, &gpio); - HAL_GPIO_WritePin(PLSR_Y3_PORT, PLSR_Y3_PIN, GPIO_PIN_RESET); /* 空闲方向脚拉低 */ + HAL_GPIO_WritePin(PLSR_Y3_PORT, PLSR_Y3_PIN, GPIO_PIN_RESET); } /** @@ -131,13 +127,13 @@ static void PlsrPulseDriverDirGpioInit(void) */ static void PlsrPulseDriverClearSegEndFallArm(TIM_HandleTypeDef *htim) { - s_seg_end_fall_armed = 0U; /* 撤末拍下降沿武装标志（不是 Forced，只撤 CC1 武装） */ + s_seg_end_fall_armed = 0U; if ((htim == (TIM_HandleTypeDef *)0) || (htim->Instance == (TIM_TypeDef *)0)) { return; } - __HAL_TIM_DISABLE_IT(htim, TIM_IT_CC1); /* 关 CC1 中断使能（DIER.CC1IE=0），撤武装 */ - __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC1); /* 清 SR.CC1IF，避免残留标志再进 ISR */ + __HAL_TIM_DISABLE_IT(htim, TIM_IT_CC1); + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC1); } /** @@ -152,13 +148,12 @@ static void PlsrPulseDriverOcForceLowKeepRun(TIM_HandleTypeDef *htim) return; } - ccmr = htim->Instance->CCMR1;//Output Compare 1 Mode，输出比较 1 模式 - //- CC1E=1：OC 通道输出到 GPIO 引脚； - //- CC1E=0：通道输出关闭，引脚由 GPIO 寄存器控制。 - ccmr &= (uint32_t)(~TIM_CCMR1_OC1M);//把[6:4]变成0 - ccmr |= TIM_OCMODE_FORCED_INACTIVE;//写入0b100 强制无效模式到OC1M域 - htim->Instance->CCMR1 = ccmr; /* 写回 CCMR1：OC1M=Forced inactive → 比较输出强制无效电平（极性高时=低） */ - htim->Instance->CCER |= TIM_CCER_CC1E; /* CC1E=1：通道输出仍接到 GPIO，Forced 才能真正把脚拉低 */ + ccmr = htim->Instance->CCMR1; + + ccmr &= (uint32_t)(~TIM_CCMR1_OC1M); + ccmr |= TIM_OCMODE_FORCED_INACTIVE; + htim->Instance->CCMR1 = ccmr; + htim->Instance->CCER |= TIM_CCER_CC1E; } /** @@ -173,21 +168,20 @@ static void PlsrPulseDriverOcHoldLow(TIM_HandleTypeDef *htim) return; } - __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE); /* 关 UPDATE 中断（DIER.UIE=0），停表后不再进脉冲 ISR */ - PlsrPulseDriverClearSegEndFallArm(htim); /* 撤 CC1 武装（关 CC1 IT），不是 Forced */ - htim->Instance->CR1 &= (uint32_t)(~(TIM_CR1_CEN | TIM_CR1_OPM)); /* 清 CEN=停计数；清 OPM=退出单脉冲模式 */ + __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE); + PlsrPulseDriverClearSegEndFallArm(htim); + htim->Instance->CR1 &= (uint32_t)(~(TIM_CR1_CEN | TIM_CR1_OPM)); + + ccmr = htim->Instance->CCMR1; - ccmr = htim->Instance->CCMR1;//Output Compare 1 Mode，输出比较 1 模式 - //- CC1E=1：OC 通道输出到 GPIO 引脚； - //- CC1E=0：通道输出关闭，引脚由 GPIO 寄存器控制。 - ccmr &= (uint32_t)(~TIM_CCMR1_OC1M);//把[6:4]变成0 - ccmr |= TIM_OCMODE_FORCED_INACTIVE;//写入0b100 强制无效模式到OC1M域 - htim->Instance->CCMR1 = ccmr; /* 写回：OC1M=Forced inactive，空闲主动拉低 */ - htim->Instance->CCER |= TIM_CCER_CC1E; /* 保持 CC1E=1，不关通道，避免 AF 高阻耦合假脉冲 */ + ccmr &= (uint32_t)(~TIM_CCMR1_OC1M); + ccmr |= TIM_OCMODE_FORCED_INACTIVE; + htim->Instance->CCMR1 = ccmr; + htim->Instance->CCER |= TIM_CCER_CC1E; - __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); /* 清 SR.UIF */ - __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); /* 清 UPDATE 中断挂起 */ - TIM_CHANNEL_STATE_SET(htim, TIM_CHANNEL_1, HAL_TIM_CHANNEL_STATE_READY); /* HAL 通道状态回到 READY */ + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); + __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); + TIM_CHANNEL_STATE_SET(htim, TIM_CHANNEL_1, HAL_TIM_CHANNEL_STATE_READY); } /** @@ -198,41 +192,41 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim) { GPIO_InitTypeDef gpio = {0}; - __HAL_RCC_GPIOF_CLK_ENABLE(); /* Y0/Y1/Y2 脉冲脚都在 GPIOF */ + __HAL_RCC_GPIOF_CLK_ENABLE(); + + gpio.Mode = GPIO_MODE_AF_PP; - gpio.Mode = GPIO_MODE_AF_PP; /* 复用推挽：TIM CH1 驱动引脚 */ - /* 下拉：通道若短暂高阻时保持低，避免上拉导致看不到开表上升沿 */ gpio.Pull = GPIO_PULLDOWN; gpio.Speed = GPIO_SPEED_FREQ_HIGH; if (htim->Instance == TIM10) { - /* TIM10 CH1 → PF6 = Y0，IRQ=TIM1_UP_TIM10 */ - __HAL_RCC_TIM10_CLK_ENABLE(); /* 打开 TIM10 外设时钟（APB2） */ - gpio.Pin = PLSR_Y0_PIN; /* PF6 */ - gpio.Alternate = GPIO_AF3_TIM10; /* AF3：TIM10_CH1 */ + + __HAL_RCC_TIM10_CLK_ENABLE(); + gpio.Pin = PLSR_Y0_PIN; + gpio.Alternate = GPIO_AF3_TIM10; HAL_GPIO_Init(PLSR_Y0_PORT, &gpio); - HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 5, 0); /* TIM10 与 TIM1_UP 共用向量 */ + HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 5, 0); HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); } else if (htim->Instance == TIM11) { - /* TIM11_CH1 = PF7 = Y2 */ - __HAL_RCC_TIM11_CLK_ENABLE(); /* 打开 TIM11 外设时钟（APB2） */ - gpio.Pin = PLSR_Y2_PIN; /* PF7 = Y2 */ - gpio.Alternate = GPIO_AF3_TIM11; /* AF3：TIM11_CH1 */ + + __HAL_RCC_TIM11_CLK_ENABLE(); + gpio.Pin = PLSR_Y2_PIN; + gpio.Alternate = GPIO_AF3_TIM11; HAL_GPIO_Init(PLSR_Y2_PORT, &gpio); - HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM11_IRQn, 5, 0); /* TIM11 共用 TIM1_TRG_COM 向量 */ + HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM11_IRQn, 5, 0); HAL_NVIC_EnableIRQ(TIM1_TRG_COM_TIM11_IRQn); } else if (htim->Instance == TIM13) { - /* TIM13_CH1 = PF8 = Y1 */ - __HAL_RCC_TIM13_CLK_ENABLE(); /* 打开 TIM13 外设时钟（APB1，84MHz） */ - gpio.Pin = PLSR_Y1_PIN; /* PF8 = Y1（端子号与 TIM 交叉） */ - gpio.Alternate = GPIO_AF9_TIM13; /* AF9：TIM13_CH1 */ + + __HAL_RCC_TIM13_CLK_ENABLE(); + gpio.Pin = PLSR_Y1_PIN; + gpio.Alternate = GPIO_AF9_TIM13; HAL_GPIO_Init(PLSR_Y1_PORT, &gpio); - HAL_NVIC_SetPriority(TIM8_UP_TIM13_IRQn, 5, 0); /* TIM13 与 TIM8_UP 共用向量 */ + HAL_NVIC_SetPriority(TIM8_UP_TIM13_IRQn, 5, 0); HAL_NVIC_EnableIRQ(TIM8_UP_TIM13_IRQn); } } @@ -246,34 +240,34 @@ static void PlsrPulseDriverTimInitOne(TIM_HandleTypeDef *htim, TIM_TypeDef *inst { TIM_OC_InitTypeDef oc = {0}; - htim->Instance = inst; /* 绑定硬件：TIM10=Y0 / TIM11=Y2 / TIM13=Y1 */ - htim->Init.Prescaler = 167U; /* 初始 PSC=167 → 分频=168；稍后 LoadRegs 会按频率重算 */ - htim->Init.CounterMode = TIM_COUNTERMODE_UP; /* 向上计数：CNT 从 0 数到 ARR 产生 UPDATE */ - htim->Init.Period = 999U; /* 初始 ARR=999 → 周期计数=1000；正式运行前会被改写 */ - htim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; /* 死区/滤波时钟不分频 */ + htim->Instance = inst; + htim->Init.Prescaler = 167U; + htim->Init.CounterMode = TIM_COUNTERMODE_UP; + htim->Init.Period = 999U; + htim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; /* * ARR/CCR 预装载打开：改频只写入影子寄存器，在下一 UPDATE 生效。 * 避免关预装载时在 ISR 里直接改 ARR/CCR 产生毛刺边沿（段越长多计越多， * 如设定 5000 实测 5007）。软件侧超前一拍写频率补偿延迟。 * polarity极性 */ - htim->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; /* ARPE=1：写 ARR 进影子，UPDATE 时装入工作寄存器 */ + htim->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; if (HAL_TIM_PWM_Init(htim) != HAL_OK) { Error_Handler(); } - oc.OCMode = TIM_OCMODE_PWM1; /* OC1M=PWM1：CNTInstance->CCMR1 |= TIM_CCMR1_OC1PE; /* OC1PE=1：写 CCR1 进影子，UPDATE 时装入，与 ARPE 配套 */ - /* 复用保持：强制无效，推挽空闲拉低；开跑后 PWM1 每周期仍先高后低 */ - PlsrPulseDriverOcHoldLow(htim); /* CEN=0 + Forced inactive + CC1E=1：空闲脚低、计数停 */ + htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; + + PlsrPulseDriverOcHoldLow(htim); } /** @brief 驱动初始化 */ @@ -293,12 +287,12 @@ void PlsrPulseDriverInit(void) s_protect_start_period = 0U; s_protect_arr = 0U; s_protect_ccr = 0U; - s_active_htim = &htim10; /* 默认活动路：TIM10=Y0 */ - PlsrPulseDriverDirGpioInit(); /* Y3=PF9 方向脚 */ - /* 三路都 Init + 空闲拉低，计数器关闭；出脉冲时只 Start 选中那路 */ - PlsrPulseDriverTimInitOne(&htim10, TIM10); /* TIM10 → PF6=Y0 */ - PlsrPulseDriverTimInitOne(&htim11, TIM11); /* TIM11 → PF7=Y2 */ - PlsrPulseDriverTimInitOne(&htim13, TIM13); /* TIM13 → PF8=Y1 */ + s_active_htim = &htim10; + PlsrPulseDriverDirGpioInit(); + + PlsrPulseDriverTimInitOne(&htim10, TIM10); + PlsrPulseDriverTimInitOne(&htim11, TIM11); + PlsrPulseDriverTimInitOne(&htim13, TIM13); } /** @brief 写方向脚 */ @@ -308,14 +302,14 @@ void PlsrPulseDriverSetDir(uint8_t forward) if (g_plsr_config.dir_logic == PLSR_DIR_LOGIC_POS) { - level = (forward != 0U) ? 1U : 0U; /* 正逻辑：正转=高 */ + level = (forward != 0U) ? 1U : 0U; } else { - level = (forward != 0U) ? 0U : 1U; /* 负逻辑：正转=低 */ + level = (forward != 0U) ? 0U : 1U; } HAL_GPIO_WritePin(PLSR_Y3_PORT, PLSR_Y3_PIN, - level ? GPIO_PIN_SET : GPIO_PIN_RESET); /* 只动 Y3=PF9，不动任何 TIM */ + level ? GPIO_PIN_SET : GPIO_PIN_RESET); g_plsr_direction_forward = (forward != 0U) ? 1U : 0U; g_plsr_direction_valid = 1U; } @@ -323,7 +317,7 @@ void PlsrPulseDriverSetDir(uint8_t forward) /** @brief 方向脚拉低 */ void PlsrPulseDriverClearDir(void) { - HAL_GPIO_WritePin(PLSR_Y3_PORT, PLSR_Y3_PIN, GPIO_PIN_RESET); /* Y3 拉低 */ + HAL_GPIO_WritePin(PLSR_Y3_PORT, PLSR_Y3_PIN, GPIO_PIN_RESET); g_plsr_direction_forward = 0U; g_plsr_direction_valid = 0U; } @@ -331,14 +325,10 @@ void PlsrPulseDriverClearDir(void) /** @brief 是否脉冲 TIM */ uint8_t PlsrPulseDriverIsPulseTim(TIM_TypeDef *instance) { - /* 本文件三路：TIM10=Y0、TIM11=Y2、TIM13=Y1；TIM5 不在此列 */ + return ((instance == TIM10) || (instance == TIM11) || (instance == TIM13)) ? 1U : 0U; } -/*============================================================================*/ -/* PSC / ARR 量化 */ -/*============================================================================*/ - /* * STM32 定时器频率换算 * clk_hz — TIM 计数时钟（APB2=168MHz 或 APB1=84MHz） @@ -357,42 +347,42 @@ static void PlsrPulseDriverCalcArrWithPsc(uint32_t clk_hz, uint32_t freq_hz, uin if (clk_hz < 1U) { - clk_hz = PLSR_TIM_CLK_APB2_HZ; /* 兜底按 APB2=168MHz（TIM10/11） */ + clk_hz = PLSR_TIM_CLK_APB2_HZ; } if (freq_hz < 1U) { - freq_hz = 1U; /* 下限 1Hz */ + freq_hz = 1U; } if (freq_hz > 100000U) { - freq_hz = 100000U; /* 上限 100kHz */ + freq_hz = 100000U; } - ticks = clk_hz / freq_hz; /* ticks≈(PSC+1)×(ARR+1)：一脉冲需要多少个计数时钟周期 */ + ticks = clk_hz / freq_hz; if (ticks < 2UL) { - ticks = 2UL; /* 至少 2 tick，否则无边沿 */ + ticks = 2UL; } - a = ticks / (psc + 1UL); /* 周期计数个数 = ticks / 分频；稍后 a-=1 才是 ARR 寄存器值 */ + a = ticks / (psc + 1UL); if (a < 2UL) { - a = 2UL; /* ARR+1 至少为 2，保证能产生高低边沿 */ + a = 2UL; } if (a > 65536UL) { - a = 65536UL; /* 16 位定时器：ARR+1 最大 65536 → ARR 最大 65535 */ + a = 65536UL; } - a -= 1UL; /* 寄存器值 = 周期计数 − 1 */ + a -= 1UL; - c = (a + 1UL) / 2UL; /* ≈50% 占空：CCR ≈ (ARR+1)/2 */ + c = (a + 1UL) / 2UL; if (c == 0UL) { - c = 1UL; /* CCR 至少 1，避免 PWM1 整拍无高电平 */ + c = 1UL; } - *arr = a; /* 写出 ARR 寄存器值 */ - *ccr = c; /* 写出 CCR 寄存器值 */ + *arr = a; + *ccr = c; } /** @@ -417,26 +407,26 @@ static void PlsrPulseDriverCalcPscArr(uint32_t clk_hz, uint32_t freq_hz, freq_hz = 100000U; } - ticks = clk_hz / freq_hz; /* 同上：一脉冲所需计数时钟 tick 数 */ + ticks = clk_hz / freq_hz; if (ticks < 2UL) { - ticks = 2UL; /* 至少 2 tick */ + ticks = 2UL; } /* 选最小 PSC 使 ARR 落在 16 位内 */ - p = (ticks + 65536UL - 1UL) / 65536UL; /* 向上取整：需要的最小分频倍数 (PSC+1) */ + p = (ticks + 65536UL - 1UL) / 65536UL; if (p == 0UL) { - p = 1UL; /* 分频至少 1（即 PSC=0） */ + p = 1UL; } - p -= 1UL; /* 分频倍数 → PSC 寄存器值 */ + p -= 1UL; if (p > 65535UL) { - p = 65535UL; /* PSC 也是 16 位 */ + p = 65535UL; } *psc = p; - PlsrPulseDriverCalcArrWithPsc(clk_hz, freq_hz, p, arr, ccr); /* 用选定 PSC 再算 ARR/CCR */ + PlsrPulseDriverCalcArrWithPsc(clk_hz, freq_hz, p, arr, ccr); } /** @@ -451,7 +441,7 @@ void PlsrPulseDriverLockPscRange(uint32_t f_min_hz, uint32_t f_max_hz) uint32_t psc_div_min; uint32_t div; uint32_t clk_hz = PlsrPulseDriverTimClkHz( - PlsrPulseDriverHtimByY(PlsrPulseDriverSelectY())); /* Y0/Y2=168M，Y1=84M */ + PlsrPulseDriverHtimByY(PlsrPulseDriverSelectY())); if (f_min_hz < 1U) { @@ -461,15 +451,15 @@ void PlsrPulseDriverLockPscRange(uint32_t f_min_hz, uint32_t f_max_hz) { f_max_hz = 1U; } - if (f_max_hz < f_min_hz) /* 交换，保证 min≤max */ + if (f_max_hz < f_min_hz) { uint32_t temp = f_min_hz; f_min_hz = f_max_hz; f_max_hz = temp; } - ticks_max = clk_hz / f_min_hz; /* 低频 → 最大 tick（周期最长） */ - ticks_min = clk_hz / f_max_hz; /* 高频 → 最小 tick（周期最短） */ + ticks_max = clk_hz / f_min_hz; + ticks_min = clk_hz / f_max_hz; if (ticks_max < 2UL) { ticks_max = 2UL; @@ -479,13 +469,12 @@ void PlsrPulseDriverLockPscRange(uint32_t f_min_hz, uint32_t f_max_hz) ticks_min = 2UL; } - /* psc_div=PSC+1：低频要够大、高频要够小 */ - psc_div_min = (ticks_max + 65535UL) / 65536UL; /* ARR 顶满(65535)时，盖住低频所需的最小分频 */ + psc_div_min = (ticks_max + 65535UL) / 65536UL; if (psc_div_min < 1UL) { psc_div_min = 1UL; } - psc_div_max = ticks_min / 2UL; /* ARR 至少 2 时，高频允许的最大分频 */ + psc_div_max = ticks_min / 2UL; if (psc_div_max < 1UL) { psc_div_max = 1UL; @@ -493,15 +482,15 @@ void PlsrPulseDriverLockPscRange(uint32_t f_min_hz, uint32_t f_max_hz) if (psc_div_min > psc_div_max) { - /* 单一 PSC 盖不住跨度 → 不锁，交 SetFreq 自适应 */ + s_psc_locked = 0U; s_locked_psc = 0U; return; } - div = psc_div_min; /* 取最小分频，ARR 尽量大以提高分辨率 */ - s_locked_psc = div - 1UL; /* 分频倍数 → PSC 寄存器值 */ - s_psc_locked = 1U; /* 本段锁住：ISR/SetFreq 只改 ARR，不换 PSC */ + div = psc_div_min; + s_locked_psc = div - 1UL; + s_psc_locked = 1U; } /** @brief 解除 PSC 锁 */ @@ -514,13 +503,9 @@ void PlsrPulseDriverUnlockPsc(void) /** @brief 停单路：Forced inactive 拉低，关 CEN/UPDATE */ static void PlsrPulseDriverStopHtim(TIM_HandleTypeDef *htim) { - PlsrPulseDriverOcHoldLow(htim); /* 完整停通道：关 CEN/OPM/IT + Forced 拉低 */ + PlsrPulseDriverOcHoldLow(htim); } -/*============================================================================*/ -/* 启停 / 改频 */ -/*============================================================================*/ - /** * @brief 关预装载后直写 ARR/CCR 工作寄存器，再打开预装载并复写影子 * @note 仅 UG 不可靠时（首拍 CCR 工作值仍为 0）使用：避免 CNT=0 整拍无高电平 @@ -528,14 +513,14 @@ static void PlsrPulseDriverStopHtim(TIM_HandleTypeDef *htim) static void PlsrPulseDriverForceArrCcr(TIM_HandleTypeDef *htim, uint32_t arr, uint32_t ccr) { - htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_ARPE); /* 清 ARPE：随后写 ARR 直达工作寄存器 */ - htim->Instance->CCMR1 &= (uint32_t)(~TIM_CCMR1_OC1PE); /* 清 OC1PE：随后写 CCR1 直达工作寄存器 */ - htim->Instance->ARR = arr; /* 直写工作 ARR（本周期立刻用这套周期） */ - htim->Instance->CCR1 = ccr; /* 直写工作 CCR1（本周期立刻用这套占空） */ - htim->Instance->CR1 |= TIM_CR1_ARPE; /* 再开 ARPE：之后写 ARR 进影子 */ - htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; /* 再开 OC1PE：之后写 CCR 进影子 */ - htim->Instance->ARR = arr; /* 复写影子 ARR，与工作值一致，避免影子残留旧频 */ - htim->Instance->CCR1 = ccr; /* 复写影子 CCR1，与工作值一致 */ + htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_ARPE); + htim->Instance->CCMR1 &= (uint32_t)(~TIM_CCMR1_OC1PE); + htim->Instance->ARR = arr; + htim->Instance->CCR1 = ccr; + htim->Instance->CR1 |= TIM_CR1_ARPE; + htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; + htim->Instance->ARR = arr; + htim->Instance->CCR1 = ccr; } /** @@ -547,44 +532,44 @@ static void PlsrPulseDriverLoadRegs(uint32_t freq_hz) uint32_t psc; uint32_t arr; uint32_t ccr; - uint8_t y = PlsrPulseDriverSelectY(); /* 当前配置的 Y0/Y1/Y2 */ - TIM_HandleTypeDef *htim = PlsrPulseDriverHtimByY(y); /* 映射到 TIM10/13/11 */ + uint8_t y = PlsrPulseDriverSelectY(); + TIM_HandleTypeDef *htim = PlsrPulseDriverHtimByY(y); uint32_t clk_hz; - s_req_pending = 0U; /* 重装寄存器时丢弃 ISR 挂起改频 */ + s_req_pending = 0U; if ((s_active_htim != (TIM_HandleTypeDef *)0) && (s_active_htim != htim)) { - PlsrPulseDriverStopHtim(s_active_htim); /* 换路：先停旧 TIM，避免两路同时出脉冲 */ + PlsrPulseDriverStopHtim(s_active_htim); } - s_active_htim = htim; /* 记下当前活动定时器 */ - clk_hz = PlsrPulseDriverTimClkHz(htim); /* TIM10/11→168M，TIM13→84M */ + s_active_htim = htim; + clk_hz = PlsrPulseDriverTimClkHz(htim); if (freq_hz == 0U) { - PlsrPulseDriverStopHtim(htim); /* freq=0：停表 + Forced 拉低 */ + PlsrPulseDriverStopHtim(htim); g_plsr_output_freq_hz = 0U; g_plsr_pwm_running = 0U; s_last_psc = 0xFFFFFFFFUL; s_last_arr = 0xFFFFFFFFUL; s_last_ccr = 0U; - s_regs_prepared = 0U; /* 未处于「已预装待 Commit」状态 */ + s_regs_prepared = 0U; return; } if (s_psc_locked != 0U) { - psc = s_locked_psc; /* 段内锁 PSC：只用锁定值算 ARR */ + psc = s_locked_psc; PlsrPulseDriverCalcArrWithPsc(clk_hz, freq_hz, psc, &arr, &ccr); } else { - PlsrPulseDriverCalcPscArr(clk_hz, freq_hz, &psc, &arr, &ccr); /* 未锁：自适应选 PSC+ARR */ + PlsrPulseDriverCalcPscArr(clk_hz, freq_hz, &psc, &arr, &ccr); } if (ccr < 1U) { - ccr = 1U; /* CCR 最小 1，保证 PWM1 有高电平段 */ + ccr = 1U; } g_plsr_output_freq_hz = freq_hz; @@ -592,22 +577,22 @@ static void PlsrPulseDriverLoadRegs(uint32_t freq_hz) s_last_arr = arr; s_last_ccr = ccr; - __HAL_TIM_DISABLE(htim); /* CEN=0：关计数，DIR_WAIT 期间禁止空转 */ - __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE); /* 关 UPDATE IT，装载过程不进 ISR */ - htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_OPM); /* 清 OPM：退出单脉冲，恢复连续可跑 */ + __HAL_TIM_DISABLE(htim); + __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE); + htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_OPM); + + __HAL_TIM_SET_PRESCALER(htim, psc); + PlsrPulseDriverForceArrCcr(htim, arr, ccr); - __HAL_TIM_SET_PRESCALER(htim, psc); /* 写 PSC（预分频）；真正生效要靠后面 UG */ - PlsrPulseDriverForceArrCcr(htim, arr, ccr); /* 直写工作+影子 ARR/CCR */ + PlsrPulseDriverOcHoldLow(htim); - PlsrPulseDriverOcHoldLow(htim); /* Forced inactive 拉低 + 关 CEN/OPM/IT */ - /* UG：PSC 生效；随后再钉 ARR/CCR/CNT，避免只进影子 */ - htim->Instance->EGR = TIM_EGR_UG; /* 软件触发 UPDATE：装入 PSC，并产生一次更新事件 */ - __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); /* 清 UG 带来的 UIF，避免误进 ISR */ + htim->Instance->EGR = TIM_EGR_UG; + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); - PlsrPulseDriverForceArrCcr(htim, arr, ccr); /* UG 可能改动工作值，再钉一次 ARR/CCR */ - htim->Instance->CNT = 0U; /* 计数归零；此时仍 CEN=0，等 Commit 再开 */ + PlsrPulseDriverForceArrCcr(htim, arr, ccr); + htim->Instance->CNT = 0U; - s_regs_prepared = 1U; /* 标记：寄存器已预装，Commit 可直接开沿 */ + s_regs_prepared = 1U; } /** @@ -620,7 +605,7 @@ static void PlsrPulseDriverLoadRegs(uint32_t freq_hz) */ static void PlsrPulseDriverCommitOutput(void) { - TIM_HandleTypeDef *htim = s_active_htim; /* 当前活动路：TIM10/11/13 之一 */ + TIM_HandleTypeDef *htim = s_active_htim; uint32_t ccmr; uint32_t ccr; @@ -629,16 +614,15 @@ static void PlsrPulseDriverCommitOutput(void) return; } - ccr = (s_last_ccr < 1U) ? 1U : s_last_ccr; /* 用 LoadRegs 记下的 CCR，至少为 1 */ + ccr = (s_last_ccr < 1U) ? 1U : s_last_ccr; - __HAL_TIM_DISABLE(htim); /* 开沿前先确保 CEN=0，寄存器改完再开 */ - __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE); /* 改寄存器窗口先关 UPDATE IT */ + __HAL_TIM_DISABLE(htim); + __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE); - /* 关预装载，直写工作寄存器 */ - htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_ARPE); /* 清 ARPE：ARR 直写工作寄存器 */ - htim->Instance->CCMR1 &= (uint32_t)(~TIM_CCMR1_OC1PE); /* 清 OC1PE：CCR 直写工作寄存器 */ - htim->Instance->ARR = s_last_arr; /* 工作 ARR = 起跳频周期 */ - htim->Instance->CCR1 = ccr; /* 工作 CCR = 起跳占空 */ + htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_ARPE); + htim->Instance->CCMR1 &= (uint32_t)(~TIM_CCMR1_OC1PE); + htim->Instance->ARR = s_last_arr; + htim->Instance->CCR1 = ccr; /* * 先把 CNT 放到周期末端，再切回 PWM 模式。 @@ -649,36 +633,34 @@ static void PlsrPulseDriverCommitOutput(void) * 先写 CNT=ARR 并保持 Forced inactive，再切 PWM1，模式切换期间 * 输出始终为低；真正的首个上升沿只会由后续 UPDATE 产生。 */ - htim->Instance->CNT = s_last_arr; /* CNT=ARR：下一节拍溢出→UPDATE，产生首上升沿 */ + htim->Instance->CNT = s_last_arr; - ccmr = htim->Instance->CCMR1; /* 读 CCMR1，准备改 OC1M */ - ccmr &= (uint32_t)(~TIM_CCMR1_OC1M); /* 清 OC1M[6:4] */ - ccmr |= TIM_OCMODE_PWM1; /* 写入 PWM1：CNTInstance->CCMR1 = ccmr; /* 从 Forced inactive 切回 PWM1 */ - htim->Instance->CCER |= TIM_CCER_CC1E; /* CC1E=1：CH1 输出到 GPIO */ + ccmr = htim->Instance->CCMR1; + ccmr &= (uint32_t)(~TIM_CCMR1_OC1M); + ccmr |= TIM_OCMODE_PWM1; + htim->Instance->CCMR1 = ccmr; + htim->Instance->CCER |= TIM_CCER_CC1E; /* * 先打开预装载并把影子钉成同一频率，再 ENABLE。 * 否则首拍 ISR 的 SetFreqIsr 可能落在 ARPE=0 窗口，直接改写工作 ARR。 */ - htim->Instance->CR1 |= TIM_CR1_ARPE; /* 开 ARPE：后续改频写影子 */ - htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; /* 开 OC1PE：CCR 同样走影子 */ - htim->Instance->ARR = s_last_arr; /* 影子 ARR = 起跳频（与工作一致） */ - htim->Instance->CCR1 = ccr; /* 影子 CCR 同步 */ + htim->Instance->CR1 |= TIM_CR1_ARPE; + htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; + htim->Instance->ARR = s_last_arr; + htim->Instance->CCR1 = ccr; - /* CNT 已在切换 PWM 模式前归位，下面仅清状态并开表。 */ - __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); /* 清 UIF，避免一开 IT 就误进 */ + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); - __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE); /* 开 UPDATE IT：每周期末进脉冲 ISR 改频/计数 */ - __HAL_TIM_ENABLE(htim); /* CEN=1：开始计数，下一拍 UPDATE 出首沿 */ + __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE); + __HAL_TIM_ENABLE(htim); g_plsr_pwm_running = 1U; - /* 记录起跳工作寄存器，供首拍 ISR 保护当前周期 */ s_protect_arr = s_last_arr; s_protect_ccr = ccr; - s_protect_start_period = 1U; /* 下一次 SetFreqIsr 须先钉住工作 ARR */ + s_protect_start_period = 1U; - s_regs_prepared = 0U; /* Commit 完成，预装标志清掉 */ + s_regs_prepared = 0U; } /** @@ -688,18 +670,18 @@ static void PlsrPulseDriverStartCommon(uint32_t freq_hz) { if ((s_regs_prepared == 0U) || (freq_hz != g_plsr_output_freq_hz) || (freq_hz == 0U)) { - PlsrPulseDriverLoadRegs(freq_hz); /* 未预装或频率变了 → 重装 */ + PlsrPulseDriverLoadRegs(freq_hz); } if (freq_hz != 0U) { - PlsrPulseDriverCommitOutput(); /* 开 CEN + PWM1 */ + PlsrPulseDriverCommitOutput(); } } /** @brief 换向延时期间预装寄存器、CEN 保持关（DIR_WAIT） */ void PlsrPulseDriverPrepare(uint32_t freq_hz) { - PlsrPulseDriverLoadRegs(freq_hz); /* 只装 PSC/ARR/CCR，CEN 仍关；TIM5 延时到期再 Start */ + PlsrPulseDriverLoadRegs(freq_hz); } /** @brief 任务上下文改频（可换 PSC；同 PSC 只写影子） */ @@ -713,17 +695,17 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz) if (freq_hz == g_plsr_output_freq_hz) { - return; /* 频率未变 */ + return; } htim = (s_active_htim != (TIM_HandleTypeDef *)0) ? - s_active_htim : PlsrPulseDriverHtimByY(PlsrPulseDriverSelectY()); /* 无活动句柄则按配置选 TIM */ + s_active_htim : PlsrPulseDriverHtimByY(PlsrPulseDriverSelectY()); s_active_htim = htim; - clk_hz = PlsrPulseDriverTimClkHz(htim); /* 按 TIM10/11/13 取 168M 或 84M */ + clk_hz = PlsrPulseDriverTimClkHz(htim); if (freq_hz == 0U) { - PlsrPulseDriverStopHtim(htim); /* 停表拉低 */ + PlsrPulseDriverStopHtim(htim); g_plsr_output_freq_hz = 0U; g_plsr_pwm_running = 0U; s_last_psc = 0xFFFFFFFFUL; @@ -734,40 +716,40 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz) if (s_psc_locked != 0U) { psc = s_locked_psc; - PlsrPulseDriverCalcArrWithPsc(clk_hz, freq_hz, psc, &arr, &ccr); /* 锁 PSC：只算 ARR/CCR */ + PlsrPulseDriverCalcArrWithPsc(clk_hz, freq_hz, psc, &arr, &ccr); } else { - PlsrPulseDriverCalcPscArr(clk_hz, freq_hz, &psc, &arr, &ccr); /* 未锁：可换 PSC */ + PlsrPulseDriverCalcPscArr(clk_hz, freq_hz, &psc, &arr, &ccr); } if ((psc == s_last_psc) && (arr == s_last_arr)) { - g_plsr_output_freq_hz = freq_hz; /* 量化结果相同，只更新镜像 */ + g_plsr_output_freq_hz = freq_hz; return; } if ((s_psc_locked != 0U) || (psc == s_last_psc)) { - /* 同 PSC：写影子，下一 UPDATE 切换 */ - __HAL_TIM_SET_AUTORELOAD(htim, arr); /* 写 ARR 影子（ARPE=1 时本周期不受影响） */ - __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr); /* 写 CCR1 影子（OC1PE=1） */ - __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE); /* 确保 UPDATE IT 开着，ISR 能继续改频 */ + + __HAL_TIM_SET_AUTORELOAD(htim, arr); + __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr); + __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE); } else { - /* 换 PSC：须停表再装载 */ - __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE); /* 先关 UPDATE IT */ - __HAL_TIM_DISABLE(htim); /* CEN=0：停计数才能安全换 PSC */ - __HAL_TIM_SET_PRESCALER(htim, psc); /* 写新 PSC */ - __HAL_TIM_SET_AUTORELOAD(htim, arr); /* 写新 ARR */ - __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr); /* 写新 CCR */ - __HAL_TIM_SET_COUNTER(htim, 0U); /* CNT=0 */ - htim->Instance->EGR = TIM_EGR_UG; /* UG：让新 PSC 立即生效，并产生 UPDATE */ - __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); /* 清 UG 产生的 UIF */ + + __HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE); + __HAL_TIM_DISABLE(htim); + __HAL_TIM_SET_PRESCALER(htim, psc); + __HAL_TIM_SET_AUTORELOAD(htim, arr); + __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr); + __HAL_TIM_SET_COUNTER(htim, 0U); + htim->Instance->EGR = TIM_EGR_UG; + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); - __HAL_TIM_ENABLE(htim); /* CEN=1：重新开跑 */ - __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE); /* 再开 UPDATE IT */ + __HAL_TIM_ENABLE(htim); + __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE); } g_plsr_output_freq_hz = freq_hz; @@ -784,17 +766,17 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) uint32_t clk_hz; TIM_HandleTypeDef *htim; - htim = s_active_htim; /* ISR 只改当前活动 TIM */ + htim = s_active_htim; if (htim == (TIM_HandleTypeDef *)0) { if (freq_hz == 0U) { s_req_freq = 0U; - s_req_pending = 1U; /* 无句柄：停表请求挂起给任务 */ + s_req_pending = 1U; return; } s_req_freq = freq_hz; - s_req_pending = 1U; /* 无句柄：改频请求挂起给任务 */ + s_req_pending = 1U; return; } @@ -805,22 +787,21 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) */ if (s_protect_start_period != 0U) { - s_protect_start_period = 0U; /* 首拍保护只用一次 */ - htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_ARPE); /* 临时关 ARPE，直写工作 ARR */ - htim->Instance->CCMR1 &= (uint32_t)(~TIM_CCMR1_OC1PE); /* 临时关 OC1PE，直写工作 CCR */ - htim->Instance->ARR = s_protect_arr; /* 钉住本拍工作 ARR=起跳频，防止被下一频覆盖 */ + s_protect_start_period = 0U; + htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_ARPE); + htim->Instance->CCMR1 &= (uint32_t)(~TIM_CCMR1_OC1PE); + htim->Instance->ARR = s_protect_arr; htim->Instance->CCR1 = (s_protect_ccr < 1U) ? 1U : s_protect_ccr; - htim->Instance->CR1 |= TIM_CR1_ARPE; /* 立刻再开 ARPE：后面写影子才安全 */ - htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; /* 再开 OC1PE */ + htim->Instance->CR1 |= TIM_CR1_ARPE; + htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; } else { - /* 常规路径：确保预装载开启，禁止直写工作寄存器 */ - htim->Instance->CR1 |= TIM_CR1_ARPE; /* 保证 ARPE=1 */ - htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; /* 保证 OC1PE=1 */ + + htim->Instance->CR1 |= TIM_CR1_ARPE; + htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; } - /* 匀速热路径：频率未变 → 不算量化、不写寄存器 */ if (freq_hz == g_plsr_output_freq_hz) { return; @@ -829,7 +810,7 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) if (freq_hz == 0U) { s_req_freq = 0U; - s_req_pending = 1U; /* ISR 内不停表，停表留给任务 ApplyPending */ + s_req_pending = 1U; return; } @@ -838,14 +819,14 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) if (s_psc_locked != 0U) { psc = s_locked_psc; - PlsrPulseDriverCalcArrWithPsc(clk_hz, freq_hz, psc, &arr, &ccr); /* 锁 PSC：ISR 只改 ARR */ + PlsrPulseDriverCalcArrWithPsc(clk_hz, freq_hz, psc, &arr, &ccr); } else { PlsrPulseDriverCalcPscArr(clk_hz, freq_hz, &psc, &arr, &ccr); if (psc != s_last_psc) { - /* ISR 内禁止停表换 PSC，留给任务 */ + s_req_freq = freq_hz; s_req_pending = 1U; return; @@ -854,7 +835,7 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) if ((psc == s_last_psc) && (arr == s_last_arr)) { - g_plsr_output_freq_hz = freq_hz; /* 量化未变，只更新镜像 */ + g_plsr_output_freq_hz = freq_hz; return; } @@ -863,9 +844,8 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) ccr = 1U; } - /* 此时 ARPE 已开：只改影子，下一 UPDATE 才切换，当前起跳周期不受影响 */ - __HAL_TIM_SET_AUTORELOAD(htim, arr); /* 写 ARR 影子 */ - __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr); /* 写 CCR1 影子 */ + __HAL_TIM_SET_AUTORELOAD(htim, arr); + __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr); g_plsr_output_freq_hz = freq_hz; s_last_psc = psc; @@ -876,13 +856,13 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) /** @brief 丢弃挂起 */ void PlsrPulseDriverClearPending(void) { - s_req_pending = 0U; /* 丢掉 ISR 挂起的改频请求 */ + s_req_pending = 0U; } /** @brief 取消冷启动首拍保护*/ void PlsrPulseDriverClearStartPeriodProtect(void) { - s_protect_start_period = 0U; /* 进入匀速后不再钉工作 ARR */ + s_protect_start_period = 0U; } /** @brief 消费挂起改频 */ @@ -892,22 +872,22 @@ void PlsrPulseDriverApplyPending(void) if (s_req_pending == 0U) { - return; /* 无挂起 */ + return; } s_req_pending = 0U; f = s_req_freq; if (f == 0U) { - PlsrPulseDriverStop(); /* 挂起的是停表 */ + PlsrPulseDriverStop(); return; } - PlsrPulseDriverSetFreq(f); /* 任务上下文可换 PSC */ + PlsrPulseDriverSetFreq(f); } /** @brief 开 PWM + UPDATE 中断 */ void PlsrPulseDriverStart(uint32_t freq_hz) { - PlsrPulseDriverStartCommon(freq_hz); /* LoadRegs（必要时）→ Commit：PWM1 + CEN + UPDATE IT */ + PlsrPulseDriverStartCommon(freq_hz); } /** @brief 停三路 */ @@ -916,9 +896,9 @@ void PlsrPulseDriverStop(void) s_req_pending = 0U; s_regs_prepared = 0U; s_protect_start_period = 0U; - PlsrPulseDriverStopHtim(&htim10); /* 停 TIM10=Y0：Forced + 关 CEN */ - PlsrPulseDriverStopHtim(&htim11); /* 停 TIM11=Y2 */ - PlsrPulseDriverStopHtim(&htim13); /* 停 TIM13=Y1 */ + PlsrPulseDriverStopHtim(&htim10); + PlsrPulseDriverStopHtim(&htim11); + PlsrPulseDriverStopHtim(&htim13); g_plsr_output_freq_hz = 0U; g_plsr_pwm_running = 0U; s_last_psc = 0xFFFFFFFFUL; @@ -939,35 +919,34 @@ void PlsrPulseDriverArmActStop(void) return; } - /* OPM 先武装：当前周期结束的 UPDATE 到来时自动关 CEN。 */ - htim->Instance->CR1 |= TIM_CR1_OPM; /* OPM=1：单脉冲模式，下一次 UPDATE 后自动清 CEN */ + htim->Instance->CR1 |= TIM_CR1_OPM; - cnt = htim->Instance->CNT; /* 读当前计数位置 */ - ccr = htim->Instance->CCR1; /* 读比较值：CNTInstance->CNT; + ccr = htim->Instance->CCR1; if (((htim->Instance->CR1 & TIM_CR1_CEN) != 0U) && (cnt < ccr)) { - /* 当前仍为高电平：等本拍 CC1 下降沿后再锁低。 */ - s_seg_end_fall_armed = 1U; /* 武装：等 CC1 匹配（≈下降沿）回调里 Forced */ - __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC1); /* 清旧 CC1IF，避免立刻误触发 */ - __HAL_TIM_ENABLE_IT(htim, TIM_IT_CC1); /* 开 CC1 IT：高→低时进 PulseFinishedCallback */ + + s_seg_end_fall_armed = 1U; + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC1); + __HAL_TIM_ENABLE_IT(htim, TIM_IT_CC1); /* * 读取到高电平后，CC1 可能恰好在开中断前已经过去。 * 再检查一次；若已进入低电平或 OPM 已停表，立即锁低。 */ - cnt = htim->Instance->CNT; /* 再读 CNT，防竞态漏边沿 */ + cnt = htim->Instance->CNT; if (((htim->Instance->CR1 & TIM_CR1_CEN) == 0U) || (cnt >= ccr)) { - PlsrPulseDriverClearSegEndFallArm(htim); /* 撤 CC1 武装（关 CC1 IT），不是 Forced */ - PlsrPulseDriverOcForceLowKeepRun(htim); /* Forced inactive 拉低，CEN/OPM 继续跑到周期末 */ + PlsrPulseDriverClearSegEndFallArm(htim); + PlsrPulseDriverOcForceLowKeepRun(htim); } } else { - /* ACT 落在低电平阶段（或边界已停表）：无需再等 CC1。 */ - PlsrPulseDriverClearSegEndFallArm(htim); /* 撤 CC1 武装 */ - PlsrPulseDriverOcForceLowKeepRun(htim); /* 立刻 Forced 拉低，等 OPM 在周期末关 CEN */ + + PlsrPulseDriverClearSegEndFallArm(htim); + PlsrPulseDriverOcForceLowKeepRun(htim); } } @@ -983,10 +962,10 @@ void PlsrPulseDriverArmSegEndStop(void) return; } - htim->Instance->CR1 |= TIM_CR1_OPM; /* 周期末自动关 CEN */ - s_seg_end_fall_armed = 1U; /* 武装末拍下降沿拉低 */ - __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC1); /* 清 CC1IF */ - __HAL_TIM_ENABLE_IT(htim, TIM_IT_CC1); /* 末拍下降沿 → Forced 拉低 */ + htim->Instance->CR1 |= TIM_CR1_OPM; + s_seg_end_fall_armed = 1U; + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_CC1); + __HAL_TIM_ENABLE_IT(htim, TIM_IT_CC1); } /** @brief HAL PWM 脉宽结束（CC1 匹配 ≈ 下降沿） */ @@ -996,11 +975,11 @@ void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) (htim != s_active_htim) || (PlsrPulseDriverIsPulseTim(htim->Instance) == 0U)) { - return; /* 未武装 / 非活动路 / 非脉冲 TIM（TIM10/11/13）→ 忽略 */ + return; } - PlsrPulseDriverClearSegEndFallArm(htim); /* 先撤 CC1 武装（关 IT），不是 Forced */ - PlsrPulseDriverOcForceLowKeepRun(htim); /* 再 Forced inactive 拉低；CEN 仍跑，等 OPM 关表 */ + PlsrPulseDriverClearSegEndFallArm(htim); + PlsrPulseDriverOcForceLowKeepRun(htim); } /** @brief 取消 OPM，并撤 CC1 下降沿武装（不停表、不 Forced） */ @@ -1012,8 +991,8 @@ void PlsrPulseDriverClearOnePulseStop(void) { return; } - htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_OPM); /* 清 OPM：恢复连续计数，段衔接时不停表 */ - PlsrPulseDriverClearSegEndFallArm(htim); /* 仅撤 CC1 武装 */ + htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_OPM); + PlsrPulseDriverClearSegEndFallArm(htim); } /** @brief OPM 段末 UPDATE 后立刻拉低，消除 CNT 回零 PWM1 假上升沿 */ @@ -1025,6 +1004,6 @@ void PlsrPulseDriverHoldOutputLow(void) { return; } - PlsrPulseDriverStopHtim(htim); /* Forced + 关 CEN/OPM/IT，避免 CNT 回零时 PWM1 假上升沿 */ + PlsrPulseDriverStopHtim(htim); g_plsr_pwm_running = 0U; } diff --git a/plsr/pulse_driver/plsr_pulse_driver.h b/plsr/pulse_driver/plsr_pulse_driver.h index 8a65b08..1d14748 100644 --- a/plsr/pulse_driver/plsr_pulse_driver.h +++ b/plsr/pulse_driver/plsr_pulse_driver.h @@ -34,11 +34,10 @@ extern TIM_HandleTypeDef htim10; extern TIM_HandleTypeDef htim11; extern TIM_HandleTypeDef htim13; -/* 驱动拥有的真实输出状态，run_control 直接读取，不再保存第二份。 */ -extern volatile uint32_t g_plsr_output_freq_hz; /* 当前输出频率 Hz */ -extern volatile uint8_t g_plsr_pwm_running; /* 1=PWM 正在跑 */ -extern uint8_t g_plsr_direction_forward; /* 1=正转方向语义 */ -extern uint8_t g_plsr_direction_valid; /* 1=方向脚已有效写出 */ +extern volatile uint32_t g_plsr_output_freq_hz; +extern volatile uint8_t g_plsr_pwm_running; +extern uint8_t g_plsr_direction_forward; +extern uint8_t g_plsr_direction_valid; /** * @brief 初始化三路脉冲 TIM + 方向 GPIO diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index dc7a04b..2c06196 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -20,10 +20,10 @@ * RC_WAIT_COND — 段后等待：TIM5 时间 或 WAIT/EXT 沿 */ typedef enum { - RC_IDLE = 0, /* 空闲 */ - RC_DIR_WAIT, /* 等换向延时 */ - RC_RUN, /* 正在发脉冲 */ - RC_WAIT_COND /* 段后等待 */ + RC_IDLE = 0, + RC_DIR_WAIT, + RC_RUN, + RC_WAIT_COND } RcState_e; /* @@ -33,43 +33,41 @@ typedef enum { * PH_DECEL — 段末过渡 → f_end（可升可降） */ typedef enum { - PH_APPROACH = 0, /* 起速爬峰 */ - PH_CONST, /* 匀速 */ - PH_DECEL /* 段末过渡 */ + PH_APPROACH = 0, + PH_CONST, + PH_DECEL } RunPhase_e; -volatile uint8_t g_plsr_busy; /* 运行忙标志（上位机监控） */ -volatile uint16_t g_plsr_cur_seg_idx; /* 当前段下标（0-based） */ -volatile int32_t g_plsr_accumulated_pulses; /* 全局累计脉冲（有符号） */ -static volatile RcState_e s_state; /* 主状态机：IDLE/DIR_WAIT/RUN/WAIT_COND */ -static volatile uint8_t s_forward; /* 当前段脉冲方向：1=正转，0=反转 */ -static volatile int32_t s_seg_pulses_done; /* 本段已发脉冲数 */ -static volatile int32_t s_seg_pulse_target; /* 本段目标脉冲数（绝对值） */ -static int32_t s_absolute_origin; /* 绝对定位坐标原点 */ -static uint8_t s_absolute_origin_locked; /* 原点是否已锁定（CLR 后重锁） */ -static volatile uint8_t s_act_timer_armed; /* ACT 定时器是否已启动 */ -static volatile uint8_t s_act_expired; /* ACT 到期请求（TickMs 消费） */ -static volatile uint8_t s_act_waiting_for_pulse_boundary; /* ACT 到期后等脉冲边界切段 */ -static volatile uint8_t s_keep_freq_after_act; /* ACT 切段后保持当前频率衔接 */ -static volatile uint8_t s_wait_time_expired; /* 段后 WAIT 时间到期请求 */ -static uint8_t s_waiting_for_input_signal; /* 段后等 WAIT/EXT 输入沿 */ -static volatile RunPhase_e s_phase; /* 段内相：加速/匀速/减速 */ -static uint8_t s_continue_without_stopping; /* 段间不停表衔接（FOLLOW/keep） */ -static uint32_t s_next_seg_start_freq_hz; /* 段间衔接起始频率 Hz */ -static uint8_t s_next_seg_start_freq_valid; /* 衔接频率是否有效 */ -static uint8_t s_stop_after_last_pulse; /* 末拍后完整停表（完成模式） */ -static volatile uint8_t s_waiting_for_last_period; /* 等末拍 UPDATE 再收尾 */ -static OS_EVENT *s_wake_sem; /* 唤醒 PlsrTask 的信号量 */ - -/* 策略层解析后交给 PlanSeg 的段端点 */ +volatile uint8_t g_plsr_busy; +volatile uint16_t g_plsr_cur_seg_idx; +volatile int32_t g_plsr_accumulated_pulses; +static volatile RcState_e s_state; +static volatile uint8_t s_forward; +static volatile int32_t s_seg_pulses_done; +static volatile int32_t s_seg_pulse_target; +static int32_t s_absolute_origin; +static uint8_t s_absolute_origin_locked; +static volatile uint8_t s_act_timer_armed; +static volatile uint8_t s_act_expired; +static volatile uint8_t s_act_waiting_for_pulse_boundary; +static volatile uint8_t s_keep_freq_after_act; +static volatile uint8_t s_wait_time_expired; +static uint8_t s_waiting_for_input_signal; +static volatile RunPhase_e s_phase; +static uint8_t s_continue_without_stopping; +static uint32_t s_next_seg_start_freq_hz; +static uint8_t s_next_seg_start_freq_valid; +static uint8_t s_stop_after_last_pulse; +static volatile uint8_t s_waiting_for_last_period; +static OS_EVENT *s_wake_sem; + typedef struct { - uint32_t freq_start_hz; /* 本段入口频率 Hz（>=1，由衔接/起速解析） */ - uint32_t freq_end_hz; /* 本段出口频率 Hz（由 wait/send/方向解析） */ - uint16_t accel_ms; /* 本段加速参考时间 ms */ - uint16_t decel_ms; /* 本段减速参考时间 ms */ + uint32_t freq_start_hz; + uint32_t freq_end_hz; + uint16_t accel_ms; + uint16_t decel_ms; } PlsrSegPlanEndpoints_t; -/* 静态辅助函数前置声明 */ static void PlsrRunControlWakeDrain(void); static void PlsrRunControlWakePost(void); static int16_t PlsrRunControlNextSeg(uint16_t cur_seg_idx); @@ -114,38 +112,34 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, uint32_t f_tgt, uint32_t f_end, uint16_t accel_ms, uint16_t decel_ms); -/*============================================================================*/ -/* 公共 API */ -/*============================================================================*/ - /** @brief 运行控制初始化 */ void PlsrRunControlInit(void) { - s_state = RC_IDLE; /* 主状态机：空闲 */ - g_plsr_busy = 0U; /* 运行忙标志（上位机监控） */ - s_forward = 1U; /* 当前段脉冲方向：1=正转 */ - g_plsr_cur_seg_idx = 0U; /* 当前段下标（0-based） */ - s_seg_pulses_done = 0; /* 本段已发脉冲数 */ - s_seg_pulse_target = 0; /* 本段目标脉冲数（绝对值） */ - g_plsr_accumulated_pulses = 0; /* 全局累计脉冲（有符号） */ - s_absolute_origin = 0; /* 绝对定位坐标原点 */ - s_absolute_origin_locked = 0U; /* 原点是否已锁定（CLR 后重锁） */ - s_continue_without_stopping = 0U; /* 段间不停表衔接（FOLLOW/keep） */ - s_next_seg_start_freq_hz = 0U; /* 段间衔接起始频率 Hz */ - s_next_seg_start_freq_valid = 0U; /* 衔接频率是否有效 */ - g_plsr_accel_plan.accel_pulses = 0U; /* 规划：加速相脉冲预算 */ - g_plsr_accel_plan.decel_pulses = 0U; /* 规划：减速相脉冲预算 */ - g_plsr_accel_runtime.total_pulses = 0U; /* 当前相计划总脉冲 */ - s_stop_after_last_pulse = 0U; /* 末拍后完整停表（完成模式） */ - s_waiting_for_last_period = 0U; /* 等末拍 UPDATE 再收尾 */ - s_phase = PH_CONST; /* 段内相：加速/匀速/减速 */ - s_act_timer_armed = 0U; /* ACT 定时器是否已启动 */ - s_act_expired = 0U; /* ACT 到期请求（TickMs 消费） */ - s_act_waiting_for_pulse_boundary = 0U; /* ACT 到期后等脉冲边界切段 */ - s_keep_freq_after_act = 0U; /* ACT 切段后保持当前频率衔接 */ - s_wait_time_expired = 0U; /* 段后 WAIT 时间到期请求 */ - s_waiting_for_input_signal = 0U; /* 段后等 WAIT/EXT 输入沿 */ - /* s_wake_sem 在 OSInit 后由 WakeInit 创建 */ + s_state = RC_IDLE; + g_plsr_busy = 0U; + s_forward = 1U; + g_plsr_cur_seg_idx = 0U; + s_seg_pulses_done = 0; + s_seg_pulse_target = 0; + g_plsr_accumulated_pulses = 0; + s_absolute_origin = 0; + s_absolute_origin_locked = 0U; + s_continue_without_stopping = 0U; + s_next_seg_start_freq_hz = 0U; + s_next_seg_start_freq_valid = 0U; + g_plsr_accel_plan.accel_pulses = 0U; + g_plsr_accel_plan.decel_pulses = 0U; + g_plsr_accel_runtime.total_pulses = 0U; + s_stop_after_last_pulse = 0U; + s_waiting_for_last_period = 0U; + s_phase = PH_CONST; + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; + s_waiting_for_input_signal = 0U; + } /** @@ -153,9 +147,9 @@ void PlsrRunControlInit(void) */ void PlsrRunControlWakeInit(void) { - if (s_wake_sem == (OS_EVENT *)0) /* 信号量还没建 */ + if (s_wake_sem == (OS_EVENT *)0) { - s_wake_sem = OSSemCreate(0U); /* 建一个空信号量 */ + s_wake_sem = OSSemCreate(0U); } } @@ -166,17 +160,17 @@ void PlsrRunControlWakePend(uint16_t timeout_ticks) { INT8U err; - if (s_wake_sem == (OS_EVENT *)0) /* 没信号量就退回延时 */ + if (s_wake_sem == (OS_EVENT *)0) { - if (timeout_ticks < 1U) /* 至少延 1 tick */ + if (timeout_ticks < 1U) { - timeout_ticks = 1U; /* 补成最短延时 */ + timeout_ticks = 1U; } - OSTimeDly((INT32U)timeout_ticks); /* 没有唤醒量就干等 */ - return; /* 直接回去 */ + OSTimeDly((INT32U)timeout_ticks); + return; } - OSSemPend(s_wake_sem, timeout_ticks, &err); /* 挂起等唤醒或超时 */ - (void)err; /* 忽略返回码 */ + OSSemPend(s_wake_sem, timeout_ticks, &err); + (void)err; } /** @@ -184,15 +178,15 @@ void PlsrRunControlWakePend(uint16_t timeout_ticks) */ uint8_t PlsrRunControlIsPreciseWait(void) { - if (s_state == RC_DIR_WAIT) /* 正在等换向延时 */ + if (s_state == RC_DIR_WAIT) { - return 1U; /* 是精确等待 */ + return 1U; } - if ((s_state == RC_WAIT_COND) && (s_waiting_for_input_signal == 0U)) /* 段后纯时间等待（不读沿） */ + if ((s_state == RC_WAIT_COND) && (s_waiting_for_input_signal == 0U)) { - return 1U; /* 也是精确等待 */ + return 1U; } - return 0U; /* 其它不算精确等待 */ + return 0U; } /** @brief 启动多段序列 */ @@ -200,47 +194,47 @@ uint8_t PlsrStart(uint16_t start_seg) { uint16_t n; uint16_t first_seg; - uint32_t f_chk;/* 解析后的首段目标频率；此处仅作合法性检查，结果不用 */ + uint32_t f_chk; - if (g_plsr_busy != 0U) /* 已经在跑就拒绝再启 */ + if (g_plsr_busy != 0U) { - return 0U; /* 忙着，启不动 */ + return 0U; } - n = PlsrParamGetSegCount();/*获取段的数量*/ - if (n < 1U) /* 一段都没有 */ + n = PlsrParamGetSegCount(); + if (n < 1U) { - return 0U; /* 没段可跑 */ + return 0U; } - if ((start_seg < 1U) || (start_seg > n)) /* 起始段号越界 */ + if ((start_seg < 1U) || (start_seg > n)) { - start_seg = PlsrParamGetStartSeg();/*获取起始段*/ + start_seg = PlsrParamGetStartSeg(); } - first_seg = (uint16_t)(start_seg - 1U); /* 改成 0 起算下标 */ - /* 启动前先检查首段频率，非法时不置忙、不开定时器。 */ - if (PlsrRunControlGetSegTargetFreq(g_plsr_segs[first_seg].freq_hz, /* 首段频率合法性检查 */ + first_seg = (uint16_t)(start_seg - 1U); + + if (PlsrRunControlGetSegTargetFreq(g_plsr_segs[first_seg].freq_hz, g_plsr_config.default_speed, &f_chk) == 0U) { - PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); /* 频率不合法，报故障 */ - return 0U; /* 不置忙、不开表 */ + PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); + return 0U; } - s_continue_without_stopping = 0U;/* 清段间不停表衔接标志 */ - s_next_seg_start_freq_valid = 0U;/* 清段间衔接频率有效标志 */ - s_next_seg_start_freq_hz = 0U;/*段间衔接频率置零*/ + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; /* * 绝对原点：仅在首次 Start（或 ClearAccPulse 之后的第一次）锁定。 * 相对跑完再切绝对，仍相对「第一次启动时刻」坐标，不以当前位置重定原点。 */ - if (s_absolute_origin_locked == 0U) /* 原点还没锁过 */ + if (s_absolute_origin_locked == 0U) { - s_absolute_origin = g_plsr_accumulated_pulses;/*已经累计的脉冲*/ - s_absolute_origin_locked = 1U;/*绝对原点锁定标志*/ + s_absolute_origin = g_plsr_accumulated_pulses; + s_absolute_origin_locked = 1U; } - PlsrSignalIoClearEdges();/*清除io信号边沿*/ - PlsrRunControlBeginSeg(first_seg);/*根据段号开一段*/ - return 1U; /* 启动成功 */ + PlsrSignalIoClearEdges(); + PlsrRunControlBeginSeg(first_seg); + return 1U; } /** @@ -248,137 +242,137 @@ uint8_t PlsrStart(uint16_t start_seg) */ void PlsrStop(void) { - PlsrSignalIoCancelDelay(); /* 取消 TIM5 方向/ACT/WAIT 延时 */ - PlsrPulseDriverStop(); /* 停掉脉冲输出 */ - PlsrPulseDriverUnlockPsc(); /* 解开 PSC 锁定 */ - PlsrPulseDriverClearDir(); /* 方向脚拉回默认 */ - g_plsr_busy = 0U; /* 清忙标志 */ - s_state = RC_IDLE; /* 回到空闲 */ - s_continue_without_stopping = 0U; /* 清不停表衔接 */ - s_next_seg_start_freq_valid = 0U; /* 清链式起速 */ - s_next_seg_start_freq_hz = 0U; /* 衔接频率清零 */ - s_act_timer_armed = 0U; /* 清「ACT 定时已开」标志 */ - s_act_expired = 0U; /* 清 ACT 到期请求 */ - s_act_waiting_for_pulse_boundary = 0U; /* 清脉冲边界切段 */ - s_keep_freq_after_act = 0U; /* 清 ACT 频率衔接 */ - s_wait_time_expired = 0U; /* 清 WAIT 时间到期 */ - s_waiting_for_last_period = 0U; /* 清末拍收尾等待 */ + PlsrSignalIoCancelDelay(); + PlsrPulseDriverStop(); + PlsrPulseDriverUnlockPsc(); + PlsrPulseDriverClearDir(); + g_plsr_busy = 0U; + s_state = RC_IDLE; + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; + s_waiting_for_last_period = 0U; } /** @brief 运行中改频 */ uint8_t PlsrChangeFreq(uint32_t new_tgt_hz) { - uint32_t remain; /* 本段剩余脉冲，作新规划总量 */ - uint32_t f_end; /* 保留原出口频率 */ + uint32_t remain; + uint32_t f_end; - if (s_state != RC_RUN) /* 不在发脉冲就别改 */ + if (s_state != RC_RUN) { - return 0U; /* 拒收 */ + return 0U; } - /* 0x04：动态改频目标非法则拒收，保持原规划 */ - if ((new_tgt_hz < 1U) || (new_tgt_hz > 100000U)) /* 新目标超出 1~100kHz */ + + if ((new_tgt_hz < 1U) || (new_tgt_hz > 100000U)) { - PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); /* 频率越界报故障 */ - return 0U; /* 拒收新频率 */ + PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); + return 0U; } - if (s_seg_pulses_done >= s_seg_pulse_target) /* 本段脉冲已经发完 */ + if (s_seg_pulses_done >= s_seg_pulse_target) { - return 0U; /* 已发完，改频无意义 */ + return 0U; } - remain = (uint32_t)(s_seg_pulse_target - s_seg_pulses_done); /* 算还剩多少脉冲 */ - f_end = g_plsr_accel_plan.end_freq_hz; /* 出口频率先留着 */ - /* 从当前输出频重规划到新目标，出口不变 */ - PlsrRunControlPlanSeg(remain, g_plsr_output_freq_hz, new_tgt_hz, f_end, /* 按剩余脉冲重规划加减速 */ + remain = (uint32_t)(s_seg_pulse_target - s_seg_pulses_done); + f_end = g_plsr_accel_plan.end_freq_hz; + + PlsrRunControlPlanSeg(remain, g_plsr_output_freq_hz, new_tgt_hz, f_end, g_plsr_config.accel_ms, g_plsr_config.decel_ms); - s_seg_pulse_target = (int32_t)remain; /* 剩余作新段目标 */ - s_seg_pulses_done = 0; /* 计数归零对齐新规划 */ - PlsrRunControlRefreshProfile(0U); /* 已在跑：只刷新下一拍频率 */ - return 1U; /* 改频成功 */ + s_seg_pulse_target = (int32_t)remain; + s_seg_pulses_done = 0; + PlsrRunControlRefreshProfile(0U); + return 1U; } /** @brief 清累计与绝对原点 */ void PlsrClearAccPulse(void) { - g_plsr_accumulated_pulses = 0; /* 累计脉冲清零 */ - s_absolute_origin = 0; /* 原点也清掉 */ - s_absolute_origin_locked = 0U; /* 下次 Start 重新锁定原点 */ + g_plsr_accumulated_pulses = 0; + s_absolute_origin = 0; + s_absolute_origin_locked = 0U; } /** @brief 上电恢复累计 */ void PlsrRunControlRestoreAccPulse(int32_t acc_pulse) { - g_plsr_accumulated_pulses = acc_pulse; /* 写回上电保存的累计 */ - s_absolute_origin = 0; /* 原点待下次 Start 再锁 */ - s_absolute_origin_locked = 0U; /* 下次 Start 以当前累计为绝对原点 */ + g_plsr_accumulated_pulses = acc_pulse; + s_absolute_origin = 0; + s_absolute_origin_locked = 0U; } /** @brief 毫秒任务节拍 */ void PlsrRunControlTickMs(void) { - /* ACT 到期请求：切下一段（含 keep 频率衔接） */ - if (s_act_expired != 0U) /* 有 ACT 时间到请求 */ + + if (s_act_expired != 0U) { - s_act_expired = 0U; /* 先把请求清掉 */ - if ((s_state == RC_RUN) || (s_keep_freq_after_act != 0U)) /* 正在跑或还要频率衔接 */ + s_act_expired = 0U; + if ((s_state == RC_RUN) || (s_keep_freq_after_act != 0U)) { - PlsrRunControlActExpire(); /* ACT 时间到期处理 */ + PlsrRunControlActExpire(); } - return; /* 本拍处理完就回 */ + return; } - /* WAIT 时间到期：清标志，开下一段或结束 */ - if (s_wait_time_expired != 0U) /* 有段后 WAIT 时间到 */ + + if (s_wait_time_expired != 0U) { - s_wait_time_expired = 0U; /* 清掉到期请求 */ - if (s_state == RC_WAIT_COND) /* 还在段后等待才认 */ + s_wait_time_expired = 0U; + if (s_state == RC_WAIT_COND) { - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 时间到了，开下一段或收工 */ + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); } - return; /* 本拍处理完 */ + return; } - /* 方向延时：精确等待只靠 TIM5，此处不改状态 */ - if (s_state == RC_DIR_WAIT) /* 等换向延时中 */ + + if (s_state == RC_DIR_WAIT) { - /* 精确延时只靠 TIM5；此处占位，避免任务空转改状态 */ - return; /* 交给 TIM5，这里不动 */ + + return; } - /* RUN：消费挂起改频；EXT/EXT_OR_DONE 读下降沿切段 */ - if (s_state == RC_RUN) /* 正在发脉冲 */ + + if (s_state == RC_RUN) { - PlsrPulseDriverApplyPending(); /* 消费挂起改频 */ + PlsrPulseDriverApplyPending(); - if ((g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) || /* 本段要看外部 EXT 沿 */ + if ((g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) || (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE)) { - if (PlsrSignalIoTakeExtFalling() != 0U) /* 等到外部下降沿 */ + if (PlsrSignalIoTakeExtFalling() != 0U) { - PlsrRunControlCutSegToNext(); /* EXT 沿触发中途切段 */ - return; /* 切段完回 */ + PlsrRunControlCutSegToNext(); + return; } } - return; /* 本拍没有别的事 */ + return; } - /* WAIT_COND：等 WAIT/EXT 沿后开下一段 */ - if (s_state == RC_WAIT_COND) /* 段后等待里 */ + + if (s_state == RC_WAIT_COND) { - if (s_waiting_for_input_signal != 0U) /* 在等输入沿 */ + if (s_waiting_for_input_signal != 0U) { - if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) /* 等的是 EXT 下降沿 */ + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) { - if (PlsrSignalIoTakeExtFalling() != 0U) /* EXT 下降沿来了 */ + if (PlsrSignalIoTakeExtFalling() != 0U) { - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 开下一段或收工 */ + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); } } else { - if (PlsrSignalIoTakeWaitFalling() != 0U) /* 等的是 WAIT 口下降沿 */ + if (PlsrSignalIoTakeWaitFalling() != 0U) { - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 开下一段或收工 */ + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); } } } - /* 时间 WAIT / ACT 剩余：TIM5 OnDelayTimer 置 wait_expire_req */ + } } @@ -388,115 +382,111 @@ void PlsrRunControlTickMs(void) */ void PlsrRunControlOnDelayTimer(void) { - if (s_state == RC_DIR_WAIT) /* 换向延时到了 */ + if (s_state == RC_DIR_WAIT) { - s_state = RC_RUN; /* 切成正在发脉冲 */ - PlsrRunControlRefreshProfile(1U); /* 方向延时到：开表写频 */ - PlsrRunControlArmActExtOnPulseStart(); /* EXT 清沿 / ACT 开定时 */ - PlsrRunControlWakePost(); /* 唤醒 PlsrTask */ - return; /* 换向开表流程做完 */ + s_state = RC_RUN; + PlsrRunControlRefreshProfile(1U); + PlsrRunControlArmActExtOnPulseStart(); + PlsrRunControlWakePost(); + return; } - if ((s_state == RC_RUN) && (s_act_timer_armed != 0U)) /* 跑着且 ACT 定时已开着 */ + if ((s_state == RC_RUN) && (s_act_timer_armed != 0U)) { - /* 到点：当前脉冲下降沿后锁低，周期结束时 OPM 停表。 */ - s_act_timer_armed = 0U; /* ACT 定时用完，清掉 */ - s_act_waiting_for_pulse_boundary = 1U; /* 等脉冲边界再切段 */ - PlsrPulseDriverArmActStop(); /* 当前脉冲发完后锁低停表 */ - return; /* 等脉冲边界再切段 */ + + s_act_timer_armed = 0U; + s_act_waiting_for_pulse_boundary = 1U; + PlsrPulseDriverArmActStop(); + return; } - if ((s_state == RC_WAIT_COND) && (s_waiting_for_input_signal == 0U)) /* 段后纯时间等待到期 */ + if ((s_state == RC_WAIT_COND) && (s_waiting_for_input_signal == 0U)) { - s_wait_time_expired = 1U; /* 时间 WAIT/ACT 剩余到期 → TickMs 消费 */ - PlsrRunControlWakePost(); /* 叫醒任务去开下一段 */ + s_wait_time_expired = 1U; + PlsrRunControlWakePost(); } } /** @brief 脉冲 UPDATE ISR */ void PlsrOnPulseIsr(void) { - uint32_t remain; /* 本段剩余脉冲 */ - uint32_t next; /* 本拍写出频率 */ + uint32_t remain; + uint32_t next; - if (s_state != RC_RUN) /* 不是正在发脉冲就别管 */ + if (s_state != RC_RUN) { - return; /* 直接退出 ISR */ + return; } - /* 完成模式：末拍 OPM 停表后再收尾 */ - if (s_waiting_for_last_period != 0U) /* 在等末拍那一下 */ + if (s_waiting_for_last_period != 0U) { - s_waiting_for_last_period = 0U; /* 末拍收尾标志清掉 */ - s_act_waiting_for_pulse_boundary = 0U; /* 顺带清 ACT 边界等待 */ + s_waiting_for_last_period = 0U; + s_act_waiting_for_pulse_boundary = 0U; - PlsrPulseDriverHoldOutputLow(); /* 锁低输出 */ - PlsrRunControlAfterSegDone(); /* 进段后等待/下一段 */ - return; /* 末拍处理完 */ + PlsrPulseDriverHoldOutputLow(); + PlsrRunControlAfterSegDone(); + return; } - s_seg_pulses_done++; /* 本段已发 +1 */ - if (s_forward != 0U) /* 正转：累计加一 */ + s_seg_pulses_done++; + if (s_forward != 0U) { - g_plsr_accumulated_pulses++; /* 正转累计 */ + g_plsr_accumulated_pulses++; } else { - g_plsr_accumulated_pulses--; /* 反转累计 */ + g_plsr_accumulated_pulses--; } - /* ACT 到期：等当前脉冲边界再切段 */ - if (s_act_waiting_for_pulse_boundary != 0U) /* ACT 时间到，卡在等当前脉冲边界 */ + if (s_act_waiting_for_pulse_boundary != 0U) { - s_act_waiting_for_pulse_boundary = 0U; /* 边界到了，清等待 */ - s_keep_freq_after_act = 1U; /* 边界停表后仍可频率衔接 */ + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 1U; - PlsrPulseDriverHoldOutputLow(); /* 输出锁低 */ - s_state = RC_IDLE; /* 暂时回到空闲 */ - s_act_expired = 1U; /* 交给 TickMs 消费 */ - PlsrRunControlWakePost(); /* 唤醒任务处理 ACT 到期 */ - return; /* 交给 TickMs 去做 ACT 切段 */ + PlsrPulseDriverHoldOutputLow(); + s_state = RC_IDLE; + s_act_expired = 1U; + PlsrRunControlWakePost(); + return; } - /* 本段脉冲发完 */ - if (s_seg_pulses_done >= s_seg_pulse_target) /* 本段脉冲数发满了 */ + if (s_seg_pulses_done >= s_seg_pulse_target) { - if (s_stop_after_last_pulse != 0U) /* 需要末拍后再完整停表 */ + if (s_stop_after_last_pulse != 0U) { - /* 完成模式：OPM 再跑一拍，下一 UPDATE 才 AfterSegDone */ - PlsrPulseDriverArmSegEndStop(); /* 装上末拍停表 */ - s_waiting_for_last_period = 1U; /* 下一拍 UPDATE 再收尾 */ - return; /* 先回去，别急着切段 */ + + PlsrPulseDriverArmSegEndStop(); + s_waiting_for_last_period = 1U; + return; } - /* FOLLOW 不停表衔接：立刻切段，PWM 连续 */ - PlsrRunControlAfterSegDone(); /* 立刻走段后/下一段 */ - return; /* 段间不停表接着跑：马上切段 */ + + PlsrRunControlAfterSegDone(); + return; } - remain = PlsrRunControlRemainPulses(); /* 算还剩几拍 */ + remain = PlsrRunControlRemainPulses(); - if (s_phase == PH_CONST) /* 匀速相 */ + if (s_phase == PH_CONST) { - /* 匀速：不改频、不写 ARR，仅在 remain 进入减速窗口时切相并查表改频 */ - PlsrPulseDriverClearStartPeriodProtect(); /* 匀速相清开表保护 */ - if (PlsrRunControlShouldEnterDecel(remain) != 0U) /* 该进段末过渡了 */ + + PlsrPulseDriverClearStartPeriodProtect(); + if (PlsrRunControlShouldEnterDecel(remain) != 0U) { - /* remain 落入减速预算 → 进 DECEL 并写本拍频率 */ - PlsrRunControlEnterDecel(g_plsr_accel_plan.target_freq_hz, /* 从峰值切入段末过渡 */ + + PlsrRunControlEnterDecel(g_plsr_accel_plan.target_freq_hz, PlsrRunControlDecelEnterBudget(remain)); - next = PlsrAccelNextFreq(&g_plsr_accel_runtime); /* 斜坡算出下一频 */ - next = PlsrRunControlDecelOutFreq(next, remain); /* 末拍钉止速 */ - g_plsr_accel_runtime.cur_freq_hz = next; /* 记下当前输出频 */ - PlsrRunControlApplyOutFreqIsr(next); /* ISR 里写 ARR */ + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); + next = PlsrRunControlDecelOutFreq(next, remain); + g_plsr_accel_runtime.cur_freq_hz = next; + PlsrRunControlApplyOutFreqIsr(next); } } - else if (s_phase == PH_APPROACH) /* 起速爬峰相 */ + else if (s_phase == PH_APPROACH) { - /* 每拍 Step 得下一频；若加速相结束则进 CONST 或 DECEL */ - next = PlsrAccelNextFreq(&g_plsr_accel_runtime); /* 加速斜坡取下一频 */ - /* 加速结束：预算用尽 / 已达 f_tgt */ - if ((g_plsr_accel_plan.accel_pulses == 0U) || /* 加速是否该收尾（预算/到顶/到峰值） */ + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); + + if ((g_plsr_accel_plan.accel_pulses == 0U) || (g_plsr_accel_runtime.is_active == 0U) || (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_plan.accel_pulses) || ((g_plsr_accel_plan.target_freq_hz >= g_plsr_accel_plan.start_freq_hz) && @@ -504,100 +494,91 @@ void PlsrOnPulseIsr(void) ((g_plsr_accel_plan.target_freq_hz < g_plsr_accel_plan.start_freq_hz) && (next <= g_plsr_accel_plan.target_freq_hz))) { - next = g_plsr_accel_plan.target_freq_hz; /* 钉峰值 */ - PlsrRunControlOnApproachDone(); /* 0x03 加速诊断 */ - if (PlsrRunControlShouldEnterDecel(remain) != 0U) /* 剩脉冲已进减速窗口 */ + next = g_plsr_accel_plan.target_freq_hz; + PlsrRunControlOnApproachDone(); + if (PlsrRunControlShouldEnterDecel(remain) != 0U) { - /* 已进减速窗口：直接进 DECEL */ - PlsrRunControlEnterDecel(next, /* 直接进段末过渡 */ + + PlsrRunControlEnterDecel(next, PlsrRunControlDecelEnterBudget(remain)); - next = PlsrAccelNextFreq(&g_plsr_accel_runtime); /* 再取一拍斜坡频 */ - next = PlsrRunControlDecelOutFreq(next, remain); /* 末拍附近钉好止速 */ - g_plsr_accel_runtime.cur_freq_hz = next; /* 更新当前频 */ - PlsrRunControlApplyOutFreqIsr(next); /* 写到硬件 */ + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); + next = PlsrRunControlDecelOutFreq(next, remain); + g_plsr_accel_runtime.cur_freq_hz = next; + PlsrRunControlApplyOutFreqIsr(next); } else { - PlsrRunControlEnterConstHoldIsr(); /* 进匀速，ISR 侧写 f_tgt */ + PlsrRunControlEnterConstHoldIsr(); } } else { - PlsrRunControlApplyOutFreqIsr(next); /* 加速中：写下一拍 */ + PlsrRunControlApplyOutFreqIsr(next); } } - else /* PH_DECEL：段末过渡 */ + else { - /* 末拍且曲线步进已用尽 → 直接钉止速 */ - if ((remain <= 1U) && /* 末拍且斜坡步数用尽 */ + + if ((remain <= 1U) && (g_plsr_accel_runtime.total_pulses > 0U) && (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_runtime.total_pulses)) { - next = g_plsr_accel_plan.end_freq_hz; /* 直接用出口频率 */ + next = g_plsr_accel_plan.end_freq_hz; } else { - next = PlsrAccelNextFreq(&g_plsr_accel_runtime); /* 继续斜坡 */ + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); } - next = PlsrRunControlDecelOutFreq(next, remain); /* 按止速规则修一下 */ - g_plsr_accel_runtime.cur_freq_hz = next; /* 记下 */ - PlsrRunControlApplyOutFreqIsr(next); /* ISR 写出 */ + next = PlsrRunControlDecelOutFreq(next, remain); + g_plsr_accel_runtime.cur_freq_hz = next; + PlsrRunControlApplyOutFreqIsr(next); } } -/*============================================================================*/ -/* 唤醒 */ -/*============================================================================*/ - /** * ISR/任务：唤醒 PlsrTask（二进制语义：先排空再 Post，避免残留令牌使下一段秒醒） */ static void PlsrRunControlWakeDrain(void) { - if (s_wake_sem != (OS_EVENT *)0) /* 有信号量才排 */ + if (s_wake_sem != (OS_EVENT *)0) { - while (OSSemAccept(s_wake_sem) > 0U) /* 有令牌就掏光 */ + while (OSSemAccept(s_wake_sem) > 0U) { - /* 全部丢弃令牌 */ + } } } -/* 投递一枚唤醒令牌给 PlsrTask */ static void PlsrRunControlWakePost(void) { - if (s_wake_sem != (OS_EVENT *)0) /* 有信号量才投 */ + if (s_wake_sem != (OS_EVENT *)0) { - (void)OSSemPost(s_wake_sem); /* 投一枚叫醒 PlsrTask */ + (void)OSSemPost(s_wake_sem); } } -/*============================================================================*/ -/* 段导航 */ -/*============================================================================*/ - /** * @brief 解析下一段索引 * @note jump=0：末段结束，非末段顺序下一段；jump=1..N：跳到对应段；其它结束 */ static int16_t PlsrRunControlNextSeg(uint16_t cur_seg_idx) { - uint16_t n = PlsrParamGetSegCount(); /* 总段数 */ - uint16_t jump = g_plsr_segs[cur_seg_idx].jump_seg; /* 本段跳转字段 */ + uint16_t n = PlsrParamGetSegCount(); + uint16_t jump = g_plsr_segs[cur_seg_idx].jump_seg; - if (jump == 0U) /* jump=0：顺序走 */ + if (jump == 0U) { - if ((n >= 1U) && (cur_seg_idx + 1U < n)) /* 后面还有段 */ + if ((n >= 1U) && (cur_seg_idx + 1U < n)) { - return (int16_t)(cur_seg_idx + 1U); /* 下一段下标 */ + return (int16_t)(cur_seg_idx + 1U); } - return -1; /* 已经是末段 → 结束 */ + return -1; } - if ((jump >= 1U) && (jump <= n)) /* jump 指向合法段号 */ + if ((jump >= 1U) && (jump <= n)) { - return (int16_t)(jump - 1U); /* 跳到该段（改 0 起算） */ + return (int16_t)(jump - 1U); } - return -1; /* 非法 jump → 当结束 */ + return -1; } /** @@ -605,7 +586,7 @@ static int16_t PlsrRunControlNextSeg(uint16_t cur_seg_idx) */ static uint8_t PlsrRunControlIsEmptyPlaceholderSeg(uint16_t seg_idx) { - return ((g_plsr_segs[seg_idx].freq_hz == 0) && /* 频率0且脉冲0：占位空段 */ + return ((g_plsr_segs[seg_idx].freq_hz == 0) && (g_plsr_segs[seg_idx].pulse_cnt == 0)) ? 1U : 0U; } @@ -615,19 +596,19 @@ static uint8_t PlsrRunControlIsEmptyPlaceholderSeg(uint16_t seg_idx) */ static int16_t PlsrRunControlNextEffectiveSeg(uint16_t cur_seg_idx) { - int16_t next = PlsrRunControlNextSeg(cur_seg_idx); /* 先取逻辑下一段 */ - uint16_t guard = 0U; /* 防死循环计数 */ + int16_t next = PlsrRunControlNextSeg(cur_seg_idx); + uint16_t guard = 0U; - while ((next >= 0) && (guard < PLSR_SEG_MAX)) /* 还有下一段且没转太久 */ + while ((next >= 0) && (guard < PLSR_SEG_MAX)) { - guard++; /* guard 加一 */ - if (PlsrRunControlIsEmptyPlaceholderSeg((uint16_t)next) == 0U) /* 不是空占位 */ + guard++; + if (PlsrRunControlIsEmptyPlaceholderSeg((uint16_t)next) == 0U) { - return next; /* 找到有效段 */ + return next; } - next = PlsrRunControlNextSeg((uint16_t)next); /* 空段就再跳一步 */ + next = PlsrRunControlNextSeg((uint16_t)next); } - return -1; /* 没有有效段了 */ + return -1; } /** @@ -637,21 +618,21 @@ static uint8_t PlsrRunControlIsForward(uint16_t seg_idx, int32_t acc_pulse) { int32_t move; - if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) /* 绝对模式看目标减当前位置 */ + if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) { - move = g_plsr_segs[seg_idx].pulse_cnt - acc_pulse; /* 还要走多少（有符号） */ + move = g_plsr_segs[seg_idx].pulse_cnt - acc_pulse; } else { - move = g_plsr_segs[seg_idx].pulse_cnt; /* 相对模式直接看脉冲符号 */ + move = g_plsr_segs[seg_idx].pulse_cnt; } - return (move >= 0) ? 1U : 0U; /* >=0 正转，否则反转 */ + return (move >= 0) ? 1U : 0U; } /** 绝对模式：相对本次启动原点的位置 */ static int32_t PlsrRunControlAbsLocalPos(void) { - return g_plsr_accumulated_pulses - s_absolute_origin; /* 累计减去锁定原点 */ + return g_plsr_accumulated_pulses - s_absolute_origin; } /** @@ -659,15 +640,15 @@ static int32_t PlsrRunControlAbsLocalPos(void) */ static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg) { - int16_t next_seg_idx = PlsrRunControlNextSeg(cur_seg); /* 解析下一段 */ + int16_t next_seg_idx = PlsrRunControlNextSeg(cur_seg); - if (next_seg_idx < 0) /* 没有下一段 */ + if (next_seg_idx < 0) { - PlsrRunControlFinishAll(); /* 全部收工 */ + PlsrRunControlFinishAll(); } else { - PlsrRunControlBeginSeg((uint16_t)next_seg_idx); /* 开下一段 */ + PlsrRunControlBeginSeg((uint16_t)next_seg_idx); } } @@ -676,21 +657,21 @@ static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg) */ static void PlsrRunControlFinishAll(void) { - PlsrSignalIoCancelDelay(); /* 取消未到期定时 */ - PlsrPulseDriverStop(); /* 停脉冲 */ - PlsrPulseDriverUnlockPsc(); /* 解锁 PSC */ - PlsrPulseDriverClearDir(); /* 清方向脚 */ - g_plsr_busy = 0U; /* 不忙了 */ - s_state = RC_IDLE; /* 回到空闲 */ - s_continue_without_stopping = 0U; /* 清段间不停表标志 */ - s_next_seg_start_freq_valid = 0U; /* 清衔接频率有效 */ - s_next_seg_start_freq_hz = 0U; /* 衔接频率清零 */ - s_act_timer_armed = 0U; /* 清 ACT 定时已开标志 */ - s_act_expired = 0U; /* 清 ACT 时间到请求 */ - s_act_waiting_for_pulse_boundary = 0U; /* 清等脉冲边界 */ - s_keep_freq_after_act = 0U; /* 清 ACT 后频率衔接 */ - s_wait_time_expired = 0U; /* 清 WAIT 时间到 */ - s_waiting_for_last_period = 0U; /* 清末拍收尾等待 */ + PlsrSignalIoCancelDelay(); + PlsrPulseDriverStop(); + PlsrPulseDriverUnlockPsc(); + PlsrPulseDriverClearDir(); + g_plsr_busy = 0U; + s_state = RC_IDLE; + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; + s_waiting_for_last_period = 0U; } /** @@ -698,47 +679,46 @@ static void PlsrRunControlFinishAll(void) */ static void PlsrRunControlBeginSeg(uint16_t seg_idx) { - int32_t cnt; /* 段表 pulse_cnt */ - int32_t move; /* 本段实际位移（有符号） */ - uint32_t f_tgt; /* 本段目标频率 */ - uint32_t total; /* 本段目标脉冲绝对值 */ - PlsrSegPlanEndpoints_t endpoints; /* 策略层解析的起/止速与加减速 ms */ - /* 如果段号大于总段数，直接结束输出 */ - if (seg_idx >= PlsrParamGetSegCount()) /* 段号越界 */ + int32_t cnt; + int32_t move; + uint32_t f_tgt; + uint32_t total; + PlsrSegPlanEndpoints_t endpoints; + + if (seg_idx >= PlsrParamGetSegCount()) { - PlsrRunControlFinishAll(); /* 直接全收工 */ - return; /* 回去 */ + PlsrRunControlFinishAll(); + return; } - g_plsr_cur_seg_idx = seg_idx; /* 记下当前段 */ - s_waiting_for_last_period = 0U; /* 等末拍 UPDATE 再收尾标志 */ - cnt = g_plsr_segs[seg_idx].pulse_cnt; /* 读取段表脉冲数 */ + g_plsr_cur_seg_idx = seg_idx; + s_waiting_for_last_period = 0U; + cnt = g_plsr_segs[seg_idx].pulse_cnt; - /* 读取运行模式，绝对模式以启动位置为绝对原点 */ - if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) /* 读运行模式 */ + if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) { - /* 以首次启动锁定原点：move = 段目标 − 相对原点位置 */ - move = cnt - PlsrRunControlAbsLocalPos(); /* 目标减当前位置 */ + + move = cnt - PlsrRunControlAbsLocalPos(); } else { - move = cnt; /* 相对模式：位移量 = 脉冲数 */ + move = cnt; } - if (move == 0) /* 不用动了 */ + if (move == 0) { /* 相对 0 脉冲 / 无效占位(频率0且脉冲0) / 绝对已在目标：立刻段后逻辑 */ - g_plsr_busy = 1U; /* 仍置忙，方便段后逻辑 */ - s_seg_pulse_target = 0; /* 目标脉冲 0 */ - s_seg_pulses_done = 0; /* 已发也 0 */ - PlsrRunControlAfterSegDone(); /* 段正常结束 */ - return; /* 直接走段后 */ + g_plsr_busy = 1U; + s_seg_pulse_target = 0; + s_seg_pulses_done = 0; + PlsrRunControlAfterSegDone(); + return; } - if (move >= 0) /* 正方向位移 */ + if (move >= 0) { - s_forward = 1U; /* 正向 */ - s_seg_pulse_target = move; /* 目标脉冲 = 位移 */ + s_forward = 1U; + s_seg_pulse_target = move; } else { @@ -747,130 +727,122 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx) * 被解读为 -2147483648）时取反溢出，s_seg_pulse_target 仍为负数， * 首拍后 s_seg_pulses_done>=s_seg_pulse_target 恒真，会误判段结束且累计每启一次减 1。 */ - if (move == (int32_t)(-2147483647L - 1L)) /* 反向极端值，取反会炸 */ + if (move == (int32_t)(-2147483647L - 1L)) { - PlsrCommandSetFault(PLSR_ERR_PULSE_RANGE); /* 脉冲范围故障 */ - PlsrRunControlFinishAll(); /* 全段结束 */ - return; /* 回去 */ + PlsrCommandSetFault(PLSR_ERR_PULSE_RANGE); + PlsrRunControlFinishAll(); + return; } - s_forward = 0U; /* 反向 */ - s_seg_pulse_target = -move; /* 目标脉冲 = |位移| */ + s_forward = 0U; + s_seg_pulse_target = -move; } - /* 0x04：本段（或默认速度）频率非法 */ - if (PlsrRunControlGetSegTargetFreq(g_plsr_segs[seg_idx].freq_hz, /* 本段目标频率合法性检查 */ + if (PlsrRunControlGetSegTargetFreq(g_plsr_segs[seg_idx].freq_hz, g_plsr_config.default_speed, &f_tgt) == 0U) { - PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); /* 频率非法报故障 */ - PlsrRunControlFinishAll(); /* 收工 */ - return; /* 回去 */ + PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); + PlsrRunControlFinishAll(); + return; } - /* 策略层解析起/止速与加减速；段内只跑 PlanSeg */ - (void)PlsrRunControlResolveSegPlanEndpoints(seg_idx, f_tgt, &endpoints); /* 解析入口/出口/加减速时间 */ + (void)PlsrRunControlResolveSegPlanEndpoints(seg_idx, f_tgt, &endpoints); - total = (uint32_t)s_seg_pulse_target; /* 本段要发的脉冲总数 */ + total = (uint32_t)s_seg_pulse_target; - /* 规划本段并锁 PSC / 进初始相 */ - PlsrRunControlPlanSeg(total, endpoints.freq_start_hz, f_tgt, endpoints.freq_end_hz, /* 按端点规划本段曲线 */ + PlsrRunControlPlanSeg(total, endpoints.freq_start_hz, f_tgt, endpoints.freq_end_hz, endpoints.accel_ms, endpoints.decel_ms); - s_seg_pulses_done = 0; /* 已发脉冲数清零 */ - g_plsr_busy = 1U; /* 置忙 */ + s_seg_pulses_done = 0; + g_plsr_busy = 1U; { - /* 用本段结束后的坐标判 keep，与 AfterSegDone 一致；结果锁进 s_stop_after_last_pulse */ - int32_t end_pos = PlsrRunControlAbsLocalPos(); /* 预估段末局部坐标 */ - if (s_forward != 0U) /* 正转估段末位置 */ + int32_t end_pos = PlsrRunControlAbsLocalPos(); + + if (s_forward != 0U) { - end_pos += s_seg_pulse_target; /* 正转：末位 = 现在 + 目标 */ + end_pos += s_seg_pulse_target; } else { - end_pos -= s_seg_pulse_target; /* 反转：末位 = 现在 − 目标 */ + end_pos -= s_seg_pulse_target; } - /* 1=末拍完整停表；0=FOLLOW 不停表衔接 */ - s_stop_after_last_pulse = /* 接得上就不停表；接不上末拍停表 */ + + s_stop_after_last_pulse = (PlsrRunControlWillFollowKeepAt(seg_idx, end_pos) == 0U) ? 1U : 0U; } - if (total == 0U) /* 总数为 0（理论上少见） */ + if (total == 0U) { - PlsrRunControlAfterSegDone(); /* 零脉冲：直接段后 */ - return; /* 回去 */ + PlsrRunControlAfterSegDone(); + return; } - /* 段间不停表衔接（后续模式/ACT 切段） */ - if (s_continue_without_stopping != 0U) /* 上一段留下「不停表接着跑」 */ + if (s_continue_without_stopping != 0U) { - /* 分支 A：段间无缝（后续模式或 ACT 切段） */ - PlsrPulseDriverClearOnePulseStop(); /* 清掉单脉冲停止 */ + + PlsrPulseDriverClearOnePulseStop(); /* * 必须先 RC_RUN 再 RefreshProfile/Start：否则冷启动首拍 UPDATE 时 * OnPulseIsr 仍见非 RUN 直接 return，影子停在起跳频，第 1、2 拍同频。 */ - s_state = RC_RUN; /* 先标成正在发脉冲 */ - s_continue_without_stopping = 0U; /* 用过即清 */ + s_state = RC_RUN; + s_continue_without_stopping = 0U; /* * FOLLOW 真不停表：do_start=0 只改频。 * ACT 经 OPM 边界停表后 g_plsr_pwm_running 已为 0：须 Start 重开，否则无脉冲卡死。 * 1=需开 PWM；0=已在跑只刷新下一拍频率 */ - PlsrRunControlRefreshProfile((g_plsr_pwm_running == 0U) ? 1U : 0U); /* 停过就真正开表，否则只改频 */ - /* 开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ - PlsrRunControlArmActExtOnPulseStart(); /* EXT 清沿 / ACT 开定时 */ + PlsrRunControlRefreshProfile((g_plsr_pwm_running == 0U) ? 1U : 0U); + + PlsrRunControlArmActExtOnPulseStart(); } - else if ((g_plsr_direction_valid != 0U) && (s_forward == g_plsr_direction_forward)) /* 同向且方向脚已对 */ + else if ((g_plsr_direction_valid != 0U) && (s_forward == g_plsr_direction_forward)) { - /* 分支 B：本段与上一段同向，方向脚已正确，跳过延时 */ - PlsrPulseDriverSetDir(s_forward); /* 再确认写一次方向 */ - s_state = RC_RUN; /* 直接进入正在发脉冲 */ - PlsrRunControlRefreshProfile(1U); /* 冷开：Start + 写频 */ - /* 开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ - PlsrRunControlArmActExtOnPulseStart(); /* EXT 清沿 / ACT 开定时 */ + + PlsrPulseDriverSetDir(s_forward); + s_state = RC_RUN; + PlsrRunControlRefreshProfile(1U); + + PlsrRunControlArmActExtOnPulseStart(); } - else /* 要换向或第一次开跑 */ + else { - /* 分支 C：需要换向或首次开跑 — 先打方向，再决定立刻开或 DIR_WAIT */ - PlsrPulseDriverSetDir(s_forward); /* 设置方向脚 */ - if (g_plsr_config.dir_delay_ms == 0U) /* 换向延时配成 0 */ + + PlsrPulseDriverSetDir(s_forward); + if (g_plsr_config.dir_delay_ms == 0U) { - s_state = RC_RUN; /* 不等，直接正在发脉冲 */ - PlsrRunControlRefreshProfile(1U); /* 无延时：立刻开表 */ - /* 开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ - PlsrRunControlArmActExtOnPulseStart(); /* EXT 清沿 / ACT 开定时 */ + s_state = RC_RUN; + PlsrRunControlRefreshProfile(1U); + + PlsrRunControlArmActExtOnPulseStart(); } else { /* * 先置 DIR_WAIT 再开 TIM5，避免极短延时下 IRQ 早于状态切换。 */ - s_state = RC_DIR_WAIT; /* 先标成等换向延时 */ - PlsrRunControlWakeDrain(); /* 排空残留唤醒令牌 */ - /* 设置方向延时 */ - PlsrSignalIoScheduleDelayMs((uint32_t)g_plsr_config.dir_delay_ms); /* 开 TIM5 换向延时 */ - if (g_plsr_accel_runtime.cur_freq_hz >= 1U) /* 起跳频有效 */ + s_state = RC_DIR_WAIT; + PlsrRunControlWakeDrain(); + + PlsrSignalIoScheduleDelayMs((uint32_t)g_plsr_config.dir_delay_ms); + if (g_plsr_accel_runtime.cur_freq_hz >= 1U) { - /* 换向延时期间预装寄存器、CEN 保持关 */ - PlsrPulseDriverPrepare(g_plsr_accel_runtime.cur_freq_hz); /* 延时期间预装定时器，先不开 CEN */ + + PlsrPulseDriverPrepare(g_plsr_accel_runtime.cur_freq_hz); } } } } -/*============================================================================*/ -/* 衔接 / 停表 */ -/*============================================================================*/ - /** * @brief 停表并清频率链 */ static void PlsrRunControlClearChainAndStop(void) { - PlsrPulseDriverStop(); /* 停脉冲输出 */ - s_continue_without_stopping = 0U; /* 清不停表衔接 */ - s_next_seg_start_freq_valid = 0U; /* 清衔接频率有效 */ - s_next_seg_start_freq_hz = 0U; /* 衔接频率清零 */ + PlsrPulseDriverStop(); + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; } /** @@ -879,33 +851,33 @@ static void PlsrRunControlClearChainAndStop(void) */ static void PlsrRunControlApplyKeepOrStop(uint8_t allow_act_handoff) { - int16_t next_seg_idx; /* 下一段索引；<0 无下一段 */ - uint8_t keep; /* 1=不停表衔接 */ - uint8_t pwm_ok; /* PWM 还在跑，或 ACT 时间到先停表后仍允许接着开下一段 */ + int16_t next_seg_idx; + uint8_t keep; + uint8_t pwm_ok; - next_seg_idx = PlsrRunControlNextSeg(g_plsr_cur_seg_idx); /* 看看有没有下一段 */ - pwm_ok = (g_plsr_pwm_running != 0U) || /* PWM 在跑，或允许 ACT 切段衔接 */ + next_seg_idx = PlsrRunControlNextSeg(g_plsr_cur_seg_idx); + pwm_ok = (g_plsr_pwm_running != 0U) || ((allow_act_handoff != 0U) && (s_keep_freq_after_act != 0U)); - keep = 0U; /* 默认先当要停表 */ - /* 有下一段 + PWM 可用 + 当前频有效 + 同向 → keep */ - if ((next_seg_idx >= 0) && (pwm_ok != 0U) && (g_plsr_output_freq_hz >= 1U) && /* 下一段在、能跑、频有效、同向 */ + keep = 0U; + + if ((next_seg_idx >= 0) && (pwm_ok != 0U) && (g_plsr_output_freq_hz >= 1U) && (PlsrRunControlIsForward((uint16_t)next_seg_idx, PlsrRunControlAbsLocalPos()) == s_forward)) { - keep = 1U; /* 可以段间不停表接着跑 */ + keep = 1U; } - if (keep != 0U) /* 决定不停表衔接 */ + if (keep != 0U) { - PlsrPulseDriverClearOnePulseStop(); /* 清掉单脉冲停表残留 */ - s_continue_without_stopping = 1U; /* 下一段无缝开 */ - s_next_seg_start_freq_valid = 1U; /* 入口频率有效 */ - s_next_seg_start_freq_hz = g_plsr_output_freq_hz; /* 入口=当前频 */ + PlsrPulseDriverClearOnePulseStop(); + s_continue_without_stopping = 1U; + s_next_seg_start_freq_valid = 1U; + s_next_seg_start_freq_hz = g_plsr_output_freq_hz; } else { - PlsrRunControlClearChainAndStop(); /* 停表并清频率链 */ - PlsrPulseDriverClearOnePulseStop(); /* 顺带清单脉冲停 */ + PlsrRunControlClearChainAndStop(); + PlsrPulseDriverClearOnePulseStop(); } } @@ -915,14 +887,14 @@ static void PlsrRunControlApplyKeepOrStop(uint8_t allow_act_handoff) */ static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg) { - if ((g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_TIME) || /* 这些等待类型必须停表再等 */ + if ((g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_TIME) || (g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_SIGNAL) || (g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_ACT) || (g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_EXT)) { - return 1U; /* 禁止段间不停表接着跑 */ + return 1U; } - return 0U; /* 其它类型允许（含 EXT_OR_DONE） */ + return 0U; } /** @@ -934,38 +906,34 @@ static uint8_t PlsrRunControlWillFollowKeepAt(uint16_t cur_seg, int32_t local_po { int16_t next_eff; - if (g_plsr_config.send_mode != PLSR_SEND_FOLLOW) /* 不是后续模式 */ + if (g_plsr_config.send_mode != PLSR_SEND_FOLLOW) { - return 0U; /* 不能不停表衔接 */ + return 0U; } - if (PlsrRunControlNextSeg(cur_seg) < 0) /* 没有逻辑下一段 */ + if (PlsrRunControlNextSeg(cur_seg) < 0) { - return 0U; /* 接不了 */ + return 0U; } - next_eff = PlsrRunControlNextEffectiveSeg(cur_seg); /* 找下一有效段 */ - if (next_eff < 0) /* 有效段也没有 */ + next_eff = PlsrRunControlNextEffectiveSeg(cur_seg); + if (next_eff < 0) { - return 0U; /* 接不了 */ + return 0U; } /* * 直接下一段为无效占位时不能「跳过」该段，但仍可与下一有效段同向 keep * （EXT_OR_DONE 同向不应因中间占位而强制停到 0 再爬）。 */ - if (PlsrRunControlIsForward((uint16_t)next_eff, local_pos) != s_forward) /* 和下一段方向不一致 */ + if (PlsrRunControlIsForward((uint16_t)next_eff, local_pos) != s_forward) { - return 0U; /* 换向必须停表 */ + return 0U; } - if (PlsrRunControlBlocksFollowKeep(cur_seg) != 0U) /* 本段等待类型要求停表等条件 */ + if (PlsrRunControlBlocksFollowKeep(cur_seg) != 0U) { - return 0U; /* 必须停表 */ + return 0U; } - return 1U; /* 可以段间不停表接着跑 */ + return 1U; } -/*============================================================================*/ -/* ACT / EXT / 段后等待 */ -/*============================================================================*/ - /** * @brief 开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ @@ -973,26 +941,23 @@ static void PlsrRunControlArmActExtOnPulseStart(void) { uint16_t act_time_ms; - /* EXT / EXT_OR_DONE：清 X4/X5 旧沿，防止开跑立刻误切段 */ - if ((g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) || /* EXT / 外部或发完：先清旧沿 */ + if ((g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) || (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE)) { - PlsrSignalIoClearEdges(); /* 免得一开跑就误切段 */ + PlsrSignalIoClearEdges(); } - /* 开跑前清 ACT/WAIT 残留，避免上一段标志串进来 */ - s_act_timer_armed = 0U; /* 清「ACT 定时已开」标志 */ - s_act_expired = 0U; /* 清 ACT 到期请求 */ - s_act_waiting_for_pulse_boundary = 0U; /* 清脉冲边界切段等待 */ - s_keep_freq_after_act = 0U; /* 清 ACT 频率衔接 */ - s_wait_time_expired = 0U; /* 清 WAIT 时间到期 */ + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; - /* WAIT_ACT：开 TIM5 计 ACT 时间，到点后等当前这拍发完再切段 */ - if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_ACT) /* 本段是 ACT 时间等待 */ + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_ACT) { - act_time_ms = g_plsr_segs[g_plsr_cur_seg_idx].act_ms; /* 读段表 ACT 毫秒 */ - s_act_timer_armed = 1U; /* 记下：ACT 定时已经开了 */ - PlsrSignalIoScheduleDelayMs((uint32_t)act_time_ms); /* 开 TIM5 倒计时 */ + act_time_ms = g_plsr_segs[g_plsr_cur_seg_idx].act_ms; + s_act_timer_armed = 1U; + PlsrSignalIoScheduleDelayMs((uint32_t)act_time_ms); } } @@ -1004,16 +969,16 @@ static void PlsrRunControlArmActExtOnPulseStart(void) */ static void PlsrRunControlCutSegToNext(void) { - PlsrSignalIoCancelDelay(); /* 取消 ACT/方向等未到期定时 */ - s_state = RC_IDLE; /* 暂时回到空闲 */ - s_act_timer_armed = 0U; /* 清 ACT 定时已开 */ - s_act_expired = 0U; /* 清 ACT 时间到请求 */ - s_act_waiting_for_pulse_boundary = 0U; /* 清等脉冲边界 */ - s_keep_freq_after_act = 0U; /* 清 ACT 频率衔接 */ - s_wait_time_expired = 0U; /* 清 WAIT 时间到 */ - - PlsrRunControlApplyKeepOrStop(0U); /* EXT 切段：不允许「停表后还接着跑」那种衔接 */ - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 去开下一段或收工 */ + PlsrSignalIoCancelDelay(); + s_state = RC_IDLE; + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; + + PlsrRunControlApplyKeepOrStop(0U); + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); } /** @@ -1025,15 +990,15 @@ static void PlsrRunControlCutSegToNext(void) */ static void PlsrRunControlActExpire(void) { - s_act_timer_armed = 0U; /* 清 ACT 定时已开 */ - s_act_expired = 0U; /* 清到期请求（已在消费） */ - s_wait_time_expired = 0U; /* 顺带清 WAIT 到期 */ - s_state = RC_IDLE; /* 先回到空闲再切段 */ - /* 同向且有有效频率就段间不停表接着开；否则停表清链。传 1=允许 ACT 时间到后停表再接着跑 */ - PlsrRunControlApplyKeepOrStop(1U); /* 允许 ACT 边界后仍频率衔接 */ - s_keep_freq_after_act = 0U; /* 用过即清 */ - - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 丢掉本段剩余，开下一段 */ + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_wait_time_expired = 0U; + s_state = RC_IDLE; + + PlsrRunControlApplyKeepOrStop(1U); + s_keep_freq_after_act = 0U; + + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); } /** @@ -1044,50 +1009,50 @@ static void PlsrRunControlActExpire(void) */ static void PlsrRunControlEnterPostWaitOrNext(void) { - uint16_t wms; /* WAIT 时间 ms */ + uint16_t wms; - if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_TIME) /* 段后等一段时间 */ + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_TIME) { - wms = g_plsr_segs[g_plsr_cur_seg_idx].wait_ms; /* 读等待毫秒 */ - if (wms == 0U) /* 等待时间写成 0 */ + wms = g_plsr_segs[g_plsr_cur_seg_idx].wait_ms; + if (wms == 0U) { - wms = 1U; /* 0 视为最短 1ms */ + wms = 1U; } - PlsrRunControlClearChainAndStop(); /* 先停表清频率链 */ - s_waiting_for_input_signal = 0U; /* 纯时间等，不读沿 */ - PlsrRunControlWakeDrain(); /* 清理过期令牌 */ - s_state = RC_WAIT_COND; /* 进入段后等待 */ - PlsrSignalIoScheduleDelayMs((uint32_t)wms); /* TIM5 到期后 TickMs 开下一段 */ - return; /* TIM5 到期后由 TickMs 接下一段 */ - } - if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_SIGNAL) /* 段后等 WAIT 口信号 */ - { - /* 与 WAIT 时间相同：本段发完停表，再等 EXTI 确认后的下降沿 */ - PlsrRunControlClearChainAndStop(); /* 停表清链 */ - if (PlsrSignalIoTakeWaitFalling() != 0U) /* 沿已经在了 */ + PlsrRunControlClearChainAndStop(); + s_waiting_for_input_signal = 0U; + PlsrRunControlWakeDrain(); + s_state = RC_WAIT_COND; + PlsrSignalIoScheduleDelayMs((uint32_t)wms); + return; + } + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_SIGNAL) + { + + PlsrRunControlClearChainAndStop(); + if (PlsrSignalIoTakeWaitFalling() != 0U) { - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 沿已到：立刻下一段 */ - return; /* 沿已消费，回去 */ + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); + return; } - s_waiting_for_input_signal = 1U; /* TickMs 轮询 WAIT 沿 */ - s_state = RC_WAIT_COND; /* 进入段后等待 */ - return; /* 回去等 TickMs 轮询 */ + s_waiting_for_input_signal = 1U; + s_state = RC_WAIT_COND; + return; } - if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) /* 段后还要等外部下降沿 */ + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) { - /* EXT信号：脉冲发完后仍要等 EXT 沿才跳转 */ - PlsrRunControlClearChainAndStop(); /* 停表清链 */ - /* 只清消抖中的假沿；已确认的 fell 保留，供下面 TakeExtFalling 消费 */ - PlsrSignalIoClearPending(); /* 清消抖里还没确认的假沿 */ - if (PlsrSignalIoTakeExtFalling() != 0U) /* EXT 沿已经确认到了 */ + + PlsrRunControlClearChainAndStop(); + + PlsrSignalIoClearPending(); + if (PlsrSignalIoTakeExtFalling() != 0U) { - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 沿已到：立刻下一段 */ - return; /* 沿已消费，回去 */ + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); + return; } - s_waiting_for_input_signal = 1U; /* TickMs 轮询 EXT 沿 */ - s_state = RC_WAIT_COND; /* 进入段后等待 */ - return; /* 回去等 TickMs 看 EXT */ + s_waiting_for_input_signal = 1U; + s_state = RC_WAIT_COND; + return; } /* @@ -1095,7 +1060,7 @@ static void PlsrRunControlEnterPostWaitOrNext(void) * EXT 先到：RUN 里已 CutSegToNext，走不到这里。 * 其它无等待类型：同样直接 GotoNext。 */ - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 没额外等待：直接下一段或收工 */ + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); } /** @@ -1103,85 +1068,78 @@ static void PlsrRunControlEnterPostWaitOrNext(void) */ static void PlsrRunControlAfterSegDone(void) { - uint8_t keep_pwm; /* 1=不停表衔接下一段 */ + uint8_t keep_pwm; - /* 段末诊断：0x01 脉冲个数；若在减速相再核 0x03 */ - PlsrRunControlCheckPulseCntFault(); /* 核对脉冲个数 */ - PlsrRunControlCheckDecelCurveFault(); /* 若在减速相再核曲线 */ + PlsrRunControlCheckPulseCntFault(); + PlsrRunControlCheckDecelCurveFault(); - /* 先脱离 RUN，避免脉冲 ISR 重入 */ - s_state = RC_IDLE; /* 先脱离正在发脉冲，防 ISR 重入 */ + s_state = RC_IDLE; - /* 脉冲已发完但 ACT 时间未到 → 停表，等到到期再开下一段 */ - if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_ACT) /* 本段是 ACT：脉冲完了时间可能还没到 */ + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_ACT) { - PlsrPulseDriverStop(); /* 先停表 */ - PlsrPulseDriverClearOnePulseStop(); /* 清单脉冲停残留 */ - s_continue_without_stopping = 0U; /* 不许不停表衔接 */ - s_next_seg_start_freq_valid = 0U; /* 清衔接频率有效 */ - s_next_seg_start_freq_hz = 0U; /* 衔接频率清零 */ - s_act_expired = 0U; /* 清 ACT 到期请求 */ - if (s_act_timer_armed != 0U) /* ACT 定时还在走 */ + PlsrPulseDriverStop(); + PlsrPulseDriverClearOnePulseStop(); + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; + s_act_expired = 0U; + if (s_act_timer_armed != 0U) { - uint32_t remain_ms = PlsrSignalIoDelayRemainMs(); /* ACT 剩余时间 */ + uint32_t remain_ms = PlsrSignalIoDelayRemainMs(); - PlsrSignalIoCancelDelay(); /* 取消旧定时，改用剩余时间重开 */ - s_act_timer_armed = 0U; /* 旧定时卸掉 */ - if (remain_ms >= 1U) /* 还剩至少 1ms */ + PlsrSignalIoCancelDelay(); + s_act_timer_armed = 0U; + if (remain_ms >= 1U) { - s_waiting_for_input_signal = 0U; /* 纯时间等，不读沿 */ - PlsrRunControlWakeDrain(); /* 排空唤醒令牌 */ - s_state = RC_WAIT_COND; /* 等剩余 ACT 时间 */ - PlsrSignalIoScheduleDelayMs(remain_ms); /* 用剩余时间重开 TIM5 */ - return; /* 等 ACT 剩余时间到 */ + s_waiting_for_input_signal = 0U; + PlsrRunControlWakeDrain(); + s_state = RC_WAIT_COND; + PlsrSignalIoScheduleDelayMs(remain_ms); + return; } } else { - PlsrSignalIoCancelDelay(); /* 计时已到，取消定时 */ + PlsrSignalIoCancelDelay(); } - s_act_timer_armed = 0U; /* 确保 ACT 定时标志清掉 */ - PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); /* 剩余为 0：立刻下一段 */ - return; /* ACT 剩余为 0：下一段开跑 */ + s_act_timer_armed = 0U; + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); + return; } - s_act_timer_armed = 0U; /* 非 ACT：清 ACT 定时标志 */ - s_act_expired = 0U; /* 清 ACT 时间到请求 */ - s_wait_time_expired = 0U; /* 清 WAIT 时间到 */ - /* 段初已判：0=末拍停表；1=FOLLOW keep 不停表 */ - keep_pwm = (s_stop_after_last_pulse == 0U) ? 1U : 0U; /* 0=可不停表接着跑；1=末拍已停 */ + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_wait_time_expired = 0U; - if (g_plsr_config.send_mode == PLSR_SEND_COMPLETE) /* 完成模式 */ + keep_pwm = (s_stop_after_last_pulse == 0U) ? 1U : 0U; + + if (g_plsr_config.send_mode == PLSR_SEND_COMPLETE) { - /* 段正常结束：下一段起速优先用本段落地频（EXT_OR_DONE 完成时常为峰值） */ - s_next_seg_start_freq_hz = g_plsr_accel_plan.end_freq_hz; /* 下一段起速用本段落地频 */ - s_next_seg_start_freq_valid = 1U; /* 标记衔接频率有效 */ + + s_next_seg_start_freq_hz = g_plsr_accel_plan.end_freq_hz; + s_next_seg_start_freq_valid = 1U; } - if (keep_pwm != 0U) /* 可以段间不停表接着跑 */ + if (keep_pwm != 0U) { - /* 后续同向且不用停下来等条件：不停表直接 BeginSeg */ - s_continue_without_stopping = 1U; /* 告诉 BeginSeg：别停表接着开 */ + + s_continue_without_stopping = 1U; } else { - PlsrPulseDriverStop(); /* 必须停表 */ - PlsrPulseDriverClearOnePulseStop(); /* 清单脉冲停 */ - s_continue_without_stopping = 0U; /* 清不停表标志 */ - if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) /* 后续模式但这次接不上 */ + PlsrPulseDriverStop(); + PlsrPulseDriverClearOnePulseStop(); + s_continue_without_stopping = 0U; + if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) { - /* FOLLOW 但必须停表（换向/要等条件）：清掉「接着用上一频」的标记 */ - s_next_seg_start_freq_valid = 0U; /* 清掉链式起速，下一段冷爬 */ + + s_next_seg_start_freq_valid = 0U; } } - PlsrRunControlEnterPostWaitOrNext(); /* 按 wait_type 进等待或下一段 */ + PlsrRunControlEnterPostWaitOrNext(); } -/*============================================================================*/ -/* 故障诊断 */ -/*============================================================================*/ - /** * 0x01 脉冲个数诊断 * 时机：段正常结束进入 AfterSegDone 时。 @@ -1190,39 +1148,37 @@ static void PlsrRunControlAfterSegDone(void) */ static void PlsrRunControlCheckPulseCntFault(void) { - if ((s_seg_pulse_target > 0) && (s_seg_pulses_done != s_seg_pulse_target)) /* 目标>0 且实发对不上 */ + if ((s_seg_pulse_target > 0) && (s_seg_pulses_done != s_seg_pulse_target)) { - PlsrCommandSetFault(PLSR_ERR_PULSE_CNT); /* 报脉冲个数故障（不打断切段） */ + PlsrCommandSetFault(PLSR_ERR_PULSE_CNT); } } -/* 0x03 加速曲线诊断 */ static void PlsrRunControlCheckApproachCurveFault(uint8_t by_pulse_budget) { - if (g_plsr_accel_plan.accel_pulses == 0U) /* 没加速预算就不用查 */ + if (g_plsr_accel_plan.accel_pulses == 0U) { - return; /* 直接过 */ + return; } - if (by_pulse_budget != 0U) /* 按预算精确比对 */ + if (by_pulse_budget != 0U) { - if (g_plsr_accel_runtime.completed_pulses != g_plsr_accel_plan.accel_pulses) /* 走完的步数 ≠ 预算 */ + if (g_plsr_accel_runtime.completed_pulses != g_plsr_accel_plan.accel_pulses) { - PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); /* 加速曲线对不上 */ + PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); } } - else if (g_plsr_accel_runtime.completed_pulses > g_plsr_accel_plan.accel_pulses) /* 提前到频：只查有没有超额 */ + else if (g_plsr_accel_runtime.completed_pulses > g_plsr_accel_plan.accel_pulses) { - PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); /* 多走了，报曲线故障 */ + PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); } } -/* 0x03 减速曲线诊断 */ static void PlsrRunControlCheckDecelCurveFault(void) { - if ((s_phase == PH_DECEL) && (g_plsr_accel_runtime.total_pulses > 0U) && /* 减速相还没走完预算就结束了 */ + if ((s_phase == PH_DECEL) && (g_plsr_accel_runtime.total_pulses > 0U) && (g_plsr_accel_runtime.completed_pulses < g_plsr_accel_runtime.total_pulses)) { - PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); /* 减速曲线对不上 */ + PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); } } @@ -1231,20 +1187,16 @@ static void PlsrRunControlOnApproachDone(void) { uint8_t by_budget; - if (s_phase != PH_APPROACH) /* 本来就不在加速相 */ + if (s_phase != PH_APPROACH) { - return; /* 没事做 */ + return; } - /* n 已走到预算 → 按预算收尾精确比对；否则按提前到频只查超额 */ - by_budget = ((g_plsr_accel_plan.accel_pulses > 0U) /* 是否按加速预算精确比对 */ + + by_budget = ((g_plsr_accel_plan.accel_pulses > 0U) && (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_plan.accel_pulses)) ? 1U : 0U; - PlsrRunControlCheckApproachCurveFault(by_budget); /* 按规则做加速诊断 */ + PlsrRunControlCheckApproachCurveFault(by_budget); } -/*============================================================================*/ -/* 频率 / 相控制 */ -/*============================================================================*/ - /** * @brief 解析段目标频率 * @note freq_hz==0 用 default_spd；结果须落在 [1, 100000]，否则返回 0（调用方写 0x04） @@ -1256,54 +1208,54 @@ static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz, { uint32_t f; - if (freq_hz < 0) /* 频率写成负数 */ + if (freq_hz < 0) { - return 0U;/*频率小于0，直接结束*/ + return 0U; } - if (freq_hz == 0) /* 频率写 0：改用默认速度 */ + if (freq_hz == 0) { - f = default_spd;/*频率为零，替换为默认速度*/ + f = default_spd; } else { - f = (uint32_t)freq_hz;/*频率不为零，替换为传入的频率*/ + f = (uint32_t)freq_hz; } - /* 合法范围：1Hz ~ 100kHz（含） */ - if ((f < 1U) || (f > 100000U)) /* 最终频率越界 */ + + if ((f < 1U) || (f > 100000U)) { - return 0U;/*频率范围不合法直接结束*/ + return 0U; } - *out_hz = f;/*将最终的频率传回去*/ - return 1U; /* 频率可用 */ + *out_hz = f; + return 1U; } /** 任务侧运行输出：0=停表；任务上下文 SetFreq / Start */ static void PlsrRunControlApplyOutFreq(uint32_t freq, uint8_t do_start) { - if (freq == 0U) /* 频率 0：直接停表 */ + if (freq == 0U) { - PlsrPulseDriverStop(); /* 停掉 PWM */ - return; /* 回去 */ + PlsrPulseDriverStop(); + return; } - if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) /* 要开表，或当前根本没在跑 */ + if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) { - PlsrPulseDriverStart(freq); /* 真正开表写频 */ + PlsrPulseDriverStart(freq); } - else if (freq != g_plsr_output_freq_hz) /* 已在跑且频率有变 */ + else if (freq != g_plsr_output_freq_hz) { - PlsrPulseDriverSetFreq(freq); /* 只改频率 */ + PlsrPulseDriverSetFreq(freq); } } /** 脉冲 ISR：只走 ARR 预装载路径 */ static void PlsrRunControlApplyOutFreqIsr(uint32_t freq) { - if (freq < 1U) /* ISR 里频率至少 1Hz */ + if (freq < 1U) { - freq = 1U; /* 兜底成 1 */ + freq = 1U; } - PlsrPulseDriverSetFreqIsr(freq); /* 预装下一拍 ARR */ + PlsrPulseDriverSetFreqIsr(freq); } /** @brief 是否有第二相（减速/谷底回升）脉冲预算 */ @@ -1313,47 +1265,44 @@ static uint8_t PlsrRunControlHasDecel(void) * 第二相脉冲预算 >0 即需要收尾斜坡。 * 谷底形（起/止速 > 目标）：第二相是从目标再加速回止速，f_end > f_tgt。 */ - return (g_plsr_accel_plan.decel_pulses > 0U) ? 1U : 0U; /* 有第二相预算就返回 1 */ + return (g_plsr_accel_plan.decel_pulses > 0U) ? 1U : 0U; } -/* 本段剩余脉冲数（目标 − 已发）；已发完返回 0 */ static uint32_t PlsrRunControlRemainPulses(void) { - if (s_seg_pulse_target > s_seg_pulses_done) /* 还没发完 */ + if (s_seg_pulse_target > s_seg_pulses_done) { - return (uint32_t)(s_seg_pulse_target - s_seg_pulses_done); /* 剩余 = 目标 − 已发 */ + return (uint32_t)(s_seg_pulse_target - s_seg_pulses_done); } - return 0U; /* 发完了当 0 */ + return 0U; } -/* remain 落入减速预算窗口则应进 DECEL */ static uint8_t PlsrRunControlShouldEnterDecel(uint32_t remain) { - if (PlsrRunControlHasDecel() == 0U) /* 没有第二相 */ + if (PlsrRunControlHasDecel() == 0U) { - return 0U; /* 无第二相 */ + return 0U; } - return (remain <= g_plsr_accel_plan.decel_pulses) ? 1U : 0U; /* 剩余落入减速窗口 */ + return (remain <= g_plsr_accel_plan.decel_pulses) ? 1U : 0U; } -/* 进减速时的曲线步进预算：取 min(规划, remain)，至少 1 */ static uint32_t PlsrRunControlDecelEnterBudget(uint32_t remain) { - uint32_t planned = g_plsr_accel_plan.decel_pulses; /* 规划减速脉冲数 */ + uint32_t planned = g_plsr_accel_plan.decel_pulses; - if (planned < 1U) /* 规划里没有减速？至少按 1 */ + if (planned < 1U) { - planned = 1U; /* 兜底 1 */ + planned = 1U; } - if (remain >= planned) /* 剩余够跑完整减速 */ + if (remain >= planned) { - return planned; /* 剩余够跑完整减速 */ + return planned; } - if (remain < 1U) /* 剩余已经没了 */ + if (remain < 1U) { - return 1U; /* 最少给 1 */ + return 1U; } - return remain; /* 剩余不足：压缩预算 */ + return remain; } /** @@ -1362,32 +1311,31 @@ static uint32_t PlsrRunControlDecelEnterBudget(uint32_t remain) */ static uint32_t PlsrRunControlDecelOutFreq(uint32_t freq, uint32_t remain) { - uint32_t end_hz = g_plsr_accel_plan.end_freq_hz; /* 本段止速 */ + uint32_t end_hz = g_plsr_accel_plan.end_freq_hz; - if (end_hz < 1U) /* 止速非法就当 1 */ + if (end_hz < 1U) { - end_hz = 1U; /* 兜底 */ + end_hz = 1U; } - if (remain <= 1U) /* 最后一拍 */ + if (remain <= 1U) { - return end_hz; /* 末拍钉止速 */ + return end_hz; } - /* 降频：提前碰到 end 则让出 +1Hz */ - if (g_plsr_accel_runtime.freq_rising == 0U) /* 正在降频 */ + if (g_plsr_accel_runtime.freq_rising == 0U) { - if ((freq <= end_hz) && (end_hz < 100000U)) /* 提前撞到止速 */ + if ((freq <= end_hz) && (end_hz < 100000U)) { - return end_hz + 1U; /* 让出 1Hz，留给末拍 */ + return end_hz + 1U; } } - /* 升频（谷底回升）：提前碰到 end 则让出 −1Hz */ - else if ((freq >= end_hz) && (end_hz > 1U)) /* 正在升频回止速 */ + + else if ((freq >= end_hz) && (end_hz > 1U)) { - return end_hz - 1U; /* 提前撞到就让出 −1Hz */ + return end_hz - 1U; } - return freq; /* 正常斜坡值原样回 */ + return freq; } /** @@ -1395,61 +1343,58 @@ static uint32_t PlsrRunControlDecelOutFreq(uint32_t freq, uint32_t remain) */ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget) { - PlsrAccelPlan_t decel_plan = g_plsr_accel_plan; /* 局部副本，只改第二相字段 */ + PlsrAccelPlan_t decel_plan = g_plsr_accel_plan; - if (budget < 1U) /* 预算至少 1 */ + if (budget < 1U) { - budget = 1U; /* 兜底 */ + budget = 1U; } - s_phase = PH_DECEL; /* 切到段末过渡相 */ + s_phase = PH_DECEL; - decel_plan.accel_pulses = 0U; /* 第二相不用加速预算 */ - decel_plan.const_pulses = 0U; /* 第二相不用匀速预算 */ - decel_plan.decel_pulses = budget; /* 本段第二相步进数 */ - decel_plan.target_freq_hz = from_hz; /* 减速起点频 */ - PlsrAccelBeginDecel(&g_plsr_accel_runtime, &decel_plan); /* 按副本启动减速斜坡 */ - g_plsr_accel_runtime.cur_freq_hz = from_hz; /* 当前频记成减速起点 */ + decel_plan.accel_pulses = 0U; + decel_plan.const_pulses = 0U; + decel_plan.decel_pulses = budget; + decel_plan.target_freq_hz = from_hz; + PlsrAccelBeginDecel(&g_plsr_accel_runtime, &decel_plan); + g_plsr_accel_runtime.cur_freq_hz = from_hz; } -/* 开表纯减速：只 Start(f1)。下一拍起由 ISR NextFreq 写入 f2…f_end */ static void PlsrRunControlPrimeDecelOnStart(void) { - uint32_t next; /* 局部暂存 f1 */ + uint32_t next; - next = PlsrAccelNextFreq(&g_plsr_accel_runtime); /* 取 f1 */ - PlsrRunControlApplyOutFreq(next, 1U); /* Start PWM */ + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); + PlsrRunControlApplyOutFreq(next, 1U); } -/* 进匀速：写一次 f_tgt，之后 ISR 不改 ARR（复用缓存）*/ static void PlsrRunControlEnterConstHold(uint8_t do_start) { - uint32_t f_hold = g_plsr_accel_plan.target_freq_hz; /* 匀速保持频 */ + uint32_t f_hold = g_plsr_accel_plan.target_freq_hz; - if (f_hold < 1U) /* 峰值非法兜底 */ + if (f_hold < 1U) { - f_hold = 1U; /* 至少 1Hz */ + f_hold = 1U; } - s_phase = PH_CONST; /* 标成匀速相 */ - PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); /* 曲线侧进匀速 */ - PlsrPulseDriverClearPending(); /* 清挂起改频 */ - PlsrPulseDriverClearStartPeriodProtect(); /* 清开表保护 */ - PlsrRunControlApplyOutFreq(f_hold, do_start); /* 写出匀速频率 */ + s_phase = PH_CONST; + PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); + PlsrPulseDriverClearPending(); + PlsrPulseDriverClearStartPeriodProtect(); + PlsrRunControlApplyOutFreq(f_hold, do_start); } -/* ISR 侧进匀速：写 f_tgt，不走任务侧 Start */ static void PlsrRunControlEnterConstHoldIsr(void) { - uint32_t f_hold = g_plsr_accel_plan.target_freq_hz; /* 匀速保持频 */ + uint32_t f_hold = g_plsr_accel_plan.target_freq_hz; - if (f_hold < 1U) /* 峰值非法兜底 */ + if (f_hold < 1U) { - f_hold = 1U; /* 至少 1Hz */ + f_hold = 1U; } - s_phase = PH_CONST; /* 标成匀速相 */ - PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); /* 曲线侧进匀速 */ - PlsrPulseDriverClearPending(); /* 清挂起改频 */ - PlsrPulseDriverClearStartPeriodProtect(); /* 清开表保护 */ - PlsrRunControlApplyOutFreqIsr(f_hold); /* ISR 写 f_tgt */ + s_phase = PH_CONST; + PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); + PlsrPulseDriverClearPending(); + PlsrPulseDriverClearStartPeriodProtect(); + PlsrRunControlApplyOutFreqIsr(f_hold); } /** @@ -1458,53 +1403,51 @@ static void PlsrRunControlEnterConstHoldIsr(void) */ static void PlsrRunControlRefreshProfile(uint8_t do_start) { - uint32_t next; /* 本拍应写频率 */ - uint32_t done_n; /* 已发脉冲（查表用） */ - uint32_t remain; /* 剩余脉冲 */ + uint32_t next; + uint32_t done_n; + uint32_t remain; - done_n = 0U; /* 默认已发 0 */ - if (s_seg_pulses_done > 0) /* 有已发脉冲 */ + done_n = 0U; + if (s_seg_pulses_done > 0) { - done_n = (uint32_t)s_seg_pulses_done; /* 转成无符号给查表 */ + done_n = (uint32_t)s_seg_pulses_done; } - remain = PlsrRunControlRemainPulses(); /* 算剩余 */ + remain = PlsrRunControlRemainPulses(); - /* 匀速但已进减速窗口：先切相再往下写频 */ - if ((s_phase == PH_CONST) && /* 匀速但剩余已进减速窗 */ + if ((s_phase == PH_CONST) && (PlsrRunControlShouldEnterDecel(remain) != 0U)) { - PlsrRunControlEnterDecel(g_plsr_accel_plan.target_freq_hz, /* 先切到段末过渡 */ + PlsrRunControlEnterDecel(g_plsr_accel_plan.target_freq_hz, PlsrRunControlDecelEnterBudget(remain)); } - if (s_phase == PH_CONST) /* 仍是匀速相 */ + if (s_phase == PH_CONST) { - /* 匀速：仅冷启动/未跑时写 f_tgt，跑着则复用 ARR */ - if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) /* 冷启动或还没跑 */ + + if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) { - PlsrRunControlApplyOutFreq(g_plsr_accel_plan.target_freq_hz, do_start); /* 写峰值（真正开表或改频） */ + PlsrRunControlApplyOutFreq(g_plsr_accel_plan.target_freq_hz, do_start); } - return; /* 跑着匀速：ARR 复用，不写 */ + return; } - /* 纯减速开表：Start(f1)，f2…f_end 由后续 UPDATE ISR 写入 */ - if ((s_phase == PH_DECEL) && /* 纯减速且刚开表、还没发过 */ + if ((s_phase == PH_DECEL) && (do_start != 0U) && (s_seg_pulses_done == 0) && (g_plsr_accel_runtime.completed_pulses == 0U)) { - PlsrRunControlPrimeDecelOnStart(); /* Start(f1)，后面 ISR 接着写 */ - return; /* 这拍处理完 */ + PlsrRunControlPrimeDecelOnStart(); + return; } - next = PlsrAccelCurveFreqAtPulse(&g_plsr_accel_plan, done_n); /* 按已发拍查频 */ - if (next < 1U) /* 查表得到的频无效 */ + next = PlsrAccelCurveFreqAtPulse(&g_plsr_accel_plan, done_n); + if (next < 1U) { - next = 1U; /* 兜底 1Hz */ + next = 1U; } - g_plsr_accel_runtime.cur_freq_hz = next; /* 同步运行时当前频 */ + g_plsr_accel_runtime.cur_freq_hz = next; - if (s_phase == PH_APPROACH) /* 还在起速爬峰 */ + if (s_phase == PH_APPROACH) { /* * 加速相结束条件（任一满足即进 CONST 或 DECEL）： @@ -1521,32 +1464,27 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) ((g_plsr_accel_plan.target_freq_hz < g_plsr_accel_plan.start_freq_hz) && (next <= g_plsr_accel_plan.target_freq_hz))) { - next = g_plsr_accel_plan.target_freq_hz; /* 钉峰值 */ - PlsrRunControlOnApproachDone(); /* 加速结束诊断曲线 */ - if (PlsrRunControlShouldEnterDecel(remain) != 0U) /* 该进减速 */ + next = g_plsr_accel_plan.target_freq_hz; + PlsrRunControlOnApproachDone(); + if (PlsrRunControlShouldEnterDecel(remain) != 0U) { - PlsrRunControlEnterDecel(next, /* 切入段末过渡 */ + PlsrRunControlEnterDecel(next, PlsrRunControlDecelEnterBudget(remain)); } else { - PlsrRunControlEnterConstHold(do_start); /* 进匀速并写表 */ - return; /* 匀速路径写完即回 */ + PlsrRunControlEnterConstHold(do_start); + return; } } } - /* 频率有变 / 需开表 / 未在跑 → 写出 */ - if ((next != g_plsr_output_freq_hz) || (do_start != 0U) || (g_plsr_pwm_running == 0U)) /* 频变了 / 要开表 / 当前没跑 */ + if ((next != g_plsr_output_freq_hz) || (do_start != 0U) || (g_plsr_pwm_running == 0U)) { - PlsrRunControlApplyOutFreq(next, do_start); /* 任务侧写出频率 */ + PlsrRunControlApplyOutFreq(next, do_start); } } -/*============================================================================*/ -/* 规划 / 开段 */ -/*============================================================================*/ - /** * @brief 策略层解析本段端点：freq_start / freq_end / accel_ms / decel_ms * @note 起速看 keep/链式落地/冷启动；止速看 EXT_OR_DONE 同向衔接或配置止速 @@ -1554,84 +1492,81 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx, uint32_t f_tgt, PlsrSegPlanEndpoints_t *endpoints) { - endpoints->accel_ms = g_plsr_config.accel_ms; /* 加速参考 ms */ - endpoints->decel_ms = g_plsr_config.decel_ms; /* 减速参考 ms */ + endpoints->accel_ms = g_plsr_config.accel_ms; + endpoints->decel_ms = g_plsr_config.decel_ms; /* —— 起速：不停表衔接 / 链式落地频 / 冷启动起速 —— */ - if (((s_continue_without_stopping != 0U) || (g_plsr_pwm_running != 0U)) /* 还在跑或要不停表接着跑，且当前频有效 */ + if (((s_continue_without_stopping != 0U) || (g_plsr_pwm_running != 0U)) && (g_plsr_output_freq_hz >= 1U)) { - /* FOLLOW/ACT keep：入口 = 当前输出频率 */ - endpoints->freq_start_hz = g_plsr_output_freq_hz; /* 入口就用现在的输出频 */ + + endpoints->freq_start_hz = g_plsr_output_freq_hz; } - else if ((s_next_seg_start_freq_valid != 0U) && (s_next_seg_start_freq_hz >= 1U)) /* 有上一段留下的落地起速 */ + else if ((s_next_seg_start_freq_valid != 0U) && (s_next_seg_start_freq_hz >= 1U)) { - /* 完成模式段末停表后：用本段落地频作起速（如 EXT_OR_DONE 的匀速峰值） */ - endpoints->freq_start_hz = s_next_seg_start_freq_hz; /* 用链式落地频 */ - s_next_seg_start_freq_valid = 0U; /* 用过即清 */ - s_next_seg_start_freq_hz = 0U; /* 用过清掉数值 */ + + endpoints->freq_start_hz = s_next_seg_start_freq_hz; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; } else { - /* 停表后冷启动：按起速参数解析 */ - endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(g_plsr_config.start_speed, /* 按起速参数算出入口频率 */ + + endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(g_plsr_config.start_speed, f_tgt, g_plsr_config.default_speed, endpoints->accel_ms, endpoints->decel_ms); } - /* —— 止速：EXT_OR_DONE 同向衔接 vs 配置止速 —— */ { - int16_t next_eff = PlsrRunControlNextEffectiveSeg(seg_idx); /* 跳过占位后的下一段 */ + int16_t next_eff = PlsrRunControlNextEffectiveSeg(seg_idx); - if ((g_plsr_segs[seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE) && /* EXT_OR_DONE 且后面还有有效段 */ + if ((g_plsr_segs[seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE) && (next_eff >= 0)) { - int32_t end_pos = PlsrRunControlAbsLocalPos(); /* 本段结束时局部坐标 */ - uint8_t next_fwd; /* 下一段方向暂存 */ + int32_t end_pos = PlsrRunControlAbsLocalPos(); + uint8_t next_fwd; - if (s_forward != 0U) /* 正转：末位往上加 */ + if (s_forward != 0U) { - end_pos += s_seg_pulse_target; /* 估段末坐标 */ + end_pos += s_seg_pulse_target; } else { - end_pos -= s_seg_pulse_target; /* 反转：末位往下减 */ + end_pos -= s_seg_pulse_target; } - next_fwd = PlsrRunControlIsForward((uint16_t)next_eff, end_pos); /* 下一段在段末坐标下的方向 */ + next_fwd = PlsrRunControlIsForward((uint16_t)next_eff, end_pos); - /* 与下一有效段同向 → 后续/完成衔接（换向才落到下方止速） */ - if (next_fwd == s_forward) /* 同向，可以谈衔接止速 */ + if (next_fwd == s_forward) { - if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) /* 后续模式 */ + if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) { - /* FOLLOW：出口钉下一段目标频，便于无缝衔接 */ - if (PlsrRunControlGetSegTargetFreq( /* 读下一段目标频当出口 */ + + if (PlsrRunControlGetSegTargetFreq( g_plsr_segs[(uint16_t)next_eff].freq_hz, g_plsr_config.default_speed, &endpoints->freq_end_hz) != 0U) { - return 1U; /* 成功：出口已是下一段目标 */ + return 1U; } - endpoints->freq_end_hz = f_tgt; /* 下一段频非法则退回本段峰值 */ - return 1U; /* 返回（端点填好） */ + endpoints->freq_end_hz = f_tgt; + return 1U; } - /* 完成模式同向：出口钉本段峰值（段末停表后作下一段起速） */ - endpoints->freq_end_hz = f_tgt; /* 完成模式同向：出口钉本段峰值 */ - return 1U; /* 端点填好 */ + + endpoints->freq_end_hz = f_tgt; + return 1U; } - /* 与下一有效段反向：落入下方规划到止速 */ + } } - /* 默认：按配置止速参数解析出口频率 */ - endpoints->freq_end_hz = PlsrAccelCurveResolveEndHz(g_plsr_config.end_speed, /* 默认按配置止速解析出口 */ + endpoints->freq_end_hz = PlsrAccelCurveResolveEndHz(g_plsr_config.end_speed, f_tgt, g_plsr_config.default_speed, endpoints->accel_ms, endpoints->decel_ms); - return 1U; /* 端点解析完成 */ + return 1U; } /** @@ -1644,12 +1579,11 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, uint32_t f_tgt, uint32_t f_end, uint16_t accel_ms, uint16_t decel_ms) { - uint32_t f_start; /* 规划后实际入口频率 */ - uint32_t f_min; /* 本段最低频（锁 PSC） */ - uint32_t f_max; /* 本段最高频（锁 PSC） */ + uint32_t f_start; + uint32_t f_min; + uint32_t f_max; - /* 传给曲线规划模块进行曲线规划 */ - PlsrAccelCurvePlan(&g_plsr_accel_plan, /* 丢给曲线模块做规划 */ + PlsrAccelCurvePlan(&g_plsr_accel_plan, total, f_from, f_tgt, @@ -1659,98 +1593,90 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, decel_ms, g_plsr_config.accel_mode); - /* 0x02：规划峰值达不到设定 → 三角波压峰告警 */ - if ((f_tgt > f_from) && (g_plsr_accel_plan.target_freq_hz < f_tgt)) /* 要爬升但规划峰够不着 */ + if ((f_tgt > f_from) && (g_plsr_accel_plan.target_freq_hz < f_tgt)) { - PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); /* 报频率达不到 */ + PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); } - else if ((f_tgt < f_from) && (g_plsr_accel_plan.target_freq_hz > f_tgt)) /* 要下降但规划峰压不下去 */ + else if ((f_tgt < f_from) && (g_plsr_accel_plan.target_freq_hz > f_tgt)) { - PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); /* 同样报达不到 */ + PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); } - g_plsr_accel_runtime.total_pulses = 0U; /* 开跑前清当前相步进计数 */ + g_plsr_accel_runtime.total_pulses = 0U; - /* S/正弦：在任务上下文预建频率表，ISR 只查表 */ - PlsrAccelPrebuildFreqTables(&g_plsr_accel_plan); /* S/正弦表预建好 */ + PlsrAccelPrebuildFreqTables(&g_plsr_accel_plan); - /* 运行层使用规划后的实际起始频率 */ - f_start = g_plsr_accel_plan.start_freq_hz; /* 规划后的实际入口 */ + f_start = g_plsr_accel_plan.start_freq_hz; - /* 脉冲域：ISR 用 PlsrAccelNextFreq（直线现场算 / S·正弦查预建表）*/ - /* 本段频率范围锁 PSC，升降只改 ARR；跨度过大则 LockPscRange 自动不锁 */ - /* 本段可能出现的最低/最高频率，取 入口、目标、出口 三者的 min/max */ - f_min = g_plsr_accel_plan.target_freq_hz; /* 先用目标频当上下限种子 */ - f_max = g_plsr_accel_plan.target_freq_hz; /* 最高频种子 */ - /* 纳入入口频 */ - if (f_start > f_max) /* 入口比现在最高还高 */ + f_min = g_plsr_accel_plan.target_freq_hz; + f_max = g_plsr_accel_plan.target_freq_hz; + + if (f_start > f_max) { - f_max = f_start; /* 抬高上限 */ + f_max = f_start; } - if (f_start >= 1U) /* 入口频率合法 */ + if (f_start >= 1U) { - if (f_start < f_min) /* 入口更低 */ + if (f_start < f_min) { - f_min = f_start; /* 压低下限 */ + f_min = f_start; } } else { - f_min = 1U; /* 入口无效：下限至少 1 */ + f_min = 1U; } - /* 纳入出口频 */ - if (g_plsr_accel_plan.end_freq_hz > f_max) /* 出口比上限高 */ + if (g_plsr_accel_plan.end_freq_hz > f_max) { - f_max = g_plsr_accel_plan.end_freq_hz; /* 抬高上限 */ + f_max = g_plsr_accel_plan.end_freq_hz; } - if (g_plsr_accel_plan.end_freq_hz >= 1U) /* 出口频率合法 */ + if (g_plsr_accel_plan.end_freq_hz >= 1U) { - if (g_plsr_accel_plan.end_freq_hz < f_min) /* 出口更低 */ + if (g_plsr_accel_plan.end_freq_hz < f_min) { - f_min = g_plsr_accel_plan.end_freq_hz; /* 压低下限 */ + f_min = g_plsr_accel_plan.end_freq_hz; } } else { - if (f_min > 1U) /* 出口无效时看下限 */ + if (f_min > 1U) { - f_min = 1U; /* 下限收到 1 */ + f_min = 1U; } } - if (f_min < 1U) /* 下限再兜底 */ + if (f_min < 1U) { - f_min = 1U; /* 至少 1Hz */ + f_min = 1U; } - if (f_max < f_min) /* 上限反比下限还小 */ + if (f_max < f_min) { - f_max = f_min; /* 兜底：保证 f_max >= f_min */ + f_max = f_min; } - /* 后续同向不停表时 PWM 仍在跑：禁止重锁 PSC */ - if (g_plsr_pwm_running == 0U) /* 当前没在跑才许锁 PSC */ + + if (g_plsr_pwm_running == 0U) { - PlsrPulseDriverLockPscRange(f_min, f_max); /* 按本段频宽锁分频 */ + PlsrPulseDriverLockPscRange(f_min, f_max); } - /* 据规划选初始相：纯减 / 匀速 / 加速 */ - if ((g_plsr_accel_plan.decel_pulses >= total) && /* 全程几乎都是减速且入口已在峰上/之上 */ + if ((g_plsr_accel_plan.decel_pulses >= total) && (total > 0U) && (g_plsr_accel_plan.end_freq_hz != g_plsr_accel_plan.target_freq_hz) && (f_start >= g_plsr_accel_plan.target_freq_hz)) { - /* 纯减开段：全程不够爬峰，直接从入口进减速 */ - PlsrRunControlEnterDecel(f_start, g_plsr_accel_plan.decel_pulses); /* 直接从入口进段末过渡 */ + + PlsrRunControlEnterDecel(f_start, g_plsr_accel_plan.decel_pulses); } - else if (f_start == g_plsr_accel_plan.target_freq_hz) /* 入口正好等于峰值 */ + else if (f_start == g_plsr_accel_plan.target_freq_hz) { - s_phase = PH_CONST; /* 入口已是峰值 → 匀速 */ - PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); /* 曲线侧进匀速 */ + s_phase = PH_CONST; + PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); } - else /* 入口和峰值不同 */ + else { - s_phase = PH_APPROACH; /* 入口 ≠ 峰值 → 加速相 */ - PlsrAccelBeginAccel(&g_plsr_accel_runtime, &g_plsr_accel_plan); /* 曲线侧开始加速 */ + s_phase = PH_APPROACH; + PlsrAccelBeginAccel(&g_plsr_accel_runtime, &g_plsr_accel_plan); } } diff --git a/plsr/signal_io/plsr_signal_io.c b/plsr/signal_io/plsr_signal_io.c index c6020a9..e8f0c49 100644 --- a/plsr/signal_io/plsr_signal_io.c +++ b/plsr/signal_io/plsr_signal_io.c @@ -34,16 +34,16 @@ /* ---------- 硬件引脚 ---------- */ #define PLSR_X4_PORT GPIOB -#define PLSR_X4_PIN GPIO_PIN_5 /* X4：WAIT/EXT 可选 */ +#define PLSR_X4_PIN GPIO_PIN_5 #define PLSR_X5_PORT GPIOG -#define PLSR_X5_PIN GPIO_PIN_12 /* X5：WAIT/EXT 可选 */ +#define PLSR_X5_PIN GPIO_PIN_12 /* * TIM5 时基：84MHz / (8399+1) = 10kHz → 100µs/格。 * PSC=8399 落在 16 位内 */ #define PLSR_TIM5_PSC_100US 8399U -#define PLSR_TIM5_TICKS_PER_MS 10U /* 10×100µs = 1ms */ +#define PLSR_TIM5_TICKS_PER_MS 10U #define PLSR_DELAY_MS_MAX 65535U /** OS 节拍消抖窗口（ms），与 PlsrTask tick 配合 */ @@ -51,17 +51,17 @@ /** 单路 X 口消抖状态（pending / fell 由 ISR 与任务共享，volatile） */ typedef struct { - volatile uint8_t pending; /* 1=EXTI 已见下降沿，待 Confirm */ - volatile uint8_t stamp_ok; /* 1=已记 arm_tick，正在计时 */ - volatile INT32U arm_tick; /* 消抖起点 OS tick */ - volatile uint8_t fell; /* 1=消抖确认的有效下降沿，Take* 消费 */ + volatile uint8_t pending; + volatile uint8_t stamp_ok; + volatile INT32U arm_tick; + volatile uint8_t fell; GPIO_TypeDef *port; uint16_t pin; } PlsrEdge_t; -static PlsrEdge_t s_debounce_x4; /* X4 消抖状态 */ -static PlsrEdge_t s_debounce_x5; /* X5 消抖状态 */ -static volatile uint8_t s_delay_armed; /* 1=TIM5 单次延时正在计时 */ +static PlsrEdge_t s_debounce_x4; +static PlsrEdge_t s_debounce_x5; +static volatile uint8_t s_delay_armed; /** 毫秒 → OS tick 数（向上取整，至少 1 tick） */ static INT32U PlsrSignalIoMsToTicks(uint32_t ms) @@ -164,14 +164,14 @@ static void PlsrSignalIoTim5Init(void) { __HAL_RCC_TIM5_CLK_ENABLE(); - TIM5->CR1 = 0U; /* 停表：向上计、关 OPM */ - TIM5->DIER = 0U; /* 关 UPDATE 中断 */ - TIM5->SR = 0U; /* 清状态标志 */ - TIM5->PSC = PLSR_TIM5_PSC_100US; /* PSC=8399 → 10kHz（100µs/格），非 1MHz */ - TIM5->ARR = 0xFFFFFFFFUL; /* 预装最大 ARR，真正延时在 Schedule 重写 */ + TIM5->CR1 = 0U; + TIM5->DIER = 0U; + TIM5->SR = 0U; + TIM5->PSC = PLSR_TIM5_PSC_100US; + TIM5->ARR = 0xFFFFFFFFUL; TIM5->CNT = 0U; - TIM5->EGR = TIM_EGR_UG; /* UG：PSC 立刻生效（会置 UIF） */ - TIM5->SR = 0U; /* 清 UG 带来的 UIF */ + TIM5->EGR = TIM_EGR_UG; + TIM5->SR = 0U; HAL_NVIC_SetPriority(TIM5_IRQn, 7, 0); HAL_NVIC_EnableIRQ(TIM5_IRQn); @@ -198,7 +198,7 @@ void PlsrSignalIoInit(void) PlsrSignalIoGpioExtiInit(); PlsrSignalIoTim5Init(); - /* 配完 NVIC 后再清一次，避免配置期误沿 */ + __HAL_GPIO_EXTI_CLEAR_IT(PLSR_X4_PIN); __HAL_GPIO_EXTI_CLEAR_IT(PLSR_X5_PIN); s_debounce_x4.fell = 0U; @@ -229,13 +229,13 @@ void PlsrSignalIoScheduleDelayMs(uint32_t delay_ms) TIM5->SR = 0U; TIM5->PSC = PLSR_TIM5_PSC_100US; TIM5->ARR = ticks - 1U; - TIM5->CR1 = TIM_CR1_URS; /* 仅溢出产生 UPDATE */ - TIM5->EGR = TIM_EGR_UG; /* PSC/ARR 立刻生效 */ - TIM5->SR = 0U; /* 清 UG 带来的 UIF */ + TIM5->CR1 = TIM_CR1_URS; + TIM5->EGR = TIM_EGR_UG; + TIM5->SR = 0U; TIM5->CNT = 0U; TIM5->DIER = TIM_DIER_UIE; s_delay_armed = 1U; - TIM5->CR1 = (uint32_t)(TIM_CR1_CEN | TIM_CR1_OPM | TIM_CR1_URS); /* 开表单次 */ + TIM5->CR1 = (uint32_t)(TIM_CR1_CEN | TIM_CR1_OPM | TIM_CR1_URS); } /** @brief 取消 TIM5 延时并清武装 */ @@ -305,7 +305,7 @@ void PlsrSignalIoClearPending(void) /** @brief 取 WAIT 口有效下降沿（读后清） */ uint8_t PlsrSignalIoTakeWaitFalling(void) { - /* wait_x_sel：0=X4，1=X5 */ + if (g_plsr_config.wait_x_sel == 0U) { if (s_debounce_x4.fell != 0U) @@ -325,7 +325,7 @@ uint8_t PlsrSignalIoTakeWaitFalling(void) /** @brief 取 EXT 口有效下降沿（读后清） */ uint8_t PlsrSignalIoTakeExtFalling(void) { - /* ext_x_sel：0=X4，1=X5 */ + if (g_plsr_config.ext_x_sel == 0U) { if (s_debounce_x4.fell != 0U) @@ -353,7 +353,7 @@ void PlsrSignalIoOnTim5Irq(void) if (s_delay_armed != 0U) { s_delay_armed = 0U; - PlsrRunControlOnDelayTimer(); /* 方向/WAIT/ACT 到期 */ + PlsrRunControlOnDelayTimer(); } } }