hanyongwei
/
TrainCamp_hanyongwei_StageTwo
1
0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

修改了直线加减速频率算法，输出频率与脉冲个数都不准了。

dev1
hanyongwei 3 weeks ago
parent
0119fda627
commit
4732a0a3e5
5 changed files with 387 additions and 133 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +267
    -105
      plsr/accel_curve/plsr_accel_curve.c
  2. +19
    -6
      plsr/accel_curve/plsr_accel_curve.h
  3. +2
    -2
      plsr/param/plsr_param.h
  4. +6
    -7
      plsr/pulse_driver/plsr_pulse_driver.c
  5. +93
    -13
      plsr/run_control/plsr_run_control.c

+ 267
- 105
plsr/accel_curve/plsr_accel_curve.c View File

@@ -1,6 +1,6 @@
/**
* @file plsr_accel_curve.c
* @brief 脉冲域规划/取频：加速 f^2=start^2+2an；减速 f^2=end^2+2ak(k=N…1)；S/正弦时间轴
* @brief 脉冲域规划/取频：直线邻项平均；S/正弦时间轴查表
*
* ============================================================================
* 【阅读指南 — 先看这个再往下翻 if/else】
@@ -20,8 +20,10 @@
* decel_rate_hz_per_s 减速斜率 Hz/s ≈ default_speed*1000/deceleration_time_ms
*
* 二、三种曲线模式（curve_mode）
* LINEAR 加速：f=sqrt(start^2+2a·n)；减速：从终点往峰值加
* f=sqrt(end^2+2a·k)，k=N…1（与加速对称，勿从峰值减计数）
* LINEAR S_x=sqrt(f0^2+2a·x)；第 k 拍命令频=(S_{k-1}+S_k)/2；末拍钉相终点
* N=floor(|ft^2-f0^2|/(2a))（非零斜坡且算得 0 → 1）
* 减速以终点为 f0 往峰值建 S_N…S_0，首减拍 (S_N+S_{N-1})/2
* ARPE：冷启动须 Prime/NextFrequency 一次再 Start，避免首频写两遍
* S 时间域 jerk 梯形；规划/ISR 用离散 Δt=1/frequency_hz 仿真；ISR 查预建表
* SINE raised-cosine 时间剖面；同上，查预建表
*
@@ -39,7 +41,7 @@
*
* 五、FreqAtPulse 的 completed_segment_pulses 含义（易混）
* completed_segment_pulses = 已完成脉冲数（不是「当前第几拍」）
* done=0 → 第 1 拍频率 = start_frequency_hz；done=1 → 第 2 拍 ≈ sqrt(start_frequency_hz^2+2a) …
* done=0 → 第 1 拍命令频 = (S_0+S_1)/2；done=k → 第 k+1 拍；末拍钉终点
*
* @details 模块职责
* 实现 plsr_accel_curve.h 全部 API。核心路径：
@@ -74,25 +76,39 @@ static uint32_t PlsrAccelCurveAbsoluteDifference(uint32_t first_value, uint32_t
(first_value - second_value) : (second_value - first_value);
}

/** 64 位整数平方根（牛顿法），支持 frequency_hz^2 + 2an */
/** 64 位整数平方根（牛顿法）。初值必须 ≥ 真根，否则一步上冲会误停。 */
static uint32_t PlsrAccelCurveIntegerSquareRoot(uint64_t squared_frequency)
{
uint64_t estimate;
uint64_t next_estimate;
uint32_t bit;
uint32_t loop_index;

if (squared_frequency <= 1ULL)
{
return (uint32_t)squared_frequency;
}
estimate = squared_frequency;
if (estimate > 100000ULL)

/* 按最高位置初值：略大于 √x，保证从上方收敛（过低会一步上冲误停） */
bit = 0U;
{
uint64_t temp = squared_frequency;

while (temp > 1ULL)
{
temp >>= 1;
bit++;
}
}
estimate = 1ULL << ((bit + 2U) / 2U);
if (estimate < 1ULL)
{
estimate = 100000ULL;
estimate = 1ULL;
}
for (loop_index = 0U; loop_index < 40U; loop_index++)

for (loop_index = 0U; loop_index < 48U; loop_index++)
{
next_estimate = (estimate + squared_frequency / estimate) / 2ULL;
next_estimate = (estimate + (squared_frequency / estimate)) / 2ULL;
if (next_estimate >= estimate)
{
break;
@@ -116,7 +132,7 @@ static uint32_t PlsrAccelCurveRateFromDefaultSpeed(uint32_t default_speed_hz, ui
{
return 0U;
}
return (default_speed_hz * 1000UL) / ramp_time_ms;
return (uint32_t)(((uint64_t)default_speed_hz * 1000ULL) / (uint64_t)ramp_time_ms);
}

/** 按默认速度计算变化率；默认速度未配置时用本段频差计算。 */
@@ -166,14 +182,14 @@ static uint32_t PlsrAccelCurveCalculateJumpFrequency(uint32_t starting_frequency
}

/**
* 解析配置起速/止速 → 本段实际入口或出口频率。
* 解析配置起速/止速 → 本段数学入口或出口频率 f0 / f_end。
*
* 三分支（按顺序判断，命中即 return）：
* ① configured_frequency_hz > target_frequency_hz → 直接用 configured_frequency_hz（例如起速 500、目标 200：先高位再减）
* ② target_frequency_hz < jump_frequency_hz → 用 target_frequency_hz（目标比起跳还低，没必要从 jump_frequency_hz 起）
* ③ 否则 → 用 jump_frequency_hz = sqrt(configured_frequency_hz^2 + 2a)
* 直线邻项平均：S_x=√(f0²+2ax)，首拍 (f0+S1)/2；起速=0 时 f0=0、S1=√(2a)，
* 不再把「起跳 √(f0²+2a)」当作段入口（否则首频变成 (S1+S2)/2≈170）。
*
* resolving_start=1 用 acceleration_time_ms 算 a；resolving_start=0 用 deceleration_time_ms（ResolveEndHz 止速侧）。
* ① configured > target → 用 configured（高位起再降）
* ② a=0 且 configured=0 → 用 target（阶跃）
* ③ 否则 → 用 configured（可为 0）
*/
static uint32_t PlsrAccelCurveResolveBoundaryFrequency(uint32_t configured_frequency_hz,
uint32_t target_frequency_hz,
@@ -184,7 +200,6 @@ static uint32_t PlsrAccelCurveResolveBoundaryFrequency(uint32_t configured_frequ
{
uint32_t ramp_time_ms;
uint32_t acceleration_hz_per_s;
uint32_t jump_frequency_hz;

configured_frequency_hz = PlsrAccelCurveClampFrequencyHz(configured_frequency_hz);
target_frequency_hz = PlsrAccelCurveClampFrequencyHz(target_frequency_hz);
@@ -210,14 +225,7 @@ static uint32_t PlsrAccelCurveResolveBoundaryFrequency(uint32_t configured_frequ
return (configured_frequency_hz >= 1U) ? configured_frequency_hz : target_frequency_hz;
}

jump_frequency_hz = PlsrAccelCurveCalculateJumpFrequency(configured_frequency_hz, acceleration_hz_per_s);

/* 目标 < 起跳 → 用目标；目标 >= 起跳 → 用起跳 */
if (target_frequency_hz < jump_frequency_hz)
{
return target_frequency_hz;
}
return jump_frequency_hz;
return configured_frequency_hz;
}

uint32_t PlsrAccelCurveResolveStartHz(uint32_t configured_frequency_hz,
@@ -297,8 +305,8 @@ static uint32_t PlsrAccelCurveInterpolate(uint32_t start_value,
}

/**
* 脉冲闭合：N = ceil(|end_frequency_hz^2 - start_frequency_hz^2| / (2a))。
* a=0 且频差非 0 → 0（阶跃，无斜坡脉冲）。
* 脉冲闭合：N = floor(|end_frequency_hz^2 - start_frequency_hz^2| / (2a))。
* 非零斜坡且算得 0 → 1。a=0 且频差非 0 → 0（阶跃，无斜坡脉冲）。
*/
static uint32_t PlsrAccelCurveCalculateLinearRampPulses(uint32_t start_frequency_hz, uint32_t end_frequency_hz,
uint32_t acceleration_hz_per_s)
@@ -320,9 +328,11 @@ static uint32_t PlsrAccelCurveCalculateLinearRampPulses(uint32_t start_frequency

start_frequency_squared = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz;
end_frequency_squared = (uint64_t)end_frequency_hz * (uint64_t)end_frequency_hz;
squared_frequency_difference = (end_frequency_squared > start_frequency_squared) ? (end_frequency_squared - start_frequency_squared) : (start_frequency_squared - end_frequency_squared);
squared_frequency_difference = (end_frequency_squared > start_frequency_squared) ?
(end_frequency_squared - start_frequency_squared) :
(start_frequency_squared - end_frequency_squared);
twice_acceleration = 2ULL * (uint64_t)acceleration_hz_per_s;
pulse_count = (squared_frequency_difference + twice_acceleration - 1ULL) / twice_acceleration;
pulse_count = squared_frequency_difference / twice_acceleration;
if (pulse_count == 0ULL)
{
pulse_count = 1ULL;
@@ -559,11 +569,11 @@ static uint32_t PlsrAccelCurveCalculateRampPulses(uint32_t start_frequency_hz, u
}

/**
* 直线取频。
* 加速(rising)：f = sqrt(start^2 + 2·a·pulse_count)，不超过 target_limit
* 减速(!rising)：从终点往峰值加 — pulse_count 为相内已完成步数(0…N-1)时
* k = N - pulse_count，f = sqrt(end_limit^2 + 2·a·k)，不超过 start(峰值)
* 例 N=5：第 1 个减速拍 k=5→end^2+10a；末拍 k=1→end^2+2a
* 直线邻项平均取频（任务侧预览）。
* pulse_count = 相内已完成脉冲数 → 返回第 (pulse_count+1) 拍命令频。
* rising：S_x=sqrt(start^2+2a·x)，S_0=start；第 k 拍 (S_{k-1}+S_k)/2
* 末拍：S_N 取相终点 target_limit，(S_{N-1}+target_limit)/2（不钉死终点）
* 减速：S_m=sqrt(end^2+2a·m)；末拍 (S_1+end)/2
*/
static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz, uint32_t acceleration_hz_per_s,
uint32_t pulse_count, uint8_t frequency_rising,
@@ -571,8 +581,11 @@ static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz
uint32_t phase_total_pulses)
{
uint64_t squared_frequency;
uint32_t s_prev;
uint32_t s_next;
uint32_t frequency_hz;
uint32_t k_from_end;
uint32_t k_hi;
uint32_t k_lo;

if (acceleration_hz_per_s == 0U)
{
@@ -580,46 +593,86 @@ static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz
return PlsrAccelCurveClampFrequencyHz((frequency_hz < 1U) ? 1U : frequency_hz);
}

if (phase_total_pulses < 1U)
{
frequency_hz = target_limit_hz;
if (frequency_hz < 1U)
{
frequency_hz = 1U;
}
return PlsrAccelCurveClampFrequencyHz(frequency_hz);
}

if (frequency_rising != 0U)
{
if (pulse_count + 1U >= phase_total_pulses)
{
/* 升频末拍：直接相终点（进匀速后锁定，不再平均） */
frequency_hz = target_limit_hz;
if (frequency_hz < 1U)
{
frequency_hz = 1U;
}
return PlsrAccelCurveClampFrequencyHz(frequency_hz);
}
if (pulse_count == 0U)
{
frequency_hz = start_frequency_hz;
s_prev = start_frequency_hz;
}
else
{
squared_frequency = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz
+ (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)pulse_count);
frequency_hz = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
if (frequency_hz > target_limit_hz)
{
frequency_hz = target_limit_hz;
}
s_prev = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
}
squared_frequency = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz
+ (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)(pulse_count + 1U));
s_next = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
frequency_hz = (s_prev / 2U) + (s_next / 2U) + ((s_prev & 1U) + (s_next & 1U)) / 2U;
}
else
{
/* 减速：以终点 target_limit_hz 为 f0，k 从 N 倒数到 1 */
if (phase_total_pulses < 1U)
{
frequency_hz = target_limit_hz;
}
else if (pulse_count >= phase_total_pulses)
if (pulse_count + 1U >= phase_total_pulses)
{
frequency_hz = target_limit_hz;
/* 末拍：(S_1 + end)/2 */
squared_frequency = (uint64_t)target_limit_hz * (uint64_t)target_limit_hz
+ (2ULL * (uint64_t)acceleration_hz_per_s);
s_prev = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
if (s_prev > start_frequency_hz)
{
s_prev = start_frequency_hz;
}
s_next = target_limit_hz;
}
else
{
k_from_end = phase_total_pulses - pulse_count;
k_hi = phase_total_pulses - pulse_count;
k_lo = k_hi - 1U;
squared_frequency = (uint64_t)target_limit_hz * (uint64_t)target_limit_hz
+ (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)k_from_end);
frequency_hz = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
if (frequency_hz > start_frequency_hz)
+ (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)k_hi);
s_prev = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
if (s_prev > start_frequency_hz)
{
s_prev = start_frequency_hz;
}
if (k_lo == 0U)
{
s_next = target_limit_hz;
}
else
{
frequency_hz = start_frequency_hz;
squared_frequency = (uint64_t)target_limit_hz * (uint64_t)target_limit_hz
+ (2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)k_lo);
s_next = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
if (s_next > start_frequency_hz)
{
s_next = start_frequency_hz;
}
}
}
frequency_hz = (s_prev / 2U) + (s_next / 2U) + ((s_prev & 1U) + (s_next & 1U)) / 2U;
}

if (frequency_hz < 1U)
{
frequency_hz = 1U;
@@ -824,17 +877,6 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,

selected_mode = (curve_mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : curve_mode;

if (start_frequency_hz < 1U)
{
start_frequency_hz = PlsrAccelCurveResolveStartHz(0U, target_frequency_hz, default_speed_hz,
acceleration_time_ms, deceleration_time_ms);
}
if (end_frequency_hz < 1U)
{
end_frequency_hz = PlsrAccelCurveResolveEndHz(0U, target_frequency_hz, default_speed_hz,
acceleration_time_ms, deceleration_time_ms);
}

accel_rate_hz_per_s =
PlsrAccelCurveCalculateRate(default_speed_hz,
start_frequency_hz,
@@ -846,6 +888,35 @@ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan,
end_frequency_hz,
deceleration_time_ms);

/*
* 直线：允许 f0/f_end=0（邻项平均首末拍用）。
* S/正弦：时间仿真不能从 0 起，入口/出口 <1 时用起跳/落地 √(2a)。
*/
if (start_frequency_hz < 1U)
{
if (selected_mode == PLSR_ACCEL_LINEAR)
{
start_frequency_hz = 0U;
}
else
{
start_frequency_hz =
PlsrAccelCurveCalculateJumpFrequency(0U, accel_rate_hz_per_s);
}
}
if (end_frequency_hz < 1U)
{
if (selected_mode == PLSR_ACCEL_LINEAR)
{
end_frequency_hz = 0U;
}
else
{
end_frequency_hz =
PlsrAccelCurveCalculateJumpFrequency(0U, decel_rate_hz_per_s);
}
}

peak_frequency_hz = target_frequency_hz;

plan->start_frequency_hz = start_frequency_hz;
@@ -1006,7 +1077,8 @@ static uint32_t PlsrAccelCurveTimedFrequencyAtPulse(
* completed_segment_pulses ∈ [accel_pulses+constant_pulses, …) → 减速相（target_frequency_hz → end_frequency_hz）
*
* 注意 completed_segment_pulses 是「已经发完的个数」：
* Start 后第 1 拍进行中时 done 仍为 0，FreqAtPulse(0)=start_frequency_hz。
* Start 后第 1 拍进行中时 done 仍为 0，直线 FreqAtPulse(0)=(S0+S1)/2。
* ARPE 冷启动勿只靠本函数：须 Prime 把 runtime.completed 推到 1，否则 ISR 会再写一遍首频。
*/
uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
uint32_t completed_segment_pulses)
@@ -1148,6 +1220,9 @@ static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime,
uint32_t decel_rate_hz_per_s,
PlsrAccelMode_e curve_mode)
{
uint64_t squared_frequency;
uint32_t s_n;

runtime->start_frequency_hz = start_frequency_hz;
runtime->end_frequency_hz = end_frequency_hz;
runtime->current_frequency_hz = start_frequency_hz;
@@ -1164,6 +1239,26 @@ static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime,
runtime->frequency_table_id = 0U;
runtime->table_length = 0U;
runtime->table_stride = 1U;
/* 直线：升频 prev=S0=start；降频 prev=S_N（从终点往峰值建） */
runtime->linear_prev_s_hz = start_frequency_hz;
if ((curve_mode == PLSR_ACCEL_LINEAR) &&
(runtime->frequency_rising == 0U) &&
(runtime->acceleration_hz_per_s > 0U) &&
(total_pulses > 0U))
{
squared_frequency = (uint64_t)end_frequency_hz * (uint64_t)end_frequency_hz
+ (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)total_pulses);
s_n = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
if (s_n > start_frequency_hz)
{
s_n = start_frequency_hz;
}
if (s_n < 1U)
{
s_n = 1U;
}
runtime->linear_prev_s_hz = s_n;
}
PlsrAccelRuntimeCalculateTimes(runtime);
}

@@ -1416,59 +1511,78 @@ void PlsrAccelBeginConstantSpeed(PlsrAccelRuntime_t *runtime,
runtime->jerk_time_us = 0U;
runtime->constant_accel_time_us = 0U;
runtime->ramp_time_us = 0U;
runtime->linear_prev_s_hz = 0U;
}

/**
* 直线 ISR 取频（开方，与规划公式一致）。
* 加速：f = sqrt(start^2 + 2·a·completed)
* 减速：k = total-completed+1，f = sqrt(end^2 + 2·a·k)（从终点往峰值加）
* 直线 ISR 取频：邻项平均。
* S_x=√(f0²+2a·x) 是瞬时速度，不是命令频率；命令频=(S_{k-1}+S_k)/2。
* linear_prev_s_hz 只存上一拍的 S（开方值），绝不能存输出频率。
* 升频末拍直接钉相终点（进匀速锁定）；减速末拍仍 (S1+end)/2。
*/
static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime)
{
uint64_t target_frequency_squared;
uint64_t squared_frequency;
uint32_t s_new;
uint32_t frequency_hz;
uint32_t k_from_end;
uint32_t k_lo;

if (runtime->acceleration_hz_per_s == 0U)
{
return PlsrAccelCurveClampFrequencyHz(runtime->end_frequency_hz);
return PlsrAccelCurveClampFrequencyHz(
(runtime->end_frequency_hz < 1U) ? 1U : runtime->end_frequency_hz);
}

/* 升频相结束：锁定峰值，不再开方/平均 */
if ((runtime->total_pulses > 0U) &&
(runtime->completed_pulses >= runtime->total_pulses) &&
(runtime->frequency_rising != 0U))
{
return PlsrAccelCurveClampFrequencyHz(
(runtime->end_frequency_hz < 1U) ? 1U : runtime->end_frequency_hz);
}

if (runtime->frequency_rising != 0U)
if ((runtime->total_pulses > 0U) &&
(runtime->completed_pulses >= runtime->total_pulses))
{
/* 减速末拍：与相终点平均 */
s_new = runtime->end_frequency_hz;
}
else if (runtime->frequency_rising != 0U)
{
target_frequency_squared =
squared_frequency =
(uint64_t)runtime->start_frequency_hz * (uint64_t)runtime->start_frequency_hz
+ (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)runtime->completed_pulses);
frequency_hz = PlsrAccelCurveIntegerSquareRoot(target_frequency_squared);
if (frequency_hz > runtime->end_frequency_hz)
s_new = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
if (s_new > runtime->end_frequency_hz)
{
frequency_hz = runtime->end_frequency_hz;
s_new = runtime->end_frequency_hz;
}
}
else
{
if (runtime->total_pulses < 1U)
k_lo = runtime->total_pulses - runtime->completed_pulses;
if (k_lo == 0U)
{
k_from_end = 1U;
}
else if (runtime->completed_pulses >= runtime->total_pulses)
{
k_from_end = 1U;
s_new = runtime->end_frequency_hz;
}
else
{
k_from_end = runtime->total_pulses - runtime->completed_pulses + 1U;
}
target_frequency_squared =
(uint64_t)runtime->end_frequency_hz * (uint64_t)runtime->end_frequency_hz
+ (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)k_from_end);
frequency_hz = PlsrAccelCurveIntegerSquareRoot(target_frequency_squared);
if (frequency_hz > runtime->start_frequency_hz)
{
frequency_hz = runtime->start_frequency_hz;
squared_frequency =
(uint64_t)runtime->end_frequency_hz * (uint64_t)runtime->end_frequency_hz
+ (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)k_lo);
s_new = PlsrAccelCurveIntegerSquareRoot(squared_frequency);
if (s_new > runtime->start_frequency_hz)
{
s_new = runtime->start_frequency_hz;
}
}
}

