修复ACT时间切段异常；增加默认起始速度/默认终止速度为0时的起跳频率计算，使用起跳频率=根号下加速斜率，加速斜率采用Hz/s

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

plsr/accel_curve/plsr_accel_curve.c

@@ -26,6 +26,78 @@ static uint32_t PlsrAccelCurveAbsDiff(uint32_t a, uint32_t b)
     return (a >= b) ? (a - b) : (b - a);
 }
 
+/**
+ * 整数平方根（牛顿法）。
+ */
+static uint32_t PlsrAccelCurveIsqrt(uint32_t val)
+{
+    uint32_t x;
+    uint32_t x2;
+    uint32_t i;
+
+    if (val <= 1U)
+    {
+        return val;
+    }
+    x = val;
+    if (x > 10000U)
+    {
+        x = 10000U;
+    }
+    for (i = 0U; i < 30U; i++)
+    {
+        x2 = (x + val / x) / 2U;
+        if (x2 >= x)
+        {
+            break;
+        }
+        x = x2;
+    }
+    return x;
+}
+
+/**
+ * 起跳频率：f_jump = sqrt(a_Hz_per_s)。
+ * 加速度 a = df * 1000 / t_ms（Hz/s），df = |f_to - f_from|。
+ * 当 t_ms=0 或 df=0 时返回 1。
+ */
+uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
+                                uint32_t t_ms)
+{
+    uint32_t df;
+    uint32_t a_hz_s;
+    uint32_t f;
+
+    if (t_ms == 0U)
+    {
+        return 1U;
+    }
+    df = PlsrAccelCurveAbsDiff(f_from, f_to);
+    if (df == 0U)
+    {
+        return 1U;
+    }
+    /* a = df / (t_ms / 1000) = df * 1000 / t_ms，可能很大 */
+    if (df <= 4294967U)
+    {
+        a_hz_s = (df * 1000U) / t_ms;
+    }
+    else
+    {
+        a_hz_s = df / t_ms * 1000U;
+    }
+    if (a_hz_s == 0U)
+    {
+        return 1U;
+    }
+    f = PlsrAccelCurveIsqrt(a_hz_s);
+    if (f < 1U)
+    {
+        f = 1U;
+    }
+    return f;
+}
+
 static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd,
                                        uint32_t primary_ref,
                                        uint32_t fallback_ref)
@@ -656,9 +728,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
     f_end = PlsrAccelCurveClampFreq(f_end);
 
     m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode;
-    plan->f_cur = f_cur;
-    plan->f_end = f_end;
-    plan->mode = m;
 
     f_peak = f_tgt;
     t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
@@ -667,6 +736,60 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
     t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd,
                                      start_spd_ref, end_spd_ref,
                                      accel_ms, decel_ms);
