修正起始频率

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

plsr/accel_curve/plsr_accel_curve.c

@@ -67,11 +67,11 @@ static uint32_t PlsrAccelCurveIsqrt64(uint64_t val)
 }
 
 /**
- * 起跳频率：f = sqrt(f_from^2 + 2a)，a = df*1000/t_ms。
- * f_from=0 → sqrt(2a)；超过 f_to 钳到 f_to。
+ * 起跳频率（原始）：f = sqrt(f_from^2 + 2a)，a = df*1000/t_ms。
+ * 不钳到 f_to，供与目标频比较；f_from=0 → sqrt(2a)。
  */
-static uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
-                                 uint32_t t_ms)
+static uint32_t PlsrAccelCurveJumpFreqRaw(uint32_t f_from, uint32_t f_to,
+                                          uint32_t t_ms)
 {
     uint32_t df;
     uint32_t a_hz_s;
@@ -89,7 +89,7 @@ static uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
     df = PlsrAccelCurveAbsDiff(f_from, f_to);
     if (df == 0U)
     {
-        return f_to;
+        return (f_from >= 1U) ? f_from : f_to;
     }
     if (df <= 4294967U)
     {
@@ -112,6 +112,18 @@ static uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
     {
         f = 1U;
     }
+    return PlsrAccelCurveClampFreq(f);
+}
+
+/**
+ * 起跳频率：f = sqrt(f_from^2 + 2a)，a = df*1000/t_ms。
+ * f_from=0 → sqrt(2a)；超过 f_to 钳到 f_to。
+ */
+static uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
+                                 uint32_t t_ms)
+{
+    uint32_t f = PlsrAccelCurveJumpFreqRaw(f_from, f_to, t_ms);
+
     if (f > f_to)
     {
         f = f_to;
@@ -179,13 +191,22 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
     return t;
 }
 
-uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
-                                      uint32_t f_tgt,
-                                      uint32_t default_spd,
-                                      uint32_t accel_ms,
-                                      uint32_t decel_ms)
+/**
+ * 按「默认起/止速、目标、起跳」三分支解析边界频率：
+ *   1) 默认速 > 目标 → 用默认速（再减速到目标 / 出口用止速）
+ *   2) 默认速 < 目标 且 目标 < 起跳 → 用目标
+ *   3) 默认速 < 目标 且 目标 > 起跳 → 用起跳
+ * 起跳 = sqrt(f_cfg^2+2a)（不先钳到目标，便于与目标比较）。
+ */
+static uint32_t PlsrAccelCurveResolveBoundHz(uint32_t f_cfg,
+                                            uint32_t f_tgt,
+                                            uint32_t default_spd,
+                                            uint32_t accel_ms,
+                                            uint32_t decel_ms,
+                                            uint8_t is_start)
 {
-    uint32_t t_acc;
+    uint32_t t_ms;
+    uint32_t jump;
 
     f_cfg = PlsrAccelCurveClampFreq(f_cfg);
     f_tgt = PlsrAccelCurveClampFreq(f_tgt);
@@ -194,10 +215,40 @@ uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
         return 1U;
     }
 
-    /* 起速 0 或非 0：都用 JumpFreq = sqrt(f_cfg^2 + 2a) */
-    t_acc = PlsrAccelCurveRampTimeMs(f_cfg, f_tgt, default_spd,
-                                     accel_ms, decel_ms);
-    return PlsrAccelCurveJumpFreq(f_cfg, f_tgt, t_acc);
+    /* 默认起/止速 > 目标：用默认速 */
+    if (f_cfg > f_tgt)
+    {
+        return f_cfg;
+    }
+
+    if (is_start != 0U)
+    {
+        t_ms = PlsrAccelCurveRampTimeMs(f_cfg, f_tgt, default_spd,
+                                        accel_ms, decel_ms);
+    }
+    else
+    {
+        t_ms = PlsrAccelCurveRampTimeMs(f_tgt, f_cfg, default_spd,
+                                        accel_ms, decel_ms);
+    }
+    jump = PlsrAccelCurveJumpFreqRaw(f_cfg, f_tgt, t_ms);
+
+    /* 目标 < 起跳 → 用目标；目标 > 起跳 → 用起跳 */
+    if (f_tgt < jump)
+    {
+        return f_tgt;
+    }
+    return jump;
+}
+
+uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
+                                      uint32_t f_tgt,
+                                      uint32_t default_spd,
+                                      uint32_t accel_ms,
+                                      uint32_t decel_ms)
+{
+    return PlsrAccelCurveResolveBoundHz(f_cfg, f_tgt, default_spd,
+                                       accel_ms, decel_ms, 1U);
 }
 
 uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