frequency_hz = (runtime->linear_prev_s_hz / 2U) + (s_new / 2U)
+ ((runtime->linear_prev_s_hz & 1U) + (s_new & 1U)) / 2U;
runtime->linear_prev_s_hz = s_new;

if (frequency_hz < 1U)
{
frequency_hz = 1U;
@@ -1476,6 +1590,50 @@ static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime)
return PlsrAccelCurveClampFrequencyHz(frequency_hz);
}

uint32_t PlsrAccelCurveLinearMinCommandHz(uint32_t boundary_frequency_hz,
uint32_t rate_hz_per_s)
{
uint32_t s1;
uint32_t min_hz;

if (boundary_frequency_hz >= 1U)
{
return PlsrAccelCurveClampFrequencyHz(boundary_frequency_hz);
}
if (rate_hz_per_s < 1U)
{
return 1U;
}
s1 = PlsrAccelCurveCalculateJumpFrequency(0U, rate_hz_per_s);
min_hz = s1 / 2U;
if (min_hz < 1U)
{
min_hz = 1U;
}
return PlsrAccelCurveClampFrequencyHz(min_hz);
}

/**
* 直线冷启动：推进一拍得到第 1 拍命令频（ARPE Start 用），runtime.completed=1。
*/
uint32_t PlsrAccelPrimeLinearStartFrequency(PlsrAccelRuntime_t *runtime)
{
if (runtime == (PlsrAccelRuntime_t *)0)
{
return 0U;
}
if (runtime->curve_mode != PLSR_ACCEL_LINEAR)
{
return PlsrAccelCurveClampFrequencyHz(runtime->current_frequency_hz);
}
if (runtime->is_active == 0U)
{
return PlsrAccelCurveClampFrequencyHz(
(runtime->current_frequency_hz < 1U) ? 1U : runtime->current_frequency_hz);
}
return PlsrAccelNextFrequency(runtime);
}

