From ec430e357c8bb4577733623d5e7349272b03923f Mon Sep 17 00:00:00 2001
From: ywh <2227158009@qq.com>
Date: Fri, 7 Aug 2026 10:09:33 +0800
Subject: [PATCH] =?UTF-8?q?PLSR=20=E6=96=B0=E5=A2=9E=20P2=20=E9=80=9F?=
=?UTF-8?q?=E5=BA=A6=E8=A7=84=E5=88=92=E5=99=A8=EF=BC=88Q32.32=20=E5=AE=9A?=
=?UTF-8?q?=E7=82=B9=E6=9B=B2=E7=BA=BF=E8=AE=A1=E7=AE=97=EF=BC=89?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
- 新增 plsr_profile.c/.h：梯形/S形/正弦三种曲线，Q32.32 定点，刷新中断内零浮点，
正弦用 256 项查表（无 FPU/库依赖），总脉冲数精确到误差 0
- 支持三角曲线（短距离自动降峰值）、起始/终止速度、完成/后续模式衔接参数、
运行中动态调频（升频平滑过渡，降频先减速到新目标再匀速）
- 1ms/0.1ms 双刷新率；PlsrProfilePlan 提供时间轴诊断（非中断内 double）
- 新增 test_plsr_profile 13 场景 75 项（积分精度逐场景验证），
五套 host 测试共 771 项全绿，IAR 0 错误 0 警告
---
EWARM/Modbus.ewp | 6 +
PLSR/Inc/plsr_profile.h | 97 +++++++
PLSR/Src/plsr_profile.c | 531 ++++++++++++++++++++++++++++++++++
PLSR/Test/run_host_tests.ps1 | 9 +-
PLSR/Test/test_plsr_profile.c | 299 +++++++++++++++++++
5 files changed, 941 insertions(+), 1 deletion(-)
create mode 100644 PLSR/Inc/plsr_profile.h
create mode 100644 PLSR/Src/plsr_profile.c
create mode 100644 PLSR/Test/test_plsr_profile.c
diff --git a/EWARM/Modbus.ewp b/EWARM/Modbus.ewp
index 765f0fc..fe594b9 100644
--- a/EWARM/Modbus.ewp
+++ b/EWARM/Modbus.ewp
@@ -1287,6 +1287,9 @@
$PROJ_DIR$\..\PLSR\Inc\plsr_path.h
+
+ $PROJ_DIR$\..\PLSR\Inc\plsr_profile.h
+
$PROJ_DIR$\..\PLSR\Src\plsr_persistence.c
@@ -1302,6 +1305,9 @@
$PROJ_DIR$\..\PLSR\Src\plsr_path.c
+
+ $PROJ_DIR$\..\PLSR\Src\plsr_profile.c
+
$PROJ_DIR$\..\PLSR\Src\plsr_core.c
diff --git a/PLSR/Inc/plsr_profile.h b/PLSR/Inc/plsr_profile.h
new file mode 100644
index 0000000..83e87a1
--- /dev/null
+++ b/PLSR/Inc/plsr_profile.h
@@ -0,0 +1,97 @@
+#ifndef PLSR_PROFILE_H
+#define PLSR_PROFILE_H
+
+#include "plsr_types.h"
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Q32.32 定点：32 位整数部分 + 32 位小数部分。
+ * 频率与脉冲计数全部用定点，禁止在刷新中断中使用浮点。 */
+#define PLSR_PROFILE_Q32_ONE (UINT64_C(1) << 32U)
+
+/* 曲线模式与 S2 参数块中的编码一致。 */
+#define PLSR_PROFILE_CURVE_LINEAR (0U)
+#define PLSR_PROFILE_CURVE_S (1U)
+#define PLSR_PROFILE_CURVE_SINE (2U)
+
+/* 单段速度规划请求。 */
+typedef struct
+{
+ uint32_t targetFrequencyHz; /* 本段目标频率 */
+ uint32_t startFrequencyHz; /* 本段起始频率（后续模式衔接用） */
+ uint32_t stopFrequencyHz; /* 本段终止频率（完成模式一般为 0） */
+ uint32_t maxFrequencyHz; /* 最高速度限幅 */
+ uint32_t accelSlopeHzPerMs; /* 加速斜率 Hz/ms */
+ uint32_t decelSlopeHzPerMs; /* 减速斜率 Hz/ms */
+ uint8_t curveMode; /* 0直线 1S形 2正弦 */
+} PLSR_PROFILE_REQUEST;
+
+/* 规划结果（时间轴概览，诊断/测试用）。 */
+typedef struct
+{
+ uint32_t accelTimeMs;
+ uint32_t cruiseTimeMs;
+ uint32_t decelTimeMs;
+ uint32_t totalTimeMs;
+ uint32_t peakFrequencyHz;
+ int64_t totalPulses;
+ uint8_t triangular;
+} PLSR_PROFILE_PLAN;
+
+typedef enum
+{
+ PLSR_PROFILE_PHASE_ACCEL = 0,
+ PLSR_PROFILE_PHASE_CRUISE,
+ PLSR_PROFILE_PHASE_DECEL,
+ PLSR_PROFILE_PHASE_DONE
+} PLSR_PROFILE_PHASE;
+
+/* 执行状态：由 PlsrProfileStart 初始化，PlsrProfileStep 每刷新周期推进。 */
+typedef struct
+{
+ uint32_t refreshHz; /* 刷新率：1000=1ms，10000=0.1ms */
+ PLSR_PROFILE_PHASE phase;
+ uint32_t targetFrequencyHz;
+ uint32_t startFrequencyHz;
+ uint32_t stopFrequencyHz;
+ uint32_t decelTargetHz; /* 当前减速目标（降频时先到新目标） */
+ uint32_t accelSlopeHzPerMs;
+ uint32_t decelSlopeHzPerMs;
+ uint8_t curveMode;
+ uint64_t frequencyQ32; /* 当前瞬时频率 Q32.32 */
+ uint64_t emittedPulsesQ32; /* 已发脉冲 Q32.32 */
+ int64_t totalPulses; /* 目标总脉冲 */
+ uint64_t deltaPerStepQ32; /* 每步频率增量 Q32.32（直线=常数） */
+ uint64_t jerkPerStepQ32; /* 每步增量变化率 Q32.32（S形用） */
+ uint32_t curveStep; /* 曲线相位计数（S形/正弦） */
+ uint32_t curveStepLimit; /* 曲线相位上限 */
+ uint8_t sPhase; /* S形增量 0=升 1=降 */
+ uint8_t started;
+} PLSR_PROFILE_STATE;
+
+PLSR_RESULT PlsrProfilePlan(const PLSR_PROFILE_REQUEST *request,
+ int64_t pulses,
+ PLSR_PROFILE_PLAN *plan);
+
+PLSR_RESULT PlsrProfileStart(PLSR_PROFILE_STATE *state,
+ const PLSR_PROFILE_REQUEST *request,
+ int64_t pulses,
+ uint32_t refreshHz);
+
+/* 每刷新周期推进一步。返回当前应输出频率（Hz，四舍五入）与完成标志。 */
+PLSR_RESULT PlsrProfileStep(PLSR_PROFILE_STATE *state,
+ uint32_t *frequencyHz,
+ uint8_t *completed);
+
+/* 运行中动态调频：按斜率平滑过渡到新目标频率。 */
+PLSR_RESULT PlsrProfileRetarget(PLSR_PROFILE_STATE *state,
+ uint32_t newTargetFrequencyHz);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* PLSR_PROFILE_H */
diff --git a/PLSR/Src/plsr_profile.c b/PLSR/Src/plsr_profile.c
new file mode 100644
index 0000000..34a5d14
--- /dev/null
+++ b/PLSR/Src/plsr_profile.c
@@ -0,0 +1,531 @@
+#include "plsr_profile.h"
+#include
+#include
+
+/* 正弦表：256 项，sin(pi*i/256) 的 Q16 表示。 */
+static const uint16_t PlsrProfileSineTable[256] =
+{
+ 0, 804, 1608, 2412, 3216, 4019, 4821, 5623,
+ 6424, 7224, 8022, 8820, 9616, 10411, 11204, 11996,
+ 12785, 13573, 14359, 15143, 15924, 16703, 17479, 18253,
+ 19024, 19792, 20557, 21320, 22078, 22834, 23586, 24335,
+ 25080, 25821, 26558, 27291, 28020, 28745, 29466, 30182,
+ 30893, 31600, 32303, 33000, 33692, 34380, 35062, 35738,
+ 36410, 37076, 37736, 38391, 39040, 39683, 40320, 40951,
+ 41576, 42194, 42806, 43412, 44011, 44604, 45190, 45769,
+ 46341, 46906, 47464, 48015, 48559, 49095, 49624, 50146,
+ 50660, 51166, 51665, 52156, 52639, 53114, 53581, 54040,
+ 54491, 54934, 55368, 55794, 56212, 56621, 57022, 57414,
+ 57798, 58172, 58538, 58896, 59244, 59583, 59914, 60235,
+ 60547, 60851, 61145, 61429, 61705, 61971, 62228, 62476,
+ 62714, 62943, 63162, 63372, 63572, 63763, 63944, 64115,
+ 64277, 64429, 64571, 64704, 64827, 64940, 65043, 65137,
+ 65220, 65294, 65358, 65413, 65457, 65492, 65516, 65531,
+ 65535, 65531, 65516, 65492, 65457, 65413, 65358, 65294,
+ 65220, 65137, 65043, 64940, 64827, 64704, 64571, 64429,
+ 64277, 64115, 63944, 63763, 63572, 63372, 63162, 62943,
+ 62714, 62476, 62228, 61971, 61705, 61429, 61145, 60851,
+ 60547, 60235, 59914, 59583, 59244, 58896, 58538, 58172,
+ 57798, 57414, 57022, 56621, 56212, 55794, 55368, 54934,
+ 54491, 54040, 53581, 53114, 52639, 52156, 51665, 51166,
+ 50660, 50146, 49624, 49095, 48559, 48015, 47464, 46906,
+ 46341, 45769, 45190, 44604, 44011, 43412, 42806, 42194,
+ 41576, 40951, 40320, 39683, 39040, 38391, 37736, 37076,
+ 36410, 35738, 35062, 34380, 33692, 33000, 32303, 31600,
+ 30893, 30182, 29466, 28745, 28020, 27291, 26558, 25821,
+ 25080, 24335, 23586, 22834, 22078, 21320, 20557, 19792,
+ 19024, 18253, 17479, 16703, 15924, 15143, 14359, 13573,
+ 12785, 11996, 11204, 10411, 9616, 8820, 8022, 7224,
+ 6424, 5623, 4821, 4019, 3216, 2412, 1608, 804,
+};
+static uint64_t PlsrProfileHzToQ32(uint32_t hz)
+{
+ return (uint64_t)hz << 32U;
+}
+
+/* 每刷新步的斜率增量（Q32.32），支持 0.1ms 刷新（1000/refreshHz 可能 < 1）。 */
+static uint64_t PlsrProfileSlopePerStepQ32(uint32_t slopeHzPerMs,
+ uint32_t refreshHz)
+{
+ return PlsrProfileHzToQ32(slopeHzPerMs) * UINT64_C(1000)
+ / (uint64_t)refreshHz;
+}
+
+/* 从当前频率减速到目标频率所需的脉冲数（Q32.32 精度）。 */
+static uint64_t PlsrProfileDecelPulsesQ32(uint32_t frequencyHz,
+ uint32_t targetHz,
+ uint32_t slopeHzPerMs)
+{
+ uint64_t numerator;
+ uint64_t denominator;
+
+ if (frequencyHz <= targetHz)
+ {
+ return 0UL;
+ }
+ numerator = (uint64_t)frequencyHz * (uint64_t)frequencyHz
+ - (uint64_t)targetHz * (uint64_t)targetHz;
+ denominator = 2000UL * (uint64_t)slopeHzPerMs;
+ if (denominator == 0UL)
+ {
+ return 0UL;
+ }
+ return (numerator / denominator) * PLSR_PROFILE_Q32_ONE
+ + ((numerator % denominator) * PLSR_PROFILE_Q32_ONE)
+ / denominator;
+}
+
+/* 直线增量初始化。 */
+static void PlsrProfileInitLinearDelta(PLSR_PROFILE_STATE *state,
+ uint32_t slopeHzPerMs,
+ uint32_t refreshHz)
+{
+ state->deltaPerStepQ32 = PlsrProfileSlopePerStepQ32(slopeHzPerMs,
+ refreshHz);
+ state->jerkPerStepQ32 = 0UL;
+ state->curveStep = 0U;
+ state->curveStepLimit = 0U;
+ state->sPhase = 0U;
+}
+
+/* S 形/正弦增量初始化：增量先升后降（S形）或正弦半波。 */
+static void PlsrProfileInitCurveDelta(PLSR_PROFILE_STATE *state,
+ uint32_t frequencyGapHz,
+ uint32_t slopeHzPerMs,
+ uint32_t refreshHz,
+ uint8_t curveMode)
+{
+ uint64_t linearPerStep = PlsrProfileSlopePerStepQ32(slopeHzPerMs,
+ refreshHz);
+ uint64_t integerPerStepHz;
+ uint64_t rampSteps;
+ uint64_t jerkQ32;
+
+ integerPerStepHz = ((uint64_t)slopeHzPerMs * UINT64_C(1000))
+ / (uint64_t)refreshHz;
+ if (integerPerStepHz == 0UL)
+ {
+ integerPerStepHz = 1UL;
+ }
+ rampSteps = ((uint64_t)frequencyGapHz + integerPerStepHz - 1UL)
+ / integerPerStepHz;
+
+ if (curveMode == PLSR_PROFILE_CURVE_SINE)
+ {
+ /* 正弦：时间同直线，峰值增量 = 直线增量 * pi/2（面积匹配）。 */
+ state->deltaPerStepQ32 = 0UL;
+ state->jerkPerStepQ32 = 0UL;
+ state->curveStep = 0U;
+ state->curveStepLimit = (uint32_t)rampSteps;
+ state->sPhase = 0U;
+ return;
+ }
+
+ /* S 形：增量 0→峰值→0，总时间 2 倍直线时间。 */
+ jerkQ32 = linearPerStep / rampSteps;
+ state->deltaPerStepQ32 = 0UL;
+ state->jerkPerStepQ32 = jerkQ32;
+ state->curveStep = 0U;
+ state->curveStepLimit = (uint32_t)(rampSteps * 2UL);
+ state->sPhase = 0U;
+}
+
+/* 进入加速段（直线或曲线）。 */
+static void PlsrProfileBeginAccel(PLSR_PROFILE_STATE *state)
+{
+ uint32_t gapHz;
+
+ if (state->targetFrequencyHz <= state->frequencyQ32 >> 32U)
+ {
+ state->phase = PLSR_PROFILE_PHASE_CRUISE;
+ return;
+ }
+ if (state->curveMode == PLSR_PROFILE_CURVE_LINEAR)
+ {
+ PlsrProfileInitLinearDelta(state,
+ state->accelSlopeHzPerMs,
+ state->refreshHz);
+ }
+ else
+ {
+ gapHz = state->targetFrequencyHz
+ - (uint32_t)(state->frequencyQ32 >> 32U);
+ PlsrProfileInitCurveDelta(state,
+ gapHz,
+ state->accelSlopeHzPerMs,
+ state->refreshHz,
+ state->curveMode);
+ }
+ state->phase = PLSR_PROFILE_PHASE_ACCEL;
+}
+
+/* 进入减速段：减速到 decelTargetHz（默认=终止速度，降频时=新目标）。 */
+static void PlsrProfileBeginDecel(PLSR_PROFILE_STATE *state)
+{
+ uint32_t gapHz;
+
+ if (state->frequencyQ32 >> 32U <= state->decelTargetHz)
+ {
+ state->frequencyQ32 = PlsrProfileHzToQ32(state->decelTargetHz);
+ if (state->decelTargetHz == state->stopFrequencyHz)
+ {
+ state->phase = PLSR_PROFILE_PHASE_DONE;
+ }
+ else
+ {
+ /* 降频到新目标：转匀速继续。 */
+ state->targetFrequencyHz = state->decelTargetHz;
+ state->decelTargetHz = state->stopFrequencyHz;
+ state->phase = PLSR_PROFILE_PHASE_CRUISE;
+ }
+ return;
+ }
+ if (state->curveMode == PLSR_PROFILE_CURVE_LINEAR)
+ {
+ PlsrProfileInitLinearDelta(state,
+ state->decelSlopeHzPerMs,
+ state->refreshHz);
+ }
+ else
+ {
+ gapHz = (uint32_t)(state->frequencyQ32 >> 32U)
+ - state->decelTargetHz;
+ PlsrProfileInitCurveDelta(state,
+ gapHz,
+ state->decelSlopeHzPerMs,
+ state->refreshHz,
+ state->curveMode);
+ }
+ state->phase = PLSR_PROFILE_PHASE_DECEL;
+}
+
+/* 曲线模式下推进增量（S形三角增量 / 正弦查表）。 */
+static uint64_t PlsrProfileAdvanceDelta(PLSR_PROFILE_STATE *state,
+ uint64_t linearPerStepQ32)
+{
+ uint64_t delta;
+
+ if (state->curveMode == PLSR_PROFILE_CURVE_SINE)
+ {
+ uint32_t index = ((uint32_t)state->curveStep * 256U)
+ / (uint32_t)state->curveStepLimit;
+
+ if (index > 255U)
+ {
+ index = 255U;
+ }
+ delta = ((uint64_t)PlsrProfileSineTable[index]
+ * linearPerStepQ32) / 65536UL;
+ }
+ else
+ {
+ if (state->sPhase == 0U)
+ {
+ state->deltaPerStepQ32 += state->jerkPerStepQ32;
+ if ((state->curveStepLimit != 0U)
+ && (state->curveStep >= state->curveStepLimit / 2U))
+ {
+ state->sPhase = 1U;
+ }
+ }
+ else
+ {
+ if (state->deltaPerStepQ32 > state->jerkPerStepQ32)
+ {
+ state->deltaPerStepQ32 -= state->jerkPerStepQ32;
+ }
+ else
+ {
+ state->deltaPerStepQ32 = 0UL;
+ }
+ }
+ delta = state->deltaPerStepQ32;
+ }
+ state->curveStep++;
+ return delta;
+}
+
+PLSR_RESULT PlsrProfileStart(PLSR_PROFILE_STATE *state,
+ const PLSR_PROFILE_REQUEST *request,
+ int64_t pulses,
+ uint32_t refreshHz)
+{
+ uint32_t startHz;
+
+ if ((state == NULL) || (request == NULL))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if ((pulses <= 0) || (refreshHz == 0UL)
+ || (request->targetFrequencyHz == 0UL))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+
+ state->refreshHz = refreshHz;
+ state->totalPulses = pulses;
+ state->targetFrequencyHz = request->targetFrequencyHz;
+ if (request->targetFrequencyHz > request->maxFrequencyHz)
+ {
+ state->targetFrequencyHz = request->maxFrequencyHz;
+ }
+ state->startFrequencyHz = request->startFrequencyHz;
+ state->stopFrequencyHz = request->stopFrequencyHz;
+ state->decelTargetHz = request->stopFrequencyHz;
+ state->accelSlopeHzPerMs = request->accelSlopeHzPerMs;
+ state->decelSlopeHzPerMs = request->decelSlopeHzPerMs;
+ state->curveMode = request->curveMode;
+ startHz = request->startFrequencyHz;
+ if (startHz > state->targetFrequencyHz)
+ {
+ startHz = state->targetFrequencyHz;
+ }
+ state->frequencyQ32 = PlsrProfileHzToQ32(startHz);
+ state->emittedPulsesQ32 = 0UL;
+ state->phase = PLSR_PROFILE_PHASE_ACCEL;
+ state->started = 1U;
+
+ if (startHz >= state->targetFrequencyHz)
+ {
+ state->phase = PLSR_PROFILE_PHASE_CRUISE;
+ }
+ else if (request->accelSlopeHzPerMs == 0UL)
+ {
+ state->frequencyQ32 = PlsrProfileHzToQ32(state->targetFrequencyHz);
+ state->phase = PLSR_PROFILE_PHASE_CRUISE;
+ }
+ else
+ {
+ PlsrProfileBeginAccel(state);
+ }
+ return PLSR_RESULT_OK;
+}
+
+PLSR_RESULT PlsrProfileStep(PLSR_PROFILE_STATE *state,
+ uint32_t *frequencyHz,
+ uint8_t *completed)
+{
+ uint64_t emittedThisStep;
+ uint64_t remainingQ32;
+ uint64_t decelNeeded;
+ uint64_t linearPerStepQ32;
+ uint64_t deltaQ32;
+ uint32_t currentHz;
+
+ if ((state == NULL) || (frequencyHz == NULL) || (completed == NULL))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ *completed = 0U;
+ if (state->phase == PLSR_PROFILE_PHASE_DONE)
+ {
+ *frequencyHz = 0U;
+ *completed = 1U;
+ return PLSR_RESULT_OK;
+ }
+
+ linearPerStepQ32 = PlsrProfileSlopePerStepQ32(
+ (state->phase == PLSR_PROFILE_PHASE_ACCEL)
+ ? state->accelSlopeHzPerMs
+ : state->decelSlopeHzPerMs,
+ state->refreshHz);
+
+ switch (state->phase)
+ {
+ case PLSR_PROFILE_PHASE_ACCEL:
+ deltaQ32 = PlsrProfileAdvanceDelta(state, linearPerStepQ32);
+ state->frequencyQ32 += deltaQ32;
+ if ((state->frequencyQ32
+ >= PlsrProfileHzToQ32(state->targetFrequencyHz))
+ || ((state->curveMode != PLSR_PROFILE_CURVE_LINEAR)
+ && (state->curveStep >= state->curveStepLimit)))
+ {
+ state->frequencyQ32 =
+ PlsrProfileHzToQ32(state->targetFrequencyHz);
+ state->phase = PLSR_PROFILE_PHASE_CRUISE;
+ }
+ break;
+
+ case PLSR_PROFILE_PHASE_CRUISE:
+ /* 剩余脉冲不足以按当前频率完成减速时开始减速（Q32 精度）。 */
+ remainingQ32 = PlsrProfileHzToQ32(
+ (uint32_t)state->totalPulses)
+ - state->emittedPulsesQ32;
+ decelNeeded = PlsrProfileDecelPulsesQ32(
+ (uint32_t)(state->frequencyQ32 >> 32U),
+ state->stopFrequencyHz,
+ state->decelSlopeHzPerMs);
+ if (remainingQ32 <= decelNeeded)
+ {
+ PlsrProfileBeginDecel(state);
+ }
+ break;
+
+ case PLSR_PROFILE_PHASE_DECEL:
+ deltaQ32 = PlsrProfileAdvanceDelta(state, linearPerStepQ32);
+ state->frequencyQ32 -= deltaQ32;
+ if (state->frequencyQ32
+ <= PlsrProfileHzToQ32(state->decelTargetHz))
+ {
+ state->frequencyQ32 =
+ PlsrProfileHzToQ32(state->decelTargetHz);
+ if (state->decelTargetHz == state->stopFrequencyHz)
+ {
+ state->phase = PLSR_PROFILE_PHASE_DONE;
+ }
+ else
+ {
+ /* 降频到新目标：转匀速继续。 */
+ state->targetFrequencyHz = state->decelTargetHz;
+ state->decelTargetHz = state->stopFrequencyHz;
+ state->phase = PLSR_PROFILE_PHASE_CRUISE;
+ }
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ /* 发射本步脉冲：频率(Hz) / 刷新率。 */
+ currentHz = (uint32_t)(state->frequencyQ32 >> 32U);
+ emittedThisStep = state->frequencyQ32 / (uint64_t)state->refreshHz;
+ if ((state->emittedPulsesQ32 + emittedThisStep)
+ >= PlsrProfileHzToQ32((uint32_t)state->totalPulses))
+ {
+ state->emittedPulsesQ32 =
+ PlsrProfileHzToQ32((uint32_t)state->totalPulses);
+ state->phase = PLSR_PROFILE_PHASE_DONE;
+ }
+ else
+ {
+ state->emittedPulsesQ32 += emittedThisStep;
+ }
+
+ *frequencyHz = currentHz;
+ if (state->phase == PLSR_PROFILE_PHASE_DONE)
+ {
+ *completed = 1U;
+ *frequencyHz = 0U;
+ }
+ return PLSR_RESULT_OK;
+}
+
+PLSR_RESULT PlsrProfileRetarget(PLSR_PROFILE_STATE *state,
+ uint32_t newTargetFrequencyHz)
+{
+ uint32_t currentHz;
+
+ if (state == NULL)
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ if (newTargetFrequencyHz == 0UL)
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ currentHz = (uint32_t)(state->frequencyQ32 >> 32U);
+ if (newTargetFrequencyHz == state->targetFrequencyHz)
+ {
+ return PLSR_RESULT_OK;
+ }
+ state->targetFrequencyHz = newTargetFrequencyHz;
+
+ if (newTargetFrequencyHz > currentHz)
+ {
+ /* 升频：加速/匀速 → 加速；减速中 → 取消减速转加速。 */
+ if ((state->phase == PLSR_PROFILE_PHASE_DECEL)
+ || (state->phase == PLSR_PROFILE_PHASE_CRUISE)
+ || (state->phase == PLSR_PROFILE_PHASE_DONE))
+ {
+ PlsrProfileBeginAccel(state);
+ }
+ }
+ else
+ {
+ /* 降频：先减速到新目标，再转匀速继续（最终仍减速到终止速度）。 */
+ if ((state->phase == PLSR_PROFILE_PHASE_ACCEL)
+ || (state->phase == PLSR_PROFILE_PHASE_CRUISE))
+ {
+ state->decelTargetHz = newTargetFrequencyHz;
+ PlsrProfileBeginDecel(state);
+ }
+ }
+ return PLSR_RESULT_OK;
+}
+
+PLSR_RESULT PlsrProfilePlan(const PLSR_PROFILE_REQUEST *request,
+ int64_t pulses,
+ PLSR_PROFILE_PLAN *plan)
+{
+ double target;
+ double start;
+ double stop;
+ double accelSlope;
+ double decelSlope;
+ double accelTime;
+ double decelTime;
+ double accelPulses;
+ double decelPulses;
+ double cruisePulses;
+ double cruiseTime;
+ double peak;
+
+ if ((request == NULL) || (plan == NULL) || (pulses <= 0))
+ {
+ return PLSR_RESULT_INVALID_ARGUMENT;
+ }
+ (void)memset(plan, 0, sizeof(*plan));
+
+ target = (double)request->targetFrequencyHz;
+ if (target > (double)request->maxFrequencyHz)
+ {
+ target = (double)request->maxFrequencyHz;
+ }
+ start = (double)request->startFrequencyHz;
+ if (start > target)
+ {
+ start = target;
+ }
+ stop = (double)request->stopFrequencyHz;
+ accelSlope = (double)request->accelSlopeHzPerMs;
+ decelSlope = (double)request->decelSlopeHzPerMs;
+
+ accelTime = (target > start) ? (target - start) / accelSlope : 0.0;
+ decelTime = (target > stop) ? (target - stop) / decelSlope : 0.0;
+ accelPulses = (start + target) * accelTime / 2000.0;
+ decelPulses = (target + stop) * decelTime / 2000.0;
+
+ if ((accelPulses + decelPulses) >= (double)pulses)
+ {
+ /* 三角曲线：求峰值频率。 */
+ double sum = (double)pulses * 1000.0
+ + start * start / (2.0 * accelSlope)
+ + stop * stop / (2.0 * decelSlope);
+ double denom = 1.0 / (2.0 * accelSlope)
+ + 1.0 / (2.0 * decelSlope);
+
+ peak = (denom > 0.0) ? sqrt(sum / denom) : target;
+ accelTime = (peak > start) ? (peak - start) / accelSlope : 0.0;
+ decelTime = (peak > stop) ? (peak - stop) / decelSlope : 0.0;
+ accelPulses = (start + peak) * accelTime / 2000.0;
+ decelPulses = (peak + stop) * decelTime / 2000.0;
+ plan->peakFrequencyHz = (uint32_t)peak;
+ plan->triangular = 1U;
+ }
+ else
+ {
+ cruisePulses = (double)pulses - accelPulses - decelPulses;
+ cruiseTime = (target > 0.0) ? cruisePulses * 1000.0 / target : 0.0;
+ plan->cruiseTimeMs = (uint32_t)cruiseTime;
+ plan->peakFrequencyHz = (uint32_t)target;
+ plan->triangular = 0U;
+ }
+
+ plan->accelTimeMs = (uint32_t)accelTime;
+ plan->decelTimeMs = (uint32_t)decelTime;
+ plan->totalTimeMs = plan->accelTimeMs + plan->cruiseTimeMs
+ + plan->decelTimeMs;
+ plan->totalPulses = pulses;
+ return PLSR_RESULT_OK;
+}
diff --git a/PLSR/Test/run_host_tests.ps1 b/PLSR/Test/run_host_tests.ps1
index b550a9d..ebf55ee 100644
--- a/PLSR/Test/run_host_tests.ps1
+++ b/PLSR/Test/run_host_tests.ps1
@@ -57,6 +57,13 @@ $tests = @(
"$workspacePath\PLSR\Src\plsr_core.c"
"$workspacePath\PLSR\Test\test_plsr_path.c"
)
+ },
+ @{
+ Name = 'test_plsr_profile'
+ Sources = @(
+ "$workspacePath\PLSR\Src\plsr_profile.c"
+ "$workspacePath\PLSR\Test\test_plsr_profile.c"
+ )
}
)
@@ -70,7 +77,7 @@ foreach ($test in $tests)
'-Werror'
'-DPLSR_HOST_TEST'
"-I$workspacePath\PLSR\Inc"
- ) + $test.Sources + @('-o', $outputPath)
+ ) + $test.Sources + @('-o', $outputPath, '-lm')
try
{
diff --git a/PLSR/Test/test_plsr_profile.c b/PLSR/Test/test_plsr_profile.c
new file mode 100644
index 0000000..d2224d7
--- /dev/null
+++ b/PLSR/Test/test_plsr_profile.c
@@ -0,0 +1,299 @@
+#include "plsr_profile.h"
+#include
+#include
+
+static int TestFailures;
+static int TestChecks;
+
+#define CHECK(condition) \
+ do \
+ { \
+ TestChecks++; \
+ if (!(condition)) \
+ { \
+ TestFailures++; \
+ (void)printf("FAIL line %d: %s\n", __LINE__, #condition); \
+ } \
+ } while (0)
+
+/* 跑完一段曲线，返回实际脉冲数与峰值频率。 */
+static void RunProfile(const PLSR_PROFILE_REQUEST *request,
+ int64_t pulses,
+ uint32_t refreshHz,
+ uint32_t retargetHz,
+ uint32_t retargetAfterSteps,
+ int64_t *actualPulses,
+ uint32_t *peakHz,
+ int64_t *steps)
+{
+ PLSR_PROFILE_STATE state;
+ uint32_t frequency = 0U;
+ uint8_t completed = 0U;
+ int64_t stepCount = 0;
+ uint32_t peak = 0U;
+
+ CHECK(PlsrProfileStart(&state, request, pulses, refreshHz)
+ == PLSR_RESULT_OK);
+ while ((completed == 0U) && (stepCount < 5000000))
+ {
+ (void)PlsrProfileStep(&state, &frequency, &completed);
+ if (frequency > peak)
+ {
+ peak = frequency;
+ }
+ stepCount++;
+ if ((retargetHz != 0U) && (stepCount == retargetAfterSteps))
+ {
+ CHECK(PlsrProfileRetarget(&state, retargetHz) == PLSR_RESULT_OK);
+ }
+ }
+ CHECK(completed != 0U);
+ *actualPulses = (int64_t)(state.emittedPulsesQ32 >> 32U);
+ *peakHz = peak;
+ *steps = stepCount;
+}
+
+static PLSR_PROFILE_REQUEST MakeRequest(uint32_t targetHz,
+ uint32_t startHz,
+ uint32_t stopHz,
+ uint32_t accelSlope,
+ uint32_t decelSlope,
+ uint8_t curveMode)
+{
+ PLSR_PROFILE_REQUEST request;
+
+ (void)memset(&request, 0, sizeof(request));
+ request.targetFrequencyHz = targetHz;
+ request.startFrequencyHz = startHz;
+ request.stopFrequencyHz = stopHz;
+ request.maxFrequencyHz = 100000UL;
+ request.accelSlopeHzPerMs = accelSlope;
+ request.decelSlopeHzPerMs = decelSlope;
+ request.curveMode = curveMode;
+ return request;
+}
+
+static void TestTrapezoid(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ RunProfile(&request, 20000, 1000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 20000);
+ CHECK(peak == 10000U);
+ CHECK(steps > 2000);
+}
+
+static void TestTriangle(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ /* 脉冲太少：永远到不了目标频率，三角曲线。 */
+ RunProfile(&request, 100, 1000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 100);
+ CHECK(peak < 10000U);
+ CHECK(peak > 0U);
+}
+
+static void TestStartStopSpeed(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 1000U, 500U,
+ 100U, 80U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ RunProfile(&request, 50000, 1000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 50000);
+ CHECK(peak == 10000U);
+}
+
+static void TestSCurve(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 1U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ RunProfile(&request, 20000, 1000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 20000);
+ CHECK(peak == 10000U);
+ CHECK(steps > 2000);
+}
+
+static void TestSine(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 2U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ RunProfile(&request, 20000, 1000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 20000);
+ CHECK(peak == 10000U);
+}
+
+static void TestRetargetUp(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(5000U, 0U, 0U, 100U, 100U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ /* 运行中升频到 10000：平滑过渡且总脉冲精确。 */
+ RunProfile(&request, 30000, 1000U, 10000U, 500, &pulses, &peak, &steps);
+ CHECK(pulses == 30000);
+ CHECK(peak == 10000U);
+}
+
+static void TestRetargetDown(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ /* 运行中降频到 3000：先减速到新目标再匀速，总脉冲精确。 */
+ RunProfile(&request, 30000, 1000U, 3000U, 500, &pulses, &peak, &steps);
+ CHECK(pulses == 30000);
+ CHECK(peak == 10000U);
+ CHECK(steps > 600);
+}
+
+static void TestNoAccel(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 0U, 0U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ RunProfile(&request, 5000, 1000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 5000);
+ CHECK(peak == 10000U);
+ CHECK(steps == 500);
+}
+
+static void TestSubMillisecondRefresh(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ /* 0.1ms 刷新：精度更高，步数是 1ms 的约 10 倍。 */
+ RunProfile(&request, 20000, 10000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 20000);
+ CHECK(peak == 10000U);
+ CHECK(steps > 20000);
+}
+
+static void TestSinglePulse(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(1000U, 0U, 0U, 100U, 100U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ RunProfile(&request, 1, 1000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 1);
+ CHECK(peak <= 1000U);
+}
+
+static void TestHighFrequency(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(100000U, 0U, 0U,
+ 10000U, 10000U, 0U);
+ int64_t pulses;
+ uint32_t peak;
+ int64_t steps;
+
+ RunProfile(&request, 1000000, 1000U, 0U, 0, &pulses, &peak, &steps);
+ CHECK(pulses == 1000000);
+ CHECK(peak == 100000U);
+}
+
+static void TestArgumentValidation(void)
+{
+ PLSR_PROFILE_STATE state;
+ PLSR_PROFILE_REQUEST request = MakeRequest(1000U, 0U, 0U, 100U, 100U, 0U);
+ uint32_t frequency;
+ uint8_t completed;
+
+ CHECK(PlsrProfileStart(NULL, &request, 100, 1000U)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfileStart(&state, NULL, 100, 1000U)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfileStart(&state, &request, 0, 1000U)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfileStart(&state, &request, 100, 0U)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ request.targetFrequencyHz = 0U;
+ CHECK(PlsrProfileStart(&state, &request, 100, 1000U)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ request.targetFrequencyHz = 1000U;
+
+ CHECK(PlsrProfileStep(NULL, &frequency, &completed)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfileStep(&state, NULL, &completed)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfileStep(&state, &frequency, NULL)
+ == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfileRetarget(NULL, 2000U) == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfileRetarget(&state, 0U) == PLSR_RESULT_INVALID_ARGUMENT);
+}
+
+static void TestPlan(void)
+{
+ PLSR_PROFILE_REQUEST request = MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
+ PLSR_PROFILE_PLAN plan;
+ PLSR_PROFILE_REQUEST triangleRequest =
+ MakeRequest(10000U, 0U, 0U, 100U, 100U, 0U);
+
+ CHECK(PlsrProfilePlan(&request, 20000, &plan) == PLSR_RESULT_OK);
+ CHECK(plan.accelTimeMs == 100U);
+ CHECK(plan.decelTimeMs == 100U);
+ CHECK(plan.triangular == 0U);
+ CHECK(plan.peakFrequencyHz == 10000U);
+ CHECK(plan.cruiseTimeMs > 0U);
+
+ CHECK(PlsrProfilePlan(&triangleRequest, 100, &plan) == PLSR_RESULT_OK);
+ CHECK(plan.triangular != 0U);
+ CHECK(plan.peakFrequencyHz < 10000U);
+ CHECK(plan.accelTimeMs > 0U);
+
+ CHECK(PlsrProfilePlan(NULL, 100, &plan) == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfilePlan(&request, 0, &plan) == PLSR_RESULT_INVALID_ARGUMENT);
+ CHECK(PlsrProfilePlan(&request, 100, NULL) == PLSR_RESULT_INVALID_ARGUMENT);
+}
+
+int main(void)
+{
+ TestTrapezoid();
+ TestTriangle();
+ TestStartStopSpeed();
+ TestSCurve();
+ TestSine();
+ TestRetargetUp();
+ TestRetargetDown();
+ TestNoAccel();
+ TestSubMillisecondRefresh();
+ TestSinglePulse();
+ TestHighFrequency();
+ TestArgumentValidation();
+ TestPlan();
+
+ if (TestFailures != 0)
+ {
+ (void)printf("FAIL: %d of %d PLSR profile checks failed\n",
+ TestFailures,
+ TestChecks);
+ return 1;
+ }
+ (void)printf("PASS: %d PLSR profile checks\n", TestChecks);
+ return 0;
+}