yuwenhao
/
Modbus-rtu
1
0
Fork 0
Koodi Ongelmat 0 Pull-pyynnöt 0 Julkaisut 0 Wiki Toiminta
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
38 Commitit
3 Branchit
138 MiB
 
 
 
 
 
 
Puu: 6c61682617
Branchit Tagit
${ item.name }
Create branch ${ searchTerm }
from '6c61682617'
${ noResults }
Modbus-rtu/PLSR/Src/plsr_core.c

1880 rivejä
62 KiB
Raaka Blame Historia 

  1. #include "plsr_core.h"

  2. #include "plc_device.h"

  3. #include "plsr_address_map.h"

  4. #include "plsr_hal_f407.h"

  5. #include "plsr_path.h"

  6. #include "plsr_profile.h"

  7. #include "plsr_resource.h"

  8. #include <string.h>

  9. 

  10. #define PLSR_CORE_SFD_AXIS_STRIDE      (130U)

  11. 

  12. #ifndef PLSR_HOST_TEST

  13. #include "stm32f4xx.h"

  14. #include "ucos_ii.h"

  15. #endif

  16. 

  17. typedef struct

  18. {

  19.     PLSR_STATE state;

  20.     PLSR_OUTPUT_MODE outputMode;

  21.     PLSR_ERROR error;

  22.     uint16_t compatibleErrorCode;

  23.     uint16_t compatibleErrorBlock;

  24.     PLSR_STOP_REASON stopReason;

  25.     PLSR_STATE pendingTerminal;

  26.     PLSR_RESOURCE_LEASE lease;

  27.     PLSR_JOB_SNAPSHOT job;

  28.     PLSR_PARSE_DETAIL parseDetail;

  29.     volatile uint32_t pendingEvents;

  30.     uint32_t lastCommandSequence;

  31.     PLSR_RESULT lastCommandResult;

  32.     uint32_t illegalTransitionCount;

  33.     int64_t logicalPosition;

  34.     int64_t taskPulses;

  35.     int64_t totalPulses;

  36.     int64_t segmentAccountedPulses;

  37.     int64_t equivalentCommandRemainder;

  38.     PLSR_EQUIVALENT_CONFIG equivalent;

  39.     PLSR_PATH_CONTEXT path;

  40.     PLSR_PROFILE_STATE profile;

  41.     uint8_t profileActive;

  42.     uint8_t profileWasAccel;

  43.     uint8_t hasLastCommand;

  44.     uint8_t done;

  45.     uint8_t directionPositive;

  46.     uint8_t immediateStopPending;

  47.     uint8_t positionValid;

  48.     uint8_t jobValid;

  49.     uint8_t positionOverflow;

  50.     uint8_t segmentAccountingActive;

  51. } PLSR_AXIS;

  52. 

  53. typedef struct

  54. {

  55.     PLSR_COMMAND command;

  56.     PLSR_START_REQUEST start;

  57.     PLSR_CALL call;

  58.     uint32_t ticket;

  59.     uint8_t hasStart;

  60.     uint8_t hasCall;

  61.     uint8_t occupied;

  62. } PLSR_COMMAND_SLOT;

  63. 

  64. static PLSR_AXIS PlsrAxes[PLSR_AXIS_COUNT];

  65. static PLSR_COMMAND_SLOT PlsrCommandQueue[PLSR_COMMAND_QUEUE_DEPTH];

  66. static uint32_t PlsrNextTicket;

  67. static uint8_t PlsrInitialized;

  68. static PLSR_JOB_SNAPSHOT PlsrJobScratch;

  69. 

  70. static void PlsrStopSegmentHardware(uint8_t axis, PLSR_AXIS *axisObject);

  71. static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis,

  72.                                             PLSR_AXIS *axisObject);

  73. static void PlsrAccountHardwarePulses(uint8_t axis,

  74.                                       PLSR_AXIS *axisObject);

  75. 

  76. static uint32_t PlsrCoreEnterCritical(void)

  77. {

  78. #ifdef PLSR_HOST_TEST

  79.     return 0UL;

  80. #else

  81.     uint32_t interruptState = __get_PRIMASK();

  82.     __disable_irq();

  83.     __DMB();

  84.     return interruptState;

  85. #endif

  86. }

  87. 

  88. static void PlsrCoreExitCritical(uint32_t interruptState)

  89. {

  90. #ifdef PLSR_HOST_TEST

  91.     (void)interruptState;

  92. #else

  93.     __DMB();

  94.     if (interruptState == 0UL)

  95.     {

  96.         __enable_irq();

  97.     }

  98. #endif

  99. }

  100. 

  101. static uint8_t PlsrStateIsBusy(PLSR_STATE state)

  102. {

  103.     return ((state == PLSR_STATE_ACCEL) || (state == PLSR_STATE_RUN)

  104.             || (state == PLSR_STATE_DECEL) || (state == PLSR_STATE_WAIT)

  105.             || (state == PLSR_STATE_PAUSED))

  106.                ? 1U

  107.                : 0U;

  108. }

  109. 

  110. static uint8_t PlsrStateIsPulseActive(PLSR_STATE state)

  111. {

  112.     return ((state == PLSR_STATE_ACCEL) || (state == PLSR_STATE_RUN)

  113.             || (state == PLSR_STATE_DECEL))

  114.                ? 1U

  115.                : 0U;

  116. }

  117. 

  118. static void PlsrLoadAxisEquivalentConfig(uint8_t axis,

  119.                                          PLSR_EQUIVALENT_CONFIG *config)

  120. {

  121.     uint16_t base = (uint16_t)(PLSR_SFD_CONFIG_START

  122.                                + (uint16_t)axis

  123.                                      * PLSR_CORE_SFD_AXIS_STRIDE);

  124.     uint16_t word;

  125.     uint16_t lowWord;

  126.     uint16_t highWord;

  127. 

  128.     config->unitCode = 0U;

  129.     config->pulsesPerRevolution = 1UL;

  130.     config->movementPerRevolution = 1UL;

  131.     if (PlcDeviceReadSfd(base, &word) != PLC_DEVICE_OK)

  132.     {

  133.         return;

  134.     }

  135.     config->unitCode = (uint8_t)((word >> 8U) & 0x07U);

  136.     if ((PlcDeviceReadSfd((uint16_t)(base + 2U), &lowWord)

  137.          != PLC_DEVICE_OK)

  138.         || (PlcDeviceReadSfd((uint16_t)(base + 3U), &highWord)

  139.             != PLC_DEVICE_OK))

  140.     {

  141.         config->unitCode = 0U;

  142.         return;

  143.     }

  144.     config->pulsesPerRevolution = (uint32_t)lowWord

  145.                                   | ((uint32_t)highWord << 16U);

  146.     if ((PlcDeviceReadSfd((uint16_t)(base + 4U), &lowWord)

  147.          != PLC_DEVICE_OK)

  148.         || (PlcDeviceReadSfd((uint16_t)(base + 5U), &highWord)

  149.             != PLC_DEVICE_OK))

  150.     {

  151.         config->unitCode = 0U;

  152.         return;

  153.     }

  154.     config->movementPerRevolution = (uint32_t)lowWord

  155.                                     | ((uint32_t)highWord << 16U);

  156.     if (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK)

  157.     {

  158.         config->unitCode = 0U;

  159.         config->pulsesPerRevolution = 1UL;

  160.         config->movementPerRevolution = 1UL;

  161.     }

  162. }

  163. 

  164. static void PlsrPublishSdDword(uint8_t axis,

  165.                                uint8_t lowItem,

  166.                                int32_t value)

  167. {

  168.     (void)PlcDevicePublishSdDword(axis, lowItem, value);

  169. }

  170. 

  171. static int32_t PlsrGetCompatibleSegmentPulses(const PLSR_AXIS *axisObject)

  172. {

  173.     int64_t signedPulses = (axisObject->directionPositive != 0U)

  174.                                ? axisObject->segmentAccountedPulses

  175.                                : -axisObject->segmentAccountedPulses;

  176. 

  177.     if (signedPulses > INT32_MAX)

  178.     {

  179.         return INT32_MAX;

  180.     }

  181.     if (signedPulses < INT32_MIN)

  182.     {

  183.         return INT32_MIN;

  184.     }

  185.     return (int32_t)signedPulses;

  186. }

  187. 

  188. static int32_t PlsrClampCompatibleInt32(int64_t value)

  189. {

  190.     if (value > INT32_MAX) return INT32_MAX;

  191.     if (value < INT32_MIN) return INT32_MIN;

  192.     return (int32_t)value;

  193. }

  194. 

  195. static void PlsrPublishRuntime(uint8_t axis)

  196. {

  197.     PLSR_AXIS *axisObject = &PlsrAxes[axis];

  198.     uint16_t currentSegment = (axisObject->jobValid != 0U)

  199.                                   ? PlsrPathGetCurrentSegment(

  200.                                         &axisObject->path)

  201.                                   : 0U;

  202.     int32_t segmentPulses = PlsrGetCompatibleSegmentPulses(axisObject);

  203.     int32_t frequencyHz = (int32_t)PlsrHwGetCurrentFrequencyHz(axis);

  204.     int64_t segmentEquivalent64 = segmentPulses;

  205.     uint32_t speed = (uint32_t)frequencyHz;

  206.     uint16_t publishedError;

  207.     uint16_t publishedBlock;

  208. 

  209.     if (PlsrPositionPulsesToUnits(&axisObject->equivalent,

  210.                                   segmentPulses,

  211.                                   &segmentEquivalent64) != PLSR_RESULT_OK)

  212.     {

  213.         axisObject->positionOverflow = 1U;

  214.         segmentEquivalent64 = (segmentPulses < 0) ? INT32_MIN : INT32_MAX;

  215.     }

  216.     if (PlsrPositionPulseFrequencyToSpeed(&axisObject->equivalent,

  217.                                           (uint32_t)frequencyHz,

  218.                                           &speed) != PLSR_RESULT_OK)

  219.     {

  220.         speed = UINT32_MAX;

  221.     }

  222. 

  223.     PlsrPublishSdDword(axis,

  224.                        PLSR_SD_ITEM_SEGMENT,

  225.                        (int32_t)currentSegment);

  226.     PlsrPublishSdDword(axis,

  227.                        PLSR_SD_ITEM_SEGMENT_PULSES,

  228.                        segmentPulses);

  229.     PlsrPublishSdDword(axis,

  230.                        PLSR_SD_ITEM_SEGMENT_EQUIV,

  231.                        PlsrClampCompatibleInt32(segmentEquivalent64));

  232.     PlsrPublishSdDword(axis, PLSR_SD_ITEM_FREQUENCY, frequencyHz);

  233.     PlsrPublishSdDword(axis,

  234.                        PLSR_SD_ITEM_SPEED,

  235.                        (speed > (uint32_t)INT32_MAX) ? INT32_MAX

  236.                                                     : (int32_t)speed);

  237.     publishedError = (axisObject->compatibleErrorCode != 0U)

  238.                          ? axisObject->compatibleErrorCode

  239.                          : (uint16_t)axisObject->error;

  240.     publishedBlock = (axisObject->compatibleErrorCode != 0U)

  241.                          ? axisObject->compatibleErrorBlock

  242.                          : (((axisObject->error != PLSR_ERROR_NONE)

  243.                              && (axisObject->jobValid != 0U))

  244.                                 ? currentSegment

  245.                                 : 0U);

  246.     (void)PlcDevicePublishSd(axis,

  247.                              PLSR_SD_ITEM_ERROR_CODE,

  248.                              (int32_t)publishedError);

  249.     (void)PlcDevicePublishSd(axis,

  250.                              PLSR_SD_ITEM_ERROR_BLOCK,

  251.                              (int32_t)publishedBlock);

  252. }

  253. 

  254. static void PlsrSetCompatibleEquivalentError(uint8_t axis,

  255.                                               PLSR_AXIS *axisObject)

  256. {

  257.     uint16_t commonBase = (uint16_t)(PLSR_SFD_CONFIG_START

  258.                                       + (uint16_t)axis

  259.                                             * PLSR_CORE_SFD_AXIS_STRIDE);

  260. 

  261.     if ((axisObject->parseDetail.result == PLSR_RESULT_INVALID_S2)

  262.         && (axisObject->parseDetail.block == PLSR_PARSE_BLOCK_S2)

  263.         && ((axisObject->parseDetail.address

  264.              == (uint32_t)(commonBase + 2U))

  265.             || (axisObject->parseDetail.address

  266.                 == (uint32_t)(commonBase + 4U))))

  267.     {

  268.         axisObject->compatibleErrorCode = 2U;

  269.         axisObject->compatibleErrorBlock = 0U;

  270.     }

  271. }

  272. 

  273. static void PlsrPublishAxis(uint8_t axis)

  274. {

  275.     PLSR_AXIS *axisObject = &PlsrAxes[axis];

  276.     uint8_t compatibleRun;

  277. 

  278.     compatibleRun = (uint8_t)((PlsrStateIsPulseActive(axisObject->state) != 0U)

  279.                               || (axisObject->state == PLSR_STATE_WAIT));

  280.     (void)PlcDevicePublishSm(axis,

  281.                              compatibleRun,

  282.                              axisObject->directionPositive);

  283.     PlsrPublishRuntime(axis);

  284. }

  285. 

  286. static void PlsrSetStopReason(PLSR_AXIS *axis,

  287.                               PLSR_STOP_REASON stopReason)

  288. {

  289.     if (stopReason > axis->stopReason)

  290.     {

  291.         axis->stopReason = stopReason;

  292.     }

  293. }

  294. 

  295. /* 将 64 位逻辑位置发布为 HSD 的 32 位兼容值。

  296.  * 超出范围时保持最近一次合法值，只锁存诊断，禁止回绕或静默截断。 */

  297. static void PlsrPublishPosition(uint8_t axis, PLSR_AXIS *axisObject)

  298. {

  299.     int32_t compatValue;

  300.     int64_t equivalentPosition;

  301.     uint16_t base = (uint16_t)((uint16_t)axis

  302.                                * PLSR_HSD_RUNTIME_AXIS_COUNT);

  303. 

  304.     if ((axisObject->logicalPosition > INT32_MAX)

  305.         || (axisObject->logicalPosition < INT32_MIN))

  306.     {

  307.         axisObject->positionOverflow = 1U;

  308.         return;

  309.     }

  310.     compatValue = (int32_t)axisObject->logicalPosition;

  311.     (void)PlcDevicePublishHsdDword(base, compatValue);

  312.     if ((PlsrPositionPulsesToUnits(&axisObject->equivalent,

  313.                                    axisObject->logicalPosition,

  314.                                    &equivalentPosition) != PLSR_RESULT_OK)

  315.         || (equivalentPosition > INT32_MAX)

  316.         || (equivalentPosition < INT32_MIN))

  317.     {

  318.         axisObject->positionOverflow = 1U;

  319.         return;

  320.     }

  321.     (void)PlcDevicePublishHsdDword((uint16_t)(base + 2U),

  322.                                    (int32_t)equivalentPosition);

  323. }

  324. 

  325. static uint8_t PlsrAddInt64Checked(int64_t left,

  326.                                    int64_t right,

  327.                                    int64_t *result)

  328. {

  329.     if ((result == NULL)

  330.         || ((right > 0) && (left > INT64_MAX - right))

  331.         || ((right < 0) && (left < INT64_MIN - right)))

  332.     {

  333.         return 0U;

  334.     }

  335.     *result = left + right;

  336.     return 1U;

  337. }

  338. 

  339. /* 将 HAL 实际完成的完整脉冲/AB周期合并到64位位置。

  340.  * emittedPulses 每段从0开始，因此用 segmentAccountedPulses 做差量去重。 */

  341. static void PlsrAccountHardwarePulses(uint8_t axis,

  342.                                       PLSR_AXIS *axisObject)

  343. {

  344.     int64_t emittedPulses;

  345.     int64_t delta;

  346.     int64_t signedDelta;

  347.     int64_t newLogicalPosition;

  348.     int64_t newTaskPulses;

  349.     int64_t newTotalPulses;

  350. 

  351.     if (axisObject->segmentAccountingActive == 0U)

  352.     {

  353.         return;

  354.     }

  355.     emittedPulses = PlsrHwGetEmittedPulses(axis);

  356.     if ((emittedPulses < 0)

  357.         || (emittedPulses < axisObject->segmentAccountedPulses))

  358.     {

  359.         axisObject->positionOverflow = 1U;

  360.         (void)PlsrPostEvent(axis, PLSR_EVENT_COUNTER_FAULT);

  361.         return;

  362.     }

  363.     delta = emittedPulses - axisObject->segmentAccountedPulses;

  364.     if (delta == 0)

  365.     {

  366.         return;

  367.     }

  368.     signedDelta = (axisObject->directionPositive != 0U) ? delta : -delta;

  369.     if ((PlsrAddInt64Checked(axisObject->logicalPosition,

  370.                              signedDelta,

  371.                              &newLogicalPosition) == 0U)

  372.         || (PlsrAddInt64Checked(axisObject->taskPulses,

  373.                                 signedDelta,

  374.                                 &newTaskPulses) == 0U)

  375.         || (PlsrAddInt64Checked(axisObject->totalPulses,

  376.                                 delta,

  377.                                 &newTotalPulses) == 0U))

  378.     {

  379.         axisObject->positionOverflow = 1U;

  380.         axisObject->segmentAccountedPulses = emittedPulses;

  381.         (void)PlsrPostEvent(axis, PLSR_EVENT_COUNTER_FAULT);

  382.         return;

  383.     }

  384. 

  385.     axisObject->logicalPosition = newLogicalPosition;

  386.     axisObject->taskPulses = newTaskPulses;

  387.     axisObject->totalPulses = newTotalPulses;

  388.     axisObject->segmentAccountedPulses = emittedPulses;

  389.     PlsrPublishPosition(axis, axisObject);

  390.     PlsrPublishRuntime(axis);

  391. }

  392. 

  393. static uint8_t PlsrTransitionIsAllowed(PLSR_STATE current,

  394.                                        PLSR_STATE target)

  395. {

  396.     if (current == target)

  397.     {

  398.         return 1U;

  399.     }

  400.     if (target == PLSR_STATE_ERROR)

  401.     {

  402.         return 1U;

  403.     }

  404. 

  405.     switch (current)

  406.     {

  407.         case PLSR_STATE_UNINITIALIZED:

  408.             return (target == PLSR_STATE_IDLE) ? 1U : 0U;

  409. 

  410.         case PLSR_STATE_IDLE:

  411.         case PLSR_STATE_COMPLETED:

  412.         case PLSR_STATE_STOPPED:

  413.             return ((target == PLSR_STATE_ACCEL)

  414.                     || (target == PLSR_STATE_WAIT)

  415.                     || (target == PLSR_STATE_COMPLETED))

  416.                        ? 1U

  417.                        : 0U;

  418. 

  419.         case PLSR_STATE_ACCEL:

  420.         case PLSR_STATE_RUN:

  421.             return ((target == PLSR_STATE_ACCEL)

  422.                     || (target == PLSR_STATE_RUN)

  423.                     || (target == PLSR_STATE_DECEL)

  424.                     || (target == PLSR_STATE_WAIT)

  425.                     || (target == PLSR_STATE_COMPLETED)

  426.                     || (target == PLSR_STATE_STOPPED))

  427.                        ? 1U

  428.                        : 0U;

  429. 

  430.         case PLSR_STATE_DECEL:

  431.             return ((target == PLSR_STATE_ACCEL)

  432.                     || (target == PLSR_STATE_RUN)

  433.                     || (target == PLSR_STATE_WAIT)

  434.                     || (target == PLSR_STATE_PAUSED)

  435.                     || (target == PLSR_STATE_COMPLETED)

  436.                     || (target == PLSR_STATE_STOPPED))

  437.                        ? 1U

  438.                        : 0U;

  439. 

  440.         case PLSR_STATE_WAIT:

  441.             return ((target == PLSR_STATE_ACCEL)

  442.                     || (target == PLSR_STATE_RUN)

  443.                     || (target == PLSR_STATE_PAUSED)

  444.                     || (target == PLSR_STATE_COMPLETED)

  445.                     || (target == PLSR_STATE_STOPPED))

  446.                        ? 1U

  447.                        : 0U;

  448. 

  449.         case PLSR_STATE_PAUSED:

  450.             return ((target == PLSR_STATE_ACCEL)

  451.                     || (target == PLSR_STATE_RUN)

  452.                     || (target == PLSR_STATE_STOPPED))

  453.                        ? 1U

  454.                        : 0U;

  455. 

  456.         case PLSR_STATE_ERROR:

  457.             return (target == PLSR_STATE_IDLE) ? 1U : 0U;

  458. 

  459.         default:

  460.             return 0U;

  461.     }

  462. }

  463. 

  464. PLSR_RESULT PlsrStateTransition(uint8_t axis,

  465.                                 PLSR_STATE target,

  466.                                 PLSR_TRANSITION_REASON reason)

  467. {

  468.     PLSR_AXIS *axisObject;

  469. 

  470.     (void)reason;

  471.     if (axis >= PLSR_AXIS_COUNT)

  472.     {

  473.         return PLSR_RESULT_INVALID_AXIS;

  474.     }

  475.     if ((uint32_t)target > (uint32_t)PLSR_STATE_ERROR)

  476.     {

  477.         return PLSR_RESULT_INVALID_ARGUMENT;

  478.     }

  479. 

  480.     axisObject = &PlsrAxes[axis];

  481.     if (PlsrTransitionIsAllowed(axisObject->state, target) == 0U)

  482.     {

  483.         axisObject->illegalTransitionCount++;

  484.         axisObject->error = PLSR_ERROR_ILLEGAL_TRANSITION;

  485.         axisObject->stopReason = PLSR_STOP_REASON_FAULT;

  486.         axisObject->done = 0U;

  487.         axisObject->state = PLSR_STATE_ERROR;

  488.         axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

  489.         axisObject->immediateStopPending = 0U;

  490.         PlsrStopSegmentHardware(axis, axisObject);

  491.         PlsrResourceRelease(&axisObject->lease);

  492.         PlsrPublishAxis(axis);

  493.         return PLSR_RESULT_INVALID_STATE;

  494.     }

  495. 

  496.     axisObject->state = target;

  497.     if (target == PLSR_STATE_COMPLETED)

  498.     {

  499.         axisObject->done = 1U;

  500.     }

  501.     if ((target == PLSR_STATE_COMPLETED) || (target == PLSR_STATE_STOPPED)

  502.         || (target == PLSR_STATE_ERROR))

  503.     {

  504.         axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

  505.         axisObject->immediateStopPending = 0U;

  506.         PlsrPathTerminate(&axisObject->path);

  507.         PlsrStopSegmentHardware(axis, axisObject);

  508.         PlsrResourceRelease(&axisObject->lease);

  509.         /* 运动已结束：记录 lastBusy=0，保证掉电后恢复时位置仍可信。 */

  510.         (void)PlcDeviceSetHsdCheckpointMeta(axisObject->positionValid, 0U);

  511.         (void)PlcDeviceCheckpointHsd();

  512.     }

  513.     PlsrPublishAxis(axis);

  514.     return PLSR_RESULT_OK;

  515. }

  516. 

  517. static uint8_t PlsrCommandPriority(PLSR_COMMAND_OPCODE opcode)

  518. {

  519.     switch (opcode)

  520.     {

  521.         case PLSR_CMD_STOP_IMMEDIATE:

  522.             return 0U;

  523.         case PLSR_CMD_STOP_DECEL:

  524.             return 1U;

  525.         case PLSR_CMD_PAUSE:

  526.             return 2U;

  527.         case PLSR_CMD_RESUME:

  528.             return 3U;

  529.         default:

  530.             return 4U;

  531.     }

  532. }

  533. 

  534. static PLSR_RESULT PlsrQueueCommand(const PLSR_COMMAND *command,

  535.                                     const PLSR_START_REQUEST *start,

  536.                                     const PLSR_CALL *call)

  537. {

  538.     uint32_t interruptState;

  539.     uint8_t freeSlot = PLSR_COMMAND_QUEUE_DEPTH;

  540.     uint8_t index;

  541. 

  542.     interruptState = PlsrCoreEnterCritical();

  543.     if ((PlsrAxes[command->axis].hasLastCommand != 0U)

  544.         && (PlsrAxes[command->axis].lastCommandSequence == command->sequence))

  545.     {

  546.         PLSR_RESULT result = PlsrAxes[command->axis].lastCommandResult;

  547.         PlsrCoreExitCritical(interruptState);

  548.         return result;

  549.     }

  550. 

  551.     for (index = 0U; index < PLSR_COMMAND_QUEUE_DEPTH; index++)

  552.     {

  553.         if (PlsrCommandQueue[index].occupied != 0U)

  554.         {

  555.             if ((PlsrCommandQueue[index].command.axis == command->axis)

  556.                 && (PlsrCommandQueue[index].command.sequence

  557.                     == command->sequence))

  558.             {

  559.                 PlsrCoreExitCritical(interruptState);

  560.                 return PLSR_RESULT_QUEUED;

  561.             }

  562.         }

  563.         else if (freeSlot == PLSR_COMMAND_QUEUE_DEPTH)

  564.         {

  565.             freeSlot = index;

  566.         }

  567.     }

  568. 

  569.     if (freeSlot == PLSR_COMMAND_QUEUE_DEPTH)

  570.     {

  571.         PlsrCoreExitCritical(interruptState);

  572.         return PLSR_RESULT_QUEUE_FULL;

  573.     }

  574. 

  575.     PlsrCommandQueue[freeSlot].command = *command;

  576.     if (start != NULL)

  577.     {

  578.         PlsrCommandQueue[freeSlot].start = *start;

  579.         PlsrCommandQueue[freeSlot].hasStart = 1U;

  580.     }

  581.     else

  582.     {

  583.         (void)memset(&PlsrCommandQueue[freeSlot].start,

  584.                      0,

  585.                      sizeof(PlsrCommandQueue[freeSlot].start));

  586.         PlsrCommandQueue[freeSlot].hasStart = 0U;

  587.     }

  588.     if (call != NULL)

  589.     {

  590.         PlsrCommandQueue[freeSlot].call = *call;

  591.         PlsrCommandQueue[freeSlot].hasCall = 1U;

  592.     }

  593.     else

  594.     {

  595.         (void)memset(&PlsrCommandQueue[freeSlot].call,

  596.                      0,

  597.                      sizeof(PlsrCommandQueue[freeSlot].call));

  598.         PlsrCommandQueue[freeSlot].hasCall = 0U;

  599.     }

  600.     PlsrCommandQueue[freeSlot].ticket = PlsrNextTicket++;

  601.     PlsrCommandQueue[freeSlot].occupied = 1U;

  602.     PlsrCoreExitCritical(interruptState);

  603.     return PLSR_RESULT_QUEUED;

  604. }

  605. 

  606. static uint8_t PlsrPopHighestPriorityCommand(PLSR_COMMAND_SLOT *slot)

  607. {

  608.     uint32_t interruptState;

  609.     uint32_t selectedTicket = 0UL;

  610.     uint8_t selectedPriority = 0xFFU;

  611.     uint8_t selected = PLSR_COMMAND_QUEUE_DEPTH;

  612.     uint8_t index;

  613.     uint8_t priority;

  614. 

  615.     interruptState = PlsrCoreEnterCritical();

  616.     for (index = 0U; index < PLSR_COMMAND_QUEUE_DEPTH; index++)

  617.     {

  618.         if (PlsrCommandQueue[index].occupied == 0U)

  619.         {

  620.             continue;

  621.         }

  622.         priority = PlsrCommandPriority(PlsrCommandQueue[index].command.opcode);

  623.         if ((selected == PLSR_COMMAND_QUEUE_DEPTH)

  624.             || (priority < selectedPriority)

  625.             || ((priority == selectedPriority)

  626.                 && ((int32_t)(PlsrCommandQueue[index].ticket

  627.                               - selectedTicket)

  628.                     < 0)))

  629.         {

  630.             selected = index;

  631.             selectedPriority = priority;

  632.             selectedTicket = PlsrCommandQueue[index].ticket;

  633.         }

  634.     }

  635. 

  636.     if (selected == PLSR_COMMAND_QUEUE_DEPTH)

  637.     {

  638.         PlsrCoreExitCritical(interruptState);

  639.         return 0U;

  640.     }

  641. 

  642.     *slot = PlsrCommandQueue[selected];

  643.     PlsrCommandQueue[selected].occupied = 0U;

  644.     PlsrCoreExitCritical(interruptState);

  645.     return 1U;

  646. }

  647. 

  648. #ifdef PLSR_HOST_TEST

  649. PLSR_RESULT PlsrPostStart(const PLSR_START_REQUEST *request)

  650. {

  651.     PLSR_COMMAND command;

  652. 

  653.     if (request == NULL)

  654.     {

  655.         return PLSR_RESULT_INVALID_ARGUMENT;

  656.     }

  657.     if (request->axis >= PLSR_AXIS_COUNT)

  658.     {

  659.         return PLSR_RESULT_INVALID_AXIS;

  660.     }

  661.     if (PlsrInitialized == 0U)

  662.     {

  663.         return PLSR_RESULT_INVALID_STATE;

  664.     }

  665. 

  666.     command.sequence = request->sequence;

  667.     command.axis = request->axis;

  668.     command.opcode = PLSR_CMD_START;

  669.     command.argument = 0;

  670.     return PlsrQueueCommand(&command, request, NULL);

  671. }

  672. #endif /* PLSR_HOST_TEST */

  673. 

  674. PLSR_RESULT PlsrPostCall(const PLSR_CALL *call)

  675. {

  676.     PLSR_COMMAND command;

  677. 

  678.     if (call == NULL)

  679.     {

  680.         return PLSR_RESULT_INVALID_ARGUMENT;

  681.     }

  682.     if (call->dAxis >= PLSR_AXIS_COUNT)

  683.     {

  684.         return PLSR_RESULT_INVALID_AXIS;

  685.     }

  686.     if (PlsrInitialized == 0U)

  687.     {

  688.         return PLSR_RESULT_INVALID_STATE;

  689.     }

  690.     command.sequence = call->sequence;

  691.     command.axis = call->dAxis;

  692.     command.opcode = PLSR_CMD_START;

  693.     command.argument = 0;

  694.     return PlsrQueueCommand(&command, NULL, call);

  695. }

  696. 

  697. PLSR_RESULT PlsrPostCommand(const PLSR_COMMAND *command)

  698. {

  699.     if (command == NULL)

  700.     {

  701.         return PLSR_RESULT_INVALID_ARGUMENT;

  702.     }

  703.     if (command->axis >= PLSR_AXIS_COUNT)

  704.     {

  705.         return PLSR_RESULT_INVALID_AXIS;

  706.     }

  707.     if (((uint32_t)command->opcode > (uint32_t)PLSR_CMD_SELF_TEST)

  708.         || (command->opcode == PLSR_CMD_START))

  709.     {

  710.         return PLSR_RESULT_INVALID_ARGUMENT;

  711.     }

  712.     if (PlsrInitialized == 0U)

  713.     {

  714.         return PLSR_RESULT_INVALID_STATE;

  715.     }

  716.     return PlsrQueueCommand(command, NULL, NULL);

  717. }

  718. 

  719. PLSR_RESULT PlsrPostEvent(uint8_t axis, uint32_t eventMask)

  720. {

  721.     uint32_t interruptState;

  722. 

  723.     if (axis >= PLSR_AXIS_COUNT)

  724.     {

  725.         return PLSR_RESULT_INVALID_AXIS;

  726.     }

  727.     if ((eventMask == 0UL) || ((eventMask & ~PLSR_EVENT_ALL_MASK) != 0UL))

  728.     {

  729.         return PLSR_RESULT_INVALID_ARGUMENT;

  730.     }

  731. 

  732.     interruptState = PlsrCoreEnterCritical();

  733.     PlsrAxes[axis].pendingEvents |= eventMask;

  734.     PlsrCoreExitCritical(interruptState);

  735.     return PLSR_RESULT_OK;

  736. }

  737. 

  738. static uint32_t PlsrTakeEvents(uint8_t axis, uint32_t mask)

  739. {

  740.     uint32_t interruptState;

  741.     uint32_t events;

  742. 

  743.     interruptState = PlsrCoreEnterCritical();

  744.     events = PlsrAxes[axis].pendingEvents & mask;

  745.     PlsrAxes[axis].pendingEvents &= ~events;

  746.     PlsrCoreExitCritical(interruptState);

  747.     return events;

  748. }

  749. 

  750. static uint8_t PlsrAnyAxisBusy(void)

  751. {

  752.     uint8_t axis;

  753. 

  754.     for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)

  755.     {

  756.         if (PlsrStateIsBusy(PlsrAxes[axis].state) != 0U)

  757.         {

  758.             return 1U;

  759.         }

  760.     }

  761.     return 0U;

  762. }

  763. 

  764. static PLSR_RESULT PlsrStartAxis(PLSR_AXIS *axisObject,

  765.                                  const PLSR_START_REQUEST *start)

  766. {

  767.     PLSR_RESOURCE_REQUEST resourceRequest;

  768.     PLSR_RESULT result;

  769. 

  770.     if ((axisObject->state != PLSR_STATE_IDLE)

  771.         && (axisObject->state != PLSR_STATE_COMPLETED)

  772.         && (axisObject->state != PLSR_STATE_STOPPED))

  773.     {

  774.         return PLSR_RESULT_INVALID_STATE;

  775.     }

  776. 

  777.     resourceRequest.ownerAxis = start->axis;

  778.     resourceRequest.outputMode = start->outputMode;

  779.     resourceRequest.dAxis = start->axis;

  780.     resourceRequest.directionPoint = start->directionPoint;

  781.     result = PlsrResourceReserve(&resourceRequest, &axisObject->lease);

  782.     if (result != PLSR_RESULT_OK)

  783.     {

  784.         axisObject->error = (result == PLSR_RESULT_RESOURCE_CONFLICT)

  785.                                 ? PLSR_ERROR_RESOURCE_CONFLICT

  786.                                 : PLSR_ERROR_INVALID_RESOURCE;

  787.         return result;

  788.     }

  789. 

  790.     axisObject->outputMode = start->outputMode;

  791.     axisObject->directionPositive = (start->directionPositive != 0U) ? 1U : 0U;

  792.     axisObject->error = PLSR_ERROR_NONE;

  793.     axisObject->compatibleErrorCode = 0U;

  794.     axisObject->compatibleErrorBlock = 0U;

  795.     axisObject->stopReason = PLSR_STOP_REASON_NONE;

  796.     axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

  797.     axisObject->immediateStopPending = 0U;

  798.     axisObject->done = 0U;

  799.     axisObject->jobValid = 0U;

  800.     axisObject->taskPulses = 0;

  801.     axisObject->segmentAccountedPulses = 0;

  802.     axisObject->segmentAccountingActive = 0U;

  803.     result = PlsrStateTransition(start->axis,

  804.                                  PLSR_STATE_ACCEL,

  805.                                  PLSR_TRANSITION_START);

  806.     if (result != PLSR_RESULT_OK)

  807.     {

  808.         PlsrResourceRelease(&axisObject->lease);

  809.     }

  810.     else

  811.     {

  812.         /* 运动开始：掉电恢复时据此判定"断电时在运动中"。 */

  813.         (void)PlcDeviceSetHsdCheckpointMeta(axisObject->positionValid, 1U);

  814.         (void)PlcDeviceCheckpointHsd();

  815.     }

  816.     return result;

  817. }

  818. 

  819. static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject,

  820.                                  const PLSR_CALL *call)

  821. {

  822.     PLSR_RESOURCE_REQUEST resourceRequest;

  823.     PLSR_PARSE_CONTEXT parseContext;

  824.     PLSR_RESULT result;

  825. 

  826.     if ((axisObject->state != PLSR_STATE_IDLE)

  827.         && (axisObject->state != PLSR_STATE_COMPLETED)

  828.         && (axisObject->state != PLSR_STATE_STOPPED))

  829.     {

  830.         return PLSR_RESULT_INVALID_STATE;

  831.     }

  832. 

  833.     axisObject->compatibleErrorCode = 0U;

  834.     axisObject->compatibleErrorBlock = 0U;

  835. 

  836.     parseContext.logicalPosition = axisObject->logicalPosition;

  837.     parseContext.positionValid = axisObject->positionValid;

  838.     result = PlsrBuildJobSnapshot(call,

  839.                                   &parseContext,

  840.                                   &PlsrJobScratch,

  841.                                   &axisObject->parseDetail);

  842.     if (result != PLSR_RESULT_OK)

  843.     {

  844.         PlsrSetCompatibleEquivalentError(call->dAxis, axisObject);

  845.         return result;

  846.     }

  847. 

  848.     resourceRequest.ownerAxis = call->dAxis;

  849.     resourceRequest.outputMode =

  850.         (PLSR_OUTPUT_MODE)PlsrJobScratch.outputMode;

  851.     resourceRequest.dAxis = call->dAxis;

  852.     resourceRequest.directionPoint = PlsrJobScratch.directionPoint;

  853.     result = PlsrResourceReserve(&resourceRequest, &axisObject->lease);

  854.     if (result != PLSR_RESULT_OK)

  855.     {

  856.         axisObject->error = (result == PLSR_RESULT_RESOURCE_CONFLICT)

  857.                                 ? PLSR_ERROR_RESOURCE_CONFLICT

  858.                                 : PLSR_ERROR_INVALID_RESOURCE;

  859.         axisObject->parseDetail.result = result;

  860.         axisObject->parseDetail.block = PLSR_PARSE_BLOCK_OUTPUT;

  861.         return result;

  862.     }

  863. 

  864.     axisObject->job = PlsrJobScratch;

  865.     axisObject->jobValid = 1U;

  866.     if ((axisObject->equivalent.unitCode

  867.          != axisObject->job.equivalent.unitCode)

  868.         || (axisObject->equivalent.pulsesPerRevolution

  869.             != axisObject->job.equivalent.pulsesPerRevolution)

  870.         || (axisObject->equivalent.movementPerRevolution

  871.             != axisObject->job.equivalent.movementPerRevolution))

  872.     {

  873.         axisObject->equivalentCommandRemainder = 0;

  874.     }

  875.     axisObject->equivalent = axisObject->job.equivalent;

  876.     axisObject->outputMode = (PLSR_OUTPUT_MODE)axisObject->job.outputMode;

  877.     axisObject->directionPositive = axisObject->job.initialDirectionPositive;

  878.     axisObject->error = PLSR_ERROR_NONE;

  879.     axisObject->compatibleErrorCode = 0U;

  880.     axisObject->compatibleErrorBlock = 0U;

  881.     axisObject->stopReason = PLSR_STOP_REASON_NONE;

  882.     axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

  883.     axisObject->immediateStopPending = 0U;

  884.     axisObject->done = 0U;

  885.     axisObject->taskPulses = 0;

  886.     axisObject->segmentAccountedPulses = 0;

  887.     axisObject->segmentAccountingActive = 0U;

  888.     PlsrPathBegin(&axisObject->path,

  889.                   &axisObject->job,

  890.                   axisObject->logicalPosition);

  891.     if (axisObject->path.jobEnded != 0U)

  892.     {

  893.         /* 起点就是零脉冲且跳转链已结束（任务无实际脉冲）。 */

  894.         axisObject->stopReason = PLSR_STOP_REASON_NORMAL_COMPLETE;

  895.         (void)PlsrStateTransition(call->dAxis,

  896.                                   PLSR_STATE_COMPLETED,

  897.                                   PLSR_TRANSITION_JOB_COMPLETE);

  898.         axisObject->jobValid = 0U;

  899.         return PLSR_RESULT_OK;

  900.     }

  901.     result = PlsrStateTransition(call->dAxis,

  902.                                  PLSR_STATE_ACCEL,

  903.                                  PLSR_TRANSITION_START);

  904.     if (result != PLSR_RESULT_OK)

  905.     {

  906.         axisObject->jobValid = 0U;

  907.         PlsrResourceRelease(&axisObject->lease);

  908.     }

  909.     else

  910.     {

  911.         /* 启动当前段硬件输出与速度曲线。 */

  912.         /* 零脉冲跳转链耗尽本轮预算时没有实际运动段，等待下一次

  913.          * PlsrPathTick 找到非零段后再启动硬件。 */

  914.         if (axisObject->path.jumpChainPending == 0U)

  915.         {

  916.             result = PlsrStartSegmentHardware(call->dAxis, axisObject);

  917.         }

  918.         if (result != PLSR_RESULT_OK)

  919.         {

  920.             axisObject->error = PLSR_ERROR_TIMER_FAULT;

  921.             PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT);

  922.             axisObject->done = 0U;

  923.             (void)PlsrStateTransition(call->dAxis,

  924.                                       PLSR_STATE_ERROR,

  925.                                       PLSR_TRANSITION_FAULT);

  926.             return result;

  927.         }

  928.         /* 运动开始：掉电恢复时据此判定"断电时在运动中"。 */

  929.         (void)PlcDeviceSetHsdCheckpointMeta(axisObject->positionValid, 1U);

  930.         (void)PlcDeviceCheckpointHsd();

  931.     }

  932.     return result;

  933. }

  934. 

  935. static PLSR_RESULT PlsrStopImmediate(uint8_t axis)

  936. {

  937.     PLSR_AXIS *axisObject = &PlsrAxes[axis];

  938. 

  939.     if ((axisObject->state == PLSR_STATE_IDLE)

  940.         || (axisObject->state == PLSR_STATE_COMPLETED)

  941.         || (axisObject->state == PLSR_STATE_STOPPED))

  942.     {

  943.         return PLSR_RESULT_OK;

  944.     }

  945.     if (axisObject->state == PLSR_STATE_ERROR)

  946.     {

  947.         return PLSR_RESULT_INVALID_STATE;

  948.     }

  949.     if (axisObject->immediateStopPending != 0U)

  950.     {

  951.         return PLSR_RESULT_OK;

  952.     }

  953. 

  954.     PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_IMMEDIATE);

  955.     axisObject->pendingTerminal = PLSR_STATE_STOPPED;

  956.     PlsrPathTerminate(&axisObject->path);

  957.     PlsrStopSegmentHardware(axis, axisObject);

  958.     if ((axisObject->state == PLSR_STATE_WAIT)

  959.         || (axisObject->state == PLSR_STATE_PAUSED))

  960.     {

  961.         return PlsrStateTransition(axis,

  962.                                    PLSR_STATE_STOPPED,

  963.                                    PLSR_TRANSITION_STOP);

  964.     }

  965. 

  966.     axisObject->immediateStopPending = 1U;

  967.     return PLSR_RESULT_OK;

  968. }

  969. 

  970. static PLSR_RESULT PlsrStopDecel(uint8_t axis)

  971. {

  972.     PLSR_AXIS *axisObject = &PlsrAxes[axis];

  973. 

  974.     if ((axisObject->state == PLSR_STATE_IDLE)

  975.         || (axisObject->state == PLSR_STATE_COMPLETED)

  976.         || (axisObject->state == PLSR_STATE_STOPPED))

  977.     {

  978.         return PLSR_RESULT_OK;

  979.     }

  980.     if (axisObject->state == PLSR_STATE_ERROR)

  981.     {

  982.         return PLSR_RESULT_INVALID_STATE;

  983.     }

  984.     if (axisObject->stopReason >= PLSR_STOP_REASON_STOP_IMMEDIATE)

  985.     {

  986.         return PLSR_RESULT_OK;

  987.     }

  988. 

  989.     PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_DECEL);

  990.     axisObject->pendingTerminal = PLSR_STATE_STOPPED;

  991.     PlsrPathTerminate(&axisObject->path);

  992.     PlsrStopSegmentHardware(axis, axisObject);

  993.     if ((axisObject->state == PLSR_STATE_WAIT)

  994.         || (axisObject->state == PLSR_STATE_PAUSED))

  995.     {

  996.         return PlsrStateTransition(axis,

  997.                                    PLSR_STATE_STOPPED,

  998.                                    PLSR_TRANSITION_STOP);

  999.     }

  1000.     if (axisObject->state == PLSR_STATE_DECEL)

  1001.     {

  1002.         return PLSR_RESULT_OK;

  1003.     }

  1004.     return PlsrStateTransition(axis,

  1005.                                PLSR_STATE_DECEL,

  1006.                                PLSR_TRANSITION_DECEL_REQUEST);

  1007. }

  1008. 

  1009. static PLSR_RESULT PlsrPause(uint8_t axis)

  1010. {

  1011.     PLSR_AXIS *axisObject = &PlsrAxes[axis];

  1012. 

  1013.     if (axisObject->state == PLSR_STATE_PAUSED)

  1014.     {

  1015.         return PLSR_RESULT_OK;

  1016.     }

  1017.     if (axisObject->stopReason >= PLSR_STOP_REASON_STOP_DECEL)

  1018.     {

  1019.         return PLSR_RESULT_BUSY;

  1020.     }

  1021.     if (axisObject->state == PLSR_STATE_WAIT)

  1022.     {

  1023.         PlsrSetStopReason(axisObject, PLSR_STOP_REASON_PAUSE);

  1024.         PlsrPathTerminate(&axisObject->path);

  1025.         PlsrStopSegmentHardware(axis, axisObject);

  1026.         return PlsrStateTransition(axis,

  1027.                                    PLSR_STATE_PAUSED,

  1028.                                    PLSR_TRANSITION_DECEL_COMPLETE);

  1029.     }

  1030.     if ((axisObject->state != PLSR_STATE_ACCEL)

  1031.         && (axisObject->state != PLSR_STATE_RUN)

  1032.         && (axisObject->state != PLSR_STATE_DECEL))

  1033.     {

  1034.         return PLSR_RESULT_INVALID_STATE;

  1035.     }

  1036. 

  1037.     PlsrSetStopReason(axisObject, PLSR_STOP_REASON_PAUSE);

  1038.     axisObject->pendingTerminal = PLSR_STATE_PAUSED;

  1039.     PlsrPathTerminate(&axisObject->path);

  1040.     if (axisObject->state == PLSR_STATE_DECEL)

  1041.     {

  1042.         return PLSR_RESULT_OK;

  1043.     }

  1044.     return PlsrStateTransition(axis,

  1045.                                PLSR_STATE_DECEL,

  1046.                                PLSR_TRANSITION_DECEL_REQUEST);

  1047. }

  1048. 

  1049. static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot)

  1050. {

  1051.     PLSR_AXIS *axisObject = &PlsrAxes[slot->command.axis];

  1052.     PLC_DEVICE_RESULT deviceResult;

  1053.     PLSR_RESULT result;

  1054. 

  1055.     switch (slot->command.opcode)

  1056.     {

  1057.         case PLSR_CMD_START:

  1058.             if (slot->hasCall != 0U)

  1059.             {

  1060.                 result = PlsrStartCall(axisObject, &slot->call);

  1061.             }

  1062.             else

  1063.             {

  1064.                 result = (slot->hasStart != 0U)

  1065.                              ? PlsrStartAxis(axisObject, &slot->start)

  1066.                              : PLSR_RESULT_INVALID_ARGUMENT;

  1067.             }

  1068.             break;

  1069. 

  1070.         case PLSR_CMD_STOP_DECEL:

  1071.             result = PlsrStopDecel(slot->command.axis);

  1072.             break;

  1073. 

  1074.         case PLSR_CMD_STOP_IMMEDIATE:

  1075.             result = PlsrStopImmediate(slot->command.axis);

  1076.             break;

  1077. 

  1078.         case PLSR_CMD_PAUSE:

  1079.             result = PlsrPause(slot->command.axis);

  1080.             break;

  1081. 

  1082.         case PLSR_CMD_RESUME:

  1083.             if (axisObject->state != PLSR_STATE_PAUSED)

  1084.             {

  1085.                 result = PLSR_RESULT_INVALID_STATE;

  1086.             }

  1087.             else

  1088.             {

  1089.                 axisObject->stopReason = PLSR_STOP_REASON_NONE;

  1090.                 axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

  1091.                 result = PlsrStateTransition(slot->command.axis,

  1092.                                              PLSR_STATE_ACCEL,

  1093.                                              PLSR_TRANSITION_WAIT_COMPLETE);

  1094.             }

  1095.             break;

  1096. 

  1097.         case PLSR_CMD_SET_POSITION:

  1098.             if (PlsrStateIsBusy(axisObject->state) != 0U)

  1099.             {

  1100.                 result = PLSR_RESULT_BUSY;

  1101.             }

  1102.             else

  1103.             {

  1104.                 axisObject->logicalPosition = slot->command.argument;

  1105.                 axisObject->positionValid = 1U;

  1106.                 axisObject->positionOverflow = 0U;

  1107.                 PlsrPublishPosition(slot->command.axis, axisObject);

  1108.                 (void)PlcDeviceSetHsdCheckpointMeta(1U, 0U);

  1109.                 (void)PlcDeviceCheckpointHsd();

  1110.                 result = PLSR_RESULT_OK;

  1111.             }

  1112.             break;

  1113. 

  1114.         case PLSR_CMD_CLEAR_POSITION:

  1115.             if (PlsrStateIsBusy(axisObject->state) != 0U)

  1116.             {

  1117.                 result = PLSR_RESULT_BUSY;

  1118.             }

  1119.             else

  1120.             {

  1121.                 axisObject->logicalPosition = 0;

  1122.                 axisObject->positionValid = 1U;

  1123.                 axisObject->positionOverflow = 0U;

  1124.                 PlsrPublishPosition(slot->command.axis, axisObject);

  1125.                 (void)PlcDeviceSetHsdCheckpointMeta(1U, 0U);

  1126.                 (void)PlcDeviceCheckpointHsd();

  1127.                 result = PLSR_RESULT_OK;

  1128.             }

  1129.             break;

  1130. 

  1131.         case PLSR_CMD_CLEAR_TOTAL:

  1132.             if (PlsrStateIsBusy(axisObject->state) != 0U)

  1133.             {

  1134.                 result = PLSR_RESULT_BUSY;

  1135.             }

  1136.             else

  1137.             {

  1138.                 axisObject->totalPulses = 0;

  1139.                 (void)PlcDeviceSetHsdCheckpointMeta(

  1140.                     axisObject->positionValid,

  1141.                     0U);

  1142.                 (void)PlcDeviceCheckpointHsd();

  1143.                 result = PLSR_RESULT_OK;

  1144.             }

  1145.             break;

  1146. 

  1147.         case PLSR_CMD_SAVE_CONFIG:

  1148.             if (PlsrAnyAxisBusy() != 0U)

  1149.             {

  1150.                 result = PLSR_RESULT_BUSY;

  1151.             }

  1152.             else

  1153.             {

  1154.                 deviceResult = PlcDeviceSaveSfd();

  1155.                 result = (deviceResult == PLC_DEVICE_OK)

  1156.                              ? PLSR_RESULT_OK

  1157.                              : PLSR_RESULT_PERSISTENCE_ERROR;

  1158.             }

  1159.             break;

  1160. 

  1161.         case PLSR_CMD_LOAD_CONFIG:

  1162.             if (PlsrAnyAxisBusy() != 0U)

  1163.             {

  1164.                 result = PLSR_RESULT_BUSY;

  1165.             }

  1166.             else

  1167.             {

  1168.                 deviceResult = PlcDeviceLoadSfd();

  1169.                 result = (deviceResult == PLC_DEVICE_OK)

  1170.                              ? PLSR_RESULT_OK

  1171.                              : PLSR_RESULT_PERSISTENCE_ERROR;

  1172.             }

  1173.             break;

  1174. 

  1175.         case PLSR_CMD_RESET_ERROR:

  1176.             if (axisObject->state != PLSR_STATE_ERROR)

  1177.             {

  1178.                 result = PLSR_RESULT_INVALID_STATE;

  1179.             }

  1180.             else

  1181.             {

  1182.                 axisObject->error = PLSR_ERROR_NONE;

  1183.                 axisObject->compatibleErrorCode = 0U;

  1184.                 axisObject->compatibleErrorBlock = 0U;

  1185.                 axisObject->stopReason = PLSR_STOP_REASON_NONE;

  1186.                 axisObject->done = 0U;

  1187.                 result = PlsrStateTransition(slot->command.axis,

  1188.                                              PLSR_STATE_IDLE,

  1189.                                              PLSR_TRANSITION_RESET_ERROR);

  1190.             }

  1191.             break;

  1192. 

  1193.         case PLSR_CMD_SELF_TEST:

  1194.             result = ((PlsrAnyAxisBusy() == 0U)

  1195.                       && (PlsrResourceCheckInvariant() != 0U))

  1196.                          ? PLSR_RESULT_OK

  1197.                          : PLSR_RESULT_BUSY;

  1198.             break;

  1199. 

  1200.         default:

  1201.             result = PLSR_RESULT_INVALID_ARGUMENT;

  1202.             break;

  1203.     }

  1204. 

  1205.     axisObject->lastCommandSequence = slot->command.sequence;

  1206.     axisObject->lastCommandResult = result;

  1207.     axisObject->hasLastCommand = 1U;

  1208.     PlsrPublishAxis(slot->command.axis);

  1209.     return result;

  1210. }

  1211. 

  1212. static void PlsrProcessCriticalEvents(uint8_t axis, uint32_t events)

  1213. {

  1214.     PLSR_AXIS *axisObject = &PlsrAxes[axis];

  1215. 

  1216.     if ((events & PLSR_EVENT_SOFTWARE_EMERGENCY) != 0UL)

  1217.     {

  1218.         if (PlsrStateIsBusy(axisObject->state) != 0U)

  1219.         {

  1220.             PlsrSetStopReason(axisObject,

  1221.                               PLSR_STOP_REASON_SOFTWARE_EMERGENCY);

  1222.             axisObject->done = 0U;

  1223.             PlsrPathTerminate(&axisObject->path);

  1224.             PlsrStopSegmentHardware(axis, axisObject);

  1225.             (void)PlsrStateTransition(axis,

  1226.                                       PLSR_STATE_STOPPED,

  1227.                                       PLSR_TRANSITION_STOP);

  1228.         }

  1229.         return;

  1230.     }

  1231. 

  1232.     if ((events & PLSR_EVENT_LIMIT_POSITIVE) != 0UL)

  1233.     {

  1234.         if ((axisObject->directionPositive != 0U)

  1235.             && (PlsrStateIsBusy(axisObject->state) != 0U))

  1236.         {

  1237.             axisObject->error = PLSR_ERROR_LIMIT_POSITIVE;

  1238.             PlsrSetStopReason(axisObject,

  1239.                               PLSR_STOP_REASON_LIMIT_POSITIVE);

  1240.             axisObject->pendingTerminal = PLSR_STATE_STOPPED;

  1241.             PlsrPathTerminate(&axisObject->path);

  1242.             PlsrStopSegmentHardware(axis, axisObject);

  1243.             if ((axisObject->state == PLSR_STATE_WAIT)

  1244.                 || (axisObject->state == PLSR_STATE_PAUSED))

  1245.             {

  1246.                 (void)PlsrStateTransition(axis,

  1247.                                           PLSR_STATE_STOPPED,

  1248.                                           PLSR_TRANSITION_STOP);

  1249.             }

  1250.             else if (axisObject->state != PLSR_STATE_DECEL)

  1251.             {

  1252.                 (void)PlsrStateTransition(axis,

  1253.                                           PLSR_STATE_DECEL,

  1254.                                           PLSR_TRANSITION_DECEL_REQUEST);

  1255.             }

  1256.         }

  1257.         return;

  1258.     }

  1259. 

  1260.     if ((events & PLSR_EVENT_LIMIT_NEGATIVE) != 0UL)

  1261.     {

  1262.         if ((axisObject->directionPositive == 0U)

  1263.             && (PlsrStateIsBusy(axisObject->state) != 0U))

  1264.         {

  1265.             axisObject->error = PLSR_ERROR_LIMIT_NEGATIVE;

  1266.             PlsrSetStopReason(axisObject,

  1267.                               PLSR_STOP_REASON_LIMIT_NEGATIVE);

  1268.             axisObject->pendingTerminal = PLSR_STATE_STOPPED;

  1269.             PlsrPathTerminate(&axisObject->path);

  1270.             PlsrStopSegmentHardware(axis, axisObject);

  1271.             if ((axisObject->state == PLSR_STATE_WAIT)

  1272.                 || (axisObject->state == PLSR_STATE_PAUSED))

  1273.             {

  1274.                 (void)PlsrStateTransition(axis,

  1275.                                           PLSR_STATE_STOPPED,

  1276.                                           PLSR_TRANSITION_STOP);

  1277.             }

  1278.             else if (axisObject->state != PLSR_STATE_DECEL)

  1279.             {

  1280.                 (void)PlsrStateTransition(axis,

  1281.                                           PLSR_STATE_DECEL,

  1282.                                           PLSR_TRANSITION_DECEL_REQUEST);

  1283.             }

  1284.         }

  1285.         return;

  1286.     }

  1287. 

  1288.     if ((events & (PLSR_EVENT_TIMER_FAULT | PLSR_EVENT_COUNTER_FAULT)) != 0UL)

  1289.     {

  1290.         axisObject->error = ((events & PLSR_EVENT_TIMER_FAULT) != 0UL)

  1291.                                 ? PLSR_ERROR_TIMER_FAULT

  1292.                                 : PLSR_ERROR_COUNTER_FAULT;

  1293.         PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT);

  1294.         axisObject->done = 0U;

  1295.         PlsrPathTerminate(&axisObject->path);

  1296.         PlsrStopSegmentHardware(axis, axisObject);

  1297.         (void)PlsrStateTransition(axis,

  1298.                                   PLSR_STATE_ERROR,

  1299.                                   PLSR_TRANSITION_FAULT);

  1300.     }

  1301. }

  1302. 

  1303. /* 启动当前段的硬件输出与速度曲线（P3a：单轴 PULSE/DIR）。

  1304.  * 由段进入 ACCEL 时调用（任务启动 + 段间推进）。 */

  1305. static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis,

  1306.                                             PLSR_AXIS *axisObject)

  1307. {

  1308.     const PLSR_JOB_SNAPSHOT *job = &axisObject->job;

  1309.     const PLSR_SEGMENT_SNAPSHOT *segment =

  1310.         &job->segments[axisObject->path.currentSegment - 1U];

  1311.     PLSR_PROFILE_REQUEST profileRequest;

  1312.     PLSR_HW_START_PARAMS params;

  1313.     PLSR_RESULT result;

  1314.     int64_t pulses;

  1315.     int64_t signedPulses;

  1316.     int64_t targetPosition;

  1317.     int64_t nextEquivalentRemainder =

  1318.         axisObject->equivalentCommandRemainder;

  1319.     uint8_t positive;

  1320. 

  1321.     if (job->positioningMode == 0U)

  1322.     {

  1323.         result = PlsrPositionUnitsToPulses(

  1324.             &axisObject->equivalent,

  1325.             segment->pulseOrTarget,

  1326.             axisObject->equivalentCommandRemainder,

  1327.             &signedPulses,

  1328.             &nextEquivalentRemainder);

  1329.         if (result != PLSR_RESULT_OK)

  1330.         {

  1331.             return result;

  1332.         }

  1333.         if (signedPulses == INT64_MIN)

  1334.         {

  1335.             return PLSR_RESULT_POSITION_OVERFLOW;

  1336.         }

  1337.         pulses = (signedPulses < 0) ? -signedPulses : signedPulses;

  1338.         positive = (signedPulses >= 0) ? 1U : 0U;

  1339.     }

  1340.     else

  1341.     {

  1342.         /* 绝对模式使用实际硬件脉冲闭环更新后的逻辑位置计算位移。 */

  1343.         int64_t delta;

  1344. 

  1345.         result = PlsrPositionAbsoluteUnitsToPulses(

  1346.             &axisObject->equivalent,

  1347.             segment->pulseOrTarget,

  1348.             &targetPosition);

  1349.         if (result != PLSR_RESULT_OK)

  1350.         {

  1351.             return result;

  1352.         }

  1353.         if (((axisObject->logicalPosition > 0)

  1354.              && (targetPosition

  1355.                  < INT64_MIN + axisObject->logicalPosition))

  1356.             || ((axisObject->logicalPosition < 0)

  1357.                 && (targetPosition

  1358.                     > INT64_MAX + axisObject->logicalPosition)))

  1359.         {

  1360.             return PLSR_RESULT_POSITION_OVERFLOW;

  1361.         }

  1362.         delta = targetPosition - axisObject->logicalPosition;

  1363.         if (delta == INT64_MIN)

  1364.         {

  1365.             return PLSR_RESULT_POSITION_OVERFLOW;

  1366.         }

  1367. 

  1368.         pulses = (delta < 0) ? -delta : delta;

  1369.         positive = (delta >= 0) ? 1U : 0U;

  1370.     }

  1371. 

  1372.     if (pulses == 0)

  1373.     {

  1374.         /* 当量小于一个物理脉冲时保存余数并按零脉冲段推进；不启动PWM。 */

  1375.         axisObject->equivalentCommandRemainder = nextEquivalentRemainder;

  1376.         axisObject->directionPositive = positive;

  1377.         axisObject->segmentAccountedPulses = 0;

  1378.         axisObject->segmentAccountingActive = 0U;

  1379.         PlsrPublishAxis(axis);

  1380.         (void)PlsrPostEvent(axis, PLSR_EVENT_SEGMENT_COMPLETE);

  1381.         return PLSR_RESULT_OK;

  1382.     }

  1383. 

  1384.     (void)memset(&profileRequest, 0, sizeof(profileRequest));

  1385.     profileRequest.targetFrequencyHz = segment->targetFrequency;

  1386.     profileRequest.startFrequencyHz = job->s2.startSpeed;

  1387.     profileRequest.stopFrequencyHz = job->s2.stopSpeed;

  1388.     profileRequest.maxFrequencyHz = job->s2.maximumSpeed;

  1389.     profileRequest.accelSlopeHzPerMs =

  1390.         (job->s2.accelerationMs != 0U)

  1391.             ? job->s2.defaultSpeed / job->s2.accelerationMs

  1392.             : 0UL;

  1393.     profileRequest.decelSlopeHzPerMs =

  1394.         (job->s2.decelerationMs != 0U)

  1395.             ? job->s2.defaultSpeed / job->s2.decelerationMs

  1396.             : 0UL;

  1397.     profileRequest.curveMode = job->s2.curveMode;

  1398. 

  1399.     result = PlsrProfileStart(&axisObject->profile,

  1400.                               &profileRequest,

  1401.                               pulses,

  1402.                               1000U);

  1403.     if (result != PLSR_RESULT_OK)

  1404.     {

  1405.         return result;

  1406.     }

  1407. 

  1408.     params.frequencyHz = job->s2.startSpeed;

  1409.     params.targetPulses = pulses;

  1410.     params.outputMode = (PLSR_OUTPUT_MODE)job->outputMode;

  1411.     params.directionPoint = job->directionPoint;

  1412.     params.directionPositive = positive;

  1413.     params.directionDelayMs = job->s2.directionDelayMs;

  1414.     result = PlsrHwStartPulse(axis, &params);

  1415.     if (result != PLSR_RESULT_OK)

  1416.     {

  1417.         axisObject->profileActive = 0U;

  1418.         axisObject->profileWasAccel = 0U;

  1419.         return result;

  1420.     }

  1421.     axisObject->equivalentCommandRemainder = nextEquivalentRemainder;

  1422.     axisObject->directionPositive = positive;

  1423.     axisObject->segmentAccountedPulses = 0;

  1424.     axisObject->segmentAccountingActive = 1U;

  1425.     axisObject->profileActive = 1U;

  1426.     axisObject->profileWasAccel =

  1427.         (axisObject->profile.phase == PLSR_PROFILE_PHASE_ACCEL) ? 1U : 0U;

  1428.     PlsrPublishAxis(axis);

  1429.     return PLSR_RESULT_OK;

  1430. }

  1431. 

  1432. /* 停止当前段的硬件输出与速度曲线。 */

  1433. static void PlsrStopSegmentHardware(uint8_t axis, PLSR_AXIS *axisObject)

  1434. {

  1435.     PlsrAccountHardwarePulses(axis, axisObject);

  1436.     axisObject->profileActive = 0U;

  1437.     axisObject->profileWasAccel = 0U;

  1438.     (void)PlsrHwStopPulse(axis);

  1439.     PlsrAccountHardwarePulses(axis, axisObject);

  1440.     axisObject->segmentAccountingActive = 0U;

  1441.     PlsrPublishRuntime(axis);

  1442. }

  1443. 

  1444. /* 应用路径执行器的动作：段间推进、进入等待、结束、让出、错误。 */

  1445. static void PlsrApplyPathAction(uint8_t axis, PLSR_PATH_ACTION action)

  1446. {

  1447.     PLSR_AXIS *axisObject = &PlsrAxes[axis];

  1448. 

  1449.     switch (action)

  1450.     {

  1451.         case PLSR_PATH_ACTION_NEXT_SEGMENT:

  1452.             /* SEGMENT_COMPLETE 表示上一段输出边界已经结束。统一收口旧段

  1453.              * profile/HAL 后再启动新段；真实 IRQ 与测试注入事件均一致。 */

  1454.             PlsrStopSegmentHardware(axis, axisObject);

  1455.             if (axisObject->state == PLSR_STATE_WAIT)

  1456.             {

  1457.                 (void)PlsrStateTransition(axis,

  1458.                                           PLSR_STATE_ACCEL,

  1459.                                           PLSR_TRANSITION_WAIT_COMPLETE);

  1460.             }

  1461.             else if ((axisObject->state == PLSR_STATE_ACCEL)

  1462.                      || (axisObject->state == PLSR_STATE_RUN)

  1463.                      || (axisObject->state == PLSR_STATE_DECEL))

  1464.             {

  1465.                 (void)PlsrStateTransition(axis,

  1466.                                           PLSR_STATE_ACCEL,

  1467.                                           PLSR_TRANSITION_START);

  1468.             }

  1469.             /* 进入新段：重新启动硬件输出与速度曲线。 */

  1470.             if (axisObject->state == PLSR_STATE_ACCEL)

  1471.             {

  1472.                 if (PlsrStartSegmentHardware(axis, axisObject)

  1473.                     != PLSR_RESULT_OK)

  1474.                 {

  1475.                     axisObject->error = PLSR_ERROR_TIMER_FAULT;

  1476.                     PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT);

  1477.                     axisObject->done = 0U;

  1478.                     (void)PlsrStateTransition(axis,

  1479.                                               PLSR_STATE_ERROR,

  1480.                                               PLSR_TRANSITION_FAULT);

  1481.                 }

  1482.             }

  1483.             break;

  1484. 

  1485.         case PLSR_PATH_ACTION_ENTER_WAIT:

  1486.             if ((axisObject->state == PLSR_STATE_ACCEL)

  1487.                 || (axisObject->state == PLSR_STATE_RUN))

  1488.             {

  1489.                 (void)PlsrStateTransition(axis,

  1490.                                           PLSR_STATE_WAIT,

  1491.                                           PLSR_TRANSITION_WAIT_BEGIN);

  1492.             }

  1493.             break;

  1494. 

  1495.         case PLSR_PATH_ACTION_JOB_COMPLETE:

  1496.             if (PlsrStateIsBusy(axisObject->state) != 0U)

  1497.             {

  1498.                 axisObject->stopReason = PLSR_STOP_REASON_NORMAL_COMPLETE;

  1499.                 (void)PlsrStateTransition(axis,

  1500.                                           PLSR_STATE_COMPLETED,

  1501.                                           PLSR_TRANSITION_JOB_COMPLETE);

  1502.             }

  1503.             break;

  1504. 

  1505.         case PLSR_PATH_ACTION_YIELD:

  1506.             /* 预算耗尽：本轮不再推进，下个 tick 由 PlsrPathTick 恢复。 */

  1507.             break;

  1508. 

  1509.         case PLSR_PATH_ACTION_ERROR:

  1510.             axisObject->error = PLSR_ERROR_INTERNAL;

  1511.             PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT);

  1512.             axisObject->done = 0U;

  1513.             (void)PlsrStateTransition(axis,

  1514.                                       PLSR_STATE_ERROR,

  1515.                                       PLSR_TRANSITION_FAULT);

  1516.             break;

  1517. 

  1518.         default:

  1519.             break;

  1520.     }

  1521. }

  1522. 

  1523. static void PlsrProcessNormalEvents(uint8_t axis, uint32_t events)

  1524. {

  1525.     PLSR_AXIS *axisObject = &PlsrAxes[axis];

  1526. 

  1527.     if (((events & PLSR_EVENT_STOP_IMMEDIATE_DONE) != 0UL)

  1528.         && (axisObject->immediateStopPending != 0U))

  1529.     {

  1530.         (void)PlsrStateTransition(axis,

  1531.                                   PLSR_STATE_STOPPED,

  1532.                                   PLSR_TRANSITION_STOP);

  1533.         return;

  1534.     }

  1535. 

  1536.     if (((events & PLSR_EVENT_ACCEL_COMPLETE) != 0UL)

  1537.         && (axisObject->state == PLSR_STATE_ACCEL)

  1538.         && (axisObject->immediateStopPending == 0U))

  1539.     {

  1540.         (void)PlsrStateTransition(axis,

  1541.                                   PLSR_STATE_RUN,

  1542.                                   PLSR_TRANSITION_ACCEL_COMPLETE);

  1543.     }

  1544. 

  1545.     if (((events & PLSR_EVENT_DECEL_COMPLETE) != 0UL)

  1546.         && (axisObject->state == PLSR_STATE_DECEL))

  1547.     {

  1548.         if (axisObject->pendingTerminal == PLSR_STATE_PAUSED)

  1549.         {

  1550.             (void)PlsrStateTransition(axis,

  1551.                                       PLSR_STATE_PAUSED,

  1552.                                       PLSR_TRANSITION_DECEL_COMPLETE);

  1553.         }

  1554.         else if (axisObject->pendingTerminal == PLSR_STATE_STOPPED)

  1555.         {

  1556.             (void)PlsrStateTransition(axis,

  1557.                                       PLSR_STATE_STOPPED,

  1558.                                       PLSR_TRANSITION_DECEL_COMPLETE);

  1559.         }

  1560.         else

  1561.         {

  1562.             (void)PlsrStateTransition(axis,

  1563.                                       PLSR_STATE_RUN,

  1564.                                       PLSR_TRANSITION_DECEL_COMPLETE);

  1565.         }

  1566.     }

  1567. 

  1568.     if (((events & PLSR_EVENT_WAIT_BEGIN) != 0UL)

  1569.         && ((axisObject->state == PLSR_STATE_ACCEL)

  1570.             || (axisObject->state == PLSR_STATE_RUN)))

  1571.     {

  1572.         (void)PlsrStateTransition(axis,

  1573.                                   PLSR_STATE_WAIT,

  1574.                                   PLSR_TRANSITION_WAIT_BEGIN);

  1575.     }

  1576. 

  1577.     if (((events & PLSR_EVENT_WAIT_COMPLETE) != 0UL)

  1578.         && (axisObject->state == PLSR_STATE_WAIT))

  1579.     {

  1580.         (void)PlsrStateTransition(axis,

  1581.                                   PLSR_STATE_ACCEL,

  1582.                                   PLSR_TRANSITION_WAIT_COMPLETE);

  1583.     }

  1584. 

  1585.     if (((events & PLSR_EVENT_SEGMENT_COMPLETE) != 0UL)

  1586.         && ((axisObject->state == PLSR_STATE_ACCEL)

  1587.             || (axisObject->state == PLSR_STATE_RUN)

  1588.             || (axisObject->state == PLSR_STATE_DECEL))

  1589.         && (axisObject->pendingTerminal == PLSR_STATE_UNINITIALIZED)

  1590.         && (axisObject->immediateStopPending == 0U))

  1591.     {

  1592.         PLSR_PATH_ACTION action = PlsrPathOnSegmentDone(

  1593.             &axisObject->path,

  1594.             &axisObject->job,

  1595.             axisObject->logicalPosition);

  1596. 

  1597.         PlsrApplyPathAction(axis, action);

  1598.     }

  1599. 

  1600.     if (((events & PLSR_EVENT_JOB_COMPLETE) != 0UL)

  1601.         && (PlsrStateIsBusy(axisObject->state) != 0U)

  1602.         && (axisObject->pendingTerminal == PLSR_STATE_UNINITIALIZED)

  1603.         && (axisObject->immediateStopPending == 0U))

  1604.     {

  1605.         axisObject->stopReason = PLSR_STOP_REASON_NORMAL_COMPLETE;

  1606.         (void)PlsrStateTransition(axis,

  1607.                                   PLSR_STATE_COMPLETED,

  1608.                                   PLSR_TRANSITION_JOB_COMPLETE);

  1609.     }

  1610. }

  1611. 

  1612. PLSR_RESULT PlsrInit(void)

  1613. {

  1614.     int32_t restoredPosition;

  1615.     uint8_t restoredPositionValid;

  1616.     uint8_t restoredLastBusy;

  1617.     uint8_t axis;

  1618. 

  1619.     (void)memset(PlsrAxes, 0, sizeof(PlsrAxes));

  1620.     (void)memset(PlsrCommandQueue, 0, sizeof(PlsrCommandQueue));

  1621.     PlsrNextTicket = 0UL;

  1622.     PlsrResourceInit();

  1623.     (void)PlsrHwInit();

  1624.     PlsrInitialized = 1U;

  1625. 

  1626.     restoredPositionValid = PlcDeviceGetRestoredHsdPositionValid();

  1627.     restoredLastBusy = PlcDeviceGetRestoredHsdLastBusy();

  1628.     for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)

  1629.     {

  1630.         PlsrAxes[axis].state = PLSR_STATE_UNINITIALIZED;

  1631.         PlsrAxes[axis].pendingTerminal = PLSR_STATE_UNINITIALIZED;

  1632.         PlsrAxes[axis].lease.directionPoint = PLSR_DIRECTION_POINT_NONE;

  1633.         PlsrLoadAxisEquivalentConfig(axis, &PlsrAxes[axis].equivalent);

  1634.         if (PlcDeviceReadHsdDword(

  1635.                 (uint16_t)(axis * PLSR_HSD_RUNTIME_AXIS_COUNT),

  1636.                 &restoredPosition) == PLC_DEVICE_OK)

  1637.         {

  1638.             PlsrAxes[axis].logicalPosition = restoredPosition;

  1639.         }

  1640.         /* 只有上次正常停机且保存了位置有效标志，才允许绝对定位；

  1641.          * 运动中掉电（lastBusy=1）时位置不可信。 */

  1642.         if ((restoredPositionValid != 0U) && (restoredLastBusy == 0U))

  1643.         {

  1644.             PlsrAxes[axis].positionValid = 1U;

  1645.         }

  1646.         if (PlsrStateTransition(axis,

  1647.                                 PLSR_STATE_IDLE,

  1648.                                 PLSR_TRANSITION_INITIALIZED)

  1649.             != PLSR_RESULT_OK)

  1650.         {

  1651.             return PLSR_RESULT_INTERNAL_ERROR;

  1652.         }

  1653.     }

  1654.     return PLSR_RESULT_OK;

  1655. }

  1656. 

  1657. void PlsrProcess(void)

  1658. {

  1659.     PLSR_COMMAND_SLOT slot;

  1660.     uint32_t events;

  1661.     uint8_t processedCommands = 0U;

  1662.     uint8_t axis;

  1663. 

  1664.     if (PlsrInitialized == 0U)

  1665.     {

  1666.         return;

  1667.     }

  1668. 

  1669.     /* 先合并 ISR 已完成的实际脉冲，确保段完成、STOP或新命令不会在

  1670.      * HAL 计数清零前丢失最后一批位置增量。 */

  1671.     for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)

  1672.     {

  1673.         PlsrAccountHardwarePulses(axis, &PlsrAxes[axis]);

  1674.     }

  1675. 

  1676.     for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)

  1677.     {

  1678.         events = PlsrTakeEvents(axis, PLSR_EVENT_CRITICAL_MASK);

  1679.         if (events != 0UL)

  1680.         {

  1681.             PlsrProcessCriticalEvents(axis, events);

  1682.         }

  1683.     }

  1684. 

  1685.     while ((processedCommands < PLSR_COMMAND_QUEUE_DEPTH)

  1686.            && (PlsrPopHighestPriorityCommand(&slot) != 0U))

  1687.     {

  1688.         (void)PlsrExecuteCommand(&slot);

  1689.         processedCommands++;

  1690.     }

  1691. 

  1692.     for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)

  1693.     {

  1694.         events = PlsrTakeEvents(axis,

  1695.                                 PLSR_EVENT_ALL_MASK

  1696.                                     & ~PLSR_EVENT_CRITICAL_MASK);

  1697.         if (events != 0UL)

  1698.         {

  1699.             PlsrProcessNormalEvents(axis, events);

  1700.         }

  1701.     }

  1702. 

  1703.     /* 1ms tick：路径执行器推进（WAIT/ACT 计时、信号/EXT 轮询、跳转链）

  1704.      * + 速度曲线推进（P2） + HAL 状态机（DIR 延时）。 */

  1705.     for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)

  1706.     {

  1707.         PLSR_AXIS *axisObject = &PlsrAxes[axis];

  1708.         PLSR_PATH_ACTION action;

  1709.         uint32_t frequencyHz;

  1710.         uint32_t outputFrequencyHz;

  1711.         uint64_t hardwarePulses;

  1712.         uint64_t remainingPulses;

  1713.         uint8_t profileDone;

  1714.         uint8_t wasAccel;

  1715. 

  1716.         PlsrHwTick(axis);

  1717.         PlsrAccountHardwarePulses(axis, axisObject);

  1718.         /* 首次 AB 内部预热周期不属于用户运动，速度曲线也必须冻结；

  1719.          * 否则低速起步时会在隐藏周期内提前爬升十余个刷新步。 */

  1720.         if (PlsrHwIsAbStartupPriming(axis) != 0U)

  1721.         {

  1722.             continue;

  1723.         }

  1724.         if (PlsrStateIsBusy(axisObject->state) == 0U)

  1725.         {

  1726.             continue;

  1727.         }

  1728.         action = PlsrPathTick(&axisObject->path,

  1729.                               &axisObject->job,

  1730.                               axisObject->logicalPosition);

  1731.         if (action != PLSR_PATH_ACTION_NONE)

  1732.         {

  1733.             PlsrApplyPathAction(axis, action);

  1734.         }

  1735. 

  1736.         if ((axisObject->profileActive != 0U)

  1737.             && (PlsrHwGetState(axis) != PLSR_HW_STATE_DIR_SETTLING))

  1738.         {

  1739.             /* 虚拟计数校准到硬件实际计数：ARPE 预装载滞后的偏差不累积，

  1740.              * 保证 profile DONE 与硬件计数同步（消除段尾冻结频率收尾）。 */

  1741.             hardwarePulses = (uint64_t)PlsrHwGetEmittedPulses(axis);

  1742.             PlsrProfileSyncPulses(&axisObject->profile, hardwarePulses);

  1743.             wasAccel = axisObject->profileWasAccel;

  1744.             (void)PlsrProfileStep(&axisObject->profile,

  1745.                                   &frequencyHz,

  1746.                                   &profileDone);

  1747.             outputFrequencyHz = frequencyHz;

  1748.             if ((PlsrHwGetState(axis) == PLSR_HW_STATE_RUNNING)

  1749.                 && (wasAccel == 0U)

  1750.                 && ((uint64_t)axisObject->profile.totalPulses

  1751.                     > hardwarePulses + 1UL))

  1752.             {

  1753.                 /* ARR 预装载在当前脉冲结束后生效，因此计算的是

  1754.                  * “当前脉冲之后”剩余的脉冲。用位置反推减速频率，

  1755.                  * 避免 1 ms 时间曲线与预装载滞后造成段尾突停。 */

  1756.                 remainingPulses =

  1757.                     (uint64_t)axisObject->profile.totalPulses

  1758.                     - hardwarePulses - 1UL;

  1759.                 outputFrequencyHz =

  1760.                     PlsrProfileGetBrakingOutputFrequency(

  1761.                         &axisObject->profile,

  1762.                         remainingPulses);

  1763.             }

  1764.             if ((PlsrHwGetState(axis) == PLSR_HW_STATE_PWM_PENDING)

  1765.                 && (axisObject->profile.phase

  1766.                     == PLSR_PROFILE_PHASE_ACCEL))

  1767.             {

  1768.                 outputFrequencyHz =

  1769.                     PlsrProfileGetInitialOutputFrequency(

  1770.                         &axisObject->profile);

  1771.             }

  1772.             if ((profileDone == 0U) || (outputFrequencyHz != 0UL))

  1773.             {

  1774.                 (void)PlsrHwSetFrequency(axis, outputFrequencyHz);

  1775.             }

  1776.             axisObject->profileWasAccel =

  1777.                 (axisObject->profile.phase == PLSR_PROFILE_PHASE_ACCEL)

  1778.                     ? 1U

  1779.                     : 0U;

  1780.             if ((wasAccel != 0U)

  1781.                 && (axisObject->profileWasAccel == 0U)

  1782.                 && (axisObject->state == PLSR_STATE_ACCEL))

  1783.             {

  1784.                 /* 加速完成（曲线进入匀速/减速）→ 状态机推进到 RUN。 */

  1785.                 (void)PlsrPostEvent(axis, PLSR_EVENT_ACCEL_COMPLETE);

  1786.             }

  1787.         }

  1788.     }

  1789. }

  1790. 

  1791. void PlsrTask(void *argument)

  1792. {

  1793.     (void)argument;

  1794. #ifdef PLSR_HOST_TEST

  1795.     PlsrProcess();

  1796. #else

  1797.     while (1)

  1798.     {

  1799.         PlsrProcess();

  1800.         OSTimeDly(1U);

  1801.     }

  1802. #endif

  1803. }

  1804. 

  1805. PLSR_RESULT PlsrGetStatus(uint8_t axis, PLSR_STATUS *status)

  1806. {

  1807.     PLSR_AXIS *axisObject;

  1808.     uint32_t interruptState;

  1809. 

  1810.     if (axis >= PLSR_AXIS_COUNT)

  1811.     {

  1812.         return PLSR_RESULT_INVALID_AXIS;

  1813.     }

  1814.     if (status == NULL)

  1815.     {

  1816.         return PLSR_RESULT_INVALID_ARGUMENT;

  1817.     }

  1818. 

  1819.     interruptState = PlsrCoreEnterCritical();

  1820.     axisObject = &PlsrAxes[axis];

  1821.     status->state = axisObject->state;

  1822.     status->outputMode = axisObject->outputMode;

  1823.     status->error = axisObject->error;

  1824.     status->stopReason = axisObject->stopReason;

  1825.     status->lastCommandResult = axisObject->lastCommandResult;

  1826.     status->lastCommandSequence = axisObject->lastCommandSequence;

  1827.     status->illegalTransitionCount = axisObject->illegalTransitionCount;

  1828.     status->pendingEvents = axisObject->pendingEvents;

  1829.     status->logicalPosition = axisObject->logicalPosition;

  1830.     status->taskPulses = axisObject->taskPulses;

  1831.     status->totalPulses = axisObject->totalPulses;

  1832.     status->busy = PlsrStateIsBusy(axisObject->state);

  1833.     status->pulseActive = PlsrStateIsPulseActive(axisObject->state);

  1834.     status->done = axisObject->done;

  1835.     status->wait = (axisObject->state == PLSR_STATE_WAIT) ? 1U : 0U;

  1836.     status->directionPositive = axisObject->directionPositive;

  1837.     status->highResourceMask = axisObject->lease.highMask;

  1838.     status->directionPoint = (axisObject->lease.valid != 0U)

  1839.                                  ? axisObject->lease.directionPoint

  1840.                                  : PLSR_DIRECTION_POINT_NONE;

  1841.     status->positionValid = axisObject->positionValid;

  1842.     status->jobValid = axisObject->jobValid;

  1843.     status->positionOverflow = axisObject->positionOverflow;

  1844.     status->s2Set = (axisObject->jobValid != 0U) ? axisObject->job.s2Set : 0U;

  1845.     status->speedClamped = (axisObject->jobValid != 0U)

  1846.                                ? axisObject->job.speedClamped

  1847.                                : 0U;

  1848.     status->segmentCount = (axisObject->jobValid != 0U)

  1849.                                ? axisObject->job.segmentCount

  1850.                                : 0U;

  1851.     status->startSegment = (axisObject->jobValid != 0U)

  1852.                                ? axisObject->job.startSegment

  1853.                                : 0U;

  1854.     status->currentSegment = (axisObject->jobValid != 0U)

  1855.                                  ? PlsrPathGetCurrentSegment(

  1856.                                        &axisObject->path)

  1857.                                  : 0U;

  1858.     PlsrCoreExitCritical(interruptState);

  1859.     return PLSR_RESULT_OK;

  1860. }

  1861. 

  1862. PLSR_RESULT PlsrGetLastParseDetail(uint8_t axis,

  1863.                                    PLSR_PARSE_DETAIL *detail)

  1864. {

  1865.     uint32_t interruptState;

  1866. 

  1867.     if (axis >= PLSR_AXIS_COUNT)

  1868.     {

  1869.         return PLSR_RESULT_INVALID_AXIS;

  1870.     }

  1871.     if (detail == NULL)

  1872.     {

  1873.         return PLSR_RESULT_INVALID_ARGUMENT;

  1874.     }

  1875.     interruptState = PlsrCoreEnterCritical();

  1876.     *detail = PlsrAxes[axis].parseDetail;

  1877.     PlsrCoreExitCritical(interruptState);

  1878.     return PLSR_RESULT_OK;

  1879. }