From c7b851659ea26f9af8563e146bf5332c3c8d6dfd Mon Sep 17 00:00:00 2001 From: ywh <2227158009@qq.com> Date: Wed, 12 Aug 2026 16:24:09 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=202026=20PLSR=20=E6=8C=87?= =?UTF-8?q?=E4=BB=A4=E5=8F=8A=E6=9C=80=E5=B0=8F=20Modbus=20RTU?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Inc/stm32f4xx_it.h | 5 +- Core/Src/main.c | 67 +- EWARM/Modbus.ewp | 48 + EWARM/stm32f407xx_flash.icf | 4 +- HostComputer/README.md | 32 +- HostComputer/plsr_control_panel.py | 935 +++++ HostComputer/plsr_modbus_product_test.py | 1162 ++++++ .../Third_Party/Micrium/Config/app_cfg.h | 3 + Modbus/Inc/modbus_rtu_slave.h | 130 +- Modbus/Src/modbus_rtu_slave.c | 1033 ++--- PLSR/Inc/plsr.h | 94 + PLSR/Src/plsr.c | 3599 +++++++++++++++++ PLSR/Src/plsr_internal.h | 49 + PLSR/Src/plsr_platform.h | 34 + PLSR/Src/plsr_platform_f407.c | 1202 ++++++ tests/plsr_host/run_tests.ps1 | 42 + tests/plsr_host/test_plsr_host.c | 1964 +++++++++ 17 files changed, 9438 insertions(+), 965 deletions(-) create mode 100644 HostComputer/plsr_control_panel.py create mode 100644 HostComputer/plsr_modbus_product_test.py create mode 100644 PLSR/Inc/plsr.h create mode 100644 PLSR/Src/plsr.c create mode 100644 PLSR/Src/plsr_internal.h create mode 100644 PLSR/Src/plsr_platform.h create mode 100644 PLSR/Src/plsr_platform_f407.c create mode 100644 tests/plsr_host/run_tests.ps1 create mode 100644 tests/plsr_host/test_plsr_host.c diff --git a/Core/Inc/stm32f4xx_it.h b/Core/Inc/stm32f4xx_it.h index a25a65a..ae330a9 100644 --- a/Core/Inc/stm32f4xx_it.h +++ b/Core/Inc/stm32f4xx_it.h @@ -60,7 +60,10 @@ void DMA2_Stream2_IRQHandler(void); void OTG_FS_IRQHandler(void); void DMA2_Stream7_IRQHandler(void); /* USER CODE BEGIN EFP */ - +void TIM1_UP_TIM10_IRQHandler(void); +void TIM8_UP_TIM13_IRQHandler(void); +void TIM1_TRG_COM_TIM11_IRQHandler(void); +void TIM8_TRG_COM_TIM14_IRQHandler(void); /* USER CODE END EFP */ #ifdef __cplusplus diff --git a/Core/Src/main.c b/Core/Src/main.c index 5fb8f1f..f84d298 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -24,7 +24,7 @@ /* USER CODE BEGIN Includes */ #include "ucos_ii.h" #include "modbus_rtu_slave.h" -#include "stdio.h" +#include "plsr.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -46,7 +46,6 @@ UART_HandleTypeDef huart1; DMA_HandleTypeDef hdma_usart1_rx; DMA_HandleTypeDef hdma_usart1_tx; -static volatile uint8_t ConnectFlag; /* USER CODE BEGIN PV */ static OS_STK AppTaskStartStk[APP_TASK_START_STK_SIZE]; @@ -54,7 +53,6 @@ static OS_STK AppTaskStartStk[APP_TASK_START_STK_SIZE]; /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); -static HAL_StatusTypeDef BackupSramInit(void); static void MX_GPIO_Init(void); static void MX_DMA_Init(void); static void MX_USART1_UART_Init(void); @@ -68,55 +66,17 @@ static void AppTaskStart(void *pArg); static void AppTaskStart(void *pArg) { - (void)pArg; - /* - * F4作为Modbus RTU从站，触摸屏作为主站 - * 初始化函数会立即启动USART1的DMA空闲接收 - */ - (void)ModbusSlaveInit(&huart1, MODBUS_SLAVE_DEFAULT_ADDRESS); - //ModbusRetainedRegistersLoad(); + if (ModbusSlaveInit(&huart1, MODBUS_SLAVE_DEFAULT_ADDRESS) != HAL_OK) + { + Error_Handler(); + } + while (1) { - /* - * 维护几个供触摸屏首次联调读取的保持寄存器： - */ - - // (void)ModbusSlaveSetHoldingRegister( // OS时钟 - // HMI_REG_UPTIME_SECONDS, (uint16_t)(OSTimeGet() / OS_TICKS_PER_SEC)); - // (void)ModbusSlaveSetHoldingRegister( // 有效帧计数 - // HMI_REG_RX_FRAME_COUNT, - // (uint16_t)ModbusSlaveStatistics.validFrameCount); - - // (void)ModbusSlaveSetHoldingRegister( // 从站地址不匹配计数 - // D3, (uint16_t)ModbusSlaveStatistics.ignoredAddressCount); - // (void)ModbusSlaveSetHoldingRegister( // 非法功能码计数 - // D4, (uint16_t)ModbusSlaveStatistics.illegalFunctionCount); - - // (void)ModbusSlaveSetHoldingRegister( // 非法地址计数 - // D5, (uint16_t)ModbusSlaveStatistics.illegalAddressCount); - - // (void)ModbusSlaveSetHoldingRegister( // 非法数据值计数 - // D6, (uint16_t)ModbusSlaveStatistics.illegalValueCount); - - /* - * 每1ms轮询一次， - */ + PlsrPoll1ms(); ModbusSlavePoll(); - // ModbusRetainedRegistersPoll(); - - if (ModbusSlaveIsConnected(MODBUS_CONNECTION_TIMEOUT_MS) != 0U) - { - /* 主站在线 */ - ConnectFlag = 1; - } - else - { - /* 主站超时或断开 */ - ConnectFlag = 0; - } - OSTimeDly(1U); } } @@ -156,7 +116,10 @@ int main(void) MX_DMA_Init(); MX_USB_DEVICE_Init(); MX_USART1_UART_Init(); - (void)BackupSramInit(); + if (PlsrInit() == 0U) + { + Error_Handler(); + } /* USER CODE BEGIN 2 */ INT8U osError; @@ -188,14 +151,6 @@ int main(void) /* USER CODE END 3 */ } -static HAL_StatusTypeDef BackupSramInit(void) -{ - HAL_PWR_EnableBkUpAccess(); - __HAL_RCC_BKPSRAM_CLK_ENABLE(); - - return HAL_PWREx_EnableBkUpReg(); -} - /** * @brief System Clock Configuration * @retval None diff --git a/EWARM/Modbus.ewp b/EWARM/Modbus.ewp index 5c4ccb6..6817707 100644 --- a/EWARM/Modbus.ewp +++ b/EWARM/Modbus.ewp @@ -360,6 +360,7 @@ $PROJ_DIR$/../Drivers/CMSIS/Device/ST/STM32F4xx/Include $PROJ_DIR$/../Drivers/CMSIS/Include $PROJ_DIR$\..\Modbus\Inc + $PROJ_DIR$\..\PLSR\Inc $PROJ_DIR$\..\Middlewares\Third_Party\Micrium\Config $PROJ_DIR$\..\Middlewares\Third_Party\Micrium\uCOS-II\Source $PROJ_DIR$\..\Middlewares\Third_Party\Micrium\uCOS-II\Ports\ARM-Cortex-M4\IAR @@ -1260,6 +1261,53 @@ + + PLSR + + $PROJ_DIR$\..\PLSR\Inc\plsr.h + + + $PROJ_DIR$\..\PLSR\Src\plsr_internal.h + + + $PROJ_DIR$\..\PLSR\Src\plsr_platform.h + + + $PROJ_DIR$\..\PLSR\Src\plsr.c + + Modbus + + ICCARM + + 35 + 0 + 1 + + + + + + + + + + $PROJ_DIR$\..\PLSR\Src\plsr_platform_f407.c + + Modbus diff --git a/EWARM/stm32f407xx_flash.icf b/EWARM/stm32f407xx_flash.icf index 472464c..68f508e 100644 --- a/EWARM/stm32f407xx_flash.icf +++ b/EWARM/stm32f407xx_flash.icf @@ -5,7 +5,7 @@ define symbol __ICFEDIT_intvec_start__ = 0x08000000; /*-Memory Regions-*/ define symbol __ICFEDIT_region_ROM_start__ = 0x08000000; -define symbol __ICFEDIT_region_ROM_end__ = 0x080FFFFF; +define symbol __ICFEDIT_region_ROM_end__ = 0x080BFFFF; define symbol __ICFEDIT_region_RAM_start__ = 0x20000000; define symbol __ICFEDIT_region_RAM_end__ = 0x2001FFFF; define symbol __ICFEDIT_region_CCMRAM_start__ = 0x10000000; @@ -35,4 +35,4 @@ place in RAM_region { readwrite, place in CCMRAM_region { section .ccmram -}; \ No newline at end of file +}; diff --git a/HostComputer/README.md b/HostComputer/README.md index 5fefb5c..b6d282d 100644 --- a/HostComputer/README.md +++ b/HostComputer/README.md @@ -1,4 +1,34 @@ -# Modbus RTU T1.5/T3.5测试上位机 +# 上位机工具 + +## PLSR 控制面板 + +`plsr_control_panel.py`用于配置和控制2026版PLSR指令。串口参数固定为 +9600、8位数据、偶校验、1位停止位（8E1），从站地址可选1～247。 + +在仓库根目录运行： + +```powershell +py -B HostComputer\plsr_control_panel.py +``` + +界面可读写公共参数和全部10段参数，轮询显示累计位置、当前频率、运行 +状态、当前段和错误码，并可发送启动、停止、清零命令。Y0～Y3是同一条 +PLSR逻辑的可选脉冲输出，不是四条可同时运行的独立PLSR指令；方向输出为 +Y12～Y15，WAIT/EXT输入为X4或X5。 + +离线自测不会创建窗口或打开串口： + +```powershell +py -B HostComputer\plsr_control_panel.py --self-test +``` + +依赖安装： + +```powershell +py -m pip install -r HostComputer\requirements.txt +``` + +## Modbus RTU T1.5/T3.5测试上位机 ## 直接运行EXE diff --git a/HostComputer/plsr_control_panel.py b/HostComputer/plsr_control_panel.py new file mode 100644 index 0000000..551ad3e --- /dev/null +++ b/HostComputer/plsr_control_panel.py @@ -0,0 +1,935 @@ +"""Tkinter control panel for the 2026 PLSR Modbus register map.""" + +from __future__ import annotations + +import argparse +import queue +import threading +import time +import tkinter as tk +import tkinter.font as tkfont +from tkinter import messagebox, ttk + +import serial +from serial.tools import list_ports + +# Keep one RTU implementation for the board tests and this operator panel. +from plsr_modbus_product_test import ( + COMMAND_CLEAR, + COMMAND_START, + COMMAND_STOP, + COMMON_WORDS, + CONFIG_BASE, + CONTROL, + ModbusException, + RtuClient, + SEGMENT_1_BASE, + SEGMENT_WORDS, + TestFailure, + read_status, + split_i32, + split_u32, +) + + +BAUD_RATE = 9600 +SERIAL_TIMEOUT_SECONDS = 0.8 +STATUS_POLL_SECONDS = 0.25 +SEGMENT_COUNT = 10 +SEGMENT_STRIDE = 0x10 +MAX_FREQUENCY_HZ = 100_000 + +PULSE_OPTIONS = ( + ("Y0 (0)", 0), + ("Y1 (1)", 1), + ("Y2 (2)", 2), + ("Y3 (3)", 3), +) +DIRECTION_OPTIONS = ( + ("Y12 (0)", 0), + ("Y13 (1)", 1), + ("Y14 (2)", 2), + ("Y15 (3)", 3), +) +INPUT_OPTIONS = (("X4 (0)", 0), ("X5 (1)", 1)) +SEND_OPTIONS = (("脉冲发送完毕 (0)", 0), ("后续段 (1)", 1)) +LOGIC_OPTIONS = (("正逻辑 (0)", 0), ("负逻辑 (1)", 1)) +CURVE_OPTIONS = (("直线 (0)", 0), ("S 曲线 (1)", 1), ("正弦曲线 (2)", 2)) +POSITION_OPTIONS = (("相对位置 (0)", 0), ("绝对位置 (1)", 1)) +WAIT_OPTIONS = ( + ("WAIT 时间 (0)", 0), + ("WAIT 信号 (1)", 1), + ("ACT 时间 (2)", 2), + ("EXT 信号 (3)", 3), + ("EXT 或完成 (4)", 4), +) + +STATUS_NAMES = { + 0: "未初始化", + 1: "空闲", + 2: "加速", + 3: "运行", + 4: "减速", + 5: "等待", + 6: "暂停", + 7: "完成", + 8: "停止", + 9: "错误", +} +ERROR_NAMES = { + 0: "无错误", + 1: "状态转换非法", + 2: "资源冲突", + 3: "资源非法", + 4: "定时器错误", + 5: "计数错误", + 6: "正限位", + 7: "负限位", + 8: "急停", + 9: "内部错误", +} + + +def option_labels(options): + return tuple(label for label, _value in options) + + +def option_label(options, value): + for label, candidate in options: + if candidate == value: + return label + return str(value) + + +def option_value(options, label, field_name): + for candidate_label, value in options: + if candidate_label == label: + return value + raise ValueError("%s 不是有效选项" % field_name) + + +def parse_integer(text, field_name, minimum, maximum): + raw = text.strip() + if not raw: + raise ValueError("%s 不能为空" % field_name) + try: + value = int(raw, 0) + except ValueError: + try: + value = int(raw, 10) + except ValueError as error: + raise ValueError("%s 必须是整数" % field_name) from error + if value < minimum or value > maximum: + raise ValueError( + "%s 必须在 %d 到 %d 之间" % (field_name, minimum, maximum) + ) + return value + + +class SerialWorker(threading.Thread): + """Own the serial port and report all results through a queue.""" + + def __init__(self): + super().__init__(name="plsr-serial", daemon=True) + self.commands = queue.Queue() + self.results = queue.Queue() + self.stop_event = threading.Event() + self.client = None + self.next_status_poll = 0.0 + + def submit(self, command, **payload): + self.commands.put((command, payload)) + + def close(self): + self.stop_event.set() + self.commands.put(("shutdown", {})) + + def _emit(self, event, **payload): + self.results.put((event, payload)) + + def _disconnect(self, notify=True, reason=""): + client = self.client + self.client = None + if client is not None: + try: + client.__exit__(None, None, None) + except (OSError, serial.SerialException): + pass + if notify: + self._emit("connection", connected=False, reason=reason) + + def _require_client(self): + if self.client is None: + raise RuntimeError("串口尚未连接") + return self.client + + def _read_configuration(self): + client = self._require_client() + common = client.read_holding(CONFIG_BASE, COMMON_WORDS) + segments = [] + for index in range(SEGMENT_COUNT): + address = SEGMENT_1_BASE + index * SEGMENT_STRIDE + segments.append(client.read_holding(address, SEGMENT_WORDS)) + self._emit("configuration", common=common, segments=segments) + + def _handle_connection_error(self, operation, error): + self._disconnect(notify=True, reason=str(error)) + self._emit("error", operation=operation, message=str(error), modal=True) + + def _handle_command(self, command, payload): + if command == "connect": + self._disconnect(notify=False) + try: + client = RtuClient( + payload["port"], + BAUD_RATE, + payload["slave"], + SERIAL_TIMEOUT_SECONDS, + ) + client.__enter__() + self.client = client + self.next_status_poll = 0.0 + self._emit( + "connection", + connected=True, + port=payload["port"], + slave=payload["slave"], + ) + self._read_configuration() + except (OSError, serial.SerialException) as error: + self._handle_connection_error("连接", error) + except (TestFailure, ModbusException, RuntimeError) as error: + self._disconnect(notify=True, reason=str(error)) + self._emit( + "error", operation="读取参数", message=str(error), modal=True + ) + return + + if command == "disconnect": + self._disconnect(notify=True) + return + + try: + client = self._require_client() + if command == "read_configuration": + self._read_configuration() + self._emit("operation", message="参数读取完成") + elif command == "write_common": + client.write_multiple(CONFIG_BASE, payload["words"]) + common = client.read_holding(CONFIG_BASE, COMMON_WORDS) + self._emit("common", words=common) + self._emit("operation", message="公共参数写入并回读完成") + elif command == "write_segments": + for index, words in enumerate(payload["segments"]): + address = SEGMENT_1_BASE + index * SEGMENT_STRIDE + client.write_multiple(address, words) + segments = [] + for index in range(SEGMENT_COUNT): + address = SEGMENT_1_BASE + index * SEGMENT_STRIDE + segments.append(client.read_holding(address, SEGMENT_WORDS)) + self._emit("segments", words=segments) + self._emit("operation", message="10 段参数写入并回读完成") + elif command == "control": + client.write_single(CONTROL, payload["value"]) + self._emit("operation", message=payload["message"]) + else: + raise RuntimeError("未知后台命令: %s" % command) + except (OSError, serial.SerialException) as error: + self._handle_connection_error(payload.get("operation", "通信"), error) + except (TestFailure, ModbusException, RuntimeError) as error: + self._emit( + "error", + operation=payload.get("operation", "通信"), + message=str(error), + modal=True, + ) + + def _poll_status(self): + try: + status = read_status(self._require_client()) + self._emit("status", status=status) + self.next_status_poll = time.monotonic() + STATUS_POLL_SECONDS + except (OSError, serial.SerialException) as error: + self._handle_connection_error("状态轮询", error) + except (TestFailure, ModbusException, RuntimeError) as error: + self._emit( + "error", operation="状态轮询", message=str(error), modal=False + ) + self.next_status_poll = time.monotonic() + 1.0 + + def run(self): + try: + while not self.stop_event.is_set(): + try: + command, payload = self.commands.get(timeout=0.05) + except queue.Empty: + command = None + payload = None + + if command == "shutdown": + break + if command is not None: + self._handle_command(command, payload) + + if ( + self.client is not None + and time.monotonic() >= self.next_status_poll + ): + self._poll_status() + finally: + self._disconnect(notify=False) + + +class PlsrControlPanel: + def __init__(self, root): + self.root = root + self.root.title("PLSR 控制面板") + self.root.geometry("1180x720") + self.root.minsize(980, 650) + self.root.protocol("WM_DELETE_WINDOW", self._on_close) + + self.connected = False + self.operation_pending = False + self.worker = SerialWorker() + self.worker.start() + + self.port_var = tk.StringVar() + self.slave_var = tk.StringVar(value="1") + self.connection_var = tk.StringVar(value="未连接") + self.footer_var = tk.StringVar(value="请选择串口并连接") + self.status_vars = { + "position": tk.StringVar(value="--"), + "frequency": tk.StringVar(value="--"), + "state": tk.StringVar(value="--"), + "segment": tk.StringVar(value="--"), + "error": tk.StringVar(value="--"), + } + self.common_vars = {} + self.segment_vars = [] + self.connection_widgets = [] + self.action_buttons = [] + + self._configure_style() + self._build_ui() + self.refresh_ports() + self._set_connected(False) + self.root.after(50, self._drain_results) + + def _configure_style(self): + default_font = tkfont.nametofont("TkDefaultFont") + default_font.configure(family="Microsoft YaHei UI", size=10) + text_font = tkfont.nametofont("TkTextFont") + text_font.configure(family="Microsoft YaHei UI", size=10) + style = ttk.Style(self.root) + style.configure("Connected.TLabel", foreground="#18794e") + style.configure("Disconnected.TLabel", foreground="#5f6368") + style.configure("Error.TLabel", foreground="#b42318") + style.configure("StatusValue.TLabel", font=("Microsoft YaHei UI", 11, "bold")) + + def _build_ui(self): + self.root.columnconfigure(0, weight=1) + self.root.rowconfigure(2, weight=1) + + connection = ttk.LabelFrame(self.root, text="连接") + connection.grid(row=0, column=0, padx=10, pady=(10, 6), sticky="ew") + connection.columnconfigure(7, weight=1) + + ttk.Label(connection, text="串口").grid(row=0, column=0, padx=(8, 4), pady=8) + self.port_box = ttk.Combobox( + connection, textvariable=self.port_var, width=13, state="readonly" + ) + self.port_box.grid(row=0, column=1, padx=4, pady=8) + refresh_button = ttk.Button(connection, text="刷新", command=self.refresh_ports) + refresh_button.grid(row=0, column=2, padx=(0, 12), pady=8) + ttk.Label(connection, text="串口参数").grid(row=0, column=3, padx=4, pady=8) + ttk.Label(connection, text="9600, 8E1").grid(row=0, column=4, padx=(4, 12), pady=8) + ttk.Label(connection, text="从站地址").grid(row=0, column=5, padx=4, pady=8) + slave_entry = ttk.Entry(connection, textvariable=self.slave_var, width=7) + slave_entry.grid(row=0, column=6, padx=(4, 12), pady=8) + self.connect_button = ttk.Button( + connection, text="连接", command=self._toggle_connection + ) + self.connect_button.grid(row=0, column=8, padx=8, pady=8) + self.connection_label = ttk.Label( + connection, + textvariable=self.connection_var, + style="Disconnected.TLabel", + width=28, + anchor="e", + ) + self.connection_label.grid(row=0, column=7, padx=8, pady=8, sticky="e") + self.connection_widgets = [self.port_box, refresh_button, slave_entry] + + self._build_runtime_status() + + notebook = ttk.Notebook(self.root) + notebook.grid(row=2, column=0, padx=10, pady=6, sticky="nsew") + common_tab = ttk.Frame(notebook, padding=12) + segment_tab = ttk.Frame(notebook, padding=12) + notebook.add(common_tab, text="公共参数 0x1000-0x1013") + notebook.add(segment_tab, text="10 段参数 0x1100-0x1197") + self._build_common_tab(common_tab) + self._build_segment_tab(segment_tab) + + footer = ttk.Frame(self.root) + footer.grid(row=3, column=0, padx=10, pady=(4, 10), sticky="ew") + footer.columnconfigure(4, weight=1) + read_button = ttk.Button(footer, text="读取全部参数", command=self._read_all) + read_button.grid(row=0, column=0, padx=(0, 6)) + common_button = ttk.Button( + footer, text="写入公共参数", command=self._write_common + ) + common_button.grid(row=0, column=1, padx=6) + segment_button = ttk.Button( + footer, text="写入 10 段参数", command=self._write_segments + ) + segment_button.grid(row=0, column=2, padx=6) + self.action_buttons.extend([read_button, common_button, segment_button]) + self.footer_label = ttk.Label( + footer, textvariable=self.footer_var, anchor="e" + ) + self.footer_label.grid(row=0, column=4, padx=(12, 0), sticky="ew") + + def _build_runtime_status(self): + frame = ttk.LabelFrame(self.root, text="实时状态 0x2000-0x2006") + frame.grid(row=1, column=0, padx=10, pady=6, sticky="ew") + for column in range(11): + frame.columnconfigure(column, weight=1 if column % 2 else 0) + + fields = ( + ("累计位置", "position"), + ("当前频率", "frequency"), + ("运行状态", "state"), + ("当前段", "segment"), + ("错误码", "error"), + ) + for index, (label, key) in enumerate(fields): + ttk.Label(frame, text=label).grid( + row=0, column=index * 2, padx=(8, 4), pady=9, sticky="e" + ) + ttk.Label( + frame, textvariable=self.status_vars[key], style="StatusValue.TLabel" + ).grid(row=0, column=index * 2 + 1, padx=(4, 12), pady=9, sticky="w") + + commands = ttk.Frame(frame) + commands.grid(row=0, column=10, padx=8, pady=6, sticky="e") + start_button = ttk.Button( + commands, text="启动", command=lambda: self._send_control(COMMAND_START) + ) + stop_button = ttk.Button( + commands, text="停止", command=lambda: self._send_control(COMMAND_STOP) + ) + clear_button = ttk.Button(commands, text="清零", command=self._clear_position) + start_button.grid(row=0, column=0, padx=3) + stop_button.grid(row=0, column=1, padx=3) + clear_button.grid(row=0, column=2, padx=3) + self.action_buttons.extend([start_button, stop_button, clear_button]) + + def _add_entry(self, parent, row, group, key, label, default="0", width=13): + column = group * 2 + ttk.Label(parent, text=label).grid( + row=row, column=column, padx=(8, 4), pady=7, sticky="e" + ) + variable = tk.StringVar(value=default) + ttk.Entry(parent, textvariable=variable, width=width).grid( + row=row, column=column + 1, padx=(4, 18), pady=7, sticky="w" + ) + self.common_vars[key] = variable + + def _add_combo(self, parent, row, group, key, label, options): + column = group * 2 + ttk.Label(parent, text=label).grid( + row=row, column=column, padx=(8, 4), pady=7, sticky="e" + ) + variable = tk.StringVar(value=options[0][0]) + ttk.Combobox( + parent, + textvariable=variable, + values=option_labels(options), + state="readonly", + width=18, + ).grid(row=row, column=column + 1, padx=(4, 18), pady=7, sticky="w") + self.common_vars[key] = variable + + def _build_common_tab(self, parent): + for column in (1, 3, 5): + parent.columnconfigure(column, weight=1) + + self._add_combo( + parent, 0, 0, "pulse_output", "脉冲输出（单逻辑 PLSR）", PULSE_OPTIONS + ) + self._add_combo( + parent, 0, 1, "direction_output", "方向输出", DIRECTION_OPTIONS + ) + self._add_entry(parent, 0, 2, "direction_delay", "方向延时 ms") + + self._add_combo(parent, 1, 0, "wait_input", "WAIT 输入", INPUT_OPTIONS) + self._add_combo(parent, 1, 1, "ext_input", "EXT 输入", INPUT_OPTIONS) + self._add_combo(parent, 1, 2, "send_mode", "发送模式", SEND_OPTIONS) + + self._add_combo(parent, 2, 0, "negative_logic", "方向逻辑", LOGIC_OPTIONS) + self._add_combo(parent, 2, 1, "curve_mode", "曲线模式", CURVE_OPTIONS) + self._add_combo(parent, 2, 2, "position_mode", "位置模式", POSITION_OPTIONS) + + self._add_entry(parent, 3, 0, "segment_count", "段数", default="1") + self._add_entry(parent, 3, 1, "start_segment", "起始段", default="1") + self._add_entry(parent, 3, 2, "default_speed", "基准速度 Hz", default="1000") + + self._add_entry(parent, 4, 0, "start_speed", "启动速度 Hz", default="100") + self._add_entry(parent, 4, 1, "stop_speed", "停止速度 Hz", default="100") + self._add_entry(parent, 4, 2, "acceleration", "加速时间 ms") + + self._add_entry(parent, 5, 0, "deceleration", "减速时间 ms") + + def _build_segment_tab(self, parent): + headers = ( + ("段 / 基址", 15), + ("频率 Hz", 13), + ("脉冲数", 15), + ("等待类型", 19), + ("WAIT ms", 11), + ("ACT ms", 11), + ("跳转段", 9), + ) + for column, (label, _width) in enumerate(headers): + parent.columnconfigure(column, weight=1 if column in (1, 2, 3) else 0) + ttk.Label(parent, text=label, anchor="center").grid( + row=0, column=column, padx=4, pady=(0, 6), sticky="ew" + ) + + for index in range(SEGMENT_COUNT): + address = SEGMENT_1_BASE + index * SEGMENT_STRIDE + ttk.Label(parent, text="%d / 0x%04X" % (index + 1, address)).grid( + row=index + 1, column=0, padx=(2, 8), pady=4, sticky="e" + ) + variables = { + "frequency": tk.StringVar(value="1000"), + "pulses": tk.StringVar(value="0"), + "wait_type": tk.StringVar(value=WAIT_OPTIONS[0][0]), + "wait_ms": tk.StringVar(value="0"), + "act_ms": tk.StringVar(value="0"), + "jump": tk.StringVar(value="0"), + } + ttk.Entry(parent, textvariable=variables["frequency"], width=13).grid( + row=index + 1, column=1, padx=4, pady=4, sticky="ew" + ) + ttk.Entry(parent, textvariable=variables["pulses"], width=15).grid( + row=index + 1, column=2, padx=4, pady=4, sticky="ew" + ) + ttk.Combobox( + parent, + textvariable=variables["wait_type"], + values=option_labels(WAIT_OPTIONS), + state="readonly", + width=19, + ).grid(row=index + 1, column=3, padx=4, pady=4, sticky="ew") + ttk.Entry(parent, textvariable=variables["wait_ms"], width=11).grid( + row=index + 1, column=4, padx=4, pady=4 + ) + ttk.Entry(parent, textvariable=variables["act_ms"], width=11).grid( + row=index + 1, column=5, padx=4, pady=4 + ) + ttk.Entry(parent, textvariable=variables["jump"], width=9).grid( + row=index + 1, column=6, padx=4, pady=4 + ) + self.segment_vars.append(variables) + + def refresh_ports(self): + ports = [item.device for item in list_ports.comports()] + self.port_box["values"] = ports + if ports and self.port_var.get() not in ports: + self.port_var.set(ports[0]) + elif not ports: + self.port_var.set("") + + def _set_connected(self, connected, description=""): + self.connected = connected + self.connect_button.configure( + text="断开" if connected else "连接", state="normal" + ) + connection_state = "disabled" if connected else "normal" + for widget in self.connection_widgets: + if widget is self.port_box: + widget.configure(state="disabled" if connected else "readonly") + else: + widget.configure(state=connection_state) + if connected: + self.connection_var.set(description or "已连接") + self.connection_label.configure(style="Connected.TLabel") + else: + self.connection_var.set(description or "未连接") + self.connection_label.configure( + style="Error.TLabel" if description else "Disconnected.TLabel" + ) + for variable in self.status_vars.values(): + variable.set("--") + self._update_action_state() + + def _update_action_state(self): + state = "normal" if self.connected and not self.operation_pending else "disabled" + for button in self.action_buttons: + button.configure(state=state) + + def _start_operation(self, message): + if not self.connected: + messagebox.showerror("未连接", "请先连接 PLSR 从站") + return False + if self.operation_pending: + return False + self.operation_pending = True + self.footer_var.set(message) + self._update_action_state() + return True + + def _finish_operation(self, message): + self.operation_pending = False + self.footer_var.set(message) + self.footer_label.configure(style="TLabel") + self._update_action_state() + + def _toggle_connection(self): + if self.connected: + self.operation_pending = False + self.footer_var.set("正在断开") + self.worker.submit("disconnect") + return + port = self.port_var.get().strip() + if not port: + messagebox.showerror("连接参数", "请选择可用串口") + return + try: + slave = parse_integer(self.slave_var.get(), "从站地址", 1, 247) + except ValueError as error: + messagebox.showerror("连接参数", str(error)) + return + self.connect_button.configure(state="disabled") + self.operation_pending = True + self._update_action_state() + self.connection_var.set("正在连接 %s" % port) + self.footer_var.set("正在打开串口并读取参数") + self.worker.submit("connect", port=port, slave=slave) + + def _common_words(self): + values = self.common_vars + segment_count = parse_integer(values["segment_count"].get(), "段数", 1, 10) + start_segment = parse_integer( + values["start_segment"].get(), "起始段", 1, 10 + ) + if start_segment > segment_count: + raise ValueError("起始段不能大于段数") + + words = [0] * COMMON_WORDS + words[0] = option_value( + PULSE_OPTIONS, values["pulse_output"].get(), "脉冲输出" + ) + words[1] = option_value( + DIRECTION_OPTIONS, values["direction_output"].get(), "方向输出" + ) + words[2] = option_value(INPUT_OPTIONS, values["wait_input"].get(), "WAIT 输入") + words[3] = option_value(INPUT_OPTIONS, values["ext_input"].get(), "EXT 输入") + words[4] = option_value(SEND_OPTIONS, values["send_mode"].get(), "发送模式") + words[5] = parse_integer( + values["direction_delay"].get(), "方向延时", 0, 0xFFFF + ) + words[6] = option_value( + LOGIC_OPTIONS, values["negative_logic"].get(), "方向逻辑" + ) + words[7] = option_value(CURVE_OPTIONS, values["curve_mode"].get(), "曲线模式") + words[8] = option_value( + POSITION_OPTIONS, values["position_mode"].get(), "位置模式" + ) + words[9] = segment_count + words[10] = start_segment + words[11:13] = split_u32( + parse_integer( + values["default_speed"].get(), "基准速度", 1, MAX_FREQUENCY_HZ + ) + ) + words[13:15] = split_u32( + parse_integer( + values["start_speed"].get(), "启动速度", 0, MAX_FREQUENCY_HZ + ) + ) + words[15] = 0 + words[16:18] = split_u32( + parse_integer( + values["stop_speed"].get(), "停止速度", 0, MAX_FREQUENCY_HZ + ) + ) + words[18] = parse_integer(values["acceleration"].get(), "加速时间", 0, 0xFFFF) + words[19] = parse_integer(values["deceleration"].get(), "减速时间", 0, 0xFFFF) + return words + + def _segment_words(self): + segment_count = parse_integer( + self.common_vars["segment_count"].get(), "段数", 1, SEGMENT_COUNT + ) + encoded = [] + for index, variables in enumerate(self.segment_vars): + name = "第 %d 段" % (index + 1) + frequency = parse_integer( + variables["frequency"].get(), name + "频率", 1, MAX_FREQUENCY_HZ + ) + pulses = parse_integer( + variables["pulses"].get(), name + "脉冲数", -(1 << 31), (1 << 31) - 1 + ) + wait_type = option_value( + WAIT_OPTIONS, variables["wait_type"].get(), name + "等待类型" + ) + wait_ms = parse_integer( + variables["wait_ms"].get(), name + " WAIT 时间", 0, 0xFFFF + ) + act_ms = parse_integer( + variables["act_ms"].get(), name + " ACT 时间", 0, 0xFFFF + ) + jump = parse_integer(variables["jump"].get(), name + "跳转段", 0, 10) + if index < segment_count and jump > segment_count: + raise ValueError( + "%s跳转段必须为 0 或不大于脉冲总段数 %d" + % (name, segment_count) + ) + words = split_u32(frequency) + split_i32(pulses) + words.extend((wait_type, wait_ms, act_ms, jump)) + encoded.append(words) + return encoded + + def _set_common_words(self, words): + if len(words) != COMMON_WORDS: + raise ValueError("公共参数回读长度错误") + values = self.common_vars + values["pulse_output"].set(option_label(PULSE_OPTIONS, words[0])) + values["direction_output"].set(option_label(DIRECTION_OPTIONS, words[1])) + values["wait_input"].set(option_label(INPUT_OPTIONS, words[2])) + values["ext_input"].set(option_label(INPUT_OPTIONS, words[3])) + values["send_mode"].set(option_label(SEND_OPTIONS, words[4])) + values["direction_delay"].set(str(words[5])) + values["negative_logic"].set(option_label(LOGIC_OPTIONS, words[6])) + values["curve_mode"].set(option_label(CURVE_OPTIONS, words[7])) + values["position_mode"].set(option_label(POSITION_OPTIONS, words[8])) + values["segment_count"].set(str(words[9])) + values["start_segment"].set(str(words[10])) + values["default_speed"].set(str(words[11] | (words[12] << 16))) + values["start_speed"].set(str(words[13] | (words[14] << 16))) + values["stop_speed"].set(str(words[16] | (words[17] << 16))) + values["acceleration"].set(str(words[18])) + values["deceleration"].set(str(words[19])) + + def _set_segment_words(self, segments): + if len(segments) != SEGMENT_COUNT: + raise ValueError("段参数回读数量错误") + for index, words in enumerate(segments): + if len(words) != SEGMENT_WORDS: + raise ValueError("第 %d 段回读长度错误" % (index + 1)) + variables = self.segment_vars[index] + frequency = words[0] | (words[1] << 16) + pulse_bits = words[2] | (words[3] << 16) + pulses = pulse_bits - (1 << 32) if pulse_bits & 0x80000000 else pulse_bits + variables["frequency"].set(str(frequency)) + variables["pulses"].set(str(pulses)) + variables["wait_type"].set(option_label(WAIT_OPTIONS, words[4])) + variables["wait_ms"].set(str(words[5])) + variables["act_ms"].set(str(words[6])) + variables["jump"].set(str(words[7])) + + def _read_all(self): + if self._start_operation("正在读取公共参数和 10 段参数"): + self.worker.submit("read_configuration", operation="读取参数") + + def _write_common(self): + try: + words = self._common_words() + except ValueError as error: + messagebox.showerror("参数错误", str(error)) + return + if self._start_operation("正在写入公共参数"): + self.worker.submit( + "write_common", words=words, operation="写入公共参数" + ) + + def _write_segments(self): + try: + segments = self._segment_words() + except ValueError as error: + messagebox.showerror("参数错误", str(error)) + return + if self._start_operation("正在写入 10 段参数"): + self.worker.submit( + "write_segments", segments=segments, operation="写入段参数" + ) + + def _send_control(self, command): + messages = { + COMMAND_START: "启动命令已发送", + COMMAND_STOP: "停止命令已发送", + COMMAND_CLEAR: "清零命令已发送", + } + if self._start_operation("正在发送控制命令"): + self.worker.submit( + "control", + value=command, + message=messages[command], + operation="控制命令", + ) + + def _clear_position(self): + if messagebox.askyesno("确认清零", "确认将累计位置清零？"): + self._send_control(COMMAND_CLEAR) + + def _show_status(self, status): + self.status_vars["position"].set(str(status.position)) + self.status_vars["frequency"].set("%d Hz" % status.frequency_hz) + self.status_vars["state"].set( + "%s (%d)" % (STATUS_NAMES.get(status.state, "未知"), status.state) + ) + self.status_vars["segment"].set(str(status.segment)) + self.status_vars["error"].set( + "%s (%d)" % (ERROR_NAMES.get(status.error, "未知"), status.error) + ) + + def _handle_result(self, event, payload): + if event == "connection": + if payload["connected"]: + description = "%s / 从站 %d" % (payload["port"], payload["slave"]) + self._set_connected(True, description) + self.footer_var.set("已连接，正在读取参数") + else: + reason = payload.get("reason", "") + self.operation_pending = False + self.connect_button.configure(state="normal") + self._set_connected(False, reason) + self.footer_var.set("连接已断开" if not reason else "通信错误") + elif event == "configuration": + self._set_common_words(payload["common"]) + self._set_segment_words(payload["segments"]) + self._finish_operation("全部参数已读取") + elif event == "common": + self._set_common_words(payload["words"]) + elif event == "segments": + self._set_segment_words(payload["words"]) + elif event == "status": + self._show_status(payload["status"]) + elif event == "operation": + self._finish_operation(payload["message"]) + elif event == "error": + message = "%s失败：%s" % (payload["operation"], payload["message"]) + self._finish_operation(message) + self.footer_label.configure(style="Error.TLabel") + if payload.get("modal", False): + messagebox.showerror("PLSR 通信错误", message) + + def _drain_results(self): + try: + while True: + event, payload = self.worker.results.get_nowait() + try: + self._handle_result(event, payload) + except (KeyError, ValueError) as error: + self._finish_operation("界面数据错误：%s" % error) + except queue.Empty: + pass + self.root.after(50, self._drain_results) + + def _on_close(self): + self.worker.close() + self.root.destroy() + + +class _SelfTestVariable: + def __init__(self, value): + self.value = str(value) + + def get(self): + return self.value + + def set(self, value): + self.value = str(value) + + +def self_test(): + assert BAUD_RATE == 9600 + assert INPUT_OPTIONS == (("X4 (0)", 0), ("X5 (1)", 1)) + assert [SEGMENT_1_BASE + index * SEGMENT_STRIDE + for index in range(SEGMENT_COUNT)] == [ + 0x1100, 0x1110, 0x1120, 0x1130, 0x1140, + 0x1150, 0x1160, 0x1170, 0x1180, 0x1190, + ] + assert parse_integer("0x10", "测试值", 0, 16) == 16 + try: + parse_integer("17", "测试值", 0, 16) + except ValueError: + pass + else: + raise AssertionError("range validation did not reject 17") + + panel = PlsrControlPanel.__new__(PlsrControlPanel) + panel.common_vars = { + "pulse_output": _SelfTestVariable(PULSE_OPTIONS[3][0]), + "direction_output": _SelfTestVariable(DIRECTION_OPTIONS[3][0]), + "wait_input": _SelfTestVariable(INPUT_OPTIONS[1][0]), + "ext_input": _SelfTestVariable(INPUT_OPTIONS[0][0]), + "send_mode": _SelfTestVariable(SEND_OPTIONS[1][0]), + "direction_delay": _SelfTestVariable(25), + "negative_logic": _SelfTestVariable(LOGIC_OPTIONS[1][0]), + "curve_mode": _SelfTestVariable(CURVE_OPTIONS[2][0]), + "position_mode": _SelfTestVariable(POSITION_OPTIONS[1][0]), + "segment_count": _SelfTestVariable(10), + "start_segment": _SelfTestVariable(2), + "default_speed": _SelfTestVariable(100000), + "start_speed": _SelfTestVariable(0), + "stop_speed": _SelfTestVariable(65536), + "acceleration": _SelfTestVariable(1000), + "deceleration": _SelfTestVariable(2000), + } + common = panel._common_words() + assert common[:11] == [3, 3, 1, 0, 1, 25, 1, 2, 1, 10, 2] + assert common[11:20] == [34464, 1, 0, 0, 0, 0, 1, 1000, 2000] + + panel.segment_vars = [] + for index in range(SEGMENT_COUNT): + panel.segment_vars.append({ + "frequency": _SelfTestVariable(1000 + index), + "pulses": _SelfTestVariable(-1 if index == 0 else index), + "wait_type": _SelfTestVariable(WAIT_OPTIONS[4][0]), + "wait_ms": _SelfTestVariable(index), + "act_ms": _SelfTestVariable(index + 1), + "jump": _SelfTestVariable(0), + }) + segments = panel._segment_words() + assert len(segments) == SEGMENT_COUNT + assert all(len(words) == SEGMENT_WORDS for words in segments) + assert segments[0] == [1000, 0, 0xFFFF, 0xFFFF, 4, 0, 1, 0] + + panel.common_vars["segment_count"].set(1) + panel.segment_vars[0]["jump"].set(2) + try: + panel._segment_words() + except ValueError: + pass + else: + raise AssertionError("active segment jump exceeded segment count") + print("PLSR control panel self-test passed; no GUI or serial port was opened") + + +def parse_arguments(arguments=None): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--self-test", + action="store_true", + help="validate register encoding without opening the GUI or serial port", + ) + return parser.parse_args(arguments) + + +def main(arguments=None): + args = parse_arguments(arguments) + if args.self_test: + self_test() + return 0 + root = tk.Tk() + PlsrControlPanel(root) + root.mainloop() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/HostComputer/plsr_modbus_product_test.py b/HostComputer/plsr_modbus_product_test.py new file mode 100644 index 0000000..a1661a1 --- /dev/null +++ b/HostComputer/plsr_modbus_product_test.py @@ -0,0 +1,1162 @@ +"""Minimal board test for the 2026 PLSR Modbus product register map. + +The script is inert unless --run is supplied. Motion phases additionally +require --allow-motion because they drive the configured pulse output. +""" + +import argparse +import dataclasses +import re +import subprocess +import sys +import time + +import serial + + +CONFIG_BASE = 0x1000 +COMMON_WORDS = 0x14 +SEGMENT_1_BASE = 0x1100 +SEGMENT_2_BASE = 0x1110 +SEGMENT_WORDS = 8 +STATUS_BASE = 0x2000 +STATUS_WORDS = 7 +CONTROL = 0x3000 + +COMMAND_START = 0x0001 +COMMAND_STOP = 0x0002 +COMMAND_CLEAR = 0x0004 + +STATUS_IDLE = 1 +STATUS_ACCELERATING = 2 +STATUS_RUNNING = 3 +STATUS_DECELERATING = 4 +STATUS_WAITING = 5 +STATUS_COMPLETED = 7 +STATUS_STOPPED = 8 +STATUS_ERROR = 9 +ERROR_NONE = 0 + +WAIT_TIME = 0 +WAIT_SIGNAL = 1 +ACT_TIME = 2 +EXT_SIGNAL = 3 +EXT_OR_COMPLETE = 4 + +SEND_COMPLETE = 0 +SEND_SUBSEQUENT = 1 + +DEFAULT_STLINK_CLI = r"F:\ST-LINK Utility\ST-LINK_CLI.exe" + +GPIOE_CLOCK_BIT = 0x42470610 +GPIOI_CLOCK_BIT = 0x42470620 +GPIOE_PIN6_OUTPUT_BIT = 0x42420298 +GPIOI_PIN8_OUTPUT_BIT = 0x424402A0 +GPIOE_PIN6_MODE_LOW_BIT = 0x42420030 +GPIOE_PIN6_MODE_HIGH_BIT = 0x42420034 +GPIOI_PIN8_MODE_LOW_BIT = 0x42440040 +GPIOI_PIN8_MODE_HIGH_BIT = 0x42440044 +GPIOE_PIN6_PULL_LOW_BIT = 0x424201B0 +GPIOE_PIN6_PULL_HIGH_BIT = 0x424201B4 +GPIOI_PIN8_PULL_LOW_BIT = 0x424401C0 +GPIOI_PIN8_PULL_HIGH_BIT = 0x424401C4 +X4_INPUT_BIT = 0x42408214 +X5_INPUT_BIT = 0x42430230 + +OUTPUT_OFF = 1 +OUTPUT_ON = 0 + +EX_ILLEGAL_FUNCTION = 0x01 +EX_ILLEGAL_ADDRESS = 0x02 +EX_ILLEGAL_VALUE = 0x03 +EX_DEVICE_FAILURE = 0x04 +EX_DEVICE_BUSY = 0x06 + +ACTIVE_STATES = { + STATUS_ACCELERATING, + STATUS_RUNNING, + STATUS_DECELERATING, + STATUS_WAITING, +} +TERMINAL_STATES = {STATUS_COMPLETED, STATUS_STOPPED, STATUS_ERROR} + + +class TestFailure(RuntimeError): + pass + + +class ModbusException(RuntimeError): + def __init__(self, function, code): + super().__init__( + "Modbus exception: function=0x%02X code=0x%02X" + % (function, code) + ) + self.function = function + self.code = code + + +def parse_stlink_word(output, address): + pattern = r"(?im)^\s*0x%08x\s*:\s*([0-9a-f]{8})\s*$" % address + match = re.search(pattern, output) + require(match is not None, + "ST-LINK output did not contain address 0x%08X" % address) + return int(match.group(1), 16) + + +class X45Fixture: + """Drive Y4/Y5 through SWD without adding product test registers.""" + + def __init__(self, executable, probe_id, timeout): + self.executable = executable + self.probe_id = probe_id + self.timeout = timeout + + def _run(self, *arguments): + command_line = [ + self.executable, + "-c", + "ID=%d" % self.probe_id, + "SWD", + "HOTPLUG", + ] + list(arguments) + ["-Q", "-NoPrompt"] + try: + result = subprocess.run( + command_line, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + errors="replace", + timeout=self.timeout, + check=False, + ) + except (OSError, subprocess.SubprocessError) as error: + raise TestFailure("ST-LINK command failed: %s" % error) from error + require( + result.returncode == 0, + "ST-LINK command returned %d:\n%s" + % (result.returncode, result.stdout.strip()), + ) + return result.stdout + + def prepare(self): + self._run( + "-w32", hex(GPIOE_CLOCK_BIT), "1", + "-w32", hex(GPIOI_CLOCK_BIT), "1", + "-w32", hex(GPIOI_PIN8_OUTPUT_BIT), str(OUTPUT_OFF), + "-w32", hex(GPIOE_PIN6_OUTPUT_BIT), str(OUTPUT_OFF), + "-w32", hex(GPIOI_PIN8_MODE_HIGH_BIT), "0", + "-w32", hex(GPIOI_PIN8_MODE_LOW_BIT), "1", + "-w32", hex(GPIOE_PIN6_MODE_HIGH_BIT), "0", + "-w32", hex(GPIOE_PIN6_MODE_LOW_BIT), "1", + ) + time.sleep(0.050) + + def drive(self, input_selection, active): + require(input_selection in (0, 1), "input selection must be X4 or X5") + if input_selection == 0: + address = GPIOI_PIN8_OUTPUT_BIT + else: + address = GPIOE_PIN6_OUTPUT_BIT + value = OUTPUT_ON if active else OUTPUT_OFF + self._run("-w32", hex(address), str(value)) + time.sleep(0.050) + + def read(self, input_selection): + require(input_selection in (0, 1), "input selection must be X4 or X5") + address = X4_INPUT_BIT if input_selection == 0 else X5_INPUT_BIT + output = self._run("-r32", hex(address), "1") + return parse_stlink_word(output, address) & 1 + + def all_off(self): + self._run( + "-w32", hex(GPIOI_PIN8_OUTPUT_BIT), str(OUTPUT_OFF), + "-w32", hex(GPIOE_PIN6_OUTPUT_BIT), str(OUTPUT_OFF), + ) + time.sleep(0.050) + + def release(self): + self._run( + "-w32", hex(GPIOI_PIN8_OUTPUT_BIT), str(OUTPUT_OFF), + "-w32", hex(GPIOE_PIN6_OUTPUT_BIT), str(OUTPUT_OFF), + "-w32", hex(GPIOI_PIN8_MODE_LOW_BIT), "0", + "-w32", hex(GPIOI_PIN8_MODE_HIGH_BIT), "0", + "-w32", hex(GPIOI_PIN8_PULL_LOW_BIT), "0", + "-w32", hex(GPIOI_PIN8_PULL_HIGH_BIT), "0", + "-w32", hex(GPIOE_PIN6_MODE_LOW_BIT), "0", + "-w32", hex(GPIOE_PIN6_MODE_HIGH_BIT), "0", + "-w32", hex(GPIOE_PIN6_PULL_LOW_BIT), "0", + "-w32", hex(GPIOE_PIN6_PULL_HIGH_BIT), "0", + ) + + +def require(condition, message): + if not condition: + raise TestFailure(message) + + +def crc16(data): + value = 0xFFFF + for byte in data: + value ^= byte + for _ in range(8): + if value & 1: + value = (value >> 1) ^ 0xA001 + else: + value >>= 1 + return value + + +def append_crc(payload): + checksum = crc16(payload) + return bytes(payload) + bytes((checksum & 0xFF, checksum >> 8)) + + +def split_u32(value): + value &= 0xFFFFFFFF + return [value & 0xFFFF, value >> 16] + + +def split_i32(value): + require(-(1 << 31) <= value < (1 << 31), "signed value is not int32") + return split_u32(value) + + +def join_u32(low_word, high_word): + return low_word | (high_word << 16) + + +def join_i32(low_word, high_word): + value = join_u32(low_word, high_word) + return value - (1 << 32) if value & 0x80000000 else value + + +def frequency_matches(actual_hz, requested_hz): + tolerance_hz = max(1, requested_hz // 1000) + return abs(actual_hz - requested_hz) <= tolerance_hz + + +@dataclasses.dataclass +class Status: + position: int + frequency_hz: int + state: int + segment: int + error: int + + +class RtuClient: + def __init__(self, port, baud, slave, timeout): + self.port_name = port + self.baud = baud + self.slave = slave + self.timeout = timeout + self.character_seconds = 11.0 / baud + if baud > 19200: + self.frame_gap_seconds = 0.00175 + else: + self.frame_gap_seconds = 3.5 * self.character_seconds + self.serial_port = None + self.last_request_finished = 0.0 + + def __enter__(self): + self.serial_port = serial.Serial( + port=self.port_name, + baudrate=self.baud, + bytesize=serial.EIGHTBITS, + parity=serial.PARITY_EVEN, + stopbits=serial.STOPBITS_ONE, + timeout=0, + write_timeout=1, + ) + self.serial_port.reset_input_buffer() + self.serial_port.reset_output_buffer() + return self + + def __exit__(self, exc_type, exc_value, traceback): + if self.serial_port is not None: + self.serial_port.close() + self.serial_port = None + + def _exchange(self, pdu): + request = append_crc(bytes((self.slave,)) + bytes(pdu)) + now = time.monotonic() + delay = self.frame_gap_seconds - (now - self.last_request_finished) + if delay > 0: + time.sleep(delay) + + self.serial_port.reset_input_buffer() + self.serial_port.write(request) + self.serial_port.flush() + + response = bytearray() + deadline = time.monotonic() + self.timeout + expected_length = None + while time.monotonic() < deadline: + waiting = self.serial_port.in_waiting + if waiting: + response.extend(self.serial_port.read(waiting)) + if len(response) >= 2 and response[1] == (pdu[0] | 0x80): + expected_length = 5 + elif pdu[0] == 0x03 and len(response) >= 3: + expected_length = response[2] + 5 + elif pdu[0] in (0x06, 0x10): + expected_length = 8 + if expected_length is not None and len(response) >= expected_length: + break + if not waiting: + time.sleep(0.0005) + + self.last_request_finished = time.monotonic() + require(response, "no Modbus response to %s" % request.hex(" ")) + require(expected_length is not None, "response header is incomplete") + require(len(response) == expected_length, + "incomplete or oversized Modbus response: %s" % response.hex(" ")) + require(len(response) >= 5, "short Modbus response: %s" % response.hex(" ")) + received_crc = response[-2] | (response[-1] << 8) + require( + received_crc == crc16(response[:-2]), + "bad response CRC: %s" % response.hex(" "), + ) + require(response[0] == self.slave, "response slave address mismatch") + + requested_function = pdu[0] + if response[1] == (requested_function | 0x80): + require(len(response) == 5, "invalid exception response length") + raise ModbusException(requested_function, response[2]) + require(response[1] == requested_function, "response function mismatch") + return bytes(response) + + def read_holding(self, address, quantity): + require(1 <= quantity <= 125, "FC03 quantity must be 1..125") + pdu = bytes( + ( + 0x03, + address >> 8, + address & 0xFF, + quantity >> 8, + quantity & 0xFF, + ) + ) + response = self._exchange(pdu) + byte_count = response[2] + require(byte_count == quantity * 2, "FC03 byte count mismatch") + require(len(response) == byte_count + 5, "FC03 response length mismatch") + return [ + (response[3 + index * 2] << 8) | response[4 + index * 2] + for index in range(quantity) + ] + + def write_single(self, address, value): + value &= 0xFFFF + pdu = bytes( + ( + 0x06, + address >> 8, + address & 0xFF, + value >> 8, + value & 0xFF, + ) + ) + response = self._exchange(pdu) + require(response[:6] == bytes((self.slave,)) + pdu, "FC06 echo mismatch") + + def write_multiple(self, address, values): + require(1 <= len(values) <= 123, "FC10 quantity must be 1..123") + data = bytearray() + for value in values: + value &= 0xFFFF + data.extend((value >> 8, value & 0xFF)) + quantity = len(values) + pdu = bytes( + ( + 0x10, + address >> 8, + address & 0xFF, + quantity >> 8, + quantity & 0xFF, + len(data), + ) + ) + bytes(data) + response = self._exchange(pdu) + expected = bytes( + ( + self.slave, + 0x10, + address >> 8, + address & 0xFF, + quantity >> 8, + quantity & 0xFF, + ) + ) + require(response[:6] == expected, "FC10 acknowledgement mismatch") + + +def read_status(client): + words = client.read_holding(STATUS_BASE, STATUS_WORDS) + return Status( + position=join_i32(words[0], words[1]), + frequency_hz=join_u32(words[2], words[3]), + state=words[4], + segment=words[5], + error=words[6], + ) + + +def wait_for_status(client, predicate, timeout, description): + deadline = time.monotonic() + timeout + last_status = None + while time.monotonic() < deadline: + last_status = read_status(client) + if last_status.state == STATUS_ERROR: + raise TestFailure( + "%s entered ERROR (code=%d)" % (description, last_status.error) + ) + if predicate(last_status): + return last_status + time.sleep(0.005) + raise TestFailure("timeout waiting for %s; last=%r" % (description, last_status)) + + +def wait_terminal(client, timeout): + status = wait_for_status( + client, + lambda item: item.state in TERMINAL_STATES, + timeout, + "terminal state", + ) + require(status.state != STATUS_ERROR, "motion ended in ERROR %d" % status.error) + return status + + +def expect_exception(operation, expected_code, label): + try: + operation() + except ModbusException as error: + require( + error.code == expected_code, + "%s expected exception 0x%02X, got 0x%02X" + % (label, expected_code, error.code), + ) + return + raise TestFailure("%s did not return a Modbus exception" % label) + + +def make_common( + position_mode=0, + segment_count=1, + start_segment=1, + send_mode=SEND_COMPLETE, + curve_mode=0, + default_hz=1000, + start_hz=500, + stop_hz=100, + acceleration_ms=0, + deceleration_ms=0, +): + words = [0] * COMMON_WORDS + words[0x00] = 0 + words[0x01] = 0 + words[0x02] = 0 + words[0x03] = 0 + words[0x04] = send_mode + words[0x05] = 0 + words[0x06] = 0 + words[0x07] = curve_mode + words[0x08] = position_mode + words[0x09] = segment_count + words[0x0A] = start_segment + words[0x0B:0x0D] = split_u32(default_hz) + words[0x0D:0x0F] = split_u32(start_hz) + words[0x0F] = 0 + words[0x10:0x12] = split_u32(stop_hz) + words[0x12] = acceleration_ms + words[0x13] = deceleration_ms + return words + + +def make_segment( + frequency_hz, + pulses, + wait_type=EXT_OR_COMPLETE, + wait_time_ms=0, + act_time_ms=0, + jump_segment=0, +): + return ( + split_u32(frequency_hz) + + split_i32(pulses) + + [wait_type, wait_time_ms, act_time_ms, jump_segment] + ) + + +def configure(client, common, segment_1, segment_2=None): + client.write_multiple(CONFIG_BASE, common) + client.write_multiple(SEGMENT_1_BASE, segment_1) + if segment_2 is not None: + client.write_multiple(SEGMENT_2_BASE, segment_2) + + +def restore_default_configuration(client): + common = make_common( + default_hz=1000, + start_hz=100, + stop_hz=100, + acceleration_ms=100, + deceleration_ms=100, + ) + common[0x05] = 10 + client.write_multiple(CONFIG_BASE, common) + for index in range(10): + base = SEGMENT_1_BASE + index * 0x10 + pulses = 1000 if index == 0 else 0 + client.write_multiple(base, make_segment(1000, pulses)) + + +def command(client, value): + client.write_single(CONTROL, value) + + +def clear_position(client): + command(client, COMMAND_CLEAR) + status = read_status(client) + require(status.state == STATUS_IDLE, "CLEAR did not enter IDLE") + require(status.position == 0, "CLEAR did not zero logical position") + + +def run_case(name, function): + started = time.monotonic() + print("RUN %s" % name) + function() + print("PASS %s (%.3fs)" % (name, time.monotonic() - started)) + + +def test_fixed_map_and_exceptions(client): + common = client.read_holding(CONFIG_BASE, COMMON_WORDS) + segment = client.read_holding(SEGMENT_1_BASE, SEGMENT_WORDS) + status = read_status(client) + control = client.read_holding(CONTROL, 1) + require(len(common) == COMMON_WORDS, "common map read failed") + require(len(segment) == SEGMENT_WORDS, "segment map read failed") + require(0 <= status.state <= STATUS_ERROR, "status enum is out of range") + require(control == [0], "control register must read as zero") + require(client.read_holding(0x100F, 1) == [0], "reserved word is not zero") + require(client.read_holding(0x1108, 1) == [0], "segment padding is not zero") + + expect_exception( + lambda: client.read_holding(0x0000, 1), + EX_ILLEGAL_ADDRESS, + "FC03 address 0", + ) + expect_exception( + lambda: client._exchange(bytes((0x01, 0x00, 0x00, 0x00, 0x01))), + EX_ILLEGAL_FUNCTION, + "FC01 unsupported function", + ) + expect_exception( + lambda: client.read_holding(0x1197, 2), + EX_ILLEGAL_ADDRESS, + "range crossing 0x1197", + ) + expect_exception( + lambda: client.write_single(0x100F, 1), + EX_ILLEGAL_VALUE, + "nonzero reserved write", + ) + expect_exception( + lambda: client.write_single(0x100B, 1000), + EX_ILLEGAL_ADDRESS, + "single half of a DWORD", + ) + expect_exception( + lambda: client.write_single(STATUS_BASE, 0), + EX_ILLEGAL_ADDRESS, + "status write", + ) + expect_exception( + lambda: client.write_single(CONTROL, 3), + EX_ILLEGAL_VALUE, + "combined control command", + ) + + +def test_positive_negative_and_absolute_zero(client): + clear_position(client) + common = make_common(position_mode=0, start_hz=500) + configure(client, common, make_segment(500, 25)) + command(client, COMMAND_START) + status = wait_terminal(client, 3.0) + require(status.state == STATUS_COMPLETED, "positive move did not complete") + require(status.position == 25, "positive accumulation expected 25") + + configure(client, common, make_segment(500, -10)) + command(client, COMMAND_START) + status = wait_terminal(client, 3.0) + require(status.state == STATUS_COMPLETED, "negative move did not complete") + require(status.position == 15, "negative accumulation expected 15") + + absolute = make_common(position_mode=1, start_hz=500) + configure(client, absolute, make_segment(500, 15)) + before = read_status(client) + command(client, COMMAND_START) + status = wait_terminal(client, 1.0) + require(status.state == STATUS_COMPLETED, "absolute zero move did not complete") + require(status.position == before.position, "absolute zero changed position") + + +def test_all_outputs_at_100khz(client): + for output in range(4): + clear_position(client) + common = make_common( + default_hz=100000, + start_hz=100000, + stop_hz=100000, + ) + common[0x00] = output + common[0x01] = output + configure(client, common, make_segment(100000, 1000)) + command(client, COMMAND_START) + status = wait_terminal(client, 2.0) + require(status.state == STATUS_COMPLETED, + "output %d did not complete" % output) + require(status.position == 1000, + "output %d count expected 1000, got %d" + % (output, status.position)) + + +def test_short_final_profiles(client): + for curve_mode in range(3): + clear_position(client) + common = make_common( + curve_mode=curve_mode, + default_hz=100000, + start_hz=100000, + stop_hz=100, + deceleration_ms=1000, + ) + configure(client, common, make_segment(100000, 10)) + command(client, COMMAND_START) + status = wait_terminal(client, 2.0) + require(status.state == STATUS_COMPLETED, + "short curve %d did not complete" % curve_mode) + require(status.error == ERROR_NONE, + "short curve %d reported error %d" + % (curve_mode, status.error)) + require(status.position == 10, + "short curve %d count expected 10, got %d" + % (curve_mode, status.position)) + + +def test_wait_time(client): + clear_position(client) + configure( + client, + make_common(start_hz=500), + make_segment(500, 10, wait_type=WAIT_TIME, wait_time_ms=120), + ) + command(client, COMMAND_START) + waiting = wait_for_status( + client, + lambda item: item.state == STATUS_WAITING, + 2.0, + "WAIT_TIME state", + ) + waiting_at = time.monotonic() + require(waiting.position == 10, "WAIT_TIME position expected 10") + status = wait_terminal(client, 2.0) + observed_wait = time.monotonic() - waiting_at + require(status.state == STATUS_COMPLETED, "WAIT_TIME did not complete") + require(observed_wait >= 0.050, "WAIT_TIME completed implausibly early") + + +def test_act_time(client): + clear_position(client) + configure( + client, + make_common(start_hz=1000), + make_segment(1000, 2000, wait_type=ACT_TIME, act_time_ms=80), + ) + command(client, COMMAND_START) + status = wait_terminal(client, 3.0) + require(status.state == STATUS_COMPLETED, "ACT_TIME did not complete") + require(0 < status.position < 2000, "ACT_TIME did not cut the segment") + + +def test_dynamic_frequency_and_repeated_stop(client): + clear_position(client) + configure( + client, + make_common(start_hz=500, deceleration_ms=150), + make_segment(500, 3000), + ) + command(client, COMMAND_START) + wait_for_status( + client, + lambda item: item.segment == 1 and item.frequency_hz == 500, + 2.0, + "initial frequency", + ) + client.write_multiple(SEGMENT_1_BASE, split_u32(1200)) + dynamic = wait_for_status( + client, + lambda item: item.segment == 1 + and frequency_matches(item.frequency_hz, 1200), + 2.0, + "dynamic frequency", + ) + print("INFO dynamic frequency requested=1200 actual=%d" % + dynamic.frequency_hz) + expect_exception( + lambda: client.write_single(CONFIG_BASE, 1), + EX_DEVICE_BUSY, + "pulse output write while busy", + ) + command(client, COMMAND_STOP) + command(client, COMMAND_STOP) + status = wait_terminal(client, 3.0) + require(status.state == STATUS_STOPPED, "repeated STOP did not stop") + require(status.position < 3000, "STOP did not cut the active move") + + +def test_future_segment_frequency_applies_on_arrival(client): + clear_position(client) + common = make_common(segment_count=2, start_hz=400) + segment_1 = make_segment(400, 200) + segment_2 = make_segment(700, 200) + configure(client, common, segment_1, segment_2) + command(client, COMMAND_START) + wait_for_status( + client, + lambda item: item.segment == 1 and item.frequency_hz == 400, + 2.0, + "segment 1", + ) + client.write_multiple(SEGMENT_2_BASE, split_u32(900)) + status = read_status(client) + require(status.segment == 1, "future write changed the current segment") + require(status.frequency_hz == 400, "future write changed current frequency") + + wait_for_status( + client, + lambda item: item.segment == 2 + and frequency_matches(item.frequency_hz, 900), + 3.0, + "updated segment 2 frequency", + ) + status = wait_terminal(client, 3.0) + require(status.state == STATUS_COMPLETED, "two-segment move did not complete") + + +def test_subsequent_future_frequency_rebuilds_handoff(client): + clear_position(client) + common = make_common( + segment_count=2, + send_mode=SEND_SUBSEQUENT, + start_hz=400, + ) + segment_1 = make_segment(400, 200) + segment_2 = make_segment(700, 200) + configure(client, common, segment_1, segment_2) + command(client, COMMAND_START) + wait_for_status( + client, + lambda item: item.segment == 1 + and frequency_matches(item.frequency_hz, 400), + 2.0, + "subsequent segment 1", + ) + client.write_multiple(SEGMENT_2_BASE, split_u32(900)) + status = read_status(client) + require(status.segment == 1, "future write changed the current segment") + require( + frequency_matches(status.frequency_hz, 400), + "future write changed current frequency", + ) + + wait_for_status( + client, + lambda item: item.segment == 2 + and frequency_matches(item.frequency_hz, 900), + 3.0, + "rebuilt subsequent handoff frequency", + ) + status = wait_terminal(client, 3.0) + require(status.state == STATUS_COMPLETED, + "subsequent two-segment move did not complete") + require(status.position == 400, + "subsequent two-segment position expected 400") + + +def require_fixture_level(fixture, input_selection, expected, label): + actual = fixture.read(input_selection) + require( + actual == expected, + "%s expected X%d=%d, got %d" + % (label, input_selection + 4, expected, actual), + ) + + +def test_x45_electrical_scan(fixture): + fixture.all_off() + require_fixture_level(fixture, 0, 0, "both outputs off") + require_fixture_level(fixture, 1, 0, "both outputs off") + for input_selection in range(2): + other = 1 - input_selection + fixture.drive(input_selection, True) + require_fixture_level(fixture, input_selection, 1, "output on") + require_fixture_level(fixture, other, 0, "cross-channel isolation") + fixture.drive(input_selection, False) + require_fixture_level(fixture, input_selection, 0, "output off") + + +def test_wait_signal_input(client, fixture, input_selection): + fixture.drive(input_selection, False) + clear_position(client) + common = make_common( + default_hz=1000, + start_hz=1000, + stop_hz=1000, + ) + common[0x02] = input_selection + configure(client, common, make_segment(1000, 50, wait_type=WAIT_SIGNAL)) + command(client, COMMAND_START) + waiting = wait_for_status( + client, + lambda item: item.state == STATUS_WAITING, + 2.0, + "X%d WAIT_SIGNAL" % (input_selection + 4), + ) + require(waiting.position == 50, "WAIT_SIGNAL pulse count expected 50") + time.sleep(0.050) + require(read_status(client).state == STATUS_WAITING, + "WAIT_SIGNAL advanced while input was off") + fixture.drive(input_selection, True) + status = wait_terminal(client, 2.0) + require(status.state == STATUS_COMPLETED, "WAIT_SIGNAL did not complete") + require(status.position == 50, "WAIT_SIGNAL changed completed position") + fixture.drive(input_selection, False) + + +def test_ext_signal_input(client, fixture, input_selection, wait_type): + fixture.drive(input_selection, False) + clear_position(client) + common = make_common( + default_hz=1000, + start_hz=1000, + stop_hz=1000, + ) + common[0x03] = input_selection + configure(client, common, make_segment(1000, 5000, wait_type=wait_type)) + command(client, COMMAND_START) + wait_for_status( + client, + lambda item: item.state in ACTIVE_STATES and item.position >= 10, + 2.0, + "X%d external-signal active move" % (input_selection + 4), + ) + fixture.drive(input_selection, True) + status = wait_terminal(client, 2.0) + require(status.state == STATUS_COMPLETED, + "external-signal move did not complete") + require(0 < status.position < 5000, + "external signal did not cut the active segment") + fixture.drive(input_selection, False) + + +def test_ext_or_complete_natural(client, fixture, input_selection): + fixture.drive(input_selection, False) + clear_position(client) + common = make_common( + default_hz=1000, + start_hz=1000, + stop_hz=1000, + ) + common[0x03] = input_selection + configure( + client, + common, + make_segment(1000, 50, wait_type=EXT_OR_COMPLETE), + ) + command(client, COMMAND_START) + status = wait_terminal(client, 2.0) + require(status.state == STATUS_COMPLETED, + "EXT_OR_COMPLETE natural branch did not complete") + require(status.position == 50, + "EXT_OR_COMPLETE natural branch expected 50 pulses") + + +def run_x45(client, fixture, keep_config): + snapshot = ( + client.read_holding(CONFIG_BASE, COMMON_WORDS), + client.read_holding(SEGMENT_1_BASE, SEGMENT_WORDS), + client.read_holding(SEGMENT_2_BASE, SEGMENT_WORDS), + ) + try: + fixture.prepare() + run_case("x45_electrical_scan", + lambda: test_x45_electrical_scan(fixture)) + for input_selection in range(2): + name = "X%d" % (input_selection + 4) + run_case( + "%s_wait_signal" % name, + lambda selection=input_selection: + test_wait_signal_input(client, fixture, selection), + ) + run_case( + "%s_ext_signal" % name, + lambda selection=input_selection: + test_ext_signal_input( + client, fixture, selection, EXT_SIGNAL), + ) + run_case( + "%s_ext_or_complete_natural" % name, + lambda selection=input_selection: + test_ext_or_complete_natural(client, fixture, selection), + ) + run_case( + "%s_ext_or_complete_trigger" % name, + lambda selection=input_selection: + test_ext_signal_input( + client, fixture, selection, EXT_OR_COMPLETE), + ) + finally: + try: + fixture.all_off() + finally: + try: + safe_stop(client) + if not keep_config: + restore_configuration(client, snapshot) + finally: + fixture.release() + + +def safe_stop(client): + try: + status = read_status(client) + if status.state in ACTIVE_STATES: + command(client, COMMAND_STOP) + wait_terminal(client, 3.0) + except (ModbusException, TestFailure, serial.SerialException): + pass + + +def restore_configuration(client, snapshot): + safe_stop(client) + command(client, COMMAND_CLEAR) + client.write_multiple(CONFIG_BASE, snapshot[0]) + client.write_multiple(SEGMENT_1_BASE, snapshot[1]) + client.write_multiple(SEGMENT_2_BASE, snapshot[2]) + + +def run_smoke(client): + run_case("fixed_map_and_exceptions", lambda: test_fixed_map_and_exceptions(client)) + + +def run_all(client, keep_config): + snapshot = ( + client.read_holding(CONFIG_BASE, COMMON_WORDS), + client.read_holding(SEGMENT_1_BASE, SEGMENT_WORDS), + client.read_holding(SEGMENT_2_BASE, SEGMENT_WORDS), + ) + try: + run_smoke(client) + run_case( + "positive_negative_absolute_zero", + lambda: test_positive_negative_and_absolute_zero(client), + ) + run_case("all_outputs_100khz", + lambda: test_all_outputs_at_100khz(client)) + run_case("short_final_profiles", + lambda: test_short_final_profiles(client)) + run_case("wait_time", lambda: test_wait_time(client)) + run_case("act_time", lambda: test_act_time(client)) + run_case( + "dynamic_frequency_repeated_stop", + lambda: test_dynamic_frequency_and_repeated_stop(client), + ) + run_case( + "future_segment_frequency_on_arrival", + lambda: test_future_segment_frequency_applies_on_arrival(client), + ) + run_case( + "subsequent_future_frequency_handoff", + lambda: test_subsequent_future_frequency_rebuilds_handoff(client), + ) + finally: + if keep_config: + safe_stop(client) + else: + restore_configuration(client, snapshot) + + +def persistence_prepare(client): + clear_position(client) + configure( + client, + make_common(start_hz=500), + make_segment(500, 7), + ) + command(client, COMMAND_START) + status = wait_terminal(client, 3.0) + require(status.state == STATUS_COMPLETED, "persistence move did not complete") + require(status.position == 7, "persistence position expected 7") + time.sleep(1.2) + require(read_status(client).position == 7, "position changed before reset") + print("PREPARED: release COM, reset/power-cycle the MCU, then run verify phase") + + +def persistence_verify(client, cleanup): + status = read_status(client) + require(status.state == STATUS_IDLE, "post-reset state must be IDLE") + require(status.position == 7, "post-reset position expected 7") + segment = client.read_holding(SEGMENT_1_BASE, SEGMENT_WORDS) + require(join_u32(segment[0], segment[1]) == 500, "frequency was not restored") + require(join_i32(segment[2], segment[3]) == 7, "pulse target was not restored") + if cleanup: + command(client, COMMAND_CLEAR) + require(read_status(client).position == 0, "cleanup CLEAR failed") + restore_default_configuration(client) + time.sleep(1.2) + common = client.read_holding(CONFIG_BASE, COMMON_WORDS) + segment = client.read_holding(SEGMENT_1_BASE, SEGMENT_WORDS) + require(join_u32(common[0x0B], common[0x0C]) == 1000, + "cleanup default speed was not restored") + require(join_u32(segment[0], segment[1]) == 1000, + "cleanup segment frequency was not restored") + require(join_i32(segment[2], segment[3]) == 1000, + "cleanup segment pulses were not restored") + + +def defaults_verify(client): + status = read_status(client) + require(status.state == STATUS_IDLE, "default state must be IDLE") + require(status.position == 0, "default position must be zero") + common = client.read_holding(CONFIG_BASE, COMMON_WORDS) + require(common[0x05] == 10, "default direction delay expected 10 ms") + require(join_u32(common[0x0B], common[0x0C]) == 1000, + "default speed expected 1000 Hz") + require(join_u32(common[0x0D], common[0x0E]) == 100, + "start speed expected 100 Hz") + require(join_u32(common[0x10], common[0x11]) == 100, + "stop speed expected 100 Hz") + for index in range(10): + base = SEGMENT_1_BASE + index * 0x10 + segment = client.read_holding(base, SEGMENT_WORDS) + expected_pulses = 1000 if index == 0 else 0 + require(join_u32(segment[0], segment[1]) == 1000, + "default segment %d frequency mismatch" % (index + 1)) + require(join_i32(segment[2], segment[3]) == expected_pulses, + "default segment %d pulse count mismatch" % (index + 1)) + + +def self_test(): + payload = bytes.fromhex("01 01 00 00 00 01") + require(crc16(payload) == 0xCAFD, "CRC reference vector failed") + require(append_crc(payload) == bytes.fromhex("01 01 00 00 00 01 FD CA"), + "wire CRC order failed") + for value in (0, 1, 0x7FFFFFFF, -1, -123456, -0x80000000): + words = split_i32(value) + require(join_i32(words[0], words[1]) == value, "int32 word round trip failed") + require(frequency_matches(1199, 1200), "frequency tolerance rejected 1 Hz") + require(not frequency_matches(1198, 1200), + "frequency tolerance accepted excessive error") + sample = "\n0x42408214 : 00000001 \n" + require(parse_stlink_word(sample, X4_INPUT_BIT) == 1, + "ST-LINK read parser failed") + print("Self-test passed; no serial port was opened") + + +def print_preflight(args): + print("No serial port opened. Board-test preconditions:") + print(" - Firmware containing the PLSR 0x1000/0x2000/0x3000 map is flashed.") + print(" - RS-485 is %d baud, 8E1, slave %d on %s." % + (args.baud, args.slave, args.port)) + print(" - Y0 pulse and Y12 direction outputs are safe to toggle.") + print(" - No other program has the COM port open.") + print("Run smoke only:") + print(" py -B HostComputer\\plsr_modbus_product_test.py --run --phase smoke") + print("Run motion matrix:") + print(" py -B HostComputer\\plsr_modbus_product_test.py --run --allow-motion") + print("Run X4/X5 loopback tests:") + print(" py -B HostComputer\\plsr_modbus_product_test.py --run --phase x45 --allow-motion") + print("Run offline checks:") + print(" py -B HostComputer\\plsr_modbus_product_test.py --self-test") + + +def parse_arguments(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--port", default="COM5") + parser.add_argument("--baud", type=int, default=9600) + parser.add_argument("--slave", type=int, default=1) + parser.add_argument("--timeout", type=float, default=1.5) + parser.add_argument("--stlink-cli", default=DEFAULT_STLINK_CLI) + parser.add_argument("--stlink-id", type=int, default=0) + parser.add_argument("--stlink-timeout", type=float, default=20.0) + parser.add_argument( + "--phase", + choices=("smoke", "all", "x45", "persistence-prepare", + "persistence-verify", "defaults-verify"), + default="all", + ) + parser.add_argument("--run", action="store_true", + help="open the serial port and execute the selected phase") + parser.add_argument("--allow-motion", action="store_true", + help="confirm that pulse and direction outputs may toggle") + parser.add_argument("--keep-config", action="store_true", + help="do not restore the three modified config blocks") + parser.add_argument("--cleanup", action="store_true", + help="CLEAR position after persistence verification") + parser.add_argument("--self-test", action="store_true", + help="run offline CRC/word tests without opening a port") + return parser.parse_args() + + +def main(): + args = parse_arguments() + require(1 <= args.slave <= 247, "slave must be 1..247") + require(args.baud > 0, "baud must be positive") + require(args.timeout > 0, "timeout must be positive") + require(args.stlink_id >= 0, "ST-LINK ID must not be negative") + require(args.stlink_timeout > 0, "ST-LINK timeout must be positive") + + if args.self_test: + self_test() + return 0 + if not args.run: + print_preflight(args) + return 0 + + motion_phase = args.phase in ("all", "x45", "persistence-prepare") + if motion_phase and not args.allow_motion: + raise TestFailure("motion phase requires --allow-motion") + + print("Opening %s at %d 8E1, slave %d" % + (args.port, args.baud, args.slave)) + with RtuClient(args.port, args.baud, args.slave, args.timeout) as client: + if args.phase == "smoke": + run_smoke(client) + elif args.phase == "all": + run_all(client, args.keep_config) + elif args.phase == "x45": + fixture = X45Fixture( + args.stlink_cli, + args.stlink_id, + args.stlink_timeout, + ) + run_x45(client, fixture, args.keep_config) + elif args.phase == "persistence-prepare": + persistence_prepare(client) + elif args.phase == "persistence-verify": + persistence_verify(client, args.cleanup) + else: + defaults_verify(client) + print("PASS phase=%s" % args.phase) + return 0 + + +if __name__ == "__main__": + try: + sys.exit(main()) + except (TestFailure, ModbusException, serial.SerialException) as error: + print("FAIL: %s" % error, file=sys.stderr) + sys.exit(1) diff --git a/Middlewares/Third_Party/Micrium/Config/app_cfg.h b/Middlewares/Third_Party/Micrium/Config/app_cfg.h index bd83e5b..7059bcf 100644 --- a/Middlewares/Third_Party/Micrium/Config/app_cfg.h +++ b/Middlewares/Third_Party/Micrium/Config/app_cfg.h @@ -15,4 +15,7 @@ #define APP_TASK_START_STK_SIZE 256u #define APP_TASK_TEST_STK_SIZE 256u +/* The short-profile pulse IRQ needs more than the port's 128-word default. */ +#define OS_CPU_EXCEPT_STK_SIZE 256u + #endif diff --git a/Modbus/Inc/modbus_rtu_slave.h b/Modbus/Inc/modbus_rtu_slave.h index 3e139df..7c38bad 100644 --- a/Modbus/Inc/modbus_rtu_slave.h +++ b/Modbus/Inc/modbus_rtu_slave.h @@ -3,145 +3,19 @@ #include "stm32f4xx_hal.h" #include + #ifdef __cplusplus -extern "C" -{ +extern "C" { #endif -/** - * @brief Modbus 数据模型的最大地址 - * - * 任务要求规定线圈和保持寄存器地址范围均为 0～0x270F， - * 因此每种数据共有 10000 项 - */ -#define MODBUS_MAP_LAST_ADDRESS (0x270FU) // 保持寄存器或线圈地址 -#define MODBUS_MAP_ITEM_COUNT (0x2710U) // 保持寄存器或线圈数量 - #define MODBUS_SLAVE_DEFAULT_ADDRESS (1U) -#define MODBUS_CONNECTION_TIMEOUT_MS (1000U) - -/** - * @brief 与 TouchWin 触摸屏联调使用的演示地址 - * - * 可以直接访问以下地址 - */ -#define HMI_REG_DEVICE_ID (0U) -#define HMI_REG_UPTIME_SECONDS (1U) -#define HMI_REG_RX_FRAME_COUNT (2U) -#define D3 (3U) -#define D4 (4U) -#define D5 (5U) -#define D6 (6U) -#define D7 (7U) -#define HMI_REG_WRITE_TEST (10U) -#define HMI_COIL_WRITE_TEST (0U) - -#define MODBUS_RETAINED_D_START (100U) -#define MODBUS_RETAINED_D_END (120U) -#define MODBUS_RETAINED_D_COUNT (21U) - -#define MODBUS_BACKUP_MAGIC (0x44313030UL) -/** - * @brief 掉电保持寄存器在Backup SRAM中的存储格式 - */ -typedef struct -{ - uint32_t magic; ///< 掉电保持数据有效标志 - uint16_t retainedD[MODBUS_RETAINED_D_COUNT]; ///< D100～D120保持值 -} MODBUS_BACKUP_DATA; -/** - * @brief Modbus 从站通信统计信息 - */ -typedef struct -{ - uint32_t rxEventCount; ///< 串口接收事件总数 - uint32_t validFrameCount; ///< CRC 和从站地址均有效的帧数 - uint32_t txFrameCount; ///< 成功启动 DMA 发送的帧数 - uint32_t crcErrorCount; ///< CRC 校验错误帧数 - uint32_t ignoredAddressCount; ///< 因从站地址不匹配而忽略的帧数 - uint32_t illegalFunctionCount; ///< 非法功能码计数 - uint32_t illegalAddressCount; ///< 非法数据地址计数 - uint32_t illegalValueCount; ///< 非法数据值计数 - uint32_t droppedFrameCount; ///< 接收槽占用或长度错误导致的丢帧数 - uint32_t uartErrorCount; ///< HAL 串口错误计数 -} MODBUS_SLAVE_STATS; -/** @brief Modbus 从站通信统计数据 */ -extern volatile MODBUS_SLAVE_STATS ModbusSlaveStatistics; -/** - * @brief 初始化 Modbus 从站并启动 USART DMA 空闲接收 - * @param[in] huart 已由 HAL 初始化完成的串口句柄 - * @param[in] slaveAddress 从站地址，有效范围为 1～247 - * @retval HAL_OK 初始化成功且 DMA 接收已启动 - * @retval HAL_ERROR 输入参数无效 - * @retval HAL_BUSY 串口或 DMA 当前忙 - */ HAL_StatusTypeDef ModbusSlaveInit(UART_HandleTypeDef *huart, uint8_t slaveAddress); -/** - * @brief 校验并处理一帧待处理请求 - * - * 本函数应在 uC/OS-II 任务中周期调用中断只负责接收数据， - * CRC 校验、协议解析和响应发送均在任务上下文中执行 - */ void ModbusSlavePoll(void); -/** - * @brief 处理 HAL 串口 DMA 接收事件 - * @param[in] huart 产生接收事件的串口句柄 - * @param[in] size 本次接收到的字节数 - */ void ModbusSlaveOnRxEvent(UART_HandleTypeDef *huart, uint16_t size); -/** - * @brief 处理 HAL 串口 DMA 发送完成事件 - * @param[in] huart 完成发送的串口句柄 - */ void ModbusSlaveOnTxComplete(UART_HandleTypeDef *huart); -/** - * @brief 处理 HAL 串口错误事件并恢复接收 - * @param[in] huart 发生错误的串口句柄 - */ void ModbusSlaveOnUartError(UART_HandleTypeDef *huart); -/** - * @brief 写入一个保持寄存器 - * @param[in] address 保持寄存器地址 - * @param[in] value 待写入的 16 位数据 - * @retval 1 写入成功 - * @retval 0 保持寄存器地址无效 - */ -uint8_t ModbusSlaveSetHoldingRegister(uint16_t address, uint16_t value); -/** - * @brief 读取一个保持寄存器 - * @param[in] address 保持寄存器地址 - * @param[out] value 用于保存读取结果的指针 - * @retval 1 读取成功 - * @retval 0 保持寄存器地址无效或输出指针为空 - */ -uint8_t ModbusSlaveGetHoldingRegister(uint16_t address, uint16_t *value); -/** - * @brief 设置一个线圈的状态 - * @param[in] address 线圈地址 - * @param[in] state 0 表示复位，非 0 表示置位 - * @retval 1 设置成功 - * @retval 0 线圈地址无效 - */ -uint8_t ModbusSlaveSetCoil(uint16_t address, uint8_t state); -/** - * @brief 读取一个线圈的状态 - * @param[in] address 线圈地址 - * @param[out] state 用于保存线圈状态的指针，结果为 0 或 1 - * @retval 1 读取成功 - * @retval 0 线圈地址无效或输出指针为空 - */ -uint8_t ModbusSlaveGetCoil(uint16_t address, uint8_t *state); -/** - * @brief 判断指定时间内是否收到过有效请求 - * @param[in] timeoutMs 连接超时时间，单位为毫秒 - * @retval 1 从站在指定时间内收到过有效请求 - * @retval 0 尚未收到有效请求或连接已经超时 - */ -uint8_t ModbusSlaveIsConnected(uint32_t timeoutMs); -void ModbusRetainedRegistersLoad(void); -void ModbusRetainedRegistersPoll(void); #ifdef __cplusplus } diff --git a/Modbus/Src/modbus_rtu_slave.c b/Modbus/Src/modbus_rtu_slave.c index bf00866..e08adb0 100644 --- a/Modbus/Src/modbus_rtu_slave.c +++ b/Modbus/Src/modbus_rtu_slave.c @@ -1,77 +1,66 @@ #include "modbus_rtu_slave.h" +#include "plsr.h" #include -#define MODBUS_RTU_ADU_SIZE_MAX (256U) // Modbus RTU 最大 ADU 长度，单位为字节 +#define MODBUS_RTU_ADU_SIZE_MAX (256U) +#define MODBUS_RTU_BITS_PER_CHAR (11UL) +#define MODBUS_RTU_HIGH_BAUD_LIMIT (19200UL) +#define MODBUS_RTU_T15_US (750UL) +#define MODBUS_RTU_T35_US (1750UL) -#define MODBUS_RTU_T15_US (750UL) // 高波特率下固定T1.5时间，单位us -#define MODBUS_RTU_T35_US (1750UL) // 高波特率下固定T3.5时间，单位us -#define MODBUS_RTU_BITS_PER_CHAR (11UL) // 8E1包含11个传输位 -#define MODBUS_RTU_HIGH_BAUD_LIMIT (19200UL) // 高低波特率计算方式的分界值 +#define MODBUS_BROADCAST_ADDRESS (0U) +#define MODBUS_SLAVE_ADDRESS_MAX (247U) -#define MODBUS_BROADCAST_ADDRESS (0U) // 广播地址 +#define MODBUS_FC_READ_HOLDING (0x03U) +#define MODBUS_FC_WRITE_SINGLE (0x06U) +#define MODBUS_FC_WRITE_MULTIPLE (0x10U) -#define MODBUS_EX_ILLEGAL_FUNCTION (0x01U) // 非法功能码的异常码 -#define MODBUS_EX_ILLEGAL_ADDRESS (0x02U) // 非法地址的异常码 -#define MODBUS_EX_ILLEGAL_VALUE (0x03U) // 非法数据的异常码 +#define MODBUS_EX_ILLEGAL_FUNCTION (0x01U) +#define MODBUS_EX_ILLEGAL_ADDRESS (0x02U) +#define MODBUS_EX_ILLEGAL_VALUE (0x03U) +#define MODBUS_EX_SERVER_FAILURE (0x04U) +#define MODBUS_EX_SERVER_BUSY (0x06U) -#define MODBUS_READ_COILS_MAX (2000U) // 一次ADU最大线圈读取数量 -#define MODBUS_READ_REGS_MAX (125U) // 一次ADU最大寄存器读取数量 -#define MODBUS_WRITE_COILS_MAX (1968U) // 一次ADU最大线圈写入数量 -#define MODBUS_WRITE_REGS_MAX (123U) // 一次ADU最大寄存器写入数量 - -#define MODBUS_COIL_VALUE_ON (0xFF00U) -#define MODBUS_COIL_VALUE_OFF (0x0000U) +#define MODBUS_READ_REGISTERS_MAX (125U) +#define MODBUS_WRITE_REGISTERS_MAX (123U) static UART_HandleTypeDef *ModbusUart; static uint8_t ModbusSlaveAddress; -static HAL_StatusTypeDef ModbusStartReceive(void); -/*最近一次字节尾到字节头的间隔周期数*/ -static volatile uint32_t ModbusLastInterFrameGapCycles; -/** - * DMA 缓冲区只由 DMA 写入；帧缓冲区在接收回调中完成一次快照， - * 随后由任务解析，避免 DMA 重启后覆盖尚未处理的数据 - */ + static uint8_t ModbusRxDmaBuffer[MODBUS_RTU_ADU_SIZE_MAX]; -/* 保存被UART IDLE事件分开的DMA片段，达到T3.5后再提交解析 */ static uint8_t ModbusRxAssemblyBuffer[MODBUS_RTU_ADU_SIZE_MAX]; static uint8_t ModbusRxFrame[MODBUS_RTU_ADU_SIZE_MAX]; static uint8_t ModbusTxFrame[MODBUS_RTU_ADU_SIZE_MAX]; -/* D100～D120 上一次已保存的值，用于检测数据是否变化 */ -static uint16_t ModbusRetainedSnapshot[MODBUS_RETAINED_D_COUNT]; +static uint16_t ModbusRegisterScratch[MODBUS_READ_REGISTERS_MAX]; + +static volatile uint16_t ModbusRxAssemblyLength; +static volatile uint8_t ModbusRxAssemblyInvalid; +static volatile uint32_t ModbusRxLastByteCycle; static volatile uint16_t ModbusRxFrameLength; static volatile uint8_t ModbusRxFrameReady; static volatile uint8_t ModbusTxBusy; -static volatile uint16_t ModbusRxAssemblyLength; // 当前拼帧长度 -static volatile uint8_t ModbusRxAssemblyInvalid; // 帧内间隔超过T1.5时置1 -static volatile uint32_t ModbusRxLastByteCycle; // 上一片段末字节结束时刻 -static uint32_t ModbusRtuT15Cycles; // T1.5对应的CPU周期数 -static uint32_t ModbusRtuT35Cycles; // T3.5对应的CPU周期数 -static uint32_t ModbusRtuCharCycles; // 一个UART字符对应的CPU周期数 -static volatile uint32_t ModbusLastValidFrameTick; -static volatile uint8_t ModbusHasReceivedValidFrame; -static volatile MODBUS_BACKUP_DATA *ModbusBackupData = - (volatile MODBUS_BACKUP_DATA *)BKPSRAM_BASE; - -/** - * 10000 个保持寄存器占用 20000 字节；10000 个线圈按位存储， - * 占用 1250 字节 - */ -static uint16_t ModbusHoldingRegisters[40000]; - -#pragma location = ".ccmram" -#pragma data_alignment = 4 -__root static uint16_t ModbusRegistersCcm[29999]; - -static uint8_t ModbusCoils[(MODBUS_MAP_ITEM_COUNT + 7U) / 8U]; - -volatile MODBUS_SLAVE_STATS ModbusSlaveStatistics; - -/** - * @brief 计算 Modbus RTU CRC16 校验值 - * @param[in] data 待校验数据 - * @param[in] length 待校验数据长度 - * @return CRC16 校验值 - */ + +static uint32_t ModbusRtuCharCycles; +static uint32_t ModbusRtuT15Cycles; +static uint32_t ModbusRtuT35Cycles; + +static HAL_StatusTypeDef ModbusStartReceive(void); + +static uint32_t ModbusEnterCritical(void) +{ + uint32_t primask = __get_PRIMASK(); + + __disable_irq(); + __DMB(); + return primask; +} + +static void ModbusExitCritical(uint32_t primask) +{ + __DMB(); + __set_PRIMASK(primask); +} + static uint16_t ModbusCrc16(const uint8_t *data, uint16_t length) { uint16_t crc = 0xFFFFU; @@ -81,10 +70,9 @@ static uint16_t ModbusCrc16(const uint8_t *data, uint16_t length) for (index = 0U; index < length; index++) { crc ^= data[index]; - for (bit = 0U; bit < 8U; bit++) { - if ((crc & 0x0001U) != 0U) + if ((crc & 1U) != 0U) { crc = (uint16_t)((crc >> 1U) ^ 0xA001U); } @@ -94,107 +82,39 @@ static uint16_t ModbusCrc16(const uint8_t *data, uint16_t length) } } } - return crc; } -/** - * @brief 提取一个 16 位无符号整数 - * @param[in] data 两个字节的数据地址 - * @return 转换后的 16 位无符号整数 - */ static uint16_t ModbusGetU16Be(const uint8_t *data) { return (uint16_t)(((uint16_t)data[0] << 8U) | data[1]); } -/** - * @brief 提取一个 24 位无符号整数 - * @param[in] data 3个字节的数据地址 - * @return 转换后的 24 位无符号整数 - */ -static uint32_t ModbusGetU24Be(const uint8_t *data) -{ - return (uint32_t)(((uint32_t)data[0] << 16U) | ((uint32_t)data[1] << 8U) - | (uint32_t)data[2]); -} - -/** - * @brief 检查连续数据地址范围是否合法 - * @param[in] start 起始地址 - * @param[in] quantity 数据项数量 - */ -static uint8_t ModbusAddressRangeIsValid(uint16_t start, uint16_t quantity) -{ - if (start >= MODBUS_MAP_ITEM_COUNT) - { - return 0U; - } - - // 先做减法再比较，避免溢出 - return (quantity <= (MODBUS_MAP_ITEM_COUNT - start)) ? 1U : 0U; -} - -/** - * @brief 读取已经确认地址合法的线圈 - * @param[in] address 线圈地址 - * @return 线圈状态，取值为 0 或 1 - */ -static uint8_t ModbusCoilGetUnchecked(uint16_t address) -{ - uint8_t mask = (uint8_t)(1U << (address & 0x0007U)); - - return ((ModbusCoils[address >> 3U] & mask) != 0U) ? 1U : 0U; -} - -/** - * @brief 设置已经确认地址合法的线圈 - * @param[in] address 线圈地址 - * @param[in] state 0 表示复位，非 0 表示置位 - */ -static void ModbusCoilSetUnchecked(uint16_t address, uint8_t state) -{ - uint8_t mask = (uint8_t)(1U << (address & 0x0007U)); - - if (state != 0U) - { - ModbusCoils[address >> 3U] |= mask; - } - else - { - ModbusCoils[address >> 3U] &= (uint8_t)(~mask); - } -} - -/** - * @brief 初始化用于RTU帧间隔测量的DWT周期计数器 - * @param[in] baudRate 当前串口波特率 - */ static void ModbusRtuTimingInit(uint32_t baudRate) { - uint64_t coreClock; - /* 开启DWT周期计数器，CYCCNT每经过一个CPU时钟周期自动加1 */ + uint64_t coreClock = SystemCoreClock; + uint64_t charCycleNumerator = coreClock * MODBUS_RTU_BITS_PER_CHAR; + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; DWT->CYCCNT = 0U; DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; - coreClock = SystemCoreClock; - /* 一个字符周期数=CPU频率*每字符位数/波特率 */ ModbusRtuCharCycles = - (uint32_t)((coreClock * MODBUS_RTU_BITS_PER_CHAR) / baudRate); + (uint32_t)((charCycleNumerator + baudRate - 1UL) / baudRate); + if (ModbusRtuCharCycles == 0UL) + { + ModbusRtuCharCycles = 1UL; + } if (baudRate > MODBUS_RTU_HIGH_BAUD_LIMIT) { - /* 高波特率使用固定750us和1750us，999999用于向上取整 */ - ModbusRtuT15Cycles = - (uint32_t)((coreClock * MODBUS_RTU_T15_US + 999999UL) / 1000000UL); - - ModbusRtuT35Cycles = - (uint32_t)((coreClock * MODBUS_RTU_T35_US + 999999UL) / 1000000UL); + ModbusRtuT15Cycles = (uint32_t)( + (coreClock * MODBUS_RTU_T15_US + 999999UL) / 1000000UL); + ModbusRtuT35Cycles = (uint32_t)( + (coreClock * MODBUS_RTU_T35_US + 999999UL) / 1000000UL); } else { - /* 低波特率按照1.5个字符时间和3.5个字符时间计算 */ ModbusRtuT15Cycles = (uint32_t)(((uint64_t)ModbusRtuCharCycles * 3UL + 1UL) / 2UL); ModbusRtuT35Cycles = @@ -202,50 +122,57 @@ static void ModbusRtuTimingInit(uint32_t baudRate) } } -/** - * @brief 结束当前RTU接收组帧 - * @note 调用本函数时必须保证不会与串口接收中断并发执行 - */ static void ModbusRxAssemblyFinalize(void) { - if (ModbusRxAssemblyLength == 0U) + uint16_t length = ModbusRxAssemblyLength; + + if (length == 0U) { return; } - /*拼帧区是否有数据*/ + if ((ModbusRxAssemblyInvalid == 0U) && (ModbusRxFrameReady == 0U)) { - /* 当前帧未违反T1.5且解析缓冲区空闲时提交完整帧 */ - (void)memcpy(ModbusRxFrame, ModbusRxAssemblyBuffer, - ModbusRxAssemblyLength); - ModbusRxFrameLength = ModbusRxAssemblyLength; + (void)memcpy(ModbusRxFrame, ModbusRxAssemblyBuffer, length); + ModbusRxFrameLength = length; ModbusRxFrameReady = 1U; } - else - { - /* T1.5无效帧或上一帧尚未处理完成时丢弃 */ - ModbusSlaveStatistics.droppedFrameCount++; - } ModbusRxAssemblyLength = 0U; ModbusRxAssemblyInvalid = 0U; } -/** - * @brief 静默时间达到T3.5后，将接收数据交给协议解析任务 - */ +static HAL_StatusTypeDef ModbusStartReceive(void) +{ + HAL_StatusTypeDef status; + + if ((ModbusUart == NULL) || (ModbusTxBusy != 0U)) + { + return HAL_BUSY; + } + + status = HAL_UARTEx_ReceiveToIdle_DMA(ModbusUart, + ModbusRxDmaBuffer, + sizeof(ModbusRxDmaBuffer)); + if (status == HAL_OK) + { + __HAL_DMA_DISABLE_IT(ModbusUart->hdmarx, DMA_IT_HT); + } + return status; +} + static void ModbusTryFinalizeReceive(void) { uint32_t now; + uint32_t primask; + uint8_t restartReceive; - if (ModbusRxAssemblyLength == 0U) + if ((ModbusUart == NULL) || (ModbusRxAssemblyLength == 0U)) { return; } - /* DMA缓冲区出现新数据时，说明串口仍在接收当前片段 */ - if ((ModbusUart->hdmarx != NULL) && /*串口DMA使能*/ - ((ModbusUart->Instance->CR3 & USART_CR3_DMAR) != 0U) + if (((ModbusUart->Instance->CR3 & USART_CR3_DMAR) != 0U) && (__HAL_DMA_GET_COUNTER(ModbusUart->hdmarx) < MODBUS_RTU_ADU_SIZE_MAX)) { @@ -253,427 +180,193 @@ static void ModbusTryFinalizeReceive(void) } now = DWT->CYCCNT; - /* 从末字节结束时刻开始计算静默时间，未达到T3.5时继续等待 */ if ((uint32_t)(now - ModbusRxLastByteCycle) < ModbusRtuT35Cycles) { return; } - /* 静默达到T3.5，当前RTU帧结束，发送响应前停止接收DMA */ - (void)HAL_UART_AbortReceive(ModbusUart); - - __disable_irq(); - ModbusRxAssemblyFinalize(); - __enable_irq(); -/* 无效帧被丢弃后，重新启动DMA接收。 */ - if (ModbusRxFrameReady == 0U) + if (HAL_UART_AbortReceive(ModbusUart) != HAL_OK) { + primask = ModbusEnterCritical(); + ModbusRxAssemblyLength = 0U; + ModbusRxAssemblyInvalid = 0U; + ModbusExitCritical(primask); + (void)HAL_UART_Abort(ModbusUart); (void)ModbusStartReceive(); + return; } + primask = ModbusEnterCritical(); + ModbusRxAssemblyFinalize(); + restartReceive = (ModbusRxFrameReady == 0U) ? 1U : 0U; + ModbusExitCritical(primask); -} - -/** - * @brief 启动 USART DMA 空闲接收 - * @retval HAL_OK DMA 接收启动成功 - * @retval HAL_BUSY 串口未配置或发送尚未结束 - * @return 其他 HAL 状态表示 DMA 接收启动失败 - */ -static HAL_StatusTypeDef ModbusStartReceive(void) -{ - HAL_StatusTypeDef status; - - if (ModbusTxBusy != 0U) - { - return HAL_BUSY; - } - - status = HAL_UARTEx_ReceiveToIdle_DMA(ModbusUart, ModbusRxDmaBuffer, - sizeof(ModbusRxDmaBuffer)); - - if ((status == HAL_OK) && (ModbusUart->hdmarx != NULL)) + if (restartReceive != 0U) { - /* - * 普通 Modbus 帧只应在 IDLE 或缓冲区满时交给应用 - * 关闭 DMA 半传输中断，避免长帧在一半位置被误认为完整帧 - */ - __HAL_DMA_DISABLE_IT(ModbusUart->hdmarx, DMA_IT_HT); + (void)ModbusStartReceive(); } - - return status; } -/** - * @brief 在 RTU 帧末尾追加 CRC 低字节和高字节 - * @param[in,out] frame 待追加 CRC 的帧缓冲区 - * @param[in] payloadLength 不包含 CRC 的有效载荷长度 - */ static void ModbusAppendCrc(uint8_t *frame, uint16_t payloadLength) { uint16_t crc = ModbusCrc16(frame, payloadLength); - /* Modbus RTU 在线路上传输 CRC 低字节在前、高字节在后 */ frame[payloadLength] = (uint8_t)(crc & 0x00FFU); frame[payloadLength + 1U] = (uint8_t)(crc >> 8U); } -/** - * @brief 构造 Modbus 异常响应 - * @param[in] function 请求功能码 - * @param[in] exception 异常码 - * @return 异常响应 ADU 长度 - */ static uint16_t ModbusBuildException(uint8_t function, uint8_t exception) { ModbusTxFrame[0] = ModbusSlaveAddress; ModbusTxFrame[1] = (uint8_t)(function | 0x80U); ModbusTxFrame[2] = exception; ModbusAppendCrc(ModbusTxFrame, 3U); - return 5U; } -/** - * @brief 处理读线圈功能码 0x01 - * @param[in] request RTU 请求帧 - * @param[in] requestLength 请求帧长度 - * @return 待发送响应长度，异常请求返回异常响应长度 - */ -static uint16_t ModbusProcessReadCoils(const uint8_t *request, - uint16_t requestLength) +static uint16_t ModbusBuildPlsrException(uint8_t function, + PLSR_MB_RESULT result, + uint8_t isBroadcast) { + uint8_t exception; - uint16_t start; - uint16_t quantity; - uint16_t index; - uint8_t byteCount; - - if (requestLength != 8U) - { - ModbusSlaveStatistics.illegalValueCount++; - return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - } - - start = ModbusGetU16Be(&request[2]); - quantity = ModbusGetU16Be(&request[4]); - - if ((quantity == 0U) - || (quantity > MODBUS_READ_COILS_MAX)) // 数量0或者数量大于最大值 + if (isBroadcast != 0U) { - ModbusSlaveStatistics.illegalValueCount++; - return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); + return 0U; } - if (ModbusAddressRangeIsValid(start, quantity) == 0U) // 地址检查 + switch (result) { - ModbusSlaveStatistics.illegalAddressCount++; - return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_ADDRESS); + case PLSR_MB_NOT_HANDLED: + case PLSR_MB_ILLEGAL_ADDRESS: + exception = MODBUS_EX_ILLEGAL_ADDRESS; + break; + case PLSR_MB_ILLEGAL_VALUE: + exception = MODBUS_EX_ILLEGAL_VALUE; + break; + case PLSR_MB_DEVICE_BUSY: + exception = MODBUS_EX_SERVER_BUSY; + break; + case PLSR_MB_SERVER_FAILURE: + default: + exception = MODBUS_EX_SERVER_FAILURE; + break; } - - byteCount = (uint8_t)((quantity + 7U) / 8U); - ModbusTxFrame[0] = ModbusSlaveAddress; - ModbusTxFrame[1] = 0X01; - ModbusTxFrame[2] = byteCount; - (void)memset(&ModbusTxFrame[3], 0, byteCount); - - for (index = 0U; index < quantity; index++) - { - if (ModbusCoilGetUnchecked((uint16_t)(start + index)) != 0U) - { - ModbusTxFrame[3U + (index >> 3U)] |= - (uint8_t)(1U << (index & 0x0007U)); - } - } - - ModbusAppendCrc(ModbusTxFrame, (uint16_t)(3U + byteCount)); - return (uint16_t)(5U + byteCount); + return ModbusBuildException(function, exception); } -/** - * @brief 处理读保持寄存器功能码 0x03 - * @param[in] request RTU 请求帧 - * @param[in] requestLength 请求帧长度 - * @return 待发送响应长度，异常请求返回异常响应长度 - */ static uint16_t ModbusProcessReadHolding(const uint8_t *request, uint16_t requestLength) - { uint16_t start; uint16_t quantity; uint16_t index; - uint16_t value; + PLSR_MB_RESULT result; if (requestLength != 8U) { - ModbusSlaveStatistics.illegalValueCount++; return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); } start = ModbusGetU16Be(&request[2]); quantity = ModbusGetU16Be(&request[4]); - - if ((quantity == 0U) - || (quantity > MODBUS_READ_REGS_MAX)) // 数量0或者数量大于最大值 + if ((quantity == 0U) || (quantity > MODBUS_READ_REGISTERS_MAX)) { - ModbusSlaveStatistics.illegalValueCount++; return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); } - if (ModbusAddressRangeIsValid(start, quantity) == 0U) // 地址检查 + result = PlsrModbusReadHolding(start, quantity, ModbusRegisterScratch); + if (result != PLSR_MB_OK) { - if ((start >= 20000U) && (start < 25000U) && (quantity > 0U) - && (quantity <= (25000U - start))) - goto tx; - ModbusSlaveStatistics.illegalAddressCount++; - return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_ADDRESS); + return ModbusBuildPlsrException(request[1], result, 0U); } -tx: + ModbusTxFrame[0] = ModbusSlaveAddress; - ModbusTxFrame[1] = 0X03; + ModbusTxFrame[1] = MODBUS_FC_READ_HOLDING; ModbusTxFrame[2] = (uint8_t)(quantity * 2U); - for (index = 0U; index < quantity; index++) { - uint16_t address = start + index; - /* D20000～D24999映射到D1、D3、D5……D9999 */ - if (address >= 20000U) - { - address = (address - 20000U) * 2U + 1U; - } - value = ModbusHoldingRegisters[address]; - ModbusTxFrame[3U + index * 2U] = (uint8_t)(value >> 8U); - ModbusTxFrame[4U + index * 2U] = (uint8_t)(value & 0x00FFU); + ModbusTxFrame[3U + index * 2U] = + (uint8_t)(ModbusRegisterScratch[index] >> 8U); + ModbusTxFrame[4U + index * 2U] = + (uint8_t)(ModbusRegisterScratch[index] & 0x00FFU); } - ModbusAppendCrc(ModbusTxFrame, (uint16_t)(3U + quantity * 2U)); return (uint16_t)(5U + quantity * 2U); } -/** - * @brief 处理写单个保持寄存器功能码 0x06 - * @param[in] request RTU 请求帧 - * @param[in] requestLength 请求帧长度 - * @param[in] isBroadcast 非 0 表示当前请求为广播 - * @return 单播响应长度；广播或无法响应时返回 0 - */ -static uint16_t ModbusProcessWriteSingleRegister(const uint8_t *request, - uint16_t requestLength, - uint8_t isBroadcast) +static uint16_t ModbusProcessWriteSingle(const uint8_t *request, + uint16_t requestLength, + uint8_t isBroadcast) { uint16_t address; uint16_t value; + PLSR_MB_RESULT result; if (requestLength != 8U) { - ModbusSlaveStatistics.illegalValueCount++; - return (isBroadcast != 0U) ? 0U: ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - - + return (isBroadcast != 0U) + ? 0U + : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); } address = ModbusGetU16Be(&request[2]); value = ModbusGetU16Be(&request[4]); - - if (address >= MODBUS_MAP_ITEM_COUNT) + result = PlsrModbusWriteHolding(address, 1U, &value); + if (result != PLSR_MB_OK) { - ModbusSlaveStatistics.illegalAddressCount++; - return (isBroadcast != 0U) ? 0U : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_ADDRESS); - - + return ModbusBuildPlsrException(request[1], result, isBroadcast); } - ModbusHoldingRegisters[address] = value; - if (isBroadcast != 0U) { return 0U; } - /* 0x06 的正常响应必须原样回显请求的前 6 个字节 */ (void)memcpy(ModbusTxFrame, request, 6U); ModbusAppendCrc(ModbusTxFrame, 6U); return 8U; } -/** - * @brief 处理写单个线圈功能码 0x05 - * @param[in] request RTU 请求帧 - * @param[in] requestLength 请求帧长度 - * @param[in] isBroadcast 非 0 表示当前请求为广播 - * @return 单播响应长度；广播或无法响应时返回 0 - */ -static uint16_t ModbusProcessWriteSingleCoil(const uint8_t *request, - uint16_t requestLength, - uint8_t isBroadcast) -{ - uint16_t address; - uint16_t value; - - if (requestLength != 8U) - { - ModbusSlaveStatistics.illegalValueCount++; - return (isBroadcast != 0U)? 0U : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - - - } - - address = ModbusGetU16Be(&request[2]); - value = ModbusGetU16Be(&request[4]); - - /* - * 0x05 只接受 0xFF00（线圈置位）和 0x0000（线圈复位）； - * 其他数值属于非法数据值 - */ - if ((value != MODBUS_COIL_VALUE_ON) && (value != MODBUS_COIL_VALUE_OFF)) - { - ModbusSlaveStatistics.illegalValueCount++; - return (isBroadcast != 0U)? 0U : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - - - } - - if (address >= MODBUS_MAP_ITEM_COUNT) - { - ModbusSlaveStatistics.illegalAddressCount++; - return (isBroadcast != 0U) ? 0U: ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_ADDRESS); - - - } - - ModbusCoilSetUnchecked(address, (value == MODBUS_COIL_VALUE_ON) ? 1U : 0U); - - if (isBroadcast != 0U) - { - return 0U; - } - - /* 0x05 正常应答原样回显请求的前 6 个字节，再追加 CRC */ - (void)memcpy(ModbusTxFrame, request, 6U); - ModbusAppendCrc(ModbusTxFrame, 6U); - return 8U; -} - -/** - * @brief 处理写多个线圈功能码 0x0F - * @param[in] request RTU 请求帧 - * @param[in] requestLength 请求帧长度 - * @param[in] isBroadcast 非 0 表示当前请求为广播 - * @return 单播响应长度；广播或无法响应时返回 0 - */ -static uint16_t ModbusProcessWriteMultipleCoils(const uint8_t *request, - uint16_t requestLength, - uint8_t isBroadcast) +static uint16_t ModbusProcessWriteMultiple(const uint8_t *request, + uint16_t requestLength, + uint8_t isBroadcast) { uint16_t start; uint16_t quantity; + uint16_t byteCount; uint16_t index; - uint8_t byteCount; - uint8_t expectedByteCount; + PLSR_MB_RESULT result; if (requestLength < 9U) { - ModbusSlaveStatistics.illegalValueCount++; - return (isBroadcast != 0U) ? 0U: ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - - + return (isBroadcast != 0U) + ? 0U + : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); } start = ModbusGetU16Be(&request[2]); quantity = ModbusGetU16Be(&request[4]); byteCount = request[6]; - expectedByteCount = (uint8_t)((quantity + 7U) / 8U); - - if ((quantity == 0U) || (quantity > MODBUS_WRITE_COILS_MAX) - || (byteCount != expectedByteCount) + if ((quantity == 0U) + || (quantity > MODBUS_WRITE_REGISTERS_MAX) + || (byteCount != (uint16_t)(quantity * 2U)) || (requestLength != (uint16_t)(9U + byteCount))) { - ModbusSlaveStatistics.illegalValueCount++; - return (isBroadcast != 0U)? 0U : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - - - } - - if (ModbusAddressRangeIsValid(start, quantity) == 0U) - { - ModbusSlaveStatistics.illegalAddressCount++; - return (isBroadcast != 0U) ? 0U: ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_ADDRESS); - - + return (isBroadcast != 0U) + ? 0U + : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); } for (index = 0U; index < quantity; index++) { - uint8_t state; - - state = (uint8_t)((request[7U + (index >> 3U)] >> (index & 0x0007U)) & 0x01U); - - ModbusCoilSetUnchecked((uint16_t)(start + index), state); - } - - if (isBroadcast != 0U) - { - return 0U; - } - - ModbusTxFrame[0] = ModbusSlaveAddress; - ModbusTxFrame[1] = 0X0F; - (void)memcpy(&ModbusTxFrame[2], &request[2], 4U); - ModbusAppendCrc(ModbusTxFrame, 6U); - return 8U; -} - -/** - * @brief 处理写多个保持寄存器功能码 0x10 - * @param[in] request RTU 请求帧 - * @param[in] requestLength 请求帧长度 - * @param[in] isBroadcast 非 0 表示当前请求为广播 - * @return 单播响应长度；广播或无法响应时返回 0 - */ -static uint16_t ModbusProcessWriteMultipleRegisters(const uint8_t *request, - uint16_t requestLength, - uint8_t isBroadcast) -{ - uint16_t start; - uint16_t quantity; - uint16_t index; - uint8_t byteCount; - - if (requestLength < 9U) - { - ModbusSlaveStatistics.illegalValueCount++; - return (isBroadcast != 0U) ? 0U : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - - - } - - start = ModbusGetU16Be(&request[2]); - quantity = ModbusGetU16Be(&request[4]); - byteCount = request[6]; - - if ((quantity == 0U) || (quantity > MODBUS_WRITE_REGS_MAX) - || (byteCount != (uint8_t)(quantity * 2U)) - || (requestLength != (uint16_t)(9U + byteCount))) - { - ModbusSlaveStatistics.illegalValueCount++; - return (isBroadcast != 0U) ? 0U : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - - - } - - if (ModbusAddressRangeIsValid(start, quantity) == 0U) - { - ModbusSlaveStatistics.illegalAddressCount++; - return (isBroadcast != 0U) ? 0U : ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_ADDRESS); - - + ModbusRegisterScratch[index] = + ModbusGetU16Be(&request[7U + index * 2U]); } - - for (index = 0U; index < quantity; index++) + result = PlsrModbusWriteHolding(start, quantity, ModbusRegisterScratch); + if (result != PLSR_MB_OK) { - ModbusHoldingRegisters[start + index] = - ModbusGetU16Be(&request[7U + index * 2U]); + return ModbusBuildPlsrException(request[1], result, isBroadcast); } if (isBroadcast != 0U) @@ -682,101 +375,15 @@ static uint16_t ModbusProcessWriteMultipleRegisters(const uint8_t *request, } ModbusTxFrame[0] = ModbusSlaveAddress; - ModbusTxFrame[1] = 0X10; + ModbusTxFrame[1] = MODBUS_FC_WRITE_MULTIPLE; (void)memcpy(&ModbusTxFrame[2], &request[2], 4U); ModbusAppendCrc(ModbusTxFrame, 6U); return 8U; } -/** - * @brief 处理读取扩展地址保持寄存器功能码0x48 - * @param[in] request RTU请求帧 - * @param[in] requestLength 请求帧长度 - * @return 待发送响应长度，异常请求返回异常响应长度 - */ -static uint16_t ModbusProcessReadBigHolding(const uint8_t *request, - uint16_t requestLength) -{ - uint32_t start; - uint32_t currentAddress; - uint16_t quantity; - uint16_t index; - uint16_t value; - - /* 请求帧：站号1 + 功能码1 + 地址3 + 数量2 + CRC2 */ - if (requestLength != 9U) - { - ModbusSlaveStatistics.illegalValueCount++; - - return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - } - - /* 提取24位起始地址和16位寄存器数量 */ - start = ModbusGetU24Be(&request[2]); - quantity = ModbusGetU16Be(&request[5]); - - /* 一次最多读取125个寄存器 */ - if ((quantity == 0U) || (quantity > MODBUS_READ_REGS_MAX)) - { - ModbusSlaveStatistics.illegalValueCount++; - - return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_VALUE); - } - - /* - * 普通SRAM有40000个寄存器，CCMRAM有29999个寄存器， - * 总地址范围为0～69998 - */ - if ((start >= 69999UL) || ((uint32_t)quantity > (69999UL - start))) - - { - ModbusSlaveStatistics.illegalAddressCount++; - - return ModbusBuildException(request[1], MODBUS_EX_ILLEGAL_ADDRESS); - } - - /* 组成正常响应帧头 */ - ModbusTxFrame[0] = ModbusSlaveAddress; - ModbusTxFrame[1] = 0x48U; - ModbusTxFrame[2] = (uint8_t)(quantity * 2U); - - for (index = 0U; index < quantity; index++) - { - currentAddress = start + (uint32_t)index; - - /* - * 地址0～39999位于普通SRAM； - * 地址40000～69998位于CCMRAM - */ - if (currentAddress < 40000UL) - { - value = ModbusHoldingRegisters[currentAddress]; - } - else - { - value = ModbusRegistersCcm[currentAddress - 40000UL]; - } - - /* 每个寄存器按照高字节、低字节装入响应帧 */ - ModbusTxFrame[3U + index * 2U] = (uint8_t)(value >> 8U); - ModbusTxFrame[4U + index * 2U] = (uint8_t)(value & 0x00FFU); - } - /* 添加CRC */ - ModbusAppendCrc(ModbusTxFrame, (uint16_t)(3U + quantity * 2U)); - - return (uint16_t)(5U + quantity * 2U); -} - -/** - * @brief 校验并分发一帧 Modbus RTU 请求 - * @param[in] request RTU 请求帧 - * @param[in] requestLength 请求帧长度 - * @return 待发送响应长度；无需响应时返回 0 - */ static uint16_t ModbusProcessRequest(const uint8_t *request, uint16_t requestLength) { - uint16_t calculatedCrc; uint16_t receivedCrc; uint8_t isBroadcast; @@ -785,64 +392,35 @@ static uint16_t ModbusProcessRequest(const uint8_t *request, return 0U; } - calculatedCrc = ModbusCrc16(request, (uint16_t)(requestLength - 2U)); receivedCrc = (uint16_t)(request[requestLength - 2U] - | ((uint16_t)request[requestLength - 1U] << 8U)); - - if (calculatedCrc != receivedCrc) // CRC校验 + | ((uint16_t)request[requestLength - 1U] << 8U)); + if (ModbusCrc16(request, (uint16_t)(requestLength - 2U)) != receivedCrc) { - ModbusSlaveStatistics.crcErrorCount++; return 0U; } if ((request[0] != ModbusSlaveAddress) - && (request[0] != MODBUS_BROADCAST_ADDRESS)) // 非法从站地址 + && (request[0] != MODBUS_BROADCAST_ADDRESS)) { - ModbusSlaveStatistics.ignoredAddressCount++; return 0U; } - isBroadcast = - (request[0] == MODBUS_BROADCAST_ADDRESS) ? 1U : 0U; // 是否广播请求 - ModbusSlaveStatistics.validFrameCount++; - ModbusLastValidFrameTick = HAL_GetTick(); - ModbusHasReceivedValidFrame = 1U; - + isBroadcast = (request[0] == MODBUS_BROADCAST_ADDRESS) ? 1U : 0U; switch (request[1]) { - case 0X01U: // 读线圈 - // 广播请求不允许读取，从站不作响应 - return (isBroadcast != 0U) - ? 0U - : ModbusProcessReadCoils(request, requestLength); - - case 0X03U: // 读保持寄存器 + case MODBUS_FC_READ_HOLDING: return (isBroadcast != 0U) ? 0U : ModbusProcessReadHolding(request, requestLength); - case 0x05U: // 写单个线圈 - return ModbusProcessWriteSingleCoil(request, requestLength, - isBroadcast); - - case 0x06U: // 写单个保持寄存器 - return ModbusProcessWriteSingleRegister(request, requestLength, - isBroadcast); - - case 0x0FU: // 写多个线圈 - return ModbusProcessWriteMultipleCoils(request, requestLength, - isBroadcast); - - case 0x10U: // 写多个保持寄存器 - return ModbusProcessWriteMultipleRegisters(request, requestLength, - isBroadcast); - - case 0x48U: // 读大地址保持寄存器 - return (isBroadcast != 0U) - ? 0U - : ModbusProcessReadBigHolding(request, requestLength); - - default: // 未知功能码 - ModbusSlaveStatistics.illegalFunctionCount++; + case MODBUS_FC_WRITE_SINGLE: + return ModbusProcessWriteSingle(request, + requestLength, + isBroadcast); + case MODBUS_FC_WRITE_MULTIPLE: + return ModbusProcessWriteMultiple(request, + requestLength, + isBroadcast); + default: return (isBroadcast != 0U) ? 0U : ModbusBuildException(request[1], @@ -852,60 +430,70 @@ static uint16_t ModbusProcessRequest(const uint8_t *request, HAL_StatusTypeDef ModbusSlaveInit(UART_HandleTypeDef *huart, uint8_t slaveAddress) - { + if ((huart == NULL) + || (huart->Instance == NULL) + || (huart->hdmarx == NULL) + || (huart->hdmatx == NULL) + || (huart->Init.BaudRate == 0UL) + || (slaveAddress == MODBUS_BROADCAST_ADDRESS) + || (slaveAddress > MODBUS_SLAVE_ADDRESS_MAX)) + { + return HAL_ERROR; + } ModbusUart = huart; ModbusSlaveAddress = slaveAddress; - /* 清空拼帧状态并根据当前波特率初始化T1.5和T3.5 */ ModbusRxAssemblyLength = 0U; ModbusRxAssemblyInvalid = 0U; + ModbusRxLastByteCycle = 0UL; + ModbusRxFrameLength = 0U; ModbusRxFrameReady = 0U; ModbusTxBusy = 0U; ModbusRtuTimingInit(huart->Init.BaudRate); - //ModbusHoldingRegisters[HMI_REG_DEVICE_ID] = 0xF407U; - return ModbusStartReceive(); } void ModbusSlavePoll(void) { uint16_t responseLength; + uint32_t primask; + HAL_StatusTypeDef txStatus = HAL_OK; + + if (ModbusUart == NULL) + { + return; + } - /* 检查末字节后的静默时间是否已经达到T3.5 */ ModbusTryFinalizeReceive(); if ((ModbusRxFrameReady == 0U) || (ModbusTxBusy != 0U)) { return; } - responseLength = ModbusProcessRequest(ModbusRxFrame, ModbusRxFrameLength); + responseLength = ModbusProcessRequest(ModbusRxFrame, + ModbusRxFrameLength); + + primask = ModbusEnterCritical(); + ModbusRxFrameReady = 0U; if (responseLength > 0U) { ModbusTxBusy = 1U; - - if (HAL_UART_Transmit_DMA(ModbusUart, ModbusTxFrame, responseLength) - == HAL_OK) - { - ModbusSlaveStatistics.txFrameCount++; - } - else + txStatus = HAL_UART_Transmit_DMA(ModbusUart, + ModbusTxFrame, + responseLength); + if (txStatus != HAL_OK) { ModbusTxBusy = 0U; - ModbusSlaveStatistics.uartErrorCount++; - (void)ModbusStartReceive(); // 重启DMA接收 } } - else + ModbusExitCritical(primask); + + if ((responseLength == 0U) || (txStatus != HAL_OK)) { (void)ModbusStartReceive(); } - - /* - * 当前帧；处理完成后再释放帧槽 - */ - ModbusRxFrameReady = 0U; } void ModbusSlaveOnRxEvent(UART_HandleTypeDef *huart, uint16_t size) @@ -915,199 +503,90 @@ void ModbusSlaveOnRxEvent(UART_HandleTypeDef *huart, uint16_t size) uint32_t lastByteCycle; uint32_t firstByteCycle; uint32_t chunkCycles; - uint32_t interFrameGap; + uint32_t interChunkGap; - ModbusSlaveStatistics.rxEventCount++; // 串口接收事件计数 - - if ((huart != ModbusUart) || (ModbusUart == NULL)) + if ((ModbusUart == NULL) || (huart != ModbusUart)) { return; } - if ((size > 0U) && (size <= MODBUS_RTU_ADU_SIZE_MAX)) + eventType = HAL_UARTEx_GetRxEventType(huart); + if (eventType == HAL_UART_RXEVENT_HT) { - now = DWT->CYCCNT; - eventType = HAL_UARTEx_GetRxEventType(huart); - - /* IDLE事件比末字节结束晚约一个字符时间，减去字符时间得到末字节时刻 */ - lastByteCycle = now; - if (eventType == HAL_UART_RXEVENT_IDLE) - { - lastByteCycle -= ModbusRtuCharCycles; - } + return; + } - /* 根据本次接收字节数反推DMA片段首字节的开始时刻 */ - chunkCycles = (uint32_t)((uint64_t)size * ModbusRtuCharCycles); - firstByteCycle = lastByteCycle - chunkCycles; + if ((size == 0U) || (size > MODBUS_RTU_ADU_SIZE_MAX)) + { + ModbusRxAssemblyLength = 0U; + ModbusRxAssemblyInvalid = 0U; + (void)ModbusStartReceive(); + return; + } - if (ModbusRxAssemblyLength > 0U) - { - interFrameGap = (uint32_t)(firstByteCycle - ModbusRxLastByteCycle); - ModbusLastInterFrameGapCycles = interFrameGap; - if (interFrameGap >= ModbusRtuT35Cycles) - { - /* 间隔达到T3.5，结束上一帧，本片段作为新帧开始 */ - ModbusRxAssemblyFinalize(); - } - else if (interFrameGap > ModbusRtuT15Cycles) - { - /* 帧内静默超过T1.5但不足T3.5，标记整帧无效 */ - ModbusRxAssemblyInvalid = 1U; - } - else - { - /* 间隔不超过T1.5，当前片段继续拼入同一帧 */ - } - } + now = DWT->CYCCNT; + lastByteCycle = now; + if (eventType == HAL_UART_RXEVENT_IDLE) + { + lastByteCycle -= ModbusRtuCharCycles; + } + chunkCycles = (uint32_t)((uint64_t)size * ModbusRtuCharCycles); + firstByteCycle = lastByteCycle - chunkCycles; - if (size - <= (uint16_t)(MODBUS_RTU_ADU_SIZE_MAX - ModbusRxAssemblyLength)) + if (ModbusRxAssemblyLength > 0U) + { + interChunkGap = (uint32_t)(firstByteCycle + - ModbusRxLastByteCycle); + if (interChunkGap >= ModbusRtuT35Cycles) { - (void)memcpy(&ModbusRxAssemblyBuffer[ModbusRxAssemblyLength], - ModbusRxDmaBuffer, size); - ModbusRxAssemblyLength += size; + ModbusRxAssemblyFinalize(); } - else + else if (interChunkGap > ModbusRtuT15Cycles) { ModbusRxAssemblyInvalid = 1U; } + } - /* 保存末字节时刻并立即重启DMA，继续等待可能的后续片段 */ - ModbusRxLastByteCycle = lastByteCycle; - (void)ModbusStartReceive(); + if (size <= (uint16_t)(MODBUS_RTU_ADU_SIZE_MAX + - ModbusRxAssemblyLength)) + { + (void)memcpy(&ModbusRxAssemblyBuffer[ModbusRxAssemblyLength], + ModbusRxDmaBuffer, + size); + ModbusRxAssemblyLength += size; } else { - // 长度异常帧 - ModbusSlaveStatistics.droppedFrameCount++; - ModbusRxAssemblyLength = 0U; - ModbusRxAssemblyInvalid = 0U; - (void)ModbusStartReceive(); + ModbusRxAssemblyInvalid = 1U; } + + ModbusRxLastByteCycle = lastByteCycle; + (void)ModbusStartReceive(); } void ModbusSlaveOnTxComplete(UART_HandleTypeDef *huart) { - if ((huart != ModbusUart) || (ModbusUart == NULL)) + if ((ModbusUart == NULL) || (huart != ModbusUart)) { return; } ModbusTxBusy = 0U; - (void)ModbusStartReceive(); // 重启DMA接收 + (void)ModbusStartReceive(); } void ModbusSlaveOnUartError(UART_HandleTypeDef *huart) { - if ((huart != ModbusUart) || (ModbusUart == NULL)) + if ((ModbusUart == NULL) || (huart != ModbusUart)) { return; } - ModbusSlaveStatistics.uartErrorCount++; ModbusTxBusy = 0U; ModbusRxAssemblyLength = 0U; ModbusRxAssemblyInvalid = 0U; - (void)HAL_UART_Abort(huart); // 立即终止这个串口当前正在进行的发送和接收操作 - (void)ModbusStartReceive(); // 重启DMA接收 -} - -uint8_t ModbusSlaveSetHoldingRegister(uint16_t address, uint16_t value) -{ - if (address >= MODBUS_MAP_ITEM_COUNT) - { - return 0U; - } - - ModbusHoldingRegisters[address] = value; - return 1U; -} - -uint8_t ModbusSlaveGetHoldingRegister(uint16_t address, uint16_t *value) -{ - if ((address >= MODBUS_MAP_ITEM_COUNT) || (value == NULL)) - { - return 0U; - } - - *value = ModbusHoldingRegisters[address]; - return 1U; -} - -uint8_t ModbusSlaveSetCoil(uint16_t address, uint8_t state) -{ - if (address >= MODBUS_MAP_ITEM_COUNT) - { - return 0U; - } - - ModbusCoilSetUnchecked(address, state); - return 1U; -} - -uint8_t ModbusSlaveGetCoil(uint16_t address, uint8_t *state) -{ - if ((address >= MODBUS_MAP_ITEM_COUNT) || (state == NULL)) - { - return 0U; - } - - *state = ModbusCoilGetUnchecked(address); - return 1U; -} - -uint8_t ModbusSlaveIsConnected(uint32_t timeoutMs) -{ - if (ModbusHasReceivedValidFrame == 0U) - { - return 0U; - } - - return ((HAL_GetTick() - ModbusLastValidFrameTick) <= timeoutMs) ? 1U : 0U; -} - -// 上电恢复函数 -void ModbusRetainedRegistersLoad(void) -{ - uint16_t index; - - if (ModbusBackupData->magic == MODBUS_BACKUP_MAGIC) - { - for (index = 0U; index < MODBUS_RETAINED_D_COUNT; index++) - { - ModbusHoldingRegisters[MODBUS_RETAINED_D_START + index] = - ModbusBackupData->retainedD[index]; - } - } - else - { - for (index = 0U; index < MODBUS_RETAINED_D_COUNT; index++) - { - ModbusHoldingRegisters[MODBUS_RETAINED_D_START + index] = 0U; - - ModbusBackupData->retainedD[index] = 0U; - } - - /* - * 数据初始化完成后最后写magic，避免初始化中途断电 - * 却把不完整数据标记成有效 - */ - ModbusBackupData->magic = MODBUS_BACKUP_MAGIC; - } -} -void ModbusRetainedRegistersPoll(void) -{ - uint16_t index; - uint16_t value; - - for (index = 0; index < MODBUS_RETAINED_D_COUNT; index++) - { - value = ModbusHoldingRegisters[MODBUS_RETAINED_D_START + index]; - if (value != ModbusRetainedSnapshot[index]) - { - ModbusBackupData->retainedD[index] = value; - } - - ModbusRetainedSnapshot[index] = value; - } + ModbusRxFrameLength = 0U; + ModbusRxFrameReady = 0U; + (void)HAL_UART_Abort(huart); + (void)ModbusStartReceive(); } diff --git a/PLSR/Inc/plsr.h b/PLSR/Inc/plsr.h new file mode 100644 index 0000000..12f5cf2 --- /dev/null +++ b/PLSR/Inc/plsr.h @@ -0,0 +1,94 @@ +#ifndef PLSR_H +#define PLSR_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define PLSR_CONFIG_FIRST_ADDRESS (0x1000U) +#define PLSR_CONFIG_LAST_ADDRESS (0x1197U) +#define PLSR_STATUS_FIRST_ADDRESS (0x2000U) +#define PLSR_STATUS_LAST_ADDRESS (0x2006U) +#define PLSR_CONTROL_ADDRESS (0x3000U) + +typedef enum +{ + PLSR_MB_NOT_HANDLED = 0, + PLSR_MB_OK, + PLSR_MB_ILLEGAL_ADDRESS, + PLSR_MB_ILLEGAL_VALUE, + PLSR_MB_DEVICE_BUSY, + PLSR_MB_SERVER_FAILURE +} PLSR_MB_RESULT; + +typedef enum +{ + PLSR_STATUS_UNINITIALIZED = 0, + PLSR_STATUS_IDLE = 1, + PLSR_STATUS_ACCELERATING = 2, + PLSR_STATUS_RUNNING = 3, + PLSR_STATUS_DECELERATING = 4, + PLSR_STATUS_WAITING = 5, + PLSR_STATUS_PAUSED = 6, + PLSR_STATUS_COMPLETED = 7, + PLSR_STATUS_STOPPED = 8, + PLSR_STATUS_ERROR = 9 +} PLSR_STATUS; + +typedef enum +{ + PLSR_ERROR_NONE = 0, + PLSR_ERROR_INVALID_TRANSITION = 1, + PLSR_ERROR_RESOURCE_CONFLICT = 2, + PLSR_ERROR_INVALID_RESOURCE = 3, + PLSR_ERROR_TIMER = 4, + PLSR_ERROR_COUNT = 5, + PLSR_ERROR_POSITIVE_LIMIT = 6, + PLSR_ERROR_NEGATIVE_LIMIT = 7, + PLSR_ERROR_EMERGENCY_STOP = 8, + PLSR_ERROR_INTERNAL = 9 +} PLSR_ERROR; + +uint8_t PlsrInit(void); +void PlsrPoll1ms(void); + +PLSR_MB_RESULT PlsrModbusReadHolding(uint16_t startAddress, + uint16_t quantity, + uint16_t *values); +PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, + uint16_t quantity, + const uint16_t *values); + +/* Called by the selected output timer update interrupt once per pulse. */ +void PlsrPulseTimerIrq(uint8_t pulseOutput); + +#ifdef PLSR_HOST_TEST +uint64_t PlsrTestDivideU64ByU32(uint64_t dividend, + uint32_t divisor, + uint32_t *remainder); +void PlsrTestSetInput(uint8_t inputSelection, uint8_t level); +void PlsrTestEmitPulses(uint32_t pulseCount); +void PlsrTestEmitPulseOnCriticalEntry(void); +void PlsrTestEmitPulseAfterCriticalEntries(uint8_t entriesToSkip); +void PlsrTestEmitPulseOnCriticalExit(void); +void PlsrTestLatchPulseOnCriticalEntry(void); +void PlsrTestServicePendingPulse(void); +void PlsrTestFailNextStart(void); +void PlsrTestFailNextFrequencyAtUpdate(void); +uint8_t PlsrTestPulseIsActive(void); +uint32_t PlsrTestOutputFrequency(void); +uint32_t PlsrTestQueuedFrequency(void); +uint8_t PlsrTestDirectionLevel(void); +void PlsrTestSetPosition(int32_t position, uint8_t positionValid); +void PlsrTestClearPersistentStorage(void); +void PlsrTestResetSaveCount(void); +uint32_t PlsrTestSaveCount(void); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* PLSR_H */ diff --git a/PLSR/Src/plsr.c b/PLSR/Src/plsr.c new file mode 100644 index 0000000..9445eb3 --- /dev/null +++ b/PLSR/Src/plsr.c @@ -0,0 +1,3599 @@ +#include "plsr.h" +#include "plsr_internal.h" +#include "plsr_platform.h" +#include +#include + +#if defined(__ICCARM__) +#include +#endif + +#define PLSR_COMMON_FIRST_ADDRESS (0x1000U) +#define PLSR_COMMON_LAST_ADDRESS (0x10FFU) +#define PLSR_SEGMENT_FIRST_ADDRESS (0x1100U) +#define PLSR_SEGMENT_STRIDE (0x0010U) +#define PLSR_SEGMENT_DEFINED_WORDS (8U) + +#define PLSR_WAIT_TIME (0U) +#define PLSR_WAIT_SIGNAL (1U) +#define PLSR_ACT_TIME (2U) +#define PLSR_EXT_SIGNAL (3U) +#define PLSR_EXT_OR_COMPLETE (4U) + +#define PLSR_SEND_COMPLETE (0U) +#define PLSR_SEND_SUBSEQUENT (1U) +#define PLSR_POSITION_RELATIVE (0U) +#define PLSR_POSITION_ABSOLUTE (1U) + +#define PLSR_COMMAND_START (0x0001U) +#define PLSR_COMMAND_STOP (0x0002U) +#define PLSR_COMMAND_CLEAR (0x0004U) + +#define PLSR_CONFIG_SAVE_DELAY_MS (1000U) +#define PLSR_POSITION_CHECKPOINT_MS (10U) +#define PLSR_SHORT_PROFILE_MAX_PULSES (65535UL) +#define PLSR_Q32_ONE (4294967296ULL) + +typedef struct +{ + uint32_t fromHz; + uint32_t toHz; + uint32_t durationMs; + uint32_t elapsedMs; + uint8_t active; +} PLSR_RAMP; + +typedef struct +{ + uint32_t startHz; + uint32_t peakHz; + uint32_t endHz; + uint16_t pulseCount; + uint16_t entryPulses; + uint16_t steadyPulses; + uint16_t exitPulses; + volatile uint16_t nextPeriod; + uint64_t rampBoundaryQ32; + uint64_t rampTotalAreaQ32; + uint64_t rampTargetAreaQ32; + uint64_t rampAreaStepQ32; + uint32_t rampAreaRemainder; + uint32_t rampRemainderAccumulator; + uint64_t lastRampPhaseStepQ32; + uint64_t entryFirstBoundaryQ32; + uint64_t entrySecondBoundaryQ32; + uint64_t exitFirstBoundaryQ32; + uint64_t exitSecondBoundaryQ32; + uint32_t lastRampFrequencyHz; + volatile uint8_t active; +} PLSR_SHORT_PROFILE; + +typedef struct +{ + uint64_t magnitude; + uint32_t firstFrequencyHz; + PLSR_SHORT_PROFILE profile; + uint8_t nextSegment; + uint8_t positive; + volatile uint8_t valid; +} PLSR_HANDOFF_PLAN; + +typedef enum +{ + PLSR_WORD_OK = 0, + PLSR_WORD_ILLEGAL_ADDRESS, + PLSR_WORD_ILLEGAL_VALUE +} PLSR_WORD_RESULT; + +typedef enum +{ + PLSR_COMMAND_MAILBOX_EMPTY = 0, + PLSR_COMMAND_MAILBOX_PENDING, + PLSR_COMMAND_MAILBOX_EXECUTING +} PLSR_COMMAND_MAILBOX_STATE; + +typedef struct +{ + PLSR_CONFIG startConfig; + uint16_t command; + volatile uint8_t state; +} PLSR_COMMAND_MAILBOX; + +static const uint16_t PlsrSineProgressQ16[65] = +{ + 0U, 39U, 158U, 355U, 630U, 982U, 1411U, 1915U, + 2494U, 3146U, 3869U, 4662U, 5522U, 6448U, 7438U, 8488U, + 9597U, 10762U, 11980U, 13248U, 14563U, 15922U, 17321U, + 18758U, 20228U, 21728U, 23256U, 24806U, 26375U, 27960U, + 29556U, 31160U, 32767U, 34375U, 35979U, 37575U, 39160U, + 40729U, 42279U, 43807U, 45307U, 46777U, 48214U, 49613U, + 50972U, 52287U, 53555U, 54773U, 55938U, 57047U, 58097U, + 59087U, 60013U, 60873U, 61666U, 62389U, 63041U, 63620U, + 64124U, 64553U, 64905U, 65180U, 65377U, 65496U, 65535U +}; + +static const uint32_t PlsrSmoothIntegralQ24[65] = +{ + 0UL, 64UL, 504UL, 1688UL, 3968UL, 7688UL, 13176UL, 20752UL, + 30720UL, 43376UL, 59000UL, 77864UL, 100224UL, 126328UL, 156408UL, + 190688UL, 229376UL, 272672UL, 320760UL, 373816UL, 432000UL, + 495464UL, 564344UL, 638768UL, 718848UL, 804688UL, 896376UL, + 993992UL, 1097600UL, 1207256UL, 1323000UL, 1444864UL, 1572864UL, + 1707008UL, 1847288UL, 1993688UL, 2146176UL, 2304712UL, 2469240UL, + 2639696UL, 2816000UL, 2998064UL, 3185784UL, 3379048UL, 3577728UL, + 3781688UL, 3990776UL, 4204832UL, 4423680UL, 4647136UL, 4875000UL, + 5107064UL, 5343104UL, 5582888UL, 5826168UL, 6072688UL, 6322176UL, + 6574352UL, 6828920UL, 7085576UL, 7344000UL, 7603864UL, 7864824UL, + 8126528UL, 8388608UL +}; + +static const uint32_t PlsrSineIntegralQ24[65] = +{ + 0UL, 53UL, 421UL, 1420UL, 3362UL, 6560UL, 11321UL, 17949UL, + 26744UL, 38000UL, 52007UL, 69047UL, 89393UL, 113314UL, 141066UL, + 172899UL, 209052UL, 249753UL, 295221UL, 345662UL, 401269UL, + 462225UL, 528698UL, 600845UL, 678806UL, 762711UL, 852672UL, + 948789UL, 1051146UL, 1159812UL, 1274841UL, 1396271UL, 1524127UL, + 1658415UL, 1799129UL, 1946244UL, 2099722UL, 2259509UL, 2425536UL, + 2597719UL, 2775958UL, 2960141UL, 3150138UL, 3345809UL, 3546997UL, + 3753534UL, 3965237UL, 4181913UL, 4403356UL, 4629347UL, 4859658UL, + 5094050UL, 5332273UL, 5574071UL, 5819175UL, 6067312UL, 6318200UL, + 6571549UL, 6827065UL, 7084448UL, 7343394UL, 7603596UL, 7864741UL, + 8126517UL, 8388608UL +}; + +static PLSR_CONFIG PlsrShadowConfig; +static PLSR_CONFIG PlsrActiveConfig; +static PLSR_CONFIG PlsrCandidateConfig; +static volatile int32_t PlsrPosition; +static volatile uint64_t PlsrRemainingPulses; +static volatile uint8_t PlsrPulseActive; +static volatile uint8_t PlsrCutRequested; +static volatile uint8_t PlsrBoundaryPending; +static volatile uint8_t PlsrBoundaryWasCut; +static volatile uint8_t PlsrCountPositive; +static volatile uint8_t PlsrCountOverflowPending; +static volatile uint8_t PlsrPositionValid; +static volatile uint8_t PlsrPositionCheckpointDirty; +static volatile uint8_t PlsrFrequencyUpdatePending; +static volatile uint8_t PlsrFrequencyUpdateSegment; +static volatile uint8_t PlsrSeamlessHandoffPending; +static volatile uint8_t PlsrTimerErrorPending; +static volatile uint8_t PlsrDeferredFrequencyPending; +static volatile uint32_t PlsrCurrentFrequencyHz; +static volatile uint32_t PlsrQueuedFrequencyHz; +static volatile uint32_t PlsrBoundaryFrequencyHz; +static volatile uint32_t PlsrFrequencyUpdateTargetHz; +static volatile uint32_t PlsrDeferredFrequencyHz; +static volatile uint32_t PlsrSegmentEpoch; + +static volatile PLSR_STATUS PlsrRunStatus = PLSR_STATUS_UNINITIALIZED; +static PLSR_ERROR PlsrError = PLSR_ERROR_NONE; +static PLSR_RAMP PlsrRamp; +static PLSR_SHORT_PROFILE PlsrShortProfile; +static PLSR_HANDOFF_PLAN PlsrHandoffPlan; +static PLSR_HANDOFF_PLAN + PlsrPreparedHandoffPlans[2][PLSR_SEGMENT_COUNT_MAX]; +static volatile uint8_t PlsrPreparedHandoffBank; +static uint8_t PlsrInitialized; +static volatile uint8_t PlsrCurrentSegment; +static uint8_t PlsrDirectionDelayActive; +static uint16_t PlsrDirectionDelayRemainingMs; +static volatile uint8_t PlsrSegmentClockStarted; +static volatile uint32_t PlsrSegmentElapsedMs; +static volatile uint32_t PlsrWaitElapsedMs; +static volatile uint8_t PlsrExtPreviousLevel; +static volatile uint8_t PlsrExtEdgePending; +static volatile uint8_t PlsrStopRequested; +static volatile uint8_t PlsrStopPulsesRemaining; +static volatile uint8_t PlsrBoundaryRampStarted; +static uint8_t PlsrLastDirectionValid; +static uint8_t PlsrLastDirectionOutput; +static uint8_t PlsrLastDirectionLevel; +static uint8_t PlsrPersistenceDirty; +static uint16_t PlsrPersistenceDelayMs; +static uint8_t PlsrPositionCheckpointElapsedMs; +static PLSR_COMMAND_MAILBOX PlsrCommandMailbox; + +static uint8_t PlsrIsBusy(void); +static void PlsrSetDefaults(PLSR_CONFIG *config); +static uint8_t PlsrConfigIsValid(const PLSR_CONFIG *config, + uint8_t validateActivePath); +static uint16_t PlsrReadConfigWord(const PLSR_CONFIG *config, + uint16_t address); +static PLSR_WORD_RESULT PlsrWriteConfigWord(PLSR_CONFIG *config, + uint16_t address, + uint16_t value); +static uint8_t PlsrAddressIsDwordHalf(uint16_t address, + uint16_t *pairedAddress); +static PLSR_MB_RESULT PlsrQueueCommand(uint16_t command); +static uint8_t PlsrPollCommandMailbox(void); +static void PlsrExecuteStart(void); +static uint8_t PlsrExecuteStop(void); +static void PlsrExecuteClear(void); +static uint8_t PlsrStartSegment(uint8_t segmentNumber, + uint8_t allowCarry, + uint32_t carryFrequencyHz); +static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz); +static void PlsrHandleBoundary(uint8_t extEdge); +static void PlsrTransitionToNext(uint8_t allowCarry); +static void PlsrFinishCompleted(void); +static void PlsrFinishStopped(void); +static void PlsrEnterError(PLSR_ERROR error); +static void PlsrMarkPersistenceDirty(uint16_t delayMs); +static void PlsrCheckpointPosition(uint8_t wasBusy); +static void PlsrPollPositionCheckpoint(void); +static uint8_t PlsrTrySubsequentHandoff(void); +static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, + uint8_t segmentNumber, + uint32_t startFrequencyHz, + uint32_t targetFrequencyHz, + uint64_t pulseCount); +static uint32_t PlsrShortProfileTakeFrequency(PLSR_SHORT_PROFILE *profile); +static uint8_t PlsrAdvanceShortProfile(uint8_t pulseOutput); +static void PlsrCopyShortProfile(PLSR_SHORT_PROFILE *destination, + const PLSR_SHORT_PROFILE *source); +static void PlsrInvalidateHandoffPlans(void); +static uint8_t PlsrBuildHandoffPlanBank( + const PLSR_CONFIG *frequencyConfig); +static uint8_t PlsrSelectPreparedHandoffPlan(PLSR_HANDOFF_PLAN *plan); +static uint8_t PlsrPrimeHandoff(void); +static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, + const PLSR_CONFIG *frequencyConfig, + PLSR_HANDOFF_PLAN *plan); + +static uint8_t PlsrIsBusy(void) +{ + return ((PlsrRunStatus == PLSR_STATUS_ACCELERATING) + || (PlsrRunStatus == PLSR_STATUS_RUNNING) + || (PlsrRunStatus == PLSR_STATUS_DECELERATING) + || (PlsrRunStatus == PLSR_STATUS_WAITING) + || (PlsrRunStatus == PLSR_STATUS_PAUSED)) ? 1U : 0U; +} + +static uint32_t PlsrJoinU32(uint16_t lowWord, uint16_t highWord) +{ + return (uint32_t)lowWord | ((uint32_t)highWord << 16U); +} + +static uint16_t PlsrLowWord(uint32_t value) +{ + return (uint16_t)(value & 0xFFFFUL); +} + +static uint16_t PlsrHighWord(uint32_t value) +{ + return (uint16_t)(value >> 16U); +} + +static void PlsrSetDefaults(PLSR_CONFIG *config) +{ + uint8_t index; + + (void)memset(config, 0, sizeof(*config)); + config->pulseOutput = 0U; + config->directionOutput = 0U; + config->waitInput = 0U; + config->extInput = 0U; + config->sendMode = PLSR_SEND_COMPLETE; + config->directionDelayMs = 10U; + config->directionNegativeLogic = 0U; + config->curveMode = 0U; + config->positionMode = PLSR_POSITION_RELATIVE; + config->segmentCount = 1U; + config->startSegment = 1U; + config->defaultSpeedHz = 1000UL; + config->startSpeedHz = 100UL; + config->stopSpeedHz = 100UL; + config->accelerationTimeMs = 100U; + config->decelerationTimeMs = 100U; + + for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++) + { + config->segments[index].frequencyHz = 1000UL; + config->segments[index].pulses = (index == 0U) ? 1000L : 0L; + config->segments[index].waitType = PLSR_EXT_OR_COMPLETE; + config->segments[index].waitTimeMs = 0U; + config->segments[index].actTimeMs = 0U; + config->segments[index].jumpSegment = 0U; + } +} + +static uint8_t PlsrConfigIsValid(const PLSR_CONFIG *config, + uint8_t validateActivePath) +{ + uint8_t index; + + if ((config->pulseOutput > 3U) || (config->directionOutput > 3U) + || (config->waitInput > 1U) || (config->extInput > 1U) + || (config->sendMode > PLSR_SEND_SUBSEQUENT) + || (config->directionNegativeLogic > 1U) + || (config->curveMode > 2U) + || (config->positionMode > PLSR_POSITION_ABSOLUTE) + || (config->segmentCount == 0U) + || (config->segmentCount > PLSR_SEGMENT_COUNT_MAX) + || (config->startSegment == 0U) + || (config->startSegment > PLSR_SEGMENT_COUNT_MAX) + || (config->defaultSpeedHz == 0UL) + || (config->defaultSpeedHz > PLSR_FREQUENCY_MAX_HZ) + || (config->startSpeedHz > PLSR_FREQUENCY_MAX_HZ) + || (config->stopSpeedHz > PLSR_FREQUENCY_MAX_HZ)) + { + return 0U; + } + + if ((validateActivePath != 0U) + && (config->startSegment > config->segmentCount)) + { + return 0U; + } + + for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++) + { + const PLSR_SEGMENT_CONFIG *segment = &config->segments[index]; + + if ((segment->frequencyHz == 0UL) + || (segment->frequencyHz > PLSR_FREQUENCY_MAX_HZ) + || (segment->waitType > PLSR_EXT_OR_COMPLETE) + || (segment->jumpSegment > PLSR_SEGMENT_COUNT_MAX)) + { + return 0U; + } + + if ((validateActivePath != 0U) + && (index < config->segmentCount) + && (segment->jumpSegment > config->segmentCount)) + { + return 0U; + } + } + + return 1U; +} + +static uint16_t PlsrReadConfigWord(const PLSR_CONFIG *config, + uint16_t address) +{ + uint16_t offset; + uint8_t segmentIndex; + const PLSR_SEGMENT_CONFIG *segment; + + switch (address) + { + case 0x1000U: return config->pulseOutput; + case 0x1001U: return config->directionOutput; + case 0x1002U: return config->waitInput; + case 0x1003U: return config->extInput; + case 0x1004U: return config->sendMode; + case 0x1005U: return config->directionDelayMs; + case 0x1006U: return config->directionNegativeLogic; + case 0x1007U: return config->curveMode; + case 0x1008U: return config->positionMode; + case 0x1009U: return config->segmentCount; + case 0x100AU: return config->startSegment; + case 0x100BU: return PlsrLowWord(config->defaultSpeedHz); + case 0x100CU: return PlsrHighWord(config->defaultSpeedHz); + case 0x100DU: return PlsrLowWord(config->startSpeedHz); + case 0x100EU: return PlsrHighWord(config->startSpeedHz); + case 0x1010U: return PlsrLowWord(config->stopSpeedHz); + case 0x1011U: return PlsrHighWord(config->stopSpeedHz); + case 0x1012U: return config->accelerationTimeMs; + case 0x1013U: return config->decelerationTimeMs; + default: break; + } + + if ((address >= PLSR_SEGMENT_FIRST_ADDRESS) + && (address <= PLSR_CONFIG_LAST_ADDRESS)) + { + offset = (uint16_t)(address - PLSR_SEGMENT_FIRST_ADDRESS); + segmentIndex = (uint8_t)(offset / PLSR_SEGMENT_STRIDE); + offset = (uint16_t)(offset % PLSR_SEGMENT_STRIDE); + segment = &config->segments[segmentIndex]; + + switch (offset) + { + case 0U: return PlsrLowWord(segment->frequencyHz); + case 1U: return PlsrHighWord(segment->frequencyHz); + case 2U: return PlsrLowWord((uint32_t)segment->pulses); + case 3U: return PlsrHighWord((uint32_t)segment->pulses); + case 4U: return segment->waitType; + case 5U: return segment->waitTimeMs; + case 6U: return segment->actTimeMs; + case 7U: return segment->jumpSegment; + default: return 0U; + } + } + + /* 0x100F, 0x1014..0x10FF, and segment padding read as zero. */ + return 0U; +} + +static PLSR_WORD_RESULT PlsrWriteConfigWord(PLSR_CONFIG *config, + uint16_t address, + uint16_t value) +{ + uint16_t offset; + uint8_t segmentIndex; + PLSR_SEGMENT_CONFIG *segment; + + switch (address) + { + case 0x1000U: config->pulseOutput = value; return PLSR_WORD_OK; + case 0x1001U: config->directionOutput = value; return PLSR_WORD_OK; + case 0x1002U: config->waitInput = value; return PLSR_WORD_OK; + case 0x1003U: config->extInput = value; return PLSR_WORD_OK; + case 0x1004U: config->sendMode = value; return PLSR_WORD_OK; + case 0x1005U: config->directionDelayMs = value; return PLSR_WORD_OK; + case 0x1006U: + config->directionNegativeLogic = value; + return PLSR_WORD_OK; + case 0x1007U: config->curveMode = value; return PLSR_WORD_OK; + case 0x1008U: config->positionMode = value; return PLSR_WORD_OK; + case 0x1009U: config->segmentCount = value; return PLSR_WORD_OK; + case 0x100AU: config->startSegment = value; return PLSR_WORD_OK; + case 0x100BU: + config->defaultSpeedHz = + PlsrJoinU32(value, PlsrHighWord(config->defaultSpeedHz)); + return PLSR_WORD_OK; + case 0x100CU: + config->defaultSpeedHz = + PlsrJoinU32(PlsrLowWord(config->defaultSpeedHz), value); + return PLSR_WORD_OK; + case 0x100DU: + config->startSpeedHz = + PlsrJoinU32(value, PlsrHighWord(config->startSpeedHz)); + return PLSR_WORD_OK; + case 0x100EU: + config->startSpeedHz = + PlsrJoinU32(PlsrLowWord(config->startSpeedHz), value); + return PLSR_WORD_OK; + case 0x100FU: + return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE; + case 0x1010U: + config->stopSpeedHz = + PlsrJoinU32(value, PlsrHighWord(config->stopSpeedHz)); + return PLSR_WORD_OK; + case 0x1011U: + config->stopSpeedHz = + PlsrJoinU32(PlsrLowWord(config->stopSpeedHz), value); + return PLSR_WORD_OK; + case 0x1012U: + config->accelerationTimeMs = value; + return PLSR_WORD_OK; + case 0x1013U: + config->decelerationTimeMs = value; + return PLSR_WORD_OK; + default: break; + } + + if ((address >= 0x1014U) && (address <= PLSR_COMMON_LAST_ADDRESS)) + { + return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE; + } + + if ((address < PLSR_SEGMENT_FIRST_ADDRESS) + || (address > PLSR_CONFIG_LAST_ADDRESS)) + { + return PLSR_WORD_ILLEGAL_ADDRESS; + } + + offset = (uint16_t)(address - PLSR_SEGMENT_FIRST_ADDRESS); + segmentIndex = (uint8_t)(offset / PLSR_SEGMENT_STRIDE); + offset = (uint16_t)(offset % PLSR_SEGMENT_STRIDE); + segment = &config->segments[segmentIndex]; + + switch (offset) + { + case 0U: + segment->frequencyHz = + PlsrJoinU32(value, PlsrHighWord(segment->frequencyHz)); + return PLSR_WORD_OK; + case 1U: + segment->frequencyHz = + PlsrJoinU32(PlsrLowWord(segment->frequencyHz), value); + return PLSR_WORD_OK; + case 2U: + segment->pulses = (int32_t)PlsrJoinU32( + value, PlsrHighWord((uint32_t)segment->pulses)); + return PLSR_WORD_OK; + case 3U: + segment->pulses = (int32_t)PlsrJoinU32( + PlsrLowWord((uint32_t)segment->pulses), value); + return PLSR_WORD_OK; + case 4U: segment->waitType = value; return PLSR_WORD_OK; + case 5U: segment->waitTimeMs = value; return PLSR_WORD_OK; + case 6U: segment->actTimeMs = value; return PLSR_WORD_OK; + case 7U: segment->jumpSegment = value; return PLSR_WORD_OK; + default: + return (value == 0U) ? PLSR_WORD_OK : PLSR_WORD_ILLEGAL_VALUE; + } +} + +static uint8_t PlsrAddressIsDwordHalf(uint16_t address, + uint16_t *pairedAddress) +{ + uint16_t offset; + + switch (address) + { + case 0x100BU: case 0x100DU: case 0x1010U: + *pairedAddress = (uint16_t)(address + 1U); + return 1U; + case 0x100CU: case 0x100EU: case 0x1011U: + *pairedAddress = (uint16_t)(address - 1U); + return 1U; + default: break; + } + + if ((address >= PLSR_SEGMENT_FIRST_ADDRESS) + && (address <= PLSR_CONFIG_LAST_ADDRESS)) + { + offset = (uint16_t)((address - PLSR_SEGMENT_FIRST_ADDRESS) + % PLSR_SEGMENT_STRIDE); + if ((offset == 0U) || (offset == 2U)) + { + *pairedAddress = (uint16_t)(address + 1U); + return 1U; + } + if ((offset == 1U) || (offset == 3U)) + { + *pairedAddress = (uint16_t)(address - 1U); + return 1U; + } + } + + return 0U; +} + +static uint8_t PlsrAddressIsProduct(uint16_t address) +{ + return (((address >= PLSR_CONFIG_FIRST_ADDRESS) + && (address <= PLSR_CONFIG_LAST_ADDRESS)) + || ((address >= PLSR_STATUS_FIRST_ADDRESS) + && (address <= PLSR_STATUS_LAST_ADDRESS)) + || (address == PLSR_CONTROL_ADDRESS)) ? 1U : 0U; +} + +static PLSR_MB_RESULT PlsrClassifyRange(uint16_t startAddress, + uint16_t quantity) +{ + uint32_t address; + uint32_t endAddress; + uint8_t foundProduct = 0U; + uint8_t foundOther = 0U; + + if (quantity == 0U) + { + return PLSR_MB_ILLEGAL_VALUE; + } + + endAddress = (uint32_t)startAddress + (uint32_t)quantity - 1UL; + if (endAddress > 0xFFFFUL) + { + return PLSR_MB_ILLEGAL_ADDRESS; + } + + for (address = startAddress; address <= endAddress; address++) + { + if (PlsrAddressIsProduct((uint16_t)address) != 0U) + { + foundProduct = 1U; + } + else + { + foundOther = 1U; + } + } + + if (foundProduct == 0U) + { + return PLSR_MB_NOT_HANDLED; + } + return (foundOther != 0U) ? PLSR_MB_ILLEGAL_ADDRESS : PLSR_MB_OK; +} + +static uint32_t PlsrCurveProgressQ16(uint32_t elapsed, + uint32_t duration, + uint16_t curveMode) +{ + uint32_t linear; + + if ((duration == 0UL) || (elapsed >= duration)) + { + return 65535UL; + } + + linear = (uint32_t)(((uint64_t)elapsed * 65535UL) / duration); + if (curveMode == 1U) + { + uint64_t x = linear; + uint64_t x2 = (x * x) / 65535UL; + return (uint32_t)((x2 * (196605UL - 2UL * x)) / 65535UL); + } + if (curveMode == 2U) + { + uint32_t scaled = linear * 64UL; + uint32_t index = scaled / 65535UL; + uint32_t fraction = scaled % 65535UL; + uint32_t first; + uint32_t second; + + if (index >= 64UL) + { + return 65535UL; + } + first = PlsrSineProgressQ16[index]; + second = PlsrSineProgressQ16[index + 1UL]; + return first + (uint32_t)(((uint64_t)(second - first) * fraction) + / 65535UL); + } + return linear; +} + +static uint32_t PlsrRampDurationMs(uint32_t fromHz, uint32_t toHz) +{ + uint32_t gap; + uint32_t baseTimeMs; + uint64_t duration; + + if (fromHz == toHz) + { + return 0UL; + } + gap = (fromHz > toHz) ? (fromHz - toHz) : (toHz - fromHz); + baseTimeMs = (toHz > fromHz) ? PlsrActiveConfig.accelerationTimeMs + : PlsrActiveConfig.decelerationTimeMs; + if (baseTimeMs == 0UL) + { + return 0UL; + } + + duration = ((uint64_t)gap * baseTimeMs + + PlsrActiveConfig.defaultSpeedHz - 1UL) + / PlsrActiveConfig.defaultSpeedHz; + if (duration > 0xFFFFFFFFUL) + { + return 0xFFFFFFFFUL; + } + return (uint32_t)duration; +} + +static void PlsrRampStart(uint32_t fromHz, uint32_t toHz) +{ + PlsrRamp.fromHz = fromHz; + PlsrRamp.toHz = toHz; + PlsrRamp.durationMs = PlsrRampDurationMs(fromHz, toHz); + PlsrRamp.elapsedMs = 0UL; + PlsrRamp.active = (PlsrRamp.durationMs != 0UL) ? 1U : 0U; + + if (toHz > fromHz) + { + PlsrRunStatus = PLSR_STATUS_ACCELERATING; + } + else if (toHz < fromHz) + { + PlsrRunStatus = PLSR_STATUS_DECELERATING; + } + else + { + PlsrRunStatus = PLSR_STATUS_RUNNING; + } +} + +static uint8_t PlsrApplyFrequencyPair(uint32_t requestedFirstFrequencyHz, + uint32_t requestedQueuedFrequencyHz, + uint32_t expectedEpoch) +{ + uint32_t criticalState; + uint32_t actualFirstFrequencyHz; + uint32_t actualQueuedFrequencyHz; + uint32_t hardwareFirstFrequencyHz = requestedFirstFrequencyHz; + uint32_t hardwareQueuedFrequencyHz = requestedQueuedFrequencyHz; + PLSR_HANDOFF_PLAN candidatePlan; + uint8_t haveCandidatePlan = 0U; + + criticalState = PlsrPlatformEnterCritical(); + if ((PlsrSegmentEpoch != expectedEpoch) + || (PlsrBoundaryPending != 0U) || (PlsrRemainingPulses == 0UL)) + { + PlsrPlatformExitCritical(criticalState); + return 1U; + } + + if (hardwareFirstFrequencyHz == 0UL) + { + if (PlsrPulseActive == 0U) + { + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrPlatformExitCritical(criticalState); + return 1U; + } + hardwareFirstFrequencyHz = 1UL; + } + if (hardwareQueuedFrequencyHz == 0UL) + { + hardwareQueuedFrequencyHz = 1UL; + } + + if (PlsrPulseActive != 0U) + { + if (PlsrHandoffPlan.valid != 0U) + { + PlsrPlatformExitCritical(criticalState); + return 1U; + } + PlsrDeferredFrequencyHz = hardwareQueuedFrequencyHz; + PlsrDeferredFrequencyPending = 1U; + PlsrPlatformExitCritical(criticalState); + return 1U; + } + else + { + if (PlsrSelectPreparedHandoffPlan(&candidatePlan) != 0U) + { + hardwareQueuedFrequencyHz = candidatePlan.firstFrequencyHz; + haveCandidatePlan = 1U; + } + if (PlsrPlatformStartPulse((uint8_t)PlsrActiveConfig.pulseOutput, + hardwareFirstFrequencyHz, + hardwareQueuedFrequencyHz, + &actualFirstFrequencyHz, + &actualQueuedFrequencyHz) == 0U) + { + PlsrPlatformExitCritical(criticalState); + return 0U; + } + PlsrPulseActive = 1U; + PlsrCurrentFrequencyHz = actualFirstFrequencyHz; + PlsrHandoffPlan.valid = 0U; + if (haveCandidatePlan != 0U) + { + PlsrHandoffPlan.magnitude = candidatePlan.magnitude; + PlsrHandoffPlan.firstFrequencyHz = actualQueuedFrequencyHz; + PlsrCopyShortProfile(&PlsrHandoffPlan.profile, + &candidatePlan.profile); + PlsrHandoffPlan.nextSegment = candidatePlan.nextSegment; + PlsrHandoffPlan.positive = candidatePlan.positive; + PlsrHandoffPlan.valid = 1U; + } + } + + PlsrQueuedFrequencyHz = actualQueuedFrequencyHz; + PlsrPlatformExitCritical(criticalState); + return 1U; +} + +static uint8_t PlsrApplyFrequency(uint32_t requestedFrequencyHz, + uint32_t expectedEpoch) +{ + return PlsrApplyFrequencyPair(requestedFrequencyHz, + requestedFrequencyHz, + expectedEpoch); +} + +static uint8_t PlsrStopDrainPulseCount(void) +{ + uint8_t deferredPending = PlsrDeferredFrequencyPending; + uint32_t deferredFrequencyHz = PlsrDeferredFrequencyHz; + uint32_t queuedFrequencyHz = PlsrQueuedFrequencyHz; + uint32_t currentFrequencyHz = PlsrCurrentFrequencyHz; + + if ((deferredPending != 0U) + && (deferredFrequencyHz != queuedFrequencyHz)) + { + return 3U; + } + PlsrDeferredFrequencyPending = 0U; + return (currentFrequencyHz == queuedFrequencyHz) ? 1U : 2U; +} + +static uint8_t PlsrCommitDeferredFrequency(uint8_t pulseOutput) +{ + uint32_t requestedFrequencyHz; + uint32_t actualFrequencyHz; + + if (PlsrDeferredFrequencyPending == 0U) + { + return 1U; + } + requestedFrequencyHz = PlsrDeferredFrequencyHz; + PlsrDeferredFrequencyPending = 0U; + if (PlsrPlatformQueueFrequency(pulseOutput, requestedFrequencyHz, + &actualFrequencyHz) == 0U) + { + return 0U; + } + PlsrQueuedFrequencyHz = actualFrequencyHz; + return 1U; +} + +static uint8_t PlsrRampAdvance(uint32_t expectedEpoch) +{ + uint32_t progress; + uint32_t frequency; + uint32_t gap; + uint32_t criticalState; + uint32_t fromHz; + uint32_t toHz; + uint32_t durationMs; + uint32_t elapsedMs; + + criticalState = PlsrPlatformEnterCritical(); + if ((PlsrSegmentEpoch != expectedEpoch) || (PlsrRamp.active == 0U)) + { + PlsrPlatformExitCritical(criticalState); + return 1U; + } + PlsrRamp.elapsedMs++; + fromHz = PlsrRamp.fromHz; + toHz = PlsrRamp.toHz; + durationMs = PlsrRamp.durationMs; + elapsedMs = PlsrRamp.elapsedMs; + PlsrPlatformExitCritical(criticalState); + + progress = PlsrCurveProgressQ16(elapsedMs, + durationMs, + PlsrActiveConfig.curveMode); + if (toHz >= fromHz) + { + gap = toHz - fromHz; + frequency = fromHz + + (uint32_t)(((uint64_t)gap * progress) / 65535UL); + } + else + { + gap = fromHz - toHz; + frequency = fromHz + - (uint32_t)(((uint64_t)gap * progress) / 65535UL); + } + + if (PlsrApplyFrequency(frequency, expectedEpoch) == 0U) + { + return 0U; + } + + criticalState = PlsrPlatformEnterCritical(); + if (PlsrSegmentEpoch != expectedEpoch) + { + PlsrPlatformExitCritical(criticalState); + return 1U; + } + if (elapsedMs >= durationMs) + { + PlsrRamp.active = 0U; + if (PlsrStopRequested == 0U) + { + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + else + { + PlsrStopPulsesRemaining = PlsrStopDrainPulseCount(); + } + } + PlsrPlatformExitCritical(criticalState); + return 1U; +} + +static uint8_t PlsrGetNextSegment(uint8_t *nextSegment) +{ + const PLSR_SEGMENT_CONFIG *segment = + &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; + + if (segment->jumpSegment != 0U) + { + *nextSegment = (uint8_t)segment->jumpSegment; + return 1U; + } + if (PlsrCurrentSegment < PlsrActiveConfig.segmentCount) + { + *nextSegment = (uint8_t)(PlsrCurrentSegment + 1U); + return 1U; + } + return 0U; +} + +static int64_t PlsrSegmentDisplacement(uint8_t segmentNumber, + int32_t referencePosition) +{ + int32_t configured = + PlsrActiveConfig.segments[segmentNumber - 1U].pulses; + + if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE) + { + return (int64_t)configured - (int64_t)referencePosition; + } + return configured; +} + +static uint8_t PlsrPredictNextDirection(uint8_t nextSegment, + uint8_t *positive) +{ + uint32_t criticalState; + uint64_t remaining; + int32_t position; + uint32_t predictedBits; + int32_t predictedPosition; + int64_t displacement; + + criticalState = PlsrPlatformEnterCritical(); + remaining = PlsrRemainingPulses; + position = PlsrPosition; + PlsrPlatformExitCritical(criticalState); + + predictedBits = (uint32_t)position; + if (PlsrCountPositive != 0U) + { + predictedBits += (uint32_t)remaining; + } + else + { + predictedBits -= (uint32_t)remaining; + } + predictedPosition = (int32_t)predictedBits; + displacement = PlsrSegmentDisplacement(nextSegment, predictedPosition); + if (displacement == 0) + { + return 0U; + } + *positive = (displacement > 0) ? 1U : 0U; + return 1U; +} + +static uint64_t PlsrRemainingSnapshot(void) +{ + uint32_t criticalState = PlsrPlatformEnterCritical(); + uint64_t remaining = PlsrRemainingPulses; + PlsrPlatformExitCritical(criticalState); + return remaining; +} + +static uint64_t PlsrRampPulseEstimate(uint32_t fromHz, uint32_t toHz) +{ + uint32_t duration = PlsrRampDurationMs(fromHz, toHz); + uint64_t sum = (uint64_t)fromHz + toHz; + + return (sum * duration + 1999UL) / 2000UL + 2UL; +} + +static uint16_t PlsrShortProfileRampTime(uint32_t fromHz, uint32_t toHz) +{ + if (toHz > fromHz) + { + return PlsrActiveConfig.accelerationTimeMs; + } + if (toHz < fromHz) + { + return PlsrActiveConfig.decelerationTimeMs; + } + return 0U; +} + +static uint64_t PlsrShortProfileRampWeight(uint32_t fromHz, + uint32_t toHz, + uint16_t timeMs) +{ + uint64_t fromSquared = (uint64_t)fromHz * fromHz; + uint64_t toSquared = (uint64_t)toHz * toHz; + uint64_t difference = (fromSquared > toSquared) + ? (fromSquared - toSquared) + : (toSquared - fromSquared); + + return difference * timeMs; +} + +static uint32_t PlsrIntegerSquareRoot(uint64_t value) +{ + uint64_t bit = (uint64_t)1U << 62U; + uint64_t root = 0UL; + + while (bit > value) + { + bit >>= 2U; + } + while (bit != 0UL) + { + if (value >= (root + bit)) + { + value -= root + bit; + root = (root >> 1U) + bit; + } + else + { + root >>= 1U; + } + bit >>= 2U; + } + return (uint32_t)root; +} + +static uint32_t PlsrShortProfilePeak(uint32_t startHz, + uint32_t targetHz, + uint32_t endHz, + uint16_t pulseCount) +{ + uint32_t upperEndpoint = (startHz > endHz) ? startHz : endHz; + uint32_t lowerEndpoint = (startHz < endHz) ? startHz : endHz; + uint16_t entryTime; + uint16_t exitTime; + uint32_t timeSum; + uint64_t weightedEndpoints; + uint64_t availableArea; + uint64_t peakSquared; + uint32_t peakHz; + + if (targetHz > upperEndpoint) + { + entryTime = PlsrShortProfileRampTime(startHz, targetHz); + exitTime = PlsrShortProfileRampTime(targetHz, endHz); + timeSum = (uint32_t)entryTime + exitTime; + if (timeSum == 0UL) + { + return targetHz; + } + weightedEndpoints = ((uint64_t)startHz * startHz * entryTime) + + ((uint64_t)endHz * endHz * exitTime); + availableArea = (uint64_t)2U * pulseCount + * PlsrActiveConfig.defaultSpeedHz * 1000UL; + peakSquared = (availableArea + weightedEndpoints) / timeSum; + peakHz = PlsrIntegerSquareRoot(peakSquared); + if (peakHz < upperEndpoint) + { + peakHz = upperEndpoint; + } + if (peakHz > targetHz) + { + peakHz = targetHz; + } + return peakHz; + } + + if (targetHz < lowerEndpoint) + { + entryTime = PlsrShortProfileRampTime(startHz, targetHz); + exitTime = PlsrShortProfileRampTime(targetHz, endHz); + timeSum = (uint32_t)entryTime + exitTime; + if (timeSum == 0UL) + { + return targetHz; + } + weightedEndpoints = ((uint64_t)startHz * startHz * entryTime) + + ((uint64_t)endHz * endHz * exitTime); + availableArea = (uint64_t)2U * pulseCount + * PlsrActiveConfig.defaultSpeedHz * 1000UL; + if (availableArea >= weightedEndpoints) + { + return targetHz; + } + peakSquared = (weightedEndpoints - availableArea) / timeSum; + peakHz = PlsrIntegerSquareRoot(peakSquared); + if (peakHz < targetHz) + { + peakHz = targetHz; + } + if (peakHz > lowerEndpoint) + { + peakHz = lowerEndpoint; + } + return peakHz; + } + + return targetHz; +} + +static uint64_t PlsrShortProfileRequiredSteps(uint64_t rampWeight) +{ + uint64_t denominator = (uint64_t)2U + * PlsrActiveConfig.defaultSpeedHz * 1000UL; + + if (rampWeight == 0UL) + { + return 0UL; + } + return (rampWeight + denominator - 1UL) / denominator; +} + +static uint32_t PlsrShortProfileReachableFrequency(uint32_t fromHz, + uint32_t towardHz, + uint16_t pulseCount) +{ + uint16_t baseTime = PlsrShortProfileRampTime(fromHz, towardHz); + uint64_t frequencySquared = (uint64_t)fromHz * fromHz; + uint64_t changeSquared; + uint32_t reachableHz; + + if ((baseTime == 0U) || (fromHz == towardHz)) + { + return towardHz; + } + changeSquared = (uint64_t)2U * pulseCount + * PlsrActiveConfig.defaultSpeedHz * 1000UL / baseTime; + if (towardHz > fromHz) + { + reachableHz = PlsrIntegerSquareRoot(frequencySquared + changeSquared); + return (reachableHz > towardHz) ? towardHz : reachableHz; + } + + frequencySquared = (changeSquared >= frequencySquared) + ? 0UL : (frequencySquared - changeSquared); + reachableHz = PlsrIntegerSquareRoot(frequencySquared); + if (((uint64_t)reachableHz * reachableHz) < frequencySquared) + { + reachableHz++; + } + return (reachableHz < towardHz) ? towardHz : reachableHz; +} + +static uint64_t PlsrCurveIntegralQ32(uint64_t progressQ32, + uint16_t curveMode) +{ + uint64_t scaled; + uint32_t index; + uint32_t fraction; + uint64_t first; + uint64_t second; + const uint32_t *table; + + if (progressQ32 >= PLSR_Q32_ONE) + { + return PLSR_Q32_ONE / 2ULL; + } + if (curveMode == 0U) + { + return (progressQ32 * progressQ32) >> 33U; + } + + table = (curveMode == 1U) ? PlsrSmoothIntegralQ24 + : PlsrSineIntegralQ24; + scaled = progressQ32 * 64ULL; + index = (uint32_t)(scaled >> 32U); + fraction = (uint32_t)scaled; + first = (uint64_t)table[index] << 8U; + second = (uint64_t)table[index + 1UL] << 8U; + return first + (((second - first) * fraction) >> 32U); +} + +static uint64_t PlsrRampAreaQ32(uint32_t fromHz, + uint32_t toHz, + uint64_t progressQ32) +{ + int64_t delta = (int64_t)toHz - (int64_t)fromHz; + int64_t area = (int64_t)((uint64_t)fromHz * progressQ32) + + delta * (int64_t)PlsrCurveIntegralQ32( + progressQ32, PlsrActiveConfig.curveMode); + return (uint64_t)area; +} + +static uint32_t PlsrRampInstantFrequency(uint32_t fromHz, + uint32_t toHz, + uint64_t progressQ32) +{ + uint64_t curveProgressQ32; + uint64_t scaled; + uint32_t index; + uint32_t tableSlopeQ32; + uint32_t gap; + const uint32_t *integralTable; + + if (progressQ32 >= PLSR_Q32_ONE) + { + return toHz; + } + if (PlsrActiveConfig.curveMode != 0U) + { + scaled = progressQ32 * 64ULL; + index = (uint32_t)(scaled >> 32U); + integralTable = (PlsrActiveConfig.curveMode == 1U) + ? PlsrSmoothIntegralQ24 + : PlsrSineIntegralQ24; + tableSlopeQ32 = + (integralTable[index + 1UL] - integralTable[index]) << 14U; + curveProgressQ32 = tableSlopeQ32; + } + else + { + curveProgressQ32 = progressQ32; + } + + if (toHz >= fromHz) + { + gap = toHz - fromHz; + return fromHz + + (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U); + } + gap = fromHz - toHz; + return fromHz + - (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U); +} + +static uint64_t PlsrExactRampBoundaryQ32(uint32_t fromHz, + uint32_t toHz, + uint64_t previousBoundaryQ32, + uint64_t targetAreaQ32) +{ + uint64_t lowerQ32 = previousBoundaryQ32; + uint64_t upperQ32 = PLSR_Q32_ONE; + uint64_t middleQ32; + uint32_t iteration; + + /* The Q32 domain is 2^32 units wide, so 32 fixed bisections are exact. */ + for (iteration = 0UL; iteration < 32UL; iteration++) + { + middleQ32 = lowerQ32 + ((upperQ32 - lowerQ32) >> 1U); + if (PlsrRampAreaQ32(fromHz, toHz, middleQ32) < targetAreaQ32) + { + lowerQ32 = middleQ32; + } + else + { + upperQ32 = middleQ32; + } + } + return upperQ32; +} + +static uint32_t PlsrCountLeadingZeros32(uint32_t value) +{ +#if defined(__ICCARM__) + return __CLZ(value); +#else + uint32_t count = 0U; + + if ((value & 0xFFFF0000UL) == 0UL) + { + count += 16U; + value <<= 16U; + } + if ((value & 0xFF000000UL) == 0UL) + { + count += 8U; + value <<= 8U; + } + if ((value & 0xF0000000UL) == 0UL) + { + count += 4U; + value <<= 4U; + } + if ((value & 0xC0000000UL) == 0UL) + { + count += 2U; + value <<= 2U; + } + if ((value & 0x80000000UL) == 0UL) + { + count++; + } + return count; +#endif +} + +static uint32_t PlsrDivideU64Low(uint32_t highWord, + uint32_t lowWord, + uint32_t divisor, + uint32_t *remainder) +{ + const uint32_t halfBase = 0x10000UL; + uint32_t shift = PlsrCountLeadingZeros32(divisor); + uint32_t normalizedDivisor = divisor << shift; + uint32_t divisorHigh = normalizedDivisor >> 16U; + uint32_t divisorLow = normalizedDivisor & 0xFFFFUL; + uint32_t normalizedHigh; + uint32_t normalizedLow = lowWord << shift; + uint32_t lowHigh = normalizedLow >> 16U; + uint32_t lowLow = normalizedLow & 0xFFFFUL; + uint32_t quotientHigh; + uint32_t quotientLow; + uint32_t partialRemainder; + uint32_t middle; + uint32_t normalizedRemainder; + + if (shift == 0U) + { + normalizedHigh = highWord; + } + else + { + normalizedHigh = (highWord << shift) + | (lowWord >> (32U - shift)); + } + + quotientHigh = normalizedHigh / divisorHigh; + partialRemainder = normalizedHigh - quotientHigh * divisorHigh; + while ((quotientHigh >= halfBase) + || (quotientHigh * divisorLow + > (partialRemainder << 16U) + lowHigh)) + { + quotientHigh--; + partialRemainder += divisorHigh; + if (partialRemainder >= halfBase) + { + break; + } + } + + middle = normalizedHigh * halfBase + lowHigh + - quotientHigh * normalizedDivisor; + quotientLow = middle / divisorHigh; + partialRemainder = middle - quotientLow * divisorHigh; + while ((quotientLow >= halfBase) + || (quotientLow * divisorLow + > (partialRemainder << 16U) + lowLow)) + { + quotientLow--; + partialRemainder += divisorHigh; + if (partialRemainder >= halfBase) + { + break; + } + } + + normalizedRemainder = middle * halfBase + lowLow + - quotientLow * normalizedDivisor; + if (remainder != NULL) + { + *remainder = normalizedRemainder >> shift; + } + return quotientHigh * halfBase + quotientLow; +} + +static uint64_t PlsrDivideU64ByU32(uint64_t dividend, + uint32_t divisor, + uint32_t *remainder) +{ + uint32_t highWord = (uint32_t)(dividend >> 32U); + uint32_t lowWord = (uint32_t)dividend; + uint32_t quotientHigh = highWord / divisor; + uint32_t highRemainder = highWord - quotientHigh * divisor; + uint32_t quotientLow = PlsrDivideU64Low(highRemainder, lowWord, + divisor, remainder); + + return ((uint64_t)quotientHigh << 32U) | quotientLow; +} + +static void PlsrPrepareShortProfileBoundaries(PLSR_SHORT_PROFILE *profile) +{ + uint64_t totalAreaQ32; + uint64_t areaStepQ32; + uint64_t targetAreaQ32; + uint32_t areaRemainder; + + if (profile->entryPulses != 0U) + { + totalAreaQ32 = PlsrRampAreaQ32( + profile->startHz, profile->peakHz, PLSR_Q32_ONE); + areaStepQ32 = totalAreaQ32 / profile->entryPulses; + areaRemainder = (uint32_t)(totalAreaQ32 % profile->entryPulses); + profile->entryFirstBoundaryQ32 = PlsrExactRampBoundaryQ32( + profile->startHz, profile->peakHz, 0ULL, areaStepQ32); + if (profile->entryPulses > 1U) + { + targetAreaQ32 = areaStepQ32 * 2ULL + + (((uint64_t)areaRemainder * 2ULL) + / profile->entryPulses); + profile->entrySecondBoundaryQ32 = PlsrExactRampBoundaryQ32( + profile->startHz, profile->peakHz, + profile->entryFirstBoundaryQ32, targetAreaQ32); + } + } + if (profile->exitPulses != 0U) + { + totalAreaQ32 = PlsrRampAreaQ32( + profile->peakHz, profile->endHz, PLSR_Q32_ONE); + areaStepQ32 = totalAreaQ32 / profile->exitPulses; + areaRemainder = (uint32_t)(totalAreaQ32 % profile->exitPulses); + profile->exitFirstBoundaryQ32 = PlsrExactRampBoundaryQ32( + profile->peakHz, profile->endHz, 0ULL, areaStepQ32); + if (profile->exitPulses > 1U) + { + targetAreaQ32 = areaStepQ32 * 2ULL + + (((uint64_t)areaRemainder * 2ULL) + / profile->exitPulses); + profile->exitSecondBoundaryQ32 = PlsrExactRampBoundaryQ32( + profile->peakHz, profile->endHz, + profile->exitFirstBoundaryQ32, targetAreaQ32); + } + } +} + +static uint64_t PlsrRampBoundaryQ32(uint32_t fromHz, + uint32_t toHz, + uint64_t previousBoundaryQ32, + uint64_t targetAreaQ32, + uint64_t predictedStepQ32) +{ + uint64_t currentAreaQ32; + uint64_t candidateQ32; + uint64_t candidateAreaQ32; + uint64_t differenceQ32; + uint64_t correctionQ32; + uint32_t derivativeHz; + + currentAreaQ32 = (previousBoundaryQ32 == 0ULL) + ? 0ULL + : PlsrRampAreaQ32(fromHz, toHz, + previousBoundaryQ32); + if (targetAreaQ32 <= currentAreaQ32) + { + return previousBoundaryQ32 + 1ULL; + } + + if (predictedStepQ32 != 0ULL) + { + if (predictedStepQ32 >= PLSR_Q32_ONE - previousBoundaryQ32) + { + candidateQ32 = PLSR_Q32_ONE; + } + else + { + candidateQ32 = previousBoundaryQ32 + predictedStepQ32; + } + } + else + { + derivativeHz = PlsrRampInstantFrequency(fromHz, toHz, + previousBoundaryQ32); + if (derivativeHz == 0UL) + { + derivativeHz = 1UL; + } + differenceQ32 = targetAreaQ32 - currentAreaQ32; + correctionQ32 = + PlsrDivideU64ByU32(differenceQ32 + derivativeHz - 1UL, + derivativeHz, NULL); + if (correctionQ32 >= PLSR_Q32_ONE - previousBoundaryQ32) + { + candidateQ32 = PLSR_Q32_ONE; + } + else + { + candidateQ32 = previousBoundaryQ32 + correctionQ32; + } + } + + candidateAreaQ32 = PlsrRampAreaQ32(fromHz, toHz, candidateQ32); + derivativeHz = PlsrRampInstantFrequency(fromHz, toHz, candidateQ32); + if (derivativeHz == 0UL) + { + derivativeHz = 1UL; + } + if (candidateAreaQ32 < targetAreaQ32) + { + differenceQ32 = targetAreaQ32 - candidateAreaQ32; + correctionQ32 = + PlsrDivideU64ByU32(differenceQ32 + derivativeHz - 1UL, + derivativeHz, NULL); + if (correctionQ32 >= PLSR_Q32_ONE - candidateQ32) + { + candidateQ32 = PLSR_Q32_ONE; + } + else + { + candidateQ32 += correctionQ32; + } + } + else if (candidateAreaQ32 > targetAreaQ32) + { + differenceQ32 = candidateAreaQ32 - targetAreaQ32; + correctionQ32 = PlsrDivideU64ByU32(differenceQ32, derivativeHz, + NULL); + if (correctionQ32 == 0ULL) + { + correctionQ32 = 1ULL; + } + if (correctionQ32 >= candidateQ32 - previousBoundaryQ32) + { + candidateQ32 = previousBoundaryQ32 + 1ULL; + } + else + { + candidateQ32 -= correctionQ32; + } + } + + /* The corrected phase step seeds the next pulse and keeps ISR work fixed. */ + return candidateQ32; +} + +static uint32_t PlsrRampAverageFrequency(uint64_t areaIncrementQ32, + uint64_t firstBoundaryQ32, + uint64_t secondBoundaryQ32) +{ + uint64_t denominator; + uint64_t frequencyHz; + + if (secondBoundaryQ32 <= firstBoundaryQ32) + { + return 1UL; + } + denominator = secondBoundaryQ32 - firstBoundaryQ32; + if (denominator == PLSR_Q32_ONE) + { + frequencyHz = (areaIncrementQ32 + denominator / 2ULL) >> 32U; + } + else + { + frequencyHz = PlsrDivideU64ByU32( + areaIncrementQ32 + denominator / 2ULL, + (uint32_t)denominator, NULL); + } + if (frequencyHz == 0UL) + { + return 1UL; + } + if (frequencyHz > PLSR_FREQUENCY_MAX_HZ) + { + return PLSR_FREQUENCY_MAX_HZ; + } + return (uint32_t)frequencyHz; +} + +static uint8_t PlsrPrepareShortProfile(PLSR_SHORT_PROFILE *profile, + uint8_t segmentNumber, + uint32_t startFrequencyHz, + uint32_t targetFrequencyHz, + uint64_t pulseCount) +{ + const PLSR_SEGMENT_CONFIG *segment; + uint32_t endFrequencyHz; + uint32_t peakFrequencyHz; + uint16_t entryTime; + uint16_t exitTime; + uint16_t totalPulses; + uint64_t entryWeight; + uint64_t exitWeight; + uint64_t entryRequired; + uint64_t exitRequired; + uint64_t totalWeight; + uint64_t scaledEntry; + + uint64_t durationWeight; + uint64_t singleFrequencyHz; + uint16_t directTime; + uint64_t directWeight; + uint64_t directRequired; + + (void)memset(profile, 0, sizeof(*profile)); + if ((pulseCount == 0UL) + || (pulseCount > PLSR_SHORT_PROFILE_MAX_PULSES) + || (segmentNumber != PlsrActiveConfig.segmentCount)) + { + return 0U; + } + + segment = &PlsrActiveConfig.segments[segmentNumber - 1U]; + if (segment->jumpSegment != 0U) + { + return 0U; + } + + endFrequencyHz = PlsrActiveConfig.stopSpeedHz; + totalPulses = (uint16_t)pulseCount; + directTime = PlsrShortProfileRampTime(startFrequencyHz, endFrequencyHz); + directWeight = PlsrShortProfileRampWeight(startFrequencyHz, + endFrequencyHz, + directTime); + directRequired = PlsrShortProfileRequiredSteps(directWeight); + if (directRequired > totalPulses) + { + profile->startHz = startFrequencyHz; + profile->peakHz = PlsrShortProfileReachableFrequency( + startFrequencyHz, endFrequencyHz, totalPulses); + profile->endHz = profile->peakHz; + profile->pulseCount = totalPulses; + profile->entryPulses = totalPulses; + profile->active = 1U; + PlsrPrepareShortProfileBoundaries(profile); + return 1U; + } + + peakFrequencyHz = PlsrShortProfilePeak(startFrequencyHz, + targetFrequencyHz, + endFrequencyHz, + (uint16_t)pulseCount); + entryTime = PlsrShortProfileRampTime(startFrequencyHz, peakFrequencyHz); + exitTime = PlsrShortProfileRampTime(peakFrequencyHz, endFrequencyHz); + entryWeight = PlsrShortProfileRampWeight(startFrequencyHz, + peakFrequencyHz, + entryTime); + exitWeight = PlsrShortProfileRampWeight(peakFrequencyHz, + endFrequencyHz, + exitTime); + entryRequired = PlsrShortProfileRequiredSteps(entryWeight); + exitRequired = PlsrShortProfileRequiredSteps(exitWeight); + if ((entryRequired == 0UL) && (exitRequired == 0UL)) + { + return 0U; + } + + profile->startHz = startFrequencyHz; + profile->peakHz = peakFrequencyHz; + profile->endHz = endFrequencyHz; + profile->pulseCount = totalPulses; + + if (totalPulses == 1U) + { + durationWeight = + (uint64_t)((peakFrequencyHz > startFrequencyHz) + ? (peakFrequencyHz - startFrequencyHz) + : (startFrequencyHz - peakFrequencyHz)) * entryTime + + (uint64_t)((peakFrequencyHz > endFrequencyHz) + ? (peakFrequencyHz - endFrequencyHz) + : (endFrequencyHz - peakFrequencyHz)) * exitTime; + if (durationWeight == 0UL) + { + return 0U; + } + singleFrequencyHz = + ((uint64_t)PlsrActiveConfig.defaultSpeedHz * 1000UL + + durationWeight / 2UL) / durationWeight; + if (singleFrequencyHz == 0UL) + { + singleFrequencyHz = 1UL; + } + if (singleFrequencyHz > PLSR_FREQUENCY_MAX_HZ) + { + singleFrequencyHz = PLSR_FREQUENCY_MAX_HZ; + } + profile->peakHz = (uint32_t)singleFrequencyHz; + profile->steadyPulses = 1U; + profile->active = 1U; + return 1U; + } + + if ((exitRequired == 0UL) && (peakFrequencyHz != endFrequencyHz)) + { + exitRequired = 1UL; + exitWeight = (uint64_t)2U + * PlsrActiveConfig.defaultSpeedHz * 1000UL; + } + + if ((entryRequired + exitRequired) <= totalPulses) + { + profile->entryPulses = (uint16_t)entryRequired; + profile->exitPulses = (uint16_t)exitRequired; + profile->steadyPulses = + (uint16_t)(totalPulses - profile->entryPulses + - profile->exitPulses); + } + else if (entryRequired == 0UL) + { + profile->exitPulses = totalPulses; + } + else if (exitRequired == 0UL) + { + profile->entryPulses = totalPulses; + } + else + { + totalWeight = entryWeight + exitWeight; + /* Peak planning bounds both weights by this move's pulse budget. */ + scaledEntry = ((uint64_t)totalPulses * entryWeight + + totalWeight / 2UL) / totalWeight; + if (scaledEntry == 0UL) + { + scaledEntry = 1UL; + } + if (scaledEntry >= totalPulses) + { + scaledEntry = totalPulses - 1U; + } + profile->entryPulses = (uint16_t)scaledEntry; + profile->exitPulses = + (uint16_t)(totalPulses - profile->entryPulses); + } + + profile->active = 1U; + PlsrPrepareShortProfileBoundaries(profile); + return 1U; +} + +static uint32_t PlsrShortProfileTakeFrequency(PLSR_SHORT_PROFILE *profile) +{ + uint16_t period = profile->nextPeriod; + uint16_t relativePeriod; + uint16_t rampPulseCount; + uint32_t fromHz; + uint32_t toHz; + uint64_t firstBoundaryQ32; + uint64_t secondBoundaryQ32; + uint64_t previousTargetAreaQ32; + uint64_t areaIncrementQ32; + uint64_t firstCachedBoundaryQ32; + uint64_t secondCachedBoundaryQ32; + uint32_t frequencyHz; + + if ((profile->active == 0U) || (period >= profile->pulseCount)) + { + return (profile->endHz == 0UL) ? 1UL : profile->endHz; + } + profile->nextPeriod = (uint16_t)(period + 1U); + + if (period < profile->entryPulses) + { + relativePeriod = period; + rampPulseCount = profile->entryPulses; + fromHz = profile->startHz; + toHz = profile->peakHz; + firstCachedBoundaryQ32 = profile->entryFirstBoundaryQ32; + secondCachedBoundaryQ32 = profile->entrySecondBoundaryQ32; + firstBoundaryQ32 = (relativePeriod == 0U) + ? 0ULL : profile->rampBoundaryQ32; + } + else if (period < (uint16_t)(profile->entryPulses + + profile->steadyPulses)) + { + profile->lastRampFrequencyHz = 0UL; + return (profile->peakHz == 0UL) ? 1UL : profile->peakHz; + } + else + { + relativePeriod = + (uint16_t)(period - profile->entryPulses + - profile->steadyPulses); + rampPulseCount = profile->exitPulses; + fromHz = profile->peakHz; + toHz = profile->endHz; + firstCachedBoundaryQ32 = profile->exitFirstBoundaryQ32; + secondCachedBoundaryQ32 = profile->exitSecondBoundaryQ32; + firstBoundaryQ32 = (relativePeriod == 0U) + ? 0ULL : profile->rampBoundaryQ32; + } + + if (relativePeriod == 0U) + { + profile->rampTotalAreaQ32 = + PlsrRampAreaQ32(fromHz, toHz, PLSR_Q32_ONE); + profile->rampAreaStepQ32 = + PlsrDivideU64ByU32(profile->rampTotalAreaQ32, + rampPulseCount, + &profile->rampAreaRemainder); + profile->rampRemainderAccumulator = 0UL; + profile->rampTargetAreaQ32 = 0ULL; + profile->rampBoundaryQ32 = 0ULL; + profile->lastRampPhaseStepQ32 = 0ULL; + profile->lastRampFrequencyHz = 0UL; + } + previousTargetAreaQ32 = profile->rampTargetAreaQ32; + profile->rampTargetAreaQ32 += profile->rampAreaStepQ32; + profile->rampRemainderAccumulator += profile->rampAreaRemainder; + if (profile->rampRemainderAccumulator >= rampPulseCount) + { + profile->rampTargetAreaQ32++; + profile->rampRemainderAccumulator -= rampPulseCount; + } + if ((uint16_t)(relativePeriod + 1U) >= rampPulseCount) + { + profile->rampTargetAreaQ32 = profile->rampTotalAreaQ32; + secondBoundaryQ32 = PLSR_Q32_ONE; + } + else if (relativePeriod == 0U) + { + secondBoundaryQ32 = firstCachedBoundaryQ32; + } + else if (relativePeriod == 1U) + { + secondBoundaryQ32 = secondCachedBoundaryQ32; + } + else + { + secondBoundaryQ32 = PlsrRampBoundaryQ32( + fromHz, toHz, firstBoundaryQ32, + profile->rampTargetAreaQ32, + (relativePeriod < 2U) ? 0ULL + : profile->lastRampPhaseStepQ32); + } + areaIncrementQ32 = + profile->rampTargetAreaQ32 - previousTargetAreaQ32; + profile->rampBoundaryQ32 = secondBoundaryQ32; + profile->lastRampPhaseStepQ32 = + secondBoundaryQ32 - firstBoundaryQ32; + frequencyHz = PlsrRampAverageFrequency(areaIncrementQ32, firstBoundaryQ32, + secondBoundaryQ32); + if ((profile->lastRampFrequencyHz != 0UL) + && (((toHz > fromHz) + && (frequencyHz < profile->lastRampFrequencyHz)) + || ((toHz < fromHz) + && (frequencyHz > profile->lastRampFrequencyHz)))) + { + frequencyHz = profile->lastRampFrequencyHz; + } + profile->lastRampFrequencyHz = frequencyHz; + return frequencyHz; +} + +static void PlsrCopyShortProfile(PLSR_SHORT_PROFILE *destination, + const PLSR_SHORT_PROFILE *source) +{ + destination->active = 0U; + destination->startHz = source->startHz; + destination->peakHz = source->peakHz; + destination->endHz = source->endHz; + destination->pulseCount = source->pulseCount; + destination->entryPulses = source->entryPulses; + destination->steadyPulses = source->steadyPulses; + destination->exitPulses = source->exitPulses; + destination->nextPeriod = source->nextPeriod; + destination->rampBoundaryQ32 = source->rampBoundaryQ32; + destination->rampTotalAreaQ32 = source->rampTotalAreaQ32; + destination->rampTargetAreaQ32 = source->rampTargetAreaQ32; + destination->rampAreaStepQ32 = source->rampAreaStepQ32; + destination->rampAreaRemainder = source->rampAreaRemainder; + destination->rampRemainderAccumulator = + source->rampRemainderAccumulator; + destination->lastRampPhaseStepQ32 = source->lastRampPhaseStepQ32; + destination->entryFirstBoundaryQ32 = source->entryFirstBoundaryQ32; + destination->entrySecondBoundaryQ32 = source->entrySecondBoundaryQ32; + destination->exitFirstBoundaryQ32 = source->exitFirstBoundaryQ32; + destination->exitSecondBoundaryQ32 = source->exitSecondBoundaryQ32; + destination->lastRampFrequencyHz = source->lastRampFrequencyHz; + destination->active = source->active; +} + +static void PlsrInvalidateHandoffPlans(void) +{ + uint8_t bank; + uint8_t index; + uint32_t criticalState = PlsrPlatformEnterCritical(); + + PlsrHandoffPlan.valid = 0U; + for (bank = 0U; bank < 2U; bank++) + { + for (index = 0U; index < PLSR_SEGMENT_COUNT_MAX; index++) + { + PlsrPreparedHandoffPlans[bank][index].valid = 0U; + } + } + PlsrPlatformExitCritical(criticalState); +} + +static uint8_t PlsrAdvanceShortProfile(uint8_t pulseOutput) +{ + uint32_t requestedFrequencyHz; + uint32_t actualFrequencyHz; + + if ((PlsrShortProfile.active == 0U) + || (PlsrShortProfile.nextPeriod >= PlsrShortProfile.pulseCount)) + { + return 1U; + } + + requestedFrequencyHz = + PlsrShortProfileTakeFrequency(&PlsrShortProfile); + PlsrDeferredFrequencyPending = 0U; + if (PlsrPlatformQueueFrequency(pulseOutput, + requestedFrequencyHz, + &actualFrequencyHz) == 0U) + { + return 0U; + } + PlsrQueuedFrequencyHz = actualFrequencyHz; + if (actualFrequencyHz > PlsrCurrentFrequencyHz) + { + PlsrRunStatus = PLSR_STATUS_ACCELERATING; + } + else if (actualFrequencyHz < PlsrCurrentFrequencyHz) + { + PlsrRunStatus = PLSR_STATUS_DECELERATING; + } + else + { + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + return 1U; +} + +static void PlsrMaybePlanBoundaryRamp(uint32_t expectedEpoch) +{ + const PLSR_SEGMENT_CONFIG *segment; + uint8_t nextSegment; + uint8_t nextPositive; + uint8_t hasNext; + uint32_t targetHz; + uint32_t criticalState; + uint64_t estimate; + uint64_t remaining; + + if ((PlsrPulseActive == 0U) || (PlsrBoundaryRampStarted != 0U) + || (PlsrShortProfile.active != 0U) + || (PlsrHandoffPlan.valid != 0U) + || (PlsrStopRequested != 0U) || (PlsrCurrentSegment == 0U)) + { + return; + } + + segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; + hasNext = PlsrGetNextSegment(&nextSegment); + targetHz = PlsrActiveConfig.stopSpeedHz; + + if ((PlsrActiveConfig.sendMode == PLSR_SEND_SUBSEQUENT) + && (hasNext != 0U) + && (segment->waitType == PLSR_EXT_OR_COMPLETE)) + { + if ((PlsrPredictNextDirection(nextSegment, &nextPositive) != 0U) + && (nextPositive == PlsrCountPositive)) + { + targetHz = PlsrActiveConfig.segments[nextSegment - 1U].frequencyHz; + } + } + + estimate = PlsrRampPulseEstimate(PlsrCurrentFrequencyHz, targetHz); + criticalState = PlsrPlatformEnterCritical(); + remaining = PlsrRemainingPulses; + if ((PlsrSegmentEpoch != expectedEpoch) + || (PlsrBoundaryPending != 0U) + || (PlsrPulseActive == 0U) + || (PlsrBoundaryRampStarted != 0U) + || (PlsrShortProfile.active != 0U) + || (PlsrHandoffPlan.valid != 0U) + || (PlsrStopRequested != 0U)) + { + PlsrPlatformExitCritical(criticalState); + return; + } + if (remaining > estimate) + { + PlsrPlatformExitCritical(criticalState); + return; + } + + PlsrBoundaryRampStarted = 1U; + PlsrRampStart(PlsrCurrentFrequencyHz, targetHz); + PlsrPlatformExitCritical(criticalState); + if (PlsrRamp.active == 0U) + { + (void)PlsrApplyFrequency(targetHz, expectedEpoch); + } +} + +static uint8_t PlsrBeginSegmentOutput(uint32_t startFrequencyHz) +{ + uint32_t targetFrequencyHz = + PlsrActiveConfig.segments[PlsrCurrentSegment - 1U].frequencyHz; + uint32_t firstFrequencyHz; + uint32_t secondFrequencyHz; + + PlsrSegmentClockStarted = 1U; + PlsrSegmentElapsedMs = 0UL; + if (PlsrPrepareShortProfile(&PlsrShortProfile, + PlsrCurrentSegment, startFrequencyHz, + targetFrequencyHz, + PlsrRemainingSnapshot()) != 0U) + { + PlsrRamp.active = 0U; + firstFrequencyHz = + PlsrShortProfileTakeFrequency(&PlsrShortProfile); + secondFrequencyHz = + (PlsrShortProfile.nextPeriod < PlsrShortProfile.pulseCount) + ? PlsrShortProfileTakeFrequency(&PlsrShortProfile) + : firstFrequencyHz; + if (secondFrequencyHz > firstFrequencyHz) + { + PlsrRunStatus = PLSR_STATUS_ACCELERATING; + } + else if (secondFrequencyHz < firstFrequencyHz) + { + PlsrRunStatus = PLSR_STATUS_DECELERATING; + } + else + { + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + if (PlsrApplyFrequencyPair(firstFrequencyHz, + secondFrequencyHz, + PlsrSegmentEpoch) == 0U) + { + PlsrShortProfile.active = 0U; + return 0U; + } + return 1U; + } + + PlsrRampStart(startFrequencyHz, targetFrequencyHz); + + if (PlsrRamp.active == 0U) + { + if (PlsrApplyFrequency(targetFrequencyHz, PlsrSegmentEpoch) == 0U) + { + return 0U; + } + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + else if ((startFrequencyHz != 0UL) + && (PlsrApplyFrequency(startFrequencyHz, + PlsrSegmentEpoch) == 0U)) + { + return 0U; + } + return 1U; +} + +static uint8_t PlsrStartSegment(uint8_t segmentNumber, + uint8_t allowCarry, + uint32_t carryFrequencyHz) +{ + uint32_t criticalState; + int32_t position; + int64_t displacement; + uint64_t magnitude; + uint8_t positive; + uint8_t directionLevel; + uint8_t directionChanged; + uint32_t startFrequencyHz; + + if ((segmentNumber == 0U) + || (segmentNumber > PlsrActiveConfig.segmentCount)) + { + return 0U; + } + + criticalState = PlsrPlatformEnterCritical(); + position = PlsrPosition; + PlsrPlatformExitCritical(criticalState); + displacement = PlsrSegmentDisplacement(segmentNumber, position); + positive = (displacement >= 0) ? 1U : 0U; + magnitude = (displacement < 0) ? (uint64_t)(-displacement) + : (uint64_t)displacement; + + PlsrSegmentEpoch++; + PlsrCurrentSegment = segmentNumber; + PlsrSegmentClockStarted = 0U; + PlsrSegmentElapsedMs = 0UL; + PlsrWaitElapsedMs = 0UL; + PlsrBoundaryRampStarted = 0U; + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrCutRequested = 0U; + PlsrStopPulsesRemaining = 0U; + PlsrFrequencyUpdatePending = 0U; + PlsrDeferredFrequencyPending = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrHandoffPlan.valid = 0U; + PlsrShortProfile.active = 0U; + PlsrShortProfile.nextPeriod = 0U; + PlsrDirectionDelayActive = 0U; + PlsrDirectionDelayRemainingMs = 0U; + PlsrExtEdgePending = 0U; + PlsrExtPreviousLevel = + PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); + + criticalState = PlsrPlatformEnterCritical(); + PlsrRemainingPulses = magnitude; + PlsrCountPositive = positive; + PlsrPlatformExitCritical(criticalState); + + if ((PlsrActiveConfig.segments[segmentNumber - 1U].waitType + == PLSR_ACT_TIME) + && (PlsrActiveConfig.segments[segmentNumber - 1U].actTimeMs == 0U)) + { + PlsrSegmentClockStarted = 1U; + PlsrRunStatus = PLSR_STATUS_RUNNING; + PlsrBoundaryFrequencyHz = (allowCarry != 0U) ? carryFrequencyHz : 0UL; + PlsrBoundaryWasCut = 1U; + PlsrBoundaryPending = 1U; + return 1U; + } + + if (magnitude == 0UL) + { + PlsrSegmentClockStarted = 1U; + PlsrRunStatus = PLSR_STATUS_RUNNING; + PlsrBoundaryFrequencyHz = 0UL; + PlsrBoundaryPending = 1U; + return 1U; + } + + directionLevel = positive; + if (PlsrActiveConfig.directionNegativeLogic != 0U) + { + directionLevel ^= 1U; + } + directionChanged = ((PlsrLastDirectionValid == 0U) + || (PlsrLastDirectionOutput + != (uint8_t)PlsrActiveConfig.directionOutput) + || (PlsrLastDirectionLevel != directionLevel)) ? 1U : 0U; + + if (PlsrPlatformPrepare((uint8_t)PlsrActiveConfig.pulseOutput, + (uint8_t)PlsrActiveConfig.directionOutput, + directionLevel) == 0U) + { + return 0U; + } + PlsrLastDirectionValid = 1U; + PlsrLastDirectionOutput = (uint8_t)PlsrActiveConfig.directionOutput; + PlsrLastDirectionLevel = directionLevel; + + if ((allowCarry != 0U) && (directionChanged == 0U) + && (carryFrequencyHz != 0UL)) + { + startFrequencyHz = carryFrequencyHz; + } + else + { + startFrequencyHz = PlsrActiveConfig.startSpeedHz; + } + + if ((directionChanged != 0U) + && (PlsrActiveConfig.directionDelayMs != 0U)) + { + PlsrDirectionDelayActive = 1U; + PlsrDirectionDelayRemainingMs = PlsrActiveConfig.directionDelayMs; + PlsrRunStatus = PLSR_STATUS_ACCELERATING; + return 1U; + } + + PlsrDirectionDelayActive = 0U; + return PlsrBeginSegmentOutput(startFrequencyHz); +} + +static void PlsrMarkPersistenceDirty(uint16_t delayMs) +{ + PlsrPersistenceDirty = 1U; + PlsrPersistenceDelayMs = delayMs; +} + +static void PlsrCheckpointPosition(uint8_t wasBusy) +{ + uint32_t criticalState; + int32_t position; + uint8_t positionValid; + + criticalState = PlsrPlatformEnterCritical(); + position = PlsrPosition; + positionValid = PlsrPositionValid; + PlsrPositionCheckpointDirty = 0U; + PlsrPlatformExitCritical(criticalState); + PlsrPlatformCheckpointPosition(position, positionValid, wasBusy); + PlsrPositionCheckpointElapsedMs = 0U; +} + +static void PlsrPollPositionCheckpoint(void) +{ + if (PlsrPositionCheckpointDirty == 0U) + { + PlsrPositionCheckpointElapsedMs = 0U; + return; + } + if (PlsrPositionCheckpointElapsedMs < PLSR_POSITION_CHECKPOINT_MS) + { + PlsrPositionCheckpointElapsedMs++; + } + if (PlsrPositionCheckpointElapsedMs >= PLSR_POSITION_CHECKPOINT_MS) + { + PlsrCheckpointPosition(1U); + } +} + +static void PlsrFinishCompleted(void) +{ + PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); + PlsrRemainingPulses = 0UL; + PlsrPulseActive = 0U; + PlsrCutRequested = 0U; + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrCurrentSegment = 0U; + PlsrSegmentClockStarted = 0U; + PlsrDirectionDelayActive = 0U; + PlsrDirectionDelayRemainingMs = 0U; + PlsrExtEdgePending = 0U; + PlsrStopRequested = 0U; + PlsrStopPulsesRemaining = 0U; + PlsrSeamlessHandoffPending = 0U; + PlsrDeferredFrequencyPending = 0U; + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrTimerErrorPending = 0U; + PlsrRamp.active = 0U; + PlsrInvalidateHandoffPlans(); + PlsrRunStatus = PLSR_STATUS_COMPLETED; + PlsrError = PLSR_ERROR_NONE; + PlsrCheckpointPosition(0U); + PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); +} + +static void PlsrFinishStopped(void) +{ + PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); + PlsrRemainingPulses = 0UL; + PlsrPulseActive = 0U; + PlsrCutRequested = 0U; + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrCurrentSegment = 0U; + PlsrSegmentClockStarted = 0U; + PlsrDirectionDelayActive = 0U; + PlsrDirectionDelayRemainingMs = 0U; + PlsrExtEdgePending = 0U; + PlsrStopRequested = 0U; + PlsrStopPulsesRemaining = 0U; + PlsrSeamlessHandoffPending = 0U; + PlsrDeferredFrequencyPending = 0U; + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrTimerErrorPending = 0U; + PlsrRamp.active = 0U; + PlsrInvalidateHandoffPlans(); + PlsrRunStatus = PLSR_STATUS_STOPPED; + PlsrError = PLSR_ERROR_NONE; + PlsrCheckpointPosition(0U); + PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); +} + +static void PlsrEnterError(PLSR_ERROR error) +{ + PlsrPlatformStopPulse((uint8_t)PlsrActiveConfig.pulseOutput); + PlsrRemainingPulses = 0UL; + PlsrPulseActive = 0U; + PlsrCutRequested = 0U; + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrCurrentSegment = 0U; + PlsrSegmentClockStarted = 0U; + PlsrDirectionDelayActive = 0U; + PlsrDirectionDelayRemainingMs = 0U; + PlsrExtEdgePending = 0U; + PlsrStopRequested = 0U; + PlsrStopPulsesRemaining = 0U; + PlsrSeamlessHandoffPending = 0U; + PlsrDeferredFrequencyPending = 0U; + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrTimerErrorPending = 0U; + PlsrRamp.active = 0U; + PlsrInvalidateHandoffPlans(); + PlsrRunStatus = PLSR_STATUS_ERROR; + PlsrError = error; + PlsrCheckpointPosition(0U); + PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); +} + +static void PlsrTransitionToNext(uint8_t allowCarry) +{ + uint8_t nextSegment; + uint32_t carryFrequencyHz = PlsrBoundaryFrequencyHz; + + if (PlsrGetNextSegment(&nextSegment) == 0U) + { + PlsrFinishCompleted(); + return; + } + + if (PlsrStartSegment(nextSegment, allowCarry, carryFrequencyHz) == 0U) + { + PlsrEnterError(PLSR_ERROR_INVALID_RESOURCE); + } +} + +static uint8_t PlsrBuildHandoffPlan(uint8_t sourceSegment, + const PLSR_CONFIG *frequencyConfig, + PLSR_HANDOFF_PLAN *plan) +{ + const PLSR_SEGMENT_CONFIG *segment; + uint8_t nextSegment; + int64_t displacement; + uint8_t positive; + + plan->valid = 0U; + if ((PlsrActiveConfig.sendMode != PLSR_SEND_SUBSEQUENT) + || (sourceSegment == 0U) + || (sourceSegment > PlsrActiveConfig.segmentCount)) + { + return 0U; + } + + segment = &PlsrActiveConfig.segments[sourceSegment - 1U]; + if (segment->waitType != PLSR_EXT_OR_COMPLETE) + { + return 0U; + } + if (segment->jumpSegment != 0U) + { + nextSegment = (uint8_t)segment->jumpSegment; + } + else if (sourceSegment < PlsrActiveConfig.segmentCount) + { + nextSegment = (uint8_t)(sourceSegment + 1U); + } + else + { + return 0U; + } + + if (PlsrActiveConfig.positionMode == PLSR_POSITION_ABSOLUTE) + { + displacement = (int64_t)PlsrActiveConfig.segments[nextSegment - 1U].pulses + - (int64_t)segment->pulses; + } + else + { + displacement = PlsrActiveConfig.segments[nextSegment - 1U].pulses; + } + if (displacement == 0) + { + return 0U; + } + positive = (displacement > 0) ? 1U : 0U; + + plan->magnitude = (displacement < 0) ? (uint64_t)(-displacement) + : (uint64_t)displacement; + plan->firstFrequencyHz = + frequencyConfig->segments[nextSegment - 1U].frequencyHz; + if (PlsrPrepareShortProfile(&plan->profile, nextSegment, + plan->firstFrequencyHz, + plan->firstFrequencyHz, + plan->magnitude) != 0U) + { + plan->firstFrequencyHz = + PlsrShortProfileTakeFrequency(&plan->profile); + } + plan->nextSegment = nextSegment; + plan->positive = positive; + plan->valid = 1U; + return 1U; +} + +static uint8_t PlsrBuildHandoffPlanBank( + const PLSR_CONFIG *frequencyConfig) +{ + uint8_t buildBank = (uint8_t)(PlsrPreparedHandoffBank ^ 1U); + uint8_t sourceSegment; + PLSR_HANDOFF_PLAN *destination; + + for (sourceSegment = 0U; + sourceSegment < PLSR_SEGMENT_COUNT_MAX; + sourceSegment++) + { + PlsrPreparedHandoffPlans[buildBank][sourceSegment].valid = 0U; + } + for (sourceSegment = 1U; + sourceSegment <= PlsrActiveConfig.segmentCount; + sourceSegment++) + { + destination = + &PlsrPreparedHandoffPlans[buildBank][sourceSegment - 1U]; + (void)PlsrBuildHandoffPlan(sourceSegment, frequencyConfig, + destination); + } + return buildBank; +} + +static uint8_t PlsrSelectPreparedHandoffPlan(PLSR_HANDOFF_PLAN *plan) +{ + const PLSR_HANDOFF_PLAN *prepared; + uint8_t preparedBank; + uint8_t currentSegment; + + plan->valid = 0U; + currentSegment = PlsrCurrentSegment; + if ((PlsrRemainingPulses != 1UL) + || (PlsrActiveConfig.sendMode != PLSR_SEND_SUBSEQUENT) + || (PlsrStopRequested != 0U) + || (PlsrCountOverflowPending != 0U) + || (PlsrCutRequested != 0U) + || (currentSegment == 0U) + || (currentSegment > PlsrActiveConfig.segmentCount)) + { + return 0U; + } + preparedBank = PlsrPreparedHandoffBank; + prepared = &PlsrPreparedHandoffPlans[preparedBank][currentSegment - 1U]; + if ((prepared->valid == 0U) + || (prepared->positive != PlsrCountPositive)) + { + return 0U; + } + + plan->magnitude = prepared->magnitude; + plan->firstFrequencyHz = prepared->firstFrequencyHz; + PlsrCopyShortProfile(&plan->profile, &prepared->profile); + plan->nextSegment = prepared->nextSegment; + plan->positive = prepared->positive; + plan->valid = 1U; + return 1U; +} + +static uint8_t PlsrPrimeHandoff(void) +{ + PLSR_HANDOFF_PLAN candidatePlan; + uint32_t actualFrequencyHz; + + PlsrHandoffPlan.valid = 0U; + if (PlsrSelectPreparedHandoffPlan(&candidatePlan) == 0U) + { + return 0U; + } + PlsrDeferredFrequencyPending = 0U; + if (PlsrPlatformQueueFrequency( + (uint8_t)PlsrActiveConfig.pulseOutput, + candidatePlan.firstFrequencyHz, &actualFrequencyHz) == 0U) + { + PlsrTimerErrorPending = 1U; + return 0U; + } + + PlsrQueuedFrequencyHz = actualFrequencyHz; + PlsrHandoffPlan.magnitude = candidatePlan.magnitude; + PlsrHandoffPlan.firstFrequencyHz = actualFrequencyHz; + PlsrCopyShortProfile(&PlsrHandoffPlan.profile, + &candidatePlan.profile); + PlsrHandoffPlan.nextSegment = candidatePlan.nextSegment; + PlsrHandoffPlan.positive = candidatePlan.positive; + PlsrHandoffPlan.valid = 1U; + return 1U; +} + +static uint8_t PlsrTrySubsequentHandoff(void) +{ + uint32_t requestedQueuedFrequencyHz; + uint32_t actualQueuedFrequencyHz; + uint32_t currentFrequencyHz; + uint32_t queuedFrequencyHz; + uint8_t nextSegment = PlsrHandoffPlan.nextSegment; + uint64_t magnitude = PlsrHandoffPlan.magnitude; + uint8_t positive = PlsrHandoffPlan.positive; + + if ((PlsrHandoffPlan.valid == 0U) + || (PlsrActiveConfig.sendMode != PLSR_SEND_SUBSEQUENT) + || (PlsrStopRequested != 0U) + || (PlsrCountOverflowPending != 0U) + || (PlsrCutRequested != 0U) + || (PlsrCurrentFrequencyHz != PlsrHandoffPlan.firstFrequencyHz) + || (nextSegment == 0U) + || (nextSegment > PlsrActiveConfig.segmentCount)) + { + return 0U; + } + + PlsrCopyShortProfile(&PlsrShortProfile, + &PlsrHandoffPlan.profile); + requestedQueuedFrequencyHz = PlsrCurrentFrequencyHz; + if ((PlsrShortProfile.active != 0U) + && (PlsrShortProfile.nextPeriod < PlsrShortProfile.pulseCount)) + { + requestedQueuedFrequencyHz = + PlsrShortProfileTakeFrequency(&PlsrShortProfile); + } + PlsrDeferredFrequencyPending = 0U; + if (PlsrPlatformQueueFrequency( + (uint8_t)PlsrActiveConfig.pulseOutput, + requestedQueuedFrequencyHz, &actualQueuedFrequencyHz) == 0U) + { + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrTimerErrorPending = 1U; + return 0U; + } + + PlsrQueuedFrequencyHz = actualQueuedFrequencyHz; + PlsrSegmentEpoch++; + PlsrCurrentSegment = nextSegment; + PlsrRemainingPulses = magnitude; + PlsrCountPositive = positive; + PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz; + PlsrSegmentClockStarted = 1U; + PlsrSegmentElapsedMs = 0UL; + PlsrWaitElapsedMs = 0UL; + PlsrBoundaryRampStarted = 0U; + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrCutRequested = 0U; + PlsrFrequencyUpdatePending = 0U; + PlsrDeferredFrequencyPending = 0U; + PlsrExtEdgePending = 0U; + PlsrExtPreviousLevel = + PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); + PlsrSeamlessHandoffPending = 1U; + PlsrHandoffPlan.valid = 0U; + if (PlsrRemainingPulses == 1UL) + { + (void)PlsrPrimeHandoff(); + } + queuedFrequencyHz = PlsrQueuedFrequencyHz; + currentFrequencyHz = PlsrCurrentFrequencyHz; + if (PlsrShortProfile.active != 0U) + { + if (queuedFrequencyHz > currentFrequencyHz) + { + PlsrRunStatus = PLSR_STATUS_ACCELERATING; + } + else if (queuedFrequencyHz < currentFrequencyHz) + { + PlsrRunStatus = PLSR_STATUS_DECELERATING; + } + else + { + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + } + else + { + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + return 1U; +} + +static void PlsrHandleBoundary(uint8_t extEdge) +{ + const PLSR_SEGMENT_CONFIG *segment; + uint8_t wasCut = PlsrBoundaryWasCut; + + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrPulseActive = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrDeferredFrequencyPending = 0U; + PlsrCheckpointPosition(1U); + + if (PlsrTimerErrorPending != 0U) + { + PlsrTimerErrorPending = 0U; + PlsrEnterError(PLSR_ERROR_TIMER); + return; + } + if (PlsrCountOverflowPending != 0U) + { + PlsrCountOverflowPending = 0U; + PlsrEnterError(PLSR_ERROR_COUNT); + return; + } + + if (PlsrStopRequested != 0U) + { + PlsrFinishStopped(); + return; + } + if ((PlsrCurrentSegment == 0U) + || (PlsrCurrentSegment > PlsrActiveConfig.segmentCount)) + { + PlsrEnterError(PLSR_ERROR_INTERNAL); + return; + } + + segment = &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; + if (wasCut != 0U) + { + PlsrTransitionToNext( + (PlsrActiveConfig.sendMode == PLSR_SEND_SUBSEQUENT) ? 1U : 0U); + return; + } + + switch (segment->waitType) + { + case PLSR_WAIT_TIME: + PlsrWaitElapsedMs = 0UL; + PlsrRunStatus = PLSR_STATUS_WAITING; + break; + + case PLSR_WAIT_SIGNAL: + if (PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.waitInput) != 0U) + { + PlsrTransitionToNext(0U); + } + else + { + PlsrRunStatus = PLSR_STATUS_WAITING; + } + break; + + case PLSR_ACT_TIME: + if (PlsrSegmentElapsedMs >= segment->actTimeMs) + { + PlsrTransitionToNext(0U); + } + else + { + PlsrRunStatus = PLSR_STATUS_WAITING; + } + break; + + case PLSR_EXT_SIGNAL: + if (extEdge != 0U) + { + PlsrTransitionToNext(0U); + } + else + { + PlsrRunStatus = PLSR_STATUS_WAITING; + } + break; + + case PLSR_EXT_OR_COMPLETE: + PlsrTransitionToNext( + (PlsrActiveConfig.sendMode == PLSR_SEND_SUBSEQUENT) ? 1U : 0U); + break; + + default: + PlsrEnterError(PLSR_ERROR_INTERNAL); + break; + } +} + +static void PlsrRequestCut(uint32_t expectedEpoch) +{ + uint32_t criticalState = PlsrPlatformEnterCritical(); + + if (PlsrSegmentEpoch != expectedEpoch) + { + PlsrPlatformExitCritical(criticalState); + return; + } + if (PlsrBoundaryPending != 0U) + { + PlsrBoundaryWasCut = 1U; + } + else if (PlsrPulseActive != 0U) + { + PlsrCutRequested = 1U; + } + else + { + PlsrBoundaryFrequencyHz = PlsrCurrentFrequencyHz; + PlsrBoundaryWasCut = 1U; + PlsrBoundaryPending = 1U; + } + PlsrPlatformExitCritical(criticalState); +} + +static void PlsrPollWaiting(uint8_t extEdge) +{ + const PLSR_SEGMENT_CONFIG *segment = + &PlsrActiveConfig.segments[PlsrCurrentSegment - 1U]; + + switch (segment->waitType) + { + case PLSR_WAIT_TIME: + PlsrWaitElapsedMs++; + if (PlsrWaitElapsedMs >= segment->waitTimeMs) + { + PlsrTransitionToNext(0U); + } + break; + case PLSR_WAIT_SIGNAL: + if (PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.waitInput) != 0U) + { + PlsrTransitionToNext(0U); + } + break; + case PLSR_ACT_TIME: + if (PlsrSegmentElapsedMs >= segment->actTimeMs) + { + PlsrTransitionToNext(0U); + } + break; + case PLSR_EXT_SIGNAL: + if (extEdge != 0U) + { + PlsrTransitionToNext(0U); + } + break; + default: + PlsrEnterError(PLSR_ERROR_INTERNAL); + break; + } +} + +static void PlsrPollPersistence(void) +{ + PLSR_PERSIST_PAYLOAD payload; + uint32_t criticalState; + + if ((PlsrPersistenceDirty == 0U) || (PlsrIsBusy() != 0U)) + { + return; + } + if (PlsrPersistenceDelayMs != 0U) + { + PlsrPersistenceDelayMs--; + return; + } + + payload.config = PlsrShadowConfig; + criticalState = PlsrPlatformEnterCritical(); + payload.position = PlsrPosition; + payload.positionValid = PlsrPositionValid; + PlsrPlatformExitCritical(criticalState); + payload.wasBusy = 0U; + payload.reserved = 0U; + if (PlsrPlatformSave(&payload) != 0U) + { + PlsrPersistenceDirty = 0U; + } + else + { + PlsrRunStatus = PLSR_STATUS_ERROR; + PlsrError = PLSR_ERROR_INTERNAL; + } +} + +uint8_t PlsrInit(void) +{ + PLSR_PERSIST_PAYLOAD payload; + + PlsrInitialized = 0U; + PlsrRunStatus = PLSR_STATUS_UNINITIALIZED; + if (PlsrPlatformInit() == 0U) + { + return 0U; + } + + if ((PlsrPlatformLoad(&payload) == 0U) + || (PlsrConfigIsValid(&payload.config, 0U) == 0U) + || (payload.positionValid > 1U) || (payload.wasBusy > 1U)) + { + PlsrSetDefaults(&PlsrShadowConfig); + PlsrPosition = 0L; + PlsrPositionValid = 1U; + PlsrPlatformCheckpointConfig(&PlsrShadowConfig); + PlsrPlatformCheckpointPosition(0L, 1U, 0U); + PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); + } + else + { + PlsrShadowConfig = payload.config; + PlsrPosition = payload.position; + PlsrPositionValid = ((payload.positionValid != 0U) + && (payload.wasBusy == 0U)) ? 1U : 0U; + PlsrPersistenceDirty = 0U; + PlsrPersistenceDelayMs = 0U; + } + + (void)memset(&PlsrActiveConfig, 0, sizeof(PlsrActiveConfig)); + (void)memset(&PlsrRamp, 0, sizeof(PlsrRamp)); + (void)memset(&PlsrShortProfile, 0, sizeof(PlsrShortProfile)); + (void)memset(&PlsrHandoffPlan, 0, sizeof(PlsrHandoffPlan)); + (void)memset(PlsrPreparedHandoffPlans, 0, + sizeof(PlsrPreparedHandoffPlans)); + PlsrPreparedHandoffBank = 0U; + PlsrRemainingPulses = 0UL; + PlsrPulseActive = 0U; + PlsrCutRequested = 0U; + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrCountOverflowPending = 0U; + PlsrPositionCheckpointDirty = 0U; + PlsrFrequencyUpdatePending = 0U; + PlsrDeferredFrequencyPending = 0U; + PlsrFrequencyUpdateSegment = 0U; + PlsrSeamlessHandoffPending = 0U; + PlsrTimerErrorPending = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrBoundaryFrequencyHz = 0UL; + PlsrFrequencyUpdateTargetHz = 0UL; + PlsrDeferredFrequencyHz = 0UL; + PlsrSegmentEpoch = 0UL; + PlsrCurrentSegment = 0U; + PlsrDirectionDelayActive = 0U; + PlsrDirectionDelayRemainingMs = 0U; + PlsrSegmentClockStarted = 0U; + PlsrExtPreviousLevel = 0U; + PlsrExtEdgePending = 0U; + PlsrStopRequested = 0U; + PlsrStopPulsesRemaining = 0U; + PlsrLastDirectionValid = 0U; + PlsrPositionCheckpointElapsedMs = 0U; + PlsrCommandMailbox.command = 0U; + PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EMPTY; + PlsrError = PLSR_ERROR_NONE; + PlsrRunStatus = PLSR_STATUS_IDLE; + PlsrInitialized = 1U; + return 1U; +} + +static PLSR_MB_RESULT PlsrQueueCommand(uint16_t command) +{ + uint32_t criticalState; + PLSR_MB_RESULT result = PLSR_MB_OK; + + if (PlsrInitialized == 0U) + { + return PLSR_MB_SERVER_FAILURE; + } + + criticalState = PlsrPlatformEnterCritical(); + /* Capacity one: repeats acknowledge the first command; conflicts wait. */ + if (PlsrCommandMailbox.state != PLSR_COMMAND_MAILBOX_EMPTY) + { + result = (PlsrCommandMailbox.command == command) + ? PLSR_MB_OK : PLSR_MB_DEVICE_BUSY; + PlsrPlatformExitCritical(criticalState); + return result; + } + + if (command == PLSR_COMMAND_START) + { + if ((PlsrIsBusy() != 0U) + || (PlsrRunStatus == PLSR_STATUS_ERROR)) + { + result = PLSR_MB_DEVICE_BUSY; + } + else if ((PlsrRunStatus != PLSR_STATUS_IDLE) + && (PlsrRunStatus != PLSR_STATUS_COMPLETED) + && (PlsrRunStatus != PLSR_STATUS_STOPPED)) + { + result = PLSR_MB_ILLEGAL_VALUE; + } + else if ((PlsrConfigIsValid(&PlsrShadowConfig, 1U) == 0U) + || ((PlsrShadowConfig.positionMode + == PLSR_POSITION_ABSOLUTE) + && (PlsrPositionValid == 0U))) + { + result = PLSR_MB_ILLEGAL_VALUE; + } + else + { + PlsrCommandMailbox.startConfig = PlsrShadowConfig; + } + } + else if ((command == PLSR_COMMAND_CLEAR) && (PlsrIsBusy() != 0U)) + { + result = PLSR_MB_DEVICE_BUSY; + } + + if (result == PLSR_MB_OK) + { + PlsrCommandMailbox.command = command; + PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_PENDING; + } + PlsrPlatformExitCritical(criticalState); + return result; +} + +static void PlsrExecuteStart(void) +{ + uint32_t criticalState; + uint8_t handoffBank; + + PlsrActiveConfig = PlsrCommandMailbox.startConfig; + handoffBank = PlsrBuildHandoffPlanBank(&PlsrActiveConfig); + criticalState = PlsrPlatformEnterCritical(); + PlsrPreparedHandoffBank = handoffBank; + PlsrHandoffPlan.valid = 0U; + PlsrPlatformExitCritical(criticalState); + PlsrStopRequested = 0U; + PlsrTimerErrorPending = 0U; + PlsrShortProfile.active = 0U; + PlsrError = PLSR_ERROR_NONE; + PlsrLastDirectionValid = 0U; + PlsrCheckpointPosition(1U); + if (PlsrStartSegment((uint8_t)PlsrActiveConfig.startSegment, 0U, 0UL) + == 0U) + { + PlsrEnterError(PLSR_ERROR_INVALID_RESOURCE); + } +} + +static uint8_t PlsrExecuteStop(void) +{ + uint32_t criticalState; + uint32_t stopTargetHz; + + if (PlsrIsBusy() == 0U) + { + return 0U; + } + criticalState = PlsrPlatformEnterCritical(); + if (PlsrStopRequested != 0U) + { + PlsrPlatformExitCritical(criticalState); + return 0U; + } + + PlsrStopRequested = 1U; + PlsrStopPulsesRemaining = 0U; + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrInvalidateHandoffPlans(); + if (PlsrBoundaryPending != 0U) + { + PlsrPlatformExitCritical(criticalState); + return 1U; + } + if (PlsrPulseActive == 0U) + { + PlsrPlatformExitCritical(criticalState); + PlsrFinishStopped(); + return 1U; + } + + stopTargetHz = PlsrActiveConfig.stopSpeedHz; + if (stopTargetHz > PlsrCurrentFrequencyHz) + { + stopTargetHz = PlsrCurrentFrequencyHz; + } + PlsrRampStart(PlsrCurrentFrequencyHz, stopTargetHz); + PlsrRunStatus = PLSR_STATUS_DECELERATING; + if (PlsrRamp.active == 0U) + { + if (PlsrApplyFrequency(stopTargetHz, PlsrSegmentEpoch) == 0U) + { + PlsrPlatformExitCritical(criticalState); + PlsrEnterError(PLSR_ERROR_TIMER); + return 1U; + } + PlsrStopPulsesRemaining = PlsrStopDrainPulseCount(); + } + PlsrPlatformExitCritical(criticalState); + return 1U; +} + +static void PlsrExecuteClear(void) +{ + uint32_t criticalState; + + criticalState = PlsrPlatformEnterCritical(); + PlsrPosition = 0L; + PlsrPositionValid = 1U; + PlsrRemainingPulses = 0UL; + PlsrPlatformExitCritical(criticalState); + PlsrPlatformCheckpointPosition(0L, 1U, 0U); + PlsrCountOverflowPending = 0U; + PlsrPositionCheckpointDirty = 0U; + PlsrPositionCheckpointElapsedMs = 0U; + PlsrCurrentSegment = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrCutRequested = 0U; + PlsrBoundaryPending = 0U; + PlsrBoundaryWasCut = 0U; + PlsrDirectionDelayActive = 0U; + PlsrDirectionDelayRemainingMs = 0U; + PlsrExtEdgePending = 0U; + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrInvalidateHandoffPlans(); + PlsrDeferredFrequencyPending = 0U; + PlsrTimerErrorPending = 0U; + PlsrRamp.active = 0U; + PlsrError = PLSR_ERROR_NONE; + PlsrRunStatus = PLSR_STATUS_IDLE; + PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); +} + +static uint8_t PlsrPollCommandMailbox(void) +{ + uint16_t command; + uint8_t endPoll = 1U; + uint32_t criticalState; + + criticalState = PlsrPlatformEnterCritical(); + if (PlsrCommandMailbox.state != PLSR_COMMAND_MAILBOX_PENDING) + { + PlsrPlatformExitCritical(criticalState); + return 0U; + } + PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EXECUTING; + command = PlsrCommandMailbox.command; + PlsrPlatformExitCritical(criticalState); + + switch (command) + { + case PLSR_COMMAND_START: PlsrExecuteStart(); break; + case PLSR_COMMAND_STOP: endPoll = PlsrExecuteStop(); break; + case PLSR_COMMAND_CLEAR: PlsrExecuteClear(); break; + default: PlsrEnterError(PLSR_ERROR_INTERNAL); break; + } + + criticalState = PlsrPlatformEnterCritical(); + PlsrCommandMailbox.command = 0U; + PlsrCommandMailbox.state = PLSR_COMMAND_MAILBOX_EMPTY; + PlsrPlatformExitCritical(criticalState); + return endPoll; +} + +static void PlsrEnterErrorIfEpoch(PLSR_ERROR error, uint32_t expectedEpoch) +{ + uint32_t criticalState = PlsrPlatformEnterCritical(); + + if (PlsrSegmentEpoch == expectedEpoch) + { + PlsrEnterError(error); + } + PlsrPlatformExitCritical(criticalState); +} + +void PlsrPoll1ms(void) +{ + uint8_t extLevel; + uint8_t extEdge; + uint8_t activeSegmentNumber; + uint8_t applyDynamicFrequency = 0U; + PLSR_SEGMENT_CONFIG *activeSegment; + uint32_t criticalState; + uint32_t newTargetHz; + uint32_t pollEpoch; + + if (PlsrInitialized == 0U) + { + return; + } + + if (PlsrPollCommandMailbox() != 0U) + { + return; + } + + PlsrPollPositionCheckpoint(); + + criticalState = PlsrPlatformEnterCritical(); + pollEpoch = PlsrSegmentEpoch; + extLevel = PlsrPlatformReadInput((uint8_t)PlsrActiveConfig.extInput); + extEdge = ((extLevel != 0U) && (PlsrExtPreviousLevel == 0U)) ? 1U : 0U; + PlsrExtPreviousLevel = extLevel; + if (PlsrExtEdgePending != 0U) + { + extEdge = 1U; + } + PlsrPlatformExitCritical(criticalState); + + if (PlsrBoundaryPending != 0U) + { + PlsrExtEdgePending = 0U; + PlsrHandleBoundary(extEdge); + PlsrPollPersistence(); + return; + } + + if (PlsrIsBusy() == 0U) + { + PlsrExtEdgePending = 0U; + PlsrPollPersistence(); + return; + } + + if (PlsrSegmentEpoch != pollEpoch) + { + return; + } + + if (PlsrDirectionDelayActive != 0U) + { + if (extEdge != 0U) + { + PlsrExtEdgePending = 1U; + } + if (PlsrDirectionDelayRemainingMs != 0U) + { + PlsrDirectionDelayRemainingMs--; + } + if (PlsrDirectionDelayRemainingMs == 0U) + { + PlsrDirectionDelayActive = 0U; + if (PlsrBeginSegmentOutput(PlsrActiveConfig.startSpeedHz) == 0U) + { + PlsrEnterError(PLSR_ERROR_TIMER); + } + } + return; + } + + criticalState = PlsrPlatformEnterCritical(); + if (PlsrSegmentEpoch != pollEpoch) + { + PlsrPlatformExitCritical(criticalState); + return; + } + PlsrExtEdgePending = 0U; + if (PlsrSegmentClockStarted != 0U) + { + PlsrSegmentElapsedMs++; + } + if (PlsrRunStatus == PLSR_STATUS_WAITING) + { + PlsrPlatformExitCritical(criticalState); + PlsrPollWaiting(extEdge); + return; + } + + activeSegmentNumber = PlsrCurrentSegment; + if ((activeSegmentNumber == 0U) + || (activeSegmentNumber > PlsrActiveConfig.segmentCount)) + { + PlsrPlatformExitCritical(criticalState); + PlsrEnterError(PLSR_ERROR_INTERNAL); + return; + } + activeSegment = &PlsrActiveConfig.segments[activeSegmentNumber - 1U]; + if (PlsrSeamlessHandoffPending != 0U) + { + PlsrSeamlessHandoffPending = 0U; + if ((PlsrStopRequested == 0U) + && (PlsrShortProfile.active == 0U) + && (PlsrFrequencyUpdatePending == 0U)) + { + PlsrRamp.active = 0U; + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + } + newTargetHz = PlsrFrequencyUpdateTargetHz; + if ((PlsrStopRequested == 0U) + && (PlsrFrequencyUpdatePending != 0U) + && (PlsrFrequencyUpdateSegment == activeSegmentNumber)) + { + PlsrFrequencyUpdatePending = 0U; + activeSegment->frequencyHz = newTargetHz; + PlsrShortProfile.active = 0U; + PlsrBoundaryRampStarted = 0U; + PlsrRampStart(PlsrCurrentFrequencyHz, newTargetHz); + if (PlsrRamp.active == 0U) + { + applyDynamicFrequency = 1U; + } + } + PlsrPlatformExitCritical(criticalState); + + if (applyDynamicFrequency != 0U) + { + if (PlsrApplyFrequency(newTargetHz, pollEpoch) == 0U) + { + PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, pollEpoch); + return; + } + criticalState = PlsrPlatformEnterCritical(); + if (PlsrSegmentEpoch == pollEpoch) + { + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + PlsrPlatformExitCritical(criticalState); + } + + if ((PlsrShortProfile.active == 0U) + && (PlsrRamp.active != 0U) + && (PlsrRampAdvance(pollEpoch) == 0U)) + { + PlsrEnterErrorIfEpoch(PLSR_ERROR_TIMER, pollEpoch); + return; + } + + criticalState = PlsrPlatformEnterCritical(); + if (PlsrSegmentEpoch != pollEpoch) + { + PlsrPlatformExitCritical(criticalState); + return; + } + if ((PlsrStopRequested != 0U) && (PlsrRamp.active == 0U)) + { + if (PlsrStopPulsesRemaining == 0U) + { + PlsrStopPulsesRemaining = PlsrStopDrainPulseCount(); + } + PlsrPlatformExitCritical(criticalState); + return; + } + PlsrPlatformExitCritical(criticalState); + + if ((activeSegment->waitType == PLSR_ACT_TIME) + && (PlsrSegmentElapsedMs >= activeSegment->actTimeMs)) + { + PlsrRequestCut(pollEpoch); + return; + } + if (((activeSegment->waitType == PLSR_EXT_SIGNAL) + || (activeSegment->waitType == PLSR_EXT_OR_COMPLETE)) + && (extEdge != 0U)) + { + PlsrRequestCut(pollEpoch); + return; + } + + PlsrMaybePlanBoundaryRamp(pollEpoch); +} + +void PlsrPulseTimerIrq(uint8_t pulseOutput) +{ + uint32_t positionBits; + uint32_t completedFrequencyHz; + uint32_t activeFrequencyHz; + + if ((PlsrPulseActive == 0U) + || (pulseOutput != (uint8_t)PlsrActiveConfig.pulseOutput)) + { + return; + } + + completedFrequencyHz = PlsrCurrentFrequencyHz; + activeFrequencyHz = PlsrPlatformActiveFrequency(pulseOutput); + if (activeFrequencyHz == 0UL) + { + PlsrTimerErrorPending = 1U; + return; + } + PlsrCurrentFrequencyHz = activeFrequencyHz; + if ((PlsrStopRequested != 0U) && (PlsrRamp.active == 0U) + && (PlsrStopPulsesRemaining != 0U)) + { + PlsrStopPulsesRemaining--; + if (PlsrStopPulsesRemaining == 0U) + { + PlsrCutRequested = 1U; + } + } + positionBits = (uint32_t)PlsrPosition; + if (PlsrCountPositive != 0U) + { + if (PlsrPosition == INT32_MAX) + { + PlsrPositionValid = 0U; + PlsrCountOverflowPending = 1U; + } + positionBits++; + } + else + { + if (PlsrPosition == INT32_MIN) + { + PlsrPositionValid = 0U; + PlsrCountOverflowPending = 1U; + } + positionBits--; + } + PlsrPosition = (int32_t)positionBits; + PlsrPositionCheckpointDirty = 1U; + + if (PlsrRemainingPulses != 0UL) + { + PlsrRemainingPulses--; + } + + if ((PlsrRemainingPulses == 0UL) || (PlsrCutRequested != 0U) + || (PlsrCountOverflowPending != 0U)) + { + if ((PlsrRemainingPulses == 0UL) + && (PlsrTrySubsequentHandoff() != 0U)) + { + return; + } + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrBoundaryFrequencyHz = completedFrequencyHz; + PlsrBoundaryWasCut = (PlsrCutRequested != 0U) ? 1U : 0U; + PlsrCutRequested = 0U; + PlsrPlatformStopPulse(pulseOutput); + PlsrPulseActive = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrBoundaryPending = 1U; + return; + } + + if ((PlsrShortProfile.active != 0U) + && (PlsrAdvanceShortProfile(pulseOutput) == 0U)) + { + PlsrTimerErrorPending = 1U; + PlsrShortProfile.active = 0U; + PlsrHandoffPlan.valid = 0U; + PlsrBoundaryFrequencyHz = completedFrequencyHz; + PlsrBoundaryWasCut = 0U; + PlsrCutRequested = 0U; + PlsrPlatformStopPulse(pulseOutput); + PlsrPulseActive = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrBoundaryPending = 1U; + return; + } + + if ((PlsrShortProfile.active == 0U) + && (PlsrRemainingPulses == 1UL)) + { + (void)PlsrPrimeHandoff(); + } + if ((PlsrShortProfile.active == 0U) + && (PlsrHandoffPlan.valid == 0U) + && (PlsrCommitDeferredFrequency(pulseOutput) == 0U)) + { + PlsrTimerErrorPending = 1U; + PlsrBoundaryFrequencyHz = completedFrequencyHz; + PlsrBoundaryWasCut = 0U; + PlsrCutRequested = 0U; + PlsrPlatformStopPulse(pulseOutput); + PlsrPulseActive = 0U; + PlsrCurrentFrequencyHz = 0UL; + PlsrQueuedFrequencyHz = 0UL; + PlsrBoundaryPending = 1U; + } +} + +PLSR_MB_RESULT PlsrModbusReadHolding(uint16_t startAddress, + uint16_t quantity, + uint16_t *values) +{ + PLSR_MB_RESULT classification; + uint16_t index; + uint32_t criticalState; + int32_t position; + uint32_t frequency; + uint16_t statusWords[7]; + + if (values == NULL) + { + return PLSR_MB_ILLEGAL_VALUE; + } + classification = PlsrClassifyRange(startAddress, quantity); + if (classification != PLSR_MB_OK) + { + return classification; + } + + criticalState = PlsrPlatformEnterCritical(); + position = PlsrPosition; + frequency = PlsrCurrentFrequencyHz; + statusWords[0] = PlsrLowWord((uint32_t)position); + statusWords[1] = PlsrHighWord((uint32_t)position); + statusWords[2] = PlsrLowWord(frequency); + statusWords[3] = PlsrHighWord(frequency); + statusWords[4] = (uint16_t)PlsrRunStatus; + statusWords[5] = PlsrCurrentSegment; + statusWords[6] = (uint16_t)PlsrError; + PlsrPlatformExitCritical(criticalState); + + for (index = 0U; index < quantity; index++) + { + uint16_t address = (uint16_t)(startAddress + index); + + if ((address >= PLSR_CONFIG_FIRST_ADDRESS) + && (address <= PLSR_CONFIG_LAST_ADDRESS)) + { + values[index] = PlsrReadConfigWord(&PlsrShadowConfig, address); + } + else if ((address >= PLSR_STATUS_FIRST_ADDRESS) + && (address <= PLSR_STATUS_LAST_ADDRESS)) + { + values[index] = statusWords[address - PLSR_STATUS_FIRST_ADDRESS]; + } + else if (address == PLSR_CONTROL_ADDRESS) + { + values[index] = 0U; + } + else + { + return PLSR_MB_ILLEGAL_ADDRESS; + } + } + return PLSR_MB_OK; +} + +PLSR_MB_RESULT PlsrModbusWriteHolding(uint16_t startAddress, + uint16_t quantity, + const uint16_t *values) +{ + PLSR_MB_RESULT classification; + PLSR_WORD_RESULT wordResult; + uint16_t index; + uint16_t pairedAddress; + uint32_t requestEnd; + uint32_t criticalState; + uint8_t handoffBank = 0U; + uint8_t updateActiveFrequencies = 0U; + uint8_t drainedToDifferentSegment = 0U; + uint8_t segmentBeforeDrain; + uint32_t drainedSegmentTargetHz = 0UL; + + if (values == NULL) + { + return PLSR_MB_ILLEGAL_VALUE; + } + classification = PlsrClassifyRange(startAddress, quantity); + if (classification != PLSR_MB_OK) + { + return classification; + } + + if ((startAddress >= PLSR_STATUS_FIRST_ADDRESS) + && (startAddress <= PLSR_STATUS_LAST_ADDRESS)) + { + return PLSR_MB_ILLEGAL_ADDRESS; + } + + if (startAddress == PLSR_CONTROL_ADDRESS) + { + if (quantity != 1U) + { + return PLSR_MB_ILLEGAL_ADDRESS; + } + switch (values[0]) + { + case 0U: return PLSR_MB_OK; + case PLSR_COMMAND_START: + case PLSR_COMMAND_STOP: + case PLSR_COMMAND_CLEAR: + return PlsrQueueCommand(values[0]); + default: return PLSR_MB_ILLEGAL_VALUE; + } + } + + if ((startAddress < PLSR_CONFIG_FIRST_ADDRESS) + || ((uint32_t)startAddress + quantity - 1UL + > PLSR_CONFIG_LAST_ADDRESS)) + { + return PLSR_MB_ILLEGAL_ADDRESS; + } + if ((PlsrIsBusy() != 0U) + && (startAddress <= 0x1001U) + && ((uint32_t)startAddress + quantity - 1UL >= 0x1000UL)) + { + return PLSR_MB_DEVICE_BUSY; + } + + requestEnd = (uint32_t)startAddress + quantity; + for (index = 0U; index < quantity; index++) + { + uint16_t address = (uint16_t)(startAddress + index); + if (PlsrAddressIsDwordHalf(address, &pairedAddress) != 0U) + { + if (((uint32_t)pairedAddress < startAddress) + || ((uint32_t)pairedAddress >= requestEnd)) + { + return PLSR_MB_ILLEGAL_ADDRESS; + } + } + } + + PlsrCandidateConfig = PlsrShadowConfig; + for (index = 0U; index < quantity; index++) + { + wordResult = PlsrWriteConfigWord(&PlsrCandidateConfig, + (uint16_t)(startAddress + index), + values[index]); + if (wordResult == PLSR_WORD_ILLEGAL_ADDRESS) + { + return PLSR_MB_ILLEGAL_ADDRESS; + } + if (wordResult == PLSR_WORD_ILLEGAL_VALUE) + { + return PLSR_MB_ILLEGAL_VALUE; + } + } + + if (PlsrConfigIsValid(&PlsrCandidateConfig, 0U) == 0U) + { + return PLSR_MB_ILLEGAL_VALUE; + } + + if (PlsrIsBusy() != 0U) + { + for (index = 0U; index < PlsrActiveConfig.segmentCount; index++) + { + if (PlsrCandidateConfig.segments[index].frequencyHz + != PlsrShadowConfig.segments[index].frequencyHz) + { + updateActiveFrequencies = 1U; + } + } + } + if (updateActiveFrequencies != 0U) + { + handoffBank = PlsrBuildHandoffPlanBank(&PlsrCandidateConfig); + } + + criticalState = PlsrPlatformEnterCritical(); + segmentBeforeDrain = PlsrCurrentSegment; + if ((updateActiveFrequencies != 0U) && (PlsrPulseActive != 0U)) + { + PlsrPlatformDrainPendingPulse( + (uint8_t)PlsrActiveConfig.pulseOutput); + drainedToDifferentSegment = + (PlsrCurrentSegment != segmentBeforeDrain) ? 1U : 0U; + if ((drainedToDifferentSegment != 0U) + && (PlsrCurrentSegment != 0U) + && (PlsrCurrentSegment <= PlsrCandidateConfig.segmentCount)) + { + drainedSegmentTargetHz = + PlsrCandidateConfig.segments[PlsrCurrentSegment - 1U].frequencyHz; + } + } + PlsrShadowConfig = PlsrCandidateConfig; + if (updateActiveFrequencies != 0U) + { + for (index = 0U; index < PlsrActiveConfig.segmentCount; index++) + { + uint8_t frequencyChanged = + (PlsrActiveConfig.segments[index].frequencyHz + != PlsrCandidateConfig.segments[index].frequencyHz) + ? 1U : 0U; + + PlsrActiveConfig.segments[index].frequencyHz = + PlsrCandidateConfig.segments[index].frequencyHz; + if ((frequencyChanged != 0U) + && (index + 1U == PlsrCurrentSegment) + && (PlsrStopRequested == 0U) + && (PlsrRunStatus != PLSR_STATUS_WAITING)) + { + PlsrFrequencyUpdateTargetHz = + PlsrCandidateConfig.segments[index].frequencyHz; + PlsrFrequencyUpdateSegment = (uint8_t)(index + 1U); + PlsrFrequencyUpdatePending = 1U; + } + } + PlsrPreparedHandoffBank = handoffBank; + PlsrHandoffPlan.valid = 0U; + if ((drainedToDifferentSegment != 0U) + && (PlsrPulseActive != 0U) + && (PlsrCurrentSegment != 0U) + && (PlsrCurrentSegment <= PlsrActiveConfig.segmentCount) + && (PlsrStopRequested == 0U) + && (PlsrRunStatus != PLSR_STATUS_WAITING) + && (PlsrCurrentFrequencyHz != drainedSegmentTargetHz)) + { + PlsrFrequencyUpdateTargetHz = + drainedSegmentTargetHz; + PlsrFrequencyUpdateSegment = PlsrCurrentSegment; + PlsrFrequencyUpdatePending = 1U; + } + if ((PlsrPulseActive != 0U) + && (PlsrShortProfile.active == 0U) + && (PlsrRemainingPulses == 1UL)) + { + (void)PlsrPrimeHandoff(); + } + } + PlsrPlatformExitCritical(criticalState); + + if ((drainedToDifferentSegment != 0U) + && (PlsrFrequencyUpdatePending != 0U)) + { + uint32_t drainEpoch = PlsrSegmentEpoch; + uint32_t actualDrainFrequencyHz; + + if (PlsrPlatformQueueFrequency( + (uint8_t)PlsrActiveConfig.pulseOutput, + drainedSegmentTargetHz, &actualDrainFrequencyHz) == 0U) + { + PlsrTimerErrorPending = 1U; + } + else + { + uint32_t currentEpoch; + uint8_t updateSegment; + uint8_t currentSegment; + + criticalState = PlsrPlatformEnterCritical(); + currentEpoch = PlsrSegmentEpoch; + updateSegment = PlsrFrequencyUpdateSegment; + currentSegment = PlsrCurrentSegment; + if ((currentEpoch == drainEpoch) + && (updateSegment == currentSegment)) + { + PlsrQueuedFrequencyHz = actualDrainFrequencyHz; + PlsrFrequencyUpdatePending = 0U; + PlsrRamp.active = 0U; + PlsrRunStatus = PLSR_STATUS_RUNNING; + } + PlsrPlatformExitCritical(criticalState); + } + } + PlsrPlatformCheckpointConfig(&PlsrShadowConfig); + PlsrMarkPersistenceDirty(PLSR_CONFIG_SAVE_DELAY_MS); + return PLSR_MB_OK; +} + +#ifdef PLSR_HOST_TEST +uint64_t PlsrTestDivideU64ByU32(uint64_t dividend, + uint32_t divisor, + uint32_t *remainder) +{ + return PlsrDivideU64ByU32(dividend, divisor, remainder); +} + +void PlsrTestSetPosition(int32_t position, uint8_t positionValid) +{ + uint32_t criticalState = PlsrPlatformEnterCritical(); + + PlsrPosition = position; + PlsrPositionValid = (positionValid != 0U) ? 1U : 0U; + PlsrRemainingPulses = 0UL; + PlsrPositionCheckpointDirty = 0U; + PlsrPlatformExitCritical(criticalState); + PlsrPlatformCheckpointPosition(position, PlsrPositionValid, 0U); +} +#endif diff --git a/PLSR/Src/plsr_internal.h b/PLSR/Src/plsr_internal.h new file mode 100644 index 0000000..54d56c2 --- /dev/null +++ b/PLSR/Src/plsr_internal.h @@ -0,0 +1,49 @@ +#ifndef PLSR_INTERNAL_H +#define PLSR_INTERNAL_H + +#include + +#define PLSR_SEGMENT_COUNT_MAX (10U) +#define PLSR_FREQUENCY_MAX_HZ (100000UL) + +typedef struct +{ + uint32_t frequencyHz; + int32_t pulses; + uint16_t waitType; + uint16_t waitTimeMs; + uint16_t actTimeMs; + uint16_t jumpSegment; +} PLSR_SEGMENT_CONFIG; + +typedef struct +{ + uint16_t pulseOutput; + uint16_t directionOutput; + uint16_t waitInput; + uint16_t extInput; + uint16_t sendMode; + uint16_t directionDelayMs; + uint16_t directionNegativeLogic; + uint16_t curveMode; + uint16_t positionMode; + uint16_t segmentCount; + uint16_t startSegment; + uint32_t defaultSpeedHz; + uint32_t startSpeedHz; + uint32_t stopSpeedHz; + uint16_t accelerationTimeMs; + uint16_t decelerationTimeMs; + PLSR_SEGMENT_CONFIG segments[PLSR_SEGMENT_COUNT_MAX]; +} PLSR_CONFIG; + +typedef struct +{ + PLSR_CONFIG config; + int32_t position; + uint8_t positionValid; + uint8_t wasBusy; + uint16_t reserved; +} PLSR_PERSIST_PAYLOAD; + +#endif /* PLSR_INTERNAL_H */ diff --git a/PLSR/Src/plsr_platform.h b/PLSR/Src/plsr_platform.h new file mode 100644 index 0000000..e7b45cd --- /dev/null +++ b/PLSR/Src/plsr_platform.h @@ -0,0 +1,34 @@ +#ifndef PLSR_PLATFORM_H +#define PLSR_PLATFORM_H + +#include "plsr_internal.h" +#include + +uint8_t PlsrPlatformInit(void); +uint8_t PlsrPlatformPrepare(uint8_t pulseOutput, + uint8_t directionOutput, + uint8_t directionLevel); +uint8_t PlsrPlatformStartPulse(uint8_t pulseOutput, + uint32_t firstFrequencyHz, + uint32_t queuedFrequencyHz, + uint32_t *actualFirstFrequencyHz, + uint32_t *actualQueuedFrequencyHz); +uint8_t PlsrPlatformQueueFrequency(uint8_t pulseOutput, + uint32_t frequencyHz, + uint32_t *actualFrequencyHz); +void PlsrPlatformDrainPendingPulse(uint8_t pulseOutput); +uint32_t PlsrPlatformActiveFrequency(uint8_t pulseOutput); +void PlsrPlatformStopPulse(uint8_t pulseOutput); +uint8_t PlsrPlatformReadInput(uint8_t inputSelection); + +uint8_t PlsrPlatformLoad(PLSR_PERSIST_PAYLOAD *payload); +uint8_t PlsrPlatformSave(const PLSR_PERSIST_PAYLOAD *payload); +void PlsrPlatformCheckpointConfig(const PLSR_CONFIG *config); +void PlsrPlatformCheckpointPosition(int32_t position, + uint8_t positionValid, + uint8_t wasBusy); + +uint32_t PlsrPlatformEnterCritical(void); +void PlsrPlatformExitCritical(uint32_t state); + +#endif /* PLSR_PLATFORM_H */ diff --git a/PLSR/Src/plsr_platform_f407.c b/PLSR/Src/plsr_platform_f407.c new file mode 100644 index 0000000..ee899dd --- /dev/null +++ b/PLSR/Src/plsr_platform_f407.c @@ -0,0 +1,1202 @@ +#include "plsr_platform.h" +#include "plsr.h" + +#ifdef PLSR_HOST_TEST + +#include + +static uint8_t PlsrHostPulseActive[4]; +static uint32_t PlsrHostFrequency[4]; +static uint32_t PlsrHostQueuedFrequency[4]; +static uint8_t PlsrHostUpdatePending[4]; +static uint8_t PlsrHostInputs[2]; +static uint8_t PlsrHostSelectedPulse; +static uint8_t PlsrHostDirectionLevel; +static uint8_t PlsrHostEmitPulseOnCriticalEntry; +static uint8_t PlsrHostEmitPulseOnCriticalExit; +static uint8_t PlsrHostLatchPulseOnCriticalEntry; +static uint8_t PlsrHostCriticalEntriesToSkip; +static uint8_t PlsrHostFailNextStart; +static uint8_t PlsrHostFailNextFrequencyAtUpdate; +static PLSR_PERSIST_PAYLOAD PlsrHostPersistentPayload; +static uint8_t PlsrHostPersistentValid; +static uint32_t PlsrHostSaveCount; + +static void PlsrHostLatchPulse(uint8_t pulseOutput) +{ + if ((pulseOutput <= 3U) + && (PlsrHostPulseActive[pulseOutput] != 0U)) + { + PlsrHostFrequency[pulseOutput] = + PlsrHostQueuedFrequency[pulseOutput]; + PlsrHostUpdatePending[pulseOutput] = 1U; + } +} + +static void PlsrHostServicePendingPulse(uint8_t pulseOutput) +{ + if ((pulseOutput <= 3U) + && (PlsrHostUpdatePending[pulseOutput] != 0U)) + { + PlsrHostUpdatePending[pulseOutput] = 0U; + PlsrPulseTimerIrq(pulseOutput); + } +} + +uint8_t PlsrPlatformInit(void) +{ + (void)memset(PlsrHostPulseActive, 0, sizeof(PlsrHostPulseActive)); + (void)memset(PlsrHostFrequency, 0, sizeof(PlsrHostFrequency)); + (void)memset(PlsrHostQueuedFrequency, 0, + sizeof(PlsrHostQueuedFrequency)); + (void)memset(PlsrHostUpdatePending, 0, + sizeof(PlsrHostUpdatePending)); + PlsrHostSelectedPulse = 0U; + PlsrHostDirectionLevel = 0U; + PlsrHostEmitPulseOnCriticalEntry = 0U; + PlsrHostEmitPulseOnCriticalExit = 0U; + PlsrHostLatchPulseOnCriticalEntry = 0U; + PlsrHostCriticalEntriesToSkip = 0U; + PlsrHostFailNextStart = 0U; + PlsrHostFailNextFrequencyAtUpdate = 0U; + return 1U; +} + +uint8_t PlsrPlatformPrepare(uint8_t pulseOutput, + uint8_t directionOutput, + uint8_t directionLevel) +{ + uint8_t index; + (void)directionOutput; + + if (PlsrHostFailNextStart != 0U) + { + PlsrHostFailNextStart = 0U; + return 0U; + } + if ((pulseOutput > 3U) || (directionOutput > 3U)) + { + return 0U; + } + for (index = 0U; index < 4U; index++) + { + PlsrHostPulseActive[index] = 0U; + PlsrHostFrequency[index] = 0UL; + PlsrHostQueuedFrequency[index] = 0UL; + PlsrHostUpdatePending[index] = 0U; + } + PlsrHostSelectedPulse = pulseOutput; + PlsrHostDirectionLevel = (directionLevel != 0U) ? 1U : 0U; + return 1U; +} + +uint8_t PlsrPlatformStartPulse(uint8_t pulseOutput, + uint32_t firstFrequencyHz, + uint32_t queuedFrequencyHz, + uint32_t *actualFirstFrequencyHz, + uint32_t *actualQueuedFrequencyHz) +{ + if ((pulseOutput > 3U) || (firstFrequencyHz == 0UL) + || (firstFrequencyHz > PLSR_FREQUENCY_MAX_HZ) + || (queuedFrequencyHz == 0UL) + || (queuedFrequencyHz > PLSR_FREQUENCY_MAX_HZ) + || (actualFirstFrequencyHz == NULL) + || (actualQueuedFrequencyHz == NULL)) + { + return 0U; + } + PlsrHostPulseActive[pulseOutput] = 1U; + PlsrHostFrequency[pulseOutput] = firstFrequencyHz; + PlsrHostQueuedFrequency[pulseOutput] = queuedFrequencyHz; + PlsrHostUpdatePending[pulseOutput] = 0U; + PlsrHostSelectedPulse = pulseOutput; + *actualFirstFrequencyHz = firstFrequencyHz; + *actualQueuedFrequencyHz = queuedFrequencyHz; + return 1U; +} + +uint8_t PlsrPlatformQueueFrequency(uint8_t pulseOutput, + uint32_t frequencyHz, + uint32_t *actualFrequencyHz) +{ + if (PlsrHostFailNextFrequencyAtUpdate != 0U) + { + PlsrHostFailNextFrequencyAtUpdate = 0U; + return 0U; + } + if ((pulseOutput > 3U) || (frequencyHz == 0UL) + || (frequencyHz > PLSR_FREQUENCY_MAX_HZ) + || (actualFrequencyHz == NULL) + || (PlsrHostPulseActive[pulseOutput] == 0U)) + { + return 0U; + } + PlsrHostQueuedFrequency[pulseOutput] = frequencyHz; + *actualFrequencyHz = frequencyHz; + return 1U; +} + +void PlsrPlatformDrainPendingPulse(uint8_t pulseOutput) +{ + PlsrHostServicePendingPulse(pulseOutput); +} + +uint32_t PlsrPlatformActiveFrequency(uint8_t pulseOutput) +{ + return (pulseOutput <= 3U) ? PlsrHostFrequency[pulseOutput] : 0UL; +} + +void PlsrPlatformStopPulse(uint8_t pulseOutput) +{ + if (pulseOutput <= 3U) + { + PlsrHostPulseActive[pulseOutput] = 0U; + PlsrHostFrequency[pulseOutput] = 0UL; + PlsrHostQueuedFrequency[pulseOutput] = 0UL; + PlsrHostUpdatePending[pulseOutput] = 0U; + } +} + +uint8_t PlsrPlatformReadInput(uint8_t inputSelection) +{ + return (inputSelection <= 1U) ? PlsrHostInputs[inputSelection] : 0U; +} + +uint8_t PlsrPlatformLoad(PLSR_PERSIST_PAYLOAD *payload) +{ + if ((payload == NULL) || (PlsrHostPersistentValid == 0U)) + { + return 0U; + } + *payload = PlsrHostPersistentPayload; + return 1U; +} + +uint8_t PlsrPlatformSave(const PLSR_PERSIST_PAYLOAD *payload) +{ + if (payload == NULL) + { + return 0U; + } + PlsrHostPersistentPayload = *payload; + PlsrHostPersistentValid = 1U; + PlsrHostSaveCount++; + return 1U; +} + +void PlsrPlatformCheckpointConfig(const PLSR_CONFIG *config) +{ + if (config != NULL) + { + PlsrHostPersistentPayload.config = *config; + PlsrHostPersistentValid = 1U; + } +} + +void PlsrPlatformCheckpointPosition(int32_t position, + uint8_t positionValid, + uint8_t wasBusy) +{ + PlsrHostPersistentPayload.position = position; + PlsrHostPersistentPayload.positionValid = positionValid; + PlsrHostPersistentPayload.wasBusy = wasBusy; + PlsrHostPersistentPayload.reserved = 0U; +} + +uint32_t PlsrPlatformEnterCritical(void) +{ + if (PlsrHostEmitPulseOnCriticalEntry != 0U) + { + if (PlsrHostCriticalEntriesToSkip != 0U) + { + PlsrHostCriticalEntriesToSkip--; + } + else + { + PlsrHostEmitPulseOnCriticalEntry = 0U; + PlsrHostLatchPulse(PlsrHostSelectedPulse); + PlsrHostServicePendingPulse(PlsrHostSelectedPulse); + } + } + if (PlsrHostLatchPulseOnCriticalEntry != 0U) + { + PlsrHostLatchPulseOnCriticalEntry = 0U; + PlsrHostLatchPulse(PlsrHostSelectedPulse); + } + return 0UL; +} + +void PlsrPlatformExitCritical(uint32_t state) +{ + (void)state; + if (PlsrHostEmitPulseOnCriticalExit != 0U) + { + PlsrHostEmitPulseOnCriticalExit = 0U; + PlsrHostLatchPulse(PlsrHostSelectedPulse); + } + PlsrHostServicePendingPulse(PlsrHostSelectedPulse); +} + +void PlsrTestSetInput(uint8_t inputSelection, uint8_t level) +{ + if (inputSelection <= 1U) + { + PlsrHostInputs[inputSelection] = (level != 0U) ? 1U : 0U; + } +} + +void PlsrTestEmitPulses(uint32_t pulseCount) +{ + while ((pulseCount != 0UL) + && (PlsrHostPulseActive[PlsrHostSelectedPulse] != 0U)) + { + PlsrHostLatchPulse(PlsrHostSelectedPulse); + PlsrHostServicePendingPulse(PlsrHostSelectedPulse); + pulseCount--; + } +} + +void PlsrTestEmitPulseOnCriticalEntry(void) +{ + PlsrHostCriticalEntriesToSkip = 0U; + PlsrHostEmitPulseOnCriticalEntry = 1U; +} + +void PlsrTestEmitPulseAfterCriticalEntries(uint8_t entriesToSkip) +{ + PlsrHostCriticalEntriesToSkip = entriesToSkip; + PlsrHostEmitPulseOnCriticalEntry = 1U; +} + +void PlsrTestEmitPulseOnCriticalExit(void) +{ + PlsrHostEmitPulseOnCriticalExit = 1U; +} + +void PlsrTestLatchPulseOnCriticalEntry(void) +{ + PlsrHostLatchPulseOnCriticalEntry = 1U; +} + +void PlsrTestServicePendingPulse(void) +{ + PlsrHostServicePendingPulse(PlsrHostSelectedPulse); +} + +void PlsrTestFailNextStart(void) +{ + PlsrHostFailNextStart = 1U; +} + +void PlsrTestFailNextFrequencyAtUpdate(void) +{ + PlsrHostFailNextFrequencyAtUpdate = 1U; +} + +uint8_t PlsrTestPulseIsActive(void) +{ + return PlsrHostPulseActive[PlsrHostSelectedPulse]; +} + +uint32_t PlsrTestOutputFrequency(void) +{ + return PlsrHostFrequency[PlsrHostSelectedPulse]; +} + +uint32_t PlsrTestQueuedFrequency(void) +{ + return PlsrHostQueuedFrequency[PlsrHostSelectedPulse]; +} + +uint8_t PlsrTestDirectionLevel(void) +{ + return PlsrHostDirectionLevel; +} + +void PlsrTestClearPersistentStorage(void) +{ + (void)memset(&PlsrHostPersistentPayload, 0, + sizeof(PlsrHostPersistentPayload)); + (void)memset(PlsrHostInputs, 0, sizeof(PlsrHostInputs)); + PlsrHostPersistentValid = 0U; + PlsrHostSaveCount = 0UL; +} + +void PlsrTestResetSaveCount(void) +{ + PlsrHostSaveCount = 0UL; +} + +uint32_t PlsrTestSaveCount(void) +{ + return PlsrHostSaveCount; +} + +#else + +#include "stm32f4xx_hal.h" +#include +#include + +#define PLSR_ENABLE_IRQ_CYCLE_DIAG (0U) + +#define PLSR_FLASH_SLOT_A_ADDRESS (0x080C0000UL) +#define PLSR_FLASH_SLOT_B_ADDRESS (0x080E0000UL) +#define PLSR_FLASH_MAGIC (0x50534C52UL) +#define PLSR_FLASH_VERSION (2U) +#define PLSR_BACKUP_CONFIG_ADDRESS (BKPSRAM_BASE + 0x0100UL) +#define PLSR_BACKUP_POSITION_ADDRESS (BKPSRAM_BASE + 0x0200UL) +#define PLSR_BACKUP_CONFIG_MAGIC (0x50434647UL) +#define PLSR_BACKUP_POSITION_MAGIC (0x50504F53UL) + +typedef struct +{ + TIM_TypeDef *timer; + GPIO_TypeDef *port; + uint16_t pin; + uint8_t pinIndex; + uint8_t alternate; + IRQn_Type irq; + uint32_t timerClockHz; +} PLSR_TIMER_MAP; + +typedef struct +{ + GPIO_TypeDef *port; + uint16_t pin; +} PLSR_GPIO_MAP; + +typedef struct +{ + uint32_t prescaler; + uint32_t period; + uint32_t compare; + uint32_t actualFrequencyHz; +} PLSR_TIMER_SETTING; + +typedef struct +{ + uint32_t magic; + uint16_t version; + uint16_t payloadSize; + uint32_t generation; + PLSR_PERSIST_PAYLOAD payload; + uint32_t crc32; +} PLSR_FLASH_RECORD; + +typedef struct +{ + uint32_t magic; + PLSR_CONFIG config; + uint32_t crc32; +} PLSR_BACKUP_CONFIG_RECORD; + +typedef struct +{ + uint32_t magic; + uint32_t generation; + int32_t position; + uint8_t positionValid; + uint8_t wasBusy; + uint16_t reserved; + uint32_t crc32; +} PLSR_BACKUP_POSITION_RECORD; + +static const PLSR_TIMER_MAP PlsrTimerMap[4] = +{ + {TIM10, GPIOF, GPIO_PIN_6, 6U, GPIO_AF3_TIM10, + TIM1_UP_TIM10_IRQn, 168000000UL}, + {TIM13, GPIOF, GPIO_PIN_8, 8U, GPIO_AF9_TIM13, + TIM8_UP_TIM13_IRQn, 84000000UL}, + {TIM11, GPIOF, GPIO_PIN_7, 7U, GPIO_AF3_TIM11, + TIM1_TRG_COM_TIM11_IRQn, 168000000UL}, + {TIM14, GPIOF, GPIO_PIN_9, 9U, GPIO_AF9_TIM14, + TIM8_TRG_COM_TIM14_IRQn, 84000000UL} +}; + +static const PLSR_GPIO_MAP PlsrDirectionMap[4] = +{ + {GPIOH, GPIO_PIN_9}, + {GPIOH, GPIO_PIN_8}, + {GPIOH, GPIO_PIN_7}, + {GPIOH, GPIO_PIN_6} +}; + +static PLSR_FLASH_RECORD PlsrFlashRecordBuffer; +static uint32_t PlsrBackupPositionGeneration; +static uint32_t PlsrTimerActiveFrequencyHz[4]; +static uint32_t PlsrTimerQueuedFrequencyHz[4]; +static uint32_t PlsrTimerQueueGeneration[4]; + +static void PlsrHandleTimerIrq(uint8_t pulseOutput); + +#if PLSR_ENABLE_IRQ_CYCLE_DIAG +volatile uint32_t PlsrIrqCount[4]; +volatile uint32_t PlsrIrqLastCycles[4]; +volatile uint32_t PlsrIrqMaxCycles[4]; +#endif + +static uint32_t PlsrCrc32(const void *data, uint32_t length) +{ + const uint8_t *bytes = (const uint8_t *)data; + uint32_t crc = 0xFFFFFFFFUL; + uint32_t index; + uint8_t bit; + + for (index = 0UL; index < length; index++) + { + crc ^= bytes[index]; + for (bit = 0U; bit < 8U; bit++) + { + crc = ((crc & 1UL) != 0UL) ? ((crc >> 1U) ^ 0xEDB88320UL) + : (crc >> 1U); + } + } + return ~crc; +} + +static uint8_t PlsrGenerationIsNewer(uint32_t first, uint32_t second) +{ + return ((int32_t)(first - second) > 0) ? 1U : 0U; +} + +static uint32_t PlsrFlashRecordCrc(const PLSR_FLASH_RECORD *record) +{ + const uint8_t *start = (const uint8_t *)&record->version; + uint32_t length = (uint32_t)(offsetof(PLSR_FLASH_RECORD, crc32) + - offsetof(PLSR_FLASH_RECORD, version)); + return PlsrCrc32(start, length); +} + +static uint8_t PlsrFlashRecordIsValid(const PLSR_FLASH_RECORD *record) +{ + return ((record->magic == PLSR_FLASH_MAGIC) + && (record->version == PLSR_FLASH_VERSION) + && (record->payloadSize == sizeof(PLSR_PERSIST_PAYLOAD)) + && (record->crc32 == PlsrFlashRecordCrc(record))) ? 1U : 0U; +} + +static uint8_t PlsrBackupConfigIsValid( + const PLSR_BACKUP_CONFIG_RECORD *record) +{ + return ((record->magic == PLSR_BACKUP_CONFIG_MAGIC) + && (record->crc32 + == PlsrCrc32(&record->config, sizeof(record->config)))) ? 1U : 0U; +} + +static uint8_t PlsrBackupPositionIsValid( + const PLSR_BACKUP_POSITION_RECORD *record) +{ + uint32_t crc = PlsrCrc32(&record->generation, + sizeof(record->generation) + + sizeof(record->position) + + sizeof(record->positionValid) + + sizeof(record->wasBusy) + + sizeof(record->reserved)); + return ((record->magic == PLSR_BACKUP_POSITION_MAGIC) + && (record->crc32 == crc)) ? 1U : 0U; +} + +static const PLSR_BACKUP_POSITION_RECORD *PlsrNewestBackupPosition(void) +{ + const PLSR_BACKUP_POSITION_RECORD *slots = + (const PLSR_BACKUP_POSITION_RECORD *)PLSR_BACKUP_POSITION_ADDRESS; + uint8_t validA = PlsrBackupPositionIsValid(&slots[0]); + uint8_t validB = PlsrBackupPositionIsValid(&slots[1]); + + if ((validA == 0U) && (validB == 0U)) + { + return NULL; + } + if (validA == 0U) + { + return &slots[1]; + } + if (validB == 0U) + { + return &slots[0]; + } + return (PlsrGenerationIsNewer(slots[1].generation, + slots[0].generation) != 0U) + ? &slots[1] : &slots[0]; +} + +static void PlsrTimerStop(TIM_TypeDef *timer) +{ + timer->DIER &= ~TIM_DIER_UIE; + timer->CR1 &= ~TIM_CR1_CEN; + timer->CCER &= ~TIM_CCER_CC1E; + timer->SR = ~TIM_SR_UIF; +} + +static void PlsrTimerInitialize(TIM_TypeDef *timer) +{ + timer->CR1 = TIM_CR1_ARPE | TIM_CR1_URS; + timer->CR2 = 0UL; + timer->SMCR = 0UL; + timer->DIER = 0UL; + timer->CCMR1 = TIM_CCMR1_OC1PE | (6UL << TIM_CCMR1_OC1M_Pos); + timer->CCER = 0UL; + timer->PSC = 0UL; + timer->ARR = 999UL; + timer->CCR1 = 500UL; + timer->CNT = 0UL; + timer->EGR = TIM_EGR_UG; + timer->SR = 0UL; +} + +static void PlsrPulsePinHoldIdle(uint8_t pulseOutput) +{ + const PLSR_TIMER_MAP *map = &PlsrTimerMap[pulseOutput]; + GPIO_InitTypeDef gpio; + + HAL_GPIO_WritePin(map->port, map->pin, GPIO_PIN_SET); + gpio.Pin = map->pin; + gpio.Mode = GPIO_MODE_OUTPUT_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + gpio.Alternate = 0U; + HAL_GPIO_Init(map->port, &gpio); +} + +static void PlsrPulsePinCaptureIdle(uint8_t pulseOutput) +{ + const PLSR_TIMER_MAP *map = &PlsrTimerMap[pulseOutput]; + uint32_t shift = (uint32_t)map->pinIndex * 2UL; + uint32_t mode = map->port->MODER; + + /* The update IRQ occurs while PWM is high; switch to GPIO high first. */ + map->port->BSRR = map->pin; + mode &= ~(3UL << shift); + mode |= 1UL << shift; + map->port->MODER = mode; + __DSB(); +} + +static void PlsrPulsePinRelease(uint8_t pulseOutput) +{ + const PLSR_TIMER_MAP *map = &PlsrTimerMap[pulseOutput]; + GPIO_InitTypeDef gpio; + + gpio.Pin = map->pin; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + gpio.Alternate = map->alternate; + HAL_GPIO_Init(map->port, &gpio); + __DSB(); +} + +static uint8_t PlsrTimerCalculate(uint8_t pulseOutput, + uint32_t frequencyHz, + PLSR_TIMER_SETTING *setting) +{ + const PLSR_TIMER_MAP *map; + uint32_t prescalerDivider; + uint32_t denominator; + uint32_t periodCounts; + + if ((pulseOutput > 3U) || (frequencyHz == 0UL) + || (frequencyHz > PLSR_FREQUENCY_MAX_HZ) + || (setting == NULL)) + { + return 0U; + } + + map = &PlsrTimerMap[pulseOutput]; + prescalerDivider = (((map->timerClockHz - 1UL) / frequencyHz) >> 16U) + + 1UL; + if (prescalerDivider > 65536UL) + { + return 0U; + } + denominator = prescalerDivider * frequencyHz; + periodCounts = (map->timerClockHz + denominator / 2UL) / denominator; + if (periodCounts < 2UL) + { + periodCounts = 2UL; + } + if (periodCounts > 65536UL) + { + periodCounts = 65536UL; + } + + setting->prescaler = prescalerDivider - 1UL; + setting->period = periodCounts - 1UL; + setting->compare = periodCounts / 2UL; + setting->actualFrequencyHz = + map->timerClockHz / (prescalerDivider * periodCounts); + return 1U; +} + +static void PlsrTimerWriteSetting(TIM_TypeDef *timer, + const PLSR_TIMER_SETTING *setting) +{ + timer->PSC = setting->prescaler; + timer->ARR = setting->period; + timer->CCR1 = setting->compare; +} + +uint8_t PlsrPlatformInit(void) +{ + GPIO_InitTypeDef gpio; + uint8_t index; + const PLSR_BACKUP_POSITION_RECORD *positionRecord; + + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOF_CLK_ENABLE(); + __HAL_RCC_GPIOG_CLK_ENABLE(); + __HAL_RCC_GPIOH_CLK_ENABLE(); + __HAL_RCC_TIM10_CLK_ENABLE(); + __HAL_RCC_TIM11_CLK_ENABLE(); + __HAL_RCC_TIM13_CLK_ENABLE(); + __HAL_RCC_TIM14_CLK_ENABLE(); + __HAL_RCC_PWR_CLK_ENABLE(); + HAL_PWR_EnableBkUpAccess(); + __HAL_RCC_BKPSRAM_CLK_ENABLE(); + if (HAL_PWREx_EnableBkUpReg() != HAL_OK) + { + return 0U; + } + +#if PLSR_ENABLE_IRQ_CYCLE_DIAG + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + DWT->CYCCNT = 0UL; + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + (void)memset((void *)PlsrIrqCount, 0, sizeof(PlsrIrqCount)); + (void)memset((void *)PlsrIrqLastCycles, 0, sizeof(PlsrIrqLastCycles)); + (void)memset((void *)PlsrIrqMaxCycles, 0, sizeof(PlsrIrqMaxCycles)); +#endif + + HAL_GPIO_WritePin(GPIOH, GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8 + | GPIO_PIN_9, GPIO_PIN_SET); + gpio.Pin = GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_8 | GPIO_PIN_9; + gpio.Mode = GPIO_MODE_OUTPUT_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + gpio.Alternate = 0U; + HAL_GPIO_Init(GPIOH, &gpio); + + gpio.Mode = GPIO_MODE_INPUT; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_LOW; + gpio.Alternate = 0U; + gpio.Pin = GPIO_PIN_5; + HAL_GPIO_Init(GPIOB, &gpio); + gpio.Pin = GPIO_PIN_12; + HAL_GPIO_Init(GPIOG, &gpio); + + for (index = 0U; index < 4U; index++) + { + PlsrTimerActiveFrequencyHz[index] = 0UL; + PlsrTimerQueuedFrequencyHz[index] = 0UL; + PlsrTimerQueueGeneration[index] = 0UL; + PlsrTimerInitialize(PlsrTimerMap[index].timer); + PlsrPulsePinHoldIdle(index); + HAL_NVIC_SetPriority(PlsrTimerMap[index].irq, 1U, 0U); + HAL_NVIC_EnableIRQ(PlsrTimerMap[index].irq); + } + + positionRecord = PlsrNewestBackupPosition(); + PlsrBackupPositionGeneration = + (positionRecord == NULL) ? 0UL : positionRecord->generation; + return 1U; +} + +uint8_t PlsrPlatformPrepare(uint8_t pulseOutput, + uint8_t directionOutput, + uint8_t directionLevel) +{ + uint8_t index; + + if ((pulseOutput > 3U) || (directionOutput > 3U)) + { + return 0U; + } + for (index = 0U; index < 4U; index++) + { + PlsrPulsePinHoldIdle(index); + PlsrTimerStop(PlsrTimerMap[index].timer); + HAL_GPIO_WritePin(PlsrDirectionMap[index].port, + PlsrDirectionMap[index].pin, + GPIO_PIN_SET); + } + if (directionLevel != 0U) + { + HAL_GPIO_WritePin(PlsrDirectionMap[directionOutput].port, + PlsrDirectionMap[directionOutput].pin, + GPIO_PIN_RESET); + } + return 1U; +} + +uint8_t PlsrPlatformStartPulse(uint8_t pulseOutput, + uint32_t firstFrequencyHz, + uint32_t queuedFrequencyHz, + uint32_t *actualFirstFrequencyHz, + uint32_t *actualQueuedFrequencyHz) +{ + TIM_TypeDef *timer; + PLSR_TIMER_SETTING firstSetting; + PLSR_TIMER_SETTING queuedSetting; + + if ((actualFirstFrequencyHz == NULL) + || (actualQueuedFrequencyHz == NULL) + || (PlsrTimerCalculate(pulseOutput, firstFrequencyHz, + &firstSetting) == 0U) + || (PlsrTimerCalculate(pulseOutput, queuedFrequencyHz, + &queuedSetting) == 0U)) + { + return 0U; + } + timer = PlsrTimerMap[pulseOutput].timer; + timer->DIER &= ~TIM_DIER_UIE; + timer->CR1 &= ~TIM_CR1_CEN; + timer->CCER &= ~TIM_CCER_CC1E; + timer->CNT = 0UL; + PlsrTimerWriteSetting(timer, &firstSetting); + timer->EGR = TIM_EGR_UG; + PlsrTimerWriteSetting(timer, &queuedSetting); + timer->SR = 0UL; + timer->CCER |= TIM_CCER_CC1E; + __DSB(); + timer->DIER |= TIM_DIER_UIE; + PlsrPulsePinRelease(pulseOutput); + timer->CR1 |= TIM_CR1_CEN; + PlsrTimerActiveFrequencyHz[pulseOutput] = + firstSetting.actualFrequencyHz; + PlsrTimerQueuedFrequencyHz[pulseOutput] = + queuedSetting.actualFrequencyHz; + PlsrTimerQueueGeneration[pulseOutput]++; + *actualFirstFrequencyHz = firstSetting.actualFrequencyHz; + *actualQueuedFrequencyHz = queuedSetting.actualFrequencyHz; + return 1U; +} + +uint8_t PlsrPlatformQueueFrequency(uint8_t pulseOutput, + uint32_t frequencyHz, + uint32_t *actualFrequencyHz) +{ + TIM_TypeDef *timer; + PLSR_TIMER_SETTING setting; + uint32_t counterBefore; + uint32_t counterAfter; + uint32_t counterFinal; + uint32_t criticalState; + uint32_t generationBefore; + uint32_t ownGeneration; + uint8_t wrappedWhileUpdatesDisabled; + + if ((pulseOutput > 3U) || (actualFrequencyHz == NULL) + || (PlsrTimerCalculate(pulseOutput, frequencyHz, &setting) == 0U)) + { + return 0U; + } + timer = PlsrTimerMap[pulseOutput].timer; + if ((timer->CR1 & TIM_CR1_CEN) == 0UL) + { + return 0U; + } + + criticalState = PlsrPlatformEnterCritical(); + generationBefore = PlsrTimerQueueGeneration[pulseOutput]; + if ((timer->SR & TIM_SR_UIF) != 0UL) + { + PlsrHandleTimerIrq(pulseOutput); + if (PlsrTimerQueueGeneration[pulseOutput] != generationBefore) + { + *actualFrequencyHz = PlsrTimerQueuedFrequencyHz[pulseOutput]; + PlsrPlatformExitCritical(criticalState); + return 1U; + } + if ((timer->CR1 & TIM_CR1_CEN) == 0UL) + { + PlsrPlatformExitCritical(criticalState); + return 0U; + } + } + + /* UDIS blocks shadow transfers while the three preload registers are + replaced. The counter and PWM output continue without interruption. */ + counterBefore = timer->CNT; + timer->CR1 |= TIM_CR1_UDIS; + __DMB(); + if ((timer->SR & TIM_SR_UIF) != 0UL) + { + timer->CR1 &= ~TIM_CR1_UDIS; + PlsrHandleTimerIrq(pulseOutput); + if ((PlsrTimerQueueGeneration[pulseOutput] != generationBefore) + || ((timer->CR1 & TIM_CR1_CEN) == 0UL)) + { + uint8_t stillRunning = + ((timer->CR1 & TIM_CR1_CEN) != 0UL) ? 1U : 0U; + + *actualFrequencyHz = PlsrTimerQueuedFrequencyHz[pulseOutput]; + PlsrPlatformExitCritical(criticalState); + return stillRunning; + } + counterBefore = timer->CNT; + timer->CR1 |= TIM_CR1_UDIS; + __DMB(); + } + PlsrTimerWriteSetting(timer, &setting); + __DMB(); + PlsrTimerQueuedFrequencyHz[pulseOutput] = setting.actualFrequencyHz; + PlsrTimerQueueGeneration[pulseOutput]++; + ownGeneration = PlsrTimerQueueGeneration[pulseOutput]; + counterAfter = timer->CNT; + timer->CR1 &= ~TIM_CR1_UDIS; + __DMB(); + counterFinal = timer->CNT; + + /* With UDIS set an overflow does not set UIF. A wrapped counter proves + that its real output edge occurred, so account for that edge once. */ + wrappedWhileUpdatesDisabled = + ((counterAfter < counterBefore) + || ((counterFinal < counterAfter) + && ((timer->SR & TIM_SR_UIF) == 0UL))) ? 1U : 0U; + if (wrappedWhileUpdatesDisabled != 0U) + { + PlsrPulseTimerIrq(pulseOutput); + } + else if ((timer->SR & TIM_SR_UIF) != 0UL) + { + PlsrHandleTimerIrq(pulseOutput); + } + *actualFrequencyHz = + (PlsrTimerQueueGeneration[pulseOutput] == ownGeneration) + ? setting.actualFrequencyHz + : PlsrTimerQueuedFrequencyHz[pulseOutput]; + PlsrPlatformExitCritical(criticalState); + return 1U; +} + +void PlsrPlatformDrainPendingPulse(uint8_t pulseOutput) +{ + if (pulseOutput <= 3U) + { + PlsrHandleTimerIrq(pulseOutput); + } +} + +uint32_t PlsrPlatformActiveFrequency(uint8_t pulseOutput) +{ + return (pulseOutput <= 3U) + ? PlsrTimerActiveFrequencyHz[pulseOutput] : 0UL; +} + +void PlsrPlatformStopPulse(uint8_t pulseOutput) +{ + if (pulseOutput <= 3U) + { + PlsrPulsePinCaptureIdle(pulseOutput); + PlsrTimerStop(PlsrTimerMap[pulseOutput].timer); + PlsrTimerActiveFrequencyHz[pulseOutput] = 0UL; + PlsrTimerQueuedFrequencyHz[pulseOutput] = 0UL; + PlsrTimerQueueGeneration[pulseOutput]++; + } +} + +uint8_t PlsrPlatformReadInput(uint8_t inputSelection) +{ + if (inputSelection == 0U) + { + return (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_SET) ? 1U : 0U; + } + if (inputSelection == 1U) + { + return (HAL_GPIO_ReadPin(GPIOG, GPIO_PIN_12) == GPIO_PIN_SET) ? 1U : 0U; + } + return 0U; +} + +uint8_t PlsrPlatformLoad(PLSR_PERSIST_PAYLOAD *payload) +{ + const PLSR_FLASH_RECORD *slotA = + (const PLSR_FLASH_RECORD *)PLSR_FLASH_SLOT_A_ADDRESS; + const PLSR_FLASH_RECORD *slotB = + (const PLSR_FLASH_RECORD *)PLSR_FLASH_SLOT_B_ADDRESS; + const PLSR_FLASH_RECORD *selected = NULL; + const PLSR_BACKUP_CONFIG_RECORD *backupConfig = + (const PLSR_BACKUP_CONFIG_RECORD *)PLSR_BACKUP_CONFIG_ADDRESS; + const PLSR_BACKUP_POSITION_RECORD *backupPosition; + uint8_t validA; + uint8_t validB; + uint8_t haveConfig = 0U; + + if (payload == NULL) + { + return 0U; + } + + validA = PlsrFlashRecordIsValid(slotA); + validB = PlsrFlashRecordIsValid(slotB); + if ((validA != 0U) && (validB != 0U)) + { + selected = (PlsrGenerationIsNewer(slotB->generation, + slotA->generation) != 0U) + ? slotB : slotA; + } + else if (validA != 0U) + { + selected = slotA; + } + else if (validB != 0U) + { + selected = slotB; + } + + if (selected != NULL) + { + *payload = selected->payload; + haveConfig = 1U; + } + else + { + (void)memset(payload, 0, sizeof(*payload)); + } + + if (PlsrBackupConfigIsValid(backupConfig) != 0U) + { + payload->config = backupConfig->config; + haveConfig = 1U; + } + backupPosition = PlsrNewestBackupPosition(); + if (backupPosition != NULL) + { + payload->position = backupPosition->position; + payload->positionValid = backupPosition->positionValid; + payload->wasBusy = backupPosition->wasBusy; + } + return haveConfig; +} + +uint8_t PlsrPlatformSave(const PLSR_PERSIST_PAYLOAD *payload) +{ + const PLSR_FLASH_RECORD *slotA = + (const PLSR_FLASH_RECORD *)PLSR_FLASH_SLOT_A_ADDRESS; + const PLSR_FLASH_RECORD *slotB = + (const PLSR_FLASH_RECORD *)PLSR_FLASH_SLOT_B_ADDRESS; + uint8_t validA; + uint8_t validB; + uint32_t newestGeneration = 0UL; + uint32_t targetAddress; + uint32_t targetSector; + uint32_t sectorError; + uint32_t index; + uint32_t wordCount; + const uint32_t *words; + FLASH_EraseInitTypeDef erase; + HAL_StatusTypeDef status = HAL_OK; + + if (payload == NULL) + { + return 0U; + } + + validA = PlsrFlashRecordIsValid(slotA); + validB = PlsrFlashRecordIsValid(slotB); + if ((validA != 0U) && (validB != 0U)) + { + if (PlsrGenerationIsNewer(slotB->generation, slotA->generation) != 0U) + { + newestGeneration = slotB->generation; + targetAddress = PLSR_FLASH_SLOT_A_ADDRESS; + targetSector = FLASH_SECTOR_10; + } + else + { + newestGeneration = slotA->generation; + targetAddress = PLSR_FLASH_SLOT_B_ADDRESS; + targetSector = FLASH_SECTOR_11; + } + } + else if (validA != 0U) + { + newestGeneration = slotA->generation; + targetAddress = PLSR_FLASH_SLOT_B_ADDRESS; + targetSector = FLASH_SECTOR_11; + } + else if (validB != 0U) + { + newestGeneration = slotB->generation; + targetAddress = PLSR_FLASH_SLOT_A_ADDRESS; + targetSector = FLASH_SECTOR_10; + } + else + { + targetAddress = PLSR_FLASH_SLOT_A_ADDRESS; + targetSector = FLASH_SECTOR_10; + } + + (void)memset(&PlsrFlashRecordBuffer, 0, sizeof(PlsrFlashRecordBuffer)); + PlsrFlashRecordBuffer.magic = PLSR_FLASH_MAGIC; + PlsrFlashRecordBuffer.version = PLSR_FLASH_VERSION; + PlsrFlashRecordBuffer.payloadSize = sizeof(PLSR_PERSIST_PAYLOAD); + PlsrFlashRecordBuffer.generation = newestGeneration + 1UL; + PlsrFlashRecordBuffer.payload = *payload; + PlsrFlashRecordBuffer.crc32 = PlsrFlashRecordCrc(&PlsrFlashRecordBuffer); + + if (HAL_FLASH_Unlock() != HAL_OK) + { + return 0U; + } + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR + | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR + | FLASH_FLAG_PGSERR); + erase.TypeErase = FLASH_TYPEERASE_SECTORS; + erase.VoltageRange = FLASH_VOLTAGE_RANGE_3; + erase.Sector = targetSector; + erase.NbSectors = 1U; + if (HAL_FLASHEx_Erase(&erase, §orError) != HAL_OK) + { + status = HAL_ERROR; + } + + words = (const uint32_t *)&PlsrFlashRecordBuffer; + wordCount = sizeof(PlsrFlashRecordBuffer) / sizeof(uint32_t); + if (status == HAL_OK) + { + for (index = 1UL; index < wordCount; index++) + { + if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, + targetAddress + index * 4UL, + words[index]) != HAL_OK) + { + status = HAL_ERROR; + break; + } + } + } + if ((status == HAL_OK) + && (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, targetAddress, + PLSR_FLASH_MAGIC) != HAL_OK)) + { + status = HAL_ERROR; + } + if (HAL_FLASH_Lock() != HAL_OK) + { + status = HAL_ERROR; + } + + if ((status == HAL_OK) + && (PlsrFlashRecordIsValid( + (const PLSR_FLASH_RECORD *)targetAddress) != 0U)) + { + return 1U; + } + return 0U; +} + +void PlsrPlatformCheckpointConfig(const PLSR_CONFIG *config) +{ + PLSR_BACKUP_CONFIG_RECORD *record = + (PLSR_BACKUP_CONFIG_RECORD *)PLSR_BACKUP_CONFIG_ADDRESS; + + if (config == NULL) + { + return; + } + record->magic = 0UL; + record->config = *config; + record->crc32 = PlsrCrc32(&record->config, sizeof(record->config)); + __DMB(); + record->magic = PLSR_BACKUP_CONFIG_MAGIC; + __DMB(); +} + +void PlsrPlatformCheckpointPosition(int32_t position, + uint8_t positionValid, + uint8_t wasBusy) +{ + PLSR_BACKUP_POSITION_RECORD *slots = + (PLSR_BACKUP_POSITION_RECORD *)PLSR_BACKUP_POSITION_ADDRESS; + PLSR_BACKUP_POSITION_RECORD *record; + + PlsrBackupPositionGeneration++; + record = &slots[PlsrBackupPositionGeneration & 1UL]; + record->magic = 0UL; + record->generation = PlsrBackupPositionGeneration; + record->position = position; + record->positionValid = (positionValid != 0U) ? 1U : 0U; + record->wasBusy = (wasBusy != 0U) ? 1U : 0U; + record->reserved = 0U; + record->crc32 = PlsrCrc32(&record->generation, + sizeof(record->generation) + + sizeof(record->position) + + sizeof(record->positionValid) + + sizeof(record->wasBusy) + + sizeof(record->reserved)); + __DMB(); + record->magic = PLSR_BACKUP_POSITION_MAGIC; + __DMB(); +} + +uint32_t PlsrPlatformEnterCritical(void) +{ + uint32_t state = __get_PRIMASK(); + __disable_irq(); + __DMB(); + return state; +} + +void PlsrPlatformExitCritical(uint32_t state) +{ + __DMB(); + if (state == 0UL) + { + __enable_irq(); + } +} + +static void PlsrHandleTimerIrq(uint8_t pulseOutput) +{ + TIM_TypeDef *timer = PlsrTimerMap[pulseOutput].timer; +#if PLSR_ENABLE_IRQ_CYCLE_DIAG + uint32_t startedAt; + uint32_t elapsedCycles; +#endif + + if (((timer->SR & TIM_SR_UIF) != 0UL) + && ((timer->DIER & TIM_DIER_UIE) != 0UL)) + { +#if PLSR_ENABLE_IRQ_CYCLE_DIAG + startedAt = DWT->CYCCNT; +#endif + timer->SR = ~TIM_SR_UIF; + PlsrTimerActiveFrequencyHz[pulseOutput] = + PlsrTimerQueuedFrequencyHz[pulseOutput]; + PlsrPulseTimerIrq(pulseOutput); +#if PLSR_ENABLE_IRQ_CYCLE_DIAG + elapsedCycles = DWT->CYCCNT - startedAt; + PlsrIrqCount[pulseOutput]++; + PlsrIrqLastCycles[pulseOutput] = elapsedCycles; + if (elapsedCycles > PlsrIrqMaxCycles[pulseOutput]) + { + PlsrIrqMaxCycles[pulseOutput] = elapsedCycles; + } +#endif + } +} + +void TIM1_UP_TIM10_IRQHandler(void) +{ + PlsrHandleTimerIrq(0U); +} + +void TIM8_UP_TIM13_IRQHandler(void) +{ + PlsrHandleTimerIrq(1U); +} + +void TIM1_TRG_COM_TIM11_IRQHandler(void) +{ + PlsrHandleTimerIrq(2U); +} + +void TIM8_TRG_COM_TIM14_IRQHandler(void) +{ + PlsrHandleTimerIrq(3U); +} + +#endif /* PLSR_HOST_TEST */ diff --git a/tests/plsr_host/run_tests.ps1 b/tests/plsr_host/run_tests.ps1 new file mode 100644 index 0000000..9c2cd76 --- /dev/null +++ b/tests/plsr_host/run_tests.ps1 @@ -0,0 +1,42 @@ +$ErrorActionPreference = "Stop" + +$gcc = "D:\Dev-Cpp\MinGW64\bin\gcc.exe" +$repoRoot = (Resolve-Path (Join-Path $PSScriptRoot "..\..")).Path +$temporaryExe = Join-Path ([System.IO.Path]::GetTempPath()) ` + ("plsr_host_tests_{0}.exe" -f [System.Guid]::NewGuid().ToString("N")) +$exitCode = 1 + +if (-not (Test-Path -LiteralPath $gcc)) { + throw "Required compiler not found: $gcc" +} + +$compileArguments = @( + "-std=c99" + "-Wall" + "-Wextra" + "-Werror" + "-DPLSR_HOST_TEST" + "-I$repoRoot\PLSR\Inc" + "-I$repoRoot\PLSR\Src" + "$repoRoot\PLSR\Src\plsr.c" + "$repoRoot\PLSR\Src\plsr_platform_f407.c" + "$PSScriptRoot\test_plsr_host.c" + "-o" + $temporaryExe +) + +try { + & $gcc @compileArguments + if ($LASTEXITCODE -ne 0) { + $exitCode = $LASTEXITCODE + } + else { + & $temporaryExe + $exitCode = $LASTEXITCODE + } +} +finally { + Remove-Item -LiteralPath $temporaryExe -Force -ErrorAction SilentlyContinue +} + +exit $exitCode diff --git a/tests/plsr_host/test_plsr_host.c b/tests/plsr_host/test_plsr_host.c new file mode 100644 index 0000000..033b6bd --- /dev/null +++ b/tests/plsr_host/test_plsr_host.c @@ -0,0 +1,1964 @@ +#include "plsr.h" + +#include +#include + +#define PLSR_WAIT_TIME (0U) +#define PLSR_WAIT_SIGNAL (1U) +#define PLSR_ACT_TIME (2U) +#define PLSR_EXT_SIGNAL (3U) +#define PLSR_EXT_OR_COMPLETE (4U) + +#define PLSR_SEND_COMPLETE (0U) +#define PLSR_SEND_SUBSEQUENT (1U) +#define PLSR_POSITION_RELATIVE (0U) +#define PLSR_POSITION_ABSOLUTE (1U) + +#define PLSR_COMMAND_START (0x0001U) +#define PLSR_COMMAND_STOP (0x0002U) +#define PLSR_COMMAND_CLEAR (0x0004U) +#define PLSR_ERROR_COUNT (5U) + +static const char *CurrentTest; +static unsigned int AssertionCount; +static unsigned int FailureCount; + +static void ExpectUnsigned(unsigned long expected, + unsigned long actual, + const char *expression, + int line) +{ + AssertionCount++; + if (expected != actual) + { + FailureCount++; + (void)printf("FAIL %s:%d: %s expected %lu, got %lu\n", + CurrentTest, line, expression, expected, actual); + } +} + +static void ExpectSigned(long expected, + long actual, + const char *expression, + int line) +{ + AssertionCount++; + if (expected != actual) + { + FailureCount++; + (void)printf("FAIL %s:%d: %s expected %ld, got %ld\n", + CurrentTest, line, expression, expected, actual); + } +} + +static void ExpectTrue(int condition, const char *expression, int line) +{ + AssertionCount++; + if (!condition) + { + FailureCount++; + (void)printf("FAIL %s:%d: expected true: %s\n", + CurrentTest, line, expression); + } +} + +#define EXPECT_U(expected, actual) \ + ExpectUnsigned((unsigned long)(expected), \ + (unsigned long)(actual), #actual, __LINE__) +#define EXPECT_I(expected, actual) \ + ExpectSigned((long)(expected), (long)(actual), #actual, __LINE__) +#define EXPECT_TRUE(expression) ExpectTrue((expression), #expression, __LINE__) + +static uint16_t LowWord(uint32_t value) +{ + return (uint16_t)(value & 0xFFFFUL); +} + +static uint16_t HighWord(uint32_t value) +{ + return (uint16_t)(value >> 16U); +} + +static void TestU64ByU32Division(void) +{ + static const uint64_t dividends[] = + { + 0ULL, 1ULL, 0xFFFFFFFFULL, 0x100000000ULL, + 0x100000001ULL, 0xFFFFFFFFFFFFFFFFULL, + 0x8000000000000000ULL, 0x7FFFFFFFFFFFFFFFULL + }; + static const uint32_t divisors[] = + { + 1UL, 2UL, 3UL, 0xFFFFUL, 0x10000UL, 100000UL, + 0x7FFFFFFFUL, 0x80000000UL, 0xFFFFFFFFUL + }; + uint32_t state = 0xA5C39E17UL; + uint64_t dividend; + uint64_t quotient; + uint32_t divisor; + uint32_t remainder; + uint32_t dividendIndex; + uint32_t divisorIndex; + uint32_t iteration; + + for (dividendIndex = 0U; + dividendIndex < (uint32_t)(sizeof(dividends) / sizeof(dividends[0])); + dividendIndex++) + { + for (divisorIndex = 0U; + divisorIndex < (uint32_t)(sizeof(divisors) / sizeof(divisors[0])); + divisorIndex++) + { + dividend = dividends[dividendIndex]; + divisor = divisors[divisorIndex]; + quotient = PlsrTestDivideU64ByU32(dividend, divisor, &remainder); + EXPECT_TRUE(quotient == dividend / divisor); + EXPECT_U((uint32_t)(dividend % divisor), remainder); + } + } + + for (iteration = 0U; iteration < 250000UL; iteration++) + { + state = state * 1664525UL + 1013904223UL; + dividend = (uint64_t)state << 32U; + state = state * 1664525UL + 1013904223UL; + dividend |= state; + state = state * 1664525UL + 1013904223UL; + divisor = state | 1UL; + + quotient = PlsrTestDivideU64ByU32(dividend, divisor, &remainder); + EXPECT_TRUE(quotient == dividend / divisor); + EXPECT_U((uint32_t)(dividend % divisor), remainder); + } +} + +static uint64_t PulsePeriodNs(uint32_t frequencyHz) +{ + return (1000000000ULL + frequencyHz / 2UL) / frequencyHz; +} + +static uint64_t UnsignedDifference64(uint64_t first, uint64_t second) +{ + return (first > second) ? (first - second) : (second - first); +} + +static uint32_t IntegerSquareRoot64(uint64_t value) +{ + uint64_t bit = (uint64_t)1U << 62U; + uint64_t root = 0ULL; + + while (bit > value) + { + bit >>= 2U; + } + while (bit != 0ULL) + { + if (value >= root + bit) + { + value -= root + bit; + root = (root >> 1U) + bit; + } + else + { + root >>= 1U; + } + bit >>= 2U; + } + return (uint32_t)root; +} + +static PLSR_MB_RESULT WriteWord(uint16_t address, uint16_t value) +{ + return PlsrModbusWriteHolding(address, 1U, &value); +} + +static PLSR_MB_RESULT WriteU32(uint16_t address, uint32_t value) +{ + uint16_t words[2]; + + words[0] = LowWord(value); + words[1] = HighWord(value); + return PlsrModbusWriteHolding(address, 2U, words); +} + +static PLSR_MB_RESULT WriteI32(uint16_t address, int32_t value) +{ + return WriteU32(address, (uint32_t)value); +} + +static uint16_t ReadWord(uint16_t address) +{ + uint16_t value = 0xDEADU; + PLSR_MB_RESULT result = PlsrModbusReadHolding(address, 1U, &value); + + EXPECT_U(PLSR_MB_OK, result); + return value; +} + +static uint32_t ReadU32(uint16_t address) +{ + uint16_t words[2] = {0xDEADU, 0xBEEFU}; + PLSR_MB_RESULT result = PlsrModbusReadHolding(address, 2U, words); + + EXPECT_U(PLSR_MB_OK, result); + return (uint32_t)words[0] | ((uint32_t)words[1] << 16U); +} + +static int32_t ReadPosition(void) +{ + return (int32_t)ReadU32(PLSR_STATUS_FIRST_ADDRESS); +} + +static uint32_t ReadFrequency(void) +{ + return ReadU32((uint16_t)(PLSR_STATUS_FIRST_ADDRESS + 2U)); +} + +static uint16_t ReadStatus(void) +{ + return ReadWord((uint16_t)(PLSR_STATUS_FIRST_ADDRESS + 4U)); +} + +static uint16_t ReadCurrentSegment(void) +{ + return ReadWord((uint16_t)(PLSR_STATUS_FIRST_ADDRESS + 5U)); +} + +static uint16_t ReadError(void) +{ + return ReadWord((uint16_t)(PLSR_STATUS_FIRST_ADDRESS + 6U)); +} + +static PLSR_MB_RESULT QueueCommand(uint16_t command) +{ + return PlsrModbusWriteHolding(PLSR_CONTROL_ADDRESS, 1U, &command); +} + +static PLSR_MB_RESULT SendCommand(uint16_t command) +{ + PLSR_MB_RESULT result = QueueCommand(command); + + if (result == PLSR_MB_OK) + { + PlsrPoll1ms(); + } + return result; +} + +static void ResetCore(void) +{ + PlsrTestClearPersistentStorage(); + EXPECT_U(1U, PlsrInit()); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); +} + +static void ConfigureCommon(uint16_t curveMode, + uint16_t positionMode, + uint16_t segmentCount, + uint16_t sendMode, + uint32_t defaultSpeedHz, + uint32_t startSpeedHz, + uint32_t stopSpeedHz, + uint16_t accelerationTimeMs, + uint16_t decelerationTimeMs) +{ + uint16_t words[0x14U]; + PLSR_MB_RESULT result; + + result = PlsrModbusReadHolding(0x1000U, 0x14U, words); + EXPECT_U(PLSR_MB_OK, result); + words[0x04U] = sendMode; + words[0x05U] = 0U; + words[0x07U] = curveMode; + words[0x08U] = positionMode; + words[0x09U] = segmentCount; + words[0x0AU] = 1U; + words[0x0BU] = LowWord(defaultSpeedHz); + words[0x0CU] = HighWord(defaultSpeedHz); + words[0x0DU] = LowWord(startSpeedHz); + words[0x0EU] = HighWord(startSpeedHz); + words[0x0FU] = 0U; + words[0x10U] = LowWord(stopSpeedHz); + words[0x11U] = HighWord(stopSpeedHz); + words[0x12U] = accelerationTimeMs; + words[0x13U] = decelerationTimeMs; + result = PlsrModbusWriteHolding(0x1000U, 0x14U, words); + EXPECT_U(PLSR_MB_OK, result); +} + +static PLSR_MB_RESULT SetSegment(uint8_t segmentNumber, + uint32_t frequencyHz, + int32_t pulses, + uint16_t waitType, + uint16_t waitTimeMs, + uint16_t actTimeMs, + uint16_t jumpSegment) +{ + uint16_t words[8]; + uint16_t address = (uint16_t)(0x1100U + + ((uint16_t)segmentNumber - 1U) * 0x10U); + + words[0] = LowWord(frequencyHz); + words[1] = HighWord(frequencyHz); + words[2] = LowWord((uint32_t)pulses); + words[3] = HighWord((uint32_t)pulses); + words[4] = waitType; + words[5] = waitTimeMs; + words[6] = actTimeMs; + words[7] = jumpSegment; + return PlsrModbusWriteHolding(address, 8U, words); +} + +static void StopAndSettle(void) +{ + unsigned int pulse; + + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_STOP)); + for (pulse = 0U; (pulse < 4U) && (PlsrTestPulseIsActive() != 0U); + pulse++) + { + PlsrTestEmitPulses(1UL); + } + PlsrPoll1ms(); + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); +} + +static void TestProtocolBoundaries(void) +{ + uint16_t value = 0U; + uint16_t values[2]; + uint16_t before; + + ResetCore(); + + EXPECT_U(PLSR_MB_NOT_HANDLED, + PlsrModbusReadHolding(0x0FFEU, 1U, &value)); + EXPECT_U(PLSR_MB_NOT_HANDLED, + PlsrModbusReadHolding(0x1198U, 1U, &value)); + EXPECT_U(PLSR_MB_ILLEGAL_ADDRESS, + PlsrModbusReadHolding(0x0FFFU, 2U, values)); + EXPECT_U(PLSR_MB_ILLEGAL_ADDRESS, + PlsrModbusReadHolding(0x1197U, 2U, values)); + EXPECT_U(PLSR_MB_ILLEGAL_ADDRESS, + PlsrModbusReadHolding(0x2006U, 2U, values)); + EXPECT_U(PLSR_MB_ILLEGAL_ADDRESS, + PlsrModbusReadHolding(0xFFFFU, 2U, values)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, + PlsrModbusReadHolding(0x1000U, 0U, &value)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, + PlsrModbusReadHolding(0x1000U, 1U, NULL)); + + EXPECT_U(0U, ReadWord(0x100FU)); + EXPECT_U(PLSR_MB_OK, WriteWord(0x100FU, 0U)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, WriteWord(0x100FU, 1U)); + EXPECT_U(0U, ReadWord(0x1108U)); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1108U, 0U)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, WriteWord(0x1108U, 1U)); + + EXPECT_U(PLSR_MB_ILLEGAL_ADDRESS, WriteWord(0x100BU, 2000U)); + EXPECT_U(PLSR_MB_OK, WriteU32(0x100BU, 2000UL)); + EXPECT_U(2000UL, ReadU32(0x100BU)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, WriteU32(0x100BU, 100001UL)); + EXPECT_U(2000UL, ReadU32(0x100BU)); + + before = ReadWord(0x1013U); + values[0] = 77U; + values[1] = 1U; + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, + PlsrModbusWriteHolding(0x1013U, 2U, values)); + EXPECT_U(before, ReadWord(0x1013U)); + + EXPECT_U(PLSR_MB_ILLEGAL_ADDRESS, + PlsrModbusWriteHolding(0x2000U, 1U, &value)); + EXPECT_U(PLSR_MB_ILLEGAL_ADDRESS, + PlsrModbusWriteHolding(0x3000U, 2U, values)); + value = 3U; + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, + PlsrModbusWriteHolding(0x3000U, 1U, &value)); + value = 0U; + EXPECT_U(PLSR_MB_OK, + PlsrModbusWriteHolding(0x3000U, 1U, &value)); + EXPECT_U(0U, ReadWord(0x3000U)); + + EXPECT_U(PLSR_MB_OK, WriteWord(0x1197U, 0U)); + EXPECT_U(0U, ReadWord(0x1197U)); + EXPECT_U(PLSR_MB_NOT_HANDLED, WriteWord(0x1198U, 0U)); +} + +static void TestWaitZeroUsesOneMillisecond(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_WAIT_TIME, 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(1UL); + + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_WAITING, ReadStatus()); + EXPECT_U(1U, ReadCurrentSegment()); + EXPECT_U(0U, PlsrTestPulseIsActive()); + + PlsrPoll1ms(); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(2L, ReadPosition()); +} + +static void TestActZeroSkipsWithoutPulse(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 100L, PLSR_ACT_TIME, 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_U(1U, ReadCurrentSegment()); + EXPECT_I(0L, ReadPosition()); + + PlsrPoll1ms(); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_I(0L, ReadPosition()); + PlsrTestEmitPulses(1UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(1L, ReadPosition()); +} + +static void TestRelativeAndAbsoluteZeroDisplacement(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 0L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_STATUS_RUNNING, ReadStatus()); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(0L, ReadPosition()); + + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 5L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(5UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(5L, ReadPosition()); + + EXPECT_U(PLSR_MB_OK, WriteWord(0x1008U, PLSR_POSITION_ABSOLUTE)); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 5L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(5L, ReadPosition()); +} + +static void TestSelfJumpCanStop(void) +{ + unsigned int loop; + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 2L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 1U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + + for (loop = 0U; loop < 3U; loop++) + { + PlsrTestEmitPulses(2UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_RUNNING, ReadStatus()); + EXPECT_U(1U, ReadCurrentSegment()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + } + + StopAndSettle(); +} + +static void TestStopDeceleratesBeforeCut(void) +{ + unsigned int tick; + uint32_t middleFrequency; + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 10000L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_STOP)); + EXPECT_U(PLSR_STATUS_DECELERATING, ReadStatus()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + + for (tick = 0U; tick < 450U; tick++) + { + PlsrPoll1ms(); + PlsrTestEmitPulses(1UL); + } + middleFrequency = PlsrTestOutputFrequency(); + EXPECT_TRUE((middleFrequency > 100UL) && (middleFrequency < 1000UL)); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_STOP)); + + for (tick = 450U; tick < 900U; tick++) + { + PlsrPoll1ms(); + PlsrTestEmitPulses(1UL); + } + EXPECT_TRUE(PlsrTestOutputFrequency() > 100UL); + EXPECT_TRUE(PlsrTestOutputFrequency() <= 103UL); + EXPECT_U(PLSR_STATUS_DECELERATING, ReadStatus()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + EXPECT_U(100UL, PlsrTestOutputFrequency()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + EXPECT_I(902L, ReadPosition()); +} + +static void TestStopAtPendingBoundary(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_STOP)); + EXPECT_U(PLSR_STATUS_RUNNING, ReadStatus()); + + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + EXPECT_U(0U, ReadCurrentSegment()); + EXPECT_I(1L, ReadPosition()); + + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + EXPECT_U(0U, PlsrTestPulseIsActive()); +} + +static void TestOneMillisecondDirectionDelay(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1005U, 1U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 10L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(0U, PlsrTestPulseIsActive()); + + PlsrPoll1ms(); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + StopAndSettle(); +} + +static void TestExtEdgeAtNaturalBoundary(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + PlsrTestSetInput(0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_SIGNAL, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + + PlsrTestEmitPulseOnCriticalEntry(); + PlsrTestSetInput(0U, 1U); + PlsrPoll1ms(); + EXPECT_U(0U, PlsrTestPulseIsActive()); + + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(0U, ReadCurrentSegment()); + EXPECT_I(1L, ReadPosition()); +} + +static void ConfigureExtBoundaryCarryRace(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 2000UL, 500UL, 1000UL, + 0U, 1000U); + PlsrTestSetInput(0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 2000UL, 1L, PLSR_EXT_SIGNAL, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 1000UL, 70000L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(2000UL, PlsrTestOutputFrequency()); + PlsrTestSetInput(0U, 1U); +} + +static void ExpectExtBoundaryCarry(void) +{ + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_U(1U, ReadCurrentSegment()); + PlsrPoll1ms(); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(2000UL, PlsrTestOutputFrequency()); + EXPECT_I(1L, ReadPosition()); +} + +static void TestExtCutAndNaturalBoundaryInterleavings(void) +{ + ConfigureExtBoundaryCarryRace(); + PlsrTestEmitPulseAfterCriticalEntries(3U); + PlsrPoll1ms(); + ExpectExtBoundaryCarry(); + + ConfigureExtBoundaryCarryRace(); + PlsrPoll1ms(); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + ExpectExtBoundaryCarry(); +} + +static void TestExtEdgeDuringDirectionDelay(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1005U, 2U)); + PlsrTestSetInput(0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_SIGNAL, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + + PlsrTestSetInput(0U, 1U); + PlsrPoll1ms(); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(1L, ReadPosition()); +} + +static void TestStoppedTaskClearsPendingExtEdge(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1005U, 2U)); + PlsrTestSetInput(0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 100L, PLSR_EXT_SIGNAL, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(0U, PlsrTestPulseIsActive()); + + PlsrTestSetInput(0U, 1U); + PlsrPoll1ms(); + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_STOP)); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + + PlsrTestSetInput(0U, 0U); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_I(0L, ReadPosition()); + StopAndSettle(); +} + +static void TestExtEdgeDoesNotCutSeamlessNextSegment(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 1000UL, 1000UL, 100UL, 0U, 0U); + PlsrTestSetInput(0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 2000UL, 4L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + + PlsrTestSetInput(0U, 1U); + PlsrTestEmitPulseAfterCriticalEntries(3U); + PlsrPoll1ms(); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(2000UL, PlsrTestOutputFrequency()); + EXPECT_I(1L, ReadPosition()); + + PlsrTestEmitPulses(4UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(5L, ReadPosition()); +} + +static void TestOldRampDoesNotRetargetSeamlessNextSegment(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 2000UL, 1000UL, 100UL, 100U, 0U); + PlsrTestSetInput(0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 2000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 3000UL, 4L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + + PlsrTestEmitPulseAfterCriticalEntries(3U); + PlsrPoll1ms(); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(3000UL, PlsrTestOutputFrequency()); + EXPECT_I(1L, ReadPosition()); + + PlsrTestEmitPulses(4UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(5L, ReadPosition()); +} + +static void TestShortProfileBoundaryMatrix(void) +{ + static const uint16_t pulseCounts[] = {1U, 2U, 10U, 99U, 100U, 101U}; + uint32_t samples[3][101]; + uint64_t previousDurationNs[3] = {0ULL, 0ULL, 0ULL}; + uint64_t durationNs[3]; + unsigned int countIndex; + uint16_t curveMode; + unsigned int pulse; + unsigned int firstPeak; + unsigned int lastPeak; + uint32_t peakFrequencyHz; + uint32_t expectedPeakHz; + uint64_t expectedDurationNs; + uint64_t toleranceNs; + uint8_t linearDiffersFromSmooth; + uint8_t smoothDiffersFromSine; + uint8_t anyLinearDiffersFromSmooth = 0U; + uint8_t anySmoothDiffersFromSine = 0U; + + for (countIndex = 0U; + countIndex < sizeof(pulseCounts) / sizeof(pulseCounts[0]); + countIndex++) + { + for (curveMode = 0U; curveMode <= 2U; curveMode++) + { + ResetCore(); + ConfigureCommon(curveMode, PLSR_POSITION_RELATIVE, 1U, + PLSR_SEND_COMPLETE, 1000UL, 100UL, 100UL, + 1000U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, pulseCounts[countIndex], + PLSR_EXT_OR_COMPLETE, 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(1U, PlsrTestPulseIsActive()); + durationNs[curveMode] = 0ULL; + + for (pulse = 0U; pulse < pulseCounts[countIndex]; pulse++) + { + samples[curveMode][pulse] = PlsrTestOutputFrequency(); + EXPECT_TRUE(samples[curveMode][pulse] >= 100UL); + EXPECT_TRUE(samples[curveMode][pulse] <= 1000UL); + durationNs[curveMode] += + PulsePeriodNs(samples[curveMode][pulse]); + PlsrTestEmitPulses(1UL); + if ((pulse + 1U) < pulseCounts[countIndex]) + { + EXPECT_U(1U, PlsrTestPulseIsActive()); + } + } + + peakFrequencyHz = samples[curveMode][0]; + firstPeak = 0U; + lastPeak = 0U; + for (pulse = 1U; pulse < pulseCounts[countIndex]; pulse++) + { + if (samples[curveMode][pulse] > peakFrequencyHz) + { + peakFrequencyHz = samples[curveMode][pulse]; + firstPeak = pulse; + lastPeak = pulse; + } + else if (samples[curveMode][pulse] == peakFrequencyHz) + { + lastPeak = pulse; + } + } + for (pulse = 1U; pulse <= firstPeak; pulse++) + { + EXPECT_TRUE(samples[curveMode][pulse] + >= samples[curveMode][pulse - 1U]); + } + for (pulse = firstPeak + 1U; pulse <= lastPeak; pulse++) + { + EXPECT_U(peakFrequencyHz, samples[curveMode][pulse]); + } + for (pulse = lastPeak + 1U; + pulse < pulseCounts[countIndex]; pulse++) + { + EXPECT_TRUE(samples[curveMode][pulse] + <= samples[curveMode][pulse - 1U]); + } + EXPECT_TRUE((samples[curveMode][0] + > samples[curveMode][pulseCounts[countIndex] - 1U] + ? samples[curveMode][0] + - samples[curveMode][pulseCounts[countIndex] - 1U] + : samples[curveMode][pulseCounts[countIndex] - 1U] + - samples[curveMode][0]) <= 2UL); + + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I((long)pulseCounts[countIndex], ReadPosition()); + EXPECT_TRUE(durationNs[curveMode] + > previousDurationNs[curveMode]); + previousDurationNs[curveMode] = durationNs[curveMode]; + + expectedPeakHz = IntegerSquareRoot64( + 10000ULL + (uint64_t)pulseCounts[countIndex] * 1000ULL); + expectedDurationNs = + (uint64_t)2U * (expectedPeakHz - 100UL) * 1000000ULL; + toleranceNs = expectedDurationNs / 8ULL + 2000000ULL; + EXPECT_TRUE(UnsignedDifference64(durationNs[curveMode], + expectedDurationNs) + <= toleranceNs); + } + + toleranceNs = durationNs[0] / 50ULL + 1000000ULL; + EXPECT_TRUE(UnsignedDifference64(durationNs[0], durationNs[1]) + <= toleranceNs); + EXPECT_TRUE(UnsignedDifference64(durationNs[1], durationNs[2]) + <= toleranceNs); + + if (pulseCounts[countIndex] >= 10U) + { + linearDiffersFromSmooth = 0U; + smoothDiffersFromSine = 0U; + for (pulse = 0U; pulse < pulseCounts[countIndex]; pulse++) + { + if (samples[0][pulse] != samples[1][pulse]) + { + linearDiffersFromSmooth = 1U; + } + if (samples[1][pulse] != samples[2][pulse]) + { + smoothDiffersFromSine = 1U; + } + } + EXPECT_U(1U, linearDiffersFromSmooth); + if (linearDiffersFromSmooth != 0U) + { + anyLinearDiffersFromSmooth = 1U; + } + if (smoothDiffersFromSine != 0U) + { + anySmoothDiffersFromSine = 1U; + } + } + } + EXPECT_U(1U, anyLinearDiffersFromSmooth); + EXPECT_U(1U, anySmoothDiffersFromSine); +} + +static void TestShortFinalDeceleratesWithoutPoll(void) +{ + uint16_t curveMode; + unsigned int pulse; + uint32_t previousFrequencyHz; + uint32_t currentFrequencyHz; + uint64_t durationNs; + + for (curveMode = 0U; curveMode <= 2U; curveMode++) + { + ResetCore(); + ConfigureCommon(curveMode, PLSR_POSITION_RELATIVE, 1U, + PLSR_SEND_COMPLETE, 100000UL, 100000UL, 100UL, + 0U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 100000UL, 10L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + previousFrequencyHz = PlsrTestOutputFrequency(); + EXPECT_TRUE(previousFrequencyHz >= 99990UL); + EXPECT_TRUE(previousFrequencyHz <= 100000UL); + durationNs = PulsePeriodNs(previousFrequencyHz); + + for (pulse = 1U; pulse < 10U; pulse++) + { + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + currentFrequencyHz = PlsrTestOutputFrequency(); + EXPECT_TRUE(currentFrequencyHz <= previousFrequencyHz); + EXPECT_TRUE(currentFrequencyHz >= 99990UL); + durationNs += PulsePeriodNs(currentFrequencyHz); + previousFrequencyHz = currentFrequencyHz; + } + EXPECT_TRUE(durationNs >= 99990ULL); + EXPECT_TRUE(durationNs <= 100010ULL); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(10L, ReadPosition()); + } +} + +static void TestZeroToMaximumRampKeepsConfiguredDuration(void) +{ + uint16_t curveMode; + uint32_t firstFrequencyHz[3]; + uint32_t previousFrequencyHz; + uint32_t currentFrequencyHz; + uint32_t frequencyStepHz; + uint32_t maximumStepHz; + uint32_t pulse; + uint64_t durationNs; + const uint64_t expectedDurationNs = 20000000ULL; + const uint64_t toleranceNs = 100000ULL; + + for (curveMode = 0U; curveMode <= 2U; curveMode++) + { + ResetCore(); + ConfigureCommon(curveMode, PLSR_POSITION_RELATIVE, 1U, + PLSR_SEND_COMPLETE, 100000UL, 0UL, 100000UL, + 20U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 100000UL, 1000L, + PLSR_EXT_OR_COMPLETE, 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(1U, PlsrTestPulseIsActive()); + + previousFrequencyHz = PlsrTestOutputFrequency(); + firstFrequencyHz[curveMode] = previousFrequencyHz; + EXPECT_TRUE(previousFrequencyHz >= 400UL); + EXPECT_TRUE(previousFrequencyHz <= 2500UL); + maximumStepHz = 0UL; + durationNs = PulsePeriodNs(previousFrequencyHz); + + for (pulse = 1UL; pulse < 1000UL; pulse++) + { + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + currentFrequencyHz = PlsrTestOutputFrequency(); + EXPECT_TRUE(currentFrequencyHz >= previousFrequencyHz); + EXPECT_TRUE(currentFrequencyHz <= 100000UL); + frequencyStepHz = currentFrequencyHz - previousFrequencyHz; + if (frequencyStepHz > maximumStepHz) + { + maximumStepHz = frequencyStepHz; + } + durationNs += PulsePeriodNs(currentFrequencyHz); + previousFrequencyHz = currentFrequencyHz; + } + + EXPECT_TRUE(maximumStepHz <= 10000UL); + EXPECT_TRUE(previousFrequencyHz >= 99000UL); + EXPECT_TRUE(UnsignedDifference64(durationNs, expectedDurationNs) + <= toleranceNs); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(1000L, ReadPosition()); + } + EXPECT_TRUE(firstFrequencyHz[0] > firstFrequencyHz[1]); + EXPECT_TRUE(firstFrequencyHz[1] > firstFrequencyHz[2]); +} + +static void TestMaximumPulseRampKeepsReachableDuration(void) +{ + uint16_t curveMode; + uint32_t previousFrequencyHz; + uint32_t currentFrequencyHz; + uint32_t frequencyStepHz; + uint32_t maximumStepHz; + uint32_t pulse; + uint32_t reachablePeakHz = IntegerSquareRoot64( + (uint64_t)2U * 65535UL * 100000UL * 1000UL / 1311UL); + uint64_t durationNs; + uint64_t expectedDurationNs = + (uint64_t)2U * 65535UL * 1000000000ULL / reachablePeakHz; + uint64_t toleranceNs = expectedDurationNs / 500ULL + 1000000ULL; + + for (curveMode = 0U; curveMode <= 2U; curveMode++) + { + ResetCore(); + ConfigureCommon(curveMode, PLSR_POSITION_RELATIVE, 1U, + PLSR_SEND_COMPLETE, 100000UL, 0UL, 100000UL, + 1311U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 100000UL, 65535L, + PLSR_EXT_OR_COMPLETE, 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(1U, PlsrTestPulseIsActive()); + + previousFrequencyHz = PlsrTestOutputFrequency(); + EXPECT_TRUE(previousFrequencyHz > 1UL); + EXPECT_TRUE(previousFrequencyHz < reachablePeakHz); + maximumStepHz = 0UL; + durationNs = PulsePeriodNs(previousFrequencyHz); + + for (pulse = 1UL; pulse < 65535UL; pulse++) + { + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + currentFrequencyHz = PlsrTestOutputFrequency(); + EXPECT_TRUE(currentFrequencyHz >= previousFrequencyHz); + EXPECT_TRUE(currentFrequencyHz <= reachablePeakHz + 5UL); + frequencyStepHz = currentFrequencyHz - previousFrequencyHz; + if (frequencyStepHz > maximumStepHz) + { + maximumStepHz = frequencyStepHz; + } + durationNs += PulsePeriodNs(currentFrequencyHz); + previousFrequencyHz = currentFrequencyHz; + } + + EXPECT_TRUE(maximumStepHz <= 5000UL); + EXPECT_TRUE(previousFrequencyHz * 100UL + >= reachablePeakHz * 99UL); + EXPECT_TRUE(UnsignedDifference64(durationNs, expectedDurationNs) + <= toleranceNs); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(65535L, ReadPosition()); + } +} + +static void TestMaximumSingleRampWithoutPoll(void) +{ + uint16_t curveMode; + uint32_t previousFrequencyHz; + uint32_t currentFrequencyHz; + uint32_t minimumFrequencyHz; + uint32_t maximumFrequencyHz; + uint32_t pulse; + + for (curveMode = 0U; curveMode <= 2U; curveMode++) + { + ResetCore(); + ConfigureCommon(curveMode, PLSR_POSITION_RELATIVE, 1U, + PLSR_SEND_COMPLETE, 1000UL, 100000UL, 0UL, + 1000U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 100000UL, 65535L, + PLSR_EXT_OR_COMPLETE, 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + previousFrequencyHz = PlsrTestOutputFrequency(); + minimumFrequencyHz = previousFrequencyHz; + maximumFrequencyHz = previousFrequencyHz; + EXPECT_TRUE(previousFrequencyHz >= 99000UL); + EXPECT_TRUE(previousFrequencyHz <= 100000UL); + + for (pulse = 1UL; pulse < 65535UL; pulse++) + { + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + currentFrequencyHz = PlsrTestOutputFrequency(); + EXPECT_TRUE(currentFrequencyHz <= previousFrequencyHz); + EXPECT_TRUE(currentFrequencyHz >= 99000UL); + if (currentFrequencyHz < minimumFrequencyHz) + { + minimumFrequencyHz = currentFrequencyHz; + } + if (currentFrequencyHz > maximumFrequencyHz) + { + maximumFrequencyHz = currentFrequencyHz; + } + previousFrequencyHz = currentFrequencyHz; + } + EXPECT_TRUE(minimumFrequencyHz > 1UL); + EXPECT_TRUE(maximumFrequencyHz <= 100000UL); + EXPECT_TRUE(minimumFrequencyHz < maximumFrequencyHz); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(65535L, ReadPosition()); + } +} + +static void TestShortProfileTimerFailure(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 100UL, 100UL, 1000U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 10L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestFailNextFrequencyAtUpdate(); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_ERROR, ReadStatus()); + EXPECT_U(PLSR_ERROR_TIMER, ReadError()); + EXPECT_I(1L, ReadPosition()); +} + +static void TestStopImmediatelyAfterSubsequentHandoff(void) +{ + unsigned int tick; + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 1000UL, 1000UL, 100UL, + 0U, 100U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 3L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 2000UL, 20L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(3UL); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_TRUE(PlsrTestOutputFrequency() <= 2000UL); + EXPECT_TRUE(PlsrTestOutputFrequency() > 100UL); + + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_STOP)); + PlsrPoll1ms(); + PlsrTestEmitPulses(1UL); + EXPECT_U(PLSR_STATUS_DECELERATING, ReadStatus()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_TRUE(PlsrTestOutputFrequency() < 2000UL); + EXPECT_TRUE(PlsrTestOutputFrequency() > 100UL); + + for (tick = 1U; tick < 190U; tick++) + { + PlsrPoll1ms(); + } + while (PlsrTestPulseIsActive() != 0U) + { + PlsrTestEmitPulses(1UL); + } + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(7L, ReadPosition()); +} + +static void TestShortProfileZeroSpeedEdges(void) +{ + unsigned int tick; + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 0UL, 0UL, 1000U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 2L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_TRUE(PlsrTestOutputFrequency() > 0UL); + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_TRUE(PlsrTestOutputFrequency() > 0UL); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(2L, ReadPosition()); + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 0UL, 0UL, 1000U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + for (tick = 0U; (tick < 10U) && (PlsrTestPulseIsActive() == 0U); + tick++) + { + PlsrPoll1ms(); + } + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(1L, ReadPosition()); +} + +static void TestShortProfileDynamicRetarget(void) +{ + unsigned int tick; + uint32_t frequencyAfterRetarget; + int32_t positionBeforeStop; + int32_t positionAfterStop; + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 100UL, 100UL, 1000U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 50L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(3UL); + EXPECT_U(PLSR_MB_OK, WriteU32(0x1100U, 2000UL)); + PlsrPoll1ms(); + PlsrTestEmitPulses(1UL); + frequencyAfterRetarget = PlsrTestOutputFrequency(); + PlsrTestEmitPulses(1UL); + EXPECT_TRUE(PlsrTestOutputFrequency() > 0UL); + for (tick = 0U; tick < 10U; tick++) + { + PlsrPoll1ms(); + PlsrTestEmitPulses(1UL); + } + EXPECT_TRUE(PlsrTestOutputFrequency() > frequencyAfterRetarget); + + positionBeforeStop = ReadPosition(); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_STOP)); + for (tick = 0U; tick < 2000U; tick++) + { + PlsrPoll1ms(); + } + while (PlsrTestPulseIsActive() != 0U) + { + PlsrTestEmitPulses(1UL); + } + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + positionAfterStop = ReadPosition(); + EXPECT_TRUE(positionAfterStop > positionBeforeStop); + EXPECT_TRUE(positionAfterStop <= positionBeforeStop + 3L); +} + +static void TestShortProfileExtCut(void) +{ + uint32_t frequencyBeforeCut; + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 100UL, 100UL, 1000U, 1000U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 50L, PLSR_EXT_SIGNAL, + 0U, 0U, 0U)); + PlsrTestSetInput(0U, 0U); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(3UL); + frequencyBeforeCut = PlsrTestOutputFrequency(); + PlsrTestSetInput(0U, 1U); + PlsrPoll1ms(); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_U(0UL, PlsrTestOutputFrequency()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(4L, ReadPosition()); + EXPECT_TRUE(frequencyBeforeCut > 100UL); +} + +static void TestCurrentFrequencyIsDynamic(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 100L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + EXPECT_U(PLSR_MB_OK, WriteU32(0x1100U, 2000UL)); + PlsrPoll1ms(); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + PlsrTestEmitPulses(1UL); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + EXPECT_U(1000UL, ReadFrequency()); + PlsrTestEmitPulses(1UL); + EXPECT_U(2000UL, PlsrTestOutputFrequency()); + EXPECT_U(2000UL, ReadFrequency()); + + EXPECT_U(PLSR_MB_OK, WriteI32(0x1102U, 1L)); + PlsrTestEmitPulses(2UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + StopAndSettle(); +} + +static void TestPollMailboxDefersPlatformFailure(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 100L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestFailNextFrequencyAtUpdate(); + EXPECT_U(PLSR_MB_OK, WriteU32(0x1100U, 2000UL)); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_RUNNING, ReadStatus()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_ERROR, ReadStatus()); + EXPECT_U(PLSR_ERROR_TIMER, ReadError()); + EXPECT_I(1L, ReadPosition()); +} + +static void TestFutureSegmentFrequencyAppliesOnArrival(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 3000UL, 10L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_MB_OK, WriteU32(0x1110U, 5000UL)); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + + PlsrTestEmitPulses(1UL); + PlsrPoll1ms(); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(5000UL, PlsrTestOutputFrequency()); + EXPECT_U(5000UL, ReadFrequency()); + StopAndSettle(); +} + +static void TestSubsequentFutureFrequencyRebuildsHandoff(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 3L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 2000UL, 4L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_MB_OK, WriteU32(0x1110U, 5000UL)); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + + PlsrTestEmitPulses(3UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(5000UL, PlsrTestOutputFrequency()); + EXPECT_U(5000UL, ReadFrequency()); + + PlsrTestEmitPulses(4UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(7L, ReadPosition()); +} + +static void TestFutureFrequencyRaceAtLastPulse(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 2000UL, 4L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + + PlsrTestEmitPulseOnCriticalExit(); + EXPECT_U(PLSR_MB_OK, WriteU32(0x1110U, 5000UL)); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(5000UL, PlsrTestOutputFrequency()); + EXPECT_I(1L, ReadPosition()); + + PlsrTestEmitPulses(4UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(5L, ReadPosition()); +} + +static void TestLatchedUpdateDrainsBeforeFutureFrequencyCommit(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 2000UL, 4L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + EXPECT_U(2000UL, PlsrTestQueuedFrequency()); + + /* The physical edge latches 2000 Hz while interrupts are masked by the + configuration commit. The ISR must consume that old handoff before + the new 5000 Hz preload replaces it. */ + PlsrTestLatchPulseOnCriticalEntry(); + EXPECT_U(PLSR_MB_OK, WriteU32(0x1110U, 5000UL)); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_I(1L, ReadPosition()); + EXPECT_U(2000UL, ReadFrequency()); + EXPECT_U(2000UL, PlsrTestOutputFrequency()); + + PlsrPoll1ms(); + EXPECT_U(5000UL, PlsrTestQueuedFrequency()); + + PlsrTestEmitPulses(1UL); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_I(2L, ReadPosition()); + EXPECT_U(5000UL, ReadFrequency()); + EXPECT_U(5000UL, PlsrTestOutputFrequency()); + + PlsrTestEmitPulses(3UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(5L, ReadPosition()); +} + +static void TestSubsequentHandoffHasNoPollGap(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 3L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 2000UL, 4L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + + PlsrTestEmitPulses(3UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(2000UL, PlsrTestOutputFrequency()); + EXPECT_I(3L, ReadPosition()); + + PlsrTestEmitPulses(4UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(7L, ReadPosition()); +} + +static void TestThreeSinglePulseSubsequentWithoutPoll(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 3U, + PLSR_SEND_SUBSEQUENT, 3000UL, 1000UL, 3000UL, + 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 2000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(3U, 3000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(1000UL, PlsrTestOutputFrequency()); + + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(2U, ReadCurrentSegment()); + EXPECT_U(2000UL, PlsrTestOutputFrequency()); + EXPECT_I(1L, ReadPosition()); + + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_U(3U, ReadCurrentSegment()); + EXPECT_U(3000UL, PlsrTestOutputFrequency()); + EXPECT_I(2L, ReadPosition()); + + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_I(3L, ReadPosition()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); +} + +static void ConfigureHandoffFailureCase(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 2U, + PLSR_SEND_SUBSEQUENT, 2000UL, 1000UL, 2000UL, + 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 3L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(2U, 2000UL, 2L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); +} + +static void ExpectHandoffTimerError(void) +{ + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_ERROR, ReadStatus()); + EXPECT_U(PLSR_ERROR_TIMER, ReadError()); + EXPECT_U(0U, ReadCurrentSegment()); + EXPECT_I(3L, ReadPosition()); +} + +static void TestHandoffQueueFailuresBecomeTimerError(void) +{ + ConfigureHandoffFailureCase(); + PlsrTestEmitPulses(1UL); + PlsrTestFailNextFrequencyAtUpdate(); + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + ExpectHandoffTimerError(); + + ConfigureHandoffFailureCase(); + PlsrTestEmitPulses(2UL); + PlsrTestFailNextFrequencyAtUpdate(); + PlsrTestEmitPulses(1UL); + ExpectHandoffTimerError(); +} + +static void TestCommandStateRestrictions(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 100L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_STOP)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_CLEAR)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, SendCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, SendCommand(PLSR_COMMAND_CLEAR)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, WriteWord(0x1000U, 1U)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, WriteWord(0x1001U, 1U)); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1002U, 1U)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, SendCommand(3U)); + StopAndSettle(); +} + +static void TestCommandMailboxStartSnapshotAndConflicts(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 1000UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 20L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_U(0U, ReadCurrentSegment()); + EXPECT_U(0UL, ReadFrequency()); + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_I(0L, ReadPosition()); + + EXPECT_U(PLSR_MB_OK, WriteI32(0x1102U, 1L)); + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, QueueCommand(PLSR_COMMAND_STOP)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, QueueCommand(PLSR_COMMAND_CLEAR)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, QueueCommand(3U)); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_U(0U, PlsrTestPulseIsActive()); + + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_RUNNING, ReadStatus()); + EXPECT_U(1U, ReadCurrentSegment()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_I(1L, ReadPosition()); + StopAndSettle(); +} + +static void TestCommandMailboxStopAndClearAreDeferred(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 1000UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 100L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(2UL); + + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_STOP)); + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_STOP)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, QueueCommand(PLSR_COMMAND_CLEAR)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, QueueCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_STATUS_RUNNING, ReadStatus()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + EXPECT_I(2L, ReadPosition()); + + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_DECELERATING, ReadStatus()); + EXPECT_U(1U, PlsrTestPulseIsActive()); + PlsrTestEmitPulses(1UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + EXPECT_U(0U, PlsrTestPulseIsActive()); + + PlsrTestSetPosition(17L, 1U); + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_CLEAR)); + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_CLEAR)); + EXPECT_U(PLSR_MB_DEVICE_BUSY, QueueCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_STATUS_STOPPED, ReadStatus()); + EXPECT_I(17L, ReadPosition()); + + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_I(0L, ReadPosition()); +} + +static void TestCommandMailboxValidatesBeforeQueue(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 1000UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 10L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + + EXPECT_U(PLSR_MB_OK, WriteWord(0x100AU, 2U)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, QueueCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_U(0U, PlsrTestPulseIsActive()); + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_STOP)); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + + EXPECT_U(PLSR_MB_OK, WriteWord(0x100AU, 1U)); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1008U, PLSR_POSITION_ABSOLUTE)); + PlsrTestSetPosition(3L, 0U); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, QueueCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_U(0U, PlsrTestPulseIsActive()); +} + +static void TestCommandMailboxStartFailureBecomesError(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 1000UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 10L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + PlsrTestFailNextStart(); + + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_ERROR, ReadStatus()); + EXPECT_U(PLSR_ERROR_INVALID_RESOURCE, ReadError()); + EXPECT_U(0U, ReadCurrentSegment()); + EXPECT_U(0U, PlsrTestPulseIsActive()); + + EXPECT_U(PLSR_MB_DEVICE_BUSY, QueueCommand(PLSR_COMMAND_START)); + EXPECT_U(PLSR_MB_OK, QueueCommand(PLSR_COMMAND_CLEAR)); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_U(PLSR_ERROR_NONE, ReadError()); +} + +static void TestWaitSignalPositivePath(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 2L, PLSR_WAIT_SIGNAL, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(2UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_WAITING, ReadStatus()); + PlsrTestSetInput(0U, 1U); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); +} + +static void TestPersistenceAcrossReinit(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 2345UL, 2345UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1000U, 2U)); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1001U, 3U)); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 2345UL, 4L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(4UL); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); + EXPECT_I(4L, ReadPosition()); + + EXPECT_U(1U, PlsrInit()); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_U(2U, ReadWord(0x1000U)); + EXPECT_U(3U, ReadWord(0x1001U)); + EXPECT_U(2345UL, ReadU32(0x1100U)); + EXPECT_I(4L, ReadPosition()); + + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_CLEAR)); + PlsrPoll1ms(); + EXPECT_U(1U, PlsrInit()); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_I(0L, ReadPosition()); +} + +static void TestPositionSchedulesFlashSave(void) +{ + unsigned int tick; + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 1L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + for (tick = 0U; tick < 1001U; tick++) + { + PlsrPoll1ms(); + } + PlsrTestResetSaveCount(); + + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(1UL); + PlsrPoll1ms(); + EXPECT_U(0UL, PlsrTestSaveCount()); + for (tick = 0U; tick < 1001U; tick++) + { + PlsrPoll1ms(); + } + EXPECT_U(1UL, PlsrTestSaveCount()); + + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_CLEAR)); + for (tick = 0U; tick < 1001U; tick++) + { + PlsrPoll1ms(); + } + EXPECT_U(2UL, PlsrTestSaveCount()); +} + +static void TestBusyResetInvalidatesAbsolutePosition(void) +{ + unsigned int tick; + + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 100L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(5UL); + for (tick = 0U; tick < 10U; tick++) + { + PlsrPoll1ms(); + } + EXPECT_I(5L, ReadPosition()); + + EXPECT_U(1U, PlsrInit()); + EXPECT_U(PLSR_STATUS_IDLE, ReadStatus()); + EXPECT_I(5L, ReadPosition()); + EXPECT_U(PLSR_MB_OK, WriteWord(0x1008U, PLSR_POSITION_ABSOLUTE)); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 5L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_ILLEGAL_VALUE, SendCommand(PLSR_COMMAND_START)); + + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_CLEAR)); + EXPECT_I(0L, ReadPosition()); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 0L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_COMPLETED, ReadStatus()); +} + +static void TestPositionOverflowStopsWithError(void) +{ + ResetCore(); + ConfigureCommon(0U, PLSR_POSITION_RELATIVE, 1U, PLSR_SEND_COMPLETE, + 1000UL, 1000UL, 100UL, 0U, 0U); + EXPECT_U(PLSR_MB_OK, + SetSegment(1U, 1000UL, 2L, PLSR_EXT_OR_COMPLETE, + 0U, 0U, 0U)); + PlsrTestSetPosition(INT32_MAX, 1U); + EXPECT_I(INT32_MAX, ReadPosition()); + EXPECT_U(PLSR_MB_OK, SendCommand(PLSR_COMMAND_START)); + PlsrTestEmitPulses(1UL); + EXPECT_U(0U, PlsrTestPulseIsActive()); + PlsrPoll1ms(); + EXPECT_U(PLSR_STATUS_ERROR, ReadStatus()); + EXPECT_U(PLSR_ERROR_COUNT, ReadError()); + EXPECT_I(INT32_MIN, ReadPosition()); +} + +typedef void (*TEST_FUNCTION)(void); + +typedef struct +{ + const char *name; + TEST_FUNCTION function; +} TEST_CASE; + +static const TEST_CASE TestCases[] = +{ + {"u64_by_u32_division", TestU64ByU32Division}, + {"protocol_boundaries", TestProtocolBoundaries}, + {"wait_zero_one_ms", TestWaitZeroUsesOneMillisecond}, + {"act_zero_no_pulse", TestActZeroSkipsWithoutPulse}, + {"relative_absolute_zero", TestRelativeAndAbsoluteZeroDisplacement}, + {"self_jump_stop", TestSelfJumpCanStop}, + {"stop_deceleration", TestStopDeceleratesBeforeCut}, + {"stop_pending_boundary", TestStopAtPendingBoundary}, + {"direction_delay_one_ms", TestOneMillisecondDirectionDelay}, + {"ext_edge_at_natural_boundary", TestExtEdgeAtNaturalBoundary}, + {"ext_cut_natural_boundary_interleavings", + TestExtCutAndNaturalBoundaryInterleavings}, + {"ext_edge_during_direction_delay", TestExtEdgeDuringDirectionDelay}, + {"stopped_clears_pending_ext", TestStoppedTaskClearsPendingExtEdge}, + {"ext_edge_seamless_epoch", TestExtEdgeDoesNotCutSeamlessNextSegment}, + {"ramp_seamless_epoch", TestOldRampDoesNotRetargetSeamlessNextSegment}, + {"short_profile_boundary_matrix", TestShortProfileBoundaryMatrix}, + {"short_final_no_poll", TestShortFinalDeceleratesWithoutPoll}, + {"zero_to_maximum_ramp_duration", + TestZeroToMaximumRampKeepsConfiguredDuration}, + {"maximum_pulse_ramp_duration", + TestMaximumPulseRampKeepsReachableDuration}, + {"maximum_single_ramp_no_poll", TestMaximumSingleRampWithoutPoll}, + {"short_timer_failure", TestShortProfileTimerFailure}, + {"stop_after_subsequent_handoff", + TestStopImmediatelyAfterSubsequentHandoff}, + {"short_zero_speed_edges", TestShortProfileZeroSpeedEdges}, + {"short_dynamic_retarget", TestShortProfileDynamicRetarget}, + {"short_ext_cut", TestShortProfileExtCut}, + {"current_frequency_dynamic", TestCurrentFrequencyIsDynamic}, + {"poll_mailbox_failure", TestPollMailboxDefersPlatformFailure}, + {"future_frequency_on_arrival", + TestFutureSegmentFrequencyAppliesOnArrival}, + {"subsequent_future_frequency_handoff", + TestSubsequentFutureFrequencyRebuildsHandoff}, + {"future_frequency_last_pulse_race", + TestFutureFrequencyRaceAtLastPulse}, + {"latched_update_before_future_frequency_commit", + TestLatchedUpdateDrainsBeforeFutureFrequencyCommit}, + {"subsequent_no_poll_gap", TestSubsequentHandoffHasNoPollGap}, + {"three_single_pulse_no_poll", TestThreeSinglePulseSubsequentWithoutPoll}, + {"handoff_queue_failures", TestHandoffQueueFailuresBecomeTimerError}, + {"command_state_restrictions", TestCommandStateRestrictions}, + {"command_mailbox_start_snapshot", + TestCommandMailboxStartSnapshotAndConflicts}, + {"command_mailbox_stop_clear_deferred", + TestCommandMailboxStopAndClearAreDeferred}, + {"command_mailbox_validation", TestCommandMailboxValidatesBeforeQueue}, + {"command_mailbox_start_failure", + TestCommandMailboxStartFailureBecomesError}, + {"wait_signal_positive", TestWaitSignalPositivePath}, + {"persistence_reinit", TestPersistenceAcrossReinit}, + {"position_flash_save", TestPositionSchedulesFlashSave}, + {"busy_reset_absolute_gate", TestBusyResetInvalidatesAbsolutePosition}, + {"position_overflow", TestPositionOverflowStopsWithError} +}; + +int main(void) +{ + unsigned int index; + + for (index = 0U; + index < (unsigned int)(sizeof(TestCases) / sizeof(TestCases[0])); + index++) + { + unsigned int failuresBefore = FailureCount; + + CurrentTest = TestCases[index].name; + (void)printf("RUN %s\n", CurrentTest); + TestCases[index].function(); + if (FailureCount == failuresBefore) + { + (void)printf("PASS %s\n", CurrentTest); + } + else + { + (void)printf("FAIL %s (%u new failure(s))\n", + CurrentTest, FailureCount - failuresBefore); + } + } + + (void)printf("SUMMARY tests=%u assertions=%u failures=%u\n", + (unsigned int)(sizeof(TestCases) / sizeof(TestCases[0])), + AssertionCount, FailureCount); + return (FailureCount == 0U) ? 0 : 1; +}