hanyongwei
/
TrainCamp_hanyongwei_StageTwo
1
0
Çatalla 0
Kod Konular 0 Değişiklik İstekleri 0 Sürümler 0 Wiki Aktivite
Kaynağa Gözat

修复S曲线规划和正弦曲线规划 

Signed-off-by: hanyongwei <2043702190@qq.com>
dev1
hanyongwei 3 hafta önce
ebeveyn
e00bd1bcaa
işleme
c93c9b4e77
3 değiştirilmiş dosya ile 514 ekleme ve 66 silme
Birleşik Görünüm
Diff Seçenekleri
İstatistikleri Göster Yama Dosyasını İndir Diff Dosyasını İndir
  1. +497
    -58
      plsr/accel_curve/plsr_accel_curve.c
  2. +15
    -6
      plsr/accel_curve/plsr_accel_curve.h
  3. +2
    -2
      plsr/param/plsr_param.h

+ 497
- 58
plsr/accel_curve/plsr_accel_curve.c Dosyayı Görüntüle

@@ -1,21 +1,19 @@
/** /**
* @file plsr_accel_curve.c * @file plsr_accel_curve.c
* @brief 脉冲域规划/取频实现：直线 f^2=f0^2±2an；S/正弦按脉冲进度 shape
* @brief 脉冲域规划/取频：直线 f^2=f0^2±2an；S/正弦按时间轴剖面
* *
* @details 模块职责 * @details 模块职责
* 实现 plsr_accel_curve.h 全部 API。核心路径： * 实现 plsr_accel_curve.h 全部 API。核心路径：
* Plan → 估 acc_n/dec_n → 不够则 FitPeak 降峰； * Plan → 估 acc_n/dec_n → 不够则 FitPeak 降峰；
* PulseRtBeginAcc/Dec/Const + Step 供 ISR 每脉冲改频。 * PulseRtBeginAcc/Dec/Const + Step 供 ISR 每脉冲改频。
* *
* S 曲线（时间域，加速度梯形 1:2:1）：
* jerk 升 → 恒加速 A → jerk 降；ISR 累计 t+=1/f 后按 f(t) 取频。
* 正弦：raised-cosine，T=π·Δf/(2A)，同样按时间取频。
*
* 关键约束 * 关键约束
* 频率钳制上限 100000Hz；运行门禁非法频率由 command/run_control 报 0x04。 * 频率钳制上限 100000Hz；运行门禁非法频率由 command/run_control 报 0x04。
* 脉冲预算不够到目标频时：降低峰值（三角，无匀速），调用方写故障 0x02。 * 脉冲预算不够到目标频时：降低峰值（三角，无匀速），调用方写故障 0x02。
*
* 符号约定（与 plsr_accel_curve.h 一致）
* f_cur/f_tgt/f_end — 段起/峰/止频率 Hz
* acc_n/dec_n/const_n — 加/减/匀速相脉冲预算（非时间 ms）
* a_acc/a_dec — 加/减速度 Hz/s
* pulse_rt — PlsrAccelPulseRt_t，ISR 逐拍改频状态机
*/ */
#include "plsr_accel_curve.h" #include "plsr_accel_curve.h"
#include <stdint.h> #include <stdint.h>
@@ -401,6 +399,340 @@ static uint32_t PlsrAccelCurvePulsesForRamp(uint32_t f0, uint32_t f1,
return (uint32_t)n; return (uint32_t)n;
} }


/*============================================================================*/
/* S / 正弦：时间轴剖面（加速度梯形 / raised-cosine） */
/*============================================================================*/

/**
* S 曲线时间参数（加速度梯形 1:2:1）：
* Tj : Ta : Tj = 1 : 2 : 1
* Δf = A*(Ta+Tj) ⇒ Tj=Δf/(3A), Ta=2Δf/(3A), T=4Δf/(3A)
*/
static void PlsrAccelCurveSTimeParams(uint32_t f0, uint32_t f1, uint32_t a_hz_s,
uint32_t *tj_us, uint32_t *ta_us,
uint32_t *tramp_us)
{
uint32_t df;
uint64_t num;
uint64_t den;
uint32_t tj;

*tj_us = 0U;
*ta_us = 0U;
*tramp_us = 0U;

df = PlsrAccelCurveAbsDiff(f0, f1);
if ((df < 1U) || (a_hz_s < 1U))
{
return;
}

/* Tj_us = ceil(df * 1e6 / (3A)) */
num = (uint64_t)df * 1000000ULL;
den = 3ULL * (uint64_t)a_hz_s;
tj = (uint32_t)((num + den - 1ULL) / den);
if (tj < 1U)
{
tj = 1U;
}
*tj_us = tj;
*ta_us = tj * 2U;
*tramp_us = tj * 4U;
}

