本批完成内容: 软限位保护(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 | # PLSR local captures and waveform analysis artifacts | ||||
| Document/PLSR_document/截图/ | Document/PLSR_document/截图/ | ||||
| Document/PLSR_document/波形/ | Document/PLSR_document/波形/ | ||||
| Document/PLSR_document/~$需求规格书.docx | |||||
| tmp/ | |||||
| @@ -202,8 +202,9 @@ int main(void) | |||||
| /* 上电自测:延时 1s 后由 Q0/Q1 输出三段 AB 正交周期(验证后关闭)。 */ | /* 上电自测:延时 1s 后由 Q0/Q1 输出三段 AB 正交周期(验证后关闭)。 */ | ||||
| HAL_Delay(1000U); | HAL_Delay(1000U); | ||||
| // (void)PlsrSelfTestQueue(); | |||||
| (void)PlsrEquivalentSelfTestQueue(); | |||||
| /* P5 board test: Q0=PULSE, Q4=DIR, controlled stop at +500. */ | |||||
| //(void)PlsrProtectionSelfTestQueue(); | |||||
| (void)PlsrProtectionSelfTestQueue(); | |||||
| OSStart(); | OSStart(); | ||||
| /* USER CODE END 2 */ | /* 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_BUSY | ||||
| } PLC_DEVICE_RESULT; | } 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 PlcDeviceInit(void); | ||||
| PLC_DEVICE_RESULT PlcDeviceReadHsd(uint16_t address, uint16_t *value); | 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, | PLC_DEVICE_RESULT PlcDeviceGetEventAddress(uint8_t axis, | ||||
| uint16_t segmentNumber, | uint16_t segmentNumber, | ||||
| uint16_t *eventAddress); | 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 PlcDeviceIsHsdDirty(void); | ||||
| uint8_t PlcDeviceIsSfdDirty(void); | uint8_t PlcDeviceIsSfdDirty(void); | ||||
| @@ -132,6 +132,17 @@ typedef struct | |||||
| uint8_t refreshCode; | uint8_t refreshCode; | ||||
| } PLSR_S2_SNAPSHOT; | } 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 | typedef struct | ||||
| { | { | ||||
| PLSR_DATA_SOURCE source; | PLSR_DATA_SOURCE source; | ||||
| @@ -139,6 +150,7 @@ typedef struct | |||||
| PLSR_DATA_REF s1; | PLSR_DATA_REF s1; | ||||
| PLSR_S2_SNAPSHOT s2; | PLSR_S2_SNAPSHOT s2; | ||||
| PLSR_EQUIVALENT_CONFIG equivalent; | PLSR_EQUIVALENT_CONFIG equivalent; | ||||
| PLSR_LIMIT_SNAPSHOT limits; | |||||
| PLSR_SEGMENT_SNAPSHOT segments[PLSR_MAX_SEGMENTS]; | PLSR_SEGMENT_SNAPSHOT segments[PLSR_MAX_SEGMENTS]; | ||||
| uint32_t inputDefaultSpeed; | uint32_t inputDefaultSpeed; | ||||
| uint32_t inputMaximumSpeed; | uint32_t inputMaximumSpeed; | ||||
| @@ -90,6 +90,9 @@ PLSR_RESULT PlsrProfileStep(PLSR_PROFILE_STATE *state, | |||||
| PLSR_RESULT PlsrProfileRetarget(PLSR_PROFILE_STATE *state, | PLSR_RESULT PlsrProfileRetarget(PLSR_PROFILE_STATE *state, | ||||
| uint32_t newTargetFrequencyHz); | uint32_t newTargetFrequencyHz); | ||||
| /* 从当前频率按既定减速斜率降到0,用于缓停、限位和暂停。 */ | |||||
| PLSR_RESULT PlsrProfileRequestStop(PLSR_PROFILE_STATE *state); | |||||
| /* 虚拟发射计数校准到硬件实际计数(消除 ARPE 预装载滞后的累积偏差, | /* 虚拟发射计数校准到硬件实际计数(消除 ARPE 预装载滞后的累积偏差, | ||||
| * 保证 DONE 判定与硬件同步,段尾不再以冻结频率补发剩余脉冲)。 */ | * 保证 DONE 判定与硬件同步,段尾不再以冻结频率补发剩余脉冲)。 */ | ||||
| void PlsrProfileSyncPulses(PLSR_PROFILE_STATE *state, uint64_t hwPulses); | void PlsrProfileSyncPulses(PLSR_PROFILE_STATE *state, uint64_t hwPulses); | ||||
| @@ -15,6 +15,10 @@ PLSR_RESULT PlsrSelfTestQueue(void); | |||||
| * 3脉冲/2单位比例下应依次输出1、2个脉冲。 */ | * 3脉冲/2单位比例下应依次输出1、2个脉冲。 */ | ||||
| PLSR_RESULT PlsrEquivalentSelfTestQueue(void); | 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 | #ifdef __cplusplus | ||||
| } | } | ||||
| #endif | #endif | ||||
| @@ -74,7 +74,10 @@ typedef enum | |||||
| PLSR_RESULT_PATH_CYCLE, | PLSR_RESULT_PATH_CYCLE, | ||||
| PLSR_RESULT_POSITION_INVALID, | PLSR_RESULT_POSITION_INVALID, | ||||
| PLSR_RESULT_POSITION_OVERFLOW, | 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; | } PLSR_RESULT; | ||||
| typedef enum | typedef enum | ||||
| @@ -87,6 +90,7 @@ typedef enum | |||||
| PLSR_ERROR_COUNTER_FAULT, | PLSR_ERROR_COUNTER_FAULT, | ||||
| PLSR_ERROR_LIMIT_POSITIVE, | PLSR_ERROR_LIMIT_POSITIVE, | ||||
| PLSR_ERROR_LIMIT_NEGATIVE, | PLSR_ERROR_LIMIT_NEGATIVE, | ||||
| PLSR_ERROR_EMERGENCY, | |||||
| PLSR_ERROR_INTERNAL | PLSR_ERROR_INTERNAL | ||||
| } PLSR_ERROR; | } PLSR_ERROR; | ||||
| @@ -180,6 +184,9 @@ typedef struct | |||||
| uint8_t s2Set; | uint8_t s2Set; | ||||
| uint8_t speedClamped; | uint8_t speedClamped; | ||||
| uint8_t positionOverflow; | uint8_t positionOverflow; | ||||
| uint8_t positiveLimitActive; | |||||
| uint8_t negativeLimitActive; | |||||
| uint8_t emergencyLatched; | |||||
| uint16_t segmentCount; | uint16_t segmentCount; | ||||
| uint16_t startSegment; | uint16_t startSegment; | ||||
| uint16_t currentSegment; | uint16_t currentSegment; | ||||
| @@ -20,6 +20,8 @@ static PLSR_HSD_DATA PlcHsdData; | |||||
| static PLSR_SFD_DATA PlcSfdData; | static PLSR_SFD_DATA PlcSfdData; | ||||
| static int32_t PlcSdRuntime[PLSR_AXIS_COUNT][PLSR_SD_AXIS_ITEM_COUNT]; | static int32_t PlcSdRuntime[PLSR_AXIS_COUNT][PLSR_SD_AXIS_ITEM_COUNT]; | ||||
| static uint8_t PlcSmFlags[PLSR_AXIS_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 PlcHsdDirty; | ||||
| static uint8_t PlcSfdDirty; | static uint8_t PlcSfdDirty; | ||||
| static uint8_t PlcSfdOperationActive; | static uint8_t PlcSfdOperationActive; | ||||
| @@ -165,12 +167,30 @@ static PLC_DEVICE_RESULT PlcDeviceResolveSm(uint16_t address, | |||||
| return PLC_DEVICE_INVALID_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) | PLC_DEVICE_RESULT PlcDeviceInit(void) | ||||
| { | { | ||||
| (void)memset(&PlcHsdData, 0, sizeof(PlcHsdData)); | (void)memset(&PlcHsdData, 0, sizeof(PlcHsdData)); | ||||
| (void)memset(&PlcSfdData, 0, sizeof(PlcSfdData)); | (void)memset(&PlcSfdData, 0, sizeof(PlcSfdData)); | ||||
| (void)memset(PlcSdRuntime, 0, sizeof(PlcSdRuntime)); | (void)memset(PlcSdRuntime, 0, sizeof(PlcSdRuntime)); | ||||
| (void)memset(PlcSmFlags, 0, sizeof(PlcSmFlags)); | (void)memset(PlcSmFlags, 0, sizeof(PlcSmFlags)); | ||||
| (void)memset(PlcEventRecords, 0, sizeof(PlcEventRecords)); | |||||
| PlcSfdOperationActive = 0U; | PlcSfdOperationActive = 0U; | ||||
| PlcHsdChangeCounter = 0UL; | PlcHsdChangeCounter = 0UL; | ||||
| PlcRestoredHsdPositionValid = 0U; | PlcRestoredHsdPositionValid = 0U; | ||||
| @@ -658,6 +678,66 @@ PLC_DEVICE_RESULT PlcDeviceGetEventAddress(uint8_t axis, | |||||
| return PLC_DEVICE_OK; | 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) | uint8_t PlcDeviceIsHsdDirty(void) | ||||
| { | { | ||||
| return PlcHsdDirty; | return PlcHsdDirty; | ||||
| @@ -48,6 +48,10 @@ typedef struct | |||||
| uint8_t jobValid; | uint8_t jobValid; | ||||
| uint8_t positionOverflow; | uint8_t positionOverflow; | ||||
| uint8_t segmentAccountingActive; | uint8_t segmentAccountingActive; | ||||
| uint8_t positiveLimitActive; | |||||
| uint8_t negativeLimitActive; | |||||
| uint8_t emergencyLatched; | |||||
| uint8_t segmentEventPublished; | |||||
| } PLSR_AXIS; | } PLSR_AXIS; | ||||
| typedef struct | typedef struct | ||||
| @@ -72,6 +76,9 @@ static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis, | |||||
| PLSR_AXIS *axisObject); | PLSR_AXIS *axisObject); | ||||
| static void PlsrAccountHardwarePulses(uint8_t axis, | static void PlsrAccountHardwarePulses(uint8_t axis, | ||||
| PLSR_AXIS *axisObject); | PLSR_AXIS *axisObject); | ||||
| static void PlsrPublishSegmentEvent(uint8_t axis, | |||||
| PLSR_AXIS *axisObject, | |||||
| PLSR_STOP_REASON reason); | |||||
| static uint32_t PlsrCoreEnterCritical(void) | static uint32_t PlsrCoreEnterCritical(void) | ||||
| { | { | ||||
| @@ -192,6 +199,29 @@ static int32_t PlsrClampCompatibleInt32(int64_t value) | |||||
| return (int32_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) | static void PlsrPublishRuntime(uint8_t axis) | ||||
| { | { | ||||
| PLSR_AXIS *axisObject = &PlsrAxes[axis]; | PLSR_AXIS *axisObject = &PlsrAxes[axis]; | ||||
| @@ -234,15 +264,12 @@ static void PlsrPublishRuntime(uint8_t axis) | |||||
| PLSR_SD_ITEM_SPEED, | PLSR_SD_ITEM_SPEED, | ||||
| (speed > (uint32_t)INT32_MAX) ? INT32_MAX | (speed > (uint32_t)INT32_MAX) ? INT32_MAX | ||||
| : (int32_t)speed); | : (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->compatibleErrorBlock | ||||
| : (((axisObject->error != PLSR_ERROR_NONE) | |||||
| && (axisObject->jobValid != 0U)) | |||||
| ? currentSegment | |||||
| : 0U); | |||||
| : 0U; | |||||
| (void)PlcDevicePublishSd(axis, | (void)PlcDevicePublishSd(axis, | ||||
| PLSR_SD_ITEM_ERROR_CODE, | PLSR_SD_ITEM_ERROR_CODE, | ||||
| (int32_t)publishedError); | (int32_t)publishedError); | ||||
| @@ -251,22 +278,76 @@ static void PlsrPublishRuntime(uint8_t axis) | |||||
| (int32_t)publishedBlock); | (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 commonBase = (uint16_t)(PLSR_SFD_CONFIG_START | ||||
| + (uint16_t)axis | + (uint16_t)axis | ||||
| * PLSR_CORE_SFD_AXIS_STRIDE); | * 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->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; | 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位位置。 | /* 将 HAL 实际完成的完整脉冲/AB周期合并到64位位置。 | ||||
| * emittedPulses 每段从0开始,因此用 segmentAccountedPulses 做差量去重。 */ | * emittedPulses 每段从0开始,因此用 segmentAccountedPulses 做差量去重。 */ | ||||
| static void PlsrAccountHardwarePulses(uint8_t axis, | static void PlsrAccountHardwarePulses(uint8_t axis, | ||||
| @@ -410,7 +636,8 @@ static uint8_t PlsrTransitionIsAllowed(PLSR_STATE current, | |||||
| case PLSR_STATE_IDLE: | case PLSR_STATE_IDLE: | ||||
| case PLSR_STATE_COMPLETED: | case PLSR_STATE_COMPLETED: | ||||
| case PLSR_STATE_STOPPED: | 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_WAIT) | ||||
| || (target == PLSR_STATE_COMPLETED)) | || (target == PLSR_STATE_COMPLETED)) | ||||
| ? 1U | ? 1U | ||||
| @@ -829,6 +1056,10 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, | |||||
| { | { | ||||
| return PLSR_RESULT_INVALID_STATE; | return PLSR_RESULT_INVALID_STATE; | ||||
| } | } | ||||
| if (axisObject->emergencyLatched != 0U) | |||||
| { | |||||
| return PLSR_RESULT_EMERGENCY_LATCHED; | |||||
| } | |||||
| axisObject->compatibleErrorCode = 0U; | axisObject->compatibleErrorCode = 0U; | ||||
| axisObject->compatibleErrorBlock = 0U; | axisObject->compatibleErrorBlock = 0U; | ||||
| @@ -841,7 +1072,35 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, | |||||
| &axisObject->parseDetail); | &axisObject->parseDetail); | ||||
| if (result != PLSR_RESULT_OK) | 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; | return result; | ||||
| } | } | ||||
| @@ -858,6 +1117,8 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, | |||||
| : PLSR_ERROR_INVALID_RESOURCE; | : PLSR_ERROR_INVALID_RESOURCE; | ||||
| axisObject->parseDetail.result = result; | axisObject->parseDetail.result = result; | ||||
| axisObject->parseDetail.block = PLSR_PARSE_BLOCK_OUTPUT; | axisObject->parseDetail.block = PLSR_PARSE_BLOCK_OUTPUT; | ||||
| axisObject->compatibleErrorCode = 26U; | |||||
| axisObject->compatibleErrorBlock = 0U; | |||||
| return result; | return result; | ||||
| } | } | ||||
| @@ -885,6 +1146,7 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, | |||||
| axisObject->taskPulses = 0; | axisObject->taskPulses = 0; | ||||
| axisObject->segmentAccountedPulses = 0; | axisObject->segmentAccountedPulses = 0; | ||||
| axisObject->segmentAccountingActive = 0U; | axisObject->segmentAccountingActive = 0U; | ||||
| axisObject->segmentEventPublished = 0U; | |||||
| PlsrPathBegin(&axisObject->path, | PlsrPathBegin(&axisObject->path, | ||||
| &axisObject->job, | &axisObject->job, | ||||
| axisObject->logicalPosition); | axisObject->logicalPosition); | ||||
| @@ -917,12 +1179,33 @@ static PLSR_RESULT PlsrStartCall(PLSR_AXIS *axisObject, | |||||
| } | } | ||||
| if (result != PLSR_RESULT_OK) | 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; | return result; | ||||
| } | } | ||||
| /* 运动开始:掉电恢复时据此判定"断电时在运动中"。 */ | /* 运动开始:掉电恢复时据此判定"断电时在运动中"。 */ | ||||
| @@ -953,18 +1236,56 @@ static PLSR_RESULT PlsrStopImmediate(uint8_t axis) | |||||
| PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_IMMEDIATE); | PlsrSetStopReason(axisObject, PLSR_STOP_REASON_STOP_IMMEDIATE); | ||||
| axisObject->pendingTerminal = PLSR_STATE_STOPPED; | axisObject->pendingTerminal = PLSR_STATE_STOPPED; | ||||
| PlsrPublishSegmentEvent(axis, | |||||
| axisObject, | |||||
| PLSR_STOP_REASON_STOP_IMMEDIATE); | |||||
| PlsrPathTerminate(&axisObject->path); | PlsrPathTerminate(&axisObject->path); | ||||
| PlsrStopSegmentHardware(axis, axisObject); | 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) | if ((axisObject->state == PLSR_STATE_WAIT) | ||||
| || (axisObject->state == PLSR_STATE_PAUSED)) | || (axisObject->state == PLSR_STATE_PAUSED)) | ||||
| { | { | ||||
| PlsrStopSegmentHardware(axis, axisObject); | |||||
| return PlsrStateTransition(axis, | 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) | static PLSR_RESULT PlsrStopDecel(uint8_t axis) | ||||
| @@ -986,24 +1307,9 @@ static PLSR_RESULT PlsrStopDecel(uint8_t axis) | |||||
| return PLSR_RESULT_OK; | 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) | static PLSR_RESULT PlsrPause(uint8_t axis) | ||||
| @@ -1020,12 +1326,9 @@ static PLSR_RESULT PlsrPause(uint8_t axis) | |||||
| } | } | ||||
| if (axisObject->state == PLSR_STATE_WAIT) | 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) | if ((axisObject->state != PLSR_STATE_ACCEL) | ||||
| && (axisObject->state != PLSR_STATE_RUN) | && (axisObject->state != PLSR_STATE_RUN) | ||||
| @@ -1034,16 +1337,9 @@ static PLSR_RESULT PlsrPause(uint8_t axis) | |||||
| return PLSR_RESULT_INVALID_STATE; | 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) | static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot) | ||||
| @@ -1173,7 +1469,14 @@ static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot) | |||||
| break; | break; | ||||
| case PLSR_CMD_RESET_ERROR: | 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; | result = PLSR_RESULT_INVALID_STATE; | ||||
| } | } | ||||
| @@ -1182,11 +1485,15 @@ static PLSR_RESULT PlsrExecuteCommand(const PLSR_COMMAND_SLOT *slot) | |||||
| axisObject->error = PLSR_ERROR_NONE; | axisObject->error = PLSR_ERROR_NONE; | ||||
| axisObject->compatibleErrorCode = 0U; | axisObject->compatibleErrorCode = 0U; | ||||
| axisObject->compatibleErrorBlock = 0U; | axisObject->compatibleErrorBlock = 0U; | ||||
| axisObject->emergencyLatched = 0U; | |||||
| axisObject->stopReason = PLSR_STOP_REASON_NONE; | axisObject->stopReason = PLSR_STOP_REASON_NONE; | ||||
| axisObject->done = 0U; | 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; | break; | ||||
| @@ -1215,74 +1522,62 @@ static void PlsrProcessCriticalEvents(uint8_t axis, uint32_t events) | |||||
| if ((events & PLSR_EVENT_SOFTWARE_EMERGENCY) != 0UL) | 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) | 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); | PlsrPathTerminate(&axisObject->path); | ||||
| PlsrStopSegmentHardware(axis, axisObject); | PlsrStopSegmentHardware(axis, axisObject); | ||||
| (void)PlsrStateTransition(axis, | (void)PlsrStateTransition(axis, | ||||
| PLSR_STATE_STOPPED, | PLSR_STATE_STOPPED, | ||||
| PLSR_TRANSITION_STOP); | PLSR_TRANSITION_STOP); | ||||
| } | } | ||||
| else | |||||
| { | |||||
| PlsrPublishAxis(axis); | |||||
| } | |||||
| return; | return; | ||||
| } | } | ||||
| if ((events & PLSR_EVENT_LIMIT_POSITIVE) != 0UL) | if ((events & PLSR_EVENT_LIMIT_POSITIVE) != 0UL) | ||||
| { | { | ||||
| axisObject->positiveLimitActive = 1U; | |||||
| if ((axisObject->directionPositive != 0U) | if ((axisObject->directionPositive != 0U) | ||||
| && (PlsrStateIsBusy(axisObject->state) != 0U)) | && (PlsrStateIsBusy(axisObject->state) != 0U)) | ||||
| { | { | ||||
| axisObject->error = PLSR_ERROR_LIMIT_POSITIVE; | 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) | if ((events & PLSR_EVENT_LIMIT_NEGATIVE) != 0UL) | ||||
| { | { | ||||
| axisObject->negativeLimitActive = 1U; | |||||
| if ((axisObject->directionPositive == 0U) | if ((axisObject->directionPositive == 0U) | ||||
| && (PlsrStateIsBusy(axisObject->state) != 0U)) | && (PlsrStateIsBusy(axisObject->state) != 0U)) | ||||
| { | { | ||||
| axisObject->error = PLSR_ERROR_LIMIT_NEGATIVE; | 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) | 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)。 | /* 启动当前段的硬件输出与速度曲线(P3a:单轴 PULSE/DIR)。 | ||||
| * 由段进入 ACCEL 时调用(任务启动 + 段间推进)。 */ | * 由段进入 ACCEL 时调用(任务启动 + 段间推进)。 */ | ||||
| static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis, | static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis, | ||||
| @@ -1369,6 +1710,21 @@ static PLSR_RESULT PlsrStartSegmentHardware(uint8_t axis, | |||||
| positive = (delta >= 0) ? 1U : 0U; | 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) | if (pulses == 0) | ||||
| { | { | ||||
| /* 当量小于一个物理脉冲时保存余数并按零脉冲段推进;不启动PWM。 */ | /* 当量小于一个物理脉冲时保存余数并按零脉冲段推进;不启动PWM。 */ | ||||
| @@ -1469,8 +1825,28 @@ static void PlsrApplyPathAction(uint8_t axis, PLSR_PATH_ACTION action) | |||||
| /* 进入新段:重新启动硬件输出与速度曲线。 */ | /* 进入新段:重新启动硬件输出与速度曲线。 */ | ||||
| if (axisObject->state == PLSR_STATE_ACCEL) | 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; | axisObject->error = PLSR_ERROR_TIMER_FAULT; | ||||
| PlsrSetStopReason(axisObject, PLSR_STOP_REASON_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->pendingTerminal == PLSR_STATE_UNINITIALIZED) | ||||
| && (axisObject->immediateStopPending == 0U)) | && (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); | PlsrApplyPathAction(axis, action); | ||||
| } | } | ||||
| @@ -1659,6 +2038,7 @@ void PlsrProcess(void) | |||||
| PLSR_COMMAND_SLOT slot; | PLSR_COMMAND_SLOT slot; | ||||
| uint32_t events; | uint32_t events; | ||||
| uint8_t processedCommands = 0U; | uint8_t processedCommands = 0U; | ||||
| uint8_t criticalAxes = 0U; | |||||
| uint8_t axis; | uint8_t axis; | ||||
| if (PlsrInitialized == 0U) | if (PlsrInitialized == 0U) | ||||
| @@ -1671,6 +2051,7 @@ void PlsrProcess(void) | |||||
| for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++) | for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++) | ||||
| { | { | ||||
| PlsrAccountHardwarePulses(axis, &PlsrAxes[axis]); | PlsrAccountHardwarePulses(axis, &PlsrAxes[axis]); | ||||
| PlsrMonitorAxisProtection(axis); | |||||
| } | } | ||||
| for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++) | for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++) | ||||
| @@ -1678,6 +2059,7 @@ void PlsrProcess(void) | |||||
| events = PlsrTakeEvents(axis, PLSR_EVENT_CRITICAL_MASK); | events = PlsrTakeEvents(axis, PLSR_EVENT_CRITICAL_MASK); | ||||
| if (events != 0UL) | if (events != 0UL) | ||||
| { | { | ||||
| criticalAxes |= (uint8_t)(1U << axis); | |||||
| PlsrProcessCriticalEvents(axis, events); | PlsrProcessCriticalEvents(axis, events); | ||||
| } | } | ||||
| } | } | ||||
| @@ -1685,7 +2067,20 @@ void PlsrProcess(void) | |||||
| while ((processedCommands < PLSR_COMMAND_QUEUE_DEPTH) | while ((processedCommands < PLSR_COMMAND_QUEUE_DEPTH) | ||||
| && (PlsrPopHighestPriorityCommand(&slot) != 0U)) | && (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++; | processedCommands++; | ||||
| } | } | ||||
| @@ -1747,6 +2142,8 @@ void PlsrProcess(void) | |||||
| outputFrequencyHz = frequencyHz; | outputFrequencyHz = frequencyHz; | ||||
| if ((PlsrHwGetState(axis) == PLSR_HW_STATE_RUNNING) | if ((PlsrHwGetState(axis) == PLSR_HW_STATE_RUNNING) | ||||
| && (wasAccel == 0U) | && (wasAccel == 0U) | ||||
| && (axisObject->pendingTerminal | |||||
| == PLSR_STATE_UNINITIALIZED) | |||||
| && ((uint64_t)axisObject->profile.totalPulses | && ((uint64_t)axisObject->profile.totalPulses | ||||
| > hardwarePulses + 1UL)) | > hardwarePulses + 1UL)) | ||||
| { | { | ||||
| @@ -1773,6 +2170,14 @@ void PlsrProcess(void) | |||||
| { | { | ||||
| (void)PlsrHwSetFrequency(axis, outputFrequencyHz); | (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->profileWasAccel = | ||||
| (axisObject->profile.phase == PLSR_PROFILE_PHASE_ACCEL) | (axisObject->profile.phase == PLSR_PROFILE_PHASE_ACCEL) | ||||
| ? 1U | ? 1U | ||||
| @@ -1841,6 +2246,9 @@ PLSR_RESULT PlsrGetStatus(uint8_t axis, PLSR_STATUS *status) | |||||
| status->positionValid = axisObject->positionValid; | status->positionValid = axisObject->positionValid; | ||||
| status->jobValid = axisObject->jobValid; | status->jobValid = axisObject->jobValid; | ||||
| status->positionOverflow = axisObject->positionOverflow; | 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->s2Set = (axisObject->jobValid != 0U) ? axisObject->job.s2Set : 0U; | ||||
| status->speedClamped = (axisObject->jobValid != 0U) | status->speedClamped = (axisObject->jobValid != 0U) | ||||
| ? axisObject->job.speedClamped | ? axisObject->job.speedClamped | ||||
| @@ -352,6 +352,10 @@ static PLSR_RESULT PlsrLoadS2(const PLSR_CALL *call, | |||||
| uint16_t commonBase; | uint16_t commonBase; | ||||
| uint16_t setBase; | uint16_t setBase; | ||||
| uint16_t word; | uint16_t word; | ||||
| uint16_t commonFlags; | |||||
| uint16_t switchLogic; | |||||
| uint16_t limitPoints; | |||||
| uint32_t rawLimit; | |||||
| int32_t selectedSet; | int32_t selectedSet; | ||||
| uint8_t useHsd; | uint8_t useHsd; | ||||
| PLSR_RESULT result; | PLSR_RESULT result; | ||||
| @@ -481,8 +485,13 @@ static PLSR_RESULT PlsrLoadS2(const PLSR_CALL *call, | |||||
| result = PlsrReadFixedWord(0U, commonBase, &word, detail); | result = PlsrReadFixedWord(0U, commonBase, &word, detail); | ||||
| if (result != PLSR_RESULT_OK) return result; | 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) | if (PlsrPositionUnitCodeIsValid(snapshot->equivalent.unitCode) == 0U) | ||||
| { | { | ||||
| PlsrSetDetail(detail, | PlsrSetDetail(detail, | ||||
| @@ -519,9 +528,89 @@ static PLSR_RESULT PlsrLoadS2(const PLSR_CALL *call, | |||||
| 0U); | 0U); | ||||
| return PLSR_RESULT_INVALID_S2; | 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) | 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_AB | ||||
| : (uint8_t)PLSR_OUTPUT_PULSE_DIR; | : (uint8_t)PLSR_OUTPUT_PULSE_DIR; | ||||
| } | } | ||||
| @@ -479,6 +479,26 @@ PLSR_RESULT PlsrProfileRetarget(PLSR_PROFILE_STATE *state, | |||||
| return PLSR_RESULT_OK; | 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, | PLSR_RESULT PlsrProfilePlan(const PLSR_PROFILE_REQUEST *request, | ||||
| int64_t pulses, | int64_t pulses, | ||||
| PLSR_PROFILE_PLAN *plan) | PLSR_PROFILE_PLAN *plan) | ||||
| @@ -85,6 +85,8 @@ PLSR_RESULT PlsrSelfTestQueue(void) | |||||
| /* 方向端子 Y4(PULSE/DIR 模式必需,接线参数)。 */ | /* 方向端子 Y4(PULSE/DIR 模式必需,接线参数)。 */ | ||||
| (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); | (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); | ||||
| (void)PlcDeviceWriteSfd(912U, 0U); | |||||
| (void)PlcDeviceWriteSfd(915U, 0xFFFFU); | |||||
| /* S0:3 段,H00 完成,顺序跳转。 */ | /* S0:3 段,H00 完成,顺序跳转。 */ | ||||
| SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 3U); | SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 3U); | ||||
| @@ -129,6 +131,8 @@ PLSR_RESULT PlsrEquivalentSelfTestQueue(void) | |||||
| SelfTestWriteSfdDword(904U, 2UL); | SelfTestWriteSfdDword(904U, 2UL); | ||||
| (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); | (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT); | ||||
| (void)PlcDeviceWriteSfd(907U, 10U); | (void)PlcDeviceWriteSfd(907U, 10U); | ||||
| (void)PlcDeviceWriteSfd(912U, 0U); | |||||
| (void)PlcDeviceWriteSfd(915U, 0xFFFFU); | |||||
| /* 当量换算后物理最高速度=90000Hz,不超过硬件100kHz。 */ | /* 当量换算后物理最高速度=90000Hz,不超过硬件100kHz。 */ | ||||
| SelfTestWriteSfdDword(956U, 60000UL); | SelfTestWriteSfdDword(956U, 60000UL); | ||||
| @@ -158,3 +162,75 @@ PLSR_RESULT PlsrEquivalentSelfTestQueue(void) | |||||
| call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR; | call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR; | ||||
| return PlsrPostCall(&call); | 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) | static void TestAddressMap(void) | ||||
| { | { | ||||
| uint16_t eventAddress; | uint16_t eventAddress; | ||||
| PLC_DEVICE_EVENT_RECORD eventRecord; | |||||
| TEST_CHECK(PlsrAxisAddressMap[0].hsdRuntimeBase == 0U); | TEST_CHECK(PlsrAxisAddressMap[0].hsdRuntimeBase == 0U); | ||||
| TEST_CHECK(PlsrAxisAddressMap[3].hsdRuntimeBase == 12U); | TEST_CHECK(PlsrAxisAddressMap[3].hsdRuntimeBase == 12U); | ||||
| @@ -36,6 +37,22 @@ static void TestAddressMap(void) | |||||
| == PLC_DEVICE_INVALID_ARGUMENT); | == PLC_DEVICE_INVALID_ARGUMENT); | ||||
| TEST_CHECK(PlcDeviceGetEventAddress(0U, 0U, &eventAddress) | TEST_CHECK(PlcDeviceGetEventAddress(0U, 0U, &eventAddress) | ||||
| == PLC_DEVICE_INVALID_ARGUMENT); | == 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) | static void TestHsdAndPersistence(void) | ||||
| @@ -195,9 +195,9 @@ static void TestStateMachineAndCommands(void) | |||||
| == PLSR_RESULT_QUEUED); | == PLSR_RESULT_QUEUED); | ||||
| PlsrProcess(); | PlsrProcess(); | ||||
| status = TestGetStatus(0U); | 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.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) | TEST_CHECK(TestPostCommand(5U, 0U, PLSR_CMD_STOP_IMMEDIATE, 0) | ||||
| == PLSR_RESULT_OK); | == PLSR_RESULT_OK); | ||||
| @@ -8,12 +8,14 @@ | |||||
| #include <string.h> | #include <string.h> | ||||
| #define TEST_WORD_CAPACITY (3000U) | #define TEST_WORD_CAPACITY (3000U) | ||||
| #define TEST_BIT_CAPACITY (128U) | |||||
| #define TEST_S0_BASE (100U) | #define TEST_S0_BASE (100U) | ||||
| #define TEST_S1_BASE (200U) | #define TEST_S1_BASE (200U) | ||||
| typedef struct | typedef struct | ||||
| { | { | ||||
| uint16_t words[3][TEST_WORD_CAPACITY]; | uint16_t words[3][TEST_WORD_CAPACITY]; | ||||
| uint8_t bits[3][TEST_BIT_CAPACITY]; | |||||
| } TEST_MEMORY; | } TEST_MEMORY; | ||||
| static int TestFailures; | static int TestFailures; | ||||
| @@ -63,10 +65,16 @@ static uint8_t TestReadBit(void *context, | |||||
| uint32_t address, | uint32_t address, | ||||
| uint8_t *value) | 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; | return 1U; | ||||
| } | } | ||||
| @@ -929,6 +937,189 @@ static void TestEquivalentCompatibleError(void) | |||||
| CHECK(PlsrHwStopPulse(0U) == PLSR_RESULT_OK); | 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) | static void TestProductionSelfTestStartsAb(void) | ||||
| { | { | ||||
| PLSR_STATUS status; | PLSR_STATUS status; | ||||
| @@ -996,6 +1187,62 @@ static void TestEquivalentSelfTest(void) | |||||
| CHECK(TestReadSdDword(1004U) == 1001); | 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) | static void TestStopStopsHardware(void) | ||||
| { | { | ||||
| TEST_MEMORY memory; | TEST_MEMORY memory; | ||||
| @@ -1052,8 +1299,11 @@ int main(void) | |||||
| TestAbsolutePositionAccounting(); | TestAbsolutePositionAccounting(); | ||||
| TestEquivalentRemainderAccounting(); | TestEquivalentRemainderAccounting(); | ||||
| TestEquivalentCompatibleError(); | TestEquivalentCompatibleError(); | ||||
| TestSoftLimitAndSegmentEvent(); | |||||
| TestHardLimitAndEmergencyLatch(); | |||||
| TestProductionSelfTestStartsAb(); | TestProductionSelfTestStartsAb(); | ||||
| TestEquivalentSelfTest(); | TestEquivalentSelfTest(); | ||||
| TestProtectionSelfTest(); | |||||
| TestStopStopsHardware(); | TestStopStopsHardware(); | ||||
| if (TestFailures != 0) | if (TestFailures != 0) | ||||
| @@ -409,6 +409,114 @@ static void TestEquivalentSnapshot(void) | |||||
| TestConfigureAxis0K1(); | 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) | static void TestCoreSubmission(void) | ||||
| { | { | ||||
| TEST_MEMORY memory; | TEST_MEMORY memory; | ||||
| @@ -491,6 +599,8 @@ int main(void) | |||||
| TestDynamicReferences(); | TestDynamicReferences(); | ||||
| TestOutputAndDivider(); | TestOutputAndDivider(); | ||||
| TestEquivalentSnapshot(); | TestEquivalentSnapshot(); | ||||
| TestLimitSnapshot(); | |||||
| TestCompatibleParseErrors(); | |||||
| TestDefaultSfdStartable(); | TestDefaultSfdStartable(); | ||||
| TestCoreSubmission(); | TestCoreSubmission(); | ||||
| @@ -342,6 +342,37 @@ static void TestBrakingOutputFrequency(void) | |||||
| == UINT32_MAX); | == 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) | static void TestPlan(void) | ||||
| { | { | ||||
| PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U); | PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U); | ||||
| @@ -383,6 +414,7 @@ int main(void) | |||||
| TestSyncPulses(); | TestSyncPulses(); | ||||
| TestInitialOutputFrequency(); | TestInitialOutputFrequency(); | ||||
| TestBrakingOutputFrequency(); | TestBrakingOutputFrequency(); | ||||
| TestControlledStop(); | |||||
| TestPlan(); | TestPlan(); | ||||
| if (TestFailures != 0) | if (TestFailures != 0) | ||||