From 27c686fb810cf4fdda317d5f05e2a3d25bed1fa8 Mon Sep 17 00:00:00 2001 From: hanyongwei <2043702190@qq.com> Date: Tue, 18 Aug 2026 16:15:27 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8DACT=E6=97=B6=E9=97=B4?= =?UTF-8?q?=E5=88=87=E6=AE=B5=E5=BC=82=E5=B8=B8=EF=BC=9B=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E9=BB=98=E8=AE=A4=E8=B5=B7=E5=A7=8B=E9=80=9F=E5=BA=A6/?= =?UTF-8?q?=E9=BB=98=E8=AE=A4=E7=BB=88=E6=AD=A2=E9=80=9F=E5=BA=A6=E4=B8=BA?= =?UTF-8?q?0=E6=97=B6=E7=9A=84=E8=B5=B7=E8=B7=B3=E9=A2=91=E7=8E=87?= =?UTF-8?q?=E8=AE=A1=E7=AE=97=EF=BC=8C=E4=BD=BF=E7=94=A8=E8=B5=B7=E8=B7=B3?= =?UTF-8?q?=E9=A2=91=E7=8E=87=3D=E6=A0=B9=E5=8F=B7=E4=B8=8B=E5=8A=A0?= =?UTF-8?q?=E9=80=9F=E6=96=9C=E7=8E=87=EF=BC=8C=E5=8A=A0=E9=80=9F=E6=96=9C?= =?UTF-8?q?=E7=8E=87=E9=87=87=E7=94=A8Hz/s?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: hanyongwei <2043702190@qq.com> --- plsr/accel_curve/plsr_accel_curve.c | 129 +++++++++++++++++++++++++++- plsr/accel_curve/plsr_accel_curve.h | 8 ++ plsr/run_control/plsr_run_control.c | 128 ++++++++++++--------------- 3 files changed, 189 insertions(+), 76 deletions(-) diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 87855fe..21f0d65 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -26,6 +26,78 @@ static uint32_t PlsrAccelCurveAbsDiff(uint32_t a, uint32_t b) return (a >= b) ? (a - b) : (b - a); } +/** + * 整数平方根（牛顿法）。 + */ +static uint32_t PlsrAccelCurveIsqrt(uint32_t val) +{ + uint32_t x; + uint32_t x2; + uint32_t i; + + if (val <= 1U) + { + return val; + } + x = val; + if (x > 10000U) + { + x = 10000U; + } + for (i = 0U; i < 30U; i++) + { + x2 = (x + val / x) / 2U; + if (x2 >= x) + { + break; + } + x = x2; + } + return x; +} + +/** + * 起跳频率：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。 + */ +uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, + uint32_t t_ms) +{ + uint32_t df; + uint32_t a_hz_s; + uint32_t f; + + if (t_ms == 0U) + { + return 1U; + } + df = PlsrAccelCurveAbsDiff(f_from, f_to); + if (df == 0U) + { + return 1U; + } + /* a = df / (t_ms / 1000) = df * 1000 / t_ms，可能很大 */ + if (df <= 4294967U) + { + a_hz_s = (df * 1000U) / t_ms; + } + else + { + a_hz_s = df / t_ms * 1000U; + } + if (a_hz_s == 0U) + { + return 1U; + } + f = PlsrAccelCurveIsqrt(a_hz_s); + if (f < 1U) + { + f = 1U; + } + return f; +} + static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd, uint32_t primary_ref, uint32_t fallback_ref) @@ -656,9 +728,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, f_end = PlsrAccelCurveClampFreq(f_end); m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode; - plan->f_cur = f_cur; - plan->f_end = f_end; - plan->mode = m; f_peak = f_tgt; t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, @@ -667,6 +736,60 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd, start_spd_ref, end_spd_ref, accel_ms, decel_ms); + + /* + * 起速=0：用起跳频率替代 0。 + * + * 默认起始/终止速度都为 0 时，按你图里的离散模型： + * a_Hz/s = a_Hz_per_ms * 1000 = (slope_Hz/ms) * 1000 + * 其中 slope_Hz/ms 由本段规划斜率参数决定。 + * + * 本实现中：当 start_spd_ref==0 && end_spd_ref==0 时， + * 由斜率定义可得 a_Hz/s = default_spd * 1000 / accel_ms（加速段）。 + */ + if ((f_cur == 0U) && (f_tgt >= 1U) && (t_acc >= 1U)) + { + if ((start_spd_ref == 0U) && (end_spd_ref == 0U) && (accel_ms > 0U) && + (default_spd > 0U)) + { + /* a_Hz/s = default_spd / accel_ms(ms) * 1000 */ + uint64_t a_hz_s = ((uint64_t)default_spd * 1000ULL) / (uint64_t)accel_ms; + f_cur = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s); + } + else + { + /* 兜底：用 df/t_ms 反推加速度再开方 */ + f_cur = PlsrAccelCurveJumpFreq(0U, f_tgt, t_acc); + } + if (f_cur > f_tgt) + { + f_cur = f_tgt; + } + } + /* 止速=0：同理，减速末端用起跳频率，避免减速尾部 1Hz 拖尾 */ + if ((f_end == 0U) && (f_tgt >= 1U) && (t_dec >= 1U)) + { + if ((start_spd_ref == 0U) && (end_spd_ref == 0U) && (decel_ms > 0U) && + (default_spd > 0U)) + { + /* a_Hz/s = default_spd / decel_ms(ms) * 1000 */ + uint64_t a_hz_s = ((uint64_t)default_spd * 1000ULL) / (uint64_t)decel_ms; + f_end = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s); + } + else + { + /* 兜底：用 df/t_ms 反推加速度再开方 */ + f_end = PlsrAccelCurveJumpFreq(0U, f_tgt, t_dec); + } + if (f_end > f_tgt) + { + f_end = f_tgt; + } + } + + plan->f_cur = f_cur; + plan->f_end = f_end; + plan->mode = m; acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m); dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m); diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index a407edc..fd550b9 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -33,6 +33,14 @@ typedef enum { uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz); +/** + * 起跳频率：f_jump = sqrt(加速度_Hz/s)。 + * 起速/止速为 0 时，第一个脉冲（或最后一个脉冲）应以此频率输出， + * 使脉冲周期恰好等于加速度产生该频率所需的时间。 + */ +uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, + uint32_t t_ms); + /** * 斜坡上按 mode 取频（µs 时间轴）。ISR 可用：O(1)。 * f = f0+(f1-f0)*shape(t/T) diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index a1cfad1..fd138b6 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -60,6 +60,8 @@ static uint16_t s_act_time_ms; /* 本段 ACT 时长（出脉冲后计） */ static uint32_t s_act_arm_ms; /* 武装时刻的 TIM3 s_ms，与改频 elapsed 脱钩 */ static volatile uint8_t s_act_armed; static volatile uint8_t s_act_expire_req; /* TIM3 置位，任务里切段（避免 ISR 里 PlanSeg） */ +static volatile uint8_t s_act_cut_pending; /* ACT 到期，等待当前脉冲 UPDATE 边界 */ +static volatile uint8_t s_act_handoff_keep; /* 边界停表后，下一段仍从当前频率衔接 */ static volatile uint8_t s_wait_expire_req; /* WAIT 时间到：ISR 只置位 */ static uint8_t s_wait_is_signal; /* RC_WAIT_COND：1=等 WAIT 沿，0=等时间 */ @@ -77,6 +79,7 @@ static uint8_t s_pwm_on; /* 本段由策略层解析后的加减速时间（段内只用这两份，不再读 wait） */ static uint16_t s_run_accel_ms; static uint16_t s_run_decel_ms; +static uint8_t s_orig_end_zero; /* 原始止速为 0（起跳频率已替代 plan.f_end） */ /** 策略层交给段处理的接口参数 */ typedef struct { @@ -103,7 +106,6 @@ static uint16_t PlsrRunControlWaitTimeMs(const PlsrSeg_t *seg); static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, uint32_t f_tgt, uint32_t f_end, uint16_t accel_ms, uint16_t decel_ms); -static void PlsrRunControlApplyActTimeCap(uint16_t act_ms); static uint32_t PlsrRunControlClampSpeed(uint32_t spd); static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz, uint32_t default_spd, @@ -124,6 +126,8 @@ static void PlsrRunControlArmActExtOnPulseStart(void) } s_act_armed = 0U; s_act_expire_req = 0U; + s_act_cut_pending = 0U; + s_act_handoff_keep = 0U; s_wait_expire_req = 0U; if (seg->wait_type == PLSR_WAIT_ACT) { @@ -142,6 +146,8 @@ static void PlsrRunControlCutSegToNext(void) s_state = RC_IDLE; s_act_armed = 0U; s_act_expire_req = 0U; + s_act_cut_pending = 0U; + s_act_handoff_keep = 0U; s_wait_expire_req = 0U; PlsrPulseDriverStop(); s_follow_cont = 0U; @@ -176,7 +182,9 @@ static void PlsrRunControlActExpire(void) next0 = PlsrPathPlanResolveAfterSeg(s_cur_seg); keep = 0U; - if ((next0 >= 0) && (s_pwm_on != 0U) && (s_cur_freq >= 1U) && + if ((next0 >= 0) && + ((s_pwm_on != 0U) || (s_act_handoff_keep != 0U)) && + (s_cur_freq >= 1U) && (PlsrPathPlanIsForward((uint16_t)next0, s_acc_pulse) == s_forward)) { keep = 1U; @@ -200,6 +208,8 @@ static void PlsrRunControlActExpire(void) s_chain_freq = 0U; } + s_act_handoff_keep = 0U; + PlsrRunControlGotoNextOrFinish(s_cur_seg); } @@ -317,7 +327,7 @@ static void PlsrRunControlAfterSegDone(void) if (cfg->send_mode == PLSR_SEND_COMPLETE) { /* 段正常结束：下一段起速 = 本段止速 f_end，不能用峰值 s_cur_freq */ - s_chain_freq = s_accel_plan.f_end; + s_chain_freq = (s_orig_end_zero != 0U) ? 0U : s_accel_plan.f_end; s_chain_valid = 1U; } @@ -393,7 +403,7 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, * ---------- 终止频率：仅由发送模式 + 是否有下一段决定 ---------- * 完成方式 / 真末段：公共止速 * 后续方式非末段：下一段目标（无缝衔接） - * ACT 不在这里改 f_end：先按梯形算出加/匀/减时间，再 ApplyActTimeCap。 + * ACT 只由运行控制层管理，不改变本段曲线的 f_end 或阶段规划。 */ (void)f_tgt; next0 = PlsrPathPlanNextSeg(seg_0); @@ -609,7 +619,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) * （拉长会在尾部堆 1Hz 空耗）。 */ if ((old_t_dec > 0U) && (old_dec_n > 0U) && - ((remain_pulses == old_dec_n) || (f_end == 0U))) + ((remain_pulses == old_dec_n) || (s_orig_end_zero != 0U))) { s_accel_plan.t_dec_ms = old_t_dec; return; @@ -646,7 +656,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) } } - if (f_end == 0U) + if (s_orig_end_zero != 0U) { s_accel_plan.t_dec_ms = (t_slope > 0U) ? t_slope : ((old_t_dec > 0U) ? old_t_dec : 1U); @@ -696,10 +706,10 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms); if (next < 1U) { - if ((s_accel_plan.f_end == 0U) && (s_seg_elapsed_ms > 0U)) + if ((s_orig_end_zero != 0U) && (s_seg_elapsed_ms > 0U)) { /* - * 时间轴已落到止速 0。剩余脉冲仍要发出并计数（ACT※4： + * 原始止速为 0，时间轴走完。剩余脉冲仍要发出并计数（ACT※4： * 三阶段走完后等时间，不能把 s_target 截掉导致计数丢失）。 */ if (s_done < s_target) @@ -734,6 +744,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, /* 段内只吃传入端点与加减速时间，不感知 wait */ s_run_accel_ms = accel_ms; s_run_decel_ms = decel_ms; + s_orig_end_zero = (f_end == 0U) ? 1U : 0U; /* * 曲线估 acc_n / dec_n，const_n = total − acc − dec。 @@ -843,41 +854,6 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, s_seg_elapsed_ms = 0U; } -/** - * 按 ACT 时间裁剪本段时间轴（规划已含加速/匀速/减速）： - * - T_acc > ACT → 加速未到目标频就被打断 - * - T_acc+T_const > ACT → 匀速期内打断，不进入减速 - * - T_acc+T_const < ACT < 三段总长 → 进入减速，可能被中途打断 - * - ACT ≥ 加速+匀速+减速 → 三阶段走完（含减速到止速），脉冲发完后等 ACT（※4） - */ -static void PlsrRunControlApplyActTimeCap(uint16_t act_ms) -{ - uint32_t t_act; - uint32_t t_acc; - uint32_t t_const_end; - uint32_t t_wave; - - t_act = (act_ms == 0U) ? 1U : (uint32_t)act_ms; - t_acc = s_accel_plan.t_acc_ms; - t_const_end = s_accel_plan.t_decel_start_ms; - if (t_const_end < t_acc) - { - t_const_end = t_acc; - } - t_wave = t_const_end + s_accel_plan.t_dec_ms; - - if ((t_act >= t_wave) || (t_act > t_const_end)) - { - /* 进入减速，或 ACT 比三段总时间还长：保留减速规划 */ - return; - } - - /* 加速中途或匀速期内就会切段：时间轴上不再走减速 */ - s_accel_plan.t_dec_ms = 0U; - s_accel_plan.dec_n = 0U; - s_accel_plan.f_end = s_accel_plan.f_tgt; -} - static void PlsrRunControlFinishAll(void) { PlsrPulseDriverStop(); @@ -892,6 +868,8 @@ static void PlsrRunControlFinishAll(void) s_chain_freq = 0U; s_act_armed = 0U; s_act_expire_req = 0U; + s_act_cut_pending = 0U; + s_act_handoff_keep = 0U; s_wait_expire_req = 0U; s_act_time_ms = 0U; s_act_arm_ms = 0U; @@ -970,11 +948,6 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0) total = (uint32_t)s_target; PlsrRunControlPlanSeg(total, io.f_from, f_tgt, io.f_end, io.accel_ms, io.decel_ms); - if (seg->wait_type == PLSR_WAIT_ACT) - { - PlsrRunControlApplyActTimeCap(PlsrRunControlActTimeMs(seg)); - } - s_done = 0; s_busy = 1U; @@ -1017,12 +990,15 @@ void PlsrRunControlInit(void) s_chain_valid = 0U; s_run_accel_ms = 0U; s_run_decel_ms = 0U; + s_orig_end_zero = 0U; s_phase = PH_CONST; s_seg_elapsed_ms = 0U; s_approach_from = 0U; s_decel_from = 0U; s_act_armed = 0U; s_act_expire_req = 0U; + s_act_cut_pending = 0U; + s_act_handoff_keep = 0U; s_wait_expire_req = 0U; s_act_time_ms = 0U; s_act_arm_ms = 0U; @@ -1074,6 +1050,8 @@ void PlsrRunControlStop(void) s_chain_freq = 0U; s_act_armed = 0U; s_act_expire_req = 0U; + s_act_cut_pending = 0U; + s_act_handoff_keep = 0U; s_wait_expire_req = 0U; s_act_time_ms = 0U; s_act_arm_ms = 0U; @@ -1101,13 +1079,6 @@ uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz) f_end = s_accel_plan.f_end; PlsrRunControlPlanSeg(remain, s_cur_freq, new_tgt_hz, f_end, s_run_accel_ms, s_run_decel_ms); - if (PlsrParamGetSeg(s_cur_seg)->wait_type == PLSR_WAIT_ACT) - { - uint32_t used = PlsrSignalIoGetMs() - s_act_arm_ms; - uint16_t left = (used < (uint32_t)s_act_time_ms) ? - (uint16_t)((uint32_t)s_act_time_ms - used) : 1U; - PlsrRunControlApplyActTimeCap(left); - } s_target = (int32_t)remain; s_done = 0; s_seg_elapsed_ms = 0U; @@ -1123,7 +1094,7 @@ void PlsrRunControlTickMs(void) if (s_act_expire_req != 0U) { s_act_expire_req = 0U; - if (s_state == RC_RUN) + if ((s_state == RC_RUN) || (s_act_handoff_keep != 0U)) { PlsrRunControlActExpire(); } @@ -1208,29 +1179,25 @@ void PlsrRunControlOn1ms(void) s_seg_elapsed_ms++; } - /* - * ACT 到点：若加/匀/减三阶段已经走完，不要按当前频率切段， - * 剩余脉冲继续发完，由 AfterSegDone ※4 停表等到点。 - */ if ((s_act_armed != 0U) && (PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) != 0U)) { - uint32_t t_wave; + /* + * ACT 是运动控制条件，不参与曲线规划：加速、匀速、减速 + * 任一阶段到期都在当前脉冲 UPDATE 边界切段。这样不会在 + * 一个 PWM 周期中间改段，也不会把边界脉冲计到下一段。 + */ + s_act_armed = 0U; + s_act_cut_pending = 1U; + } - t_wave = s_accel_plan.t_decel_start_ms + s_accel_plan.t_dec_ms; - if ((s_accel_plan.t_dec_ms >= 1U) && (s_seg_elapsed_ms >= t_wave)) - { - /* 三段已结束，等脉冲 ISR 把 s_done 走到 s_target */ - } - else - { - s_act_armed = 0U; - s_act_expire_req = 1U; - return; - } + /* 保持 ACT 到期瞬间的频率，不让 1ms 曲线继续推进到下一个边界。 */ + if (s_act_cut_pending != 0U) + { + return; } - if ((s_accel_plan.f_end == 0U) && + if ((s_orig_end_zero != 0U) && (s_accel_plan.mode != PLSR_ACCEL_LINEAR) && (s_accel_plan.t_dec_ms >= 1U) && (s_seg_elapsed_ms >= @@ -1266,6 +1233,21 @@ void PlsrRunControlOnPulseIsr(void) s_acc_pulse--; } + /* + * ACT 到期后的切段只在脉冲边界执行。当前 UPDATE 已完成本段 + * 最后一个完整脉冲，立即停表，任务再用当前频率衔接下一段。 + */ + if (s_act_cut_pending != 0U) + { + s_act_cut_pending = 0U; + PlsrPulseDriverStop(); + s_pwm_on = 0U; + s_act_handoff_keep = 1U; + s_state = RC_IDLE; + s_act_expire_req = 1U; + return; + } + if (s_done >= s_target) { PlsrRunControlAfterSegDone();