/** 正弦 raised-cosine：峰值加速度 = A ⇒ T = π·Δf / (2A) */
static void PlsrAccelCurveSineTimeParams(uint32_t f0, uint32_t f1, uint32_t a_hz_s,
uint32_t *tramp_us)
{
uint32_t df;
uint64_t num;
uint64_t den;

*tramp_us = 0U;
df = PlsrAccelCurveAbsDiff(f0, f1);
if ((df < 1U) || (a_hz_s < 1U))
{
return;
}
/* T_us = ceil(π*df*1e6 / (2A))，π≈355/113 */
num = (uint64_t)df * 1000000ULL * 355ULL;
den = 2ULL * (uint64_t)a_hz_s * 113ULL;
*tramp_us = (uint32_t)((num + den - 1ULL) / den);
if (*tramp_us < 1U)
{
*tramp_us = 1U;
}
}

/** Δf = A * t_us / 1e6 */
static uint32_t PlsrAccelCurveDeltaHzFromAt(uint32_t a_hz_s, uint32_t t_us)
{
return (uint32_t)(((uint64_t)a_hz_s * (uint64_t)t_us) / 1000000ULL);
}

/** Δf = A * t_us^2 / (2 * tj_us * 1e6) — jerk 段 */
static uint32_t PlsrAccelCurveDeltaHzFromJerk(uint32_t a_hz_s, uint32_t t_us,
uint32_t tj_us)
{
uint64_t v;

if ((tj_us < 1U) || (t_us < 1U) || (a_hz_s < 1U))
{
return 0U;
}
/* ((A * t / tj) * t) / 2e6 */
v = ((uint64_t)a_hz_s * (uint64_t)t_us) / (uint64_t)tj_us;
v = (v * (uint64_t)t_us) / 2000000ULL;
if (v > 0xFFFFFFFFULL)
{
return 0xFFFFFFFFUL;
}
return (uint32_t)v;
}

/**
* 时刻 t_us 的 S 剖面频率（相对进度 0..df，再映射到 f0→f1）。
*/
static uint32_t PlsrAccelCurveSFreqAtTime(uint32_t f0, uint32_t f1, uint32_t a_hz_s,
uint32_t tj_us, uint32_t ta_us,
uint32_t tramp_us, uint32_t t_us)
{
uint32_t df;
uint32_t g;
uint32_t t2;
uint32_t tau;
uint8_t rising;

df = PlsrAccelCurveAbsDiff(f0, f1);
rising = (f1 >= f0) ? 1U : 0U;

if ((df < 1U) || (tramp_us < 1U) || (a_hz_s < 1U))
{
return PlsrAccelCurveClampFreq(f1);
}
if (t_us >= tramp_us)
{
return PlsrAccelCurveClampFreq(f1);
}

if ((tj_us < 1U) || (t_us <= tj_us))
{
/* jerk 升：g = 0.5*(A/Tj)*t^2 */
g = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, t_us, (tj_us < 1U) ? 1U : tj_us);
}
else
{
t2 = tj_us + ta_us;
if (t_us <= t2)
{
/* 恒加速：g = 0.5*A*Tj + A*(t-Tj) */
g = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, tj_us, tj_us);
g += PlsrAccelCurveDeltaHzFromAt(a_hz_s, t_us - tj_us);
}
else
{
/* jerk 降：g = 0.5*A*Tj + A*Ta + A*τ - 0.5*(A/Tj)*τ^2 */
tau = t_us - t2;
if (tau > tj_us)
{
tau = tj_us;
}
g = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, tj_us, tj_us);
g += PlsrAccelCurveDeltaHzFromAt(a_hz_s, ta_us);
g += PlsrAccelCurveDeltaHzFromAt(a_hz_s, tau);
{
uint32_t drop = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, tau, tj_us);
if (g > drop)
{
g -= drop;
}
else
{
g = 0U;
}
}
}
}

