yuwenhao
/
Modbus-rtu
1
0
포크 0
코드 이슈 0 풀 리퀘스트 0 릴리즈 0 위키 활동
25개 이상의 토픽을 선택하실 수 없습니다. Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
41 커밋
3 브랜치
138 MiB
 
 
 
 
 
 
트리: fdbbe71aea
브랜치 태그
${ item.name }
${ searchTerm } 브랜치 생성
from 'fdbbe71aea'
${ noResults }
Modbus-rtu/PLSR/Src/plsr_self_test.c

967 lines
36 KiB
Raw Blame 히스토리 

  1. #include "plsr_self_test.h"

  2. #include "plc_device.h"

  3. #include "modbus_data_store.h"

  4. #include "plsr_core.h"

  5. #include "plsr_job.h"

  6. #include "plsr_modbus_data.h"

  7. #include <string.h>

  8. 

  9. /* 上电自测（验证后可删除）：

  10.  * - AB 模式使用 Q0(A)/Q1(B)，其余用出厂默认参数（K1）

  11.  * - 任务：3 段完整 AB 周期（H00 完成，顺序衔接）：

  12.  *   段1：2000Hz / +1000 周期（A 超前 B）

  13.  *   段2：5000Hz / +6000 周期（A 超前 B）

  14.  *   段3：1000Hz / -500 周期（B 超前 A，验证反向）

  15.  * 数据源为静态数组，仅自测使用（正式 D 设备适配器见 Modbus 阶段）。 */

  16. 

  17. #define SELF_TEST_WORD_CAPACITY     (192U)

  18. #define SELF_TEST_S0_BASE           (10U)

  19. #define SELF_TEST_S1_BASE           (60U)

  20. #define SELF_TEST_DIR_POINT         (4U)

  21. #define SELF_TEST_SFD_AXIS_STRIDE   (130U)

  22. #define SELF_TEST_SFD_SET_OFFSET    (50U)

  23. #define SELF_TEST_MODBUS_S0_BASE    (1000UL)

  24. #define SELF_TEST_MODBUS_S1_BASE    (1100UL)

  25. 

  26. static uint16_t SelfTestWords[3][SELF_TEST_WORD_CAPACITY];

  27. volatile int32_t PlsrSelfTestLiveFrequencyHz;

  28. volatile uint32_t PlsrSelfTestDynamicTick100us;

  29. volatile uint8_t PlsrSelfTestDynamicPhase;

  30. static volatile uint8_t PlsrSelfTestDynamicEnabled;

  31. 

  32. void PlsrSelfTestControlTick100us(void)

  33. {

  34.     if (PlsrSelfTestDynamicEnabled == 0U)

  35.     {

  36.         return;

  37.     }

  38.     if (PlsrSelfTestDynamicTick100us != UINT32_MAX)

  39.     {

  40.         PlsrSelfTestDynamicTick100us++;

  41.     }

  42.     switch (PlsrSelfTestDynamicTick100us)

  43.     {

  44.         case 10000UL: /* 1.0s: 1000 -> 4000Hz. */

  45.             PlsrSelfTestLiveFrequencyHz = 4000;

  46.             PlsrSelfTestDynamicPhase = 1U;

  47.             break;

  48. 

  49.         case 15000UL: /* 1.5s: 4000 -> 500Hz. */

  50.             PlsrSelfTestLiveFrequencyHz = 500;

  51.             PlsrSelfTestDynamicPhase = 2U;

  52.             break;

  53. 

  54.         case 20000UL: /* 2.0s: zero selects the 1000Hz S2 default. */

  55.             PlsrSelfTestLiveFrequencyHz = 0;

  56.             PlsrSelfTestDynamicPhase = 3U;

  57.             break;

  58. 

  59.         case 22000UL: /* 2.2s: 8000 is clamped to the 5000Hz maximum. */

  60.             PlsrSelfTestLiveFrequencyHz = 8000;

  61.             PlsrSelfTestDynamicPhase = 4U;

  62.             break;

  63. 

  64.         case 27000UL: /* 2.7s: invalid value must retain the safe target. */

  65.             PlsrSelfTestLiveFrequencyHz = -1;

  66.             PlsrSelfTestDynamicPhase = 5U;

  67.             break;

  68. 

  69.         case 29000UL: /* 2.9s: recover and hold 2000Hz. */

  70.             PlsrSelfTestLiveFrequencyHz = 2000;

  71.             PlsrSelfTestDynamicPhase = 6U;

  72.             PlsrSelfTestDynamicEnabled = 0U;

  73.             break;

  74. 

  75.         default:

  76.             break;

  77.     }

  78. }

  79. 

  80. static uint8_t SelfTestValidateWords(void *context,

  81.                                      PLSR_DEVICE_TYPE device,

  82.                                      uint32_t firstAddress,

  83.                                      uint32_t wordCount)

  84. {

  85.     (void)context;

  86.     if ((device > PLSR_DEVICE_FD) || (wordCount == 0UL))

  87.     {

  88.         return 0U;

  89.     }

  90.     return (((uint64_t)firstAddress + wordCount)

  91.             <= SELF_TEST_WORD_CAPACITY)

  92.                ? 1U

  93.                : 0U;

  94. }

  95. 

  96. static uint8_t SelfTestReadWord(void *context,

  97.                                 PLSR_DEVICE_TYPE device,

  98.                                 uint32_t address,

  99.                                 uint16_t *value)

  100. {

  101.     (void)context;

  102.     if ((device > PLSR_DEVICE_FD) || (value == NULL)

  103.         || (address >= SELF_TEST_WORD_CAPACITY))

  104.     {

  105.         return 0U;

  106.     }

  107.     *value = SelfTestWords[device][address];

  108.     return 1U;

  109. }

  110. 

  111. static uint8_t SelfTestReadDwordLive(void *context,

  112.                                      PLSR_DEVICE_TYPE device,

  113.                                      uint32_t address,

  114.                                      int32_t *value)

  115. {

  116.     uint16_t lowWord;

  117.     uint16_t highWord;

  118. 

  119.     if (value == NULL)

  120.     {

  121.         return 0U;

  122.     }

  123.     if ((device == PLSR_DEVICE_D)

  124.         && (address == SELF_TEST_S0_BASE + 10UL))

  125.     {

  126.         /* Aligned Cortex-M4 dword load: atomic source for the TIM6 ISR. */

  127.         *value = PlsrSelfTestLiveFrequencyHz;

  128.         return 1U;

  129.     }

  130.     if ((SelfTestReadWord(context, device, address, &lowWord) == 0U)

  131.         || (SelfTestReadWord(context,

  132.                              device,

  133.                              address + 1UL,

  134.                              &highWord) == 0U))

  135.     {

  136.         return 0U;

  137.     }

  138.     *value = (int32_t)(((uint32_t)highWord << 16U) | lowWord);

  139.     return 1U;

  140. }

  141. 

  142. static uint8_t SelfTestReadBit(void *context,

  143.                                PLSR_DEVICE_TYPE device,

  144.                                uint32_t address,

  145.                                uint8_t *value)

  146. {

  147.     (void)context;

  148.     (void)device;

  149.     (void)address;

  150.     *value = 0U;

  151.     return 1U;

  152. }

  153. 

  154. static void SelfTestWriteDword(PLSR_DEVICE_TYPE device,

  155.                                uint32_t address,

  156.                                uint32_t value)

  157. {

  158.     SelfTestWords[device][address] = (uint16_t)(value & 0xFFFFUL);

  159.     SelfTestWords[device][address + 1UL] = (uint16_t)(value >> 16U);

  160. }

  161. 

  162. static void SelfTestWriteSfdDword(uint16_t address, uint32_t value)

  163. {

  164.     (void)PlcDeviceWriteSfd(address, (uint16_t)(value & 0xFFFFUL));

  165.     (void)PlcDeviceWriteSfd((uint16_t)(address + 1U),

  166.                             (uint16_t)(value >> 16U));

  167. }

  168. 

  169. PLSR_RESULT PlsrSelfTestQueue(void)

  170. {

  171.     PLSR_CALL call;

  172. 

  173.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  174. 

  175.     /* 方向端子 Y4（PULSE/DIR 模式必需，接线参数）。 */

  176.     (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT);

  177.     (void)PlcDeviceWriteSfd(912U, 0U);

  178.     (void)PlcDeviceWriteSfd(915U, 0xFFFFU);

  179. 

  180.     /* S0：3 段，H00 完成，顺序跳转。 */

  181.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 3U);

  182.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10U, 2000UL);

  183.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12U, 1000UL);

  184.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 20U, 5000UL);

  185.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 22U, 6000UL);

  186.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 30U, 1000UL);

  187.     SelfTestWriteDword(PLSR_DEVICE_D,

  188.                        SELF_TEST_S0_BASE + 32U,

  189.                        (uint32_t)(int32_t)-500);

  190. 

  191.     /* S1：相对模式，起始段 0（=段1）。 */

  192.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

  193. 

  194.     (void)memset(&call, 0, sizeof(call));

  195.     call.sequence = 0xA5A5UL;

  196.     call.source.context = NULL;

  197.     call.source.validateWords = SelfTestValidateWords;

  198.     call.source.readWord = SelfTestReadWord;

  199.     call.source.readBit = SelfTestReadBit;

  200.     call.s0.device = PLSR_DEVICE_D;

  201.     call.s0.address = SELF_TEST_S0_BASE;

  202.     call.s1.device = PLSR_DEVICE_D;

  203.     call.s1.address = SELF_TEST_S1_BASE;

  204.     call.s2.type = PLSR_OPERAND_CONSTANT;

  205.     call.s2.constant = 1;

  206.     call.dAxis = 0U;

  207.     call.outputModeOverride = PLSR_OUTPUT_AB;

  208.     return PlsrPostCall(&call);

  209. }

  210. 

  211. PLSR_RESULT PlsrEquivalentSelfTestQueue(void)

  212. {

  213.     PLSR_CALL call;

  214. 

  215.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  216. 

  217.     /* SFD900 Bit10~8=001（1um当量）；3脉冲/2单位。 */

  218.     (void)PlcDeviceWriteSfd(900U, (1U << 8U));

  219.     SelfTestWriteSfdDword(902U, 3UL);

  220.     SelfTestWriteSfdDword(904U, 2UL);

  221.     (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT);

  222.     (void)PlcDeviceWriteSfd(907U, 10U);

  223.     (void)PlcDeviceWriteSfd(912U, 0U);

  224.     (void)PlcDeviceWriteSfd(915U, 0xFFFFU);

  225.     /* 当量换算后物理最高速度=90000Hz，不超过硬件100kHz。 */

  226.     SelfTestWriteSfdDword(956U, 60000UL);

  227. 

  228.     /* 板端可观察版本：两段各1001工程单位。

  229.      * 3脉冲/2单位带余数换算后分别输出1501、1502脉冲，累计3003脉冲；

  230.      * 1500Hz附近持续约2s，避免原3脉冲自检在逻辑分析仪启动前已经结束。 */

  231.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 2U);

  232.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10U, 1000UL);

  233.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12U, 1001UL);

  234.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 20U, 1000UL);

  235.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 22U, 1001UL);

  236.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

  237. 

  238.     (void)memset(&call, 0, sizeof(call));

  239.     call.sequence = 0xA5A6UL;

  240.     call.source.context = NULL;

  241.     call.source.validateWords = SelfTestValidateWords;

  242.     call.source.readWord = SelfTestReadWord;

  243.     call.source.readBit = SelfTestReadBit;

  244.     call.s0.device = PLSR_DEVICE_D;

  245.     call.s0.address = SELF_TEST_S0_BASE;

  246.     call.s1.device = PLSR_DEVICE_D;

  247.     call.s1.address = SELF_TEST_S1_BASE;

  248.     call.s2.type = PLSR_OPERAND_CONSTANT;

  249.     call.s2.constant = 1;

  250.     call.dAxis = 0U;

  251.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  252.     return PlsrPostCall(&call);

  253. }

  254. 

  255. PLSR_RESULT PlsrProtectionSelfTestQueue(void)

  256. {

  257.     PLSR_CALL call;

  258.     PLSR_COMMAND command;

  259.     PLSR_RESULT result;

  260. 

  261.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  262. 

  263.     /* Pulse unit, soft limits enabled, 1 pulse per position unit. */

  264.     (void)PlcDeviceWriteSfd(900U, (1U << 2U));

  265.     SelfTestWriteSfdDword(902U, 1UL);

  266.     SelfTestWriteSfdDword(904U, 1UL);

  267.     (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT);

  268.     (void)PlcDeviceWriteSfd(907U, 10U);

  269.     (void)PlcDeviceWriteSfd(912U, 0U);

  270.     (void)PlcDeviceWriteSfd(915U, 0xFFFFU);

  271.     SelfTestWriteSfdDword(930U, 500UL);

  272.     SelfTestWriteSfdDword(932U, (uint32_t)(int32_t)-500);

  273. 

  274.     /* K1: 1000Hz, 100ms acceleration/deceleration, 1ms refresh. */

  275.     SelfTestWriteSfdDword(950U, 1000UL);

  276.     (void)PlcDeviceWriteSfd(952U, 100U);

  277.     (void)PlcDeviceWriteSfd(953U, 100U);

  278.     (void)PlcDeviceWriteSfd(954U, 0U);

  279.     (void)PlcDeviceWriteSfd(955U, 0U);

  280.     SelfTestWriteSfdDword(956U, 100000UL);

  281.     SelfTestWriteSfdDword(958U, 1000UL);

  282.     SelfTestWriteSfdDword(960U, 0UL);

  283.     (void)PlcDeviceWriteSfd(962U, 50U);

  284.     (void)PlcDeviceWriteSfd(963U, 0U);

  285.     (void)PlcDeviceWriteSfd(964U, 0U);

  286.     SelfTestWriteSfdDword(966U, 2000UL);

  287.     SelfTestWriteSfdDword(968U, 200UL);

  288. 

  289.     /* One relative segment requests +10000 pulses; +500 must stop it. */

  290.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 1U);

  291.     SelfTestWriteDword(PLSR_DEVICE_D,

  292.                        SELF_TEST_S0_BASE + 10U,

  293.                        1000UL);

  294.     SelfTestWriteDword(PLSR_DEVICE_D,

  295.                        SELF_TEST_S0_BASE + 12U,

  296.                        10000UL);

  297.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

  298. 

  299.     (void)memset(&command, 0, sizeof(command));

  300.     command.sequence = 0xA5A7UL;

  301.     command.axis = 0U;

  302.     command.opcode = PLSR_CMD_SET_POSITION;

  303.     command.argument = 0;

  304.     result = PlsrPostCommand(&command);

  305.     if (result != PLSR_RESULT_QUEUED)

  306.     {

  307.         return result;

  308.     }

  309. 

  310.     (void)memset(&call, 0, sizeof(call));

  311.     call.sequence = 0xA5A8UL;

  312.     call.source.context = NULL;

  313.     call.source.validateWords = SelfTestValidateWords;

  314.     call.source.readWord = SelfTestReadWord;

  315.     call.source.readBit = SelfTestReadBit;

  316.     call.s0.device = PLSR_DEVICE_D;

  317.     call.s0.address = SELF_TEST_S0_BASE;

  318.     call.s1.device = PLSR_DEVICE_D;

  319.     call.s1.address = SELF_TEST_S1_BASE;

  320.     call.s2.type = PLSR_OPERAND_CONSTANT;

  321.     call.s2.constant = 1;

  322.     call.dAxis = 0U;

  323.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  324.     return PlsrPostCall(&call);

  325. }

  326. 

  327. PLSR_RESULT PlsrFourAxisSelfTestQueue(void)

  328. {

  329.     static const uint16_t s0Base[PLSR_AXIS_COUNT] =

  330.     {

  331.         10U, 40U, 70U, 100U

  332.     };

  333.     static const uint16_t s1Base[PLSR_AXIS_COUNT] =

  334.     {

  335.         160U, 164U, 168U, 172U

  336.     };

  337.     static const uint32_t frequencyHz[PLSR_AXIS_COUNT] =

  338.     {

  339.         1000UL, 2000UL, 3000UL, 4000UL

  340.     };

  341.     static const int32_t pulseCount[PLSR_AXIS_COUNT] =

  342.     {

  343.         1000, 2000, 3000, 4000

  344.     };

  345.     PLSR_CALL call;

  346.     PLSR_COMMAND command;

  347.     PLSR_RESULT result;

  348.     uint16_t commonBase;

  349.     uint16_t setBase;

  350.     uint8_t axis;

  351. 

  352.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  353. 

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

  355.     {

  356.         commonBase = (uint16_t)(900U

  357.                                 + (uint16_t)axis

  358.                                       * SELF_TEST_SFD_AXIS_STRIDE);

  359.         setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  360. 

  361.         /* Pulse unit, PULSE/DIR, no limit input, Q4..Q7 as DIR. */

  362.         (void)PlcDeviceWriteSfd(commonBase, 0U);

  363.         SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL);

  364.         SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL);

  365.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U),

  366.                                 (uint16_t)(SELF_TEST_DIR_POINT + axis));

  367.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U);

  368.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U);

  369.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU);

  370. 

  371.         /* K1 has no ramp so all four channels keep an exact fixed rate. */

  372.         SelfTestWriteSfdDword(setBase, frequencyHz[axis]);

  373.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 0U);

  374.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 0U);

  375.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U);

  376.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U);

  377.         SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL);

  378.         SelfTestWriteSfdDword((uint16_t)(setBase + 8U),

  379.                               frequencyHz[axis]);

  380.         SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL);

  381.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U);

  382.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U);

  383.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U);

  384.         SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL);

  385.         SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL);

  386. 

  387.         SelfTestWriteDword(PLSR_DEVICE_D, s0Base[axis], 1U);

  388.         SelfTestWriteDword(PLSR_DEVICE_D,

  389.                            (uint32_t)s0Base[axis] + 10UL,

  390.                            frequencyHz[axis]);

  391.         SelfTestWriteDword(PLSR_DEVICE_D,

  392.                            (uint32_t)s0Base[axis] + 12UL,

  393.                            (uint32_t)pulseCount[axis]);

  394.         SelfTestWriteDword(PLSR_DEVICE_D, s1Base[axis], 0U);

  395. 

  396.         /* Make the board-test result independent of a previously restored

  397.          * Backup SRAM position. */

  398.         (void)memset(&command, 0, sizeof(command));

  399.         command.sequence = 0xA500UL + axis;

  400.         command.axis = axis;

  401.         command.opcode = PLSR_CMD_SET_POSITION;

  402.         command.argument = 0;

  403.         result = PlsrPostCommand(&command);

  404.         if (result != PLSR_RESULT_QUEUED)

  405.         {

  406.             return result;

  407.         }

  408. 

  409.         (void)memset(&call, 0, sizeof(call));

  410.         call.sequence = 0xA600UL + axis;

  411.         call.source.context = NULL;

  412.         call.source.validateWords = SelfTestValidateWords;

  413.         call.source.readWord = SelfTestReadWord;

  414.         call.source.readBit = SelfTestReadBit;

  415.         call.s0.device = PLSR_DEVICE_D;

  416.         call.s0.address = s0Base[axis];

  417.         call.s1.device = PLSR_DEVICE_D;

  418.         call.s1.address = s1Base[axis];

  419.         call.s2.type = PLSR_OPERAND_CONSTANT;

  420.         call.s2.constant = 1;

  421.         call.dAxis = axis;

  422.         call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  423.         result = PlsrPostCall(&call);

  424.         if (result != PLSR_RESULT_QUEUED)

  425.         {

  426.             return result;

  427.         }

  428.     }

  429.     return PLSR_RESULT_QUEUED;

  430. }

  431. 

  432. PLSR_RESULT PlsrBacklashSelfTestQueue(void)

  433. {

  434.     PLSR_CALL call;

  435.     PLSR_COMMAND command;

  436.     PLSR_RESULT result;

  437. 

  438.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  439. 

  440.     /* Pulse unit, Q4 direction, +10/-20 pulse backlash. */

  441.     (void)PlcDeviceWriteSfd(900U, 0U);

  442.     SelfTestWriteSfdDword(902U, 1UL);

  443.     SelfTestWriteSfdDword(904U, 1UL);

  444.     (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT);

  445.     (void)PlcDeviceWriteSfd(907U, 10U);

  446.     (void)PlcDeviceWriteSfd(908U, 10U);

  447.     (void)PlcDeviceWriteSfd(909U, 20U);

  448.     (void)PlcDeviceWriteSfd(912U, 0U);

  449.     (void)PlcDeviceWriteSfd(915U, 0xFFFFU);

  450. 

  451.     /* K1 user segments are fixed 1kHz.  Backlash blocks use a 20ms

  452.      * acceleration/deceleration parameter. */

  453.     SelfTestWriteSfdDword(950U, 1000UL);

  454.     (void)PlcDeviceWriteSfd(952U, 0U);

  455.     (void)PlcDeviceWriteSfd(953U, 0U);

  456.     (void)PlcDeviceWriteSfd(954U, 20U);

  457.     (void)PlcDeviceWriteSfd(955U, 0U);

  458.     SelfTestWriteSfdDword(956U, 100000UL);

  459.     SelfTestWriteSfdDword(958U, 1000UL);

  460.     SelfTestWriteSfdDword(960U, 0UL);

  461.     (void)PlcDeviceWriteSfd(962U, 50U);

  462.     (void)PlcDeviceWriteSfd(963U, 0U);

  463.     (void)PlcDeviceWriteSfd(964U, 0U);

  464.     SelfTestWriteSfdDword(966U, 2000UL);

  465.     SelfTestWriteSfdDword(968U, 200UL);

  466. 

  467.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 3U);

  468.     SelfTestWriteDword(PLSR_DEVICE_D,

  469.                        SELF_TEST_S0_BASE + 10U,

  470.                        1000UL);

  471.     SelfTestWriteDword(PLSR_DEVICE_D,

  472.                        SELF_TEST_S0_BASE + 12U,

  473.                        200UL);

  474.     SelfTestWriteDword(PLSR_DEVICE_D,

  475.                        SELF_TEST_S0_BASE + 20U,

  476.                        1000UL);

  477.     SelfTestWriteDword(PLSR_DEVICE_D,

  478.                        SELF_TEST_S0_BASE + 22U,

  479.                        (uint32_t)(int32_t)-200);

  480.     SelfTestWriteDword(PLSR_DEVICE_D,

  481.                        SELF_TEST_S0_BASE + 30U,

  482.                        1000UL);

  483.     SelfTestWriteDword(PLSR_DEVICE_D,

  484.                        SELF_TEST_S0_BASE + 32U,

  485.                        100UL);

  486.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

  487. 

  488.     (void)memset(&command, 0, sizeof(command));

  489.     command.sequence = 0xA700UL;

  490.     command.axis = 0U;

  491.     command.opcode = PLSR_CMD_SET_POSITION;

  492.     command.argument = 0;

  493.     result = PlsrPostCommand(&command);

  494.     if (result != PLSR_RESULT_QUEUED)

  495.     {

  496.         return result;

  497.     }

  498. 

  499.     (void)memset(&call, 0, sizeof(call));

  500.     call.sequence = 0xA701UL;

  501.     call.source.context = NULL;

  502.     call.source.validateWords = SelfTestValidateWords;

  503.     call.source.readWord = SelfTestReadWord;

  504.     call.source.readBit = SelfTestReadBit;

  505.     call.s0.device = PLSR_DEVICE_D;

  506.     call.s0.address = SELF_TEST_S0_BASE;

  507.     call.s1.device = PLSR_DEVICE_D;

  508.     call.s1.address = SELF_TEST_S1_BASE;

  509.     call.s2.type = PLSR_OPERAND_CONSTANT;

  510.     call.s2.constant = 1;

  511.     call.dAxis = 0U;

  512.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  513.     return PlsrPostCall(&call);

  514. }

  515. 

  516. PLSR_RESULT PlsrDirectionLogicSelfTestQueue(void)

  517. {

  518.     static const uint16_t s0Base[2] = {10U, 40U};

  519.     static const uint16_t s1Base[2] = {160U, 164U};

  520.     static const uint8_t directionPoint[2] = {4U, 3U};

  521.     PLSR_CALL call;

  522.     PLSR_COMMAND command;

  523.     PLSR_RESULT result;

  524.     uint16_t commonBase;

  525.     uint16_t setBase;

  526.     uint8_t axis;

  527. 

  528.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  529. 

  530.     for (axis = 0U; axis < 2U; axis++)

  531.     {

  532.         commonBase = (uint16_t)(900U

  533.                                 + (uint16_t)axis

  534.                                       * SELF_TEST_SFD_AXIS_STRIDE);

  535.         setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  536. 

  537.         /* Axis 0 uses positive logic; axis 1 uses negative logic. */

  538.         (void)PlcDeviceWriteSfd(commonBase,

  539.                                 (axis == 0U) ? 0U : (1U << 1U));

  540.         SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL);

  541.         SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL);

  542.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U),

  543.                                 directionPoint[axis]);

  544.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U);

  545.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U);

  546.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U);

  547.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U);

  548.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU);

  549. 

  550.         SelfTestWriteSfdDword(setBase, 1000UL);

  551.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 0U);

  552.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 0U);

  553.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U);

  554.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U);

  555.         SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL);

  556.         SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL);

  557.         SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL);

  558.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U);

  559.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U);

  560.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U);

  561.         SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL);

  562.         SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL);

  563. 

  564.         SelfTestWriteDword(PLSR_DEVICE_D, s0Base[axis], 2U);

  565.         SelfTestWriteDword(PLSR_DEVICE_D,

  566.                            (uint32_t)s0Base[axis] + 10UL,

  567.                            1000UL);

  568.         SelfTestWriteDword(PLSR_DEVICE_D,

  569.                            (uint32_t)s0Base[axis] + 12UL,

  570.                            200UL);

  571.         SelfTestWriteDword(PLSR_DEVICE_D,

  572.                            (uint32_t)s0Base[axis] + 20UL,

  573.                            1000UL);

  574.         SelfTestWriteDword(PLSR_DEVICE_D,

  575.                            (uint32_t)s0Base[axis] + 22UL,

  576.                            (uint32_t)(int32_t)-200);

  577.         SelfTestWriteDword(PLSR_DEVICE_D, s1Base[axis], 0U);

  578. 

  579.         (void)memset(&command, 0, sizeof(command));

  580.         command.sequence = 0xA800UL + axis;

  581.         command.axis = axis;

  582.         command.opcode = PLSR_CMD_SET_POSITION;

  583.         command.argument = 0;

  584.         result = PlsrPostCommand(&command);

  585.         if (result != PLSR_RESULT_QUEUED)

  586.         {

  587.             return result;

  588.         }

  589. 

  590.         (void)memset(&call, 0, sizeof(call));

  591.         call.sequence = 0xA810UL + axis;

  592.         call.source.context = NULL;

  593.         call.source.validateWords = SelfTestValidateWords;

  594.         call.source.readWord = SelfTestReadWord;

  595.         call.source.readBit = SelfTestReadBit;

  596.         call.s0.device = PLSR_DEVICE_D;

  597.         call.s0.address = s0Base[axis];

  598.         call.s1.device = PLSR_DEVICE_D;

  599.         call.s1.address = s1Base[axis];

  600.         call.s2.type = PLSR_OPERAND_CONSTANT;

  601.         call.s2.constant = 1;

  602.         call.dAxis = axis;

  603.         call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  604.         result = PlsrPostCall(&call);

  605.         if (result != PLSR_RESULT_QUEUED)

  606.         {

  607.             return result;

  608.         }

  609.     }

  610.     return PLSR_RESULT_QUEUED;

  611. }

  612. 

  613. PLSR_RESULT PlsrCwCcwSelfTestQueue(void)

  614. {

  615.     const uint16_t commonBase = 900U;

  616.     const uint16_t setBase =

  617.         (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  618.     PLSR_CALL call;

  619.     PLSR_COMMAND command;

  620.     PLSR_RESULT result;

  621. 

  622.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  623.     (void)PlcDeviceWriteSfd(commonBase, 0U);

  624.     SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL);

  625.     SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL);

  626.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U),

  627.                             SELF_TEST_DIR_POINT);

  628.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U);

  629.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U);

  630.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U);

  631.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U);

  632.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU);

  633. 

  634.     SelfTestWriteSfdDword(setBase, 2000UL);

  635.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 0U);

  636.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 0U);

  637.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U);

  638.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U);

  639.     SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL);

  640.     SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL);

  641.     SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL);

  642.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U);

  643.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U);

  644.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U);

  645.     SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL);

  646.     SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL);

  647. 

  648.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 2U);

  649.     SelfTestWriteDword(PLSR_DEVICE_D,

  650.                        SELF_TEST_S0_BASE + 10UL,

  651.                        2000UL);

  652.     SelfTestWriteDword(PLSR_DEVICE_D,

  653.                        SELF_TEST_S0_BASE + 12UL,

  654.                        300UL);

  655.     SelfTestWriteDword(PLSR_DEVICE_D,

  656.                        SELF_TEST_S0_BASE + 20UL,

  657.                        1000UL);

  658.     SelfTestWriteDword(PLSR_DEVICE_D,

  659.                        SELF_TEST_S0_BASE + 22UL,

  660.                        (uint32_t)(int32_t)-200);

  661.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

  662. 

  663.     (void)memset(&command, 0, sizeof(command));

  664.     command.sequence = 0xA900UL;

  665.     command.axis = 0U;

  666.     command.opcode = PLSR_CMD_SET_POSITION;

  667.     result = PlsrPostCommand(&command);

  668.     if (result != PLSR_RESULT_QUEUED)

  669.     {

  670.         return result;

  671.     }

  672. 

  673.     (void)memset(&call, 0, sizeof(call));

  674.     call.sequence = 0xA901UL;

  675.     call.source.context = NULL;

  676.     call.source.validateWords = SelfTestValidateWords;

  677.     call.source.readWord = SelfTestReadWord;

  678.     call.source.readBit = SelfTestReadBit;

  679.     call.s0.device = PLSR_DEVICE_D;

  680.     call.s0.address = SELF_TEST_S0_BASE;

  681.     call.s1.device = PLSR_DEVICE_D;

  682.     call.s1.address = SELF_TEST_S1_BASE;

  683.     call.s2.type = PLSR_OPERAND_CONSTANT;

  684.     call.s2.constant = 1;

  685.     call.dAxis = 0U;

  686.     call.outputModeOverride = PLSR_OUTPUT_CW_CCW;

  687.     return PlsrPostCall(&call);

  688. }

  689. 

  690. PLSR_RESULT PlsrFastRefreshSelfTestQueue(void)

  691. {

  692.     static const uint16_t s0Base[2] = {10U, 40U};

  693.     static const uint16_t s1Base[2] = {160U, 164U};

  694.     static const uint8_t directionPoint[2] = {4U, 3U};

  695.     PLSR_CALL call;

  696.     PLSR_COMMAND command;

  697.     PLSR_RESULT result;

  698.     uint16_t commonBase;

  699.     uint16_t setBase;

  700.     uint8_t axis;

  701. 

  702.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  703.     for (axis = 0U; axis < 2U; axis++)

  704.     {

  705.         commonBase = (uint16_t)(900U

  706.                                 + (uint16_t)axis

  707.                                       * SELF_TEST_SFD_AXIS_STRIDE);

  708.         setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  709.         (void)PlcDeviceWriteSfd(commonBase, 0U);

  710.         SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL);

  711.         SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL);

  712.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U),

  713.                                 directionPoint[axis]);

  714.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U);

  715.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U);

  716.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U);

  717.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U);

  718.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU);

  719. 

  720.         SelfTestWriteSfdDword(setBase, 5000UL);

  721.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 100U);

  722.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 100U);

  723.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U);

  724.         /* Linear curve keeps the 1ms/0.1ms update granularity visible. */

  725.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U);

  726.         SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL);

  727.         SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 100UL);

  728.         SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 100UL);

  729.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U);

  730.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U);

  731.         (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U),

  732.                                 (axis == 0U) ? 0U : 2U);

  733.         SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL);

  734.         SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL);

  735. 

  736.         SelfTestWriteDword(PLSR_DEVICE_D, s0Base[axis], 1U);

  737.         SelfTestWriteDword(PLSR_DEVICE_D,

  738.                            (uint32_t)s0Base[axis] + 10UL,

  739.                            5000UL);

  740.         SelfTestWriteDword(PLSR_DEVICE_D,

  741.                            (uint32_t)s0Base[axis] + 12UL,

  742.                            2000UL);

  743.         SelfTestWriteDword(PLSR_DEVICE_D, s1Base[axis], 0U);

  744. 

  745.         (void)memset(&command, 0, sizeof(command));

  746.         command.sequence = 0xAA00UL + axis;

  747.         command.axis = axis;

  748.         command.opcode = PLSR_CMD_SET_POSITION;

  749.         result = PlsrPostCommand(&command);

  750.         if (result != PLSR_RESULT_QUEUED)

  751.         {

  752.             return result;

  753.         }

  754. 

  755.         (void)memset(&call, 0, sizeof(call));

  756.         call.sequence = 0xAA10UL + axis;

  757.         call.source.context = NULL;

  758.         call.source.validateWords = SelfTestValidateWords;

  759.         call.source.readWord = SelfTestReadWord;

  760.         call.source.readBit = SelfTestReadBit;

  761.         call.s0.device = PLSR_DEVICE_D;

  762.         call.s0.address = s0Base[axis];

  763.         call.s1.device = PLSR_DEVICE_D;

  764.         call.s1.address = s1Base[axis];

  765.         call.s2.type = PLSR_OPERAND_CONSTANT;

  766.         call.s2.constant = 1;

  767.         call.dAxis = axis;

  768.         call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  769.         result = PlsrPostCall(&call);

  770.         if (result != PLSR_RESULT_QUEUED)

  771.         {

  772.             return result;

  773.         }

  774.     }

  775.     return PLSR_RESULT_QUEUED;

  776. }

  777. 

  778. PLSR_RESULT PlsrDynamicFrequencySelfTestQueue(void)

  779. {

  780.     const uint16_t commonBase = 900U;

  781.     const uint16_t setBase =

  782.         (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  783.     PLSR_CALL call;

  784.     PLSR_COMMAND command;

  785.     PLSR_RESULT result;

  786. 

  787.     (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

  788.     PlsrSelfTestLiveFrequencyHz = 1000;

  789.     PlsrSelfTestDynamicTick100us = 0UL;

  790.     PlsrSelfTestDynamicPhase = 0U;

  791.     PlsrSelfTestDynamicEnabled = 1U;

  792.     PlsrSetControlTickHook(PlsrSelfTestControlTick100us);

  793.     (void)PlcDeviceWriteSfd(commonBase, 0U);

  794.     SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL);

  795.     SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL);

  796.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), 4U);

  797.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U);

  798.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U);

  799.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U);

  800.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U);

  801.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU);

  802. 

  803.     /* 1000Hz default, 5000Hz maximum, 10Hz/ms slope, 0.1ms refresh. */

  804.     SelfTestWriteSfdDword(setBase, 1000UL);

  805.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 100U);

  806.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 100U);

  807.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U);

  808.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U);

  809.     SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 5000UL);

  810.     SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL);

  811.     SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL);

  812.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U);

  813.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U);

  814.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 2U);

  815.     SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL);

  816.     SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL);

  817. 

  818.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 1U);

  819.     SelfTestWriteDword(PLSR_DEVICE_D,

  820.                        SELF_TEST_S0_BASE + 10UL,

  821.                        1000UL);

  822.     SelfTestWriteDword(PLSR_DEVICE_D,

  823.                        SELF_TEST_S0_BASE + 12UL,

  824.                        100000UL);

  825.     SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

  826. 

  827.     (void)memset(&command, 0, sizeof(command));

  828.     command.sequence = 0xAB00UL;

  829.     command.axis = 0U;

  830.     command.opcode = PLSR_CMD_SET_POSITION;

  831.     result = PlsrPostCommand(&command);

  832.     if (result != PLSR_RESULT_QUEUED)

  833.     {

  834.         return result;

  835.     }

  836. 

  837.     (void)memset(&call, 0, sizeof(call));

  838.     call.sequence = 0xAB01UL;

  839.     call.source.context = NULL;

  840.     call.source.validateWords = SelfTestValidateWords;

  841.     call.source.readWord = SelfTestReadWord;

  842.     call.source.readDword = SelfTestReadDwordLive;

  843.     call.source.readBit = SelfTestReadBit;

  844.     call.s0.device = PLSR_DEVICE_D;

  845.     call.s0.address = SELF_TEST_S0_BASE;

  846.     call.s1.device = PLSR_DEVICE_D;

  847.     call.s1.address = SELF_TEST_S1_BASE;

  848.     call.s2.type = PLSR_OPERAND_CONSTANT;

  849.     call.s2.constant = 1;

  850.     call.dAxis = 0U;

  851.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  852.     return PlsrPostCall(&call);

  853. }

  854. 

  855. PLSR_RESULT PlsrModbusDataSelfTestQueue(void)

  856. {

  857.     const uint16_t commonBase = 900U;

  858.     const uint16_t setBase =

  859.         (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  860.     uint16_t s0Words[20] = {0U};

  861.     uint16_t s1Words[4] = {0U};

  862.     PLSR_CALL call;

  863.     PLSR_COMMAND command;

  864.     PLSR_RESULT result;

  865. 

  866.     /* P12 uses D1000 as S0 and D1100 as S1.  D1010/D1011 is the live

  867.      * current-segment frequency written atomically by Modbus function 0x10. */

  868.     s0Words[0] = 1U;

  869.     s0Words[10] = 1000U;

  870.     s0Words[11] = 0U;

  871.     s0Words[12] = (uint16_t)(100000UL & 0xFFFFUL);

  872.     s0Words[13] = (uint16_t)(100000UL >> 16U);

  873.     if ((ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,

  874.                               SELF_TEST_MODBUS_S0_BASE,

  875.                               s0Words,

  876.                               20UL) == 0U)

  877.         || (ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,

  878.                                  SELF_TEST_MODBUS_S1_BASE,

  879.                                  s1Words,

  880.                                  4UL) == 0U))

  881.     {

  882.         return PLSR_RESULT_DATA_ACCESS;

  883.     }

  884. 

  885.     (void)PlcDeviceWriteSfd(commonBase, 0U);

  886.     SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL);

  887.     SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL);

  888.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), 4U);

  889.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U);

  890.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U);

  891.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U);

  892.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U);

  893.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU);

  894. 

  895.     /* Same limits as P11: 1000Hz default, 5000Hz maximum, 10Hz/ms ramp,

  896.      * and a 0.1ms live-frequency refresh. */

  897.     SelfTestWriteSfdDword(setBase, 1000UL);

  898.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 100U);

  899.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 100U);

  900.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U);

  901.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U);

  902.     SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 5000UL);

  903.     SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL);

  904.     SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL);

  905.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U);

  906.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U);

  907.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 2U);

  908.     SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL);

  909.     SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL);

  910. 

  911.     (void)memset(&command, 0, sizeof(command));

  912.     command.sequence = 0xAC00UL;

  913.     command.axis = 0U;

  914.     command.opcode = PLSR_CMD_SET_POSITION;

  915.     result = PlsrPostCommand(&command);

  916.     if (result != PLSR_RESULT_QUEUED)

  917.     {

  918.         return result;

  919.     }

  920. 

  921.     (void)memset(&call, 0, sizeof(call));

  922.     call.sequence = 0xAC01UL;

  923.     PlsrModbusDataSourceInit(&call.source);

  924.     call.s0.device = PLSR_DEVICE_D;

  925.     call.s0.address = SELF_TEST_MODBUS_S0_BASE;

  926.     call.s1.device = PLSR_DEVICE_D;

  927.     call.s1.address = SELF_TEST_MODBUS_S1_BASE;

  928.     call.s2.type = PLSR_OPERAND_CONSTANT;

  929.     call.s2.constant = 1;

  930.     call.dAxis = 0U;

  931.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  932.     return PlsrPostCall(&call);

  933. }

  934. 

  935. PLSR_RESULT PlsrModbusControlSelfTestPrepare(void)

  936. {

  937.     const uint16_t commonBase = 900U;

  938.     const uint16_t setBase =

  939.         (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  940. 

  941.     (void)PlcDeviceWriteSfd(commonBase, 0U);

  942.     SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL);

  943.     SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL);

  944.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U), 4U);

  945.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U);

  946.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 8U), 0U);

  947.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 9U), 0U);

  948.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U);

  949.     (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU);

  950. 

  951.     /* K1: 1000Hz default/start, 5000Hz maximum, 100ms ramps, 1ms refresh. */

  952.     SelfTestWriteSfdDword(setBase, 1000UL);

  953.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 100U);

  954.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 100U);

  955.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U);

  956.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U);

  957.     SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 5000UL);

  958.     SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 1000UL);

  959.     SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL);

  960.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U);

  961.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U);

  962.     (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U);

  963.     SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL);

  964.     SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL);

  965.     return PLSR_RESULT_OK;

  966. }