/**
* S/正弦时间域步进：上一拍周期推进 t，再按剖面取下一拍频率。
*/
@@ -1569,11 +1727,12 @@ static uint32_t PlsrAccelCurveSimulateRampPulses(uint32_t start_frequency_hz, ui
* 调用时机：每个 PWM UPDATE 中断里，在本段已发脉冲数加一之后。
*
* 模式分支：
* LINEAR → SquareStep：用 frequency_hz±1 逼近 sqrt(start_frequency_hz^2±2an)，不全量开方
* LINEAR → 邻项平均 (S_{k-1}+S_k)/2；prev 存 S 非输出频；末拍与相终点平均
* S/SINE+表 → 查 s_acceleration_frequency_table / s_deceleration_frequency_table（Prebuild 填好），table_index=(pulse_count-1)/table_stride
* S/SINE无表 → 兜底 TimeStep（不应出现在正常路径）
*
* pulse_count 含义：本相已 Step 次数；BeginAcc 后第 1 次 Step 时 pulse_count 变为 1，对应「第 2 拍」频率。
* pulse_count 含义：本相已 Step 次数；Begin 后第 1 次 Step 时 pulse_count=1，对应第 1 拍命令频。
* ARPE 冷启动须先 Step/Prime 再 Start，使首 UPDATE 写第 2 拍。
*/
uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime)
{
@@ -1637,11 +1796,10 @@ uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime)
if ((runtime->total_pulses > 0U) && (runtime->completed_pulses >= runtime->total_pulses))
{
/*
* 直线减速末步已是 sqrt(end^2+2a)，不要再钉成 end（否则与「从终点往上加」不一致）。
* S/正弦 / 加速相：仍钉到相终点。
* 直线：末拍已是 (prev+end)/2，勿再钉 end。
* S/正弦：仍钉到相终点。
*/
if (!((runtime->curve_mode == PLSR_ACCEL_LINEAR) &&
(runtime->frequency_rising == 0U)))
if (runtime->curve_mode != PLSR_ACCEL_LINEAR)
{
runtime->current_frequency_hz = runtime->end_frequency_hz;
}
@@ -1654,19 +1812,23 @@ uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime)
runtime->current_frequency_hz = runtime->end_frequency_hz;
runtime->is_active = 0U;
}
else if (runtime->frequency_rising != 0U)
else if (runtime->curve_mode != PLSR_ACCEL_LINEAR)
{
if (runtime->current_frequency_hz >= runtime->end_frequency_hz)
/* 时间剖面：到频即收；直线只靠 total_pulses 末拍钉终点 */
if (runtime->frequency_rising != 0U)
{
if (runtime->current_frequency_hz >= runtime->end_frequency_hz)
{
runtime->current_frequency_hz = runtime->end_frequency_hz;
runtime->is_active = 0U;
}
}
else if (runtime->current_frequency_hz <= runtime->end_frequency_hz)
{
runtime->current_frequency_hz = runtime->end_frequency_hz;
runtime->is_active = 0U;
}
}
else if (runtime->current_frequency_hz <= runtime->end_frequency_hz)
{
runtime->current_frequency_hz = runtime->end_frequency_hz;
runtime->is_active = 0U;
}
}
return PlsrAccelCurveClampFrequencyHz(runtime->current_frequency_hz);
}

