hanyongwei
/
TrainCamp_hanyongwei_StageTwo
1
0
Rozštěpit 0
Zdrojový kód Úkoly 0 Požadavky na natažení 0 Vydání 0 Wiki Aktivita
Procházet zdrojové kódy

输出脉冲个数不准

dev1
hanyongwei před 3 týdny
rodič
183bd22770
revize
0119fda627
2 změnil soubory, kde provedl 132 přidání a 197 odebrání
Rozdělené zobrazení
Diff Options
Zobrazit statistiky Stáhněte soubor opravy Stáhněte rozdílový soubor
  1. +106
    -131
      plsr/accel_curve/plsr_accel_curve.c
  2. +26
    -66
      plsr/run_control/plsr_run_control.c

+ 106
- 131
plsr/accel_curve/plsr_accel_curve.c Zobrazit soubor

@@ -1,6 +1,6 @@
/**
* @file plsr_accel_curve.c
* @brief 脉冲域规划/取频：直线 frequency_hz^2=start_frequency_hz^2±2an；S/正弦按时间轴剖面
* @brief 脉冲域规划/取频：加速 f^2=start^2+2an；减速 f^2=end^2+2ak(k=N…1)；S/正弦时间轴
*
* ============================================================================
* 【阅读指南 — 先看这个再往下翻 if/else】
@@ -20,7 +20,8 @@
* decel_rate_hz_per_s 减速斜率 Hz/s ≈ default_speed*1000/deceleration_time_ms
*
* 二、三种曲线模式（curve_mode）
* LINEAR 直线：f_n = sqrt(start_frequency_hz^2 ± 2*a*pulse_count)；ISR 里 SquareStep ±1Hz 逼近
* LINEAR 加速：f=sqrt(start^2+2a·n)；减速：从终点往峰值加
* f=sqrt(end^2+2a·k)，k=N…1（与加速对称，勿从峰值减计数）
* S 时间域 jerk 梯形；规划/ISR 用离散 Δt=1/frequency_hz 仿真；ISR 查预建表
* SINE raised-cosine 时间剖面；同上，查预建表
*
@@ -557,48 +558,65 @@ static uint32_t PlsrAccelCurveCalculateRampPulses(uint32_t start_frequency_hz, u
return PlsrAccelCurveCalculateLinearRampPulses(start_frequency_hz, end_frequency_hz, acceleration_hz_per_s);
}

/** 直线：frequency_hz = sqrt(start_frequency_hz^2 + 2*a*pulse_count)，升到/降到不超过 target_limit_hz 方向 */
/**
* 直线取频。
* 加速(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
*/
static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz, uint32_t acceleration_hz_per_s,
uint32_t pulse_count, uint8_t frequency_rising,
uint32_t target_limit_hz)
uint32_t target_limit_hz,
uint32_t phase_total_pulses)
{
uint64_t squared_frequency;
uint32_t frequency_hz;
uint32_t k_from_end;

if (pulse_count == 0U)
{
frequency_hz = start_frequency_hz;
}
else if (acceleration_hz_per_s == 0U)
if (acceleration_hz_per_s == 0U)
{
frequency_hz = target_limit_hz;
frequency_hz = frequency_rising != 0U ? target_limit_hz : start_frequency_hz;
return PlsrAccelCurveClampFrequencyHz((frequency_hz < 1U) ? 1U : frequency_hz);
}
else

if (frequency_rising != 0U)
{
squared_frequency = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz;
if (frequency_rising != 0U)
if (pulse_count == 0U)
{
frequency_hz = start_frequency_hz;
}
else
{
squared_frequency += 2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)pulse_count;
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;
}
}
}
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)
{
frequency_hz = target_limit_hz;
}
else
{
uint64_t frequency_drop = 2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)pulse_count;
if (frequency_drop >= squared_frequency)
k_from_end = phase_total_pulses - pulse_count;
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)
{
frequency_hz = target_limit_hz;
}
else
{
frequency_hz = PlsrAccelCurveIntegerSquareRoot(squared_frequency - frequency_drop);
if (frequency_hz < target_limit_hz)
{
frequency_hz = target_limit_hz;
}
frequency_hz = start_frequency_hz;
}
}
}
@@ -1015,7 +1033,8 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
uint32_t selected_rate_hz_per_s = (frequency_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s;

frequency_hz = PlsrAccelCurveLinearFrequencyAtPulse(plan->start_frequency_hz, selected_rate_hz_per_s,
completed_segment_pulses, frequency_rising, plan->target_frequency_hz);
completed_segment_pulses, frequency_rising, plan->target_frequency_hz,
plan->accel_pulses);
}
else
{
@@ -1059,7 +1078,8 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan,
uint32_t selected_rate_hz_per_s = (frequency_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s;

frequency_hz = PlsrAccelCurveLinearFrequencyAtPulse(plan->target_frequency_hz, selected_rate_hz_per_s,
pulse_count, frequency_rising, plan->end_frequency_hz);
pulse_count, frequency_rising, plan->end_frequency_hz,
plan->decel_pulses);
}
else
{
@@ -1234,18 +1254,34 @@ static uint32_t PlsrAccelRuntimeBuildFrequencyTable(PlsrAccelRuntime_t *runtime,
}
if (simulation.is_active == 0U)
{
while ((loop_index + 1U) < runtime->total_pulses)
/* 提前到终点：中间格保持最后仿真值，仅最后一格钉 end（勿 end±1 垫平台） */
{
loop_index++;
table_index = loop_index / table_stride;
if (table_index >= table_capacity)
uint32_t hold_hz = simulation.current_frequency_hz;
if (hold_hz < 1U)
{
table_index = table_capacity - 1U;
hold_hz = 1U;
}
frequency_table[table_index] = runtime->end_frequency_hz;
if ((table_index + 1U) > table_length)
while ((loop_index + 1U) < runtime->total_pulses)
{
table_length = table_index + 1U;
loop_index++;
table_index = loop_index / table_stride;
if (table_index >= table_capacity)
{
table_index = table_capacity - 1U;
}
if ((loop_index + 1U) >= runtime->total_pulses)
{
frequency_table[table_index] = runtime->end_frequency_hz;
}
else
{
frequency_table[table_index] = hold_hz;
}
if ((table_index + 1U) > table_length)
{
table_length = table_index + 1U;
}
}
}
break;
@@ -1383,70 +1419,27 @@ void PlsrAccelBeginConstantSpeed(PlsrAccelRuntime_t *runtime,
}

/**
* 直线：用上一拍 frequency_hz ±1 逼近 target_frequency_squared，不全量开方。
* 低频 frequency_difference_hz/dn 大时循环次数多，但周期也长，ISR 仍可承受。
* 直线 ISR 取频（开方，与规划公式一致）。
* 加速：f = sqrt(start^2 + 2·a·completed)
* 减速：k = total-completed+1，f = sqrt(end^2 + 2·a·k)（从终点往峰值加）
*/
static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime)
{
uint64_t target_frequency_squared;
uint64_t current_frequency_squared;
uint64_t start_frequency_squared;
uint32_t frequency_hz;
uint32_t iteration_guard;
uint32_t k_from_end;

start_frequency_squared = (uint64_t)runtime->start_frequency_hz * (uint64_t)runtime->start_frequency_hz;
if (runtime->frequency_rising != 0U)
if (runtime->acceleration_hz_per_s == 0U)
{
target_frequency_squared = start_frequency_squared + (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)runtime->completed_pulses);
}
else
{
uint64_t frequency_drop = 2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)runtime->completed_pulses;
target_frequency_squared = (frequency_drop >= start_frequency_squared) ? 0ULL : (start_frequency_squared - frequency_drop);
}