if (g > df)
{
g = df;
}
if (rising != 0U)
{
return PlsrAccelCurveClampFreq(f0 + g);
}
if (f0 > g)
{
return PlsrAccelCurveClampFreq(f0 - g);
}
return PlsrAccelCurveClampFreq(f1);
}

/**
* 正弦 raised-cosine：f = f0 + df * (1-cos(π u))/2，u=t/T
* 用现有 33 点 sine 形状表（已是 raised-cosine 千分比）。
*/
static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t f0, uint32_t f1,
uint32_t tramp_us, uint32_t t_us)
{
uint32_t u;
uint32_t ratio;
uint32_t f;

if ((tramp_us < 1U) || (f0 == f1))
{
return PlsrAccelCurveClampFreq(f1);
}
if (t_us >= tramp_us)
{
return PlsrAccelCurveClampFreq(f1);
}
u = (uint32_t)(((uint64_t)t_us * 1000ULL) / (uint64_t)tramp_us);
if (u > 1000U)
{
u = 1000U;
}
ratio = PlsrAccelCurveShapeSinePermille(u);
f = PlsrAccelCurveLerp(f0, f1, ratio);
if (f < 1U)
{
f = 1U;
}
return PlsrAccelCurveClampFreq(f);
}

/** S 斜坡脉冲数 ≈ ∫f dt（解析三段） */
static uint32_t PlsrAccelCurvePulsesForSRamp(uint32_t f0, uint32_t f1,
uint32_t a_hz_s)
{
uint32_t tj;
uint32_t ta;
uint32_t tramp;
uint32_t flo;
uint32_t fhi;
uint64_t acc_us; /* 脉冲·us，最后 /1e6 */
uint64_t n;
uint32_t g_half_j;

if (f0 == f1)
{
return 0U;
}
if (a_hz_s == 0U)
{
return 0U;
}

PlsrAccelCurveSTimeParams(f0, f1, a_hz_s, &tj, &ta, &tramp);
if (tramp < 1U)
{
return 1U;
}

flo = (f0 < f1) ? f0 : f1;
fhi = (f0 < f1) ? f1 : f0;
g_half_j = PlsrAccelCurveDeltaHzFromJerk(a_hz_s, tj, tj); /* 0.5*A*Tj */

/*
* I1 = f_start*Tj + A*Tj^2/6
* = flo*Tj + (1/3)*g_half_j*Tj （因 A*Tj^2/6 = (0.5*A*Tj)*Tj/3）
* 更稳：I1 = ∫(flo + 0.5*J t^2) = flo*tj_s + A*tj_s^2/6
* 用 us： (flo*tj + A*tj*tj/(6e6)) 作为脉冲·us
*/
acc_us = (uint64_t)flo * (uint64_t)tj;
acc_us += ((uint64_t)a_hz_s * (uint64_t)tj / 6ULL) * (uint64_t)tj / 1000000ULL;

/* I2：起点 flo+g_half_j，时长 Ta，+0.5*A*Ta^2 */
{
uint32_t f2 = flo + g_half_j;
if (f2 > fhi)
{
f2 = fhi;
}
acc_us += (uint64_t)f2 * (uint64_t)ta;
acc_us += ((uint64_t)a_hz_s * (uint64_t)ta / 2ULL) * (uint64_t)ta / 1000000ULL;
}

/* I3：与 I1 对称，用 fhi 替换 flo */
acc_us += (uint64_t)fhi * (uint64_t)tj;
acc_us -= ((uint64_t)a_hz_s * (uint64_t)tj / 6ULL) * (uint64_t)tj / 1000000ULL;

/* n = ceil(acc_us / 1e6) */
n = (acc_us + 999999ULL) / 1000000ULL;
if (n < 1ULL)
{
n = 1ULL;
}
if (n > 0xFFFFFFFFULL)
{
return 0xFFFFFFFFUL;
}
/* 略留余量，避免时间未跑完就按 n_total 收尾 */
if (n < 0xFFFFFFF0ULL)
{
n += (n / 32ULL) + 2ULL;
}
return (uint32_t)n;
}

