diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index d362c1f..10cad28 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -1,12 +1,8 @@ /** * @file plsr_accel_curve.c - * @brief 预估相脉冲；相内时间域直线/S/正弦 + * @brief 脉冲域规划/取频：f^2 = f0^2 + 2*a*n（直线）；S/正弦按脉冲进度 shape * - * 脉冲预算不够到目标频时：降低峰值，保留加减速时间（勿把 T 压成 0， - * 否则示波器上变成方波，三种模式完全无法区分）。 - - * @note：目前脉冲输出在低频时和高频时均有毛刺，需要优化 - * 已解决，原因是逻辑分析仪采样率不够，导致采样点减少，波形有毛刺。 + * 脉冲预算不够到目标频时：降低峰值（三角，无匀速）。 */ #include "plsr_accel_curve.h" #include @@ -26,40 +22,42 @@ static uint32_t PlsrAccelCurveAbsDiff(uint32_t a, uint32_t b) return (a >= b) ? (a - b) : (b - a); } -/** - * 整数平方根（牛顿法）。 - */ -static uint32_t PlsrAccelCurveIsqrt(uint32_t val) +/** 64 位整数平方根（牛顿法），支持 f^2 + 2an */ +static uint32_t PlsrAccelCurveIsqrt64(uint64_t val) { - uint32_t x; - uint32_t x2; + uint64_t x; + uint64_t x2; uint32_t i; - if (val <= 1U) + if (val <= 1ULL) { - return val; + return (uint32_t)val; } x = val; - if (x > 10000U) + if (x > 100000ULL) { - x = 10000U; + x = 100000ULL; } - for (i = 0U; i < 30U; i++) + for (i = 0U; i < 40U; i++) { - x2 = (x + val / x) / 2U; + x2 = (x + val / x) / 2ULL; if (x2 >= x) { break; } x = x2; } - return x; + return (uint32_t)x; +} + +static uint32_t PlsrAccelCurveIsqrt(uint32_t val) +{ + return PlsrAccelCurveIsqrt64((uint64_t)val); } /** - * 起跳频率：f_jump = sqrt(a_Hz_per_s)。 - * 加速度 a = df * 1000 / t_ms（Hz/s），df = |f_to - f_from|。 - * 当 t_ms=0 或 df=0 时返回 1。 + * 起跳频率：f = sqrt(f_from^2 + 2a)，a = df*1000/t_ms。 + * f_from=0 → sqrt(2a)；超过 f_to 钳到 f_to。 */ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, uint32_t t_ms) @@ -67,17 +65,21 @@ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, uint32_t df; uint32_t a_hz_s; uint32_t f; + uint64_t val; - if (t_ms == 0U) + if (f_to < 1U) { return 1U; } + if (t_ms == 0U) + { + return f_to; + } df = PlsrAccelCurveAbsDiff(f_from, f_to); if (df == 0U) { - return 1U; + return f_to; } - /* a = df / (t_ms / 1000) = df * 1000 / t_ms，可能很大 */ if (df <= 4294967U) { a_hz_s = (df * 1000U) / t_ms; @@ -88,13 +90,21 @@ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, } if (a_hz_s == 0U) { - return 1U; + 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); } - f = PlsrAccelCurveIsqrt(a_hz_s); if (f < 1U) { f = 1U; } + if (f > f_to) + { + f = f_to; + } return f; } @@ -163,44 +173,6 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from, return t; } -/** - * 从 0 爬到 f_tgt（或从 f_tgt 落到 0）时的起跳/落地频率。 - * start/end 参考速都为 0：a = f_tgt*1000/t_ms，f=sqrt(a)； - * 否则走 JumpFreq(0, f_tgt, t_ms)。 - */ -static uint32_t PlsrAccelCurveJumpFromZero(uint32_t f_tgt, - uint32_t t_ms, - uint32_t start_spd_ref, - uint32_t end_spd_ref) -{ - uint32_t f; - - if ((f_tgt < 1U) || (t_ms < 1U)) - { - return 1U; - } - - if ((start_spd_ref == 0U) && (end_spd_ref == 0U)) - { - uint64_t a_hz_s = ((uint64_t)f_tgt * 1000ULL) / (uint64_t)t_ms; - f = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s); - } - else - { - f = PlsrAccelCurveJumpFreq(0U, f_tgt, t_ms); - } - - if (f < 1U) - { - f = 1U; - } - if (f > f_tgt) - { - f = f_tgt; - } - return f; -} - uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, uint32_t f_tgt, uint32_t default_spd, @@ -213,20 +185,16 @@ uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, f_cfg = PlsrAccelCurveClampFreq(f_cfg); f_tgt = PlsrAccelCurveClampFreq(f_tgt); - if (f_cfg >= 1U) - { - return f_cfg; - } if (f_tgt < 1U) { return 1U; } - t_acc = PlsrAccelCurveRampTimeMs(0U, f_tgt, default_spd, + /* 起速 0 或非 0：都用 JumpFreq = sqrt(f_cfg^2 + 2a) */ + t_acc = PlsrAccelCurveRampTimeMs(f_cfg, f_tgt, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); - return PlsrAccelCurveJumpFromZero(f_tgt, t_acc, - start_spd_ref, end_spd_ref); + return PlsrAccelCurveJumpFreq(f_cfg, f_tgt, t_acc); } uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, @@ -241,20 +209,26 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, f_cfg = PlsrAccelCurveClampFreq(f_cfg); f_tgt = PlsrAccelCurveClampFreq(f_tgt); - if (f_cfg >= 1U) - { - return f_cfg; - } if (f_tgt < 1U) { return 1U; } - t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd, + /* + * 止速 0 或 1：按落地频率估计，避免 1Hz 拖尾。 + */ + if (f_cfg <= 1U) + { + t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd, + start_spd_ref, end_spd_ref, + accel_ms, decel_ms); + return PlsrAccelCurveJumpFreq(0U, f_tgt, t_dec); + } + + t_dec = PlsrAccelCurveRampTimeMs(f_tgt, f_cfg, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); - return PlsrAccelCurveJumpFromZero(f_tgt, t_dec, - start_spd_ref, end_spd_ref); + return PlsrAccelCurveJumpFreq(f_cfg, f_tgt, t_dec); } /** 直线：进度千分比 → 形状千分比 */ @@ -704,27 +678,126 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, } /** - * 二分峰值频率（Hz），使 1ms 模型下 acc+dec 脉冲 <= total，且尽量接近原目标频。 - * 到不了目标频时这就是三角：峰值降低，匀速脉冲必须为 0。 + * 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（阶跃，无斜坡脉冲）。 + */ +static uint32_t PlsrAccelCurvePulsesForRamp(uint32_t f0, uint32_t f1, + uint32_t a_hz_s) +{ + uint64_t f0s; + uint64_t f1s; + uint64_t df2; + uint64_t two_a; + uint64_t n; + + if (f0 == f1) + { + return 0U; + } + if (a_hz_s == 0U) + { + return 0U; + } + + f0s = (uint64_t)f0 * (uint64_t)f0; + f1s = (uint64_t)f1 * (uint64_t)f1; + df2 = (f1s > f0s) ? (f1s - f0s) : (f0s - f1s); + two_a = 2ULL * (uint64_t)a_hz_s; + n = (df2 + two_a - 1ULL) / two_a; + if (n == 0ULL) + { + n = 1ULL; + } + if (n > 0xFFFFFFFFULL) + { + return 0xFFFFFFFFUL; + } + return (uint32_t)n; +} + +/** 直线：f = sqrt(f0^2 + 2*a*n)，升到/降到不超过 limit 方向 */ +static uint32_t PlsrAccelCurveFreqKinematic(uint32_t f0, uint32_t a_hz_s, + uint32_t n, uint8_t rising, + uint32_t limit) +{ + uint64_t val; + uint32_t f; + + if (n == 0U) + { + f = f0; + } + else if (a_hz_s == 0U) + { + f = limit; + } + else + { + val = (uint64_t)f0 * (uint64_t)f0; + if (rising != 0U) + { + val += 2ULL * (uint64_t)a_hz_s * (uint64_t)n; + f = PlsrAccelCurveIsqrt64(val); + if (f > limit) + { + f = limit; + } + } + else + { + uint64_t drop = 2ULL * (uint64_t)a_hz_s * (uint64_t)n; + if (drop >= val) + { + f = limit; + } + else + { + f = PlsrAccelCurveIsqrt64(val - drop); + if (f < limit) + { + f = limit; + } + } + } + } + if (f < 1U) + { + f = 1U; + } + return PlsrAccelCurveClampFreq(f); +} + +/** + * 二分峰值：使 acc+dec 脉冲 <= total（脉冲闭合公式）。 */ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, uint32_t f_cur, uint32_t f_want, uint32_t f_end, - uint32_t default_spd, - uint32_t start_spd_ref, - uint32_t end_spd_ref, - uint32_t accel_ms, - uint32_t decel_ms, - PlsrAccelMode_e mode) + uint32_t a_acc, + uint32_t a_dec) { uint32_t lo = 0U; uint32_t hi; uint32_t mid; uint32_t best = 0U; uint32_t peak; - uint32_t t_a; - uint32_t t_d; uint32_t n_a; uint32_t n_d; uint32_t span; @@ -750,14 +823,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, { peak = f_cur - mid; } - t_a = PlsrAccelCurveRampTimeMs(f_cur, peak, default_spd, - start_spd_ref, end_spd_ref, - accel_ms, decel_ms); - t_d = PlsrAccelCurveRampTimeMs(peak, f_end, default_spd, - start_spd_ref, end_spd_ref, - accel_ms, decel_ms); - n_a = PlsrAccelCurveEstimatePulses(f_cur, peak, t_a, mode); - n_d = PlsrAccelCurveEstimatePulses(peak, f_end, t_d, mode); + n_a = PlsrAccelCurvePulsesForRamp(f_cur, peak, a_acc); + n_d = PlsrAccelCurvePulsesForRamp(peak, f_end, a_dec); if ((n_a + n_d) <= total_pulses) { @@ -806,6 +873,8 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, uint32_t acc_n; uint32_t dec_n; uint32_t f_peak; + uint32_t a_acc; + uint32_t a_dec; PlsrAccelMode_e m; if (plan == (PlsrAccelPlan_t *)0) @@ -819,10 +888,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode; - /* - * 端点须由调用方解析完毕。若仍传入 0，用同一套 Resolve 兜底， - * 避免把 0 当真实频率规划出错误斜坡；正常路径不应走到这里。 - */ if (f_cur < 1U) { f_cur = PlsrAccelCurveResolveStartHz(0U, f_tgt, default_spd, @@ -836,6 +901,26 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, accel_ms, decel_ms); } + a_acc = PlsrAccelCurveAccelHzS(default_spd, accel_ms); + a_dec = PlsrAccelCurveAccelHzS(default_spd, decel_ms); + /* 升降方向用对应斜率；无默认速度时用频差/时间回退 */ + if ((a_acc == 0U) && (accel_ms > 0U)) + { + uint32_t df = PlsrAccelCurveAbsDiff(f_cur, f_tgt); + if (df > 0U) + { + a_acc = (df * 1000UL) / accel_ms; + } + } + if ((a_dec == 0U) && (decel_ms > 0U)) + { + uint32_t df = PlsrAccelCurveAbsDiff(f_tgt, f_end); + if (df > 0U) + { + a_dec = (df * 1000UL) / decel_ms; + } + } + f_peak = f_tgt; t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, start_spd_ref, end_spd_ref, @@ -847,13 +932,14 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, plan->f_cur = f_cur; plan->f_end = f_end; plan->mode = m; - acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m); - dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m); + plan->a_acc = a_acc; + plan->a_dec = a_dec; + + acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc); + dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec); if (total_pulses == 0U) { - acc_n = 0U; - dec_n = 0U; plan->const_n = 0U; plan->t_acc_ms = 0U; plan->t_dec_ms = 0U; @@ -864,11 +950,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, return; } - /* - * f_end == f_tgt：本段无独立段末终点（仅加速到目标）。 - * 脉冲不够时不 FitPeak、不减速；梯形/三角只由 - * (f_cur,f_tgt,f_end,斜率,mode,总脉冲) 决定，与 wait 无关。 - */ if ((f_end == f_tgt) && (acc_n > total_pulses)) { plan->const_n = 0U; @@ -883,21 +964,16 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, if ((acc_n + dec_n) > total_pulses) { - /* - * 脉冲不够到目标频：降峰值（斜率不变），三角，禁止匀速。 - */ - f_peak = PlsrAccelCurveFitPeak(total_pulses, - f_cur, f_tgt, f_end, - default_spd, start_spd_ref, end_spd_ref, - accel_ms, decel_ms, m); + f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end, + a_acc, a_dec); t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); - acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m); - dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m); + acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc); + dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec); if (acc_n > total_pulses) { acc_n = total_pulses; @@ -919,13 +995,9 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, } else { - /* 够到目标频：匀速脉冲 = 总数 − 加速 − 减速 */ plan->const_n = total_pulses - acc_n - dec_n; } - /* - * 需要减速 (f_peak > f_end) 时禁止 dec_n=0。 - */ if ((f_peak > f_end) && (total_pulses > 1U) && (dec_n == 0U)) { dec_n = 1U; @@ -955,70 +1027,95 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, plan->t_dec_ms = t_dec; plan->acc_n = acc_n; plan->dec_n = dec_n; - /* - * 进入减速时刻：加速时间 + 匀速时间。 - * 三角无匀速 → t_decel_start = t_acc，减速从峰值立刻开始。 - * 止速脉冲数 = dec_n，由 1ms 模型按 (f_peak→f_end, t_dec) 估出。 - */ plan->t_decel_start_ms = t_acc + PlsrAccelCurveConstTimeMs(plan->const_n, f_peak); } -uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, - uint32_t seg_elapsed_ms) +uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, + uint32_t pulse_done) { - uint32_t t; - uint32_t T_acc_us; - uint32_t t_dec; + uint32_t const_end; + uint32_t n; + uint32_t f; uint32_t ratio; + uint8_t rising; if (plan == (PlsrAccelPlan_t *)0) { return 0U; } - if ((plan->t_acc_ms > 0U) && (seg_elapsed_ms < plan->t_acc_ms) && + const_end = plan->acc_n + plan->const_n; + + /* 加速相：0 .. acc_n-1 完成后仍在加速；pulse_done 为已完成数 */ + if ((plan->acc_n > 0U) && (pulse_done < plan->acc_n) && (plan->f_cur != plan->f_tgt)) { - T_acc_us = plan->t_acc_ms * 1000UL; + rising = (plan->f_tgt >= plan->f_cur) ? 1U : 0U; + if (plan->mode == PLSR_ACCEL_LINEAR) { - uint32_t f_acc = PlsrAccelCurveFreqOnRampUs(plan->f_cur, plan->f_tgt, - seg_elapsed_ms * 1000UL, - T_acc_us, plan->mode); - if ((f_acc < 1U) && (plan->f_tgt >= 1U)) - { - f_acc = 1U; - } - return PlsrAccelCurveClampFreq(f_acc); + f = PlsrAccelCurveFreqKinematic(plan->f_cur, plan->a_acc, + pulse_done, rising, plan->f_tgt); + } + else + { + ratio = PlsrAccelCurveShape(pulse_done, plan->acc_n, plan->mode); + f = PlsrAccelCurveLerp(plan->f_cur, plan->f_tgt, ratio); } + if ((f < 1U) && (plan->f_tgt >= 1U)) + { + f = 1U; + } + return PlsrAccelCurveClampFreq(f); } - if (seg_elapsed_ms < plan->t_decel_start_ms) + /* 匀速 */ + if (pulse_done < const_end) { return PlsrAccelCurveClampFreq(plan->f_tgt); } - if ((plan->t_dec_ms == 0U) || (plan->f_tgt == plan->f_end)) + /* 减速 / 无减速 */ + if ((plan->dec_n == 0U) || (plan->f_tgt == plan->f_end)) { return PlsrAccelCurveClampFreq(plan->f_end); } - t_dec = seg_elapsed_ms - plan->t_decel_start_ms; - if (t_dec >= plan->t_dec_ms) + n = pulse_done - const_end; + if (n >= plan->dec_n) { return PlsrAccelCurveClampFreq(plan->f_end); } + rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U; if (plan->mode == PLSR_ACCEL_LINEAR) { - return PlsrAccelCurveClampFreq( - PlsrAccelCurveFreqOnRampUs(plan->f_tgt, plan->f_end, - t_dec * 1000UL, - plan->t_dec_ms * 1000UL, - plan->mode)); + f = PlsrAccelCurveFreqKinematic(plan->f_tgt, plan->a_dec, + n, rising, plan->f_end); + } + else + { + ratio = PlsrAccelCurveShape(n, plan->dec_n, plan->mode); + f = PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio); + } + if (f < 1U) + { + f = 1U; } + return PlsrAccelCurveClampFreq(f); +} + +/** 兼容旧预览：用时间估算映射到脉冲再取频 */ +uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, + uint32_t seg_elapsed_ms) +{ + uint32_t approx_n; + uint32_t f_ref; - t = plan->t_dec_ms; - ratio = PlsrAccelCurveShape(t_dec, t, plan->mode); - return PlsrAccelCurveClampFreq( - PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio)); + if (plan == (PlsrAccelPlan_t *)0) + { + return 0U; + } + f_ref = (plan->f_tgt >= 1U) ? plan->f_tgt : 1U; + approx_n = (uint32_t)(((uint64_t)seg_elapsed_ms * (uint64_t)f_ref) / 1000ULL); + return PlsrAccelCurveFreqAtPulse(plan, approx_n); } diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index e839166..7b1d52d 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -1,12 +1,11 @@ /** * @file plsr_accel_curve.h - * @brief 预估加/匀/减脉冲；相内按时间走直线/S/正弦 + * @brief 脉冲域加/匀/减规划与取频（v^2 - v0^2 = 2ax） * - * 1ms 改频：脉冲数按 t=0..T-1 实际取频累加；到不了目标频则降峰值走三角（无匀速）。 + * 加速度 a = 默认速度*1000/加减速时间（Hz/s）。 + * 脉冲序号 n 上：f(n) = sqrt(f0^2 + 2*a*n)；初速为 0 即 f0=0。 * 匀速仅当加速+减速脉冲之和 < 总脉冲（真正到达目标频）时出现。 - * 斜率按公共参数：K_acc=默认速度/加速时间，K_dec=默认速度/减速时间（减 0，不用起/止速）。 - * 小频差时 T 可能 <1ms，无可见斜坡属正常。 - * 开段端点由运行层 ResolveStart/EndHz 解析后再传入 Plan（Plan 不解释策略 0）。 + * 开段端点由运行层 ResolveStart/EndHz 解析后再传入 Plan。 */ #ifndef PLSR_ACCEL_CURVE_H #define PLSR_ACCEL_CURVE_H @@ -21,9 +20,11 @@ typedef struct { uint32_t f_cur; uint32_t f_tgt; uint32_t f_end; - uint32_t t_acc_ms; + uint32_t a_acc; /* 加速 a（Hz/s） */ + uint32_t a_dec; /* 减速 a（Hz/s） */ + uint32_t t_acc_ms; /* 斜率时间估算（预览/兼容，运行不依赖） */ uint32_t t_dec_ms; - uint32_t t_decel_start_ms; /* 反向规划：段内时间轴上进入减速的时刻 */ + uint32_t t_decel_start_ms; /* 估算：进减速时刻（预览用） */ PlsrAccelMode_e mode; } PlsrAccelPlan_t; @@ -36,17 +37,14 @@ typedef enum { uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz); /** - * 起跳频率：f_jump = sqrt(加速度_Hz/s)。 - * 起速/止速为 0 时，第一个脉冲（或最后一个脉冲）应以此频率输出， - * 使脉冲周期恰好等于加速度产生该频率所需的时间。 + * 起跳/步进频率：f = sqrt(f_from^2 + 2a)，a = |f_to-f_from|*1000/t_ms。 + * f_from=0 时退化为 sqrt(2a)；超过 f_to 则钳到 f_to。 */ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, uint32_t t_ms); /** - * 配置起速 → 规划用真实起点频率。 - * f_cfg>=1：原样（钳位）；f_cfg==0：按斜率算起跳频率（从静止爬升）。 - * 由运行层在开段前调用；Plan 只吃已经解析好的端点。 + * 配置起速 → 规划用真实起点：一律 JumpFreq(f_cfg, f_tgt, t_acc)。 */ uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, uint32_t f_tgt, @@ -57,9 +55,8 @@ uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, uint32_t decel_ms); /** - * 配置止速 → 规划用真实终点频率。 - * f_cfg>=1：原样（钳位）；f_cfg==0：按斜率算落地频率（避免 1Hz 拖尾）。 - * 段末是否停表由运行层决定，与本函数无关。 + * 配置止速 → 规划用真实终点。 + * f_cfg<=1：按落地频率估计（避免 1Hz 拖尾）；否则 JumpFreq。 */ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, uint32_t f_tgt, @@ -69,43 +66,28 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, uint32_t accel_ms, uint32_t decel_ms); -/** - * 斜坡上按 mode 取频（µs 时间轴）。ISR 可用：O(1)。 - * f = f0+(f1-f0)*shape(t/T) - */ uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, uint32_t f1, uint32_t t_us, uint32_t T_us, PlsrAccelMode_e mode); -/** - * 与 TIM3 1ms 改频一致的斜坡取频（规划/运行共用）。 - */ uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0, uint32_t f1, uint32_t t_ms, uint32_t T_ms, PlsrAccelMode_e mode); -/** - * 斜坡第一拍频率（起速为 0 时用自洽点，避免按 1Hz 整秒吃光加速相）。 - * ISR 可用：O(1)。 - */ uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, uint32_t f1, uint32_t T_us, PlsrAccelMode_e mode); -/** - * 减速落地频率：f_end>=1 即止速；f_end==0 返回 0（停表，不用 1Hz 冒充）。 - */ uint32_t PlsrAccelCurveLandFreq(uint32_t f_end, uint32_t f_peak, uint32_t T_ms, PlsrAccelMode_e mode); -/** 拟合斜坡时间(ms)，使离散模型脉冲数尽量等于 max_pulses（任务上下文） */ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, uint32_t f_to, uint32_t max_pulses, @@ -125,8 +107,13 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, PlsrAccelMode_e mode); /** - * 1ms 改频：按段内绝对时间取频（相界由 t_decel_start_ms 决定，不用脉冲域）。 + * 按已完成脉冲数取频（任务上下文调用，勿进脉冲 ISR）。 + * pulse_done = 本段已发完的脉冲数（与 s_done 一致）。 */ +uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, + uint32_t pulse_done); + +/** @deprecated 时间轴取频；保留给旧预览，运行请用 FreqAtPulse */ uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, uint32_t seg_elapsed_ms); diff --git a/plsr/pulse_driver/plsr_pulse_driver.c b/plsr/pulse_driver/plsr_pulse_driver.c index e0ef627..cbc368b 100644 --- a/plsr/pulse_driver/plsr_pulse_driver.c +++ b/plsr/pulse_driver/plsr_pulse_driver.c @@ -556,6 +556,11 @@ void PlsrPulseDriverRequestFreq(uint32_t freq_hz) s_req_pending = 1U; } +void PlsrPulseDriverClearPending(void) +{ + s_req_pending = 0U; +} + void PlsrPulseDriverApplyPending(void) { uint32_t f; diff --git a/plsr/pulse_driver/plsr_pulse_driver.h b/plsr/pulse_driver/plsr_pulse_driver.h index b8b6805..d18964b 100644 --- a/plsr/pulse_driver/plsr_pulse_driver.h +++ b/plsr/pulse_driver/plsr_pulse_driver.h @@ -37,6 +37,8 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz); void PlsrPulseDriverRequestFreq(uint32_t freq_hz); /** 在更新中断里调用：应用挂起频率 */ void PlsrPulseDriverApplyPending(void); +/** 丢弃未生效的改频请求（进匀速锁定时用） */ +void PlsrPulseDriverClearPending(void); void PlsrPulseDriverStop(void); /** 当前拍结束后停止计数，避免多出一个上升沿 */ void PlsrPulseDriverArmOnePulseStop(void); diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index 2b154a5..cdcae30 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -12,7 +12,10 @@ * 3) 发送模式只影响本段 f_end（完成=止速；后续且不停表=下一段目标） * 4) PlanSeg / ChangeFreq 共用规划入口，便于后期动态改频 * - * 【运行节拍】TIM3 1ms：RefreshProfile 改频；脉冲 UPDATE ISR 只计 s_done。 + * 【运行节拍】脉冲 UPDATE：计 s_done；加/减速置 dirty，匀速不周期性改频。 + * 任务 TickMs：dirty 时 FreqAtPulse 改频。 + * TIM3 1ms：仅 ACT/WAIT 时基（不改频）。 + * 匀速“保持”≠禁止改频：ACT/EXT 打断、ChangeFreq、进减速、开下一段仍可 Start/SetFreq。 */ #include "plsr_run_control.h" #include "plsr_path_plan.h" @@ -58,11 +61,11 @@ static volatile uint8_t s_wait_expire_req; /* WAIT 时间到：ISR 只置位 */ static uint8_t s_wait_is_signal; /* RC_WAIT_COND：1=等 WAIT 沿，0=等时间 */ static PlsrAccelPlan_t s_accel_plan; -static RunPhase_e s_phase; -static uint32_t s_seg_elapsed_ms; /* 段内绝对时间（TIM3 1ms 累加） */ +static volatile RunPhase_e s_phase; /* ISR 读相，须 volatile */ static uint32_t s_approach_from; static uint32_t s_decel_from; static uint8_t s_follow_cont; +static volatile uint8_t s_profile_dirty; /* 脉冲推进后由任务改频 */ static uint32_t s_chain_freq; static uint8_t s_chain_valid; @@ -604,7 +607,7 @@ static uint32_t PlsrRunControlClampSpeed(uint32_t spd) return spd; } -/** 运行输出：0=停表；1ms 节拍直接 SetFreq / Start */ +/** 运行输出：0=停表；任务上下文 SetFreq / Start */ static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) { if (profile_freq == 0U) @@ -644,122 +647,138 @@ static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq) } } -static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) +static uint8_t PlsrRunControlHasDecel(void) { - PlsrCfg_t *cfg = PlsrParamGetCfg(); - uint32_t t_slope = 0U; - uint32_t t_fit; - uint32_t sum_f; - uint32_t df; - uint32_t den; - uint32_t ref_ms; - uint32_t f_end; - uint32_t old_dec_n; - uint32_t old_t_dec; + return ((s_accel_plan.dec_n > 0U) && + (s_accel_plan.f_tgt != s_accel_plan.f_end)) ? 1U : 0U; +} - old_dec_n = s_accel_plan.dec_n; - old_t_dec = s_accel_plan.t_dec_ms; +static uint32_t PlsrRunControlRemainPulses(void) +{ + if (s_target > s_done) + { + return (uint32_t)(s_target - s_done); + } + return 0U; +} +static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) +{ PlsrRunControlEnterPhase(PH_DECEL, from_hz); - f_end = s_accel_plan.f_end; + + s_accel_plan.acc_n = 0U; + s_accel_plan.const_n = 0U; + s_accel_plan.dec_n = remain_pulses; + s_accel_plan.f_tgt = from_hz; + s_decel_from = from_hz; + s_profile_dirty = 1U; if (remain_pulses == 0U) { - s_accel_plan.dec_n = 0U; s_accel_plan.t_dec_ms = 0U; return; } - s_accel_plan.dec_n = remain_pulses; - - if (from_hz == f_end) + if (from_hz == s_accel_plan.f_end) { s_accel_plan.t_dec_ms = 0U; return; } - /* - * 规划阶段已按斜率算好 t_dec：优先沿用，避免 ISR 内改歪形状。 - * 止速=0：始终用斜率时间；时间走完即停，不按剩余脉冲把 T 拉长 - * （拉长会在尾部堆 1Hz 空耗）。 - */ - if ((old_t_dec > 0U) && (old_dec_n > 0U) && - (remain_pulses == old_dec_n)) + /* 时间字段仅预览兼容；运行取频走 FreqAtPulse */ + if (s_accel_plan.t_dec_ms == 0U) { - s_accel_plan.t_dec_ms = old_t_dec; - return; + s_accel_plan.t_dec_ms = 1U; } +} - /* 参数斜率：K = 默认速度 / 加减速时间（减 0） */ - df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz); - if (f_end > from_hz) - { - ref_ms = s_run_accel_ms; - } - else - { - ref_ms = s_run_decel_ms; - } - den = cfg->default_speed; - if (den == 0U) +/** + * 进匀速：写一次 f_tgt，之后 RefreshProfile 不再周期性取频/改 ARR（防抖）。 + * 注意：这不是禁止改频——ACT/EXT 打断、ChangeFreq、进减速、开下一段 + * 都会离开 PH_CONST 或直接 Start/Stop，ApplyOutFreq 仍可改频。 + */ +static void PlsrRunControlEnterConstHold(uint8_t do_start) +{ + uint32_t f_hold = s_accel_plan.f_tgt; + + if (f_hold < 1U) { - den = df; + f_hold = 1U; } - if ((ref_ms > 0U) && (den > 0U)) + PlsrRunControlEnterPhase(PH_CONST, f_hold); + PlsrRunControlApplyOutFreq(f_hold, do_start); +} + +static void PlsrRunControlRefreshProfile(uint8_t do_start) +{ + uint32_t next; + uint32_t done_n; + uint32_t remain; + + done_n = 0U; + if (s_done > 0) { - t_slope = (df * ref_ms + den - 1UL) / den; - if (t_slope < 1U) - { - t_slope = 1U; - } + done_n = (uint32_t)s_done; } + remain = PlsrRunControlRemainPulses(); /* - * ISR 内禁止 FitRamp/EstimatePulses；用 O(1) 平均公式。 - * 脉冲被截断导致 remain 变化时才走这里。 + * 匀速 → 减速：只看剩余脉冲，不反复算频。 */ - sum_f = from_hz + f_end; - if (sum_f == 0U) - { - sum_f = 1U; - } - if (remain_pulses > 1U) - { - t_fit = (2UL * (remain_pulses - 1UL) * 1000UL + sum_f - 1UL) / sum_f; - } - else + if ((s_phase == PH_CONST) && + (PlsrRunControlHasDecel() != 0U) && + (remain <= s_accel_plan.dec_n)) { - t_fit = 1U; - } - if (t_fit < 1U) - { - t_fit = 1U; + PlsrRunControlEnterPhase(PH_DECEL, s_accel_plan.f_tgt); } - if ((t_slope > 0U) && (t_slope <= t_fit)) - { - s_accel_plan.t_dec_ms = t_slope; - } - else + /* + * 匀速保持：跳过周期性 FreqAtPulse（防顶频抖动）。 + * 仍允许：do_start / 停表后重开 写一次；打断切段走 BeginSeg/PlanSeg + * 会改 s_phase，不再走本分支。 + */ + if (s_phase == PH_CONST) { - s_accel_plan.t_dec_ms = t_fit; + if ((do_start != 0U) || (s_pwm_on == 0U)) + { + PlsrRunControlApplyOutFreq(s_accel_plan.f_tgt, do_start); + } + return; } -} -static void PlsrRunControlRefreshProfile(uint8_t do_start) -{ - uint32_t next; - - /* - * 1ms 改频：段内统一时间轴；t_decel_start_ms 由规划反向算出， - * 减速为 f_tgt→f_end 直线，自然落到止速，末拍不再强行对齐。 - */ - next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms); + /* 加速 / 减速：按脉冲域算下一频率 */ + next = PlsrAccelCurveFreqAtPulse(&s_accel_plan, done_n); if (next < 1U) { next = 1U; } + if (s_phase == PH_APPROACH) + { + /* + * 加速结束：脉冲用完或频率已到峰值 → 匀速保持（或直接进减速）。 + */ + if ((s_accel_plan.acc_n == 0U) || + (done_n >= s_accel_plan.acc_n) || + ((s_accel_plan.f_tgt >= s_accel_plan.f_cur) && + (next >= s_accel_plan.f_tgt)) || + ((s_accel_plan.f_tgt < s_accel_plan.f_cur) && + (next <= s_accel_plan.f_tgt))) + { + next = s_accel_plan.f_tgt; + if ((PlsrRunControlHasDecel() != 0U) && + (remain <= s_accel_plan.dec_n)) + { + PlsrRunControlEnterPhase(PH_DECEL, next); + } + else + { + PlsrRunControlEnterConstHold(do_start); + return; + } + } + } + if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U)) { PlsrRunControlApplyOutFreq(next, do_start); @@ -799,9 +818,8 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, f_start = s_accel_plan.f_cur; /* - * 1ms 改频模式：运行时按 TIM3 毫秒时间轴取频。 - * 禁止在这里用脉冲递推模型二次拟合 T（FitRamp），否则规划与运行模型不一致， - * 会出现中段拐折/扭曲、段末落不到设定 f_end。 + * 脉冲域：运行时按已完成脉冲数 FreqAtPulse 取频。 + * 禁止再用时间轴二次拟合，否则规划与运行不一致。 */ /* 本段频率范围锁 PSC，升降只改 ARR；跨度过大则 LockPscRange 自动不锁 */ @@ -864,9 +882,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, } /* - * 保持 AccelCurvePlan 算出的斜率时间，不再 FitRamp 拉长 T。 - * 脉冲数由 EstimatePulses 决定；运行时按相时间取频， - * 一拍周期盖住 T 时自然落到相终点。 + * 保持 AccelCurvePlan 算出的脉冲预算；运行时 FreqAtPulse 按 s_done 取频。 */ /* * 纯减速段：已在目标速（或更高）且整段预算不超过 dec_n。 @@ -887,7 +903,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, { PlsrRunControlEnterPhase(PH_APPROACH, f_start); } - s_seg_elapsed_ms = 0U; + s_profile_dirty = 1U; } static void PlsrRunControlFinishAll(void) @@ -902,6 +918,7 @@ static void PlsrRunControlFinishAll(void) s_follow_cont = 0U; s_chain_valid = 0U; s_chain_freq = 0U; + s_profile_dirty = 0U; s_act_armed = 0U; s_act_expire_req = 0U; s_act_cut_pending = 0U; @@ -996,7 +1013,7 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0) if (s_follow_cont != 0U) { PlsrPulseDriverClearOnePulseStop(); - s_seg_elapsed_ms = 0U; + s_profile_dirty = 0U; PlsrRunControlRefreshProfile(0U); s_state = RC_RUN; s_follow_cont = 0U; @@ -1027,9 +1044,9 @@ void PlsrRunControlInit(void) s_run_accel_ms = 0U; s_run_decel_ms = 0U; s_phase = PH_CONST; - s_seg_elapsed_ms = 0U; s_approach_from = 0U; s_decel_from = 0U; + s_profile_dirty = 0U; s_act_armed = 0U; s_act_expire_req = 0U; s_act_cut_pending = 0U; @@ -1083,6 +1100,7 @@ void PlsrRunControlStop(void) s_follow_cont = 0U; s_chain_valid = 0U; s_chain_freq = 0U; + s_profile_dirty = 0U; s_act_armed = 0U; s_act_expire_req = 0U; s_act_cut_pending = 0U; @@ -1116,7 +1134,7 @@ uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz) s_run_accel_ms, s_run_decel_ms); s_target = (int32_t)remain; s_done = 0; - s_seg_elapsed_ms = 0U; + s_profile_dirty = 0U; PlsrRunControlRefreshProfile(0U); return 1U; } @@ -1150,7 +1168,7 @@ void PlsrRunControlTickMs(void) if (PlsrRunControlOsTimeReached(s_dir_deadline) != 0U) { s_state = RC_RUN; - s_seg_elapsed_ms = 0U; + s_profile_dirty = 0U; PlsrRunControlRefreshProfile(1U); PlsrRunControlArmActExtOnPulseStart(); } @@ -1159,7 +1177,24 @@ void PlsrRunControlTickMs(void) if (s_state == RC_RUN) { - /* 波形改频 / ACT 计时在 TIM3 On1ms；此处只处理 EXT */ + /* + * 仅加/减速（或匀速刚到进减速点）才改频。 + * 匀速锁定期间 dirty 不会置位，避免顶频反复写 ARR。 + */ + if ((s_profile_dirty != 0U) && (s_act_cut_pending == 0U)) + { + s_profile_dirty = 0U; + PlsrRunControlRefreshProfile(0U); + } + else if ((s_phase == PH_CONST) && + (PlsrRunControlHasDecel() != 0U) && + (s_act_cut_pending == 0U) && + (PlsrRunControlRemainPulses() <= s_accel_plan.dec_n)) + { + /* 任务侧兜底：进减速 */ + PlsrRunControlRefreshProfile(0U); + } + seg = PlsrParamGetSeg(s_cur_seg); if ((seg->wait_type == PLSR_WAIT_EXT) || @@ -1199,8 +1234,8 @@ void PlsrRunControlTickMs(void) } /** - * TIM3 1ms：段内时间累加 + 按时间轴 RefreshProfile 改频。 - * ACT/WAIT 只置标志，切段在 TickMs（避免 ISR 里 PlanSeg 丢拍）。 + * TIM3 1ms：仅 ACT/WAIT 时基（不再改频）。 + * 切段在 TickMs（避免 ISR 里 PlanSeg 丢拍）。 */ void PlsrRunControlOn1ms(void) { @@ -1220,35 +1255,23 @@ void PlsrRunControlOn1ms(void) return; } - if (s_seg_elapsed_ms < 0xFFFFFFFFUL) - { - s_seg_elapsed_ms++; - } - if ((s_act_armed != 0U) && (PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) != 0U)) { /* * ACT 是运动控制条件，不参与曲线规划：加速、匀速、减速 - * 任一阶段到期都在当前脉冲 UPDATE 边界切段。这样不会在 - * 一个 PWM 周期中间改段，也不会把边界脉冲计到下一段。 + * 任一阶段到期都在当前脉冲 UPDATE 边界切段。 */ s_act_armed = 0U; s_act_cut_pending = 1U; } - - /* 保持 ACT 到期瞬间的频率，不让 1ms 曲线继续推进到下一个边界。 */ - if (s_act_cut_pending != 0U) - { - return; - } - - PlsrRunControlRefreshProfile(0U); } -/** 脉冲 UPDATE：只累计 s_done / s_acc_pulse，不改频 */ +/** 脉冲 UPDATE：计脉冲；仅加/减速置 dirty（匀速不改频） */ void PlsrRunControlOnPulseIsr(void) { + uint32_t remain; + if (s_state != RC_RUN) { return; @@ -1264,6 +1287,26 @@ void PlsrRunControlOnPulseIsr(void) s_acc_pulse--; } + /* + * 匀速：不置 dirty，定时器 ARR 保持不动。 + * 仅当剩余脉冲进入减速预算时置位，让任务切到 DECEL。 + */ + if (s_phase == PH_CONST) + { + if (PlsrRunControlHasDecel() != 0U) + { + remain = PlsrRunControlRemainPulses(); + if (remain <= s_accel_plan.dec_n) + { + s_profile_dirty = 1U; + } + } + } + else + { + s_profile_dirty = 1U; + } + /* * ACT 到期切段只在脉冲边界执行，避免半个 PWM 周期改段。 * 同向衔接：不停表，任务里用当前频率规划下一段（丢掉本段剩余）。 diff --git a/plsr/run_control/plsr_run_control.h b/plsr/run_control/plsr_run_control.h index 0c32897..de79263 100644 --- a/plsr/run_control/plsr_run_control.h +++ b/plsr/run_control/plsr_run_control.h @@ -11,9 +11,9 @@ void PlsrRunControlInit(void); uint8_t PlsrRunControlStart(uint16_t start_seg_1based); void PlsrRunControlStop(void); uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz); -void PlsrRunControlTickMs(void); -void PlsrRunControlOn1ms(void); /* TIM3 1ms：改频 + WAIT/ACT 时基 */ -void PlsrRunControlOnPulseIsr(void); /* 脉冲 UPDATE：只计脉冲 */ +void PlsrRunControlOn1ms(void); /* TIM3 1ms：仅 ACT/WAIT 时基 */ +void PlsrRunControlOnPulseIsr(void); /* 脉冲 UPDATE：计脉冲 + dirty */ +void PlsrRunControlTickMs(void); /* 任务：脉冲域改频 + EXT/到期切段 */ uint8_t PlsrRunControlIsBusy(void); int32_t PlsrRunControlGetAccPulse(void); void PlsrRunControlClearAccPulse(void);