+ 19
- 6
plsr/accel_curve/plsr_accel_curve.h View File

@@ -1,13 +1,14 @@
/**
* @file plsr_accel_curve.h
* @brief 脉冲域加/匀/减规划与取频（运动学：v^2 - v0^2 = 2ax）
* @brief 脉冲域加/匀/减规划与取频
*
* @details 按起/峰/止频率、斜率时间与总脉冲规划三相脉冲预算，并在脉冲序号上取频。
* 不读 wait_type，不碰 TIM/GPIO。
*
* 直线：下一频率由起点频率、变化率和已完成脉冲数计算（ISR 增量逼近）。
* 直线：S_x=sqrt(f0^2+2ax)；第 k 拍输出 (S_{k-1}+S_k)/2（S_0=f0）；
* 脉冲数 N=floor(|ft^2-f0^2|/(2a))；升频末拍钉峰值后进匀速锁定；减速末拍 (S1+end)/2。
* 起速/止速配置值即为 f0/f_end（可为 0）；√(f0^2+2a) 是 S1 不是入口。
* S/正弦：开相时预计算频率表；ISR 只按脉冲序号查表（匀速相不改频）。
* 规划脉冲数与建表使用同一套离散逐拍仿真。
*
* 术语（全模块统一）：
* 字段使用完整名称和单位，例如 accel_pulses、start_frequency_hz，
@@ -69,8 +70,7 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,

/**
* 单相逐脉冲运行态（ISR 热路径，每来一个 UPDATE 调 Step 一次）
* 直线：每次按目标平方值逐步逼近下一频率
* S/正弦：查预计算频率表（开相 BeginAcc/BeginDec 时填好）
* 直线：邻项平均 (S_{k-1}+S_k)/2，复用上一拍 S；S/正弦：查预建表
*/
typedef struct {
uint32_t current_frequency_hz; /* 当前输出频率 */
@@ -79,6 +79,7 @@ typedef struct {
uint32_t acceleration_hz_per_s; /* 当前相使用的变化率 */
uint32_t completed_pulses; /* 当前相已经步进的脉冲数 */
uint32_t total_pulses; /* 当前相计划步进的脉冲数 */
uint32_t linear_prev_s_hz; /* 直线：上一拍 S（瞬时 sqrt） */
uint32_t elapsed_time_us; /* 时间曲线已经经过的时间 */
uint32_t jerk_time_us; /* S 曲线单个 jerk 阶段时间 */
uint32_t constant_accel_time_us; /* S 曲线恒加速阶段时间 */
@@ -105,7 +106,19 @@ void PlsrAccelBeginDeceleration(PlsrAccelRuntime_t *runtime,
void PlsrAccelBeginConstantSpeed(PlsrAccelRuntime_t *runtime,
const PlsrAccelPlan_t *plan);

/** 本相前进 1 脉冲，返回下一拍命令频率 */
/**
* 直线加速冷启动：算出第 1 拍平均频率并预装 S1（ARPE 下 Start 用）。
* 之后 ISR NextFrequency 从第 2 拍起写影子。
*/
uint32_t PlsrAccelPrimeLinearStartFrequency(PlsrAccelRuntime_t *runtime);

/**
* 直线相边界为 0 时，实际最低命令频 ≈ (0+√(2a))/2（用于 PSC 锁定，勿用 1Hz）。
*/
uint32_t PlsrAccelCurveLinearMinCommandHz(uint32_t boundary_frequency_hz,
uint32_t rate_hz_per_s);

/** 本相前进 1 脉冲，返回下一拍命令频率（供 ARPE 写入下一周期） */
uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime);

#endif

+ 2
- 2
plsr/param/plsr_param.h View File

@@ -115,7 +115,7 @@ typedef enum {

/** 加减速曲线形状（脉冲域规划） */
typedef enum {
PLSR_ACCEL_LINEAR = 0, /* f^2 = f0^2 ± 2an */
PLSR_ACCEL_LINEAR = 0, /* S_x=√(f0²±2ax)；命令频=(S_{k-1}+S_k)/2，末拍钉终点 */
PLSR_ACCEL_S = 1, /* 时间域七段 S：加速度梯形 1:2:1 */
PLSR_ACCEL_SINE = 2 /* 时间域 raised-cosine */
} PlsrAccelMode_e;
@@ -161,7 +161,7 @@ typedef struct {
uint16_t seg_count; /* 有效段数 0~PLSR_SEG_MAX */
uint16_t start_seg; /* 1-based */
uint32_t default_speed; /* Hz；段频为 0 时作段目标；并定斜率 */
uint32_t start_speed; /* 起速；0=运行层 ResolveStartHz 起跳 */
uint32_t start_speed; /* 起速；0=直线 f0=0（首拍 (0+√2a)/2） */
uint32_t end_speed; /* 止速；0=ResolveEndHz 落地后停表 */
uint16_t accel_ms;
uint16_t decel_ms;


+ 6
- 7
plsr/pulse_driver/plsr_pulse_driver.c View File

@@ -650,14 +650,18 @@ static void PlsrPulseDriverCommitOutput(void)
htim->Instance->CCER |= TIM_CCER_CC1E;

/*
* 先打开预装载并把影子钉成同一频率，再 ENABLE。
* 否则首拍 ISR 的 SetFreqIsr 可能落在 ARPE=0 窗口，直接改写工作 ARR。
* 先打开预装载并把影子钉成同一频率；保护标志须在 ENABLE 前就绪，
* 否则 CNT=ARR 触发的首拍 UPDATE 可能早于 protect=1。
*/
htim->Instance->CR1 |= TIM_CR1_ARPE;
htim->Instance->CCMR1 |= TIM_CCMR1_OC1PE;
htim->Instance->ARR = s_last_arr;
htim->Instance->CCR1 = ccr;

s_protect_arr = s_last_arr;
s_protect_ccr = ccr;
s_protect_start_period = 1U;

/* CNT 已在切换 PWM 模式前归位，下面仅清状态并开表。 */
__HAL_TIM_CLEAR_FLAG(htim, TIM_FLAG_UPDATE);
__HAL_TIM_CLEAR_IT(htim, TIM_IT_UPDATE);
@@ -665,11 +669,6 @@ static void PlsrPulseDriverCommitOutput(void)
__HAL_TIM_ENABLE(htim);
g_plsr_pwm_running = 1U;

/* 记录起跳工作寄存器，供首拍 ISR 保护当前周期 */
s_protect_arr = s_last_arr;
s_protect_ccr = ccr;
s_protect_start_period = 1U;

s_regs_prepared = 0U;
}



+ 93
- 13
plsr/run_control/plsr_run_control.c View File

@@ -678,8 +678,7 @@ static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t
else
{
/*
* 停表后冷启动：入口由 ResolveStartHz 按起速/目标/起跳三分支决定
* （起速>目标→起速减速；否则与起跳比较取目标或起跳）。
* 停表后冷启动：入口由 ResolveStartHz 给出数学 f0（起速可为 0，不再用起跳当入口）。
*/
endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(g_plsr_config.start_speed,
f_tgt,
@@ -954,7 +953,7 @@ static void PlsrRunControlOnApproachDone(void)
}

/**
* 进匀速：写一次 f_tgt，之后 ISR 不改 ARR（复用缓存）。
* 进匀速：写一次 f_tgt 锁定，之后 ISR 不改 ARR（不再开方/平均）。
*/
static void PlsrRunControlEnterConstHold(uint8_t do_start)
{
@@ -995,8 +994,9 @@ static void PlsrRunControlEnterConstHoldIsr(void)
* 1) CONST 且 remain 进减速窗 → EnterDecel，return 前不 Start
* 2) CONST → 仅 do_start 时 Start(f_tgt)，return
* 3) DECEL 且刚开段(n=0) → PrimeDecelOnStart（Start(f1)，其余交 ISR）
* 4) APPROACH → FreqAtPulse(done_n)；若已到 f_tgt 则进 CONST 或 DECEL
* 5) 其它 → ApplyOutFreq(next, do_start)
* 4) APPROACH 直线冷启动 → PrimeLinearStartFrequency + Start（ARPE 防首频双写）
* 5) APPROACH → FreqAtPulse(done_n)；若已到 f_tgt 则进 CONST 或 DECEL
* 6) 其它 → ApplyOutFreq(next, do_start)
*/
static void PlsrRunControlRefreshProfile(uint8_t do_start)
{
@@ -1037,6 +1037,46 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
return;
}