+
+    /*
+     * 起速=0：用起跳频率替代 0。
+     *
+     * 默认起始/终止速度都为 0 时，按你图里的离散模型：
+     *   a_Hz/s = a_Hz_per_ms * 1000 = (slope_Hz/ms) * 1000
+     * 其中 slope_Hz/ms 由本段规划斜率参数决定。
+     *
+     * 本实现中：当 start_spd_ref==0 && end_spd_ref==0 时，
+     * 由斜率定义可得 a_Hz/s = default_spd * 1000 / accel_ms（加速段）。
+     */
+    if ((f_cur == 0U) && (f_tgt >= 1U) && (t_acc >= 1U))
+    {
+        if ((start_spd_ref == 0U) && (end_spd_ref == 0U) && (accel_ms > 0U) &&
+            (default_spd > 0U))
+        {
+            /* a_Hz/s = default_spd / accel_ms(ms) * 1000 */
+            uint64_t a_hz_s = ((uint64_t)default_spd * 1000ULL) / (uint64_t)accel_ms;
+            f_cur = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s);
+        }
+        else
+        {
+            /* 兜底：用 df/t_ms 反推加速度再开方 */
+            f_cur = PlsrAccelCurveJumpFreq(0U, f_tgt, t_acc);
+        }
+        if (f_cur > f_tgt)
+        {
+            f_cur = f_tgt;
+        }
+    }
+    /* 止速=0：同理，减速末端用起跳频率，避免减速尾部 1Hz 拖尾 */
+    if ((f_end == 0U) && (f_tgt >= 1U) && (t_dec >= 1U))
+    {
+        if ((start_spd_ref == 0U) && (end_spd_ref == 0U) && (decel_ms > 0U) &&
+            (default_spd > 0U))
+        {
+            /* a_Hz/s = default_spd / decel_ms(ms) * 1000 */
+            uint64_t a_hz_s = ((uint64_t)default_spd * 1000ULL) / (uint64_t)decel_ms;
+            f_end = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s);
+        }
+        else
+        {
+            /* 兜底：用 df/t_ms 反推加速度再开方 */
+            f_end = PlsrAccelCurveJumpFreq(0U, f_tgt, t_dec);
+        }
+        if (f_end > f_tgt)
+        {
+            f_end = f_tgt;
+        }
+    }
+
+    plan->f_cur = f_cur;
+    plan->f_end = f_end;
+    plan->mode = m;
     acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m);
     dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m);
 

plsr/accel_curve/plsr_accel_curve.h

@@ -33,6 +33,14 @@ typedef enum {
 
 uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz);
 
+/**
+ * 起跳频率：f_jump = sqrt(加速度_Hz/s)。
+ * 起速/止速为 0 时，第一个脉冲（或最后一个脉冲）应以此频率输出，
+ * 使脉冲周期恰好等于加速度产生该频率所需的时间。
+ */
+uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
+                                uint32_t t_ms);
+
 /**
  * 斜坡上按 mode 取频（µs 时间轴）。ISR 可用：O(1)。
  * f = f0+(f1-f0)*shape(t/T)

plsr/run_control/plsr_run_control.c

@@ -60,6 +60,8 @@ static uint16_t s_act_time_ms;     /* 本段 ACT 时长（出脉冲后计） */
 static uint32_t s_act_arm_ms;      /* 武装时刻的 TIM3 s_ms，与改频 elapsed 脱钩 */
 static volatile uint8_t s_act_armed;
 static volatile uint8_t s_act_expire_req; /* TIM3 置位，任务里切段（避免 ISR 里 PlanSeg） */
+static volatile uint8_t s_act_cut_pending; /* ACT 到期，等待当前脉冲 UPDATE 边界 */
+static volatile uint8_t s_act_handoff_keep; /* 边界停表后，下一段仍从当前频率衔接 */
 static volatile uint8_t s_wait_expire_req; /* WAIT 时间到：ISR 只置位 */
 static uint8_t  s_wait_is_signal;   /* RC_WAIT_COND：1=等 WAIT 沿，0=等时间 */
 
@@ -77,6 +79,7 @@ static uint8_t  s_pwm_on;
 /* 本段由策略层解析后的加减速时间（段内只用这两份，不再读 wait） */
 static uint16_t s_run_accel_ms;
 static uint16_t s_run_decel_ms;
+static uint8_t  s_orig_end_zero;   /* 原始止速为 0（起跳频率已替代 plan.f_end） */
 
 /** 策略层交给段处理的接口参数 */
 typedef struct {
@@ -103,7 +106,6 @@ static uint16_t PlsrRunControlWaitTimeMs(const PlsrSeg_t *seg);
 static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
                                   uint32_t f_tgt, uint32_t f_end,
                                   uint16_t accel_ms, uint16_t decel_ms);
-static void PlsrRunControlApplyActTimeCap(uint16_t act_ms);
 static uint32_t PlsrRunControlClampSpeed(uint32_t spd);
 static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz,
                                               uint32_t default_spd,
@@ -124,6 +126,8 @@ static void PlsrRunControlArmActExtOnPulseStart(void)
     }
     s_act_armed = 0U;
     s_act_expire_req = 0U;
