脉冲规划基本合理，最后一段结尾无法跳到目标频率、曲线折弯仍旧存在

Signed-off-by: hanyongwei <2043702190@qq.com>

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)

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

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))