/*
* 直线加速冷启动（ARPE）：先 Prime 写出第 1 拍平均频并 completed=1，
* 再 Start；否则 FreqAtPulse(0) 与首 UPDATE 会各写一遍首频。
*/
if ((s_phase == PH_APPROACH) &&
(do_start != 0U) &&
(s_segment_pulses_done == 0) &&
(g_plsr_accel_runtime.completed_pulses == 0U) &&
(g_plsr_accel_plan.curve_mode == PLSR_ACCEL_LINEAR) &&
(g_plsr_accel_plan.accel_pulses > 0U) &&
(g_plsr_accel_plan.start_frequency_hz != g_plsr_accel_plan.target_frequency_hz))
{
next = PlsrAccelPrimeLinearStartFrequency(&g_plsr_accel_runtime);
if (next < 1U)
{
next = 1U;
}
if ((g_plsr_accel_plan.accel_pulses == 0U) ||
(g_plsr_accel_runtime.is_active == 0U) ||
(g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_plan.accel_pulses) ||
((g_plsr_accel_plan.target_frequency_hz >= g_plsr_accel_plan.start_frequency_hz) &&
(next >= g_plsr_accel_plan.target_frequency_hz)) ||
((g_plsr_accel_plan.target_frequency_hz < g_plsr_accel_plan.start_frequency_hz) &&
(next <= g_plsr_accel_plan.target_frequency_hz)))
{
PlsrRunControlOnApproachDone();
if (PlsrRunControlShouldEnterDecel(remain) != 0U)
{
PlsrRunControlApplyOutFreq(next, do_start);
PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz,
PlsrRunControlDecelEnterBudget(remain));
return;
}
PlsrRunControlEnterConstHold(do_start);
return;
}
PlsrRunControlApplyOutFreq(next, do_start);
return;
}

