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

改为按脉冲累加发送,不再按照1ms改频

dev1
hanyongwei преди 1 месец
родител
ревизия
6ff53fdf14
променени са 6 файла, в които са добавени 438 реда и са изтрити 304 реда
  1. +253
    -156
      plsr/accel_curve/plsr_accel_curve.c
  2. +19
    -32
      plsr/accel_curve/plsr_accel_curve.h
  3. +5
    -0
      plsr/pulse_driver/plsr_pulse_driver.c
  4. +2
    -0
      plsr/pulse_driver/plsr_pulse_driver.h
  5. +156
    -113
      plsr/run_control/plsr_run_control.c
  6. +3
    -3
      plsr/run_control/plsr_run_control.h

+ 253
- 156
plsr/accel_curve/plsr_accel_curve.c Целия файл

@@ -1,12 +1,8 @@
/** /**
* @file plsr_accel_curve.c * @file plsr_accel_curve.c
* @brief 预估相脉冲;相内时间域直线/S/正弦
* @brief 脉冲域规划/取频:f^2 = f0^2 + 2*a*n(直线);S/正弦按脉冲进度 shape
* *
* 脉冲预算不够到目标频时:降低峰值,保留加减速时间(勿把 T 压成 0,
* 否则示波器上变成方波,三种模式完全无法区分)。

* @note:目前脉冲输出在低频时和高频时均有毛刺,需要优化
* 已解决,原因是逻辑分析仪采样率不够,导致采样点减少,波形有毛刺。
* 脉冲预算不够到目标频时:降低峰值(三角,无匀速)。
*/ */
#include "plsr_accel_curve.h" #include "plsr_accel_curve.h"
#include <stdint.h> #include <stdint.h>
@@ -26,40 +22,42 @@ static uint32_t PlsrAccelCurveAbsDiff(uint32_t a, uint32_t b)
return (a >= b) ? (a - b) : (b - a); return (a >= b) ? (a - b) : (b - a);
} }


/**
* 整数平方根(牛顿法)。
*/
static uint32_t PlsrAccelCurveIsqrt(uint32_t val)
/** 64 位整数平方根(牛顿法),支持 f^2 + 2an */
static uint32_t PlsrAccelCurveIsqrt64(uint64_t val)
{ {
uint32_t x;
uint32_t x2;
uint64_t x;
uint64_t x2;
uint32_t i; uint32_t i;


if (val <= 1U)
if (val <= 1ULL)
{ {
return val;
return (uint32_t)val;
} }
x = val; x = val;
if (x > 10000U)
if (x > 100000ULL)
{ {
x = 10000U;
x = 100000ULL;
} }
for (i = 0U; i < 30U; i++)
for (i = 0U; i < 40U; i++)
{ {
x2 = (x + val / x) / 2U;
x2 = (x + val / x) / 2ULL;
if (x2 >= x) if (x2 >= x)
{ {
break; break;
} }
x = x2; x = x2;
} }
return x;
return (uint32_t)x;
}

static uint32_t PlsrAccelCurveIsqrt(uint32_t val)
{
return PlsrAccelCurveIsqrt64((uint64_t)val);
} }


/** /**
* 起跳频率:f_jump = sqrt(a_Hz_per_s)。
* 加速度 a = df * 1000 / t_ms(Hz/s),df = |f_to - f_from|。
* 当 t_ms=0 或 df=0 时返回 1。
* 起跳频率:f = sqrt(f_from^2 + 2a),a = df*1000/t_ms。
* f_from=0 → sqrt(2a);超过 f_to 钳到 f_to。
*/ */
uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
uint32_t t_ms) uint32_t t_ms)
@@ -67,17 +65,21 @@ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
uint32_t df; uint32_t df;
uint32_t a_hz_s; uint32_t a_hz_s;
uint32_t f; uint32_t f;
uint64_t val;


if (t_ms == 0U)
if (f_to < 1U)
{ {
return 1U; return 1U;
} }
if (t_ms == 0U)
{
return f_to;
}
df = PlsrAccelCurveAbsDiff(f_from, f_to); df = PlsrAccelCurveAbsDiff(f_from, f_to);
if (df == 0U) if (df == 0U)
{ {
return 1U;
return f_to;
} }
/* a = df / (t_ms / 1000) = df * 1000 / t_ms,可能很大 */
if (df <= 4294967U) if (df <= 4294967U)
{ {
a_hz_s = (df * 1000U) / t_ms; a_hz_s = (df * 1000U) / t_ms;
@@ -88,13 +90,21 @@ uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
} }
if (a_hz_s == 0U) if (a_hz_s == 0U)
{ {
return 1U;
f = (f_from >= 1U) ? f_from : 1U;
}
else
{
val = (uint64_t)f_from * (uint64_t)f_from + (2ULL * (uint64_t)a_hz_s);
f = PlsrAccelCurveIsqrt64(val);
} }
f = PlsrAccelCurveIsqrt(a_hz_s);
if (f < 1U) if (f < 1U)
{ {
f = 1U; f = 1U;
} }
if (f > f_to)
{
f = f_to;
}
return f; return f;
} }


@@ -163,44 +173,6 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
return t; return t;
} }


/**
* 从 0 爬到 f_tgt(或从 f_tgt 落到 0)时的起跳/落地频率。
* start/end 参考速都为 0:a = f_tgt*1000/t_ms,f=sqrt(a);
* 否则走 JumpFreq(0, f_tgt, t_ms)。
*/
static uint32_t PlsrAccelCurveJumpFromZero(uint32_t f_tgt,
uint32_t t_ms,
uint32_t start_spd_ref,
uint32_t end_spd_ref)
{
uint32_t f;

if ((f_tgt < 1U) || (t_ms < 1U))
{
return 1U;
}

if ((start_spd_ref == 0U) && (end_spd_ref == 0U))
{
uint64_t a_hz_s = ((uint64_t)f_tgt * 1000ULL) / (uint64_t)t_ms;
f = (a_hz_s == 0ULL) ? 1U : PlsrAccelCurveIsqrt((uint32_t)a_hz_s);
}
else
{
f = PlsrAccelCurveJumpFreq(0U, f_tgt, t_ms);
}

if (f < 1U)
{
f = 1U;
}
if (f > f_tgt)
{
f = f_tgt;
}
return f;
}

uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
uint32_t f_tgt, uint32_t f_tgt,
uint32_t default_spd, uint32_t default_spd,
@@ -213,20 +185,16 @@ uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,


