hanyongwei
/
TrainCamp_hanyongwei_StageTwo
1
0
Форкнуть 0
Код Задачи 0 Pull Request'ы 0 Релизы 0 Вики Активность
Просмотр исходного кода

增加S曲线实现

master
hanyongwei 1 месяц назад
Родитель
4a498a829c
Сommit
a5eabf4a11
3 измененных файлов: 144 добавлений и 168 удалений
Разделённый вид
Опции Diff
Показать статистику Скачать Patch файл Скачать Diff файл
  1. +113
    -131
      plsr/accel_curve/plsr_accel_curve.c
  2. +14
    -3
      plsr/accel_curve/plsr_accel_curve.h
  3. +17
    -34
      plsr/run_control/plsr_run_control.c

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

@@ -6,6 +6,7 @@
* 否则示波器上变成方波，三种模式完全无法区分）。

* @note：目前脉冲输出在低频时和高频时均有毛刺，需要优化
* 已解决，原因是逻辑分析仪采样率不够，导致采样点减少，波形有毛刺。
*/
#include "plsr_accel_curve.h"

@@ -93,13 +94,94 @@ static uint32_t PlsrAccelCurveRampTimeMs(uint32_t f_from,
return t;
}

/** 直线：进度千分比 → 形状千分比 */
static uint32_t PlsrAccelCurveShapeLinearPermille(uint32_t u)
{
return (u > 1000U) ? 1000U : u;
}


/**
* 七段一侧 1:2:1：头尾更圆、中间更陡（相对直线可辨）
* u∈[0,250]/[750,1000] 为抛物线圆角，中间线性陡段。
*/
static uint32_t PlsrAccelCurveShapeS7Permille(uint32_t u)
{
uint32_t d;
uint32_t p;

if (u >= 1000U)
{
return 1000U;
}
if (u <= 250UL)
{
p = (8UL * u * u) / (3UL * 1000UL);
}
else if (u <= 750UL)
{
p = 167UL + (666UL * (u - 250UL)) / 500UL;
}
else
{
d = 1000UL - u;
p = 1000UL - (8UL * d * d) / (3UL * 1000UL);
}
return (p > 1000UL) ? 1000UL : p;
}

/** O(1)：时间进度千分比 → 曲线形状千分比 */
static uint32_t PlsrAccelCurveShapePermille(uint32_t u, PlsrAccelMode_e mode)
{
if (mode == PLSR_ACCEL_S)
{
return PlsrAccelCurveShapeS7Permille(u);
}
if (mode == PLSR_ACCEL_SINE)
{
//return PlsrAccelCurveShapeSinePermille(u);
}
return PlsrAccelCurveShapeLinearPermille(u);
}

static uint32_t PlsrAccelCurveShape(uint32_t t_ms, uint32_t T_ms, PlsrAccelMode_e mode)
{
uint32_t u;

if ((T_ms == 0U) || (t_ms >= T_ms))
{
return 1000U;
}
u = (t_ms * 1000UL) / T_ms;
if (u > 1000U)
{
u = 1000U;
}
return PlsrAccelCurveShapePermille(u, mode);
}

static uint32_t PlsrAccelCurveLerp(uint32_t a, uint32_t b, uint32_t ratio_permille)
{
if (ratio_permille >= 1000U)
{
return b;
}
if (b >= a)
{
return a + ((b - a) * ratio_permille) / 1000UL;
}
return a - ((a - b) * ratio_permille) / 1000UL;
}

/**
* 与运行时相同：f(t)=f0+(f1-f0)*t/T（µs），t>=T 时为 f1。
* 与运行时相同：f = f0+(f1-f0)*shape(t/T)，t>=T 时为 f1。
* shape 由 mode 决定；ISR 可用（O(1)）。
*/
static uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0,
uint32_t f1,
uint32_t t_us,
uint32_t T_us)
uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0,
uint32_t f1,
uint32_t t_us,
uint32_t T_us,
PlsrAccelMode_e mode)
{
uint32_t r;

@@ -112,21 +194,18 @@ static uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0,
{
r = 1000U;
}
if (f1 >= f0)
{
return f0 + ((f1 - f0) * r) / 1000UL;
}
return f0 - ((f0 - f1) * r) / 1000UL;
r = PlsrAccelCurveShapePermille(r, mode);
return PlsrAccelCurveLerp(f0, f1, r);
}