/** 正弦：∫f dt = (f0+f1)/2 * T */
static uint32_t PlsrAccelCurvePulsesForSineRamp(uint32_t f0, uint32_t f1,
uint32_t a_hz_s)
{
uint32_t tramp;
uint64_t n;
uint32_t favg;

if (f0 == f1)
{
return 0U;
}
if (a_hz_s == 0U)
{
return 0U;
}
PlsrAccelCurveSineTimeParams(f0, f1, a_hz_s, &tramp);
if (tramp < 1U)
{
return 1U;
}
favg = f0 / 2U + f1 / 2U + (((f0 & 1U) + (f1 & 1U)) / 2U);
if (favg < 1U)
{
favg = 1U;
}
n = ((uint64_t)favg * (uint64_t)tramp + 999999ULL) / 1000000ULL;
if (n < 1ULL)
{
n = 1ULL;
}
if (n < 0xFFFFFFF0ULL)
{
n += (n / 32ULL) + 2ULL;
}
if (n > 0xFFFFFFFFULL)
{
return 0xFFFFFFFFUL;
}
return (uint32_t)n;
}

static uint32_t PlsrAccelCurvePulsesForModeRamp(uint32_t f0, uint32_t f1,
uint32_t a_hz_s,
PlsrAccelMode_e mode)
{
if (mode == PLSR_ACCEL_S)
{
return PlsrAccelCurvePulsesForSRamp(f0, f1, a_hz_s);
}
if (mode == PLSR_ACCEL_SINE)
{
return PlsrAccelCurvePulsesForSineRamp(f0, f1, a_hz_s);
}
return PlsrAccelCurvePulsesForRamp(f0, f1, a_hz_s);
}

/** 直线：f = sqrt(f0^2 + 2*a*n)，升到/降到不超过 limit 方向 */ /** 直线：f = sqrt(f0^2 + 2*a*n)，升到/降到不超过 limit 方向 */
static uint32_t PlsrAccelCurveFreqKinematic(uint32_t f0, uint32_t a_hz_s, static uint32_t PlsrAccelCurveFreqKinematic(uint32_t f0, uint32_t a_hz_s,
uint32_t n, uint8_t rising, uint32_t n, uint8_t rising,
@@ -461,7 +793,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
uint32_t f_want, uint32_t f_want,
uint32_t f_end, uint32_t f_end,
uint32_t a_acc, uint32_t a_acc,
uint32_t a_dec)
uint32_t a_dec,
PlsrAccelMode_e mode)
{ {
uint32_t lo = 0U; uint32_t lo = 0U;
uint32_t hi; uint32_t hi;
@@ -488,14 +821,14 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
if (f_want >= f_cur) if (f_want >= f_cur)
{ {
peak = f_cur + mid; peak = f_cur + mid;
n_a = PlsrAccelCurvePulsesForRamp(f_cur, peak, a_acc);
n_a = PlsrAccelCurvePulsesForModeRamp(f_cur, peak, a_acc, mode);
} }
else else
{ {
peak = f_cur - mid; peak = f_cur - mid;
n_a = PlsrAccelCurvePulsesForRamp(f_cur, peak, a_dec);
n_a = PlsrAccelCurvePulsesForModeRamp(f_cur, peak, a_dec, mode);
} }
n_d = PlsrAccelCurvePulsesForRamp(peak, f_end, a_dec);
n_d = PlsrAccelCurvePulsesForModeRamp(peak, f_end, a_dec, mode);


if ((n_a + n_d) <= total_pulses) if ((n_a + n_d) <= total_pulses)
{ {
@@ -606,13 +939,13 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
if (f_cur > f_peak) if (f_cur > f_peak)
{ {
/* 入口高于峰值：首相为减速，用 a_dec（与 rampTimeMs 减速分支一致） */ /* 入口高于峰值：首相为减速，用 a_dec（与 rampTimeMs 减速分支一致） */
acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_dec);
acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_dec, m);
} }
else else
{ {
acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc);
acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_acc, m);
} }
dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec);
dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m);