+    s_act_cut_pending = 0U;
+    s_act_handoff_keep = 0U;
     s_wait_expire_req = 0U;
     if (seg->wait_type == PLSR_WAIT_ACT)
     {
@@ -142,6 +146,8 @@ static void PlsrRunControlCutSegToNext(void)
     s_state = RC_IDLE;
     s_act_armed = 0U;
     s_act_expire_req = 0U;
+    s_act_cut_pending = 0U;
+    s_act_handoff_keep = 0U;
     s_wait_expire_req = 0U;
     PlsrPulseDriverStop();
     s_follow_cont = 0U;
@@ -176,7 +182,9 @@ static void PlsrRunControlActExpire(void)
 
     next0 = PlsrPathPlanResolveAfterSeg(s_cur_seg);
     keep = 0U;
-    if ((next0 >= 0) && (s_pwm_on != 0U) && (s_cur_freq >= 1U) &&
+    if ((next0 >= 0) &&
+        ((s_pwm_on != 0U) || (s_act_handoff_keep != 0U)) &&
+        (s_cur_freq >= 1U) &&
         (PlsrPathPlanIsForward((uint16_t)next0, s_acc_pulse) == s_forward))
     {
         keep = 1U;
@@ -200,6 +208,8 @@ static void PlsrRunControlActExpire(void)
         s_chain_freq = 0U;
     }
 
+    s_act_handoff_keep = 0U;
+
     PlsrRunControlGotoNextOrFinish(s_cur_seg);
 }
 
@@ -317,7 +327,7 @@ static void PlsrRunControlAfterSegDone(void)
     if (cfg->send_mode == PLSR_SEND_COMPLETE)
     {
         /* 段正常结束：下一段起速 = 本段止速 f_end，不能用峰值 s_cur_freq */
-        s_chain_freq = s_accel_plan.f_end;
+        s_chain_freq = (s_orig_end_zero != 0U) ? 0U : s_accel_plan.f_end;
         s_chain_valid = 1U;
     }
 
@@ -393,7 +403,7 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt,
      * ---------- 终止频率：仅由发送模式 + 是否有下一段决定 ----------
      * 完成方式 / 真末段：公共止速
      * 后续方式非末段：下一段目标（无缝衔接）
-     * ACT 不在这里改 f_end：先按梯形算出加/匀/减时间，再 ApplyActTimeCap。
+     * ACT 只由运行控制层管理，不改变本段曲线的 f_end 或阶段规划。
      */
     (void)f_tgt;
     next0 = PlsrPathPlanNextSeg(seg_0);
@@ -609,7 +619,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses)
      * （拉长会在尾部堆 1Hz 空耗）。
      */
     if ((old_t_dec > 0U) && (old_dec_n > 0U) &&
-        ((remain_pulses == old_dec_n) || (f_end == 0U)))
+        ((remain_pulses == old_dec_n) || (s_orig_end_zero != 0U)))
     {
         s_accel_plan.t_dec_ms = old_t_dec;
         return;
@@ -646,7 +656,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses)
         }
     }
 
