From 4732a0a3e5f2ea5137a1d5d77ce2302a3f9499bc Mon Sep 17 00:00:00 2001 From: hanyongwei <2043702190@qq.com> Date: Sat, 29 Aug 2026 16:13:44 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E7=9B=B4=E7=BA=BF?= =?UTF-8?q?=E5=8A=A0=E5=87=8F=E9=80=9F=E9=A2=91=E7=8E=87=E7=AE=97=E6=B3=95?= =?UTF-8?q?=EF=BC=8C=E8=BE=93=E5=87=BA=E9=A2=91=E7=8E=87=E4=B8=8E=E8=84=89?= =?UTF-8?q?=E5=86=B2=E4=B8=AA=E6=95=B0=E9=83=BD=E4=B8=8D=E5=87=86=E4=BA=86?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- plsr/accel_curve/plsr_accel_curve.c | 372 ++++++++++++++++++-------- plsr/accel_curve/plsr_accel_curve.h | 25 +- plsr/param/plsr_param.h | 4 +- plsr/pulse_driver/plsr_pulse_driver.c | 13 +- plsr/run_control/plsr_run_control.c | 106 +++++++- 5 files changed, 387 insertions(+), 133 deletions(-) diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 88041d4..d64d6e1 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -1,6 +1,6 @@ /** * @file plsr_accel_curve.c - * @brief 脉冲域规划/取频：加速 f^2=start^2+2an；减速 f^2=end^2+2ak(k=N…1)；S/正弦时间轴 + * @brief 脉冲域规划/取频：直线邻项平均；S/正弦时间轴查表 * * ============================================================================ * 【阅读指南 — 先看这个再往下翻 if/else】 @@ -20,8 +20,10 @@ * decel_rate_hz_per_s 减速斜率 Hz/s ≈ default_speed*1000/deceleration_time_ms * * 二、三种曲线模式（curve_mode） - * LINEAR 加速：f=sqrt(start^2+2a·n)；减速：从终点往峰值加 - * f=sqrt(end^2+2a·k)，k=N…1（与加速对称，勿从峰值减计数） + * LINEAR S_x=sqrt(f0^2+2a·x)；第 k 拍命令频=(S_{k-1}+S_k)/2；末拍钉相终点 + * N=floor(|ft^2-f0^2|/(2a))（非零斜坡且算得 0 → 1） + * 减速以终点为 f0 往峰值建 S_N…S_0，首减拍 (S_N+S_{N-1})/2 + * ARPE：冷启动须 Prime/NextFrequency 一次再 Start，避免首频写两遍 * S 时间域 jerk 梯形；规划/ISR 用离散 Δt=1/frequency_hz 仿真；ISR 查预建表 * SINE raised-cosine 时间剖面；同上，查预建表 * @@ -39,7 +41,7 @@ * * 五、FreqAtPulse 的 completed_segment_pulses 含义（易混） * completed_segment_pulses = 已完成脉冲数（不是「当前第几拍」） - * done=0 → 第 1 拍频率 = start_frequency_hz；done=1 → 第 2 拍 ≈ sqrt(start_frequency_hz^2+2a) … + * done=0 → 第 1 拍命令频 = (S_0+S_1)/2；done=k → 第 k+1 拍；末拍钉终点 * * @details 模块职责 * 实现 plsr_accel_curve.h 全部 API。核心路径： @@ -74,25 +76,39 @@ static uint32_t PlsrAccelCurveAbsoluteDifference(uint32_t first_value, uint32_t (first_value - second_value) : (second_value - first_value); } -/** 64 位整数平方根（牛顿法），支持 frequency_hz^2 + 2an */ +/** 64 位整数平方根（牛顿法）。初值必须 ≥ 真根，否则一步上冲会误停。 */ static uint32_t PlsrAccelCurveIntegerSquareRoot(uint64_t squared_frequency) { uint64_t estimate; uint64_t next_estimate; + uint32_t bit; uint32_t loop_index; if (squared_frequency <= 1ULL) { return (uint32_t)squared_frequency; } - estimate = squared_frequency; - if (estimate > 100000ULL) + + /* 按最高位置初值：略大于 √x，保证从上方收敛（过低会一步上冲误停） */ + bit = 0U; + { + uint64_t temp = squared_frequency; + + while (temp > 1ULL) + { + temp >>= 1; + bit++; + } + } + estimate = 1ULL << ((bit + 2U) / 2U); + if (estimate < 1ULL) { - estimate = 100000ULL; + estimate = 1ULL; } - for (loop_index = 0U; loop_index < 40U; loop_index++) + + for (loop_index = 0U; loop_index < 48U; loop_index++) { - next_estimate = (estimate + squared_frequency / estimate) / 2ULL; + next_estimate = (estimate + (squared_frequency / estimate)) / 2ULL; if (next_estimate >= estimate) { break; @@ -116,7 +132,7 @@ static uint32_t PlsrAccelCurveRateFromDefaultSpeed(uint32_t default_speed_hz, ui { return 0U; } - return (default_speed_hz * 1000UL) / ramp_time_ms; + return (uint32_t)(((uint64_t)default_speed_hz * 1000ULL) / (uint64_t)ramp_time_ms); } /** 按默认速度计算变化率；默认速度未配置时用本段频差计算。 */ @@ -166,14 +182,14 @@ static uint32_t PlsrAccelCurveCalculateJumpFrequency(uint32_t starting_frequency } /** - * 解析配置起速/止速 → 本段实际入口或出口频率。 + * 解析配置起速/止速 → 本段数学入口或出口频率 f0 / f_end。 * - * 三分支（按顺序判断，命中即 return）： - * ① configured_frequency_hz > target_frequency_hz → 直接用 configured_frequency_hz（例如起速 500、目标 200：先高位再减） - * ② target_frequency_hz < jump_frequency_hz → 用 target_frequency_hz（目标比起跳还低，没必要从 jump_frequency_hz 起） - * ③ 否则 → 用 jump_frequency_hz = sqrt(configured_frequency_hz^2 + 2a) + * 直线邻项平均：S_x=√(f0²+2ax)，首拍 (f0+S1)/2；起速=0 时 f0=0、S1=√(2a)， + * 不再把「起跳 √(f0²+2a)」当作段入口（否则首频变成 (S1+S2)/2≈170）。 * - * resolving_start=1 用 acceleration_time_ms 算 a；resolving_start=0 用 deceleration_time_ms（ResolveEndHz 止速侧）。 + * ① configured > target → 用 configured（高位起再降） + * ② a=0 且 configured=0 → 用 target（阶跃） + * ③ 否则 → 用 configured（可为 0） */ static uint32_t PlsrAccelCurveResolveBoundaryFrequency(uint32_t configured_frequency_hz, uint32_t target_frequency_hz, @@ -184,7 +200,6 @@ static uint32_t PlsrAccelCurveResolveBoundaryFrequency(uint32_t configured_frequ { uint32_t ramp_time_ms; uint32_t acceleration_hz_per_s; - uint32_t jump_frequency_hz; configured_frequency_hz = PlsrAccelCurveClampFrequencyHz(configured_frequency_hz); target_frequency_hz = PlsrAccelCurveClampFrequencyHz(target_frequency_hz); @@ -210,14 +225,7 @@ static uint32_t PlsrAccelCurveResolveBoundaryFrequency(uint32_t configured_frequ return (configured_frequency_hz >= 1U) ? configured_frequency_hz : target_frequency_hz; } - jump_frequency_hz = PlsrAccelCurveCalculateJumpFrequency(configured_frequency_hz, acceleration_hz_per_s); - - /* 目标 < 起跳 → 用目标；目标 >= 起跳 → 用起跳 */ - if (target_frequency_hz < jump_frequency_hz) - { - return target_frequency_hz; - } - return jump_frequency_hz; + return configured_frequency_hz; } uint32_t PlsrAccelCurveResolveStartHz(uint32_t configured_frequency_hz, @@ -297,8 +305,8 @@ static uint32_t PlsrAccelCurveInterpolate(uint32_t start_value, } /** - * 脉冲闭合：N = ceil(|end_frequency_hz^2 - start_frequency_hz^2| / (2a))。 - * a=0 且频差非 0 → 0（阶跃，无斜坡脉冲）。 + * 脉冲闭合：N = floor(|end_frequency_hz^2 - start_frequency_hz^2| / (2a))。 + * 非零斜坡且算得 0 → 1。a=0 且频差非 0 → 0（阶跃，无斜坡脉冲）。 */ static uint32_t PlsrAccelCurveCalculateLinearRampPulses(uint32_t start_frequency_hz, uint32_t end_frequency_hz, uint32_t acceleration_hz_per_s) @@ -320,9 +328,11 @@ static uint32_t PlsrAccelCurveCalculateLinearRampPulses(uint32_t start_frequency start_frequency_squared = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz; end_frequency_squared = (uint64_t)end_frequency_hz * (uint64_t)end_frequency_hz; - squared_frequency_difference = (end_frequency_squared > start_frequency_squared) ? (end_frequency_squared - start_frequency_squared) : (start_frequency_squared - end_frequency_squared); + squared_frequency_difference = (end_frequency_squared > start_frequency_squared) ? + (end_frequency_squared - start_frequency_squared) : + (start_frequency_squared - end_frequency_squared); twice_acceleration = 2ULL * (uint64_t)acceleration_hz_per_s; - pulse_count = (squared_frequency_difference + twice_acceleration - 1ULL) / twice_acceleration; + pulse_count = squared_frequency_difference / twice_acceleration; if (pulse_count == 0ULL) { pulse_count = 1ULL; @@ -559,11 +569,11 @@ static uint32_t PlsrAccelCurveCalculateRampPulses(uint32_t start_frequency_hz, u } /** - * 直线取频。 - * 加速(rising)：f = sqrt(start^2 + 2·a·pulse_count)，不超过 target_limit - * 减速(!rising)：从终点往峰值加 — pulse_count 为相内已完成步数(0…N-1)时 - * k = N - pulse_count，f = sqrt(end_limit^2 + 2·a·k)，不超过 start(峰值) - * 例 N=5：第 1 个减速拍 k=5→end^2+10a；末拍 k=1→end^2+2a + * 直线邻项平均取频（任务侧预览）。 + * pulse_count = 相内已完成脉冲数 → 返回第 (pulse_count+1) 拍命令频。 + * rising：S_x=sqrt(start^2+2a·x)，S_0=start；第 k 拍 (S_{k-1}+S_k)/2 + * 末拍：S_N 取相终点 target_limit，(S_{N-1}+target_limit)/2（不钉死终点） + * 减速：S_m=sqrt(end^2+2a·m)；末拍 (S_1+end)/2 */ static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz, uint32_t acceleration_hz_per_s, uint32_t pulse_count, uint8_t frequency_rising, @@ -571,8 +581,11 @@ static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz uint32_t phase_total_pulses) { uint64_t squared_frequency; + uint32_t s_prev; + uint32_t s_next; uint32_t frequency_hz; - uint32_t k_from_end; + uint32_t k_hi; + uint32_t k_lo; if (acceleration_hz_per_s == 0U) { @@ -580,46 +593,86 @@ static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz return PlsrAccelCurveClampFrequencyHz((frequency_hz < 1U) ? 1U : frequency_hz); } + if (phase_total_pulses < 1U) + { + frequency_hz = target_limit_hz; + if (frequency_hz < 1U) + { + frequency_hz = 1U; + } + return PlsrAccelCurveClampFrequencyHz(frequency_hz); + } + if (frequency_rising != 0U) { + if (pulse_count + 1U >= phase_total_pulses) + { + /* 升频末拍：直接相终点（进匀速后锁定，不再平均） */ + frequency_hz = target_limit_hz; + if (frequency_hz < 1U) + { + frequency_hz = 1U; + } + return PlsrAccelCurveClampFrequencyHz(frequency_hz); + } if (pulse_count == 0U) { - frequency_hz = start_frequency_hz; + s_prev = start_frequency_hz; } else { squared_frequency = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz + (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)pulse_count); - frequency_hz = PlsrAccelCurveIntegerSquareRoot(squared_frequency); - if (frequency_hz > target_limit_hz) - { - frequency_hz = target_limit_hz; - } + s_prev = PlsrAccelCurveIntegerSquareRoot(squared_frequency); } + squared_frequency = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz + + (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)(pulse_count + 1U)); + s_next = PlsrAccelCurveIntegerSquareRoot(squared_frequency); + frequency_hz = (s_prev / 2U) + (s_next / 2U) + ((s_prev & 1U) + (s_next & 1U)) / 2U; } else { - /* 减速：以终点 target_limit_hz 为 f0，k 从 N 倒数到 1 */ - if (phase_total_pulses < 1U) - { - frequency_hz = target_limit_hz; - } - else if (pulse_count >= phase_total_pulses) + if (pulse_count + 1U >= phase_total_pulses) { - frequency_hz = target_limit_hz; + /* 末拍：(S_1 + end)/2 */ + squared_frequency = (uint64_t)target_limit_hz * (uint64_t)target_limit_hz + + (2ULL * (uint64_t)acceleration_hz_per_s); + s_prev = PlsrAccelCurveIntegerSquareRoot(squared_frequency); + if (s_prev > start_frequency_hz) + { + s_prev = start_frequency_hz; + } + s_next = target_limit_hz; } else { - k_from_end = phase_total_pulses - pulse_count; + k_hi = phase_total_pulses - pulse_count; + k_lo = k_hi - 1U; squared_frequency = (uint64_t)target_limit_hz * (uint64_t)target_limit_hz - + (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)k_from_end); - frequency_hz = PlsrAccelCurveIntegerSquareRoot(squared_frequency); - if (frequency_hz > start_frequency_hz) + + (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)k_hi); + s_prev = PlsrAccelCurveIntegerSquareRoot(squared_frequency); + if (s_prev > start_frequency_hz) + { + s_prev = start_frequency_hz; + } + if (k_lo == 0U) + { + s_next = target_limit_hz; + } + else { - frequency_hz = start_frequency_hz; + squared_frequency = (uint64_t)target_limit_hz * (uint64_t)target_limit_hz + + (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)k_lo); + s_next = PlsrAccelCurveIntegerSquareRoot(squared_frequency); + if (s_next > start_frequency_hz) + { + s_next = start_frequency_hz; + } } } + frequency_hz = (s_prev / 2U) + (s_next / 2U) + ((s_prev & 1U) + (s_next & 1U)) / 2U; } + if (frequency_hz < 1U) { frequency_hz = 1U; @@ -824,17 +877,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, selected_mode = (curve_mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : curve_mode; - if (start_frequency_hz < 1U) - { - start_frequency_hz = PlsrAccelCurveResolveStartHz(0U, target_frequency_hz, default_speed_hz, - acceleration_time_ms, deceleration_time_ms); - } - if (end_frequency_hz < 1U) - { - end_frequency_hz = PlsrAccelCurveResolveEndHz(0U, target_frequency_hz, default_speed_hz, - acceleration_time_ms, deceleration_time_ms); - } - accel_rate_hz_per_s = PlsrAccelCurveCalculateRate(default_speed_hz, start_frequency_hz, @@ -846,6 +888,35 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, end_frequency_hz, deceleration_time_ms); + /* + * 直线：允许 f0/f_end=0（邻项平均首末拍用）。 + * S/正弦：时间仿真不能从 0 起，入口/出口 <1 时用起跳/落地 √(2a)。 + */ + if (start_frequency_hz < 1U) + { + if (selected_mode == PLSR_ACCEL_LINEAR) + { + start_frequency_hz = 0U; + } + else + { + start_frequency_hz = + PlsrAccelCurveCalculateJumpFrequency(0U, accel_rate_hz_per_s); + } + } + if (end_frequency_hz < 1U) + { + if (selected_mode == PLSR_ACCEL_LINEAR) + { + end_frequency_hz = 0U; + } + else + { + end_frequency_hz = + PlsrAccelCurveCalculateJumpFrequency(0U, decel_rate_hz_per_s); + } + } + peak_frequency_hz = target_frequency_hz; plan->start_frequency_hz = start_frequency_hz; @@ -1006,7 +1077,8 @@ static uint32_t PlsrAccelCurveTimedFrequencyAtPulse( * completed_segment_pulses ∈ [accel_pulses+constant_pulses, …) → 减速相（target_frequency_hz → end_frequency_hz） * * 注意 completed_segment_pulses 是「已经发完的个数」： - * Start 后第 1 拍进行中时 done 仍为 0，FreqAtPulse(0)=start_frequency_hz。 + * Start 后第 1 拍进行中时 done 仍为 0，直线 FreqAtPulse(0)=(S0+S1)/2。 + * ARPE 冷启动勿只靠本函数：须 Prime 把 runtime.completed 推到 1，否则 ISR 会再写一遍首频。 */ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, uint32_t completed_segment_pulses) @@ -1148,6 +1220,9 @@ static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, uint32_t decel_rate_hz_per_s, PlsrAccelMode_e curve_mode) { + uint64_t squared_frequency; + uint32_t s_n; + runtime->start_frequency_hz = start_frequency_hz; runtime->end_frequency_hz = end_frequency_hz; runtime->current_frequency_hz = start_frequency_hz; @@ -1164,6 +1239,26 @@ static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, runtime->frequency_table_id = 0U; runtime->table_length = 0U; runtime->table_stride = 1U; + /* 直线：升频 prev=S0=start；降频 prev=S_N（从终点往峰值建） */ + runtime->linear_prev_s_hz = start_frequency_hz; + if ((curve_mode == PLSR_ACCEL_LINEAR) && + (runtime->frequency_rising == 0U) && + (runtime->acceleration_hz_per_s > 0U) && + (total_pulses > 0U)) + { + squared_frequency = (uint64_t)end_frequency_hz * (uint64_t)end_frequency_hz + + (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)total_pulses); + s_n = PlsrAccelCurveIntegerSquareRoot(squared_frequency); + if (s_n > start_frequency_hz) + { + s_n = start_frequency_hz; + } + if (s_n < 1U) + { + s_n = 1U; + } + runtime->linear_prev_s_hz = s_n; + } PlsrAccelRuntimeCalculateTimes(runtime); } @@ -1416,59 +1511,78 @@ void PlsrAccelBeginConstantSpeed(PlsrAccelRuntime_t *runtime, runtime->jerk_time_us = 0U; runtime->constant_accel_time_us = 0U; runtime->ramp_time_us = 0U; + runtime->linear_prev_s_hz = 0U; } /** - * 直线 ISR 取频（开方，与规划公式一致）。 - * 加速：f = sqrt(start^2 + 2·a·completed) - * 减速：k = total-completed+1，f = sqrt(end^2 + 2·a·k)（从终点往峰值加） + * 直线 ISR 取频：邻项平均。 + * S_x=√(f0²+2a·x) 是瞬时速度，不是命令频率；命令频=(S_{k-1}+S_k)/2。 + * linear_prev_s_hz 只存上一拍的 S（开方值），绝不能存输出频率。 + * 升频末拍直接钉相终点（进匀速锁定）；减速末拍仍 (S1+end)/2。 */ static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime) { - uint64_t target_frequency_squared; + uint64_t squared_frequency; + uint32_t s_new; uint32_t frequency_hz; - uint32_t k_from_end; + uint32_t k_lo; if (runtime->acceleration_hz_per_s == 0U) { - return PlsrAccelCurveClampFrequencyHz(runtime->end_frequency_hz); + return PlsrAccelCurveClampFrequencyHz( + (runtime->end_frequency_hz < 1U) ? 1U : runtime->end_frequency_hz); + } + + /* 升频相结束：锁定峰值，不再开方/平均 */ + if ((runtime->total_pulses > 0U) && + (runtime->completed_pulses >= runtime->total_pulses) && + (runtime->frequency_rising != 0U)) + { + return PlsrAccelCurveClampFrequencyHz( + (runtime->end_frequency_hz < 1U) ? 1U : runtime->end_frequency_hz); } - if (runtime->frequency_rising != 0U) + if ((runtime->total_pulses > 0U) && + (runtime->completed_pulses >= runtime->total_pulses)) + { + /* 减速末拍：与相终点平均 */ + s_new = runtime->end_frequency_hz; + } + else if (runtime->frequency_rising != 0U) { - target_frequency_squared = + squared_frequency = (uint64_t)runtime->start_frequency_hz * (uint64_t)runtime->start_frequency_hz + (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)runtime->completed_pulses); - frequency_hz = PlsrAccelCurveIntegerSquareRoot(target_frequency_squared); - if (frequency_hz > runtime->end_frequency_hz) + s_new = PlsrAccelCurveIntegerSquareRoot(squared_frequency); + if (s_new > runtime->end_frequency_hz) { - frequency_hz = runtime->end_frequency_hz; + s_new = runtime->end_frequency_hz; } } else { - if (runtime->total_pulses < 1U) + k_lo = runtime->total_pulses - runtime->completed_pulses; + if (k_lo == 0U) { - k_from_end = 1U; - } - else if (runtime->completed_pulses >= runtime->total_pulses) - { - k_from_end = 1U; + s_new = runtime->end_frequency_hz; } else { - k_from_end = runtime->total_pulses - runtime->completed_pulses + 1U; - } - target_frequency_squared = - (uint64_t)runtime->end_frequency_hz * (uint64_t)runtime->end_frequency_hz - + (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)k_from_end); - frequency_hz = PlsrAccelCurveIntegerSquareRoot(target_frequency_squared); - if (frequency_hz > runtime->start_frequency_hz) - { - frequency_hz = runtime->start_frequency_hz; + squared_frequency = + (uint64_t)runtime->end_frequency_hz * (uint64_t)runtime->end_frequency_hz + + (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)k_lo); + s_new = PlsrAccelCurveIntegerSquareRoot(squared_frequency); + if (s_new > runtime->start_frequency_hz) + { + s_new = runtime->start_frequency_hz; + } } } + frequency_hz = (runtime->linear_prev_s_hz / 2U) + (s_new / 2U) + + ((runtime->linear_prev_s_hz & 1U) + (s_new & 1U)) / 2U; + runtime->linear_prev_s_hz = s_new; + if (frequency_hz < 1U) { frequency_hz = 1U; @@ -1476,6 +1590,50 @@ static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime) return PlsrAccelCurveClampFrequencyHz(frequency_hz); } +uint32_t PlsrAccelCurveLinearMinCommandHz(uint32_t boundary_frequency_hz, + uint32_t rate_hz_per_s) +{ + uint32_t s1; + uint32_t min_hz; + + if (boundary_frequency_hz >= 1U) + { + return PlsrAccelCurveClampFrequencyHz(boundary_frequency_hz); + } + if (rate_hz_per_s < 1U) + { + return 1U; + } + s1 = PlsrAccelCurveCalculateJumpFrequency(0U, rate_hz_per_s); + min_hz = s1 / 2U; + if (min_hz < 1U) + { + min_hz = 1U; + } + return PlsrAccelCurveClampFrequencyHz(min_hz); +} + +/** + * 直线冷启动：推进一拍得到第 1 拍命令频（ARPE Start 用），runtime.completed=1。 + */ +uint32_t PlsrAccelPrimeLinearStartFrequency(PlsrAccelRuntime_t *runtime) +{ + if (runtime == (PlsrAccelRuntime_t *)0) + { + return 0U; + } + if (runtime->curve_mode != PLSR_ACCEL_LINEAR) + { + return PlsrAccelCurveClampFrequencyHz(runtime->current_frequency_hz); + } + if (runtime->is_active == 0U) + { + return PlsrAccelCurveClampFrequencyHz( + (runtime->current_frequency_hz < 1U) ? 1U : runtime->current_frequency_hz); + } + return PlsrAccelNextFrequency(runtime); +} + /** * S/正弦时间域步进：上一拍周期推进 t，再按剖面取下一拍频率。 */ @@ -1569,11 +1727,12 @@ static uint32_t PlsrAccelCurveSimulateRampPulses(uint32_t start_frequency_hz, ui * 调用时机：每个 PWM UPDATE 中断里，在本段已发脉冲数加一之后。 * * 模式分支： - * LINEAR → SquareStep：用 frequency_hz±1 逼近 sqrt(start_frequency_hz^2±2an)，不全量开方 + * LINEAR → 邻项平均 (S_{k-1}+S_k)/2；prev 存 S 非输出频；末拍与相终点平均 * S/SINE+表 → 查 s_acceleration_frequency_table / s_deceleration_frequency_table（Prebuild 填好），table_index=(pulse_count-1)/table_stride * S/SINE无表 → 兜底 TimeStep（不应出现在正常路径） * - * pulse_count 含义：本相已 Step 次数；BeginAcc 后第 1 次 Step 时 pulse_count 变为 1，对应「第 2 拍」频率。 + * pulse_count 含义：本相已 Step 次数；Begin 后第 1 次 Step 时 pulse_count=1，对应第 1 拍命令频。 + * ARPE 冷启动须先 Step/Prime 再 Start，使首 UPDATE 写第 2 拍。 */ uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime) { @@ -1637,11 +1796,10 @@ uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime) if ((runtime->total_pulses > 0U) && (runtime->completed_pulses >= runtime->total_pulses)) { /* - * 直线减速末步已是 sqrt(end^2+2a)，不要再钉成 end（否则与「从终点往上加」不一致）。 - * S/正弦 / 加速相：仍钉到相终点。 + * 直线：末拍已是 (prev+end)/2，勿再钉 end。 + * S/正弦：仍钉到相终点。 */ - if (!((runtime->curve_mode == PLSR_ACCEL_LINEAR) && - (runtime->frequency_rising == 0U))) + if (runtime->curve_mode != PLSR_ACCEL_LINEAR) { runtime->current_frequency_hz = runtime->end_frequency_hz; } @@ -1654,19 +1812,23 @@ uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime) runtime->current_frequency_hz = runtime->end_frequency_hz; runtime->is_active = 0U; } - else if (runtime->frequency_rising != 0U) + else if (runtime->curve_mode != PLSR_ACCEL_LINEAR) { - if (runtime->current_frequency_hz >= runtime->end_frequency_hz) + /* 时间剖面：到频即收；直线只靠 total_pulses 末拍钉终点 */ + if (runtime->frequency_rising != 0U) + { + if (runtime->current_frequency_hz >= runtime->end_frequency_hz) + { + runtime->current_frequency_hz = runtime->end_frequency_hz; + runtime->is_active = 0U; + } + } + else if (runtime->current_frequency_hz <= runtime->end_frequency_hz) { runtime->current_frequency_hz = runtime->end_frequency_hz; runtime->is_active = 0U; } } - else if (runtime->current_frequency_hz <= runtime->end_frequency_hz) - { - runtime->current_frequency_hz = runtime->end_frequency_hz; - runtime->is_active = 0U; - } } return PlsrAccelCurveClampFrequencyHz(runtime->current_frequency_hz); } diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index 0f32beb..d66b0fc 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -1,13 +1,14 @@ /** * @file plsr_accel_curve.h - * @brief 脉冲域加/匀/减规划与取频（运动学：v^2 - v0^2 = 2ax） + * @brief 脉冲域加/匀/减规划与取频 * * @details 按起/峰/止频率、斜率时间与总脉冲规划三相脉冲预算，并在脉冲序号上取频。 * 不读 wait_type，不碰 TIM/GPIO。 * - * 直线：下一频率由起点频率、变化率和已完成脉冲数计算（ISR 增量逼近）。 + * 直线：S_x=sqrt(f0^2+2ax)；第 k 拍输出 (S_{k-1}+S_k)/2（S_0=f0）； + * 脉冲数 N=floor(|ft^2-f0^2|/(2a))；升频末拍钉峰值后进匀速锁定；减速末拍 (S1+end)/2。 + * 起速/止速配置值即为 f0/f_end（可为 0）；√(f0^2+2a) 是 S1 不是入口。 * S/正弦：开相时预计算频率表；ISR 只按脉冲序号查表（匀速相不改频）。 - * 规划脉冲数与建表使用同一套离散逐拍仿真。 * * 术语（全模块统一）： * 字段使用完整名称和单位，例如 accel_pulses、start_frequency_hz， @@ -69,8 +70,7 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, /** * 单相逐脉冲运行态（ISR 热路径，每来一个 UPDATE 调 Step 一次） - * 直线：每次按目标平方值逐步逼近下一频率 - * S/正弦：查预计算频率表（开相 BeginAcc/BeginDec 时填好） + * 直线：邻项平均 (S_{k-1}+S_k)/2，复用上一拍 S；S/正弦：查预建表 */ typedef struct { uint32_t current_frequency_hz; /* 当前输出频率 */ @@ -79,6 +79,7 @@ typedef struct { uint32_t acceleration_hz_per_s; /* 当前相使用的变化率 */ uint32_t completed_pulses; /* 当前相已经步进的脉冲数 */ uint32_t total_pulses; /* 当前相计划步进的脉冲数 */ + uint32_t linear_prev_s_hz; /* 直线：上一拍 S（瞬时 sqrt） */ uint32_t elapsed_time_us; /* 时间曲线已经经过的时间 */ uint32_t jerk_time_us; /* S 曲线单个 jerk 阶段时间 */ uint32_t constant_accel_time_us; /* S 曲线恒加速阶段时间 */ @@ -105,7 +106,19 @@ void PlsrAccelBeginDeceleration(PlsrAccelRuntime_t *runtime, void PlsrAccelBeginConstantSpeed(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan); -/** 本相前进 1 脉冲，返回下一拍命令频率 */ +/** + * 直线加速冷启动：算出第 1 拍平均频率并预装 S1（ARPE 下 Start 用）。 + * 之后 ISR NextFrequency 从第 2 拍起写影子。 + */ +uint32_t PlsrAccelPrimeLinearStartFrequency(PlsrAccelRuntime_t *runtime); + +/** + * 直线相边界为 0 时，实际最低命令频 ≈ (0+√(2a))/2（用于 PSC 锁定，勿用 1Hz）。 + */ +uint32_t PlsrAccelCurveLinearMinCommandHz(uint32_t boundary_frequency_hz, + uint32_t rate_hz_per_s); + +/** 本相前进 1 脉冲，返回下一拍命令频率（供 ARPE 写入下一周期） */ uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime); #endif diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h index 80c368f..afb00fb 100644 --- a/plsr/param/plsr_param.h +++ b/plsr/param/plsr_param.h @@ -115,7 +115,7 @@ typedef enum { /** 加减速曲线形状（脉冲域规划） */ typedef enum { - PLSR_ACCEL_LINEAR = 0, /* f^2 = f0^2 ± 2an */ + PLSR_ACCEL_LINEAR = 0, /* S_x=√(f0²±2ax)；命令频=(S_{k-1}+S_k)/2，末拍钉终点 */ PLSR_ACCEL_S = 1, /* 时间域七段 S：加速度梯形 1:2:1 */ PLSR_ACCEL_SINE = 2 /* 时间域 raised-cosine */ } PlsrAccelMode_e; @@ -161,7 +161,7 @@ typedef struct { uint16_t seg_count; /* 有效段数 0~PLSR_SEG_MAX */ uint16_t start_seg; /* 1-based */ uint32_t default_speed; /* Hz；段频为 0 时作段目标；并定斜率 */ - uint32_t start_speed; /* 起速；0=运行层 ResolveStartHz 起跳 */ + uint32_t start_speed; /* 起速；0=直线 f0=0（首拍 (0+√2a)/2） */ uint32_t end_speed; /* 止速；0=ResolveEndHz 落地后停表 */ uint16_t accel_ms; uint16_t decel_ms; diff --git a/plsr/pulse_driver/plsr_pulse_driver.c b/plsr/pulse_driver/plsr_pulse_driver.c index 827c175..14de31c 100644 --- a/plsr/pulse_driver/plsr_pulse_driver.c +++ b/plsr/pulse_driver/plsr_pulse_driver.c @@ -650,14 +650,18 @@ static void PlsrPulseDriverCommitOutput(void) htim->Instance->CCER |= TIM_CCER_CC1E; /* - * 先打开预装载并把影子钉成同一频率，再 ENABLE。 - * 否则首拍 ISR 的 SetFreqIsr 可能落在 ARPE=0 窗口，直接改写工作 ARR。 + * 先打开预装载并把影子钉成同一频率；保护标志须在 ENABLE 前就绪， + * 否则 CNT=ARR 触发的首拍 UPDATE 可能早于 protect=1。 */ htim->Instance->CR1 |= TIM_CR1_ARPE; htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE; htim->Instance->ARR = s_last_arr; htim->Instance->CCR1 = ccr; + s_protect_arr = s_last_arr; + s_protect_ccr = ccr; + s_protect_start_period = 1U; + /* CNT 已在切换 PWM 模式前归位，下面仅清状态并开表。 */ __HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE); __HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE); @@ -665,11 +669,6 @@ static void PlsrPulseDriverCommitOutput(void) __HAL_TIM_ENABLE(htim); g_plsr_pwm_running = 1U; - /* 记录起跳工作寄存器，供首拍 ISR 保护当前周期 */ - s_protect_arr = s_last_arr; - s_protect_ccr = ccr; - s_protect_start_period = 1U; - s_regs_prepared = 0U; } diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index 6ade803..156513c 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -678,8 +678,7 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t else { /* - * 停表后冷启动：入口由 ResolveStartHz 按起速/目标/起跳三分支决定 - * （起速>目标→起速减速；否则与起跳比较取目标或起跳）。 + * 停表后冷启动：入口由 ResolveStartHz 给出数学 f0（起速可为 0，不再用起跳当入口）。 */ endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(g_plsr_config.start_speed, f_tgt, @@ -954,7 +953,7 @@ static void PlsrRunControlOnApproachDone(void) } /** - * 进匀速：写一次 f_tgt，之后 ISR 不改 ARR（复用缓存）。 + * 进匀速：写一次 f_tgt 锁定，之后 ISR 不改 ARR（不再开方/平均）。 */ static void PlsrRunControlEnterConstHold(uint8_t do_start) { @@ -995,8 +994,9 @@ static void PlsrRunControlEnterConstHoldIsr(void) * 1) CONST 且 remain 进减速窗 → EnterDecel，return 前不 Start * 2) CONST → 仅 do_start 时 Start(f_tgt)，return * 3) DECEL 且刚开段(n=0) → PrimeDecelOnStart（Start(f1)，其余交 ISR） - * 4) APPROACH → FreqAtPulse(done_n)；若已到 f_tgt 则进 CONST 或 DECEL - * 5) 其它 → ApplyOutFreq(next, do_start) + * 4) APPROACH 直线冷启动 → PrimeLinearStartFrequency + Start（ARPE 防首频双写） + * 5) APPROACH → FreqAtPulse(done_n)；若已到 f_tgt 则进 CONST 或 DECEL + * 6) 其它 → ApplyOutFreq(next, do_start) */ static void PlsrRunControlRefreshProfile(uint8_t do_start) { @@ -1037,6 +1037,46 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) return; } + /* + * 直线加速冷启动（ARPE）：先 Prime 写出第 1 拍平均频并 completed=1， + * 再 Start；否则 FreqAtPulse(0) 与首 UPDATE 会各写一遍首频。 + */ + if ((s_phase == PH_APPROACH) && + (do_start != 0U) && + (s_segment_pulses_done == 0) && + (g_plsr_accel_runtime.completed_pulses == 0U) && + (g_plsr_accel_plan.curve_mode == PLSR_ACCEL_LINEAR) && + (g_plsr_accel_plan.accel_pulses > 0U) && + (g_plsr_accel_plan.start_frequency_hz != g_plsr_accel_plan.target_frequency_hz)) + { + next = PlsrAccelPrimeLinearStartFrequency(&g_plsr_accel_runtime); + if (next < 1U) + { + next = 1U; + } + if ((g_plsr_accel_plan.accel_pulses == 0U) || + (g_plsr_accel_runtime.is_active == 0U) || + (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_plan.accel_pulses) || + ((g_plsr_accel_plan.target_frequency_hz >= g_plsr_accel_plan.start_frequency_hz) && + (next >= g_plsr_accel_plan.target_frequency_hz)) || + ((g_plsr_accel_plan.target_frequency_hz < g_plsr_accel_plan.start_frequency_hz) && + (next <= g_plsr_accel_plan.target_frequency_hz))) + { + PlsrRunControlOnApproachDone(); + if (PlsrRunControlShouldEnterDecel(remain) != 0U) + { + PlsrRunControlApplyOutFreq(next, do_start); + PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz, + PlsrRunControlDecelEnterBudget(remain)); + return; + } + PlsrRunControlEnterConstHold(do_start); + return; + } + PlsrRunControlApplyOutFreq(next, do_start); + return; + } + next = PlsrAccelCurveFreqAtPulse(&g_plsr_accel_plan, done_n); if (next < 1U) { @@ -1061,11 +1101,14 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) ((g_plsr_accel_plan.target_frequency_hz < g_plsr_accel_plan.start_frequency_hz) && (next <= g_plsr_accel_plan.target_frequency_hz))) { - next = g_plsr_accel_plan.target_frequency_hz; PlsrRunControlOnApproachDone(); if (PlsrRunControlShouldEnterDecel(remain) != 0U) { - PlsrRunControlEnterDecel(next, + if (g_plsr_accel_plan.curve_mode != PLSR_ACCEL_LINEAR) + { + next = g_plsr_accel_plan.target_frequency_hz; + } + PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz, PlsrRunControlDecelEnterBudget(remain)); } else @@ -1151,6 +1194,20 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, f_lo = f_start; } } + else if (g_plsr_accel_plan.curve_mode == PLSR_ACCEL_LINEAR) + { + /* + * f0=0 时首拍命令频≈(0+√2a)/2，不是 1Hz。 + * 若用 1Hz~100k 锁 PSC，跨度锁不住 → ISR 换 PSC 挂起 → 停表重开多计边沿。 + */ + uint32_t min_cmd = PlsrAccelCurveLinearMinCommandHz( + 0U, g_plsr_accel_plan.accel_rate_hz_per_s); + + if (min_cmd < f_lo) + { + f_lo = min_cmd; + } + } else { f_lo = 1U; @@ -1166,6 +1223,16 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, f_lo = g_plsr_accel_plan.end_frequency_hz; } } + else if (g_plsr_accel_plan.curve_mode == PLSR_ACCEL_LINEAR) + { + uint32_t min_cmd = PlsrAccelCurveLinearMinCommandHz( + 0U, g_plsr_accel_plan.decel_rate_hz_per_s); + + if (min_cmd < f_lo) + { + f_lo = min_cmd; + } + } else { if (f_lo > 1U) @@ -1810,17 +1877,30 @@ void PlsrOnPulseIsr(void) ((g_plsr_accel_plan.target_frequency_hz < g_plsr_accel_plan.start_frequency_hz) && (next <= g_plsr_accel_plan.target_frequency_hz))) { - next = g_plsr_accel_plan.target_frequency_hz; PlsrRunControlOnApproachDone(); if (PlsrRunControlShouldEnterDecel(remain) != 0U) { - PlsrRunControlEnterDecel(next, - PlsrRunControlDecelEnterBudget(remain)); - next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); - PlsrRunControlApplyOutFreqIsr(next); + /* + * 直线：本拍写入加速末拍平均频，仅切 DECEL；首减频留给下一 UPDATE（ARPE）。 + * 其它曲线：仍本拍预装首减频。 + */ + if (g_plsr_accel_plan.curve_mode == PLSR_ACCEL_LINEAR) + { + PlsrRunControlApplyOutFreqIsr(next); + PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz, + PlsrRunControlDecelEnterBudget(remain)); + } + else + { + PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz, + PlsrRunControlDecelEnterBudget(remain)); + next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); + PlsrRunControlApplyOutFreqIsr(next); + } } else { + /* 进匀速：直接锁定峰值，ISR 不再开方/平均 */ PlsrRunControlEnterConstHoldIsr(); } } @@ -1831,7 +1911,7 @@ void PlsrOnPulseIsr(void) } else /* PH_DECEL */ { - /* 直线减速：NextFrequency 按 sqrt(end^2+2a·k) 从终点往上取，末拍 k=1 */ + /* 直线减速：邻项平均；末拍 (S1+end)/2，不钉 end */ next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); PlsrRunControlApplyOutFreqIsr(next); }