Bladeren bron

清理冗余设计3

dev3
hanyongwei 3 weken geleden
bovenliggende
commit
b86e343c92
1 gewijzigde bestanden met toevoegingen van 96 en 241 verwijderingen
  1. +96
    -241
      plsr/run_control/plsr_run_control.c

+ 96
- 241
plsr/run_control/plsr_run_control.c Bestand weergeven

@@ -42,7 +42,7 @@ volatile uint8_t g_plsr_busy;
volatile uint16_t g_plsr_current_segment_index; volatile uint16_t g_plsr_current_segment_index;
volatile int32_t g_plsr_accumulated_pulses; volatile int32_t g_plsr_accumulated_pulses;
static volatile RcState_e s_state; static volatile RcState_e s_state;
static volatile uint8_t s_forward;
static volatile uint8_t s_forward;/*当前端脉冲方向*/
static volatile int32_t s_segment_pulses_done; static volatile int32_t s_segment_pulses_done;
static volatile int32_t s_segment_pulse_target; static volatile int32_t s_segment_pulse_target;
static int32_t s_absolute_origin; static int32_t s_absolute_origin;
@@ -182,7 +182,7 @@ static void PlsrRunControlClearChainAndStop(void)
} }


/** /**
* @brief 中途切段 handoff:同向且有有效频率则不停表衔接,否则停表清链
* @brief 中途切段:同向且有有效频率则不停表衔接,否则停表清链
* @param allow_act_handoff 1=允许 s_keep_frequency_after_act(边界停表后仍衔接) * @param allow_act_handoff 1=允许 s_keep_frequency_after_act(边界停表后仍衔接)
*/ */
static void PlsrRunControlApplyKeepOrStop(uint8_t allow_act_handoff) static void PlsrRunControlApplyKeepOrStop(uint8_t allow_act_handoff)
@@ -226,31 +226,25 @@ static void PlsrRunControlArmActExtOnPulseStart(void)
{ {
PlsrSignalIoClearEdges(); PlsrSignalIoClearEdges();
} }
s_act_timer_armed = 0U; s_act_timer_armed = 0U;
s_act_expired = 0U; s_act_expired = 0U;
s_act_waiting_for_pulse_boundary = 0U; s_act_waiting_for_pulse_boundary = 0U;
s_keep_frequency_after_act = 0U; s_keep_frequency_after_act = 0U;
s_wait_time_expired = 0U; s_wait_time_expired = 0U;
if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_ACT) if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_ACT)
{ {
act_time_ms = g_plsr_segments[g_plsr_current_segment_index].act_ms; act_time_ms = g_plsr_segments[g_plsr_current_segment_index].act_ms;
if (act_time_ms == 0U)
{
act_time_ms = g_plsr_segments[g_plsr_current_segment_index].wait_ms;
}
if (act_time_ms == 0U)
{
act_time_ms = 1U;
}
s_act_timer_armed = 1U; s_act_timer_armed = 1U;
PlsrSignalIoScheduleDelayMs((uint32_t)act_time_ms); PlsrSignalIoScheduleDelayMs((uint32_t)act_time_ms);
} }
} }


/** /**
* @brief EXT 触发切段(脉冲可能还在跑)
* @brief EXT信号触发切段
* *
* 同向且 PWM 在跑:不停表,下一段 f_from = 当前频率(衔接加速)
* 同向且 PWM 在跑:不停表,下一段 f_from = 当前频率。
* 反向或已停表:停表,下一段从起速爬。 * 反向或已停表:停表,下一段从起速爬。
*/ */
static void PlsrRunControlCutSegToNext(void) static void PlsrRunControlCutSegToNext(void)
@@ -290,18 +284,13 @@ static void PlsrRunControlActExpire(void)
/** /**
* @brief 段发完后的等待策略入口 * @brief 段发完后的等待策略入口
* *
* WAIT时间/信号/EXT → 停表清链后进 RC_WAIT_COND 或立即 GotoNext;
* WAIT时间/信号/EXT信号 → 停表清链后进 RC_WAIT_COND 或立即 GotoNext;
* EXT_OR_DONE 脉冲先完 → 立刻下一段;其它 → GotoNextOrFinish。 * EXT_OR_DONE 脉冲先完 → 立刻下一段;其它 → GotoNextOrFinish。
*/ */
static void PlsrRunControlEnterPostWaitOrNext(void) static void PlsrRunControlEnterPostWaitOrNext(void)
{ {
uint16_t wms; uint16_t wms;


/*
* 调用顺序策略(与段内波形无关):
* WAIT时间/信号 → 停表清链后等待,再 BeginSeg(下一段从起速/起跳开跑)
* 其它 → 立即跳转下一段
*/
if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_TIME) if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_TIME)
{ {
wms = g_plsr_segments[g_plsr_current_segment_index].wait_ms; wms = g_plsr_segments[g_plsr_current_segment_index].wait_ms;
@@ -311,17 +300,14 @@ static void PlsrRunControlEnterPostWaitOrNext(void)
} }
PlsrRunControlClearChainAndStop(); PlsrRunControlClearChainAndStop();
s_waiting_for_input_signal = 0U; s_waiting_for_input_signal = 0U;
PlsrRunControlWakeDrain();
PlsrRunControlWakeDrain();/*清理过期令牌*/
s_state = RC_WAIT_COND; s_state = RC_WAIT_COND;
PlsrSignalIoScheduleDelayMs((uint32_t)wms); PlsrSignalIoScheduleDelayMs((uint32_t)wms);
return; return;
} }
if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_SIGNAL) if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_SIGNAL)
{ {
/*
* 与 WAIT 时间相同:本段发完停表,再等 EXTI 确认后的下降沿。
* 只认锁存的沿,不读当前电平——口线虚低或脉冲耦合都会误跳下一段。
*/
/* 与 WAIT 时间相同:本段发完停表,再等 EXTI 确认后的下降沿 */
PlsrRunControlClearChainAndStop(); PlsrRunControlClearChainAndStop();
if (PlsrSignalIoTakeWaitFalling() != 0U) if (PlsrSignalIoTakeWaitFalling() != 0U)
{ {
@@ -335,10 +321,7 @@ static void PlsrRunControlEnterPostWaitOrNext(void)


if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_EXT) if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_EXT)
{ {
/*
* 纯 EXT:脉冲发完后仍要等 EXT 沿才跳转。
* 停表,下一段从起速/起跳重新规划。
*/
/* EXT信号:脉冲发完后仍要等 EXT 沿才跳转 */
PlsrRunControlClearChainAndStop(); PlsrRunControlClearChainAndStop();
/* 只清消抖中的假沿;已确认的 fell 保留,供下面 TakeExtFalling 消费 */ /* 只清消抖中的假沿;已确认的 fell 保留,供下面 TakeExtFalling 消费 */
PlsrSignalIoClearPending(); PlsrSignalIoClearPending();
@@ -353,14 +336,14 @@ static void PlsrRunControlEnterPostWaitOrNext(void)
} }


/* /*
* EXT_OR_DONE:脉冲先发完 = 条件已满足,立刻下一段(不再等 EXT)
* EXT_OR_DONE:脉冲先发完 = 条件已满足,立刻下一段。
* EXT 先到:RUN 里已 CutSegToNext,走不到这里。 * EXT 先到:RUN 里已 CutSegToNext,走不到这里。
*/ */
PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index);
} }


