From cf6828ac48ad392daf517f7180ff08d0a46cde0b Mon Sep 17 00:00:00 2001 From: hanyongwei <2043702190@qq.com> Date: Tue, 18 Aug 2026 08:24:32 +0800 Subject: [PATCH] =?UTF-8?q?=E7=94=A81ms=E8=AE=A1=E7=AE=97=E4=B8=80?= =?UTF-8?q?=E6=AC=A1=E6=96=B0=E9=A2=91=E7=8E=87=EF=BC=8C=E4=BD=86=E6=98=AF?= =?UTF-8?q?=E9=9C=80=E8=A6=81=E7=AD=89=E5=BE=85=E4=B8=8A=E4=B8=80=E4=B8=AA?= =?UTF-8?q?=E8=84=89=E5=86=B2=E5=8F=91=E5=AE=8C=EF=BC=8C=E6=96=B0=E7=9A=84?= =?UTF-8?q?=E5=8F=82=E6=95=B0=E6=89=8D=E4=BC=9A=E7=94=9F=E6=95=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: hanyongwei <2043702190@qq.com> --- plsr/accel_curve/plsr_accel_curve.c | 341 +++++++++++++------ plsr/accel_curve/plsr_accel_curve.h | 22 +- plsr/param/plsr_param.h | 4 +- plsr/pulse_driver/plsr_pulse_driver.c | 12 + plsr/run_control/plsr_run_control.c | 458 +++++++------------------- plsr/run_control/plsr_run_control.h | 4 +- 6 files changed, 391 insertions(+), 450 deletions(-) diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 8e552de..1fe62dc 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -211,8 +211,10 @@ static uint32_t PlsrAccelCurveLerp(uint32_t a, uint32_t b, uint32_t ratio_permil } /** - * 与运行时相同:f = f0+(f1-f0)*shape(t/T),t>=T 时为 f1。 - * shape 由 mode 决定;ISR 可用(O(1))。 + * 斜坡取频(µs 时间轴)。ISR 可用:O(1)。 + * + * 直线模式:f = f0 + (f1−f0) · t/T(64 位运算保证精度,每个点在 f-t 直线上)。 + * S / 正弦:f = f0 + (f1−f0) · shape(t/T)。 */ uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, uint32_t f1, @@ -226,6 +228,25 @@ uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, { return f1; } + + if (mode == PLSR_ACCEL_LINEAR) + { + /* + * 纯代数直线:f = f0 + (f1-f0)*t/T,用 64 位避免溢出/截断。 + * 不走 permille/Lerp,保证点在直线上。 + */ + if (f1 >= f0) + { + return f0 + (uint32_t)(((uint64_t)(f1 - f0) * (uint64_t)t_us) / (uint64_t)T_us); + } + else + { + uint32_t drop = (uint32_t)(((uint64_t)(f0 - f1) * (uint64_t)t_us) / (uint64_t)T_us); + return (drop < f0) ? (f0 - drop) : 0U; + } + } + + /* S / 正弦:shape(t/T) → Lerp */ r = (uint32_t)(((uint64_t)t_us * 1000ULL) / (uint64_t)T_us); if (r > 1000U) { @@ -236,7 +257,11 @@ uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, } /** - * 1ms 时基取频(与 PlsrRunControlOn1ms 一致)。 + * 斜坡取频(ms 接口;内部转 µs)。运行时相时间来自 TIM3 1ms 累加。 + */ +/** + * 斜坡取频(ms 接口,内部转 µs 调 FreqOnRampUs)。 + * 统一入口:直线/S/正弦均走 FreqOnRampUs,不再有额外特例分支。 */ uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0, uint32_t f1, @@ -244,62 +269,32 @@ uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0, uint32_t T_ms, PlsrAccelMode_e mode) { - uint32_t f; - if (T_ms < 1U) { - return (f1 >= 1U) ? f1 : 1U; - } - - if (mode == PLSR_ACCEL_LINEAR) - { - if ((f0 == 0U) && (f1 > 0U)) - { - uint32_t step = t_ms + 1U; - if (step >= T_ms) - { - return (f1 >= 1U) ? f1 : 1U; - } - f = (f1 * step) / T_ms; - if (f < 1U) - { - f = 1U; - } - return f; - } - if ((f1 == 0U) && (f0 > 0U)) - { - uint32_t step = t_ms + 1U; - uint32_t left; - if (step >= T_ms) - { - return 1U; - } - left = T_ms - step; - f = (f0 * left) / T_ms; - if (f < 1U) - { - f = 1U; - } - return f; - } - } - else if ((f0 == 0U) && (t_ms == 0U) && (f1 > 0U)) - { - f = f1 / T_ms; - if (f < 1U) - { - f = 1U; - } - return f; + return f1; } + return PlsrAccelCurveFreqOnRampUs(f0, f1, t_ms * 1000UL, T_ms * 1000UL, mode); +} - f = PlsrAccelCurveFreqOnRampUs(f0, f1, t_ms * 1000UL, T_ms * 1000UL, mode); - if (f < 1U) +/** + * 减速落地频率: + * - f_end>=1:落地即止速 + * - f_end==0:返回 0(停表)。不再用「对称首档」冒充止速—— + * 那一档常算成 1Hz,示波器上会变成脉冲发完后还在 1Hz 空转。 + */ +uint32_t PlsrAccelCurveLandFreq(uint32_t f_end, + uint32_t f_peak, + uint32_t T_ms, + PlsrAccelMode_e mode) +{ + (void)f_peak; + (void)T_ms; + (void)mode; + if (f_end >= 1U) { - f = 1U; + return f_end; } - return f; + return 0U; } /** @@ -308,6 +303,14 @@ uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0, * f0==0:t=0 曲线为 0;若强行按 1Hz 计整周期(~1s),加速时间常被第一拍吃光, * 表现为到不了目标频、曲线台阶/长拖尾。此处用 look-ahead 自洽点,且 ≥1Hz。 */ +/** + * 起速=0 时第一拍自洽频率。 + * + * 自洽条件:f 使得 FreqOnRampUs(f0, f1, 10^6/f, T_us) = f + * + * 直线模式(f0=0):解析解 f = sqrt(f1 * 10^6 / T_us)。 + * S/正弦:迭代(取两次平均收敛)。 + */ uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, uint32_t f1, uint32_t T_us, @@ -331,12 +334,40 @@ uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, return f; } - guess = (f1 >= 2U) ? (f1 / 2U) : 1U; - if (guess < 1U) + /* 直线 f0=0:f = sqrt(f1 * 1e6 / T_us),用整数牛顿法求 sqrt(val) */ + if ((mode == PLSR_ACCEL_LINEAR) && (f0 == 0U) && (f1 >= 1U)) { - guess = 1U; + uint64_t val = (uint64_t)f1 * 1000000ULL / (uint64_t)T_us; + uint32_t x; + if (val == 0ULL) + { + return 1U; + } + x = (uint32_t)val; + if (x > 10000U) + { + x = 10000U; + } + for (i = 0U; i < 20U; i++) + { + uint32_t x2 = (uint32_t)((x + (uint32_t)(val / (uint64_t)x)) / 2UL); + if (x2 == x || x2 == 0U) + { + break; + } + x = x2; + } + f = (x < 1U) ? 1U : x; + if (f > f1) + { + f = f1; + } + return f; } - for (i = 0U; i < 8U; i++) + + /* S/正弦:迭代求自洽点,用前后平均避免震荡 */ + guess = (f1 >= 4U) ? (f1 / 4U) : 1U; + for (i = 0U; i < 16U; i++) { per = (1000000UL + (guess / 2UL)) / guess; fnew = PlsrAccelCurveFreqOnRampUs(f0, f1, per, T_us, mode); @@ -348,7 +379,16 @@ uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, { break; } - guess = fnew; + /* 取平均避免震荡 */ + guess = (guess + fnew) / 2U; + if (guess < 1U) + { + guess = 1U; + } + } + if (guess > f1) + { + guess = f1; } return guess; } @@ -386,15 +426,24 @@ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, f = PlsrAccelCurveFreqOnRampUs(f_from, f_to, elapsed_us, T_us, mode); if (f < 1U) { - if ((elapsed_us == 0U) && (f_from == 0U)) + if ((elapsed_us == 0U) && (f_from == 0U) && (f_to >= 1U)) { f = PlsrAccelCurveFirstPulseFreq(f_from, f_to, T_us, mode); } + else if (f_to == 0U) + { + break; + } else { f = 1U; } } + if ((f_to == 0U) && (f <= 1U)) + { + n++; + break; + } elapsed_us += (1000000UL + (f / 2UL)) / f; n++; guard++; @@ -562,6 +611,15 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, return PlsrAccelCurveOnPath(f_cur, f_want, 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, @@ -612,6 +670,7 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, plan->const_n = 0U; plan->t_acc_ms = 0U; plan->t_dec_ms = 0U; + plan->t_decel_start_ms = 0U; plan->f_tgt = f_peak; plan->acc_n = 0U; plan->dec_n = 0U; @@ -619,11 +678,9 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, } /* - * 无独立段末终点(完成方式非末段:f_end == f_tgt): - * 脉冲不够时不 FitPeak、不减速,全程按原目标斜率加速; - * 脉冲用尽后由上层把当前频率链到下一段继续加。 - * 后续方式非末段 f_end=下一段目标,不走此分支。 - * 末段(f_end 为终止速度)不够时仍三角压峰。 + * f_end == f_tgt:本段无独立段末终点(仅加速到目标)。 + * 脉冲不够时不 FitPeak、不减速;梯形/三角只由 + * (f_cur,f_tgt,f_end,斜率,mode,总脉冲) 决定,与 wait 无关。 */ if ((f_end == f_tgt) && (acc_n > total_pulses)) { @@ -631,6 +688,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->acc_n = total_pulses; plan->dec_n = 0U; return; @@ -668,14 +726,18 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, else { acc_n = (total_pulses * acc_n) / sum; - if (acc_n >= total_pulses) - { - acc_n = total_pulses; - dec_n = 0U; - } - else + dec_n = total_pulses - acc_n; + if ((f_peak > f_end) && (dec_n == 0U) && (total_pulses > 1U)) { - dec_n = total_pulses - acc_n; + dec_n = 1U; + if (acc_n >= total_pulses) + { + acc_n = total_pulses - 1U; + } + else + { + acc_n = total_pulses - dec_n; + } } } plan->const_n = 0U; @@ -698,72 +760,157 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, 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)) + { + dec_n = 1U; + if (acc_n >= total_pulses) + { + 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); } -PlsrCurvePhase_e PlsrAccelCurvePhaseAt(const PlsrAccelPlan_t *plan, - uint32_t done_pulses) +/** + * 正向仿真 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) { - if (plan == (PlsrAccelPlan_t *)0) + 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 PLSR_CURVE_PHASE_CONST; + return 0U; } - if (done_pulses < plan->acc_n) + + elapsed_us = 0U; + n = 0U; + guard = 0U; + T_acc_us = t_acc_ms * 1000UL; + + while ((n < acc_n) && (guard < 2000000UL)) { - return PLSR_CURVE_PHASE_ACC; + if (t_acc_ms == 0U) + { + break; + } + f = PlsrAccelCurveFreqOnRampUs(f_cur, f_tgt, elapsed_us, T_acc_us, mode); + if (f < 1U) + { + if ((n == 0U) && (f_cur == 0U) && (f_tgt >= 1U)) + { + f = PlsrAccelCurveFirstPulseFreq(f_cur, f_tgt, T_acc_us, mode); + } + else if (f_tgt >= 1U) + { + f = 1U; + } + else + { + break; + } + } + elapsed_us += (1000000UL + (f / 2UL)) / f; + n++; + guard++; } - if (done_pulses < (plan->acc_n + plan->const_n)) + + f_run = (f_tgt >= 1U) ? f_tgt : 1U; + while ((n < (acc_n + const_n)) && (guard < 2000000UL)) { - return PLSR_CURVE_PHASE_CONST; + elapsed_us += (1000000UL + (f_run / 2UL)) / f_run; + n++; + guard++; } - return PLSR_CURVE_PHASE_DEC; + + return elapsed_us / 1000UL; } -uint32_t PlsrAccelCurveFreqAt(const PlsrAccelPlan_t *plan, - uint32_t done_pulses, - uint32_t phase_elapsed_ms) +uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, + uint32_t seg_elapsed_ms) { - uint32_t done = done_pulses; + uint32_t t; + uint32_t T_acc_us; + uint32_t t_dec; uint32_t ratio; - uint32_t T; if (plan == (PlsrAccelPlan_t *)0) { return 0U; } - if (done < plan->acc_n) + if ((plan->t_acc_ms > 0U) && (seg_elapsed_ms < plan->t_acc_ms) && + (plan->f_cur != plan->f_tgt)) { - T = plan->t_acc_ms; - if (T == 0U) + T_acc_us = plan->t_acc_ms * 1000UL; + if ((seg_elapsed_ms == 0U) && (plan->f_cur == 0U)) { - return PlsrAccelCurveClampFreq(plan->f_tgt); + return PlsrAccelCurveClampFreq( + PlsrAccelCurveFirstPulseFreq(plan->f_cur, plan->f_tgt, + T_acc_us, plan->mode)); } - ratio = PlsrAccelCurveShape(phase_elapsed_ms, T, plan->mode); return PlsrAccelCurveClampFreq( - PlsrAccelCurveLerp(plan->f_cur, plan->f_tgt, ratio)); + PlsrAccelCurveFreqOnRampUs(plan->f_cur, plan->f_tgt, + seg_elapsed_ms * 1000UL, + T_acc_us, plan->mode)); } - done -= plan->acc_n; - if (done < plan->const_n) + if (seg_elapsed_ms < plan->t_decel_start_ms) { return PlsrAccelCurveClampFreq(plan->f_tgt); } - if (plan->dec_n == 0U) + if ((plan->t_dec_ms == 0U) || (plan->f_tgt == plan->f_end)) { return PlsrAccelCurveClampFreq(plan->f_end); } - T = plan->t_dec_ms; - if (T == 0U) + + t_dec = seg_elapsed_ms - plan->t_decel_start_ms; + if (t_dec >= plan->t_dec_ms) { return PlsrAccelCurveClampFreq(plan->f_end); } - ratio = PlsrAccelCurveShape(phase_elapsed_ms, T, plan->mode); + + if (plan->mode == PLSR_ACCEL_LINEAR) + { + return PlsrAccelCurveClampFreq( + PlsrAccelCurveFreqOnRampUs(plan->f_tgt, plan->f_end, + t_dec * 1000UL, + plan->t_dec_ms * 1000UL, + plan->mode)); + } + + t = plan->t_dec_ms; + ratio = PlsrAccelCurveShape(t_dec, t, plan->mode); return PlsrAccelCurveClampFreq( PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio)); } diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index 61692d5..1c6fcf4 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -2,7 +2,7 @@ * @file plsr_accel_curve.h * @brief 预估加/匀/减脉冲;相内按时间走直线/S/正弦 * - * 相界按预估脉冲划分;预估与运行一致:shape(t/T) + 每脉冲改频、周期累加。 + * 1ms 改频:规划阶段反向算 t_decel_start_ms;运行时按段内绝对时间取频。 * 斜率按公共参数:K≈(默认速度−起/止速)/加减速时间;小频差时 T 可能 <1ms,无可见斜坡属正常。 */ #ifndef PLSR_ACCEL_CURVE_H @@ -20,6 +20,7 @@ typedef struct { uint32_t f_end; uint32_t t_acc_ms; uint32_t t_dec_ms; + uint32_t t_decel_start_ms; /* 反向规划:段内时间轴上进入减速的时刻 */ PlsrAccelMode_e mode; } PlsrAccelPlan_t; @@ -43,7 +44,6 @@ uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, /** * 与 TIM3 1ms 改频一致的斜坡取频(规划/运行共用)。 - * 直线起/止 0:离散台阶;其余走 shape。 */ uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0, uint32_t f1, @@ -60,6 +60,14 @@ uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, uint32_t T_us, PlsrAccelMode_e mode); +/** + * 减速落地频率:f_end>=1 即止速;f_end==0 返回 0(停表,不用 1Hz 冒充)。 + */ +uint32_t PlsrAccelCurveLandFreq(uint32_t f_end, + uint32_t f_peak, + uint32_t T_ms, + PlsrAccelMode_e mode); + /** 拟合斜坡时间(ms),使离散模型脉冲数尽量等于 max_pulses(任务上下文) */ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, uint32_t f_to, @@ -79,14 +87,10 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, uint32_t decel_ms, PlsrAccelMode_e mode); -PlsrCurvePhase_e PlsrAccelCurvePhaseAt(const PlsrAccelPlan_t *plan, - uint32_t done_pulses); - /** - * @param phase_elapsed_ms 当前相内已累计时间(脉冲周期累加) + * 1ms 改频:按段内绝对时间取频(相界由 t_decel_start_ms 决定,不用脉冲域)。 */ -uint32_t PlsrAccelCurveFreqAt(const PlsrAccelPlan_t *plan, - uint32_t done_pulses, - uint32_t phase_elapsed_ms); +uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, + uint32_t seg_elapsed_ms); #endif diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h index 271404c..77d84cb 100644 --- a/plsr/param/plsr_param.h +++ b/plsr/param/plsr_param.h @@ -102,8 +102,8 @@ typedef struct { uint16_t seg_count; uint16_t start_seg; /* 1-based */ uint32_t default_speed; /* 脉冲默认速度 Hz:定加减速斜率 K;段频为 0 时也作段目标 */ - uint32_t start_speed; /* 起速Hz;0=从静止按斜率爬升,输出≥1Hz */ - uint32_t end_speed; /* 止速Hz;0=规划减到0,输出收到≥1Hz再停 */ + uint32_t start_speed; /* 起速Hz;0=首拍跟曲线,非0=首拍即起速 */ + uint32_t end_speed; /* 止速Hz;0=末拍跟曲线后停表,非0=末拍即止速 */ uint16_t accel_ms; /* 默认速度加速时间 ms(定加速斜率) */ uint16_t decel_ms; /* 默认速度减速时间 ms(定减速斜率) */ } PlsrCfg_t; diff --git a/plsr/pulse_driver/plsr_pulse_driver.c b/plsr/pulse_driver/plsr_pulse_driver.c index 7abca76..af0d422 100644 --- a/plsr/pulse_driver/plsr_pulse_driver.c +++ b/plsr/pulse_driver/plsr_pulse_driver.c @@ -376,6 +376,11 @@ static void PlsrPulseDriverStartCommon(uint32_t freq_hz, uint8_t enable_update_i __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr); __HAL_TIM_SET_COUNTER(htim, 0U); + /* + * UG 把新 PSC/ARR/CCR 从影寄存器载入现行寄存器。 + * 它会置 UIF → 若先开 IT 会进一次假 ISR(多计一拍)。 + * 顺序:UG → 清 UIF → Start PWM → 开 IT。 + */ htim->Instance->EGR = TIM_EGR_UG; __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); @@ -386,6 +391,13 @@ static void PlsrPulseDriverStartCommon(uint32_t freq_hz, uint8_t enable_update_i (void)HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1); } + /* + * 清一次 UIF(Start 可能再触发 UG/UIF),然后才开中断。 + * 确保第一个真脉冲完成后才进 ISR,不多计。 + */ + __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); + __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); + if (enable_update_it != 0U) { __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE); diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index ff94797..c21cb7d 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -7,17 +7,16 @@ * 起始频率 f_from、终止频率 f_end、加速/减速时间(斜率基准) * 2) 段内处理:不感知 wait_type;只按 (f_from,f_tgt,f_end,accel,decel) * 做加速→匀速→减速(脉冲不够则三角) - * 3) 波形只跟斜坡计算:算 T、按相时间取频;上拍发完才更新频率。 - * ARR 预装载(ARPE)下必须提前一拍写入影寄存器,否则起速会多一拍、止速落不到末拍。 + * 3) 曲线估加/减速脉冲;匀速=总数−加速−减速。 + * 规划反向算 t_decel_start_ms;运行时 TIM3 1ms 按段内时间轴取频(直线为纯代数)。 + * 起止≠0:首拍起速、末拍止速;起止=0:首末跟曲线,段末停表。 * * 典型策略映射: - * - 完成方式:每段 f_end=止速(梯形/三角);WAIT 只改段后等待与下一段 f_from - * - WAIT时间/信号 + 后续:本段 f_end=止速;段后等待;下一段 f_from=起速 - * - 后续无缝:本段 f_end=下一段目标;不停表,f_from=当前频 + * - 完成方式:每段 f_end=止速;WAIT 只做段后等待与下一段 f_from(清链→起速) + * - 后续方式末段:f_end=止速;非末段无缝:f_end=下一段目标 + * - 梯形/三角仅由 f_from/f_tgt/f_end/斜率/mode/脉冲数决定,不读 wait_type * - * 【硬约束】脉冲 UPDATE ISR(PlsrRunControlOnPulseIsr)内禁止重计算: - * - 禁止 FitRampTimeMs / EstimatePulses 等 O(N) 或查表迭代 - * - 升降速:每脉冲 O(1) 取频改 ARR;TIM3 1ms 只服务 WAIT/ACT 时钟 + * 【运行节拍】TIM3 1ms:RefreshProfile 改频;脉冲 UPDATE ISR 只计 s_done。 */ #include "plsr_run_control.h" #include "plsr_path_plan.h" @@ -47,7 +46,6 @@ static volatile uint8_t s_busy; static volatile uint8_t s_forward; static volatile uint16_t s_cur_seg; static volatile uint32_t s_cur_freq; -static volatile uint32_t s_pending_freq; /* ARPE 影子频率,下一 UPDATE 才成为当前拍 */ static volatile int32_t s_done; static volatile int32_t s_target; static volatile int32_t s_acc_pulse; @@ -60,7 +58,7 @@ static uint8_t s_wait_is_signal; /* RC_WAIT_COND:1=等 WAIT 沿,0=等时 static PlsrAccelPlan_t s_accel_plan; static RunPhase_e s_phase; -static uint32_t s_phase_elapsed_us; /* 相内已过时间(按脉冲周期累加) */ +static uint32_t s_seg_elapsed_ms; /* 段内绝对时间(TIM3 1ms 累加) */ static uint32_t s_approach_from; static uint32_t s_decel_from; static uint8_t s_follow_cont; @@ -156,7 +154,6 @@ static void PlsrRunControlEnterPostWaitOrNext(void) s_chain_valid = 0U; s_chain_freq = 0U; s_cur_freq = 0U; - s_pending_freq = 0U; s_wait_is_signal = 0U; s_wait_deadline_ms = PlsrSignalIoDeadlineFromNow(wms); s_state = RC_WAIT_COND; @@ -170,7 +167,6 @@ static void PlsrRunControlEnterPostWaitOrNext(void) s_chain_valid = 0U; s_chain_freq = 0U; s_cur_freq = 0U; - s_pending_freq = 0U; PlsrSignalIoClearEdges(); s_wait_is_signal = 1U; s_state = RC_WAIT_COND; @@ -207,23 +203,25 @@ static void PlsrRunControlAfterSegDone(void) if (cfg->send_mode == PLSR_SEND_COMPLETE) { - s_chain_freq = s_cur_freq; + /* 段正常结束:下一段起速 = 本段止速 f_end,不能用峰值 s_cur_freq */ + s_chain_freq = s_accel_plan.f_end; s_chain_valid = 1U; } if (keep_pwm != 0U) { - /* 后续同向且无门禁:不停表直接 BeginSeg */ + /* 后续同向且无门禁:不停表直接 BeginSeg(仅衔接策略,不影响本段波形) */ s_follow_cont = 1U; } else { PlsrPulseDriverStop(); + PlsrPulseDriverClearOnePulseStop(); s_follow_cont = 0U; s_pwm_on = 0U; + s_cur_freq = 0U; if (cfg->send_mode == PLSR_SEND_FOLLOW) { - s_cur_freq = 0U; s_chain_valid = 0U; } } @@ -257,7 +255,6 @@ static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg) static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, PlsrSegCallIo_t *io) { - PlsrSeg_t *seg = PlsrParamGetSeg(seg_0); PlsrCfg_t *cfg = PlsrParamGetCfg(); int16_t next0; uint8_t end_at_stop; /* 1:本段终止频率用公共止速 */ @@ -282,10 +279,10 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, } /* - * ---------- 终止频率:策略层决定,段内不感知 wait ---------- - * 完成方式:每段都止于公共止速(与上位机预览一致;段间靠链频/WAIT 决定下一段 f_from) - * WAIT时间/信号 或 真末段:止速 - * 后续无缝:下一段目标 + * ---------- 终止频率:仅由发送模式 + 是否有下一段决定 ---------- + * 完成方式 / 真末段:公共止速 + * 后续方式非末段:下一段目标(无缝衔接) + * 不读 wait_type——等待只在段后 EnterPostWaitOrNext 处理 */ next0 = PlsrPathPlanNextSeg(seg_0); end_at_stop = (next0 < 0) ? 1U : 0U; @@ -294,11 +291,6 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, { end_at_stop = 1U; } - else if ((seg->wait_type == PLSR_WAIT_TIME) || - (seg->wait_type == PLSR_WAIT_SIGNAL)) - { - end_at_stop = 1U; - } if (end_at_stop != 0U) { @@ -306,7 +298,7 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, return 1U; } - /* 仅后续方式 + 无缝:过渡到下一段目标 */ + /* 后续方式 + 有下一段:过渡到下一段目标 */ { PlsrSeg_t *nseg = PlsrParamGetSeg((uint16_t)next0); if (PlsrRunControlGetSegTargetFreq(nseg->freq_hz, @@ -410,28 +402,14 @@ static uint32_t PlsrRunControlClampSpeed(uint32_t spd) return spd; } -/** 运行输出下限:参数起/止速为 0 时曲线可算到 0,TIM 最低按 1Hz 输出 */ -static uint32_t PlsrRunControlOutFreq(uint32_t f) -{ - if (f < 1U) - { - return 1U; - } - return f; -} - +/** 运行输出:0=停表;1ms 节拍直接 SetFreq / Start */ static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) { - /* - * 运行中只写预装载(下一拍生效),避免直接改 ARR 出毛刺多计脉冲。 - * s_cur_freq = 当前正在跑的拍;s_pending_freq = 已写入影子、下一拍的频率。 - */ if (profile_freq == 0U) { PlsrPulseDriverStop(); s_pwm_on = 0U; s_cur_freq = 0U; - s_pending_freq = 0U; return; } @@ -439,19 +417,16 @@ static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) { PlsrPulseDriverStart(profile_freq); s_pwm_on = 1U; - s_cur_freq = profile_freq; - s_pending_freq = profile_freq; } - else + else if (profile_freq != s_cur_freq) { - s_pending_freq = profile_freq; - PlsrPulseDriverRequestFreq(profile_freq); + PlsrPulseDriverSetFreq(profile_freq); } + s_cur_freq = profile_freq; } static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq) { - /* 相时间由 ISR 累加;切入减速时在 EnterDecel 里清零 */ s_phase = ph; if (ph == PH_APPROACH) { @@ -461,43 +436,10 @@ static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq) { s_decel_from = anchor_freq; } -} - -static uint32_t PlsrRunControlPeriodUs(uint32_t f_hz) -{ - if (f_hz < 1U) - { - f_hz = 1U; - } - return (1000000UL + (f_hz / 2UL)) / f_hz; -} - -static uint32_t PlsrRunControlFreqOnRampMs(uint32_t f0, uint32_t f1, - uint32_t t_ms, uint32_t T_ms, - PlsrAccelMode_e mode) -{ - return PlsrRunControlOutFreq( - PlsrAccelCurveFreqOnRampMs(f0, f1, t_ms, T_ms, mode)); -} - -/** - * 斜坡时间短于「from 频率一拍周期」:该拍结束后相时间已越过 T, - * 应直接取相终点频率。 - */ -static uint8_t PlsrRunControlRampDoneInOnePulse(uint32_t from_hz, uint32_t T_ms) -{ - uint32_t per_ms; - - if (T_ms < 1U) + else if (ph == PH_CONST) { - return 1U; - } - if (from_hz < 1U) - { - from_hz = 1U; + (void)anchor_freq; } - per_ms = (1000UL + from_hz - 1UL) / from_hz; - return (T_ms <= per_ms) ? 1U : 0U; } static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) @@ -517,7 +459,6 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) old_t_dec = s_accel_plan.t_dec_ms; PlsrRunControlEnterPhase(PH_DECEL, from_hz); - s_phase_elapsed_us = 0U; f_end = s_accel_plan.f_end; if (remain_pulses == 0U) @@ -536,17 +477,18 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) } /* - * 规划阶段已按 dec_n 拟合过 T(含 S/正弦)且剩余脉冲仍等于规划值: - * 直接沿用,避免 ISR 内 O(1) 平均公式把形状时间改歪。 - * 脉冲被改频/截断等导致 remain 变化时,再走下面 O(1) 回退。 + * 规划阶段已按斜率算好 t_dec:优先沿用,避免 ISR 内改歪形状。 + * 止速=0:始终用斜率时间;时间走完即停,不按剩余脉冲把 T 拉长 + * (拉长会在尾部堆 1Hz 空耗)。 */ - if ((old_t_dec > 0U) && (old_dec_n > 0U) && (remain_pulses == old_dec_n)) + if ((old_t_dec > 0U) && (old_dec_n > 0U) && + ((remain_pulses == old_dec_n) || (f_end == 0U))) { s_accel_plan.t_dec_ms = old_t_dec; return; } - /* 参数斜率决定的理想减速时间(用本段策略解析的 accel/decel) */ + /* 参数斜率决定的理想减速时间(本段 accel_ms/decel_ms) */ df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz); if (f_end > from_hz) { @@ -577,11 +519,16 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) } } + if (f_end == 0U) + { + s_accel_plan.t_dec_ms = (t_slope > 0U) ? t_slope : + ((old_t_dec > 0U) ? old_t_dec : 1U); + return; + } + /* - * 注意:本函数会在脉冲 UPDATE ISR 里调用,禁止再跑 FitRampTimeMs/ - * EstimatePulses 那种 O(N) 递推。段脉冲多时会卡死 ISR,定时器继续 - * 溢出 → 示波器上最后一段多出若干脉冲(500 准、5000 多 6~7 个)。 - * 这里只用 O(1) 平均公式拟合 T。 + * 止速≠0:ISR 内禁止 FitRamp/EstimatePulses;用 O(1) 平均公式。 + * 脉冲被截断导致 remain 变化时才走这里。 */ sum_f = from_hz + f_end; if (sum_f == 0U) @@ -611,163 +558,45 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) } } -static uint32_t PlsrRunControlCalcOutFreq(uint32_t elapsed_us, uint32_t remain) +static void PlsrRunControlRefreshProfile(uint8_t do_start) { uint32_t next; - uint32_t T_ms; - const PlsrAccelPlan_t *p = &s_accel_plan; /* - * 止速段:剩余脉冲进入 dec_n 时切入减速(加速相也可切,避免脉冲被加速吃光)。 + * 1ms 改频:段内统一时间轴;t_decel_start_ms 由规划反向算出, + * 减速为 f_tgt→f_end 直线,自然落到止速,末拍不再强行对齐。 */ - if ((p->dec_n > 0U) && (remain <= p->dec_n) && - (p->f_end != p->f_tgt) && - ((s_phase == PH_CONST) || (s_phase == PH_APPROACH))) - { - PlsrRunControlEnterDecel(s_cur_freq, remain); - } - - if (s_phase == PH_APPROACH) + next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms); + if (next < 1U) { - T_ms = p->t_acc_ms; - if (s_approach_from == p->f_tgt) + if ((s_accel_plan.f_cur == 0U) && (s_seg_elapsed_ms < 1U)) { - next = p->f_tgt; - PlsrRunControlEnterPhase(PH_CONST, next); - } - else if ((T_ms < 1U) || (elapsed_us >= (T_ms * 1000UL))) - { - next = p->f_tgt; - PlsrRunControlEnterPhase(PH_CONST, next); - if ((p->dec_n > 0U) && (remain <= p->dec_n)) + 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) { - PlsrRunControlEnterDecel(next, remain); - if ((remain <= 1U) || - (PlsrRunControlRampDoneInOnePulse(s_decel_from, - p->t_dec_ms) != 0U)) - { - next = p->f_end; - } - else - { - next = PlsrRunControlFreqOnRampMs(s_decel_from, p->f_end, - 0U, - p->t_dec_ms, p->mode); - } + next = 1U; } } - else - { - next = PlsrRunControlFreqOnRampMs(s_approach_from, p->f_tgt, - elapsed_us / 1000UL, - T_ms, p->mode); - } - } - else if (s_phase == PH_DECEL) - { - T_ms = p->t_dec_ms; - if ((remain <= 1U) || - (PlsrRunControlRampDoneInOnePulse(s_decel_from, T_ms) != 0U) || - ((T_ms >= 1U) && (elapsed_us >= (T_ms * 1000UL)))) - { - next = p->f_end; - } - else + else if (next == 0U) { - next = PlsrRunControlFreqOnRampMs(s_decel_from, p->f_end, - elapsed_us / 1000UL, - T_ms, p->mode); - } - } - else - { - next = p->f_tgt; - if ((p->dec_n > 0U) && (remain <= p->dec_n)) - { - PlsrRunControlEnterDecel(next, remain); - if ((remain <= 1U) || - (PlsrRunControlRampDoneInOnePulse(s_decel_from, - p->t_dec_ms) != 0U)) + if (s_done < s_target) { - next = p->f_end; + s_target = s_done + 1; } - else + if (PlsrRunControlWillFollowKeep(s_cur_seg) == 0U) { - next = PlsrRunControlFreqOnRampMs(s_decel_from, p->f_end, - 0U, - p->t_dec_ms, p->mode); + PlsrPulseDriverArmOnePulseStop(); } + return; } } - return PlsrRunControlOutFreq(next); -} - -/** - * 开跑:现行 ARR=第1拍,影寄存器提前装第2拍。 - * STM32 ARPE:UPDATE 里写的 ARR 要到再下一拍才生效;若不预装,起速会打两拍、 - * 止速永远落不到最后一拍。 - */ -static void PlsrRunControlStartWithPreload(void) -{ - uint32_t f1; - uint32_t f2; - uint32_t e1; - uint32_t rem1; - uint32_t rem2; - - rem1 = (uint32_t)s_target; - if (rem1 < 1U) - { - rem1 = 1U; - } - - s_phase_elapsed_us = 0U; - f1 = PlsrRunControlCalcOutFreq(0U, rem1); - - if (s_target <= 1) + if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U)) { - PlsrRunControlApplyOutFreq(f1, 1U); - return; - } - - e1 = PlsrRunControlPeriodUs(f1); - rem2 = (uint32_t)(s_target - 1); - f2 = PlsrRunControlCalcOutFreq(e1, rem2); - - /* ISR 从已完成拍累加相时间,开跑时尚未完成任何拍 */ - s_phase_elapsed_us = 0U; - - PlsrRunControlApplyOutFreq(f1, 1U); - s_pending_freq = f2; - PlsrPulseDriverRequestFreq(f2); - PlsrPulseDriverApplyPending(); -} - -static void PlsrRunControlRefreshProfile(uint8_t do_start) -{ - uint32_t remain; - uint32_t next; - - if (do_start != 0U) - { - PlsrRunControlStartWithPreload(); - return; - } - - if (s_done < s_target) - { - remain = (uint32_t)(s_target - s_done); - } - else - { - remain = 0U; - } - - next = PlsrRunControlCalcOutFreq(s_phase_elapsed_us, remain); - if ((next != s_pending_freq) || (s_pwm_on == 0U)) - { - PlsrRunControlApplyOutFreq(next, 0U); + PlsrRunControlApplyOutFreq(next, do_start); } } @@ -783,6 +612,10 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, s_run_accel_ms = accel_ms; s_run_decel_ms = decel_ms; + /* + * 曲线估 acc_n / dec_n,const_n = total − acc − dec。 + * 起止≠0:运行时首拍=起速、末拍=止速;起止=0:首末跟曲线,段末停表。 + */ PlsrAccelCurvePlan(&s_accel_plan, total, f_from, @@ -795,12 +628,13 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, decel_ms, cfg->accel_mode); - /* 本段频率范围锁 PSC,升降过程只改 ARR。 - * - * 注意:不要为了“起速/止速=0”把下限锁到 1Hz。 - * 1Hz 会迫使 PSC 极大,目标频附近 ARR 挤成一团,加减速在示波器上变成 - * 方波或“斜坡半截再直跳”。下限取 max(峰值/20, 20Hz),末了再停表到 0。 + /* + * 1ms 改频模式:运行时按 TIM3 毫秒时间轴取频。 + * 禁止在这里用脉冲递推模型二次拟合 T(FitRamp),否则规划与运行模型不一致, + * 会出现中段拐折/扭曲、段末落不到设定 f_end。 */ + + /* 本段频率范围锁 PSC,升降只改 ARR;跨度过大则 LockPscRange 自动不锁 */ f_lo = s_accel_plan.f_tgt; f_hi = s_accel_plan.f_tgt; if (f_from > f_hi) @@ -814,17 +648,9 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, f_lo = f_from; } } - else if (s_accel_plan.f_tgt >= 1U) + else { - f_lo = s_accel_plan.f_tgt / 20U; - if (f_lo < 20U) - { - f_lo = 20U; - } - if (f_lo > s_accel_plan.f_tgt) - { - f_lo = s_accel_plan.f_tgt; - } + f_lo = 1U; } if (s_accel_plan.f_end > f_hi) { @@ -837,16 +663,11 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, f_lo = s_accel_plan.f_end; } } - else if (s_accel_plan.f_tgt >= 1U) + else { - uint32_t flo2 = s_accel_plan.f_tgt / 20U; - if (flo2 < 20U) - { - flo2 = 20U; - } - if (flo2 < f_lo) + if (f_lo > 1U) { - f_lo = flo2; + f_lo = 1U; } } if (f_lo < 1U) @@ -877,22 +698,26 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, * 脉冲数由 EstimatePulses 决定;运行时按相时间取频, * 一拍周期盖住 T 时自然落到相终点。 */ + /* + * 纯减速段:已在目标速(或更高)且整段预算不超过 dec_n。 + * 起速 < 目标速 时必须先 APPROACH(三角/profile),不能一上来 DECEL。 + */ if ((s_accel_plan.dec_n >= total) && (total > 0U) && - (s_accel_plan.f_end != s_accel_plan.f_tgt)) + (s_accel_plan.f_end != s_accel_plan.f_tgt) && + (f_from >= s_accel_plan.f_tgt)) { PlsrRunControlEnterDecel(f_from, total); } else if (f_from == s_accel_plan.f_tgt) { PlsrRunControlEnterPhase(PH_CONST, f_from); - s_phase_elapsed_us = 0U; } else { PlsrRunControlEnterPhase(PH_APPROACH, f_from); - s_phase_elapsed_us = 0U; } + s_seg_elapsed_ms = 0U; } static void PlsrRunControlFinishAll(void) @@ -903,7 +728,6 @@ static void PlsrRunControlFinishAll(void) s_busy = 0U; s_state = RC_IDLE; s_cur_freq = 0U; - s_pending_freq = 0U; s_pwm_on = 0U; s_follow_cont = 0U; s_chain_valid = 0U; @@ -995,14 +819,9 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0) if (s_follow_cont != 0U) { - uint32_t f1; - PlsrPulseDriverClearOnePulseStop(); - /* PWM 仍在跑:只预装下一拍;再下一拍由 ISR look-ahead 补 */ - s_phase_elapsed_us = 0U; - f1 = PlsrRunControlCalcOutFreq(0U, (uint32_t)s_target); - PlsrRunControlApplyOutFreq(f1, 0U); - PlsrPulseDriverApplyPending(); + s_seg_elapsed_ms = 0U; + PlsrRunControlRefreshProfile(0U); s_state = RC_RUN; s_follow_cont = 0U; PlsrRunControlArmActExtOnPulseStart(); @@ -1022,7 +841,6 @@ void PlsrRunControlInit(void) s_forward = 1U; s_cur_seg = 0U; s_cur_freq = 0U; - s_pending_freq = 0U; s_pwm_on = 0U; s_done = 0; s_target = 0; @@ -1033,7 +851,7 @@ void PlsrRunControlInit(void) s_run_accel_ms = 0U; s_run_decel_ms = 0U; s_phase = PH_CONST; - s_phase_elapsed_us = 0U; + s_seg_elapsed_ms = 0U; s_approach_from = 0U; s_decel_from = 0U; s_act_armed = 0U; @@ -1079,7 +897,6 @@ void PlsrRunControlStop(void) s_busy = 0U; s_state = RC_IDLE; s_cur_freq = 0U; - s_pending_freq = 0U; s_pwm_on = 0U; s_follow_cont = 0U; s_chain_valid = 0U; @@ -1111,6 +928,7 @@ uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz) s_run_accel_ms, s_run_decel_ms); s_target = (int32_t)remain; s_done = 0; + s_seg_elapsed_ms = 0U; PlsrRunControlRefreshProfile(0U); return 1U; } @@ -1124,7 +942,8 @@ void PlsrRunControlTickMs(void) if (PlsrRunControlOsTimeReached(s_dir_deadline) != 0U) { s_state = RC_RUN; - PlsrRunControlStartWithPreload(); + s_seg_elapsed_ms = 0U; + PlsrRunControlRefreshProfile(1U); PlsrRunControlArmActExtOnPulseStart(); } return; @@ -1132,7 +951,7 @@ void PlsrRunControlTickMs(void) if (s_state == RC_RUN) { - /* 加减速频率改由 PlsrRunControlOn1ms(TIM3) 更新;此处只处理 ACT/EXT */ + /* 波形改频在 TIM3 On1ms;此处只处理 ACT/EXT */ seg = PlsrParamGetSeg(s_cur_seg); if ((s_act_armed != 0U) && @@ -1171,31 +990,48 @@ void PlsrRunControlTickMs(void) } /** - * TIM3 1ms:仅推进 WAIT/ACT 用的毫秒钟(在 signal_io 里 ++s_ms)。 - * 升降速改频改回每脉冲,避免低频下 1ms 改频把斜坡拉歪、减速只剩 1 拍。 + * TIM3 1ms:段内时间累加 + 按时间轴 RefreshProfile 改频。 */ void PlsrRunControlOn1ms(void) { - /* 有意留空:段波形不在 1ms 里改 */ + if (s_state != RC_RUN) + { + return; + } + + if (s_seg_elapsed_ms < 0xFFFFFFFFUL) + { + s_seg_elapsed_ms++; + } + + if ((s_accel_plan.f_end == 0U) && + (s_accel_plan.mode != PLSR_ACCEL_LINEAR) && + (s_accel_plan.t_dec_ms >= 1U) && + (s_seg_elapsed_ms >= + (s_accel_plan.t_decel_start_ms + s_accel_plan.t_dec_ms))) + { + if (s_done < s_target) + { + s_target = s_done + 1; + } + if (PlsrRunControlWillFollowKeep(s_cur_seg) == 0U) + { + PlsrPulseDriverArmOnePulseStop(); + } + return; + } + + PlsrRunControlRefreshProfile(0U); } +/** 脉冲 UPDATE:只累计 s_done / s_acc_pulse,不改频 */ void PlsrRunControlOnPulseIsr(void) { - uint32_t completed_freq; - uint32_t running_freq; - uint32_t look_elapsed; - uint32_t rem_prog; - uint32_t next; - uint32_t saved_elapsed; - uint8_t entered_decel; - if (s_state != RC_RUN) { return; } - /* 本拍刚结束:现行频率仍是完成拍的频率(pending 是影里给下一拍的) */ - completed_freq = s_cur_freq; s_done++; if (s_forward != 0U) { @@ -1212,71 +1048,13 @@ void PlsrRunControlOnPulseIsr(void) return; } - /* - * 本 UPDATE 已把影寄存器装进现行 ARR:正在跑的是原 pending。 - * ARPE 下此刻再写影寄存器,要到「再下一拍」才生效 → 必须 look-ahead。 - */ - running_freq = s_pending_freq; - s_cur_freq = running_freq; - - if (((s_phase == PH_APPROACH) || (s_phase == PH_DECEL)) && - (completed_freq >= 1U)) - { - uint32_t period_us = PlsrRunControlPeriodUs(completed_freq); - if (s_phase_elapsed_us < (0xFFFFFFFFUL - period_us)) - { - s_phase_elapsed_us += period_us; - } - else - { - s_phase_elapsed_us = 0xFFFFFFF0UL; - } - } - if (s_done >= (s_target - 1)) { - /* 最后一拍已在跑(开跑预装或上次 look-ahead 已写好),不再改频 */ if (PlsrRunControlWillFollowKeep(s_cur_seg) == 0U) { PlsrPulseDriverArmOnePulseStop(); } - return; - } - - /* 预装「再下一拍」= 第 (s_done+2) 拍 */ - look_elapsed = s_phase_elapsed_us; - if ((s_phase == PH_APPROACH) || (s_phase == PH_DECEL)) - { - uint32_t add = PlsrRunControlPeriodUs(running_freq); - if (look_elapsed < (0xFFFFFFFFUL - add)) - { - look_elapsed += add; - } - else - { - look_elapsed = 0xFFFFFFF0UL; - } - } - rem_prog = (uint32_t)(s_target - s_done - 1); - - saved_elapsed = s_phase_elapsed_us; - entered_decel = (s_phase == PH_DECEL) ? 1U : 0U; - next = PlsrRunControlCalcOutFreq(look_elapsed, rem_prog); - if ((entered_decel == 0U) && (s_phase == PH_DECEL)) - { - /* 本次 look-ahead 刚切入减速:相时间从 0 计 */ - s_phase_elapsed_us = 0U; - } - else - { - s_phase_elapsed_us = saved_elapsed; - } - - if ((next != s_pending_freq) || (s_pwm_on == 0U)) - { - PlsrRunControlApplyOutFreq(next, 0U); } - PlsrPulseDriverApplyPending(); } uint8_t PlsrRunControlIsBusy(void) diff --git a/plsr/run_control/plsr_run_control.h b/plsr/run_control/plsr_run_control.h index d707b8e..0c32897 100644 --- a/plsr/run_control/plsr_run_control.h +++ b/plsr/run_control/plsr_run_control.h @@ -12,8 +12,8 @@ uint8_t PlsrRunControlStart(uint16_t start_seg_1based); void PlsrRunControlStop(void); uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz); void PlsrRunControlTickMs(void); -void PlsrRunControlOn1ms(void); /* TIM3 节拍;升降速改在脉冲 ISR */ -void PlsrRunControlOnPulseIsr(void); +void PlsrRunControlOn1ms(void); /* TIM3 1ms:改频 + WAIT/ACT 时基 */ +void PlsrRunControlOnPulseIsr(void); /* 脉冲 UPDATE:只计脉冲 */ uint8_t PlsrRunControlIsBusy(void); int32_t PlsrRunControlGetAccPulse(void); void PlsrRunControlClearAccPulse(void);