frequency_hz = runtime->current_frequency_hz;
if (frequency_hz < 1U)
{
frequency_hz = 1U;
}

for (iteration_guard = 0U; iteration_guard < 2048U; iteration_guard++)
{
current_frequency_squared = (uint64_t)frequency_hz * (uint64_t)frequency_hz;
if (runtime->frequency_rising != 0U)
{
if ((frequency_hz < runtime->end_frequency_hz) && (((uint64_t)(frequency_hz + 1U) * (uint64_t)(frequency_hz + 1U)) <= target_frequency_squared))
{
frequency_hz++;
continue;
}
if ((frequency_hz > 1U) && (current_frequency_squared > target_frequency_squared))
{
frequency_hz--;
continue;
}
break;
}
else
{
if ((frequency_hz > runtime->end_frequency_hz) && (current_frequency_squared > target_frequency_squared))
{
frequency_hz--;
continue;
}
if ((frequency_hz < 100000U) &&
(((uint64_t)(frequency_hz + 1U) * (uint64_t)(frequency_hz + 1U)) < target_frequency_squared))
{
frequency_hz++;
continue;
}
break;
}
return PlsrAccelCurveClampFrequencyHz(runtime->end_frequency_hz);
}

if (runtime->frequency_rising != 0U)
{
target_frequency_squared =
(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)
{
frequency_hz = runtime->end_frequency_hz;
@@ -1454,11 +1447,28 @@ static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime)
}
else
{
if (frequency_hz < runtime->end_frequency_hz)
if (runtime->total_pulses < 1U)
{
frequency_hz = runtime->end_frequency_hz;
k_from_end = 1U;
}
else if (runtime->completed_pulses >= runtime->total_pulses)
{
k_from_end = 1U;
}
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;
}
}