/** /**
* @brief 解析下一段:无则 FinishAll,有则 BeginSeg
* @brief 解析下一段:无则返回-1进 FinishAll,有则 BeginSeg
*/ */
static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg) static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg)
{ {
@@ -376,8 +359,6 @@ static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg)
} }
} }


/* ---------- 故障诊断(写 0x2006,见文件头说明) ---------- */

/** /**
* 0x01 脉冲个数诊断 * 0x01 脉冲个数诊断
* 时机:段正常结束进入 AfterSegDone 时。 * 时机:段正常结束进入 AfterSegDone 时。
@@ -392,13 +373,7 @@ static void PlsrRunControlCheckPulseCntFault(void)
} }
} }


/**
* 0x03 加速曲线诊断(离开 APPROACH → 匀速/减速前调用)
* @param by_pulse_budget
* 1:因相内脉冲走到规划 acc_n 而收尾 → 要求 g_plsr_accel_runtime.completed_pulses == g_plsr_accel_plan.accel_pulses
* 0:因频率已提前到达目标而收尾 → 仅当 n 超过规划预算时报故障
* 对照量直接使用 PlanSeg 写入的 g_plsr_accel_plan.accel_pulses。
*/
/* 0x03 加速曲线诊断 */
static void PlsrRunControlCheckApproachCurveFault(uint8_t by_pulse_budget) static void PlsrRunControlCheckApproachCurveFault(uint8_t by_pulse_budget)
{ {
if (g_plsr_accel_plan.accel_pulses == 0U) if (g_plsr_accel_plan.accel_pulses == 0U)
@@ -418,11 +393,7 @@ static void PlsrRunControlCheckApproachCurveFault(uint8_t by_pulse_budget)
} }
} }


