|
|
|
@@ -394,9 +394,10 @@ uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* 斜坡脉冲数:与 1ms 时间轴上的面积一致。 |
|
|
|
* 直线:N = (f0+f1) * T_ms / 2000(平均频率 × 时间)。 |
|
|
|
* S/正弦:按 1ms 形状采样累加。禁止按脉冲周期递推(那是另一套模型)。 |
|
|
|
* 斜坡脉冲数:与运行时 TIM3 1ms 取频一致(t=0..T-1 每毫秒贡献 f 个脉冲)。 |
|
|
|
* 向上取整:减速段多留脉冲,才能在发完前走到 f_end。 |
|
|
|
* 梯形面积 (f0+f1)*T/2000 会少算末段减速(约 (f_peak-f_end)/2000 个), |
|
|
|
* 匀速被拉长,最后一段脉冲在斜坡中途用完,到不了止速。 |
|
|
|
*/ |
|
|
|
static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, |
|
|
|
uint32_t f_to, |
|
|
|
@@ -406,68 +407,54 @@ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from, |
|
|
|
uint32_t t; |
|
|
|
uint32_t f; |
|
|
|
uint32_t n; |
|
|
|
uint64_t acc; |
|
|
|
uint32_t t_use; |
|
|
|
uint64_t acc; |
|
|
|
|
|
|
|
if ((t_ms == 0U) || (f_from == f_to)) |
|
|
|
{ |
|
|
|
return 0U; |
|
|
|
} |
|
|
|
|
|
|
|
if (mode == PLSR_ACCEL_LINEAR) |
|
|
|
{ |
|
|
|
acc = ((uint64_t)f_from + (uint64_t)f_to) * (uint64_t)t_ms; |
|
|
|
n = (uint32_t)(acc / 2000ULL); |
|
|
|
if ((acc % 2000ULL) >= 1000ULL) |
|
|
|
{ |
|
|
|
n++; |
|
|
|
} |
|
|
|
if (n == 0U) |
|
|
|
{ |
|
|
|
n = 1U; |
|
|
|
} |
|
|
|
return n; |
|
|
|
} |
|
|
|
|
|
|
|
/* S/正弦:最多按 120s 采样,避免规划阶段死循环 */ |
|
|
|
t_use = t_ms; |
|
|
|
if (t_use > 120000U) |
|
|
|
{ |
|
|
|
t_use = 120000U; |
|
|
|
} |
|
|
|
|
|
|
|
acc = 0ULL; |
|
|
|
for (t = 0U; t < t_use; t++) |
|
|
|
{ |
|
|
|
f = PlsrAccelCurveFreqOnRampMs(f_from, f_to, t, t_ms, mode); |
|
|
|
if (f < 1U) |
|
|
|
{ |
|
|
|
if (f_to == 0U) |
|
|
|
if (f_to >= 1U) |
|
|
|
{ |
|
|
|
break; |
|
|
|
f = 1U; |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
f = 0U; |
|
|
|
} |
|
|
|
f = 1U; |
|
|
|
} |
|
|
|
acc += (uint64_t)f; |
|
|
|
if ((f_to == 0U) && (f <= 1U)) |
|
|
|
{ |
|
|
|
break; |
|
|
|
} |
|
|
|
} |
|
|
|
n = (uint32_t)(acc / 1000ULL); |
|
|
|
if (n == 0U) |
|
|
|
|
|
|
|
n = (uint32_t)((acc + 999ULL) / 1000ULL); |
|
|
|
if ((n == 0U) && (acc > 0ULL)) |
|
|
|
{ |
|
|
|
n = 1U; |
|
|
|
} |
|
|
|
return n; |
|
|
|
} |
|
|
|
|
|
|
|
/** 匀速时间向下取整:宁可早进减速,保证斜坡走完再落到 f_end */ |
|
|
|
static uint32_t PlsrAccelCurveConstTimeMs(uint32_t const_n, uint32_t f_hz) |
|
|
|
{ |
|
|
|
if ((const_n == 0U) || (f_hz < 1U)) |
|
|
|
{ |
|
|
|
return 0U; |
|
|
|
} |
|
|
|
return (const_n * 1000UL + (f_hz / 2UL)) / f_hz; |
|
|
|
return (const_n * 1000UL) / f_hz; |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
|