next = PlsrAccelCurveFreqAtPulse(&g_plsr_accel_plan, done_n);
if (next < 1U)
{
@@ -1061,11 +1101,14 @@ static void PlsrRunControlRefreshProfile(uint8_t do_start)
((g_plsr_accel_plan.target_frequency_hz < g_plsr_accel_plan.start_frequency_hz) &&
(next <= g_plsr_accel_plan.target_frequency_hz)))
{
next = g_plsr_accel_plan.target_frequency_hz;
PlsrRunControlOnApproachDone();
if (PlsrRunControlShouldEnterDecel(remain) != 0U)
{
PlsrRunControlEnterDecel(next,
if (g_plsr_accel_plan.curve_mode != PLSR_ACCEL_LINEAR)
{
next = g_plsr_accel_plan.target_frequency_hz;
}
PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz,
PlsrRunControlDecelEnterBudget(remain));
}
else
@@ -1151,6 +1194,20 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
f_lo = f_start;
}
}
else if (g_plsr_accel_plan.curve_mode == PLSR_ACCEL_LINEAR)
{
/*
* f0=0 时首拍命令频≈(0+√2a)/2，不是 1Hz。
* 若用 1Hz~100k 锁 PSC，跨度锁不住 → ISR 换 PSC 挂起 → 停表重开多计边沿。
*/
uint32_t min_cmd = PlsrAccelCurveLinearMinCommandHz(
0U, g_plsr_accel_plan.accel_rate_hz_per_s);

if (min_cmd < f_lo)
{
f_lo = min_cmd;
}
}
else
{
f_lo = 1U;
@@ -1166,6 +1223,16 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from,
f_lo = g_plsr_accel_plan.end_frequency_hz;
}
}
else if (g_plsr_accel_plan.curve_mode == PLSR_ACCEL_LINEAR)
{
uint32_t min_cmd = PlsrAccelCurveLinearMinCommandHz(
0U, g_plsr_accel_plan.decel_rate_hz_per_s);

if (min_cmd < f_lo)
{
f_lo = min_cmd;
}
}
else
{
if (f_lo > 1U)
@@ -1810,17 +1877,30 @@ void PlsrOnPulseIsr(void)
((g_plsr_accel_plan.target_frequency_hz < g_plsr_accel_plan.start_frequency_hz) &&
(next <= g_plsr_accel_plan.target_frequency_hz)))
{
next = g_plsr_accel_plan.target_frequency_hz;
PlsrRunControlOnApproachDone();
if (PlsrRunControlShouldEnterDecel(remain) != 0U)
{
PlsrRunControlEnterDecel(next,
PlsrRunControlDecelEnterBudget(remain));
next = PlsrAccelNextFrequency(&g_plsr_accel_runtime);
PlsrRunControlApplyOutFreqIsr(next);
/*
* 直线：本拍写入加速末拍平均频，仅切 DECEL；首减频留给下一 UPDATE（ARPE）。
* 其它曲线：仍本拍预装首减频。
*/
if (g_plsr_accel_plan.curve_mode == PLSR_ACCEL_LINEAR)
{
PlsrRunControlApplyOutFreqIsr(next);
PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz,
PlsrRunControlDecelEnterBudget(remain));
}
else
{
PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz,
PlsrRunControlDecelEnterBudget(remain));
next = PlsrAccelNextFrequency(&g_plsr_accel_runtime);
PlsrRunControlApplyOutFreqIsr(next);
}
}
else
{
/* 进匀速：直接锁定峰值，ISR 不再开方/平均 */
PlsrRunControlEnterConstHoldIsr();
}
}
@@ -1831,7 +1911,7 @@ void PlsrOnPulseIsr(void)
}
else /* PH_DECEL */
{
/* 直线减速：NextFrequency 按 sqrt(end^2+2a·k) 从终点往上取，末拍 k=1 */
/* 直线减速：邻项平均；末拍 (S1+end)/2，不钉 end */
next = PlsrAccelNextFrequency(&g_plsr_accel_runtime);
PlsrRunControlApplyOutFreqIsr(next);
}


Write Preview
Loading…
Cancel
Save