if (total_pulses == 0U) if (total_pulses == 0U)
{ {
@@ -635,16 +968,16 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
if ((acc_n + dec_n) > total_pulses) if ((acc_n + dec_n) > total_pulses)
{ {
f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end, f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end,
a_acc, a_dec);
a_acc, a_dec, m);
if (f_cur > f_peak) if (f_cur > f_peak)
{ {
acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_dec);
acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_dec, m);
} }
else else
{ {
acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc);
acc_n = PlsrAccelCurvePulsesForModeRamp(f_cur, f_peak, a_acc, m);
} }
dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec);
dec_n = PlsrAccelCurvePulsesForModeRamp(f_peak, f_end, a_dec, m);
if (acc_n > total_pulses) if (acc_n > total_pulses)
{ {
acc_n = total_pulses; acc_n = total_pulses;
@@ -700,7 +1033,6 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
uint32_t const_end; uint32_t const_end;
uint32_t n; uint32_t n;
uint32_t f; uint32_t f;
uint32_t ratio;
uint8_t rising; uint8_t rising;


if (plan == (PlsrAccelPlan_t *)0) if (plan == (PlsrAccelPlan_t *)0)
@@ -724,8 +1056,42 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
} }
else else
{ {
ratio = PlsrAccelCurveShape(pulse_done, plan->acc_n, plan->mode);
f = PlsrAccelCurveLerp(plan->f_cur, plan->f_tgt, ratio);
uint32_t tj = 0U;
uint32_t ta = 0U;
uint32_t tramp = 0U;
uint32_t t_us = 0U;
uint32_t a_use = (rising != 0U) ? plan->a_acc : plan->a_dec;
uint32_t i;

f = plan->f_cur;
if (plan->mode == PLSR_ACCEL_S)
{
PlsrAccelCurveSTimeParams(plan->f_cur, plan->f_tgt, a_use,
&tj, &ta, &tramp);
}
else
{
PlsrAccelCurveSineTimeParams(plan->f_cur, plan->f_tgt, a_use,
&tramp);
}
for (i = 0U; i < pulse_done; i++)
{
if (f < 1U)
{
f = 1U;
}
t_us += 1000000UL / f;
if (plan->mode == PLSR_ACCEL_S)
{
f = PlsrAccelCurveSFreqAtTime(plan->f_cur, plan->f_tgt, a_use,
tj, ta, tramp, t_us);
}
else
{
f = PlsrAccelCurveSineFreqAtTime(plan->f_cur, plan->f_tgt,
tramp, t_us);
}
}
} }
if ((f < 1U) && (plan->f_tgt >= 1U)) if ((f < 1U) && (plan->f_tgt >= 1U))
{ {
@@ -760,8 +1126,41 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
} }
else else
{ {
ratio = PlsrAccelCurveShape(n, plan->dec_n, plan->mode);
f = PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio);
uint32_t tj = 0U;
uint32_t ta = 0U;
uint32_t tramp = 0U;
uint32_t t_us = 0U;
uint32_t i;

f = plan->f_tgt;
if (plan->mode == PLSR_ACCEL_S)
{
PlsrAccelCurveSTimeParams(plan->f_tgt, plan->f_end, plan->a_dec,
&tj, &ta, &tramp);
}
else
{
PlsrAccelCurveSineTimeParams(plan->f_tgt, plan->f_end, plan->a_dec,
&tramp);
}
for (i = 0U; i < n; i++)
{
if (f < 1U)
{
f = 1U;
}
t_us += 1000000UL / f;
if (plan->mode == PLSR_ACCEL_S)
{
f = PlsrAccelCurveSFreqAtTime(plan->f_tgt, plan->f_end,
plan->a_dec, tj, ta, tramp, t_us);
}
else
{
f = PlsrAccelCurveSineFreqAtTime(plan->f_tgt, plan->f_end,
tramp, t_us);
}
}
} }
if (f < 1U) if (f < 1U)
{ {
@@ -775,21 +1174,32 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
/*============================================================================*/ /*============================================================================*/


/** /**
* @brief 为 S/正弦准备 Q24 步进：(1<<24)/N 商余，避免每拍除法
* @brief 为 S/正弦准备时间剖面（加速度梯形 / raised-cosine）
*/ */
static void PlsrAccelPulseRtSetupShape(PlsrAccelPulseRt_t *rt, uint32_t n_total)
static void PlsrAccelPulseRtSetupTimeProfile(PlsrAccelPulseRt_t *rt)
{ {
if (n_total < 1U)
rt->t_us = 0U;
rt->tj_us = 0U;
rt->ta_us = 0U;
rt->tramp_us = 0U;
rt->phase_q24 = 0U;
rt->step_q24 = 0U;
rt->step_rem = 0U;
rt->rem_acc = 0U;

if ((rt->a < 1U) || (rt->f0 == rt->f1))
{ {
rt->step_q24 = 0U;
rt->step_rem = 0U;
return; return;
} }
/* (1<<24)/N 商余：逐拍累加，避免每拍 index/total 除法 */
rt->step_q24 = 16777216UL / n_total;
rt->step_rem = 16777216UL % n_total;
rt->phase_q24 = 0U;
rt->rem_acc = 0U;
if (rt->mode == PLSR_ACCEL_S)
{
PlsrAccelCurveSTimeParams(rt->f0, rt->f1, rt->a,
&rt->tj_us, &rt->ta_us, &rt->tramp_us);
}
else if (rt->mode == PLSR_ACCEL_SINE)
{
PlsrAccelCurveSineTimeParams(rt->f0, rt->f1, rt->a, &rt->tramp_us);
}
} }


/** @brief 进入加速相（见 plsr_accel_curve.h） */ /** @brief 进入加速相（见 plsr_accel_curve.h） */
@@ -809,7 +1219,7 @@ void PlsrAccelPulseRtBeginAcc(PlsrAccelPulseRt_t *rt,
rt->a = (rt->rising != 0U) ? plan->a_acc : plan->a_dec; rt->a = (rt->rising != 0U) ? plan->a_acc : plan->a_dec;
rt->mode = plan->mode; rt->mode = plan->mode;
rt->active = (plan->acc_n > 0U) && (plan->f_cur != plan->f_tgt) ? 1U : 0U; rt->active = (plan->acc_n > 0U) && (plan->f_cur != plan->f_tgt) ? 1U : 0U;
PlsrAccelPulseRtSetupShape(rt, rt->n_total);
PlsrAccelPulseRtSetupTimeProfile(rt);
} }


/** @brief 进入减速相（见 plsr_accel_curve.h） */ /** @brief 进入减速相（见 plsr_accel_curve.h） */
@@ -829,7 +1239,7 @@ void PlsrAccelPulseRtBeginDec(PlsrAccelPulseRt_t *rt,
rt->rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U; rt->rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U;
rt->mode = plan->mode; rt->mode = plan->mode;
rt->active = (plan->dec_n > 0U) && (plan->f_tgt != plan->f_end) ? 1U : 0U; rt->active = (plan->dec_n > 0U) && (plan->f_tgt != plan->f_end) ? 1U : 0U;
PlsrAccelPulseRtSetupShape(rt, rt->n_total);
PlsrAccelPulseRtSetupTimeProfile(rt);
} }