if (frequency_hz < 1U)
{
frequency_hz = 1U;
@@ -1625,52 +1635,17 @@ uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime)
}

if ((runtime->total_pulses > 0U) && (runtime->completed_pulses >= runtime->total_pulses))
{
runtime->current_frequency_hz = runtime->end_frequency_hz;
runtime->is_active = 0U;
}
else if (runtime->curve_mode == PLSR_ACCEL_LINEAR)
{
/*
* 未到本相最后一拍：不要钳到 end。
* 否则减速在 n=N-1 就落到止速，ARPE 下会表现为末两拍同频。
* 直线减速末步已是 sqrt(end^2+2a)，不要再钉成 end（否则与「从终点往上加」不一致）。
* S/正弦 / 加速相：仍钉到相终点。
*/
if (runtime->frequency_rising == 0U)
if (!((runtime->curve_mode == PLSR_ACCEL_LINEAR) &&
(runtime->frequency_rising == 0U)))
{
if ((runtime->current_frequency_hz <= runtime->end_frequency_hz) &&
(runtime->end_frequency_hz < 100000U))
{
runtime->current_frequency_hz = runtime->end_frequency_hz + 1U;
}
}
else if ((runtime->current_frequency_hz >= runtime->end_frequency_hz) &&
(runtime->end_frequency_hz > 1U) &&
(runtime->total_pulses > 0U) &&
(runtime->completed_pulses < runtime->total_pulses))
{
runtime->current_frequency_hz = runtime->end_frequency_hz - 1U;
}
}
else if (runtime->frequency_table_id != 0U)
{
/* 查表：最后一拍之前若已是 end，同样让出 1Hz */
if ((runtime->total_pulses > 0U) &&
(runtime->completed_pulses < runtime->total_pulses))
{
if (runtime->frequency_rising == 0U)
{
if ((runtime->current_frequency_hz <= runtime->end_frequency_hz) &&
(runtime->end_frequency_hz < 100000U))
{
runtime->current_frequency_hz = runtime->end_frequency_hz + 1U;
}
}
else if ((runtime->current_frequency_hz >= runtime->end_frequency_hz) &&
(runtime->end_frequency_hz > 1U))
{
runtime->current_frequency_hz = runtime->end_frequency_hz - 1U;
}
runtime->current_frequency_hz = runtime->end_frequency_hz;
}
runtime->is_active = 0U;
}
else if (runtime->frequency_table_id == 0U)
{


+ 26
- 66
plsr/run_control/plsr_run_control.c Zobrazit soubor

@@ -37,10 +37,10 @@
* 5) s_segment_pulses_done >= target → OPM 末拍 or 立刻 AfterSegDone
* 6) 按 s_phase 改频：CONST 只进减速；APPROACH Step；DECEL Step
*
* ARPE（UPDATE ISR 写影子 → 下一周期才装入工作 ARR）：
* 在 remain==k 时写入的频率供下一拍使用；进减速窗为 remain<=dec_n，
* 在 remain==1 写入 f_end，末拍才能吃到止速。勿再用 remain<=dec_n+1，
* 否则会在 remain==2 就强制止速，造成末两拍同频。
* ARPE（UPDATE 写影子 → 下一周期才装入）：
* done 先自增再算 remain：done=k 表示第 k 拍上升沿已计入，remain=N-k（不含当前）。
* 本拍写入供第 k+1 拍；remain==dec_n 切入减速；remain==1 时写入 f_end 供末拍。
* done==N 时只武装停表、不再改频。
*
* 分层（阶段1薄拆分）：
* 1) 曲线（accel_curve）：只吃 (f_from,f_tgt,f_end,斜率,N,mode) → 梯形/三角；
@@ -66,9 +66,9 @@
* TIM5 one-shot：换向/WAIT/ACT/ACT剩余；ACT 到期收完当前脉冲并锁低，周期边界再切段。
* PlsrTask：DebouncePoll + TickMs + WakePend。
*
* 【段末频率 / ARPE】UPDATE 里写的影子 ARR 在下一周期生效；
* 减速在 remain==dec_n 切入，共 Step dec_n 次，最后一次（remain==1）写入 f_end。
* 纯减速开表：Start(f1)，后续由 ISR 顺序写入 f2…f_end（勿再预步进一拍）。
* 【段末频率 / ARPE】done 先++；remain=N-done（不含当前）；本拍写给下一拍：
* remain==dec_n 切入；remain==1 强制写 f_end；done==N 只停表不改频。
* 纯减速开表：Start(f1)，其余交 ISR。
*
* 【段末停表】非 FOLLOW-keep 时开段锁 s_stop_after_last_pulse：
* UPDATE≈上升沿；s_segment_pulses_done 到 target 时 ArmSegEndStop（OPM+CC1 末拍下降沿拉低），
@@ -853,6 +853,12 @@ static uint8_t PlsrRunControlHasDecel(void)