/**
* 直线加减速、脉冲起点锁频时的精确脉冲数（与 ISR 累加方式一致）：
* t=0 取 f0，周期 1/f，再取 f(t)……直到 t>=T。
* 不用平均/加权经验公式，改频率后仍与运行时一致。
* 脉冲起点锁频递推脉冲数（与 ISR 一致）；f(t) 按 mode 形状。
* 仅任务/规划上下文；禁止进脉冲 ISR。
*/
static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from,
uint32_t f_to,
uint32_t t_ms)
uint32_t t_ms,
PlsrAccelMode_e mode)
{
uint32_t T_us;
uint32_t elapsed_us;
@@ -146,7 +225,7 @@ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from,

while ((elapsed_us < T_us) && (guard < 2000000UL))
{
f = PlsrAccelCurveFreqOnRampUs(f_from, f_to, elapsed_us, T_us);
f = PlsrAccelCurveFreqOnRampUs(f_from, f_to, elapsed_us, T_us, mode);
if (f < 1U)
{
f = 1U;
@@ -160,14 +239,13 @@ static uint32_t PlsrAccelCurveEstimatePulses(uint32_t f_from,
}

/**
* 拟合斜坡时间 T：使离散直线模型下的脉冲数尽量等于 max_pulses。
* （旧逻辑取「Estimate<=max 的最大 T / 优先 t_prefer」，脉冲有余时会提前到终点再持平，
* 减速末端出现一段平肩/弯曲。）
* 拟合斜坡时间 T（任务上下文）；使离散模型脉冲数尽量等于 max_pulses。
*/
uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from,
uint32_t f_to,
uint32_t max_pulses,
uint32_t t_prefer_ms)
uint32_t t_prefer_ms,
PlsrAccelMode_e mode)
{
uint32_t lo;
uint32_t hi;
@@ -188,10 +266,9 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from,
return (t_prefer_ms > 0U) ? t_prefer_ms : 1U;
}

/* 放大上界直到脉冲数够到 max_pulses（或封顶） */
hi = (t_prefer_ms > 0U) ? t_prefer_ms : 1U;
guard = 0U;
n = PlsrAccelCurveEstimatePulses(f_from, f_to, hi);
n = PlsrAccelCurveEstimatePulses(f_from, f_to, hi, mode);
while ((n < max_pulses) && (hi < 120000U) && (guard < 24U))
{
if (hi > (120000U / 2U))
@@ -202,19 +279,19 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from,
{
hi = hi * 2U;
}
n = PlsrAccelCurveEstimatePulses(f_from, f_to, hi);
n = PlsrAccelCurveEstimatePulses(f_from, f_to, hi, mode);
guard++;
}

lo = 1U;
best = (t_prefer_ms > 0U) ? t_prefer_ms : hi;
n = PlsrAccelCurveEstimatePulses(f_from, f_to, best);
n = PlsrAccelCurveEstimatePulses(f_from, f_to, best, mode);
best_err = (n > max_pulses) ? (n - max_pulses) : (max_pulses - n);

while (lo <= hi)
{
mid = lo + ((hi - lo) / 2UL);
n = PlsrAccelCurveEstimatePulses(f_from, f_to, mid);
n = PlsrAccelCurveEstimatePulses(f_from, f_to, mid, mode);
err = (n > max_pulses) ? (n - max_pulses) : (max_pulses - n);
if ((err < best_err) ||
((err == best_err) && (t_prefer_ms > 0U) &&
@@ -245,104 +322,6 @@ uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from,
return best;
}

static uint32_t PlsrAccelCurveLerp(uint32_t a, uint32_t b, uint32_t ratio_permille)
{
if (ratio_permille >= 1000U)
{
return b;
}
if (b >= a)
{
return a + ((b - a) * ratio_permille) / 1000UL;
}
return a - ((a - b) * ratio_permille) / 1000UL;
}

/** 直线：f 对时间线性 */
static uint32_t PlsrAccelCurveShapeLinear(uint32_t t_ms, uint32_t T_ms)
{
if ((T_ms == 0U) || (t_ms >= T_ms))
{
return 1000U;
}
return (t_ms * 1000UL) / T_ms;
}

///** 正弦缓入缓出 */
//static uint32_t PlsrAccelCurveShapeSine(uint32_t t_ms, uint32_t T_ms)
//{
// uint32_t x;
// uint32_t y;
// uint32_t d;
//
// if ((T_ms == 0U) || (t_ms >= T_ms))
// {
// return 1000U;
// }
// x = (t_ms * 1000UL) / T_ms;
// if (x <= 500UL)
// {
// y = (2UL * x * x) / 1000UL;
// }
// else
// {
// d = 1000UL - x;
// y = 1000UL - (2UL * d * d) / 1000UL;
// }
// if (y > 1000UL)
// {
// y = 1000UL;
// }
// return y;
//}
//
///**
// * 七段一侧 1:2:1：相对直线，头尾更圆、中间更陡
// */
//static uint32_t PlsrAccelCurveShapeS7(uint32_t t_ms, uint32_t T_ms)
//{
// uint32_t u;
// uint32_t d;
// uint32_t p;
//
// if ((T_ms == 0U) || (t_ms >= T_ms))
// {
// return 1000U;
// }
// u = (t_ms * 1000UL) / T_ms;
// if (u <= 250UL)
// {
// p = (8UL * u * u) / (3UL * 1000UL);
// }
// else if (u <= 750UL)
// {
// p = 167UL + (666UL * (u - 250UL)) / 500UL;
// }
// else
// {
// d = 1000UL - u;
// p = 1000UL - (8UL * d * d) / (3UL * 1000UL);
// }
// if (p > 1000UL)
// {
// p = 1000UL;
// }
// return p;
//}

static uint32_t PlsrAccelCurveShape(uint32_t t_ms, uint32_t T_ms, PlsrAccelMode_e mode)
{
// if (mode == PLSR_ACCEL_S)
// {
// return PlsrAccelCurveShapeS7(t_ms, T_ms);
// }
// if (mode == PLSR_ACCEL_SINE)
// {
// return PlsrAccelCurveShapeSine(t_ms, T_ms);
// }
return PlsrAccelCurveShapeLinear(t_ms, T_ms);
}

/** 在 f_cur→f_want 路径上按千分比取点 */
static uint32_t PlsrAccelCurveOnPath(uint32_t f_cur, uint32_t f_want, uint32_t permille)
{
@@ -369,7 +348,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
uint32_t start_spd_ref,
uint32_t end_spd_ref,
uint32_t accel_ms,
uint32_t decel_ms)
uint32_t decel_ms,
PlsrAccelMode_e mode)
{
uint32_t lo = 0U;
uint32_t hi = 1000U;
@@ -392,8 +372,8 @@ static uint32_t PlsrAccelCurveFitPeak(uint32_t total_pulses,
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);
n_d = PlsrAccelCurveEstimatePulses(peak, f_end, t_d);
n_a = PlsrAccelCurveEstimatePulses(f_cur, peak, t_a, mode);
n_d = PlsrAccelCurveEstimatePulses(peak, f_end, t_d, mode);

if ((n_a + n_d) <= total_pulses)
{
@@ -434,6 +414,7 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
uint32_t acc_n;
uint32_t dec_n;
uint32_t f_peak;
PlsrAccelMode_e m;

if (plan == (PlsrAccelPlan_t *)0)
{
@@ -444,9 +425,10 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
f_tgt = PlsrAccelCurveClampFreq(f_tgt);
f_end = PlsrAccelCurveClampFreq(f_end);

m = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode;
plan->f_cur = f_cur;
plan->f_end = f_end;
plan->mode = (mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : mode;
plan->mode = m;

f_peak = f_tgt;
t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
@@ -455,8 +437,8 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc);
dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec);
acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m);
dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m);

if (total_pulses == 0U)
{
@@ -498,15 +480,15 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
f_peak = PlsrAccelCurveFitPeak(total_pulses,
f_cur, f_tgt, f_end,
default_spd, start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
accel_ms, decel_ms, m);
t_acc = PlsrAccelCurveRampTimeMs(f_cur, f_peak, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
t_dec = PlsrAccelCurveRampTimeMs(f_peak, f_end, default_spd,
start_spd_ref, end_spd_ref,
accel_ms, decel_ms);
acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc);
dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec);
acc_n = PlsrAccelCurveEstimatePulses(f_cur, f_peak, t_acc, m);
dec_n = PlsrAccelCurveEstimatePulses(f_peak, f_end, t_dec, m);

if ((acc_n + dec_n) > total_pulses)
{


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

@@ -2,7 +2,7 @@
* @file plsr_accel_curve.h
* @brief 预估加/匀/减脉冲；相内按时间走直线/S/正弦
*
* 相界按预估脉冲划分；预估与运行时一致：时间直线 + 脉冲起点锁频递推。
* 相界按预估脉冲划分；预估与运行时一致：shape(t/T) + 脉冲起点锁频递推。
* 末段脉冲不够：降低峰值做三角。非末段不够：不减速，沿斜率加到脉冲用尽。
*/
#ifndef PLSR_ACCEL_CURVE_H
@@ -31,11 +31,22 @@ typedef enum {

uint32_t PlsrAccelCurveClampFreq(uint32_t freq_hz);

/** 拟合斜坡时间(ms)，使离散直线模型脉冲数尽量等于 max_pulses */
/**
* 斜坡上按 mode 取频（µs 时间轴）。ISR 可用：O(1)。
* f = f0+(f1-f0)*shape(t/T)
*/
uint32_t PlsrAccelCurveFreqOnRampUs(uint32_t f0,
uint32_t f1,
uint32_t t_us,
uint32_t T_us,
PlsrAccelMode_e mode);

/** 拟合斜坡时间(ms)，使离散模型脉冲数尽量等于 max_pulses（任务上下文） */
uint32_t PlsrAccelCurveFitRampTimeMs(uint32_t f_from,
uint32_t f_to,
uint32_t max_pulses,
uint32_t t_prefer_ms);
uint32_t t_prefer_ms,
PlsrAccelMode_e mode);

void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
uint32_t total_pulses,


+ 17
- 34
plsr/run_control/plsr_run_control.c Просмотреть файл

@@ -147,27 +147,6 @@ static void PlsrRunControlApplyOutFreq(uint32_t profile_freq, uint8_t do_start)
}
}

static uint32_t PlsrRunControlLerpUs(uint32_t a, uint32_t b, uint32_t t_us, uint32_t T_us)
{
uint32_t r;

if ((T_us == 0U) || (t_us >= T_us))
{
return b;
}
/* r = t/T * 1000，用 64 位避免加速时间较长时溢出 */
r = (uint32_t)(((uint64_t)t_us * 1000ULL) / (uint64_t)T_us);
if (r > 1000U)
{
r = 1000U;
}
if (b >= a)
{
return a + ((b - a) * r) / 1000UL;
}
return a - ((a - b) * r) / 1000UL;
}

static void PlsrRunControlEnterPhase(RunPhase_e ph, uint32_t anchor_freq)
{
s_phase = ph;
@@ -323,8 +302,8 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
PlsrRunControlEnterDecel(next, remain);
elapsed_us = 0U;
T_us = p->t_dec_ms * 1000UL;
next = PlsrRunControlLerpUs(s_decel_from, p->f_end,
elapsed_us, T_us);
next = PlsrAccelCurveFreqOnRampUs(s_decel_from, p->f_end,
elapsed_us, T_us, p->mode);
}
}
else
@@ -335,8 +314,8 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
t_cmd = elapsed_us +
((1000000UL + (s_cur_freq / 2UL)) / s_cur_freq);
}
next = PlsrRunControlLerpUs(s_approach_from, p->f_tgt,
t_cmd, T_us);
next = PlsrAccelCurveFreqOnRampUs(s_approach_from, p->f_tgt,
t_cmd, T_us, p->mode);
}
}
else if (s_phase == PH_DECEL)
@@ -363,7 +342,8 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
per = (1000000UL + (s_cur_freq / 2UL)) / s_cur_freq;
t_cmd = elapsed_us + per;
}
next = PlsrRunControlLerpUs(s_decel_from, p->f_end, t_cmd, T_us);
next = PlsrAccelCurveFreqOnRampUs(s_decel_from, p->f_end,
t_cmd, T_us, p->mode);
}
}
else
@@ -374,8 +354,8 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
PlsrRunControlEnterDecel(next, remain);
elapsed_us = 0U;
T_us = p->t_dec_ms * 1000UL;
next = PlsrRunControlLerpUs(s_decel_from, p->f_end,
elapsed_us, T_us);
next = PlsrAccelCurveFreqOnRampUs(s_decel_from, p->f_end,
elapsed_us, T_us, p->mode);
}
}