f_cfg = PlsrAccelCurveClampFreq(f_cfg); f_cfg = PlsrAccelCurveClampFreq(f_cfg);
f_tgt = PlsrAccelCurveClampFreq(f_tgt); f_tgt = PlsrAccelCurveClampFreq(f_tgt);
if (f_cfg >= 1U)
{
return f_cfg;
}
if (f_tgt < 1U) if (f_tgt < 1U)
{ {
return 1U; return 1U;
} }


t_acc = PlsrAccelCurveRampTimeMs(0U, f_tgt, default_spd,
/* 起速 0 或非 0:都用 JumpFreq = sqrt(f_cfg^2 + 2a) */
t_acc = PlsrAccelCurveRampTimeMs(f_cfg, f_tgt, default_spd,
start_spd_ref, end_spd_ref, start_spd_ref, end_spd_ref,
accel_ms, decel_ms); accel_ms, decel_ms);
return PlsrAccelCurveJumpFromZero(f_tgt, t_acc,
start_spd_ref, end_spd_ref);
return PlsrAccelCurveJumpFreq(f_cfg, f_tgt, t_acc);
} }


uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
@@ -241,20 +209,26 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,


f_cfg = PlsrAccelCurveClampFreq(f_cfg); f_cfg = PlsrAccelCurveClampFreq(f_cfg);
f_tgt = PlsrAccelCurveClampFreq(f_tgt); f_tgt = PlsrAccelCurveClampFreq(f_tgt);
if (f_cfg >= 1U)
{
return f_cfg;
}
if (f_tgt < 1U) if (f_tgt < 1U)
{ {
return 1U; return 1U;
} }


t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd,
/*
* 止速 0 或 1:按落地频率估计,避免 1Hz 拖尾。
*/
if (f_cfg <= 1U)
{
t_dec = PlsrAccelCurveRampTimeMs(f_tgt, 0U, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
return PlsrAccelCurveJumpFreq(0U, f_tgt, t_dec);
}

t_dec = PlsrAccelCurveRampTimeMs(f_tgt, f_cfg, default_spd,
start_spd_ref, end_spd_ref, start_spd_ref, end_spd_ref,
accel_ms, decel_ms); accel_ms, decel_ms);
return PlsrAccelCurveJumpFromZero(f_tgt, t_dec,
start_spd_ref, end_spd_ref);
return PlsrAccelCurveJumpFreq(f_cfg, f_tgt, t_dec);
} }


/** 直线:进度千分比 → 形状千分比 */ /** 直线:进度千分比 → 形状千分比 */
@@ -704,27 +678,126 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from,
} }


/** /**
* 二分峰值频率(Hz),使 1ms 模型下 acc+dec 脉冲 <= total,且尽量接近原目标频。
* 到不了目标频时这就是三角:峰值降低,匀速脉冲必须为 0。
* a = default_spd * 1000 / ramp_ms(Hz/s)。ramp_ms=0 → a=0(阶跃)。
*/
static uint32_t PlsrAccelCurveAccelHzS(uint32_t default_spd, uint32_t ramp_ms)
{
if (ramp_ms == 0U)
{
return 0U;
}
if (default_spd == 0U)
{
return 0U;
}
return (default_spd * 1000UL) / ramp_ms;
}

/**
* 脉冲闭合:N = ceil(|f1^2 - f0^2| / (2a))。
* a=0 且频差非 0 → 0(阶跃,无斜坡脉冲)。
*/
static uint32_t PlsrAccelCurvePulsesForRamp(uint32_t f0, uint32_t f1,
uint32_t a_hz_s)
{
uint64_t f0s;
uint64_t f1s;
uint64_t df2;
uint64_t two_a;
uint64_t n;

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

f0s = (uint64_t)f0 * (uint64_t)f0;
f1s = (uint64_t)f1 * (uint64_t)f1;
df2 = (f1s > f0s) ? (f1s - f0s) : (f0s - f1s);
two_a = 2ULL * (uint64_t)a_hz_s;
n = (df2 + two_a - 1ULL) / two_a;
if (n == 0ULL)
{
n = 1ULL;
}
if (n > 0xFFFFFFFFULL)
{
return 0xFFFFFFFFUL;
}
return (uint32_t)n;
}

/** 直线:f = sqrt(f0^2 + 2*a*n),升到/降到不超过 limit 方向 */
static uint32_t PlsrAccelCurveFreqKinematic(uint32_t f0, uint32_t a_hz_s,
uint32_t n, uint8_t rising,
uint32_t limit)
{
uint64_t val;
uint32_t f;

if (n == 0U)
{
f = f0;
}
else if (a_hz_s == 0U)
{
f = limit;
}
else
{
val = (uint64_t)f0 * (uint64_t)f0;
if (rising != 0U)
{
val += 2ULL * (uint64_t)a_hz_s * (uint64_t)n;
f = PlsrAccelCurveIsqrt64(val);
if (f > limit)
{
f = limit;
}
}
else
{
uint64_t drop = 2ULL * (uint64_t)a_hz_s * (uint64_t)n;
if (drop >= val)
{
f = limit;
}
else
{
f = PlsrAccelCurveIsqrt64(val - drop);
if (f < limit)
{
f = limit;
}
}
}
}
if (f < 1U)
{
f = 1U;
}
return PlsrAccelCurveClampFreq(f);
}

