diff --git a/Core/Src/main.c b/Core/Src/main.c
index 07b5ff5..16c8ae1 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -202,7 +202,8 @@ int main(void)
/* 上电自测：延时 1s 后由 Q0/Q1 输出三段 AB 正交周期（验证后关闭）。 */
HAL_Delay(1000U);
- (void)PlsrSelfTestQueue();
+ // (void)PlsrSelfTestQueue();
+ (void)PlsrEquivalentSelfTestQueue();
OSStart();
/* USER CODE END 2 */
diff --git a/EWARM/Modbus.ewp b/EWARM/Modbus.ewp
index 870b464..9c07e0e 100644
--- a/EWARM/Modbus.ewp
+++ b/EWARM/Modbus.ewp
@@ -1290,6 +1290,9 @@
$PROJ_DIR$\..\PLSR\Inc\plsr_profile.h
+
+ $PROJ_DIR$\..\PLSR\Inc\plsr_position.h
+
$PROJ_DIR$\..\PLSR\Inc\plsr_hal_f407.h
@@ -1314,6 +1317,9 @@
$PROJ_DIR$\..\PLSR\Src\plsr_profile.c
+
+ $PROJ_DIR$\..\PLSR\Src\plsr_position.c
+
$PROJ_DIR$\..\PLSR\Src\plsr_hal_f407.c
diff --git a/PLSR/Inc/plc_device.h b/PLSR/Inc/plc_device.h
index 13a0485..8bfd422 100644
--- a/PLSR/Inc/plc_device.h
+++ b/PLSR/Inc/plc_device.h
@@ -49,9 +49,13 @@ PLC_DEVICE_RESULT PlcDevicePublishSm(uint8_t axis,
PLC_DEVICE_RESULT PlcDeviceReadSd(uint16_t address, int32_t *value);
PLC_DEVICE_RESULT PlcDeviceWriteSd(uint16_t address, int32_t value);
+PLC_DEVICE_RESULT PlcDeviceReadSdDword(uint16_t lowAddress, int32_t *value);
PLC_DEVICE_RESULT PlcDevicePublishSd(uint8_t axis,
uint8_t item,
int32_t value);
+PLC_DEVICE_RESULT PlcDevicePublishSdDword(uint8_t axis,
+ uint8_t lowItem,
+ int32_t value);
PLC_DEVICE_RESULT PlcDeviceGetEventAddress(uint8_t axis,
uint16_t segmentNumber,
diff --git a/PLSR/Inc/plsr_address_map.h b/PLSR/Inc/plsr_address_map.h
index 5580be3..8d23252 100644
--- a/PLSR/Inc/plsr_address_map.h
+++ b/PLSR/Inc/plsr_address_map.h
@@ -22,6 +22,14 @@ extern "C" {
#define PLSR_SD_AXIS_ITEM_COUNT (12U)
#define PLSR_EVENT_AXIS_ITEM_COUNT (100U)
+#define PLSR_SD_ITEM_SEGMENT (0U)
+#define PLSR_SD_ITEM_SEGMENT_PULSES (2U)
+#define PLSR_SD_ITEM_SEGMENT_EQUIV (4U)
+#define PLSR_SD_ITEM_FREQUENCY (6U)
+#define PLSR_SD_ITEM_SPEED (8U)
+#define PLSR_SD_ITEM_ERROR_CODE (10U)
+#define PLSR_SD_ITEM_ERROR_BLOCK (11U)
+
typedef struct
{
uint16_t hsdRuntimeBase;
diff --git a/PLSR/Inc/plsr_hal_f407.h b/PLSR/Inc/plsr_hal_f407.h
index b01e5ed..86fc4c3 100644
--- a/PLSR/Inc/plsr_hal_f407.h
+++ b/PLSR/Inc/plsr_hal_f407.h
@@ -38,7 +38,9 @@ PLSR_RESULT PlsrHwStopPulse(uint8_t axis);
uint8_t PlsrHwIsPulseActive(uint8_t axis);
PLSR_HW_STATE PlsrHwGetState(uint8_t axis);
uint32_t PlsrHwGetTimerClockHz(uint8_t axis);
+uint32_t PlsrHwGetCurrentFrequencyHz(uint8_t axis);
int64_t PlsrHwGetEmittedPulses(uint8_t axis);
+uint8_t PlsrHwIsAbStartupPriming(uint8_t axis);
/* 每 1ms tick 推进 HAL 状态机（DIR 延时等）。 */
void PlsrHwTick(uint8_t axis);
diff --git a/PLSR/Inc/plsr_job.h b/PLSR/Inc/plsr_job.h
index 685f6a1..379e7f4 100644
--- a/PLSR/Inc/plsr_job.h
+++ b/PLSR/Inc/plsr_job.h
@@ -1,6 +1,7 @@
#ifndef PLSR_JOB_H
#define PLSR_JOB_H
+#include "plsr_position.h"
#include "plsr_types.h"
#include
@@ -137,7 +138,10 @@ typedef struct
PLSR_DATA_REF s0;
PLSR_DATA_REF s1;
PLSR_S2_SNAPSHOT s2;
+ PLSR_EQUIVALENT_CONFIG equivalent;
PLSR_SEGMENT_SNAPSHOT segments[PLSR_MAX_SEGMENTS];
+ uint32_t inputDefaultSpeed;
+ uint32_t inputMaximumSpeed;
uint32_t timerClockHz;
uint32_t pairedTimerClockHz;
uint16_t segmentCount;
diff --git a/PLSR/Inc/plsr_position.h b/PLSR/Inc/plsr_position.h
new file mode 100644
index 0000000..410f8e8
--- /dev/null
+++ b/PLSR/Inc/plsr_position.h
@@ -0,0 +1,56 @@
+#ifndef PLSR_POSITION_H
+#define PLSR_POSITION_H
+
+#include "plsr_types.h"
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct
+{
+ uint32_t pulsesPerRevolution;
+ uint32_t movementPerRevolution;
+ uint8_t unitCode;
+} PLSR_EQUIVALENT_CONFIG;
+
+uint8_t PlsrPositionUnitCodeIsValid(uint8_t unitCode);
+uint8_t PlsrPositionUsesEquivalent(const PLSR_EQUIVALENT_CONFIG *config);
+PLSR_RESULT PlsrPositionValidateEquivalent(
+ const PLSR_EQUIVALENT_CONFIG *config);
+
+/* 带余数的相对距离换算。remainder/newRemainder 的分母恒为
+ * movementPerRevolution，允许负值以保证正反向运动可相互抵消。 */
+PLSR_RESULT PlsrPositionUnitsToPulses(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ int32_t units,
+ int64_t remainder,
+ int64_t *pulses,
+ int64_t *newRemainder);
+
+PLSR_RESULT PlsrPositionAbsoluteUnitsToPulses(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ int32_t units,
+ int64_t *pulses);
+
+PLSR_RESULT PlsrPositionPulsesToUnits(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ int64_t pulses,
+ int64_t *units);
+
+PLSR_RESULT PlsrPositionSpeedToPulseFrequency(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ uint32_t speed,
+ uint32_t *frequencyHz);
+
+PLSR_RESULT PlsrPositionPulseFrequencyToSpeed(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ uint32_t frequencyHz,
+ uint32_t *speed);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PLSR_POSITION_H */
diff --git a/PLSR/Inc/plsr_self_test.h b/PLSR/Inc/plsr_self_test.h
index 2781265..00b2870 100644
--- a/PLSR/Inc/plsr_self_test.h
+++ b/PLSR/Inc/plsr_self_test.h
@@ -11,6 +11,10 @@ extern "C" {
* 用于 P3b-2 上板验证，验证完成后应关闭。 */
PLSR_RESULT PlsrSelfTestQueue(void);
+/* P4-2 当量换算上板自测：Q0=PULSE、Y4=DIR，两段各1工程单位；
+ * 3脉冲/2单位比例下应依次输出1、2个脉冲。 */
+PLSR_RESULT PlsrEquivalentSelfTestQueue(void);
+
#ifdef __cplusplus
}
#endif
diff --git a/PLSR/Inc/plsr_types.h b/PLSR/Inc/plsr_types.h
index 838f038..759da73 100644
--- a/PLSR/Inc/plsr_types.h
+++ b/PLSR/Inc/plsr_types.h
@@ -166,6 +166,7 @@ typedef struct
uint32_t illegalTransitionCount;
uint32_t pendingEvents;
int64_t logicalPosition;
+ int64_t taskPulses;
int64_t totalPulses;
uint8_t busy;
uint8_t pulseActive;
diff --git a/PLSR/Src/plc_device.c b/PLSR/Src/plc_device.c
index 5f2c844..5f37c9a 100644
--- a/PLSR/Src/plc_device.c
+++ b/PLSR/Src/plc_device.c
@@ -549,6 +549,7 @@ PLC_DEVICE_RESULT PlcDevicePublishSm(uint8_t axis,
PLC_DEVICE_RESULT PlcDeviceReadSd(uint16_t address, int32_t *value)
{
int32_t *source;
+ uint32_t interruptState;
if (value == NULL)
{
@@ -560,7 +561,9 @@ PLC_DEVICE_RESULT PlcDeviceReadSd(uint16_t address, int32_t *value)
return PLC_DEVICE_INVALID_ADDRESS;
}
+ interruptState = PlcDeviceEnterCritical();
*value = *source;
+ PlcDeviceExitCritical(interruptState);
return PLC_DEVICE_OK;
}
@@ -576,12 +579,63 @@ PLC_DEVICE_RESULT PlcDevicePublishSd(uint8_t axis,
uint8_t item,
int32_t value)
{
+ uint32_t interruptState;
+
if ((axis >= PLSR_AXIS_COUNT) || (item >= PLSR_SD_AXIS_ITEM_COUNT))
{
return PLC_DEVICE_INVALID_ARGUMENT;
}
+ interruptState = PlcDeviceEnterCritical();
PlcSdRuntime[axis][item] = value;
+ PlcDeviceExitCritical(interruptState);
+ return PLC_DEVICE_OK;
+}
+
+PLC_DEVICE_RESULT PlcDeviceReadSdDword(uint16_t lowAddress, int32_t *value)
+{
+ int32_t *source;
+ uint32_t rawValue;
+ uint32_t interruptState;
+ uint8_t axis;
+ uint8_t item;
+
+ if (value == NULL)
+ {
+ return PLC_DEVICE_NULL_POINTER;
+ }
+ source = PlcDeviceResolveSd(lowAddress, &axis, &item);
+ if ((source == NULL) || ((item & 1U) != 0U)
+ || (item > PLSR_SD_ITEM_SPEED))
+ {
+ return PLC_DEVICE_INVALID_ADDRESS;
+ }
+ interruptState = PlcDeviceEnterCritical();
+ rawValue = (uint32_t)PlcSdRuntime[axis][item] & 0xFFFFUL;
+ rawValue |= ((uint32_t)PlcSdRuntime[axis][item + 1U] & 0xFFFFUL)
+ << 16U;
+ PlcDeviceExitCritical(interruptState);
+ *value = (int32_t)rawValue;
+ return PLC_DEVICE_OK;
+}
+
+PLC_DEVICE_RESULT PlcDevicePublishSdDword(uint8_t axis,
+ uint8_t lowItem,
+ int32_t value)
+{
+ uint32_t rawValue;
+ uint32_t interruptState;
+
+ if ((axis >= PLSR_AXIS_COUNT) || ((lowItem & 1U) != 0U)
+ || (lowItem > PLSR_SD_ITEM_SPEED))
+ {
+ return PLC_DEVICE_INVALID_ARGUMENT;
+ }
+ rawValue = (uint32_t)value;
+ interruptState = PlcDeviceEnterCritical();
+ PlcSdRuntime[axis][lowItem] = (int32_t)(rawValue & 0xFFFFUL);
+ PlcSdRuntime[axis][lowItem + 1U] = (int32_t)(rawValue >> 16U);
+ PlcDeviceExitCritical(interruptState);
return PLC_DEVICE_OK;
}
diff --git a/PLSR/Src/plsr_core.c b/PLSR/Src/plsr_core.c
index ce7b748..00cd3bc 100644
--- a/PLSR/Src/plsr_core.c
+++ b/PLSR/Src/plsr_core.c
@@ -7,6 +7,8 @@
#include "plsr_resource.h"
#include
+#define PLSR_CORE_SFD_AXIS_STRIDE (130U)
+
#ifndef PLSR_HOST_TEST
#include "stm32f4xx.h"
#include "ucos_ii.h"
@@ -17,6 +19,8 @@ typedef struct
PLSR_STATE state;
PLSR_OUTPUT_MODE outputMode;
PLSR_ERROR error;
+ uint16_t compatibleErrorCode;
+ uint16_t compatibleErrorBlock;
PLSR_STOP_REASON stopReason;
PLSR_STATE pendingTerminal;
PLSR_RESOURCE_LEASE lease;
@@ -27,7 +31,11 @@ typedef struct
PLSR_RESULT lastCommandResult;
uint32_t illegalTransitionCount;
int64_t logicalPosition;
+ int64_t taskPulses;
int64_t totalPulses;
+ int64_t segmentAccountedPulses;
+ int64_t equivalentCommandRemainder;
+ PLSR_EQUIVALENT_CONFIG equivalent;
PLSR_PATH_CONTEXT path;
PLSR_PROFILE_STATE profile;
uint8_t profileActive;
@@ -39,6 +47,7 @@ typedef struct
uint8_t positionValid;
uint8_t jobValid;
uint8_t positionOverflow;
+ uint8_t segmentAccountingActive;
} PLSR_AXIS;
typedef struct
@@ -61,6 +70,8 @@ static PLSR_JOB_SNAPSHOT PlsrJobScratch;
static void PlsrStopSegmentHardware(uint8_t axis, PLSR_AXIS *axisObject);
static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis,
PLSR_AXIS *axisObject);
+static void PlsrAccountHardwarePulses(uint8_t axis,
+ PLSR_AXIS *axisObject);
static uint32_t PlsrCoreEnterCritical(void)
{
@@ -104,6 +115,161 @@ static uint8_t PlsrStateIsPulseActive(PLSR_STATE state)
: 0U;
}
+static void PlsrLoadAxisEquivalentConfig(uint8_t axis,
+ PLSR_EQUIVALENT_CONFIG *config)
+{
+ uint16_t base = (uint16_t)(PLSR_SFD_CONFIG_START
+ + (uint16_t)axis
+ * PLSR_CORE_SFD_AXIS_STRIDE);
+ uint16_t word;
+ uint16_t lowWord;
+ uint16_t highWord;
+
+ config->unitCode = 0U;
+ config->pulsesPerRevolution = 1UL;
+ config->movementPerRevolution = 1UL;
+ if (PlcDeviceReadSfd(base, &word) != PLC_DEVICE_OK)
+ {
+ return;
+ }
+ config->unitCode = (uint8_t)((word >> 8U) & 0x07U);
+ if ((PlcDeviceReadSfd((uint16_t)(base + 2U), &lowWord)
+ != PLC_DEVICE_OK)
+ || (PlcDeviceReadSfd((uint16_t)(base + 3U), &highWord)
+ != PLC_DEVICE_OK))
+ {
+ config->unitCode = 0U;
+ return;
+ }
+ config->pulsesPerRevolution = (uint32_t)lowWord
+ | ((uint32_t)highWord << 16U);
+ if ((PlcDeviceReadSfd((uint16_t)(base + 4U), &lowWord)
+ != PLC_DEVICE_OK)
+ || (PlcDeviceReadSfd((uint16_t)(base + 5U), &highWord)
+ != PLC_DEVICE_OK))
+ {
+ config->unitCode = 0U;
+ return;
+ }
+ config->movementPerRevolution = (uint32_t)lowWord
+ | ((uint32_t)highWord << 16U);
+ if (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK)
+ {
+ config->unitCode = 0U;
+ config->pulsesPerRevolution = 1UL;
+ config->movementPerRevolution = 1UL;
+ }
+}
+
+static void PlsrPublishSdDword(uint8_t axis,
+ uint8_t lowItem,
+ int32_t value)
+{
+ (void)PlcDevicePublishSdDword(axis, lowItem, value);
+}
+
+static int32_t PlsrGetCompatibleSegmentPulses(const PLSR_AXIS *axisObject)
+{
+ int64_t signedPulses = (axisObject->directionPositive != 0U)
+ ? axisObject->segmentAccountedPulses
+ : -axisObject->segmentAccountedPulses;
+
+ if (signedPulses > INT32_MAX)
+ {
+ return INT32_MAX;
+ }
+ if (signedPulses < INT32_MIN)
+ {
+ return INT32_MIN;
+ }
+ return (int32_t)signedPulses;
+}
+
+static int32_t PlsrClampCompatibleInt32(int64_t value)
+{
+ if (value > INT32_MAX) return INT32_MAX;
+ if (value < INT32_MIN) return INT32_MIN;
+ return (int32_t)value;
+}
+
+static void PlsrPublishRuntime(uint8_t axis)
+{
+ PLSR_AXIS *axisObject = &PlsrAxes[axis];
+ uint16_t currentSegment = (axisObject->jobValid != 0U)
+ ? PlsrPathGetCurrentSegment(
+ &axisObject->path)
+ : 0U;
+ int32_t segmentPulses = PlsrGetCompatibleSegmentPulses(axisObject);
+ int32_t frequencyHz = (int32_t)PlsrHwGetCurrentFrequencyHz(axis);
+ int64_t segmentEquivalent64 = segmentPulses;
+ uint32_t speed = (uint32_t)frequencyHz;
+ uint16_t publishedError;
+ uint16_t publishedBlock;
+
+ if (PlsrPositionPulsesToUnits(&axisObject->equivalent,
+ segmentPulses,
+ &segmentEquivalent64) != PLSR_RESULT_OK)
+ {
+ axisObject->positionOverflow = 1U;
+ segmentEquivalent64 = (segmentPulses < 0) ? INT32_MIN : INT32_MAX;
+ }
+ if (PlsrPositionPulseFrequencyToSpeed(&axisObject->equivalent,
+ (uint32_t)frequencyHz,
+ &speed) != PLSR_RESULT_OK)
+ {
+ speed = UINT32_MAX;
+ }
+
+ PlsrPublishSdDword(axis,
+ PLSR_SD_ITEM_SEGMENT,
+ (int32_t)currentSegment);
+ PlsrPublishSdDword(axis,
+ PLSR_SD_ITEM_SEGMENT_PULSES,
+ segmentPulses);
+ PlsrPublishSdDword(axis,
+ PLSR_SD_ITEM_SEGMENT_EQUIV,
+ PlsrClampCompatibleInt32(segmentEquivalent64));
+ PlsrPublishSdDword(axis, PLSR_SD_ITEM_FREQUENCY, frequencyHz);
+ PlsrPublishSdDword(axis,
+ PLSR_SD_ITEM_SPEED,
+ (speed > (uint32_t)INT32_MAX) ? INT32_MAX
+ : (int32_t)speed);
+ publishedError = (axisObject->compatibleErrorCode != 0U)
+ ? axisObject->compatibleErrorCode
+ : (uint16_t)axisObject->error;
+ publishedBlock = (axisObject->compatibleErrorCode != 0U)
+ ? axisObject->compatibleErrorBlock
+ : (((axisObject->error != PLSR_ERROR_NONE)
+ && (axisObject->jobValid != 0U))
+ ? currentSegment
+ : 0U);
+ (void)PlcDevicePublishSd(axis,
+ PLSR_SD_ITEM_ERROR_CODE,
+ (int32_t)publishedError);
+ (void)PlcDevicePublishSd(axis,
+ PLSR_SD_ITEM_ERROR_BLOCK,
+ (int32_t)publishedBlock);
+}
+
+static void PlsrSetCompatibleEquivalentError(uint8_t axis,
+ PLSR_AXIS *axisObject)
+{
+ uint16_t commonBase = (uint16_t)(PLSR_SFD_CONFIG_START
+ + (uint16_t)axis
+ * PLSR_CORE_SFD_AXIS_STRIDE);
+
+ if ((axisObject->parseDetail.result == PLSR_RESULT_INVALID_S2)
+ && (axisObject->parseDetail.block == PLSR_PARSE_BLOCK_S2)
+ && ((axisObject->parseDetail.address
+ == (uint32_t)(commonBase + 2U))
+ || (axisObject->parseDetail.address
+ == (uint32_t)(commonBase + 4U))))
+ {
+ axisObject->compatibleErrorCode = 2U;
+ axisObject->compatibleErrorBlock = 0U;
+ }
+}
+
static void PlsrPublishAxis(uint8_t axis)
{
PLSR_AXIS *axisObject = &PlsrAxes[axis];
@@ -114,6 +280,7 @@ static void PlsrPublishAxis(uint8_t axis)
(void)PlcDevicePublishSm(axis,
compatibleRun,
axisObject->directionPositive);
+ PlsrPublishRuntime(axis);
}
static void PlsrSetStopReason(PLSR_AXIS *axis,
@@ -125,21 +292,102 @@ static void PlsrSetStopReason(PLSR_AXIS *axis,
}
}
-/* 将 64 位逻辑位置发布为 HSD 的 32 位兼容值：
- * 超出 INT32 范围时回绕（与信捷 32 位寄存器一致）并置溢出诊断标志。 */
+/* 将 64 位逻辑位置发布为 HSD 的 32 位兼容值。
+ * 超出范围时保持最近一次合法值，只锁存诊断，禁止回绕或静默截断。 */
static void PlsrPublishPosition(uint8_t axis, PLSR_AXIS *axisObject)
{
int32_t compatValue;
+ int64_t equivalentPosition;
+ uint16_t base = (uint16_t)((uint16_t)axis
+ * PLSR_HSD_RUNTIME_AXIS_COUNT);
if ((axisObject->logicalPosition > INT32_MAX)
|| (axisObject->logicalPosition < INT32_MIN))
{
axisObject->positionOverflow = 1U;
+ return;
+ }
+ compatValue = (int32_t)axisObject->logicalPosition;
+ (void)PlcDevicePublishHsdDword(base, compatValue);
+ if ((PlsrPositionPulsesToUnits(&axisObject->equivalent,
+ axisObject->logicalPosition,
+ &equivalentPosition) != PLSR_RESULT_OK)
+ || (equivalentPosition > INT32_MAX)
+ || (equivalentPosition < INT32_MIN))
+ {
+ axisObject->positionOverflow = 1U;
+ return;
+ }
+ (void)PlcDevicePublishHsdDword((uint16_t)(base + 2U),
+ (int32_t)equivalentPosition);
+}
+
+static uint8_t PlsrAddInt64Checked(int64_t left,
+ int64_t right,
+ int64_t *result)
+{
+ if ((result == NULL)
+ || ((right > 0) && (left > INT64_MAX - right))
+ || ((right < 0) && (left < INT64_MIN - right)))
+ {
+ return 0U;
+ }
+ *result = left + right;
+ return 1U;
+}
+
+/* 将 HAL 实际完成的完整脉冲/AB周期合并到64位位置。
+ * emittedPulses 每段从0开始，因此用 segmentAccountedPulses 做差量去重。 */
+static void PlsrAccountHardwarePulses(uint8_t axis,
+ PLSR_AXIS *axisObject)
+{
+ int64_t emittedPulses;
+ int64_t delta;
+ int64_t signedDelta;
+ int64_t newLogicalPosition;
+ int64_t newTaskPulses;
+ int64_t newTotalPulses;
+
+ if (axisObject->segmentAccountingActive == 0U)
+ {
+ return;
+ }
+ emittedPulses = PlsrHwGetEmittedPulses(axis);
+ if ((emittedPulses < 0)
+ || (emittedPulses < axisObject->segmentAccountedPulses))
+ {
+ axisObject->positionOverflow = 1U;
+ (void)PlsrPostEvent(axis, PLSR_EVENT_COUNTER_FAULT);
+ return;
+ }
+ delta = emittedPulses - axisObject->segmentAccountedPulses;
+ if (delta == 0)
+ {
+ return;
+ }
+ signedDelta = (axisObject->directionPositive != 0U) ? delta : -delta;
+ if ((PlsrAddInt64Checked(axisObject->logicalPosition,
+ signedDelta,
+ &newLogicalPosition) == 0U)
+ || (PlsrAddInt64Checked(axisObject->taskPulses,
+ signedDelta,
+ &newTaskPulses) == 0U)
+ || (PlsrAddInt64Checked(axisObject->totalPulses,
+ delta,
+ &newTotalPulses) == 0U))
+ {
+ axisObject->positionOverflow = 1U;
+ axisObject->segmentAccountedPulses = emittedPulses;
+ (void)PlsrPostEvent(axis, PLSR_EVENT_COUNTER_FAULT);
+ return;
}
- compatValue = (int32_t)(uint32_t)axisObject->logicalPosition;
- (void)PlcDevicePublishHsdDword(
- (uint16_t)((uint16_t)axis * PLSR_HSD_RUNTIME_AXIS_COUNT),
- compatValue);
+
+ axisObject->logicalPosition = newLogicalPosition;
+ axisObject->taskPulses = newTaskPulses;
+ axisObject->totalPulses = newTotalPulses;
+ axisObject->segmentAccountedPulses = emittedPulses;
+ PlsrPublishPosition(axis, axisObject);
+ PlsrPublishRuntime(axis);
}
static uint8_t PlsrTransitionIsAllowed(PLSR_STATE current,
@@ -542,11 +790,16 @@ static PLSR_RESULT PlsrStartAxis(PLSR_AXIS *axisObject,
axisObject->outputMode = start->outputMode;
axisObject->directionPositive = (start->directionPositive != 0U) ? 1U : 0U;
axisObject->error = PLSR_ERROR_NONE;
+ axisObject->compatibleErrorCode = 0U;
+ axisObject->compatibleErrorBlock = 0U;
axisObject->stopReason = PLSR_STOP_REASON_NONE;
axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;
axisObject->immediateStopPending = 0U;
axisObject->done = 0U;
axisObject->jobValid = 0U;
+ axisObject->taskPulses = 0;
+ axisObject->segmentAccountedPulses = 0;
+ axisObject->segmentAccountingActive = 0U;
result = PlsrStateTransition(start->axis,
PLSR_STATE_ACCEL,
PLSR_TRANSITION_START);
@@ -577,6 +830,9 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject,
return PLSR_RESULT_INVALID_STATE;
}
+ axisObject->compatibleErrorCode = 0U;
+ axisObject->compatibleErrorBlock = 0U;
+
parseContext.logicalPosition = axisObject->logicalPosition;
parseContext.positionValid = axisObject->positionValid;
result = PlsrBuildJobSnapshot(call,
@@ -585,6 +841,7 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject,
&axisObject->parseDetail);
if (result != PLSR_RESULT_OK)
{
+ PlsrSetCompatibleEquivalentError(call->dAxis, axisObject);
return result;
}
@@ -606,13 +863,28 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject,
axisObject->job = PlsrJobScratch;
axisObject->jobValid = 1U;
+ if ((axisObject->equivalent.unitCode
+ != axisObject->job.equivalent.unitCode)
+ || (axisObject->equivalent.pulsesPerRevolution
+ != axisObject->job.equivalent.pulsesPerRevolution)
+ || (axisObject->equivalent.movementPerRevolution
+ != axisObject->job.equivalent.movementPerRevolution))
+ {
+ axisObject->equivalentCommandRemainder = 0;
+ }
+ axisObject->equivalent = axisObject->job.equivalent;
axisObject->outputMode = (PLSR_OUTPUT_MODE)axisObject->job.outputMode;
axisObject->directionPositive = axisObject->job.initialDirectionPositive;
axisObject->error = PLSR_ERROR_NONE;
+ axisObject->compatibleErrorCode = 0U;
+ axisObject->compatibleErrorBlock = 0U;
axisObject->stopReason = PLSR_STOP_REASON_NONE;
axisObject->pendingTerminal = PLSR_STATE_UNINITIALIZED;
axisObject->immediateStopPending = 0U;
axisObject->done = 0U;
+ axisObject->taskPulses = 0;
+ axisObject->segmentAccountedPulses = 0;
+ axisObject->segmentAccountingActive = 0U;
PlsrPathBegin(&axisObject->path,
&axisObject->job,
axisObject->logicalPosition);
@@ -864,6 +1136,10 @@ static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot)
else
{
axisObject->totalPulses = 0;
+ (void)PlcDeviceSetHsdCheckpointMeta(
+ axisObject->positionValid,
+ 0U);
+ (void)PlcDeviceCheckpointHsd();
result = PLSR_RESULT_OK;
}
break;
@@ -904,6 +1180,8 @@ static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot)
else
{
axisObject->error = PLSR_ERROR_NONE;
+ axisObject->compatibleErrorCode = 0U;
+ axisObject->compatibleErrorBlock = 0U;
axisObject->stopReason = PLSR_STOP_REASON_NONE;
axisObject->done = 0U;
result = PlsrStateTransition(slot->command.axis,
@@ -1034,25 +1312,75 @@ static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis,
PLSR_HW_START_PARAMS params;
PLSR_RESULT result;
int64_t pulses;
+ int64_t signedPulses;
+ int64_t targetPosition;
+ int64_t nextEquivalentRemainder =
+ axisObject->equivalentCommandRemainder;
uint8_t positive;
if (job->positioningMode == 0U)
{
- pulses = (segment->pulseOrTarget < 0)
- ? -(int64_t)segment->pulseOrTarget
- : (int64_t)segment->pulseOrTarget;
- positive = (segment->pulseOrTarget >= 0) ? 1U : 0U;
+ result = PlsrPositionUnitsToPulses(
+ &axisObject->equivalent,
+ segment->pulseOrTarget,
+ axisObject->equivalentCommandRemainder,
+ &signedPulses,
+ &nextEquivalentRemainder);
+ if (result != PLSR_RESULT_OK)
+ {
+ return result;
+ }
+ if (signedPulses == INT64_MIN)
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ pulses = (signedPulses < 0) ? -signedPulses : signedPulses;
+ positive = (signedPulses >= 0) ? 1U : 0U;
}
else
{
- /* 绝对模式：P4 位置闭环后完善；P3a 按当前位置计算。 */
- int64_t delta = (int64_t)segment->pulseOrTarget
- - axisObject->logicalPosition;
+ /* 绝对模式使用实际硬件脉冲闭环更新后的逻辑位置计算位移。 */
+ int64_t delta;
+
+ result = PlsrPositionAbsoluteUnitsToPulses(
+ &axisObject->equivalent,
+ segment->pulseOrTarget,
+ &targetPosition);
+ if (result != PLSR_RESULT_OK)
+ {
+ return result;
+ }
+ if (((axisObject->logicalPosition > 0)
+ && (targetPosition
+ < INT64_MIN + axisObject->logicalPosition))
+ || ((axisObject->logicalPosition < 0)
+ && (targetPosition
+ > INT64_MAX + axisObject->logicalPosition)))
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ delta = targetPosition - axisObject->logicalPosition;
+ if (delta == INT64_MIN)
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
pulses = (delta < 0) ? -delta : delta;
positive = (delta >= 0) ? 1U : 0U;
}
+ if (pulses == 0)
+ {
+ /* 当量小于一个物理脉冲时保存余数并按零脉冲段推进；不启动PWM。 */
+ axisObject->equivalentCommandRemainder = nextEquivalentRemainder;
+ axisObject->directionPositive = positive;
+ axisObject->segmentAccountedPulses = 0;
+ axisObject->segmentAccountingActive = 0U;
+ PlsrPublishAxis(axis);
+ (void)PlsrPostEvent(axis, PLSR_EVENT_SEGMENT_COMPLETE);
+ return PLSR_RESULT_OK;
+ }
+
(void)memset(&profileRequest, 0, sizeof(profileRequest));
profileRequest.targetFrequencyHz = segment->targetFrequency;
profileRequest.startFrequencyHz = job->s2.startSpeed;
@@ -1090,18 +1418,27 @@ static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis,
axisObject->profileWasAccel = 0U;
return result;
}
+ axisObject->equivalentCommandRemainder = nextEquivalentRemainder;
+ axisObject->directionPositive = positive;
+ axisObject->segmentAccountedPulses = 0;
+ axisObject->segmentAccountingActive = 1U;
axisObject->profileActive = 1U;
axisObject->profileWasAccel =
(axisObject->profile.phase == PLSR_PROFILE_PHASE_ACCEL) ? 1U : 0U;
+ PlsrPublishAxis(axis);
return PLSR_RESULT_OK;
}
/* 停止当前段的硬件输出与速度曲线。 */
static void PlsrStopSegmentHardware(uint8_t axis, PLSR_AXIS *axisObject)
{
+ PlsrAccountHardwarePulses(axis, axisObject);
axisObject->profileActive = 0U;
axisObject->profileWasAccel = 0U;
(void)PlsrHwStopPulse(axis);
+ PlsrAccountHardwarePulses(axis, axisObject);
+ axisObject->segmentAccountingActive = 0U;
+ PlsrPublishRuntime(axis);
}
/* 应用路径执行器的动作：段间推进、进入等待、结束、让出、错误。 */
@@ -1293,6 +1630,7 @@ PLSR_RESULT PlsrInit(void)
PlsrAxes[axis].state = PLSR_STATE_UNINITIALIZED;
PlsrAxes[axis].pendingTerminal = PLSR_STATE_UNINITIALIZED;
PlsrAxes[axis].lease.directionPoint = PLSR_DIRECTION_POINT_NONE;
+ PlsrLoadAxisEquivalentConfig(axis, &PlsrAxes[axis].equivalent);
if (PlcDeviceReadHsdDword(
(uint16_t)(axis * PLSR_HSD_RUNTIME_AXIS_COUNT),
&restoredPosition) == PLC_DEVICE_OK)
@@ -1328,6 +1666,13 @@ void PlsrProcess(void)
return;
}
+ /* 先合并 ISR 已完成的实际脉冲，确保段完成、STOP或新命令不会在
+ * HAL 计数清零前丢失最后一批位置增量。 */
+ for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
+ {
+ PlsrAccountHardwarePulses(axis, &PlsrAxes[axis]);
+ }
+
for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
{
events = PlsrTakeEvents(axis, PLSR_EVENT_CRITICAL_MASK);
@@ -1369,6 +1714,13 @@ void PlsrProcess(void)
uint8_t wasAccel;
PlsrHwTick(axis);
+ PlsrAccountHardwarePulses(axis, axisObject);
+ /* 首次 AB 内部预热周期不属于用户运动，速度曲线也必须冻结；
+ * 否则低速起步时会在隐藏周期内提前爬升十余个刷新步。 */
+ if (PlsrHwIsAbStartupPriming(axis) != 0U)
+ {
+ continue;
+ }
if (PlsrStateIsBusy(axisObject->state) == 0U)
{
continue;
@@ -1475,6 +1827,7 @@ PLSR_RESULT PlsrGetStatus(uint8_t axis, PLSR_STATUS *status)
status->illegalTransitionCount = axisObject->illegalTransitionCount;
status->pendingEvents = axisObject->pendingEvents;
status->logicalPosition = axisObject->logicalPosition;
+ status->taskPulses = axisObject->taskPulses;
status->totalPulses = axisObject->totalPulses;
status->busy = PlsrStateIsBusy(axisObject->state);
status->pulseActive = PlsrStateIsPulseActive(axisObject->state);
diff --git a/PLSR/Src/plsr_hal_f407.c b/PLSR/Src/plsr_hal_f407.c
index 970cd28..5f4cced 100644
--- a/PLSR/Src/plsr_hal_f407.c
+++ b/PLSR/Src/plsr_hal_f407.c
@@ -148,6 +148,8 @@ typedef struct
uint8_t directionPositive;
uint8_t abQuarter;
uint8_t abCountAxis;
+ uint8_t abStartupPriming;
+ uint8_t abOutputPrimed;
uint16_t abActiveBasePsc;
uint16_t abActivePairPsc;
uint16_t abActiveArr;
@@ -688,6 +690,7 @@ static void PlsrHwBeginAbOutput(uint8_t axis, uint8_t debugReason)
lagStart = periodTicks / 2UL;
state->abCountAxis = lagAxis;
state->abQuarter = 0U;
+ state->abStartupPriming = (state->abOutputPrimed == 0U) ? 1U : 0U;
#ifndef PLSR_HOST_TEST
interruptState = __get_PRIMASK();
@@ -733,14 +736,17 @@ static void PlsrHwBeginAbOutput(uint8_t axis, uint8_t debugReason)
PlsrHwTimerSetCc1e(pairAxis, 1UL);
PlsrHwTimerSetPwmMode1(axis);
PlsrHwTimerSetPwmMode1(pairAxis);
- /* OCREF 已在低电平位置稳定后再把物理引脚交还定时器。 */
PlsrHwTimerClearCc1if(axis);
PlsrHwTimerClearCc1if(pairAxis);
- PlsrHwReleasePulsePin(axis);
- PlsrHwReleasePulsePin(pairAxis);
+ if (state->abStartupPriming == 0U)
+ {
+ /* 完成过首次预热后，段间/调频重定相均从已验证的 00 边界
+ * 直接交还 AF，不额外吞掉用户周期。 */
+ PlsrHwReleasePulsePin(axis);
+ PlsrHwReleasePulsePin(pairAxis);
+ }
PlsrHwTimerSetCen(axis, 1UL);
PlsrHwTimerSetCen(pairAxis, 1UL);
- /* 最后才允许落后相计数中断。 */
PlsrHwTimerClearCc1if(axis);
PlsrHwTimerClearCc1if(pairAxis);
PlsrHwTimerSetCc1ie(lagAxis, 1UL);
@@ -797,6 +803,7 @@ static void PlsrHwStopActiveOutput(uint8_t axis,
#endif
PlsrHwStopPwmTimer(axis);
PlsrHwStopPwmTimer(pairAxis);
+ PlsrHwAxes[axis].abStartupPriming = 0U;
#ifndef PLSR_HOST_TEST
__DMB();
if (interruptState == 0UL)
@@ -1062,6 +1069,16 @@ uint32_t PlsrHwGetTimerClockHz(uint8_t axis)
return PlsrHwAxisMap[axis].timerClockHz;
}
+uint32_t PlsrHwGetCurrentFrequencyHz(uint8_t axis)
+{
+ if ((axis >= PLSR_HW_AXIS_COUNT)
+ || (PlsrHwAxes[axis].state != PLSR_HW_STATE_RUNNING))
+ {
+ return 0UL;
+ }
+ return PlsrHwAxes[axis].currentFrequencyHz;
+}
+
/* 硬件已发出的脉冲数（profile 虚拟计数校准用，中断内递增）。 */
int64_t PlsrHwGetEmittedPulses(uint8_t axis)
{
@@ -1090,6 +1107,15 @@ int64_t PlsrHwGetEmittedPulses(uint8_t axis)
return emittedPulses;
}
+uint8_t PlsrHwIsAbStartupPriming(uint8_t axis)
+{
+ if ((axis >= PLSR_HW_AXIS_COUNT) || (PlsrHwIsAbBaseAxis(axis) == 0U))
+ {
+ return 0U;
+ }
+ return PlsrHwAxes[axis].abStartupPriming;
+}
+
void PlsrHwTick(uint8_t axis)
{
PLSR_HW_AXIS_STATE *state;
@@ -1163,6 +1189,19 @@ void PlsrHwOnTimerUpdate(uint8_t axis)
{
return;
}
+ if (state->abStartupPriming != 0U)
+ {
+ /* F407 首次切换 OC 模式时 OC1REF 初态不可直接作为物理AB相。
+ * GPIO 保持 00 隐藏首个内部周期；落后相下降沿是真实 00
+ * 边界，此时再交还 AF，且该隐藏周期绝不能计入 emitted。 */
+ state->abStartupPriming = 0U;
+ state->abOutputPrimed = 1U;
+#ifndef PLSR_HOST_TEST
+ PlsrHwReleasePulsePin(ownerAxis);
+ PlsrHwReleasePulsePin(PlsrHwGetPairedAxis(ownerAxis));
+#endif
+ return;
+ }
state->emittedPulses++;
if (state->emittedPulses >= state->targetPulses)
{
diff --git a/PLSR/Src/plsr_job.c b/PLSR/Src/plsr_job.c
index bb97b22..45e17e3 100644
--- a/PLSR/Src/plsr_job.c
+++ b/PLSR/Src/plsr_job.c
@@ -482,6 +482,43 @@ static PLSR_RESULT PlsrLoadS2(const PLSR_CALL *call,
result = PlsrReadFixedWord(0U, commonBase, &word, detail);
if (result != PLSR_RESULT_OK) return result;
snapshot->directionActiveHigh = ((word & (1U << 1U)) != 0U) ? 1U : 0U;
+ snapshot->equivalent.unitCode = (uint8_t)((word >> 8U) & 0x07U);
+ if (PlsrPositionUnitCodeIsValid(snapshot->equivalent.unitCode) == 0U)
+ {
+ PlsrSetDetail(detail,
+ PLSR_RESULT_INVALID_S2,
+ PLSR_PARSE_BLOCK_S2,
+ commonBase,
+ snapshot->equivalent.unitCode,
+ 0U);
+ return PLSR_RESULT_INVALID_S2;
+ }
+ result = PlsrReadFixedDword(0U,
+ (uint16_t)(commonBase + 2U),
+ &snapshot->equivalent.pulsesPerRevolution,
+ detail);
+ if (result != PLSR_RESULT_OK) return result;
+ result = PlsrReadFixedDword(0U,
+ (uint16_t)(commonBase + 4U),
+ &snapshot->equivalent.movementPerRevolution,
+ detail);
+ if (result != PLSR_RESULT_OK) return result;
+ if (PlsrPositionValidateEquivalent(&snapshot->equivalent)
+ != PLSR_RESULT_OK)
+ {
+ uint16_t invalidAddress =
+ (snapshot->equivalent.pulsesPerRevolution == 0UL)
+ ? (uint16_t)(commonBase + 2U)
+ : (uint16_t)(commonBase + 4U);
+
+ PlsrSetDetail(detail,
+ PLSR_RESULT_INVALID_S2,
+ PLSR_PARSE_BLOCK_S2,
+ invalidAddress,
+ 0,
+ 0U);
+ return PLSR_RESULT_INVALID_S2;
+ }
if (call->outputModeOverride == PLSR_OUTPUT_MODE_FROM_SFD)
{
snapshot->outputMode = ((word & (1U << 13U)) != 0U)
@@ -550,6 +587,8 @@ static PLSR_RESULT PlsrLoadS2(const PLSR_CALL *call,
snapshot->s2.stopSpeed = snapshot->s2.maximumSpeed;
snapshot->speedClamped = 1U;
}
+ snapshot->inputDefaultSpeed = snapshot->s2.defaultSpeed;
+ snapshot->inputMaximumSpeed = snapshot->s2.maximumSpeed;
return PLSR_RESULT_OK;
}
@@ -835,9 +874,12 @@ static PLSR_RESULT PlsrCheckInitialPosition(
{
uint8_t visited[PLSR_MAX_SEGMENTS];
int64_t position = context->logicalPosition;
- int32_t movement;
+ int64_t movement;
+ int64_t targetPosition;
+ int64_t remainder = 0;
int32_t next;
uint16_t segment = snapshot->startSegment;
+ PLSR_RESULT result;
if ((snapshot->positioningMode != 0U) && (context->positionValid == 0U))
{
@@ -858,7 +900,24 @@ static PLSR_RESULT PlsrCheckInitialPosition(
visited[segment - 1U] = 1U;
if (snapshot->positioningMode == 0U)
{
- movement = segmentData->pulseOrTarget;
+ result = PlsrPositionUnitsToPulses(&snapshot->equivalent,
+ segmentData->pulseOrTarget,
+ remainder,
+ &movement,
+ &remainder);
+ if (result != PLSR_RESULT_OK)
+ {
+ PlsrSetDetail(detail,
+ PLSR_RESULT_POSITION_OVERFLOW,
+ PLSR_PARSE_BLOCK_POSITION,
+ snapshot->s0.address
+ + (uint32_t)segment
+ * PLSR_S0_SEGMENT_WORDS
+ + 2UL,
+ segmentData->pulseOrTarget,
+ segment);
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
if (((movement > 0) && (position > INT64_MAX - movement))
|| ((movement < 0) && (position < INT64_MIN - movement)))
{
@@ -877,10 +936,18 @@ static PLSR_RESULT PlsrCheckInitialPosition(
}
else
{
- movement = (segmentData->pulseOrTarget > position)
+ result = PlsrPositionAbsoluteUnitsToPulses(
+ &snapshot->equivalent,
+ segmentData->pulseOrTarget,
+ &targetPosition);
+ if (result != PLSR_RESULT_OK)
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ movement = (targetPosition > position)
? 1
- : ((segmentData->pulseOrTarget < position) ? -1 : 0);
- position = segmentData->pulseOrTarget;
+ : ((targetPosition < position) ? -1 : 0);
+ position = targetPosition;
}
if ((movement != 0) && (snapshot->initialDirectionPositive == 0U))
{
@@ -910,6 +977,84 @@ static PLSR_RESULT PlsrCheckInitialPosition(
return PLSR_RESULT_OK;
}
+static PLSR_RESULT PlsrConvertSnapshotSpeeds(
+ PLSR_JOB_SNAPSHOT *snapshot,
+ PLSR_PARSE_DETAIL *detail)
+{
+ uint16_t segment;
+ PLSR_RESULT result;
+
+ result = PlsrPositionSpeedToPulseFrequency(&snapshot->equivalent,
+ snapshot->s2.defaultSpeed,
+ &snapshot->s2.defaultSpeed);
+ if (result != PLSR_RESULT_OK) return result;
+ result = PlsrPositionSpeedToPulseFrequency(&snapshot->equivalent,
+ snapshot->s2.maximumSpeed,
+ &snapshot->s2.maximumSpeed);
+ if (result != PLSR_RESULT_OK) return result;
+ result = PlsrPositionSpeedToPulseFrequency(&snapshot->equivalent,
+ snapshot->s2.startSpeed,
+ &snapshot->s2.startSpeed);
+ if (result != PLSR_RESULT_OK) return result;
+ result = PlsrPositionSpeedToPulseFrequency(&snapshot->equivalent,
+ snapshot->s2.stopSpeed,
+ &snapshot->s2.stopSpeed);
+ if (result != PLSR_RESULT_OK) return result;
+ result = PlsrPositionSpeedToPulseFrequency(&snapshot->equivalent,
+ snapshot->s2.zrnHighSpeed,
+ &snapshot->s2.zrnHighSpeed);
+ if (result != PLSR_RESULT_OK) return result;
+ result = PlsrPositionSpeedToPulseFrequency(&snapshot->equivalent,
+ snapshot->s2.zrnCrawlSpeed,
+ &snapshot->s2.zrnCrawlSpeed);
+ if (result != PLSR_RESULT_OK) return result;
+
+ if ((snapshot->s2.maximumSpeed < PLSR_FREQUENCY_MIN_HZ)
+ || (snapshot->s2.maximumSpeed > PLSR_FREQUENCY_MAX_HZ))
+ {
+ PlsrSetDetail(detail,
+ PLSR_RESULT_INVALID_FREQUENCY,
+ PLSR_PARSE_BLOCK_S2,
+ 0UL,
+ (int32_t)snapshot->s2.maximumSpeed,
+ 0U);
+ return PLSR_RESULT_INVALID_FREQUENCY;
+ }
+ for (segment = 1U; segment <= snapshot->segmentCount; segment++)
+ {
+ PLSR_SEGMENT_SNAPSHOT *segmentData =
+ &snapshot->segments[segment - 1U];
+
+ if (segmentData->targetFrequency == 0UL)
+ {
+ continue;
+ }
+ result = PlsrPositionSpeedToPulseFrequency(
+ &snapshot->equivalent,
+ segmentData->targetFrequency,
+ &segmentData->targetFrequency);
+ if ((result != PLSR_RESULT_OK)
+ || (segmentData->targetFrequency < PLSR_FREQUENCY_MIN_HZ)
+ || (segmentData->targetFrequency > PLSR_FREQUENCY_MAX_HZ))
+ {
+ PlsrSetDetail(detail,
+ PLSR_RESULT_INVALID_FREQUENCY,
+ PLSR_PARSE_BLOCK_S0,
+ snapshot->s0.address
+ + (uint32_t)segment * PLSR_S0_SEGMENT_WORDS,
+ (int32_t)segmentData->targetFrequency,
+ segment);
+ return PLSR_RESULT_INVALID_FREQUENCY;
+ }
+ result = PlsrValidateFrequencyDivider(snapshot,
+ segmentData->targetFrequency,
+ detail,
+ segment);
+ if (result != PLSR_RESULT_OK) return result;
+ }
+ return PLSR_RESULT_OK;
+}
+
PLSR_RESULT PlsrBuildJobSnapshot(const PLSR_CALL *call,
const PLSR_PARSE_CONTEXT *context,
PLSR_JOB_SNAPSHOT *snapshot,
@@ -1186,14 +1331,6 @@ PLSR_RESULT PlsrBuildJobSnapshot(const PLSR_CALL *call,
segmentData->flags |= PLSR_SEGMENT_FLAG_SPEED_CLAMPED;
snapshot->speedClamped = 1U;
}
- if (segmentData->targetFrequency != 0UL)
- {
- result = PlsrValidateFrequencyDivider(snapshot,
- segmentData->targetFrequency,
- detail,
- segment);
- if (result != PLSR_RESULT_OK) return result;
- }
result = PlsrValidateWait(call, segmentData, segment, detail);
if (result != PLSR_RESULT_OK) return result;
result = PlsrValidateJump(call,
@@ -1205,6 +1342,8 @@ PLSR_RESULT PlsrBuildJobSnapshot(const PLSR_CALL *call,
if (result != PLSR_RESULT_OK) return result;
}
+ result = PlsrConvertSnapshotSpeeds(snapshot, detail);
+ if (result != PLSR_RESULT_OK) return result;
result = PlsrCheckConstantPath(snapshot, detail);
if (result != PLSR_RESULT_OK) return result;
result = PlsrCheckInitialPosition(snapshot, context, detail);
@@ -1246,14 +1385,22 @@ PLSR_RESULT PlsrResolveLiveFrequency(const PLSR_JOB_SNAPSHOT *snapshot,
NULL);
if (result != PLSR_RESULT_OK) return result;
if (rawFrequency < 0) return PLSR_RESULT_INVALID_FREQUENCY;
- *frequency = (rawFrequency == 0) ? snapshot->s2.defaultSpeed
+ *frequency = (rawFrequency == 0) ? snapshot->inputDefaultSpeed
: (uint32_t)rawFrequency;
*clamped = 0U;
- if (*frequency > snapshot->s2.maximumSpeed)
+ if (*frequency > snapshot->inputMaximumSpeed)
{
- *frequency = snapshot->s2.maximumSpeed;
+ *frequency = snapshot->inputMaximumSpeed;
*clamped = 1U;
}
if (*frequency == 0UL) return PLSR_RESULT_INVALID_FREQUENCY;
+ result = PlsrPositionSpeedToPulseFrequency(&snapshot->equivalent,
+ *frequency,
+ frequency);
+ if ((result != PLSR_RESULT_OK) || (*frequency == 0UL)
+ || (*frequency > PLSR_FREQUENCY_MAX_HZ))
+ {
+ return PLSR_RESULT_INVALID_FREQUENCY;
+ }
return PlsrValidateFrequencyDivider(snapshot, *frequency, NULL, segment);
}
diff --git a/PLSR/Src/plsr_path.c b/PLSR/Src/plsr_path.c
index 39d832f..cbb3015 100644
--- a/PLSR/Src/plsr_path.c
+++ b/PLSR/Src/plsr_path.c
@@ -88,13 +88,18 @@ static uint8_t PlsrPathSegmentIsZeroPulse(const PLSR_JOB_SNAPSHOT *job,
{
const PLSR_SEGMENT_SNAPSHOT *segmentSnapshot =
&job->segments[segment - 1U];
+ int64_t targetPosition;
if ((segmentSnapshot->flags & PLSR_SEGMENT_FLAG_ZERO_PULSE) != 0U)
{
return 1U;
}
if ((job->positioningMode == 1U)
- && (segmentSnapshot->pulseOrTarget == (int32_t)logicalPosition))
+ && (PlsrPositionAbsoluteUnitsToPulses(
+ &job->equivalent,
+ segmentSnapshot->pulseOrTarget,
+ &targetPosition) == PLSR_RESULT_OK)
+ && (targetPosition == logicalPosition))
{
return 1U;
}
diff --git a/PLSR/Src/plsr_position.c b/PLSR/Src/plsr_position.c
new file mode 100644
index 0000000..0f50faf
--- /dev/null
+++ b/PLSR/Src/plsr_position.c
@@ -0,0 +1,227 @@
+#include "plsr_position.h"
+#include
+#include
+
+uint8_t PlsrPositionUnitCodeIsValid(uint8_t unitCode)
+{
+ return ((unitCode == 0U) || (unitCode == 1U) || (unitCode == 3U)
+ || (unitCode == 5U) || (unitCode == 7U))
+ ? 1U
+ : 0U;
+}
+
+uint8_t PlsrPositionUsesEquivalent(const PLSR_EQUIVALENT_CONFIG *config)
+{
+ return ((config != NULL) && (config->unitCode != 0U)) ? 1U : 0U;
+}
+
+PLSR_RESULT PlsrPositionValidateEquivalent(
+ const PLSR_EQUIVALENT_CONFIG *config)
+{
+ if ((config == NULL)
+ || (PlsrPositionUnitCodeIsValid(config->unitCode) == 0U))
+ {
+ return PLSR_RESULT_INVALID_S2;
+ }
+ if ((config->unitCode != 0U)
+ && ((config->pulsesPerRevolution == 0UL)
+ || (config->movementPerRevolution == 0UL)))
+ {
+ return PLSR_RESULT_INVALID_S2;
+ }
+ return PLSR_RESULT_OK;
+}
+
+PLSR_RESULT PlsrPositionUnitsToPulses(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ int32_t units,
+ int64_t remainder,
+ int64_t *pulses,
+ int64_t *newRemainder)
+{
+ int64_t numerator;
+ int64_t pulseValue;
+ uint32_t denominator;
+
+ if ((pulses == NULL) || (newRemainder == NULL)
+ || (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if (config->unitCode == 0U)
+ {
+ *pulses = units;
+ *newRemainder = 0;
+ return PLSR_RESULT_OK;
+ }
+ denominator = config->movementPerRevolution;
+ if ((remainder <= -(int64_t)denominator)
+ || (remainder >= (int64_t)denominator))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+
+ /* int32 * uint32 本身落在 int64 范围内；仅余数相加需要边界检查。 */
+ numerator = (int64_t)units
+ * (int64_t)config->pulsesPerRevolution;
+ if (((remainder > 0) && (numerator > INT64_MAX - remainder))
+ || ((remainder < 0) && (numerator < INT64_MIN - remainder)))
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ numerator += remainder;
+ pulseValue = numerator / (int64_t)denominator;
+ *newRemainder = numerator % (int64_t)denominator;
+ *pulses = pulseValue;
+ return PLSR_RESULT_OK;
+}
+
+PLSR_RESULT PlsrPositionAbsoluteUnitsToPulses(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ int32_t units,
+ int64_t *pulses)
+{
+ int64_t ignoredRemainder;
+
+ return PlsrPositionUnitsToPulses(config,
+ units,
+ 0,
+ pulses,
+ &ignoredRemainder);
+}
+
+static uint64_t PlsrPositionMagnitude(int64_t value)
+{
+ return (value < 0) ? (uint64_t)(-(value + 1)) + 1ULL
+ : (uint64_t)value;
+}
+
+PLSR_RESULT PlsrPositionPulsesToUnits(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ int64_t pulses,
+ int64_t *units)
+{
+ uint64_t magnitude;
+ uint64_t quotient;
+ uint64_t remainder;
+ uint64_t whole;
+ uint64_t fraction;
+ uint64_t scaled;
+ uint64_t numerator;
+ uint64_t denominator;
+
+ if ((units == NULL)
+ || (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if (config->unitCode == 0U)
+ {
+ *units = pulses;
+ return PLSR_RESULT_OK;
+ }
+
+ magnitude = PlsrPositionMagnitude(pulses);
+ numerator = config->movementPerRevolution;
+ denominator = config->pulsesPerRevolution;
+ quotient = magnitude / denominator;
+ remainder = magnitude % denominator;
+ if ((quotient != 0ULL) && (numerator > UINT64_MAX / quotient))
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ whole = quotient * numerator;
+ /* remainder 与 numerator 均为 uint32 范围，乘积不会超过 uint64。 */
+ fraction = (remainder * numerator) / denominator;
+ if (whole > UINT64_MAX - fraction)
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ scaled = whole + fraction;
+ if ((pulses >= 0) && (scaled > (uint64_t)INT64_MAX))
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ if ((pulses < 0) && (scaled > (UINT64_C(1) << 63U)))
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ if ((pulses < 0) && (scaled == (UINT64_C(1) << 63U)))
+ {
+ *units = INT64_MIN;
+ }
+ else
+ {
+ *units = (pulses < 0) ? -(int64_t)scaled : (int64_t)scaled;
+ }
+ return PLSR_RESULT_OK;
+}
+
+static PLSR_RESULT PlsrPositionScaleUnsigned(uint32_t value,
+ uint32_t numerator,
+ uint32_t denominator,
+ uint32_t *result)
+{
+ uint64_t product;
+ uint64_t scaled;
+
+ if ((result == NULL) || (denominator == 0UL))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ product = (uint64_t)value * (uint64_t)numerator;
+ /* 速度采用四舍五入；累计位置仍采用截断并由绝对累计避免漂移。 */
+ scaled = product / denominator;
+ if ((product % denominator) >= ((uint64_t)denominator + 1ULL) / 2ULL)
+ {
+ scaled++;
+ }
+ if (scaled > UINT32_MAX)
+ {
+ return PLSR_RESULT_POSITION_OVERFLOW;
+ }
+ *result = (uint32_t)scaled;
+ return PLSR_RESULT_OK;
+}
+
+PLSR_RESULT PlsrPositionSpeedToPulseFrequency(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ uint32_t speed,
+ uint32_t *frequencyHz)
+{
+ if ((frequencyHz == NULL)
+ || (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if (config->unitCode == 0U)
+ {
+ *frequencyHz = speed;
+ return PLSR_RESULT_OK;
+ }
+ return PlsrPositionScaleUnsigned(speed,
+ config->pulsesPerRevolution,
+ config->movementPerRevolution,
+ frequencyHz);
+}
+
+PLSR_RESULT PlsrPositionPulseFrequencyToSpeed(
+ const PLSR_EQUIVALENT_CONFIG *config,
+ uint32_t frequencyHz,
+ uint32_t *speed)
+{
+ if ((speed == NULL)
+ || (PlsrPositionValidateEquivalent(config) != PLSR_RESULT_OK))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if (config->unitCode == 0U)
+ {
+ *speed = frequencyHz;
+ return PLSR_RESULT_OK;
+ }
+ return PlsrPositionScaleUnsigned(frequencyHz,
+ config->movementPerRevolution,
+ config->pulsesPerRevolution,
+ speed);
+}
diff --git a/PLSR/Src/plsr_self_test.c b/PLSR/Src/plsr_self_test.c
index 3544009..2403142 100644
--- a/PLSR/Src/plsr_self_test.c
+++ b/PLSR/Src/plsr_self_test.c
@@ -70,6 +70,13 @@ static void SelfTestWriteDword(PLSR_DEVICE_TYPE device,
SelfTestWords[device][address + 1UL] = (uint16_t)(value >> 16U);
}
+static void SelfTestWriteSfdDword(uint16_t address, uint32_t value)
+{
+ (void)PlcDeviceWriteSfd(address, (uint16_t)(value & 0xFFFFUL));
+ (void)PlcDeviceWriteSfd((uint16_t)(address + 1U),
+ (uint16_t)(value >> 16U));
+}
+
PLSR_RESULT PlsrSelfTestQueue(void)
{
PLSR_CALL call;
@@ -109,3 +116,45 @@ PLSR_RESULT PlsrSelfTestQueue(void)
call.outputModeOverride = PLSR_OUTPUT_AB;
return PlsrPostCall(&call);
}
+
+PLSR_RESULT PlsrEquivalentSelfTestQueue(void)
+{
+ PLSR_CALL call;
+
+ (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));
+
+ /* SFD900 Bit10~8=001（1um当量）；3脉冲/2单位。 */
+ (void)PlcDeviceWriteSfd(900U, (1U << 8U));
+ SelfTestWriteSfdDword(902U, 3UL);
+ SelfTestWriteSfdDword(904U, 2UL);
+ (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT);
+ (void)PlcDeviceWriteSfd(907U, 10U);
+ /* 当量换算后物理最高速度=90000Hz，不超过硬件100kHz。 */
+ SelfTestWriteSfdDword(956U, 60000UL);
+
+ /* 板端可观察版本：两段各1001工程单位。
+ * 3脉冲/2单位带余数换算后分别输出1501、1502脉冲，累计3003脉冲；
+ * 1500Hz附近持续约2s，避免原3脉冲自检在逻辑分析仪启动前已经结束。 */
+ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 2U);
+ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10U, 1000UL);
+ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12U, 1001UL);
+ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 20U, 1000UL);
+ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 22U, 1001UL);
+ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);
+
+ (void)memset(&call, 0, sizeof(call));
+ call.sequence = 0xA5A6UL;
+ call.source.context = NULL;
+ call.source.validateWords = SelfTestValidateWords;
+ call.source.readWord = SelfTestReadWord;
+ call.source.readBit = SelfTestReadBit;
+ call.s0.device = PLSR_DEVICE_D;
+ call.s0.address = SELF_TEST_S0_BASE;
+ call.s1.device = PLSR_DEVICE_D;
+ call.s1.address = SELF_TEST_S1_BASE;
+ call.s2.type = PLSR_OPERAND_CONSTANT;
+ call.s2.constant = 1;
+ call.dAxis = 0U;
+ call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;
+ return PlsrPostCall(&call);
+}
diff --git a/PLSR/Test/run_host_tests.ps1 b/PLSR/Test/run_host_tests.ps1
index 571a63c..95ec94c 100644
--- a/PLSR/Test/run_host_tests.ps1
+++ b/PLSR/Test/run_host_tests.ps1
@@ -14,9 +14,17 @@ if (-not (Test-Path -LiteralPath $outputDirectory))
}
$tests = @(
+ @{
+ Name = 'test_plsr_position'
+ Sources = @(
+ "$workspacePath\PLSR\Src\plsr_position.c"
+ "$workspacePath\PLSR\Test\test_plsr_position.c"
+ )
+ },
@{
Name = 'test_plc_device'
Sources = @(
+ "$workspacePath\PLSR\Src\plsr_position.c"
"$workspacePath\PLSR\Src\plc_device.c"
"$workspacePath\PLSR\Src\plsr_persistence.c"
"$workspacePath\PLSR\Test\test_plc_device.c"
@@ -25,6 +33,7 @@ $tests = @(
@{
Name = 'test_plsr_core'
Sources = @(
+ "$workspacePath\PLSR\Src\plsr_position.c"
"$workspacePath\PLSR\Src\plc_device.c"
"$workspacePath\PLSR\Src\plsr_persistence.c"
"$workspacePath\PLSR\Src\plsr_resource.c"
@@ -39,6 +48,7 @@ $tests = @(
@{
Name = 'test_plsr_job'
Sources = @(
+ "$workspacePath\PLSR\Src\plsr_position.c"
"$workspacePath\PLSR\Src\plc_device.c"
"$workspacePath\PLSR\Src\plsr_persistence.c"
"$workspacePath\PLSR\Src\plsr_resource.c"
@@ -53,6 +63,7 @@ $tests = @(
@{
Name = 'test_plsr_path'
Sources = @(
+ "$workspacePath\PLSR\Src\plsr_position.c"
"$workspacePath\PLSR\Src\plc_device.c"
"$workspacePath\PLSR\Src\plsr_persistence.c"
"$workspacePath\PLSR\Src\plsr_resource.c"
@@ -67,6 +78,7 @@ $tests = @(
@{
Name = 'test_plsr_profile'
Sources = @(
+ "$workspacePath\PLSR\Src\plsr_position.c"
"$workspacePath\PLSR\Src\plsr_profile.c"
"$workspacePath\PLSR\Test\test_plsr_profile.c"
)
@@ -74,6 +86,7 @@ $tests = @(
@{
Name = 'test_plsr_hal'
Sources = @(
+ "$workspacePath\PLSR\Src\plsr_position.c"
"$workspacePath\PLSR\Src\plc_device.c"
"$workspacePath\PLSR\Src\plsr_persistence.c"
"$workspacePath\PLSR\Src\plsr_resource.c"
diff --git a/PLSR/Test/test_plc_device.c b/PLSR/Test/test_plc_device.c
index 83c443c..304ed84 100644
--- a/PLSR/Test/test_plc_device.c
+++ b/PLSR/Test/test_plc_device.c
@@ -269,6 +269,16 @@ static void TestSmAndSd(void)
TEST_CHECK(PlcDeviceWriteSd(1071U, 0) == PLC_DEVICE_READ_ONLY);
TEST_CHECK(PlcDeviceReadSd(1012U, &value) == PLC_DEVICE_INVALID_ADDRESS);
TEST_CHECK(PlcDevicePublishSd(4U, 0U, 0) == PLC_DEVICE_INVALID_ARGUMENT);
+ TEST_CHECK(PlcDevicePublishSdDword(2U, 2U, -123456)
+ == PLC_DEVICE_OK);
+ TEST_CHECK(PlcDeviceReadSdDword(1042U, &value) == PLC_DEVICE_OK);
+ TEST_CHECK(value == -123456);
+ TEST_CHECK(PlcDeviceReadSdDword(1043U, &value)
+ == PLC_DEVICE_INVALID_ADDRESS);
+ TEST_CHECK(PlcDevicePublishSdDword(2U, 9U, 0)
+ == PLC_DEVICE_INVALID_ARGUMENT);
+ TEST_CHECK(PlcDeviceReadSdDword(1070U, &value)
+ == PLC_DEVICE_INVALID_ADDRESS);
TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
TEST_CHECK(PlcDeviceReadSm(1040U, &state) == PLC_DEVICE_OK);
diff --git a/PLSR/Test/test_plsr_core.c b/PLSR/Test/test_plsr_core.c
index a2084d0..89c92ef 100644
--- a/PLSR/Test/test_plsr_core.c
+++ b/PLSR/Test/test_plsr_core.c
@@ -348,6 +348,7 @@ static void TestPositionCheckpointing(void)
{
PLSR_STATUS status;
int32_t hsdPosition;
+ int32_t hsdEquivalent;
/* 1. SET_POSITION 后立即写入 HSD 检查点。 */
TestReset();
@@ -360,6 +361,8 @@ static void TestPositionCheckpointing(void)
TEST_CHECK(PlcDeviceIsHsdDirty() == 0U);
TEST_CHECK(PlcDeviceReadHsdDword(0U, &hsdPosition) == PLC_DEVICE_OK);
TEST_CHECK(hsdPosition == 500);
+ TEST_CHECK(PlcDeviceReadHsdDword(2U, &hsdEquivalent) == PLC_DEVICE_OK);
+ TEST_CHECK(hsdEquivalent == 500);
/* 2. 模拟重启：位置与 positionValid 应恢复到上次正常停机的检查点。 */
TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
@@ -377,7 +380,7 @@ static void TestPositionCheckpointing(void)
TEST_CHECK(status.positionValid == 0U);
TEST_CHECK(status.logicalPosition == 500);
- /* 4. 64位位置超出 INT32 范围：对外回绕并置溢出诊断。 */
+ /* 4. 64位位置超出 INT32 范围：保留内部值、HSD保持最近合法值。 */
TEST_CHECK(TestPostCommand(2U,
0U,
PLSR_CMD_SET_POSITION,
@@ -387,8 +390,24 @@ static void TestPositionCheckpointing(void)
status = TestGetStatus(0U);
TEST_CHECK(status.lastCommandResult == PLSR_RESULT_OK);
TEST_CHECK(status.positionOverflow != 0U);
+ TEST_CHECK(status.logicalPosition == INT64_C(0x100000000));
+ TEST_CHECK(PlcDeviceReadHsdDword(0U, &hsdPosition) == PLC_DEVICE_OK);
+ TEST_CHECK(hsdPosition == 500);
+ TEST_CHECK(PlcDeviceReadHsdDword(2U, &hsdEquivalent) == PLC_DEVICE_OK);
+ TEST_CHECK(hsdEquivalent == 500);
+
+ /* 5. 清零位置重新建立有效坐标，并解除兼容发布溢出锁存。 */
+ TEST_CHECK(TestPostCommand(3U, 0U, PLSR_CMD_CLEAR_POSITION, 0)
+ == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus(0U);
+ TEST_CHECK(status.logicalPosition == 0);
+ TEST_CHECK(status.positionValid != 0U);
+ TEST_CHECK(status.positionOverflow == 0U);
TEST_CHECK(PlcDeviceReadHsdDword(0U, &hsdPosition) == PLC_DEVICE_OK);
TEST_CHECK(hsdPosition == 0);
+ TEST_CHECK(PlcDeviceReadHsdDword(2U, &hsdEquivalent) == PLC_DEVICE_OK);
+ TEST_CHECK(hsdEquivalent == 0);
}
int main(void)
diff --git a/PLSR/Test/test_plsr_hal.c b/PLSR/Test/test_plsr_hal.c
index f644a69..f40fec1 100644
--- a/PLSR/Test/test_plsr_hal.c
+++ b/PLSR/Test/test_plsr_hal.c
@@ -81,6 +81,14 @@ static void TestWriteDword(TEST_MEMORY *memory,
memory->words[device][address + 1UL] = (uint16_t)(raw >> 16U);
}
+static void TestWriteSfdDword(uint16_t address, uint32_t value)
+{
+ CHECK(PlcDeviceWriteSfd(address, (uint16_t)(value & 0xFFFFUL))
+ == PLC_DEVICE_OK);
+ CHECK(PlcDeviceWriteSfd((uint16_t)(address + 1U),
+ (uint16_t)(value >> 16U)) == PLC_DEVICE_OK);
+}
+
static void TestSetSegment(TEST_MEMORY *memory,
uint16_t number,
uint32_t frequency,
@@ -104,6 +112,20 @@ static void TestResetEnvironment(void)
CHECK(PlsrInit() == PLSR_RESULT_OK);
}
+static void TestCompleteFirstAbPrime(uint8_t axis)
+{
+ int quarter;
+
+ CHECK(PlsrHwIsAbStartupPriming(axis) != 0U);
+ for (quarter = 0; quarter < 4; quarter++)
+ {
+ PlsrHwTestAdvanceAbQuarter(axis);
+ }
+ CHECK(PlsrHwIsAbStartupPriming(axis) == 0U);
+ CHECK(PlsrHwGetEmittedPulses(axis) == 0);
+ CHECK(PlsrHwTestGetAbQuarter(axis) == 0U);
+}
+
static PLSR_CALL TestMakeCall(TEST_MEMORY *memory)
{
PLSR_CALL call;
@@ -134,6 +156,20 @@ static PLSR_STATUS TestGetStatus(void)
return status;
}
+static int32_t TestReadSdDword(uint16_t lowAddress)
+{
+ int32_t lowWord = 0;
+ int32_t highWord = 0;
+ uint32_t rawValue;
+
+ CHECK(PlcDeviceReadSd(lowAddress, &lowWord) == PLC_DEVICE_OK);
+ CHECK(PlcDeviceReadSd((uint16_t)(lowAddress + 1U), &highWord)
+ == PLC_DEVICE_OK);
+ rawValue = ((uint32_t)lowWord & 0xFFFFUL)
+ | (((uint32_t)highWord & 0xFFFFUL) << 16U);
+ return (int32_t)rawValue;
+}
+
/* ---- HAL 单测 ---- */
static void TestMapping(void)
@@ -316,6 +352,7 @@ static void TestAbPhaseAndCounting(void)
CHECK(PlsrHwTestGetCnt(0U)
== ((PlsrHwTestGetArr(0U) + 1UL) * 3UL) / 4UL);
CHECK(PlsrHwTestGetCnt(1U) == PlsrHwTestGetCcr(1U));
+ TestCompleteFirstAbPrime(0U);
/* 任一物理 timer update 不能直接计作完整 AB 周期。 */
PlsrHwTestTriggerUpdate(0U);
@@ -433,6 +470,9 @@ static void TestTwoAbAxesIndependent(void)
CHECK(PlsrHwTestGetPwmEnabled(2U) != 0U);
CHECK(PlsrHwTestGetPwmEnabled(3U) != 0U);
+ TestCompleteFirstAbPrime(0U);
+ TestCompleteFirstAbPrime(2U);
+
for (quarter = 0; quarter < 4; quarter++)
{
PlsrHwTestAdvanceAbQuarter(0U);
@@ -473,6 +513,8 @@ static void TestAbFrequencyLimits(void)
== 2UL * (PlsrHwTestGetPsc(1U) + 1UL));
CHECK(PlsrHwTestGetArr(0U) <= 65535UL);
+ TestCompleteFirstAbPrime(0U);
+
CHECK(PlsrHwSetFrequency(0U, 100000UL) == PLSR_RESULT_OK);
for (quarter = 0; quarter < 4; quarter++)
{
@@ -580,6 +622,11 @@ static void TestEndToEndTwoSegments(void)
status = TestGetStatus();
CHECK(status.state == PLSR_STATE_ACCEL);
CHECK(status.currentSegment == 2U);
+ CHECK(status.logicalPosition == 100);
+ CHECK(status.taskPulses == 100);
+ CHECK(status.totalPulses == 100);
+ CHECK(TestReadSdDword(1000U) == 2);
+ CHECK(TestReadSdDword(1002U) == 0);
/* 段2：DIR 延时 → PWM → 加速 → RUN。 */
for (ticks = 0; ticks < 600; ticks++)
@@ -603,6 +650,23 @@ static void TestEndToEndTwoSegments(void)
status = TestGetStatus();
CHECK(status.state == PLSR_STATE_COMPLETED);
CHECK(status.done != 0U);
+ CHECK(status.logicalPosition == 300);
+ CHECK(status.taskPulses == 300);
+ CHECK(status.totalPulses == 300);
+ CHECK(TestReadSdDword(1000U) == 2);
+ CHECK(TestReadSdDword(1002U) == 200);
+ CHECK(TestReadSdDword(1004U) == 200);
+ CHECK(TestReadSdDword(1006U) == 0);
+ {
+ int32_t hsdPulses;
+ int32_t hsdEquivalent;
+
+ CHECK(PlcDeviceReadHsdDword(0U, &hsdPulses) == PLC_DEVICE_OK);
+ CHECK(hsdPulses == 300);
+ CHECK(PlcDeviceReadHsdDword(2U, &hsdEquivalent)
+ == PLC_DEVICE_OK);
+ CHECK(hsdEquivalent == 300);
+ }
/* 终态转换后 HAL 回 IDLE（允许重新启动），脉冲已停止。 */
CHECK(PlsrHwIsPulseActive(0U) == 0U);
CHECK(PlsrHwTestGetPwmEnabled(0U) == 0U);
@@ -633,6 +697,8 @@ static void TestEndToEndAbSegment(void)
CHECK(PlsrHwTestGetPwmEnabled(0U) != 0U);
CHECK(PlsrHwTestGetPwmEnabled(1U) != 0U);
+ TestCompleteFirstAbPrime(0U);
+
/* 负脉冲选择反向相序，完整两个周期后由同一事件链结束任务。 */
PlsrHwTestAdvanceAbQuarter(0U);
CHECK(PlsrHwTestGetAbPhaseA(0U) == 0U);
@@ -647,11 +713,222 @@ static void TestEndToEndAbSegment(void)
status = TestGetStatus();
CHECK(status.state == PLSR_STATE_COMPLETED);
CHECK(status.done != 0U);
+ CHECK(status.directionPositive == 0U);
+ CHECK(status.logicalPosition == -2);
+ CHECK(status.taskPulses == -2);
+ CHECK(status.totalPulses == 2);
+ CHECK(TestReadSdDword(1002U) == -2);
+ CHECK(TestReadSdDword(1004U) == -2);
CHECK(status.highResourceMask == 0U);
CHECK(PlsrHwTestGetPwmEnabled(0U) == 0U);
CHECK(PlsrHwTestGetPwmEnabled(1U) == 0U);
}
+static void TestPositionOnImmediateStop(void)
+{
+ TEST_MEMORY memory;
+ PLSR_CALL call;
+ PLSR_COMMAND command;
+ PLSR_STATUS status;
+ int pulse;
+ int tick;
+
+ TestResetEnvironment();
+ (void)memset(&memory, 0, sizeof(memory));
+ TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S0_BASE, 1);
+ TestSetSegment(&memory, 1U, 1000U, 100);
+ call = TestMakeCall(&memory);
+ call.sequence = 30UL;
+ CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ for (tick = 0; tick < 10; tick++)
+ {
+ PlsrProcess();
+ }
+ CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING);
+
+ for (pulse = 0; pulse < 37; pulse++)
+ {
+ PlsrHwTestTriggerUpdate(0U);
+ }
+ PlsrProcess();
+ status = TestGetStatus();
+ CHECK(status.logicalPosition == 37);
+ CHECK(status.taskPulses == 37);
+ CHECK(status.totalPulses == 37);
+
+ command.sequence = 31UL;
+ command.axis = 0U;
+ command.opcode = PLSR_CMD_STOP_IMMEDIATE;
+ command.argument = 0;
+ CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_IDLE);
+ CHECK(PlsrPostEvent(0U, PLSR_EVENT_STOP_IMMEDIATE_DONE)
+ == PLSR_RESULT_OK);
+ PlsrProcess();
+ status = TestGetStatus();
+ CHECK(status.state == PLSR_STATE_STOPPED);
+ CHECK(status.logicalPosition == 37);
+ CHECK(status.taskPulses == 37);
+ CHECK(status.totalPulses == 37);
+ CHECK(TestReadSdDword(1002U) == 37);
+
+ command.sequence = 32UL;
+ command.opcode = PLSR_CMD_CLEAR_TOTAL;
+ CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus();
+ CHECK(status.logicalPosition == 37);
+ CHECK(status.totalPulses == 0);
+}
+
+static void TestAbsolutePositionAccounting(void)
+{
+ TEST_MEMORY memory;
+ PLSR_CALL call;
+ PLSR_COMMAND command;
+ PLSR_STATUS status;
+ int32_t hsdPosition;
+ int pulse;
+ int tick;
+
+ TestResetEnvironment();
+ command.sequence = 40UL;
+ command.axis = 0U;
+ command.opcode = PLSR_CMD_SET_POSITION;
+ command.argument = 100;
+ CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+
+ (void)memset(&memory, 0, sizeof(memory));
+ TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S0_BASE, 1);
+ TestSetSegment(&memory, 1U, 1000U, 130);
+ TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S1_BASE, 1);
+ call = TestMakeCall(&memory);
+ call.sequence = 41UL;
+ CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ for (tick = 0; tick < 10; tick++)
+ {
+ PlsrProcess();
+ }
+ CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING);
+ for (pulse = 0; pulse < 30; pulse++)
+ {
+ PlsrHwTestTriggerUpdate(0U);
+ }
+ PlsrProcess();
+ status = TestGetStatus();
+ CHECK(status.state == PLSR_STATE_COMPLETED);
+ CHECK(status.logicalPosition == 130);
+ CHECK(status.taskPulses == 30);
+ CHECK(status.totalPulses == 30);
+ CHECK(status.positionValid != 0U);
+ CHECK(PlcDeviceReadHsdDword(0U, &hsdPosition) == PLC_DEVICE_OK);
+ CHECK(hsdPosition == 130);
+}
+
+static void TestEquivalentRemainderAccounting(void)
+{
+ TEST_MEMORY memory;
+ PLSR_CALL call;
+ PLSR_STATUS status;
+ int32_t hsdPulses;
+ int32_t hsdEquivalent;
+
+ TestResetEnvironment();
+ CHECK(PlcDeviceWriteSfd(900U, (1U << 8U)) == PLC_DEVICE_OK);
+ TestWriteSfdDword(902U, 3UL);
+ TestWriteSfdDword(904U, 2UL);
+ TestWriteSfdDword(956U, 60000UL);
+ CHECK(PlcDeviceWriteSfd(907U, 0U) == PLC_DEVICE_OK);
+
+ (void)memset(&memory, 0, sizeof(memory));
+ TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S0_BASE, 1);
+ TestSetSegment(&memory, 1U, 1000U, 1);
+ call = TestMakeCall(&memory);
+ call.sequence = 30UL;
+ call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;
+
+ /* 3脉冲/2单位：第一次1单位只输出1脉冲并保存1/2余数。 */
+ CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING);
+ PlsrHwTestTriggerUpdate(0U);
+ PlsrProcess();
+ status = TestGetStatus();
+ CHECK(status.state == PLSR_STATE_COMPLETED);
+ CHECK(status.logicalPosition == 1);
+ CHECK(status.taskPulses == 1);
+ CHECK(status.totalPulses == 1);
+ CHECK(PlcDeviceReadHsdDword(0U, &hsdPulses) == PLC_DEVICE_OK);
+ CHECK(PlcDeviceReadHsdDword(2U, &hsdEquivalent) == PLC_DEVICE_OK);
+ CHECK(hsdPulses == 1);
+ CHECK(hsdEquivalent == 0);
+
+ /* 第二次1单位合并余数后输出2脉冲；两次合计精确为3脉冲/2单位。 */
+ call.sequence = 31UL;
+ CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ PlsrHwTestTriggerUpdate(0U);
+ CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING);
+ PlsrHwTestTriggerUpdate(0U);
+ PlsrProcess();
+ status = TestGetStatus();
+ CHECK(status.state == PLSR_STATE_COMPLETED);
+ CHECK(status.logicalPosition == 3);
+ CHECK(status.taskPulses == 2);
+ CHECK(status.totalPulses == 3);
+ CHECK(PlcDeviceReadHsdDword(0U, &hsdPulses) == PLC_DEVICE_OK);
+ CHECK(PlcDeviceReadHsdDword(2U, &hsdEquivalent) == PLC_DEVICE_OK);
+ CHECK(hsdPulses == 3);
+ CHECK(hsdEquivalent == 2);
+ CHECK(TestReadSdDword(1002U) == 2);
+ CHECK(TestReadSdDword(1004U) == 1);
+}
+
+static void TestEquivalentCompatibleError(void)
+{
+ TEST_MEMORY memory;
+ PLSR_CALL call;
+ PLSR_STATUS status;
+ int32_t errorCode = -1;
+ int32_t errorBlock = -1;
+
+ TestResetEnvironment();
+ CHECK(PlcDeviceWriteSfd(900U, (1U << 8U)) == PLC_DEVICE_OK);
+ TestWriteSfdDword(902U, 0UL);
+ TestWriteSfdDword(904U, 2UL);
+ TestWriteSfdDword(956U, 60000UL);
+
+ (void)memset(&memory, 0, sizeof(memory));
+ TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S0_BASE, 1);
+ TestSetSegment(&memory, 1U, 1000U, 1);
+ call = TestMakeCall(&memory);
+ call.sequence = 32UL;
+ call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;
+
+ CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ status = TestGetStatus();
+ CHECK(status.lastCommandResult == PLSR_RESULT_INVALID_S2);
+ CHECK(status.state == PLSR_STATE_IDLE);
+ CHECK(PlcDeviceReadSd(1010U, &errorCode) == PLC_DEVICE_OK);
+ CHECK(PlcDeviceReadSd(1011U, &errorBlock) == PLC_DEVICE_OK);
+ CHECK(errorCode == 2);
+ CHECK(errorBlock == 0);
+
+ /* A valid retry clears the compatible parameter error. */
+ TestWriteSfdDword(902U, 3UL);
+ call.sequence = 33UL;
+ CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+ CHECK(PlcDeviceReadSd(1010U, &errorCode) == PLC_DEVICE_OK);
+ CHECK(errorCode == 0);
+ CHECK(PlsrHwStopPulse(0U) == PLSR_RESULT_OK);
+}
+
static void TestProductionSelfTestStartsAb(void)
{
PLSR_STATUS status;
@@ -670,6 +947,55 @@ static void TestProductionSelfTestStartsAb(void)
CHECK(PlsrHwStopPulse(0U) == PLSR_RESULT_OK);
}
+static void TestEquivalentSelfTest(void)
+{
+ PLSR_STATUS status;
+ int32_t hsdPulses;
+ int32_t hsdEquivalent;
+ int ticks;
+
+ TestResetEnvironment();
+ CHECK(PlsrEquivalentSelfTestQueue() == PLSR_RESULT_QUEUED);
+ PlsrProcess();
+
+ /* SFD907=10ms. Advance the simulated hardware delay before segment 1. */
+ for (ticks = 0; ticks < 10; ticks++)
+ {
+ PlsrProcess();
+ }
+ CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING);
+
+ /* 3 pulses / 2 units: 1001 units emit 1501 pulses and keep 1/2 remainder. */
+ for (ticks = 0; ticks < 1501; ticks++)
+ {
+ PlsrHwTestTriggerUpdate(0U);
+ }
+ PlsrProcess();
+ CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING);
+
+ /* Segment 2 consumes the remainder and therefore emits 1502 pulses. */
+ for (ticks = 0; ticks < 1502; ticks++)
+ {
+ PlsrHwTestTriggerUpdate(0U);
+ }
+ PlsrProcess();
+
+ status = TestGetStatus();
+ CHECK(status.lastCommandResult == PLSR_RESULT_OK);
+ CHECK(status.outputMode == PLSR_OUTPUT_PULSE_DIR);
+ CHECK(status.state == PLSR_STATE_COMPLETED);
+ CHECK(status.currentSegment == 2U);
+ CHECK(status.logicalPosition == 3003);
+ CHECK(status.taskPulses == 3003);
+ CHECK(status.totalPulses == 3003);
+ CHECK(PlcDeviceReadHsdDword(0U, &hsdPulses) == PLC_DEVICE_OK);
+ CHECK(PlcDeviceReadHsdDword(2U, &hsdEquivalent) == PLC_DEVICE_OK);
+ CHECK(hsdPulses == 3003);
+ CHECK(hsdEquivalent == 2002);
+ CHECK(TestReadSdDword(1002U) == 1502);
+ CHECK(TestReadSdDword(1004U) == 1001);
+}
+
static void TestStopStopsHardware(void)
{
TEST_MEMORY memory;
@@ -722,7 +1048,12 @@ int main(void)
TestStopAndInvalidArgs();
TestEndToEndTwoSegments();
TestEndToEndAbSegment();
+ TestPositionOnImmediateStop();
+ TestAbsolutePositionAccounting();
+ TestEquivalentRemainderAccounting();
+ TestEquivalentCompatibleError();
TestProductionSelfTestStartsAb();
+ TestEquivalentSelfTest();
TestStopStopsHardware();
if (TestFailures != 0)
diff --git a/PLSR/Test/test_plsr_job.c b/PLSR/Test/test_plsr_job.c
index 545f80b..ad6876c 100644
--- a/PLSR/Test/test_plsr_job.c
+++ b/PLSR/Test/test_plsr_job.c
@@ -362,6 +362,53 @@ static void TestOutputAndDivider(void)
CHECK(PlcDeviceWriteSfd(900U, 0) == PLC_DEVICE_OK);
}
+static void TestEquivalentSnapshot(void)
+{
+ TEST_MEMORY memory;
+ PLSR_CALL call;
+ PLSR_PARSE_CONTEXT context = {0, 1U};
+ PLSR_JOB_SNAPSHOT snapshot;
+ PLSR_PARSE_DETAIL detail;
+ uint32_t liveFrequency;
+ uint8_t clamped;
+
+ TestBuildValidBlocks(&memory);
+ TestWriteDword(&memory, PLSR_DEVICE_D, 110U, 40000);
+ call = TestMakeCall(&memory);
+ CHECK(PlcDeviceWriteSfd(900U, (1U << 8U)) == PLC_DEVICE_OK);
+ TestWriteSfdDword(902U, 3UL);
+ TestWriteSfdDword(904U, 2UL);
+ TestWriteSfdDword(956U, 60000UL);
+ TestWriteSfdDword(958U, 0UL);
+
+ CHECK(PlsrBuildJobSnapshot(&call, &context, &snapshot, &detail)
+ == PLSR_RESULT_OK);
+ CHECK(snapshot.equivalent.unitCode == 1U);
+ CHECK(snapshot.equivalent.pulsesPerRevolution == 3UL);
+ CHECK(snapshot.equivalent.movementPerRevolution == 2UL);
+ CHECK(snapshot.inputDefaultSpeed == 1000UL);
+ CHECK(snapshot.inputMaximumSpeed == 60000UL);
+ CHECK(snapshot.s2.defaultSpeed == 1500UL);
+ CHECK(snapshot.s2.maximumSpeed == 90000UL);
+ CHECK(snapshot.segments[0].targetFrequency == 60000UL);
+ CHECK(snapshot.segments[1].targetFrequency == 1500UL);
+
+ TestWriteDword(&memory, PLSR_DEVICE_D, 110U, 20000);
+ CHECK(PlsrResolveLiveFrequency(&snapshot,
+ 1U,
+ &liveFrequency,
+ &clamped) == PLSR_RESULT_OK);
+ CHECK(liveFrequency == 30000UL);
+ CHECK(clamped == 0U);
+
+ TestWriteSfdDword(902U, 0UL);
+ CHECK(PlsrBuildJobSnapshot(&call, &context, &snapshot, &detail)
+ == PLSR_RESULT_INVALID_S2);
+ CHECK(detail.address == 902U);
+
+ TestConfigureAxis0K1();
+}
+
static void TestCoreSubmission(void)
{
TEST_MEMORY memory;
@@ -443,6 +490,7 @@ int main(void)
TestValidationFailures();
TestDynamicReferences();
TestOutputAndDivider();
+ TestEquivalentSnapshot();
TestDefaultSfdStartable();
TestCoreSubmission();
diff --git a/PLSR/Test/test_plsr_position.c b/PLSR/Test/test_plsr_position.c
new file mode 100644
index 0000000..c208086
--- /dev/null
+++ b/PLSR/Test/test_plsr_position.c
@@ -0,0 +1,93 @@
+#include "plsr_position.h"
+#include
+#include
+
+static int TestChecks;
+static int TestFailures;
+
+#define CHECK(condition) \
+ do \
+ { \
+ TestChecks++; \
+ if (!(condition)) \
+ { \
+ TestFailures++; \
+ (void)printf("FAIL line %d: %s\n", __LINE__, #condition); \
+ } \
+ } while (0)
+
+int main(void)
+{
+ PLSR_EQUIVALENT_CONFIG config = {3UL, 2UL, 1U};
+ PLSR_EQUIVALENT_CONFIG pulseConfig = {0UL, 0UL, 0U};
+ int64_t pulses;
+ int64_t units;
+ int64_t remainder;
+ uint32_t speed;
+
+ CHECK(PlsrPositionUnitCodeIsValid(0U) != 0U);
+ CHECK(PlsrPositionUnitCodeIsValid(1U) != 0U);
+ CHECK(PlsrPositionUnitCodeIsValid(3U) != 0U);
+ CHECK(PlsrPositionUnitCodeIsValid(5U) != 0U);
+ CHECK(PlsrPositionUnitCodeIsValid(7U) != 0U);
+ CHECK(PlsrPositionUnitCodeIsValid(2U) == 0U);
+ CHECK(PlsrPositionValidateEquivalent(&config) == PLSR_RESULT_OK);
+
+ CHECK(PlsrPositionUnitsToPulses(&config, 1, 0, &pulses, &remainder)
+ == PLSR_RESULT_OK);
+ CHECK(pulses == 1);
+ CHECK(remainder == 1);
+ CHECK(PlsrPositionUnitsToPulses(&config,
+ 1,
+ remainder,
+ &pulses,
+ &remainder) == PLSR_RESULT_OK);
+ CHECK(pulses == 2);
+ CHECK(remainder == 0);
+
+ CHECK(PlsrPositionUnitsToPulses(&config, -1, 0, &pulses, &remainder)
+ == PLSR_RESULT_OK);
+ CHECK(pulses == -1);
+ CHECK(remainder == -1);
+ CHECK(PlsrPositionUnitsToPulses(&config,
+ -1,
+ remainder,
+ &pulses,
+ &remainder) == PLSR_RESULT_OK);
+ CHECK(pulses == -2);
+ CHECK(remainder == 0);
+
+ CHECK(PlsrPositionAbsoluteUnitsToPulses(&config, 10, &pulses)
+ == PLSR_RESULT_OK);
+ CHECK(pulses == 15);
+ CHECK(PlsrPositionPulsesToUnits(&config, 3, &units) == PLSR_RESULT_OK);
+ CHECK(units == 2);
+ CHECK(PlsrPositionPulsesToUnits(&config, -3, &units) == PLSR_RESULT_OK);
+ CHECK(units == -2);
+ CHECK(PlsrPositionSpeedToPulseFrequency(&config, 200U, &speed)
+ == PLSR_RESULT_OK);
+ CHECK(speed == 300U);
+ CHECK(PlsrPositionPulseFrequencyToSpeed(&config, 300U, &speed)
+ == PLSR_RESULT_OK);
+ CHECK(speed == 200U);
+
+ CHECK(PlsrPositionUnitsToPulses(&pulseConfig,
+ INT32_MIN,
+ 99,
+ &pulses,
+ &remainder) == PLSR_RESULT_OK);
+ CHECK(pulses == INT32_MIN);
+ CHECK(remainder == 0);
+ config.pulsesPerRevolution = 0UL;
+ CHECK(PlsrPositionValidateEquivalent(&config) == PLSR_RESULT_INVALID_S2);
+
+ if (TestFailures != 0)
+ {
+ (void)printf("FAIL: %d of %d PLSR position checks failed\n",
+ TestFailures,
+ TestChecks);
+ return 1;
+ }
+ (void)printf("PASS: %d PLSR position checks\n", TestChecks);
+ return 0;
+}
diff --git a/tmp/pdfs/xinjie_page_30.png b/tmp/pdfs/xinjie_page_30.png
new file mode 100644
index 0000000..7a6ee1f
Binary files /dev/null and b/tmp/pdfs/xinjie_page_30.png differ
diff --git a/tmp/pdfs/xinjie_page_31.png b/tmp/pdfs/xinjie_page_31.png
new file mode 100644
index 0000000..ac66b07
Binary files /dev/null and b/tmp/pdfs/xinjie_page_31.png differ