Преглед на файлове

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

Signed-off-by: hanyongwei <2043702190@qq.com>
master
hanyongwei преди 1 месец
родител
ревизия
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, static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from,
uint32_t f_to, uint32_t f_to,
@@ -406,68 +407,54 @@ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from,
uint32_t t; uint32_t t;
uint32_t f; uint32_t f;
uint32_t n; uint32_t n;
uint64_t acc;
uint32_t t_use; uint32_t t_use;
uint64_t acc;


if ((t_ms == 0U) || (f_from == f_to)) if ((t_ms == 0U) || (f_from == f_to))
{ {
return 0U; 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; t_use = t_ms;
if (t_use > 120000U) if (t_use > 120000U)
{ {
t_use = 120000U; t_use = 120000U;
} }

acc = 0ULL; acc = 0ULL;
for (t = 0U; t < t_use; t++) for (t = 0U; t < t_use; t++)
{ {
f = PlsrAccelCurveFreqOnRampMs(f_from, f_to, t, t_ms, mode); f = PlsrAccelCurveFreqOnRampMs(f_from, f_to, t, t_ms, mode);
if (f < 1U) if (f < 1U)
{ {
if (f_to == 0U)
if (f_to >= 1U)
{ {
break;
f = 1U;
}
else
{
f = 0U;
} }
f = 1U;
} }
acc += (uint64_t)f; 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; n = 1U;
} }
return n; return n;
} }


/** 匀速时间向下取整:宁可早进减速,保证斜坡走完再落到 f_end */
static uint32_t PlsrAccelCurveConstTimeMs(uint32_t const_n, uint32_t f_hz) static uint32_t PlsrAccelCurveConstTimeMs(uint32_t const_n, uint32_t f_hz)
{ {
if ((const_n == 0U) || (f_hz < 1U)) if ((const_n == 0U) || (f_hz < 1U))
{ {
return 0U; 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 * @file plsr_accel_curve.h
* @brief 预估加/匀/减脉冲;相内按时间走直线/S/正弦 * @brief 预估加/匀/减脉冲;相内按时间走直线/S/正弦
* *
* 1ms 改频:直线脉冲按面积 N=(f0+f1)*T/2000;到不了目标频则降峰值走三角(无匀速)。
* 1ms 改频:脉冲数按 t=0..T-1 实际取频累加;到不了目标频则降峰值走三角(无匀速)。
* 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。 * 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。
* 斜率按公共参数:K≈(默认速度−起/止速)/加减速时间;小频差时 T 可能 <1ms,无可见斜坡属正常。 * 斜率按公共参数:K≈(默认速度−起/止速)/加减速时间;小频差时 T 可能 <1ms,无可见斜坡属正常。
*/ */


Зареждане…
Отказ
Запис