yuwenhao
/
Modbus-rtu
1
0
Fork 0
Code Issues 0 Pull-Requests 0 Releases 0 Wiki Aktivität
Du kannst nicht mehr als 25 Themen auswählen Themen müssen entweder mit einem Buchstaben oder einer Ziffer beginnen. Sie können Bindestriche („-“) enthalten und bis zu 35 Zeichen lang sein.
28 Commits
3 Branches
138 MiB
 
 
 
 
 
 
Struktur: ffd715ae75
Branches Tags
${ item.name }
Erstelle Branch ${ searchTerm }
von „ffd715ae75“
${ noResults }
Modbus-rtu/PLSR/Src/plsr_core.c

1007 Zeilen
30 KiB
Originalformat Blame Verlauf 

  1. #include "plsr_core.h"

  2. #include "plc_device.h"

  3. #include "plsr_address_map.h"

  4. #include "plsr_resource.h"

  5. #include <string.h>

  6. 

  7. #ifndef PLSR_HOST_TEST

  8. #include "stm32f4xx.h"

  9. #include "ucos_ii.h"

  10. #endif

  11. 

  12. typedef struct

  13. {

  14.     PLSR_STATE state;

  15.     PLSR_OUTPUT_MODE outputMode;

  16.     PLSR_ERROR error;

  17.     PLSR_STOP_REASON stopReason;

  18.     PLSR_STATE pendingTerminal;

  19.     PLSR_RESOURCE_LEASE lease;

  20.     volatile uint32_t pendingEvents;

  21.     uint32_t lastCommandSequence;

  22.     PLSR_RESULT lastCommandResult;

  23.     uint32_t illegalTransitionCount;

  24.     int64_t logicalPosition;

  25.     int64_t totalPulses;

  26.     uint8_t hasLastCommand;

  27.     uint8_t done;

  28.     uint8_t directionPositive;

  29.     uint8_t immediateStopPending;

  30. } PLSR_AXIS;

  31. 

  32. typedef struct

  33. {

  34.     PLSR_COMMAND command;

  35.     PLSR_START_REQUEST start;

  36.     uint32_t ticket;

  37.     uint8_t hasStart;

  38.     uint8_t occupied;

  39. } PLSR_COMMAND_SLOT;

  40. 

  41. static PLSR_AXIS PlsrAxes[PLSR_AXIS_COUNT];

  42. static PLSR_COMMAND_SLOT PlsrCommandQueue[PLSR_COMMAND_QUEUE_DEPTH];

  43. static uint32_t PlsrNextTicket;

  44. static uint8_t PlsrInitialized;

  45. 

  46. static uint32_t PlsrCoreEnterCritical(void)

  47. {

  48. #ifdef PLSR_HOST_TEST

  49.     return 0UL;

  50. #else

  51.     uint32_t interruptState = __get_PRIMASK();

  52.     __disable_irq();

  53.     __DMB();

  54.     return interruptState;

  55. #endif

  56. }

  57. 

  58. static void PlsrCoreExitCritical(uint32_t interruptState)

  59. {

  60. #ifdef PLSR_HOST_TEST

  61.     (void)interruptState;

  62. #else

  63.     __DMB();

  64.     if (interruptState == 0UL)

  65.     {

  66.         __enable_irq();

  67.     }

  68. #endif

  69. }

  70. 

  71. static uint8_t PlsrStateIsBusy(PLSR_STATE state)

  72. {

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

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

  75.             || (state == PLSR_STATE_PAUSED))

  76.                ? 1U

  77.                : 0U;

  78. }

  79. 

  80. static uint8_t PlsrStateIsPulseActive(PLSR_STATE state)

  81. {

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

  83.             || (state == PLSR_STATE_DECEL))

  84.                ? 1U

  85.                : 0U;

  86. }

  87. 

  88. static void PlsrPublishAxis(uint8_t axis)

  89. {

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

  91.     uint8_t compatibleRun;

  92. 

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

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

  95.     (void)PlcDevicePublishSm(axis,

  96.                              compatibleRun,

  97.                              axisObject->directionPositive);

  98. }

  99. 

  100. static void PlsrSetStopReason(PLSR_AXIS *axis,

  101.                               PLSR_STOP_REASON stopReason)

  102. {

  103.     if (stopReason > axis->stopReason)

  104.     {

  105.         axis->stopReason = stopReason;

  106.     }

  107. }

  108. 

  109. static uint8_t PlsrTransitionIsAllowed(PLSR_STATE current,

  110.                                        PLSR_STATE target)

  111. {

  112.     if (current == target)

  113.     {

  114.         return 1U;

  115.     }

  116.     if (target == PLSR_STATE_ERROR)

  117.     {

  118.         return 1U;

  119.     }

  120. 

  121.     switch (current)

  122.     {

  123.         case PLSR_STATE_UNINITIALIZED:

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

  125. 

  126.         case PLSR_STATE_IDLE:

  127.         case PLSR_STATE_COMPLETED:

  128.         case PLSR_STATE_STOPPED:

  129.             return ((target == PLSR_STATE_ACCEL)

  130.                     || (target == PLSR_STATE_WAIT)

  131.                     || (target == PLSR_STATE_COMPLETED))

  132.                        ? 1U

  133.                        : 0U;

  134. 

  135.         case PLSR_STATE_ACCEL:

  136.         case PLSR_STATE_RUN:

  137.             return ((target == PLSR_STATE_RUN)

  138.                     || (target == PLSR_STATE_DECEL)

  139.                     || (target == PLSR_STATE_WAIT)

  140.                     || (target == PLSR_STATE_COMPLETED)

  141.                     || (target == PLSR_STATE_STOPPED))

  142.                        ? 1U

  143.                        : 0U;

  144. 

  145.         case PLSR_STATE_DECEL:

  146.             return ((target == PLSR_STATE_RUN)

  147.                     || (target == PLSR_STATE_WAIT)

  148.                     || (target == PLSR_STATE_PAUSED)

  149.                     || (target == PLSR_STATE_COMPLETED)

  150.                     || (target == PLSR_STATE_STOPPED))

  151.                        ? 1U

  152.                        : 0U;

  153. 

  154.         case PLSR_STATE_WAIT:

  155.             return ((target == PLSR_STATE_ACCEL)

  156.                     || (target == PLSR_STATE_RUN)

  157.                     || (target == PLSR_STATE_PAUSED)

  158.                     || (target == PLSR_STATE_COMPLETED)

  159.                     || (target == PLSR_STATE_STOPPED))

  160.                        ? 1U

  161.                        : 0U;

  162. 

  163.         case PLSR_STATE_PAUSED:

  164.             return ((target == PLSR_STATE_ACCEL)

  165.                     || (target == PLSR_STATE_RUN)

  166.                     || (target == PLSR_STATE_STOPPED))

  167.                        ? 1U

  168.                        : 0U;

  169. 

  170.         case PLSR_STATE_ERROR:

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

  172. 

  173.         default:

  174.             return 0U;

  175.     }

  176. }

  177. 

  178. PLSR_RESULT PlsrStateTransition(uint8_t axis,

  179.                                 PLSR_STATE target,

  180.                                 PLSR_TRANSITION_REASON reason)

  181. {

  182.     PLSR_AXIS *axisObject;

  183. 

  184.     (void)reason;

  185.     if (axis >= PLSR_AXIS_COUNT)

  186.     {

  187.         return PLSR_RESULT_INVALID_AXIS;

  188.     }

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

  190.     {

  191.         return PLSR_RESULT_INVALID_ARGUMENT;

  192.     }

  193. 

  194.     axisObject = &PlsrAxes[axis];

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

  196.     {

  197.         axisObject->illegalTransitionCount++;

  198.         axisObject->error = PLSR_ERROR_ILLEGAL_TRANSITION;

  199.         axisObject->stopReason = PLSR_STOP_REASON_FAULT;

  200.         axisObject->done = 0U;

  201.         axisObject->state = PLSR_STATE_ERROR;

  202.         axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

  203.         axisObject->immediateStopPending = 0U;

  204.         PlsrResourceRelease(&axisObject->lease);

  205.         PlsrPublishAxis(axis);

  206.         return PLSR_RESULT_INVALID_STATE;

  207.     }

  208. 

  209.     axisObject->state = target;

  210.     if (target == PLSR_STATE_COMPLETED)

  211.     {

  212.         axisObject->done = 1U;

  213.     }

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

  215.         || (target == PLSR_STATE_ERROR))

  216.     {

  217.         axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

  218.         axisObject->immediateStopPending = 0U;

  219.         PlsrResourceRelease(&axisObject->lease);

  220.     }

  221.     PlsrPublishAxis(axis);

  222.     return PLSR_RESULT_OK;

  223. }

  224. 

  225. static uint8_t PlsrCommandPriority(PLSR_COMMAND_OPCODE opcode)

  226. {

  227.     switch (opcode)

  228.     {

  229.         case PLSR_CMD_STOP_IMMEDIATE:

  230.             return 0U;

  231.         case PLSR_CMD_STOP_DECEL:

  232.             return 1U;

  233.         case PLSR_CMD_PAUSE:

  234.             return 2U;

  235.         case PLSR_CMD_RESUME:

  236.             return 3U;

  237.         default:

  238.             return 4U;

  239.     }

  240. }

  241. 

  242. static PLSR_RESULT PlsrQueueCommand(const PLSR_COMMAND *command,

  243.                                     const PLSR_START_REQUEST *start)

  244. {

  245.     uint32_t interruptState;

  246.     uint8_t freeSlot = PLSR_COMMAND_QUEUE_DEPTH;

  247.     uint8_t index;

  248. 

  249.     interruptState = PlsrCoreEnterCritical();

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

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

  252.     {

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

  254.         PlsrCoreExitCritical(interruptState);

  255.         return result;

  256.     }

  257. 

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

  259.     {

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

  261.         {

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

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

  264.                     == command->sequence))

  265.             {

  266.                 PlsrCoreExitCritical(interruptState);

  267.                 return PLSR_RESULT_QUEUED;

  268.             }

  269.         }

  270.         else if (freeSlot == PLSR_COMMAND_QUEUE_DEPTH)

  271.         {

  272.             freeSlot = index;

  273.         }

  274.     }

  275. 

  276.     if (freeSlot == PLSR_COMMAND_QUEUE_DEPTH)

  277.     {

  278.         PlsrCoreExitCritical(interruptState);

  279.         return PLSR_RESULT_QUEUE_FULL;

  280.     }

  281. 

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

  283.     if (start != NULL)

  284.     {

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

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

  287.     }

  288.     else

  289.     {

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

  291.                      0,

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

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

  294.     }

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

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

  297.     PlsrCoreExitCritical(interruptState);

  298.     return PLSR_RESULT_QUEUED;

  299. }

  300. 

  301. static uint8_t PlsrPopHighestPriorityCommand(PLSR_COMMAND_SLOT *slot)

  302. {

  303.     uint32_t interruptState;

  304.     uint32_t selectedTicket = 0UL;

  305.     uint8_t selectedPriority = 0xFFU;

  306.     uint8_t selected = PLSR_COMMAND_QUEUE_DEPTH;

  307.     uint8_t index;

  308.     uint8_t priority;

  309. 

  310.     interruptState = PlsrCoreEnterCritical();

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

  312.     {

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

  314.         {

  315.             continue;

  316.         }

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

  318.         if ((selected == PLSR_COMMAND_QUEUE_DEPTH)

  319.             || (priority < selectedPriority)

  320.             || ((priority == selectedPriority)

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

  322.                               - selectedTicket)

  323.                     < 0)))

  324.         {

  325.             selected = index;

  326.             selectedPriority = priority;

  327.             selectedTicket = PlsrCommandQueue[index].ticket;

  328.         }

  329.     }

  330. 

  331.     if (selected == PLSR_COMMAND_QUEUE_DEPTH)

  332.     {

  333.         PlsrCoreExitCritical(interruptState);

  334.         return 0U;

  335.     }

  336. 

  337.     *slot = PlsrCommandQueue[selected];

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

  339.     PlsrCoreExitCritical(interruptState);

  340.     return 1U;

  341. }

  342. 

  343. PLSR_RESULT PlsrPostStart(const PLSR_START_REQUEST *request)

  344. {

  345.     PLSR_COMMAND command;

  346. 

  347.     if (request == NULL)

  348.     {

  349.         return PLSR_RESULT_INVALID_ARGUMENT;

  350.     }

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

  352.     {

  353.         return PLSR_RESULT_INVALID_AXIS;

  354.     }

  355.     if (PlsrInitialized == 0U)

  356.     {

  357.         return PLSR_RESULT_INVALID_STATE;

  358.     }

  359. 

  360.     command.sequence = request->sequence;

  361.     command.axis = request->axis;

  362.     command.opcode = PLSR_CMD_START;

  363.     command.argument = 0;

  364.     return PlsrQueueCommand(&command, request);

  365. }

  366. 

  367. PLSR_RESULT PlsrPostCommand(const PLSR_COMMAND *command)

  368. {

  369.     if (command == NULL)

  370.     {

  371.         return PLSR_RESULT_INVALID_ARGUMENT;

  372.     }

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

  374.     {

  375.         return PLSR_RESULT_INVALID_AXIS;

  376.     }

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

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

  379.     {

  380.         return PLSR_RESULT_INVALID_ARGUMENT;

  381.     }

  382.     if (PlsrInitialized == 0U)

  383.     {

  384.         return PLSR_RESULT_INVALID_STATE;

  385.     }

  386.     return PlsrQueueCommand(command, NULL);

  387. }

  388. 

  389. PLSR_RESULT PlsrPostEvent(uint8_t axis, uint32_t eventMask)

  390. {

  391.     uint32_t interruptState;

  392. 

  393.     if (axis >= PLSR_AXIS_COUNT)

  394.     {

  395.         return PLSR_RESULT_INVALID_AXIS;

  396.     }

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

  398.     {

  399.         return PLSR_RESULT_INVALID_ARGUMENT;

  400.     }

  401. 

  402.     interruptState = PlsrCoreEnterCritical();

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

  404.     PlsrCoreExitCritical(interruptState);

  405.     return PLSR_RESULT_OK;

  406. }

  407. 

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

  409. {

  410.     uint32_t interruptState;

  411.     uint32_t events;

  412. 

  413.     interruptState = PlsrCoreEnterCritical();

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

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

  416.     PlsrCoreExitCritical(interruptState);

  417.     return events;

  418. }

  419. 

  420. static uint8_t PlsrAnyAxisBusy(void)

  421. {

  422.     uint8_t axis;

  423. 

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

  425.     {

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

  427.         {

  428.             return 1U;

  429.         }

  430.     }

  431.     return 0U;

  432. }

  433. 

  434. static PLSR_RESULT PlsrStartAxis(PLSR_AXIS *axisObject,

  435.                                  const PLSR_START_REQUEST *start)

  436. {

  437.     PLSR_RESOURCE_REQUEST resourceRequest;

  438.     PLSR_RESULT result;

  439. 

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

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

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

  443.     {

  444.         return PLSR_RESULT_INVALID_STATE;

  445.     }

  446. 

  447.     resourceRequest.ownerAxis = start->axis;

  448.     resourceRequest.outputMode = start->outputMode;

  449.     resourceRequest.dAxis = start->axis;

  450.     resourceRequest.directionPoint = start->directionPoint;

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

  452.     if (result != PLSR_RESULT_OK)

  453.     {

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

  455.                                 ? PLSR_ERROR_RESOURCE_CONFLICT

  456.                                 : PLSR_ERROR_INVALID_RESOURCE;

  457.         return result;

  458.     }

  459. 

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

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

  462.     axisObject->error = PLSR_ERROR_NONE;

  463.     axisObject->stopReason = PLSR_STOP_REASON_NONE;

  464.     axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

  465.     axisObject->immediateStopPending = 0U;

  466.     axisObject->done = 0U;

  467.     result = PlsrStateTransition(start->axis,

  468.                                  PLSR_STATE_ACCEL,

  469.                                  PLSR_TRANSITION_START);

  470.     if (result != PLSR_RESULT_OK)

  471.     {

  472.         PlsrResourceRelease(&axisObject->lease);

  473.     }

  474.     return result;

  475. }

  476. 

  477. static PLSR_RESULT PlsrStopImmediate(uint8_t axis)

  478. {

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

  480. 

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

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

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

  484.     {

  485.         return PLSR_RESULT_OK;

  486.     }

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

  488.     {

  489.         return PLSR_RESULT_INVALID_STATE;

  490.     }

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

  492.     {

  493.         return PLSR_RESULT_OK;

  494.     }

  495. 

  496.     PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_IMMEDIATE);

  497.     axisObject->pendingTerminal = PLSR_STATE_STOPPED;

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

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

  500.     {

  501.         return PlsrStateTransition(axis,

  502.                                    PLSR_STATE_STOPPED,

  503.                                    PLSR_TRANSITION_STOP);

  504.     }

  505. 

  506.     axisObject->immediateStopPending = 1U;

  507.     return PLSR_RESULT_OK;

  508. }

  509. 

  510. static PLSR_RESULT PlsrStopDecel(uint8_t axis)

  511. {

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

  513. 

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

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

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

  517.     {

  518.         return PLSR_RESULT_OK;

  519.     }

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

  521.     {

  522.         return PLSR_RESULT_INVALID_STATE;

  523.     }

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

  525.     {

  526.         return PLSR_RESULT_OK;

  527.     }

  528. 

  529.     PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_DECEL);

  530.     axisObject->pendingTerminal = PLSR_STATE_STOPPED;

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

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

  533.     {

  534.         return PlsrStateTransition(axis,

  535.                                    PLSR_STATE_STOPPED,

  536.                                    PLSR_TRANSITION_STOP);

  537.     }

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

  539.     {

  540.         return PLSR_RESULT_OK;

  541.     }

  542.     return PlsrStateTransition(axis,

  543.                                PLSR_STATE_DECEL,

  544.                                PLSR_TRANSITION_DECEL_REQUEST);

  545. }

  546. 

  547. static PLSR_RESULT PlsrPause(uint8_t axis)

  548. {

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

  550. 

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

  552.     {

  553.         return PLSR_RESULT_OK;

  554.     }

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

  556.     {

  557.         return PLSR_RESULT_BUSY;

  558.     }

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

  560.     {

  561.         PlsrSetStopReason(axisObject, PLSR_STOP_REASON_PAUSE);

  562.         return PlsrStateTransition(axis,

  563.                                    PLSR_STATE_PAUSED,

  564.                                    PLSR_TRANSITION_DECEL_COMPLETE);

  565.     }

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

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

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

  569.     {

  570.         return PLSR_RESULT_INVALID_STATE;

  571.     }

  572. 

  573.     PlsrSetStopReason(axisObject, PLSR_STOP_REASON_PAUSE);

  574.     axisObject->pendingTerminal = PLSR_STATE_PAUSED;

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

  576.     {

  577.         return PLSR_RESULT_OK;

  578.     }

  579.     return PlsrStateTransition(axis,

  580.                                PLSR_STATE_DECEL,

  581.                                PLSR_TRANSITION_DECEL_REQUEST);

  582. }

  583. 

  584. static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot)

  585. {

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

  587.     PLC_DEVICE_RESULT deviceResult;

  588.     PLSR_RESULT result;

  589. 

  590.     switch (slot->command.opcode)

  591.     {

  592.         case PLSR_CMD_START:

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

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

  595.                          : PLSR_RESULT_INVALID_ARGUMENT;

  596.             break;

  597. 

  598.         case PLSR_CMD_STOP_DECEL:

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

  600.             break;

  601. 

  602.         case PLSR_CMD_STOP_IMMEDIATE:

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

  604.             break;

  605. 

  606.         case PLSR_CMD_PAUSE:

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

  608.             break;

  609. 

  610.         case PLSR_CMD_RESUME:

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

  612.             {

  613.                 result = PLSR_RESULT_INVALID_STATE;

  614.             }

  615.             else

  616.             {

  617.                 axisObject->stopReason = PLSR_STOP_REASON_NONE;

  618.                 axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;

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

  620.                                              PLSR_STATE_ACCEL,

  621.                                              PLSR_TRANSITION_WAIT_COMPLETE);

  622.             }

  623.             break;

  624. 

  625.         case PLSR_CMD_SET_POSITION:

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

  627.             {

  628.                 result = PLSR_RESULT_BUSY;

  629.             }

  630.             else

  631.             {

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

  633.                 (void)PlcDevicePublishHsdPair(

  634.                     (uint16_t)(slot->command.axis

  635.                                * PLSR_HSD_RUNTIME_AXIS_COUNT),

  636.                     axisObject->logicalPosition);

  637.                 result = PLSR_RESULT_OK;

  638.             }

  639.             break;

  640. 

  641.         case PLSR_CMD_CLEAR_POSITION:

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

  643.             {

  644.                 result = PLSR_RESULT_BUSY;

  645.             }

  646.             else

  647.             {

  648.                 axisObject->logicalPosition = 0;

  649.                 (void)PlcDevicePublishHsdPair(

  650.                     (uint16_t)(slot->command.axis

  651.                                * PLSR_HSD_RUNTIME_AXIS_COUNT),

  652.                     0);

  653.                 result = PLSR_RESULT_OK;

  654.             }

  655.             break;

  656. 

  657.         case PLSR_CMD_CLEAR_TOTAL:

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

  659.             {

  660.                 result = PLSR_RESULT_BUSY;

  661.             }

  662.             else

  663.             {

  664.                 axisObject->totalPulses = 0;

  665.                 result = PLSR_RESULT_OK;

  666.             }

  667.             break;

  668. 

  669.         case PLSR_CMD_SAVE_CONFIG:

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

  671.             {

  672.                 result = PLSR_RESULT_BUSY;

  673.             }

  674.             else

  675.             {

  676.                 deviceResult = PlcDeviceSaveSfd();

  677.                 result = (deviceResult == PLC_DEVICE_OK)

  678.                              ? PLSR_RESULT_OK

  679.                              : PLSR_RESULT_PERSISTENCE_ERROR;

  680.             }

  681.             break;

  682. 

  683.         case PLSR_CMD_LOAD_CONFIG:

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

  685.             {

  686.                 result = PLSR_RESULT_BUSY;

  687.             }

  688.             else

  689.             {

  690.                 deviceResult = PlcDeviceLoadSfd();

  691.                 result = (deviceResult == PLC_DEVICE_OK)

  692.                              ? PLSR_RESULT_OK

  693.                              : PLSR_RESULT_PERSISTENCE_ERROR;

  694.             }

  695.             break;

  696. 

  697.         case PLSR_CMD_RESET_ERROR:

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

  699.             {

  700.                 result = PLSR_RESULT_INVALID_STATE;

  701.             }

  702.             else

  703.             {

  704.                 axisObject->error = PLSR_ERROR_NONE;

  705.                 axisObject->stopReason = PLSR_STOP_REASON_NONE;

  706.                 axisObject->done = 0U;

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

  708.                                              PLSR_STATE_IDLE,

  709.                                              PLSR_TRANSITION_RESET_ERROR);

  710.             }

  711.             break;

  712. 

  713.         case PLSR_CMD_SELF_TEST:

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

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

  716.                          ? PLSR_RESULT_OK

  717.                          : PLSR_RESULT_BUSY;

  718.             break;

  719. 

  720.         default:

  721.             result = PLSR_RESULT_INVALID_ARGUMENT;

  722.             break;

  723.     }

  724. 

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

  726.     axisObject->lastCommandResult = result;

  727.     axisObject->hasLastCommand = 1U;

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

  729.     return result;

  730. }

  731. 

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

  733. {

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

  735. 

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

  737.     {

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

  739.         {

  740.             PlsrSetStopReason(axisObject,

  741.                               PLSR_STOP_REASON_SOFTWARE_EMERGENCY);

  742.             axisObject->done = 0U;

  743.             (void)PlsrStateTransition(axis,

  744.                                       PLSR_STATE_STOPPED,

  745.                                       PLSR_TRANSITION_STOP);

  746.         }

  747.         return;

  748.     }

  749. 

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

  751.     {

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

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

  754.         {

  755.             axisObject->error = PLSR_ERROR_LIMIT_POSITIVE;

  756.             PlsrSetStopReason(axisObject,

  757.                               PLSR_STOP_REASON_LIMIT_POSITIVE);

  758.             axisObject->pendingTerminal = PLSR_STATE_STOPPED;

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

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

  761.             {

  762.                 (void)PlsrStateTransition(axis,

  763.                                           PLSR_STATE_STOPPED,

  764.                                           PLSR_TRANSITION_STOP);

  765.             }

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

  767.             {

  768.                 (void)PlsrStateTransition(axis,

  769.                                           PLSR_STATE_DECEL,

  770.                                           PLSR_TRANSITION_DECEL_REQUEST);

  771.             }

  772.         }

  773.         return;

  774.     }

  775. 

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

  777.     {

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

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

  780.         {

  781.             axisObject->error = PLSR_ERROR_LIMIT_NEGATIVE;

  782.             PlsrSetStopReason(axisObject,

  783.                               PLSR_STOP_REASON_LIMIT_NEGATIVE);

  784.             axisObject->pendingTerminal = PLSR_STATE_STOPPED;

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

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

  787.             {

  788.                 (void)PlsrStateTransition(axis,

  789.                                           PLSR_STATE_STOPPED,

  790.                                           PLSR_TRANSITION_STOP);

  791.             }

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

  793.             {

  794.                 (void)PlsrStateTransition(axis,

  795.                                           PLSR_STATE_DECEL,

  796.                                           PLSR_TRANSITION_DECEL_REQUEST);

  797.             }

  798.         }

  799.         return;

  800.     }

  801. 

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

  803.     {

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

  805.                                 ? PLSR_ERROR_TIMER_FAULT

  806.                                 : PLSR_ERROR_COUNTER_FAULT;

  807.         PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT);

  808.         axisObject->done = 0U;

  809.         (void)PlsrStateTransition(axis,

  810.                                   PLSR_STATE_ERROR,

  811.                                   PLSR_TRANSITION_FAULT);

  812.     }

  813. }

  814. 

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

  816. {

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

  818. 

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

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

  821.     {

  822.         (void)PlsrStateTransition(axis,

  823.                                   PLSR_STATE_STOPPED,

  824.                                   PLSR_TRANSITION_STOP);

  825.         return;

  826.     }

  827. 

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

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

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

  831.     {

  832.         (void)PlsrStateTransition(axis,

  833.                                   PLSR_STATE_RUN,

  834.                                   PLSR_TRANSITION_ACCEL_COMPLETE);

  835.     }

  836. 

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

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

  839.     {

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

  841.         {

  842.             (void)PlsrStateTransition(axis,

  843.                                       PLSR_STATE_PAUSED,

  844.                                       PLSR_TRANSITION_DECEL_COMPLETE);

  845.         }

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

  847.         {

  848.             (void)PlsrStateTransition(axis,

  849.                                       PLSR_STATE_STOPPED,

  850.                                       PLSR_TRANSITION_DECEL_COMPLETE);

  851.         }

  852.         else

  853.         {

  854.             (void)PlsrStateTransition(axis,

  855.                                       PLSR_STATE_RUN,

  856.                                       PLSR_TRANSITION_DECEL_COMPLETE);

  857.         }

  858.     }

  859. 

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

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

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

  863.     {

  864.         (void)PlsrStateTransition(axis,

  865.                                   PLSR_STATE_WAIT,

  866.                                   PLSR_TRANSITION_WAIT_BEGIN);

  867.     }

  868. 

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

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

  871.     {

  872.         (void)PlsrStateTransition(axis,

  873.                                   PLSR_STATE_ACCEL,

  874.                                   PLSR_TRANSITION_WAIT_COMPLETE);

  875.     }

  876. 

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

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

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

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

  881.     {

  882.         axisObject->stopReason = PLSR_STOP_REASON_NORMAL_COMPLETE;

  883.         (void)PlsrStateTransition(axis,

  884.                                   PLSR_STATE_COMPLETED,

  885.                                   PLSR_TRANSITION_JOB_COMPLETE);

  886.     }

  887. }

  888. 

  889. PLSR_RESULT PlsrInit(void)

  890. {

  891.     uint8_t axis;

  892. 

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

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

  895.     PlsrNextTicket = 0UL;

  896.     PlsrResourceInit();

  897.     PlsrInitialized = 1U;

  898. 

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

  900.     {

  901.         PlsrAxes[axis].state = PLSR_STATE_UNINITIALIZED;

  902.         PlsrAxes[axis].pendingTerminal = PLSR_STATE_UNINITIALIZED;

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

  904.         if (PlsrStateTransition(axis,

  905.                                 PLSR_STATE_IDLE,

  906.                                 PLSR_TRANSITION_INITIALIZED)

  907.             != PLSR_RESULT_OK)

  908.         {

  909.             return PLSR_RESULT_INTERNAL_ERROR;

  910.         }

  911.     }

  912.     return PLSR_RESULT_OK;

  913. }

  914. 

  915. void PlsrProcess(void)

  916. {

  917.     PLSR_COMMAND_SLOT slot;

  918.     uint32_t events;

  919.     uint8_t processedCommands = 0U;

  920.     uint8_t axis;

  921. 

  922.     if (PlsrInitialized == 0U)

  923.     {

  924.         return;

  925.     }

  926. 

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

  928.     {

  929.         events = PlsrTakeEvents(axis, PLSR_EVENT_CRITICAL_MASK);

  930.         if (events != 0UL)

  931.         {

  932.             PlsrProcessCriticalEvents(axis, events);

  933.         }

  934.     }

  935. 

  936.     while ((processedCommands < PLSR_COMMAND_QUEUE_DEPTH)

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

  938.     {

  939.         (void)PlsrExecuteCommand(&slot);

  940.         processedCommands++;

  941.     }

  942. 

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

  944.     {

  945.         events = PlsrTakeEvents(axis,

  946.                                 PLSR_EVENT_ALL_MASK

  947.                                     & ~PLSR_EVENT_CRITICAL_MASK);

  948.         if (events != 0UL)

  949.         {

  950.             PlsrProcessNormalEvents(axis, events);

  951.         }

  952.     }

  953. }

  954. 

  955. void PlsrTask(void *argument)

  956. {

  957.     (void)argument;

  958. #ifdef PLSR_HOST_TEST

  959.     PlsrProcess();

  960. #else

  961.     while (1)

  962.     {

  963.         PlsrProcess();

  964.         OSTimeDly(1U);

  965.     }

  966. #endif

  967. }

  968. 

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

  970. {

  971.     PLSR_AXIS *axisObject;

  972.     uint32_t interruptState;

  973. 

  974.     if (axis >= PLSR_AXIS_COUNT)

  975.     {

  976.         return PLSR_RESULT_INVALID_AXIS;

  977.     }

  978.     if (status == NULL)

  979.     {

  980.         return PLSR_RESULT_INVALID_ARGUMENT;

  981.     }

  982. 

  983.     interruptState = PlsrCoreEnterCritical();

  984.     axisObject = &PlsrAxes[axis];

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  1002.                                  ? axisObject->lease.directionPoint

  1003.                                  : PLSR_DIRECTION_POINT_NONE;

  1004.     PlsrCoreExitCritical(interruptState);

  1005.     return PLSR_RESULT_OK;

  1006. }