diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 4dcc1c9..808ca27 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -6,6 +6,7 @@ * 否则示波器上变成方波，三种模式完全无法区分）。 * @note：目前脉冲输出在低频时和高频时均有毛刺，需要优化 + * 已解决，原因是逻辑分析仪采样率不够，导致采样点减少，波形有毛刺。 */ #include "plsr_accel_curve.h" @@ -93,13 +94,94 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from, return t; } +/** 直线：进度千分比 → 形状千分比 */ +static uint32_t PlsrAccelCurveShapeLinearPermille(uint32_t u) +{ + return (u > 1000U) ? 1000U : u; +} + + +/** + * 七段一侧 1:2:1：头尾更圆、中间更陡（相对直线可辨） + * u∈[0,250]/[750,1000] 为抛物线圆角，中间线性陡段。 + */ +static uint32_t PlsrAccelCurveShapeS7Permille(uint32_t u) +{ + uint32_t d; + uint32_t p; + + if (u >= 1000U) + { + return 1000U; + } + if (u <= 250UL) + { + p = (8UL * u * u) / (3UL * 1000UL); + } + else if (u <= 750UL) + { + p = 167UL + (666UL * (u - 250UL)) / 500UL; + } + else + { + d = 1000UL - u; + p = 1000UL - (8UL * d * d) / (3UL * 1000UL); + } + return (p > 1000UL) ? 1000UL : p; +} + +/** O(1)：时间进度千分比 → 曲线形状千分比 */ +static uint32_t PlsrAccelCurveShapePermille(uint32_t u, PlsrAccelMode_e mode) +{ + if (mode == PLSR_ACCEL_S) + { + return PlsrAccelCurveShapeS7Permille(u); + } + if (mode == PLSR_ACCEL_SINE) + { + //return PlsrAccelCurveShapeSinePermille(u); + } + return PlsrAccelCurveShapeLinearPermille(u); +} + +static uint32_t PlsrAccelCurveShape(uint32_t t_ms, uint32_t T_ms, PlsrAccelMode_e mode) +{ + uint32_t u; + + if ((T_ms == 0U) || (t_ms >= T_ms)) + { + return 1000U; + } + u = (t_ms * 1000UL) / T_ms; + if (u > 1000U) + { + u = 1000U; + } + return PlsrAccelCurveShapePermille(u, mode); +} + +static uint32_t PlsrAccelCurveLerp(uint32_t a, uint32_t b, uint32_t ratio_permille) +{ + if (ratio_permille >= 1000U) + { + return b; + } + if (b >= a) + { + return a + ((b - a) * ratio_permille) / 1000UL; + } + return a - ((a - b) * ratio_permille) / 1000UL; +} + /** - * 与运行时相同：f(t)=f0+(f1-f0)*t/T（µs），t>=T 时为 f1。 + * 与运行时相同：f = f0+(f1-f0)*shape(t/T)，t>=T 时为 f1。 + * shape 由 mode 决定；ISR 可用（O(1)）。 */ -static uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, - uint32_t f1, - uint32_t t_us, - uint32_t T_us) +uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, + uint32_t f1, + uint32_t t_us, + uint32_t T_us, + PlsrAccelMode_e mode) { uint32_t r; @@ -112,21 +194,18 @@ static uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, { r = 1000U; } - if (f1 >= f0) - { - return f0 + ((f1 - f0) * r) / 1000UL; - } - return f0 - ((f0 - f1) * r) / 1000UL; + r = PlsrAccelCurveShapePermille(r, mode); + return PlsrAccelCurveLerp(f0, f1, r); } /** - * 直线加减速、脉冲起点锁频时的精确脉冲数（与 ISR 累加方式一致）： - * t=0 取 f0，周期 1/f，再取 f(t)……直到 t>=T。 - * 不用平均/加权经验公式，改频率后仍与运行时一致。 + * 脉冲起点锁频递推脉冲数（与 ISR 一致）；f(t) 按 mode 形状。 + * 仅任务/规划上下文；禁止进脉冲 ISR。 */ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, uint32_t f_to, - uint32_t t_ms) + uint32_t t_ms, + PlsrAccelMode_e mode) { uint32_t T_us; uint32_t elapsed_us; @@ -146,7 +225,7 @@ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, while ((elapsed_us < T_us) && (guard < 2000000UL)) { - f = PlsrAccelCurveFreqOnRampUs(f_from, f_to, elapsed_us, T_us); + f = PlsrAccelCurveFreqOnRampUs(f_from, f_to, elapsed_us, T_us, mode); if (f < 1U) { f = 1U; @@ -160,14 +239,13 @@ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, } /** - * 拟合斜坡时间 T：使离散直线模型下的脉冲数尽量等于 max_pulses。 - * （旧逻辑取「Estimate<=max 的最大 T / 优先 t_prefer」，脉冲有余时会提前到终点再持平， - * 减速末端出现一段平肩/弯曲。） + * 拟合斜坡时间 T（任务上下文）；使离散模型脉冲数尽量等于 max_pulses。 */ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, uint32_t f_to, uint32_t max_pulses, - uint32_t t_prefer_ms) + uint32_t t_prefer_ms, + PlsrAccelMode_e mode) { uint32_t lo; uint32_t hi; @@ -188,10 +266,9 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, return (t_prefer_ms > 0U) ? t_prefer_ms : 1U; } - /* 放大上界直到脉冲数够到 max_pulses（或封顶） */ hi = (t_prefer_ms > 0U) ? t_prefer_ms : 1U; guard = 0U; - n = PlsrAccelCurveEstimatePulses(f_from, f_to, hi); + n = PlsrAccelCurveEstimatePulses(f_from, f_to, hi, mode); while ((n < max_pulses) && (hi < 120000U) && (guard < 24U)) { if (hi > (120000U / 2U)) @@ -202,19 +279,19 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, { hi = hi * 2U; } - n = PlsrAccelCurveEstimatePulses(f_from, f_to, hi); + n = PlsrAccelCurveEstimatePulses(f_from, f_to, hi, mode); guard++; } lo = 1U; best = (t_prefer_ms > 0U) ? t_prefer_ms : hi; - n = PlsrAccelCurveEstimatePulses(f_from, f_to, best); + n = PlsrAccelCurveEstimatePulses(f_from, f_to, best, mode); best_err = (n > max_pulses) ? (n - max_pulses) : (max_pulses - n); while (lo <= hi) { mid = lo + ((hi - lo) / 2UL); - n = PlsrAccelCurveEstimatePulses(f_from, f_to, mid); + n = PlsrAccelCurveEstimatePulses(f_from, f_to, mid, mode); err = (n > max_pulses) ? (n - max_pulses) : (max_pulses - n); if ((err < best_err) || ((err == best_err) && (t_prefer_ms > 0U) && @@ -245,104 +322,6 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, return best; } -static uint32_t PlsrAccelCurveLerp(uint32_t a, uint32_t b, uint32_t ratio_permille) -{ - if (ratio_permille >= 1000U) - { - return b; - } - if (b >= a) - { - return a + ((b - a) * ratio_permille) / 1000UL; - } - return a - ((a - b) * ratio_permille) / 1000UL; -} - -/** 直线：f 对时间线性 */ -static uint32_t PlsrAccelCurveShapeLinear(uint32_t t_ms, uint32_t T_ms) -{ - if ((T_ms == 0U) || (t_ms >= T_ms)) - { - return 1000U; - } - return (t_ms * 1000UL) / T_ms; -} - -///** 正弦缓入缓出 */ -//static uint32_t PlsrAccelCurveShapeSine(uint32_t t_ms, uint32_t T_ms) -//{ -// uint32_t x; -// uint32_t y; -// uint32_t d; -// -// if ((T_ms == 0U) || (t_ms >= T_ms)) -// { -// return 1000U; -// } -// x = (t_ms * 1000UL) / T_ms; -// if (x <= 500UL) -// { -// y = (2UL * x * x) / 1000UL; -// } -// else -// { -// d = 1000UL - x; -// y = 1000UL - (2UL * d * d) / 1000UL; -// } -// if (y > 1000UL) -// { -// y = 1000UL; -// } -// return y; -//} -// -///** -// * 七段一侧 1:2:1：相对直线，头尾更圆、中间更陡 -// */ -//static uint32_t PlsrAccelCurveShapeS7(uint32_t t_ms, uint32_t T_ms) -//{ -// uint32_t u; -// uint32_t d; -// uint32_t p; -// -// if ((T_ms == 0U) || (t_ms >= T_ms)) -// { -// return 1000U; -// } -// u = (t_ms * 1000UL) / T_ms; -// if (u <= 250UL) -// { -// p = (8UL * u * u) / (3UL * 1000UL); -// } -// else if (u <= 750UL) -// { -// p = 167UL + (666UL * (u - 250UL)) / 500UL; -// } -// else -// { -// d = 1000UL - u; -// p = 1000UL - (8UL * d * d) / (3UL * 1000UL); -// } -// if (p > 1000UL) -// { -// p = 1000UL; -// } -// return p; -//} - -static uint32_t PlsrAccelCurveShape(uint32_t t_ms, uint32_t T_ms, PlsrAccelMode_e mode) -{ -// if (mode == PLSR_ACCEL_S) -// { -// return PlsrAccelCurveShapeS7(t_ms, T_ms); -// } -// if (mode == PLSR_ACCEL_SINE) -// { -// return PlsrAccelCurveShapeSine(t_ms, T_ms); -// } - return PlsrAccelCurveShapeLinear(t_ms, T_ms); -} - /** 在 f_cur→f_want 路径上按千分比取点 */ static uint32_t PlsrAccelCurveOnPath(uint32_t f_cur, uint32_t f_want, uint32_t permille) { @@ -369,7 +348,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, uint32_t start_spd_ref, uint32_t end_spd_ref, uint32_t accel_ms, - uint32_t decel_ms) + uint32_t decel_ms, + PlsrAccelMode_e mode) { uint32_t lo = 0U; uint32_t hi = 1000U; @@ -392,8 +372,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, t_d = PlsrAccelCurveRampTimeMs(peak, f_end, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); - n_a = PlsrAccelCurveEstimatePulses(f_cur, peak, t_a); - n_d = PlsrAccelCurveEstimatePulses(peak, f_end, t_d); + n_a = PlsrAccelCurveEstimatePulses(f_cur, peak, t_a, mode); + n_d = PlsrAccelCurveEstimatePulses(peak, f_end, t_d, mode); if ((n_a + n_d) <= total_pulses) { @@ -434,6 +414,7 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, uint32_t acc_n; uint32_t dec_n; uint32_t f_peak; + PlsrAccelMode_e m; if (plan == (PlsrAccelPlan_t *)0) { @@ -444,9 +425,10 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, f_tgt = PlsrAccelCurveClampFreq(f_tgt); 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 = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode; + plan->mode = m; f_peak = f_tgt; t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, @@ -455,8 +437,8 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); - acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc); - dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec); + acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m); + dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m); if (total_pulses == 0U) { @@ -498,15 +480,15 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end, default_spd, start_spd_ref, end_spd_ref, - accel_ms, decel_ms); + accel_ms, decel_ms, m); t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); - acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc); - dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec); + 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) { diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index 853ee26..613de9d 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) + 脉冲起点锁频递推。 * 末段脉冲不够：降低峰值做三角。非末段不够：不减速，沿斜率加到脉冲用尽。 */ #ifndef PLSR_ACCEL_CURVE_H @@ -31,11 +31,22 @@ typedef enum { uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz); -/** 拟合斜坡时间(ms)，使离散直线模型脉冲数尽量等于 max_pulses */ +/** + * 斜坡上按 mode 取频（µs 时间轴）。ISR 可用：O(1)。 + * f = f0+(f1-f0)*shape(t/T) + */ +uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, + uint32_t f1, + uint32_t t_us, + uint32_t T_us, + PlsrAccelMode_e mode); + +/** 拟合斜坡时间(ms)，使离散模型脉冲数尽量等于 max_pulses（任务上下文） */ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, uint32_t f_to, uint32_t max_pulses, - uint32_t t_prefer_ms); + uint32_t t_prefer_ms, + PlsrAccelMode_e mode); void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, uint32_t total_pulses, diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index b0723cd..29d0f5d 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -147,27 +147,6 @@ static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) } } -static uint32_t PlsrRunControlLerpUs(uint32_t a, uint32_t b, uint32_t t_us, uint32_t T_us) -{ - uint32_t r; - - if ((T_us == 0U) || (t_us >= T_us)) - { - return b; - } - /* r = t/T * 1000，用 64 位避免加速时间较长时溢出 */ - r = (uint32_t)(((uint64_t)t_us * 1000ULL) / (uint64_t)T_us); - if (r > 1000U) - { - r = 1000U; - } - if (b >= a) - { - return a + ((b - a) * r) / 1000UL; - } - return a - ((a - b) * r) / 1000UL; -} - static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq) { s_phase = ph; @@ -323,8 +302,8 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) PlsrRunControlEnterDecel(next, remain); elapsed_us = 0U; T_us = p->t_dec_ms * 1000UL; - next = PlsrRunControlLerpUs(s_decel_from, p->f_end, - elapsed_us, T_us); + next = PlsrAccelCurveFreqOnRampUs(s_decel_from, p->f_end, + elapsed_us, T_us, p->mode); } } else @@ -335,8 +314,8 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) t_cmd = elapsed_us + ((1000000UL + (s_cur_freq / 2UL)) / s_cur_freq); } - next = PlsrRunControlLerpUs(s_approach_from, p->f_tgt, - t_cmd, T_us); + next = PlsrAccelCurveFreqOnRampUs(s_approach_from, p->f_tgt, + t_cmd, T_us, p->mode); } } else if (s_phase == PH_DECEL) @@ -363,7 +342,8 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) per = (1000000UL + (s_cur_freq / 2UL)) / s_cur_freq; t_cmd = elapsed_us + per; } - next = PlsrRunControlLerpUs(s_decel_from, p->f_end, t_cmd, T_us); + next = PlsrAccelCurveFreqOnRampUs(s_decel_from, p->f_end, + t_cmd, T_us, p->mode); } } else @@ -374,8 +354,8 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) PlsrRunControlEnterDecel(next, remain); elapsed_us = 0U; T_us = p->t_dec_ms * 1000UL; - next = PlsrRunControlLerpUs(s_decel_from, p->f_end, - elapsed_us, T_us); + next = PlsrAccelCurveFreqOnRampUs(s_decel_from, p->f_end, + elapsed_us, T_us, p->mode); } } @@ -409,7 +389,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, cfg->end_speed, cfg->accel_ms, cfg->decel_ms, - PLSR_ACCEL_LINEAR); + cfg->accel_mode); /* 本段频率范围锁 PSC，升降过程只改 ARR */ f_lo = s_accel_plan.f_tgt; @@ -456,7 +436,8 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, PlsrAccelCurveFitRampTimeMs(f_from, s_accel_plan.f_tgt, s_accel_plan.acc_n, - s_accel_plan.t_acc_ms); + s_accel_plan.t_acc_ms, + s_accel_plan.mode); } if (s_accel_plan.t_acc_ms < 1U) { @@ -817,10 +798,12 @@ void PlsrRunControlTickMs(void) uint32_t T_us = s_accel_plan.t_acc_ms * 1000UL; uint32_t per = (1000000UL + (s_approach_from / 2UL)) / s_approach_from; - uint32_t f2 = PlsrRunControlLerpUs(s_approach_from, - s_accel_plan.f_tgt, - per, - T_us); + uint32_t f2 = PlsrAccelCurveFreqOnRampUs( + s_approach_from, + s_accel_plan.f_tgt, + per, + T_us, + s_accel_plan.mode); if (f2 < 1U) { f2 = 1U;