-    if (f_end == 0U)
+    if (s_orig_end_zero != 0U)
     {
         s_accel_plan.t_dec_ms = (t_slope > 0U) ? t_slope :
                                 ((old_t_dec > 0U) ? old_t_dec : 1U);
@@ -696,10 +706,10 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
     next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms);
     if (next < 1U)
     {
-        if ((s_accel_plan.f_end == 0U) && (s_seg_elapsed_ms > 0U))
+        if ((s_orig_end_zero != 0U) && (s_seg_elapsed_ms > 0U))
         {
             /*
-             * 时间轴已落到止速 0。剩余脉冲仍要发出并计数（ACT※4：
+             * 原始止速为 0，时间轴走完。剩余脉冲仍要发出并计数（ACT※4：
              * 三阶段走完后等时间，不能把 s_target 截掉导致计数丢失）。
              */
             if (s_done < s_target)
@@ -734,6 +744,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
     /* 段内只吃传入端点与加减速时间，不感知 wait */
     s_run_accel_ms = accel_ms;
     s_run_decel_ms = decel_ms;
+    s_orig_end_zero = (f_end == 0U) ? 1U : 0U;
 
     /*
      * 曲线估 acc_n / dec_n，const_n = total − acc − dec。
@@ -843,41 +854,6 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
     s_seg_elapsed_ms = 0U;
 }
 
-/**
- * 按 ACT 时间裁剪本段时间轴（规划已含加速/匀速/减速）：
- * - T_acc > ACT                → 加速未到目标频就被打断
- * - T_acc+T_const > ACT        → 匀速期内打断，不进入减速
- * - T_acc+T_const < ACT < 三段总长 → 进入减速，可能被中途打断
- * - ACT ≥ 加速+匀速+减速       → 三阶段走完（含减速到止速），脉冲发完后等 ACT（※4）
- */
-static void PlsrRunControlApplyActTimeCap(uint16_t act_ms)
-{
-    uint32_t t_act;
-    uint32_t t_acc;
-    uint32_t t_const_end;
-    uint32_t t_wave;
-
-    t_act = (act_ms == 0U) ? 1U : (uint32_t)act_ms;
-    t_acc = s_accel_plan.t_acc_ms;
-    t_const_end = s_accel_plan.t_decel_start_ms;
-    if (t_const_end < t_acc)
-    {
-        t_const_end = t_acc;
-    }
-    t_wave = t_const_end + s_accel_plan.t_dec_ms;
-
-    if ((t_act >= t_wave) || (t_act > t_const_end))
-    {
-        /* 进入减速，或 ACT 比三段总时间还长：保留减速规划 */
-        return;
-    }
-
-    /* 加速中途或匀速期内就会切段：时间轴上不再走减速 */
-    s_accel_plan.t_dec_ms = 0U;
-    s_accel_plan.dec_n = 0U;
-    s_accel_plan.f_end = s_accel_plan.f_tgt;
-}
-
 static void PlsrRunControlFinishAll(void)
 {
     PlsrPulseDriverStop();
@@ -892,6 +868,8 @@ static void PlsrRunControlFinishAll(void)
     s_chain_freq = 0U;
     s_act_armed = 0U;
     s_act_expire_req = 0U;
+    s_act_cut_pending = 0U;
+    s_act_handoff_keep = 0U;
     s_wait_expire_req = 0U;
     s_act_time_ms = 0U;
     s_act_arm_ms = 0U;
@@ -970,11 +948,6 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0)
     total = (uint32_t)s_target;
     PlsrRunControlPlanSeg(total, io.f_from, f_tgt, io.f_end,
                           io.accel_ms, io.decel_ms);
-    if (seg->wait_type == PLSR_WAIT_ACT)
-    {
-        PlsrRunControlApplyActTimeCap(PlsrRunControlActTimeMs(seg));
-    }
-
     s_done = 0;
     s_busy = 1U;
 
@@ -1017,12 +990,15 @@ void PlsrRunControlInit(void)
     s_chain_valid = 0U;
     s_run_accel_ms = 0U;
     s_run_decel_ms = 0U;
+    s_orig_end_zero = 0U;
     s_phase = PH_CONST;
     s_seg_elapsed_ms = 0U;
     s_approach_from = 0U;
     s_decel_from = 0U;
     s_act_armed = 0U;
     s_act_expire_req = 0U;
+    s_act_cut_pending = 0U;
+    s_act_handoff_keep = 0U;
     s_wait_expire_req = 0U;
     s_act_time_ms = 0U;
     s_act_arm_ms = 0U;
@@ -1074,6 +1050,8 @@ void PlsrRunControlStop(void)
     s_chain_freq = 0U;
     s_act_armed = 0U;
     s_act_expire_req = 0U;
+    s_act_cut_pending = 0U;
+    s_act_handoff_keep = 0U;
     s_wait_expire_req = 0U;
     s_act_time_ms = 0U;
     s_act_arm_ms = 0U;
@@ -1101,13 +1079,6 @@ uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz)
     f_end = s_accel_plan.f_end;
     PlsrRunControlPlanSeg(remain, s_cur_freq, new_tgt_hz, f_end,
                           s_run_accel_ms, s_run_decel_ms);
-    if (PlsrParamGetSeg(s_cur_seg)->wait_type == PLSR_WAIT_ACT)
-    {
-        uint32_t used = PlsrSignalIoGetMs() - s_act_arm_ms;
-        uint16_t left = (used < (uint32_t)s_act_time_ms) ?
-                        (uint16_t)((uint32_t)s_act_time_ms - used) : 1U;
-        PlsrRunControlApplyActTimeCap(left);
-    }
     s_target = (int32_t)remain;
     s_done = 0;
     s_seg_elapsed_ms = 0U;
@@ -1123,7 +1094,7 @@ void PlsrRunControlTickMs(void)
     if (s_act_expire_req != 0U)
     {
         s_act_expire_req = 0U;
-        if (s_state == RC_RUN)
+        if ((s_state == RC_RUN) || (s_act_handoff_keep != 0U))
         {
             PlsrRunControlActExpire();
         }
@@ -1208,29 +1179,25 @@ void PlsrRunControlOn1ms(void)
         s_seg_elapsed_ms++;
     }
 
-    /*
-     * ACT 到点：若加/匀/减三阶段已经走完，不要按当前频率切段，
-     * 剩余脉冲继续发完，由 AfterSegDone ※4 停表等到点。
-     */
     if ((s_act_armed != 0U) &&
         (PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) != 0U))
     {
-        uint32_t t_wave;
+        /*
+         * ACT 是运动控制条件，不参与曲线规划：加速、匀速、减速
+         * 任一阶段到期都在当前脉冲 UPDATE 边界切段。这样不会在
+         * 一个 PWM 周期中间改段，也不会把边界脉冲计到下一段。
+         */
+        s_act_armed = 0U;
+        s_act_cut_pending = 1U;
+    }
 
