Просмотр исходного кода

受1ms更新一次频率限制,最后一拍不保证一定落在终止频率上

Signed-off-by: hanyongwei <2043702190@qq.com>
master
hanyongwei 1 месяц назад
Родитель
Сommit
875e484696
2 измененных файлов: 18 добавлений и 31 удалений
  1. +17
    -30
      plsr/accel_curve/plsr_accel_curve.c
  2. +1
    -1
      plsr/accel_curve/plsr_accel_curve.h

+ 17
- 30
plsr/accel_curve/plsr_accel_curve.c Просмотреть файл

@@ -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;
}

/**


+ 1
- 1
plsr/accel_curve/plsr_accel_curve.h Просмотреть файл

@@ -2,7 +2,7 @@
* @file plsr_accel_curve.h
* @brief 预估加/匀/减脉冲;相内按时间走直线/S/正弦
*
* 1ms 改频:直线脉冲按面积 N=(f0+f1)*T/2000;到不了目标频则降峰值走三角(无匀速)。
* 1ms 改频:脉冲数按 t=0..T-1 实际取频累加;到不了目标频则降峰值走三角(无匀速)。
* 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。
* 斜率按公共参数:K≈(默认速度−起/止速)/加减速时间;小频差时 T 可能 <1ms,无可见斜坡属正常。
*/


Загрузка…
Отмена
Сохранить