From 8ebd1b73823d5ad6442db22c45b15e944e5d690f Mon Sep 17 00:00:00 2001
From: hanyongwei <2043702190@qq.com>
Date: Wed, 26 Aug 2026 11:32:12 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E8=BE=93=E5=87=BA=E8=B5=B7?=
=?UTF-8?q?=E5=A7=8B=E9=A2=91=E7=8E=87=E4=B8=8D=E5=AF=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Signed-off-by: hanyongwei <2043702190@qq.com>
---
iar/plsr.dep | 4 -
plsr/accel_curve/plsr_accel_curve.c | 818 ++++++++++++++------------
plsr/accel_curve/plsr_accel_curve.h | 30 +-
plsr/pulse_driver/plsr_pulse_driver.c | 110 +++-
plsr/pulse_driver/plsr_pulse_driver.h | 5 +
plsr/run_control/plsr_run_control.c | 14 +-
6 files changed, 555 insertions(+), 426 deletions(-)
diff --git a/iar/plsr.dep b/iar/plsr.dep
index d833f96..4ebe714 100644
--- a/iar/plsr.dep
+++ b/iar/plsr.dep
@@ -2775,10 +2775,6 @@
BICOMP
171
-
- ICCARM
- 190
-
diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c
index 82c505b..b7087b4 100644
--- a/plsr/accel_curve/plsr_accel_curve.c
+++ b/plsr/accel_curve/plsr_accel_curve.c
@@ -4,7 +4,7 @@
*
* @details 模块职责
* 实现 plsr_accel_curve.h 全部 API。核心路径：
- * Plan → 估 acc_n/dec_n → 不够则 FitPeak 降峰；
+ * Plan → 估 acc_n/dec_n → 不够则先砍 const_n，再 FitPeak 降峰；
* PulseRtBeginAcc/Dec/Const + Step 供 ISR 每脉冲改频。
*
* S 曲线（时间域，加速度梯形 1:2:1）：
@@ -67,136 +67,50 @@ static uint32_t PlsrAccelCurveIsqrt64(uint64_t val)
}
/**
- * 起跳频率（原始）：f = sqrt(f_from^2 + 2a)，a = df*1000/t_ms。
- * 不钳到 f_to，供与目标频比较；f_from=0 → sqrt(2a)。
+ * a = default_spd * 1000 / ramp_ms（Hz/s）。ramp_ms=0 → a=0（阶跃）。
+ * 与 Plan 里 a_acc/a_dec 同一公式，起跳/落地必须共用，否则首末频对手算对不上。
*/
-static uint32_t PlsrAccelCurveJumpFreqRaw(uint32_t f_from, uint32_t f_to,
- uint32_t t_ms)
+static uint32_t PlsrAccelCurveAccelHzS(uint32_t default_spd, uint32_t ramp_ms)
{
- uint32_t df;
- uint32_t a_hz_s;
- uint32_t f;
- uint64_t val;
-
- if (f_to < 1U)
- {
- return 1U;
- }
- if (t_ms == 0U)
- {
- return f_to;
- }
- df = PlsrAccelCurveAbsDiff(f_from, f_to);
- if (df == 0U)
- {
- return (f_from >= 1U) ? f_from : f_to;
- }
- if (df <= 4294967U)
- {
- a_hz_s = (df * 1000U) / t_ms;
- }
- else
- {
- a_hz_s = df / t_ms * 1000U;
- }
- if (a_hz_s == 0U)
- {
- f = (f_from >= 1U) ? f_from : 1U;
- }
- else
- {
- val = (uint64_t)f_from * (uint64_t)f_from + (2ULL * (uint64_t)a_hz_s);
- f = PlsrAccelCurveIsqrt64(val);
- }
- if (f < 1U)
+ if (ramp_ms == 0U)
{
- f = 1U;
+ return 0U;
}
- return PlsrAccelCurveClampFreq(f);
-}
-
-/**
- * 起跳频率：f = sqrt(f_from^2 + 2a)，a = df*1000/t_ms。
- * f_from=0 → sqrt(2a)；超过 f_to 钳到 f_to。
- */
-static uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
- uint32_t t_ms)
-{
- uint32_t f = PlsrAccelCurveJumpFreqRaw(f_from, f_to, t_ms);
-
- if (f > f_to)
+ if (default_spd == 0U)
{
- f = f_to;
+ return 0U;
}
- return f;
+ return (default_spd * 1000UL) / ramp_ms;
}
/**
- * 斜率分母：K = 默认速度 / 加减速时间，即 (默认速度 − 0)。
- * 起/止速只决定端点，不参与斜率。
+ * 起跳/落地：f = sqrt(f_from^2 + 2a)，a 与斜坡规划相同。
+ * 不钳到 f_to；f_from=0 → sqrt(2a)。
*/
-static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd)
-{
- return default_spd;
-}
-
-static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
- uint32_t f_to,
- uint32_t default_spd,
- uint32_t accel_ms,
- uint32_t decel_ms)
+static uint32_t PlsrAccelCurveJumpFreqFromA(uint32_t f_from, uint32_t a_hz_s)
{
- uint32_t df;
- uint32_t den;
- uint32_t ref_ms;
- uint32_t t;
-
- if (f_from == f_to)
- {
- return 0U;
- }
-
- df = PlsrAccelCurveAbsDiff(f_from, f_to);
- if (f_to > f_from)
- {
- ref_ms = accel_ms;
- den = PlsrAccelCurveSlopeDen(default_spd);
- }
- else
- {
- ref_ms = decel_ms;
- den = PlsrAccelCurveSlopeDen(default_spd);
- }
+ uint64_t val;
+ uint32_t f;
- if (ref_ms == 0U)
- {
- return 0U;
- }
- /* 默认速度未设时，用频差作分母 */
- if (den == 0U)
- {
- den = df;
- }
- if (den == 0U)
+ if (a_hz_s == 0U)
{
- return 0U;
+ return (f_from >= 1U) ? PlsrAccelCurveClampFreq(f_from) : 1U;
}
-
- /* 向上取整，避免 (df*ref)/den 截成 0 导致直接跳到目标频 */
- t = (df * ref_ms + den - 1UL) / den;
- if (t < 1U)
+ val = (uint64_t)f_from * (uint64_t)f_from + (2ULL * (uint64_t)a_hz_s);
+ f = PlsrAccelCurveIsqrt64(val);
+ if (f < 1U)
{
- t = 1U;
+ f = 1U;
}
- return t;
+ return PlsrAccelCurveClampFreq(f);
}
/**
- * 按「默认起/止速、目标、起跳」三分支解析边界频率：
- * 1) 默认速 > 目标 → 用默认速（再减速到目标 / 出口用止速）
- * 2) 默认速 < 目标 且 目标 < 起跳 → 用目标
- * 3) 默认速 < 目标 且 目标 > 起跳 → 用起跳
- * 起跳 = sqrt(f_cfg^2+2a)（不先钳到目标，便于与目标比较）。
+ * 按「配置起/止速、目标、起跳」三分支解析边界频率：
+ * 1) 配置速 > 目标 → 用配置速
+ * 2) 配置速 < 目标 且 目标 < 起跳 → 用目标
+ * 3) 配置速 < 目标 且 目标 > 起跳 → 用起跳
+ * 起跳 = sqrt(f_cfg^2+2a)，a = default_spd*1000/accel_ms（止速用 decel_ms）。
*/
static uint32_t PlsrAccelCurveResolveBoundHz(uint32_t f_cfg,
uint32_t f_tgt,
@@ -205,7 +119,8 @@ static uint32_t PlsrAccelCurveResolveBoundHz(uint32_t f_cfg,
uint32_t decel_ms,
uint8_t is_start)
{
- uint32_t t_ms;
+ uint32_t ramp_ms;
+ uint32_t a_hz_s;
uint32_t jump;
f_cfg = PlsrAccelCurveClampFreq(f_cfg);
@@ -215,25 +130,32 @@ static uint32_t PlsrAccelCurveResolveBoundHz(uint32_t f_cfg,
return 1U;
}
- /* 默认起/止速 > 目标：用默认速 */
+ /* 配置起/止速 > 目标：用配置速 */
if (f_cfg > f_tgt)
{
return f_cfg;
}
- if (is_start != 0U)
+ ramp_ms = (is_start != 0U) ? accel_ms : decel_ms;
+ a_hz_s = PlsrAccelCurveAccelHzS(default_spd, ramp_ms);
+ /* 默认速度未设：回退为本段频差/时间，与 Plan 回退一致 */
+ if ((a_hz_s == 0U) && (ramp_ms > 0U))
{
- t_ms = PlsrAccelCurveRampTimeMs(f_cfg, f_tgt, default_spd,
- accel_ms, decel_ms);
+ uint32_t df = PlsrAccelCurveAbsDiff(f_cfg, f_tgt);
+
+ if (df > 0U)
+ {
+ a_hz_s = (df * 1000UL) / ramp_ms;
+ }
}
- else
+ if (a_hz_s == 0U)
{
- t_ms = PlsrAccelCurveRampTimeMs(f_tgt, f_cfg, default_spd,
- accel_ms, decel_ms);
+ return (f_cfg >= 1U) ? f_cfg : f_tgt;
}
- jump = PlsrAccelCurveJumpFreqRaw(f_cfg, f_tgt, t_ms);
- /* 目标 < 起跳 → 用目标；目标 > 起跳 → 用起跳 */
+ jump = PlsrAccelCurveJumpFreqFromA(f_cfg, a_hz_s);
+
+ /* 目标 < 起跳 → 用目标；目标 >= 起跳 → 用起跳 */
if (f_tgt < jump)
{
return f_tgt;
@@ -261,17 +183,12 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
accel_ms, decel_ms, 0U);
}
-/** 直线：进度千分比 → 形状千分比 */
-static uint32_t PlsrAccelCurveShapeLinearPermille(uint32_t u)
-{
- return (u > 1000U) ? 1000U : u;
-}
-
/**
* 正弦加减速形状：raised-cosine p=(1-cos(πu))/2
*
* 不用 libm sin/cos（软浮点很重，不宜进脉冲 ISR）。
- * 用 33 点预计算表 + 线性插值，O(1)、Flash 约 66 字节，示波器上与直线/S7 可辨。
+ * 用 33 点预计算表 + 线性插值，O(1)、Flash 约 66 字节。
+ * 仅供 SineFreqAtTime；S 曲线走 jerk 梯形解析式，不走形状表。
*/
static const uint16_t s_plsr_sine_shape_tab[33] = {
0U, 2U, 10U, 22U, 38U, 59U, 84U, 113U,
@@ -304,65 +221,6 @@ static uint32_t PlsrAccelCurveShapeSinePermille(uint32_t u)
return a + (((b - a) * frac) / 1000UL);
}
-/**
- * 七段一侧 1:2:1：头尾更圆、中间更陡（相对直线可辨）
- * u∈[0,250]/[750,1000] 为抛物线圆角，中间线性陡段。
- */
-static uint32_t PlsrAccelCurveShapeS7Permille(uint32_t u)
-{
- uint32_t d;
- uint32_t p;
-
- if (u >= 1000U)
- {
- return 1000U;
- }
- if (u <= 250UL)
- {
- p = (8UL * u * u) / (3UL * 1000UL);
- }
- else if (u <= 750UL)
- {
- p = 167UL + (666UL * (u - 250UL)) / 500UL;
- }
- else
- {
- d = 1000UL - u;
- p = 1000UL - (8UL * d * d) / (3UL * 1000UL);
- }
- return (p > 1000UL) ? 1000UL : p;
-}
-
-/** O(1)：时间进度千分比 → 曲线形状千分比 */
-static uint32_t PlsrAccelCurveShapePermille(uint32_t u, PlsrAccelMode_e mode)
-{
- if (mode == PLSR_ACCEL_S)
- {
- return PlsrAccelCurveShapeS7Permille(u);
- }
- if (mode == PLSR_ACCEL_SINE)
- {
- return PlsrAccelCurveShapeSinePermille(u);
- }
- return PlsrAccelCurveShapeLinearPermille(u);
-}
-
-static uint32_t PlsrAccelCurveShape(uint32_t t_ms, uint32_t T_ms, PlsrAccelMode_e mode)
-{
- uint32_t u;
-
- if ((T_ms == 0U) || (t_ms >= T_ms))
- {
- return 1000U;
- }
- u = (t_ms * 1000UL) / T_ms;
- if (u > 1000U)
- {
- u = 1000U;
- }
- return PlsrAccelCurveShapePermille(u, mode);
-}
-
static uint32_t PlsrAccelCurveLerp(uint32_t a, uint32_t b, uint32_t ratio_permille)
{
if (ratio_permille >= 1000U)
@@ -376,22 +234,6 @@ static uint32_t PlsrAccelCurveLerp(uint32_t a, uint32_t b, uint32_t ratio_permil
return a - ((a - b) * ratio_permille) / 1000UL;
}
-/**
- * a = default_spd * 1000 / ramp_ms（Hz/s）。ramp_ms=0 → a=0（阶跃）。
- */
-static uint32_t PlsrAccelCurveAccelHzS(uint32_t default_spd, uint32_t ramp_ms)
-{
- if (ramp_ms == 0U)
- {
- return 0U;
- }
- if (default_spd == 0U)
- {
- return 0U;
- }
- return (default_spd * 1000UL) / ramp_ms;
-}
-
/**
* 脉冲闭合：N = ceil(|f1^2 - f0^2| / (2a))。
* a=0 且频差非 0 → 0（阶跃，无斜坡脉冲）。
@@ -633,133 +475,21 @@ static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t f0, uint32_t f1,
return PlsrAccelCurveClampFreq(f);
}
-/** S 斜坡脉冲数 ≈ ∫f dt（解析三段） */
-static uint32_t PlsrAccelCurvePulsesForSRamp(uint32_t f0, uint32_t f1,
- uint32_t a_hz_s)
-{
- uint32_t tj;
- uint32_t ta;
- uint32_t tramp;
- uint32_t flo;
- uint32_t fhi;
- uint64_t acc_us; /* 脉冲·us，最后 /1e6 */
- uint64_t n;
- uint32_t g_half_j;
-
- if (f0 == f1)
- {
- return 0U;
- }
- if (a_hz_s == 0U)
- {
- return 0U;
- }
-
- PlsrAccelCurveSTimeParams(f0, f1, a_hz_s, &tj, &ta, &tramp);
- if (tramp < 1U)
- {
- return 1U;
- }
-
- flo = (f0 < f1) ? f0 : f1;
- fhi = (f0 < f1) ? f1 : f0;
- g_half_j = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, tj, tj); /* 0.5*A*Tj */
-
- /*
- * I1 = f_start*Tj + A*Tj^2/6
- * = flo*Tj + (1/3)*g_half_j*Tj （因 A*Tj^2/6 = (0.5*A*Tj)*Tj/3）
- * 更稳：I1 = ∫(flo + 0.5*J t^2) = flo*tj_s + A*tj_s^2/6
- * 用 us： (flo*tj + A*tj*tj/(6e6)) 作为脉冲·us
- */
- acc_us = (uint64_t)flo * (uint64_t)tj;
- acc_us += ((uint64_t)a_hz_s * (uint64_t)tj / 6ULL) * (uint64_t)tj / 1000000ULL;
-
- /* I2：起点 flo+g_half_j，时长 Ta，+0.5*A*Ta^2 */
- {
- uint32_t f2 = flo + g_half_j;
- if (f2 > fhi)
- {
- f2 = fhi;
- }
- acc_us += (uint64_t)f2 * (uint64_t)ta;
- acc_us += ((uint64_t)a_hz_s * (uint64_t)ta / 2ULL) * (uint64_t)ta / 1000000ULL;
- }
-
- /* I3：与 I1 对称，用 fhi 替换 flo */
- acc_us += (uint64_t)fhi * (uint64_t)tj;
- acc_us -= ((uint64_t)a_hz_s * (uint64_t)tj / 6ULL) * (uint64_t)tj / 1000000ULL;
-
- /* n = ceil(acc_us / 1e6) */
- n = (acc_us + 999999ULL) / 1000000ULL;
- if (n < 1ULL)
- {
- n = 1ULL;
- }
- if (n > 0xFFFFFFFFULL)
- {
- return 0xFFFFFFFFUL;
- }
- /* 略留余量，避免时间未跑完就按 n_total 收尾 */
- if (n < 0xFFFFFFF0ULL)
- {
- n += (n / 32ULL) + 2ULL;
- }
- return (uint32_t)n;
-}
-
-/** 正弦：∫f dt = (f0+f1)/2 * T */
-static uint32_t PlsrAccelCurvePulsesForSineRamp(uint32_t f0, uint32_t f1,
- uint32_t a_hz_s)
-{
- uint32_t tramp;
- uint64_t n;
- uint32_t favg;
-
- if (f0 == f1)
- {
- return 0U;
- }
- if (a_hz_s == 0U)
- {
- return 0U;
- }
- PlsrAccelCurveSineTimeParams(f0, f1, a_hz_s, &tramp);
- if (tramp < 1U)
- {
- return 1U;
- }
- favg = f0 / 2U + f1 / 2U + (((f0 & 1U) + (f1 & 1U)) / 2U);
- if (favg < 1U)
- {
- favg = 1U;
- }
- n = ((uint64_t)favg * (uint64_t)tramp + 999999ULL) / 1000000ULL;
- if (n < 1ULL)
- {
- n = 1ULL;
- }
- if (n < 0xFFFFFFF0ULL)
- {
- n += (n / 32ULL) + 2ULL;
- }
- if (n > 0xFFFFFFFFULL)
- {
- return 0xFFFFFFFFUL;
- }
- return (uint32_t)n;
-}
+/**
+ * 与 ISR / 建表一致：逐拍 Δt=1/f 推进时间剖面，得到 S/正弦斜坡脉冲数。
+ * （旧版 ∫f dt 解析估算已废弃，避免与离散运行不一致。）
+ */
+static uint32_t PlsrAccelCurvePulsesForDiscreteProfile(uint32_t f0, uint32_t f1,
+ uint32_t a_hz_s,
+ PlsrAccelMode_e mode);
static uint32_t PlsrAccelCurvePulsesForModeRamp(uint32_t f0, uint32_t f1,
uint32_t a_hz_s,
PlsrAccelMode_e mode)
{
- if (mode == PLSR_ACCEL_S)
- {
- return PlsrAccelCurvePulsesForSRamp(f0, f1, a_hz_s);
- }
- if (mode == PLSR_ACCEL_SINE)
+ if ((mode == PLSR_ACCEL_SINE) || (mode == PLSR_ACCEL_S))
{
- return PlsrAccelCurvePulsesForSineRamp(f0, f1, a_hz_s);
+ return PlsrAccelCurvePulsesForDiscreteProfile(f0, f1, a_hz_s, mode);
}
return PlsrAccelCurvePulsesForRamp(f0, f1, a_hz_s);
}
@@ -892,14 +622,89 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
return f_cur - best;
}
+/** 按峰值重算加速/减速脉冲预算 */
+static void PlsrAccelCurveRecalcRampNs(uint32_t f_cur,
+ uint32_t f_peak,
+ uint32_t f_end,
+ uint32_t a_acc,
+ uint32_t a_dec,
+ PlsrAccelMode_e mode,
+ uint32_t *acc_n,
+ uint32_t *dec_n)
+{
+ if (f_cur > f_peak)
+ {
+ *acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_dec, mode);
+ }
+ else
+ {
+ *acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_acc, mode);
+ }
+ if (f_end > f_peak)
+ {
+ *dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_acc, mode);
+ }
+ else
+ {
+ *dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, mode);
+ }
+}
+
+/**
+ * FitPeak 后再对齐离散取整：禁止只截断 dec_n，逐步降峰直到 acc+dec<=total。
+ */
+static uint32_t PlsrAccelCurveFitPeakToBudget(uint32_t total_pulses,
+ uint32_t f_cur,
+ uint32_t f_want,
+ uint32_t f_end,
+ uint32_t a_acc,
+ uint32_t a_dec,
+ PlsrAccelMode_e mode,
+ uint32_t *acc_n,
+ uint32_t *dec_n)
+{
+ uint32_t f_peak;
+ uint32_t guard;
+
+ f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_want, f_end,
+ a_acc, a_dec, mode);
+ PlsrAccelCurveRecalcRampNs(f_cur, f_peak, f_end, a_acc, a_dec, mode,
+ acc_n, dec_n);
+
+ guard = 0U;
+ while ((*acc_n + *dec_n) > total_pulses && (guard < 100000U))
+ {
+ guard++;
+ if (f_want >= f_cur)
+ {
+ if (f_peak <= f_cur)
+ {
+ break;
+ }
+ f_peak--;
+ }
+ else
+ {
+ if (f_peak >= f_cur)
+ {
+ break;
+ }
+ f_peak++;
+ }
+ PlsrAccelCurveRecalcRampNs(f_cur, f_peak, f_end, a_acc, a_dec, mode,
+ acc_n, dec_n);
+ }
+ return f_peak;
+}
+
/**
* @brief 规划一整段脉冲域（见 plsr_accel_curve.h）
*
* 算法步骤：
* 1. 钳频、解析 f_cur/f_end 若仍 <1；
- * 2. 由 default_spd 与 accel/decel_ms 算 a_acc/a_dec（无默认速则频差回退）；
- * 3. PulsesForRamp 估 acc_n/dec_n；若和 > total → FitPeak 降峰重估；
- * 4. 写出 plan 三相预算。
+ * 2. 由 default_spd 与 accel/decel_ms 算 a_acc/a_dec；
+ * 3. 估 acc_n/dec_n；够则 const_n=total-acc-dec；
+ * 4. 不够则 const_n=0，再 FitPeak 降峰（不单独砍 dec_n）。
*/
void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
uint32_t total_pulses,
@@ -1005,40 +810,26 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
return;
}
- if ((acc_n + dec_n) > total_pulses)
+ if (acc_n + dec_n <= total_pulses)
{
- f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end,
- a_acc, a_dec, m);
- if (f_cur > f_peak)
- {
- acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_dec, m);
- }
- else
- {
- acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_acc, m);
- }
- if (f_end > f_peak)
- {
- dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_acc, m);
- }
- else
- {
- dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m);
- }
- if (acc_n > total_pulses)
- {
- acc_n = total_pulses;
- dec_n = 0U;
- }
- else if ((acc_n + dec_n) > total_pulses)
- {
- dec_n = total_pulses - acc_n;
- }
- plan->const_n = 0U;
+ plan->const_n = total_pulses - acc_n - dec_n;
+ plan->f_tgt = f_peak;
+ plan->acc_n = acc_n;
+ plan->dec_n = dec_n;
}
else
{
- plan->const_n = total_pulses - acc_n - dec_n;
+ /*
+ * 脉冲不够：先砍掉匀速段，保证加减速按完整剖面预算；
+ * 仍超出则降峰成三角，禁止 dec_n=total-acc_n 式截断。
+ */
+ plan->const_n = 0U;
+ f_peak = PlsrAccelCurveFitPeakToBudget(total_pulses, f_cur, f_tgt, f_end,
+ a_acc, a_dec, m,
+ &acc_n, &dec_n);
+ plan->f_tgt = f_peak;
+ plan->acc_n = acc_n;
+ plan->dec_n = dec_n;
}
if ((f_peak > f_end) && (total_pulses > 1U) && (dec_n == 0U))
@@ -1226,16 +1017,26 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
/**
* @brief 为 S/正弦准备时间剖面（加速度梯形 / raised-cosine）
*/
+static uint32_t PlsrAccelPulseRtTimeStep(PlsrAccelPulseRt_t *rt);
+
+/* S/正弦频率预计算表：PlanSeg 时填好；ISR 只切换/查表。匀速不改频。 */
+#define PLSR_RAMP_TBL_MAX 4096U
+static uint32_t s_acc_tbl[PLSR_RAMP_TBL_MAX];
+static uint32_t s_dec_tbl[PLSR_RAMP_TBL_MAX];
+static uint32_t s_acc_tbl_len;
+static uint32_t s_acc_tbl_stride;
+static uint32_t s_acc_tbl_n;
+static uint32_t s_dec_tbl_len;
+static uint32_t s_dec_tbl_stride;
+static uint32_t s_dec_tbl_n;
+static uint32_t *s_rt_tbl; /* ISR 当前相使用的表 */
+
static void PlsrAccelPulseRtSetupTimeProfile(PlsrAccelPulseRt_t *rt)
{
rt->t_us = 0U;
rt->tj_us = 0U;
rt->ta_us = 0U;
rt->tramp_us = 0U;
- rt->phase_q24 = 0U;
- rt->step_q24 = 0U;
- rt->step_rem = 0U;
- rt->rem_acc = 0U;
if ((rt->a < 1U) || (rt->f0 == rt->f1))
{
@@ -1252,6 +1053,189 @@ static void PlsrAccelPulseRtSetupTimeProfile(PlsrAccelPulseRt_t *rt)
}
}
+/** 离散仿真一步：与建表 / 脉冲预算共用 */
+static void PlsrAccelPulseRtSimOne(PlsrAccelPulseRt_t *rt)
+{
+ if (rt->n < 0xFFFFFFFFUL)
+ {
+ rt->n++;
+ }
+ rt->f = PlsrAccelPulseRtTimeStep(rt);
+ if ((rt->tramp_us > 0U) && (rt->t_us >= rt->tramp_us))
+ {
+ rt->f = rt->f1;
+ rt->active = 0U;
+ }
+ else if (rt->rising != 0U)
+ {
+ if (rt->f >= rt->f1)
+ {
+ rt->f = rt->f1;
+ rt->active = 0U;
+ }
+ }
+ else
+ {
+ if (rt->f <= rt->f1)
+ {
+ rt->f = rt->f1;
+ rt->active = 0U;
+ }
+ }
+}
+
+/**
+ * 将本相离散剖面写入 dst[]，返回写入长度；*stride_out 为抽样步长。
+ */
+static uint32_t PlsrAccelPulseRtFillTable(PlsrAccelPulseRt_t *rt,
+ uint32_t *dst,
+ uint32_t dst_max,
+ uint32_t *stride_out)
+{
+ PlsrAccelPulseRt_t sim;
+ uint32_t i;
+ uint32_t idx;
+ uint32_t stride;
+ uint32_t len;
+
+ *stride_out = 1U;
+ if ((rt->active == 0U) || (rt->n_total < 1U) ||
+ (rt->mode == PLSR_ACCEL_LINEAR) || (dst == (uint32_t *)0) ||
+ (dst_max < 1U))
+ {
+ return 0U;
+ }
+
+ stride = 1U;
+ if (rt->n_total > dst_max)
+ {
+ stride = (rt->n_total + dst_max - 1U) / dst_max;
+ }
+ *stride_out = stride;
+
+ sim = *rt;
+ sim.n = 0U;
+ sim.f = rt->f0;
+ sim.active = 1U;
+ PlsrAccelPulseRtSetupTimeProfile(&sim);
+ if (sim.tramp_us < 1U)
+ {
+ dst[0] = rt->f1;
+ return 1U;
+ }
+
+ len = 0U;
+ for (i = 0U; i < rt->n_total; i++)
+ {
+ PlsrAccelPulseRtSimOne(&sim);
+ idx = (sim.n - 1U) / stride;
+ if (idx >= dst_max)
+ {
+ idx = dst_max - 1U;
+ }
+ dst[idx] = sim.f;
+ if ((idx + 1U) > len)
+ {
+ len = idx + 1U;
+ }
+ if (sim.active == 0U)
+ {
+ while ((i + 1U) < rt->n_total)
+ {
+ i++;
+ idx = i / stride;
+ if (idx >= dst_max)
+ {
+ idx = dst_max - 1U;
+ }
+ dst[idx] = rt->f1;
+ if ((idx + 1U) > len)
+ {
+ len = idx + 1U;
+ }
+ }
+ break;
+ }
+ }
+
+ if (len > 0U)
+ {
+ dst[len - 1U] = rt->f1;
+ }
+ return len;
+}
+
+/**
+ * PlanSeg 后预建加/减速表（任务上下文，勿在 ISR 里做）。
+ */
+void PlsrAccelPulseRtPrebuild(const PlsrAccelPlan_t *plan)
+{
+ PlsrAccelPulseRt_t tmp;
+
+ s_acc_tbl_len = 0U;
+ s_dec_tbl_len = 0U;
+ s_acc_tbl_stride = 1U;
+ s_dec_tbl_stride = 1U;
+ s_acc_tbl_n = 0U;
+ s_dec_tbl_n = 0U;
+ s_rt_tbl = (uint32_t *)0;
+
+ if ((plan == (const PlsrAccelPlan_t *)0) ||
+ (plan->mode == PLSR_ACCEL_LINEAR))
+ {
+ return;
+ }
+
+ if ((plan->acc_n > 0U) && (plan->f_cur != plan->f_tgt))
+ {
+ tmp.f0 = plan->f_cur;
+ tmp.f1 = plan->f_tgt;
+ tmp.f = plan->f_cur;
+ tmp.n = 0U;
+ tmp.n_total = plan->acc_n;
+ tmp.rising = (plan->f_tgt >= plan->f_cur) ? 1U : 0U;
+ tmp.a = (tmp.rising != 0U) ? plan->a_acc : plan->a_dec;
+ tmp.mode = plan->mode;
+ tmp.active = 1U;
+ tmp.use_tbl = 0U;
+ s_acc_tbl_n = plan->acc_n;
+ s_acc_tbl_len = PlsrAccelPulseRtFillTable(&tmp, s_acc_tbl,
+ PLSR_RAMP_TBL_MAX,
+ &s_acc_tbl_stride);
+ }
+
+ if ((plan->dec_n > 0U) && (plan->f_tgt != plan->f_end))
+ {
+ tmp.f0 = plan->f_tgt;
+ tmp.f1 = plan->f_end;
+ tmp.f = plan->f_tgt;
+ tmp.n = 0U;
+ tmp.n_total = plan->dec_n;
+ tmp.rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U;
+ tmp.a = (tmp.rising != 0U) ? plan->a_acc : plan->a_dec;
+ tmp.mode = plan->mode;
+ tmp.active = 1U;
+ tmp.use_tbl = 0U;
+ s_dec_tbl_n = plan->dec_n;
+ s_dec_tbl_len = PlsrAccelPulseRtFillTable(&tmp, s_dec_tbl,
+ PLSR_RAMP_TBL_MAX,
+ &s_dec_tbl_stride);
+ }
+}
+
+static void PlsrAccelPulseRtAttachTable(PlsrAccelPulseRt_t *rt,
+ uint32_t *tbl,
+ uint32_t tbl_len,
+ uint32_t tbl_stride,
+ uint32_t built_n)
+{
+ (void)built_n;
+ s_rt_tbl = tbl;
+ rt->tbl_len = tbl_len;
+ rt->tbl_stride = (tbl_stride < 1U) ? 1U : tbl_stride;
+ rt->use_tbl = ((tbl != (uint32_t *)0) && (tbl_len > 0U)) ? 1U : 0U;
+}
+
/** @brief 进入加速相（见 plsr_accel_curve.h） */
void PlsrAccelPulseRtBeginAcc(PlsrAccelPulseRt_t *rt,
const PlsrAccelPlan_t *plan)
@@ -1270,6 +1254,8 @@ void PlsrAccelPulseRtBeginAcc(PlsrAccelPulseRt_t *rt,
rt->mode = plan->mode;
rt->active = (plan->acc_n > 0U) && (plan->f_cur != plan->f_tgt) ? 1U : 0U;
PlsrAccelPulseRtSetupTimeProfile(rt);
+ PlsrAccelPulseRtAttachTable(rt, s_acc_tbl, s_acc_tbl_len,
+ s_acc_tbl_stride, s_acc_tbl_n);
}
/** @brief 进入减速相（见 plsr_accel_curve.h） */
@@ -1291,6 +1277,9 @@ void PlsrAccelPulseRtBeginDec(PlsrAccelPulseRt_t *rt,
rt->mode = plan->mode;
rt->active = (plan->dec_n > 0U) && (plan->f_tgt != plan->f_end) ? 1U : 0U;
PlsrAccelPulseRtSetupTimeProfile(rt);
+ /* 表已在 PlanSeg/Prebuild 填好；ISR 内禁止再建表 */
+ PlsrAccelPulseRtAttachTable(rt, s_dec_tbl, s_dec_tbl_len,
+ s_dec_tbl_stride, s_dec_tbl_n);
}
/** @brief 进入匀速相（见 plsr_accel_curve.h） */
@@ -1309,15 +1298,14 @@ void PlsrAccelPulseRtBeginConst(PlsrAccelPulseRt_t *rt,
rt->n_total = plan->const_n;
rt->rising = 1U;
rt->mode = plan->mode;
- rt->active = 0U; /* 匀速：确认频率没变 → 复用 → 结束 */
+ rt->active = 0U; /* 匀速：ISR 不改频 */
+ rt->use_tbl = 0U;
+ rt->tbl_len = 0U;
+ rt->tbl_stride = 1U;
rt->t_us = 0U;
rt->tj_us = 0U;
rt->ta_us = 0U;
rt->tramp_us = 0U;
- rt->step_q24 = 0U;
- rt->step_rem = 0U;
- rt->phase_q24 = 0U;
- rt->rem_acc = 0U;
}
/**
@@ -1452,11 +1440,60 @@ static uint32_t PlsrAccelPulseRtTimeStep(PlsrAccelPulseRt_t *rt)
return f;
}
+static uint32_t PlsrAccelCurvePulsesForDiscreteProfile(uint32_t f0, uint32_t f1,
+ uint32_t a_hz_s,
+ PlsrAccelMode_e mode)
+{
+ PlsrAccelPulseRt_t rt;
+ uint32_t guard;
+
+ if (f0 == f1 || a_hz_s == 0U)
+ {
+ return 0U;
+ }
+
+ rt.f0 = f0;
+ rt.f1 = f1;
+ rt.f = f0;
+ rt.n = 0U;
+ rt.rising = (f1 >= f0) ? 1U : 0U;
+ rt.a = a_hz_s;
+ rt.mode = mode;
+ rt.active = 1U;
+ rt.t_us = 0U;
+ rt.tj_us = 0U;
+ rt.ta_us = 0U;
+ rt.tramp_us = 0U;
+ rt.use_tbl = 0U;
+ rt.tbl_len = 0U;
+ rt.tbl_stride = 1U;
+ PlsrAccelPulseRtSetupTimeProfile(&rt);
+ if (rt.tramp_us < 1U)
+ {
+ return 1U;
+ }
+
+ guard = 0U;
+ while ((rt.active != 0U) && (guard < 5000000U))
+ {
+ guard++;
+ PlsrAccelPulseRtSimOne(&rt);
+ }
+
+ if (rt.n == 0U)
+ {
+ return 1U;
+ }
+ return rt.n;
+}
+
/**
* @brief 本相前进一脉冲（见 plsr_accel_curve.h）
*/
uint32_t PlsrAccelPulseRtStep(PlsrAccelPulseRt_t *rt)
{
+ uint32_t idx;
+
if (rt == (PlsrAccelPulseRt_t *)0)
{
return 0U;
@@ -1482,21 +1519,47 @@ uint32_t PlsrAccelPulseRtStep(PlsrAccelPulseRt_t *rt)
rt->f = PlsrAccelPulseRtSquareStep(rt);
}
}
+ else if ((rt->use_tbl != 0U) && (s_rt_tbl != (uint32_t *)0))
+ {
+ /* ISR 热路径：只查表，不做 S/正弦实时积分 */
+ if (rt->tbl_len < 1U)
+ {
+ rt->f = rt->f1;
+ }
+ else
+ {
+ uint32_t stride = (rt->tbl_stride < 1U) ? 1U : rt->tbl_stride;
+ idx = (rt->n - 1U) / stride;
+ if (idx >= rt->tbl_len)
+ {
+ idx = rt->tbl_len - 1U;
+ }
+ rt->f = s_rt_tbl[idx];
+ }
+ }
else
{
+ /* 建表失败兜底：仍走时间步进 */
rt->f = PlsrAccelPulseRtTimeStep(rt);
}
- if (((rt->n_total > 0U) && (rt->n >= rt->n_total)) ||
- ((rt->mode != PLSR_ACCEL_LINEAR) && (rt->tramp_us > 0U) &&
- (rt->t_us >= rt->tramp_us)))
+ if ((rt->n_total > 0U) && (rt->n >= rt->n_total))
{
rt->f = rt->f1;
rt->active = 0U;
}
- else if (rt->mode != PLSR_ACCEL_LINEAR)
+ else if (rt->mode == PLSR_ACCEL_LINEAR)
{
- if (rt->rising != 0U)
+ /* 直线由 SquareStep 钳位 */
+ }
+ else if (rt->use_tbl == 0U)
+ {
+ if ((rt->tramp_us > 0U) && (rt->t_us >= rt->tramp_us))
+ {
+ rt->f = rt->f1;
+ rt->active = 0U;
+ }
+ else if (rt->rising != 0U)
{
if (rt->f >= rt->f1)
{
@@ -1504,14 +1567,11 @@ uint32_t PlsrAccelPulseRtStep(PlsrAccelPulseRt_t *rt)
rt->active = 0U;
}
}
- else
+ else if (rt->f <= rt->f1)
{
- if (rt->f <= rt->f1)
- {
- rt->f = rt->f1;
- rt->active = 0U;
- }
+ rt->f = rt->f1;
+ rt->active = 0U;
}
}
- return rt->f;
+ return PlsrAccelCurveClampFreq(rt->f);
}
diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h
index 40b8a2f..7e507e8 100644
--- a/plsr/accel_curve/plsr_accel_curve.h
+++ b/plsr/accel_curve/plsr_accel_curve.h
@@ -5,9 +5,9 @@
* @details 按起/峰/止频率、斜率时间与总脉冲规划三相脉冲预算，并在脉冲序号上取频。
* 不读 wait_type，不碰 TIM/GPIO。
*
- * 直线：f_n = sqrt(f0^2 ± 2 a n)（脉冲域）。
- * S/正弦：时间轴规划——S 为 jerk 升/恒加速/jerk 降（加速度梯形 1:2:1）；
- * 正弦为 raised-cosine；ISR 按累计时间取频，每拍推进 Δt=1/f。
+ * 直线：f_n = sqrt(f0^2 ± 2 a n)（脉冲域，ISR 增量逼近）。
+ * S/正弦：开相时预计算频率表；ISR 只按脉冲序号查表（匀速相不改频）。
+ * acc_n/dec_n 与建表同一套离散逐拍仿真。
*
* 术语（全模块统一）：
* acc_n / dec_n / const_n — 加速/减速/匀速相各需的脉冲个数（规划预算，非 ms）
@@ -52,7 +52,7 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
/**
* 规划一整段脉冲域三相预算与实际峰值
- * 脉冲不够爬到 f_tgt 时 FitPeak 降峰（调用方可能写故障 0x02）
+ * 脉冲不够爬到 f_tgt 时先 const_n=0，再 FitPeak 降峰（调用方可能写故障 0x02）
*/
void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
uint32_t total_pulses,
@@ -70,8 +70,8 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
/**
* 单相逐脉冲运行态（ISR 热路径，每来一个 UPDATE 调 Step 一次）
- * 直线：±1Hz 逼近 sqrt(f0²±2an)
- * S/正弦：累计 t_us，按时间剖面取频；每拍 t += 1e6/f
+ * 直线：±1Hz 逼近 sqrt(f0^2±2an)
+ * S/正弦：查预计算频率表（开相 BeginAcc/BeginDec 时填好）
*/
typedef struct {
uint32_t f; /* 当前输出频率 Hz */
@@ -80,19 +80,21 @@ typedef struct {
uint32_t a; /* 本相加速度 Hz/s */
uint32_t n; /* 本相已步进脉冲数 */
uint32_t n_total; /* 本相总步进预算（= plan 里 acc_n 或 dec_n） */
- uint32_t t_us; /* S/正弦：本相已累计时间 us */
- uint32_t tj_us; /* S：jerk 段时长 us；正弦未用 */
- uint32_t ta_us; /* S：恒加速段时长 us；正弦未用 */
- uint32_t tramp_us; /* S/正弦：本相斜坡总时长 us */
- uint32_t phase_q24; /* 保留 */
- uint32_t step_q24;
- uint32_t step_rem;
- uint32_t rem_acc;
+ uint32_t t_us; /* 建表用：累计时间 us */
+ uint32_t tj_us; /* 建表用：S jerk 段 us */
+ uint32_t ta_us; /* 建表用：S 恒加速段 us */
+ uint32_t tramp_us; /* 建表用：斜坡总时长 us */
+ uint32_t tbl_stride; /* 查表：每 stride 个脉冲对应 1 个表项 */
+ uint32_t tbl_len; /* 表有效长度 */
uint8_t rising; /* 1=频率升高，0=降低 */
uint8_t active; /* 1=本相仍在步进；0=已到 f1 */
+ uint8_t use_tbl; /* 1=S/正弦走查表 */
PlsrAccelMode_e mode;
} PlsrAccelPulseRt_t;
+/** PlanSeg 后调用：预建 S/正弦加减速频率表（勿在 ISR） */
+void PlsrAccelPulseRtPrebuild(const PlsrAccelPlan_t *plan);
+
void PlsrAccelPulseRtBeginAcc(PlsrAccelPulseRt_t *pulse_rt,
const PlsrAccelPlan_t *plan);
void PlsrAccelPulseRtBeginDec(PlsrAccelPulseRt_t *pulse_rt,
diff --git a/plsr/pulse_driver/plsr_pulse_driver.c b/plsr/pulse_driver/plsr_pulse_driver.c
index 1ab9359..8bdb98b 100644
--- a/plsr/pulse_driver/plsr_pulse_driver.c
+++ b/plsr/pulse_driver/plsr_pulse_driver.c
@@ -56,6 +56,14 @@ static volatile uint8_t s_req_pending;
*/
static uint8_t s_regs_prepared;
static volatile uint8_t s_seg_end_fall_armed; /* 1=末拍 CC1 下降沿后强制拉低 */
+/*
+ * 冷启动首拍保护：Commit 后第一次 SetFreqIsr 必须先钉住工作 ARR=起跳频，
+ * 再把下一拍写入影子。否则 ARPE 未生效时 ISR 会直接改写工作寄存器，
+ * 示波器上首周期频率变成「第 2 个脉冲」的频率（起跳越高越明显）。
+ */
+static volatile uint8_t s_protect_start_period;
+static uint32_t s_protect_arr;
+static uint32_t s_protect_ccr;
/*============================================================================*/
/* 路选择 / 时钟 / GPIO */
@@ -271,6 +279,9 @@ void PlsrPulseDriverInit(void)
s_locked_psc = 0U;
s_req_pending = 0U;
s_req_freq = 0U;
+ s_protect_start_period = 0U;
+ s_protect_arr = 0U;
+ s_protect_ccr = 0U;
s_active_htim = &htim10;
PlsrPulseDriverDirGpioInit();
/* 三路都 Init + 空闲拉低，计数器关闭；出脉冲时只 Start 选中那路 */
@@ -583,22 +594,26 @@ static void PlsrPulseDriverLoadRegs(uint32_t freq_hz)
}
/**
- * @brief 开沿：先 Forced Active 出首上升沿，再切 PWM1 连续发脉冲
+ * @brief 开沿：直写工作 ARR 后立刻打开预装载并同步影子，再使能计数
*
- * @note WAIT/换向后若首拍 CCR 工作值为 0，PWM1 会整拍低电平，示波器上就是
- * 「延时 + 本段第一脉冲周期」。Forced Active 与 CNT/CCR 无关，首沿必须出现。
- * Commit 内不再 UG，避免预装载影子把 CCR 又打回旧值/0。
+ * @note 必须先 ARPE+影子=起跳频，再 ENABLE。若先开表再开 ARPE，
+ * 高频下首拍 UPDATE 的 ISR 可能在 ARPE 仍关时改 ARR，工作寄存器
+ * 被直接写成 f(1)，示波器上首周期就像「x=2」而不是起跳频。
+ * CNT=ARR：下一节拍产生 UPDATE，建立 PWM1 首上升沿并进入脉冲计数。
*/
static void PlsrPulseDriverCommitOutput(void)
{
TIM_HandleTypeDef *htim = s_active_htim;
uint32_t ccmr;
+ uint32_t ccr;
if ((htim == (TIM_HandleTypeDef *)0) || (htim->Instance == (TIM_TypeDef *)0))
{
return;
}
+ ccr = (s_last_ccr < 1U) ? 1U : s_last_ccr;
+
__HAL_TIM_DISABLE(htim);
__HAL_TIM_DISABLE_IT(htim, TIM_IT_UPDATE);
@@ -606,32 +621,33 @@ static void PlsrPulseDriverCommitOutput(void)
htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_ARPE);
htim->Instance->CCMR1 &= (uint32_t)(~TIM_CCMR1_OC1PE);
htim->Instance->ARR = s_last_arr;
- htim->Instance->CCR1 = (s_last_ccr < 1U) ? 1U : s_last_ccr;
- htim->Instance->CNT = 0U;
+ htim->Instance->CCR1 = ccr;
- /*
- * 唯一的脉冲启动入口。通道必须在 CEN 前使能；TIM10/11/13 在
- * 运行中由 CC1E=0 -> 1 不保证重新建立当前 PWM1 电平，会保持无输出。
- * CNT 从 ARR 开始，下一计数节拍产生真实 UPDATE：该 UPDATE 同时建立
- * PWM1 首上升沿并进入现有脉冲 ISR 计数，边沿与 s_done 保持一一对应。
- */
ccmr = htim->Instance->CCMR1;
ccmr &= (uint32_t)(~TIM_CCMR1_OC1M);
ccmr |= TIM_OCMODE_PWM1;
htim->Instance->CCMR1 = ccmr;
htim->Instance->CCER |= TIM_CCER_CC1E;
+ /*
+ * 先打开预装载并把影子钉成同一频率，再 ENABLE。
+ * 否则首拍 ISR 的 SetFreqIsr 可能落在 ARPE=0 窗口，直接改写工作 ARR。
+ */
+ htim->Instance->CR1 |= TIM_CR1_ARPE;
+ htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE;
+ htim->Instance->ARR = s_last_arr;
+ htim->Instance->CCR1 = ccr;
+
htim->Instance->CNT = s_last_arr;
__HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE);
__HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE);
__HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE);
__HAL_TIM_ENABLE(htim);
- /* 运行期改频仍走预装载 */
- htim->Instance->CR1 |= TIM_CR1_ARPE;
- htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE;
- htim->Instance->ARR = s_last_arr;
- htim->Instance->CCR1 = (s_last_ccr < 1U) ? 1U : s_last_ccr;
+ /* 记录起跳工作寄存器，供首拍 ISR 保护当前周期 */
+ s_protect_arr = s_last_arr;
+ s_protect_ccr = ccr;
+ s_protect_start_period = 1U;
s_regs_prepared = 0U;
}
@@ -737,23 +753,53 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz)
uint32_t clk_hz;
TIM_HandleTypeDef *htim;
- /* 匀速热路径：频率未变 → 不算量化、不写寄存器 */
- if (freq_hz == s_last_freq)
+ htim = s_active_htim;
+ if (htim == (TIM_HandleTypeDef *)0)
{
+ if (freq_hz == 0U)
+ {
+ s_req_freq = 0U;
+ s_req_pending = 1U;
+ return;
+ }
+ s_req_freq = freq_hz;
+ s_req_pending = 1U;
return;
}
- if (freq_hz == 0U)
+ /*
+ * 冷启动首拍：无论下一频是否变化，先把工作 ARR 钉回起跳频，再开预装载。
+ * 必须在「freq==last 早退」之前做，否则首拍保护标志清不掉。
+ */
+ if (s_protect_start_period != 0U)
+ {
+ s_protect_start_period = 0U;
+ htim->Instance->CR1 &= (uint32_t)(~TIM_CR1_ARPE);
+ htim->Instance->CCMR1 &= (uint32_t)(~TIM_CCMR1_OC1PE);
+ htim->Instance->ARR = s_protect_arr;
+ htim->Instance->CCR1 = (s_protect_ccr < 1U) ? 1U : s_protect_ccr;
+ htim->Instance->CR1 |= TIM_CR1_ARPE;
+ htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE;
+ /* 影子也先钉成起跳，下面再写入真正的下一拍频率 */
+ htim->Instance->ARR = s_protect_arr;
+ htim->Instance->CCR1 = (s_protect_ccr < 1U) ? 1U : s_protect_ccr;
+ }
+ else
+ {
+ /* 常规路径：确保预装载开启，禁止直写工作寄存器 */
+ htim->Instance->CR1 |= TIM_CR1_ARPE;
+ htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE;
+ }
+
+ /* 匀速热路径：频率未变 → 不算量化、不写寄存器 */
+ if (freq_hz == s_last_freq)
{
- s_req_freq = 0U;
- s_req_pending = 1U;
return;
}
- htim = s_active_htim;
- if (htim == (TIM_HandleTypeDef *)0)
+ if (freq_hz == 0U)
{
- s_req_freq = freq_hz;
+ s_req_freq = 0U;
s_req_pending = 1U;
return;
}
@@ -783,12 +829,19 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz)
return;
}
+ if (ccr < 1U)
+ {
+ ccr = 1U;
+ }
+
+ /* 此时 ARPE 已开：只改影子，下一 UPDATE 才切换，当前起跳周期不受影响 */
__HAL_TIM_SET_AUTORELOAD(htim, arr);
__HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr);
s_last_freq = freq_hz;
s_last_psc = psc;
s_last_arr = arr;
+ s_last_ccr = ccr;
}
/** @brief 丢弃挂起（见 plsr_pulse_driver.h） */
@@ -797,6 +850,12 @@ void PlsrPulseDriverClearPending(void)
s_req_pending = 0U;
}
+/** @brief 取消冷启动首拍保护（见 plsr_pulse_driver.h） */
+void PlsrPulseDriverClearStartPeriodProtect(void)
+{
+ s_protect_start_period = 0U;
+}
+
/** @brief 消费挂起改频（见 plsr_pulse_driver.h） */
void PlsrPulseDriverApplyPending(void)
{
@@ -827,6 +886,7 @@ void PlsrPulseDriverStop(void)
{
s_req_pending = 0U;
s_regs_prepared = 0U;
+ s_protect_start_period = 0U;
PlsrPulseDriverStopHtim(&htim10);
PlsrPulseDriverStopHtim(&htim11);
PlsrPulseDriverStopHtim(&htim13);
diff --git a/plsr/pulse_driver/plsr_pulse_driver.h b/plsr/pulse_driver/plsr_pulse_driver.h
index 6324ee3..8c30da3 100644
--- a/plsr/pulse_driver/plsr_pulse_driver.h
+++ b/plsr/pulse_driver/plsr_pulse_driver.h
@@ -94,6 +94,11 @@ void PlsrPulseDriverApplyPending(void);
/** @brief 丢弃未生效的改频请求（进匀速锁定时用） */
void PlsrPulseDriverClearPending(void);
+/**
+ * @brief 取消冷启动首拍 ARR 保护（进入匀速、无首拍改频时调用）
+ */
+void PlsrPulseDriverClearStartPeriodProtect(void);
+
/**
* @brief 停止三路脉冲 TIM，空闲 Forced inactive 拉低
* @note 不清方向脚；不清 PSC 锁（由上层 Unlock）
diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c
index d408a80..5d2174f 100644
--- a/plsr/run_control/plsr_run_control.c
+++ b/plsr/run_control/plsr_run_control.c
@@ -1009,6 +1009,7 @@ static void PlsrRunControlEnterConstHold(uint8_t do_start)
s_phase = PH_CONST;
PlsrAccelPulseRtBeginConst(&s_pulse_rt, &s_accel_plan);
PlsrPulseDriverClearPending();
+ PlsrPulseDriverClearStartPeriodProtect();
PlsrRunControlApplyOutFreq(f_hold, do_start);
}
@@ -1023,6 +1024,7 @@ static void PlsrRunControlEnterConstHoldIsr(void)
s_phase = PH_CONST;
PlsrAccelPulseRtBeginConst(&s_pulse_rt, &s_accel_plan);
PlsrPulseDriverClearPending();
+ PlsrPulseDriverClearStartPeriodProtect();
PlsrRunControlApplyOutFreqIsr(f_hold);
}
@@ -1161,12 +1163,15 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
s_plan_dec_n = s_accel_plan.dec_n;
s_decel_budget = 0U;
+ /* S/正弦：在任务上下文预建频率表，ISR 只查表 */
+ PlsrAccelPulseRtPrebuild(&s_accel_plan);
+
/* 运行层使用规划后的实际起始频率。 */
f_start = s_accel_plan.f_cur;
/*
- * 脉冲域：运行时按已完成脉冲数 FreqAtPulse 取频。
- * 禁止再用时间轴二次拟合，否则规划与运行不一致。
+ * 脉冲域：ISR 用 PulseRtStep（直线现场算 / S·正弦查预建表）。
+ * FreqAtPulse 仅任务侧 RefreshProfile 对齐用，勿再做时间轴二次拟合。
*/
/* 本段频率范围锁 PSC，升降只改 ARR；跨度过大则 LockPscRange 自动不锁 */
@@ -1829,9 +1834,10 @@ void PlsrRunControlOnPulseIsr(void)
if (s_phase == PH_CONST)
{
/*
- * 匀速热路径：频率未变 → 不写寄存器。
- * ARPE 超前：remain<=dec_n+1 进减速，使末拍 ARR 为 f_end。
+ * 匀速：不改频、不写 ARR（热路径几乎空转）。
+ * 仅在 remain 进入减速窗口时切相并查表改频。
*/
+ PlsrPulseDriverClearStartPeriodProtect();
if (PlsrRunControlShouldEnterDecel(remain) != 0U)
{
PlsrRunControlEnterDecel(s_accel_plan.f_tgt,