From 0119fda62719381cdcaff9acb0935e4c5028afe0 Mon Sep 17 00:00:00 2001 From: hanyongwei <2043702190@qq.com> Date: Sat, 29 Aug 2026 15:03:26 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BE=93=E5=87=BA=E8=84=89=E5=86=B2=E4=B8=AA?= =?UTF-8?q?=E6=95=B0=E4=B8=8D=E5=87=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- plsr/accel_curve/plsr_accel_curve.c | 237 +++++++++++++--------------- plsr/run_control/plsr_run_control.c | 92 +++-------- 2 files changed, 132 insertions(+), 197 deletions(-) diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 8c54807..88041d4 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 脉冲域规划/取频：直线 frequency_hz^2=start_frequency_hz^2±2an；S/正弦按时间轴剖面 + * @brief 脉冲域规划/取频：加速 f^2=start^2+2an；减速 f^2=end^2+2ak(k=N…1)；S/正弦时间轴 * * ============================================================================ * 【阅读指南 — 先看这个再往下翻 if/else】 @@ -20,7 +20,8 @@ * decel_rate_hz_per_s 减速斜率 Hz/s ≈ default_speed*1000/deceleration_time_ms * * 二、三种曲线模式（curve_mode） - * LINEAR 直线：f_n = sqrt(start_frequency_hz^2 ± 2*a*pulse_count)；ISR 里 SquareStep ±1Hz 逼近 + * LINEAR 加速：f=sqrt(start^2+2a·n)；减速：从终点往峰值加 + * f=sqrt(end^2+2a·k)，k=N…1（与加速对称，勿从峰值减计数） * S 时间域 jerk 梯形；规划/ISR 用离散 Δt=1/frequency_hz 仿真；ISR 查预建表 * SINE raised-cosine 时间剖面；同上，查预建表 * @@ -557,48 +558,65 @@ static uint32_t PlsrAccelCurveCalculateRampPulses(uint32_t start_frequency_hz, u return PlsrAccelCurveCalculateLinearRampPulses(start_frequency_hz, end_frequency_hz, acceleration_hz_per_s); } -/** 直线：frequency_hz = sqrt(start_frequency_hz^2 + 2*a*pulse_count)，升到/降到不超过 target_limit_hz 方向 */ +/** + * 直线取频。 + * 加速(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 + */ static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz, uint32_t acceleration_hz_per_s, uint32_t pulse_count, uint8_t frequency_rising, - uint32_t target_limit_hz) + uint32_t target_limit_hz, + uint32_t phase_total_pulses) { uint64_t squared_frequency; uint32_t frequency_hz; + uint32_t k_from_end; - if (pulse_count == 0U) - { - frequency_hz = start_frequency_hz; - } - else if (acceleration_hz_per_s == 0U) + if (acceleration_hz_per_s == 0U) { - frequency_hz = target_limit_hz; + frequency_hz = frequency_rising != 0U ? target_limit_hz : start_frequency_hz; + return PlsrAccelCurveClampFrequencyHz((frequency_hz < 1U) ? 1U : frequency_hz); } - else + + if (frequency_rising != 0U) { - squared_frequency = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz; - if (frequency_rising != 0U) + if (pulse_count == 0U) + { + frequency_hz = start_frequency_hz; + } + else { - squared_frequency += 2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)pulse_count; + 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; } } + } + 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) + { + frequency_hz = target_limit_hz; + } else { - uint64_t frequency_drop = 2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)pulse_count; - if (frequency_drop >= squared_frequency) + k_from_end = phase_total_pulses - pulse_count; + 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) { - frequency_hz = target_limit_hz; - } - else - { - frequency_hz = PlsrAccelCurveIntegerSquareRoot(squared_frequency - frequency_drop); - if (frequency_hz < target_limit_hz) - { - frequency_hz = target_limit_hz; - } + frequency_hz = start_frequency_hz; } } } @@ -1015,7 +1033,8 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, uint32_t selected_rate_hz_per_s = (frequency_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; frequency_hz = PlsrAccelCurveLinearFrequencyAtPulse(plan->start_frequency_hz, selected_rate_hz_per_s, - completed_segment_pulses, frequency_rising, plan->target_frequency_hz); + completed_segment_pulses, frequency_rising, plan->target_frequency_hz, + plan->accel_pulses); } else { @@ -1059,7 +1078,8 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, uint32_t selected_rate_hz_per_s = (frequency_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; frequency_hz = PlsrAccelCurveLinearFrequencyAtPulse(plan->target_frequency_hz, selected_rate_hz_per_s, - pulse_count, frequency_rising, plan->end_frequency_hz); + pulse_count, frequency_rising, plan->end_frequency_hz, + plan->decel_pulses); } else { @@ -1234,18 +1254,34 @@ static uint32_t PlsrAccelRuntimeBuildFrequencyTable(PlsrAccelRuntime_t *runtime, } if (simulation.is_active == 0U) { - while ((loop_index + 1U) < runtime->total_pulses) + /* 提前到终点：中间格保持最后仿真值，仅最后一格钉 end（勿 end±1 垫平台） */ { - loop_index++; - table_index = loop_index / table_stride; - if (table_index >= table_capacity) + uint32_t hold_hz = simulation.current_frequency_hz; + + if (hold_hz < 1U) { - table_index = table_capacity - 1U; + hold_hz = 1U; } - frequency_table[table_index] = runtime->end_frequency_hz; - if ((table_index + 1U) > table_length) + while ((loop_index + 1U) < runtime->total_pulses) { - table_length = table_index + 1U; + loop_index++; + table_index = loop_index / table_stride; + if (table_index >= table_capacity) + { + table_index = table_capacity - 1U; + } + if ((loop_index + 1U) >= runtime->total_pulses) + { + frequency_table[table_index] = runtime->end_frequency_hz; + } + else + { + frequency_table[table_index] = hold_hz; + } + if ((table_index + 1U) > table_length) + { + table_length = table_index + 1U; + } } } break; @@ -1383,70 +1419,27 @@ void PlsrAccelBeginConstantSpeed(PlsrAccelRuntime_t *runtime, } /** - * 直线：用上一拍 frequency_hz ±1 逼近 target_frequency_squared，不全量开方。 - * 低频 frequency_difference_hz/dn 大时循环次数多，但周期也长，ISR 仍可承受。 + * 直线 ISR 取频（开方，与规划公式一致）。 + * 加速：f = sqrt(start^2 + 2·a·completed) + * 减速：k = total-completed+1，f = sqrt(end^2 + 2·a·k)（从终点往峰值加） */ static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime) { uint64_t target_frequency_squared; - uint64_t current_frequency_squared; - uint64_t start_frequency_squared; uint32_t frequency_hz; - uint32_t iteration_guard; + uint32_t k_from_end; - start_frequency_squared = (uint64_t)runtime->start_frequency_hz * (uint64_t)runtime->start_frequency_hz; - if (runtime->frequency_rising != 0U) + if (runtime->acceleration_hz_per_s == 0U) { - target_frequency_squared = start_frequency_squared + (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)runtime->completed_pulses); - } - else - { - uint64_t frequency_drop = 2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)runtime->completed_pulses; - target_frequency_squared = (frequency_drop >= start_frequency_squared) ? 0ULL : (start_frequency_squared - frequency_drop); - } - - frequency_hz = runtime->current_frequency_hz; - if (frequency_hz < 1U) - { - frequency_hz = 1U; - } - - for (iteration_guard = 0U; iteration_guard < 2048U; iteration_guard++) - { - current_frequency_squared = (uint64_t)frequency_hz * (uint64_t)frequency_hz; - if (runtime->frequency_rising != 0U) - { - if ((frequency_hz < runtime->end_frequency_hz) && (((uint64_t)(frequency_hz + 1U) * (uint64_t)(frequency_hz + 1U)) <= target_frequency_squared)) - { - frequency_hz++; - continue; - } - if ((frequency_hz > 1U) && (current_frequency_squared > target_frequency_squared)) - { - frequency_hz--; - continue; - } - break; - } - else - { - if ((frequency_hz > runtime->end_frequency_hz) && (current_frequency_squared > target_frequency_squared)) - { - frequency_hz--; - continue; - } - if ((frequency_hz < 100000U) && - (((uint64_t)(frequency_hz + 1U) * (uint64_t)(frequency_hz + 1U)) < target_frequency_squared)) - { - frequency_hz++; - continue; - } - break; - } + return PlsrAccelCurveClampFrequencyHz(runtime->end_frequency_hz); } if (runtime->frequency_rising != 0U) { + target_frequency_squared = + (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) { frequency_hz = runtime->end_frequency_hz; @@ -1454,11 +1447,28 @@ static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime) } else { - if (frequency_hz < runtime->end_frequency_hz) + if (runtime->total_pulses < 1U) { - frequency_hz = runtime->end_frequency_hz; + k_from_end = 1U; + } + else if (runtime->completed_pulses >= runtime->total_pulses) + { + k_from_end = 1U; + } + 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; } } + if (frequency_hz < 1U) { frequency_hz = 1U; @@ -1625,52 +1635,17 @@ uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime) } if ((runtime->total_pulses > 0U) && (runtime->completed_pulses >= runtime->total_pulses)) - { - runtime->current_frequency_hz = runtime->end_frequency_hz; - runtime->is_active = 0U; - } - else if (runtime->curve_mode == PLSR_ACCEL_LINEAR) { /* - * 未到本相最后一拍：不要钳到 end。 - * 否则减速在 n=N-1 就落到止速，ARPE 下会表现为末两拍同频。 + * 直线减速末步已是 sqrt(end^2+2a)，不要再钉成 end（否则与「从终点往上加」不一致）。 + * S/正弦 / 加速相：仍钉到相终点。 */ - if (runtime->frequency_rising == 0U) + if (!((runtime->curve_mode == PLSR_ACCEL_LINEAR) && + (runtime->frequency_rising == 0U))) { - if ((runtime->current_frequency_hz <= runtime->end_frequency_hz) && - (runtime->end_frequency_hz < 100000U)) - { - runtime->current_frequency_hz = runtime->end_frequency_hz + 1U; - } - } - else if ((runtime->current_frequency_hz >= runtime->end_frequency_hz) && - (runtime->end_frequency_hz > 1U) && - (runtime->total_pulses > 0U) && - (runtime->completed_pulses < runtime->total_pulses)) - { - runtime->current_frequency_hz = runtime->end_frequency_hz - 1U; - } - } - else if (runtime->frequency_table_id != 0U) - { - /* 查表：最后一拍之前若已是 end，同样让出 1Hz */ - if ((runtime->total_pulses > 0U) && - (runtime->completed_pulses < runtime->total_pulses)) - { - if (runtime->frequency_rising == 0U) - { - if ((runtime->current_frequency_hz <= runtime->end_frequency_hz) && - (runtime->end_frequency_hz < 100000U)) - { - runtime->current_frequency_hz = runtime->end_frequency_hz + 1U; - } - } - else if ((runtime->current_frequency_hz >= runtime->end_frequency_hz) && - (runtime->end_frequency_hz > 1U)) - { - runtime->current_frequency_hz = runtime->end_frequency_hz - 1U; - } + runtime->current_frequency_hz = runtime->end_frequency_hz; } + runtime->is_active = 0U; } else if (runtime->frequency_table_id == 0U) { diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index 58f9499..6ade803 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -37,10 +37,10 @@ * 5) s_segment_pulses_done >= target → OPM 末拍 or 立刻 AfterSegDone * 6) 按 s_phase 改频：CONST 只进减速；APPROACH Step；DECEL Step * - * ARPE（UPDATE ISR 写影子 → 下一周期才装入工作 ARR）： - * 在 remain==k 时写入的频率供下一拍使用；进减速窗为 remain<=dec_n， - * 在 remain==1 写入 f_end，末拍才能吃到止速。勿再用 remain<=dec_n+1， - * 否则会在 remain==2 就强制止速，造成末两拍同频。 + * ARPE（UPDATE 写影子 → 下一周期才装入）： + * done 先自增再算 remain：done=k 表示第 k 拍上升沿已计入，remain=N-k（不含当前）。 + * 本拍写入供第 k+1 拍；remain==dec_n 切入减速；remain==1 时写入 f_end 供末拍。 + * done==N 时只武装停表、不再改频。 * * 分层（阶段1薄拆分）： * 1) 曲线（accel_curve）：只吃 (f_from,f_tgt,f_end,斜率,N,mode) → 梯形/三角； @@ -66,9 +66,9 @@ * TIM5 one-shot：换向/WAIT/ACT/ACT剩余；ACT 到期收完当前脉冲并锁低，周期边界再切段。 * PlsrTask：DebouncePoll + TickMs + WakePend。 * - * 【段末频率 / ARPE】UPDATE 里写的影子 ARR 在下一周期生效； - * 减速在 remain==dec_n 切入，共 Step dec_n 次，最后一次（remain==1）写入 f_end。 - * 纯减速开表：Start(f1)，后续由 ISR 顺序写入 f2…f_end（勿再预步进一拍）。 + * 【段末频率 / ARPE】done 先++；remain=N-done（不含当前）；本拍写给下一拍： + * remain==dec_n 切入；remain==1 强制写 f_end；done==N 只停表不改频。 + * 纯减速开表：Start(f1)，其余交 ISR。 * * 【段末停表】非 FOLLOW-keep 时开段锁 s_stop_after_last_pulse： * UPDATE≈上升沿；s_segment_pulses_done 到 target 时 ArmSegEndStop（OPM+CC1 末拍下降沿拉低）， @@ -853,6 +853,12 @@ static uint8_t PlsrRunControlHasDecel(void) static uint32_t PlsrRunControlRemainPulses(void) { + /* + * 注意：OnPulseIsr 里先 done++ 再调用本函数。 + * 此时 done=k 表示「第 k 拍上升沿已计入」，当前正在跑第 k 拍。 + * 返回值 = target-done = 当前拍之后还剩几拍（不含当前）。 + * 例 N=15、done=14 → remain=1（之后只剩第 15 拍，本拍写入供第 15 拍用）。 + */ if (s_segment_pulse_target > s_segment_pulses_done) { return (uint32_t)(s_segment_pulse_target - s_segment_pulses_done); @@ -861,9 +867,11 @@ static uint32_t PlsrRunControlRemainPulses(void) } /** - * ARPE：本拍 ISR 写入的影子 ARR 在下一周期生效。 - * remain==dec_n 时切入并写入 f(1)，…，remain==1 写入 f_end → 末拍为止速。 - * 若已晚进（remain < planned），预算用满剩余拍数，避免再减 1 导致末两拍同为止速。 + * 先 done++ 后的 remain（不含当前）与「下一拍序号」关系： + * next_pulse = done + 1 = target - remain + 1 + * 减速输出落在第 (N-dec_n+1)…N 拍；故在 done==N-dec_n（remain==dec_n） + * 时开始写入（供下一拍，即减速第 1 拍）。 + * 晚进时预算用满剩余「还可写入的次数」= remain（done 到 N-1 共 remain 次）。 */ static uint8_t PlsrRunControlShouldEnterDecel(uint32_t remain) { @@ -899,38 +907,6 @@ static int32_t PlsrRunControlAbsLocalPos(void) return g_plsr_accumulated_pulses - s_absolute_origin; } -/** - * 减速相写出频率：仅 remain==1（供末拍）允许钉止速； - * 更早写入若已碰到 end，则让出 1Hz，避免末两拍同频。 - */ -static uint32_t PlsrRunControlDecelOutFreq(uint32_t profile_freq, uint32_t remain) -{ - uint32_t end_hz = g_plsr_accel_plan.end_frequency_hz; - - if (end_hz < 1U) - { - end_hz = 1U; - } - - if (remain <= 1U) - { - return end_hz; - } - - if (g_plsr_accel_runtime.frequency_rising == 0U) - { - if ((profile_freq <= end_hz) && (end_hz < 100000U)) - { - return end_hz + 1U; - } - } - else if ((profile_freq >= end_hz) && (end_hz > 1U)) - { - return end_hz - 1U; - } - return profile_freq; -} - /** * @brief 进入减速相：budget 为曲线步进总数（非物理 remain），BeginDec */ @@ -953,8 +929,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget) } /** - * 开表纯减速：只 Start(f1)。下一拍起由 ISR NextFrequency 写入 f2…f_end。 - * （旧逻辑再预装 f2 会在 ARPE 下让首拍直接吃到 f2，并多步进一次。） + * 开表纯减速：Start(f1)，后续 f2…f_end 由 ISR 超前写入。 */ static void PlsrRunControlPrimeDecelOnStart(void) { @@ -1801,8 +1776,11 @@ void PlsrOnPulseIsr(void) remain = PlsrRunControlRemainPulses(); /* - * --- 段内三相：按 s_phase 改频（匀速几乎空转）--- - * remain = 还剩几拍（含当前正在进行的这一拍） + * done=k → 当前第 k 拍；写入的 ARR 给第 k+1 拍。 + * remain=N-k（不含当前）。N=15,dec=5 时： + * done=10,remain=5 → 写 f11（减速第 1 拍） + * done=14,remain=1 → 写 f15=f_end（末拍） + * done=15 → 上面已 ArmSegEndStop，不会走到这里 */ if (s_phase == PH_CONST) { @@ -1816,8 +1794,6 @@ void PlsrOnPulseIsr(void) PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz, PlsrRunControlDecelEnterBudget(remain)); next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); - next = PlsrRunControlDecelOutFreq(next, remain); - g_plsr_accel_runtime.current_frequency_hz = next; PlsrRunControlApplyOutFreqIsr(next); } } @@ -1841,8 +1817,6 @@ void PlsrOnPulseIsr(void) PlsrRunControlEnterDecel(next, PlsrRunControlDecelEnterBudget(remain)); next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); - next = PlsrRunControlDecelOutFreq(next, remain); - g_plsr_accel_runtime.current_frequency_hz = next; PlsrRunControlApplyOutFreqIsr(next); } else @@ -1857,22 +1831,8 @@ void PlsrOnPulseIsr(void) } else /* PH_DECEL */ { - /* - * ARPE：remain==k 写入 → 下一拍使用。 - * remain==1 必须写出 f_end（末拍）；remain>1 禁止提前钉止速。 - */ - if ((remain <= 1U) && - (g_plsr_accel_runtime.total_pulses > 0U) && - (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_runtime.total_pulses)) - { - next = g_plsr_accel_plan.end_frequency_hz; - } - else - { - next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); - } - next = PlsrRunControlDecelOutFreq(next, remain); - g_plsr_accel_runtime.current_frequency_hz = next; + /* 直线减速：NextFrequency 按 sqrt(end^2+2a·k) 从终点往上取，末拍 k=1 */ + next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); PlsrRunControlApplyOutFreqIsr(next); } }