/**
* 二分峰值:使 acc+dec 脉冲 <= total(脉冲闭合公式)。
*/ */
static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses, static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
uint32_t f_cur, uint32_t f_cur,
uint32_t f_want, uint32_t f_want,
uint32_t f_end, uint32_t f_end,
uint32_t default_spd,
uint32_t start_spd_ref,
uint32_t end_spd_ref,
uint32_t accel_ms,
uint32_t decel_ms,
PlsrAccelMode_e mode)
uint32_t a_acc,
uint32_t a_dec)
{ {
uint32_t lo = 0U; uint32_t lo = 0U;
uint32_t hi; uint32_t hi;
uint32_t mid; uint32_t mid;
uint32_t best = 0U; uint32_t best = 0U;
uint32_t peak; uint32_t peak;
uint32_t t_a;
uint32_t t_d;
uint32_t n_a; uint32_t n_a;
uint32_t n_d; uint32_t n_d;
uint32_t span; uint32_t span;
@@ -750,14 +823,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
{ {
peak = f_cur - mid; peak = f_cur - mid;
} }
t_a = PlsrAccelCurveRampTimeMs(f_cur, peak, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
t_d = PlsrAccelCurveRampTimeMs(peak, f_end, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
n_a = PlsrAccelCurveEstimatePulses(f_cur, peak, t_a, mode);
n_d = PlsrAccelCurveEstimatePulses(peak, f_end, t_d, mode);
n_a = PlsrAccelCurvePulsesForRamp(f_cur, peak, a_acc);
n_d = PlsrAccelCurvePulsesForRamp(peak, f_end, a_dec);


if ((n_a + n_d) <= total_pulses) if ((n_a + n_d) <= total_pulses)
{ {
@@ -806,6 +873,8 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
uint32_t acc_n; uint32_t acc_n;
uint32_t dec_n; uint32_t dec_n;
uint32_t f_peak; uint32_t f_peak;
uint32_t a_acc;
uint32_t a_dec;
PlsrAccelMode_e m; PlsrAccelMode_e m;


if (plan == (PlsrAccelPlan_t *)0) if (plan == (PlsrAccelPlan_t *)0)
@@ -819,10 +888,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,


m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode; m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode;


/*
* 端点须由调用方解析完毕。若仍传入 0,用同一套 Resolve 兜底,
* 避免把 0 当真实频率规划出错误斜坡;正常路径不应走到这里。
*/
if (f_cur < 1U) if (f_cur < 1U)
{ {
f_cur = PlsrAccelCurveResolveStartHz(0U, f_tgt, default_spd, f_cur = PlsrAccelCurveResolveStartHz(0U, f_tgt, default_spd,
@@ -836,6 +901,26 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
accel_ms, decel_ms); accel_ms, decel_ms);
} }


a_acc = PlsrAccelCurveAccelHzS(default_spd, accel_ms);
a_dec = PlsrAccelCurveAccelHzS(default_spd, decel_ms);
/* 升降方向用对应斜率;无默认速度时用频差/时间回退 */
if ((a_acc == 0U) && (accel_ms > 0U))
{
uint32_t df = PlsrAccelCurveAbsDiff(f_cur, f_tgt);
if (df > 0U)
{
a_acc = (df * 1000UL) / accel_ms;
}
}
if ((a_dec == 0U) && (decel_ms > 0U))
{
uint32_t df = PlsrAccelCurveAbsDiff(f_tgt, f_end);
if (df > 0U)
{
a_dec = (df * 1000UL) / decel_ms;
}
}

f_peak = f_tgt; f_peak = f_tgt;
t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
start_spd_ref, end_spd_ref, start_spd_ref, end_spd_ref,
@@ -847,13 +932,14 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
plan->f_cur = f_cur; plan->f_cur = f_cur;
plan->f_end = f_end; plan->f_end = f_end;
plan->mode = m; plan->mode = m;
acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m);
dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m);
plan->a_acc = a_acc;
plan->a_dec = a_dec;

acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc);
dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec);


if (total_pulses == 0U) if (total_pulses == 0U)
{ {
acc_n = 0U;
dec_n = 0U;
plan->const_n = 0U; plan->const_n = 0U;
plan->t_acc_ms = 0U; plan->t_acc_ms = 0U;
plan->t_dec_ms = 0U; plan->t_dec_ms = 0U;
@@ -864,11 +950,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
return; return;
} }


/*
* f_end == f_tgt:本段无独立段末终点(仅加速到目标)。
* 脉冲不够时不 FitPeak、不减速;梯形/三角只由
* (f_cur,f_tgt,f_end,斜率,mode,总脉冲) 决定,与 wait 无关。
*/
if ((f_end == f_tgt) && (acc_n > total_pulses)) if ((f_end == f_tgt) && (acc_n > total_pulses))
{ {
plan->const_n = 0U; plan->const_n = 0U;
@@ -883,21 +964,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,
default_spd, start_spd_ref, end_spd_ref,
accel_ms, decel_ms, m);
f_peak = PlsrAccelCurveFitPeak(total_pulses, f_cur, f_tgt, f_end,
a_acc, a_dec);
t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd, t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
start_spd_ref, end_spd_ref, start_spd_ref, end_spd_ref,
accel_ms, decel_ms); accel_ms, decel_ms);
t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd, t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd,
start_spd_ref, end_spd_ref, start_spd_ref, end_spd_ref,
accel_ms, decel_ms); accel_ms, decel_ms);
acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m);
dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m);
acc_n = PlsrAccelCurvePulsesForRamp(f_cur, f_peak, a_acc);
dec_n = PlsrAccelCurvePulsesForRamp(f_peak, f_end, a_dec);
if (acc_n > total_pulses) if (acc_n > total_pulses)
{ {
acc_n = total_pulses; acc_n = total_pulses;
@@ -919,13 +995,9 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
} }
else else
{ {
/* 够到目标频:匀速脉冲 = 总数 − 加速 − 减速 */
plan->const_n = total_pulses - acc_n - dec_n; plan->const_n = total_pulses - acc_n - dec_n;
} }


/*
* 需要减速 (f_peak > f_end) 时禁止 dec_n=0。
*/
if ((f_peak > f_end) && (total_pulses > 1U) && (dec_n == 0U)) if ((f_peak > f_end) && (total_pulses > 1U) && (dec_n == 0U))
{ {
dec_n = 1U; dec_n = 1U;
@@ -955,70 +1027,95 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
plan->t_dec_ms = t_dec; plan->t_dec_ms = t_dec;
plan->acc_n = acc_n; plan->acc_n = acc_n;
plan->dec_n = dec_n; plan->dec_n = dec_n;
/*
* 进入减速时刻:加速时间 + 匀速时间。
* 三角无匀速 → t_decel_start = t_acc,减速从峰值立刻开始。
* 止速脉冲数 = dec_n,由 1ms 模型按 (f_peak→f_end, t_dec) 估出。
*/
plan->t_decel_start_ms = t_acc + PlsrAccelCurveConstTimeMs(plan->const_n, f_peak); plan->t_decel_start_ms = t_acc + PlsrAccelCurveConstTimeMs(plan->const_n, f_peak);
} }


uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan,
uint32_t seg_elapsed_ms)
uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
uint32_t pulse_done)
{ {
uint32_t t;
uint32_t T_acc_us;
uint32_t t_dec;
uint32_t const_end;
uint32_t n;
uint32_t f;
uint32_t ratio; uint32_t ratio;
uint8_t rising;


if (plan == (PlsrAccelPlan_t *)0) if (plan == (PlsrAccelPlan_t *)0)
{ {
return 0U; return 0U;
} }


if ((plan->t_acc_ms > 0U) && (seg_elapsed_ms < plan->t_acc_ms) &&
const_end = plan->acc_n + plan->const_n;

/* 加速相:0 .. acc_n-1 完成后仍在加速;pulse_done 为已完成数 */
if ((plan->acc_n > 0U) && (pulse_done < plan->acc_n) &&
(plan->f_cur != plan->f_tgt)) (plan->f_cur != plan->f_tgt))
{ {
T_acc_us = plan->t_acc_ms * 1000UL;
rising = (plan->f_tgt >= plan->f_cur) ? 1U : 0U;
if (plan->mode == PLSR_ACCEL_LINEAR)
{ {
uint32_t f_acc = PlsrAccelCurveFreqOnRampUs(plan->f_cur, plan->f_tgt,
seg_elapsed_ms * 1000UL,
T_acc_us, plan->mode);
if ((f_acc < 1U) && (plan->f_tgt >= 1U))
{
f_acc = 1U;
}
return PlsrAccelCurveClampFreq(f_acc);
f = PlsrAccelCurveFreqKinematic(plan->f_cur, plan->a_acc,
pulse_done, rising, plan->f_tgt);
}
else
{
ratio = PlsrAccelCurveShape(pulse_done, plan->acc_n, plan->mode);
f = PlsrAccelCurveLerp(plan->f_cur, plan->f_tgt, ratio);
} }
if ((f < 1U) && (plan->f_tgt >= 1U))
{
f = 1U;
}
return PlsrAccelCurveClampFreq(f);
} }


if (seg_elapsed_ms < plan->t_decel_start_ms)
/* 匀速 */
if (pulse_done < const_end)
{ {
return PlsrAccelCurveClampFreq(plan->f_tgt); return PlsrAccelCurveClampFreq(plan->f_tgt);
} }


if ((plan->t_dec_ms == 0U) || (plan->f_tgt == plan->f_end))
/* 减速 / 无减速 */
if ((plan->dec_n == 0U) || (plan->f_tgt == plan->f_end))
{ {
return PlsrAccelCurveClampFreq(plan->f_end); return PlsrAccelCurveClampFreq(plan->f_end);
} }


t_dec = seg_elapsed_ms - plan->t_decel_start_ms;
if (t_dec >= plan->t_dec_ms)
n = pulse_done - const_end;
if (n >= plan->dec_n)
{ {
return PlsrAccelCurveClampFreq(plan->f_end); return PlsrAccelCurveClampFreq(plan->f_end);
} }


rising = (plan->f_end >= plan->f_tgt) ? 1U : 0U;
if (plan->mode == PLSR_ACCEL_LINEAR) if (plan->mode == PLSR_ACCEL_LINEAR)
{ {
return PlsrAccelCurveClampFreq(
PlsrAccelCurveFreqOnRampUs(plan->f_tgt, plan->f_end,
t_dec * 1000UL,
plan->t_dec_ms * 1000UL,
plan->mode));
f = PlsrAccelCurveFreqKinematic(plan->f_tgt, plan->a_dec,
n, rising, plan->f_end);
}
else
{
ratio = PlsrAccelCurveShape(n, plan->dec_n, plan->mode);
f = PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio);
}
if (f < 1U)
{
f = 1U;
} }
return PlsrAccelCurveClampFreq(f);
}

/** 兼容旧预览:用时间估算映射到脉冲再取频 */
uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan,
uint32_t seg_elapsed_ms)
{
uint32_t approx_n;
uint32_t f_ref;


t = plan->t_dec_ms;
ratio = PlsrAccelCurveShape(t_dec, t, plan->mode);
return PlsrAccelCurveClampFreq(
PlsrAccelCurveLerp(plan->f_tgt, plan->f_end, ratio));
if (plan == (PlsrAccelPlan_t *)0)
{
return 0U;
}
f_ref = (plan->f_tgt >= 1U) ? plan->f_tgt : 1U;
approx_n = (uint32_t)(((uint64_t)seg_elapsed_ms * (uint64_t)f_ref) / 1000ULL);
return PlsrAccelCurveFreqAtPulse(plan, approx_n);
} }

+ 19
- 32
plsr/accel_curve/plsr_accel_curve.h Целия файл

@@ -1,12 +1,11 @@
/** /**
* @file plsr_accel_curve.h * @file plsr_accel_curve.h
* @brief 预估加/匀/减脉冲;相内按时间走直线/S/正弦
* @brief 脉冲域加/匀/减规划与取频(v^2 - v0^2 = 2ax)
* *
* 1ms 改频:脉冲数按 t=0..T-1 实际取频累加;到不了目标频则降峰值走三角(无匀速)。
* 加速度 a = 默认速度*1000/加减速时间(Hz/s)。
* 脉冲序号 n 上:f(n) = sqrt(f0^2 + 2*a*n);初速为 0 即 f0=0。
* 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。 * 匀速仅当加速+减速脉冲之和 < 总脉冲(真正到达目标频)时出现。
* 斜率按公共参数:K_acc=默认速度/加速时间,K_dec=默认速度/减速时间(减 0,不用起/止速)。
* 小频差时 T 可能 <1ms,无可见斜坡属正常。
* 开段端点由运行层 ResolveStart/EndHz 解析后再传入 Plan(Plan 不解释策略 0)。
* 开段端点由运行层 ResolveStart/EndHz 解析后再传入 Plan。
*/ */
#ifndef PLSR_ACCEL_CURVE_H #ifndef PLSR_ACCEL_CURVE_H
#define PLSR_ACCEL_CURVE_H #define PLSR_ACCEL_CURVE_H
@@ -21,9 +20,11 @@ typedef struct {
uint32_t f_cur; uint32_t f_cur;
uint32_t f_tgt; uint32_t f_tgt;
uint32_t f_end; uint32_t f_end;
uint32_t t_acc_ms;
uint32_t a_acc; /* 加速 a(Hz/s) */
uint32_t a_dec; /* 减速 a(Hz/s) */
uint32_t t_acc_ms; /* 斜率时间估算(预览/兼容,运行不依赖) */
uint32_t t_dec_ms; uint32_t t_dec_ms;
uint32_t t_decel_start_ms; /* 反向规划:段内时间轴上进入减速的时刻 */
uint32_t t_decel_start_ms; /* 估算:进减速时刻(预览用) */
PlsrAccelMode_e mode; PlsrAccelMode_e mode;
} PlsrAccelPlan_t; } PlsrAccelPlan_t;


@@ -36,17 +37,14 @@ typedef enum {
uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz); uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz);


/** /**
* 起跳频率:f_jump = sqrt(加速度_Hz/s)。
* 起速/止速为 0 时,第一个脉冲(或最后一个脉冲)应以此频率输出,
* 使脉冲周期恰好等于加速度产生该频率所需的时间。
* 起跳/步进频率:f = sqrt(f_from^2 + 2a),a = |f_to-f_from|*1000/t_ms。
* f_from=0 时退化为 sqrt(2a);超过 f_to 则钳到 f_to。
*/ */
uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to, uint32_t PlsrAccelCurveJumpFreq(uint32_t f_from, uint32_t f_to,
uint32_t t_ms); uint32_t t_ms);