@@ -206,28 +257,8 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
                                     uint32_t accel_ms,
                                     uint32_t decel_ms)
 {
-    uint32_t t_dec;
-
-    f_cfg = PlsrAccelCurveClampFreq(f_cfg);
-    f_tgt = PlsrAccelCurveClampFreq(f_tgt);
-    if (f_tgt < 1U)
-    {
-        return 1U;
-    }
-
-    /*
-     * 止速 0 或 1：按落地频率估计，避免 1Hz 拖尾。
-     */
-    if (f_cfg <= 1U)
-    {
-        t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd,
-                                         accel_ms, decel_ms);
-        return PlsrAccelCurveJumpFreq(0U, f_tgt, t_dec);
-    }
-
-    t_dec = PlsrAccelCurveRampTimeMs(f_tgt, f_cfg, default_spd,
-                                     accel_ms, decel_ms);
-    return PlsrAccelCurveJumpFreq(f_cfg, f_tgt, t_dec);
+    return PlsrAccelCurveResolveBoundHz(f_cfg, f_tgt, default_spd,
+                                       accel_ms, decel_ms, 0U);
 }
 
 /** 直线：进度千分比 → 形状千分比 */
@@ -828,7 +859,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
             peak = f_cur - mid;
             n_a = PlsrAccelCurvePulsesForModeRamp(f_cur, peak, a_dec, mode);
         }
-        n_d = PlsrAccelCurvePulsesForModeRamp(peak, f_end, a_dec, mode);
+        n_d = PlsrAccelCurvePulsesForModeRamp(peak, f_end,
+                (f_end > peak) ? a_acc : a_dec, mode);
 
         if ((n_a + n_d) <= total_pulses)
         {
@@ -938,14 +970,22 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
 
     if (f_cur > f_peak)
     {
-        /* 入口高于峰值：首相为减速，用 a_dec（与 rampTimeMs 减速分支一致） */
+        /* 入口高于目标：首相减速到目标（谷底左侧 \） */
         acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_dec, m);
     }
     else
     {
         acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_acc, m);
     }
-    dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m);
+    /* 第二相：目标→出口；出口更高用 a_acc（谷底右侧 /），否则用 a_dec */
+    if (f_end > f_peak)
+    {
+        dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_acc, m);
+    }
+    else
+    {
+        dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m);
+    }
 
     if (total_pulses == 0U)
     {
@@ -977,7 +1017,14 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
         {
             acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_acc, m);
         }