@@ -409,7 +389,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
cfg->end_speed,
cfg->accel_ms,
cfg->decel_ms,
PLSR_ACCEL_LINEAR);
cfg->accel_mode);

/* 本段频率范围锁 PSC，升降过程只改 ARR */
f_lo = s_accel_plan.f_tgt;
@@ -456,7 +436,8 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
PlsrAccelCurveFitRampTimeMs(f_from,
s_accel_plan.f_tgt,
s_accel_plan.acc_n,
s_accel_plan.t_acc_ms);
s_accel_plan.t_acc_ms,
s_accel_plan.mode);
}
if (s_accel_plan.t_acc_ms < 1U)
{
@@ -817,10 +798,12 @@ void PlsrRunControlTickMs(void)
uint32_t T_us = s_accel_plan.t_acc_ms * 1000UL;
uint32_t per = (1000000UL + (s_approach_from / 2UL)) /
s_approach_from;
uint32_t f2 = PlsrRunControlLerpUs(s_approach_from,
s_accel_plan.f_tgt,
per,
T_us);
uint32_t f2 = PlsrAccelCurveFreqOnRampUs(
s_approach_from,
s_accel_plan.f_tgt,
per,
T_us,
s_accel_plan.mode);
if (f2 < 1U)
{
f2 = 1U;


Редактирование Предпросмотр
Загрузка…
Отмена
Сохранить