/** /**
* 配置起速 → 规划用真实起点频率。
* f_cfg>=1:原样(钳位);f_cfg==0:按斜率算起跳频率(从静止爬升)。
* 由运行层在开段前调用;Plan 只吃已经解析好的端点。
* 配置起速 → 规划用真实起点:一律 JumpFreq(f_cfg, f_tgt, t_acc)。
*/ */
uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg, uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
uint32_t f_tgt, uint32_t f_tgt,
@@ -57,9 +55,8 @@ uint32_t PlsrAccelCurveResolveStartHz(uint32_t f_cfg,
uint32_t decel_ms); uint32_t decel_ms);


/** /**
* 配置止速 → 规划用真实终点频率。
* f_cfg>=1:原样(钳位);f_cfg==0:按斜率算落地频率(避免 1Hz 拖尾)。
* 段末是否停表由运行层决定,与本函数无关。
* 配置止速 → 规划用真实终点。
* f_cfg<=1:按落地频率估计(避免 1Hz 拖尾);否则 JumpFreq。
*/ */
uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg, uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
uint32_t f_tgt, uint32_t f_tgt,
@@ -69,43 +66,28 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t f_cfg,
uint32_t accel_ms, uint32_t accel_ms,
uint32_t decel_ms); uint32_t decel_ms);


/**
* 斜坡上按 mode 取频(µs 时间轴)。ISR 可用:O(1)。
* f = f0+(f1-f0)*shape(t/T)
*/
uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0, uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0,
uint32_t f1, uint32_t f1,
uint32_t t_us, uint32_t t_us,
uint32_t T_us, uint32_t T_us,
PlsrAccelMode_e mode); PlsrAccelMode_e mode);


/**
* 与 TIM3 1ms 改频一致的斜坡取频(规划/运行共用)。
*/
uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0, uint32_t PlsrAccelCurveFreqOnRampMs(uint32_t f0,
uint32_t f1, uint32_t f1,
uint32_t t_ms, uint32_t t_ms,
uint32_t T_ms, uint32_t T_ms,
PlsrAccelMode_e mode); PlsrAccelMode_e mode);


/**
* 斜坡第一拍频率(起速为 0 时用自洽点,避免按 1Hz 整秒吃光加速相)。
* ISR 可用:O(1)。
*/
uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0, uint32_t PlsrAccelCurveFirstPulseFreq(uint32_t f0,
uint32_t f1, uint32_t f1,
uint32_t T_us, uint32_t T_us,
PlsrAccelMode_e mode); PlsrAccelMode_e mode);


/**
* 减速落地频率:f_end>=1 即止速;f_end==0 返回 0(停表,不用 1Hz 冒充)。
*/
uint32_t PlsrAccelCurveLandFreq(uint32_t f_end, uint32_t PlsrAccelCurveLandFreq(uint32_t f_end,
uint32_t f_peak, uint32_t f_peak,
uint32_t T_ms, uint32_t T_ms,
PlsrAccelMode_e mode); PlsrAccelMode_e mode);


/** 拟合斜坡时间(ms),使离散模型脉冲数尽量等于 max_pulses(任务上下文) */
uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from, uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from,
uint32_t f_to, uint32_t f_to,
uint32_t max_pulses, uint32_t max_pulses,
@@ -125,8 +107,13 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
PlsrAccelMode_e mode); PlsrAccelMode_e mode);


/** /**
* 1ms 改频:按段内绝对时间取频(相界由 t_decel_start_ms 决定,不用脉冲域)。
* 按已完成脉冲数取频(任务上下文调用,勿进脉冲 ISR)。
* pulse_done = 本段已发完的脉冲数(与 s_done 一致)。
*/ */
uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
uint32_t pulse_done);

/** @deprecated 时间轴取频;保留给旧预览,运行请用 FreqAtPulse */
uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan, uint32_t PlsrAccelCurveFreqAtSegTime(const PlsrAccelPlan_t *plan,
uint32_t seg_elapsed_ms); uint32_t seg_elapsed_ms);




+ 5
- 0
plsr/pulse_driver/plsr_pulse_driver.c Целия файл

@@ -556,6 +556,11 @@ void PlsrPulseDriverRequestFreq(uint32_t freq_hz)
s_req_pending = 1U; s_req_pending = 1U;
} }


void PlsrPulseDriverClearPending(void)
{
s_req_pending = 0U;
}

void PlsrPulseDriverApplyPending(void) void PlsrPulseDriverApplyPending(void)
{ {
uint32_t f; uint32_t f;


+ 2
- 0
plsr/pulse_driver/plsr_pulse_driver.h Целия файл

@@ -37,6 +37,8 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz);
void PlsrPulseDriverRequestFreq(uint32_t freq_hz); void PlsrPulseDriverRequestFreq(uint32_t freq_hz);
/** 在更新中断里调用:应用挂起频率 */ /** 在更新中断里调用:应用挂起频率 */
void PlsrPulseDriverApplyPending(void); void PlsrPulseDriverApplyPending(void);
/** 丢弃未生效的改频请求(进匀速锁定时用) */
void PlsrPulseDriverClearPending(void);
void PlsrPulseDriverStop(void); void PlsrPulseDriverStop(void);
/** 当前拍结束后停止计数,避免多出一个上升沿 */ /** 当前拍结束后停止计数,避免多出一个上升沿 */
void PlsrPulseDriverArmOnePulseStop(void); void PlsrPulseDriverArmOnePulseStop(void);


+ 156
- 113
plsr/run_control/plsr_run_control.c Целия файл

@@ -12,7 +12,10 @@
* 3) 发送模式只影响本段 f_end(完成=止速;后续且不停表=下一段目标) * 3) 发送模式只影响本段 f_end(完成=止速;后续且不停表=下一段目标)
* 4) PlanSeg / ChangeFreq 共用规划入口,便于后期动态改频 * 4) PlanSeg / ChangeFreq 共用规划入口,便于后期动态改频
* *
* 【运行节拍】TIM3 1ms:RefreshProfile 改频;脉冲 UPDATE ISR 只计 s_done。
* 【运行节拍】脉冲 UPDATE:计 s_done;加/减速置 dirty,匀速不周期性改频。
* 任务 TickMs:dirty 时 FreqAtPulse 改频。
* TIM3 1ms:仅 ACT/WAIT 时基(不改频)。
* 匀速“保持”≠禁止改频:ACT/EXT 打断、ChangeFreq、进减速、开下一段仍可 Start/SetFreq。
*/ */
#include "plsr_run_control.h" #include "plsr_run_control.h"
#include "plsr_path_plan.h" #include "plsr_path_plan.h"
@@ -58,11 +61,11 @@ static volatile uint8_t s_wait_expire_req; /* WAIT 时间到:ISR 只置位 */
static uint8_t s_wait_is_signal; /* RC_WAIT_COND:1=等 WAIT 沿,0=等时间 */ static uint8_t s_wait_is_signal; /* RC_WAIT_COND:1=等 WAIT 沿,0=等时间 */


