Ver código fonte

P5 软限位保护 + 受控停止落地:限位提前减速真机验证 + 已知问题清单

本批完成内容:

软限位保护(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
ywh 1 mês atrás
pai
commit
c776438bcd
18 arquivos alterados com 1294 adições e 128 exclusões
  1. +2
    -0
      .gitignore
  2. +3
    -2
      Core/Src/main.c
  3. +42
    -0
      Document/PLSR_document/已知问题清单_2026-08-09.md
  4. +13
    -0
      PLSR/Inc/plc_device.h
  5. +12
    -0
      PLSR/Inc/plsr_job.h
  6. +3
    -0
      PLSR/Inc/plsr_profile.h
  7. +4
    -0
      PLSR/Inc/plsr_self_test.h
  8. +8
    -1
      PLSR/Inc/plsr_types.h
  9. +80
    -0
      PLSR/Src/plc_device.c
  10. +524
    -116
      PLSR/Src/plsr_core.c
  11. +92
    -3
      PLSR/Src/plsr_job.c
  12. +20
    -0
      PLSR/Src/plsr_profile.c
  13. +76
    -0
      PLSR/Src/plsr_self_test.c
  14. +17
    -0
      PLSR/Test/test_plc_device.c
  15. +2
    -2
      PLSR/Test/test_plsr_core.c
  16. +254
    -4
      PLSR/Test/test_plsr_hal.c
  17. +110
    -0
      PLSR/Test/test_plsr_job.c
  18. +32
    -0
      PLSR/Test/test_plsr_profile.c

+ 2
- 0
.gitignore Ver arquivo

@@ -30,3 +30,5 @@ Desktop.ini
# PLSR local captures and waveform analysis artifacts
Document/PLSR_document/截图/
Document/PLSR_document/波形/
Document/PLSR_document/~$需求规格书.docx
tmp/

+ 3
- 2
Core/Src/main.c Ver arquivo

@@ -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 */



+ 42
- 0
Document/PLSR_document/已知问题清单_2026-08-09.md Ver arquivo

@@ -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 按上面"验证方法"做实验定位后再修,避免盲改

+ 13
- 0
PLSR/Inc/plc_device.h Ver arquivo

@@ -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);


+ 12
- 0
PLSR/Inc/plsr_job.h Ver arquivo

@@ -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;


+ 3
- 0
PLSR/Inc/plsr_profile.h Ver arquivo

@@ -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);


+ 4
- 0
PLSR/Inc/plsr_self_test.h Ver arquivo

@@ -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


+ 8
- 1
PLSR/Inc/plsr_types.h Ver arquivo

@@ -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;


+ 80
- 0
PLSR/Src/plc_device.c Ver arquivo

@@ -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;


+ 524
- 116
PLSR/Src/plsr_core.c Ver arquivo

@@ -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


+ 92
- 3
PLSR/Src/plsr_job.c Ver arquivo

@@ -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;
}


+ 20
- 0
PLSR/Src/plsr_profile.c Ver arquivo

@@ -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)


+ 76
- 0
PLSR/Src/plsr_self_test.c Ver arquivo

@@ -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);
}

+ 17
- 0
PLSR/Test/test_plc_device.c Ver arquivo

@@ -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)


+ 2
- 2
PLSR/Test/test_plsr_core.c Ver arquivo

@@ -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);



+ 254
- 4
PLSR/Test/test_plsr_hal.c Ver arquivo

@@ -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)


+ 110
- 0
PLSR/Test/test_plsr_job.c Ver arquivo

@@ -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();



+ 32
- 0
PLSR/Test/test_plsr_profile.c Ver arquivo

@@ -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)


Carregando…
Cancelar
Salvar