/**
* 0x03 减速曲线诊断
* 时机:段结束 AfterSegDone;仅当仍处 DECEL 相。
* 条件:相内步进须至少跑满进入时锁定的预算(ARPE 超前下允许 n==budget)。
*/
/* 0x03 减速曲线诊断 */
static void PlsrRunControlCheckDecelCurveFault(void) static void PlsrRunControlCheckDecelCurveFault(void)
{ {
if ((s_phase == PH_DECEL) && (g_plsr_accel_runtime.total_pulses > 0U) && if ((s_phase == PH_DECEL) && (g_plsr_accel_runtime.total_pulses > 0U) &&
@@ -446,10 +417,7 @@ static void PlsrRunControlAfterSegDone(void)
/* 先脱离 RUN,避免脉冲 ISR 重入 */ /* 先脱离 RUN,避免脉冲 ISR 重入 */
s_state = RC_IDLE; s_state = RC_IDLE;


/*
* ACT ※4:脉冲已发完但 ACT 时间未到 → 停表,等到到期再开下一段。
* 不从当前频率衔接(本段已经走完停表)。
*/
/* 脉冲已发完但 ACT 时间未到 → 停表,等到到期再开下一段 */
if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_ACT) if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_ACT)
{ {
PlsrPulseDriverStop(); PlsrPulseDriverStop();
@@ -462,7 +430,7 @@ static void PlsrRunControlAfterSegDone(void)
{ {
uint32_t remain_ms = PlsrSignalIoDelayRemainMs(); uint32_t remain_ms = PlsrSignalIoDelayRemainMs();


PlsrSignalIoCancelDelay();
PlsrSignalIoCancelDelay();//段发完后,wait时间取消旧定时器,重新计算剩余wait时间
s_act_timer_armed = 0U; s_act_timer_armed = 0U;
if (remain_ms >= 1U) if (remain_ms >= 1U)
{ {
@@ -475,7 +443,7 @@ static void PlsrRunControlAfterSegDone(void)
} }
else else
{ {
PlsrSignalIoCancelDelay();
PlsrSignalIoCancelDelay();//计时已到,取消定时
} }
s_act_timer_armed = 0U; s_act_timer_armed = 0U;
PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index);
@@ -485,7 +453,7 @@ static void PlsrRunControlAfterSegDone(void)
s_act_timer_armed = 0U; s_act_timer_armed = 0U;
s_act_expired = 0U; s_act_expired = 0U;
s_wait_time_expired = 0U; s_wait_time_expired = 0U;
/*硬件脉冲是否关闭,1不关,0关闭*/
keep_pwm = (s_stop_after_last_pulse == 0U) ? 1U : 0U; keep_pwm = (s_stop_after_last_pulse == 0U) ? 1U : 0U;


if (g_plsr_config.send_mode == PLSR_SEND_COMPLETE) if (g_plsr_config.send_mode == PLSR_SEND_COMPLETE)
@@ -497,7 +465,7 @@ static void PlsrRunControlAfterSegDone(void)


if (keep_pwm != 0U) if (keep_pwm != 0U)
{ {
/* 后续同向且无门禁:不停表直接 BeginSeg(仅衔接策略,不影响本段波形) */
/* 后续同向且无门禁:不停表直接 BeginSeg */
s_continue_without_stopping = 1U; s_continue_without_stopping = 1U;
} }
else else
@@ -515,17 +483,11 @@ static void PlsrRunControlAfterSegDone(void)
} }


/** /**
* @brief 哪些等待类型禁止 FOLLOW 不停表衔接
* @brief 哪些等待类型禁止后续模式不停表衔接
* @return 1=必须停表等条件;0=允许(含 EXT_OR_DONE 脉冲先完) * @return 1=必须停表等条件;0=允许(含 EXT_OR_DONE 脉冲先完)
*/ */
static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg) static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg)
{ {
/*
* WAIT时间/信号/ACT:发完后停表等条件,禁止后续无缝衔接。
* EXT:发完后仍等 EXT 沿,必须停表。
* EXT_OR_DONE:脉冲先完则条件已满足,允许后续衔接;
* EXT 先到由 CutSegToNext 自行判断 keep。
*/
if ((g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_TIME) || if ((g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_TIME) ||
(g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_SIGNAL) || (g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_SIGNAL) ||
(g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_ACT) || (g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_ACT) ||
@@ -536,31 +498,13 @@ static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg)
return 0U; return 0U;
} }


/**
* 解析本段规划端点:freq_start / freq_end / accel_ms / decel_ms
*
* 【freq_start 四选一,按 if 顺序】
* follow_cont 且 cur_freq 有效 → 无缝衔接(ACT/EXT 切段)
* pwm 仍在跑 → 同频衔接(未停表)
* chain_valid → 上段落地频(完成模式停表后)
* 否则 → ResolveStartHz(冷启动/起跳)
*
* 【freq_end 大分支】
* wait=EXT_OR_DONE 且有下一有效段 → 看同向/反向、FOLLOW/COMPLETE
* 其余(TIME/SIGNAL/ACT/EXT/末段)→ ResolveEndHz(止速/起跳三分支)
*/
/* 解析本段规划端点:freq_start / freq_end / accel_ms / decel_ms */
static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t f_tgt, static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t f_tgt,
PlsrSegPlanEndpoints_t *endpoints) PlsrSegPlanEndpoints_t *endpoints)
{ {
endpoints->accel_ms = g_plsr_config.accel_ms; endpoints->accel_ms = g_plsr_config.accel_ms;
endpoints->decel_ms = g_plsr_config.decel_ms; endpoints->decel_ms = g_plsr_config.decel_ms;


/* ---------- 起始频率 ----------
* 中途切段(ACT/EXT)会置 follow_cont:用打断瞬间的频率衔接下一段。
* ACT 为了对齐脉冲边界可能先停表,此时 pwm_on=0,但仍要跟当前频率,
* 不能当成「段后停表」去走起跳。
* 真正段后停表:follow_cont/chain 已清,从配置起速(0 则起跳)开跑。
*/
if ((s_continue_without_stopping != 0U) && (g_plsr_output_frequency_hz >= 1U)) if ((s_continue_without_stopping != 0U) && (g_plsr_output_frequency_hz >= 1U))
{ {
endpoints->freq_start_hz = g_plsr_output_frequency_hz; endpoints->freq_start_hz = g_plsr_output_frequency_hz;
@@ -578,10 +522,7 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t
} }
else else
{ {
/*
* 停表后冷启动:入口由 ResolveStartHz 按起速/目标/起跳三分支决定
* (起速>目标→起速减速;否则与起跳比较取目标或起跳)。
*/
/* 停表后冷启动 */
endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(g_plsr_config.start_speed, endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(g_plsr_config.start_speed,
f_tgt, f_tgt,
g_plsr_config.default_speed, g_plsr_config.default_speed,
@@ -589,13 +530,7 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t
endpoints->decel_ms); endpoints->decel_ms);
} }


/*
* ---------- 终止频率 ----------
* EXT_OR_DONE:同向/反向只看「下一有效段」(跳过频率0且脉冲0 的占位段)。
* 同向 + 后续 → 过渡到下一有效段目标;同向 + 完成 → 匀速峰值收尾。
* 真末段 / TIME/SIGNAL/ACT/EXT / 换向 → ResolveEndHz(止速/目标/起跳三分支,
* 不再默认往 0 收;止速>目标则用止速,否则按起跳与目标比较)。
*/
/* 终止频率 */
{ {
int16_t next_eff = PlsrRunControlNextEffectiveSeg(seg_idx0); int16_t next_eff = PlsrRunControlNextEffectiveSeg(seg_idx0);


@@ -615,7 +550,7 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t
} }
next_fwd = PlsrRunControlIsForward((uint16_t)next_eff, end_pos); next_fwd = PlsrRunControlIsForward((uint16_t)next_eff, end_pos);


/* 与下一有效段同向 → FOLLOW/COMPLETE 衔接(换向才落到下方止速) */
/* 与下一有效段同向 → 后续/完成衔接(换向才落到下方止速) */
if (next_fwd == s_forward) if (next_fwd == s_forward)
{ {
if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW)
@@ -647,7 +582,7 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t


/** /**
* @brief 段末是否可不停表衔接下一段 * @brief 段末是否可不停表衔接下一段
* @param local_pos 用于判下一段方向的局部坐标(应用「本段结束后」的位置)
* @param local_pos 用于判下一段方向的局部坐标
* @note 须 FOLLOW + 有同向下一段 + 无 BlocksFollowKeep * @note 须 FOLLOW + 有同向下一段 + 无 BlocksFollowKeep
*/ */
static uint8_t PlsrRunControlWillFollowKeepAt(uint16_t cur_seg, int32_t local_pos) static uint8_t PlsrRunControlWillFollowKeepAt(uint16_t cur_seg, int32_t local_pos)
@@ -713,10 +648,10 @@ static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz,
return 1U; return 1U;
} }


/** 运行输出:0=停表;任务上下文 SetFreq / Start */
static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start)
/** 任务侧运行输出:0=停表;任务上下文 SetFreq / Start */
static void PlsrRunControlApplyOutFreq(uint32_t freq, uint8_t do_start)
{ {
if (profile_freq == 0U)
if (freq == 0U)
{ {
PlsrPulseDriverStop(); PlsrPulseDriverStop();
return; return;
@@ -724,30 +659,29 @@ static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start)


if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) if ((do_start != 0U) || (g_plsr_pwm_running == 0U))
{ {
PlsrPulseDriverStart(profile_freq);
PlsrPulseDriverStart(freq);
} }
else if (profile_freq != g_plsr_output_frequency_hz)
else if (freq != g_plsr_output_frequency_hz)
{ {
PlsrPulseDriverSetFreq(profile_freq);
PlsrPulseDriverSetFreq(freq);
} }
} }


