本批完成内容: 软限位保护(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 方向正向正常。 - 边界偏差已记录到已知问题清单,待定位(判定位置源/ 提前量公式)。master
| @@ -30,3 +30,5 @@ Desktop.ini | |||
| # PLSR local captures and waveform analysis artifacts | |||
| Document/PLSR_document/截图/ | |||
| Document/PLSR_document/波形/ | |||
| Document/PLSR_document/~$需求规格书.docx | |||
| tmp/ | |||
| @@ -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 */ | |||
| @@ -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 按上面"验证方法"做实验定位后再修,避免盲改 | |||
| @@ -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); | |||
| @@ -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; | |||
| @@ -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); | |||
| @@ -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 | |||
| @@ -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; | |||
| @@ -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; | |||
| @@ -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 | |||
| @@ -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; | |||
| } | |||
| @@ -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) | |||
| @@ -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); | |||
| } | |||
| @@ -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) | |||
| @@ -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); | |||
| @@ -8,12 +8,14 @@ | |||
| #include <string.h> | |||
| #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) | |||
| @@ -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(); | |||
| @@ -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) | |||