Modbus-rtu/PLSR/Src/plsr_self_test.c

#include "plsr_self_test.h"
#include "plc_device.h"
#include "plsr_core.h"
#include "plsr_job.h"
#include <string.h>

/* 上电自测（验证后可删除）：
 * - 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     (64U)
#define SELF_TEST_S0_BASE           (10U)
#define SELF_TEST_S1_BASE           (60U)
#define SELF_TEST_DIR_POINT         (4U)

static uint16_t SelfTestWords[3][SELF_TEST_WORD_CAPACITY];

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 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);
}