static PlsrAccelPlan_t s_accel_plan; static PlsrAccelPlan_t s_accel_plan;
static RunPhase_e s_phase;
static uint32_t s_seg_elapsed_ms; /* 段内绝对时间(TIM3 1ms 累加) */
static volatile RunPhase_e s_phase; /* ISR 读相,须 volatile */
static uint32_t s_approach_from; static uint32_t s_approach_from;
static uint32_t s_decel_from; static uint32_t s_decel_from;
static uint8_t s_follow_cont; static uint8_t s_follow_cont;
static volatile uint8_t s_profile_dirty; /* 脉冲推进后由任务改频 */


static uint32_t s_chain_freq; static uint32_t s_chain_freq;
static uint8_t s_chain_valid; static uint8_t s_chain_valid;
@@ -604,7 +607,7 @@ static uint32_t PlsrRunControlClampSpeed(uint32_t spd)
return spd; return spd;
} }


/** 运行输出:0=停表;1ms 节拍直接 SetFreq / Start */
/** 运行输出:0=停表;任务上下文 SetFreq / Start */
static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start) static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start)
{ {
if (profile_freq == 0U) if (profile_freq == 0U)
@@ -644,122 +647,138 @@ static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq)
} }
} }


static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses)
static uint8_t PlsrRunControlHasDecel(void)
{ {
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;
uint32_t old_dec_n;
uint32_t old_t_dec;
return ((s_accel_plan.dec_n > 0U) &&
(s_accel_plan.f_tgt != s_accel_plan.f_end)) ? 1U : 0U;
}


old_dec_n = s_accel_plan.dec_n;
old_t_dec = s_accel_plan.t_dec_ms;
static uint32_t PlsrRunControlRemainPulses(void)
{
if (s_target > s_done)
{
return (uint32_t)(s_target - s_done);
}
return 0U;
}


static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t remain_pulses)
{
PlsrRunControlEnterPhase(PH_DECEL, from_hz); PlsrRunControlEnterPhase(PH_DECEL, from_hz);
f_end = s_accel_plan.f_end;

s_accel_plan.acc_n = 0U;
s_accel_plan.const_n = 0U;
s_accel_plan.dec_n = remain_pulses;
s_accel_plan.f_tgt = from_hz;
s_decel_from = from_hz;
s_profile_dirty = 1U;


if (remain_pulses == 0U) if (remain_pulses == 0U)
{ {
s_accel_plan.dec_n = 0U;
s_accel_plan.t_dec_ms = 0U; s_accel_plan.t_dec_ms = 0U;
return; return;
} }


s_accel_plan.dec_n = remain_pulses;

if (from_hz == f_end)
if (from_hz == s_accel_plan.f_end)
{ {
s_accel_plan.t_dec_ms = 0U; s_accel_plan.t_dec_ms = 0U;
return; return;
} }


/*
* 规划阶段已按斜率算好 t_dec:优先沿用,避免 ISR 内改歪形状。
* 止速=0:始终用斜率时间;时间走完即停,不按剩余脉冲把 T 拉长
* (拉长会在尾部堆 1Hz 空耗)。
*/
if ((old_t_dec > 0U) && (old_dec_n > 0U) &&
(remain_pulses == old_dec_n))
/* 时间字段仅预览兼容;运行取频走 FreqAtPulse */
if (s_accel_plan.t_dec_ms == 0U)
{ {
s_accel_plan.t_dec_ms = old_t_dec;
return;
s_accel_plan.t_dec_ms = 1U;
} }
}


/* 参数斜率:K = 默认速度 / 加减速时间(减 0) */
df = (from_hz > f_end) ? (from_hz - f_end) : (f_end - from_hz);
if (f_end > from_hz)
{
ref_ms = s_run_accel_ms;
}
else
{
ref_ms = s_run_decel_ms;
}
den = cfg->default_speed;
if (den == 0U)
/**
* 进匀速:写一次 f_tgt,之后 RefreshProfile 不再周期性取频/改 ARR(防抖)。
* 注意:这不是禁止改频——ACT/EXT 打断、ChangeFreq、进减速、开下一段
* 都会离开 PH_CONST 或直接 Start/Stop,ApplyOutFreq 仍可改频。
*/
static void PlsrRunControlEnterConstHold(uint8_t do_start)
{
uint32_t f_hold = s_accel_plan.f_tgt;

if (f_hold < 1U)
{ {
den = df;
f_hold = 1U;
} }
if ((ref_ms > 0U) && (den > 0U))
PlsrRunControlEnterPhase(PH_CONST, f_hold);
PlsrRunControlApplyOutFreq(f_hold, do_start);
}

static void PlsrRunControlRefreshProfile(uint8_t do_start)
{
uint32_t next;
uint32_t done_n;
uint32_t remain;

done_n = 0U;
if (s_done > 0)
{ {
t_slope = (df * ref_ms + den - 1UL) / den;
if (t_slope < 1U)
{
t_slope = 1U;
}
done_n = (uint32_t)s_done;
} }
remain = PlsrRunControlRemainPulses();


/* /*
* ISR 内禁止 FitRamp/EstimatePulses;用 O(1) 平均公式。
* 脉冲被截断导致 remain 变化时才走这里。
* 匀速 → 减速:只看剩余脉冲,不反复算频。
*/ */
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
if ((s_phase == PH_CONST) &&
(PlsrRunControlHasDecel() != 0U) &&
(remain <= s_accel_plan.dec_n))
{ {
t_fit = 1U;
}
if (t_fit < 1U)
{
t_fit = 1U;
PlsrRunControlEnterPhase(PH_DECEL, s_accel_plan.f_tgt);
} }


if ((t_slope > 0U) && (t_slope <= t_fit))
{
s_accel_plan.t_dec_ms = t_slope;
}
else
/*
* 匀速保持:跳过周期性 FreqAtPulse(防顶频抖动)。
* 仍允许:do_start / 停表后重开 写一次;打断切段走 BeginSeg/PlanSeg
* 会改 s_phase,不再走本分支。
*/
if (s_phase == PH_CONST)
{ {
s_accel_plan.t_dec_ms = t_fit;
if ((do_start != 0U) || (s_pwm_on == 0U))
{
PlsrRunControlApplyOutFreq(s_accel_plan.f_tgt, do_start);
}
return;
} }
}