/** @brief 进入匀速相（见 plsr_accel_curve.h） */ /** @brief 进入匀速相（见 plsr_accel_curve.h） */
@@ -849,6 +1259,10 @@ void PlsrAccelPulseRtBeginConst(PlsrAccelPulseRt_t *rt,
rt->rising = 1U; rt->rising = 1U;
rt->mode = plan->mode; rt->mode = plan->mode;
rt->active = 0U; /* 匀速：确认频率没变 → 复用 → 结束 */ rt->active = 0U; /* 匀速：确认频率没变 → 复用 → 结束 */
rt->t_us = 0U;
rt->tj_us = 0U;
rt->ta_us = 0U;
rt->tramp_us = 0U;
rt->step_q24 = 0U; rt->step_q24 = 0U;
rt->step_rem = 0U; rt->step_rem = 0U;
rt->phase_q24 = 0U; rt->phase_q24 = 0U;
@@ -940,47 +1354,51 @@ static uint32_t PlsrAccelPulseRtSquareStep(PlsrAccelPulseRt_t *rt)
} }


/** /**
* @brief S/正弦：累加 Q24 进度 → shape 千分比 → Lerp(f0,f1)
* S/正弦时间域步进：上一拍周期推进 t，再按剖面取下一拍频率。
*/ */
static uint32_t PlsrAccelPulseRtShapeStep(PlsrAccelPulseRt_t *rt)
static uint32_t PlsrAccelPulseRtTimeStep(PlsrAccelPulseRt_t *rt)
{ {
uint32_t ratio;
uint32_t f_prev;
uint32_t dt_us;
uint32_t f; uint32_t f;


if (rt->n_total < 1U)
if ((rt->tramp_us < 1U) || (rt->a < 1U))
{ {
return PlsrAccelCurveClampFreq(rt->f1); return PlsrAccelCurveClampFreq(rt->f1);
} }


if (rt->n >= rt->n_total)
f_prev = rt->f;
if (f_prev < 1U)
{ {
return PlsrAccelCurveClampFreq(rt->f1);
f_prev = 1U;
} }
dt_us = 1000000UL / f_prev;


rt->phase_q24 += rt->step_q24;
rt->rem_acc += rt->step_rem;
if (rt->rem_acc >= rt->n_total)
if (rt->t_us < (0xFFFFFFFFUL - dt_us))
{ {
rt->rem_acc -= rt->n_total;
rt->phase_q24 += 1U;
rt->t_us += dt_us;
} }
if (rt->phase_q24 > 16777216UL)
else
{ {
rt->phase_q24 = 16777216UL;
rt->t_us = 0xFFFFFFFFUL;
} }


ratio = (uint32_t)(((uint64_t)rt->phase_q24 * 1000ULL) / 16777216ULL);
if (ratio > 1000U)
if (rt->t_us >= rt->tramp_us)
{ {
ratio = 1000U;
return PlsrAccelCurveClampFreq(rt->f1);
} }
ratio = PlsrAccelCurveShapePermille(ratio, rt->mode);
f = PlsrAccelCurveLerp(rt->f0, rt->f1, ratio);
if (f < 1U)

if (rt->mode == PLSR_ACCEL_S)
{ {
f = 1U;
f = PlsrAccelCurveSFreqAtTime(rt->f0, rt->f1, rt->a,
rt->tj_us, rt->ta_us, rt->tramp_us,
rt->t_us);
} }
return PlsrAccelCurveClampFreq(f);
else
{
f = PlsrAccelCurveSineFreqAtTime(rt->f0, rt->f1, rt->tramp_us, rt->t_us);
}
return f;
} }