static uint32_t PlsrRunControlRemainPulses(void)
{
/*
* 注意：OnPulseIsr 里先 done++ 再调用本函数。
* 此时 done=k 表示「第 k 拍上升沿已计入」，当前正在跑第 k 拍。
* 返回值 = target-done = 当前拍之后还剩几拍（不含当前）。
* 例 N=15、done=14 → remain=1（之后只剩第 15 拍，本拍写入供第 15 拍用）。
*/
if (s_segment_pulse_target > s_segment_pulses_done)
{
return (uint32_t)(s_segment_pulse_target - s_segment_pulses_done);
@@ -861,9 +867,11 @@ static uint32_t PlsrRunControlRemainPulses(void)
}

/**
* ARPE：本拍 ISR 写入的影子 ARR 在下一周期生效。
* remain==dec_n 时切入并写入 f(1)，…，remain==1 写入 f_end → 末拍为止速。
* 若已晚进（remain < planned），预算用满剩余拍数，避免再减 1 导致末两拍同为止速。
* 先 done++ 后的 remain（不含当前）与「下一拍序号」关系：
* next_pulse = done + 1 = target - remain + 1
* 减速输出落在第 (N-dec_n+1)…N 拍；故在 done==N-dec_n（remain==dec_n）
* 时开始写入（供下一拍，即减速第 1 拍）。
* 晚进时预算用满剩余「还可写入的次数」= remain（done 到 N-1 共 remain 次）。
*/
static uint8_t PlsrRunControlShouldEnterDecel(uint32_t remain)
{
@@ -899,38 +907,6 @@ static int32_t PlsrRunControlAbsLocalPos(void)
return g_plsr_accumulated_pulses - s_absolute_origin;
}

/**
* 减速相写出频率：仅 remain==1（供末拍）允许钉止速；
* 更早写入若已碰到 end，则让出 1Hz，避免末两拍同频。
*/
static uint32_t PlsrRunControlDecelOutFreq(uint32_t profile_freq, uint32_t remain)
{
uint32_t end_hz = g_plsr_accel_plan.end_frequency_hz;

if (end_hz < 1U)
{
end_hz = 1U;
}

if (remain <= 1U)
{
return end_hz;
}

if (g_plsr_accel_runtime.frequency_rising == 0U)
{
if ((profile_freq <= end_hz) && (end_hz < 100000U))
{
return end_hz + 1U;
}
}
else if ((profile_freq >= end_hz) && (end_hz > 1U))
{
return end_hz - 1U;
}
return profile_freq;
}

/**
* @brief 进入减速相：budget 为曲线步进总数（非物理 remain），BeginDec
*/
@@ -953,8 +929,7 @@ static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget)
}

