diff --git a/Core/Src/main.c b/Core/Src/main.c
index 30ec68e..1f8c2e9 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -25,6 +25,7 @@
#include "ucos_ii.h"
#include "modbus_rtu_slave.h"
#include "plc_device.h"
+#include "plsr_core.h"
#include "stdio.h"
/* USER CODE END Includes */
@@ -51,6 +52,7 @@ static volatile uint8_t ConnectFlag;
/* USER CODE BEGIN PV */
static OS_STK AppTaskStartStk[APP_TASK_START_STK_SIZE];
+static OS_STK PlsrTaskStk[APP_TASK_PLSR_STK_SIZE];
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
@@ -165,12 +167,27 @@ int main(void)
{
Error_Handler();
}
+ if (PlsrInit() != PLSR_RESULT_OK)
+ {
+ Error_Handler();
+ }
/* USER CODE BEGIN 2 */
INT8U osError;
OSInit();
+ osError = OSTaskCreateExt(PlsrTask, 0,
+ &PlsrTaskStk[APP_TASK_PLSR_STK_SIZE - 1u],
+ APP_TASK_PLSR_PRIO, APP_TASK_PLSR_PRIO,
+ &PlsrTaskStk[0], APP_TASK_PLSR_STK_SIZE, 0,
+ (OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
+
+ if (osError != OS_ERR_NONE)
+ {
+ Error_Handler();
+ }
+
osError = OSTaskCreateExt(AppTaskStart, 0,
&AppTaskStartStk[APP_TASK_START_STK_SIZE - 1u],
APP_TASK_START_PRIO, APP_TASK_START_PRIO,
diff --git a/EWARM/Modbus.ewp b/EWARM/Modbus.ewp
index 33fbc95..3987a39 100644
--- a/EWARM/Modbus.ewp
+++ b/EWARM/Modbus.ewp
@@ -1272,12 +1272,27 @@
$PROJ_DIR$\..\PLSR\Inc\plc_device.h
+
+ $PROJ_DIR$\..\PLSR\Inc\plsr_types.h
+
+
+ $PROJ_DIR$\..\PLSR\Inc\plsr_resource.h
+
+
+ $PROJ_DIR$\..\PLSR\Inc\plsr_core.h
+
$PROJ_DIR$\..\PLSR\Src\plsr_persistence.c
$PROJ_DIR$\..\PLSR\Src\plc_device.c
+
+ $PROJ_DIR$\..\PLSR\Src\plsr_resource.c
+
+
+ $PROJ_DIR$\..\PLSR\Src\plsr_core.c
+
Modbus
diff --git a/Middlewares/Third_Party/Micrium/Config/app_cfg.h b/Middlewares/Third_Party/Micrium/Config/app_cfg.h
index bd83e5b..ee292b8 100644
--- a/Middlewares/Third_Party/Micrium/Config/app_cfg.h
+++ b/Middlewares/Third_Party/Micrium/Config/app_cfg.h
@@ -7,6 +7,7 @@
*/
#define APP_TASK_START_PRIO 5u
#define APP_TASK_TEST_PRIO 6u
+#define APP_TASK_PLSR_PRIO 4u
/*
* 任务栈大小的单位是OS_STK元素。当前OS_STK为32位，
@@ -14,5 +15,6 @@
*/
#define APP_TASK_START_STK_SIZE 256u
#define APP_TASK_TEST_STK_SIZE 256u
+#define APP_TASK_PLSR_STK_SIZE 256u
#endif
diff --git a/PLSR/Inc/plsr_core.h b/PLSR/Inc/plsr_core.h
new file mode 100644
index 0000000..89e2b3e
--- /dev/null
+++ b/PLSR/Inc/plsr_core.h
@@ -0,0 +1,28 @@
+#ifndef PLSR_CORE_H
+#define PLSR_CORE_H
+
+#include "plsr_types.h"
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+PLSR_RESULT PlsrInit(void);
+void PlsrTask(void *argument);
+void PlsrProcess(void);
+
+PLSR_RESULT PlsrPostStart(const PLSR_START_REQUEST *request);
+PLSR_RESULT PlsrPostCommand(const PLSR_COMMAND *command);
+PLSR_RESULT PlsrPostEvent(uint8_t axis, uint32_t eventMask);
+PLSR_RESULT PlsrGetStatus(uint8_t axis, PLSR_STATUS *status);
+
+PLSR_RESULT PlsrStateTransition(uint8_t axis,
+ PLSR_STATE target,
+ PLSR_TRANSITION_REASON reason);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PLSR_CORE_H */
diff --git a/PLSR/Inc/plsr_resource.h b/PLSR/Inc/plsr_resource.h
new file mode 100644
index 0000000..39af986
--- /dev/null
+++ b/PLSR/Inc/plsr_resource.h
@@ -0,0 +1,46 @@
+#ifndef PLSR_RESOURCE_H
+#define PLSR_RESOURCE_H
+
+#include "plsr_types.h"
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct
+{
+ uint8_t ownerAxis;
+ PLSR_OUTPUT_MODE outputMode;
+ uint8_t dAxis;
+ uint8_t directionPoint;
+} PLSR_RESOURCE_REQUEST;
+
+typedef struct
+{
+ uint32_t outputMask;
+ uint8_t highMask;
+ uint8_t ownerAxis;
+ uint8_t directionPoint;
+ PLSR_OUTPUT_MODE outputMode;
+ uint8_t valid;
+} PLSR_RESOURCE_LEASE;
+
+typedef struct
+{
+ uint32_t outputMask;
+ uint8_t highMask;
+} PLSR_RESOURCE_STATUS;
+
+void PlsrResourceInit(void);
+PLSR_RESULT PlsrResourceReserve(const PLSR_RESOURCE_REQUEST *request,
+ PLSR_RESOURCE_LEASE *lease);
+void PlsrResourceRelease(PLSR_RESOURCE_LEASE *lease);
+void PlsrResourceGetStatus(PLSR_RESOURCE_STATUS *status);
+uint8_t PlsrResourceCheckInvariant(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PLSR_RESOURCE_H */
diff --git a/PLSR/Inc/plsr_types.h b/PLSR/Inc/plsr_types.h
new file mode 100644
index 0000000..2c948f0
--- /dev/null
+++ b/PLSR/Inc/plsr_types.h
@@ -0,0 +1,168 @@
+#ifndef PLSR_TYPES_H
+#define PLSR_TYPES_H
+
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define PLSR_COMMAND_QUEUE_DEPTH (16U)
+#define PLSR_DIRECTION_POINT_NONE (0xFFU)
+
+typedef enum
+{
+ PLSR_STATE_UNINITIALIZED = 0,
+ PLSR_STATE_IDLE,
+ PLSR_STATE_ACCEL,
+ PLSR_STATE_RUN,
+ PLSR_STATE_DECEL,
+ PLSR_STATE_WAIT,
+ PLSR_STATE_PAUSED,
+ PLSR_STATE_COMPLETED,
+ PLSR_STATE_STOPPED,
+ PLSR_STATE_ERROR
+} PLSR_STATE;
+
+typedef enum
+{
+ PLSR_OUTPUT_PULSE_DIR = 0,
+ PLSR_OUTPUT_AB,
+ PLSR_OUTPUT_CW_CCW
+} PLSR_OUTPUT_MODE;
+
+typedef enum
+{
+ PLSR_CMD_START = 0,
+ PLSR_CMD_STOP_DECEL,
+ PLSR_CMD_STOP_IMMEDIATE,
+ PLSR_CMD_PAUSE,
+ PLSR_CMD_RESUME,
+ PLSR_CMD_SET_POSITION,
+ PLSR_CMD_CLEAR_POSITION,
+ PLSR_CMD_CLEAR_TOTAL,
+ PLSR_CMD_SAVE_CONFIG,
+ PLSR_CMD_LOAD_CONFIG,
+ PLSR_CMD_RESET_ERROR,
+ PLSR_CMD_SELF_TEST
+} PLSR_COMMAND_OPCODE;
+
+typedef enum
+{
+ PLSR_RESULT_OK = 0,
+ PLSR_RESULT_QUEUED,
+ PLSR_RESULT_INVALID_ARGUMENT,
+ PLSR_RESULT_INVALID_AXIS,
+ PLSR_RESULT_INVALID_STATE,
+ PLSR_RESULT_QUEUE_FULL,
+ PLSR_RESULT_RESOURCE_CONFLICT,
+ PLSR_RESULT_INVALID_RESOURCE,
+ PLSR_RESULT_BUSY,
+ PLSR_RESULT_PERSISTENCE_ERROR,
+ PLSR_RESULT_NOT_SUPPORTED,
+ PLSR_RESULT_INTERNAL_ERROR
+} PLSR_RESULT;
+
+typedef enum
+{
+ PLSR_ERROR_NONE = 0,
+ PLSR_ERROR_ILLEGAL_TRANSITION,
+ PLSR_ERROR_RESOURCE_CONFLICT,
+ PLSR_ERROR_INVALID_RESOURCE,
+ PLSR_ERROR_TIMER_FAULT,
+ PLSR_ERROR_COUNTER_FAULT,
+ PLSR_ERROR_LIMIT_POSITIVE,
+ PLSR_ERROR_LIMIT_NEGATIVE,
+ PLSR_ERROR_INTERNAL
+} PLSR_ERROR;
+
+typedef enum
+{
+ PLSR_STOP_REASON_NONE = 0,
+ PLSR_STOP_REASON_NORMAL_COMPLETE,
+ PLSR_STOP_REASON_PAUSE,
+ PLSR_STOP_REASON_STOP_DECEL,
+ PLSR_STOP_REASON_STOP_IMMEDIATE,
+ PLSR_STOP_REASON_LIMIT_POSITIVE,
+ PLSR_STOP_REASON_LIMIT_NEGATIVE,
+ PLSR_STOP_REASON_SOFTWARE_EMERGENCY,
+ PLSR_STOP_REASON_FAULT
+} PLSR_STOP_REASON;
+
+typedef enum
+{
+ PLSR_TRANSITION_INITIALIZED = 0,
+ PLSR_TRANSITION_START,
+ PLSR_TRANSITION_ACCEL_COMPLETE,
+ PLSR_TRANSITION_DECEL_REQUEST,
+ PLSR_TRANSITION_DECEL_COMPLETE,
+ PLSR_TRANSITION_WAIT_BEGIN,
+ PLSR_TRANSITION_WAIT_COMPLETE,
+ PLSR_TRANSITION_JOB_COMPLETE,
+ PLSR_TRANSITION_STOP,
+ PLSR_TRANSITION_FAULT,
+ PLSR_TRANSITION_RESET_ERROR
+} PLSR_TRANSITION_REASON;
+
+#define PLSR_EVENT_SOFTWARE_EMERGENCY (1UL << 0U)
+#define PLSR_EVENT_LIMIT_POSITIVE (1UL << 1U)
+#define PLSR_EVENT_LIMIT_NEGATIVE (1UL << 2U)
+#define PLSR_EVENT_TIMER_FAULT (1UL << 3U)
+#define PLSR_EVENT_COUNTER_FAULT (1UL << 4U)
+#define PLSR_EVENT_STOP_IMMEDIATE_DONE (1UL << 5U)
+#define PLSR_EVENT_ACCEL_COMPLETE (1UL << 8U)
+#define PLSR_EVENT_DECEL_COMPLETE (1UL << 9U)
+#define PLSR_EVENT_WAIT_BEGIN (1UL << 10U)
+#define PLSR_EVENT_WAIT_COMPLETE (1UL << 11U)
+#define PLSR_EVENT_JOB_COMPLETE (1UL << 12U)
+
+#define PLSR_EVENT_ALL_MASK (0x00001F3FUL)
+#define PLSR_EVENT_CRITICAL_MASK (PLSR_EVENT_SOFTWARE_EMERGENCY \
+ | PLSR_EVENT_LIMIT_POSITIVE \
+ | PLSR_EVENT_LIMIT_NEGATIVE \
+ | PLSR_EVENT_TIMER_FAULT \
+ | PLSR_EVENT_COUNTER_FAULT)
+
+typedef struct
+{
+ uint32_t sequence;
+ uint8_t axis;
+ PLSR_COMMAND_OPCODE opcode;
+ int64_t argument;
+} PLSR_COMMAND;
+
+typedef struct
+{
+ uint32_t sequence;
+ uint8_t axis;
+ PLSR_OUTPUT_MODE outputMode;
+ uint8_t directionPoint;
+ uint8_t directionPositive;
+} PLSR_START_REQUEST;
+
+typedef struct
+{
+ PLSR_STATE state;
+ PLSR_OUTPUT_MODE outputMode;
+ PLSR_ERROR error;
+ PLSR_STOP_REASON stopReason;
+ PLSR_RESULT lastCommandResult;
+ uint32_t lastCommandSequence;
+ uint32_t illegalTransitionCount;
+ uint32_t pendingEvents;
+ int64_t logicalPosition;
+ int64_t totalPulses;
+ uint8_t busy;
+ uint8_t pulseActive;
+ uint8_t done;
+ uint8_t wait;
+ uint8_t directionPositive;
+ uint8_t highResourceMask;
+ uint8_t directionPoint;
+} PLSR_STATUS;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PLSR_TYPES_H */
diff --git a/PLSR/Src/plsr_core.c b/PLSR/Src/plsr_core.c
new file mode 100644
index 0000000..87b22ce
--- /dev/null
+++ b/PLSR/Src/plsr_core.c
@@ -0,0 +1,1006 @@
+#include "plsr_core.h"
+#include "plc_device.h"
+#include "plsr_address_map.h"
+#include "plsr_resource.h"
+#include
+
+#ifndef PLSR_HOST_TEST
+#include "stm32f4xx.h"
+#include "ucos_ii.h"
+#endif
+
+typedef struct
+{
+ PLSR_STATE state;
+ PLSR_OUTPUT_MODE outputMode;
+ PLSR_ERROR error;
+ PLSR_STOP_REASON stopReason;
+ PLSR_STATE pendingTerminal;
+ PLSR_RESOURCE_LEASE lease;
+ volatile uint32_t pendingEvents;
+ uint32_t lastCommandSequence;
+ PLSR_RESULT lastCommandResult;
+ uint32_t illegalTransitionCount;
+ int64_t logicalPosition;
+ int64_t totalPulses;
+ uint8_t hasLastCommand;
+ uint8_t done;
+ uint8_t directionPositive;
+ uint8_t immediateStopPending;
+} PLSR_AXIS;
+
+typedef struct
+{
+ PLSR_COMMAND command;
+ PLSR_START_REQUEST start;
+ uint32_t ticket;
+ uint8_t hasStart;
+ uint8_t occupied;
+} PLSR_COMMAND_SLOT;
+
+static PLSR_AXIS PlsrAxes[PLSR_AXIS_COUNT];
+static PLSR_COMMAND_SLOT PlsrCommandQueue[PLSR_COMMAND_QUEUE_DEPTH];
+static uint32_t PlsrNextTicket;
+static uint8_t PlsrInitialized;
+
+static uint32_t PlsrCoreEnterCritical(void)
+{
+#ifdef PLSR_HOST_TEST
+ return 0UL;
+#else
+ uint32_t interruptState = __get_PRIMASK();
+ __disable_irq();
+ __DMB();
+ return interruptState;
+#endif
+}
+
+static void PlsrCoreExitCritical(uint32_t interruptState)
+{
+#ifdef PLSR_HOST_TEST
+ (void)interruptState;
+#else
+ __DMB();
+ if (interruptState == 0UL)
+ {
+ __enable_irq();
+ }
+#endif
+}
+
+static uint8_t PlsrStateIsBusy(PLSR_STATE state)
+{
+ return ((state == PLSR_STATE_ACCEL) || (state == PLSR_STATE_RUN)
+ || (state == PLSR_STATE_DECEL) || (state == PLSR_STATE_WAIT)
+ || (state == PLSR_STATE_PAUSED))
+ ? 1U
+ : 0U;
+}
+
+static uint8_t PlsrStateIsPulseActive(PLSR_STATE state)
+{
+ return ((state == PLSR_STATE_ACCEL) || (state == PLSR_STATE_RUN)
+ || (state == PLSR_STATE_DECEL))
+ ? 1U
+ : 0U;
+}
+
+static void PlsrPublishAxis(uint8_t axis)
+{
+ PLSR_AXIS *axisObject = &PlsrAxes[axis];
+ uint8_t compatibleRun;
+
+ compatibleRun = (uint8_t)((PlsrStateIsPulseActive(axisObject->state) != 0U)
+ || (axisObject->state == PLSR_STATE_WAIT));
+ (void)PlcDevicePublishSm(axis,
+ compatibleRun,
+ axisObject->directionPositive);
+}
+
+static void PlsrSetStopReason(PLSR_AXIS *axis,
+ PLSR_STOP_REASON stopReason)
+{
+ if (stopReason > axis->stopReason)
+ {
+ axis->stopReason = stopReason;
+ }
+}
+
+static uint8_t PlsrTransitionIsAllowed(PLSR_STATE current,
+ PLSR_STATE target)
+{
+ if (current == target)
+ {
+ return 1U;
+ }
+ if (target == PLSR_STATE_ERROR)
+ {
+ return 1U;
+ }
+
+ switch (current)
+ {
+ case PLSR_STATE_UNINITIALIZED:
+ return (target == PLSR_STATE_IDLE) ? 1U : 0U;
+
+ case PLSR_STATE_IDLE:
+ case PLSR_STATE_COMPLETED:
+ case PLSR_STATE_STOPPED:
+ return ((target == PLSR_STATE_ACCEL)
+ || (target == PLSR_STATE_WAIT)
+ || (target == PLSR_STATE_COMPLETED))
+ ? 1U
+ : 0U;
+
+ case PLSR_STATE_ACCEL:
+ case PLSR_STATE_RUN:
+ return ((target == PLSR_STATE_RUN)
+ || (target == PLSR_STATE_DECEL)
+ || (target == PLSR_STATE_WAIT)
+ || (target == PLSR_STATE_COMPLETED)
+ || (target == PLSR_STATE_STOPPED))
+ ? 1U
+ : 0U;
+
+ case PLSR_STATE_DECEL:
+ return ((target == PLSR_STATE_RUN)
+ || (target == PLSR_STATE_WAIT)
+ || (target == PLSR_STATE_PAUSED)
+ || (target == PLSR_STATE_COMPLETED)
+ || (target == PLSR_STATE_STOPPED))
+ ? 1U
+ : 0U;
+
+ case PLSR_STATE_WAIT:
+ return ((target == PLSR_STATE_ACCEL)
+ || (target == PLSR_STATE_RUN)
+ || (target == PLSR_STATE_PAUSED)
+ || (target == PLSR_STATE_COMPLETED)
+ || (target == PLSR_STATE_STOPPED))
+ ? 1U
+ : 0U;
+
+ case PLSR_STATE_PAUSED:
+ return ((target == PLSR_STATE_ACCEL)
+ || (target == PLSR_STATE_RUN)
+ || (target == PLSR_STATE_STOPPED))
+ ? 1U
+ : 0U;
+
+ case PLSR_STATE_ERROR:
+ return (target == PLSR_STATE_IDLE) ? 1U : 0U;
+
+ default:
+ return 0U;
+ }
+}
+
+PLSR_RESULT PlsrStateTransition(uint8_t axis,
+ PLSR_STATE target,
+ PLSR_TRANSITION_REASON reason)
+{
+ PLSR_AXIS *axisObject;
+
+ (void)reason;
+ if (axis >= PLSR_AXIS_COUNT)
+ {
+ return PLSR_RESULT_INVALID_AXIS;
+ }
+ if ((uint32_t)target > (uint32_t)PLSR_STATE_ERROR)
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+
+ axisObject = &PlsrAxes[axis];
+ if (PlsrTransitionIsAllowed(axisObject->state, target) == 0U)
+ {
+ axisObject->illegalTransitionCount++;
+ axisObject->error = PLSR_ERROR_ILLEGAL_TRANSITION;
+ axisObject->stopReason = PLSR_STOP_REASON_FAULT;
+ axisObject->done = 0U;
+ axisObject->state = PLSR_STATE_ERROR;
+ axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;
+ axisObject->immediateStopPending = 0U;
+ PlsrResourceRelease(&axisObject->lease);
+ PlsrPublishAxis(axis);
+ return PLSR_RESULT_INVALID_STATE;
+ }
+
+ axisObject->state = target;
+ if (target == PLSR_STATE_COMPLETED)
+ {
+ axisObject->done = 1U;
+ }
+ if ((target == PLSR_STATE_COMPLETED) || (target == PLSR_STATE_STOPPED)
+ || (target == PLSR_STATE_ERROR))
+ {
+ axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;
+ axisObject->immediateStopPending = 0U;
+ PlsrResourceRelease(&axisObject->lease);
+ }
+ PlsrPublishAxis(axis);
+ return PLSR_RESULT_OK;
+}
+
+static uint8_t PlsrCommandPriority(PLSR_COMMAND_OPCODE opcode)
+{
+ switch (opcode)
+ {
+ case PLSR_CMD_STOP_IMMEDIATE:
+ return 0U;
+ case PLSR_CMD_STOP_DECEL:
+ return 1U;
+ case PLSR_CMD_PAUSE:
+ return 2U;
+ case PLSR_CMD_RESUME:
+ return 3U;
+ default:
+ return 4U;
+ }
+}
+
+static PLSR_RESULT PlsrQueueCommand(const PLSR_COMMAND *command,
+ const PLSR_START_REQUEST *start)
+{
+ uint32_t interruptState;
+ uint8_t freeSlot = PLSR_COMMAND_QUEUE_DEPTH;
+ uint8_t index;
+
+ interruptState = PlsrCoreEnterCritical();
+ if ((PlsrAxes[command->axis].hasLastCommand != 0U)
+ && (PlsrAxes[command->axis].lastCommandSequence == command->sequence))
+ {
+ PLSR_RESULT result = PlsrAxes[command->axis].lastCommandResult;
+ PlsrCoreExitCritical(interruptState);
+ return result;
+ }
+
+ for (index = 0U; index < PLSR_COMMAND_QUEUE_DEPTH; index++)
+ {
+ if (PlsrCommandQueue[index].occupied != 0U)
+ {
+ if ((PlsrCommandQueue[index].command.axis == command->axis)
+ && (PlsrCommandQueue[index].command.sequence
+ == command->sequence))
+ {
+ PlsrCoreExitCritical(interruptState);
+ return PLSR_RESULT_QUEUED;
+ }
+ }
+ else if (freeSlot == PLSR_COMMAND_QUEUE_DEPTH)
+ {
+ freeSlot = index;
+ }
+ }
+
+ if (freeSlot == PLSR_COMMAND_QUEUE_DEPTH)
+ {
+ PlsrCoreExitCritical(interruptState);
+ return PLSR_RESULT_QUEUE_FULL;
+ }
+
+ PlsrCommandQueue[freeSlot].command = *command;
+ if (start != NULL)
+ {
+ PlsrCommandQueue[freeSlot].start = *start;
+ PlsrCommandQueue[freeSlot].hasStart = 1U;
+ }
+ else
+ {
+ (void)memset(&PlsrCommandQueue[freeSlot].start,
+ 0,
+ sizeof(PlsrCommandQueue[freeSlot].start));
+ PlsrCommandQueue[freeSlot].hasStart = 0U;
+ }
+ PlsrCommandQueue[freeSlot].ticket = PlsrNextTicket++;
+ PlsrCommandQueue[freeSlot].occupied = 1U;
+ PlsrCoreExitCritical(interruptState);
+ return PLSR_RESULT_QUEUED;
+}
+
+static uint8_t PlsrPopHighestPriorityCommand(PLSR_COMMAND_SLOT *slot)
+{
+ uint32_t interruptState;
+ uint32_t selectedTicket = 0UL;
+ uint8_t selectedPriority = 0xFFU;
+ uint8_t selected = PLSR_COMMAND_QUEUE_DEPTH;
+ uint8_t index;
+ uint8_t priority;
+
+ interruptState = PlsrCoreEnterCritical();
+ for (index = 0U; index < PLSR_COMMAND_QUEUE_DEPTH; index++)
+ {
+ if (PlsrCommandQueue[index].occupied == 0U)
+ {
+ continue;
+ }
+ priority = PlsrCommandPriority(PlsrCommandQueue[index].command.opcode);
+ if ((selected == PLSR_COMMAND_QUEUE_DEPTH)
+ || (priority < selectedPriority)
+ || ((priority == selectedPriority)
+ && ((int32_t)(PlsrCommandQueue[index].ticket
+ - selectedTicket)
+ < 0)))
+ {
+ selected = index;
+ selectedPriority = priority;
+ selectedTicket = PlsrCommandQueue[index].ticket;
+ }
+ }
+
+ if (selected == PLSR_COMMAND_QUEUE_DEPTH)
+ {
+ PlsrCoreExitCritical(interruptState);
+ return 0U;
+ }
+
+ *slot = PlsrCommandQueue[selected];
+ PlsrCommandQueue[selected].occupied = 0U;
+ PlsrCoreExitCritical(interruptState);
+ return 1U;
+}
+
+PLSR_RESULT PlsrPostStart(const PLSR_START_REQUEST *request)
+{
+ PLSR_COMMAND command;
+
+ if (request == NULL)
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if (request->axis >= PLSR_AXIS_COUNT)
+ {
+ return PLSR_RESULT_INVALID_AXIS;
+ }
+ if (PlsrInitialized == 0U)
+ {
+ return PLSR_RESULT_INVALID_STATE;
+ }
+
+ command.sequence = request->sequence;
+ command.axis = request->axis;
+ command.opcode = PLSR_CMD_START;
+ command.argument = 0;
+ return PlsrQueueCommand(&command, request);
+}
+
+PLSR_RESULT PlsrPostCommand(const PLSR_COMMAND *command)
+{
+ if (command == NULL)
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if (command->axis >= PLSR_AXIS_COUNT)
+ {
+ return PLSR_RESULT_INVALID_AXIS;
+ }
+ if (((uint32_t)command->opcode > (uint32_t)PLSR_CMD_SELF_TEST)
+ || (command->opcode == PLSR_CMD_START))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if (PlsrInitialized == 0U)
+ {
+ return PLSR_RESULT_INVALID_STATE;
+ }
+ return PlsrQueueCommand(command, NULL);
+}
+
+PLSR_RESULT PlsrPostEvent(uint8_t axis, uint32_t eventMask)
+{
+ uint32_t interruptState;
+
+ if (axis >= PLSR_AXIS_COUNT)
+ {
+ return PLSR_RESULT_INVALID_AXIS;
+ }
+ if ((eventMask == 0UL) || ((eventMask & ~PLSR_EVENT_ALL_MASK) != 0UL))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+
+ interruptState = PlsrCoreEnterCritical();
+ PlsrAxes[axis].pendingEvents |= eventMask;
+ PlsrCoreExitCritical(interruptState);
+ return PLSR_RESULT_OK;
+}
+
+static uint32_t PlsrTakeEvents(uint8_t axis, uint32_t mask)
+{
+ uint32_t interruptState;
+ uint32_t events;
+
+ interruptState = PlsrCoreEnterCritical();
+ events = PlsrAxes[axis].pendingEvents & mask;
+ PlsrAxes[axis].pendingEvents &= ~events;
+ PlsrCoreExitCritical(interruptState);
+ return events;
+}
+
+static uint8_t PlsrAnyAxisBusy(void)
+{
+ uint8_t axis;
+
+ for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
+ {
+ if (PlsrStateIsBusy(PlsrAxes[axis].state) != 0U)
+ {
+ return 1U;
+ }
+ }
+ return 0U;
+}
+
+static PLSR_RESULT PlsrStartAxis(PLSR_AXIS *axisObject,
+ const PLSR_START_REQUEST *start)
+{
+ PLSR_RESOURCE_REQUEST resourceRequest;
+ PLSR_RESULT result;
+
+ if ((axisObject->state != PLSR_STATE_IDLE)
+ && (axisObject->state != PLSR_STATE_COMPLETED)
+ && (axisObject->state != PLSR_STATE_STOPPED))
+ {
+ return PLSR_RESULT_INVALID_STATE;
+ }
+
+ resourceRequest.ownerAxis = start->axis;
+ resourceRequest.outputMode = start->outputMode;
+ resourceRequest.dAxis = start->axis;
+ resourceRequest.directionPoint = start->directionPoint;
+ result = PlsrResourceReserve(&resourceRequest, &axisObject->lease);
+ if (result != PLSR_RESULT_OK)
+ {
+ axisObject->error = (result == PLSR_RESULT_RESOURCE_CONFLICT)
+ ? PLSR_ERROR_RESOURCE_CONFLICT
+ : PLSR_ERROR_INVALID_RESOURCE;
+ return result;
+ }
+
+ axisObject->outputMode = start->outputMode;
+ axisObject->directionPositive = (start->directionPositive != 0U) ? 1U : 0U;
+ axisObject->error = PLSR_ERROR_NONE;
+ axisObject->stopReason = PLSR_STOP_REASON_NONE;
+ axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;
+ axisObject->immediateStopPending = 0U;
+ axisObject->done = 0U;
+ result = PlsrStateTransition(start->axis,
+ PLSR_STATE_ACCEL,
+ PLSR_TRANSITION_START);
+ if (result != PLSR_RESULT_OK)
+ {
+ PlsrResourceRelease(&axisObject->lease);
+ }
+ return result;
+}
+
+static PLSR_RESULT PlsrStopImmediate(uint8_t axis)
+{
+ PLSR_AXIS *axisObject = &PlsrAxes[axis];
+
+ if ((axisObject->state == PLSR_STATE_IDLE)
+ || (axisObject->state == PLSR_STATE_COMPLETED)
+ || (axisObject->state == PLSR_STATE_STOPPED))
+ {
+ return PLSR_RESULT_OK;
+ }
+ if (axisObject->state == PLSR_STATE_ERROR)
+ {
+ return PLSR_RESULT_INVALID_STATE;
+ }
+ if (axisObject->immediateStopPending != 0U)
+ {
+ return PLSR_RESULT_OK;
+ }
+
+ PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_IMMEDIATE);
+ axisObject->pendingTerminal = PLSR_STATE_STOPPED;
+ if ((axisObject->state == PLSR_STATE_WAIT)
+ || (axisObject->state == PLSR_STATE_PAUSED))
+ {
+ return PlsrStateTransition(axis,
+ PLSR_STATE_STOPPED,
+ PLSR_TRANSITION_STOP);
+ }
+
+ axisObject->immediateStopPending = 1U;
+ return PLSR_RESULT_OK;
+}
+
+static PLSR_RESULT PlsrStopDecel(uint8_t axis)
+{
+ PLSR_AXIS *axisObject = &PlsrAxes[axis];
+
+ if ((axisObject->state == PLSR_STATE_IDLE)
+ || (axisObject->state == PLSR_STATE_COMPLETED)
+ || (axisObject->state == PLSR_STATE_STOPPED))
+ {
+ return PLSR_RESULT_OK;
+ }
+ if (axisObject->state == PLSR_STATE_ERROR)
+ {
+ return PLSR_RESULT_INVALID_STATE;
+ }
+ if (axisObject->stopReason >= PLSR_STOP_REASON_STOP_IMMEDIATE)
+ {
+ return PLSR_RESULT_OK;
+ }
+
+ PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_DECEL);
+ axisObject->pendingTerminal = PLSR_STATE_STOPPED;
+ if ((axisObject->state == PLSR_STATE_WAIT)
+ || (axisObject->state == PLSR_STATE_PAUSED))
+ {
+ return PlsrStateTransition(axis,
+ PLSR_STATE_STOPPED,
+ PLSR_TRANSITION_STOP);
+ }
+ if (axisObject->state == PLSR_STATE_DECEL)
+ {
+ return PLSR_RESULT_OK;
+ }
+ return PlsrStateTransition(axis,
+ PLSR_STATE_DECEL,
+ PLSR_TRANSITION_DECEL_REQUEST);
+}
+
+static PLSR_RESULT PlsrPause(uint8_t axis)
+{
+ PLSR_AXIS *axisObject = &PlsrAxes[axis];
+
+ if (axisObject->state == PLSR_STATE_PAUSED)
+ {
+ return PLSR_RESULT_OK;
+ }
+ if (axisObject->stopReason >= PLSR_STOP_REASON_STOP_DECEL)
+ {
+ return PLSR_RESULT_BUSY;
+ }
+ if (axisObject->state == PLSR_STATE_WAIT)
+ {
+ PlsrSetStopReason(axisObject, PLSR_STOP_REASON_PAUSE);
+ return PlsrStateTransition(axis,
+ PLSR_STATE_PAUSED,
+ PLSR_TRANSITION_DECEL_COMPLETE);
+ }
+ if ((axisObject->state != PLSR_STATE_ACCEL)
+ && (axisObject->state != PLSR_STATE_RUN)
+ && (axisObject->state != PLSR_STATE_DECEL))
+ {
+ return PLSR_RESULT_INVALID_STATE;
+ }
+
+ PlsrSetStopReason(axisObject, PLSR_STOP_REASON_PAUSE);
+ axisObject->pendingTerminal = PLSR_STATE_PAUSED;
+ if (axisObject->state == PLSR_STATE_DECEL)
+ {
+ return PLSR_RESULT_OK;
+ }
+ return PlsrStateTransition(axis,
+ PLSR_STATE_DECEL,
+ PLSR_TRANSITION_DECEL_REQUEST);
+}
+
+static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot)
+{
+ PLSR_AXIS *axisObject = &PlsrAxes[slot->command.axis];
+ PLC_DEVICE_RESULT deviceResult;
+ PLSR_RESULT result;
+
+ switch (slot->command.opcode)
+ {
+ case PLSR_CMD_START:
+ result = (slot->hasStart != 0U)
+ ? PlsrStartAxis(axisObject, &slot->start)
+ : PLSR_RESULT_INVALID_ARGUMENT;
+ break;
+
+ case PLSR_CMD_STOP_DECEL:
+ result = PlsrStopDecel(slot->command.axis);
+ break;
+
+ case PLSR_CMD_STOP_IMMEDIATE:
+ result = PlsrStopImmediate(slot->command.axis);
+ break;
+
+ case PLSR_CMD_PAUSE:
+ result = PlsrPause(slot->command.axis);
+ break;
+
+ case PLSR_CMD_RESUME:
+ if (axisObject->state != PLSR_STATE_PAUSED)
+ {
+ result = PLSR_RESULT_INVALID_STATE;
+ }
+ else
+ {
+ axisObject->stopReason = PLSR_STOP_REASON_NONE;
+ axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;
+ result = PlsrStateTransition(slot->command.axis,
+ PLSR_STATE_ACCEL,
+ PLSR_TRANSITION_WAIT_COMPLETE);
+ }
+ break;
+
+ case PLSR_CMD_SET_POSITION:
+ if (PlsrStateIsBusy(axisObject->state) != 0U)
+ {
+ result = PLSR_RESULT_BUSY;
+ }
+ else
+ {
+ axisObject->logicalPosition = slot->command.argument;
+ (void)PlcDevicePublishHsdPair(
+ (uint16_t)(slot->command.axis
+ * PLSR_HSD_RUNTIME_AXIS_COUNT),
+ axisObject->logicalPosition);
+ result = PLSR_RESULT_OK;
+ }
+ break;
+
+ case PLSR_CMD_CLEAR_POSITION:
+ if (PlsrStateIsBusy(axisObject->state) != 0U)
+ {
+ result = PLSR_RESULT_BUSY;
+ }
+ else
+ {
+ axisObject->logicalPosition = 0;
+ (void)PlcDevicePublishHsdPair(
+ (uint16_t)(slot->command.axis
+ * PLSR_HSD_RUNTIME_AXIS_COUNT),
+ 0);
+ result = PLSR_RESULT_OK;
+ }
+ break;
+
+ case PLSR_CMD_CLEAR_TOTAL:
+ if (PlsrStateIsBusy(axisObject->state) != 0U)
+ {
+ result = PLSR_RESULT_BUSY;
+ }
+ else
+ {
+ axisObject->totalPulses = 0;
+ result = PLSR_RESULT_OK;
+ }
+ break;
+
+ case PLSR_CMD_SAVE_CONFIG:
+ if (PlsrAnyAxisBusy() != 0U)
+ {
+ result = PLSR_RESULT_BUSY;
+ }
+ else
+ {
+ deviceResult = PlcDeviceSaveSfd();
+ result = (deviceResult == PLC_DEVICE_OK)
+ ? PLSR_RESULT_OK
+ : PLSR_RESULT_PERSISTENCE_ERROR;
+ }
+ break;
+
+ case PLSR_CMD_LOAD_CONFIG:
+ if (PlsrAnyAxisBusy() != 0U)
+ {
+ result = PLSR_RESULT_BUSY;
+ }
+ else
+ {
+ deviceResult = PlcDeviceLoadSfd();
+ result = (deviceResult == PLC_DEVICE_OK)
+ ? PLSR_RESULT_OK
+ : PLSR_RESULT_PERSISTENCE_ERROR;
+ }
+ break;
+
+ case PLSR_CMD_RESET_ERROR:
+ if (axisObject->state != PLSR_STATE_ERROR)
+ {
+ result = PLSR_RESULT_INVALID_STATE;
+ }
+ else
+ {
+ axisObject->error = PLSR_ERROR_NONE;
+ axisObject->stopReason = PLSR_STOP_REASON_NONE;
+ axisObject->done = 0U;
+ result = PlsrStateTransition(slot->command.axis,
+ PLSR_STATE_IDLE,
+ PLSR_TRANSITION_RESET_ERROR);
+ }
+ break;
+
+ case PLSR_CMD_SELF_TEST:
+ result = ((PlsrAnyAxisBusy() == 0U)
+ && (PlsrResourceCheckInvariant() != 0U))
+ ? PLSR_RESULT_OK
+ : PLSR_RESULT_BUSY;
+ break;
+
+ default:
+ result = PLSR_RESULT_INVALID_ARGUMENT;
+ break;
+ }
+
+ axisObject->lastCommandSequence = slot->command.sequence;
+ axisObject->lastCommandResult = result;
+ axisObject->hasLastCommand = 1U;
+ PlsrPublishAxis(slot->command.axis);
+ return result;
+}
+
+static void PlsrProcessCriticalEvents(uint8_t axis, uint32_t events)
+{
+ PLSR_AXIS *axisObject = &PlsrAxes[axis];
+
+ if ((events & PLSR_EVENT_SOFTWARE_EMERGENCY) != 0UL)
+ {
+ if (PlsrStateIsBusy(axisObject->state) != 0U)
+ {
+ PlsrSetStopReason(axisObject,
+ PLSR_STOP_REASON_SOFTWARE_EMERGENCY);
+ axisObject->done = 0U;
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_STOPPED,
+ PLSR_TRANSITION_STOP);
+ }
+ return;
+ }
+
+ if ((events & PLSR_EVENT_LIMIT_POSITIVE) != 0UL)
+ {
+ if ((axisObject->directionPositive != 0U)
+ && (PlsrStateIsBusy(axisObject->state) != 0U))
+ {
+ axisObject->error = PLSR_ERROR_LIMIT_POSITIVE;
+ PlsrSetStopReason(axisObject,
+ PLSR_STOP_REASON_LIMIT_POSITIVE);
+ axisObject->pendingTerminal = PLSR_STATE_STOPPED;
+ if ((axisObject->state == PLSR_STATE_WAIT)
+ || (axisObject->state == PLSR_STATE_PAUSED))
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_STOPPED,
+ PLSR_TRANSITION_STOP);
+ }
+ else if (axisObject->state != PLSR_STATE_DECEL)
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_DECEL,
+ PLSR_TRANSITION_DECEL_REQUEST);
+ }
+ }
+ return;
+ }
+
+ if ((events & PLSR_EVENT_LIMIT_NEGATIVE) != 0UL)
+ {
+ if ((axisObject->directionPositive == 0U)
+ && (PlsrStateIsBusy(axisObject->state) != 0U))
+ {
+ axisObject->error = PLSR_ERROR_LIMIT_NEGATIVE;
+ PlsrSetStopReason(axisObject,
+ PLSR_STOP_REASON_LIMIT_NEGATIVE);
+ axisObject->pendingTerminal = PLSR_STATE_STOPPED;
+ if ((axisObject->state == PLSR_STATE_WAIT)
+ || (axisObject->state == PLSR_STATE_PAUSED))
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_STOPPED,
+ PLSR_TRANSITION_STOP);
+ }
+ else if (axisObject->state != PLSR_STATE_DECEL)
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_DECEL,
+ PLSR_TRANSITION_DECEL_REQUEST);
+ }
+ }
+ return;
+ }
+
+ if ((events & (PLSR_EVENT_TIMER_FAULT | PLSR_EVENT_COUNTER_FAULT)) != 0UL)
+ {
+ axisObject->error = ((events & PLSR_EVENT_TIMER_FAULT) != 0UL)
+ ? PLSR_ERROR_TIMER_FAULT
+ : PLSR_ERROR_COUNTER_FAULT;
+ PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT);
+ axisObject->done = 0U;
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_ERROR,
+ PLSR_TRANSITION_FAULT);
+ }
+}
+
+static void PlsrProcessNormalEvents(uint8_t axis, uint32_t events)
+{
+ PLSR_AXIS *axisObject = &PlsrAxes[axis];
+
+ if (((events & PLSR_EVENT_STOP_IMMEDIATE_DONE) != 0UL)
+ && (axisObject->immediateStopPending != 0U))
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_STOPPED,
+ PLSR_TRANSITION_STOP);
+ return;
+ }
+
+ if (((events & PLSR_EVENT_ACCEL_COMPLETE) != 0UL)
+ && (axisObject->state == PLSR_STATE_ACCEL)
+ && (axisObject->immediateStopPending == 0U))
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_RUN,
+ PLSR_TRANSITION_ACCEL_COMPLETE);
+ }
+
+ if (((events & PLSR_EVENT_DECEL_COMPLETE) != 0UL)
+ && (axisObject->state == PLSR_STATE_DECEL))
+ {
+ if (axisObject->pendingTerminal == PLSR_STATE_PAUSED)
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_PAUSED,
+ PLSR_TRANSITION_DECEL_COMPLETE);
+ }
+ else if (axisObject->pendingTerminal == PLSR_STATE_STOPPED)
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_STOPPED,
+ PLSR_TRANSITION_DECEL_COMPLETE);
+ }
+ else
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_RUN,
+ PLSR_TRANSITION_DECEL_COMPLETE);
+ }
+ }
+
+ if (((events & PLSR_EVENT_WAIT_BEGIN) != 0UL)
+ && ((axisObject->state == PLSR_STATE_ACCEL)
+ || (axisObject->state == PLSR_STATE_RUN)))
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_WAIT,
+ PLSR_TRANSITION_WAIT_BEGIN);
+ }
+
+ if (((events & PLSR_EVENT_WAIT_COMPLETE) != 0UL)
+ && (axisObject->state == PLSR_STATE_WAIT))
+ {
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_ACCEL,
+ PLSR_TRANSITION_WAIT_COMPLETE);
+ }
+
+ if (((events & PLSR_EVENT_JOB_COMPLETE) != 0UL)
+ && (PlsrStateIsBusy(axisObject->state) != 0U)
+ && (axisObject->pendingTerminal == PLSR_STATE_UNINITIALIZED)
+ && (axisObject->immediateStopPending == 0U))
+ {
+ axisObject->stopReason = PLSR_STOP_REASON_NORMAL_COMPLETE;
+ (void)PlsrStateTransition(axis,
+ PLSR_STATE_COMPLETED,
+ PLSR_TRANSITION_JOB_COMPLETE);
+ }
+}
+
+PLSR_RESULT PlsrInit(void)
+{
+ uint8_t axis;
+
+ (void)memset(PlsrAxes, 0, sizeof(PlsrAxes));
+ (void)memset(PlsrCommandQueue, 0, sizeof(PlsrCommandQueue));
+ PlsrNextTicket = 0UL;
+ PlsrResourceInit();
+ PlsrInitialized = 1U;
+
+ for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
+ {
+ PlsrAxes[axis].state = PLSR_STATE_UNINITIALIZED;
+ PlsrAxes[axis].pendingTerminal = PLSR_STATE_UNINITIALIZED;
+ PlsrAxes[axis].lease.directionPoint = PLSR_DIRECTION_POINT_NONE;
+ if (PlsrStateTransition(axis,
+ PLSR_STATE_IDLE,
+ PLSR_TRANSITION_INITIALIZED)
+ != PLSR_RESULT_OK)
+ {
+ return PLSR_RESULT_INTERNAL_ERROR;
+ }
+ }
+ return PLSR_RESULT_OK;
+}
+
+void PlsrProcess(void)
+{
+ PLSR_COMMAND_SLOT slot;
+ uint32_t events;
+ uint8_t processedCommands = 0U;
+ uint8_t axis;
+
+ if (PlsrInitialized == 0U)
+ {
+ return;
+ }
+
+ for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
+ {
+ events = PlsrTakeEvents(axis, PLSR_EVENT_CRITICAL_MASK);
+ if (events != 0UL)
+ {
+ PlsrProcessCriticalEvents(axis, events);
+ }
+ }
+
+ while ((processedCommands < PLSR_COMMAND_QUEUE_DEPTH)
+ && (PlsrPopHighestPriorityCommand(&slot) != 0U))
+ {
+ (void)PlsrExecuteCommand(&slot);
+ processedCommands++;
+ }
+
+ for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
+ {
+ events = PlsrTakeEvents(axis,
+ PLSR_EVENT_ALL_MASK
+ & ~PLSR_EVENT_CRITICAL_MASK);
+ if (events != 0UL)
+ {
+ PlsrProcessNormalEvents(axis, events);
+ }
+ }
+}
+
+void PlsrTask(void *argument)
+{
+ (void)argument;
+#ifdef PLSR_HOST_TEST
+ PlsrProcess();
+#else
+ while (1)
+ {
+ PlsrProcess();
+ OSTimeDly(1U);
+ }
+#endif
+}
+
+PLSR_RESULT PlsrGetStatus(uint8_t axis, PLSR_STATUS *status)
+{
+ PLSR_AXIS *axisObject;
+ uint32_t interruptState;
+
+ if (axis >= PLSR_AXIS_COUNT)
+ {
+ return PLSR_RESULT_INVALID_AXIS;
+ }
+ if (status == NULL)
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+
+ interruptState = PlsrCoreEnterCritical();
+ axisObject = &PlsrAxes[axis];
+ status->state = axisObject->state;
+ status->outputMode = axisObject->outputMode;
+ status->error = axisObject->error;
+ status->stopReason = axisObject->stopReason;
+ status->lastCommandResult = axisObject->lastCommandResult;
+ status->lastCommandSequence = axisObject->lastCommandSequence;
+ status->illegalTransitionCount = axisObject->illegalTransitionCount;
+ status->pendingEvents = axisObject->pendingEvents;
+ status->logicalPosition = axisObject->logicalPosition;
+ status->totalPulses = axisObject->totalPulses;
+ status->busy = PlsrStateIsBusy(axisObject->state);
+ status->pulseActive = PlsrStateIsPulseActive(axisObject->state);
+ status->done = axisObject->done;
+ status->wait = (axisObject->state == PLSR_STATE_WAIT) ? 1U : 0U;
+ status->directionPositive = axisObject->directionPositive;
+ status->highResourceMask = axisObject->lease.highMask;
+ status->directionPoint = (axisObject->lease.valid != 0U)
+ ? axisObject->lease.directionPoint
+ : PLSR_DIRECTION_POINT_NONE;
+ PlsrCoreExitCritical(interruptState);
+ return PLSR_RESULT_OK;
+}
diff --git a/PLSR/Src/plsr_resource.c b/PLSR/Src/plsr_resource.c
new file mode 100644
index 0000000..a7b09e8
--- /dev/null
+++ b/PLSR/Src/plsr_resource.c
@@ -0,0 +1,209 @@
+#include "plsr_resource.h"
+#include "plsr_address_map.h"
+#include
+
+#ifndef PLSR_HOST_TEST
+#include "stm32f4xx.h"
+#endif
+
+#define PLSR_HIGH_OUTPUT_MASK (0x0000000FUL)
+#define PLSR_VALID_OUTPUT_MASK (0x0013FCFFUL)
+#define PLSR_OUTPUT_POINT_COUNT (21U)
+#define PLSR_RESOURCE_OWNER_NONE (0xFFU)
+
+static uint32_t PlsrUsedOutputMask;
+static uint8_t PlsrUsedHighMask;
+static uint8_t PlsrOutputOwners[PLSR_OUTPUT_POINT_COUNT];
+
+static uint32_t PlsrResourceEnterCritical(void)
+{
+#ifdef PLSR_HOST_TEST
+ return 0UL;
+#else
+ uint32_t interruptState = __get_PRIMASK();
+ __disable_irq();
+ __DMB();
+ return interruptState;
+#endif
+}
+
+static void PlsrResourceExitCritical(uint32_t interruptState)
+{
+#ifdef PLSR_HOST_TEST
+ (void)interruptState;
+#else
+ __DMB();
+ if (interruptState == 0UL)
+ {
+ __enable_irq();
+ }
+#endif
+}
+
+static uint8_t PlsrResourceOutputPointIsValid(uint8_t point)
+{
+ if (point >= PLSR_OUTPUT_POINT_COUNT)
+ {
+ return 0U;
+ }
+ return ((PLSR_VALID_OUTPUT_MASK & (1UL << point)) != 0UL) ? 1U : 0U;
+}
+
+void PlsrResourceInit(void)
+{
+ uint32_t interruptState = PlsrResourceEnterCritical();
+
+ PlsrUsedOutputMask = 0UL;
+ PlsrUsedHighMask = 0U;
+ (void)memset(PlsrOutputOwners,
+ PLSR_RESOURCE_OWNER_NONE,
+ sizeof(PlsrOutputOwners));
+ PlsrResourceExitCritical(interruptState);
+}
+
+PLSR_RESULT PlsrResourceReserve(const PLSR_RESOURCE_REQUEST *request,
+ PLSR_RESOURCE_LEASE *lease)
+{
+ uint32_t requestedOutputMask;
+ uint32_t interruptState;
+ uint8_t requestedHighMask;
+ uint8_t point;
+
+ if ((request == NULL) || (lease == NULL))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if ((request->ownerAxis >= PLSR_AXIS_COUNT)
+ || (request->dAxis >= PLSR_AXIS_COUNT)
+ || (request->ownerAxis != request->dAxis))
+ {
+ return PLSR_RESULT_INVALID_AXIS;
+ }
+ if (lease->valid != 0U)
+ {
+ return PLSR_RESULT_BUSY;
+ }
+
+ if (request->outputMode == PLSR_OUTPUT_PULSE_DIR)
+ {
+ if ((PlsrResourceOutputPointIsValid(request->directionPoint) == 0U)
+ || (request->directionPoint == request->dAxis))
+ {
+ return PLSR_RESULT_INVALID_RESOURCE;
+ }
+ requestedHighMask = (uint8_t)(1U << request->dAxis);
+ requestedOutputMask = (uint32_t)requestedHighMask
+ | (1UL << request->directionPoint);
+ }
+ else if ((request->outputMode == PLSR_OUTPUT_AB)
+ || (request->outputMode == PLSR_OUTPUT_CW_CCW))
+ {
+ if ((request->dAxis != 0U) && (request->dAxis != 2U))
+ {
+ return PLSR_RESULT_INVALID_RESOURCE;
+ }
+ requestedHighMask = (uint8_t)(0x03U << request->dAxis);
+ requestedOutputMask = (uint32_t)requestedHighMask;
+ }
+ else
+ {
+ return PLSR_RESULT_INVALID_RESOURCE;
+ }
+
+ interruptState = PlsrResourceEnterCritical();
+ if ((PlsrUsedOutputMask & requestedOutputMask) != 0UL)
+ {
+ PlsrResourceExitCritical(interruptState);
+ return PLSR_RESULT_RESOURCE_CONFLICT;
+ }
+
+ PlsrUsedOutputMask |= requestedOutputMask;
+ PlsrUsedHighMask |= requestedHighMask;
+ for (point = 0U; point < PLSR_OUTPUT_POINT_COUNT; point++)
+ {
+ if ((requestedOutputMask & (1UL << point)) != 0UL)
+ {
+ PlsrOutputOwners[point] = request->ownerAxis;
+ }
+ }
+ PlsrResourceExitCritical(interruptState);
+
+ lease->outputMask = requestedOutputMask;
+ lease->highMask = requestedHighMask;
+ lease->ownerAxis = request->ownerAxis;
+ lease->directionPoint = (request->outputMode == PLSR_OUTPUT_PULSE_DIR)
+ ? request->directionPoint
+ : PLSR_DIRECTION_POINT_NONE;
+ lease->outputMode = request->outputMode;
+ lease->valid = 1U;
+ return PLSR_RESULT_OK;
+}
+
+void PlsrResourceRelease(PLSR_RESOURCE_LEASE *lease)
+{
+ uint32_t releaseMask = 0UL;
+ uint32_t interruptState;
+ uint8_t point;
+
+ if ((lease == NULL) || (lease->valid == 0U))
+ {
+ return;
+ }
+
+ interruptState = PlsrResourceEnterCritical();
+ for (point = 0U; point < PLSR_OUTPUT_POINT_COUNT; point++)
+ {
+ if (((lease->outputMask & (1UL << point)) != 0UL)
+ && (PlsrOutputOwners[point] == lease->ownerAxis))
+ {
+ releaseMask |= (1UL << point);
+ PlsrOutputOwners[point] = PLSR_RESOURCE_OWNER_NONE;
+ }
+ }
+ PlsrUsedOutputMask &= ~releaseMask;
+ PlsrUsedHighMask = (uint8_t)(PlsrUsedOutputMask & PLSR_HIGH_OUTPUT_MASK);
+ PlsrResourceExitCritical(interruptState);
+
+ (void)memset(lease, 0, sizeof(*lease));
+ lease->directionPoint = PLSR_DIRECTION_POINT_NONE;
+}
+
+void PlsrResourceGetStatus(PLSR_RESOURCE_STATUS *status)
+{
+ uint32_t interruptState;
+
+ if (status == NULL)
+ {
+ return;
+ }
+
+ interruptState = PlsrResourceEnterCritical();
+ status->outputMask = PlsrUsedOutputMask;
+ status->highMask = PlsrUsedHighMask;
+ PlsrResourceExitCritical(interruptState);
+}
+
+uint8_t PlsrResourceCheckInvariant(void)
+{
+ uint32_t interruptState;
+ uint8_t valid = 1U;
+ uint8_t point;
+
+ interruptState = PlsrResourceEnterCritical();
+ if (PlsrUsedHighMask
+ != (uint8_t)(PlsrUsedOutputMask & PLSR_HIGH_OUTPUT_MASK))
+ {
+ valid = 0U;
+ }
+
+ for (point = 0U; point < PLSR_OUTPUT_POINT_COUNT; point++)
+ {
+ if (((PlsrUsedOutputMask & (1UL << point)) == 0UL)
+ != (PlsrOutputOwners[point] == PLSR_RESOURCE_OWNER_NONE))
+ {
+ valid = 0U;
+ }
+ }
+ PlsrResourceExitCritical(interruptState);
+ return valid;
+}
diff --git a/PLSR/Test/run_host_tests.ps1 b/PLSR/Test/run_host_tests.ps1
index 9a11e9e..3432abb 100644
--- a/PLSR/Test/run_host_tests.ps1
+++ b/PLSR/Test/run_host_tests.ps1
@@ -7,45 +7,64 @@ if ($null -eq $compiler)
throw 'gcc.exe was not found; PLSR host tests cannot run.'
}
-$outputPath = Join-Path $workspacePath 'tmp\test_plc_device.exe'
-$outputDirectory = Split-Path -Parent $outputPath
+$outputDirectory = Join-Path $workspacePath 'tmp'
if (-not (Test-Path -LiteralPath $outputDirectory))
{
New-Item -ItemType Directory -Path $outputDirectory | Out-Null
}
-$arguments = @(
- '-std=c11',
- '-Wall',
- '-Wextra',
- '-Werror',
- '-DPLSR_HOST_TEST',
- "-I$workspacePath\PLSR\Inc",
- "$workspacePath\PLSR\Src\plc_device.c",
- "$workspacePath\PLSR\Src\plsr_persistence.c",
- "$workspacePath\PLSR\Test\test_plc_device.c",
- '-o',
- $outputPath
+$tests = @(
+ @{
+ Name = 'test_plc_device'
+ Sources = @(
+ "$workspacePath\PLSR\Src\plc_device.c"
+ "$workspacePath\PLSR\Src\plsr_persistence.c"
+ "$workspacePath\PLSR\Test\test_plc_device.c"
+ )
+ },
+ @{
+ Name = 'test_plsr_core'
+ Sources = @(
+ "$workspacePath\PLSR\Src\plc_device.c"
+ "$workspacePath\PLSR\Src\plsr_persistence.c"
+ "$workspacePath\PLSR\Src\plsr_resource.c"
+ "$workspacePath\PLSR\Src\plsr_core.c"
+ "$workspacePath\PLSR\Test\test_plsr_core.c"
+ )
+ }
)
-try
+foreach ($test in $tests)
{
- & $compiler.Source @arguments
- if ($LASTEXITCODE -ne 0)
- {
- throw "PLSR host-test compilation failed with exit code $LASTEXITCODE."
- }
+ $outputPath = Join-Path $outputDirectory "$($test.Name).exe"
+ $arguments = @(
+ '-std=c11'
+ '-Wall'
+ '-Wextra'
+ '-Werror'
+ '-DPLSR_HOST_TEST'
+ "-I$workspacePath\PLSR\Inc"
+ ) + $test.Sources + @('-o', $outputPath)
- & $outputPath
- if ($LASTEXITCODE -ne 0)
+ try
{
- throw "PLSR host tests failed with exit code $LASTEXITCODE."
+ & $compiler.Source @arguments
+ if ($LASTEXITCODE -ne 0)
+ {
+ throw "$($test.Name) compilation failed with exit code $LASTEXITCODE."
+ }
+
+ & $outputPath
+ if ($LASTEXITCODE -ne 0)
+ {
+ throw "$($test.Name) failed with exit code $LASTEXITCODE."
+ }
}
-}
-finally
-{
- if (Test-Path -LiteralPath $outputPath)
+ finally
{
- Remove-Item -LiteralPath $outputPath -Force
+ if (Test-Path -LiteralPath $outputPath)
+ {
+ Remove-Item -LiteralPath $outputPath -Force
+ }
}
}
diff --git a/PLSR/Test/test_plsr_core.c b/PLSR/Test/test_plsr_core.c
new file mode 100644
index 0000000..dc7f8a5
--- /dev/null
+++ b/PLSR/Test/test_plsr_core.c
@@ -0,0 +1,356 @@
+#include "plc_device.h"
+#include "plsr_core.h"
+#include "plsr_persistence.h"
+#include "plsr_resource.h"
+#include
+#include
+#include
+
+static unsigned int TestCount;
+
+#define TEST_CHECK(condition) \
+ do \
+ { \
+ TestCount++; \
+ if (!(condition)) \
+ { \
+ (void)fprintf(stderr, "FAIL line %d: %s\n", __LINE__, #condition); \
+ exit(EXIT_FAILURE); \
+ } \
+ } while (0)
+
+static void TestReset(void)
+{
+ PlsrPersistenceTestResetStorage();
+ TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+ TEST_CHECK(PlsrInit() == PLSR_RESULT_OK);
+}
+
+static PLSR_RESULT TestPostCommand(uint32_t sequence,
+ uint8_t axis,
+ PLSR_COMMAND_OPCODE opcode,
+ int64_t argument)
+{
+ PLSR_COMMAND command;
+
+ command.sequence = sequence;
+ command.axis = axis;
+ command.opcode = opcode;
+ command.argument = argument;
+ return PlsrPostCommand(&command);
+}
+
+static PLSR_RESULT TestPostStart(uint32_t sequence,
+ uint8_t axis,
+ PLSR_OUTPUT_MODE mode,
+ uint8_t directionPoint,
+ uint8_t directionPositive)
+{
+ PLSR_START_REQUEST request;
+
+ request.sequence = sequence;
+ request.axis = axis;
+ request.outputMode = mode;
+ request.directionPoint = directionPoint;
+ request.directionPositive = directionPositive;
+ return PlsrPostStart(&request);
+}
+
+static PLSR_STATUS TestGetStatus(uint8_t axis)
+{
+ PLSR_STATUS status;
+
+ (void)memset(&status, 0, sizeof(status));
+ TEST_CHECK(PlsrGetStatus(axis, &status) == PLSR_RESULT_OK);
+ return status;
+}
+
+static void TestResourceManager(void)
+{
+ PLSR_RESOURCE_REQUEST request;
+ PLSR_RESOURCE_LEASE first;
+ PLSR_RESOURCE_LEASE second;
+ PLSR_RESOURCE_STATUS status;
+
+ (void)memset(&first, 0, sizeof(first));
+ (void)memset(&second, 0, sizeof(second));
+ PlsrResourceInit();
+
+ request.ownerAxis = 0U;
+ request.outputMode = PLSR_OUTPUT_PULSE_DIR;
+ request.dAxis = 0U;
+ request.directionPoint = 4U;
+ TEST_CHECK(PlsrResourceReserve(&request, &first) == PLSR_RESULT_OK);
+ TEST_CHECK(first.highMask == 0x01U);
+ TEST_CHECK(first.outputMask == ((1UL << 0U) | (1UL << 4U)));
+
+ request.ownerAxis = 1U;
+ request.dAxis = 1U;
+ request.directionPoint = 4U;
+ TEST_CHECK(PlsrResourceReserve(&request, &second)
+ == PLSR_RESULT_RESOURCE_CONFLICT);
+ TEST_CHECK(second.valid == 0U);
+
+ request.directionPoint = 8U;
+ TEST_CHECK(PlsrResourceReserve(&request, &second)
+ == PLSR_RESULT_INVALID_RESOURCE);
+ request.directionPoint = 1U;
+ TEST_CHECK(PlsrResourceReserve(&request, &second)
+ == PLSR_RESULT_INVALID_RESOURCE);
+
+ PlsrResourceRelease(&first);
+ PlsrResourceRelease(&first);
+ request.ownerAxis = 0U;
+ request.outputMode = PLSR_OUTPUT_AB;
+ request.dAxis = 0U;
+ request.directionPoint = PLSR_DIRECTION_POINT_NONE;
+ TEST_CHECK(PlsrResourceReserve(&request, &first) == PLSR_RESULT_OK);
+ TEST_CHECK(first.highMask == 0x03U);
+
+ request.ownerAxis = 2U;
+ request.outputMode = PLSR_OUTPUT_PULSE_DIR;
+ request.dAxis = 2U;
+ request.directionPoint = 4U;
+ TEST_CHECK(PlsrResourceReserve(&request, &second) == PLSR_RESULT_OK);
+ TEST_CHECK(second.highMask == 0x04U);
+
+ request.ownerAxis = 1U;
+ request.outputMode = PLSR_OUTPUT_CW_CCW;
+ request.dAxis = 1U;
+ request.directionPoint = PLSR_DIRECTION_POINT_NONE;
+ PlsrResourceRelease(&second);
+ TEST_CHECK(PlsrResourceReserve(&request, &second)
+ == PLSR_RESULT_INVALID_RESOURCE);
+
+ PlsrResourceGetStatus(&status);
+ TEST_CHECK(status.highMask == 0x03U);
+ TEST_CHECK(PlsrResourceCheckInvariant() != 0U);
+ PlsrResourceRelease(&first);
+ PlsrResourceGetStatus(&status);
+ TEST_CHECK(status.outputMask == 0UL);
+}
+
+static void TestStateMachineAndCommands(void)
+{
+ PLSR_RESOURCE_STATUS resources;
+ PLSR_STATUS status;
+
+ TestReset();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_IDLE);
+ TEST_CHECK(status.directionPoint == PLSR_DIRECTION_POINT_NONE);
+
+ TEST_CHECK(TestPostStart(1U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_ACCEL);
+ TEST_CHECK(status.busy != 0U);
+ TEST_CHECK(status.pulseActive != 0U);
+ TEST_CHECK(status.done == 0U);
+ TEST_CHECK(status.highResourceMask == 0x01U);
+ TEST_CHECK(status.directionPoint == 4U);
+ TEST_CHECK(status.lastCommandSequence == 1U);
+ TEST_CHECK(status.lastCommandResult == PLSR_RESULT_OK);
+ TEST_CHECK(TestPostStart(1U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
+ == PLSR_RESULT_OK);
+
+ TEST_CHECK(TestPostStart(1U, 1U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus(1U);
+ TEST_CHECK(status.state == PLSR_STATE_IDLE);
+ TEST_CHECK(status.lastCommandResult == PLSR_RESULT_RESOURCE_CONFLICT);
+ TEST_CHECK(status.error == PLSR_ERROR_RESOURCE_CONFLICT);
+
+ TEST_CHECK(PlsrPostEvent(0U, PLSR_EVENT_ACCEL_COMPLETE)
+ == PLSR_RESULT_OK);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(0U).state == PLSR_STATE_RUN);
+
+ TEST_CHECK(TestPostCommand(2U, 0U, PLSR_CMD_SET_POSITION, 500)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(0U).lastCommandResult == PLSR_RESULT_BUSY);
+
+ TEST_CHECK(TestPostCommand(3U, 0U, PLSR_CMD_PAUSE, 0)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(0U).state == PLSR_STATE_DECEL);
+ TEST_CHECK(PlsrPostEvent(0U, PLSR_EVENT_DECEL_COMPLETE)
+ == PLSR_RESULT_OK);
+ PlsrProcess();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_PAUSED);
+ TEST_CHECK(status.busy != 0U);
+ TEST_CHECK(status.pulseActive == 0U);
+ TEST_CHECK(status.highResourceMask == 0x01U);
+
+ TEST_CHECK(TestPostCommand(4U, 0U, PLSR_CMD_RESUME, 0)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(0U).state == PLSR_STATE_ACCEL);
+
+ TEST_CHECK(TestPostCommand(5U, 0U, PLSR_CMD_STOP_IMMEDIATE, 0)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_ACCEL);
+ TEST_CHECK(status.stopReason == PLSR_STOP_REASON_STOP_IMMEDIATE);
+ TEST_CHECK(status.highResourceMask == 0x01U);
+ TEST_CHECK(TestPostCommand(5U, 0U, PLSR_CMD_STOP_IMMEDIATE, 0)
+ == PLSR_RESULT_OK);
+
+ TEST_CHECK(PlsrPostEvent(0U, PLSR_EVENT_STOP_IMMEDIATE_DONE)
+ == PLSR_RESULT_OK);
+ PlsrProcess();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_STOPPED);
+ TEST_CHECK(status.busy == 0U);
+ TEST_CHECK(status.done == 0U);
+ TEST_CHECK(status.highResourceMask == 0U);
+ PlsrResourceGetStatus(&resources);
+ TEST_CHECK(resources.outputMask == 0UL);
+
+ TEST_CHECK(TestPostStart(6U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 0U)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(PlsrPostEvent(0U, PLSR_EVENT_JOB_COMPLETE)
+ == PLSR_RESULT_OK);
+ PlsrProcess();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_COMPLETED);
+ TEST_CHECK(status.done != 0U);
+ TEST_CHECK(status.stopReason == PLSR_STOP_REASON_NORMAL_COMPLETE);
+
+ TEST_CHECK(TestPostStart(7U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(0U).done == 0U);
+ TEST_CHECK(PlsrPostEvent(0U,
+ PLSR_EVENT_JOB_COMPLETE
+ | PLSR_EVENT_SOFTWARE_EMERGENCY)
+ == PLSR_RESULT_OK);
+ PlsrProcess();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_STOPPED);
+ TEST_CHECK(status.done == 0U);
+ TEST_CHECK(status.stopReason == PLSR_STOP_REASON_SOFTWARE_EMERGENCY);
+
+ TEST_CHECK(PlsrStateTransition(0U,
+ PLSR_STATE_RUN,
+ PLSR_TRANSITION_ACCEL_COMPLETE)
+ == PLSR_RESULT_INVALID_STATE);
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_ERROR);
+ TEST_CHECK(status.error == PLSR_ERROR_ILLEGAL_TRANSITION);
+ TEST_CHECK(status.illegalTransitionCount == 1UL);
+ TEST_CHECK(TestPostCommand(8U, 0U, PLSR_CMD_RESET_ERROR, 0)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(0U).state == PLSR_STATE_IDLE);
+}
+
+static void TestLimitWaitAndPairs(void)
+{
+ PLSR_STATUS status;
+
+ TestReset();
+ TEST_CHECK(TestPostStart(10U, 0U, PLSR_OUTPUT_PULSE_DIR, 4U, 1U)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(PlsrPostEvent(0U,
+ PLSR_EVENT_LIMIT_POSITIVE
+ | PLSR_EVENT_DECEL_COMPLETE)
+ == PLSR_RESULT_OK);
+ PlsrProcess();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.state == PLSR_STATE_STOPPED);
+ TEST_CHECK(status.stopReason == PLSR_STOP_REASON_LIMIT_POSITIVE);
+ TEST_CHECK(status.error == PLSR_ERROR_LIMIT_POSITIVE);
+
+ TEST_CHECK(TestPostStart(11U,
+ 2U,
+ PLSR_OUTPUT_AB,
+ PLSR_DIRECTION_POINT_NONE,
+ 0U)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus(2U);
+ TEST_CHECK(status.highResourceMask == 0x0CU);
+ TEST_CHECK(status.outputMode == PLSR_OUTPUT_AB);
+ TEST_CHECK(PlsrPostEvent(2U, PLSR_EVENT_WAIT_BEGIN) == PLSR_RESULT_OK);
+ PlsrProcess();
+ status = TestGetStatus(2U);
+ TEST_CHECK(status.state == PLSR_STATE_WAIT);
+ TEST_CHECK(status.busy != 0U);
+ TEST_CHECK(status.pulseActive == 0U);
+ TEST_CHECK(status.highResourceMask == 0x0CU);
+
+ TEST_CHECK(TestPostCommand(12U, 2U, PLSR_CMD_PAUSE, 0)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(2U).state == PLSR_STATE_PAUSED);
+ TEST_CHECK(TestPostCommand(13U, 2U, PLSR_CMD_STOP_DECEL, 0)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus(2U);
+ TEST_CHECK(status.state == PLSR_STATE_STOPPED);
+ TEST_CHECK(status.highResourceMask == 0U);
+
+ TEST_CHECK(TestPostStart(14U,
+ 1U,
+ PLSR_OUTPUT_CW_CCW,
+ PLSR_DIRECTION_POINT_NONE,
+ 1U)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus(1U);
+ TEST_CHECK(status.state == PLSR_STATE_IDLE);
+ TEST_CHECK(status.lastCommandResult == PLSR_RESULT_INVALID_RESOURCE);
+}
+
+static void TestQueueAndInputValidation(void)
+{
+ PLSR_COMMAND command;
+ uint32_t sequence;
+
+ TestReset();
+ TEST_CHECK(PlsrPostEvent(4U, PLSR_EVENT_JOB_COMPLETE)
+ == PLSR_RESULT_INVALID_AXIS);
+ TEST_CHECK(PlsrPostEvent(0U, 0UL) == PLSR_RESULT_INVALID_ARGUMENT);
+ TEST_CHECK(PlsrPostEvent(0U, 0x80000000UL)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ TEST_CHECK(PlsrGetStatus(0U, NULL) == PLSR_RESULT_INVALID_ARGUMENT);
+ TEST_CHECK(PlsrGetStatus(4U, NULL) == PLSR_RESULT_INVALID_AXIS);
+
+ command.axis = 1U;
+ command.opcode = PLSR_CMD_CLEAR_POSITION;
+ command.argument = 0;
+ for (sequence = 100U;
+ sequence < 100U + PLSR_COMMAND_QUEUE_DEPTH;
+ sequence++)
+ {
+ command.sequence = sequence;
+ TEST_CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED);
+ }
+ command.sequence = 1000U;
+ TEST_CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUE_FULL);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(1U).lastCommandResult == PLSR_RESULT_OK);
+ TEST_CHECK(TestPostCommand(2000U, 0U, PLSR_CMD_SELF_TEST, 0)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ TEST_CHECK(TestGetStatus(0U).lastCommandResult == PLSR_RESULT_OK);
+}
+
+int main(void)
+{
+ TestResourceManager();
+ TestStateMachineAndCommands();
+ TestLimitWaitAndPairs();
+ TestQueueAndInputValidation();
+
+ (void)printf("PASS: %u PLSR core checks\n", TestCount);
+ return EXIT_SUCCESS;
+}