|
|
|
@@ -7,6 +7,11 @@ |
|
|
|
* - 升降相按时间走完,不按脉冲数切相 |
|
|
|
* - 剩余脉冲 <= 预留减速脉冲时进入减速;非末段不预留减速 |
|
|
|
* - 末段减到终止速度后停机(落到 0) |
|
|
|
* |
|
|
|
* 【硬约束】脉冲 UPDATE ISR(PlsrRunControlOnPulseIsr)内禁止重计算: |
|
|
|
* - 禁止 FitRampTimeMs / EstimatePulses 等 O(N) 或查表迭代 |
|
|
|
* - 禁止 S 曲线、正弦等将来更重的轮廓递推/拟合 |
|
|
|
* - ISR 只做:计数、O(1) 插值取频、改预装载 ARR;耗时规划放在 BeginSeg/任务上下文 |
|
|
|
*/ |
|
|
|
#include "plsr_run_control.h" |
|
|
|
#include "plsr_path_plan.h" |
|
|
|
@@ -35,6 +40,7 @@ static volatile uint8_t s_busy; |
|
|
|
static volatile uint8_t s_forward; |
|
|
|
static volatile uint16_t s_cur_seg; |
|
|
|
static volatile uint32_t s_cur_freq; |
|
|
|
static volatile uint32_t s_pending_freq; /* ARPE 影子频率,下一 UPDATE 才成为当前拍 */ |
|
|
|
static volatile int32_t s_done; |
|
|
|
static volatile int32_t s_target; |
|
|
|
static volatile int32_t s_acc_pulse; |
|
|
|
@@ -115,15 +121,15 @@ static uint32_t PlsrRunControlClampSpeed(uint32_t spd) |
|
|
|
static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) |
|
|
|
{ |
|
|
|
/* |
|
|
|
* 完全按轮廓频率输出:0 就是停。 |
|
|
|
* 运行中改频只挂起,等脉冲周期边界再落到硬件,避免中途改 ARR 出毛刺。 |
|
|
|
* 运行中只写预装载(下一拍生效),避免直接改 ARR 出毛刺多计脉冲。 |
|
|
|
* s_cur_freq = 当前正在跑的拍;s_pending_freq = 已写入影子、下一拍的频率。 |
|
|
|
*/ |
|
|
|
s_cur_freq = profile_freq; |
|
|
|
|
|
|
|
if (profile_freq == 0U) |
|
|
|
{ |
|
|
|
PlsrPulseDriverStop(); |
|
|
|
s_pwm_on = 0U; |
|
|
|
s_cur_freq = 0U; |
|
|
|
s_pending_freq = 0U; |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
@@ -131,9 +137,12 @@ static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) |
|
|
|
{ |
|
|
|
PlsrPulseDriverStart(profile_freq); |
|
|
|
s_pwm_on = 1U; |
|
|
|
s_cur_freq = profile_freq; |
|
|
|
s_pending_freq = profile_freq; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
s_pending_freq = profile_freq; |
|
|
|
PlsrPulseDriverRequestFreq(profile_freq); |
|
|
|
} |
|
|
|
} |
|
|
|
@@ -174,6 +183,100 @@ static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq) |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses) |
|
|
|
{ |
|
|
|
PlsrCfg_t *cfg = PlsrParamGetCfg(); |
|
|
|
uint32_t t_slope = 0U; |
|
|
|
uint32_t t_fit; |
|
|
|
uint32_t sum_f; |
|
|
|
uint32_t df; |
|
|
|
uint32_t den; |
|
|
|
uint32_t ref_ms; |
|
|
|
uint32_t f_end; |
|
|
|
|
|
|
|
PlsrRunControlEnterPhase(PH_DECEL, from_hz); |
|
|
|
f_end = s_accel_plan.f_end; |
|
|
|
|
|
|
|
if (remain_pulses == 0U) |
|
|
|
{ |
|
|
|
s_accel_plan.dec_n = 0U; |
|
|
|
s_accel_plan.t_dec_ms = 0U; |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
s_accel_plan.dec_n = remain_pulses; |
|
|
|
|
|
|
|
if (from_hz == f_end) |
|
|
|
{ |
|
|
|
s_accel_plan.t_dec_ms = 0U; |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
/* 参数斜率决定的理想减速时间 */ |
|
|
|
df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz); |
|
|
|
if (f_end > from_hz) |
|
|
|
{ |
|
|
|
ref_ms = cfg->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 = cfg->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); |
|
|
|
} |
|
|
|
if (den == 0U) |
|
|
|
{ |
|
|
|
den = df; |
|
|
|
} |
|
|
|
if ((ref_ms > 0U) && (den > 0U)) |
|
|
|
{ |
|
|
|
t_slope = (df * ref_ms + den - 1UL) / den; |
|
|
|
if (t_slope < 1U) |
|
|
|
{ |
|
|
|
t_slope = 1U; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
/* |
|
|
|
* 注意:本函数会在脉冲 UPDATE ISR 里调用,禁止再跑 FitRampTimeMs/ |
|
|
|
* EstimatePulses 那种 O(N) 递推。段脉冲多时会卡死 ISR,定时器继续 |
|
|
|
* 溢出 → 示波器上最后一段多出若干脉冲(500 准、5000 多 6~7 个)。 |
|
|
|
* 这里只用 O(1) 平均公式拟合 T。 |
|
|
|
*/ |
|
|
|
sum_f = from_hz + f_end; |
|
|
|
if (sum_f == 0U) |
|
|
|
{ |
|
|
|
sum_f = 1U; |
|
|
|
} |
|
|
|
if (remain_pulses > 1U) |
|
|
|
{ |
|
|
|
t_fit = (2UL * (remain_pulses - 1UL) * 1000UL + sum_f - 1UL) / sum_f; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
t_fit = 1U; |
|
|
|
} |
|
|
|
if (t_fit < 1U) |
|
|
|
{ |
|
|
|
t_fit = 1U; |
|
|
|
} |
|
|
|
|
|
|
|
if ((t_slope > 0U) && (t_slope <= t_fit)) |
|
|
|
{ |
|
|
|
s_accel_plan.t_dec_ms = t_slope; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
s_accel_plan.t_dec_ms = t_fit; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
static void PlsrRunControlRefreshProfile(uint8_t do_start) |
|
|
|
{ |
|
|
|
uint32_t next; |
|
|
|
@@ -199,7 +302,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) |
|
|
|
*/ |
|
|
|
if ((s_phase == PH_CONST) && (p->dec_n > 0U) && (remain <= p->dec_n)) |
|
|
|
{ |
|
|
|
PlsrRunControlEnterPhase(PH_DECEL, s_cur_freq); |
|
|
|
PlsrRunControlEnterDecel(s_cur_freq, remain); |
|
|
|
elapsed_us = 0U; |
|
|
|
} |
|
|
|
|
|
|
|
@@ -217,7 +320,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) |
|
|
|
PlsrRunControlEnterPhase(PH_CONST, next); |
|
|
|
if ((p->dec_n > 0U) && (remain <= p->dec_n)) |
|
|
|
{ |
|
|
|
PlsrRunControlEnterPhase(PH_DECEL, next); |
|
|
|
PlsrRunControlEnterDecel(next, remain); |
|
|
|
elapsed_us = 0U; |
|
|
|
T_us = p->t_dec_ms * 1000UL; |
|
|
|
next = PlsrRunControlLerpUs(s_decel_from, p->f_end, |
|
|
|
@@ -226,22 +329,41 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
/* 时间线性:f = f0 + (f1-f0)*t/T,按 µs 连续变化,不是 10ms 台阶 */ |
|
|
|
uint32_t t_cmd = elapsed_us; |
|
|
|
if ((s_pwm_on != 0U) && (s_cur_freq >= 1U)) |
|
|
|
{ |
|
|
|
t_cmd = elapsed_us + |
|
|
|
((1000000UL + (s_cur_freq / 2UL)) / s_cur_freq); |
|
|
|
} |
|
|
|
next = PlsrRunControlLerpUs(s_approach_from, p->f_tgt, |
|
|
|
elapsed_us, T_us); |
|
|
|
t_cmd, T_us); |
|
|
|
} |
|
|
|
} |
|
|
|
else if (s_phase == PH_DECEL) |
|
|
|
{ |
|
|
|
T_us = p->t_dec_ms * 1000UL; |
|
|
|
if (T_us == 0U) |
|
|
|
/* |
|
|
|
* ARPE 超前一拍:这里算出的 next 是“再下一拍”的预装载。 |
|
|
|
* remain==2 时预装载最后一拍 → 必须为 f_end;remain==1 最后一拍已在跑。 |
|
|
|
*/ |
|
|
|
if ((remain <= 1U) || (T_us == 0U)) |
|
|
|
{ |
|
|
|
next = p->f_end; |
|
|
|
} |
|
|
|
else if (remain == 2U) |
|
|
|
{ |
|
|
|
next = p->f_end; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
next = PlsrRunControlLerpUs(s_decel_from, p->f_end, |
|
|
|
elapsed_us, T_us); |
|
|
|
uint32_t t_cmd = elapsed_us; |
|
|
|
uint32_t per; |
|
|
|
if (s_cur_freq >= 1U) |
|
|
|
{ |
|
|
|
per = (1000000UL + (s_cur_freq / 2UL)) / s_cur_freq; |
|
|
|
t_cmd = elapsed_us + per; |
|
|
|
} |
|
|
|
next = PlsrRunControlLerpUs(s_decel_from, p->f_end, t_cmd, T_us); |
|
|
|
} |
|
|
|
} |
|
|
|
else |
|
|
|
@@ -249,7 +371,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) |
|
|
|
next = p->f_tgt; |
|
|
|
if ((p->dec_n > 0U) && (remain <= p->dec_n)) |
|
|
|
{ |
|
|
|
PlsrRunControlEnterPhase(PH_DECEL, next); |
|
|
|
PlsrRunControlEnterDecel(next, remain); |
|
|
|
elapsed_us = 0U; |
|
|
|
T_us = p->t_dec_ms * 1000UL; |
|
|
|
next = PlsrRunControlLerpUs(s_decel_from, p->f_end, |
|
|
|
@@ -257,7 +379,7 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start) |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
if ((do_start != 0U) || (next != s_cur_freq)) |
|
|
|
if ((do_start != 0U) || (next != s_pending_freq) || (s_pwm_on == 0U)) |
|
|
|
{ |
|
|
|
PlsrRunControlApplyOutFreq(next, do_start); |
|
|
|
} |
|
|
|
@@ -324,6 +446,18 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
/* |
|
|
|
* 加速时间也按 acc_n 拟合,避免 T 偏短导致前端几拍“又陡又弯”、 |
|
|
|
* 后半段早就贴在目标频上。 |
|
|
|
*/ |
|
|
|
if (s_accel_plan.acc_n > 0U) |
|
|
|
{ |
|
|
|
s_accel_plan.t_acc_ms = |
|
|
|
PlsrAccelCurveFitRampTimeMs(f_from, |
|
|
|
s_accel_plan.f_tgt, |
|
|
|
s_accel_plan.acc_n, |
|
|
|
s_accel_plan.t_acc_ms); |
|
|
|
} |
|
|
|
if (s_accel_plan.t_acc_ms < 1U) |
|
|
|
{ |
|
|
|
s_accel_plan.t_acc_ms = 1U; |
|
|
|
@@ -340,6 +474,7 @@ static void PlsrRunControlFinishAll(void) |
|
|
|
s_busy = 0U; |
|
|
|
s_state = RC_IDLE; |
|
|
|
s_cur_freq = 0U; |
|
|
|
s_pending_freq = 0U; |
|
|
|
s_pwm_on = 0U; |
|
|
|
s_follow_cont = 0U; |
|
|
|
s_chain_valid = 0U; |
|
|
|
@@ -521,7 +656,10 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0) |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
/* 完成:非末段停在本段目标;末段收到终止速度 */ |
|
|
|
/* |
|
|
|
* 完成:非末段 f_end=本段目标 → 无减速相;脉冲不够时沿加速度斜率 |
|
|
|
* 一直加到脉冲用尽,下一段从链频继续加。仅末段收到终止速度并减速。 |
|
|
|
*/ |
|
|
|
f_end = (is_last != 0U) ? |
|
|
|
PlsrRunControlClampSpeed(cfg->end_speed) : f_tgt; |
|
|
|
} |
|
|
|
@@ -559,6 +697,7 @@ void PlsrRunControlInit(void) |
|
|
|
s_forward = 1U; |
|
|
|
s_cur_seg = 0U; |
|
|
|
s_cur_freq = 0U; |
|
|
|
s_pending_freq = 0U; |
|
|
|
s_pwm_on = 0U; |
|
|
|
s_done = 0; |
|
|
|
s_target = 0; |
|
|
|
@@ -610,6 +749,7 @@ void PlsrRunControlStop(void) |
|
|
|
s_busy = 0U; |
|
|
|
s_state = RC_IDLE; |
|
|
|
s_cur_freq = 0U; |
|
|
|
s_pending_freq = 0U; |
|
|
|
s_pwm_on = 0U; |
|
|
|
s_follow_cont = 0U; |
|
|
|
s_chain_valid = 0U; |
|
|
|
@@ -668,6 +808,26 @@ void PlsrRunControlTickMs(void) |
|
|
|
} |
|
|
|
/* 第一个脉冲 = 脉冲起始速度(不是目标频、不是 1Hz) */ |
|
|
|
PlsrRunControlApplyOutFreq(s_approach_from, 1U); |
|
|
|
/* |
|
|
|
* ARPE:立刻预装载第 2 拍频率,否则第 2 拍仍是起速(等于多锁一拍低频)。 |
|
|
|
*/ |
|
|
|
if ((s_phase == PH_APPROACH) && (s_approach_from >= 1U) && |
|
|
|
(s_approach_from != s_accel_plan.f_tgt)) |
|
|
|
{ |
|
|
|
uint32_t T_us = s_accel_plan.t_acc_ms * 1000UL; |
|
|
|
uint32_t per = (1000000UL + (s_approach_from / 2UL)) / |
|
|
|
s_approach_from; |
|
|
|
uint32_t f2 = PlsrRunControlLerpUs(s_approach_from, |
|
|
|
s_accel_plan.f_tgt, |
|
|
|
per, |
|
|
|
T_us); |
|
|
|
if (f2 < 1U) |
|
|
|
{ |
|
|
|
f2 = 1U; |
|
|
|
} |
|
|
|
PlsrRunControlApplyOutFreq(f2, 0U); |
|
|
|
PlsrPulseDriverApplyPending(); |
|
|
|
} |
|
|
|
} |
|
|
|
return; |
|
|
|
} |
|
|
|
@@ -711,24 +871,11 @@ void PlsrRunControlOnPulseIsr(void) |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
/* 上一周期结束:先落到挂起频率 */ |
|
|
|
PlsrPulseDriverApplyPending(); |
|
|
|
|
|
|
|
f_prev = s_cur_freq; |
|
|
|
|
|
|
|
/* |
|
|
|
* 加/减速:按本脉冲周期推进相时间(µs),再算下一个频率。 |
|
|
|
* 这样 f-t 是连续直线,而不是 OS 10ms 台阶。 |
|
|
|
* UPDATE:上一拍结束;预装载已装入,新拍刚开始。 |
|
|
|
* f_prev = 刚结束那一拍;随后 s_cur_freq 切到本拍(上一拍写入的 pending)。 |
|
|
|
*/ |
|
|
|
if ((s_phase == PH_APPROACH) || (s_phase == PH_DECEL)) |
|
|
|
{ |
|
|
|
if (f_prev >= 1U) |
|
|
|
{ |
|
|
|
s_phase_elapsed_us += (1000000UL + (f_prev / 2UL)) / f_prev; |
|
|
|
} |
|
|
|
PlsrRunControlRefreshProfile(0U); |
|
|
|
PlsrPulseDriverApplyPending(); |
|
|
|
} |
|
|
|
f_prev = s_cur_freq; |
|
|
|
|
|
|
|
s_done++; |
|
|
|
if (s_forward != 0U) |
|
|
|
@@ -746,7 +893,24 @@ void PlsrRunControlOnPulseIsr(void) |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
if (s_phase == PH_CONST) |
|
|
|
s_cur_freq = s_pending_freq; |
|
|
|
|
|
|
|
/* 最后一拍:下一 UPDATE 停表,避免再多出一个上升沿 */ |
|
|
|
if (s_done == (s_target - 1)) |
|
|
|
{ |
|
|
|
PlsrPulseDriverArmOnePulseStop(); |
|
|
|
} |
|
|
|
|
|
|
|
if ((s_phase == PH_APPROACH) || (s_phase == PH_DECEL)) |
|
|
|
{ |
|
|
|
if (f_prev >= 1U) |
|
|
|
{ |
|
|
|
s_phase_elapsed_us += (1000000UL + (f_prev / 2UL)) / f_prev; |
|
|
|
} |
|
|
|
PlsrRunControlRefreshProfile(0U); |
|
|
|
PlsrPulseDriverApplyPending(); |
|
|
|
} |
|
|
|
else if (s_phase == PH_CONST) |
|
|
|
{ |
|
|
|
remain = (uint32_t)(s_target - s_done); |
|
|
|
if ((s_accel_plan.dec_n > 0U) && (remain <= s_accel_plan.dec_n)) |
|
|
|
|