diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index dd2b4ee..82c505b 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/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); diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index 30e64a7..40b8a2f 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/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, diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index f9f9a66..d408a80 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/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)