From c776438bcdabc861b4406cfcb991463b698b5bfa Mon Sep 17 00:00:00 2001 From: ywh <2227158009@qq.com> Date: Sun, 9 Aug 2026 16:12:27 +0800 Subject: [PATCH] =?UTF-8?q?P5=20=E8=BD=AF=E9=99=90=E4=BD=8D=E4=BF=9D?= =?UTF-8?q?=E6=8A=A4=20+=20=E5=8F=97=E6=8E=A7=E5=81=9C=E6=AD=A2=E8=90=BD?= =?UTF-8?q?=E5=9C=B0=EF=BC=9A=E9=99=90=E4=BD=8D=E6=8F=90=E5=89=8D=E5=87=8F?= =?UTF-8?q?=E9=80=9F=E7=9C=9F=E6=9C=BA=E9=AA=8C=E8=AF=81=20+=20=E5=B7=B2?= =?UTF-8?q?=E7=9F=A5=E9=97=AE=E9=A2=98=E6=B8=85=E5=8D=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 本批完成内容： 软限位保护（P5）： - profile 新增受控停止入口 PlsrProfileRequestStop：目标频率/终止 频率置 0，按减速斜率平滑减速至停（无斜率时直接停止）。 - 软限位判定接入运动循环：位置到达限位前按减速距离提前触发 受控停止，避免撞击限位后急停。 - 自测新增 PlsrProtectionSelfTestQueue：Q0 脉冲 + Q4 方向， 目标 +10000 脉冲、正软限位 +500、1000Hz、减速 100ms、 方向延时 10ms（SFD900 软限位使能、SFD912/915 端子配置）。 - main.c 上电自测入口切换为保护测试（验证后可删除）。 文档： - 新增《已知问题清单_2026-08-09.md》：记录 AB 启动边沿 （7501）与软限位边界精度（502）两个未关闭问题及根因方向。 上板验证（逻辑分析仪实测）： - 恒定 1000Hz 输出 460 脉冲后进入减速段（84.5ms 平滑减速 至 134Hz），共 502 个脉冲停止——验收 499~501，**超停 1~2 个脉冲**（触发点晚 ~9 个），停止后零残余边沿、无毛刺， Q4 方向正向正常。 - 边界偏差已记录到已知问题清单，待定位（判定位置源/ 提前量公式）。 --- .gitignore | 2 + Core/Src/main.c | 5 +- .../已知问题清单_2026-08-09.md | 42 ++ PLSR/Inc/plc_device.h | 13 + PLSR/Inc/plsr_job.h | 12 + PLSR/Inc/plsr_profile.h | 3 + PLSR/Inc/plsr_self_test.h | 4 + PLSR/Inc/plsr_types.h | 9 +- PLSR/Src/plc_device.c | 80 +++ PLSR/Src/plsr_core.c | 640 ++++++++++++++---- PLSR/Src/plsr_job.c | 95 ++- PLSR/Src/plsr_profile.c | 20 + PLSR/Src/plsr_self_test.c | 76 +++ PLSR/Test/test_plc_device.c | 17 + PLSR/Test/test_plsr_core.c | 4 +- PLSR/Test/test_plsr_hal.c | 258 ++++++- PLSR/Test/test_plsr_job.c | 110 +++ PLSR/Test/test_plsr_profile.c | 32 + 18 files changed, 1294 insertions(+), 128 deletions(-) create mode 100644 Document/PLSR_document/已知问题清单_2026-08-09.md diff --git a/.gitignore b/.gitignore index f89d952..0b74749 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,5 @@ Desktop.ini # PLSR local captures and waveform analysis artifacts Document/PLSR_document/截图/ Document/PLSR_document/波形/ +Document/PLSR_document/~$需求规格书.docx +tmp/ diff --git a/Core/Src/main.c b/Core/Src/main.c index 16c8ae1..f73af31 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -202,8 +202,9 @@ int main(void) /* 上电自测：延时 1s 后由 Q0/Q1 输出三段 AB 正交周期（验证后关闭）。 */ HAL_Delay(1000U); - // (void)PlsrSelfTestQueue(); - (void)PlsrEquivalentSelfTestQueue(); + /* P5 board test: Q0=PULSE, Q4=DIR, controlled stop at +500. */ + //(void)PlsrProtectionSelfTestQueue(); + (void)PlsrProtectionSelfTestQueue(); OSStart(); /* USER CODE END 2 */ diff --git a/Document/PLSR_document/已知问题清单_2026-08-09.md b/Document/PLSR_document/已知问题清单_2026-08-09.md new file mode 100644 index 0000000..444c74d --- /dev/null +++ b/Document/PLSR_document/已知问题清单_2026-08-09.md @@ -0,0 +1,42 @@ +# PLSR 已知问题清单（2026-08-09） + +> 记录上板验证中发现、尚未关闭的两个边界精度问题。 +> 两者均**不阻塞主流程**，但都卡对应验收标准，且属于"逻辑/时序偏差"而非随机抖动。 + +--- + +## 问题 1：AB 启动边沿（Q0/Q1 上升沿 7501 而非 7500） + +| 项 | 内容 | +|---|---| +| **现象** | 段1 起步瞬间 A-R 与 B-R **同时上升**（间隔 0.33~0.37µs），每相各多 1 个上升沿 → Q0=Q1=7501（应 7500）。第一周期从 11 开始，而非标准 00→10→11 | +| **复现** | 稳定复现 6 版固件：00:06、13:41、13:57、13:59、14:15（波形 bin 见 `波形/2026-08-09_*.bin`），现象、位置、结构完全一致 | +| **影响** | 启动瞬间 1 个非法跳变（00→11）；多数驱动器正交解码忽略，位置误差最多 1 脉冲 | +| **已排除** | 调频毛刺（重定相路径无同升，稳定段/加速段 <10µs 间隔 0 个）——问题只在**首次启动**路径 | +| **代码现状** | `PlsrHwBeginAbOutput`（plsr_hal_f407.c）：GPIO 保持 00 → UG → CNT 初值（lead 3/4T、lag 1/2T）→ 切 PWM1 → CEN → 由 update 中断分时释放引脚。**未提交工作区已加 `abStartupPriming` 预热机制（首次启动吞周期），待上板验证** | +| **根因方向** | 两路"释放"几乎同时 → 两路第一次 update 同时 → CNT 初值 1/4T 相位差未体现；或 CC1E 先于 PWM1 模式切换（RM0090：模式切换瞬间 OCREF 跳变）+ 落后相 CNT 写入 = CCR 触发比较事件 | +| **验证方法** | ① 两个 ISR 入口各翻转调试 GPIO，逻辑分析仪同时抓，确认两路 update 中断先后；② 把 `SetPwmMode1` 移到 `SetCc1e` 之前（CC1E=0 时切模式）；③ 启动后读两路 CNT 确认初值 | +| **状态** | 🔴 未关闭（验收：Q0/Q1 严格 7500） | + +--- + +## 问题 2：正软限位边界精度（502 脉冲 vs 验收 499~501） + +| 项 | 内容 | +|---|---| +| **现象** | 测试：目标 +10000 脉冲、正软限位 +500、1000Hz、减速 100ms。实测 **Q0=502 个上升沿**（验收 499~501），**超停 1~2 个脉冲** | +| **波形结构** | 恒定段 rise0..459（460 个 @1000Hz 精确）→ 减速段 rise460..501（42 个，84.5ms，535→…→134Hz 平滑）→ 停止后零边沿、无毛刺 | +| **与预期偏差** | 验收"接近第 450 个脉冲开始减速"→ 实测 **459 开始（晚 9 个）**；理论提前量 = 1000Hz×100ms/2 = 50 脉冲，应在 500-50=**450** 触发、正好停 500 | +| **影响** | 超停 1~2 个工程单位；软限位是**安全功能**，偏差为系统性（非随机），速度/限位配置变化时超停量会随之变化 | +| **根因方向** | ① 限位触发判定的位置源（hardwarePulses / 虚拟计数 / logicalPosition）与判定 tick 采样点；② 提前量（减速距离）计算与实际减速段脉冲数（42 vs 50）不一致；③ 收尾差 1 个与 PULSE/DIR "DONE vs 硬件计数"边界可能同源 | +| **验证方法** | 调限位值/速度复测多组（如 +200/+1000、500Hz/2000Hz），统计"触发位置偏差"是否随参数线性变化，定位是判定滞后还是提前量公式 | +| **状态** | 🔴 未关闭（验收：Q0=499~501） | + +--- + +## 共同结论 + +- 两者都是**小影响、非安全级、但卡验收标准**的边界精度问题 +- 问题 1 偏**时序毛刺**（启动路径），问题 2 偏**逻辑偏差**（判定位置源/提前量） +- 建议优先级：不阻塞 Modbus/四轴主流程；问题 2 略优先（安全功能 + 系统性偏差） +- 均由 Codex 按上面"验证方法"做实验定位后再修，避免盲改 diff --git a/PLSR/Inc/plc_device.h b/PLSR/Inc/plc_device.h index 8bfd422..635b2db 100644 --- a/PLSR/Inc/plc_device.h +++ b/PLSR/Inc/plc_device.h @@ -21,6 +21,13 @@ typedef enum PLC_DEVICE_BUSY } PLC_DEVICE_RESULT; +typedef struct +{ + uint32_t count; + uint16_t lastReason; + uint8_t pending; +} PLC_DEVICE_EVENT_RECORD; + PLC_DEVICE_RESULT PlcDeviceInit(void); PLC_DEVICE_RESULT PlcDeviceReadHsd(uint16_t address, uint16_t *value); @@ -60,6 +67,12 @@ PLC_DEVICE_RESULT PlcDevicePublishSdDword(uint8_t axis, PLC_DEVICE_RESULT PlcDeviceGetEventAddress(uint8_t axis, uint16_t segmentNumber, uint16_t *eventAddress); +PLC_DEVICE_RESULT PlcDevicePublishEvent(uint8_t axis, + uint16_t segmentNumber, + uint16_t reason); +PLC_DEVICE_RESULT PlcDeviceReadEvent(uint16_t eventAddress, + PLC_DEVICE_EVENT_RECORD *record); +PLC_DEVICE_RESULT PlcDeviceAcknowledgeEvent(uint16_t eventAddress); uint8_t PlcDeviceIsHsdDirty(void); uint8_t PlcDeviceIsSfdDirty(void); diff --git a/PLSR/Inc/plsr_job.h b/PLSR/Inc/plsr_job.h index 379e7f4..e88335f 100644 --- a/PLSR/Inc/plsr_job.h +++ b/PLSR/Inc/plsr_job.h @@ -132,6 +132,17 @@ typedef struct uint8_t refreshCode; } PLSR_S2_SNAPSHOT; +typedef struct +{ + int64_t positiveSoftLimitPulses; + int64_t negativeSoftLimitPulses; + uint8_t softLimitEnabled; + uint8_t positiveInputPoint; + uint8_t negativeInputPoint; + uint8_t positiveInputActiveLow; + uint8_t negativeInputActiveLow; +} PLSR_LIMIT_SNAPSHOT; + typedef struct { PLSR_DATA_SOURCE source; @@ -139,6 +150,7 @@ typedef struct PLSR_DATA_REF s1; PLSR_S2_SNAPSHOT s2; PLSR_EQUIVALENT_CONFIG equivalent; + PLSR_LIMIT_SNAPSHOT limits; PLSR_SEGMENT_SNAPSHOT segments[PLSR_MAX_SEGMENTS]; uint32_t inputDefaultSpeed; uint32_t inputMaximumSpeed; diff --git a/PLSR/Inc/plsr_profile.h b/PLSR/Inc/plsr_profile.h index 3435c48..ec3deed 100644 --- a/PLSR/Inc/plsr_profile.h +++ b/PLSR/Inc/plsr_profile.h @@ -90,6 +90,9 @@ PLSR_RESULT PlsrProfileStep(PLSR_PROFILE_STATE *state, PLSR_RESULT PlsrProfileRetarget(PLSR_PROFILE_STATE *state, uint32_t newTargetFrequencyHz); +/* 从当前频率按既定减速斜率降到0，用于缓停、限位和暂停。 */ +PLSR_RESULT PlsrProfileRequestStop(PLSR_PROFILE_STATE *state); + /* 虚拟发射计数校准到硬件实际计数（消除 ARPE 预装载滞后的累积偏差， * 保证 DONE 判定与硬件同步，段尾不再以冻结频率补发剩余脉冲）。 */ void PlsrProfileSyncPulses(PLSR_PROFILE_STATE *state, uint64_t hwPulses); diff --git a/PLSR/Inc/plsr_self_test.h b/PLSR/Inc/plsr_self_test.h index 00b2870..3b8df58 100644 --- a/PLSR/Inc/plsr_self_test.h +++ b/PLSR/Inc/plsr_self_test.h @@ -15,6 +15,10 @@ PLSR_RESULT PlsrSelfTestQueue(void); * 3脉冲/2单位比例下应依次输出1、2个脉冲。 */ PLSR_RESULT PlsrEquivalentSelfTestQueue(void); +/* P5 protection-chain board test: Q0=PULSE, Q4=DIR. The commanded + * distance is deliberately longer than the +500-pulse soft limit. */ +PLSR_RESULT PlsrProtectionSelfTestQueue(void); + #ifdef __cplusplus } #endif diff --git a/PLSR/Inc/plsr_types.h b/PLSR/Inc/plsr_types.h index 759da73..40ee5d3 100644 --- a/PLSR/Inc/plsr_types.h +++ b/PLSR/Inc/plsr_types.h @@ -74,7 +74,10 @@ typedef enum PLSR_RESULT_PATH_CYCLE, PLSR_RESULT_POSITION_INVALID, PLSR_RESULT_POSITION_OVERFLOW, - PLSR_RESULT_DIVIDER_UNREPRESENTABLE + PLSR_RESULT_DIVIDER_UNREPRESENTABLE, + PLSR_RESULT_LIMIT_POSITIVE, + PLSR_RESULT_LIMIT_NEGATIVE, + PLSR_RESULT_EMERGENCY_LATCHED } PLSR_RESULT; typedef enum @@ -87,6 +90,7 @@ typedef enum PLSR_ERROR_COUNTER_FAULT, PLSR_ERROR_LIMIT_POSITIVE, PLSR_ERROR_LIMIT_NEGATIVE, + PLSR_ERROR_EMERGENCY, PLSR_ERROR_INTERNAL } PLSR_ERROR; @@ -180,6 +184,9 @@ typedef struct uint8_t s2Set; uint8_t speedClamped; uint8_t positionOverflow; + uint8_t positiveLimitActive; + uint8_t negativeLimitActive; + uint8_t emergencyLatched; uint16_t segmentCount; uint16_t startSegment; uint16_t currentSegment; diff --git a/PLSR/Src/plc_device.c b/PLSR/Src/plc_device.c index 5f37c9a..0d2644b 100644 --- a/PLSR/Src/plc_device.c +++ b/PLSR/Src/plc_device.c @@ -20,6 +20,8 @@ static PLSR_HSD_DATA PlcHsdData; static PLSR_SFD_DATA PlcSfdData; static int32_t PlcSdRuntime[PLSR_AXIS_COUNT][PLSR_SD_AXIS_ITEM_COUNT]; static uint8_t PlcSmFlags[PLSR_AXIS_COUNT]; +static PLC_DEVICE_EVENT_RECORD + PlcEventRecords[PLSR_AXIS_COUNT][PLSR_EVENT_AXIS_ITEM_COUNT]; static uint8_t PlcHsdDirty; static uint8_t PlcSfdDirty; static uint8_t PlcSfdOperationActive; @@ -165,12 +167,30 @@ static PLC_DEVICE_RESULT PlcDeviceResolveSm(uint16_t address, return PLC_DEVICE_INVALID_ADDRESS; } +static PLC_DEVICE_EVENT_RECORD *PlcDeviceResolveEvent(uint16_t address) +{ + uint8_t axis; + uint16_t base; + + for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++) + { + base = PlsrAxisAddressMap[axis].eventBase; + if ((address >= base) + && (address < base + PLSR_EVENT_AXIS_ITEM_COUNT)) + { + return &PlcEventRecords[axis][address - base]; + } + } + return NULL; +} + PLC_DEVICE_RESULT PlcDeviceInit(void) { (void)memset(&PlcHsdData, 0, sizeof(PlcHsdData)); (void)memset(&PlcSfdData, 0, sizeof(PlcSfdData)); (void)memset(PlcSdRuntime, 0, sizeof(PlcSdRuntime)); (void)memset(PlcSmFlags, 0, sizeof(PlcSmFlags)); + (void)memset(PlcEventRecords, 0, sizeof(PlcEventRecords)); PlcSfdOperationActive = 0U; PlcHsdChangeCounter = 0UL; PlcRestoredHsdPositionValid = 0U; @@ -658,6 +678,66 @@ PLC_DEVICE_RESULT PlcDeviceGetEventAddress(uint8_t axis, return PLC_DEVICE_OK; } +PLC_DEVICE_RESULT PlcDevicePublishEvent(uint8_t axis, + uint16_t segmentNumber, + uint16_t reason) +{ + PLC_DEVICE_EVENT_RECORD *record; + uint32_t interruptState; + + if ((axis >= PLSR_AXIS_COUNT) || (segmentNumber == 0U) + || (segmentNumber > PLSR_EVENT_AXIS_ITEM_COUNT)) + { + return PLC_DEVICE_INVALID_ARGUMENT; + } + record = &PlcEventRecords[axis][segmentNumber - 1U]; + interruptState = PlcDeviceEnterCritical(); + if (record->count != UINT32_MAX) + { + record->count++; + } + record->lastReason = reason; + record->pending = 1U; + PlcDeviceExitCritical(interruptState); + return PLC_DEVICE_OK; +} + +PLC_DEVICE_RESULT PlcDeviceReadEvent(uint16_t eventAddress, + PLC_DEVICE_EVENT_RECORD *record) +{ + PLC_DEVICE_EVENT_RECORD *source; + uint32_t interruptState; + + if (record == NULL) + { + return PLC_DEVICE_NULL_POINTER; + } + source = PlcDeviceResolveEvent(eventAddress); + if (source == NULL) + { + return PLC_DEVICE_INVALID_ADDRESS; + } + interruptState = PlcDeviceEnterCritical(); + *record = *source; + PlcDeviceExitCritical(interruptState); + return PLC_DEVICE_OK; +} + +PLC_DEVICE_RESULT PlcDeviceAcknowledgeEvent(uint16_t eventAddress) +{ + PLC_DEVICE_EVENT_RECORD *record = PlcDeviceResolveEvent(eventAddress); + uint32_t interruptState; + + if (record == NULL) + { + return PLC_DEVICE_INVALID_ADDRESS; + } + interruptState = PlcDeviceEnterCritical(); + record->pending = 0U; + PlcDeviceExitCritical(interruptState); + return PLC_DEVICE_OK; +} + uint8_t PlcDeviceIsHsdDirty(void) { return PlcHsdDirty; diff --git a/PLSR/Src/plsr_core.c b/PLSR/Src/plsr_core.c index 00cd3bc..945e11f 100644 --- a/PLSR/Src/plsr_core.c +++ b/PLSR/Src/plsr_core.c @@ -48,6 +48,10 @@ typedef struct uint8_t jobValid; uint8_t positionOverflow; uint8_t segmentAccountingActive; + uint8_t positiveLimitActive; + uint8_t negativeLimitActive; + uint8_t emergencyLatched; + uint8_t segmentEventPublished; } PLSR_AXIS; typedef struct @@ -72,6 +76,9 @@ static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis, PLSR_AXIS *axisObject); static void PlsrAccountHardwarePulses(uint8_t axis, PLSR_AXIS *axisObject); +static void PlsrPublishSegmentEvent(uint8_t axis, + PLSR_AXIS *axisObject, + PLSR_STOP_REASON reason); static uint32_t PlsrCoreEnterCritical(void) { @@ -192,6 +199,29 @@ static int32_t PlsrClampCompatibleInt32(int64_t value) return (int32_t)value; } +static void PlsrPublishSegmentEvent(uint8_t axis, + PLSR_AXIS *axisObject, + PLSR_STOP_REASON reason) +{ + uint16_t segment; + + if ((axisObject->jobValid == 0U) + || (axisObject->segmentEventPublished != 0U)) + { + return; + } + segment = PlsrPathGetCurrentSegment(&axisObject->path); + if ((segment == 0U) || (segment > PLSR_MAX_SEGMENTS)) + { + return; + } + if (PlcDevicePublishEvent(axis, segment, (uint16_t)reason) + == PLC_DEVICE_OK) + { + axisObject->segmentEventPublished = 1U; + } +} + static void PlsrPublishRuntime(uint8_t axis) { PLSR_AXIS *axisObject = &PlsrAxes[axis]; @@ -234,15 +264,12 @@ static void PlsrPublishRuntime(uint8_t axis) PLSR_SD_ITEM_SPEED, (speed > (uint32_t)INT32_MAX) ? INT32_MAX : (int32_t)speed); - publishedError = (axisObject->compatibleErrorCode != 0U) - ? axisObject->compatibleErrorCode - : (uint16_t)axisObject->error; - publishedBlock = (axisObject->compatibleErrorCode != 0U) + /* SD(B+10/+11)只发布信捷兼容码；项目内部符号错误保留在状态API， + * 不得用枚举数值占用信捷固定错误码。 */ + publishedError = axisObject->compatibleErrorCode; + publishedBlock = (publishedError != 0U) ? axisObject->compatibleErrorBlock - : (((axisObject->error != PLSR_ERROR_NONE) - && (axisObject->jobValid != 0U)) - ? currentSegment - : 0U); + : 0U; (void)PlcDevicePublishSd(axis, PLSR_SD_ITEM_ERROR_CODE, (int32_t)publishedError); @@ -251,22 +278,76 @@ static void PlsrPublishRuntime(uint8_t axis) (int32_t)publishedBlock); } -static void PlsrSetCompatibleEquivalentError(uint8_t axis, - PLSR_AXIS *axisObject) +static void PlsrSetCompatibleParseError(uint8_t axis, + PLSR_AXIS *axisObject) { uint16_t commonBase = (uint16_t)(PLSR_SFD_CONFIG_START + (uint16_t)axis * PLSR_CORE_SFD_AXIS_STRIDE); + uint16_t dynamicBase = (uint16_t)(PLSR_HSD_CONFIG_START + + (uint16_t)axis * 20U); + uint16_t set; - if ((axisObject->parseDetail.result == PLSR_RESULT_INVALID_S2) - && (axisObject->parseDetail.block == PLSR_PARSE_BLOCK_S2) - && ((axisObject->parseDetail.address - == (uint32_t)(commonBase + 2U)) - || (axisObject->parseDetail.address - == (uint32_t)(commonBase + 4U)))) + axisObject->compatibleErrorCode = 0U; + axisObject->compatibleErrorBlock = 0U; + if ((axisObject->parseDetail.result == PLSR_RESULT_RESOURCE_CONFLICT) + || (axisObject->parseDetail.result == PLSR_RESULT_INVALID_RESOURCE)) + { + axisObject->compatibleErrorCode = 26U; + return; + } + if ((axisObject->parseDetail.block == PLSR_PARSE_BLOCK_S0) + && (axisObject->parseDetail.segment != 0U)) + { + axisObject->compatibleErrorCode = 1U; + axisObject->compatibleErrorBlock = axisObject->parseDetail.segment; + return; + } + if ((axisObject->parseDetail.address == (uint32_t)(commonBase + 2U)) + || (axisObject->parseDetail.address + == (uint32_t)(commonBase + 4U))) { axisObject->compatibleErrorCode = 2U; - axisObject->compatibleErrorBlock = 0U; + return; + } + if ((axisObject->parseDetail.address == (uint32_t)(dynamicBase + 12U)) + || (axisObject->parseDetail.address + == (uint32_t)(dynamicBase + 13U))) + { + axisObject->compatibleErrorCode = + (axisObject->parseDetail.address + == (uint32_t)(dynamicBase + 12U)) ? 15U : 16U; + return; + } + for (set = 0U; set < 4U; set++) + { + uint32_t setBase = (uint32_t)commonBase + 50UL + + (uint32_t)set * 20UL; + + if (axisObject->parseDetail.address == setBase + 12UL) + { + axisObject->compatibleErrorCode = 15U; + return; + } + if (axisObject->parseDetail.address == setBase + 13UL) + { + axisObject->compatibleErrorCode = 16U; + return; + } + } + if ((axisObject->parseDetail.address == (uint32_t)(commonBase + 30U)) + || (axisObject->parseDetail.address + == (uint32_t)(commonBase + 32U)) + || (axisObject->parseDetail.result == PLSR_RESULT_SEGMENT_OVERFLOW) + || (axisObject->parseDetail.result == PLSR_RESULT_BLOCK_OVERLAP) + || (axisObject->parseDetail.result == PLSR_RESULT_ADDRESS_OVERFLOW)) + { + axisObject->compatibleErrorCode = 4U; + return; + } + if (axisObject->parseDetail.block == PLSR_PARSE_BLOCK_S2) + { + axisObject->compatibleErrorCode = 3U; } } @@ -336,6 +417,151 @@ static uint8_t PlsrAddInt64Checked(int64_t left, return 1U; } +static PLSR_RESULT PlsrReadLimitInput(const PLSR_JOB_SNAPSHOT *job, + uint8_t point, + uint8_t activeLow, + uint8_t *active) +{ + uint8_t rawLevel; + + if ((job == NULL) || (active == NULL)) + { + return PLSR_RESULT_INVALID_ARGUMENT; + } + if (point == 0xFFU) + { + *active = 0U; + return PLSR_RESULT_OK; + } + if ((job->source.readBit == NULL) + || (job->source.readBit(job->source.context, + PLSR_DEVICE_X, + point, + &rawLevel) == 0U)) + { + return PLSR_RESULT_DATA_ACCESS; + } + *active = (activeLow != 0U) + ? ((rawLevel == 0U) ? 1U : 0U) + : ((rawLevel != 0U) ? 1U : 0U); + return PLSR_RESULT_OK; +} + +static int64_t PlsrGetBrakingDistance(const PLSR_AXIS *axisObject) +{ + uint64_t frequencyHz; + uint64_t denominator; + uint64_t numerator; + + if ((axisObject->profileActive == 0U) + || (axisObject->profile.decelSlopeHzPerMs == 0UL)) + { + return 0; + } + frequencyHz = axisObject->profile.frequencyQ32 >> 32U; + numerator = frequencyHz * frequencyHz; + denominator = UINT64_C(2000) + * axisObject->profile.decelSlopeHzPerMs; + return (int64_t)((numerator + denominator - 1UL) / denominator); +} + +static PLSR_RESULT PlsrUpdateLimitState(PLSR_AXIS *axisObject, + const PLSR_JOB_SNAPSHOT *job, + uint8_t includeBrakingDistance) +{ + int64_t brakingDistance = 0; + uint8_t positiveHard; + uint8_t negativeHard; + uint8_t positiveSoft = 0U; + uint8_t negativeSoft = 0U; + PLSR_RESULT result; + + result = PlsrReadLimitInput(job, + job->limits.positiveInputPoint, + job->limits.positiveInputActiveLow, + &positiveHard); + if (result != PLSR_RESULT_OK) return result; + result = PlsrReadLimitInput(job, + job->limits.negativeInputPoint, + job->limits.negativeInputActiveLow, + &negativeHard); + if (result != PLSR_RESULT_OK) return result; + + if ((job->limits.softLimitEnabled != 0U) + && (axisObject->positionValid != 0U)) + { + if (includeBrakingDistance != 0U) + { + brakingDistance = PlsrGetBrakingDistance(axisObject); + } + if (axisObject->logicalPosition + >= job->limits.positiveSoftLimitPulses) + { + positiveSoft = 1U; + } + else if ((axisObject->directionPositive != 0U) + && (brakingDistance > 0) + && ((axisObject->logicalPosition + > INT64_MAX - brakingDistance) + || (axisObject->logicalPosition + brakingDistance + >= job->limits.positiveSoftLimitPulses))) + { + positiveSoft = 1U; + } + if (axisObject->logicalPosition + <= job->limits.negativeSoftLimitPulses) + { + negativeSoft = 1U; + } + else if ((axisObject->directionPositive == 0U) + && (brakingDistance > 0) + && ((axisObject->logicalPosition + < INT64_MIN + brakingDistance) + || (axisObject->logicalPosition - brakingDistance + <= job->limits.negativeSoftLimitPulses))) + { + negativeSoft = 1U; + } + } + axisObject->positiveLimitActive = + ((positiveHard != 0U) || (positiveSoft != 0U)) ? 1U : 0U; + axisObject->negativeLimitActive = + ((negativeHard != 0U) || (negativeSoft != 0U)) ? 1U : 0U; + return PLSR_RESULT_OK; +} + +static PLSR_RESULT PlsrCheckStartProtection(PLSR_AXIS *axisObject, + const PLSR_JOB_SNAPSHOT *job) +{ + PLSR_RESULT result; + + if (axisObject->emergencyLatched != 0U) + { + return PLSR_RESULT_EMERGENCY_LATCHED; + } + if ((job->limits.softLimitEnabled != 0U) + && (axisObject->positionValid == 0U)) + { + return PLSR_RESULT_POSITION_INVALID; + } + result = PlsrUpdateLimitState(axisObject, job, 0U); + if (result != PLSR_RESULT_OK) + { + return result; + } + if ((job->initialDirectionPositive != 0U) + && (axisObject->positiveLimitActive != 0U)) + { + return PLSR_RESULT_LIMIT_POSITIVE; + } + if ((job->initialDirectionPositive == 0U) + && (axisObject->negativeLimitActive != 0U)) + { + return PLSR_RESULT_LIMIT_NEGATIVE; + } + return PLSR_RESULT_OK; +} + /* 将 HAL 实际完成的完整脉冲/AB周期合并到64位位置。 * emittedPulses 每段从0开始，因此用 segmentAccountedPulses 做差量去重。 */ static void PlsrAccountHardwarePulses(uint8_t axis, @@ -410,7 +636,8 @@ static uint8_t PlsrTransitionIsAllowed(PLSR_STATE current, case PLSR_STATE_IDLE: case PLSR_STATE_COMPLETED: case PLSR_STATE_STOPPED: - return ((target == PLSR_STATE_ACCEL) + return ((target == PLSR_STATE_IDLE) + || (target == PLSR_STATE_ACCEL) || (target == PLSR_STATE_WAIT) || (target == PLSR_STATE_COMPLETED)) ? 1U @@ -829,6 +1056,10 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, { return PLSR_RESULT_INVALID_STATE; } + if (axisObject->emergencyLatched != 0U) + { + return PLSR_RESULT_EMERGENCY_LATCHED; + } axisObject->compatibleErrorCode = 0U; axisObject->compatibleErrorBlock = 0U; @@ -841,7 +1072,35 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, &axisObject->parseDetail); if (result != PLSR_RESULT_OK) { - PlsrSetCompatibleEquivalentError(call->dAxis, axisObject); + PlsrSetCompatibleParseError(call->dAxis, axisObject); + return result; + } + result = PlsrCheckStartProtection(axisObject, &PlsrJobScratch); + if (result != PLSR_RESULT_OK) + { + if (result == PLSR_RESULT_LIMIT_POSITIVE) + { + axisObject->error = PLSR_ERROR_LIMIT_POSITIVE; + axisObject->compatibleErrorCode = 5U; + axisObject->stopReason = PLSR_STOP_REASON_LIMIT_POSITIVE; + } + else if (result == PLSR_RESULT_LIMIT_NEGATIVE) + { + axisObject->error = PLSR_ERROR_LIMIT_NEGATIVE; + axisObject->compatibleErrorCode = 6U; + axisObject->stopReason = PLSR_STOP_REASON_LIMIT_NEGATIVE; + } + else if (result == PLSR_RESULT_EMERGENCY_LATCHED) + { + axisObject->error = PLSR_ERROR_EMERGENCY; + axisObject->stopReason = PLSR_STOP_REASON_SOFTWARE_EMERGENCY; + } + else if (result == PLSR_RESULT_DATA_ACCESS) + { + axisObject->error = PLSR_ERROR_INTERNAL; + axisObject->compatibleErrorCode = 26U; + axisObject->stopReason = PLSR_STOP_REASON_FAULT; + } return result; } @@ -858,6 +1117,8 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, : PLSR_ERROR_INVALID_RESOURCE; axisObject->parseDetail.result = result; axisObject->parseDetail.block = PLSR_PARSE_BLOCK_OUTPUT; + axisObject->compatibleErrorCode = 26U; + axisObject->compatibleErrorBlock = 0U; return result; } @@ -885,6 +1146,7 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, axisObject->taskPulses = 0; axisObject->segmentAccountedPulses = 0; axisObject->segmentAccountingActive = 0U; + axisObject->segmentEventPublished = 0U; PlsrPathBegin(&axisObject->path, &axisObject->job, axisObject->logicalPosition); @@ -917,12 +1179,33 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, } if (result != PLSR_RESULT_OK) { - axisObject->error = PLSR_ERROR_TIMER_FAULT; - PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT); - axisObject->done = 0U; - (void)PlsrStateTransition(call->dAxis, - PLSR_STATE_ERROR, - PLSR_TRANSITION_FAULT); + if ((result == PLSR_RESULT_LIMIT_POSITIVE) + || (result == PLSR_RESULT_LIMIT_NEGATIVE)) + { + axisObject->error = + (result == PLSR_RESULT_LIMIT_POSITIVE) + ? PLSR_ERROR_LIMIT_POSITIVE + : PLSR_ERROR_LIMIT_NEGATIVE; + axisObject->compatibleErrorCode = + (result == PLSR_RESULT_LIMIT_POSITIVE) ? 5U : 6U; + PlsrSetStopReason( + axisObject, + (result == PLSR_RESULT_LIMIT_POSITIVE) + ? PLSR_STOP_REASON_LIMIT_POSITIVE + : PLSR_STOP_REASON_LIMIT_NEGATIVE); + (void)PlsrStateTransition(call->dAxis, + PLSR_STATE_STOPPED, + PLSR_TRANSITION_STOP); + } + else + { + axisObject->error = PLSR_ERROR_TIMER_FAULT; + PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT); + axisObject->done = 0U; + (void)PlsrStateTransition(call->dAxis, + PLSR_STATE_ERROR, + PLSR_TRANSITION_FAULT); + } return result; } /* 运动开始：掉电恢复时据此判定"断电时在运动中"。 */ @@ -953,18 +1236,56 @@ static PLSR_RESULT PlsrStopImmediate(uint8_t axis) PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_IMMEDIATE); axisObject->pendingTerminal = PLSR_STATE_STOPPED; + PlsrPublishSegmentEvent(axis, + axisObject, + PLSR_STOP_REASON_STOP_IMMEDIATE); PlsrPathTerminate(&axisObject->path); PlsrStopSegmentHardware(axis, axisObject); + return PlsrStateTransition(axis, + PLSR_STATE_STOPPED, + PLSR_TRANSITION_STOP); +} + +static PLSR_RESULT PlsrRequestControlledStop(uint8_t axis, + PLSR_STATE terminal, + PLSR_STOP_REASON reason) +{ + PLSR_AXIS *axisObject = &PlsrAxes[axis]; + PLSR_RESULT result; + + PlsrSetStopReason(axisObject, reason); + axisObject->pendingTerminal = terminal; + PlsrPublishSegmentEvent(axis, axisObject, reason); + PlsrPathTerminate(&axisObject->path); + if ((axisObject->state == PLSR_STATE_WAIT) || (axisObject->state == PLSR_STATE_PAUSED)) { + PlsrStopSegmentHardware(axis, axisObject); return PlsrStateTransition(axis, - PLSR_STATE_STOPPED, - PLSR_TRANSITION_STOP); + terminal, + PLSR_TRANSITION_DECEL_COMPLETE); } - - axisObject->immediateStopPending = 1U; - return PLSR_RESULT_OK; + if (axisObject->profileActive == 0U) + { + return (axisObject->state == PLSR_STATE_DECEL) + ? PLSR_RESULT_OK + : PlsrStateTransition(axis, + PLSR_STATE_DECEL, + PLSR_TRANSITION_DECEL_REQUEST); + } + result = PlsrProfileRequestStop(&axisObject->profile); + if (result != PLSR_RESULT_OK) + { + return result; + } + if (axisObject->state == PLSR_STATE_DECEL) + { + return PLSR_RESULT_OK; + } + return PlsrStateTransition(axis, + PLSR_STATE_DECEL, + PLSR_TRANSITION_DECEL_REQUEST); } static PLSR_RESULT PlsrStopDecel(uint8_t axis) @@ -986,24 +1307,9 @@ static PLSR_RESULT PlsrStopDecel(uint8_t axis) return PLSR_RESULT_OK; } - PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_DECEL); - axisObject->pendingTerminal = PLSR_STATE_STOPPED; - PlsrPathTerminate(&axisObject->path); - PlsrStopSegmentHardware(axis, axisObject); - if ((axisObject->state == PLSR_STATE_WAIT) - || (axisObject->state == PLSR_STATE_PAUSED)) - { - return PlsrStateTransition(axis, - PLSR_STATE_STOPPED, - PLSR_TRANSITION_STOP); - } - if (axisObject->state == PLSR_STATE_DECEL) - { - return PLSR_RESULT_OK; - } - return PlsrStateTransition(axis, - PLSR_STATE_DECEL, - PLSR_TRANSITION_DECEL_REQUEST); + return PlsrRequestControlledStop(axis, + PLSR_STATE_STOPPED, + PLSR_STOP_REASON_STOP_DECEL); } static PLSR_RESULT PlsrPause(uint8_t axis) @@ -1020,12 +1326,9 @@ static PLSR_RESULT PlsrPause(uint8_t axis) } if (axisObject->state == PLSR_STATE_WAIT) { - PlsrSetStopReason(axisObject, PLSR_STOP_REASON_PAUSE); - PlsrPathTerminate(&axisObject->path); - PlsrStopSegmentHardware(axis, axisObject); - return PlsrStateTransition(axis, - PLSR_STATE_PAUSED, - PLSR_TRANSITION_DECEL_COMPLETE); + return PlsrRequestControlledStop(axis, + PLSR_STATE_PAUSED, + PLSR_STOP_REASON_PAUSE); } if ((axisObject->state != PLSR_STATE_ACCEL) && (axisObject->state != PLSR_STATE_RUN) @@ -1034,16 +1337,9 @@ static PLSR_RESULT PlsrPause(uint8_t axis) return PLSR_RESULT_INVALID_STATE; } - PlsrSetStopReason(axisObject, PLSR_STOP_REASON_PAUSE); - axisObject->pendingTerminal = PLSR_STATE_PAUSED; - PlsrPathTerminate(&axisObject->path); - if (axisObject->state == PLSR_STATE_DECEL) - { - return PLSR_RESULT_OK; - } - return PlsrStateTransition(axis, - PLSR_STATE_DECEL, - PLSR_TRANSITION_DECEL_REQUEST); + return PlsrRequestControlledStop(axis, + PLSR_STATE_PAUSED, + PLSR_STOP_REASON_PAUSE); } static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot) @@ -1173,7 +1469,14 @@ static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot) break; case PLSR_CMD_RESET_ERROR: - if (axisObject->state != PLSR_STATE_ERROR) + if (PlsrStateIsBusy(axisObject->state) != 0U) + { + result = PLSR_RESULT_BUSY; + } + else if ((axisObject->state != PLSR_STATE_ERROR) + && (axisObject->error == PLSR_ERROR_NONE) + && (axisObject->emergencyLatched == 0U) + && (axisObject->compatibleErrorCode == 0U)) { result = PLSR_RESULT_INVALID_STATE; } @@ -1182,11 +1485,15 @@ static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot) axisObject->error = PLSR_ERROR_NONE; axisObject->compatibleErrorCode = 0U; axisObject->compatibleErrorBlock = 0U; + axisObject->emergencyLatched = 0U; axisObject->stopReason = PLSR_STOP_REASON_NONE; axisObject->done = 0U; - result = PlsrStateTransition(slot->command.axis, - PLSR_STATE_IDLE, - PLSR_TRANSITION_RESET_ERROR); + result = (axisObject->state == PLSR_STATE_IDLE) + ? PLSR_RESULT_OK + : PlsrStateTransition( + slot->command.axis, + PLSR_STATE_IDLE, + PLSR_TRANSITION_RESET_ERROR); } break; @@ -1215,74 +1522,62 @@ static void PlsrProcessCriticalEvents(uint8_t axis, uint32_t events) if ((events & PLSR_EVENT_SOFTWARE_EMERGENCY) != 0UL) { + axisObject->emergencyLatched = 1U; + axisObject->error = PLSR_ERROR_EMERGENCY; + PlsrSetStopReason(axisObject, + PLSR_STOP_REASON_SOFTWARE_EMERGENCY); + axisObject->done = 0U; if (PlsrStateIsBusy(axisObject->state) != 0U) { - PlsrSetStopReason(axisObject, - PLSR_STOP_REASON_SOFTWARE_EMERGENCY); - axisObject->done = 0U; + PlsrPublishSegmentEvent( + axis, + axisObject, + PLSR_STOP_REASON_SOFTWARE_EMERGENCY); PlsrPathTerminate(&axisObject->path); PlsrStopSegmentHardware(axis, axisObject); (void)PlsrStateTransition(axis, PLSR_STATE_STOPPED, PLSR_TRANSITION_STOP); } + else + { + PlsrPublishAxis(axis); + } return; } if ((events & PLSR_EVENT_LIMIT_POSITIVE) != 0UL) { + axisObject->positiveLimitActive = 1U; if ((axisObject->directionPositive != 0U) && (PlsrStateIsBusy(axisObject->state) != 0U)) { axisObject->error = PLSR_ERROR_LIMIT_POSITIVE; - PlsrSetStopReason(axisObject, - PLSR_STOP_REASON_LIMIT_POSITIVE); - axisObject->pendingTerminal = PLSR_STATE_STOPPED; - PlsrPathTerminate(&axisObject->path); - PlsrStopSegmentHardware(axis, axisObject); - if ((axisObject->state == PLSR_STATE_WAIT) - || (axisObject->state == PLSR_STATE_PAUSED)) - { - (void)PlsrStateTransition(axis, - PLSR_STATE_STOPPED, - PLSR_TRANSITION_STOP); - } - else if (axisObject->state != PLSR_STATE_DECEL) - { - (void)PlsrStateTransition(axis, - PLSR_STATE_DECEL, - PLSR_TRANSITION_DECEL_REQUEST); - } + axisObject->compatibleErrorCode = 5U; + axisObject->compatibleErrorBlock = 0U; + (void)PlsrRequestControlledStop( + axis, + PLSR_STATE_STOPPED, + PLSR_STOP_REASON_LIMIT_POSITIVE); + return; } - return; } if ((events & PLSR_EVENT_LIMIT_NEGATIVE) != 0UL) { + axisObject->negativeLimitActive = 1U; if ((axisObject->directionPositive == 0U) && (PlsrStateIsBusy(axisObject->state) != 0U)) { axisObject->error = PLSR_ERROR_LIMIT_NEGATIVE; - PlsrSetStopReason(axisObject, - PLSR_STOP_REASON_LIMIT_NEGATIVE); - axisObject->pendingTerminal = PLSR_STATE_STOPPED; - PlsrPathTerminate(&axisObject->path); - PlsrStopSegmentHardware(axis, axisObject); - if ((axisObject->state == PLSR_STATE_WAIT) - || (axisObject->state == PLSR_STATE_PAUSED)) - { - (void)PlsrStateTransition(axis, - PLSR_STATE_STOPPED, - PLSR_TRANSITION_STOP); - } - else if (axisObject->state != PLSR_STATE_DECEL) - { - (void)PlsrStateTransition(axis, - PLSR_STATE_DECEL, - PLSR_TRANSITION_DECEL_REQUEST); - } + axisObject->compatibleErrorCode = 6U; + axisObject->compatibleErrorBlock = 0U; + (void)PlsrRequestControlledStop( + axis, + PLSR_STATE_STOPPED, + PLSR_STOP_REASON_LIMIT_NEGATIVE); + return; } - return; } if ((events & (PLSR_EVENT_TIMER_FAULT | PLSR_EVENT_COUNTER_FAULT)) != 0UL) @@ -1300,6 +1595,52 @@ static void PlsrProcessCriticalEvents(uint8_t axis, uint32_t events) } } +static void PlsrMonitorAxisProtection(uint8_t axis) +{ + PLSR_AXIS *axisObject = &PlsrAxes[axis]; + PLSR_RESULT result; + + if ((axisObject->jobValid == 0U) + || (PlsrStateIsBusy(axisObject->state) == 0U)) + { + return; + } + result = PlsrUpdateLimitState(axisObject, &axisObject->job, 1U); + if (result != PLSR_RESULT_OK) + { + axisObject->error = PLSR_ERROR_INTERNAL; + axisObject->compatibleErrorCode = 26U; + axisObject->compatibleErrorBlock = 0U; + PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT); + axisObject->done = 0U; + PlsrPublishSegmentEvent(axis, + axisObject, + PLSR_STOP_REASON_FAULT); + PlsrPathTerminate(&axisObject->path); + PlsrStopSegmentHardware(axis, axisObject); + (void)PlsrStateTransition(axis, + PLSR_STATE_ERROR, + PLSR_TRANSITION_FAULT); + return; + } + if ((axisObject->directionPositive != 0U) + && (axisObject->positiveLimitActive != 0U) + && !((axisObject->pendingTerminal == PLSR_STATE_STOPPED) + && (axisObject->stopReason + == PLSR_STOP_REASON_LIMIT_POSITIVE))) + { + (void)PlsrPostEvent(axis, PLSR_EVENT_LIMIT_POSITIVE); + } + else if ((axisObject->directionPositive == 0U) + && (axisObject->negativeLimitActive != 0U) + && !((axisObject->pendingTerminal == PLSR_STATE_STOPPED) + && (axisObject->stopReason + == PLSR_STOP_REASON_LIMIT_NEGATIVE))) + { + (void)PlsrPostEvent(axis, PLSR_EVENT_LIMIT_NEGATIVE); + } +} + /* 启动当前段的硬件输出与速度曲线（P3a：单轴 PULSE/DIR）。 * 由段进入 ACCEL 时调用（任务启动 + 段间推进）。 */ static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis, @@ -1369,6 +1710,21 @@ static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis, positive = (delta >= 0) ? 1U : 0U; } + result = PlsrUpdateLimitState(axisObject, job, 0U); + if (result != PLSR_RESULT_OK) + { + return result; + } + if ((positive != 0U) && (axisObject->positiveLimitActive != 0U)) + { + return PLSR_RESULT_LIMIT_POSITIVE; + } + if ((positive == 0U) && (axisObject->negativeLimitActive != 0U)) + { + return PLSR_RESULT_LIMIT_NEGATIVE; + } + axisObject->segmentEventPublished = 0U; + if (pulses == 0) { /* 当量小于一个物理脉冲时保存余数并按零脉冲段推进；不启动PWM。 */ @@ -1469,8 +1825,28 @@ static void PlsrApplyPathAction(uint8_t axis, PLSR_PATH_ACTION action) /* 进入新段：重新启动硬件输出与速度曲线。 */ if (axisObject->state == PLSR_STATE_ACCEL) { - if (PlsrStartSegmentHardware(axis, axisObject) - != PLSR_RESULT_OK) + PLSR_RESULT startResult = + PlsrStartSegmentHardware(axis, axisObject); + + if ((startResult == PLSR_RESULT_LIMIT_POSITIVE) + || (startResult == PLSR_RESULT_LIMIT_NEGATIVE)) + { + axisObject->error = + (startResult == PLSR_RESULT_LIMIT_POSITIVE) + ? PLSR_ERROR_LIMIT_POSITIVE + : PLSR_ERROR_LIMIT_NEGATIVE; + axisObject->compatibleErrorCode = + (startResult == PLSR_RESULT_LIMIT_POSITIVE) ? 5U : 6U; + PlsrSetStopReason( + axisObject, + (startResult == PLSR_RESULT_LIMIT_POSITIVE) + ? PLSR_STOP_REASON_LIMIT_POSITIVE + : PLSR_STOP_REASON_LIMIT_NEGATIVE); + (void)PlsrStateTransition(axis, + PLSR_STATE_STOPPED, + PLSR_TRANSITION_STOP); + } + else if (startResult != PLSR_RESULT_OK) { axisObject->error = PLSR_ERROR_TIMER_FAULT; PlsrSetStopReason(axisObject, PLSR_STOP_REASON_FAULT); @@ -1589,11 +1965,14 @@ static void PlsrProcessNormalEvents(uint8_t axis, uint32_t events) && (axisObject->pendingTerminal == PLSR_STATE_UNINITIALIZED) && (axisObject->immediateStopPending == 0U)) { - PLSR_PATH_ACTION action = PlsrPathOnSegmentDone( - &axisObject->path, - &axisObject->job, - axisObject->logicalPosition); + PLSR_PATH_ACTION action; + PlsrPublishSegmentEvent(axis, + axisObject, + PLSR_STOP_REASON_NORMAL_COMPLETE); + action = PlsrPathOnSegmentDone(&axisObject->path, + &axisObject->job, + axisObject->logicalPosition); PlsrApplyPathAction(axis, action); } @@ -1659,6 +2038,7 @@ void PlsrProcess(void) PLSR_COMMAND_SLOT slot; uint32_t events; uint8_t processedCommands = 0U; + uint8_t criticalAxes = 0U; uint8_t axis; if (PlsrInitialized == 0U) @@ -1671,6 +2051,7 @@ void PlsrProcess(void) for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++) { PlsrAccountHardwarePulses(axis, &PlsrAxes[axis]); + PlsrMonitorAxisProtection(axis); } for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++) @@ -1678,6 +2059,7 @@ void PlsrProcess(void) events = PlsrTakeEvents(axis, PLSR_EVENT_CRITICAL_MASK); if (events != 0UL) { + criticalAxes |= (uint8_t)(1U << axis); PlsrProcessCriticalEvents(axis, events); } } @@ -1685,7 +2067,20 @@ void PlsrProcess(void) while ((processedCommands < PLSR_COMMAND_QUEUE_DEPTH) && (PlsrPopHighestPriorityCommand(&slot) != 0U)) { - (void)PlsrExecuteCommand(&slot); + if ((slot.command.opcode == PLSR_CMD_RESET_ERROR) + && ((criticalAxes & (uint8_t)(1U << slot.command.axis)) != 0U)) + { + PLSR_AXIS *axisObject = &PlsrAxes[slot.command.axis]; + + axisObject->lastCommandSequence = slot.command.sequence; + axisObject->lastCommandResult = PLSR_RESULT_BUSY; + axisObject->hasLastCommand = 1U; + PlsrPublishAxis(slot.command.axis); + } + else + { + (void)PlsrExecuteCommand(&slot); + } processedCommands++; } @@ -1747,6 +2142,8 @@ void PlsrProcess(void) outputFrequencyHz = frequencyHz; if ((PlsrHwGetState(axis) == PLSR_HW_STATE_RUNNING) && (wasAccel == 0U) + && (axisObject->pendingTerminal + == PLSR_STATE_UNINITIALIZED) && ((uint64_t)axisObject->profile.totalPulses > hardwarePulses + 1UL)) { @@ -1773,6 +2170,14 @@ void PlsrProcess(void) { (void)PlsrHwSetFrequency(axis, outputFrequencyHz); } + if ((profileDone != 0U) + && (axisObject->pendingTerminal + != PLSR_STATE_UNINITIALIZED)) + { + (void)PlsrHwSetFrequency(axis, 0UL); + axisObject->profileActive = 0U; + (void)PlsrPostEvent(axis, PLSR_EVENT_DECEL_COMPLETE); + } axisObject->profileWasAccel = (axisObject->profile.phase == PLSR_PROFILE_PHASE_ACCEL) ? 1U @@ -1841,6 +2246,9 @@ PLSR_RESULT PlsrGetStatus(uint8_t axis, PLSR_STATUS *status) status->positionValid = axisObject->positionValid; status->jobValid = axisObject->jobValid; status->positionOverflow = axisObject->positionOverflow; + status->positiveLimitActive = axisObject->positiveLimitActive; + status->negativeLimitActive = axisObject->negativeLimitActive; + status->emergencyLatched = axisObject->emergencyLatched; status->s2Set = (axisObject->jobValid != 0U) ? axisObject->job.s2Set : 0U; status->speedClamped = (axisObject->jobValid != 0U) ? axisObject->job.speedClamped diff --git a/PLSR/Src/plsr_job.c b/PLSR/Src/plsr_job.c index 45e17e3..586eed9 100644 --- a/PLSR/Src/plsr_job.c +++ b/PLSR/Src/plsr_job.c @@ -352,6 +352,10 @@ static PLSR_RESULT PlsrLoadS2(const PLSR_CALL *call, uint16_t commonBase; uint16_t setBase; uint16_t word; + uint16_t commonFlags; + uint16_t switchLogic; + uint16_t limitPoints; + uint32_t rawLimit; int32_t selectedSet; uint8_t useHsd; PLSR_RESULT result; @@ -481,8 +485,13 @@ static PLSR_RESULT PlsrLoadS2(const PLSR_CALL *call, result = PlsrReadFixedWord(0U, commonBase, &word, detail); if (result != PLSR_RESULT_OK) return result; - snapshot->directionActiveHigh = ((word & (1U << 1U)) != 0U) ? 1U : 0U; - snapshot->equivalent.unitCode = (uint8_t)((word >> 8U) & 0x07U); + commonFlags = word; + snapshot->directionActiveHigh = + ((commonFlags & (1U << 1U)) != 0U) ? 1U : 0U; + snapshot->limits.softLimitEnabled = + ((commonFlags & (1U << 2U)) != 0U) ? 1U : 0U; + snapshot->equivalent.unitCode = + (uint8_t)((commonFlags >> 8U) & 0x07U); if (PlsrPositionUnitCodeIsValid(snapshot->equivalent.unitCode) == 0U) { PlsrSetDetail(detail, @@ -519,9 +528,89 @@ static PLSR_RESULT PlsrLoadS2(const PLSR_CALL *call, 0U); return PLSR_RESULT_INVALID_S2; } + + result = PlsrReadFixedWord(0U, + (uint16_t)(commonBase + 12U), + &switchLogic, + detail); + if (result != PLSR_RESULT_OK) return result; + result = PlsrReadFixedWord(0U, + (uint16_t)(commonBase + 15U), + &limitPoints, + detail); + if (result != PLSR_RESULT_OK) return result; + snapshot->limits.positiveInputPoint = (uint8_t)(limitPoints & 0xFFU); + snapshot->limits.negativeInputPoint = (uint8_t)(limitPoints >> 8U); + snapshot->limits.positiveInputActiveLow = + ((switchLogic & (1U << 2U)) != 0U) ? 1U : 0U; + snapshot->limits.negativeInputActiveLow = + ((switchLogic & (1U << 3U)) != 0U) ? 1U : 0U; + if (((snapshot->limits.positiveInputPoint != 0xFFU) + || (snapshot->limits.negativeInputPoint != 0xFFU)) + && (call->source.readBit == NULL)) + { + PlsrSetDetail(detail, + PLSR_RESULT_DATA_ACCESS, + PLSR_PARSE_BLOCK_S2, + (uint16_t)(commonBase + 15U), + limitPoints, + 0U); + return PLSR_RESULT_DATA_ACCESS; + } + + result = PlsrReadFixedDword(0U, + (uint16_t)(commonBase + 30U), + &rawLimit, + detail); + if (result != PLSR_RESULT_OK) return result; + result = PlsrPositionAbsoluteUnitsToPulses( + &snapshot->equivalent, + (int32_t)rawLimit, + &snapshot->limits.positiveSoftLimitPulses); + if (result != PLSR_RESULT_OK) + { + PlsrSetDetail(detail, + PLSR_RESULT_POSITION_OVERFLOW, + PLSR_PARSE_BLOCK_POSITION, + (uint16_t)(commonBase + 30U), + (int32_t)rawLimit, + 0U); + return PLSR_RESULT_POSITION_OVERFLOW; + } + result = PlsrReadFixedDword(0U, + (uint16_t)(commonBase + 32U), + &rawLimit, + detail); + if (result != PLSR_RESULT_OK) return result; + result = PlsrPositionAbsoluteUnitsToPulses( + &snapshot->equivalent, + (int32_t)rawLimit, + &snapshot->limits.negativeSoftLimitPulses); + if (result != PLSR_RESULT_OK) + { + PlsrSetDetail(detail, + PLSR_RESULT_POSITION_OVERFLOW, + PLSR_PARSE_BLOCK_POSITION, + (uint16_t)(commonBase + 32U), + (int32_t)rawLimit, + 0U); + return PLSR_RESULT_POSITION_OVERFLOW; + } + if ((snapshot->limits.softLimitEnabled != 0U) + && (snapshot->limits.positiveSoftLimitPulses + <= snapshot->limits.negativeSoftLimitPulses)) + { + PlsrSetDetail(detail, + PLSR_RESULT_INVALID_S2, + PLSR_PARSE_BLOCK_S2, + (uint16_t)(commonBase + 30U), + (int32_t)rawLimit, + 0U); + return PLSR_RESULT_INVALID_S2; + } if (call->outputModeOverride == PLSR_OUTPUT_MODE_FROM_SFD) { - snapshot->outputMode = ((word & (1U << 13U)) != 0U) + snapshot->outputMode = ((commonFlags & (1U << 13U)) != 0U) ? (uint8_t)PLSR_OUTPUT_AB : (uint8_t)PLSR_OUTPUT_PULSE_DIR; } diff --git a/PLSR/Src/plsr_profile.c b/PLSR/Src/plsr_profile.c index a36175f..8dd8ec2 100644 --- a/PLSR/Src/plsr_profile.c +++ b/PLSR/Src/plsr_profile.c @@ -479,6 +479,26 @@ PLSR_RESULT PlsrProfileRetarget(PLSR_PROFILE_STATE *state, return PLSR_RESULT_OK; } +PLSR_RESULT PlsrProfileRequestStop(PLSR_PROFILE_STATE *state) +{ + if ((state == NULL) || (state->started == 0U)) + { + return PLSR_RESULT_INVALID_ARGUMENT; + } + state->stopFrequencyHz = 0U; + state->decelTargetHz = 0U; + if (state->decelSlopeHzPerMs == 0UL) + { + state->frequencyQ32 = 0UL; + state->phase = PLSR_PROFILE_PHASE_DONE; + } + else + { + PlsrProfileBeginDecel(state); + } + return PLSR_RESULT_OK; +} + PLSR_RESULT PlsrProfilePlan(const PLSR_PROFILE_REQUEST *request, int64_t pulses, PLSR_PROFILE_PLAN *plan) diff --git a/PLSR/Src/plsr_self_test.c b/PLSR/Src/plsr_self_test.c index 2403142..53f112c 100644 --- a/PLSR/Src/plsr_self_test.c +++ b/PLSR/Src/plsr_self_test.c @@ -85,6 +85,8 @@ PLSR_RESULT PlsrSelfTestQueue(void) /* 方向端子 Y4（PULSE/DIR 模式必需，接线参数）。 */ (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); + (void)PlcDeviceWriteSfd(912U, 0U); + (void)PlcDeviceWriteSfd(915U, 0xFFFFU); /* S0：3 段，H00 完成，顺序跳转。 */ SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 3U); @@ -129,6 +131,8 @@ PLSR_RESULT PlsrEquivalentSelfTestQueue(void) SelfTestWriteSfdDword(904U, 2UL); (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); (void)PlcDeviceWriteSfd(907U, 10U); + (void)PlcDeviceWriteSfd(912U, 0U); + (void)PlcDeviceWriteSfd(915U, 0xFFFFU); /* 当量换算后物理最高速度=90000Hz，不超过硬件100kHz。 */ SelfTestWriteSfdDword(956U, 60000UL); @@ -158,3 +162,75 @@ PLSR_RESULT PlsrEquivalentSelfTestQueue(void) call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR; return PlsrPostCall(&call); } + +PLSR_RESULT PlsrProtectionSelfTestQueue(void) +{ + PLSR_CALL call; + PLSR_COMMAND command; + PLSR_RESULT result; + + (void)memset(SelfTestWords, 0, sizeof(SelfTestWords)); + + /* Pulse unit, soft limits enabled, 1 pulse per position unit. */ + (void)PlcDeviceWriteSfd(900U, (1U << 2U)); + SelfTestWriteSfdDword(902U, 1UL); + SelfTestWriteSfdDword(904U, 1UL); + (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); + (void)PlcDeviceWriteSfd(907U, 10U); + (void)PlcDeviceWriteSfd(912U, 0U); + (void)PlcDeviceWriteSfd(915U, 0xFFFFU); + SelfTestWriteSfdDword(930U, 500UL); + SelfTestWriteSfdDword(932U, (uint32_t)(int32_t)-500); + + /* K1: 1000Hz, 100ms acceleration/deceleration, 1ms refresh. */ + SelfTestWriteSfdDword(950U, 1000UL); + (void)PlcDeviceWriteSfd(952U, 100U); + (void)PlcDeviceWriteSfd(953U, 100U); + (void)PlcDeviceWriteSfd(954U, 0U); + (void)PlcDeviceWriteSfd(955U, 0U); + SelfTestWriteSfdDword(956U, 100000UL); + SelfTestWriteSfdDword(958U, 1000UL); + SelfTestWriteSfdDword(960U, 0UL); + (void)PlcDeviceWriteSfd(962U, 50U); + (void)PlcDeviceWriteSfd(963U, 0U); + (void)PlcDeviceWriteSfd(964U, 0U); + SelfTestWriteSfdDword(966U, 2000UL); + SelfTestWriteSfdDword(968U, 200UL); + + /* One relative segment requests +10000 pulses; +500 must stop it. */ + SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 1U); + SelfTestWriteDword(PLSR_DEVICE_D, + SELF_TEST_S0_BASE + 10U, + 1000UL); + SelfTestWriteDword(PLSR_DEVICE_D, + SELF_TEST_S0_BASE + 12U, + 10000UL); + SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U); + + (void)memset(&command, 0, sizeof(command)); + command.sequence = 0xA5A7UL; + command.axis = 0U; + command.opcode = PLSR_CMD_SET_POSITION; + command.argument = 0; + result = PlsrPostCommand(&command); + if (result != PLSR_RESULT_QUEUED) + { + return result; + } + + (void)memset(&call, 0, sizeof(call)); + call.sequence = 0xA5A8UL; + call.source.context = NULL; + call.source.validateWords = SelfTestValidateWords; + call.source.readWord = SelfTestReadWord; + call.source.readBit = SelfTestReadBit; + call.s0.device = PLSR_DEVICE_D; + call.s0.address = SELF_TEST_S0_BASE; + call.s1.device = PLSR_DEVICE_D; + call.s1.address = SELF_TEST_S1_BASE; + call.s2.type = PLSR_OPERAND_CONSTANT; + call.s2.constant = 1; + call.dAxis = 0U; + call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR; + return PlsrPostCall(&call); +} diff --git a/PLSR/Test/test_plc_device.c b/PLSR/Test/test_plc_device.c index 304ed84..40403a5 100644 --- a/PLSR/Test/test_plc_device.c +++ b/PLSR/Test/test_plc_device.c @@ -20,6 +20,7 @@ static unsigned int TestCount; static void TestAddressMap(void) { uint16_t eventAddress; + PLC_DEVICE_EVENT_RECORD eventRecord; TEST_CHECK(PlsrAxisAddressMap[0].hsdRuntimeBase == 0U); TEST_CHECK(PlsrAxisAddressMap[3].hsdRuntimeBase == 12U); @@ -36,6 +37,22 @@ static void TestAddressMap(void) == PLC_DEVICE_INVALID_ARGUMENT); TEST_CHECK(PlcDeviceGetEventAddress(0U, 0U, &eventAddress) == PLC_DEVICE_INVALID_ARGUMENT); + + TEST_CHECK(PlcDevicePublishEvent(0U, 1U, 5U) == PLC_DEVICE_OK); + TEST_CHECK(PlcDevicePublishEvent(0U, 1U, 6U) == PLC_DEVICE_OK); + TEST_CHECK(PlcDeviceReadEvent(6000U, &eventRecord) == PLC_DEVICE_OK); + TEST_CHECK(eventRecord.count == 2UL); + TEST_CHECK(eventRecord.lastReason == 6U); + TEST_CHECK(eventRecord.pending == 1U); + TEST_CHECK(PlcDeviceAcknowledgeEvent(6000U) == PLC_DEVICE_OK); + TEST_CHECK(PlcDeviceReadEvent(6000U, &eventRecord) == PLC_DEVICE_OK); + TEST_CHECK(eventRecord.count == 2UL); + TEST_CHECK(eventRecord.pending == 0U); + TEST_CHECK(PlcDevicePublishEvent(4U, 1U, 0U) + == PLC_DEVICE_INVALID_ARGUMENT); + TEST_CHECK(PlcDeviceReadEvent(5999U, &eventRecord) + == PLC_DEVICE_INVALID_ADDRESS); + TEST_CHECK(PlcDeviceReadEvent(6000U, NULL) == PLC_DEVICE_NULL_POINTER); } static void TestHsdAndPersistence(void) diff --git a/PLSR/Test/test_plsr_core.c b/PLSR/Test/test_plsr_core.c index 89c92ef..d59d3f3 100644 --- a/PLSR/Test/test_plsr_core.c +++ b/PLSR/Test/test_plsr_core.c @@ -195,9 +195,9 @@ static void TestStateMachineAndCommands(void) == PLSR_RESULT_QUEUED); PlsrProcess(); status = TestGetStatus(0U); - TEST_CHECK(status.state == PLSR_STATE_ACCEL); + TEST_CHECK(status.state == PLSR_STATE_STOPPED); TEST_CHECK(status.stopReason == PLSR_STOP_REASON_STOP_IMMEDIATE); - TEST_CHECK(status.highResourceMask == 0x01U); + TEST_CHECK(status.highResourceMask == 0U); TEST_CHECK(TestPostCommand(5U, 0U, PLSR_CMD_STOP_IMMEDIATE, 0) == PLSR_RESULT_OK); diff --git a/PLSR/Test/test_plsr_hal.c b/PLSR/Test/test_plsr_hal.c index f40fec1..1acd23b 100644 --- a/PLSR/Test/test_plsr_hal.c +++ b/PLSR/Test/test_plsr_hal.c @@ -8,12 +8,14 @@ #include #define TEST_WORD_CAPACITY (3000U) +#define TEST_BIT_CAPACITY (128U) #define TEST_S0_BASE (100U) #define TEST_S1_BASE (200U) typedef struct { uint16_t words[3][TEST_WORD_CAPACITY]; + uint8_t bits[3][TEST_BIT_CAPACITY]; } TEST_MEMORY; static int TestFailures; @@ -63,10 +65,16 @@ static uint8_t TestReadBit(void *context, uint32_t address, uint8_t *value) { - (void)context; - (void)device; - (void)address; - *value = 0U; + TEST_MEMORY *memory = (TEST_MEMORY *)context; + uint8_t index; + + if ((memory == NULL) || (value == NULL) || (device < PLSR_DEVICE_X) + || (device > PLSR_DEVICE_HM) || (address >= TEST_BIT_CAPACITY)) + { + return 0U; + } + index = (uint8_t)(device - PLSR_DEVICE_X); + *value = memory->bits[index][address]; return 1U; } @@ -929,6 +937,189 @@ static void TestEquivalentCompatibleError(void) CHECK(PlsrHwStopPulse(0U) == PLSR_RESULT_OK); } +static void TestSoftLimitAndSegmentEvent(void) +{ + TEST_MEMORY memory; + PLSR_CALL call; + PLSR_COMMAND command; + PLSR_STATUS status; + PLC_DEVICE_EVENT_RECORD eventRecord; + int32_t errorCode; + int pulse; + int tick; + + TestResetEnvironment(); + CHECK(PlcDeviceWriteSfd(900U, (1U << 2U)) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(907U, 0U) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(912U, 0U) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(915U, 0xFFFFU) == PLC_DEVICE_OK); + TestWriteSfdDword(930U, 100UL); + TestWriteSfdDword(932U, (uint32_t)(int32_t)-100); + + command.sequence = 40UL; + command.axis = 0U; + command.opcode = PLSR_CMD_SET_POSITION; + command.argument = 100; + CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED); + PlsrProcess(); + + (void)memset(&memory, 0, sizeof(memory)); + TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S0_BASE, 1); + TestSetSegment(&memory, 1U, 1000U, 10); + call = TestMakeCall(&memory); + call.sequence = 41UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + status = TestGetStatus(); + CHECK(status.lastCommandResult == PLSR_RESULT_LIMIT_POSITIVE); + CHECK(status.state == PLSR_STATE_IDLE); + CHECK(status.positiveLimitActive != 0U); + CHECK(status.error == PLSR_ERROR_LIMIT_POSITIVE); + CHECK(PlcDeviceReadSd(1010U, &errorCode) == PLC_DEVICE_OK); + CHECK(errorCode == 5); + + /* 正限位上只禁止正向，反向离开仍可正常完成。 */ + TestSetSegment(&memory, 1U, 1000U, -10); + call.sequence = 42UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING); + for (pulse = 0; pulse < 10; pulse++) + { + PlsrHwTestTriggerUpdate(0U); + } + PlsrProcess(); + status = TestGetStatus(); + CHECK(status.state == PLSR_STATE_COMPLETED); + CHECK(status.logicalPosition == 90); + CHECK(status.error == PLSR_ERROR_NONE); + CHECK(PlcDeviceReadEvent(6000U, &eventRecord) == PLC_DEVICE_OK); + CHECK(eventRecord.count == 1UL); + CHECK(eventRecord.lastReason == PLSR_STOP_REASON_NORMAL_COMPLETE); + CHECK(eventRecord.pending != 0U); + + /* 运行中按预计制动距离触发软限位，PWM保持运行并按曲线缓停。 */ + TestResetEnvironment(); + CHECK(PlcDeviceWriteSfd(900U, (1U << 2U)) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(907U, 0U) == PLC_DEVICE_OK); + TestWriteSfdDword(930U, 50UL); + TestWriteSfdDword(932U, (uint32_t)(int32_t)-50); + command.sequence = 43UL; + command.opcode = PLSR_CMD_SET_POSITION; + command.argument = 0; + CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED); + PlsrProcess(); + (void)memset(&memory, 0, sizeof(memory)); + TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S0_BASE, 1); + TestSetSegment(&memory, 1U, 1000U, 10000); + call = TestMakeCall(&memory); + call.sequence = 44UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + for (pulse = 0; pulse < 49; pulse++) + { + PlsrHwTestTriggerUpdate(0U); + } + PlsrProcess(); + status = TestGetStatus(); + CHECK(status.state == PLSR_STATE_DECEL); + CHECK(status.stopReason == PLSR_STOP_REASON_LIMIT_POSITIVE); + CHECK(status.positiveLimitActive != 0U); + CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING); + CHECK(PlcDeviceReadSd(1010U, &errorCode) == PLC_DEVICE_OK); + CHECK(errorCode == 5); + for (tick = 0; tick < 20; tick++) + { + PlsrProcess(); + } + status = TestGetStatus(); + CHECK(status.state == PLSR_STATE_STOPPED); + CHECK(status.logicalPosition == 49); + CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_IDLE); + CHECK(PlcDeviceReadEvent(6000U, &eventRecord) == PLC_DEVICE_OK); + CHECK(eventRecord.count == 1UL); + CHECK(eventRecord.lastReason == PLSR_STOP_REASON_LIMIT_POSITIVE); +} + +static void TestHardLimitAndEmergencyLatch(void) +{ + TEST_MEMORY memory; + PLSR_CALL call; + PLSR_COMMAND command; + PLSR_STATUS status; + PLC_DEVICE_EVENT_RECORD eventRecord; + + TestResetEnvironment(); + CHECK(PlcDeviceWriteSfd(907U, 0U) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(912U, 0U) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(915U, 0xFF03U) == PLC_DEVICE_OK); + (void)memset(&memory, 0, sizeof(memory)); + TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S0_BASE, 1); + TestSetSegment(&memory, 1U, 1000U, 10000); + call = TestMakeCall(&memory); + call.sequence = 50UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING); + memory.bits[0][3U] = 1U; + PlsrProcess(); + status = TestGetStatus(); + CHECK(status.state == PLSR_STATE_DECEL); + CHECK(status.positiveLimitActive != 0U); + CHECK(status.stopReason == PLSR_STOP_REASON_LIMIT_POSITIVE); + CHECK(PlcDeviceReadEvent(6000U, &eventRecord) == PLC_DEVICE_OK); + CHECK(eventRecord.lastReason == PLSR_STOP_REASON_LIMIT_POSITIVE); + + TestResetEnvironment(); + CHECK(PlcDeviceWriteSfd(907U, 0U) == PLC_DEVICE_OK); + (void)memset(&memory, 0, sizeof(memory)); + TestWriteDword(&memory, PLSR_DEVICE_D, TEST_S0_BASE, 1); + TestSetSegment(&memory, 1U, 1000U, 10000); + call = TestMakeCall(&memory); + call.sequence = 51UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + command.sequence = 52UL; + command.axis = 0U; + command.opcode = PLSR_CMD_RESET_ERROR; + command.argument = 0; + CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED); + CHECK(PlsrPostEvent(0U, + PLSR_EVENT_LIMIT_POSITIVE + | PLSR_EVENT_SOFTWARE_EMERGENCY) + == PLSR_RESULT_OK); + PlsrProcess(); + status = TestGetStatus(); + CHECK(status.state == PLSR_STATE_STOPPED); + CHECK(status.stopReason == PLSR_STOP_REASON_SOFTWARE_EMERGENCY); + CHECK(status.error == PLSR_ERROR_EMERGENCY); + CHECK(status.emergencyLatched != 0U); + CHECK(status.lastCommandSequence == 52UL); + CHECK(status.lastCommandResult == PLSR_RESULT_BUSY); + CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_IDLE); + CHECK(PlcDeviceReadEvent(6000U, &eventRecord) == PLC_DEVICE_OK); + CHECK(eventRecord.count == 1UL); + CHECK(eventRecord.lastReason == PLSR_STOP_REASON_SOFTWARE_EMERGENCY); + + call.sequence = 53UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + status = TestGetStatus(); + CHECK(status.lastCommandResult == PLSR_RESULT_EMERGENCY_LATCHED); + CHECK(status.state == PLSR_STATE_STOPPED); + + command.sequence = 54UL; + command.axis = 0U; + command.opcode = PLSR_CMD_RESET_ERROR; + command.argument = 0; + CHECK(PlsrPostCommand(&command) == PLSR_RESULT_QUEUED); + PlsrProcess(); + status = TestGetStatus(); + CHECK(status.state == PLSR_STATE_IDLE); + CHECK(status.error == PLSR_ERROR_NONE); + CHECK(status.emergencyLatched == 0U); +} + static void TestProductionSelfTestStartsAb(void) { PLSR_STATUS status; @@ -996,6 +1187,62 @@ static void TestEquivalentSelfTest(void) CHECK(TestReadSdDword(1004U) == 1001); } +static void TestProtectionSelfTest(void) +{ + PLC_DEVICE_EVENT_RECORD eventRecord; + PLSR_STATUS status; + uint32_t pulseAccumulator = 0UL; + int32_t value; + int ticks; + + TestResetEnvironment(); + CHECK(PlsrProtectionSelfTestQueue() == PLSR_RESULT_QUEUED); + PlsrProcess(); + + /* Complete the 10ms direction-settle interval. */ + for (ticks = 0; ticks < 10; ticks++) + { + PlsrProcess(); + } + CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_RUNNING); + + /* Convert the current frequency into simulated hardware updates over + * each 1ms process tick. This also models the falling pulse density + * during the controlled stop. */ + status = TestGetStatus(); + for (ticks = 0; ticks < 600; ticks++) + { + pulseAccumulator += PlsrHwGetCurrentFrequencyHz(0U); + while ((pulseAccumulator >= 1000UL) + && (PlsrHwIsPulseActive(0U) != 0U)) + { + PlsrHwTestTriggerUpdate(0U); + pulseAccumulator -= 1000UL; + } + PlsrProcess(); + status = TestGetStatus(); + if (status.state == PLSR_STATE_STOPPED) + { + break; + } + } + + status = TestGetStatus(); + CHECK(status.state == PLSR_STATE_STOPPED); + CHECK(status.stopReason == PLSR_STOP_REASON_LIMIT_POSITIVE); + CHECK(status.error == PLSR_ERROR_LIMIT_POSITIVE); + CHECK(status.emergencyLatched == 0U); + CHECK(status.logicalPosition >= 499); + CHECK(status.logicalPosition <= 501); + CHECK(PlsrHwGetState(0U) == PLSR_HW_STATE_IDLE); + CHECK(PlcDeviceReadSd(1010U, &value) == PLC_DEVICE_OK); + CHECK(value == 5U); + CHECK(PlcDeviceReadEvent(6000U, &eventRecord) == PLC_DEVICE_OK); + CHECK(eventRecord.count == 1UL); + CHECK(eventRecord.pending != 0U); + CHECK(eventRecord.lastReason == PLSR_STOP_REASON_LIMIT_POSITIVE); +} + static void TestStopStopsHardware(void) { TEST_MEMORY memory; @@ -1052,8 +1299,11 @@ int main(void) TestAbsolutePositionAccounting(); TestEquivalentRemainderAccounting(); TestEquivalentCompatibleError(); + TestSoftLimitAndSegmentEvent(); + TestHardLimitAndEmergencyLatch(); TestProductionSelfTestStartsAb(); TestEquivalentSelfTest(); + TestProtectionSelfTest(); TestStopStopsHardware(); if (TestFailures != 0) diff --git a/PLSR/Test/test_plsr_job.c b/PLSR/Test/test_plsr_job.c index ad6876c..33c5a06 100644 --- a/PLSR/Test/test_plsr_job.c +++ b/PLSR/Test/test_plsr_job.c @@ -409,6 +409,114 @@ static void TestEquivalentSnapshot(void) TestConfigureAxis0K1(); } +static void TestLimitSnapshot(void) +{ + TEST_MEMORY memory; + PLSR_CALL call; + PLSR_PARSE_CONTEXT context = {0, 1U}; + PLSR_JOB_SNAPSHOT snapshot; + PLSR_PARSE_DETAIL detail; + + TestBuildValidBlocks(&memory); + call = TestMakeCall(&memory); + CHECK(PlcDeviceWriteSfd(900U, (1U << 2U)) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(912U, (1U << 2U) | (1U << 3U)) + == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(915U, 0x0703U) == PLC_DEVICE_OK); + TestWriteSfdDword(930U, 100UL); + TestWriteSfdDword(932U, (uint32_t)(int32_t)-100); + + CHECK(PlsrBuildJobSnapshot(&call, &context, &snapshot, &detail) + == PLSR_RESULT_OK); + CHECK(snapshot.limits.softLimitEnabled == 1U); + CHECK(snapshot.limits.positiveSoftLimitPulses == 100); + CHECK(snapshot.limits.negativeSoftLimitPulses == -100); + CHECK(snapshot.limits.positiveInputPoint == 3U); + CHECK(snapshot.limits.negativeInputPoint == 7U); + CHECK(snapshot.limits.positiveInputActiveLow == 1U); + CHECK(snapshot.limits.negativeInputActiveLow == 1U); + + TestWriteSfdDword(930U, (uint32_t)(int32_t)-100); + TestWriteSfdDword(932U, 100UL); + CHECK(PlsrBuildJobSnapshot(&call, &context, &snapshot, &detail) + == PLSR_RESULT_INVALID_S2); + CHECK(detail.address == 930U); + + TestConfigureAxis0K1(); + CHECK(PlcDeviceWriteSfd(912U, 0U) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(915U, 0xFFFFU) == PLC_DEVICE_OK); + TestWriteSfdDword(930U, 0UL); + TestWriteSfdDword(932U, 0UL); +} + +static void TestCompatibleParseErrors(void) +{ + TEST_MEMORY memory; + PLSR_CALL call; + int32_t code; + int32_t block; + + PlsrPersistenceTestResetStorage(); + CHECK(PlcDeviceInit() == PLC_DEVICE_OK); + TestConfigureAxis0K1(); + CHECK(PlsrInit() == PLSR_RESULT_OK); + TestBuildValidBlocks(&memory); + call = TestMakeCall(&memory); + + TestWriteDword(&memory, PLSR_DEVICE_D, 110U, -1); + call.sequence = 700UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + CHECK(PlcDeviceReadSd(1010U, &code) == PLC_DEVICE_OK); + CHECK(PlcDeviceReadSd(1011U, &block) == PLC_DEVICE_OK); + CHECK(code == 1); + CHECK(block == 1); + + TestWriteDword(&memory, PLSR_DEVICE_D, 110U, 1000); + call.s2.constant = 5; + call.sequence = 701UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + CHECK(PlcDeviceReadSd(1010U, &code) == PLC_DEVICE_OK); + CHECK(code == 3); + + call.s2.constant = 1; + CHECK(PlcDeviceWriteSfd(962U, 0U) == PLC_DEVICE_OK); + call.sequence = 702UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + CHECK(PlcDeviceReadSd(1010U, &code) == PLC_DEVICE_OK); + CHECK(code == 15); + + CHECK(PlcDeviceWriteSfd(962U, 50U) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(963U, 101U) == PLC_DEVICE_OK); + call.sequence = 703UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + CHECK(PlcDeviceReadSd(1010U, &code) == PLC_DEVICE_OK); + CHECK(code == 16); + + CHECK(PlcDeviceWriteSfd(963U, 0U) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(900U, (1U << 2U)) == PLC_DEVICE_OK); + TestWriteSfdDword(930U, 0UL); + TestWriteSfdDword(932U, 0UL); + call.sequence = 704UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + CHECK(PlcDeviceReadSd(1010U, &code) == PLC_DEVICE_OK); + CHECK(code == 4); + + CHECK(PlcDeviceWriteSfd(900U, 0U) == PLC_DEVICE_OK); + CHECK(PlcDeviceWriteSfd(906U, 0xFFU) == PLC_DEVICE_OK); + call.sequence = 705UL; + CHECK(PlsrPostCall(&call) == PLSR_RESULT_QUEUED); + PlsrProcess(); + CHECK(PlcDeviceReadSd(1010U, &code) == PLC_DEVICE_OK); + CHECK(code == 26); + + TestConfigureAxis0K1(); +} + static void TestCoreSubmission(void) { TEST_MEMORY memory; @@ -491,6 +599,8 @@ int main(void) TestDynamicReferences(); TestOutputAndDivider(); TestEquivalentSnapshot(); + TestLimitSnapshot(); + TestCompatibleParseErrors(); TestDefaultSfdStartable(); TestCoreSubmission(); diff --git a/PLSR/Test/test_plsr_profile.c b/PLSR/Test/test_plsr_profile.c index 36bc83f..4a72a6d 100644 --- a/PLSR/Test/test_plsr_profile.c +++ b/PLSR/Test/test_plsr_profile.c @@ -342,6 +342,37 @@ static void TestBrakingOutputFrequency(void) == UINT32_MAX); } +static void TestControlledStop(void) +{ + PLSR_PROFILE_REQUEST request = MakeRequest(1000U, 1000U, 0U, + 10U, 10U, 0U); + PLSR_PROFILE_STATE state; + uint32_t frequency = 0U; + uint8_t completed = 0U; + int step; + + CHECK(PlsrProfileStart(&state, &request, 10000, 1000U) + == PLSR_RESULT_OK); + CHECK(state.phase == PLSR_PROFILE_PHASE_CRUISE); + CHECK(PlsrProfileRequestStop(&state) == PLSR_RESULT_OK); + CHECK(state.phase == PLSR_PROFILE_PHASE_DECEL); + for (step = 0; (step < 110) && (completed == 0U); step++) + { + CHECK(PlsrProfileStep(&state, &frequency, &completed) + == PLSR_RESULT_OK); + } + CHECK(completed != 0U); + CHECK(frequency == 0U); + CHECK(state.phase == PLSR_PROFILE_PHASE_DONE); + + request.decelSlopeHzPerMs = 0U; + CHECK(PlsrProfileStart(&state, &request, 10000, 1000U) + == PLSR_RESULT_OK); + CHECK(PlsrProfileRequestStop(&state) == PLSR_RESULT_OK); + CHECK(state.phase == PLSR_PROFILE_PHASE_DONE); + CHECK(PlsrProfileRequestStop(NULL) == PLSR_RESULT_INVALID_ARGUMENT); +} + static void TestPlan(void) { PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U); @@ -383,6 +414,7 @@ int main(void) TestSyncPulses(); TestInitialOutputFrequency(); TestBrakingOutputFrequency(); + TestControlledStop(); TestPlan(); if (TestFailures != 0)