/** 脉冲 ISR:只走 ARR 预装载路径 */ /** 脉冲 ISR:只走 ARR 预装载路径 */
static void PlsrRunControlApplyOutFreqIsr(uint32_t profile_freq)
static void PlsrRunControlApplyOutFreqIsr(uint32_t freq)
{ {
if (profile_freq < 1U)
if (freq < 1U)
{ {
profile_freq = 1U;
freq = 1U;
} }
PlsrPulseDriverSetFreqIsr(profile_freq);
PlsrPulseDriverSetFreqIsr(freq);
} }


static uint8_t PlsrRunControlHasDecel(void) static uint8_t PlsrRunControlHasDecel(void)
{ {
/* /*
* 第二相脉冲预算 >0 即需要收尾斜坡。 * 第二相脉冲预算 >0 即需要收尾斜坡。
* 谷底形(起/止速 > 目标):第二相是从目标再加速回止速,f_end > f_tgt,
* 不可再用「f_tgt==f_end 则无减速」这种假定。
* 谷底形(起/止速 > 目标):第二相是从目标再加速回止速,f_end > f_tgt。
*/ */
return (g_plsr_accel_plan.decel_pulses > 0U) ? 1U : 0U; return (g_plsr_accel_plan.decel_pulses > 0U) ? 1U : 0U;
} }
@@ -761,11 +695,7 @@ static uint32_t PlsrRunControlRemainPulses(void)
return 0U; return 0U;
} }


/**
* ARPE:本拍 ISR 写入的影子 ARR 在下一周期生效。
* remain==dec_n 时切入并写入 f(1),…,remain==1 写入 f_end → 末拍为止速。
* 若已晚进(remain < planned),预算用满剩余拍数,避免再减 1 导致末两拍同为止速。
*/
/* remain==dec_n 时进入减速 */
static uint8_t PlsrRunControlShouldEnterDecel(uint32_t remain) static uint8_t PlsrRunControlShouldEnterDecel(uint32_t remain)
{ {
if (PlsrRunControlHasDecel() == 0U) if (PlsrRunControlHasDecel() == 0U)
@@ -801,10 +731,10 @@ static int32_t PlsrRunControlAbsLocalPos(void)
} }


/** /**
* 减速相写出频率:仅 remain==1(供末拍)允许钉止速;
* 减速相写出频率:仅 remain==1允许钉止速;
* 更早写入若已碰到 end,则让出 1Hz,避免末两拍同频。 * 更早写入若已碰到 end,则让出 1Hz,避免末两拍同频。
*/ */
static uint32_t PlsrRunControlDecelOutFreq(uint32_t profile_freq, uint32_t remain)
static uint32_t PlsrRunControlDecelOutFreq(uint32_t freq, uint32_t remain)
{ {
uint32_t end_hz = g_plsr_accel_plan.end_frequency_hz; uint32_t end_hz = g_plsr_accel_plan.end_frequency_hz;


@@ -820,20 +750,20 @@ static uint32_t PlsrRunControlDecelOutFreq(uint32_t profile_freq, uint32_t remai


if (g_plsr_accel_runtime.frequency_rising == 0U) if (g_plsr_accel_runtime.frequency_rising == 0U)
{ {
if ((profile_freq <= end_hz) && (end_hz < 100000U))
if ((freq <= end_hz) && (end_hz < 100000U))
{ {
return end_hz + 1U; return end_hz + 1U;
} }
} }
else if ((profile_freq >= end_hz) && (end_hz > 1U))
else if ((freq >= end_hz) && (end_hz > 1U))
{ {
return end_hz - 1U; return end_hz - 1U;
} }
return profile_freq;
return freq;
} }


/** /**
* @brief 进入减速相:budget 为曲线步进总数(非物理 remain),BeginDec
* @brief 进入减速相:budget 为曲线步进总数,BeginDec
*/ */
static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget) static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget)
{ {
@@ -853,10 +783,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget)
g_plsr_accel_runtime.current_frequency_hz = from_hz; g_plsr_accel_runtime.current_frequency_hz = from_hz;
} }