static void PlsrRunControlRefreshProfile(uint8_t do_start)
{
uint32_t next;

/*
* 1ms 改频:段内统一时间轴;t_decel_start_ms 由规划反向算出,
* 减速为 f_tgt→f_end 直线,自然落到止速,末拍不再强行对齐。
*/
next = PlsrAccelCurveFreqAtSegTime(&s_accel_plan, s_seg_elapsed_ms);
/* 加速 / 减速:按脉冲域算下一频率 */
next = PlsrAccelCurveFreqAtPulse(&s_accel_plan, done_n);
if (next < 1U) if (next < 1U)
{ {
next = 1U; next = 1U;
} }


if (s_phase == PH_APPROACH)
{
/*
* 加速结束:脉冲用完或频率已到峰值 → 匀速保持(或直接进减速)。
*/
if ((s_accel_plan.acc_n == 0U) ||
(done_n >= s_accel_plan.acc_n) ||
((s_accel_plan.f_tgt >= s_accel_plan.f_cur) &&
(next >= s_accel_plan.f_tgt)) ||
((s_accel_plan.f_tgt < s_accel_plan.f_cur) &&
(next <= s_accel_plan.f_tgt)))
{
next = s_accel_plan.f_tgt;
if ((PlsrRunControlHasDecel() != 0U) &&
(remain <= s_accel_plan.dec_n))
{
PlsrRunControlEnterPhase(PH_DECEL, next);
}
else
{
PlsrRunControlEnterConstHold(do_start);
return;
}
}
}

if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U)) if ((next != s_cur_freq) || (do_start != 0U) || (s_pwm_on == 0U))
{ {
PlsrRunControlApplyOutFreq(next, do_start); PlsrRunControlApplyOutFreq(next, do_start);
@@ -799,9 +818,8 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
f_start = s_accel_plan.f_cur; f_start = s_accel_plan.f_cur;


/* /*
* 1ms 改频模式:运行时按 TIM3 毫秒时间轴取频。
* 禁止在这里用脉冲递推模型二次拟合 T(FitRamp),否则规划与运行模型不一致,
* 会出现中段拐折/扭曲、段末落不到设定 f_end。
* 脉冲域:运行时按已完成脉冲数 FreqAtPulse 取频。
* 禁止再用时间轴二次拟合,否则规划与运行不一致。
*/ */


/* 本段频率范围锁 PSC,升降只改 ARR;跨度过大则 LockPscRange 自动不锁 */ /* 本段频率范围锁 PSC,升降只改 ARR;跨度过大则 LockPscRange 自动不锁 */
@@ -864,9 +882,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
} }


/* /*
* 保持 AccelCurvePlan 算出的斜率时间,不再 FitRamp 拉长 T。
* 脉冲数由 EstimatePulses 决定;运行时按相时间取频,
* 一拍周期盖住 T 时自然落到相终点。
* 保持 AccelCurvePlan 算出的脉冲预算;运行时 FreqAtPulse 按 s_done 取频。
*/ */
/* /*
* 纯减速段:已在目标速(或更高)且整段预算不超过 dec_n。 * 纯减速段:已在目标速(或更高)且整段预算不超过 dec_n。
@@ -887,7 +903,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
{ {
PlsrRunControlEnterPhase(PH_APPROACH, f_start); PlsrRunControlEnterPhase(PH_APPROACH, f_start);
} }
s_seg_elapsed_ms = 0U;
s_profile_dirty = 1U;
} }


static void PlsrRunControlFinishAll(void) static void PlsrRunControlFinishAll(void)
@@ -902,6 +918,7 @@ static void PlsrRunControlFinishAll(void)
s_follow_cont = 0U; s_follow_cont = 0U;
s_chain_valid = 0U; s_chain_valid = 0U;
s_chain_freq = 0U; s_chain_freq = 0U;
s_profile_dirty = 0U;
s_act_armed = 0U; s_act_armed = 0U;
s_act_expire_req = 0U; s_act_expire_req = 0U;
s_act_cut_pending = 0U; s_act_cut_pending = 0U;
@@ -996,7 +1013,7 @@ static void PlsrRunControlBeginSeg(uint16_t seg_0)
if (s_follow_cont != 0U) if (s_follow_cont != 0U)
{ {
PlsrPulseDriverClearOnePulseStop(); PlsrPulseDriverClearOnePulseStop();
s_seg_elapsed_ms = 0U;
s_profile_dirty = 0U;
PlsrRunControlRefreshProfile(0U); PlsrRunControlRefreshProfile(0U);
s_state = RC_RUN; s_state = RC_RUN;
s_follow_cont = 0U; s_follow_cont = 0U;
@@ -1027,9 +1044,9 @@ void PlsrRunControlInit(void)
s_run_accel_ms = 0U; s_run_accel_ms = 0U;
s_run_decel_ms = 0U; s_run_decel_ms = 0U;
s_phase = PH_CONST; s_phase = PH_CONST;
s_seg_elapsed_ms = 0U;
s_approach_from = 0U; s_approach_from = 0U;
s_decel_from = 0U; s_decel_from = 0U;
s_profile_dirty = 0U;
s_act_armed = 0U; s_act_armed = 0U;
s_act_expire_req = 0U; s_act_expire_req = 0U;
s_act_cut_pending = 0U; s_act_cut_pending = 0U;
@@ -1083,6 +1100,7 @@ void PlsrRunControlStop(void)
s_follow_cont = 0U; s_follow_cont = 0U;
s_chain_valid = 0U; s_chain_valid = 0U;
s_chain_freq = 0U; s_chain_freq = 0U;
s_profile_dirty = 0U;
s_act_armed = 0U; s_act_armed = 0U;
s_act_expire_req = 0U; s_act_expire_req = 0U;
s_act_cut_pending = 0U; s_act_cut_pending = 0U;
@@ -1116,7 +1134,7 @@ uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz)
s_run_accel_ms, s_run_decel_ms); s_run_accel_ms, s_run_decel_ms);
s_target = (int32_t)remain; s_target = (int32_t)remain;
s_done = 0; s_done = 0;
s_seg_elapsed_ms = 0U;
s_profile_dirty = 0U;
PlsrRunControlRefreshProfile(0U); PlsrRunControlRefreshProfile(0U);
return 1U; return 1U;
} }
@@ -1150,7 +1168,7 @@ void PlsrRunControlTickMs(void)
if (PlsrRunControlOsTimeReached(s_dir_deadline) != 0U) if (PlsrRunControlOsTimeReached(s_dir_deadline) != 0U)
{ {
s_state = RC_RUN; s_state = RC_RUN;
s_seg_elapsed_ms = 0U;
s_profile_dirty = 0U;
PlsrRunControlRefreshProfile(1U); PlsrRunControlRefreshProfile(1U);
PlsrRunControlArmActExtOnPulseStart(); PlsrRunControlArmActExtOnPulseStart();
} }
@@ -1159,7 +1177,24 @@ void PlsrRunControlTickMs(void)


if (s_state == RC_RUN) if (s_state == RC_RUN)
{ {
/* 波形改频 / ACT 计时在 TIM3 On1ms;此处只处理 EXT */
/*
* 仅加/减速(或匀速刚到进减速点)才改频。
* 匀速锁定期间 dirty 不会置位,避免顶频反复写 ARR。
*/
if ((s_profile_dirty != 0U) && (s_act_cut_pending == 0U))
{
s_profile_dirty = 0U;
PlsrRunControlRefreshProfile(0U);
}
else if ((s_phase == PH_CONST) &&
(PlsrRunControlHasDecel() != 0U) &&
(s_act_cut_pending == 0U) &&
(PlsrRunControlRemainPulses() <= s_accel_plan.dec_n))
{
/* 任务侧兜底:进减速 */
PlsrRunControlRefreshProfile(0U);
}