-        dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m);
+        if (f_end > f_peak)
+        {
+            dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_acc, m);
+        }
+        else
+        {
+            dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m);
+        }
         if (acc_n > total_pulses)
         {
             acc_n = total_pulses;
@@ -1121,7 +1168,9 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
     rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U;
     if (plan->mode == PLSR_ACCEL_LINEAR)
     {
-        f = PlsrAccelCurveFreqKinematic(plan->f_tgt, plan->a_dec,
+        uint32_t a_use = (rising != 0U) ? plan->a_acc : plan->a_dec;
+
+        f = PlsrAccelCurveFreqKinematic(plan->f_tgt, a_use,
                                         n, rising, plan->f_end);
     }
     else
@@ -1131,16 +1180,17 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
         uint32_t tramp = 0U;
         uint32_t t_us = 0U;
         uint32_t i;
+        uint32_t a_use = (rising != 0U) ? plan->a_acc : plan->a_dec;
 
         f = plan->f_tgt;
         if (plan->mode == PLSR_ACCEL_S)
         {
-            PlsrAccelCurveSTimeParams(plan->f_tgt, plan->f_end, plan->a_dec,
+            PlsrAccelCurveSTimeParams(plan->f_tgt, plan->f_end, a_use,
                                       &tj, &ta, &tramp);
         }
         else
         {
-            PlsrAccelCurveSineTimeParams(plan->f_tgt, plan->f_end, plan->a_dec,
+            PlsrAccelCurveSineTimeParams(plan->f_tgt, plan->f_end, a_use,
                                          &tramp);
         }
         for (i = 0U; i < n; i++)
@@ -1153,7 +1203,7 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
             if (plan->mode == PLSR_ACCEL_S)
             {
                 f = PlsrAccelCurveSFreqAtTime(plan->f_tgt, plan->f_end,
-                                              plan->a_dec, tj, ta, tramp, t_us);
+                                              a_use, tj, ta, tramp, t_us);
             }
             else
             {
@@ -1233,10 +1283,11 @@ void PlsrAccelPulseRtBeginDec(PlsrAccelPulseRt_t *rt,
     rt->f0 = plan->f_tgt;
     rt->f1 = plan->f_end;
     rt->f = plan->f_tgt;
-    rt->a = plan->a_dec;
     rt->n = 0U;
     rt->n_total = plan->dec_n;
     rt->rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U;
+    /* 谷底右侧爬升用 a_acc，正常减速用 a_dec */
+    rt->a = (rt->rising != 0U) ? plan->a_acc : plan->a_dec;
     rt->mode = plan->mode;
     rt->active = (plan->dec_n > 0U) && (plan->f_tgt != plan->f_end) ? 1U : 0U;
     PlsrAccelPulseRtSetupTimeProfile(rt);

plsr/accel_curve/plsr_accel_curve.h

@@ -36,14 +36,14 @@ typedef struct {
     PlsrAccelMode_e mode;     /* 直线 / S / 正弦 */
 } PlsrAccelPlan_t;
 
-/** 配置起速 → 规划用真实起点（0=Jump 起跳；非 0 且 >f_tgt 则直接用起速走减速） */
+/** 配置起速 → 规划入口：起速>目标用起速；否则目标与起跳比较取其一 */
 uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
                                       uint32_t f_tgt,
                                       uint32_t default_spd,
                                       uint32_t accel_ms,
                                       uint32_t decel_ms);
 
-/** 配置止速 → 规划用真实终点（f_cfg<=1 按落地估计） */
+/** 配置止速 → 规划出口：止速>目标用止速；否则目标与起跳比较取其一（不默认到 0） */
 uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
                                     uint32_t f_tgt,
                                     uint32_t default_spd,

plsr/run_control/plsr_run_control.c

@@ -672,36 +672,26 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t
     else
     {
         /*
-         * 停表后冷启动：仅本次运行的启动段、且目标频低于配置起速时，
-         * 入口直接用起速（再规划减速到目标）；其余仍走起跳频率。
+         * 停表后冷启动：入口由 ResolveStartHz 按起速/目标/起跳三分支决定
+         * （起速>目标→起速减速；否则与起跳比较取目标或起跳）。
          */
-        if ((s_run_first_entry != 0U) &&
-            (seg_idx0 == s_run_entry_seg0) &&
-            (cfg->start_speed >= 1U) && (f_tgt < cfg->start_speed))
+        if ((s_run_first_entry != 0U) && (seg_idx0 == s_run_entry_seg0))
         {
             s_run_first_entry = 0U;
-            endpoints->freq_start_hz = PlsrRunControlClampSpeed(cfg->start_speed);
-        }
-        else
-        {
-            if ((s_run_first_entry != 0U) && (seg_idx0 == s_run_entry_seg0))
-            {
-                s_run_first_entry = 0U;
-            }
-            endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(cfg->start_speed,
-                                                       f_tgt,
-                                                       cfg->default_speed,
-                                                       endpoints->accel_ms,
-                                                       endpoints->decel_ms);
         }
+        endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(cfg->start_speed,
+                                                   f_tgt,
+                                                   cfg->default_speed,
+                                                   endpoints->accel_ms,
+                                                   endpoints->decel_ms);
     }
 
     /*
      * ---------- 终止频率 ----------
      * EXT_OR_DONE：同向/反向只看「下一有效段」（跳过频率0且脉冲0 的占位段）。
-     *   是否向止速规划 = 与下一有效段是否换向；夹无效段本身不单独强制减速。
      *   同向 + 后续 → 过渡到下一有效段目标；同向 + 完成 → 匀速峰值收尾。
-     * 真末段 / TIME/SIGNAL/ACT/EXT → 一律规划到公共止速
+     * 真末段 / TIME/SIGNAL/ACT/EXT / 换向 → ResolveEndHz（止速/目标/起跳三分支，
+     *   不再默认往 0 收；止速>目标则用止速，否则按起跳与目标比较）。
      */
     {
         int16_t next_eff = PlsrRunControlNextEffectiveSeg(seg_idx0);
@@ -898,8 +888,12 @@ static void PlsrRunControlApplyOutFreqIsr(uint32_t profile_freq)
 
 static uint8_t PlsrRunControlHasDecel(void)
 {
-    return ((s_plan_dec_n > 0U) &&
-            (s_accel_plan.f_tgt != s_accel_plan.f_end)) ? 1U : 0U;
+    /*
+     * 第二相脉冲预算 >0 即需要收尾斜坡。
+     * 谷底形（起/止速 > 目标）：第二相是从目标再加速回止速，f_end > f_tgt，
+     * 不可再用「f_tgt==f_end 则无减速」这种假定。
+     */
+    return (s_plan_dec_n > 0U) ? 1U : 0U;
 }
 
 static uint32_t PlsrRunControlRemainPulses(void)