diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 17761f3..87855fe 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -394,9 +394,10 @@ uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, } /** - * 斜坡脉冲数:与 1ms 时间轴上的面积一致。 - * 直线:N = (f0+f1) * T_ms / 2000(平均频率 × 时间)。 - * S/正弦:按 1ms 形状采样累加。禁止按脉冲周期递推(那是另一套模型)。 + * 斜坡脉冲数:与运行时 TIM3 1ms 取频一致(t=0..T-1 每毫秒贡献 f 个脉冲)。 + * 向上取整:减速段多留脉冲,才能在发完前走到 f_end。 + * 梯形面积 (f0+f1)*T/2000 会少算末段减速(约 (f_peak-f_end)/2000 个), + * 匀速被拉长,最后一段脉冲在斜坡中途用完,到不了止速。 */ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, uint32_t f_to, @@ -406,68 +407,54 @@ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, uint32_t t; uint32_t f; uint32_t n; - uint64_t acc; uint32_t t_use; + uint64_t acc; if ((t_ms == 0U) || (f_from == f_to)) { return 0U; } - if (mode == PLSR_ACCEL_LINEAR) - { - acc = ((uint64_t)f_from + (uint64_t)f_to) * (uint64_t)t_ms; - n = (uint32_t)(acc / 2000ULL); - if ((acc % 2000ULL) >= 1000ULL) - { - n++; - } - if (n == 0U) - { - n = 1U; - } - return n; - } - - /* S/正弦:最多按 120s 采样,避免规划阶段死循环 */ t_use = t_ms; if (t_use > 120000U) { t_use = 120000U; } + acc = 0ULL; for (t = 0U; t < t_use; t++) { f = PlsrAccelCurveFreqOnRampMs(f_from, f_to, t, t_ms, mode); if (f < 1U) { - if (f_to == 0U) + if (f_to >= 1U) { - break; + f = 1U; + } + else + { + f = 0U; } - f = 1U; } acc += (uint64_t)f; - if ((f_to == 0U) && (f <= 1U)) - { - break; - } } - n = (uint32_t)(acc / 1000ULL); - if (n == 0U) + + n = (uint32_t)((acc + 999ULL) / 1000ULL); + if ((n == 0U) && (acc > 0ULL)) { n = 1U; } return n; } +/** 匀速时间向下取整:宁可早进减速,保证斜坡走完再落到 f_end */ static uint32_t PlsrAccelCurveConstTimeMs(uint32_t const_n, uint32_t f_hz) { if ((const_n == 0U) || (f_hz < 1U)) { return 0U; } - return (const_n * 1000UL + (f_hz / 2UL)) / f_hz; + return (const_n * 1000UL) / f_hz; } /** diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index 4644470..a407edc 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -2,7 +2,7 @@ * @file plsr_accel_curve.h * @brief 预估加/匀/减脉冲;相内按时间走直线/S/正弦 * - * 1ms 改频:直线脉冲按面积 N=(f0+f1)*T/2000;到不了目标频则降峰值走三角(无匀速)。 + * 1ms 改频:脉冲数按 t=0..T-1 实际取频累加;到不了目标频则降峰值走三角(无匀速)。 * 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。 * 斜率按公共参数:K≈(默认速度−起/止速)/加减速时间;小频差时 T 可能 <1ms,无可见斜坡属正常。 */