diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h index aefc603..b677577 100644 --- a/plsr/param/plsr_param.h +++ b/plsr/param/plsr_param.h @@ -83,7 +83,7 @@ typedef enum { typedef enum { PLSR_WAIT_TIME = 0, /* 发完后等 wait_ms，再跳转 */ PLSR_WAIT_SIGNAL = 1, /* 发完后等 WAIT 口下降沿（EXTI），再跳转 */ - PLSR_WAIT_ACT = 2, /* 出脉冲满 act_ms 后中途切段并立即跳转 */ + PLSR_WAIT_ACT = 2, /* ACT：满 act_ms 从当前频率切下一段；脉冲先完则等到点 */ PLSR_WAIT_EXT = 3, /* 运行中 EXT 下降沿中途切段并立即跳转 */ PLSR_WAIT_EXT_OR_DONE = 4 /* EXT 沿或脉冲发完，先到先跳转 */ } PlsrWaitType_e; diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index f8abfbe..a1cfad1 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -14,6 +14,10 @@ * 典型策略映射： * - 完成方式：每段 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 * * 【运行节拍】TIM3 1ms：RefreshProfile 改频；脉冲 UPDATE ISR 只计 s_done。 @@ -51,9 +55,12 @@ static volatile int32_t s_target; static volatile int32_t s_acc_pulse; static INT32U s_dir_deadline; -static uint32_t s_wait_deadline_ms; /* TIM3：段后 WAIT 时间 */ -static uint32_t s_act_deadline_ms; /* TIM3：ACT 脉冲输出时长 */ -static uint8_t s_act_armed; +static uint32_t s_wait_deadline_ms; /* TIM3：段后 WAIT 时间 / ACT※4 剩余 */ +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_wait_expire_req; /* WAIT 时间到：ISR 只置位 */ static uint8_t s_wait_is_signal; /* RC_WAIT_COND：1=等 WAIT 沿，0=等时间 */ static PlsrAccelPlan_t s_accel_plan; @@ -85,7 +92,9 @@ static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg); static void PlsrRunControlFinishAll(void); static void PlsrRunControlArmActExtOnPulseStart(void); static void PlsrRunControlCutSegToNext(void); +static void PlsrRunControlActExpire(void); static void PlsrRunControlEnterPostWaitOrNext(void); +static uint16_t PlsrRunControlActTimeMs(const PlsrSeg_t *seg); static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg); static uint8_t PlsrRunControlWillFollowKeep(uint16_t cur_seg); static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, @@ -94,6 +103,7 @@ 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, @@ -113,20 +123,26 @@ static void PlsrRunControlArmActExtOnPulseStart(void) PlsrSignalIoClearEdges(); } s_act_armed = 0U; - if ((seg->wait_type == PLSR_WAIT_ACT) && (seg->act_ms > 0U)) + s_act_expire_req = 0U; + s_wait_expire_req = 0U; + if (seg->wait_type == PLSR_WAIT_ACT) { + /* 先写时长/起点再武装。计时用自由运行 s_ms，不被 PlanSeg 清 elapsed */ + s_act_time_ms = PlsrRunControlActTimeMs(seg); + s_act_arm_ms = PlsrSignalIoGetMs(); s_act_armed = 1U; - s_act_deadline_ms = PlsrSignalIoDeadlineFromNow(seg->act_ms); } } -/** ACT/EXT 中途切段：停表后立即按 jump 跳转（不再做段后 WAIT） */ +/** EXT 中途切段：停表后立即按 jump 跳转 */ static void PlsrRunControlCutSegToNext(void) { PlsrCfg_t *cfg = PlsrParamGetCfg(); s_state = RC_IDLE; s_act_armed = 0U; + s_act_expire_req = 0U; + s_wait_expire_req = 0U; PlsrPulseDriverStop(); s_follow_cont = 0U; s_pwm_on = 0U; @@ -143,6 +159,50 @@ static void PlsrRunControlCutSegToNext(void) PlsrRunControlGotoNextOrFinish(s_cur_seg); } +/** + * ACT 时间到（脉冲可能还没发完）：从当前频率切下一段。 + * 同向：不停表，下一段 f_from=当前频率（※2 加速相 / ※3 减速相）。 + * 反向或无 PWM：停表，下一段从起速爬。 + */ +static void PlsrRunControlActExpire(void) +{ + int16_t next0; + uint8_t keep; + + s_act_armed = 0U; + s_act_expire_req = 0U; + s_wait_expire_req = 0U; + s_state = RC_IDLE; + + 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)) + { + 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); +} + static void PlsrRunControlEnterPostWaitOrNext(void) { PlsrSeg_t *seg = PlsrParamGetSeg(s_cur_seg); @@ -209,11 +269,48 @@ static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg) static void PlsrRunControlAfterSegDone(void) { PlsrCfg_t *cfg = PlsrParamGetCfg(); + PlsrSeg_t *seg = PlsrParamGetSeg(s_cur_seg); uint8_t keep_pwm; /* 先脱离 RUN，避免脉冲 ISR 重入 */ s_state = RC_IDLE; + + /* + * ACT ※4：脉冲已发完但 ACT 时间未到 → 停表，等到到期再开下一段。 + * 不从当前频率衔接（本段已经走完停表）。 + */ + if (seg->wait_type == PLSR_WAIT_ACT) + { + uint16_t remain; + + PlsrPulseDriverStop(); + PlsrPulseDriverClearOnePulseStop(); + s_follow_cont = 0U; + s_pwm_on = 0U; + s_cur_freq = 0U; + s_chain_valid = 0U; + s_chain_freq = 0U; + s_act_expire_req = 0U; + if ((s_act_armed != 0U) && + (PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) == 0U)) + { + uint32_t used = PlsrSignalIoGetMs() - s_act_arm_ms; + remain = (used < (uint32_t)s_act_time_ms) ? + (uint16_t)((uint32_t)s_act_time_ms - used) : 1U; + s_act_armed = 0U; + s_wait_is_signal = 0U; + s_wait_deadline_ms = PlsrSignalIoDeadlineFromNow(remain); + s_state = RC_WAIT_COND; + return; + } + s_act_armed = 0U; + PlsrRunControlGotoNextOrFinish(s_cur_seg); + return; + } + s_act_armed = 0U; + s_act_expire_req = 0U; + s_wait_expire_req = 0U; keep_pwm = PlsrRunControlWillFollowKeep(s_cur_seg); @@ -275,8 +372,6 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, int16_t next0; uint8_t end_at_stop; /* 1：本段终止频率用公共止速 */ - (void)f_tgt; - io->accel_ms = cfg->accel_ms; io->decel_ms = cfg->decel_ms; @@ -298,8 +393,9 @@ static uint8_t PlsrRunControlResolveSegCallIo(uint16_t seg_0, uint32_t f_tgt, * ---------- 终止频率：仅由发送模式 + 是否有下一段决定 ---------- * 完成方式 / 真末段：公共止速 * 后续方式非末段：下一段目标（无缝衔接） - * 不读 wait_type——等待只在段后 EnterPostWaitOrNext 处理 + * ACT 不在这里改 f_end：先按梯形算出加/匀/减时间，再 ApplyActTimeCap。 */ + (void)f_tgt; next0 = PlsrPathPlanNextSeg(seg_0); end_at_stop = (next0 < 0) ? 1U : 0U; @@ -337,6 +433,21 @@ static uint16_t PlsrRunControlWaitTimeMs(const PlsrSeg_t *seg) return seg->wait_ms; } +/** ACT 时间：优先 act_ms；0 则回退 wait_ms（旧上位机曾把 ACT 写在 WAIT 列） */ +static uint16_t PlsrRunControlActTimeMs(const PlsrSeg_t *seg) +{ + uint16_t ms = seg->act_ms; + if (ms == 0U) + { + ms = seg->wait_ms; + } + if (ms == 0U) + { + return 1U; + } + return ms; +} + static uint8_t PlsrRunControlWillFollowKeep(uint16_t cur_seg) { PlsrCfg_t *cfg = PlsrParamGetCfg(); @@ -587,17 +698,23 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) { if ((s_accel_plan.f_end == 0U) && (s_seg_elapsed_ms > 0U)) { + /* + * 时间轴已落到止速 0。剩余脉冲仍要发出并计数（ACT※4： + * 三阶段走完后等时间，不能把 s_target 截掉导致计数丢失）。 + */ if (s_done < s_target) { - s_target = s_done + 1; + next = 1U; } - if (PlsrRunControlWillFollowKeep(s_cur_seg) == 0U) + else { - PlsrPulseDriverArmOnePulseStop(); + return; } - return; } - next = 1U; + else + { + next = 1U; + } } if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U)) @@ -726,6 +843,41 @@ 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(); @@ -738,6 +890,11 @@ static void PlsrRunControlFinishAll(void) s_follow_cont = 0U; s_chain_valid = 0U; s_chain_freq = 0U; + s_act_armed = 0U; + s_act_expire_req = 0U; + s_wait_expire_req = 0U; + s_act_time_ms = 0U; + s_act_arm_ms = 0U; } static void PlsrRunControlBeginSeg(uint16_t seg_0) @@ -813,6 +970,10 @@ 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; @@ -861,9 +1022,12 @@ void PlsrRunControlInit(void) s_approach_from = 0U; s_decel_from = 0U; s_act_armed = 0U; + s_act_expire_req = 0U; + s_wait_expire_req = 0U; + s_act_time_ms = 0U; + s_act_arm_ms = 0U; s_wait_is_signal = 0U; s_wait_deadline_ms = 0U; - s_act_deadline_ms = 0U; } uint8_t PlsrRunControlStart(uint16_t start_seg_1based) @@ -909,6 +1073,10 @@ void PlsrRunControlStop(void) s_chain_valid = 0U; s_chain_freq = 0U; s_act_armed = 0U; + s_act_expire_req = 0U; + s_wait_expire_req = 0U; + s_act_time_ms = 0U; + s_act_arm_ms = 0U; } uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz) @@ -933,6 +1101,13 @@ 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; @@ -944,6 +1119,26 @@ void PlsrRunControlTickMs(void) { PlsrSeg_t *seg; + /* ACT / WAIT 到期切段放任务里做（PlanSeg/BeginSeg 不能堵在 TIM3 ISR） */ + if (s_act_expire_req != 0U) + { + s_act_expire_req = 0U; + if (s_state == RC_RUN) + { + PlsrRunControlActExpire(); + } + return; + } + if (s_wait_expire_req != 0U) + { + s_wait_expire_req = 0U; + if (s_state == RC_WAIT_COND) + { + PlsrRunControlGotoNextOrFinish(s_cur_seg); + } + return; + } + if (s_state == RC_DIR_WAIT) { if (PlsrRunControlOsTimeReached(s_dir_deadline) != 0U) @@ -958,16 +1153,9 @@ void PlsrRunControlTickMs(void) if (s_state == RC_RUN) { - /* 波形改频在 TIM3 On1ms；此处只处理 ACT/EXT */ + /* 波形改频 / ACT 计时在 TIM3 On1ms；此处只处理 EXT */ seg = PlsrParamGetSeg(s_cur_seg); - if ((s_act_armed != 0U) && - (PlsrSignalIoMsReached(s_act_deadline_ms) != 0U)) - { - PlsrRunControlCutSegToNext(); - return; - } - if ((seg->wait_type == PLSR_WAIT_EXT) || (seg->wait_type == PLSR_WAIT_EXT_OR_DONE)) { @@ -989,18 +1177,27 @@ void PlsrRunControlTickMs(void) PlsrRunControlGotoNextOrFinish(s_cur_seg); } } - else if (PlsrSignalIoMsReached(s_wait_deadline_ms) != 0U) - { - PlsrRunControlGotoNextOrFinish(s_cur_seg); - } + /* WAIT 时间 / ACT 剩余到期在 On1ms */ } } /** * TIM3 1ms：段内时间累加 + 按时间轴 RefreshProfile 改频。 + * ACT/WAIT 只置标志，切段在 TickMs（避免 ISR 里 PlanSeg 丢拍）。 */ void PlsrRunControlOn1ms(void) { + if (s_state == RC_WAIT_COND) + { + /* WAIT 时间 / ACT※4 剩余：跟 TIM3 1ms，不走 OS 节拍 */ + if ((s_wait_is_signal == 0U) && + (PlsrSignalIoMsReached(s_wait_deadline_ms) != 0U)) + { + s_wait_expire_req = 1U; + } + return; + } + if (s_state != RC_RUN) { return; @@ -1011,17 +1208,37 @@ 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; + + 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; + } + } + if ((s_accel_plan.f_end == 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) - { - s_target = s_done + 1; - } - if (PlsrRunControlWillFollowKeep(s_cur_seg) == 0U) + /* 非直线：时间轴走完减速后，剩余脉冲仍按最后频率发完并计数 */ + if ((s_done >= s_target) && + (PlsrRunControlWillFollowKeep(s_cur_seg) == 0U)) { PlsrPulseDriverArmOnePulseStop(); }