/**
* 开表纯减速:只 Start(f1)。下一拍起由 ISR NextFrequency 写入 f2…f_end。
* (旧逻辑再预装 f2 会在 ARPE 下让首拍直接吃到 f2,并多步进一次。)
*/
/* 开表纯减速:只 Start(f1)。下一拍起由 ISR NextFrequency 写入 f2…f_end */
static void PlsrRunControlPrimeDecelOnStart(void) static void PlsrRunControlPrimeDecelOnStart(void)
{ {
uint32_t next; uint32_t next;
@@ -865,7 +792,7 @@ static void PlsrRunControlPrimeDecelOnStart(void)
PlsrRunControlApplyOutFreq(next, 1U); PlsrRunControlApplyOutFreq(next, 1U);
} }


/** 离开加速相时触发 0x03 加速诊断(仅当前相为 APPROACH 时有效) */
/** 离开加速相时触发 0x03 加速诊断 */
static void PlsrRunControlOnApproachDone(void) static void PlsrRunControlOnApproachDone(void)
{ {
uint8_t by_budget; uint8_t by_budget;
@@ -874,14 +801,13 @@ static void PlsrRunControlOnApproachDone(void)
{ {
return; return;
} }
/* n 已走到预算 → 按「预算收尾」精确比对;否则按「提前到频」只查超额 */
by_budget = ((g_plsr_accel_plan.accel_pulses > 0U) && (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_plan.accel_pulses)) ? 1U : 0U;
/* n 已走到预算 → 按预算收尾精确比对;否则按提前到频只查超额 */
by_budget = ((g_plsr_accel_plan.accel_pulses > 0U)
&& (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_plan.accel_pulses)) ? 1U : 0U;
PlsrRunControlCheckApproachCurveFault(by_budget); PlsrRunControlCheckApproachCurveFault(by_budget);
} }


