#include "plsr_self_test.h" #include "plc_device.h" #include "modbus_data_store.h" #include "plsr_core.h" #include "plsr_job.h" #include "plsr_modbus_data.h" #include /* 上电自测（验证后可删除）： * - AB 模式使用 Q0(A)/Q1(B)，其余用出厂默认参数（K1） * - 任务：3 段完整 AB 周期（H00 完成，顺序衔接）： * 段1：2000Hz / +1000 周期（A 超前 B） * 段2：5000Hz / +6000 周期（A 超前 B） * 段3：1000Hz / -500 周期（B 超前 A，验证反向） * 数据源为静态数组，仅自测使用（正式 D 设备适配器见 Modbus 阶段）。 */ #define SELF_TEST_WORD_CAPACITY (192U) #define SELF_TEST_S0_BASE (10U) #define SELF_TEST_S1_BASE (60U) #define SELF_TEST_DIR_POINT (4U) #define SELF_TEST_SFD_AXIS_STRIDE (130U) #define SELF_TEST_SFD_SET_OFFSET (50U) #define SELF_TEST_MODBUS_S0_BASE (1000UL) #define SELF_TEST_MODBUS_S1_BASE (1100UL) static uint16_t SelfTestWords[3][SELF_TEST_WORD_CAPACITY]; volatile int32_t PlsrSelfTestLiveFrequencyHz; volatile uint32_t PlsrSelfTestDynamicTick100us; volatile uint8_t PlsrSelfTestDynamicPhase; static volatile uint8_t PlsrSelfTestDynamicEnabled; void PlsrSelfTestControlTick100us(void) { if (PlsrSelfTestDynamicEnabled == 0U) { return; } if (PlsrSelfTestDynamicTick100us != UINT32_MAX) { PlsrSelfTestDynamicTick100us++; } switch (PlsrSelfTestDynamicTick100us) { case 10000UL: /* 1.0s: 1000 -> 4000Hz. */ PlsrSelfTestLiveFrequencyHz = 4000; PlsrSelfTestDynamicPhase = 1U; break; case 15000UL: /* 1.5s: 4000 -> 500Hz. */ PlsrSelfTestLiveFrequencyHz = 500; PlsrSelfTestDynamicPhase = 2U; break; case 20000UL: /* 2.0s: zero selects the 1000Hz S2 default. */ PlsrSelfTestLiveFrequencyHz = 0; PlsrSelfTestDynamicPhase = 3U; break; case 22000UL: /* 2.2s: 8000 is clamped to the 5000Hz maximum. */ PlsrSelfTestLiveFrequencyHz = 8000; PlsrSelfTestDynamicPhase = 4U; break; case 27000UL: /* 2.7s: invalid value must retain the safe target. */ PlsrSelfTestLiveFrequencyHz = -1; PlsrSelfTestDynamicPhase = 5U; break; case 29000UL: /* 2.9s: recover and hold 2000Hz. */ PlsrSelfTestLiveFrequencyHz = 2000; PlsrSelfTestDynamicPhase = 6U; PlsrSelfTestDynamicEnabled = 0U; break; default: break; } } static uint8_t SelfTestValidateWords(void *context, PLSR_DEVICE_TYPE device, uint32_t firstAddress, uint32_t wordCount) { (void)context; if ((device > PLSR_DEVICE_FD) || (wordCount == 0UL)) { return 0U; } return (((uint64_t)firstAddress + wordCount) <= SELF_TEST_WORD_CAPACITY) ? 1U : 0U; } static uint8_t SelfTestReadWord(void *context, PLSR_DEVICE_TYPE device, uint32_t address, uint16_t *value) { (void)context; if ((device > PLSR_DEVICE_FD) || (value == NULL) || (address >= SELF_TEST_WORD_CAPACITY)) { return 0U; } *value = SelfTestWords[device][address]; return 1U; } static uint8_t SelfTestReadDwordLive(void *context, PLSR_DEVICE_TYPE device, uint32_t address, int32_t *value) { uint16_t lowWord; uint16_t highWord; if (value == NULL) { return 0U; } if ((device == PLSR_DEVICE_D) && (address == SELF_TEST_S0_BASE + 10UL)) { /* Aligned Cortex-M4 dword load: atomic source for the TIM6 ISR. */ *value = PlsrSelfTestLiveFrequencyHz; return 1U; } if ((SelfTestReadWord(context, device, address, &lowWord) == 0U) || (SelfTestReadWord(context, device, address + 1UL, &highWord) == 0U)) { return 0U; } *value = (int32_t)(((uint32_t)highWord << 16U) | lowWord); return 1U; } static uint8_t SelfTestReadBit(void *context, PLSR_DEVICE_TYPE device, uint32_t address, uint8_t *value) { (void)context; (void)device; (void)address; *value = 0U; return 1U; } static void SelfTestWriteDword(PLSR_DEVICE_TYPE device, uint32_t address, uint32_t value) { SelfTestWords[device][address] = (uint16_t)(value & 0xFFFFUL); 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; (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); /* 方向端子 Y4（PULSE/DIR 模式必需，接线参数）。 */ (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); (void)PlcDeviceWriteSfd(912U, 0U); (void)PlcDeviceWriteSfd(915U, 0xFFFFU); /* S0：3 段，H00 完成，顺序跳转。 */ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 3U); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10U, 2000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12U, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 20U, 5000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 22U, 6000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 30U, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 32U, (uint32_t)(int32_t)-500); /* S1：相对模式，起始段 0（=段1）。 */ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U); (void)memset(&call, 0, sizeof(call)); call.sequence = 0xA5A5UL; 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_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); (void)PlcDeviceWriteSfd(912U, 0U); (void)PlcDeviceWriteSfd(915U, 0xFFFFU); /* 当量换算后物理最高速度=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); } PLSR_RESULT PlsrProtectionSelfTestQueue(void) { PLSR_CALL call; PLSR_COMMAND command; PLSR_RESULT result; (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); /* Pulse unit, soft limits enabled, 1 pulse per position unit. */ (void)PlcDeviceWriteSfd(900U, (1U << 2U)); SelfTestWriteSfdDword(902U, 1UL); SelfTestWriteSfdDword(904U, 1UL); (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); (void)PlcDeviceWriteSfd(907U, 10U); (void)PlcDeviceWriteSfd(912U, 0U); (void)PlcDeviceWriteSfd(915U, 0xFFFFU); SelfTestWriteSfdDword(930U, 500UL); SelfTestWriteSfdDword(932U, (uint32_t)(int32_t)-500); /* K1: 1000Hz, 100ms acceleration/deceleration, 1ms refresh. */ SelfTestWriteSfdDword(950U, 1000UL); (void)PlcDeviceWriteSfd(952U, 100U); (void)PlcDeviceWriteSfd(953U, 100U); (void)PlcDeviceWriteSfd(954U, 0U); (void)PlcDeviceWriteSfd(955U, 0U); SelfTestWriteSfdDword(956U, 100000UL); SelfTestWriteSfdDword(958U, 1000UL); SelfTestWriteSfdDword(960U, 0UL); (void)PlcDeviceWriteSfd(962U, 50U); (void)PlcDeviceWriteSfd(963U, 0U); (void)PlcDeviceWriteSfd(964U, 0U); SelfTestWriteSfdDword(966U, 2000UL); SelfTestWriteSfdDword(968U, 200UL); /* One relative segment requests +10000 pulses; +500 must stop it. */ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 1U); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10U, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12U, 10000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U); (void)memset(&command, 0, sizeof(command)); command.sequence = 0xA5A7UL; command.axis = 0U; command.opcode = PLSR_CMD_SET_POSITION; command.argument = 0; result = PlsrPostCommand(&command); if (result != PLSR_RESULT_QUEUED) { return result; } (void)memset(&call, 0, sizeof(call)); call.sequence = 0xA5A8UL; 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); } PLSR_RESULT PlsrFourAxisSelfTestQueue(void) { static const uint16_t s0Base[PLSR_AXIS_COUNT] = { 10U, 40U, 70U, 100U }; static const uint16_t s1Base[PLSR_AXIS_COUNT] = { 160U, 164U, 168U, 172U }; static const uint32_t frequencyHz[PLSR_AXIS_COUNT] = { 1000UL, 2000UL, 3000UL, 4000UL }; static const int32_t pulseCount[PLSR_AXIS_COUNT] = { 1000, 2000, 3000, 4000 }; PLSR_CALL call; PLSR_COMMAND command; PLSR_RESULT result; uint16_t commonBase; uint16_t setBase; uint8_t axis; (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++) { commonBase = (uint16_t)(900U + (uint16_t)axis * SELF_TEST_SFD_AXIS_STRIDE); setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET); /* Pulse unit, PULSE/DIR, no limit input, Q4..Q7 as DIR. */ (void)PlcDeviceWriteSfd(commonBase, 0U); SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL); SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), (uint16_t)(SELF_TEST_DIR_POINT + axis)); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU); /* K1 has no ramp so all four channels keep an exact fixed rate. */ SelfTestWriteSfdDword(setBase, frequencyHz[axis]); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 8U), frequencyHz[axis]); SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL); SelfTestWriteDword(PLSR_DEVICE_D, s0Base[axis], 1U); SelfTestWriteDword(PLSR_DEVICE_D, (uint32_t)s0Base[axis] + 10UL, frequencyHz[axis]); SelfTestWriteDword(PLSR_DEVICE_D, (uint32_t)s0Base[axis] + 12UL, (uint32_t)pulseCount[axis]); SelfTestWriteDword(PLSR_DEVICE_D, s1Base[axis], 0U); /* Make the board-test result independent of a previously restored * Backup SRAM position. */ (void)memset(&command, 0, sizeof(command)); command.sequence = 0xA500UL + axis; command.axis = axis; command.opcode = PLSR_CMD_SET_POSITION; command.argument = 0; result = PlsrPostCommand(&command); if (result != PLSR_RESULT_QUEUED) { return result; } (void)memset(&call, 0, sizeof(call)); call.sequence = 0xA600UL + axis; 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 = s0Base[axis]; call.s1.device = PLSR_DEVICE_D; call.s1.address = s1Base[axis]; call.s2.type = PLSR_OPERAND_CONSTANT; call.s2.constant = 1; call.dAxis = axis; call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR; result = PlsrPostCall(&call); if (result != PLSR_RESULT_QUEUED) { return result; } } return PLSR_RESULT_QUEUED; } PLSR_RESULT PlsrBacklashSelfTestQueue(void) { PLSR_CALL call; PLSR_COMMAND command; PLSR_RESULT result; (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); /* Pulse unit, Q4 direction, +10/-20 pulse backlash. */ (void)PlcDeviceWriteSfd(900U, 0U); SelfTestWriteSfdDword(902U, 1UL); SelfTestWriteSfdDword(904U, 1UL); (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); (void)PlcDeviceWriteSfd(907U, 10U); (void)PlcDeviceWriteSfd(908U, 10U); (void)PlcDeviceWriteSfd(909U, 20U); (void)PlcDeviceWriteSfd(912U, 0U); (void)PlcDeviceWriteSfd(915U, 0xFFFFU); /* K1 user segments are fixed 1kHz. Backlash blocks use a 20ms * acceleration/deceleration parameter. */ SelfTestWriteSfdDword(950U, 1000UL); (void)PlcDeviceWriteSfd(952U, 0U); (void)PlcDeviceWriteSfd(953U, 0U); (void)PlcDeviceWriteSfd(954U, 20U); (void)PlcDeviceWriteSfd(955U, 0U); SelfTestWriteSfdDword(956U, 100000UL); SelfTestWriteSfdDword(958U, 1000UL); SelfTestWriteSfdDword(960U, 0UL); (void)PlcDeviceWriteSfd(962U, 50U); (void)PlcDeviceWriteSfd(963U, 0U); (void)PlcDeviceWriteSfd(964U, 0U); SelfTestWriteSfdDword(966U, 2000UL); SelfTestWriteSfdDword(968U, 200UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 3U); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10U, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12U, 200UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 20U, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 22U, (uint32_t)(int32_t)-200); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 30U, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 32U, 100UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U); (void)memset(&command, 0, sizeof(command)); command.sequence = 0xA700UL; command.axis = 0U; command.opcode = PLSR_CMD_SET_POSITION; command.argument = 0; result = PlsrPostCommand(&command); if (result != PLSR_RESULT_QUEUED) { return result; } (void)memset(&call, 0, sizeof(call)); call.sequence = 0xA701UL; 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); } PLSR_RESULT PlsrDirectionLogicSelfTestQueue(void) { static const uint16_t s0Base[2] = {10U, 40U}; static const uint16_t s1Base[2] = {160U, 164U}; static const uint8_t directionPoint[2] = {4U, 3U}; PLSR_CALL call; PLSR_COMMAND command; PLSR_RESULT result; uint16_t commonBase; uint16_t setBase; uint8_t axis; (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); for (axis = 0U; axis < 2U; axis++) { commonBase = (uint16_t)(900U + (uint16_t)axis * SELF_TEST_SFD_AXIS_STRIDE); setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET); /* Axis 0 uses positive logic; axis 1 uses negative logic. */ (void)PlcDeviceWriteSfd(commonBase, (axis == 0U) ? 0U : (1U << 1U)); SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL); SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), directionPoint[axis]); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU); SelfTestWriteSfdDword(setBase, 1000UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL); SelfTestWriteDword(PLSR_DEVICE_D, s0Base[axis], 2U); SelfTestWriteDword(PLSR_DEVICE_D, (uint32_t)s0Base[axis] + 10UL, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, (uint32_t)s0Base[axis] + 12UL, 200UL); SelfTestWriteDword(PLSR_DEVICE_D, (uint32_t)s0Base[axis] + 20UL, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, (uint32_t)s0Base[axis] + 22UL, (uint32_t)(int32_t)-200); SelfTestWriteDword(PLSR_DEVICE_D, s1Base[axis], 0U); (void)memset(&command, 0, sizeof(command)); command.sequence = 0xA800UL + axis; command.axis = axis; command.opcode = PLSR_CMD_SET_POSITION; command.argument = 0; result = PlsrPostCommand(&command); if (result != PLSR_RESULT_QUEUED) { return result; } (void)memset(&call, 0, sizeof(call)); call.sequence = 0xA810UL + axis; 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 = s0Base[axis]; call.s1.device = PLSR_DEVICE_D; call.s1.address = s1Base[axis]; call.s2.type = PLSR_OPERAND_CONSTANT; call.s2.constant = 1; call.dAxis = axis; call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR; result = PlsrPostCall(&call); if (result != PLSR_RESULT_QUEUED) { return result; } } return PLSR_RESULT_QUEUED; } PLSR_RESULT PlsrCwCcwSelfTestQueue(void) { const uint16_t commonBase = 900U; const uint16_t setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET); PLSR_CALL call; PLSR_COMMAND command; PLSR_RESULT result; (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); (void)PlcDeviceWriteSfd(commonBase, 0U); SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL); SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), SELF_TEST_DIR_POINT); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU); SelfTestWriteSfdDword(setBase, 2000UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 2U); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10UL, 2000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12UL, 300UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 20UL, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 22UL, (uint32_t)(int32_t)-200); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U); (void)memset(&command, 0, sizeof(command)); command.sequence = 0xA900UL; command.axis = 0U; command.opcode = PLSR_CMD_SET_POSITION; result = PlsrPostCommand(&command); if (result != PLSR_RESULT_QUEUED) { return result; } (void)memset(&call, 0, sizeof(call)); call.sequence = 0xA901UL; 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_CW_CCW; return PlsrPostCall(&call); } PLSR_RESULT PlsrFastRefreshSelfTestQueue(void) { static const uint16_t s0Base[2] = {10U, 40U}; static const uint16_t s1Base[2] = {160U, 164U}; static const uint8_t directionPoint[2] = {4U, 3U}; PLSR_CALL call; PLSR_COMMAND command; PLSR_RESULT result; uint16_t commonBase; uint16_t setBase; uint8_t axis; (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); for (axis = 0U; axis < 2U; axis++) { commonBase = (uint16_t)(900U + (uint16_t)axis * SELF_TEST_SFD_AXIS_STRIDE); setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET); (void)PlcDeviceWriteSfd(commonBase, 0U); SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL); SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), directionPoint[axis]); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU); SelfTestWriteSfdDword(setBase, 5000UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 100U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 100U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U); /* Linear curve keeps the 1ms/0.1ms update granularity visible. */ (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 100UL); SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 100UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), (axis == 0U) ? 0U : 2U); SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL); SelfTestWriteDword(PLSR_DEVICE_D, s0Base[axis], 1U); SelfTestWriteDword(PLSR_DEVICE_D, (uint32_t)s0Base[axis] + 10UL, 5000UL); SelfTestWriteDword(PLSR_DEVICE_D, (uint32_t)s0Base[axis] + 12UL, 2000UL); SelfTestWriteDword(PLSR_DEVICE_D, s1Base[axis], 0U); (void)memset(&command, 0, sizeof(command)); command.sequence = 0xAA00UL + axis; command.axis = axis; command.opcode = PLSR_CMD_SET_POSITION; result = PlsrPostCommand(&command); if (result != PLSR_RESULT_QUEUED) { return result; } (void)memset(&call, 0, sizeof(call)); call.sequence = 0xAA10UL + axis; 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 = s0Base[axis]; call.s1.device = PLSR_DEVICE_D; call.s1.address = s1Base[axis]; call.s2.type = PLSR_OPERAND_CONSTANT; call.s2.constant = 1; call.dAxis = axis; call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR; result = PlsrPostCall(&call); if (result != PLSR_RESULT_QUEUED) { return result; } } return PLSR_RESULT_QUEUED; } PLSR_RESULT PlsrDynamicFrequencySelfTestQueue(void) { const uint16_t commonBase = 900U; const uint16_t setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET); PLSR_CALL call; PLSR_COMMAND command; PLSR_RESULT result; (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); PlsrSelfTestLiveFrequencyHz = 1000; PlsrSelfTestDynamicTick100us = 0UL; PlsrSelfTestDynamicPhase = 0U; PlsrSelfTestDynamicEnabled = 1U; PlsrSetControlTickHook(PlsrSelfTestControlTick100us); (void)PlcDeviceWriteSfd(commonBase, 0U); SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL); SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), 4U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU); /* 1000Hz default, 5000Hz maximum, 10Hz/ms slope, 0.1ms refresh. */ SelfTestWriteSfdDword(setBase, 1000UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 100U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 100U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 5000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 2U); SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 1U); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10UL, 1000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12UL, 100000UL); SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U); (void)memset(&command, 0, sizeof(command)); command.sequence = 0xAB00UL; command.axis = 0U; command.opcode = PLSR_CMD_SET_POSITION; result = PlsrPostCommand(&command); if (result != PLSR_RESULT_QUEUED) { return result; } (void)memset(&call, 0, sizeof(call)); call.sequence = 0xAB01UL; call.source.context = NULL; call.source.validateWords = SelfTestValidateWords; call.source.readWord = SelfTestReadWord; call.source.readDword = SelfTestReadDwordLive; 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); } PLSR_RESULT PlsrModbusDataSelfTestQueue(void) { const uint16_t commonBase = 900U; const uint16_t setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET); uint16_t s0Words[20] = {0U}; uint16_t s1Words[4] = {0U}; PLSR_CALL call; PLSR_COMMAND command; PLSR_RESULT result; /* P12 uses D1000 as S0 and D1100 as S1. D1010/D1011 is the live * current-segment frequency written atomically by Modbus function 0x10. */ s0Words[0] = 1U; s0Words[10] = 1000U; s0Words[11] = 0U; s0Words[12] = (uint16_t)(100000UL & 0xFFFFUL); s0Words[13] = (uint16_t)(100000UL >> 16U); if ((ModbusDataWriteWords(MODBUS_DATA_DEVICE_D, SELF_TEST_MODBUS_S0_BASE, s0Words, 20UL) == 0U) || (ModbusDataWriteWords(MODBUS_DATA_DEVICE_D, SELF_TEST_MODBUS_S1_BASE, s1Words, 4UL) == 0U)) { return PLSR_RESULT_DATA_ACCESS; } (void)PlcDeviceWriteSfd(commonBase, 0U); SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL); SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), 4U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU); /* Same limits as P11: 1000Hz default, 5000Hz maximum, 10Hz/ms ramp, * and a 0.1ms live-frequency refresh. */ SelfTestWriteSfdDword(setBase, 1000UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 100U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 100U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 5000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 2U); SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL); (void)memset(&command, 0, sizeof(command)); command.sequence = 0xAC00UL; command.axis = 0U; command.opcode = PLSR_CMD_SET_POSITION; result = PlsrPostCommand(&command); if (result != PLSR_RESULT_QUEUED) { return result; } (void)memset(&call, 0, sizeof(call)); call.sequence = 0xAC01UL; PlsrModbusDataSourceInit(&call.source); call.s0.device = PLSR_DEVICE_D; call.s0.address = SELF_TEST_MODBUS_S0_BASE; call.s1.device = PLSR_DEVICE_D; call.s1.address = SELF_TEST_MODBUS_S1_BASE; call.s2.type = PLSR_OPERAND_CONSTANT; call.s2.constant = 1; call.dAxis = 0U; call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR; return PlsrPostCall(&call); } PLSR_RESULT PlsrModbusControlSelfTestPrepare(void) { const uint16_t commonBase = 900U; const uint16_t setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET); (void)PlcDeviceWriteSfd(commonBase, 0U); SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL); SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), 4U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU); /* K1: 1000Hz default/start, 5000Hz maximum, 100ms ramps, 1ms refresh. */ SelfTestWriteSfdDword(setBase, 1000UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 100U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 100U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 5000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U); (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U); SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL); SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL); return PLSR_RESULT_OK; }