yuwenhao
/
Modbus-rtu
1
0
Atdalīts 0
Kods Problēmas 0 Izmaiņu pieprasījumi 0 Laidieni 0 Vikivietne Aktivitāte
Nevar pievienot vairāk kā 25 tēmas Tēmai ir jāsākas ar burtu vai ciparu, tā var saturēt domu zīmes ('-') un var būt līdz 35 simboliem gara.
42 Revīzijas
3 Atzari
138 MiB
 
 
 
 
 
 
Koks: f96f713e09
Atzari Tagi
${ item.name }
Izveidot atzaru ${ searchTerm }
no 'f96f713e09'
${ noResults }
Modbus-rtu/PLSR/Src/plsr_self_test.c

1110 rindas
44 KiB
Neapstrādāts Vainot Vēsture 

  1. #include "plsr_self_test.h"

  2. #include "plsr_build_config.h"

  3. #include "plc_device.h"

  4. #include "modbus_data_store.h"

  5. #include "plsr_core.h"

  6. #include "plsr_job.h"

  7. #include "plsr_modbus_data.h"

  8. #include <string.h>

  9. 

  10. #if PLSR_ENABLE_BOARD_SELF_TEST != 0U

  11. 

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

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

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

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

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

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

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

  19. 

  20. #define SELF_TEST_WORD_CAPACITY     (192U)

  21. #define SELF_TEST_S0_BASE           (10U)

  22. #define SELF_TEST_S1_BASE           (60U)

  23. #define SELF_TEST_DIR_POINT         (4U)

  24. #define SELF_TEST_SFD_AXIS_STRIDE   (130U)

  25. #define SELF_TEST_SFD_SET_OFFSET    (50U)

  26. #define SELF_TEST_MODBUS_S0_BASE    (1000UL)

  27. #define SELF_TEST_MODBUS_S1_BASE    (1100UL)

  28. 

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

  30. volatile int32_t PlsrSelfTestLiveFrequencyHz;

  31. volatile uint32_t PlsrSelfTestDynamicTick100us;

  32. volatile uint8_t PlsrSelfTestDynamicPhase;

  33. static volatile uint8_t PlsrSelfTestDynamicEnabled;

  34. 

  35. void PlsrSelfTestControlTick100us(void)

  36. {

  37.     if (PlsrSelfTestDynamicEnabled == 0U)

  38.     {

  39.         return;

  40.     }

  41.     if (PlsrSelfTestDynamicTick100us != UINT32_MAX)

  42.     {

  43.         PlsrSelfTestDynamicTick100us++;

  44.     }

  45.     switch (PlsrSelfTestDynamicTick100us)

  46.     {

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

  48.             PlsrSelfTestLiveFrequencyHz = 4000;

  49.             PlsrSelfTestDynamicPhase = 1U;

  50.             break;

  51. 

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

  53.             PlsrSelfTestLiveFrequencyHz = 500;

  54.             PlsrSelfTestDynamicPhase = 2U;

  55.             break;

  56. 

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

  58.             PlsrSelfTestLiveFrequencyHz = 0;

  59.             PlsrSelfTestDynamicPhase = 3U;

  60.             break;

  61. 

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

  63.             PlsrSelfTestLiveFrequencyHz = 8000;

  64.             PlsrSelfTestDynamicPhase = 4U;

  65.             break;

  66. 

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

  68.             PlsrSelfTestLiveFrequencyHz = -1;

  69.             PlsrSelfTestDynamicPhase = 5U;

  70.             break;

  71. 

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

  73.             PlsrSelfTestLiveFrequencyHz = 2000;

  74.             PlsrSelfTestDynamicPhase = 6U;

  75.             PlsrSelfTestDynamicEnabled = 0U;

  76.             break;

  77. 

  78.         default:

  79.             break;

  80.     }

  81. }

  82. 

  83. static uint8_t SelfTestValidateWords(void *context,

  84.                                      PLSR_DEVICE_TYPE device,

  85.                                      uint32_t firstAddress,

  86.                                      uint32_t wordCount)

  87. {

  88.     (void)context;

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

  90.     {

  91.         return 0U;

  92.     }

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

  94.             <= SELF_TEST_WORD_CAPACITY)

  95.                ? 1U

  96.                : 0U;

  97. }

  98. 

  99. static uint8_t SelfTestReadWord(void *context,

  100.                                 PLSR_DEVICE_TYPE device,

  101.                                 uint32_t address,

  102.                                 uint16_t *value)

  103. {

  104.     (void)context;

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

  106.         || (address >= SELF_TEST_WORD_CAPACITY))

  107.     {

  108.         return 0U;

  109.     }

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

  111.     return 1U;

  112. }

  113. 

  114. static uint8_t SelfTestReadDwordLive(void *context,

  115.                                      PLSR_DEVICE_TYPE device,

  116.                                      uint32_t address,

  117.                                      int32_t *value)

  118. {

  119.     uint16_t lowWord;

  120.     uint16_t highWord;

  121. 

  122.     if (value == NULL)

  123.     {

  124.         return 0U;

  125.     }

  126.     if ((device == PLSR_DEVICE_D)

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

  128.     {

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

  130.         *value = PlsrSelfTestLiveFrequencyHz;

  131.         return 1U;

  132.     }

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

  134.         || (SelfTestReadWord(context,

  135.                              device,

  136.                              address + 1UL,

  137.                              &highWord) == 0U))

  138.     {

  139.         return 0U;

  140.     }

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

  142.     return 1U;

  143. }

  144. 

  145. static uint8_t SelfTestReadBit(void *context,

  146.                                PLSR_DEVICE_TYPE device,

  147.                                uint32_t address,

  148.                                uint8_t *value)

  149. {

  150.     (void)context;

  151.     (void)device;

  152.     (void)address;

  153.     *value = 0U;

  154.     return 1U;

  155. }

  156. 

  157. static void SelfTestWriteDword(PLSR_DEVICE_TYPE device,

  158.                                uint32_t address,

  159.                                uint32_t value)

  160. {

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

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

  163. }

  164. 

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

  166. {

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

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

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

  170. }

  171. 

  172. PLSR_RESULT PlsrSelfTestQueue(void)

  173. {

  174.     PLSR_CALL call;

  175. 

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

  177. 

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

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

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

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

  182. 

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

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

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

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

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

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

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

  190.     SelfTestWriteDword(PLSR_DEVICE_D,

  191.                        SELF_TEST_S0_BASE + 32U,

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

  193. 

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

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

  196. 

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

  198.     call.sequence = 0xA5A5UL;

  199.     call.source.context = NULL;

  200.     call.source.validateWords = SelfTestValidateWords;

  201.     call.source.readWord = SelfTestReadWord;

  202.     call.source.readBit = SelfTestReadBit;

  203.     call.s0.device = PLSR_DEVICE_D;

  204.     call.s0.address = SELF_TEST_S0_BASE;

  205.     call.s1.device = PLSR_DEVICE_D;

  206.     call.s1.address = SELF_TEST_S1_BASE;

  207.     call.s2.type = PLSR_OPERAND_CONSTANT;

  208.     call.s2.constant = 1;

  209.     call.dAxis = 0U;

  210.     call.outputModeOverride = PLSR_OUTPUT_AB;

  211.     return PlsrPostCall(&call);

  212. }

  213. 

  214. PLSR_RESULT PlsrEquivalentSelfTestQueue(void)

  215. {

  216.     PLSR_CALL call;

  217. 

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

  219. 

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

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

  222.     SelfTestWriteSfdDword(902U, 3UL);

  223.     SelfTestWriteSfdDword(904U, 2UL);

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

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

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

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

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

  229.     SelfTestWriteSfdDword(956U, 60000UL);

  230. 

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

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

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

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

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

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

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

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

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

  240. 

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

  242.     call.sequence = 0xA5A6UL;

  243.     call.source.context = NULL;

  244.     call.source.validateWords = SelfTestValidateWords;

  245.     call.source.readWord = SelfTestReadWord;

  246.     call.source.readBit = SelfTestReadBit;

  247.     call.s0.device = PLSR_DEVICE_D;

  248.     call.s0.address = SELF_TEST_S0_BASE;

  249.     call.s1.device = PLSR_DEVICE_D;

  250.     call.s1.address = SELF_TEST_S1_BASE;

  251.     call.s2.type = PLSR_OPERAND_CONSTANT;

  252.     call.s2.constant = 1;

  253.     call.dAxis = 0U;

  254.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  255.     return PlsrPostCall(&call);

  256. }

  257. 

  258. PLSR_RESULT PlsrProtectionSelfTestQueue(void)

  259. {

  260.     PLSR_CALL call;

  261.     PLSR_COMMAND command;

  262.     PLSR_RESULT result;

  263. 

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

  265. 

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

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

  268.     SelfTestWriteSfdDword(902U, 1UL);

  269.     SelfTestWriteSfdDword(904U, 1UL);

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

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

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

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

  274.     SelfTestWriteSfdDword(930U, 500UL);

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

  276. 

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

  278.     SelfTestWriteSfdDword(950U, 1000UL);

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

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

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

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

  283.     SelfTestWriteSfdDword(956U, 100000UL);

  284.     SelfTestWriteSfdDword(958U, 1000UL);

  285.     SelfTestWriteSfdDword(960U, 0UL);

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

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

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

  289.     SelfTestWriteSfdDword(966U, 2000UL);

  290.     SelfTestWriteSfdDword(968U, 200UL);

  291. 

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

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

  294.     SelfTestWriteDword(PLSR_DEVICE_D,

  295.                        SELF_TEST_S0_BASE + 10U,

  296.                        1000UL);

  297.     SelfTestWriteDword(PLSR_DEVICE_D,

  298.                        SELF_TEST_S0_BASE + 12U,

  299.                        10000UL);

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

  301. 

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

  303.     command.sequence = 0xA5A7UL;

  304.     command.axis = 0U;

  305.     command.opcode = PLSR_CMD_SET_POSITION;

  306.     command.argument = 0;

  307.     result = PlsrPostCommand(&command);

  308.     if (result != PLSR_RESULT_QUEUED)

  309.     {

  310.         return result;

  311.     }

  312. 

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

  314.     call.sequence = 0xA5A8UL;

  315.     call.source.context = NULL;

  316.     call.source.validateWords = SelfTestValidateWords;

  317.     call.source.readWord = SelfTestReadWord;

  318.     call.source.readBit = SelfTestReadBit;

  319.     call.s0.device = PLSR_DEVICE_D;

  320.     call.s0.address = SELF_TEST_S0_BASE;

  321.     call.s1.device = PLSR_DEVICE_D;

  322.     call.s1.address = SELF_TEST_S1_BASE;

  323.     call.s2.type = PLSR_OPERAND_CONSTANT;

  324.     call.s2.constant = 1;

  325.     call.dAxis = 0U;

  326.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  327.     return PlsrPostCall(&call);

  328. }

  329. 

  330. PLSR_RESULT PlsrFourAxisSelfTestQueue(void)

  331. {

  332.     static const uint16_t s0Base[PLSR_AXIS_COUNT] =

  333.     {

  334.         10U, 40U, 70U, 100U

  335.     };

  336.     static const uint16_t s1Base[PLSR_AXIS_COUNT] =

  337.     {

  338.         160U, 164U, 168U, 172U

  339.     };

  340.     static const uint32_t frequencyHz[PLSR_AXIS_COUNT] =

  341.     {

  342.         1000UL, 2000UL, 3000UL, 4000UL

  343.     };

  344.     static const int32_t pulseCount[PLSR_AXIS_COUNT] =

  345.     {

  346.         1000, 2000, 3000, 4000

  347.     };

  348.     PLSR_CALL call;

  349.     PLSR_COMMAND command;

  350.     PLSR_RESULT result;

  351.     uint16_t commonBase;

  352.     uint16_t setBase;

  353.     uint8_t axis;

  354. 

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

  356. 

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

  358.     {

  359.         commonBase = (uint16_t)(900U

  360.                                 + (uint16_t)axis

  361.                                       * SELF_TEST_SFD_AXIS_STRIDE);

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

  363. 

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

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

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

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

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

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

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

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

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

  373. 

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

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

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

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

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

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

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

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

  382.                               frequencyHz[axis]);

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

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

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

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

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

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

  389. 

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

  391.         SelfTestWriteDword(PLSR_DEVICE_D,

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

  393.                            frequencyHz[axis]);

  394.         SelfTestWriteDword(PLSR_DEVICE_D,

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

  396.                            (uint32_t)pulseCount[axis]);

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

  398. 

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

  400.          * Backup SRAM position. */

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

  402.         command.sequence = 0xA500UL + axis;

  403.         command.axis = axis;

  404.         command.opcode = PLSR_CMD_SET_POSITION;

  405.         command.argument = 0;

  406.         result = PlsrPostCommand(&command);

  407.         if (result != PLSR_RESULT_QUEUED)

  408.         {

  409.             return result;

  410.         }

  411. 

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

  413.         call.sequence = 0xA600UL + axis;

  414.         call.source.context = NULL;

  415.         call.source.validateWords = SelfTestValidateWords;

  416.         call.source.readWord = SelfTestReadWord;

  417.         call.source.readBit = SelfTestReadBit;

  418.         call.s0.device = PLSR_DEVICE_D;

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

  420.         call.s1.device = PLSR_DEVICE_D;

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

  422.         call.s2.type = PLSR_OPERAND_CONSTANT;

  423.         call.s2.constant = 1;

  424.         call.dAxis = axis;

  425.         call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  426.         result = PlsrPostCall(&call);

  427.         if (result != PLSR_RESULT_QUEUED)

  428.         {

  429.             return result;

  430.         }

  431.     }

  432.     return PLSR_RESULT_QUEUED;

  433. }

  434. 

  435. PLSR_RESULT PlsrBacklashSelfTestQueue(void)

  436. {

  437.     PLSR_CALL call;

  438.     PLSR_COMMAND command;

  439.     PLSR_RESULT result;

  440. 

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

  442. 

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

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

  445.     SelfTestWriteSfdDword(902U, 1UL);

  446.     SelfTestWriteSfdDword(904U, 1UL);

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

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

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

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

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

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

  453. 

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

  455.      * acceleration/deceleration parameter. */

  456.     SelfTestWriteSfdDword(950U, 1000UL);

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

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

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

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

  461.     SelfTestWriteSfdDword(956U, 100000UL);

  462.     SelfTestWriteSfdDword(958U, 1000UL);

  463.     SelfTestWriteSfdDword(960U, 0UL);

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

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

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

  467.     SelfTestWriteSfdDword(966U, 2000UL);

  468.     SelfTestWriteSfdDword(968U, 200UL);

  469. 

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

  471.     SelfTestWriteDword(PLSR_DEVICE_D,

  472.                        SELF_TEST_S0_BASE + 10U,

  473.                        1000UL);

  474.     SelfTestWriteDword(PLSR_DEVICE_D,

  475.                        SELF_TEST_S0_BASE + 12U,

  476.                        200UL);

  477.     SelfTestWriteDword(PLSR_DEVICE_D,

  478.                        SELF_TEST_S0_BASE + 20U,

  479.                        1000UL);

  480.     SelfTestWriteDword(PLSR_DEVICE_D,

  481.                        SELF_TEST_S0_BASE + 22U,

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

  483.     SelfTestWriteDword(PLSR_DEVICE_D,

  484.                        SELF_TEST_S0_BASE + 30U,

  485.                        1000UL);

  486.     SelfTestWriteDword(PLSR_DEVICE_D,

  487.                        SELF_TEST_S0_BASE + 32U,

  488.                        100UL);

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

  490. 

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

  492.     command.sequence = 0xA700UL;

  493.     command.axis = 0U;

  494.     command.opcode = PLSR_CMD_SET_POSITION;

  495.     command.argument = 0;

  496.     result = PlsrPostCommand(&command);

  497.     if (result != PLSR_RESULT_QUEUED)

  498.     {

  499.         return result;

  500.     }

  501. 

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

  503.     call.sequence = 0xA701UL;

  504.     call.source.context = NULL;

  505.     call.source.validateWords = SelfTestValidateWords;

  506.     call.source.readWord = SelfTestReadWord;

  507.     call.source.readBit = SelfTestReadBit;

  508.     call.s0.device = PLSR_DEVICE_D;

  509.     call.s0.address = SELF_TEST_S0_BASE;

  510.     call.s1.device = PLSR_DEVICE_D;

  511.     call.s1.address = SELF_TEST_S1_BASE;

  512.     call.s2.type = PLSR_OPERAND_CONSTANT;

  513.     call.s2.constant = 1;

  514.     call.dAxis = 0U;

  515.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  516.     return PlsrPostCall(&call);

  517. }

  518. 

  519. PLSR_RESULT PlsrDirectionLogicSelfTestQueue(void)

  520. {

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

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

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

  524.     PLSR_CALL call;

  525.     PLSR_COMMAND command;

  526.     PLSR_RESULT result;

  527.     uint16_t commonBase;

  528.     uint16_t setBase;

  529.     uint8_t axis;

  530. 

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

  532. 

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

  534.     {

  535.         commonBase = (uint16_t)(900U

  536.                                 + (uint16_t)axis

  537.                                       * SELF_TEST_SFD_AXIS_STRIDE);

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

  539. 

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

  541.         (void)PlcDeviceWriteSfd(commonBase,

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

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

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

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

  546.                                 directionPoint[axis]);

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

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

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

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

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

  552. 

  553.         SelfTestWriteSfdDword(setBase, 1000UL);

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

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

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

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

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

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

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

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

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

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

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

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

  566. 

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

  568.         SelfTestWriteDword(PLSR_DEVICE_D,

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

  570.                            1000UL);

  571.         SelfTestWriteDword(PLSR_DEVICE_D,

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

  573.                            200UL);

  574.         SelfTestWriteDword(PLSR_DEVICE_D,

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

  576.                            1000UL);

  577.         SelfTestWriteDword(PLSR_DEVICE_D,

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

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

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

  581. 

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

  583.         command.sequence = 0xA800UL + axis;

  584.         command.axis = axis;

  585.         command.opcode = PLSR_CMD_SET_POSITION;

  586.         command.argument = 0;

  587.         result = PlsrPostCommand(&command);

  588.         if (result != PLSR_RESULT_QUEUED)

  589.         {

  590.             return result;

  591.         }

  592. 

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

  594.         call.sequence = 0xA810UL + axis;

  595.         call.source.context = NULL;

  596.         call.source.validateWords = SelfTestValidateWords;

  597.         call.source.readWord = SelfTestReadWord;

  598.         call.source.readBit = SelfTestReadBit;

  599.         call.s0.device = PLSR_DEVICE_D;

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

  601.         call.s1.device = PLSR_DEVICE_D;

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

  603.         call.s2.type = PLSR_OPERAND_CONSTANT;

  604.         call.s2.constant = 1;

  605.         call.dAxis = axis;

  606.         call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  607.         result = PlsrPostCall(&call);

  608.         if (result != PLSR_RESULT_QUEUED)

  609.         {

  610.             return result;

  611.         }

  612.     }

  613.     return PLSR_RESULT_QUEUED;

  614. }

  615. 

  616. PLSR_RESULT PlsrCwCcwSelfTestQueue(void)

  617. {

  618.     const uint16_t commonBase = 900U;

  619.     const uint16_t setBase =

  620.         (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  621.     PLSR_CALL call;

  622.     PLSR_COMMAND command;

  623.     PLSR_RESULT result;

  624. 

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

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

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

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

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

  630.                             SELF_TEST_DIR_POINT);

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

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

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

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

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

  636. 

  637.     SelfTestWriteSfdDword(setBase, 2000UL);

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

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

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

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

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

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

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

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

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

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

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

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

  650. 

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

  652.     SelfTestWriteDword(PLSR_DEVICE_D,

  653.                        SELF_TEST_S0_BASE + 10UL,

  654.                        2000UL);

  655.     SelfTestWriteDword(PLSR_DEVICE_D,

  656.                        SELF_TEST_S0_BASE + 12UL,

  657.                        300UL);

  658.     SelfTestWriteDword(PLSR_DEVICE_D,

  659.                        SELF_TEST_S0_BASE + 20UL,

  660.                        1000UL);

  661.     SelfTestWriteDword(PLSR_DEVICE_D,

  662.                        SELF_TEST_S0_BASE + 22UL,

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

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

  665. 

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

  667.     command.sequence = 0xA900UL;

  668.     command.axis = 0U;

  669.     command.opcode = PLSR_CMD_SET_POSITION;

  670.     result = PlsrPostCommand(&command);

  671.     if (result != PLSR_RESULT_QUEUED)

  672.     {

  673.         return result;

  674.     }

  675. 

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

  677.     call.sequence = 0xA901UL;

  678.     call.source.context = NULL;

  679.     call.source.validateWords = SelfTestValidateWords;

  680.     call.source.readWord = SelfTestReadWord;

  681.     call.source.readBit = SelfTestReadBit;

  682.     call.s0.device = PLSR_DEVICE_D;

  683.     call.s0.address = SELF_TEST_S0_BASE;

  684.     call.s1.device = PLSR_DEVICE_D;

  685.     call.s1.address = SELF_TEST_S1_BASE;

  686.     call.s2.type = PLSR_OPERAND_CONSTANT;

  687.     call.s2.constant = 1;

  688.     call.dAxis = 0U;

  689.     call.outputModeOverride = PLSR_OUTPUT_CW_CCW;

  690.     return PlsrPostCall(&call);

  691. }

  692. 

  693. PLSR_RESULT PlsrFastRefreshSelfTestQueue(void)

  694. {

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

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

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

  698.     PLSR_CALL call;

  699.     PLSR_COMMAND command;

  700.     PLSR_RESULT result;

  701.     uint16_t commonBase;

  702.     uint16_t setBase;

  703.     uint8_t axis;

  704. 

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

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

  707.     {

  708.         commonBase = (uint16_t)(900U

  709.                                 + (uint16_t)axis

  710.                                       * SELF_TEST_SFD_AXIS_STRIDE);

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

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

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

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

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

  716.                                 directionPoint[axis]);

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

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

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

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

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

  722. 

  723.         SelfTestWriteSfdDword(setBase, 5000UL);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  738. 

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

  740.         SelfTestWriteDword(PLSR_DEVICE_D,

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

  742.                            5000UL);

  743.         SelfTestWriteDword(PLSR_DEVICE_D,

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

  745.                            2000UL);

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

  747. 

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

  749.         command.sequence = 0xAA00UL + axis;

  750.         command.axis = axis;

  751.         command.opcode = PLSR_CMD_SET_POSITION;

  752.         result = PlsrPostCommand(&command);

  753.         if (result != PLSR_RESULT_QUEUED)

  754.         {

  755.             return result;

  756.         }

  757. 

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

  759.         call.sequence = 0xAA10UL + axis;

  760.         call.source.context = NULL;

  761.         call.source.validateWords = SelfTestValidateWords;

  762.         call.source.readWord = SelfTestReadWord;

  763.         call.source.readBit = SelfTestReadBit;

  764.         call.s0.device = PLSR_DEVICE_D;

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

  766.         call.s1.device = PLSR_DEVICE_D;

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

  768.         call.s2.type = PLSR_OPERAND_CONSTANT;

  769.         call.s2.constant = 1;

  770.         call.dAxis = axis;

  771.         call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  772.         result = PlsrPostCall(&call);

  773.         if (result != PLSR_RESULT_QUEUED)

  774.         {

  775.             return result;

  776.         }

  777.     }

  778.     return PLSR_RESULT_QUEUED;

  779. }

  780. 

  781. PLSR_RESULT PlsrDynamicFrequencySelfTestQueue(void)

  782. {

  783.     const uint16_t commonBase = 900U;

  784.     const uint16_t setBase =

  785.         (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  786.     PLSR_CALL call;

  787.     PLSR_COMMAND command;

  788.     PLSR_RESULT result;

  789. 

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

  791.     PlsrSelfTestLiveFrequencyHz = 1000;

  792.     PlsrSelfTestDynamicTick100us = 0UL;

  793.     PlsrSelfTestDynamicPhase = 0U;

  794.     PlsrSelfTestDynamicEnabled = 1U;

  795.     PlsrSetControlTickHook(PlsrSelfTestControlTick100us);

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

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

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

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

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

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

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

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

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

  805. 

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

  807.     SelfTestWriteSfdDword(setBase, 1000UL);

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

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

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

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

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

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

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

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

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

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

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

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

  820. 

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

  822.     SelfTestWriteDword(PLSR_DEVICE_D,

  823.                        SELF_TEST_S0_BASE + 10UL,

  824.                        1000UL);

  825.     SelfTestWriteDword(PLSR_DEVICE_D,

  826.                        SELF_TEST_S0_BASE + 12UL,

  827.                        100000UL);

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

  829. 

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

  831.     command.sequence = 0xAB00UL;

  832.     command.axis = 0U;

  833.     command.opcode = PLSR_CMD_SET_POSITION;

  834.     result = PlsrPostCommand(&command);

  835.     if (result != PLSR_RESULT_QUEUED)

  836.     {

  837.         return result;

  838.     }

  839. 

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

  841.     call.sequence = 0xAB01UL;

  842.     call.source.context = NULL;

  843.     call.source.validateWords = SelfTestValidateWords;

  844.     call.source.readWord = SelfTestReadWord;

  845.     call.source.readDword = SelfTestReadDwordLive;

  846.     call.source.readBit = SelfTestReadBit;

  847.     call.s0.device = PLSR_DEVICE_D;

  848.     call.s0.address = SELF_TEST_S0_BASE;

  849.     call.s1.device = PLSR_DEVICE_D;

  850.     call.s1.address = SELF_TEST_S1_BASE;

  851.     call.s2.type = PLSR_OPERAND_CONSTANT;

  852.     call.s2.constant = 1;

  853.     call.dAxis = 0U;

  854.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  855.     return PlsrPostCall(&call);

  856. }

  857. 

  858. PLSR_RESULT PlsrModbusDataSelfTestQueue(void)

  859. {

  860.     const uint16_t commonBase = 900U;

  861.     const uint16_t setBase =

  862.         (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

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

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

  865.     PLSR_CALL call;

  866.     PLSR_COMMAND command;

  867.     PLSR_RESULT result;

  868. 

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

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

  871.     s0Words[0] = 1U;

  872.     s0Words[10] = 1000U;

  873.     s0Words[11] = 0U;

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

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

  876.     if ((ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,

  877.                               SELF_TEST_MODBUS_S0_BASE,

  878.                               s0Words,

  879.                               20UL) == 0U)

  880.         || (ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,

  881.                                  SELF_TEST_MODBUS_S1_BASE,

  882.                                  s1Words,

  883.                                  4UL) == 0U))

  884.     {

  885.         return PLSR_RESULT_DATA_ACCESS;

  886.     }

  887. 

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

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

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

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

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

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

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

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

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

  897. 

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

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

  900.     SelfTestWriteSfdDword(setBase, 1000UL);

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

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

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

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

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

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

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

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

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

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

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

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

  913. 

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

  915.     command.sequence = 0xAC00UL;

  916.     command.axis = 0U;

  917.     command.opcode = PLSR_CMD_SET_POSITION;

  918.     result = PlsrPostCommand(&command);

  919.     if (result != PLSR_RESULT_QUEUED)

  920.     {

  921.         return result;

  922.     }

  923. 

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

  925.     call.sequence = 0xAC01UL;

  926.     PlsrModbusDataSourceInit(&call.source);

  927.     call.s0.device = PLSR_DEVICE_D;

  928.     call.s0.address = SELF_TEST_MODBUS_S0_BASE;

  929.     call.s1.device = PLSR_DEVICE_D;

  930.     call.s1.address = SELF_TEST_MODBUS_S1_BASE;

  931.     call.s2.type = PLSR_OPERAND_CONSTANT;

  932.     call.s2.constant = 1;

  933.     call.dAxis = 0U;

  934.     call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;

  935.     return PlsrPostCall(&call);

  936. }

  937. 

  938. PLSR_RESULT PlsrModbusControlSelfTestPrepare(void)

  939. {

  940.     const uint16_t commonBase = 900U;

  941.     const uint16_t setBase =

  942.         (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

  943. 

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

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

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

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

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

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

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

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

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

  953. 

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

  955.     SelfTestWriteSfdDword(setBase, 1000UL);

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

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

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

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

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

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

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

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

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

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

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

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

  968.     return PLSR_RESULT_OK;

  969. }

  970. 

  971. PLSR_RESULT PlsrHardwareCounterSelfTestPrepare(void)

  972. {

  973.     uint16_t commonBase;

  974.     uint16_t setBase;

  975.     uint16_t limitSetBase;

  976.     uint16_t abSetBase;

  977.     uint32_t limitFrequencyHz;

  978.     uint8_t axis;

  979. 

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

  981.     {

  982.         commonBase = (uint16_t)(900U

  983.                                 + (uint16_t)axis

  984.                                       * SELF_TEST_SFD_AXIS_STRIDE);

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

  986.         limitSetBase = (uint16_t)(setBase + 20U);

  987.         abSetBase = (uint16_t)(setBase + 40U);

  988. 

  989.         /* PULSE/DIR, pulse-unit soft limits, one direction point per axis. */

  990.         (void)PlcDeviceWriteSfd(commonBase, (1U << 2U));

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

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

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

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

  995.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 0U);

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

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

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

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

  1000.         SelfTestWriteSfdDword((uint16_t)(commonBase + 30U), 1000000UL);

  1001.         SelfTestWriteSfdDword((uint16_t)(commonBase + 32U),

  1002.                               (uint32_t)(int32_t)-1000000);

  1003. 

  1004.         /* Exact 100kHz plateau.  200000-pulse S0 jobs cross the 16-bit

  1005.          * counter boundary three times while avoiding profile ramp effects. */

  1006.         SelfTestWriteSfdDword(setBase, 100000UL);

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

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

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

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

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

  1012.         SelfTestWriteSfdDword((uint16_t)(setBase + 8U), 100000UL);

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

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

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

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

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

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

  1019. 

  1020.         /* K2: P15 limit matrix.  Even axes use 500Hz, odd axes 2000Hz;

  1021.          * start at target speed and decelerate for 100ms at a soft limit. */

  1022.         limitFrequencyHz = ((axis & 1U) == 0U) ? 500UL : 2000UL;

  1023.         SelfTestWriteSfdDword(limitSetBase, limitFrequencyHz);

  1024.         (void)PlcDeviceWriteSfd((uint16_t)(limitSetBase + 2U), 0U);

  1025.         (void)PlcDeviceWriteSfd((uint16_t)(limitSetBase + 3U), 100U);

  1026.         (void)PlcDeviceWriteSfd((uint16_t)(limitSetBase + 4U), 0U);

  1027.         (void)PlcDeviceWriteSfd((uint16_t)(limitSetBase + 5U), 0U);

  1028.         SelfTestWriteSfdDword((uint16_t)(limitSetBase + 6U),

  1029.                               limitFrequencyHz);

  1030.         SelfTestWriteSfdDword((uint16_t)(limitSetBase + 8U),

  1031.                               limitFrequencyHz);

  1032.         SelfTestWriteSfdDword((uint16_t)(limitSetBase + 10U), 0UL);

  1033.         (void)PlcDeviceWriteSfd((uint16_t)(limitSetBase + 12U), 50U);

  1034.         (void)PlcDeviceWriteSfd((uint16_t)(limitSetBase + 13U), 0U);

  1035.         (void)PlcDeviceWriteSfd((uint16_t)(limitSetBase + 14U), 0U);

  1036.         SelfTestWriteSfdDword((uint16_t)(limitSetBase + 16U), 2000UL);

  1037.         SelfTestWriteSfdDword((uint16_t)(limitSetBase + 18U), 200UL);

  1038. 

  1039.         /* K3: dual-AB 100kHz plateau.  TIM9/TIM12 count complete-cycle

  1040.          * source events so the output timers only interrupt for rephase and

  1041.          * the guarded final 00 boundary. */

  1042.         SelfTestWriteSfdDword(abSetBase, 100000UL);

  1043.         (void)PlcDeviceWriteSfd((uint16_t)(abSetBase + 2U), 0U);

  1044.         (void)PlcDeviceWriteSfd((uint16_t)(abSetBase + 3U), 0U);

  1045.         (void)PlcDeviceWriteSfd((uint16_t)(abSetBase + 4U), 0U);

  1046.         (void)PlcDeviceWriteSfd((uint16_t)(abSetBase + 5U), 0U);

  1047.         SelfTestWriteSfdDword((uint16_t)(abSetBase + 6U), 100000UL);

  1048.         SelfTestWriteSfdDword((uint16_t)(abSetBase + 8U), 100000UL);

  1049.         SelfTestWriteSfdDword((uint16_t)(abSetBase + 10U), 0UL);

  1050.         (void)PlcDeviceWriteSfd((uint16_t)(abSetBase + 12U), 50U);

  1051.         (void)PlcDeviceWriteSfd((uint16_t)(abSetBase + 13U), 0U);

  1052.         (void)PlcDeviceWriteSfd((uint16_t)(abSetBase + 14U), 0U);

  1053.         SelfTestWriteSfdDword((uint16_t)(abSetBase + 16U), 2000UL);

  1054.         SelfTestWriteSfdDword((uint16_t)(abSetBase + 18U), 200UL);

  1055.     }

  1056.     return PLSR_RESULT_OK;

  1057. }

  1058. 

  1059. PLSR_RESULT PlsrLongStressSelfTestPrepare(void)

  1060. {

  1061.     uint16_t commonBase;

  1062.     uint16_t longSetBase;

  1063.     uint8_t axis;

  1064. 

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

  1066.     {

  1067.         commonBase = (uint16_t)(900U

  1068.                                 + (uint16_t)axis

  1069.                                       * SELF_TEST_SFD_AXIS_STRIDE);

  1070.         longSetBase = (uint16_t)(commonBase

  1071.                                  + SELF_TEST_SFD_SET_OFFSET

  1072.                                  + 60U); /* K4 occupies the final 20 words. */

  1073. 

  1074.         /* P18 is a bench-only PULSE/DIR endurance fixture.  Keep the proven

  1075.          * Q4..Q7 direction mapping and disable both hard-input assignments and

  1076.          * soft limits so the deliberately long positive jobs cannot stop at

  1077.          * the P14/P15 +/-1000000-pulse validation boundary. */

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

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

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

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

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

  1083.         (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 0U);

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

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

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

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

  1088.         SelfTestWriteSfdDword((uint16_t)(commonBase + 30U), 0UL);

  1089.         SelfTestWriteSfdDword((uint16_t)(commonBase + 32U), 0UL);

  1090. 

  1091.         /* K4: exact 100kHz plateau, no acceleration/deceleration ramp. */

  1092.         SelfTestWriteSfdDword(longSetBase, 100000UL);

  1093.         (void)PlcDeviceWriteSfd((uint16_t)(longSetBase + 2U), 0U);

  1094.         (void)PlcDeviceWriteSfd((uint16_t)(longSetBase + 3U), 0U);

  1095.         (void)PlcDeviceWriteSfd((uint16_t)(longSetBase + 4U), 0U);

  1096.         (void)PlcDeviceWriteSfd((uint16_t)(longSetBase + 5U), 0U);

  1097.         SelfTestWriteSfdDword((uint16_t)(longSetBase + 6U), 100000UL);

  1098.         SelfTestWriteSfdDword((uint16_t)(longSetBase + 8U), 100000UL);

  1099.         SelfTestWriteSfdDword((uint16_t)(longSetBase + 10U), 0UL);

  1100.         (void)PlcDeviceWriteSfd((uint16_t)(longSetBase + 12U), 50U);

  1101.         (void)PlcDeviceWriteSfd((uint16_t)(longSetBase + 13U), 0U);

  1102.         (void)PlcDeviceWriteSfd((uint16_t)(longSetBase + 14U), 0U);

  1103.         SelfTestWriteSfdDword((uint16_t)(longSetBase + 16U), 2000UL);

  1104.         SelfTestWriteSfdDword((uint16_t)(longSetBase + 18U), 200UL);

  1105.     }

  1106.     return PLSR_RESULT_OK;

  1107. }

  1108. 

  1109. #endif