diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 1fe62dc..17761f3 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -394,64 +394,82 @@ uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, } /** - * 脉冲起点锁频递推脉冲数(与每脉冲改频一致);f(t) 按 mode 形状。 - * 仅任务/规划上下文;禁止进脉冲 ISR。 - * - * 注意:不可再用「每 ms 贡献 f/1000」——低频时一个脉冲周期 >> 1ms, - * 那种估法会把 dec_n 估成 1,示波器上像没有减速。 + * 斜坡脉冲数:与 1ms 时间轴上的面积一致。 + * 直线:N = (f0+f1) * T_ms / 2000(平均频率 × 时间)。 + * S/正弦:按 1ms 形状采样累加。禁止按脉冲周期递推(那是另一套模型)。 */ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, uint32_t f_to, uint32_t t_ms, PlsrAccelMode_e mode) { - uint32_t T_us; - uint32_t elapsed_us; - uint32_t n; + uint32_t t; uint32_t f; - uint32_t guard; + uint32_t n; + uint64_t acc; + uint32_t t_use; if ((t_ms == 0U) || (f_from == f_to)) { return 0U; } - T_us = t_ms * 1000UL; - elapsed_us = 0U; - n = 0U; - guard = 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; + } - while ((elapsed_us < T_us) && (guard < 2000000UL)) + /* S/正弦:最多按 120s 采样,避免规划阶段死循环 */ + t_use = t_ms; + if (t_use > 120000U) + { + t_use = 120000U; + } + acc = 0ULL; + for (t = 0U; t < t_use; t++) { - f = PlsrAccelCurveFreqOnRampUs(f_from, f_to, elapsed_us, T_us, mode); + f = PlsrAccelCurveFreqOnRampMs(f_from, f_to, t, t_ms, mode); if (f < 1U) { - if ((elapsed_us == 0U) && (f_from == 0U) && (f_to >= 1U)) - { - f = PlsrAccelCurveFirstPulseFreq(f_from, f_to, T_us, mode); - } - else if (f_to == 0U) + if (f_to == 0U) { break; } - else - { - f = 1U; - } + f = 1U; } + acc += (uint64_t)f; if ((f_to == 0U) && (f <= 1U)) { - n++; break; } - elapsed_us += (1000000UL + (f / 2UL)) / f; - n++; - guard++; } - + n = (uint32_t)(acc / 1000ULL); + if (n == 0U) + { + n = 1U; + } return n; } +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; +} + /** * 拟合斜坡时间 T(任务上下文);使离散模型脉冲数尽量等于 max_pulses。 */ @@ -536,23 +554,9 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, return best; } -/** 在 f_cur→f_want 路径上按千分比取点 */ -static uint32_t PlsrAccelCurveOnPath(uint32_t f_cur, uint32_t f_want, uint32_t permille) -{ - if (permille >= 1000U) - { - return f_want; - } - if (f_want >= f_cur) - { - return f_cur + ((f_want - f_cur) * permille) / 1000UL; - } - return f_cur - ((f_cur - f_want) * permille) / 1000UL; -} - /** - * 二分“路径进度”,使加+减估算脉冲 <= total,且尽量接近原目标频。 - * 时间按峰值重算(斜率不变),避免旧逻辑把 T 按脉冲比例压扁。 + * 二分峰值频率(Hz),使 1ms 模型下 acc+dec 脉冲 <= total,且尽量接近原目标频。 + * 到不了目标频时这就是三角:峰值降低,匀速脉冲必须为 0。 */ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, uint32_t f_cur, @@ -566,7 +570,7 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, PlsrAccelMode_e mode) { uint32_t lo = 0U; - uint32_t hi = 1000U; + uint32_t hi; uint32_t mid; uint32_t best = 0U; uint32_t peak; @@ -574,12 +578,29 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, uint32_t t_d; uint32_t n_a; uint32_t n_d; - uint32_t i; + uint32_t span; - for (i = 0U; i < 12U; i++) + if (f_want >= f_cur) + { + span = f_want - f_cur; + } + else + { + span = f_cur - f_want; + } + hi = span; + + while (lo <= hi) { mid = lo + ((hi - lo) / 2UL); - peak = PlsrAccelCurveOnPath(f_cur, f_want, mid); + if (f_want >= f_cur) + { + peak = f_cur + mid; + } + else + { + peak = f_cur - mid; + } t_a = PlsrAccelCurveRampTimeMs(f_cur, peak, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); @@ -592,6 +613,10 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, if ((n_a + n_d) <= total_pulses) { best = mid; + if (mid == span) + { + break; + } lo = mid + 1UL; if (lo > hi) { @@ -608,18 +633,13 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, } } - return PlsrAccelCurveOnPath(f_cur, f_want, best); + if (f_want >= f_cur) + { + return f_cur + best; + } + return f_cur - best; } -static uint32_t PlsrAccelCurveSimDecelStartMs(uint32_t f_cur, - uint32_t f_tgt, - uint32_t f_end, - uint32_t acc_n, - uint32_t const_n, - uint32_t t_acc_ms, - uint32_t t_dec_ms, - PlsrAccelMode_e mode); - void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, uint32_t total_pulses, uint32_t f_cur, @@ -688,7 +708,7 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, plan->f_tgt = f_peak; plan->t_acc_ms = t_acc; plan->t_dec_ms = 0U; - plan->t_decel_start_ms = 0U; + plan->t_decel_start_ms = t_acc; plan->acc_n = total_pulses; plan->dec_n = 0U; return; @@ -697,9 +717,7 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, if ((acc_n + dec_n) > total_pulses) { /* - * 旧逻辑:按脉冲比例压缩 t_acc/t_dec → 高速/少脉冲时 T→1ms, - * 示波器上看成方波,直线/S/正弦无差别。 - * 新逻辑:降低峰值,按斜率重算时间,保留可辨认的相时间。 + * 脉冲不够到目标频:降峰值(斜率不变),三角,禁止匀速。 */ f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end, @@ -713,56 +731,33 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, accel_ms, decel_ms); acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m); dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m); - - if ((acc_n + dec_n) > total_pulses) + if (acc_n > total_pulses) { - /* 仍略超:按比例切脉冲,但时间至少保留 1ms/相,且不按 raw 比例把 T 打没 */ - uint32_t sum = acc_n + dec_n; - if (sum == 0U) - { - acc_n = total_pulses / 2UL; - dec_n = total_pulses - acc_n; - } - else - { - acc_n = (total_pulses * acc_n) / sum; - dec_n = total_pulses - acc_n; - if ((f_peak > f_end) && (dec_n == 0U) && (total_pulses > 1U)) - { - dec_n = 1U; - if (acc_n >= total_pulses) - { - acc_n = total_pulses - 1U; - } - else - { - acc_n = total_pulses - dec_n; - } - } - } - plan->const_n = 0U; - if ((t_acc == 0U) && (acc_n > 0U)) - { - t_acc = 1U; - } - if ((t_dec == 0U) && (dec_n > 0U)) - { - t_dec = 1U; - } + acc_n = total_pulses; + dec_n = 0U; } - else + else if ((acc_n + dec_n) > total_pulses) { - plan->const_n = total_pulses - acc_n - dec_n; + dec_n = total_pulses - acc_n; + } + plan->const_n = 0U; + if ((t_acc == 0U) && (acc_n > 0U) && (f_cur != f_peak)) + { + t_acc = 1U; + } + if ((t_dec == 0U) && (dec_n > 0U) && (f_peak != f_end)) + { + t_dec = 1U; } } else { + /* 够到目标频:匀速脉冲 = 总数 − 加速 − 减速 */ plan->const_n = total_pulses - acc_n - dec_n; } /* * 需要减速 (f_peak > f_end) 时禁止 dec_n=0。 - * 按比例切脉冲时 acc 常远大于 dec,会把 dec_n 切成 0 → 末段到顶速即停。 */ if ((f_peak > f_end) && (total_pulses > 1U) && (dec_n == 0U)) { @@ -771,88 +766,34 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, { acc_n = total_pulses - 1U; } - plan->const_n = total_pulses - acc_n - dec_n; - } - - plan->f_tgt = f_peak; - plan->t_acc_ms = t_acc; - plan->t_dec_ms = t_dec; - plan->acc_n = acc_n; - plan->dec_n = dec_n; - plan->t_decel_start_ms = PlsrAccelCurveSimDecelStartMs(f_cur, f_peak, f_end, - acc_n, plan->const_n, - t_acc, t_dec, m); -} - -/** - * 正向仿真 acc+const 脉冲耗时,得到段内时间轴上进入减速的时刻。 - * t_dec 由 (f_tgt,f_end) 与减速斜率决定;本函数只反推“何时该开始减速”。 - */ -static uint32_t PlsrAccelCurveSimDecelStartMs(uint32_t f_cur, - uint32_t f_tgt, - uint32_t f_end, - uint32_t acc_n, - uint32_t const_n, - uint32_t t_acc_ms, - uint32_t t_dec_ms, - PlsrAccelMode_e mode) -{ - uint32_t elapsed_us; - uint32_t n; - uint32_t f; - uint32_t guard; - uint32_t T_acc_us; - uint32_t f_run; - - (void)f_end; - (void)t_dec_ms; - - if ((acc_n == 0U) && (const_n == 0U)) - { - return 0U; - } - - elapsed_us = 0U; - n = 0U; - guard = 0U; - T_acc_us = t_acc_ms * 1000UL; - - while ((n < acc_n) && (guard < 2000000UL)) - { - if (t_acc_ms == 0U) - { - break; - } - f = PlsrAccelCurveFreqOnRampUs(f_cur, f_tgt, elapsed_us, T_acc_us, mode); - if (f < 1U) + if (plan->const_n > 0U) { - if ((n == 0U) && (f_cur == 0U) && (f_tgt >= 1U)) - { - f = PlsrAccelCurveFirstPulseFreq(f_cur, f_tgt, T_acc_us, mode); - } - else if (f_tgt >= 1U) + if (plan->const_n >= 1U) { - f = 1U; + plan->const_n -= 1U; } else { - break; + plan->const_n = 0U; } } - elapsed_us += (1000000UL + (f / 2UL)) / f; - n++; - guard++; - } - - f_run = (f_tgt >= 1U) ? f_tgt : 1U; - while ((n < (acc_n + const_n)) && (guard < 2000000UL)) - { - elapsed_us += (1000000UL + (f_run / 2UL)) / f_run; - n++; - guard++; + else + { + plan->const_n = total_pulses - acc_n - dec_n; + } } - return elapsed_us / 1000UL; + plan->f_tgt = f_peak; + plan->t_acc_ms = t_acc; + 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, @@ -872,16 +813,16 @@ uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, (plan->f_cur != plan->f_tgt)) { T_acc_us = plan->t_acc_ms * 1000UL; - if ((seg_elapsed_ms == 0U) && (plan->f_cur == 0U)) { - return PlsrAccelCurveClampFreq( - PlsrAccelCurveFirstPulseFreq(plan->f_cur, plan->f_tgt, - T_acc_us, plan->mode)); + 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); } - return PlsrAccelCurveClampFreq( - PlsrAccelCurveFreqOnRampUs(plan->f_cur, plan->f_tgt, - seg_elapsed_ms * 1000UL, - T_acc_us, plan->mode)); } if (seg_elapsed_ms < plan->t_decel_start_ms) diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index 1c6fcf4..4644470 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -2,7 +2,8 @@ * @file plsr_accel_curve.h * @brief 预估加/匀/减脉冲;相内按时间走直线/S/正弦 * - * 1ms 改频:规划阶段反向算 t_decel_start_ms;运行时按段内绝对时间取频。 + * 1ms 改频:直线脉冲按面积 N=(f0+f1)*T/2000;到不了目标频则降峰值走三角(无匀速)。 + * 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。 * 斜率按公共参数:K≈(默认速度−起/止速)/加减速时间;小频差时 T 可能 <1ms,无可见斜坡属正常。 */ #ifndef PLSR_ACCEL_CURVE_H diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index c21cb7d..ef39e4a 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -569,18 +569,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms); if (next < 1U) { - if ((s_accel_plan.f_cur == 0U) && (s_seg_elapsed_ms < 1U)) - { - next = PlsrAccelCurveFirstPulseFreq(s_accel_plan.f_cur, s_accel_plan.f_tgt, - (s_accel_plan.t_acc_ms >= 1U) ? - (s_accel_plan.t_acc_ms * 1000UL) : 1U, - s_accel_plan.mode); - if (next < 1U) - { - next = 1U; - } - } - else if (next == 0U) + if ((s_accel_plan.f_end == 0U) && (s_seg_elapsed_ms > 0U)) { if (s_done < s_target) { @@ -592,6 +581,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) } return; } + next = 1U; } if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U))