| @@ -1,12 +1,8 @@ | |||||
| /** | /** | ||||
| * @file plsr_accel_curve.c | * @file plsr_accel_curve.c | ||||
| * @brief 预估相脉冲;相内时间域直线/S/正弦 | |||||
| * @brief 脉冲域规划/取频:f^2 = f0^2 + 2*a*n(直线);S/正弦按脉冲进度 shape | |||||
| * | * | ||||
| * 脉冲预算不够到目标频时:降低峰值,保留加减速时间(勿把 T 压成 0, | |||||
| * 否则示波器上变成方波,三种模式完全无法区分)。 | |||||
| * @note:目前脉冲输出在低频时和高频时均有毛刺,需要优化 | |||||
| * 已解决,原因是逻辑分析仪采样率不够,导致采样点减少,波形有毛刺。 | |||||
| * 脉冲预算不够到目标频时:降低峰值(三角,无匀速)。 | |||||
| */ | */ | ||||
| #include "plsr_accel_curve.h" | #include "plsr_accel_curve.h" | ||||
| #include <stdint.h> | #include <stdint.h> | ||||
| @@ -26,40 +22,42 @@ static uint32_t PlsrAccelCurveAbsDiff(uint32_t a, uint32_t b) | |||||
| return (a >= b) ? (a - b) : (b - a); | return (a >= b) ? (a - b) : (b - a); | ||||
| } | } | ||||
| /** | |||||
| * 整数平方根(牛顿法)。 | |||||
| */ | |||||
| static uint32_t PlsrAccelCurveIsqrt(uint32_t val) | |||||
| /** 64 位整数平方根(牛顿法),支持 f^2 + 2an */ | |||||
| static uint32_t PlsrAccelCurveIsqrt64(uint64_t val) | |||||
| { | { | ||||
| uint32_t x; | |||||
| uint32_t x2; | |||||
| uint64_t x; | |||||
| uint64_t x2; | |||||
| uint32_t i; | uint32_t i; | ||||
| if (val <= 1U) | |||||
| if (val <= 1ULL) | |||||
| { | { | ||||
| return val; | |||||
| return (uint32_t)val; | |||||
| } | } | ||||
| x = val; | x = val; | ||||
| if (x > 10000U) | |||||
| if (x > 100000ULL) | |||||
| { | { | ||||
| x = 10000U; | |||||
| x = 100000ULL; | |||||
| } | } | ||||
| for (i = 0U; i < 30U; i++) | |||||
| for (i = 0U; i < 40U; i++) | |||||
| { | { | ||||
| x2 = (x + val / x) / 2U; | |||||
| x2 = (x + val / x) / 2ULL; | |||||
| if (x2 >= x) | if (x2 >= x) | ||||
| { | { | ||||
| break; | break; | ||||
| } | } | ||||
| x = x2; | x = x2; | ||||
| } | } | ||||
| return x; | |||||
| return (uint32_t)x; | |||||
| } | |||||
| static uint32_t PlsrAccelCurveIsqrt(uint32_t val) | |||||
| { | |||||
| return PlsrAccelCurveIsqrt64((uint64_t)val); | |||||
| } | } | ||||
| /** | /** | ||||
| * 起跳频率: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。 | |||||
| * 起跳频率:f = sqrt(f_from^2 + 2a),a = df*1000/t_ms。 | |||||
| * f_from=0 → sqrt(2a);超过 f_to 钳到 f_to。 | |||||
| */ | */ | ||||
| uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, | uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, | ||||
| uint32_t t_ms) | uint32_t t_ms) | ||||
| @@ -67,17 +65,21 @@ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, | |||||
| uint32_t df; | uint32_t df; | ||||
| uint32_t a_hz_s; | uint32_t a_hz_s; | ||||
| uint32_t f; | uint32_t f; | ||||
| uint64_t val; | |||||
| if (t_ms == 0U) | |||||
| if (f_to < 1U) | |||||
| { | { | ||||
| return 1U; | return 1U; | ||||
| } | } | ||||
| if (t_ms == 0U) | |||||
| { | |||||
| return f_to; | |||||
| } | |||||
| df = PlsrAccelCurveAbsDiff(f_from, f_to); | df = PlsrAccelCurveAbsDiff(f_from, f_to); | ||||
| if (df == 0U) | if (df == 0U) | ||||
| { | { | ||||
| return 1U; | |||||
| return f_to; | |||||
| } | } | ||||
| /* a = df / (t_ms / 1000) = df * 1000 / t_ms,可能很大 */ | |||||
| if (df <= 4294967U) | if (df <= 4294967U) | ||||
| { | { | ||||
| a_hz_s = (df * 1000U) / t_ms; | a_hz_s = (df * 1000U) / t_ms; | ||||
| @@ -88,13 +90,21 @@ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, | |||||
| } | } | ||||
| if (a_hz_s == 0U) | if (a_hz_s == 0U) | ||||
| { | { | ||||
| return 1U; | |||||
| f = (f_from >= 1U) ? f_from : 1U; | |||||
| } | |||||
| else | |||||
| { | |||||
| val = (uint64_t)f_from * (uint64_t)f_from + (2ULL * (uint64_t)a_hz_s); | |||||
| f = PlsrAccelCurveIsqrt64(val); | |||||
| } | } | ||||
| f = PlsrAccelCurveIsqrt(a_hz_s); | |||||
| if (f < 1U) | if (f < 1U) | ||||
| { | { | ||||
| f = 1U; | f = 1U; | ||||
| } | } | ||||
| if (f > f_to) | |||||
| { | |||||
| f = f_to; | |||||
| } | |||||
| return f; | return f; | ||||
| } | } | ||||
| @@ -163,44 +173,6 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from, | |||||
| return t; | return t; | ||||
| } | } | ||||
| /** | |||||
| * 从 0 爬到 f_tgt(或从 f_tgt 落到 0)时的起跳/落地频率。 | |||||
| * start/end 参考速都为 0:a = f_tgt*1000/t_ms,f=sqrt(a); | |||||
| * 否则走 JumpFreq(0, f_tgt, t_ms)。 | |||||
| */ | |||||
| static uint32_t PlsrAccelCurveJumpFromZero(uint32_t f_tgt, | |||||
| uint32_t t_ms, | |||||
| uint32_t start_spd_ref, | |||||
| uint32_t end_spd_ref) | |||||
| { | |||||
| uint32_t f; | |||||
| if ((f_tgt < 1U) || (t_ms < 1U)) | |||||
| { | |||||
| return 1U; | |||||
| } | |||||
| if ((start_spd_ref == 0U) && (end_spd_ref == 0U)) | |||||
| { | |||||
| uint64_t a_hz_s = ((uint64_t)f_tgt * 1000ULL) / (uint64_t)t_ms; | |||||
| f = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s); | |||||
| } | |||||
| else | |||||
| { | |||||
| f = PlsrAccelCurveJumpFreq(0U, f_tgt, t_ms); | |||||
| } | |||||
| if (f < 1U) | |||||
| { | |||||
| f = 1U; | |||||
| } | |||||
| if (f > f_tgt) | |||||
| { | |||||
| f = f_tgt; | |||||
| } | |||||
| return f; | |||||
| } | |||||
| uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, | uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, | ||||
| uint32_t f_tgt, | uint32_t f_tgt, | ||||
| uint32_t default_spd, | uint32_t default_spd, | ||||
| @@ -213,20 +185,16 @@ uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, | |||||
| f_cfg = PlsrAccelCurveClampFreq(f_cfg); | f_cfg = PlsrAccelCurveClampFreq(f_cfg); | ||||
| f_tgt = PlsrAccelCurveClampFreq(f_tgt); | f_tgt = PlsrAccelCurveClampFreq(f_tgt); | ||||
| if (f_cfg >= 1U) | |||||
| { | |||||
| return f_cfg; | |||||
| } | |||||
| if (f_tgt < 1U) | if (f_tgt < 1U) | ||||
| { | { | ||||
| return 1U; | return 1U; | ||||
| } | } | ||||
| t_acc = PlsrAccelCurveRampTimeMs(0U, f_tgt, default_spd, | |||||
| /* 起速 0 或非 0:都用 JumpFreq = sqrt(f_cfg^2 + 2a) */ | |||||
| t_acc = PlsrAccelCurveRampTimeMs(f_cfg, f_tgt, default_spd, | |||||
| start_spd_ref, end_spd_ref, | start_spd_ref, end_spd_ref, | ||||
| accel_ms, decel_ms); | accel_ms, decel_ms); | ||||
| return PlsrAccelCurveJumpFromZero(f_tgt, t_acc, | |||||
| start_spd_ref, end_spd_ref); | |||||
| return PlsrAccelCurveJumpFreq(f_cfg, f_tgt, t_acc); | |||||
| } | } | ||||
| uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, | uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, | ||||
| @@ -241,20 +209,26 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, | |||||
| f_cfg = PlsrAccelCurveClampFreq(f_cfg); | f_cfg = PlsrAccelCurveClampFreq(f_cfg); | ||||
| f_tgt = PlsrAccelCurveClampFreq(f_tgt); | f_tgt = PlsrAccelCurveClampFreq(f_tgt); | ||||
| if (f_cfg >= 1U) | |||||
| { | |||||
| return f_cfg; | |||||
| } | |||||
| if (f_tgt < 1U) | if (f_tgt < 1U) | ||||
| { | { | ||||
| return 1U; | return 1U; | ||||
| } | } | ||||
| t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd, | |||||
| /* | |||||
| * 止速 0 或 1:按落地频率估计,避免 1Hz 拖尾。 | |||||
| */ | |||||
| if (f_cfg <= 1U) | |||||
| { | |||||
| t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd, | |||||
| start_spd_ref, end_spd_ref, | |||||
| accel_ms, decel_ms); | |||||
| return PlsrAccelCurveJumpFreq(0U, f_tgt, t_dec); | |||||
| } | |||||
| t_dec = PlsrAccelCurveRampTimeMs(f_tgt, f_cfg, default_spd, | |||||
| start_spd_ref, end_spd_ref, | start_spd_ref, end_spd_ref, | ||||
| accel_ms, decel_ms); | accel_ms, decel_ms); | ||||
| return PlsrAccelCurveJumpFromZero(f_tgt, t_dec, | |||||
| start_spd_ref, end_spd_ref); | |||||
| return PlsrAccelCurveJumpFreq(f_cfg, f_tgt, t_dec); | |||||
| } | } | ||||
| /** 直线:进度千分比 → 形状千分比 */ | /** 直线:进度千分比 → 形状千分比 */ | ||||
| @@ -704,27 +678,126 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, | |||||
| } | } | ||||
| /** | /** | ||||
| * 二分峰值频率(Hz),使 1ms 模型下 acc+dec 脉冲 <= total,且尽量接近原目标频。 | |||||
| * 到不了目标频时这就是三角:峰值降低,匀速脉冲必须为 0。 | |||||
| * a = default_spd * 1000 / ramp_ms(Hz/s)。ramp_ms=0 → a=0(阶跃)。 | |||||
| */ | |||||
| static uint32_t PlsrAccelCurveAccelHzS(uint32_t default_spd, uint32_t ramp_ms) | |||||
| { | |||||
| if (ramp_ms == 0U) | |||||
| { | |||||
| return 0U; | |||||
| } | |||||
| if (default_spd == 0U) | |||||
| { | |||||
| return 0U; | |||||
| } | |||||
| return (default_spd * 1000UL) / ramp_ms; | |||||
| } | |||||
| /** | |||||
| * 脉冲闭合:N = ceil(|f1^2 - f0^2| / (2a))。 | |||||
| * a=0 且频差非 0 → 0(阶跃,无斜坡脉冲)。 | |||||
| */ | |||||
| static uint32_t PlsrAccelCurvePulsesForRamp(uint32_t f0, uint32_t f1, | |||||
| uint32_t a_hz_s) | |||||
| { | |||||
| uint64_t f0s; | |||||
| uint64_t f1s; | |||||
| uint64_t df2; | |||||
| uint64_t two_a; | |||||
| uint64_t n; | |||||
| if (f0 == f1) | |||||
| { | |||||
| return 0U; | |||||
| } | |||||
| if (a_hz_s == 0U) | |||||
| { | |||||
| return 0U; | |||||
| } | |||||
| f0s = (uint64_t)f0 * (uint64_t)f0; | |||||
| f1s = (uint64_t)f1 * (uint64_t)f1; | |||||
| df2 = (f1s > f0s) ? (f1s - f0s) : (f0s - f1s); | |||||
| two_a = 2ULL * (uint64_t)a_hz_s; | |||||
| n = (df2 + two_a - 1ULL) / two_a; | |||||
| if (n == 0ULL) | |||||
| { | |||||
| n = 1ULL; | |||||
| } | |||||
| if (n > 0xFFFFFFFFULL) | |||||
| { | |||||
| return 0xFFFFFFFFUL; | |||||
| } | |||||
| return (uint32_t)n; | |||||
| } | |||||
| /** 直线:f = sqrt(f0^2 + 2*a*n),升到/降到不超过 limit 方向 */ | |||||
| static uint32_t PlsrAccelCurveFreqKinematic(uint32_t f0, uint32_t a_hz_s, | |||||
| uint32_t n, uint8_t rising, | |||||
| uint32_t limit) | |||||
| { | |||||
| uint64_t val; | |||||
| uint32_t f; | |||||
| if (n == 0U) | |||||
| { | |||||
| f = f0; | |||||
| } | |||||
| else if (a_hz_s == 0U) | |||||
| { | |||||
| f = limit; | |||||
| } | |||||
| else | |||||
| { | |||||
| val = (uint64_t)f0 * (uint64_t)f0; | |||||
| if (rising != 0U) | |||||
| { | |||||
| val += 2ULL * (uint64_t)a_hz_s * (uint64_t)n; | |||||
| f = PlsrAccelCurveIsqrt64(val); | |||||
| if (f > limit) | |||||
| { | |||||
| f = limit; | |||||
| } | |||||
| } | |||||
| else | |||||
| { | |||||
| uint64_t drop = 2ULL * (uint64_t)a_hz_s * (uint64_t)n; | |||||
| if (drop >= val) | |||||
| { | |||||
| f = limit; | |||||
| } | |||||
| else | |||||
| { | |||||
| f = PlsrAccelCurveIsqrt64(val - drop); | |||||
| if (f < limit) | |||||
| { | |||||
| f = limit; | |||||
| } | |||||
| } | |||||
| } | |||||
| } | |||||
| if (f < 1U) | |||||
| { | |||||
| f = 1U; | |||||
| } | |||||
| return PlsrAccelCurveClampFreq(f); | |||||
| } | |||||
| /** | |||||
| * 二分峰值:使 acc+dec 脉冲 <= total(脉冲闭合公式)。 | |||||
| */ | */ | ||||
| static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, | static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, | ||||
| uint32_t f_cur, | uint32_t f_cur, | ||||
| uint32_t f_want, | uint32_t f_want, | ||||
| uint32_t f_end, | uint32_t f_end, | ||||
| uint32_t default_spd, | |||||
| uint32_t start_spd_ref, | |||||
| uint32_t end_spd_ref, | |||||
| uint32_t accel_ms, | |||||
| uint32_t decel_ms, | |||||
| PlsrAccelMode_e mode) | |||||
| uint32_t a_acc, | |||||
| uint32_t a_dec) | |||||
| { | { | ||||
| uint32_t lo = 0U; | uint32_t lo = 0U; | ||||
| uint32_t hi; | uint32_t hi; | ||||
| uint32_t mid; | uint32_t mid; | ||||
| uint32_t best = 0U; | uint32_t best = 0U; | ||||
| uint32_t peak; | uint32_t peak; | ||||
| uint32_t t_a; | |||||
| uint32_t t_d; | |||||
| uint32_t n_a; | uint32_t n_a; | ||||
| uint32_t n_d; | uint32_t n_d; | ||||
| uint32_t span; | uint32_t span; | ||||
| @@ -750,14 +823,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, | |||||
| { | { | ||||
| peak = f_cur - mid; | peak = f_cur - mid; | ||||
| } | } | ||||
| t_a = PlsrAccelCurveRampTimeMs(f_cur, peak, default_spd, | |||||
| start_spd_ref, end_spd_ref, | |||||
| accel_ms, decel_ms); | |||||
| 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, mode); | |||||
| n_d = PlsrAccelCurveEstimatePulses(peak, f_end, t_d, mode); | |||||
| n_a = PlsrAccelCurvePulsesForRamp(f_cur, peak, a_acc); | |||||
| n_d = PlsrAccelCurvePulsesForRamp(peak, f_end, a_dec); | |||||
| if ((n_a + n_d) <= total_pulses) | if ((n_a + n_d) <= total_pulses) | ||||
| { | { | ||||
| @@ -806,6 +873,8 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| uint32_t acc_n; | uint32_t acc_n; | ||||
| uint32_t dec_n; | uint32_t dec_n; | ||||
| uint32_t f_peak; | uint32_t f_peak; | ||||
| uint32_t a_acc; | |||||
| uint32_t a_dec; | |||||
| PlsrAccelMode_e m; | PlsrAccelMode_e m; | ||||
| if (plan == (PlsrAccelPlan_t *)0) | if (plan == (PlsrAccelPlan_t *)0) | ||||
| @@ -819,10 +888,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode; | m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode; | ||||
| /* | |||||
| * 端点须由调用方解析完毕。若仍传入 0,用同一套 Resolve 兜底, | |||||
| * 避免把 0 当真实频率规划出错误斜坡;正常路径不应走到这里。 | |||||
| */ | |||||
| if (f_cur < 1U) | if (f_cur < 1U) | ||||
| { | { | ||||
| f_cur = PlsrAccelCurveResolveStartHz(0U, f_tgt, default_spd, | f_cur = PlsrAccelCurveResolveStartHz(0U, f_tgt, default_spd, | ||||
| @@ -836,6 +901,26 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| accel_ms, decel_ms); | accel_ms, decel_ms); | ||||
| } | } | ||||
| a_acc = PlsrAccelCurveAccelHzS(default_spd, accel_ms); | |||||
| a_dec = PlsrAccelCurveAccelHzS(default_spd, decel_ms); | |||||
| /* 升降方向用对应斜率;无默认速度时用频差/时间回退 */ | |||||
| if ((a_acc == 0U) && (accel_ms > 0U)) | |||||
| { | |||||
| uint32_t df = PlsrAccelCurveAbsDiff(f_cur, f_tgt); | |||||
| if (df > 0U) | |||||
| { | |||||
| a_acc = (df * 1000UL) / accel_ms; | |||||
| } | |||||
| } | |||||
| if ((a_dec == 0U) && (decel_ms > 0U)) | |||||
| { | |||||
| uint32_t df = PlsrAccelCurveAbsDiff(f_tgt, f_end); | |||||
| if (df > 0U) | |||||
| { | |||||
| a_dec = (df * 1000UL) / decel_ms; | |||||
| } | |||||
| } | |||||
| f_peak = f_tgt; | f_peak = f_tgt; | ||||
| t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, | t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, | ||||
| start_spd_ref, end_spd_ref, | start_spd_ref, end_spd_ref, | ||||
| @@ -847,13 +932,14 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| plan->f_cur = f_cur; | plan->f_cur = f_cur; | ||||
| plan->f_end = f_end; | plan->f_end = f_end; | ||||
| plan->mode = m; | plan->mode = m; | ||||
| acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m); | |||||
| dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m); | |||||
| plan->a_acc = a_acc; | |||||
| plan->a_dec = a_dec; | |||||
| acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc); | |||||
| dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec); | |||||
| if (total_pulses == 0U) | if (total_pulses == 0U) | ||||
| { | { | ||||
| acc_n = 0U; | |||||
| dec_n = 0U; | |||||
| plan->const_n = 0U; | plan->const_n = 0U; | ||||
| plan->t_acc_ms = 0U; | plan->t_acc_ms = 0U; | ||||
| plan->t_dec_ms = 0U; | plan->t_dec_ms = 0U; | ||||
| @@ -864,11 +950,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| return; | return; | ||||
| } | } | ||||
| /* | |||||
| * f_end == f_tgt:本段无独立段末终点(仅加速到目标)。 | |||||
| * 脉冲不够时不 FitPeak、不减速;梯形/三角只由 | |||||
| * (f_cur,f_tgt,f_end,斜率,mode,总脉冲) 决定,与 wait 无关。 | |||||
| */ | |||||
| if ((f_end == f_tgt) && (acc_n > total_pulses)) | if ((f_end == f_tgt) && (acc_n > total_pulses)) | ||||
| { | { | ||||
| plan->const_n = 0U; | plan->const_n = 0U; | ||||
| @@ -883,21 +964,16 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| if ((acc_n + dec_n) > total_pulses) | if ((acc_n + dec_n) > total_pulses) | ||||
| { | { | ||||
| /* | |||||
| * 脉冲不够到目标频:降峰值(斜率不变),三角,禁止匀速。 | |||||
| */ | |||||
| f_peak = PlsrAccelCurveFitPeak(total_pulses, | |||||
| f_cur, f_tgt, f_end, | |||||
| default_spd, start_spd_ref, end_spd_ref, | |||||
| accel_ms, decel_ms, m); | |||||
| f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end, | |||||
| a_acc, a_dec); | |||||
| t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, | t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, | ||||
| start_spd_ref, end_spd_ref, | start_spd_ref, end_spd_ref, | ||||
| accel_ms, decel_ms); | accel_ms, decel_ms); | ||||
| t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd, | t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd, | ||||
| start_spd_ref, end_spd_ref, | start_spd_ref, end_spd_ref, | ||||
| accel_ms, decel_ms); | accel_ms, decel_ms); | ||||
| acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m); | |||||
| dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m); | |||||
| acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc); | |||||
| dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec); | |||||
| if (acc_n > total_pulses) | if (acc_n > total_pulses) | ||||
| { | { | ||||
| acc_n = total_pulses; | acc_n = total_pulses; | ||||
| @@ -919,13 +995,9 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| /* 够到目标频:匀速脉冲 = 总数 − 加速 − 减速 */ | |||||
| plan->const_n = total_pulses - acc_n - dec_n; | plan->const_n = total_pulses - acc_n - dec_n; | ||||
| } | } | ||||
| /* | |||||
| * 需要减速 (f_peak > f_end) 时禁止 dec_n=0。 | |||||
| */ | |||||
| if ((f_peak > f_end) && (total_pulses > 1U) && (dec_n == 0U)) | if ((f_peak > f_end) && (total_pulses > 1U) && (dec_n == 0U)) | ||||
| { | { | ||||
| dec_n = 1U; | dec_n = 1U; | ||||
| @@ -955,70 +1027,95 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| plan->t_dec_ms = t_dec; | plan->t_dec_ms = t_dec; | ||||
| plan->acc_n = acc_n; | plan->acc_n = acc_n; | ||||
| plan->dec_n = dec_n; | plan->dec_n = dec_n; | ||||
| /* | |||||
| * 进入减速时刻:加速时间 + 匀速时间。 | |||||
| * 三角无匀速 → t_decel_start = t_acc,减速从峰值立刻开始。 | |||||
| * 止速脉冲数 = dec_n,由 1ms 模型按 (f_peak→f_end, t_dec) 估出。 | |||||
| */ | |||||
| plan->t_decel_start_ms = t_acc + PlsrAccelCurveConstTimeMs(plan->const_n, f_peak); | plan->t_decel_start_ms = t_acc + PlsrAccelCurveConstTimeMs(plan->const_n, f_peak); | ||||
| } | } | ||||
| uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, | |||||
| uint32_t seg_elapsed_ms) | |||||
| uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, | |||||
| uint32_t pulse_done) | |||||
| { | { | ||||
| uint32_t t; | |||||
| uint32_t T_acc_us; | |||||
| uint32_t t_dec; | |||||
| uint32_t const_end; | |||||
| uint32_t n; | |||||
| uint32_t f; | |||||
| uint32_t ratio; | uint32_t ratio; | ||||
| uint8_t rising; | |||||
| if (plan == (PlsrAccelPlan_t *)0) | if (plan == (PlsrAccelPlan_t *)0) | ||||
| { | { | ||||
| return 0U; | return 0U; | ||||
| } | } | ||||
| if ((plan->t_acc_ms > 0U) && (seg_elapsed_ms < plan->t_acc_ms) && | |||||
| const_end = plan->acc_n + plan->const_n; | |||||
| /* 加速相:0 .. acc_n-1 完成后仍在加速;pulse_done 为已完成数 */ | |||||
| if ((plan->acc_n > 0U) && (pulse_done < plan->acc_n) && | |||||
| (plan->f_cur != plan->f_tgt)) | (plan->f_cur != plan->f_tgt)) | ||||
| { | { | ||||
| T_acc_us = plan->t_acc_ms * 1000UL; | |||||
| rising = (plan->f_tgt >= plan->f_cur) ? 1U : 0U; | |||||
| if (plan->mode == PLSR_ACCEL_LINEAR) | |||||
| { | { | ||||
| uint32_t f_acc = PlsrAccelCurveFreqOnRampUs(plan->f_cur, plan->f_tgt, | |||||
| seg_elapsed_ms * 1000UL, | |||||
| T_acc_us, plan->mode); | |||||
| if ((f_acc < 1U) && (plan->f_tgt >= 1U)) | |||||
| { | |||||
| f_acc = 1U; | |||||
| } | |||||
| return PlsrAccelCurveClampFreq(f_acc); | |||||
| f = PlsrAccelCurveFreqKinematic(plan->f_cur, plan->a_acc, | |||||
| pulse_done, rising, plan->f_tgt); | |||||
| } | |||||
| else | |||||
| { | |||||
| ratio = PlsrAccelCurveShape(pulse_done, plan->acc_n, plan->mode); | |||||
| f = PlsrAccelCurveLerp(plan->f_cur, plan->f_tgt, ratio); | |||||
| } | } | ||||
| if ((f < 1U) && (plan->f_tgt >= 1U)) | |||||
| { | |||||
| f = 1U; | |||||
| } | |||||
| return PlsrAccelCurveClampFreq(f); | |||||
| } | } | ||||
| if (seg_elapsed_ms < plan->t_decel_start_ms) | |||||
| /* 匀速 */ | |||||
| if (pulse_done < const_end) | |||||
| { | { | ||||
| return PlsrAccelCurveClampFreq(plan->f_tgt); | return PlsrAccelCurveClampFreq(plan->f_tgt); | ||||
| } | } | ||||
| if ((plan->t_dec_ms == 0U) || (plan->f_tgt == plan->f_end)) | |||||
| /* 减速 / 无减速 */ | |||||
| if ((plan->dec_n == 0U) || (plan->f_tgt == plan->f_end)) | |||||
| { | { | ||||
| return PlsrAccelCurveClampFreq(plan->f_end); | return PlsrAccelCurveClampFreq(plan->f_end); | ||||
| } | } | ||||
| t_dec = seg_elapsed_ms - plan->t_decel_start_ms; | |||||
| if (t_dec >= plan->t_dec_ms) | |||||
| n = pulse_done - const_end; | |||||
| if (n >= plan->dec_n) | |||||
| { | { | ||||
| return PlsrAccelCurveClampFreq(plan->f_end); | return PlsrAccelCurveClampFreq(plan->f_end); | ||||
| } | } | ||||
| rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U; | |||||
| if (plan->mode == PLSR_ACCEL_LINEAR) | 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)); | |||||
| f = PlsrAccelCurveFreqKinematic(plan->f_tgt, plan->a_dec, | |||||
| n, rising, plan->f_end); | |||||
| } | |||||
| else | |||||
| { | |||||
| ratio = PlsrAccelCurveShape(n, plan->dec_n, plan->mode); | |||||
| f = PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio); | |||||
| } | |||||
| if (f < 1U) | |||||
| { | |||||
| f = 1U; | |||||
| } | } | ||||
| return PlsrAccelCurveClampFreq(f); | |||||
| } | |||||
| /** 兼容旧预览:用时间估算映射到脉冲再取频 */ | |||||
| uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, | |||||
| uint32_t seg_elapsed_ms) | |||||
| { | |||||
| uint32_t approx_n; | |||||
| uint32_t f_ref; | |||||
| t = plan->t_dec_ms; | |||||
| ratio = PlsrAccelCurveShape(t_dec, t, plan->mode); | |||||
| return PlsrAccelCurveClampFreq( | |||||
| PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio)); | |||||
| if (plan == (PlsrAccelPlan_t *)0) | |||||
| { | |||||
| return 0U; | |||||
| } | |||||
| f_ref = (plan->f_tgt >= 1U) ? plan->f_tgt : 1U; | |||||
| approx_n = (uint32_t)(((uint64_t)seg_elapsed_ms * (uint64_t)f_ref) / 1000ULL); | |||||
| return PlsrAccelCurveFreqAtPulse(plan, approx_n); | |||||
| } | } | ||||
| @@ -1,12 +1,11 @@ | |||||
| /** | /** | ||||
| * @file plsr_accel_curve.h | * @file plsr_accel_curve.h | ||||
| * @brief 预估加/匀/减脉冲;相内按时间走直线/S/正弦 | |||||
| * @brief 脉冲域加/匀/减规划与取频(v^2 - v0^2 = 2ax) | |||||
| * | * | ||||
| * 1ms 改频:脉冲数按 t=0..T-1 实际取频累加;到不了目标频则降峰值走三角(无匀速)。 | |||||
| * 加速度 a = 默认速度*1000/加减速时间(Hz/s)。 | |||||
| * 脉冲序号 n 上:f(n) = sqrt(f0^2 + 2*a*n);初速为 0 即 f0=0。 | |||||
| * 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。 | * 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。 | ||||
| * 斜率按公共参数:K_acc=默认速度/加速时间,K_dec=默认速度/减速时间(减 0,不用起/止速)。 | |||||
| * 小频差时 T 可能 <1ms,无可见斜坡属正常。 | |||||
| * 开段端点由运行层 ResolveStart/EndHz 解析后再传入 Plan(Plan 不解释策略 0)。 | |||||
| * 开段端点由运行层 ResolveStart/EndHz 解析后再传入 Plan。 | |||||
| */ | */ | ||||
| #ifndef PLSR_ACCEL_CURVE_H | #ifndef PLSR_ACCEL_CURVE_H | ||||
| #define PLSR_ACCEL_CURVE_H | #define PLSR_ACCEL_CURVE_H | ||||
| @@ -21,9 +20,11 @@ typedef struct { | |||||
| uint32_t f_cur; | uint32_t f_cur; | ||||
| uint32_t f_tgt; | uint32_t f_tgt; | ||||
| uint32_t f_end; | uint32_t f_end; | ||||
| uint32_t t_acc_ms; | |||||
| uint32_t a_acc; /* 加速 a(Hz/s) */ | |||||
| uint32_t a_dec; /* 减速 a(Hz/s) */ | |||||
| uint32_t t_acc_ms; /* 斜率时间估算(预览/兼容,运行不依赖) */ | |||||
| uint32_t t_dec_ms; | uint32_t t_dec_ms; | ||||
| uint32_t t_decel_start_ms; /* 反向规划:段内时间轴上进入减速的时刻 */ | |||||
| uint32_t t_decel_start_ms; /* 估算:进减速时刻(预览用) */ | |||||
| PlsrAccelMode_e mode; | PlsrAccelMode_e mode; | ||||
| } PlsrAccelPlan_t; | } PlsrAccelPlan_t; | ||||
| @@ -36,17 +37,14 @@ typedef enum { | |||||
| uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz); | uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz); | ||||
| /** | /** | ||||
| * 起跳频率:f_jump = sqrt(加速度_Hz/s)。 | |||||
| * 起速/止速为 0 时,第一个脉冲(或最后一个脉冲)应以此频率输出, | |||||
| * 使脉冲周期恰好等于加速度产生该频率所需的时间。 | |||||
| * 起跳/步进频率:f = sqrt(f_from^2 + 2a),a = |f_to-f_from|*1000/t_ms。 | |||||
| * f_from=0 时退化为 sqrt(2a);超过 f_to 则钳到 f_to。 | |||||
| */ | */ | ||||
| uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, | uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, | ||||
| uint32_t t_ms); | uint32_t t_ms); | ||||
| /** | /** | ||||
| * 配置起速 → 规划用真实起点频率。 | |||||
| * f_cfg>=1:原样(钳位);f_cfg==0:按斜率算起跳频率(从静止爬升)。 | |||||
| * 由运行层在开段前调用;Plan 只吃已经解析好的端点。 | |||||
| * 配置起速 → 规划用真实起点:一律 JumpFreq(f_cfg, f_tgt, t_acc)。 | |||||
| */ | */ | ||||
| uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, | uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, | ||||
| uint32_t f_tgt, | uint32_t f_tgt, | ||||
| @@ -57,9 +55,8 @@ uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, | |||||
| uint32_t decel_ms); | uint32_t decel_ms); | ||||
| /** | /** | ||||
| * 配置止速 → 规划用真实终点频率。 | |||||
| * f_cfg>=1:原样(钳位);f_cfg==0:按斜率算落地频率(避免 1Hz 拖尾)。 | |||||
| * 段末是否停表由运行层决定,与本函数无关。 | |||||
| * 配置止速 → 规划用真实终点。 | |||||
| * f_cfg<=1:按落地频率估计(避免 1Hz 拖尾);否则 JumpFreq。 | |||||
| */ | */ | ||||
| uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, | uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, | ||||
| uint32_t f_tgt, | uint32_t f_tgt, | ||||
| @@ -69,43 +66,28 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, | |||||
| uint32_t accel_ms, | uint32_t accel_ms, | ||||
| uint32_t decel_ms); | uint32_t decel_ms); | ||||
| /** | |||||
| * 斜坡上按 mode 取频(µs 时间轴)。ISR 可用:O(1)。 | |||||
| * f = f0+(f1-f0)*shape(t/T) | |||||
| */ | |||||
| uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, | uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, | ||||
| uint32_t f1, | uint32_t f1, | ||||
| uint32_t t_us, | uint32_t t_us, | ||||
| uint32_t T_us, | uint32_t T_us, | ||||
| PlsrAccelMode_e mode); | PlsrAccelMode_e mode); | ||||
| /** | |||||
| * 与 TIM3 1ms 改频一致的斜坡取频(规划/运行共用)。 | |||||
| */ | |||||
| uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0, | uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0, | ||||
| uint32_t f1, | uint32_t f1, | ||||
| uint32_t t_ms, | uint32_t t_ms, | ||||
| uint32_t T_ms, | uint32_t T_ms, | ||||
| PlsrAccelMode_e mode); | PlsrAccelMode_e mode); | ||||
| /** | |||||
| * 斜坡第一拍频率(起速为 0 时用自洽点,避免按 1Hz 整秒吃光加速相)。 | |||||
| * ISR 可用:O(1)。 | |||||
| */ | |||||
| uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, | uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, | ||||
| uint32_t f1, | uint32_t f1, | ||||
| uint32_t T_us, | uint32_t T_us, | ||||
| PlsrAccelMode_e mode); | PlsrAccelMode_e mode); | ||||
| /** | |||||
| * 减速落地频率:f_end>=1 即止速;f_end==0 返回 0(停表,不用 1Hz 冒充)。 | |||||
| */ | |||||
| uint32_t PlsrAccelCurveLandFreq(uint32_t f_end, | uint32_t PlsrAccelCurveLandFreq(uint32_t f_end, | ||||
| uint32_t f_peak, | uint32_t f_peak, | ||||
| uint32_t T_ms, | uint32_t T_ms, | ||||
| PlsrAccelMode_e mode); | PlsrAccelMode_e mode); | ||||
| /** 拟合斜坡时间(ms),使离散模型脉冲数尽量等于 max_pulses(任务上下文) */ | |||||
| uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, | uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, | ||||
| uint32_t f_to, | uint32_t f_to, | ||||
| uint32_t max_pulses, | uint32_t max_pulses, | ||||
| @@ -125,8 +107,13 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, | |||||
| PlsrAccelMode_e mode); | PlsrAccelMode_e mode); | ||||
| /** | /** | ||||
| * 1ms 改频:按段内绝对时间取频(相界由 t_decel_start_ms 决定,不用脉冲域)。 | |||||
| * 按已完成脉冲数取频(任务上下文调用,勿进脉冲 ISR)。 | |||||
| * pulse_done = 本段已发完的脉冲数(与 s_done 一致)。 | |||||
| */ | */ | ||||
| uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, | |||||
| uint32_t pulse_done); | |||||
| /** @deprecated 时间轴取频;保留给旧预览,运行请用 FreqAtPulse */ | |||||
| uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, | uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, | ||||
| uint32_t seg_elapsed_ms); | uint32_t seg_elapsed_ms); | ||||
| @@ -556,6 +556,11 @@ void PlsrPulseDriverRequestFreq(uint32_t freq_hz) | |||||
| s_req_pending = 1U; | s_req_pending = 1U; | ||||
| } | } | ||||
| void PlsrPulseDriverClearPending(void) | |||||
| { | |||||
| s_req_pending = 0U; | |||||
| } | |||||
| void PlsrPulseDriverApplyPending(void) | void PlsrPulseDriverApplyPending(void) | ||||
| { | { | ||||
| uint32_t f; | uint32_t f; | ||||
| @@ -37,6 +37,8 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz); | |||||
| void PlsrPulseDriverRequestFreq(uint32_t freq_hz); | void PlsrPulseDriverRequestFreq(uint32_t freq_hz); | ||||
| /** 在更新中断里调用:应用挂起频率 */ | /** 在更新中断里调用:应用挂起频率 */ | ||||
| void PlsrPulseDriverApplyPending(void); | void PlsrPulseDriverApplyPending(void); | ||||
| /** 丢弃未生效的改频请求(进匀速锁定时用) */ | |||||
| void PlsrPulseDriverClearPending(void); | |||||
| void PlsrPulseDriverStop(void); | void PlsrPulseDriverStop(void); | ||||
| /** 当前拍结束后停止计数,避免多出一个上升沿 */ | /** 当前拍结束后停止计数,避免多出一个上升沿 */ | ||||
| void PlsrPulseDriverArmOnePulseStop(void); | void PlsrPulseDriverArmOnePulseStop(void); | ||||
| @@ -12,7 +12,10 @@ | |||||
| * 3) 发送模式只影响本段 f_end(完成=止速;后续且不停表=下一段目标) | * 3) 发送模式只影响本段 f_end(完成=止速;后续且不停表=下一段目标) | ||||
| * 4) PlanSeg / ChangeFreq 共用规划入口,便于后期动态改频 | * 4) PlanSeg / ChangeFreq 共用规划入口,便于后期动态改频 | ||||
| * | * | ||||
| * 【运行节拍】TIM3 1ms:RefreshProfile 改频;脉冲 UPDATE ISR 只计 s_done。 | |||||
| * 【运行节拍】脉冲 UPDATE:计 s_done;加/减速置 dirty,匀速不周期性改频。 | |||||
| * 任务 TickMs:dirty 时 FreqAtPulse 改频。 | |||||
| * TIM3 1ms:仅 ACT/WAIT 时基(不改频)。 | |||||
| * 匀速“保持”≠禁止改频:ACT/EXT 打断、ChangeFreq、进减速、开下一段仍可 Start/SetFreq。 | |||||
| */ | */ | ||||
| #include "plsr_run_control.h" | #include "plsr_run_control.h" | ||||
| #include "plsr_path_plan.h" | #include "plsr_path_plan.h" | ||||
| @@ -58,11 +61,11 @@ static volatile uint8_t s_wait_expire_req; /* WAIT 时间到:ISR 只置位 */ | |||||
| static uint8_t s_wait_is_signal; /* RC_WAIT_COND:1=等 WAIT 沿,0=等时间 */ | static uint8_t s_wait_is_signal; /* RC_WAIT_COND:1=等 WAIT 沿,0=等时间 */ | ||||
| static PlsrAccelPlan_t s_accel_plan; | static PlsrAccelPlan_t s_accel_plan; | ||||
| static RunPhase_e s_phase; | |||||
| static uint32_t s_seg_elapsed_ms; /* 段内绝对时间(TIM3 1ms 累加) */ | |||||
| static volatile RunPhase_e s_phase; /* ISR 读相,须 volatile */ | |||||
| static uint32_t s_approach_from; | static uint32_t s_approach_from; | ||||
| static uint32_t s_decel_from; | static uint32_t s_decel_from; | ||||
| static uint8_t s_follow_cont; | static uint8_t s_follow_cont; | ||||
| static volatile uint8_t s_profile_dirty; /* 脉冲推进后由任务改频 */ | |||||
| static uint32_t s_chain_freq; | static uint32_t s_chain_freq; | ||||
| static uint8_t s_chain_valid; | static uint8_t s_chain_valid; | ||||
| @@ -604,7 +607,7 @@ static uint32_t PlsrRunControlClampSpeed(uint32_t spd) | |||||
| return spd; | return spd; | ||||
| } | } | ||||
| /** 运行输出:0=停表;1ms 节拍直接 SetFreq / Start */ | |||||
| /** 运行输出:0=停表;任务上下文 SetFreq / Start */ | |||||
| static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) | static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) | ||||
| { | { | ||||
| if (profile_freq == 0U) | if (profile_freq == 0U) | ||||
| @@ -644,122 +647,138 @@ static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq) | |||||
| } | } | ||||
| } | } | ||||
| static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) | |||||
| static uint8_t PlsrRunControlHasDecel(void) | |||||
| { | { | ||||
| PlsrCfg_t *cfg = PlsrParamGetCfg(); | |||||
| uint32_t t_slope = 0U; | |||||
| uint32_t t_fit; | |||||
| uint32_t sum_f; | |||||
| uint32_t df; | |||||
| uint32_t den; | |||||
| uint32_t ref_ms; | |||||
| uint32_t f_end; | |||||
| uint32_t old_dec_n; | |||||
| uint32_t old_t_dec; | |||||
| return ((s_accel_plan.dec_n > 0U) && | |||||
| (s_accel_plan.f_tgt != s_accel_plan.f_end)) ? 1U : 0U; | |||||
| } | |||||
| old_dec_n = s_accel_plan.dec_n; | |||||
| old_t_dec = s_accel_plan.t_dec_ms; | |||||
| static uint32_t PlsrRunControlRemainPulses(void) | |||||
| { | |||||
| if (s_target > s_done) | |||||
| { | |||||
| return (uint32_t)(s_target - s_done); | |||||
| } | |||||
| return 0U; | |||||
| } | |||||
| static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) | |||||
| { | |||||
| PlsrRunControlEnterPhase(PH_DECEL, from_hz); | PlsrRunControlEnterPhase(PH_DECEL, from_hz); | ||||
| f_end = s_accel_plan.f_end; | |||||
| s_accel_plan.acc_n = 0U; | |||||
| s_accel_plan.const_n = 0U; | |||||
| s_accel_plan.dec_n = remain_pulses; | |||||
| s_accel_plan.f_tgt = from_hz; | |||||
| s_decel_from = from_hz; | |||||
| s_profile_dirty = 1U; | |||||
| if (remain_pulses == 0U) | if (remain_pulses == 0U) | ||||
| { | { | ||||
| s_accel_plan.dec_n = 0U; | |||||
| s_accel_plan.t_dec_ms = 0U; | s_accel_plan.t_dec_ms = 0U; | ||||
| return; | return; | ||||
| } | } | ||||
| s_accel_plan.dec_n = remain_pulses; | |||||
| if (from_hz == f_end) | |||||
| if (from_hz == s_accel_plan.f_end) | |||||
| { | { | ||||
| s_accel_plan.t_dec_ms = 0U; | s_accel_plan.t_dec_ms = 0U; | ||||
| return; | return; | ||||
| } | } | ||||
| /* | |||||
| * 规划阶段已按斜率算好 t_dec:优先沿用,避免 ISR 内改歪形状。 | |||||
| * 止速=0:始终用斜率时间;时间走完即停,不按剩余脉冲把 T 拉长 | |||||
| * (拉长会在尾部堆 1Hz 空耗)。 | |||||
| */ | |||||
| if ((old_t_dec > 0U) && (old_dec_n > 0U) && | |||||
| (remain_pulses == old_dec_n)) | |||||
| /* 时间字段仅预览兼容;运行取频走 FreqAtPulse */ | |||||
| if (s_accel_plan.t_dec_ms == 0U) | |||||
| { | { | ||||
| s_accel_plan.t_dec_ms = old_t_dec; | |||||
| return; | |||||
| s_accel_plan.t_dec_ms = 1U; | |||||
| } | } | ||||
| } | |||||
| /* 参数斜率:K = 默认速度 / 加减速时间(减 0) */ | |||||
| df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz); | |||||
| if (f_end > from_hz) | |||||
| { | |||||
| ref_ms = s_run_accel_ms; | |||||
| } | |||||
| else | |||||
| { | |||||
| ref_ms = s_run_decel_ms; | |||||
| } | |||||
| den = cfg->default_speed; | |||||
| if (den == 0U) | |||||
| /** | |||||
| * 进匀速:写一次 f_tgt,之后 RefreshProfile 不再周期性取频/改 ARR(防抖)。 | |||||
| * 注意:这不是禁止改频——ACT/EXT 打断、ChangeFreq、进减速、开下一段 | |||||
| * 都会离开 PH_CONST 或直接 Start/Stop,ApplyOutFreq 仍可改频。 | |||||
| */ | |||||
| static void PlsrRunControlEnterConstHold(uint8_t do_start) | |||||
| { | |||||
| uint32_t f_hold = s_accel_plan.f_tgt; | |||||
| if (f_hold < 1U) | |||||
| { | { | ||||
| den = df; | |||||
| f_hold = 1U; | |||||
| } | } | ||||
| if ((ref_ms > 0U) && (den > 0U)) | |||||
| PlsrRunControlEnterPhase(PH_CONST, f_hold); | |||||
| PlsrRunControlApplyOutFreq(f_hold, do_start); | |||||
| } | |||||
| static void PlsrRunControlRefreshProfile(uint8_t do_start) | |||||
| { | |||||
| uint32_t next; | |||||
| uint32_t done_n; | |||||
| uint32_t remain; | |||||
| done_n = 0U; | |||||
| if (s_done > 0) | |||||
| { | { | ||||
| t_slope = (df * ref_ms + den - 1UL) / den; | |||||
| if (t_slope < 1U) | |||||
| { | |||||
| t_slope = 1U; | |||||
| } | |||||
| done_n = (uint32_t)s_done; | |||||
| } | } | ||||
| remain = PlsrRunControlRemainPulses(); | |||||
| /* | /* | ||||
| * ISR 内禁止 FitRamp/EstimatePulses;用 O(1) 平均公式。 | |||||
| * 脉冲被截断导致 remain 变化时才走这里。 | |||||
| * 匀速 → 减速:只看剩余脉冲,不反复算频。 | |||||
| */ | */ | ||||
| sum_f = from_hz + f_end; | |||||
| if (sum_f == 0U) | |||||
| { | |||||
| sum_f = 1U; | |||||
| } | |||||
| if (remain_pulses > 1U) | |||||
| { | |||||
| t_fit = (2UL * (remain_pulses - 1UL) * 1000UL + sum_f - 1UL) / sum_f; | |||||
| } | |||||
| else | |||||
| if ((s_phase == PH_CONST) && | |||||
| (PlsrRunControlHasDecel() != 0U) && | |||||
| (remain <= s_accel_plan.dec_n)) | |||||
| { | { | ||||
| t_fit = 1U; | |||||
| } | |||||
| if (t_fit < 1U) | |||||
| { | |||||
| t_fit = 1U; | |||||
| PlsrRunControlEnterPhase(PH_DECEL, s_accel_plan.f_tgt); | |||||
| } | } | ||||
| if ((t_slope > 0U) && (t_slope <= t_fit)) | |||||
| { | |||||
| s_accel_plan.t_dec_ms = t_slope; | |||||
| } | |||||
| else | |||||
| /* | |||||
| * 匀速保持:跳过周期性 FreqAtPulse(防顶频抖动)。 | |||||
| * 仍允许:do_start / 停表后重开 写一次;打断切段走 BeginSeg/PlanSeg | |||||
| * 会改 s_phase,不再走本分支。 | |||||
| */ | |||||
| if (s_phase == PH_CONST) | |||||
| { | { | ||||
| s_accel_plan.t_dec_ms = t_fit; | |||||
| if ((do_start != 0U) || (s_pwm_on == 0U)) | |||||
| { | |||||
| PlsrRunControlApplyOutFreq(s_accel_plan.f_tgt, do_start); | |||||
| } | |||||
| return; | |||||
| } | } | ||||
| } | |||||
| static void PlsrRunControlRefreshProfile(uint8_t do_start) | |||||
| { | |||||
| uint32_t next; | |||||
| /* | |||||
| * 1ms 改频:段内统一时间轴;t_decel_start_ms 由规划反向算出, | |||||
| * 减速为 f_tgt→f_end 直线,自然落到止速,末拍不再强行对齐。 | |||||
| */ | |||||
| next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms); | |||||
| /* 加速 / 减速:按脉冲域算下一频率 */ | |||||
| next = PlsrAccelCurveFreqAtPulse(&s_accel_plan, done_n); | |||||
| if (next < 1U) | if (next < 1U) | ||||
| { | { | ||||
| next = 1U; | next = 1U; | ||||
| } | } | ||||
| if (s_phase == PH_APPROACH) | |||||
| { | |||||
| /* | |||||
| * 加速结束:脉冲用完或频率已到峰值 → 匀速保持(或直接进减速)。 | |||||
| */ | |||||
| if ((s_accel_plan.acc_n == 0U) || | |||||
| (done_n >= s_accel_plan.acc_n) || | |||||
| ((s_accel_plan.f_tgt >= s_accel_plan.f_cur) && | |||||
| (next >= s_accel_plan.f_tgt)) || | |||||
| ((s_accel_plan.f_tgt < s_accel_plan.f_cur) && | |||||
| (next <= s_accel_plan.f_tgt))) | |||||
| { | |||||
| next = s_accel_plan.f_tgt; | |||||
| if ((PlsrRunControlHasDecel() != 0U) && | |||||
| (remain <= s_accel_plan.dec_n)) | |||||
| { | |||||
| PlsrRunControlEnterPhase(PH_DECEL, next); | |||||
| } | |||||
| else | |||||
| { | |||||
| PlsrRunControlEnterConstHold(do_start); | |||||
| return; | |||||
| } | |||||
| } | |||||
| } | |||||
| if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U)) | if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U)) | ||||
| { | { | ||||
| PlsrRunControlApplyOutFreq(next, do_start); | PlsrRunControlApplyOutFreq(next, do_start); | ||||
| @@ -799,9 +818,8 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, | |||||
| f_start = s_accel_plan.f_cur; | f_start = s_accel_plan.f_cur; | ||||
| /* | /* | ||||
| * 1ms 改频模式:运行时按 TIM3 毫秒时间轴取频。 | |||||
| * 禁止在这里用脉冲递推模型二次拟合 T(FitRamp),否则规划与运行模型不一致, | |||||
| * 会出现中段拐折/扭曲、段末落不到设定 f_end。 | |||||
| * 脉冲域:运行时按已完成脉冲数 FreqAtPulse 取频。 | |||||
| * 禁止再用时间轴二次拟合,否则规划与运行不一致。 | |||||
| */ | */ | ||||
| /* 本段频率范围锁 PSC,升降只改 ARR;跨度过大则 LockPscRange 自动不锁 */ | /* 本段频率范围锁 PSC,升降只改 ARR;跨度过大则 LockPscRange 自动不锁 */ | ||||
| @@ -864,9 +882,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, | |||||
| } | } | ||||
| /* | /* | ||||
| * 保持 AccelCurvePlan 算出的斜率时间,不再 FitRamp 拉长 T。 | |||||
| * 脉冲数由 EstimatePulses 决定;运行时按相时间取频, | |||||
| * 一拍周期盖住 T 时自然落到相终点。 | |||||
| * 保持 AccelCurvePlan 算出的脉冲预算;运行时 FreqAtPulse 按 s_done 取频。 | |||||
| */ | */ | ||||
| /* | /* | ||||
| * 纯减速段:已在目标速(或更高)且整段预算不超过 dec_n。 | * 纯减速段:已在目标速(或更高)且整段预算不超过 dec_n。 | ||||
| @@ -887,7 +903,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, | |||||
| { | { | ||||
| PlsrRunControlEnterPhase(PH_APPROACH, f_start); | PlsrRunControlEnterPhase(PH_APPROACH, f_start); | ||||
| } | } | ||||
| s_seg_elapsed_ms = 0U; | |||||
| s_profile_dirty = 1U; | |||||
| } | } | ||||
| static void PlsrRunControlFinishAll(void) | static void PlsrRunControlFinishAll(void) | ||||
| @@ -902,6 +918,7 @@ static void PlsrRunControlFinishAll(void) | |||||
| s_follow_cont = 0U; | s_follow_cont = 0U; | ||||
| s_chain_valid = 0U; | s_chain_valid = 0U; | ||||
| s_chain_freq = 0U; | s_chain_freq = 0U; | ||||
| s_profile_dirty = 0U; | |||||
| s_act_armed = 0U; | s_act_armed = 0U; | ||||
| s_act_expire_req = 0U; | s_act_expire_req = 0U; | ||||
| s_act_cut_pending = 0U; | s_act_cut_pending = 0U; | ||||
| @@ -996,7 +1013,7 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0) | |||||
| if (s_follow_cont != 0U) | if (s_follow_cont != 0U) | ||||
| { | { | ||||
| PlsrPulseDriverClearOnePulseStop(); | PlsrPulseDriverClearOnePulseStop(); | ||||
| s_seg_elapsed_ms = 0U; | |||||
| s_profile_dirty = 0U; | |||||
| PlsrRunControlRefreshProfile(0U); | PlsrRunControlRefreshProfile(0U); | ||||
| s_state = RC_RUN; | s_state = RC_RUN; | ||||
| s_follow_cont = 0U; | s_follow_cont = 0U; | ||||
| @@ -1027,9 +1044,9 @@ void PlsrRunControlInit(void) | |||||
| s_run_accel_ms = 0U; | s_run_accel_ms = 0U; | ||||
| s_run_decel_ms = 0U; | s_run_decel_ms = 0U; | ||||
| s_phase = PH_CONST; | s_phase = PH_CONST; | ||||
| s_seg_elapsed_ms = 0U; | |||||
| s_approach_from = 0U; | s_approach_from = 0U; | ||||
| s_decel_from = 0U; | s_decel_from = 0U; | ||||
| s_profile_dirty = 0U; | |||||
| s_act_armed = 0U; | s_act_armed = 0U; | ||||
| s_act_expire_req = 0U; | s_act_expire_req = 0U; | ||||
| s_act_cut_pending = 0U; | s_act_cut_pending = 0U; | ||||
| @@ -1083,6 +1100,7 @@ void PlsrRunControlStop(void) | |||||
| s_follow_cont = 0U; | s_follow_cont = 0U; | ||||
| s_chain_valid = 0U; | s_chain_valid = 0U; | ||||
| s_chain_freq = 0U; | s_chain_freq = 0U; | ||||
| s_profile_dirty = 0U; | |||||
| s_act_armed = 0U; | s_act_armed = 0U; | ||||
| s_act_expire_req = 0U; | s_act_expire_req = 0U; | ||||
| s_act_cut_pending = 0U; | s_act_cut_pending = 0U; | ||||
| @@ -1116,7 +1134,7 @@ uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz) | |||||
| s_run_accel_ms, s_run_decel_ms); | s_run_accel_ms, s_run_decel_ms); | ||||
| s_target = (int32_t)remain; | s_target = (int32_t)remain; | ||||
| s_done = 0; | s_done = 0; | ||||
| s_seg_elapsed_ms = 0U; | |||||
| s_profile_dirty = 0U; | |||||
| PlsrRunControlRefreshProfile(0U); | PlsrRunControlRefreshProfile(0U); | ||||
| return 1U; | return 1U; | ||||
| } | } | ||||
| @@ -1150,7 +1168,7 @@ void PlsrRunControlTickMs(void) | |||||
| if (PlsrRunControlOsTimeReached(s_dir_deadline) != 0U) | if (PlsrRunControlOsTimeReached(s_dir_deadline) != 0U) | ||||
| { | { | ||||
| s_state = RC_RUN; | s_state = RC_RUN; | ||||
| s_seg_elapsed_ms = 0U; | |||||
| s_profile_dirty = 0U; | |||||
| PlsrRunControlRefreshProfile(1U); | PlsrRunControlRefreshProfile(1U); | ||||
| PlsrRunControlArmActExtOnPulseStart(); | PlsrRunControlArmActExtOnPulseStart(); | ||||
| } | } | ||||
| @@ -1159,7 +1177,24 @@ void PlsrRunControlTickMs(void) | |||||
| if (s_state == RC_RUN) | if (s_state == RC_RUN) | ||||
| { | { | ||||
| /* 波形改频 / ACT 计时在 TIM3 On1ms;此处只处理 EXT */ | |||||
| /* | |||||
| * 仅加/减速(或匀速刚到进减速点)才改频。 | |||||
| * 匀速锁定期间 dirty 不会置位,避免顶频反复写 ARR。 | |||||
| */ | |||||
| if ((s_profile_dirty != 0U) && (s_act_cut_pending == 0U)) | |||||
| { | |||||
| s_profile_dirty = 0U; | |||||
| PlsrRunControlRefreshProfile(0U); | |||||
| } | |||||
| else if ((s_phase == PH_CONST) && | |||||
| (PlsrRunControlHasDecel() != 0U) && | |||||
| (s_act_cut_pending == 0U) && | |||||
| (PlsrRunControlRemainPulses() <= s_accel_plan.dec_n)) | |||||
| { | |||||
| /* 任务侧兜底:进减速 */ | |||||
| PlsrRunControlRefreshProfile(0U); | |||||
| } | |||||
| seg = PlsrParamGetSeg(s_cur_seg); | seg = PlsrParamGetSeg(s_cur_seg); | ||||
| if ((seg->wait_type == PLSR_WAIT_EXT) || | if ((seg->wait_type == PLSR_WAIT_EXT) || | ||||
| @@ -1199,8 +1234,8 @@ void PlsrRunControlTickMs(void) | |||||
| } | } | ||||
| /** | /** | ||||
| * TIM3 1ms:段内时间累加 + 按时间轴 RefreshProfile 改频。 | |||||
| * ACT/WAIT 只置标志,切段在 TickMs(避免 ISR 里 PlanSeg 丢拍)。 | |||||
| * TIM3 1ms:仅 ACT/WAIT 时基(不再改频)。 | |||||
| * 切段在 TickMs(避免 ISR 里 PlanSeg 丢拍)。 | |||||
| */ | */ | ||||
| void PlsrRunControlOn1ms(void) | void PlsrRunControlOn1ms(void) | ||||
| { | { | ||||
| @@ -1220,35 +1255,23 @@ void PlsrRunControlOn1ms(void) | |||||
| return; | return; | ||||
| } | } | ||||
| if (s_seg_elapsed_ms < 0xFFFFFFFFUL) | |||||
| { | |||||
| s_seg_elapsed_ms++; | |||||
| } | |||||
| if ((s_act_armed != 0U) && | if ((s_act_armed != 0U) && | ||||
| (PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) != 0U)) | (PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) != 0U)) | ||||
| { | { | ||||
| /* | /* | ||||
| * ACT 是运动控制条件,不参与曲线规划:加速、匀速、减速 | * ACT 是运动控制条件,不参与曲线规划:加速、匀速、减速 | ||||
| * 任一阶段到期都在当前脉冲 UPDATE 边界切段。这样不会在 | |||||
| * 一个 PWM 周期中间改段,也不会把边界脉冲计到下一段。 | |||||
| * 任一阶段到期都在当前脉冲 UPDATE 边界切段。 | |||||
| */ | */ | ||||
| s_act_armed = 0U; | s_act_armed = 0U; | ||||
| s_act_cut_pending = 1U; | s_act_cut_pending = 1U; | ||||
| } | } | ||||
| /* 保持 ACT 到期瞬间的频率,不让 1ms 曲线继续推进到下一个边界。 */ | |||||
| if (s_act_cut_pending != 0U) | |||||
| { | |||||
| return; | |||||
| } | |||||
| PlsrRunControlRefreshProfile(0U); | |||||
| } | } | ||||
| /** 脉冲 UPDATE:只累计 s_done / s_acc_pulse,不改频 */ | |||||
| /** 脉冲 UPDATE:计脉冲;仅加/减速置 dirty(匀速不改频) */ | |||||
| void PlsrRunControlOnPulseIsr(void) | void PlsrRunControlOnPulseIsr(void) | ||||
| { | { | ||||
| uint32_t remain; | |||||
| if (s_state != RC_RUN) | if (s_state != RC_RUN) | ||||
| { | { | ||||
| return; | return; | ||||
| @@ -1264,6 +1287,26 @@ void PlsrRunControlOnPulseIsr(void) | |||||
| s_acc_pulse--; | s_acc_pulse--; | ||||
| } | } | ||||
| /* | |||||
| * 匀速:不置 dirty,定时器 ARR 保持不动。 | |||||
| * 仅当剩余脉冲进入减速预算时置位,让任务切到 DECEL。 | |||||
| */ | |||||
| if (s_phase == PH_CONST) | |||||
| { | |||||
| if (PlsrRunControlHasDecel() != 0U) | |||||
| { | |||||
| remain = PlsrRunControlRemainPulses(); | |||||
| if (remain <= s_accel_plan.dec_n) | |||||
| { | |||||
| s_profile_dirty = 1U; | |||||
| } | |||||
| } | |||||
| } | |||||
| else | |||||
| { | |||||
| s_profile_dirty = 1U; | |||||
| } | |||||
| /* | /* | ||||
| * ACT 到期切段只在脉冲边界执行,避免半个 PWM 周期改段。 | * ACT 到期切段只在脉冲边界执行,避免半个 PWM 周期改段。 | ||||
| * 同向衔接:不停表,任务里用当前频率规划下一段(丢掉本段剩余)。 | * 同向衔接:不停表,任务里用当前频率规划下一段(丢掉本段剩余)。 | ||||
| @@ -11,9 +11,9 @@ void PlsrRunControlInit(void); | |||||
| uint8_t PlsrRunControlStart(uint16_t start_seg_1based); | uint8_t PlsrRunControlStart(uint16_t start_seg_1based); | ||||
| void PlsrRunControlStop(void); | void PlsrRunControlStop(void); | ||||
| uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz); | uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz); | ||||
| void PlsrRunControlTickMs(void); | |||||
| void PlsrRunControlOn1ms(void); /* TIM3 1ms:改频 + WAIT/ACT 时基 */ | |||||
| void PlsrRunControlOnPulseIsr(void); /* 脉冲 UPDATE:只计脉冲 */ | |||||
| void PlsrRunControlOn1ms(void); /* TIM3 1ms:仅 ACT/WAIT 时基 */ | |||||
| void PlsrRunControlOnPulseIsr(void); /* 脉冲 UPDATE:计脉冲 + dirty */ | |||||
| void PlsrRunControlTickMs(void); /* 任务:脉冲域改频 + EXT/到期切段 */ | |||||
| uint8_t PlsrRunControlIsBusy(void); | uint8_t PlsrRunControlIsBusy(void); | ||||
| int32_t PlsrRunControlGetAccPulse(void); | int32_t PlsrRunControlGetAccPulse(void); | ||||
| void PlsrRunControlClearAccPulse(void); | void PlsrRunControlClearAccPulse(void); | ||||