From 6cc6c31564c09b7a61d55b2167c65868ff703a8a Mon Sep 17 00:00:00 2001 From: hanyongwei <2043702190@qq.com> Date: Wed, 19 Aug 2026 09:59:58 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E5=8A=A0=E9=80=9F=E6=96=9C?= =?UTF-8?q?=E7=8E=87=E5=92=8C=E5=87=8F=E9=80=9F=E6=96=9C=E7=8E=87=E7=9A=84?= =?UTF-8?q?=E5=AE=9A=E4=B9=89=EF=BC=9A=E5=8A=A0=E9=80=9F=E6=96=9C=E7=8E=87?= =?UTF-8?q?=3D=EF=BC=88=E8=84=89=E5=86=B2=E9=BB=98=E8=AE=A4=E9=80=9F?= =?UTF-8?q?=E5=BA=A6-0=EF=BC=89/=E9=BB=98=E8=AE=A4=E5=8A=A0=E9=80=9F?= =?UTF-8?q?=E6=97=B6=E9=97=B4=EF=BC=9B=E5=87=8F=E9=80=9F=E6=96=9C=E7=8E=87?= =?UTF-8?q?=3D=EF=BC=88=E8=84=89=E5=86=B2=E9=BB=98=E8=AE=A4=E9=80=9F?= =?UTF-8?q?=E5=BA=A6-0=EF=BC=89/=E9=BB=98=E8=AE=A4=E5=87=8F=E9=80=9F?= =?UTF-8?q?=E6=97=B6=E9=97=B4?= 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 | 187 +++++++++++++------- plsr/accel_curve/plsr_accel_curve.h | 30 +++- plsr/param/plsr_param.h | 6 +- plsr/run_control/plsr_run_control.c | 263 +++++++++++++++------------- plsr/signal_io/plsr_signal_io.c | 6 + plsr/signal_io/plsr_signal_io.h | 5 +- 6 files changed, 310 insertions(+), 187 deletions(-) diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 21f0d65..d362c1f 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -98,20 +98,13 @@ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, return f; } -static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd, - uint32_t primary_ref, - uint32_t fallback_ref) +/** + * 斜率分母：K = 默认速度 / 加减速时间，即 (默认速度 − 0)。 + * 起/止速只决定端点，不参与斜率。 + */ +static uint32_t PlsrAccelCurveSlopeDen(uint32_t default_spd) { - uint32_t den = PlsrAccelCurveAbsDiff(default_spd, primary_ref); - if (den == 0U) - { - den = PlsrAccelCurveAbsDiff(default_spd, fallback_ref); - } - if (den == 0U) - { - den = default_spd; - } - return den; + return default_spd; } static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from, @@ -127,6 +120,9 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from, uint32_t ref_ms; uint32_t t; + (void)start_spd_ref; + (void)end_spd_ref; + if (f_from == f_to) { return 0U; @@ -136,12 +132,12 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from, if (f_to > f_from) { ref_ms = accel_ms; - den = PlsrAccelCurveSlopeDen(default_spd, start_spd_ref, end_spd_ref); + den = PlsrAccelCurveSlopeDen(default_spd); } else { ref_ms = decel_ms; - den = PlsrAccelCurveSlopeDen(default_spd, end_spd_ref, start_spd_ref); + den = PlsrAccelCurveSlopeDen(default_spd); } if (ref_ms == 0U) @@ -167,6 +163,100 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from, 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 f_tgt, + uint32_t default_spd, + uint32_t start_spd_ref, + uint32_t end_spd_ref, + uint32_t accel_ms, + uint32_t decel_ms) +{ + uint32_t t_acc; + + f_cfg = PlsrAccelCurveClampFreq(f_cfg); + f_tgt = PlsrAccelCurveClampFreq(f_tgt); + if (f_cfg >= 1U) + { + return f_cfg; + } + if (f_tgt < 1U) + { + return 1U; + } + + t_acc = PlsrAccelCurveRampTimeMs(0U, f_tgt, default_spd, + start_spd_ref, end_spd_ref, + accel_ms, decel_ms); + return PlsrAccelCurveJumpFromZero(f_tgt, t_acc, + start_spd_ref, end_spd_ref); +} + +uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, + uint32_t f_tgt, + uint32_t default_spd, + uint32_t start_spd_ref, + uint32_t end_spd_ref, + uint32_t accel_ms, + uint32_t decel_ms) +{ + uint32_t t_dec; + + f_cfg = PlsrAccelCurveClampFreq(f_cfg); + f_tgt = PlsrAccelCurveClampFreq(f_tgt); + if (f_cfg >= 1U) + { + return f_cfg; + } + if (f_tgt < 1U) + { + return 1U; + } + + t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd, + start_spd_ref, end_spd_ref, + accel_ms, decel_ms); + return PlsrAccelCurveJumpFromZero(f_tgt, t_dec, + start_spd_ref, end_spd_ref); +} + /** 直线：进度千分比 → 形状千分比 */ static uint32_t PlsrAccelCurveShapeLinearPermille(uint32_t u) { @@ -729,6 +819,23 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode; + /* + * 端点须由调用方解析完毕。若仍传入 0，用同一套 Resolve 兜底， + * 避免把 0 当真实频率规划出错误斜坡；正常路径不应走到这里。 + */ + if (f_cur < 1U) + { + f_cur = PlsrAccelCurveResolveStartHz(0U, f_tgt, default_spd, + start_spd_ref, end_spd_ref, + accel_ms, decel_ms); + } + if (f_end < 1U) + { + f_end = PlsrAccelCurveResolveEndHz(0U, f_tgt, default_spd, + start_spd_ref, end_spd_ref, + accel_ms, decel_ms); + } + f_peak = f_tgt; t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, start_spd_ref, end_spd_ref, @@ -737,56 +844,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, 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; diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index fd550b9..e839166 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -4,7 +4,9 @@ * * 1ms 改频：脉冲数按 t=0..T-1 实际取频累加；到不了目标频则降峰值走三角（无匀速）。 * 匀速仅当加速+减速脉冲之和 < 总脉冲（真正到达目标频）时出现。 - * 斜率按公共参数：K≈(默认速度−起/止速)/加减速时间；小频差时 T 可能 <1ms，无可见斜坡属正常。 + * 斜率按公共参数：K_acc=默认速度/加速时间，K_dec=默认速度/减速时间（减 0，不用起/止速）。 + * 小频差时 T 可能 <1ms，无可见斜坡属正常。 + * 开段端点由运行层 ResolveStart/EndHz 解析后再传入 Plan（Plan 不解释策略 0）。 */ #ifndef PLSR_ACCEL_CURVE_H #define PLSR_ACCEL_CURVE_H @@ -41,6 +43,32 @@ uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz); uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, uint32_t t_ms); +/** + * 配置起速 → 规划用真实起点频率。 + * f_cfg>=1：原样（钳位）；f_cfg==0：按斜率算起跳频率（从静止爬升）。 + * 由运行层在开段前调用；Plan 只吃已经解析好的端点。 + */ +uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, + uint32_t f_tgt, + uint32_t default_spd, + uint32_t start_spd_ref, + uint32_t end_spd_ref, + uint32_t accel_ms, + uint32_t decel_ms); + +/** + * 配置止速 → 规划用真实终点频率。 + * f_cfg>=1：原样（钳位）；f_cfg==0：按斜率算落地频率（避免 1Hz 拖尾）。 + * 段末是否停表由运行层决定，与本函数无关。 + */ +uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, + uint32_t f_tgt, + uint32_t default_spd, + uint32_t start_spd_ref, + uint32_t end_spd_ref, + uint32_t accel_ms, + uint32_t decel_ms); + /** * 斜坡上按 mode 取频（µs 时间轴）。ISR 可用：O(1)。 * f = f0+(f1-f0)*shape(t/T) diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h index b677577..1fd79bd 100644 --- a/plsr/param/plsr_param.h +++ b/plsr/param/plsr_param.h @@ -101,9 +101,9 @@ typedef struct { PlsrPosMode_e run_mode; uint16_t seg_count; uint16_t start_seg; /* 1-based */ - uint32_t default_speed; /* 脉冲默认速度 Hz：定加减速斜率 K；段频为 0 时也作段目标 */ - uint32_t start_speed; /* 起速Hz；0=首拍跟曲线，非0=首拍即起速 */ - uint32_t end_speed; /* 止速Hz；0=末拍跟曲线后停表，非0=末拍即止速 */ + uint32_t default_speed; /* 脉冲默认速度 Hz：斜率 K=默认速度/加减速时间；段频为 0 时也作段目标 */ + uint32_t start_speed; /* 起速Hz；0=从静止起跳（运行层 ResolveStartHz） */ + uint32_t end_speed; /* 止速Hz；0=落地频率后停表（运行层 ResolveEndHz） */ uint16_t accel_ms; /* 默认速度加速时间 ms（定加速斜率） */ uint16_t decel_ms; /* 默认速度减速时间 ms（定减速斜率） */ } PlsrCfg_t; diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index fd138b6..7f4bf6f 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -1,24 +1,16 @@ /** * @file plsr_run_control.c - * @brief 完成方式 / 后续方式 运行控制（加减速形状由 accel_mode 决定） + * @brief 运行控制：等待策略 + 开段端点 + 执行驱动 * - * 分层： - * 1) 调用/等待策略：只决定调用顺序，以及传给段处理的 - * 起始频率 f_from、终止频率 f_end、加速/减速时间（斜率基准） - * 2) 段内处理：不感知 wait_type；只按 (f_from,f_tgt,f_end,accel,decel) - * 做加速→匀速→减速（脉冲不够则三角） - * 3) 曲线估加/减速脉冲；匀速=总数−加速−减速。 - * 规划反向算 t_decel_start_ms；运行时 TIM3 1ms 按段内时间轴取频（直线为纯代数）。 - * 起止≠0：首拍起速、末拍止速；起止=0：首末跟曲线，段末停表。 - * - * 典型策略映射： - * - 完成方式：每段 f_end=止速；WAIT 只做段后等待与下一段 f_from（清链→起速） - * - 后续方式末段：f_end=止速；非末段无缝：f_end=下一段目标 - * - ACT：先按完成/后续规划加速/匀速/减速时间，再和 act_ms 比较： - * 加速 > ACT → 加速中途打断；加速+匀速 > ACT → 不进减速； - * 加速+匀速 < ACT < 三段总时间 → 减速中途打断； - * ACT ≥ 加速+匀速+减速 → 三阶段（含减速）走完，脉冲照常计数，停表等到 ACT 再下一段（※4）。 - * - 梯形/三角仅由 f_from/f_tgt/f_end/斜率/mode/脉冲数决定，不读 wait_type + * 分层（阶段1薄拆分）： + * 1) 曲线（accel_curve）：只吃 (f_from,f_tgt,f_end,斜率,N,mode) → 梯形/三角； + * 不读 wait_type。端点须为真实频率（>=1）；配置起/止=0 由运行层 + * ResolveStart/EndHz 算起跳/落地后再传入。 + * 2) 等待策略：只决定何时打断/段后等待；统一 handoff： + * - 仍在输出且 freq>=1 → 链式传给下一段 + * - 已停表/输出为 0 → 下一段从配置起速（0 则起跳频率）开跑 + * 3) 发送模式只影响本段 f_end（完成=止速；后续且不停表=下一段目标） + * 4) PlanSeg / ChangeFreq 共用规划入口，便于后期动态改频 * * 【运行节拍】TIM3 1ms：RefreshProfile 改频；脉冲 UPDATE ISR 只计 s_done。 */ @@ -79,7 +71,6 @@ 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 { @@ -138,10 +129,15 @@ static void PlsrRunControlArmActExtOnPulseStart(void) } } -/** EXT 中途切段：停表后立即按 jump 跳转 */ +/** + * EXT 触发切段（脉冲可能还在跑）。 + * 同向且 PWM 在跑：不停表，下一段 f_from = 当前频率（衔接加速）。 + * 反向或已停表：停表，下一段从起速爬。 + */ static void PlsrRunControlCutSegToNext(void) { - PlsrCfg_t *cfg = PlsrParamGetCfg(); + int16_t next0; + uint8_t keep; s_state = RC_IDLE; s_act_armed = 0U; @@ -149,26 +145,40 @@ static void PlsrRunControlCutSegToNext(void) s_act_cut_pending = 0U; s_act_handoff_keep = 0U; s_wait_expire_req = 0U; - PlsrPulseDriverStop(); - s_follow_cont = 0U; - s_pwm_on = 0U; - if (cfg->send_mode == PLSR_SEND_COMPLETE) + + next0 = PlsrPathPlanResolveAfterSeg(s_cur_seg); + keep = 0U; + if ((next0 >= 0) && (s_pwm_on != 0U) && (s_cur_freq >= 1U) && + (PlsrPathPlanIsForward((uint16_t)next0, s_acc_pulse) == s_forward)) { - s_chain_freq = s_cur_freq; + keep = 1U; + } + + if (keep != 0U) + { + PlsrPulseDriverClearOnePulseStop(); + s_follow_cont = 1U; s_chain_valid = 1U; + s_chain_freq = s_cur_freq; } else { + PlsrPulseDriverStop(); + PlsrPulseDriverClearOnePulseStop(); + s_follow_cont = 0U; + s_pwm_on = 0U; s_cur_freq = 0U; s_chain_valid = 0U; + s_chain_freq = 0U; } + PlsrRunControlGotoNextOrFinish(s_cur_seg); } /** - * ACT 时间到（脉冲可能还没发完）：从当前频率切下一段。 - * 同向：不停表，下一段 f_from=当前频率（※2 加速相 / ※3 减速相）。 - * 反向或无 PWM：停表，下一段从起速爬。 + * ACT 时间到（脉冲可能还没发完）：丢掉本段剩余脉冲/减速，从当前频率切下一段。 + * 同向：下一段 f_from=当前频率（逐层跳，不进本段减速）。 + * 反向或无频率：停表，下一段从起速爬。 */ static void PlsrRunControlActExpire(void) { @@ -220,7 +230,7 @@ static void PlsrRunControlEnterPostWaitOrNext(void) /* * 调用顺序策略（与段内波形无关）： - * WAIT时间/信号 → 停表清链后等待，再 BeginSeg（下一段 f_from=起速） + * WAIT时间/信号 → 停表清链后等待，再 BeginSeg（下一段从起速/起跳开跑） * 其它 → 立即跳转下一段 */ if (seg->wait_type == PLSR_WAIT_TIME) @@ -259,6 +269,31 @@ static void PlsrRunControlEnterPostWaitOrNext(void) return; } + if ((seg->wait_type == PLSR_WAIT_EXT) || + (seg->wait_type == PLSR_WAIT_EXT_OR_DONE)) + { + /* + * EXT 脉冲发完后等待：停表，输出已经是 0。 + * 下一段从配置起速（0 则起跳）重新规划；不是接着本段 f_end 往下跑。 + */ + PlsrPulseDriverStop(); + s_follow_cont = 0U; + s_pwm_on = 0U; + s_chain_valid = 0U; + s_chain_freq = 0U; + s_cur_freq = 0U; + /* 只清消抖中的假沿；已确认的 fell 保留，供下面 TakeExtFalling 消费 */ + PlsrSignalIoClearPending(); + if (PlsrSignalIoTakeExtFalling() != 0U) + { + PlsrRunControlGotoNextOrFinish(s_cur_seg); + return; + } + s_wait_is_signal = 1U; + s_state = RC_WAIT_COND; + return; + } + PlsrRunControlGotoNextOrFinish(s_cur_seg); } @@ -326,8 +361,8 @@ static void PlsrRunControlAfterSegDone(void) if (cfg->send_mode == PLSR_SEND_COMPLETE) { - /* 段正常结束：下一段起速 = 本段止速 f_end，不能用峰值 s_cur_freq */ - s_chain_freq = (s_orig_end_zero != 0U) ? 0U : s_accel_plan.f_end; + /* 段正常结束：下一段起速 = 本段实际止速 plan.f_end，不能用峰值 s_cur_freq */ + s_chain_freq = s_accel_plan.f_end; s_chain_valid = 1U; } @@ -356,7 +391,13 @@ static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg) { PlsrSeg_t *seg = PlsrParamGetSeg(cur_seg); - /* WAIT时间(含0→1ms)、WAIT信号、ACT/EXT：均禁止无缝衔接 */ + /* + * WAIT时间/信号/ACT：禁止后续无缝衔接（发完后停表等条件）。 + * EXT/EXT_OR_DONE：脉冲发完之前 EXT 可能尚未触发，发完后进等待态， + * 此时 PWM 必须停；所以也禁止无缝衔接。 + * （EXT 触发时若脉冲还在跑，由 CutSegToNext 判断 keep， + * 那条路不走 BlocksFollowKeep。） + */ if ((seg->wait_type == PLSR_WAIT_TIME) || (seg->wait_type == PLSR_WAIT_SIGNAL) || (seg->wait_type == PLSR_WAIT_ACT) || @@ -385,38 +426,60 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, io->accel_ms = cfg->accel_ms; io->decel_ms = cfg->decel_ms; - /* ---------- 起始频率：由衔接状态决定（WAIT 会清链 → 起速） ---------- */ - if (s_follow_cont != 0U) + /* ---------- 起始频率 ---------- + * 中途切段（ACT/EXT）会置 follow_cont：用打断瞬间的频率衔接下一段。 + * ACT 为了对齐脉冲边界可能先停表，此时 pwm_on=0，但仍要跟当前频率， + * 不能当成「段后停表」去走起跳。 + * 真正段后停表：follow_cont/chain 已清，从配置起速（0 则起跳）开跑。 + */ + if ((s_follow_cont != 0U) && (s_cur_freq >= 1U)) { io->f_from = s_cur_freq; } - else if ((cfg->send_mode == PLSR_SEND_COMPLETE) && (s_chain_valid != 0U)) + else if ((s_pwm_on != 0U) && (s_cur_freq >= 1U)) + { + io->f_from = s_cur_freq; + } + else if ((s_chain_valid != 0U) && (s_chain_freq >= 1U) && + ((s_pwm_on != 0U) || (s_follow_cont != 0U))) { io->f_from = s_chain_freq; } else { - io->f_from = PlsrRunControlClampSpeed(cfg->start_speed); + io->f_from = PlsrAccelCurveResolveStartHz(cfg->start_speed, + f_tgt, + cfg->default_speed, + cfg->start_speed, + cfg->end_speed, + io->accel_ms, + io->decel_ms); } /* - * ---------- 终止频率：仅由发送模式 + 是否有下一段决定 ---------- - * 完成方式 / 真末段：公共止速 - * 后续方式非末段：下一段目标（无缝衔接） - * ACT 只由运行控制层管理，不改变本段曲线的 f_end 或阶段规划。 + * ---------- 终止频率 ---------- + * 完成方式 / 真末段 / 发完后要停表等（WAIT时间/信号）：本段规划到止速。 + * ACT/EXT 中途打断时剩余减速本来就会丢弃，不影响「进不进减速」。 + * 后续方式且无门禁：过渡到下一段目标。 */ - (void)f_tgt; next0 = PlsrPathPlanNextSeg(seg_0); end_at_stop = (next0 < 0) ? 1U : 0U; - if (cfg->send_mode == PLSR_SEND_COMPLETE) + if ((cfg->send_mode == PLSR_SEND_COMPLETE) || + (PlsrRunControlBlocksFollowKeep(seg_0) != 0U)) { end_at_stop = 1U; } if (end_at_stop != 0U) { - io->f_end = PlsrRunControlClampSpeed(cfg->end_speed); + io->f_end = PlsrAccelCurveResolveEndHz(cfg->end_speed, + f_tgt, + cfg->default_speed, + cfg->start_speed, + cfg->end_speed, + io->accel_ms, + io->decel_ms); return 1U; } @@ -619,30 +682,23 @@ 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) || (s_orig_end_zero != 0U))) + (remain_pulses == old_dec_n)) { s_accel_plan.t_dec_ms = old_t_dec; return; } - /* 参数斜率决定的理想减速时间（本段 accel_ms/decel_ms） */ + /* 参数斜率：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; - den = (cfg->default_speed > cfg->start_speed) ? - (cfg->default_speed - cfg->start_speed) : - ((cfg->default_speed > cfg->end_speed) ? - (cfg->default_speed - cfg->end_speed) : cfg->default_speed); } else { ref_ms = s_run_decel_ms; - den = (cfg->default_speed > cfg->end_speed) ? - (cfg->default_speed - cfg->end_speed) : - ((cfg->default_speed > cfg->start_speed) ? - (cfg->default_speed - cfg->start_speed) : cfg->default_speed); } + den = cfg->default_speed; if (den == 0U) { den = df; @@ -656,15 +712,8 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) } } - 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); - return; - } - /* - * 止速≠0：ISR 内禁止 FitRamp/EstimatePulses；用 O(1) 平均公式。 + * ISR 内禁止 FitRamp/EstimatePulses；用 O(1) 平均公式。 * 脉冲被截断导致 remain 变化时才走这里。 */ sum_f = from_hz + f_end; @@ -706,25 +755,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms); if (next < 1U) { - if ((s_orig_end_zero != 0U) && (s_seg_elapsed_ms > 0U)) - { - /* - * 原始止速为 0，时间轴走完。剩余脉冲仍要发出并计数（ACT※4： - * 三阶段走完后等时间，不能把 s_target 截掉导致计数丢失）。 - */ - if (s_done < s_target) - { - next = 1U; - } - else - { - return; - } - } - else - { - next = 1U; - } + next = 1U; } if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U)) @@ -738,17 +769,17 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, uint16_t accel_ms, uint16_t decel_ms) { PlsrCfg_t *cfg = PlsrParamGetCfg(); + uint32_t f_start; uint32_t f_lo; uint32_t f_hi; /* 段内只吃传入端点与加减速时间，不感知 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。 - * 起止≠0：运行时首拍=起速、末拍=止速；起止=0：首末跟曲线，段末停表。 + * f_from/f_end 已由策略层解析为真实频率；配置起止=0 时已换成起跳/落地。 */ PlsrAccelCurvePlan(&s_accel_plan, total, @@ -762,6 +793,9 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, decel_ms, cfg->accel_mode); + /* 运行层使用规划后的实际起始频率。 */ + f_start = s_accel_plan.f_cur; + /* * 1ms 改频模式：运行时按 TIM3 毫秒时间轴取频。 * 禁止在这里用脉冲递推模型二次拟合 T（FitRamp），否则规划与运行模型不一致， @@ -771,15 +805,15 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, /* 本段频率范围锁 PSC，升降只改 ARR；跨度过大则 LockPscRange 自动不锁 */ f_lo = s_accel_plan.f_tgt; f_hi = s_accel_plan.f_tgt; - if (f_from > f_hi) + if (f_start > f_hi) { - f_hi = f_from; + f_hi = f_start; } - if (f_from >= 1U) + if (f_start >= 1U) { - if (f_from < f_lo) + if (f_start < f_lo) { - f_lo = f_from; + f_lo = f_start; } } else @@ -820,11 +854,11 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, PlsrPulseDriverLockPscRange(f_lo, f_hi); } - s_approach_from = f_from; + s_approach_from = f_start; s_decel_from = s_accel_plan.f_tgt; if (s_pwm_on == 0U) { - s_cur_freq = f_from; + s_cur_freq = f_start; } /* @@ -839,17 +873,17 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, if ((s_accel_plan.dec_n >= total) && (total > 0U) && (s_accel_plan.f_end != s_accel_plan.f_tgt) && - (f_from >= s_accel_plan.f_tgt)) + (f_start >= s_accel_plan.f_tgt)) { - PlsrRunControlEnterDecel(f_from, total); + PlsrRunControlEnterDecel(f_start, total); } - else if (f_from == s_accel_plan.f_tgt) + else if (f_start == s_accel_plan.f_tgt) { - PlsrRunControlEnterPhase(PH_CONST, f_from); + PlsrRunControlEnterPhase(PH_CONST, f_start); } else { - PlsrRunControlEnterPhase(PH_APPROACH, f_from); + PlsrRunControlEnterPhase(PH_APPROACH, f_start); } s_seg_elapsed_ms = 0U; } @@ -990,7 +1024,6 @@ 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; @@ -1143,9 +1176,21 @@ void PlsrRunControlTickMs(void) { if (s_wait_is_signal != 0U) { - if (PlsrSignalIoTakeWaitFalling() != 0U) + seg = PlsrParamGetSeg(s_cur_seg); + if ((seg->wait_type == PLSR_WAIT_EXT) || + (seg->wait_type == PLSR_WAIT_EXT_OR_DONE)) { - PlsrRunControlGotoNextOrFinish(s_cur_seg); + if (PlsrSignalIoTakeExtFalling() != 0U) + { + PlsrRunControlGotoNextOrFinish(s_cur_seg); + } + } + else + { + if (PlsrSignalIoTakeWaitFalling() != 0U) + { + PlsrRunControlGotoNextOrFinish(s_cur_seg); + } } } /* WAIT 时间 / ACT 剩余到期在 On1ms */ @@ -1197,21 +1242,6 @@ void PlsrRunControlOn1ms(void) return; } - if ((s_orig_end_zero != 0U) && - (s_accel_plan.mode != PLSR_ACCEL_LINEAR) && - (s_accel_plan.t_dec_ms >= 1U) && - (s_seg_elapsed_ms >= - (s_accel_plan.t_decel_start_ms + s_accel_plan.t_dec_ms))) - { - /* 非直线：时间轴走完减速后，剩余脉冲仍按最后频率发完并计数 */ - if ((s_done >= s_target) && - (PlsrRunControlWillFollowKeep(s_cur_seg) == 0U)) - { - PlsrPulseDriverArmOnePulseStop(); - } - return; - } - PlsrRunControlRefreshProfile(0U); } @@ -1234,14 +1264,13 @@ void PlsrRunControlOnPulseIsr(void) } /* - * ACT 到期后的切段只在脉冲边界执行。当前 UPDATE 已完成本段 - * 最后一个完整脉冲，立即停表，任务再用当前频率衔接下一段。 + * ACT 到期切段只在脉冲边界执行，避免半个 PWM 周期改段。 + * 同向衔接：不停表，任务里用当前频率规划下一段（丢掉本段剩余）。 + * 停表会把 pwm_on 清掉，Resolve 若再要求 pwm_on 就会误走起跳。 */ 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; diff --git a/plsr/signal_io/plsr_signal_io.c b/plsr/signal_io/plsr_signal_io.c index ead5058..f5797cc 100644 --- a/plsr/signal_io/plsr_signal_io.c +++ b/plsr/signal_io/plsr_signal_io.c @@ -174,6 +174,12 @@ void PlsrSignalIoClearEdges(void) s_db_x5.pending = 0U; } +void PlsrSignalIoClearPending(void) +{ + s_db_x4.pending = 0U; + s_db_x5.pending = 0U; +} + static PlsrEdge_t *PlsrSignalIoDbBySel(uint16_t sel_01) { return (sel_01 != 0U) ? &s_db_x5 : &s_db_x4; diff --git a/plsr/signal_io/plsr_signal_io.h b/plsr/signal_io/plsr_signal_io.h index 9a2f731..4741dd5 100644 --- a/plsr/signal_io/plsr_signal_io.h +++ b/plsr/signal_io/plsr_signal_io.h @@ -21,9 +21,12 @@ uint32_t PlsrSignalIoDeadlineFromNow(uint32_t ms); /** 1=已到或过期（含无符号环绕） */ uint8_t PlsrSignalIoMsReached(uint32_t deadline_ms); -/** 清 WAIT/EXT 下降沿锁存 */ +/** 清 WAIT/EXT 下降沿锁存（包含已确认的 fell） */ void PlsrSignalIoClearEdges(void); +/** 只清消抖 pending，保留已确认的 fell（发完后过滤脉冲耦合假沿用） */ +void PlsrSignalIoClearPending(void); + /** WAIT 口当前是否为低（已接地）。仅段末判断※2 时读一次，不作周期轮询触发 */ uint8_t PlsrSignalIoWaitIsOn(void);