/**
* 进匀速:写一次 f_tgt,之后 ISR 不改 ARR(复用缓存)。
*/
/* 进匀速:写一次 f_tgt,之后 ISR 不改 ARR(复用缓存)*/
static void PlsrRunControlEnterConstHold(uint8_t do_start) static void PlsrRunControlEnterConstHold(uint8_t do_start)
{ {
uint32_t f_hold = g_plsr_accel_plan.target_frequency_hz; uint32_t f_hold = g_plsr_accel_plan.target_frequency_hz;
@@ -914,15 +840,7 @@ static void PlsrRunControlEnterConstHoldIsr(void)


/** /**
* 任务侧:按当前段内相 s_phase,决定开表/改频用哪个频率。 * 任务侧:按当前段内相 s_phase,决定开表/改频用哪个频率。
*
* @param do_start 1=需要 Start PWM(RefreshProfile(1));0=已在跑只改频 * @param do_start 1=需要 Start PWM(RefreshProfile(1));0=已在跑只改频
*
* 【分支顺序】
* 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)
*/ */
static void PlsrRunControlRefreshProfile(uint8_t do_start) static void PlsrRunControlRefreshProfile(uint8_t do_start)
{ {
@@ -988,7 +906,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
(next <= g_plsr_accel_plan.target_frequency_hz))) (next <= g_plsr_accel_plan.target_frequency_hz)))
{ {
next = g_plsr_accel_plan.target_frequency_hz; next = g_plsr_accel_plan.target_frequency_hz;
PlsrRunControlOnApproachDone();
PlsrRunControlOnApproachDone();/* 加速结束诊断曲线 */
if (PlsrRunControlShouldEnterDecel(remain) != 0U) if (PlsrRunControlShouldEnterDecel(remain) != 0U)
{ {
PlsrRunControlEnterDecel(next, PlsrRunControlEnterDecel(next,
@@ -1019,13 +937,9 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
uint16_t accel_ms, uint16_t decel_ms) uint16_t accel_ms, uint16_t decel_ms)
{ {
uint32_t f_start; uint32_t f_start;
uint32_t f_lo;
uint32_t f_hi;
uint32_t f_min;
uint32_t f_max;


/*
* 曲线估 acc_n / dec_n,const_n = total − acc − dec。
* f_from/f_end 已由策略层解析为真实频率;配置起止=0 时已换成起跳/落地。
*/
PlsrAccelCurvePlan(&g_plsr_accel_plan, PlsrAccelCurvePlan(&g_plsr_accel_plan,
total, total,
f_from, f_from,
@@ -1036,11 +950,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
decel_ms, decel_ms,
g_plsr_config.accel_mode); g_plsr_config.accel_mode);


/*
* 0x02 频率可达性诊断
* AccelCurvePlan 后:若脉冲不够爬升/下降到设定目标,规划会压低/抬高实际峰值
* (典型为三角波)。此时写故障码 2,仍按压后的规划继续运行。
*/
/* 0x02 频率是否与设定一致诊断/\或\/ */
if ((f_tgt > f_from) && (g_plsr_accel_plan.target_frequency_hz < f_tgt)) if ((f_tgt > f_from) && (g_plsr_accel_plan.target_frequency_hz < f_tgt))
{ {
PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH);
@@ -1058,77 +968,65 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
/* 运行层使用规划后的实际起始频率。 */ /* 运行层使用规划后的实际起始频率。 */
f_start = g_plsr_accel_plan.start_frequency_hz; f_start = g_plsr_accel_plan.start_frequency_hz;


/*
* 脉冲域:ISR 用 PlsrAccelNextFrequency(直线现场算 / S·正弦查预建表)。
* FreqAtPulse 仅任务侧 RefreshProfile 对齐用,勿再做时间轴二次拟合。
*/

/* 脉冲域:ISR 用 PlsrAccelNextFrequency(直线现场算 / S·正弦查预建表)*/
/* 本段频率范围锁 PSC,升降只改 ARR;跨度过大则 LockPscRange 自动不锁 */ /* 本段频率范围锁 PSC,升降只改 ARR;跨度过大则 LockPscRange 自动不锁 */
f_lo = g_plsr_accel_plan.target_frequency_hz;
f_hi = g_plsr_accel_plan.target_frequency_hz;
if (f_start > f_hi)
/* 本段可能出现的最低/最高频率,取 入口、目标、出口 三者的 min/max */
f_min = g_plsr_accel_plan.target_frequency_hz;
f_max = g_plsr_accel_plan.target_frequency_hz;
if (f_start > f_max)
{ {
f_hi = f_start;
f_max = f_start;
} }
if (f_start >= 1U) if (f_start >= 1U)
{ {
if (f_start < f_lo)
if (f_start < f_min)
{ {
f_lo = f_start;
f_min = f_start;
} }
} }
else else
{ {
f_lo = 1U;
f_min = 1U;
} }
if (g_plsr_accel_plan.end_frequency_hz > f_hi)
if (g_plsr_accel_plan.end_frequency_hz > f_max)
{ {
f_hi = g_plsr_accel_plan.end_frequency_hz;
f_max = g_plsr_accel_plan.end_frequency_hz;
} }
if (g_plsr_accel_plan.end_frequency_hz >= 1U) if (g_plsr_accel_plan.end_frequency_hz >= 1U)
{ {
if (g_plsr_accel_plan.end_frequency_hz < f_lo)
if (g_plsr_accel_plan.end_frequency_hz < f_min)
{ {
f_lo = g_plsr_accel_plan.end_frequency_hz;
f_min = g_plsr_accel_plan.end_frequency_hz;
} }
} }
else else
{ {
if (f_lo > 1U)
if (f_min > 1U)
{ {
f_lo = 1U;
f_min = 1U;
} }
} }
if (f_lo < 1U)
if (f_min < 1U)
{ {
f_lo = 1U;
f_min = 1U;
} }
if (f_hi < f_lo)
if (f_max < f_min)
{ {
f_hi = f_lo;
f_max = f_min;
} }
/*
* 后续同向不停表时 PWM 仍在跑:禁止重锁 PSC。
*/
/* 后续同向不停表时 PWM 仍在跑:禁止重锁 PSC */
if (g_plsr_pwm_running == 0U) if (g_plsr_pwm_running == 0U)
{ {
PlsrPulseDriverLockPscRange(f_lo, f_hi);
PlsrPulseDriverLockPscRange(f_min, f_max);
} }


/*
* 保持 AccelCurvePlan 算出的脉冲预算;运行时逐拍计算下一频率。
*/
/*
* 进入段内初始相(决定 RefreshProfile / ISR 从哪开始):
* 纯减段(dec_n>=total 且已在高位)→ 直接 PH_DECEL
* 入口=目标 → PH_CONST(无加速)
* 否则 → PH_APPROACH
*/
/* 保持 AccelCurvePlan 算出的脉冲预算;运行时逐拍计算下一频率 */
if ((g_plsr_accel_plan.decel_pulses >= total) && if ((g_plsr_accel_plan.decel_pulses >= total) &&
(total > 0U) && (total > 0U) &&
(g_plsr_accel_plan.end_frequency_hz != g_plsr_accel_plan.target_frequency_hz) && (g_plsr_accel_plan.end_frequency_hz != g_plsr_accel_plan.target_frequency_hz) &&
(f_start >= g_plsr_accel_plan.target_frequency_hz)) (f_start >= g_plsr_accel_plan.target_frequency_hz))
{ {
/*纯减开段,脉冲数不足以减速到目标频率 */
PlsrRunControlEnterDecel(f_start, g_plsr_accel_plan.decel_pulses); PlsrRunControlEnterDecel(f_start, g_plsr_accel_plan.decel_pulses);
} }
else if (f_start == g_plsr_accel_plan.target_frequency_hz) else if (f_start == g_plsr_accel_plan.target_frequency_hz)
@@ -1165,14 +1063,7 @@ static void PlsrRunControlFinishAll(void)
s_waiting_for_last_period = 0U; s_waiting_for_last_period = 0U;
} }


/**
* @brief 开一段:算位移/方向 → PlanSeg → 选「无缝 / 同向直开 / 换向延时」
*
* 【开表三分支 — BeginSeg 末尾】
* follow_cont 上段 FOLLOW/ACT 衔接:不停表,RefreshProfile(0)
* 同向且 dir 已对 跳过 DIR_WAIT,RefreshProfile(1) 开表
* 否则 设方向脚;dir_delay=0 立刻开,>0 进 RC_DIR_WAIT+Prepare
*/
/* 开一段:算位移/方向 → PlanSeg → 选无缝/同向直开/换向延时 */
static void PlsrRunControlBeginSeg(uint16_t seg_idx0) static void PlsrRunControlBeginSeg(uint16_t seg_idx0)
{ {
int32_t cnt; int32_t cnt;
@@ -1232,14 +1123,11 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx0)
s_forward = 0U; s_forward = 0U;
s_segment_pulse_target = -move; s_segment_pulse_target = -move;
} }

/* 0x04:本段(或默认速度)频率非法 */
if (PlsrRunControlGetSegTargetFreq(g_plsr_segments[seg_idx0].freq_hz, if (PlsrRunControlGetSegTargetFreq(g_plsr_segments[seg_idx0].freq_hz,
g_plsr_config.default_speed, &f_tgt) == 0U) g_plsr_config.default_speed, &f_tgt) == 0U)
{ {
/*
* 0x04:本段(或默认速度)频率非法。
* 禁止启动该段:清忙并结束整次运行,不 AfterSegDone 以免误切下一段。
*/
PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL);
PlsrRunControlFinishAll(); PlsrRunControlFinishAll();
return; return;
@@ -1277,7 +1165,7 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx0)


if (s_continue_without_stopping != 0U) if (s_continue_without_stopping != 0U)
{ {
/* 分支 A:段间无缝(FOLLOW 或 ACT handoff) */
/* 分支 A:段间无缝(后续模式或 ACT切段) */
PlsrPulseDriverClearOnePulseStop(); PlsrPulseDriverClearOnePulseStop();
/* /*
* 必须先 RC_RUN 再 RefreshProfile/Start:否则冷启动首拍 UPDATE 时 * 必须先 RC_RUN 再 RefreshProfile/Start:否则冷启动首拍 UPDATE 时
@@ -1326,7 +1214,7 @@ static void PlsrRunControlBeginSeg(uint16_t seg_idx0)
} }
} }


/** @brief 运行控制初始化(见 plsr_run_control.h) */
/** @brief 运行控制初始化 */
void PlsrRunControlInit(void) void PlsrRunControlInit(void)
{ {
s_state = RC_IDLE; s_state = RC_IDLE;
@@ -1365,7 +1253,7 @@ static void PlsrRunControlWakeDrain(void)
{ {
while (OSSemAccept(s_wake_sem) > 0U) while (OSSemAccept(s_wake_sem) > 0U)
{ {
/* drain */
/* 全部丢弃令牌 */
} }
} }
} }
@@ -1379,7 +1267,7 @@ static void PlsrRunControlWakePost(void)
} }


/** /**
* @brief 创建任务唤醒信号量(见 plsr_run_control.h)
* @brief 创建任务唤醒信号量
*/ */
void PlsrRunControlWakeInit(void) void PlsrRunControlWakeInit(void)
{ {
@@ -1390,7 +1278,7 @@ void PlsrRunControlWakeInit(void)
} }


/** /**
* @brief PlsrTask 休眠(见 plsr_run_control.h)
* @brief PlsrTask 休眠
*/ */
void PlsrRunControlWakePend(uint16_t timeout_ticks) void PlsrRunControlWakePend(uint16_t timeout_ticks)
{ {
@@ -1410,7 +1298,7 @@ void PlsrRunControlWakePend(uint16_t timeout_ticks)
} }


/** /**
* @brief 是否在 TIM5 精确等待(见 plsr_run_control.h)
* @brief 是否在 TIM5 精确等待
*/ */
uint8_t PlsrRunControlIsPreciseWait(void) uint8_t PlsrRunControlIsPreciseWait(void)
{ {
@@ -1425,7 +1313,7 @@ uint8_t PlsrRunControlIsPreciseWait(void)
return 0U; return 0U;
} }


/** @brief 启动多段序列(见 plsr_run_control.h) */
/** @brief 启动多段序列 */
uint8_t PlsrStart(uint16_t start_seg_1based) uint8_t PlsrStart(uint16_t start_seg_1based)
{ {
uint16_t n; uint16_t n;
@@ -1473,7 +1361,7 @@ uint8_t PlsrStart(uint16_t start_seg_1based)
return 1U; return 1U;
} }


/** @brief 急停(见 plsr_run_control.h) */
/** @brief 急停 */
void PlsrStop(void) void PlsrStop(void)
{ {
PlsrSignalIoCancelDelay(); PlsrSignalIoCancelDelay();
@@ -1493,7 +1381,7 @@ void PlsrStop(void)
s_waiting_for_last_period = 0U; s_waiting_for_last_period = 0U;
} }


/** @brief 运行中改频(见 plsr_run_control.h) */
/** @brief 运行中改频 */
uint8_t PlsrChangeFreq(uint32_t new_tgt_hz) uint8_t PlsrChangeFreq(uint32_t new_tgt_hz)
{ {
uint32_t remain; uint32_t remain;
@@ -1524,7 +1412,7 @@ uint8_t PlsrChangeFreq(uint32_t new_tgt_hz)
return 1U; return 1U;
} }


/** @brief 毫秒任务节拍(见 plsr_run_control.h) */
/** @brief 毫秒任务节拍 */
void PlsrRunControlTickMs(void) void PlsrRunControlTickMs(void)
{ {
if (s_act_expired != 0U) if (s_act_expired != 0U)
@@ -1621,30 +1509,22 @@ void PlsrRunControlOnDelayTimer(void)
} }
} }


/** @brief 脉冲 UPDATE ISR — 本文件最核心、if 最多的函数 */
/** @brief 脉冲 UPDATE ISR */
void PlsrOnPulseIsr(void) void PlsrOnPulseIsr(void)
{ {
uint32_t remain; uint32_t remain;
uint32_t next; uint32_t next;


/* 只有 RC_RUN 才计数、改频;WAIT/IDLE 里进 ISR 是空操作 */
if (s_state != RC_RUN) if (s_state != RC_RUN)
{ {
return; return;
} }


/*
* 末拍整周期已跑完:本 UPDATE 由 OPM 停表产生,不再计入脉冲。
* 先于 s_segment_pulses_done++,保证个数 = target 且示波器末拍完整。
*/
if (s_waiting_for_last_period != 0U) if (s_waiting_for_last_period != 0U)
{ {
s_waiting_for_last_period = 0U; s_waiting_for_last_period = 0U;
s_act_waiting_for_pulse_boundary = 0U; s_act_waiting_for_pulse_boundary = 0U;
/*
* 末拍下降沿已在 CC1 中断拉低;本 UPDATE 为 OPM 周期结束,不再 ++。
* 再 HoldOutputLow 完整停表,交 AfterSegDone。
*/
PlsrPulseDriverHoldOutputLow(); PlsrPulseDriverHoldOutputLow();
PlsrRunControlAfterSegDone(); PlsrRunControlAfterSegDone();
return; return;
@@ -1660,20 +1540,11 @@ void PlsrOnPulseIsr(void)
g_plsr_accumulated_pulses--; g_plsr_accumulated_pulses--;
} }


/*
* ACT 到期切段只在脉冲边界执行,避免半个 PWM 周期改段。
* 到点由 TIM5 置 cut_pending 并 OPM;此处收尾切段。
* 加减速下周期长短不定,不按脉冲个数预测,墙钟可能多出「当前这一拍」的剩余。
*/
if (s_act_waiting_for_pulse_boundary != 0U) if (s_act_waiting_for_pulse_boundary != 0U)
{ {
s_act_waiting_for_pulse_boundary = 0U; s_act_waiting_for_pulse_boundary = 0U;
s_keep_frequency_after_act = 1U; s_keep_frequency_after_act = 1U;
/*
* TIM5 已 ArmActStop:输出在本拍下降沿后保持低,UPDATE 后计数器会停。
* 必须清 g_plsr_pwm_running,否则 ActExpire 同向衔接走 RefreshProfile(0)
* 时误判「表仍在跑」而不 Start,卡死在下一段入口。
*/

PlsrPulseDriverHoldOutputLow(); PlsrPulseDriverHoldOutputLow();
s_state = RC_IDLE; s_state = RC_IDLE;
s_act_expired = 1U; s_act_expired = 1U;
@@ -1685,11 +1556,6 @@ void PlsrOnPulseIsr(void)
{ {
if (s_stop_after_last_pulse != 0U) if (s_stop_after_last_pulse != 0U)
{ {
/*
* 段末停表模式:本 UPDATE 是「最后一拍」的上升沿(s_segment_pulses_done 已等于 target)。
* 武装 OPM+CC1:下降沿拉低,下一 UPDATE 进 s_waiting_for_last_period 再 AfterSegDone。
* FOLLOW-keep 时 s_stop_after_last_pulse=0,走 else 立刻切段不停表。
*/
PlsrPulseDriverArmSegEndStop(); PlsrPulseDriverArmSegEndStop();
s_waiting_for_last_period = 1U; s_waiting_for_last_period = 1U;
return; return;
@@ -1701,16 +1567,9 @@ void PlsrOnPulseIsr(void)


remain = PlsrRunControlRemainPulses(); remain = PlsrRunControlRemainPulses();


/*
* --- 段内三相:按 s_phase 改频(匀速几乎空转)---
* remain = 还剩几拍(含当前正在进行的这一拍)
*/
if (s_phase == PH_CONST) if (s_phase == PH_CONST)
{ {
/*
* 匀速:不改频、不写 ARR(热路径几乎空转)。
* 仅在 remain 进入减速窗口时切相并查表改频。
*/
/* 匀速:不改频、不写 ARR,仅在 remain 进入减速窗口时切相并查表改频 */
PlsrPulseDriverClearStartPeriodProtect(); PlsrPulseDriverClearStartPeriodProtect();
if (PlsrRunControlShouldEnterDecel(remain) != 0U) if (PlsrRunControlShouldEnterDecel(remain) != 0U)
{ {
@@ -1724,7 +1583,7 @@ void PlsrOnPulseIsr(void)
} }
else if (s_phase == PH_APPROACH) else if (s_phase == PH_APPROACH)
{ {
/* 每拍 Step 得下一频;若加速相结束则进 CONST 或 DECEL(条件同 RefreshProfile) */
/* 每拍 Step 得下一频;若加速相结束则进 CONST 或 DECEL */
next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); next = PlsrAccelNextFrequency(&g_plsr_accel_runtime);


if ((g_plsr_accel_plan.accel_pulses == 0U) || if ((g_plsr_accel_plan.accel_pulses == 0U) ||
@@ -1758,10 +1617,6 @@ void PlsrOnPulseIsr(void)
} }
else /* PH_DECEL */ else /* PH_DECEL */
{ {
/*
* ARPE:remain==k 写入 → 下一拍使用。
* remain==1 必须写出 f_end(末拍);remain>1 禁止提前钉止速。
*/
if ((remain <= 1U) && if ((remain <= 1U) &&
(g_plsr_accel_runtime.total_pulses > 0U) && (g_plsr_accel_runtime.total_pulses > 0U) &&
(g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_runtime.total_pulses)) (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_runtime.total_pulses))
@@ -1778,7 +1633,7 @@ void PlsrOnPulseIsr(void)
} }
} }


/** @brief 清累计与绝对原点(见 plsr_run_control.h) */
/** @brief 清累计与绝对原点 */
void PlsrClearAccPulse(void) void PlsrClearAccPulse(void)
{ {
g_plsr_accumulated_pulses = 0; g_plsr_accumulated_pulses = 0;
@@ -1786,7 +1641,7 @@ void PlsrClearAccPulse(void)
s_absolute_origin_locked = 0U; /* 下次 Start 重新锁定原点 */ s_absolute_origin_locked = 0U; /* 下次 Start 重新锁定原点 */
} }


/** @brief 上电恢复累计(见 plsr_run_control.h) */
/** @brief 上电恢复累计 */
void PlsrRunControlRestoreAccPulse(int32_t acc_pulse) void PlsrRunControlRestoreAccPulse(int32_t acc_pulse)
{ {
g_plsr_accumulated_pulses = acc_pulse; g_plsr_accumulated_pulses = acc_pulse;


Laden…
Annuleren
Opslaan