diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 7af7227..dd2b4ee 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -1,21 +1,19 @@ /** * @file plsr_accel_curve.c - * @brief 脉冲域规划/取频实现:直线 f^2=f0^2±2an;S/正弦按脉冲进度 shape + * @brief 脉冲域规划/取频:直线 f^2=f0^2±2an;S/正弦按时间轴剖面 * * @details 模块职责 * 实现 plsr_accel_curve.h 全部 API。核心路径: * Plan → 估 acc_n/dec_n → 不够则 FitPeak 降峰; * PulseRtBeginAcc/Dec/Const + Step 供 ISR 每脉冲改频。 * + * S 曲线(时间域,加速度梯形 1:2:1): + * jerk 升 → 恒加速 A → jerk 降;ISR 累计 t+=1/f 后按 f(t) 取频。 + * 正弦:raised-cosine,T=π·Δf/(2A),同样按时间取频。 + * * 关键约束 * 频率钳制上限 100000Hz;运行门禁非法频率由 command/run_control 报 0x04。 * 脉冲预算不够到目标频时:降低峰值(三角,无匀速),调用方写故障 0x02。 - * - * 符号约定(与 plsr_accel_curve.h 一致) - * f_cur/f_tgt/f_end — 段起/峰/止频率 Hz - * acc_n/dec_n/const_n — 加/减/匀速相脉冲预算(非时间 ms) - * a_acc/a_dec — 加/减速度 Hz/s - * pulse_rt — PlsrAccelPulseRt_t,ISR 逐拍改频状态机 */ #include "plsr_accel_curve.h" #include @@ -401,6 +399,340 @@ static uint32_t PlsrAccelCurvePulsesForRamp(uint32_t f0, uint32_t f1, return (uint32_t)n; } +/*============================================================================*/ +/* S / 正弦:时间轴剖面(加速度梯形 / raised-cosine) */ +/*============================================================================*/ + +/** + * S 曲线时间参数(加速度梯形 1:2:1): + * Tj : Ta : Tj = 1 : 2 : 1 + * Δf = A*(Ta+Tj) ⇒ Tj=Δf/(3A), Ta=2Δf/(3A), T=4Δf/(3A) + */ +static void PlsrAccelCurveSTimeParams(uint32_t f0, uint32_t f1, uint32_t a_hz_s, + uint32_t *tj_us, uint32_t *ta_us, + uint32_t *tramp_us) +{ + uint32_t df; + uint64_t num; + uint64_t den; + uint32_t tj; + + *tj_us = 0U; + *ta_us = 0U; + *tramp_us = 0U; + + df = PlsrAccelCurveAbsDiff(f0, f1); + if ((df < 1U) || (a_hz_s < 1U)) + { + return; + } + + /* Tj_us = ceil(df * 1e6 / (3A)) */ + num = (uint64_t)df * 1000000ULL; + den = 3ULL * (uint64_t)a_hz_s; + tj = (uint32_t)((num + den - 1ULL) / den); + if (tj < 1U) + { + tj = 1U; + } + *tj_us = tj; + *ta_us = tj * 2U; + *tramp_us = tj * 4U; +} + +/** 正弦 raised-cosine:峰值加速度 = A ⇒ T = π·Δf / (2A) */ +static void PlsrAccelCurveSineTimeParams(uint32_t f0, uint32_t f1, uint32_t a_hz_s, + uint32_t *tramp_us) +{ + uint32_t df; + uint64_t num; + uint64_t den; + + *tramp_us = 0U; + df = PlsrAccelCurveAbsDiff(f0, f1); + if ((df < 1U) || (a_hz_s < 1U)) + { + return; + } + /* T_us = ceil(π*df*1e6 / (2A)),π≈355/113 */ + num = (uint64_t)df * 1000000ULL * 355ULL; + den = 2ULL * (uint64_t)a_hz_s * 113ULL; + *tramp_us = (uint32_t)((num + den - 1ULL) / den); + if (*tramp_us < 1U) + { + *tramp_us = 1U; + } +} + +/** Δf = A * t_us / 1e6 */ +static uint32_t PlsrAccelCurveDeltaHzFromAt(uint32_t a_hz_s, uint32_t t_us) +{ + return (uint32_t)(((uint64_t)a_hz_s * (uint64_t)t_us) / 1000000ULL); +} + +/** Δf = A * t_us^2 / (2 * tj_us * 1e6) — jerk 段 */ +static uint32_t PlsrAccelCurveDeltaHzFromJerk(uint32_t a_hz_s, uint32_t t_us, + uint32_t tj_us) +{ + uint64_t v; + + if ((tj_us < 1U) || (t_us < 1U) || (a_hz_s < 1U)) + { + return 0U; + } + /* ((A * t / tj) * t) / 2e6 */ + v = ((uint64_t)a_hz_s * (uint64_t)t_us) / (uint64_t)tj_us; + v = (v * (uint64_t)t_us) / 2000000ULL; + if (v > 0xFFFFFFFFULL) + { + return 0xFFFFFFFFUL; + } + return (uint32_t)v; +} + +/** + * 时刻 t_us 的 S 剖面频率(相对进度 0..df,再映射到 f0→f1)。 + */ +static uint32_t PlsrAccelCurveSFreqAtTime(uint32_t f0, uint32_t f1, uint32_t a_hz_s, + uint32_t tj_us, uint32_t ta_us, + uint32_t tramp_us, uint32_t t_us) +{ + uint32_t df; + uint32_t g; + uint32_t t2; + uint32_t tau; + uint8_t rising; + + df = PlsrAccelCurveAbsDiff(f0, f1); + rising = (f1 >= f0) ? 1U : 0U; + + if ((df < 1U) || (tramp_us < 1U) || (a_hz_s < 1U)) + { + return PlsrAccelCurveClampFreq(f1); + } + if (t_us >= tramp_us) + { + return PlsrAccelCurveClampFreq(f1); + } + + if ((tj_us < 1U) || (t_us <= tj_us)) + { + /* jerk 升:g = 0.5*(A/Tj)*t^2 */ + g = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, t_us, (tj_us < 1U) ? 1U : tj_us); + } + else + { + t2 = tj_us + ta_us; + if (t_us <= t2) + { + /* 恒加速:g = 0.5*A*Tj + A*(t-Tj) */ + g = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, tj_us, tj_us); + g += PlsrAccelCurveDeltaHzFromAt(a_hz_s, t_us - tj_us); + } + else + { + /* jerk 降:g = 0.5*A*Tj + A*Ta + A*τ - 0.5*(A/Tj)*τ^2 */ + tau = t_us - t2; + if (tau > tj_us) + { + tau = tj_us; + } + g = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, tj_us, tj_us); + g += PlsrAccelCurveDeltaHzFromAt(a_hz_s, ta_us); + g += PlsrAccelCurveDeltaHzFromAt(a_hz_s, tau); + { + uint32_t drop = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, tau, tj_us); + if (g > drop) + { + g -= drop; + } + else + { + g = 0U; + } + } + } + } + + if (g > df) + { + g = df; + } + if (rising != 0U) + { + return PlsrAccelCurveClampFreq(f0 + g); + } + if (f0 > g) + { + return PlsrAccelCurveClampFreq(f0 - g); + } + return PlsrAccelCurveClampFreq(f1); +} + +/** + * 正弦 raised-cosine:f = f0 + df * (1-cos(π u))/2,u=t/T + * 用现有 33 点 sine 形状表(已是 raised-cosine 千分比)。 + */ +static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t f0, uint32_t f1, + uint32_t tramp_us, uint32_t t_us) +{ + uint32_t u; + uint32_t ratio; + uint32_t f; + + if ((tramp_us < 1U) || (f0 == f1)) + { + return PlsrAccelCurveClampFreq(f1); + } + if (t_us >= tramp_us) + { + return PlsrAccelCurveClampFreq(f1); + } + u = (uint32_t)(((uint64_t)t_us * 1000ULL) / (uint64_t)tramp_us); + if (u > 1000U) + { + u = 1000U; + } + ratio = PlsrAccelCurveShapeSinePermille(u); + f = PlsrAccelCurveLerp(f0, f1, ratio); + if (f < 1U) + { + f = 1U; + } + 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; +} + +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) + { + return PlsrAccelCurvePulsesForSineRamp(f0, f1, a_hz_s); + } + return PlsrAccelCurvePulsesForRamp(f0, f1, a_hz_s); +} + /** 直线: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, @@ -461,7 +793,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, uint32_t f_want, uint32_t f_end, uint32_t a_acc, - uint32_t a_dec) + uint32_t a_dec, + PlsrAccelMode_e mode) { uint32_t lo = 0U; uint32_t hi; @@ -488,14 +821,14 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, if (f_want >= f_cur) { peak = f_cur + mid; - n_a = PlsrAccelCurvePulsesForRamp(f_cur, peak, a_acc); + n_a = PlsrAccelCurvePulsesForModeRamp(f_cur, peak, a_acc, mode); } else { peak = f_cur - mid; - n_a = PlsrAccelCurvePulsesForRamp(f_cur, peak, a_dec); + n_a = PlsrAccelCurvePulsesForModeRamp(f_cur, peak, a_dec, mode); } - n_d = PlsrAccelCurvePulsesForRamp(peak, f_end, a_dec); + n_d = PlsrAccelCurvePulsesForModeRamp(peak, f_end, a_dec, mode); if ((n_a + n_d) <= total_pulses) { @@ -606,13 +939,13 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, if (f_cur > f_peak) { /* 入口高于峰值:首相为减速,用 a_dec(与 rampTimeMs 减速分支一致) */ - acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_dec); + acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_dec, m); } else { - acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc); + acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_acc, m); } - dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec); + dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m); if (total_pulses == 0U) { @@ -635,16 +968,16 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, if ((acc_n + dec_n) > total_pulses) { f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end, - a_acc, a_dec); + a_acc, a_dec, m); if (f_cur > f_peak) { - acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_dec); + acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_dec, m); } else { - acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc); + acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_acc, m); } - dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec); + dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m); if (acc_n > total_pulses) { acc_n = total_pulses; @@ -700,7 +1033,6 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, uint32_t const_end; uint32_t n; uint32_t f; - uint32_t ratio; uint8_t rising; if (plan == (PlsrAccelPlan_t *)0) @@ -724,8 +1056,42 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, } else { - ratio = PlsrAccelCurveShape(pulse_done, plan->acc_n, plan->mode); - f = PlsrAccelCurveLerp(plan->f_cur, plan->f_tgt, ratio); + uint32_t tj = 0U; + uint32_t ta = 0U; + uint32_t tramp = 0U; + uint32_t t_us = 0U; + uint32_t a_use = (rising != 0U) ? plan->a_acc : plan->a_dec; + uint32_t i; + + f = plan->f_cur; + if (plan->mode == PLSR_ACCEL_S) + { + PlsrAccelCurveSTimeParams(plan->f_cur, plan->f_tgt, a_use, + &tj, &ta, &tramp); + } + else + { + PlsrAccelCurveSineTimeParams(plan->f_cur, plan->f_tgt, a_use, + &tramp); + } + for (i = 0U; i < pulse_done; i++) + { + if (f < 1U) + { + f = 1U; + } + t_us += 1000000UL / f; + if (plan->mode == PLSR_ACCEL_S) + { + f = PlsrAccelCurveSFreqAtTime(plan->f_cur, plan->f_tgt, a_use, + tj, ta, tramp, t_us); + } + else + { + f = PlsrAccelCurveSineFreqAtTime(plan->f_cur, plan->f_tgt, + tramp, t_us); + } + } } if ((f < 1U) && (plan->f_tgt >= 1U)) { @@ -760,8 +1126,41 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, } else { - ratio = PlsrAccelCurveShape(n, plan->dec_n, plan->mode); - f = PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio); + uint32_t tj = 0U; + uint32_t ta = 0U; + uint32_t tramp = 0U; + uint32_t t_us = 0U; + uint32_t i; + + f = plan->f_tgt; + if (plan->mode == PLSR_ACCEL_S) + { + PlsrAccelCurveSTimeParams(plan->f_tgt, plan->f_end, plan->a_dec, + &tj, &ta, &tramp); + } + else + { + PlsrAccelCurveSineTimeParams(plan->f_tgt, plan->f_end, plan->a_dec, + &tramp); + } + for (i = 0U; i < n; i++) + { + if (f < 1U) + { + f = 1U; + } + t_us += 1000000UL / f; + if (plan->mode == PLSR_ACCEL_S) + { + f = PlsrAccelCurveSFreqAtTime(plan->f_tgt, plan->f_end, + plan->a_dec, tj, ta, tramp, t_us); + } + else + { + f = PlsrAccelCurveSineFreqAtTime(plan->f_tgt, plan->f_end, + tramp, t_us); + } + } } if (f < 1U) { @@ -775,21 +1174,32 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, /*============================================================================*/ /** - * @brief 为 S/正弦准备 Q24 步进:(1<<24)/N 商余,避免每拍除法 + * @brief 为 S/正弦准备时间剖面(加速度梯形 / raised-cosine) */ -static void PlsrAccelPulseRtSetupShape(PlsrAccelPulseRt_t *rt, uint32_t n_total) +static void PlsrAccelPulseRtSetupTimeProfile(PlsrAccelPulseRt_t *rt) { - if (n_total < 1U) + 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)) { - rt->step_q24 = 0U; - rt->step_rem = 0U; return; } - /* (1<<24)/N 商余:逐拍累加,避免每拍 index/total 除法 */ - rt->step_q24 = 16777216UL / n_total; - rt->step_rem = 16777216UL % n_total; - rt->phase_q24 = 0U; - rt->rem_acc = 0U; + if (rt->mode == PLSR_ACCEL_S) + { + PlsrAccelCurveSTimeParams(rt->f0, rt->f1, rt->a, + &rt->tj_us, &rt->ta_us, &rt->tramp_us); + } + else if (rt->mode == PLSR_ACCEL_SINE) + { + PlsrAccelCurveSineTimeParams(rt->f0, rt->f1, rt->a, &rt->tramp_us); + } } /** @brief 进入加速相(见 plsr_accel_curve.h) */ @@ -809,7 +1219,7 @@ void PlsrAccelPulseRtBeginAcc(PlsrAccelPulseRt_t *rt, rt->a = (rt->rising != 0U) ? plan->a_acc : plan->a_dec; rt->mode = plan->mode; rt->active = (plan->acc_n > 0U) && (plan->f_cur != plan->f_tgt) ? 1U : 0U; - PlsrAccelPulseRtSetupShape(rt, rt->n_total); + PlsrAccelPulseRtSetupTimeProfile(rt); } /** @brief 进入减速相(见 plsr_accel_curve.h) */ @@ -829,7 +1239,7 @@ void PlsrAccelPulseRtBeginDec(PlsrAccelPulseRt_t *rt, rt->rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U; rt->mode = plan->mode; rt->active = (plan->dec_n > 0U) && (plan->f_tgt != plan->f_end) ? 1U : 0U; - PlsrAccelPulseRtSetupShape(rt, rt->n_total); + PlsrAccelPulseRtSetupTimeProfile(rt); } /** @brief 进入匀速相(见 plsr_accel_curve.h) */ @@ -849,6 +1259,10 @@ void PlsrAccelPulseRtBeginConst(PlsrAccelPulseRt_t *rt, rt->rising = 1U; rt->mode = plan->mode; rt->active = 0U; /* 匀速:确认频率没变 → 复用 → 结束 */ + 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; @@ -940,47 +1354,51 @@ static uint32_t PlsrAccelPulseRtSquareStep(PlsrAccelPulseRt_t *rt) } /** - * @brief S/正弦:累加 Q24 进度 → shape 千分比 → Lerp(f0,f1) + * S/正弦时间域步进:上一拍周期推进 t,再按剖面取下一拍频率。 */ -static uint32_t PlsrAccelPulseRtShapeStep(PlsrAccelPulseRt_t *rt) +static uint32_t PlsrAccelPulseRtTimeStep(PlsrAccelPulseRt_t *rt) { - uint32_t ratio; + uint32_t f_prev; + uint32_t dt_us; uint32_t f; - if (rt->n_total < 1U) + if ((rt->tramp_us < 1U) || (rt->a < 1U)) { return PlsrAccelCurveClampFreq(rt->f1); } - if (rt->n >= rt->n_total) + f_prev = rt->f; + if (f_prev < 1U) { - return PlsrAccelCurveClampFreq(rt->f1); + f_prev = 1U; } + dt_us = 1000000UL / f_prev; - rt->phase_q24 += rt->step_q24; - rt->rem_acc += rt->step_rem; - if (rt->rem_acc >= rt->n_total) + if (rt->t_us < (0xFFFFFFFFUL - dt_us)) { - rt->rem_acc -= rt->n_total; - rt->phase_q24 += 1U; + rt->t_us += dt_us; } - if (rt->phase_q24 > 16777216UL) + else { - rt->phase_q24 = 16777216UL; + rt->t_us = 0xFFFFFFFFUL; } - ratio = (uint32_t)(((uint64_t)rt->phase_q24 * 1000ULL) / 16777216ULL); - if (ratio > 1000U) + if (rt->t_us >= rt->tramp_us) { - ratio = 1000U; + return PlsrAccelCurveClampFreq(rt->f1); } - ratio = PlsrAccelCurveShapePermille(ratio, rt->mode); - f = PlsrAccelCurveLerp(rt->f0, rt->f1, ratio); - if (f < 1U) + + if (rt->mode == PLSR_ACCEL_S) { - f = 1U; + f = PlsrAccelCurveSFreqAtTime(rt->f0, rt->f1, rt->a, + rt->tj_us, rt->ta_us, rt->tramp_us, + rt->t_us); } - return PlsrAccelCurveClampFreq(f); + else + { + f = PlsrAccelCurveSineFreqAtTime(rt->f0, rt->f1, rt->tramp_us, rt->t_us); + } + return f; } /** @@ -1015,13 +1433,34 @@ uint32_t PlsrAccelPulseRtStep(PlsrAccelPulseRt_t *rt) } else { - rt->f = PlsrAccelPulseRtShapeStep(rt); + rt->f = PlsrAccelPulseRtTimeStep(rt); } - if ((rt->n_total > 0U) && (rt->n >= rt->n_total)) + 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))) { rt->f = rt->f1; rt->active = 0U; } + else if (rt->mode != PLSR_ACCEL_LINEAR) + { + 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; + } + } + } return rt->f; } diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index 942c335..30e64a7 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -5,6 +5,10 @@ * @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。 + * * 术语(全模块统一): * acc_n / dec_n / const_n — 加速/减速/匀速相各需的脉冲个数(规划预算,非 ms) * f_cur / f_tgt / f_end — 段入口频率、规划峰值、段出口频率(Hz) @@ -66,19 +70,24 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, /** * 单相逐脉冲运行态(ISR 热路径,每来一个 UPDATE 调 Step 一次) - * 直线:±1Hz 逼近 sqrt(f0²±2an);S/正弦:Q24 相位累加 + shape 表 + * 直线:±1Hz 逼近 sqrt(f0²±2an) + * S/正弦:累计 t_us,按时间剖面取频;每拍 t += 1e6/f */ typedef struct { uint32_t f; /* 当前输出频率 Hz */ uint32_t f0; /* 本相起点频率 Hz */ uint32_t f1; /* 本相终点频率 Hz */ - uint32_t a; /* 本相加速度 Hz/s(直线用) */ + uint32_t a; /* 本相加速度 Hz/s */ uint32_t n; /* 本相已步进脉冲数 */ uint32_t n_total; /* 本相总步进预算(= plan 里 acc_n 或 dec_n) */ - uint32_t phase_q24; /* S/正弦:形状进度 Q24 累加器 */ - uint32_t step_q24; /* 每脉冲相位增量 = (1<<24)/n_total */ - uint32_t step_rem; /* 相位累加余数(商余法,避免每拍除法) */ - uint32_t rem_acc; /* 余数累加 */ + 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; uint8_t rising; /* 1=频率升高,0=降低 */ uint8_t active; /* 1=本相仍在步进;0=已到 f1 */ PlsrAccelMode_e mode; diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h index 4804ce9..9ae13eb 100644 --- a/plsr/param/plsr_param.h +++ b/plsr/param/plsr_param.h @@ -116,8 +116,8 @@ typedef enum { /** 加减速曲线形状(脉冲域规划) */ typedef enum { PLSR_ACCEL_LINEAR = 0, /* f^2 = f0^2 ± 2an */ - PLSR_ACCEL_S = 1, /* 七段 S 曲线按脉冲进度 */ - PLSR_ACCEL_SINE = 2 /* raised-cosine */ + PLSR_ACCEL_S = 1, /* 时间域七段 S:加速度梯形 1:2:1 */ + PLSR_ACCEL_SINE = 2 /* 时间域 raised-cosine */ } PlsrAccelMode_e; /**