/** /**
@@ -1015,13 +1433,34 @@ uint32_t PlsrAccelPulseRtStep(PlsrAccelPulseRt_t *rt)
} }
else else
{ {
rt->f = PlsrAccelPulseRtShapeStep(rt);
rt->f = PlsrAccelPulseRtTimeStep(rt);
} }


if ((rt->n_total > 0U) && (rt->n >= rt->n_total))
if (((rt->n_total > 0U) && (rt->n >= rt->n_total)) ||
((rt->mode != PLSR_ACCEL_LINEAR) && (rt->tramp_us > 0U) &&
(rt->t_us >= rt->tramp_us)))
{ {
rt->f = rt->f1; rt->f = rt->f1;
rt->active = 0U; rt->active = 0U;
} }
else if (rt->mode != PLSR_ACCEL_LINEAR)
{
if (rt->rising != 0U)
{
if (rt->f >= rt->f1)
{
rt->f = rt->f1;
rt->active = 0U;
}
}
else
{
if (rt->f <= rt->f1)
{
rt->f = rt->f1;
rt->active = 0U;
}
}
}
return rt->f; return rt->f;
} }

+ 15
- 6
plsr/accel_curve/plsr_accel_curve.h Dosyayı Görüntüle

@@ -5,6 +5,10 @@
* @details 按起/峰/止频率、斜率时间与总脉冲规划三相脉冲预算，并在脉冲序号上取频。 * @details 按起/峰/止频率、斜率时间与总脉冲规划三相脉冲预算，并在脉冲序号上取频。
* 不读 wait_type，不碰 TIM/GPIO。 * 不读 wait_type，不碰 TIM/GPIO。
* *
* 直线：f_n = sqrt(f0^2 ± 2 a n)（脉冲域）。
* S/正弦：时间轴规划——S 为 jerk 升/恒加速/jerk 降（加速度梯形 1:2:1）；
* 正弦为 raised-cosine；ISR 按累计时间取频，每拍推进 Δt=1/f。
*
* 术语（全模块统一）： * 术语（全模块统一）：
* acc_n / dec_n / const_n — 加速/减速/匀速相各需的脉冲个数（规划预算，非 ms） * acc_n / dec_n / const_n — 加速/减速/匀速相各需的脉冲个数（规划预算，非 ms）
* f_cur / f_tgt / f_end — 段入口频率、规划峰值、段出口频率（Hz） * f_cur / f_tgt / f_end — 段入口频率、规划峰值、段出口频率（Hz）
@@ -66,19 +70,24 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,


/** /**
* 单相逐脉冲运行态（ISR 热路径，每来一个 UPDATE 调 Step 一次） * 单相逐脉冲运行态（ISR 热路径，每来一个 UPDATE 调 Step 一次）
* 直线：±1Hz 逼近 sqrt(f0²±2an)；S/正弦：Q24 相位累加 + shape 表
* 直线：±1Hz 逼近 sqrt(f0²±2an)
* S/正弦：累计 t_us，按时间剖面取频；每拍 t += 1e6/f
*/ */
typedef struct { typedef struct {
uint32_t f; /* 当前输出频率 Hz */ uint32_t f; /* 当前输出频率 Hz */
uint32_t f0; /* 本相起点频率 Hz */ uint32_t f0; /* 本相起点频率 Hz */
uint32_t f1; /* 本相终点频率 Hz */ uint32_t f1; /* 本相终点频率 Hz */
uint32_t a; /* 本相加速度 Hz/s（直线用） */
uint32_t a; /* 本相加速度 Hz/s */
uint32_t n; /* 本相已步进脉冲数 */ uint32_t n; /* 本相已步进脉冲数 */
uint32_t n_total; /* 本相总步进预算（= plan 里 acc_n 或 dec_n） */ uint32_t n_total; /* 本相总步进预算（= plan 里 acc_n 或 dec_n） */
uint32_t phase_q24; /* S/正弦：形状进度 Q24 累加器 */
uint32_t step_q24; /* 每脉冲相位增量 = (1<<24)/n_total */
uint32_t step_rem; /* 相位累加余数（商余法，避免每拍除法） */
uint32_t rem_acc; /* 余数累加 */
uint32_t t_us; /* S/正弦：本相已累计时间 us */
uint32_t tj_us; /* S：jerk 段时长 us；正弦未用 */
uint32_t ta_us; /* S：恒加速段时长 us；正弦未用 */
uint32_t tramp_us; /* S/正弦：本相斜坡总时长 us */
uint32_t phase_q24; /* 保留 */
uint32_t step_q24;
uint32_t step_rem;
uint32_t rem_acc;
uint8_t rising; /* 1=频率升高，0=降低 */ uint8_t rising; /* 1=频率升高，0=降低 */
uint8_t active; /* 1=本相仍在步进；0=已到 f1 */ uint8_t active; /* 1=本相仍在步进；0=已到 f1 */
PlsrAccelMode_e mode; PlsrAccelMode_e mode;


+ 2
- 2
plsr/param/plsr_param.h Dosyayı Görüntüle

@@ -116,8 +116,8 @@ typedef enum {
/** 加减速曲线形状（脉冲域规划） */ /** 加减速曲线形状（脉冲域规划） */
typedef enum { typedef enum {
PLSR_ACCEL_LINEAR = 0, /* f^2 = f0^2 ± 2an */ PLSR_ACCEL_LINEAR = 0, /* f^2 = f0^2 ± 2an */
PLSR_ACCEL_S = 1, /* 七段 S 曲线按脉冲进度 */
PLSR_ACCEL_SINE = 2 /* raised-cosine */
PLSR_ACCEL_S = 1, /* 时间域七段 S：加速度梯形 1:2:1 */
PLSR_ACCEL_SINE = 2 /* 时间域 raised-cosine */
} PlsrAccelMode_e; } PlsrAccelMode_e;


/** /**


Yaz Önizleme
Yükleniyor…
İptal
Kaydet