/**
* 开表纯减速：只 Start(f1)。下一拍起由 ISR NextFrequency 写入 f2…f_end。
* （旧逻辑再预装 f2 会在 ARPE 下让首拍直接吃到 f2，并多步进一次。）
* 开表纯减速：Start(f1)，后续 f2…f_end 由 ISR 超前写入。
*/
static void PlsrRunControlPrimeDecelOnStart(void)
{
@@ -1801,8 +1776,11 @@ void PlsrOnPulseIsr(void)
remain = PlsrRunControlRemainPulses();

/*
* --- 段内三相：按 s_phase 改频（匀速几乎空转）---
* remain = 还剩几拍（含当前正在进行的这一拍）
* done=k → 当前第 k 拍；写入的 ARR 给第 k+1 拍。
* remain=N-k（不含当前）。N=15,dec=5 时：
* done=10,remain=5 → 写 f11（减速第 1 拍）
* done=14,remain=1 → 写 f15=f_end（末拍）
* done=15 → 上面已 ArmSegEndStop，不会走到这里
*/
if (s_phase == PH_CONST)
{
@@ -1816,8 +1794,6 @@ void PlsrOnPulseIsr(void)
PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz,
PlsrRunControlDecelEnterBudget(remain));
next = PlsrAccelNextFrequency(&g_plsr_accel_runtime);
next = PlsrRunControlDecelOutFreq(next, remain);
g_plsr_accel_runtime.current_frequency_hz = next;
PlsrRunControlApplyOutFreqIsr(next);
}
}
@@ -1841,8 +1817,6 @@ void PlsrOnPulseIsr(void)
PlsrRunControlEnterDecel(next,
PlsrRunControlDecelEnterBudget(remain));
next = PlsrAccelNextFrequency(&g_plsr_accel_runtime);
next = PlsrRunControlDecelOutFreq(next, remain);
g_plsr_accel_runtime.current_frequency_hz = next;
PlsrRunControlApplyOutFreqIsr(next);
}
else
@@ -1857,22 +1831,8 @@ void PlsrOnPulseIsr(void)
}
else /* PH_DECEL */
{
/*
* ARPE：remain==k 写入 → 下一拍使用。
* remain==1 必须写出 f_end（末拍）；remain>1 禁止提前钉止速。
*/
if ((remain <= 1U) &&
(g_plsr_accel_runtime.total_pulses > 0U) &&
(g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_runtime.total_pulses))
{
next = g_plsr_accel_plan.end_frequency_hz;
}
else
{
next = PlsrAccelNextFrequency(&g_plsr_accel_runtime);
}
next = PlsrRunControlDecelOutFreq(next, remain);
g_plsr_accel_runtime.current_frequency_hz = next;
/* 直线减速：NextFrequency 按 sqrt(end^2+2a·k) 从终点往上取，末拍 k=1 */
next = PlsrAccelNextFrequency(&g_plsr_accel_runtime);
PlsrRunControlApplyOutFreqIsr(next);
}
}


Zapsat Náhled
Načítá se…
Zrušit
Uložit