seg = PlsrParamGetSeg(s_cur_seg); seg = PlsrParamGetSeg(s_cur_seg);


if ((seg->wait_type == PLSR_WAIT_EXT) || if ((seg->wait_type == PLSR_WAIT_EXT) ||
@@ -1199,8 +1234,8 @@ void PlsrRunControlTickMs(void)
} }


/** /**
* TIM3 1ms:段内时间累加 + 按时间轴 RefreshProfile 改频
* ACT/WAIT 只置标志,切段在 TickMs(避免 ISR 里 PlanSeg 丢拍)。
* TIM3 1ms:仅 ACT/WAIT 时基(不再改频)
* 切段在 TickMs(避免 ISR 里 PlanSeg 丢拍)。
*/ */
void PlsrRunControlOn1ms(void) void PlsrRunControlOn1ms(void)
{ {
@@ -1220,35 +1255,23 @@ void PlsrRunControlOn1ms(void)
return; return;
} }


if (s_seg_elapsed_ms < 0xFFFFFFFFUL)
{
s_seg_elapsed_ms++;
}

if ((s_act_armed != 0U) && if ((s_act_armed != 0U) &&
(PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) != 0U)) (PlsrSignalIoMsReached(s_act_arm_ms + (uint32_t)s_act_time_ms) != 0U))
{ {
/* /*
* ACT 是运动控制条件,不参与曲线规划:加速、匀速、减速 * ACT 是运动控制条件,不参与曲线规划:加速、匀速、减速
* 任一阶段到期都在当前脉冲 UPDATE 边界切段。这样不会在
* 一个 PWM 周期中间改段,也不会把边界脉冲计到下一段。
* 任一阶段到期都在当前脉冲 UPDATE 边界切段。
*/ */
s_act_armed = 0U; s_act_armed = 0U;
s_act_cut_pending = 1U; s_act_cut_pending = 1U;
} }

/* 保持 ACT 到期瞬间的频率,不让 1ms 曲线继续推进到下一个边界。 */
if (s_act_cut_pending != 0U)
{
return;
}

PlsrRunControlRefreshProfile(0U);
} }


/** 脉冲 UPDATE:只累计 s_done / s_acc_pulse,不改频 */
/** 脉冲 UPDATE:计脉冲;仅加/减速置 dirty(匀速不改频) */
void PlsrRunControlOnPulseIsr(void) void PlsrRunControlOnPulseIsr(void)
{ {
uint32_t remain;

if (s_state != RC_RUN) if (s_state != RC_RUN)
{ {
return; return;
@@ -1264,6 +1287,26 @@ void PlsrRunControlOnPulseIsr(void)
s_acc_pulse--; s_acc_pulse--;
} }


/*
* 匀速:不置 dirty,定时器 ARR 保持不动。
* 仅当剩余脉冲进入减速预算时置位,让任务切到 DECEL。
*/
if (s_phase == PH_CONST)
{
if (PlsrRunControlHasDecel() != 0U)
{
remain = PlsrRunControlRemainPulses();
if (remain <= s_accel_plan.dec_n)
{
s_profile_dirty = 1U;
}
}
}
else
{
s_profile_dirty = 1U;
}

/* /*
* ACT 到期切段只在脉冲边界执行,避免半个 PWM 周期改段。 * ACT 到期切段只在脉冲边界执行,避免半个 PWM 周期改段。
* 同向衔接:不停表,任务里用当前频率规划下一段(丢掉本段剩余)。 * 同向衔接:不停表,任务里用当前频率规划下一段(丢掉本段剩余)。


+ 3
- 3
plsr/run_control/plsr_run_control.h Целия файл

@@ -11,9 +11,9 @@ void PlsrRunControlInit(void);
uint8_t PlsrRunControlStart(uint16_t start_seg_1based); uint8_t PlsrRunControlStart(uint16_t start_seg_1based);
void PlsrRunControlStop(void); void PlsrRunControlStop(void);
uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz); uint8_t PlsrRunControlChangeFreq(uint32_t new_tgt_hz);
void PlsrRunControlTickMs(void);
void PlsrRunControlOn1ms(void); /* TIM3 1ms:改频 + WAIT/ACT 时基 */
void PlsrRunControlOnPulseIsr(void); /* 脉冲 UPDATE:只计脉冲 */
void PlsrRunControlOn1ms(void); /* TIM3 1ms:仅 ACT/WAIT 时基 */
void PlsrRunControlOnPulseIsr(void); /* 脉冲 UPDATE:计脉冲 + dirty */
void PlsrRunControlTickMs(void); /* 任务:脉冲域改频 + EXT/到期切段 */
uint8_t PlsrRunControlIsBusy(void); uint8_t PlsrRunControlIsBusy(void);
int32_t PlsrRunControlGetAccPulse(void); int32_t PlsrRunControlGetAccPulse(void);
void PlsrRunControlClearAccPulse(void); void PlsrRunControlClearAccPulse(void);


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