-        t_wave = s_accel_plan.t_decel_start_ms + s_accel_plan.t_dec_ms;
-        if ((s_accel_plan.t_dec_ms >= 1U) && (s_seg_elapsed_ms >= t_wave))
-        {
-            /* 三段已结束，等脉冲 ISR 把 s_done 走到 s_target */
-        }
-        else
-        {
-            s_act_armed = 0U;
-            s_act_expire_req = 1U;
-            return;
-        }
+    /* 保持 ACT 到期瞬间的频率，不让 1ms 曲线继续推进到下一个边界。 */
+    if (s_act_cut_pending != 0U)
+    {
+        return;
     }
 
-    if ((s_accel_plan.f_end == 0U) &&
+    if ((s_orig_end_zero != 0U) &&
         (s_accel_plan.mode != PLSR_ACCEL_LINEAR) &&
         (s_accel_plan.t_dec_ms >= 1U) &&
         (s_seg_elapsed_ms >=
@@ -1266,6 +1233,21 @@ void PlsrRunControlOnPulseIsr(void)
         s_acc_pulse--;
     }
 
+    /*
+     * ACT 到期后的切段只在脉冲边界执行。当前 UPDATE 已完成本段
+     * 最后一个完整脉冲，立即停表，任务再用当前频率衔接下一段。
+     */
+    if (s_act_cut_pending != 0U)
+    {
+        s_act_cut_pending = 0U;
+        PlsrPulseDriverStop();
+        s_pwm_on = 0U;
+        s_act_handoff_keep = 1U;
+        s_state = RC_IDLE;
+        s_act_expire_req = 1U;
+        return;
+    }
+
     if (s_done >= s_target)
     {
         PlsrRunControlAfterSegDone();