diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index 239904a..5c6a8ae 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -1,6 +1,6 @@ /** * @file plsr_accel_curve.c - * @brief 脉冲域规划/取频：直线 frequency_hz^2=start_frequency_hz^2±2an；S/正弦按时间 + * @brief 脉冲域规划/取频：直线 freq_hz^2=start_freq_hz^2±2an；S/正弦按时间 */ #include "plsr_accel_curve.h" #include @@ -8,46 +8,661 @@ PlsrAccelPlan_t g_plsr_accel_plan; PlsrAccelRuntime_t g_plsr_accel_runtime; -/* 频率大于 100k 时暂时钳到 100k；启动门禁另报 0x04 */ -static uint32_t PlsrAccelCurveClampFrequencyHz(uint32_t frequency_hz) +/* 正弦 raised-cosine 形状表（千分比） */ +static const uint16_t s_plsr_sine_shape_tab[33] = { + 0U, 2U, 10U, 22U, 38U, 59U, 84U, 113U, + 146U, 183U, 222U, 264U, 309U, 355U, 402U, 451U, + 500U, 549U, 598U, 645U, 691U, 736U, 778U, 817U, + 854U, 887U, 916U, 941U, 962U, 978U, 990U, 998U, + 1000U +}; + +/* S/正弦频率预计算表：PlanSeg 时填好；ISR 只切换/查表。匀速不改频。 */ +#define PLSR_RAMP_TBL_MAX 4096U +static uint32_t s_accel_freq_table[PLSR_RAMP_TBL_MAX]; +static uint32_t s_decel_freq_table[PLSR_RAMP_TBL_MAX]; +static uint32_t s_accel_table_length; +static uint32_t s_accel_table_stride; +static uint32_t s_decel_table_length; +static uint32_t s_decel_table_stride; + +/* 静态辅助函数前置声明 */ +static uint32_t PlsrAccelCurveClampFreqHz(uint32_t freq_hz); +static uint32_t PlsrAccelCurveAbsDiff(uint32_t first_val, uint32_t second_val); +static uint32_t PlsrAccelCurveIntSqrt(uint64_t squared_freq); +static uint32_t PlsrAccelCurveRateFromDefaultSpeed(uint32_t default_speed_hz, uint32_t ramp_time_ms); +static uint32_t PlsrAccelCurveCalcRate(uint32_t default_speed_hz, + uint32_t start_freq_hz, + uint32_t end_freq_hz, + uint32_t ramp_time_ms); +static uint32_t PlsrAccelCurveCalcJumpFreq(uint32_t starting_freq_hz, uint32_t accel_hz_per_s); +static uint32_t PlsrAccelCurveResolveBoundaryFreq(uint32_t configured_freq_hz, + uint32_t target_freq_hz, + uint32_t default_speed_hz, + uint32_t accel_time_ms, + uint32_t decel_time_ms, + uint8_t resolving_start); +static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, + uint32_t end_val, + uint32_t ratio_permille); +static uint32_t PlsrAccelCurveCalcLinearRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, + uint32_t accel_hz_per_s); +static uint32_t PlsrAccelCurveLinearFreqAtPulse(uint32_t start_freq_hz, uint32_t accel_hz_per_s, + uint32_t pulse_count, uint8_t freq_rising, + uint32_t target_limit_hz); +static uint32_t PlsrAccelNextFreqLinear(PlsrAccelRuntime_t *runtime); +static uint32_t PlsrAccelCurveShapeSinePermille(uint32_t progress_permille); +static void PlsrAccelCurveCalcSCurveTimes(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s, + uint32_t *jerk_time_us_out, + uint32_t *const_accel_time_us_out, + uint32_t *ramp_time_us_out); +static void PlsrAccelCurveCalcSineCurveTime(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s, + uint32_t *ramp_time_us); +static uint32_t PlsrAccelCurveDeltaHzFromAt(uint32_t accel_hz_per_s, uint32_t elapsed_time_us); +static uint32_t PlsrAccelCurveDeltaHzFromJerk(uint32_t accel_hz_per_s, uint32_t elapsed_time_us, + uint32_t jerk_time_us); +static uint32_t PlsrAccelCurveSCurveFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s, + uint32_t jerk_time_us, uint32_t const_accel_time_us, + uint32_t ramp_time_us, uint32_t elapsed_time_us); +static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, + uint32_t ramp_time_us, uint32_t elapsed_time_us); +static uint32_t PlsrAccelCurveTimedFreqAtPulse( + uint32_t start_freq_hz, + uint32_t end_freq_hz, + uint32_t rate_hz_per_s, + PlsrAccelMode_e curve_mode, + uint32_t completed_pulses); +static uint32_t PlsrAccelCurveSimulateRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, + uint32_t accel_hz_per_s, + PlsrAccelMode_e curve_mode); +static uint32_t PlsrAccelNextFreqTimed(PlsrAccelRuntime_t *runtime); +static uint32_t PlsrAccelCurveCalcRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, + uint32_t accel_hz_per_s, + PlsrAccelMode_e curve_mode); +static uint32_t PlsrAccelCurveFitTargetFreq(uint32_t total_pulses, + uint32_t start_freq_hz, + uint32_t requested_target_freq_hz, + uint32_t end_freq_hz, + uint32_t accel_rate_hz_per_s, + uint32_t decel_rate_hz_per_s, + PlsrAccelMode_e curve_mode); +static void PlsrAccelCurveCalcPhasePulses(uint32_t start_freq_hz, + uint32_t peak_freq_hz, + uint32_t end_freq_hz, + uint32_t accel_rate_hz_per_s, + uint32_t decel_rate_hz_per_s, + PlsrAccelMode_e curve_mode, + uint32_t *accel_pulses, + uint32_t *decel_pulses); +static uint32_t PlsrAccelCurveFitTargetToPulseBudget(uint32_t total_pulses, + uint32_t start_freq_hz, + uint32_t requested_target_freq_hz, + uint32_t end_freq_hz, + uint32_t accel_rate_hz_per_s, + uint32_t decel_rate_hz_per_s, + PlsrAccelMode_e curve_mode, + uint32_t *accel_pulses, + uint32_t *decel_pulses); +static void PlsrAccelRuntimeCalcTimes(PlsrAccelRuntime_t *runtime); +static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, + uint32_t start_freq_hz, + uint32_t end_freq_hz, + uint32_t total_pulses, + uint32_t accel_rate_hz_per_s, + uint32_t decel_rate_hz_per_s, + PlsrAccelMode_e curve_mode); +static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime); +static uint32_t PlsrAccelRuntimeBuildFreqTable(PlsrAccelRuntime_t *runtime, + uint32_t *freq_table, + uint32_t table_capacity, + uint32_t *table_stride_out); +static void PlsrAccelRuntimeSelectFreqTable(PlsrAccelRuntime_t *runtime, + uint8_t freq_table_id, + uint32_t table_length, + uint32_t table_stride); + +/*============================================================================*/ +/* 公共 API */ +/*============================================================================*/ + +uint32_t PlsrAccelCurveResolveStartHz(uint32_t configured_freq_hz, + uint32_t target_freq_hz, + uint32_t default_speed_hz, + uint32_t accel_time_ms, + uint32_t decel_time_ms) { - if (frequency_hz > 100000U) + return PlsrAccelCurveResolveBoundaryFreq(configured_freq_hz, target_freq_hz, default_speed_hz, + accel_time_ms, decel_time_ms, 1U); +} + +uint32_t PlsrAccelCurveResolveEndHz(uint32_t configured_freq_hz, + uint32_t target_freq_hz, + uint32_t default_speed_hz, + uint32_t accel_time_ms, + uint32_t decel_time_ms) +{ + return PlsrAccelCurveResolveBoundaryFreq(configured_freq_hz, target_freq_hz, default_speed_hz, + accel_time_ms, decel_time_ms, 0U); +} + +/** + * @brief 规划一整段脉冲域（见 plsr_accel_curve.h） + */ +void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, + uint32_t total_pulses, + uint32_t start_freq_hz, + uint32_t target_freq_hz, + uint32_t end_freq_hz, + uint32_t default_speed_hz, + uint32_t accel_time_ms, + uint32_t decel_time_ms, + PlsrAccelMode_e curve_mode) +{ + uint32_t accel_pulses; + uint32_t decel_pulses; + uint32_t peak_freq_hz; + uint32_t accel_rate_hz_per_s; + uint32_t decel_rate_hz_per_s; + PlsrAccelMode_e selected_mode; + + if (plan == (PlsrAccelPlan_t *)0) + { + return; + } + + start_freq_hz = PlsrAccelCurveClampFreqHz(start_freq_hz); + target_freq_hz = PlsrAccelCurveClampFreqHz(target_freq_hz); + end_freq_hz = PlsrAccelCurveClampFreqHz(end_freq_hz); + + selected_mode = (curve_mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : curve_mode; + + if (start_freq_hz < 1U) + { + start_freq_hz = PlsrAccelCurveResolveStartHz(0U, target_freq_hz, default_speed_hz, + accel_time_ms, decel_time_ms); + } + if (end_freq_hz < 1U) + { + end_freq_hz = PlsrAccelCurveResolveEndHz(0U, target_freq_hz, default_speed_hz, + accel_time_ms, decel_time_ms); + } + + accel_rate_hz_per_s = + PlsrAccelCurveCalcRate(default_speed_hz, + start_freq_hz, + target_freq_hz, + accel_time_ms); + decel_rate_hz_per_s = + PlsrAccelCurveCalcRate(default_speed_hz, + target_freq_hz, + end_freq_hz, + decel_time_ms); + + peak_freq_hz = target_freq_hz; + + plan->start_freq_hz = start_freq_hz; + plan->end_freq_hz = end_freq_hz; + plan->curve_mode = selected_mode; + plan->accel_rate_hz_per_s = accel_rate_hz_per_s; + plan->decel_rate_hz_per_s = decel_rate_hz_per_s; + + PlsrAccelCurveCalcPhasePulses(start_freq_hz, + peak_freq_hz, + end_freq_hz, + accel_rate_hz_per_s, + decel_rate_hz_per_s, + selected_mode, + &accel_pulses, + &decel_pulses); + + /* --- 特例：本段 0 脉冲 --- */ + if (total_pulses == 0U) + { + plan->const_pulses = 0U; + plan->target_freq_hz = peak_freq_hz; + plan->accel_pulses = 0U; + plan->decel_pulses = 0U; + return; + } + + /* + * --- 特例：出口=目标 且 光加速就要用光 N 个脉冲 --- + * 整段当纯加速/纯减，无 dec、无 const（例如短段只爬到目标就停） + */ + if ((end_freq_hz == target_freq_hz) && (accel_pulses > total_pulses)) + { + plan->const_pulses = 0U; + plan->target_freq_hz = peak_freq_hz; + plan->accel_pulses = total_pulses; + plan->decel_pulses = 0U; + return; + } + + /* + * --- 主分支：脉冲够不够装下 acc + dec + const --- + * 够：const_pulses = 余量（标准梯形） + * 不够：const_pulses=0，FitPeak 降低 peak_freq_hz 直到 accel_pulses+decel_pulses <= N（三角波，可能报 0x02） + */ + if (accel_pulses + decel_pulses <= total_pulses) + { + plan->const_pulses = total_pulses - accel_pulses - decel_pulses; + } + else + { + /* + * 脉冲不够：先砍掉匀速段，保证加减速按完整剖面预算； + * 仍超出则降峰成三角，禁止 decel_pulses=total-accel_pulses 式截断。 + */ + plan->const_pulses = 0U; + peak_freq_hz = PlsrAccelCurveFitTargetToPulseBudget(total_pulses, start_freq_hz, target_freq_hz, end_freq_hz, + accel_rate_hz_per_s, decel_rate_hz_per_s, selected_mode, + &accel_pulses, &decel_pulses); + } + + /* + * --- 补丁：峰值 > 出口 但 decel_pulses 算出来为 0 --- + * 强制留 1 个脉冲做收尾，从 acc 或 const 里挪 1 个给 dec + */ + if ((peak_freq_hz > end_freq_hz) && (total_pulses > 1U) && (decel_pulses == 0U)) + { + decel_pulses = 1U; + if (accel_pulses >= total_pulses) + { + accel_pulses = total_pulses - 1U; + } + if (plan->const_pulses > 0U) + { + plan->const_pulses--; + } + else + { + plan->const_pulses = total_pulses - accel_pulses - decel_pulses; + } + } + + plan->target_freq_hz = peak_freq_hz; + plan->accel_pulses = accel_pulses; + plan->decel_pulses = decel_pulses; +} + +/** + * @brief 按已完成脉冲数取频（任务侧；ISR 用 PlsrAccelNextFreq） + */ +uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, + uint32_t completed_seg_pulses) +{ + uint32_t const_phase_end; + uint32_t pulse_count; + uint32_t freq_hz; + uint8_t freq_rising; + + if (plan == (PlsrAccelPlan_t *)0) + { + return 0U; + } + + const_phase_end = plan->accel_pulses + plan->const_pulses; + + /* 加速相：0 .. accel_pulses-1 完成后仍在加速；completed_seg_pulses 为已完成数 */ + if ((plan->accel_pulses > 0U) && (completed_seg_pulses < plan->accel_pulses) && + (plan->start_freq_hz != plan->target_freq_hz)) + { + freq_rising = (plan->target_freq_hz >= plan->start_freq_hz) ? 1U : 0U; + if (plan->curve_mode == PLSR_ACCEL_LINEAR) + { + uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; + + freq_hz = PlsrAccelCurveLinearFreqAtPulse(plan->start_freq_hz, selected_rate_hz_per_s, + completed_seg_pulses, freq_rising, plan->target_freq_hz); + } + else + { + uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; + + freq_hz = PlsrAccelCurveTimedFreqAtPulse( + plan->start_freq_hz, + plan->target_freq_hz, + selected_rate_hz_per_s, + plan->curve_mode, + completed_seg_pulses); + } + if ((freq_hz < 1U) && (plan->target_freq_hz >= 1U)) + { + freq_hz = 1U; + } + return PlsrAccelCurveClampFreqHz(freq_hz); + } + + /* 匀速 */ + if (completed_seg_pulses < const_phase_end) + { + return PlsrAccelCurveClampFreqHz(plan->target_freq_hz); + } + + /* 减速 / 无减速 */ + if ((plan->decel_pulses == 0U) || (plan->target_freq_hz == plan->end_freq_hz)) + { + return PlsrAccelCurveClampFreqHz(plan->end_freq_hz); + } + + pulse_count = completed_seg_pulses - const_phase_end; + if (pulse_count >= plan->decel_pulses) + { + return PlsrAccelCurveClampFreqHz(plan->end_freq_hz); + } + + freq_rising = (plan->end_freq_hz >= plan->target_freq_hz) ? 1U : 0U; + if (plan->curve_mode == PLSR_ACCEL_LINEAR) + { + uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; + + freq_hz = PlsrAccelCurveLinearFreqAtPulse(plan->target_freq_hz, selected_rate_hz_per_s, + pulse_count, freq_rising, plan->end_freq_hz); + } + else + { + uint32_t selected_rate_hz_per_s = (freq_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; + + freq_hz = PlsrAccelCurveTimedFreqAtPulse( + plan->target_freq_hz, + plan->end_freq_hz, + selected_rate_hz_per_s, + plan->curve_mode, + pulse_count); + } + if (freq_hz < 1U) + { + freq_hz = 1U; + } + return PlsrAccelCurveClampFreqHz(freq_hz); +} + +/** + * PlanSeg 后预建加/减速表（任务上下文，勿在 ISR 里做）。 + */ +void PlsrAccelPrebuildFreqTables(const PlsrAccelPlan_t *plan) +{ + PlsrAccelRuntime_t temporary_runtime; + + s_accel_table_length = 0U; + s_decel_table_length = 0U; + s_accel_table_stride = 1U; + s_decel_table_stride = 1U; + + if ((plan == (const PlsrAccelPlan_t *)0) || + (plan->curve_mode == PLSR_ACCEL_LINEAR)) + { + return; + } + + if ((plan->accel_pulses > 0U) && (plan->start_freq_hz != plan->target_freq_hz)) + { + PlsrAccelRuntimeBeginRamp(&temporary_runtime, + plan->start_freq_hz, + plan->target_freq_hz, + plan->accel_pulses, + plan->accel_rate_hz_per_s, + plan->decel_rate_hz_per_s, + plan->curve_mode); + s_accel_table_length = PlsrAccelRuntimeBuildFreqTable(&temporary_runtime, s_accel_freq_table, + PLSR_RAMP_TBL_MAX, + &s_accel_table_stride); + } + + if ((plan->decel_pulses > 0U) && (plan->target_freq_hz != plan->end_freq_hz)) + { + PlsrAccelRuntimeBeginRamp(&temporary_runtime, + plan->target_freq_hz, + plan->end_freq_hz, + plan->decel_pulses, + plan->accel_rate_hz_per_s, + plan->decel_rate_hz_per_s, + plan->curve_mode); + s_decel_table_length = PlsrAccelRuntimeBuildFreqTable(&temporary_runtime, s_decel_freq_table, + PLSR_RAMP_TBL_MAX, + &s_decel_table_stride); + } +} + +/** @brief 进入加速相 */ +void PlsrAccelBeginAccel(PlsrAccelRuntime_t *runtime, + const PlsrAccelPlan_t *plan) +{ + if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + { + return; + } + PlsrAccelRuntimeBeginRamp(runtime, + plan->start_freq_hz, + plan->target_freq_hz, + plan->accel_pulses, + plan->accel_rate_hz_per_s, + plan->decel_rate_hz_per_s, + plan->curve_mode); + PlsrAccelRuntimeSelectFreqTable(runtime, 1U, s_accel_table_length, + s_accel_table_stride); +} + +/** @brief 进入减速相 */ +void PlsrAccelBeginDecel(PlsrAccelRuntime_t *runtime, + const PlsrAccelPlan_t *plan) +{ + if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + { + return; + } + PlsrAccelRuntimeBeginRamp(runtime, + plan->target_freq_hz, + plan->end_freq_hz, + plan->decel_pulses, + plan->accel_rate_hz_per_s, + plan->decel_rate_hz_per_s, + plan->curve_mode); + /* 表已在 PlanSeg/Prebuild 填好；ISR 内禁止再建表 */ + PlsrAccelRuntimeSelectFreqTable(runtime, 2U, s_decel_table_length, + s_decel_table_stride); +} + +/** @brief 进入匀速相 */ +void PlsrAccelBeginConstSpeed(PlsrAccelRuntime_t *runtime, + const PlsrAccelPlan_t *plan) +{ + if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + { + return; + } + runtime->start_freq_hz = plan->target_freq_hz; + runtime->end_freq_hz = plan->target_freq_hz; + runtime->cur_freq_hz = plan->target_freq_hz; + runtime->accel_hz_per_s = 0U; + runtime->completed_pulses = 0U; + runtime->total_pulses = plan->const_pulses; + runtime->freq_rising = 1U; + runtime->curve_mode = plan->curve_mode; + runtime->is_active = 0U; /* 匀速：ISR 不改频 */ + runtime->freq_table_id = 0U; + runtime->table_length = 0U; + runtime->table_stride = 1U; + runtime->elapsed_time_us = 0U; + runtime->jerk_time_us = 0U; + runtime->const_accel_time_us = 0U; + runtime->ramp_time_us = 0U; +} + +/** + * @brief ISR 热路径：本相再前进 1 脉冲，返回下一拍应写入 ARR 的频率 + */ +uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime) +{ + const uint32_t *freq_table; + uint32_t table_index; + + if (runtime == (PlsrAccelRuntime_t *)0) + { + return 0U; + } + if (runtime->is_active == 0U) + { + return PlsrAccelCurveClampFreqHz(runtime->cur_freq_hz); + } + + if (runtime->completed_pulses < 0xFFFFFFFFUL) + { + runtime->completed_pulses++; + } + + if (runtime->curve_mode == PLSR_ACCEL_LINEAR) + { + if (runtime->accel_hz_per_s == 0U) + { + runtime->cur_freq_hz = runtime->end_freq_hz; + } + else + { + runtime->cur_freq_hz = PlsrAccelNextFreqLinear(runtime); + } + } + else if (runtime->freq_table_id != 0U) + { + /* ISR 热路径：只查表，不做 S/正弦实时积分 */ + if (runtime->table_length < 1U) + { + runtime->cur_freq_hz = runtime->end_freq_hz; + } + else + { + uint32_t table_stride = (runtime->table_stride < 1U) ? + 1U : runtime->table_stride; + + table_index = (runtime->completed_pulses - 1U) / table_stride; + if (table_index >= runtime->table_length) + { + table_index = runtime->table_length - 1U; + } + freq_table = (runtime->freq_table_id == 1U) ? + s_accel_freq_table : + s_decel_freq_table; + runtime->cur_freq_hz = freq_table[table_index]; + } + } + else + { + /* 建表失败兜底：仍走时间步进 */ + runtime->cur_freq_hz = PlsrAccelNextFreqTimed(runtime); + } + + if ((runtime->total_pulses > 0U) && (runtime->completed_pulses >= runtime->total_pulses)) + { + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; + } + else if (runtime->curve_mode == PLSR_ACCEL_LINEAR) + { + /* + * 未到本相最后一拍：不要钳到 end。 + * 否则减速在 n=N-1 就落到止速，ARPE 下会表现为末两拍同频。 + */ + if (runtime->freq_rising == 0U) + { + if ((runtime->cur_freq_hz <= runtime->end_freq_hz) && + (runtime->end_freq_hz < 100000U)) + { + runtime->cur_freq_hz = runtime->end_freq_hz + 1U; + } + } + else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && + (runtime->end_freq_hz > 1U) && + (runtime->total_pulses > 0U) && + (runtime->completed_pulses < runtime->total_pulses)) + { + runtime->cur_freq_hz = runtime->end_freq_hz - 1U; + } + } + else if (runtime->freq_table_id != 0U) + { + /* 查表：最后一拍之前若已是 end，同样让出 1Hz */ + if ((runtime->total_pulses > 0U) && + (runtime->completed_pulses < runtime->total_pulses)) + { + if (runtime->freq_rising == 0U) + { + if ((runtime->cur_freq_hz <= runtime->end_freq_hz) && + (runtime->end_freq_hz < 100000U)) + { + runtime->cur_freq_hz = runtime->end_freq_hz + 1U; + } + } + else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && + (runtime->end_freq_hz > 1U)) + { + runtime->cur_freq_hz = runtime->end_freq_hz - 1U; + } + } + } + else if (runtime->freq_table_id == 0U) { - frequency_hz = 100000U; + if ((runtime->ramp_time_us > 0U) && (runtime->elapsed_time_us >= runtime->ramp_time_us)) + { + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; + } + else if (runtime->freq_rising != 0U) + { + if (runtime->cur_freq_hz >= runtime->end_freq_hz) + { + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; + } + } + else if (runtime->cur_freq_hz <= runtime->end_freq_hz) + { + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; + } } - return frequency_hz; + return PlsrAccelCurveClampFreqHz(runtime->cur_freq_hz); } /*============================================================================*/ -/* 整数开方 / 斜率时间 */ +/* 工具 */ /*============================================================================*/ +/* 频率大于 100k 时暂时钳到 100k；启动门禁另报 0x04 */ +static uint32_t PlsrAccelCurveClampFreqHz(uint32_t freq_hz) +{ + if (freq_hz > 100000U) + { + freq_hz = 100000U; + } + return freq_hz; +} + /** @brief 两个无符号数之间的距离 */ -static uint32_t PlsrAccelCurveAbsoluteDifference(uint32_t first_value, uint32_t second_value) +static uint32_t PlsrAccelCurveAbsDiff(uint32_t first_val, uint32_t second_val) { - return (first_value >= second_value) ? - (first_value - second_value) : (second_value - first_value); + return (first_val >= second_val) ? + (first_val - second_val) : (second_val - first_val); } -/** 64 位整数平方根（牛顿法），支持 frequency_hz^2 + 2an */ -static uint32_t PlsrAccelCurveIntegerSquareRoot(uint64_t squared_frequency) +/** 64 位整数平方根（牛顿法），支持 freq_hz^2 + 2an */ +static uint32_t PlsrAccelCurveIntSqrt(uint64_t squared_freq) { uint64_t estimate; uint64_t next_estimate; uint32_t loop_index; - if (squared_frequency <= 1ULL) + if (squared_freq <= 1ULL) { - return (uint32_t)squared_frequency; + return (uint32_t)squared_freq; } - estimate = squared_frequency; + estimate = squared_freq; if (estimate > 100000ULL) { estimate = 100000ULL; } for (loop_index = 0U; loop_index < 40U; loop_index++) { - next_estimate = (estimate + squared_frequency / estimate) / 2ULL; + next_estimate = (estimate + squared_freq / estimate) / 2ULL; if (next_estimate >= estimate) { break; @@ -75,13 +690,13 @@ static uint32_t PlsrAccelCurveRateFromDefaultSpeed(uint32_t default_speed_hz, ui } /** 按默认速度计算变化率；默认速度未配置时用本段频差计算。 */ -static uint32_t PlsrAccelCurveCalculateRate(uint32_t default_speed_hz, - uint32_t start_frequency_hz, - uint32_t end_frequency_hz, +static uint32_t PlsrAccelCurveCalcRate(uint32_t default_speed_hz, + uint32_t start_freq_hz, + uint32_t end_freq_hz, uint32_t ramp_time_ms) { uint32_t rate_hz_per_s; - uint32_t frequency_difference_hz; + uint32_t freq_diff_hz; rate_hz_per_s = PlsrAccelCurveRateFromDefaultSpeed(default_speed_hz, ramp_time_ms); if ((rate_hz_per_s != 0U) || (ramp_time_ms == 0U)) @@ -89,195 +704,140 @@ static uint32_t PlsrAccelCurveCalculateRate(uint32_t default_speed_hz, return rate_hz_per_s; } - frequency_difference_hz = - PlsrAccelCurveAbsoluteDifference(start_frequency_hz, end_frequency_hz); - if (frequency_difference_hz == 0U) + freq_diff_hz = + PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); + if (freq_diff_hz == 0U) { return 0U; } - return (frequency_difference_hz * 1000UL) / ramp_time_ms; + return (freq_diff_hz * 1000UL) / ramp_time_ms; } /** - * 起跳/落地：frequency_hz = sqrt(starting_frequency_hz^2 + 2a)，a 与斜坡规划相同。 - * 不钳到 f_to；starting_frequency_hz=0 → sqrt(2a)。 + * 起跳/落地：freq_hz = sqrt(starting_freq_hz^2 + 2a)，a 与斜坡规划相同。 + * 不钳到 f_to；starting_freq_hz=0 → sqrt(2a)。 */ -static uint32_t PlsrAccelCurveCalculateJumpFrequency(uint32_t starting_frequency_hz, uint32_t acceleration_hz_per_s) +static uint32_t PlsrAccelCurveCalcJumpFreq(uint32_t starting_freq_hz, uint32_t accel_hz_per_s) { - uint64_t squared_frequency; - uint32_t frequency_hz; + uint64_t squared_freq; + uint32_t freq_hz; - if (acceleration_hz_per_s == 0U) + if (accel_hz_per_s == 0U) { - return (starting_frequency_hz >= 1U) ? PlsrAccelCurveClampFrequencyHz(starting_frequency_hz) : 1U; + return (starting_freq_hz >= 1U) ? PlsrAccelCurveClampFreqHz(starting_freq_hz) : 1U; } - squared_frequency = (uint64_t)starting_frequency_hz * (uint64_t)starting_frequency_hz + (2ULL * (uint64_t)acceleration_hz_per_s); - frequency_hz = PlsrAccelCurveIntegerSquareRoot(squared_frequency); - if (frequency_hz < 1U) + squared_freq = (uint64_t)starting_freq_hz * (uint64_t)starting_freq_hz + (2ULL * (uint64_t)accel_hz_per_s); + freq_hz = PlsrAccelCurveIntSqrt(squared_freq); + if (freq_hz < 1U) { - frequency_hz = 1U; + freq_hz = 1U; } - return PlsrAccelCurveClampFrequencyHz(frequency_hz); + return PlsrAccelCurveClampFreqHz(freq_hz); } /** * 解析配置起速/止速 → 本段实际入口或出口频率。 * * 三分支（按顺序判断，命中即 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) + * ① configured_freq_hz > target_freq_hz → 直接用 configured_freq_hz（例如起速 500、目标 200：先高位再减） + * ② target_freq_hz < jump_freq_hz → 用 target_freq_hz（目标比起跳还低，没必要从 jump_freq_hz 起） + * ③ 否则 → 用 jump_freq_hz = sqrt(configured_freq_hz^2 + 2a) * - * resolving_start=1 用 acceleration_time_ms 算 a；resolving_start=0 用 deceleration_time_ms（ResolveEndHz 止速侧）。 + * resolving_start=1 用 accel_time_ms 算 a；resolving_start=0 用 decel_time_ms（ResolveEndHz 止速侧）。 */ -static uint32_t PlsrAccelCurveResolveBoundaryFrequency(uint32_t configured_frequency_hz, - uint32_t target_frequency_hz, +static uint32_t PlsrAccelCurveResolveBoundaryFreq(uint32_t configured_freq_hz, + uint32_t target_freq_hz, uint32_t default_speed_hz, - uint32_t acceleration_time_ms, - uint32_t deceleration_time_ms, + uint32_t accel_time_ms, + uint32_t decel_time_ms, uint8_t resolving_start) { uint32_t ramp_time_ms; - uint32_t acceleration_hz_per_s; - uint32_t jump_frequency_hz; + uint32_t accel_hz_per_s; + uint32_t jump_freq_hz; - configured_frequency_hz = PlsrAccelCurveClampFrequencyHz(configured_frequency_hz); - target_frequency_hz = PlsrAccelCurveClampFrequencyHz(target_frequency_hz); - if (target_frequency_hz < 1U) + configured_freq_hz = PlsrAccelCurveClampFreqHz(configured_freq_hz); + target_freq_hz = PlsrAccelCurveClampFreqHz(target_freq_hz); + if (target_freq_hz < 1U) { return 1U; } /* 配置起/止速 > 目标：用配置速 */ - if (configured_frequency_hz > target_frequency_hz) + if (configured_freq_hz > target_freq_hz) { - return configured_frequency_hz; + return configured_freq_hz; } - ramp_time_ms = (resolving_start != 0U) ? acceleration_time_ms : deceleration_time_ms; - acceleration_hz_per_s = - PlsrAccelCurveCalculateRate(default_speed_hz, - configured_frequency_hz, - target_frequency_hz, + ramp_time_ms = (resolving_start != 0U) ? accel_time_ms : decel_time_ms; + accel_hz_per_s = + PlsrAccelCurveCalcRate(default_speed_hz, + configured_freq_hz, + target_freq_hz, ramp_time_ms); - if (acceleration_hz_per_s == 0U) + if (accel_hz_per_s == 0U) { - return (configured_frequency_hz >= 1U) ? configured_frequency_hz : target_frequency_hz; + return (configured_freq_hz >= 1U) ? configured_freq_hz : target_freq_hz; } - jump_frequency_hz = PlsrAccelCurveCalculateJumpFrequency(configured_frequency_hz, acceleration_hz_per_s); + jump_freq_hz = PlsrAccelCurveCalcJumpFreq(configured_freq_hz, accel_hz_per_s); /* 目标 < 起跳 → 用目标；目标 >= 起跳 → 用起跳 */ - if (target_frequency_hz < jump_frequency_hz) - { - return target_frequency_hz; - } - return jump_frequency_hz; -} - -uint32_t PlsrAccelCurveResolveStartHz(uint32_t configured_frequency_hz, - uint32_t target_frequency_hz, - uint32_t default_speed_hz, - uint32_t acceleration_time_ms, - uint32_t deceleration_time_ms) -{ - return PlsrAccelCurveResolveBoundaryFrequency(configured_frequency_hz, target_frequency_hz, default_speed_hz, - acceleration_time_ms, deceleration_time_ms, 1U); -} - -uint32_t PlsrAccelCurveResolveEndHz(uint32_t configured_frequency_hz, - uint32_t target_frequency_hz, - uint32_t default_speed_hz, - uint32_t acceleration_time_ms, - uint32_t deceleration_time_ms) -{ - return PlsrAccelCurveResolveBoundaryFrequency(configured_frequency_hz, target_frequency_hz, default_speed_hz, - acceleration_time_ms, deceleration_time_ms, 0U); -} - -/** - * 正弦加减速形状：raised-cosine p=(1-cos(πu))/2 - * - * 不用 libm sin/cos（软浮点很重，不宜进脉冲 ISR）。 - * 用 33 点预计算表 + 线性插值，O(1)、Flash 约 66 字节。 - * 仅供 SineFreqAtTime；S 曲线走 jerk 梯形解析式，不走形状表。 - */ -static const uint16_t s_plsr_sine_shape_tab[33] = { - 0U, 2U, 10U, 22U, 38U, 59U, 84U, 113U, - 146U, 183U, 222U, 264U, 309U, 355U, 402U, 451U, - 500U, 549U, 598U, 645U, 691U, 736U, 778U, 817U, - 854U, 887U, 916U, 941U, 962U, 978U, 990U, 998U, - 1000U -}; - -static uint32_t PlsrAccelCurveShapeSinePermille(uint32_t progress_permille) -{ - uint32_t table_index; - uint32_t segment_progress; - uint32_t lower_value; - uint32_t upper_value; - - if (progress_permille >= 1000U) - { - return 1000U; - } - /* progress_permille/1000 → 表下标 0..32，相邻点线性插值 */ - table_index = (progress_permille * 32UL) / 1000UL; - if (table_index >= 32U) + if (target_freq_hz < jump_freq_hz) { - return 1000U; + return target_freq_hz; } - segment_progress = (progress_permille * 32UL) - (table_index * 1000UL); /* 0..999 对应段内进度 */ - lower_value = (uint32_t)s_plsr_sine_shape_tab[table_index]; - upper_value = (uint32_t)s_plsr_sine_shape_tab[table_index + 1U]; - return lower_value + - (((upper_value - lower_value) * segment_progress) / 1000UL); + return jump_freq_hz; } -static uint32_t PlsrAccelCurveInterpolate(uint32_t start_value, - uint32_t end_value, +static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, + uint32_t end_val, uint32_t ratio_permille) { if (ratio_permille >= 1000U) { - return end_value; + return end_val; } - if (end_value >= start_value) + if (end_val >= start_val) { - return start_value + - ((end_value - start_value) * ratio_permille) / 1000UL; + return start_val + + ((end_val - start_val) * ratio_permille) / 1000UL; } - return start_value - - ((start_value - end_value) * ratio_permille) / 1000UL; + return start_val - + ((start_val - end_val) * ratio_permille) / 1000UL; } +/*============================================================================*/ +/* 直线 */ +/*============================================================================*/ + /** - * 脉冲闭合：N = ceil(|end_frequency_hz^2 - start_frequency_hz^2| / (2a))。 + * 脉冲闭合：N = ceil(|end_freq_hz^2 - start_freq_hz^2| / (2a))。 * a=0 且频差非 0 → 0（阶跃，无斜坡脉冲）。 */ -static uint32_t PlsrAccelCurveCalculateLinearRampPulses(uint32_t start_frequency_hz, uint32_t end_frequency_hz, - uint32_t acceleration_hz_per_s) +static uint32_t PlsrAccelCurveCalcLinearRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, + uint32_t accel_hz_per_s) { - uint64_t start_frequency_squared; - uint64_t end_frequency_squared; - uint64_t squared_frequency_difference; - uint64_t twice_acceleration; + uint64_t start_freq_squared; + uint64_t end_freq_squared; + uint64_t squared_freq_diff; + uint64_t twice_accel; uint64_t pulse_count; - if (start_frequency_hz == end_frequency_hz) + if (start_freq_hz == end_freq_hz) { return 0U; } - if (acceleration_hz_per_s == 0U) + if (accel_hz_per_s == 0U) { return 0U; } - 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); - twice_acceleration = 2ULL * (uint64_t)acceleration_hz_per_s; - pulse_count = (squared_frequency_difference + twice_acceleration - 1ULL) / twice_acceleration; + start_freq_squared = (uint64_t)start_freq_hz * (uint64_t)start_freq_hz; + end_freq_squared = (uint64_t)end_freq_hz * (uint64_t)end_freq_hz; + squared_freq_diff = (end_freq_squared > start_freq_squared) ? (end_freq_squared - start_freq_squared) : (start_freq_squared - end_freq_squared); + twice_accel = 2ULL * (uint64_t)accel_hz_per_s; + pulse_count = (squared_freq_diff + twice_accel - 1ULL) / twice_accel; if (pulse_count == 0ULL) { pulse_count = 1ULL; @@ -289,1331 +849,861 @@ static uint32_t PlsrAccelCurveCalculateLinearRampPulses(uint32_t start_frequency return (uint32_t)pulse_count; } -/*============================================================================*/ -/* S / 正弦 */ -/*============================================================================*/ - -/** - * S 曲线时间参数（加速度梯形 1:2:1）： - * Tj : Ta : Tj = 1 : 2 : 1 - * Δf = A*(Ta+Tj) ⇒ Tj=Δf/(3A), Ta=2Δf/(3A), T=4Δf/(3A) - */ -static void PlsrAccelCurveCalculateSCurveTimes(uint32_t start_frequency_hz, uint32_t end_frequency_hz, uint32_t acceleration_hz_per_s, - uint32_t *jerk_time_us_out, - uint32_t *constant_accel_time_us_out, - uint32_t *ramp_time_us_out) -{ - uint32_t frequency_difference_hz; - uint64_t numerator; - uint64_t denominator; - uint32_t calculated_jerk_time_us; - - *jerk_time_us_out = 0U; - *constant_accel_time_us_out = 0U; - *ramp_time_us_out = 0U; - - frequency_difference_hz = PlsrAccelCurveAbsoluteDifference(start_frequency_hz, end_frequency_hz); - if ((frequency_difference_hz < 1U) || (acceleration_hz_per_s < 1U)) - { - return; - } - - /* Tj_us = ceil(frequency_difference_hz * 1e6 / (3A)) */ - numerator = (uint64_t)frequency_difference_hz * 1000000ULL; - denominator = 3ULL * (uint64_t)acceleration_hz_per_s; - calculated_jerk_time_us = - (uint32_t)((numerator + denominator - 1ULL) / denominator); - if (calculated_jerk_time_us < 1U) - { - calculated_jerk_time_us = 1U; - } - *jerk_time_us_out = calculated_jerk_time_us; - *constant_accel_time_us_out = calculated_jerk_time_us * 2U; - *ramp_time_us_out = calculated_jerk_time_us * 4U; -} - -/** 正弦 raised-cosine：峰值加速度 = A ⇒ T = π·Δf / (2A) */ -static void PlsrAccelCurveCalculateSineCurveTime(uint32_t start_frequency_hz, uint32_t end_frequency_hz, uint32_t acceleration_hz_per_s, - uint32_t *ramp_time_us) -{ - uint32_t frequency_difference_hz; - uint64_t numerator; - uint64_t denominator; - - *ramp_time_us = 0U; - frequency_difference_hz = PlsrAccelCurveAbsoluteDifference(start_frequency_hz, end_frequency_hz); - if ((frequency_difference_hz < 1U) || (acceleration_hz_per_s < 1U)) - { - return; - } - /* T_us = ceil(π*frequency_difference_hz*1e6 / (2A))，π≈355/113 */ - numerator = (uint64_t)frequency_difference_hz * 1000000ULL * 355ULL; - denominator = 2ULL * (uint64_t)acceleration_hz_per_s * 113ULL; - *ramp_time_us = (uint32_t)((numerator + denominator - 1ULL) / denominator); - if (*ramp_time_us < 1U) - { - *ramp_time_us = 1U; - } -} - -/** Δf = A * elapsed_time_us / 1e6 */ -static uint32_t PlsrAccelCurveDeltaHzFromAt(uint32_t acceleration_hz_per_s, uint32_t elapsed_time_us) -{ - return (uint32_t)(((uint64_t)acceleration_hz_per_s * (uint64_t)elapsed_time_us) / 1000000ULL); -} - -/** Δf = A * elapsed_time_us^2 / (2 * jerk_time_us * 1e6) — jerk 段 */ -static uint32_t PlsrAccelCurveDeltaHzFromJerk(uint32_t acceleration_hz_per_s, uint32_t elapsed_time_us, - uint32_t jerk_time_us) -{ - uint64_t calculation; - - if ((jerk_time_us < 1U) || (elapsed_time_us < 1U) || (acceleration_hz_per_s < 1U)) - { - return 0U; - } - /* ((A * t / jerk_time_us) * t) / 2e6 */ - calculation = ((uint64_t)acceleration_hz_per_s * (uint64_t)elapsed_time_us) / (uint64_t)jerk_time_us; - calculation = (calculation * (uint64_t)elapsed_time_us) / 2000000ULL; - if (calculation > 0xFFFFFFFFULL) - { - return 0xFFFFFFFFUL; - } - return (uint32_t)calculation; -} - -/** - * 时刻 elapsed_time_us 的频率（相对进度 0..frequency_difference_hz，再映射到 start_frequency_hz→end_frequency_hz）。 - */ -static uint32_t PlsrAccelCurveSCurveFrequencyAtTime(uint32_t start_frequency_hz, uint32_t end_frequency_hz, uint32_t acceleration_hz_per_s, - uint32_t jerk_time_us, uint32_t constant_accel_time_us, - uint32_t ramp_time_us, uint32_t elapsed_time_us) -{ - uint32_t frequency_difference_hz; - uint32_t frequency_change_hz; - uint32_t deceleration_start_time_us; - uint32_t deceleration_elapsed_us; - uint8_t frequency_rising; - - frequency_difference_hz = PlsrAccelCurveAbsoluteDifference(start_frequency_hz, end_frequency_hz); - frequency_rising = (end_frequency_hz >= start_frequency_hz) ? 1U : 0U; - - if ((frequency_difference_hz < 1U) || (ramp_time_us < 1U) || (acceleration_hz_per_s < 1U)) - { - return PlsrAccelCurveClampFrequencyHz(end_frequency_hz); - } - if (elapsed_time_us >= ramp_time_us) - { - return PlsrAccelCurveClampFrequencyHz(end_frequency_hz); - } - - if ((jerk_time_us < 1U) || (elapsed_time_us <= jerk_time_us)) - { - /* jerk 升：frequency_change_hz = 0.5*(A/Tj)*t^2 */ - frequency_change_hz = PlsrAccelCurveDeltaHzFromJerk(acceleration_hz_per_s, elapsed_time_us, (jerk_time_us < 1U) ? 1U : jerk_time_us); - } - else - { - deceleration_start_time_us = jerk_time_us + constant_accel_time_us; - if (elapsed_time_us <= deceleration_start_time_us) - { - /* 恒加速：frequency_change_hz = 0.5*A*Tj + A*(t-Tj) */ - frequency_change_hz = PlsrAccelCurveDeltaHzFromJerk(acceleration_hz_per_s, jerk_time_us, jerk_time_us); - frequency_change_hz += PlsrAccelCurveDeltaHzFromAt(acceleration_hz_per_s, elapsed_time_us - jerk_time_us); - } - else - { - /* jerk 降：frequency_change_hz = 0.5*A*Tj + A*Ta + A*τ - 0.5*(A/Tj)*τ^2 */ - deceleration_elapsed_us = elapsed_time_us - deceleration_start_time_us; - if (deceleration_elapsed_us > jerk_time_us) - { - deceleration_elapsed_us = jerk_time_us; - } - frequency_change_hz = PlsrAccelCurveDeltaHzFromJerk(acceleration_hz_per_s, jerk_time_us, jerk_time_us); - frequency_change_hz += PlsrAccelCurveDeltaHzFromAt(acceleration_hz_per_s, constant_accel_time_us); - frequency_change_hz += PlsrAccelCurveDeltaHzFromAt(acceleration_hz_per_s, deceleration_elapsed_us); - { - uint32_t frequency_drop = PlsrAccelCurveDeltaHzFromJerk(acceleration_hz_per_s, deceleration_elapsed_us, jerk_time_us); - if (frequency_change_hz > frequency_drop) - { - frequency_change_hz -= frequency_drop; - } - else - { - frequency_change_hz = 0U; - } - } - } - } - - if (frequency_change_hz > frequency_difference_hz) - { - frequency_change_hz = frequency_difference_hz; - } - if (frequency_rising != 0U) - { - return PlsrAccelCurveClampFrequencyHz(start_frequency_hz + frequency_change_hz); - } - if (start_frequency_hz > frequency_change_hz) - { - return PlsrAccelCurveClampFrequencyHz(start_frequency_hz - frequency_change_hz); - } - return PlsrAccelCurveClampFrequencyHz(end_frequency_hz); -} - -/** - * 正弦 raised-cosine：frequency_hz = start_frequency_hz + frequency_difference_hz * (1-cos(π progress_permille))/2，progress_permille=t/T - * 用现有 33 点 sine 形状表（已是 raised-cosine 千分比）。 - */ -static uint32_t PlsrAccelCurveSineFrequencyAtTime(uint32_t start_frequency_hz, uint32_t end_frequency_hz, - uint32_t ramp_time_us, uint32_t elapsed_time_us) -{ - uint32_t progress_permille; - uint32_t ratio_permille; - uint32_t frequency_hz; - - if ((ramp_time_us < 1U) || (start_frequency_hz == end_frequency_hz)) - { - return PlsrAccelCurveClampFrequencyHz(end_frequency_hz); - } - if (elapsed_time_us >= ramp_time_us) - { - return PlsrAccelCurveClampFrequencyHz(end_frequency_hz); - } - progress_permille = (uint32_t)(((uint64_t)elapsed_time_us * 1000ULL) / (uint64_t)ramp_time_us); - if (progress_permille > 1000U) - { - progress_permille = 1000U; - } - ratio_permille = PlsrAccelCurveShapeSinePermille(progress_permille); - frequency_hz = PlsrAccelCurveInterpolate(start_frequency_hz, end_frequency_hz, ratio_permille); - if (frequency_hz < 1U) - { - frequency_hz = 1U; - } - return PlsrAccelCurveClampFrequencyHz(frequency_hz); -} - -/** - * 与 ISR / 建表一致：逐拍 Δt=1/frequency_hz 推进时间，得到 S/正弦斜坡脉冲数。 - */ -static uint32_t PlsrAccelCurveSimulateRampPulses(uint32_t start_frequency_hz, uint32_t end_frequency_hz, - uint32_t acceleration_hz_per_s, - PlsrAccelMode_e curve_mode); - -static uint32_t PlsrAccelCurveCalculateRampPulses(uint32_t start_frequency_hz, uint32_t end_frequency_hz, - uint32_t acceleration_hz_per_s, - PlsrAccelMode_e curve_mode) -{ - if ((curve_mode == PLSR_ACCEL_SINE) || (curve_mode == PLSR_ACCEL_S)) - { - return PlsrAccelCurveSimulateRampPulses(start_frequency_hz, end_frequency_hz, acceleration_hz_per_s, curve_mode); - } - 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 方向 */ -static uint32_t PlsrAccelCurveLinearFrequencyAtPulse(uint32_t start_frequency_hz, uint32_t acceleration_hz_per_s, - uint32_t pulse_count, uint8_t frequency_rising, +/** 直线：freq_hz = sqrt(start_freq_hz^2 + 2*a*pulse_count)，升到/降到不超过 target_limit_hz 方向 */ +static uint32_t PlsrAccelCurveLinearFreqAtPulse(uint32_t start_freq_hz, uint32_t accel_hz_per_s, + uint32_t pulse_count, uint8_t freq_rising, uint32_t target_limit_hz) { - uint64_t squared_frequency; - uint32_t frequency_hz; + uint64_t squared_freq; + uint32_t freq_hz; if (pulse_count == 0U) { - frequency_hz = start_frequency_hz; - } - else if (acceleration_hz_per_s == 0U) - { - frequency_hz = target_limit_hz; - } - else - { - squared_frequency = (uint64_t)start_frequency_hz * (uint64_t)start_frequency_hz; - if (frequency_rising != 0U) - { - squared_frequency += 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 - { - uint64_t frequency_drop = 2ULL * (uint64_t)acceleration_hz_per_s * (uint64_t)pulse_count; - if (frequency_drop >= squared_frequency) - { - frequency_hz = target_limit_hz; - } - else - { - frequency_hz = PlsrAccelCurveIntegerSquareRoot(squared_frequency - frequency_drop); - if (frequency_hz < target_limit_hz) - { - frequency_hz = target_limit_hz; - } - } - } - } - if (frequency_hz < 1U) - { - frequency_hz = 1U; + freq_hz = start_freq_hz; } - return PlsrAccelCurveClampFrequencyHz(frequency_hz); -} - -/** - * 二分峰值：使 acc+dec 脉冲 <= total（脉冲闭合公式）。 - */ -static uint32_t PlsrAccelCurveFitTargetFrequency(uint32_t total_pulses, - uint32_t start_frequency_hz, - uint32_t requested_target_frequency_hz, - uint32_t end_frequency_hz, - uint32_t accel_rate_hz_per_s, - uint32_t decel_rate_hz_per_s, - PlsrAccelMode_e curve_mode) -{ - uint32_t lowest_offset_hz = 0U; - uint32_t highest_offset_hz; - uint32_t candidate_offset_hz; - uint32_t best_offset_hz = 0U; - uint32_t candidate_frequency_hz; - uint32_t acceleration_pulses; - uint32_t deceleration_pulses; - uint32_t search_range_hz; - - if (requested_target_frequency_hz >= start_frequency_hz) + else if (accel_hz_per_s == 0U) { - search_range_hz = requested_target_frequency_hz - start_frequency_hz; + freq_hz = target_limit_hz; } else - { - search_range_hz = start_frequency_hz - requested_target_frequency_hz; - } - highest_offset_hz = search_range_hz; - - while (lowest_offset_hz <= highest_offset_hz) - { - candidate_offset_hz = lowest_offset_hz + ((highest_offset_hz - lowest_offset_hz) / 2UL); - if (requested_target_frequency_hz >= start_frequency_hz) - { - candidate_frequency_hz = start_frequency_hz + candidate_offset_hz; - acceleration_pulses = PlsrAccelCurveCalculateRampPulses(start_frequency_hz, candidate_frequency_hz, accel_rate_hz_per_s, curve_mode); - } - else - { - candidate_frequency_hz = start_frequency_hz - candidate_offset_hz; - acceleration_pulses = PlsrAccelCurveCalculateRampPulses(start_frequency_hz, candidate_frequency_hz, decel_rate_hz_per_s, curve_mode); - } - deceleration_pulses = PlsrAccelCurveCalculateRampPulses(candidate_frequency_hz, end_frequency_hz, - (end_frequency_hz > candidate_frequency_hz) ? accel_rate_hz_per_s : decel_rate_hz_per_s, curve_mode); - - if ((acceleration_pulses + deceleration_pulses) <= total_pulses) - { - best_offset_hz = candidate_offset_hz; - if (candidate_offset_hz == search_range_hz) - { - break; - } - lowest_offset_hz = candidate_offset_hz + 1UL; - if (lowest_offset_hz > highest_offset_hz) + { + squared_freq = (uint64_t)start_freq_hz * (uint64_t)start_freq_hz; + if (freq_rising != 0U) + { + squared_freq += 2ULL * (uint64_t)accel_hz_per_s * (uint64_t)pulse_count; + freq_hz = PlsrAccelCurveIntSqrt(squared_freq); + if (freq_hz > target_limit_hz) { - break; + freq_hz = target_limit_hz; } } else { - if (candidate_offset_hz == 0U) + uint64_t freq_drop = 2ULL * (uint64_t)accel_hz_per_s * (uint64_t)pulse_count; + if (freq_drop >= squared_freq) { - break; + freq_hz = target_limit_hz; + } + else + { + freq_hz = PlsrAccelCurveIntSqrt(squared_freq - freq_drop); + if (freq_hz < target_limit_hz) + { + freq_hz = target_limit_hz; + } } - highest_offset_hz = candidate_offset_hz - 1UL; } } - - if (requested_target_frequency_hz >= start_frequency_hz) + if (freq_hz < 1U) { - return start_frequency_hz + best_offset_hz; + freq_hz = 1U; } - return start_frequency_hz - best_offset_hz; + return PlsrAccelCurveClampFreqHz(freq_hz); } -/** 按峰值重算加速/减速脉冲预算 */ -static void PlsrAccelCurveCalculatePhasePulses(uint32_t start_frequency_hz, - uint32_t peak_frequency_hz, - uint32_t end_frequency_hz, - uint32_t accel_rate_hz_per_s, - uint32_t decel_rate_hz_per_s, - PlsrAccelMode_e curve_mode, - uint32_t *accel_pulses, - uint32_t *decel_pulses) +/** + * 直线：用上一拍 freq_hz ±1 逼近 target_freq_squared，不全量开方。 + * 低频 freq_diff_hz/dn 大时循环次数多，但周期也长，ISR 仍可承受。 + */ +static uint32_t PlsrAccelNextFreqLinear(PlsrAccelRuntime_t *runtime) { - if (start_frequency_hz > peak_frequency_hz) + uint64_t target_freq_squared; + uint64_t cur_freq_squared; + uint64_t start_freq_squared; + uint32_t freq_hz; + uint32_t iteration_guard; + + start_freq_squared = (uint64_t)runtime->start_freq_hz * (uint64_t)runtime->start_freq_hz; + if (runtime->freq_rising != 0U) { - *accel_pulses = PlsrAccelCurveCalculateRampPulses(start_frequency_hz, peak_frequency_hz, decel_rate_hz_per_s, curve_mode); + target_freq_squared = start_freq_squared + (2ULL * (uint64_t)runtime->accel_hz_per_s * (uint64_t)runtime->completed_pulses); } else { - *accel_pulses = PlsrAccelCurveCalculateRampPulses(start_frequency_hz, peak_frequency_hz, accel_rate_hz_per_s, curve_mode); - } - if (end_frequency_hz > peak_frequency_hz) - { - *decel_pulses = PlsrAccelCurveCalculateRampPulses(peak_frequency_hz, end_frequency_hz, accel_rate_hz_per_s, curve_mode); + uint64_t freq_drop = 2ULL * (uint64_t)runtime->accel_hz_per_s * (uint64_t)runtime->completed_pulses; + target_freq_squared = (freq_drop >= start_freq_squared) ? 0ULL : (start_freq_squared - freq_drop); } - else + + freq_hz = runtime->cur_freq_hz; + if (freq_hz < 1U) { - *decel_pulses = PlsrAccelCurveCalculateRampPulses(peak_frequency_hz, end_frequency_hz, decel_rate_hz_per_s, curve_mode); + freq_hz = 1U; } -} - -/** - * FitPeak 后再对齐离散取整：禁止只截断 decel_pulses，逐步降峰直到 acc+dec<=total。 - */ -static uint32_t PlsrAccelCurveFitTargetToPulseBudget(uint32_t total_pulses, - uint32_t start_frequency_hz, - uint32_t requested_target_frequency_hz, - uint32_t end_frequency_hz, - uint32_t accel_rate_hz_per_s, - uint32_t decel_rate_hz_per_s, - PlsrAccelMode_e curve_mode, - uint32_t *accel_pulses, - uint32_t *decel_pulses) -{ - uint32_t peak_frequency_hz; - uint32_t iteration_guard; - - peak_frequency_hz = PlsrAccelCurveFitTargetFrequency(total_pulses, start_frequency_hz, requested_target_frequency_hz, end_frequency_hz, - accel_rate_hz_per_s, decel_rate_hz_per_s, curve_mode); - PlsrAccelCurveCalculatePhasePulses(start_frequency_hz, peak_frequency_hz, end_frequency_hz, accel_rate_hz_per_s, decel_rate_hz_per_s, curve_mode, - accel_pulses, decel_pulses); - iteration_guard = 0U; - while ((*accel_pulses + *decel_pulses) > total_pulses && (iteration_guard < 100000U)) + for (iteration_guard = 0U; iteration_guard < 2048U; iteration_guard++) { - iteration_guard++; - if (requested_target_frequency_hz >= start_frequency_hz) + cur_freq_squared = (uint64_t)freq_hz * (uint64_t)freq_hz; + if (runtime->freq_rising != 0U) { - if (peak_frequency_hz <= start_frequency_hz) + if ((freq_hz < runtime->end_freq_hz) && (((uint64_t)(freq_hz + 1U) * (uint64_t)(freq_hz + 1U)) <= target_freq_squared)) { - break; + freq_hz++; + continue; } - peak_frequency_hz--; + if ((freq_hz > 1U) && (cur_freq_squared > target_freq_squared)) + { + freq_hz--; + continue; + } + break; } else { - if (peak_frequency_hz >= start_frequency_hz) + if ((freq_hz > runtime->end_freq_hz) && (cur_freq_squared > target_freq_squared)) { - break; + freq_hz--; + continue; + } + if ((freq_hz < 100000U) && + (((uint64_t)(freq_hz + 1U) * (uint64_t)(freq_hz + 1U)) < target_freq_squared)) + { + freq_hz++; + continue; } - peak_frequency_hz++; + break; } - PlsrAccelCurveCalculatePhasePulses(start_frequency_hz, peak_frequency_hz, end_frequency_hz, accel_rate_hz_per_s, decel_rate_hz_per_s, curve_mode, - accel_pulses, decel_pulses); } - return peak_frequency_hz; -} - -/** - * @brief 规划一整段脉冲域（见 plsr_accel_curve.h） - */ -void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, - uint32_t total_pulses, - uint32_t start_frequency_hz, - uint32_t target_frequency_hz, - uint32_t end_frequency_hz, - uint32_t default_speed_hz, - uint32_t acceleration_time_ms, - uint32_t deceleration_time_ms, - PlsrAccelMode_e curve_mode) -{ - uint32_t accel_pulses; - uint32_t decel_pulses; - uint32_t peak_frequency_hz; - uint32_t accel_rate_hz_per_s; - uint32_t decel_rate_hz_per_s; - PlsrAccelMode_e selected_mode; - if (plan == (PlsrAccelPlan_t *)0) + if (runtime->freq_rising != 0U) { - return; + if (freq_hz > runtime->end_freq_hz) + { + freq_hz = runtime->end_freq_hz; + } + } + else + { + if (freq_hz < runtime->end_freq_hz) + { + freq_hz = runtime->end_freq_hz; + } + } + if (freq_hz < 1U) + { + freq_hz = 1U; } + return PlsrAccelCurveClampFreqHz(freq_hz); +} - start_frequency_hz = PlsrAccelCurveClampFrequencyHz(start_frequency_hz); - target_frequency_hz = PlsrAccelCurveClampFrequencyHz(target_frequency_hz); - end_frequency_hz = PlsrAccelCurveClampFrequencyHz(end_frequency_hz); +/*============================================================================*/ +/* S / 正弦 */ +/*============================================================================*/ - selected_mode = (curve_mode > PLSR_ACCEL_SINE) ? PLSR_ACCEL_LINEAR : curve_mode; +static uint32_t PlsrAccelCurveShapeSinePermille(uint32_t progress_permille) +{ + uint32_t table_index; + uint32_t seg_progress; + uint32_t min_val; + uint32_t max_val; - if (start_frequency_hz < 1U) + if (progress_permille >= 1000U) { - start_frequency_hz = PlsrAccelCurveResolveStartHz(0U, target_frequency_hz, default_speed_hz, - acceleration_time_ms, deceleration_time_ms); + return 1000U; } - if (end_frequency_hz < 1U) + /* progress_permille/1000 → 表下标 0..32，相邻点线性插值 */ + table_index = (progress_permille * 32UL) / 1000UL; + if (table_index >= 32U) { - end_frequency_hz = PlsrAccelCurveResolveEndHz(0U, target_frequency_hz, default_speed_hz, - acceleration_time_ms, deceleration_time_ms); + return 1000U; } + seg_progress = (progress_permille * 32UL) - (table_index * 1000UL); /* 0..999 对应段内进度 */ + min_val = (uint32_t)s_plsr_sine_shape_tab[table_index]; + max_val = (uint32_t)s_plsr_sine_shape_tab[table_index + 1U]; + return min_val + + (((max_val - min_val) * seg_progress) / 1000UL); +} - accel_rate_hz_per_s = - PlsrAccelCurveCalculateRate(default_speed_hz, - start_frequency_hz, - target_frequency_hz, - acceleration_time_ms); - decel_rate_hz_per_s = - PlsrAccelCurveCalculateRate(default_speed_hz, - target_frequency_hz, - end_frequency_hz, - deceleration_time_ms); - - peak_frequency_hz = target_frequency_hz; - - plan->start_frequency_hz = start_frequency_hz; - plan->end_frequency_hz = end_frequency_hz; - plan->curve_mode = selected_mode; - plan->accel_rate_hz_per_s = accel_rate_hz_per_s; - plan->decel_rate_hz_per_s = decel_rate_hz_per_s; +/** + * S 曲线时间参数（加速度梯形 1:2:1）： + * Tj : Ta : Tj = 1 : 2 : 1 + * Δf = A*(Ta+Tj) ⇒ Tj=Δf/(3A), Ta=2Δf/(3A), T=4Δf/(3A) + */ +static void PlsrAccelCurveCalcSCurveTimes(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s, + uint32_t *jerk_time_us_out, + uint32_t *const_accel_time_us_out, + uint32_t *ramp_time_us_out) +{ + uint32_t freq_diff_hz; + uint64_t numerator; + uint64_t denominator; + uint32_t calc_jerk_time_us; - PlsrAccelCurveCalculatePhasePulses(start_frequency_hz, - peak_frequency_hz, - end_frequency_hz, - accel_rate_hz_per_s, - decel_rate_hz_per_s, - selected_mode, - &accel_pulses, - &decel_pulses); + *jerk_time_us_out = 0U; + *const_accel_time_us_out = 0U; + *ramp_time_us_out = 0U; - /* --- 特例：本段 0 脉冲 --- */ - if (total_pulses == 0U) + freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); + if ((freq_diff_hz < 1U) || (accel_hz_per_s < 1U)) { - plan->constant_pulses = 0U; - plan->target_frequency_hz = peak_frequency_hz; - plan->accel_pulses = 0U; - plan->decel_pulses = 0U; return; } - /* - * --- 特例：出口=目标 且 光加速就要用光 N 个脉冲 --- - * 整段当纯加速/纯减，无 dec、无 const（例如短段只爬到目标就停） - */ - if ((end_frequency_hz == target_frequency_hz) && (accel_pulses > total_pulses)) + /* Tj_us = ceil(freq_diff_hz * 1e6 / (3A)) */ + numerator = (uint64_t)freq_diff_hz * 1000000ULL; + denominator = 3ULL * (uint64_t)accel_hz_per_s; + calc_jerk_time_us = + (uint32_t)((numerator + denominator - 1ULL) / denominator); + if (calc_jerk_time_us < 1U) { - plan->constant_pulses = 0U; - plan->target_frequency_hz = peak_frequency_hz; - plan->accel_pulses = total_pulses; - plan->decel_pulses = 0U; - return; + calc_jerk_time_us = 1U; } + *jerk_time_us_out = calc_jerk_time_us; + *const_accel_time_us_out = calc_jerk_time_us * 2U; + *ramp_time_us_out = calc_jerk_time_us * 4U; +} - /* - * --- 主分支：脉冲够不够装下 acc + dec + const --- - * 够：constant_pulses = 余量（标准梯形） - * 不够：constant_pulses=0，FitPeak 降低 peak_frequency_hz 直到 accel_pulses+decel_pulses <= N（三角波，可能报 0x02） - */ - if (accel_pulses + decel_pulses <= total_pulses) - { - plan->constant_pulses = total_pulses - accel_pulses - decel_pulses; - } - else +/** 正弦 raised-cosine：峰值加速度 = A ⇒ T = π·Δf / (2A) */ +static void PlsrAccelCurveCalcSineCurveTime(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s, + uint32_t *ramp_time_us) +{ + uint32_t freq_diff_hz; + uint64_t numerator; + uint64_t denominator; + + *ramp_time_us = 0U; + freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); + if ((freq_diff_hz < 1U) || (accel_hz_per_s < 1U)) { - /* - * 脉冲不够：先砍掉匀速段，保证加减速按完整剖面预算； - * 仍超出则降峰成三角，禁止 decel_pulses=total-accel_pulses 式截断。 - */ - plan->constant_pulses = 0U; - peak_frequency_hz = PlsrAccelCurveFitTargetToPulseBudget(total_pulses, start_frequency_hz, target_frequency_hz, end_frequency_hz, - accel_rate_hz_per_s, decel_rate_hz_per_s, selected_mode, - &accel_pulses, &decel_pulses); + return; } - - /* - * --- 补丁：峰值 > 出口 但 decel_pulses 算出来为 0 --- - * 强制留 1 个脉冲做收尾，从 acc 或 const 里挪 1 个给 dec - */ - if ((peak_frequency_hz > end_frequency_hz) && (total_pulses > 1U) && (decel_pulses == 0U)) + /* T_us = ceil(π*freq_diff_hz*1e6 / (2A))，π≈355/113 */ + numerator = (uint64_t)freq_diff_hz * 1000000ULL * 355ULL; + denominator = 2ULL * (uint64_t)accel_hz_per_s * 113ULL; + *ramp_time_us = (uint32_t)((numerator + denominator - 1ULL) / denominator); + if (*ramp_time_us < 1U) { - decel_pulses = 1U; - if (accel_pulses >= total_pulses) - { - accel_pulses = total_pulses - 1U; - } - if (plan->constant_pulses > 0U) - { - plan->constant_pulses--; - } - else - { - plan->constant_pulses = total_pulses - accel_pulses - decel_pulses; - } + *ramp_time_us = 1U; } +} - plan->target_frequency_hz = peak_frequency_hz; - plan->accel_pulses = accel_pulses; - plan->decel_pulses = decel_pulses; +/** Δf = A * elapsed_time_us / 1e6 */ +static uint32_t PlsrAccelCurveDeltaHzFromAt(uint32_t accel_hz_per_s, uint32_t elapsed_time_us) +{ + return (uint32_t)(((uint64_t)accel_hz_per_s * (uint64_t)elapsed_time_us) / 1000000ULL); } -/** 按 S/正弦时间曲线计算指定脉冲位置的频率。 */ -static uint32_t PlsrAccelCurveTimedFrequencyAtPulse( - uint32_t start_frequency_hz, - uint32_t end_frequency_hz, - uint32_t rate_hz_per_s, - PlsrAccelMode_e curve_mode, - uint32_t completed_pulses) +/** Δf = A * elapsed_time_us^2 / (2 * jerk_time_us * 1e6) — jerk 段 */ +static uint32_t PlsrAccelCurveDeltaHzFromJerk(uint32_t accel_hz_per_s, uint32_t elapsed_time_us, + uint32_t jerk_time_us) { - uint32_t jerk_time_us = 0U; - uint32_t constant_accel_time_us = 0U; - uint32_t ramp_time_us = 0U; - uint32_t elapsed_time_us = 0U; - uint32_t current_frequency_hz = start_frequency_hz; - uint32_t pulse_index; + uint64_t calc; - if (curve_mode == PLSR_ACCEL_S) - { - PlsrAccelCurveCalculateSCurveTimes(start_frequency_hz, - end_frequency_hz, - rate_hz_per_s, - &jerk_time_us, - &constant_accel_time_us, - &ramp_time_us); - } - else + if ((jerk_time_us < 1U) || (elapsed_time_us < 1U) || (accel_hz_per_s < 1U)) { - PlsrAccelCurveCalculateSineCurveTime(start_frequency_hz, - end_frequency_hz, - rate_hz_per_s, - &ramp_time_us); + return 0U; } - - for (pulse_index = 0U; pulse_index < completed_pulses; pulse_index++) + /* ((A * t / jerk_time_us) * t) / 2e6 */ + calc = ((uint64_t)accel_hz_per_s * (uint64_t)elapsed_time_us) / (uint64_t)jerk_time_us; + calc = (calc * (uint64_t)elapsed_time_us) / 2000000ULL; + if (calc > 0xFFFFFFFFULL) { - if (current_frequency_hz < 1U) - { - current_frequency_hz = 1U; - } - elapsed_time_us += 1000000UL / current_frequency_hz; - - if (curve_mode == PLSR_ACCEL_S) - { - current_frequency_hz = - PlsrAccelCurveSCurveFrequencyAtTime(start_frequency_hz, - end_frequency_hz, - rate_hz_per_s, - jerk_time_us, - constant_accel_time_us, - ramp_time_us, - elapsed_time_us); - } - else - { - current_frequency_hz = - PlsrAccelCurveSineFrequencyAtTime(start_frequency_hz, - end_frequency_hz, - ramp_time_us, - elapsed_time_us); - } + return 0xFFFFFFFFUL; } - return current_frequency_hz; + return (uint32_t)calc; } /** - * @brief 按已完成脉冲数取频（任务侧；ISR 用 PlsrAccelNextFrequency） + * 时刻 elapsed_time_us 的频率（相对进度 0..freq_diff_hz，再映射到 start_freq_hz→end_freq_hz）。 */ -uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, - uint32_t completed_segment_pulses) +static uint32_t PlsrAccelCurveSCurveFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t accel_hz_per_s, + uint32_t jerk_time_us, uint32_t const_accel_time_us, + uint32_t ramp_time_us, uint32_t elapsed_time_us) { - uint32_t constant_phase_end; - uint32_t pulse_count; - uint32_t frequency_hz; - uint8_t frequency_rising; - - if (plan == (PlsrAccelPlan_t *)0) - { - return 0U; - } + uint32_t freq_diff_hz; + uint32_t freq_change_hz; + uint32_t decel_start_time_us; + uint32_t decel_elapsed_us; + uint8_t freq_rising; - constant_phase_end = plan->accel_pulses + plan->constant_pulses; + freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); + freq_rising = (end_freq_hz >= start_freq_hz) ? 1U : 0U; - /* 加速相：0 .. accel_pulses-1 完成后仍在加速；completed_segment_pulses 为已完成数 */ - if ((plan->accel_pulses > 0U) && (completed_segment_pulses < plan->accel_pulses) && - (plan->start_frequency_hz != plan->target_frequency_hz)) + if ((freq_diff_hz < 1U) || (ramp_time_us < 1U) || (accel_hz_per_s < 1U)) { - frequency_rising = (plan->target_frequency_hz >= plan->start_frequency_hz) ? 1U : 0U; - if (plan->curve_mode == PLSR_ACCEL_LINEAR) - { - 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); - } - else - { - uint32_t selected_rate_hz_per_s = (frequency_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; - - frequency_hz = PlsrAccelCurveTimedFrequencyAtPulse( - plan->start_frequency_hz, - plan->target_frequency_hz, - selected_rate_hz_per_s, - plan->curve_mode, - completed_segment_pulses); - } - if ((frequency_hz < 1U) && (plan->target_frequency_hz >= 1U)) - { - frequency_hz = 1U; - } - return PlsrAccelCurveClampFrequencyHz(frequency_hz); + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - - /* 匀速 */ - if (completed_segment_pulses < constant_phase_end) + if (elapsed_time_us >= ramp_time_us) { - return PlsrAccelCurveClampFrequencyHz(plan->target_frequency_hz); + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - /* 减速 / 无减速 */ - if ((plan->decel_pulses == 0U) || (plan->target_frequency_hz == plan->end_frequency_hz)) + if ((jerk_time_us < 1U) || (elapsed_time_us <= jerk_time_us)) { - return PlsrAccelCurveClampFrequencyHz(plan->end_frequency_hz); + /* jerk 升：freq_change_hz = 0.5*(A/Tj)*t^2 */ + freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, elapsed_time_us, (jerk_time_us < 1U) ? 1U : jerk_time_us); } - - pulse_count = completed_segment_pulses - constant_phase_end; - if (pulse_count >= plan->decel_pulses) + else { - return PlsrAccelCurveClampFrequencyHz(plan->end_frequency_hz); + decel_start_time_us = jerk_time_us + const_accel_time_us; + if (elapsed_time_us <= decel_start_time_us) + { + /* 恒加速：freq_change_hz = 0.5*A*Tj + A*(t-Tj) */ + freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, jerk_time_us, jerk_time_us); + freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, elapsed_time_us - jerk_time_us); + } + else + { + /* jerk 降：freq_change_hz = 0.5*A*Tj + A*Ta + A*τ - 0.5*(A/Tj)*τ^2 */ + decel_elapsed_us = elapsed_time_us - decel_start_time_us; + if (decel_elapsed_us > jerk_time_us) + { + decel_elapsed_us = jerk_time_us; + } + freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, jerk_time_us, jerk_time_us); + freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, const_accel_time_us); + freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, decel_elapsed_us); + { + uint32_t freq_drop = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, decel_elapsed_us, jerk_time_us); + if (freq_change_hz > freq_drop) + { + freq_change_hz -= freq_drop; + } + else + { + freq_change_hz = 0U; + } + } + } } - frequency_rising = (plan->end_frequency_hz >= plan->target_frequency_hz) ? 1U : 0U; - if (plan->curve_mode == PLSR_ACCEL_LINEAR) + if (freq_change_hz > freq_diff_hz) { - 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); + freq_change_hz = freq_diff_hz; } - else + if (freq_rising != 0U) { - uint32_t selected_rate_hz_per_s = (frequency_rising != 0U) ? plan->accel_rate_hz_per_s : plan->decel_rate_hz_per_s; - - frequency_hz = PlsrAccelCurveTimedFrequencyAtPulse( - plan->target_frequency_hz, - plan->end_frequency_hz, - selected_rate_hz_per_s, - plan->curve_mode, - pulse_count); + return PlsrAccelCurveClampFreqHz(start_freq_hz + freq_change_hz); } - if (frequency_hz < 1U) + if (start_freq_hz > freq_change_hz) { - frequency_hz = 1U; + return PlsrAccelCurveClampFreqHz(start_freq_hz - freq_change_hz); } - return PlsrAccelCurveClampFrequencyHz(frequency_hz); + return PlsrAccelCurveClampFreqHz(end_freq_hz); } -/*============================================================================*/ -/* 逐拍运行态（ISR） */ -/*============================================================================*/ - /** - * @brief 为 S/正弦准备下一拍频率 + * 正弦 raised-cosine：freq_hz = start_freq_hz + freq_diff_hz * (1-cos(π progress_permille))/2，progress_permille=t/T + * 用现有 33 点 sine 形状表（已是 raised-cosine 千分比）。 */ -static uint32_t PlsrAccelNextFrequencyTimed(PlsrAccelRuntime_t *runtime); - -/* S/正弦频率预计算表：PlanSeg 时填好；ISR 只切换/查表。匀速不改频。 */ -#define PLSR_RAMP_TBL_MAX 4096U -static uint32_t s_acceleration_frequency_table[PLSR_RAMP_TBL_MAX]; -static uint32_t s_deceleration_frequency_table[PLSR_RAMP_TBL_MAX]; -static uint32_t s_acceleration_table_length; -static uint32_t s_acceleration_table_stride; -static uint32_t s_deceleration_table_length; -static uint32_t s_deceleration_table_stride; - -static void PlsrAccelRuntimeCalculateTimes(PlsrAccelRuntime_t *runtime) +static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, + uint32_t ramp_time_us, uint32_t elapsed_time_us) { - runtime->elapsed_time_us = 0U; - runtime->jerk_time_us = 0U; - runtime->constant_accel_time_us = 0U; - runtime->ramp_time_us = 0U; + uint32_t progress_permille; + uint32_t ratio_permille; + uint32_t freq_hz; - if ((runtime->acceleration_hz_per_s < 1U) || (runtime->start_frequency_hz == runtime->end_frequency_hz)) + if ((ramp_time_us < 1U) || (start_freq_hz == end_freq_hz)) { - return; + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - if (runtime->curve_mode == PLSR_ACCEL_S) + if (elapsed_time_us >= ramp_time_us) { - PlsrAccelCurveCalculateSCurveTimes(runtime->start_frequency_hz, runtime->end_frequency_hz, runtime->acceleration_hz_per_s, - &runtime->jerk_time_us, &runtime->constant_accel_time_us, &runtime->ramp_time_us); + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - else if (runtime->curve_mode == PLSR_ACCEL_SINE) + progress_permille = (uint32_t)(((uint64_t)elapsed_time_us * 1000ULL) / (uint64_t)ramp_time_us); + if (progress_permille > 1000U) + { + progress_permille = 1000U; + } + ratio_permille = PlsrAccelCurveShapeSinePermille(progress_permille); + freq_hz = PlsrAccelCurveInterpolate(start_freq_hz, end_freq_hz, ratio_permille); + if (freq_hz < 1U) { - PlsrAccelCurveCalculateSineCurveTime(runtime->start_frequency_hz, runtime->end_frequency_hz, runtime->acceleration_hz_per_s, &runtime->ramp_time_us); + freq_hz = 1U; } + return PlsrAccelCurveClampFreqHz(freq_hz); } -/** 填写一个加速或减速相的运行数据。 */ -static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, - uint32_t start_frequency_hz, - uint32_t end_frequency_hz, - uint32_t total_pulses, - uint32_t accel_rate_hz_per_s, - uint32_t decel_rate_hz_per_s, - PlsrAccelMode_e curve_mode) +/** 按 S/正弦时间曲线计算指定脉冲位置的频率。 */ +static uint32_t PlsrAccelCurveTimedFreqAtPulse( + uint32_t start_freq_hz, + uint32_t end_freq_hz, + uint32_t rate_hz_per_s, + PlsrAccelMode_e curve_mode, + uint32_t completed_pulses) { - runtime->start_frequency_hz = start_frequency_hz; - runtime->end_frequency_hz = end_frequency_hz; - runtime->current_frequency_hz = start_frequency_hz; - runtime->completed_pulses = 0U; - runtime->total_pulses = total_pulses; - runtime->frequency_rising = - (end_frequency_hz >= start_frequency_hz) ? 1U : 0U; - runtime->acceleration_hz_per_s = - (runtime->frequency_rising != 0U) ? - accel_rate_hz_per_s : decel_rate_hz_per_s; - runtime->curve_mode = curve_mode; - runtime->is_active = ((total_pulses > 0U) && - (start_frequency_hz != end_frequency_hz)) ? 1U : 0U; - runtime->frequency_table_id = 0U; - runtime->table_length = 0U; - runtime->table_stride = 1U; - PlsrAccelRuntimeCalculateTimes(runtime); -} + uint32_t jerk_time_us = 0U; + uint32_t const_accel_time_us = 0U; + uint32_t ramp_time_us = 0U; + uint32_t elapsed_time_us = 0U; + uint32_t cur_freq_hz = start_freq_hz; + uint32_t pulse_index; -/** 离散仿真一步：与建表 / 脉冲预算共用 */ -static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime) -{ - if (runtime->completed_pulses < 0xFFFFFFFFUL) + if (curve_mode == PLSR_ACCEL_S) { - runtime->completed_pulses++; + PlsrAccelCurveCalcSCurveTimes(start_freq_hz, + end_freq_hz, + rate_hz_per_s, + &jerk_time_us, + &const_accel_time_us, + &ramp_time_us); } - runtime->current_frequency_hz = PlsrAccelNextFrequencyTimed(runtime); - if ((runtime->ramp_time_us > 0U) && (runtime->elapsed_time_us >= runtime->ramp_time_us)) + else { - runtime->current_frequency_hz = runtime->end_frequency_hz; - runtime->is_active = 0U; + PlsrAccelCurveCalcSineCurveTime(start_freq_hz, + end_freq_hz, + rate_hz_per_s, + &ramp_time_us); } - else if (runtime->frequency_rising != 0U) + + for (pulse_index = 0U; pulse_index < completed_pulses; pulse_index++) { - if (runtime->current_frequency_hz >= runtime->end_frequency_hz) + if (cur_freq_hz < 1U) { - runtime->current_frequency_hz = runtime->end_frequency_hz; - runtime->is_active = 0U; + cur_freq_hz = 1U; } - } - else - { - if (runtime->current_frequency_hz <= runtime->end_frequency_hz) + elapsed_time_us += 1000000UL / cur_freq_hz; + + if (curve_mode == PLSR_ACCEL_S) { - runtime->current_frequency_hz = runtime->end_frequency_hz; - runtime->is_active = 0U; + cur_freq_hz = + PlsrAccelCurveSCurveFreqAtTime(start_freq_hz, + end_freq_hz, + rate_hz_per_s, + jerk_time_us, + const_accel_time_us, + ramp_time_us, + elapsed_time_us); + } + else + { + cur_freq_hz = + PlsrAccelCurveSineFreqAtTime(start_freq_hz, + end_freq_hz, + ramp_time_us, + elapsed_time_us); } } + return cur_freq_hz; } -/** - * 将本相离散频率写入 frequency_table[]，返回写入长度；*table_stride_out 为抽样步长。 - */ -static uint32_t PlsrAccelRuntimeBuildFrequencyTable(PlsrAccelRuntime_t *runtime, - uint32_t *frequency_table, - uint32_t table_capacity, - uint32_t *table_stride_out) +/*仿真离散脉冲频率，算出该段加减速一共需要输出多少个脉冲*/ +static uint32_t PlsrAccelCurveSimulateRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, + uint32_t accel_hz_per_s, + PlsrAccelMode_e curve_mode) { - PlsrAccelRuntime_t simulation; - uint32_t loop_index; - uint32_t table_index; - uint32_t table_stride; - uint32_t table_length; + PlsrAccelRuntime_t runtime; + uint32_t iteration_guard; - *table_stride_out = 1U; - if ((runtime->is_active == 0U) || (runtime->total_pulses < 1U) || - (runtime->curve_mode == PLSR_ACCEL_LINEAR) || (frequency_table == (uint32_t *)0) || - (table_capacity < 1U)) + if (start_freq_hz == end_freq_hz || accel_hz_per_s == 0U) { return 0U; } - table_stride = 1U; - if (runtime->total_pulses > table_capacity) - { - table_stride = (runtime->total_pulses + table_capacity - 1U) / table_capacity; - } - *table_stride_out = table_stride; - - simulation = *runtime; - simulation.completed_pulses = 0U; - simulation.current_frequency_hz = runtime->start_frequency_hz; - simulation.is_active = 1U; - PlsrAccelRuntimeCalculateTimes(&simulation); - if (simulation.ramp_time_us < 1U) + PlsrAccelRuntimeBeginRamp(&runtime, + start_freq_hz, + end_freq_hz, + 0xFFFFFFFFUL, + accel_hz_per_s, + accel_hz_per_s, + curve_mode); + if (runtime.ramp_time_us < 1U) { - frequency_table[0] = runtime->end_frequency_hz; return 1U; } - table_length = 0U; - for (loop_index = 0U; loop_index < runtime->total_pulses; loop_index++) + iteration_guard = 0U; + while ((runtime.is_active != 0U) && (iteration_guard < 5000000U)) { - PlsrAccelRuntimeSimulateOnePulse(&simulation); - table_index = (simulation.completed_pulses - 1U) / table_stride; - if (table_index >= table_capacity) - { - table_index = table_capacity - 1U; - } - frequency_table[table_index] = simulation.current_frequency_hz; - if ((table_index + 1U) > table_length) - { - table_length = table_index + 1U; - } - if (simulation.is_active == 0U) - { - while ((loop_index + 1U) < runtime->total_pulses) - { - loop_index++; - table_index = loop_index / table_stride; - if (table_index >= table_capacity) - { - table_index = table_capacity - 1U; - } - frequency_table[table_index] = runtime->end_frequency_hz; - if ((table_index + 1U) > table_length) - { - table_length = table_index + 1U; - } - } - break; - } + iteration_guard++; + PlsrAccelRuntimeSimulateOnePulse(&runtime); } - if (table_length > 0U) + if (runtime.completed_pulses == 0U) { - frequency_table[table_length - 1U] = runtime->end_frequency_hz; + return 1U; } - return table_length; + return runtime.completed_pulses; } /** - * PlanSeg 后预建加/减速表（任务上下文，勿在 ISR 里做）。 + * S/正弦时间域步进：上一拍周期推进 t，再按时间取下一拍频率。 */ -void PlsrAccelPrebuildFrequencyTables(const PlsrAccelPlan_t *plan) +static uint32_t PlsrAccelNextFreqTimed(PlsrAccelRuntime_t *runtime) { - PlsrAccelRuntime_t temporary_runtime; - - s_acceleration_table_length = 0U; - s_deceleration_table_length = 0U; - s_acceleration_table_stride = 1U; - s_deceleration_table_stride = 1U; + uint32_t previous_freq_hz; + uint32_t pulse_period_us; + uint32_t freq_hz; - if ((plan == (const PlsrAccelPlan_t *)0) || - (plan->curve_mode == PLSR_ACCEL_LINEAR)) + if ((runtime->ramp_time_us < 1U) || (runtime->accel_hz_per_s < 1U)) { - return; + return PlsrAccelCurveClampFreqHz(runtime->end_freq_hz); } - if ((plan->accel_pulses > 0U) && (plan->start_frequency_hz != plan->target_frequency_hz)) + previous_freq_hz = runtime->cur_freq_hz; + if (previous_freq_hz < 1U) { - PlsrAccelRuntimeBeginRamp(&temporary_runtime, - plan->start_frequency_hz, - plan->target_frequency_hz, - plan->accel_pulses, - plan->accel_rate_hz_per_s, - plan->decel_rate_hz_per_s, - plan->curve_mode); - s_acceleration_table_length = PlsrAccelRuntimeBuildFrequencyTable(&temporary_runtime, s_acceleration_frequency_table, - PLSR_RAMP_TBL_MAX, - &s_acceleration_table_stride); + previous_freq_hz = 1U; } + pulse_period_us = 1000000UL / previous_freq_hz; - if ((plan->decel_pulses > 0U) && (plan->target_frequency_hz != plan->end_frequency_hz)) + if (runtime->elapsed_time_us < (0xFFFFFFFFUL - pulse_period_us)) { - PlsrAccelRuntimeBeginRamp(&temporary_runtime, - plan->target_frequency_hz, - plan->end_frequency_hz, - plan->decel_pulses, - plan->accel_rate_hz_per_s, - plan->decel_rate_hz_per_s, - plan->curve_mode); - s_deceleration_table_length = PlsrAccelRuntimeBuildFrequencyTable(&temporary_runtime, s_deceleration_frequency_table, - PLSR_RAMP_TBL_MAX, - &s_deceleration_table_stride); + runtime->elapsed_time_us += pulse_period_us; + } + else + { + runtime->elapsed_time_us = 0xFFFFFFFFUL; } -} - -/** 选择当前相使用的加速表或减速表，并记录表长度和抽样步长。 */ -static void PlsrAccelRuntimeSelectFrequencyTable(PlsrAccelRuntime_t *runtime, - uint8_t frequency_table_id, - uint32_t table_length, - uint32_t table_stride) -{ - runtime->table_length = table_length; - runtime->table_stride = (table_stride < 1U) ? 1U : table_stride; - runtime->frequency_table_id = (table_length > 0U) ? frequency_table_id : 0U; -} -/** @brief 进入加速相 */ -void PlsrAccelBeginAcceleration(PlsrAccelRuntime_t *runtime, - const PlsrAccelPlan_t *plan) -{ - if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + if (runtime->elapsed_time_us >= runtime->ramp_time_us) { - return; + return PlsrAccelCurveClampFreqHz(runtime->end_freq_hz); } - PlsrAccelRuntimeBeginRamp(runtime, - plan->start_frequency_hz, - plan->target_frequency_hz, - plan->accel_pulses, - plan->accel_rate_hz_per_s, - plan->decel_rate_hz_per_s, - plan->curve_mode); - PlsrAccelRuntimeSelectFrequencyTable(runtime, 1U, s_acceleration_table_length, - s_acceleration_table_stride); -} -/** @brief 进入减速相 */ -void PlsrAccelBeginDeceleration(PlsrAccelRuntime_t *runtime, - const PlsrAccelPlan_t *plan) -{ - if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + if (runtime->curve_mode == PLSR_ACCEL_S) { - return; + freq_hz = PlsrAccelCurveSCurveFreqAtTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, + runtime->jerk_time_us, runtime->const_accel_time_us, runtime->ramp_time_us, + runtime->elapsed_time_us); } - PlsrAccelRuntimeBeginRamp(runtime, - plan->target_frequency_hz, - plan->end_frequency_hz, - plan->decel_pulses, - plan->accel_rate_hz_per_s, - plan->decel_rate_hz_per_s, - plan->curve_mode); - /* 表已在 PlanSeg/Prebuild 填好；ISR 内禁止再建表 */ - PlsrAccelRuntimeSelectFrequencyTable(runtime, 2U, s_deceleration_table_length, - s_deceleration_table_stride); + else + { + freq_hz = PlsrAccelCurveSineFreqAtTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->ramp_time_us, runtime->elapsed_time_us); + } + return freq_hz; } -/** @brief 进入匀速相 */ -void PlsrAccelBeginConstantSpeed(PlsrAccelRuntime_t *runtime, - const PlsrAccelPlan_t *plan) +/*============================================================================*/ +/* 规划 */ +/*============================================================================*/ + +static uint32_t PlsrAccelCurveCalcRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, + uint32_t accel_hz_per_s, + PlsrAccelMode_e curve_mode) { - if ((runtime == (PlsrAccelRuntime_t *)0) || (plan == (const PlsrAccelPlan_t *)0)) + if ((curve_mode == PLSR_ACCEL_SINE) || (curve_mode == PLSR_ACCEL_S)) { - return; + return PlsrAccelCurveSimulateRampPulses(start_freq_hz, end_freq_hz, accel_hz_per_s, curve_mode); } - runtime->start_frequency_hz = plan->target_frequency_hz; - runtime->end_frequency_hz = plan->target_frequency_hz; - runtime->current_frequency_hz = plan->target_frequency_hz; - runtime->acceleration_hz_per_s = 0U; - runtime->completed_pulses = 0U; - runtime->total_pulses = plan->constant_pulses; - runtime->frequency_rising = 1U; - runtime->curve_mode = plan->curve_mode; - runtime->is_active = 0U; /* 匀速：ISR 不改频 */ - runtime->frequency_table_id = 0U; - runtime->table_length = 0U; - runtime->table_stride = 1U; - runtime->elapsed_time_us = 0U; - runtime->jerk_time_us = 0U; - runtime->constant_accel_time_us = 0U; - runtime->ramp_time_us = 0U; + return PlsrAccelCurveCalcLinearRampPulses(start_freq_hz, end_freq_hz, accel_hz_per_s); } /** - * 直线：用上一拍 frequency_hz ±1 逼近 target_frequency_squared，不全量开方。 - * 低频 frequency_difference_hz/dn 大时循环次数多，但周期也长，ISR 仍可承受。 + * 二分峰值：使 acc+dec 脉冲 <= total（脉冲闭合公式）。 */ -static uint32_t PlsrAccelNextFrequencyLinear(PlsrAccelRuntime_t *runtime) +static uint32_t PlsrAccelCurveFitTargetFreq(uint32_t total_pulses, + uint32_t start_freq_hz, + uint32_t requested_target_freq_hz, + uint32_t end_freq_hz, + uint32_t accel_rate_hz_per_s, + uint32_t decel_rate_hz_per_s, + PlsrAccelMode_e curve_mode) { - 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 lowest_offset_hz = 0U; + uint32_t highest_offset_hz; + uint32_t candidate_offset_hz; + uint32_t best_offset_hz = 0U; + uint32_t candidate_freq_hz; + uint32_t accel_pulses; + uint32_t decel_pulses; + uint32_t search_range_hz; - start_frequency_squared = (uint64_t)runtime->start_frequency_hz * (uint64_t)runtime->start_frequency_hz; - if (runtime->frequency_rising != 0U) + if (requested_target_freq_hz >= start_freq_hz) { - target_frequency_squared = start_frequency_squared + (2ULL * (uint64_t)runtime->acceleration_hz_per_s * (uint64_t)runtime->completed_pulses); + search_range_hz = requested_target_freq_hz - start_freq_hz; } 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; + search_range_hz = start_freq_hz - requested_target_freq_hz; } + highest_offset_hz = search_range_hz; - for (iteration_guard = 0U; iteration_guard < 2048U; iteration_guard++) + while (lowest_offset_hz <= highest_offset_hz) { - current_frequency_squared = (uint64_t)frequency_hz * (uint64_t)frequency_hz; - if (runtime->frequency_rising != 0U) + candidate_offset_hz = lowest_offset_hz + ((highest_offset_hz - lowest_offset_hz) / 2UL); + if (requested_target_freq_hz >= start_freq_hz) + { + candidate_freq_hz = start_freq_hz + candidate_offset_hz; + accel_pulses = PlsrAccelCurveCalcRampPulses(start_freq_hz, candidate_freq_hz, accel_rate_hz_per_s, curve_mode); + } + else { - if ((frequency_hz < runtime->end_frequency_hz) && (((uint64_t)(frequency_hz + 1U) * (uint64_t)(frequency_hz + 1U)) <= target_frequency_squared)) + candidate_freq_hz = start_freq_hz - candidate_offset_hz; + accel_pulses = PlsrAccelCurveCalcRampPulses(start_freq_hz, candidate_freq_hz, decel_rate_hz_per_s, curve_mode); + } + decel_pulses = PlsrAccelCurveCalcRampPulses(candidate_freq_hz, end_freq_hz, + (end_freq_hz > candidate_freq_hz) ? accel_rate_hz_per_s : decel_rate_hz_per_s, curve_mode); + + if ((accel_pulses + decel_pulses) <= total_pulses) + { + best_offset_hz = candidate_offset_hz; + if (candidate_offset_hz == search_range_hz) { - frequency_hz++; - continue; + break; } - if ((frequency_hz > 1U) && (current_frequency_squared > target_frequency_squared)) + lowest_offset_hz = candidate_offset_hz + 1UL; + if (lowest_offset_hz > highest_offset_hz) { - frequency_hz--; - continue; + break; } - 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)) + if (candidate_offset_hz == 0U) { - frequency_hz++; - continue; + break; } - break; + highest_offset_hz = candidate_offset_hz - 1UL; } } - if (runtime->frequency_rising != 0U) + if (requested_target_freq_hz >= start_freq_hz) { - if (frequency_hz > runtime->end_frequency_hz) - { - frequency_hz = runtime->end_frequency_hz; - } + return start_freq_hz + best_offset_hz; + } + return start_freq_hz - best_offset_hz; +} + +/** 按峰值重算加速/减速脉冲预算 */ +static void PlsrAccelCurveCalcPhasePulses(uint32_t start_freq_hz, + uint32_t peak_freq_hz, + uint32_t end_freq_hz, + uint32_t accel_rate_hz_per_s, + uint32_t decel_rate_hz_per_s, + PlsrAccelMode_e curve_mode, + uint32_t *accel_pulses, + uint32_t *decel_pulses) +{ + if (start_freq_hz > peak_freq_hz) + { + *accel_pulses = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_freq_hz, decel_rate_hz_per_s, curve_mode); } else { - if (frequency_hz < runtime->end_frequency_hz) - { - frequency_hz = runtime->end_frequency_hz; - } + *accel_pulses = PlsrAccelCurveCalcRampPulses(start_freq_hz, peak_freq_hz, accel_rate_hz_per_s, curve_mode); + } + if (end_freq_hz > peak_freq_hz) + { + *decel_pulses = PlsrAccelCurveCalcRampPulses(peak_freq_hz, end_freq_hz, accel_rate_hz_per_s, curve_mode); } - if (frequency_hz < 1U) + else { - frequency_hz = 1U; + *decel_pulses = PlsrAccelCurveCalcRampPulses(peak_freq_hz, end_freq_hz, decel_rate_hz_per_s, curve_mode); } - return PlsrAccelCurveClampFrequencyHz(frequency_hz); } /** - * S/正弦时间域步进：上一拍周期推进 t，再按时间取下一拍频率。 + * FitPeak 后再对齐离散取整：禁止只截断 decel_pulses，逐步降峰直到 acc+dec<=total。 */ -static uint32_t PlsrAccelNextFrequencyTimed(PlsrAccelRuntime_t *runtime) +static uint32_t PlsrAccelCurveFitTargetToPulseBudget(uint32_t total_pulses, + uint32_t start_freq_hz, + uint32_t requested_target_freq_hz, + uint32_t end_freq_hz, + uint32_t accel_rate_hz_per_s, + uint32_t decel_rate_hz_per_s, + PlsrAccelMode_e curve_mode, + uint32_t *accel_pulses, + uint32_t *decel_pulses) { - uint32_t previous_frequency_hz; - uint32_t pulse_period_us; - uint32_t frequency_hz; + uint32_t peak_freq_hz; + uint32_t iteration_guard; - if ((runtime->ramp_time_us < 1U) || (runtime->acceleration_hz_per_s < 1U)) - { - return PlsrAccelCurveClampFrequencyHz(runtime->end_frequency_hz); - } + peak_freq_hz = PlsrAccelCurveFitTargetFreq(total_pulses, start_freq_hz, requested_target_freq_hz, end_freq_hz, + accel_rate_hz_per_s, decel_rate_hz_per_s, curve_mode); + PlsrAccelCurveCalcPhasePulses(start_freq_hz, peak_freq_hz, end_freq_hz, accel_rate_hz_per_s, decel_rate_hz_per_s, curve_mode, + accel_pulses, decel_pulses); - previous_frequency_hz = runtime->current_frequency_hz; - if (previous_frequency_hz < 1U) + iteration_guard = 0U; + while ((*accel_pulses + *decel_pulses) > total_pulses && (iteration_guard < 100000U)) { - previous_frequency_hz = 1U; + iteration_guard++; + if (requested_target_freq_hz >= start_freq_hz) + { + if (peak_freq_hz <= start_freq_hz) + { + break; + } + peak_freq_hz--; + } + else + { + if (peak_freq_hz >= start_freq_hz) + { + break; + } + peak_freq_hz++; + } + PlsrAccelCurveCalcPhasePulses(start_freq_hz, peak_freq_hz, end_freq_hz, accel_rate_hz_per_s, decel_rate_hz_per_s, curve_mode, + accel_pulses, decel_pulses); } - pulse_period_us = 1000000UL / previous_frequency_hz; + return peak_freq_hz; +} - if (runtime->elapsed_time_us < (0xFFFFFFFFUL - pulse_period_us)) - { - runtime->elapsed_time_us += pulse_period_us; - } - else - { - runtime->elapsed_time_us = 0xFFFFFFFFUL; - } +/*============================================================================*/ +/* 运行态 */ +/*============================================================================*/ - if (runtime->elapsed_time_us >= runtime->ramp_time_us) +static void PlsrAccelRuntimeCalcTimes(PlsrAccelRuntime_t *runtime) +{ + runtime->elapsed_time_us = 0U; + runtime->jerk_time_us = 0U; + runtime->const_accel_time_us = 0U; + runtime->ramp_time_us = 0U; + + if ((runtime->accel_hz_per_s < 1U) || (runtime->start_freq_hz == runtime->end_freq_hz)) { - return PlsrAccelCurveClampFrequencyHz(runtime->end_frequency_hz); + return; } - if (runtime->curve_mode == PLSR_ACCEL_S) { - frequency_hz = PlsrAccelCurveSCurveFrequencyAtTime(runtime->start_frequency_hz, runtime->end_frequency_hz, runtime->acceleration_hz_per_s, - runtime->jerk_time_us, runtime->constant_accel_time_us, runtime->ramp_time_us, - runtime->elapsed_time_us); + PlsrAccelCurveCalcSCurveTimes(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, + &runtime->jerk_time_us, &runtime->const_accel_time_us, &runtime->ramp_time_us); } - else + else if (runtime->curve_mode == PLSR_ACCEL_SINE) { - frequency_hz = PlsrAccelCurveSineFrequencyAtTime(runtime->start_frequency_hz, runtime->end_frequency_hz, runtime->ramp_time_us, runtime->elapsed_time_us); + PlsrAccelCurveCalcSineCurveTime(runtime->start_freq_hz, runtime->end_freq_hz, runtime->accel_hz_per_s, &runtime->ramp_time_us); } - return frequency_hz; } -/*仿真离散脉冲频率，算出该段加减速一共需要输出多少个脉冲*/ -static uint32_t PlsrAccelCurveSimulateRampPulses(uint32_t start_frequency_hz, uint32_t end_frequency_hz, - uint32_t acceleration_hz_per_s, - PlsrAccelMode_e curve_mode) +/** 填写一个加速或减速相的运行数据。 */ +static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, + uint32_t start_freq_hz, + uint32_t end_freq_hz, + uint32_t total_pulses, + uint32_t accel_rate_hz_per_s, + uint32_t decel_rate_hz_per_s, + PlsrAccelMode_e curve_mode) { - PlsrAccelRuntime_t runtime; - uint32_t iteration_guard; + runtime->start_freq_hz = start_freq_hz; + runtime->end_freq_hz = end_freq_hz; + runtime->cur_freq_hz = start_freq_hz; + runtime->completed_pulses = 0U; + runtime->total_pulses = total_pulses; + runtime->freq_rising = + (end_freq_hz >= start_freq_hz) ? 1U : 0U; + runtime->accel_hz_per_s = + (runtime->freq_rising != 0U) ? + accel_rate_hz_per_s : decel_rate_hz_per_s; + runtime->curve_mode = curve_mode; + runtime->is_active = ((total_pulses > 0U) && + (start_freq_hz != end_freq_hz)) ? 1U : 0U; + runtime->freq_table_id = 0U; + runtime->table_length = 0U; + runtime->table_stride = 1U; + PlsrAccelRuntimeCalcTimes(runtime); +} - if (start_frequency_hz == end_frequency_hz || acceleration_hz_per_s == 0U) +/** 离散仿真一步：与建表 / 脉冲预算共用 */ +static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime) +{ + if (runtime->completed_pulses < 0xFFFFFFFFUL) { - return 0U; + runtime->completed_pulses++; } - - PlsrAccelRuntimeBeginRamp(&runtime, - start_frequency_hz, - end_frequency_hz, - 0xFFFFFFFFUL, - acceleration_hz_per_s, - acceleration_hz_per_s, - curve_mode); - if (runtime.ramp_time_us < 1U) + runtime->cur_freq_hz = PlsrAccelNextFreqTimed(runtime); + if ((runtime->ramp_time_us > 0U) && (runtime->elapsed_time_us >= runtime->ramp_time_us)) { - return 1U; + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; } - - iteration_guard = 0U; - while ((runtime.is_active != 0U) && (iteration_guard < 5000000U)) + else if (runtime->freq_rising != 0U) { - iteration_guard++; - PlsrAccelRuntimeSimulateOnePulse(&runtime); + if (runtime->cur_freq_hz >= runtime->end_freq_hz) + { + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; + } } - - if (runtime.completed_pulses == 0U) + else { - return 1U; + if (runtime->cur_freq_hz <= runtime->end_freq_hz) + { + runtime->cur_freq_hz = runtime->end_freq_hz; + runtime->is_active = 0U; + } } - return runtime.completed_pulses; } /** - * @brief ISR 热路径：本相再前进 1 脉冲，返回下一拍应写入 ARR 的频率 + * 将本相离散频率写入 freq_table[]，返回写入长度；*table_stride_out 为抽样步长。 */ -uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime) +static uint32_t PlsrAccelRuntimeBuildFreqTable(PlsrAccelRuntime_t *runtime, + uint32_t *freq_table, + uint32_t table_capacity, + uint32_t *table_stride_out) { - const uint32_t *frequency_table; + PlsrAccelRuntime_t simulation; + uint32_t loop_index; uint32_t table_index; + uint32_t table_stride; + uint32_t table_length; - if (runtime == (PlsrAccelRuntime_t *)0) + *table_stride_out = 1U; + if ((runtime->is_active == 0U) || (runtime->total_pulses < 1U) || + (runtime->curve_mode == PLSR_ACCEL_LINEAR) || (freq_table == (uint32_t *)0) || + (table_capacity < 1U)) { return 0U; } - if (runtime->is_active == 0U) - { - return PlsrAccelCurveClampFrequencyHz(runtime->current_frequency_hz); - } - - if (runtime->completed_pulses < 0xFFFFFFFFUL) - { - runtime->completed_pulses++; - } - if (runtime->curve_mode == PLSR_ACCEL_LINEAR) + table_stride = 1U; + if (runtime->total_pulses > table_capacity) { - if (runtime->acceleration_hz_per_s == 0U) - { - runtime->current_frequency_hz = runtime->end_frequency_hz; - } - else - { - runtime->current_frequency_hz = PlsrAccelNextFrequencyLinear(runtime); - } + table_stride = (runtime->total_pulses + table_capacity - 1U) / table_capacity; } - else if (runtime->frequency_table_id != 0U) - { - /* ISR 热路径：只查表，不做 S/正弦实时积分 */ - if (runtime->table_length < 1U) - { - runtime->current_frequency_hz = runtime->end_frequency_hz; - } - else - { - uint32_t table_stride = (runtime->table_stride < 1U) ? - 1U : runtime->table_stride; + *table_stride_out = table_stride; - table_index = (runtime->completed_pulses - 1U) / table_stride; - if (table_index >= runtime->table_length) - { - table_index = runtime->table_length - 1U; - } - frequency_table = (runtime->frequency_table_id == 1U) ? - s_acceleration_frequency_table : - s_deceleration_frequency_table; - runtime->current_frequency_hz = frequency_table[table_index]; - } - } - else + simulation = *runtime; + simulation.completed_pulses = 0U; + simulation.cur_freq_hz = runtime->start_freq_hz; + simulation.is_active = 1U; + PlsrAccelRuntimeCalcTimes(&simulation); + if (simulation.ramp_time_us < 1U) { - /* 建表失败兜底：仍走时间步进 */ - runtime->current_frequency_hz = PlsrAccelNextFrequencyTimed(runtime); + freq_table[0] = runtime->end_freq_hz; + return 1U; } - 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) + table_length = 0U; + for (loop_index = 0U; loop_index < runtime->total_pulses; loop_index++) { - /* - * 未到本相最后一拍：不要钳到 end。 - * 否则减速在 n=N-1 就落到止速，ARPE 下会表现为末两拍同频。 - */ - if (runtime->frequency_rising == 0U) + PlsrAccelRuntimeSimulateOnePulse(&simulation); + table_index = (simulation.completed_pulses - 1U) / table_stride; + if (table_index >= table_capacity) { - if ((runtime->current_frequency_hz <= runtime->end_frequency_hz) && - (runtime->end_frequency_hz < 100000U)) - { - runtime->current_frequency_hz = runtime->end_frequency_hz + 1U; - } + table_index = table_capacity - 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)) + freq_table[table_index] = simulation.cur_freq_hz; + if ((table_index + 1U) > table_length) { - runtime->current_frequency_hz = runtime->end_frequency_hz - 1U; + table_length = table_index + 1U; } - } - else if (runtime->frequency_table_id != 0U) - { - /* 查表：最后一拍之前若已是 end，同样让出 1Hz */ - if ((runtime->total_pulses > 0U) && - (runtime->completed_pulses < runtime->total_pulses)) + if (simulation.is_active == 0U) { - if (runtime->frequency_rising == 0U) + while ((loop_index + 1U) < runtime->total_pulses) { - if ((runtime->current_frequency_hz <= runtime->end_frequency_hz) && - (runtime->end_frequency_hz < 100000U)) + loop_index++; + table_index = loop_index / table_stride; + if (table_index >= table_capacity) { - runtime->current_frequency_hz = runtime->end_frequency_hz + 1U; + table_index = table_capacity - 1U; + } + freq_table[table_index] = runtime->end_freq_hz; + if ((table_index + 1U) > table_length) + { + table_length = table_index + 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; - } + break; } } - else if (runtime->frequency_table_id == 0U) + + if (table_length > 0U) { - if ((runtime->ramp_time_us > 0U) && (runtime->elapsed_time_us >= runtime->ramp_time_us)) - { - runtime->current_frequency_hz = runtime->end_frequency_hz; - runtime->is_active = 0U; - } - else 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; - } + freq_table[table_length - 1U] = runtime->end_freq_hz; } - return PlsrAccelCurveClampFrequencyHz(runtime->current_frequency_hz); + return table_length; +} + +/** 选择当前相使用的加速表或减速表，并记录表长度和抽样步长。 */ +static void PlsrAccelRuntimeSelectFreqTable(PlsrAccelRuntime_t *runtime, + uint8_t freq_table_id, + uint32_t table_length, + uint32_t table_stride) +{ + runtime->table_length = table_length; + runtime->table_stride = (table_stride < 1U) ? 1U : table_stride; + runtime->freq_table_id = (table_length > 0U) ? freq_table_id : 0U; } + diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index f1fd23c..f498b1d 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -17,29 +17,29 @@ */ typedef struct { uint32_t accel_pulses; /* 入口到目标频率需要的脉冲数 */ - uint32_t constant_pulses; /* 保持目标频率的脉冲数 */ + uint32_t const_pulses; /* 保持目标频率的脉冲数 */ uint32_t decel_pulses; /* 目标到出口频率需要的脉冲数 */ - uint32_t start_frequency_hz; /* 本段实际入口频率 */ - uint32_t target_frequency_hz; /* 实际目标频率，短段可能被降低 */ - uint32_t end_frequency_hz; /* 本段出口频率 */ + uint32_t start_freq_hz; /* 本段实际入口频率 */ + uint32_t target_freq_hz; /* 实际目标频率，短段可能被降低 */ + uint32_t end_freq_hz; /* 本段出口频率 */ uint32_t accel_rate_hz_per_s; /* 升高频率时使用的变化率 */ uint32_t decel_rate_hz_per_s; /* 降低频率时使用的变化率 */ PlsrAccelMode_e curve_mode; /* 直线 / S / 正弦 */ } PlsrAccelPlan_t; /** 配置起速 → 规划入口：起速>目标用起速；否则目标与起跳比较取其一 */ -uint32_t PlsrAccelCurveResolveStartHz(uint32_t configured_frequency_hz, - uint32_t target_frequency_hz, +uint32_t PlsrAccelCurveResolveStartHz(uint32_t configured_freq_hz, + uint32_t target_freq_hz, uint32_t default_speed_hz, - uint32_t acceleration_time_ms, - uint32_t deceleration_time_ms); + uint32_t accel_time_ms, + uint32_t decel_time_ms); /** 配置止速 → 规划出口：止速>目标用止速；否则目标与起跳比较取其一（不默认到 0） */ -uint32_t PlsrAccelCurveResolveEndHz(uint32_t configured_frequency_hz, - uint32_t target_frequency_hz, +uint32_t PlsrAccelCurveResolveEndHz(uint32_t configured_freq_hz, + uint32_t target_freq_hz, uint32_t default_speed_hz, - uint32_t acceleration_time_ms, - uint32_t deceleration_time_ms); + uint32_t accel_time_ms, + uint32_t decel_time_ms); /** * 规划一整段脉冲域三相预算与实际峰值 @@ -47,17 +47,17 @@ uint32_t PlsrAccelCurveResolveEndHz(uint32_t configured_frequency_hz, */ void PlsrAccelCurvePlan(PlsrAccelPlan_t *plan, uint32_t total_pulses, - uint32_t start_frequency_hz, - uint32_t target_frequency_hz, - uint32_t end_frequency_hz, + uint32_t start_freq_hz, + uint32_t target_freq_hz, + uint32_t end_freq_hz, uint32_t default_speed_hz, - uint32_t acceleration_time_ms, - uint32_t deceleration_time_ms, + uint32_t accel_time_ms, + uint32_t decel_time_ms, PlsrAccelMode_e curve_mode); -/** 按已完成脉冲数取频（任务/预览；ISR 热路径用 PlsrAccelNextFrequency） */ +/** 按已完成脉冲数取频（任务/预览；ISR 热路径用 PlsrAccelNextFreq） */ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, - uint32_t completed_segment_pulses); + uint32_t completed_seg_pulses); /** * 单相逐脉冲运行态（ISR 热路径，每来一个 UPDATE 调 Step 一次） @@ -65,21 +65,21 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, * S/正弦：查预计算频率表（开相 BeginAcc/BeginDec 时填好） */ typedef struct { - uint32_t current_frequency_hz; /* 当前输出频率 */ - uint32_t start_frequency_hz; /* 当前相起点频率 */ - uint32_t end_frequency_hz; /* 当前相终点频率 */ - uint32_t acceleration_hz_per_s; /* 当前相使用的变化率 */ + uint32_t cur_freq_hz; /* 当前输出频率 */ + uint32_t start_freq_hz; /* 当前相起点频率 */ + uint32_t end_freq_hz; /* 当前相终点频率 */ + uint32_t accel_hz_per_s; /* 当前相使用的变化率 */ uint32_t completed_pulses; /* 当前相已经步进的脉冲数 */ uint32_t total_pulses; /* 当前相计划步进的脉冲数 */ uint32_t elapsed_time_us; /* 时间曲线已经经过的时间 */ uint32_t jerk_time_us; /* S 曲线单个 jerk 阶段时间 */ - uint32_t constant_accel_time_us; /* S 曲线恒加速阶段时间 */ + uint32_t const_accel_time_us; /* S 曲线恒加速阶段时间 */ uint32_t ramp_time_us; /* 整个斜坡时间 */ uint32_t table_stride; /* 每几个脉冲读取一个表项 */ uint32_t table_length; /* 表中有效项数 */ - uint8_t frequency_rising; /* 1=频率升高，0=降低 */ + uint8_t freq_rising; /* 1=频率升高，0=降低 */ uint8_t is_active; /* 1=当前相仍在运行 */ - uint8_t frequency_table_id; /* 0=不用表，1=加速表，2=减速表 */ + uint8_t freq_table_id; /* 0=不用表，1=加速表，2=减速表 */ PlsrAccelMode_e curve_mode; } PlsrAccelRuntime_t; @@ -88,16 +88,16 @@ extern PlsrAccelPlan_t g_plsr_accel_plan; extern PlsrAccelRuntime_t g_plsr_accel_runtime; /** PlanSeg 后调用：预建 S/正弦加减速频率表（勿在 ISR） */ -void PlsrAccelPrebuildFrequencyTables(const PlsrAccelPlan_t *plan); +void PlsrAccelPrebuildFreqTables(const PlsrAccelPlan_t *plan); -void PlsrAccelBeginAcceleration(PlsrAccelRuntime_t *runtime, +void PlsrAccelBeginAccel(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan); -void PlsrAccelBeginDeceleration(PlsrAccelRuntime_t *runtime, +void PlsrAccelBeginDecel(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan); -void PlsrAccelBeginConstantSpeed(PlsrAccelRuntime_t *runtime, +void PlsrAccelBeginConstSpeed(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan); /** 本相前进 1 脉冲，返回下一拍命令频率 */ -uint32_t PlsrAccelNextFrequency(PlsrAccelRuntime_t *runtime); +uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime); #endif diff --git a/plsr/command/plsr_command.c b/plsr/command/plsr_command.c index f7c3fe3..02d120f 100644 --- a/plsr/command/plsr_command.c +++ b/plsr/command/plsr_command.c @@ -57,9 +57,9 @@ static void PlsrCommandPublishMonitor(void) if (g_plsr_busy != 0U) { - PlsrCommandWriteDWord(PLSR_MON_FREQ_L, g_plsr_output_frequency_hz); + PlsrCommandWriteDWord(PLSR_MON_FREQ_L, g_plsr_output_freq_hz); WriteHoldReg(PLSR_MON_RUN_STATUS, 1U); - WriteHoldReg(PLSR_MON_CUR_SEG, (uint16_t)(g_plsr_current_segment_index + 1U)); + WriteHoldReg(PLSR_MON_CUR_SEG, (uint16_t)(g_plsr_cur_seg_idx + 1U)); } else { @@ -118,7 +118,7 @@ void PlsrCommandInit(void) */ void PlsrCommandOnSegFreqHoldWrite(uint16_t start_addr, uint16_t quantity) { - uint16_t current_seg;//当前运行段 + uint16_t cur_seg;//当前运行段 uint16_t seg;//当前改频段 uint16_t offset;//偏移量 uint32_t freqency; @@ -147,8 +147,8 @@ void PlsrCommandOnSegFreqHoldWrite(uint16_t start_addr, uint16_t quantity) return; } - current_seg = (uint16_t)(g_plsr_current_segment_index + 1U); - if ((current_seg < 1U) || ((uint16_t)(current_seg - 1U) != seg)) + cur_seg = (uint16_t)(g_plsr_cur_seg_idx + 1U); + if ((cur_seg < 1U) || ((uint16_t)(cur_seg - 1U) != seg)) { return; } @@ -162,7 +162,7 @@ void PlsrCommandOnSegFreqHoldWrite(uint16_t start_addr, uint16_t quantity) return; } - g_plsr_segments[seg].freq_hz = (int32_t)freqency; + g_plsr_segs[seg].freq_hz = (int32_t)freqency; /* 运行中改频只把该段频率写入 BKP */ PlsrPersistSaveSegFreqToBkp(seg, freqency); diff --git a/plsr/param/plsr_param.c b/plsr/param/plsr_param.c index 16ddb6e..98cc3df 100644 --- a/plsr/param/plsr_param.c +++ b/plsr/param/plsr_param.c @@ -3,7 +3,7 @@ * @brief 参数块管理实现：运行映像 + 分帧导入/导出 + ParamBlockTask * * @details 模块职责 - * 维护公共参数 g_plsr_config 和段参数 g_plsr_segments[]，实现寄存器与 + * 维护公共参数 g_plsr_config 和段参数 g_plsr_segs[]，实现寄存器与 * 运行参数之间的双向转换，以及「5+N 分帧」收齐后的异步导入 * * 与其它模块关系 @@ -18,7 +18,7 @@ #include "ucos_ii.h" PlsrCfg_t g_plsr_config; -PlsrSeg_t g_plsr_segments[PLSR_SEG_MAX]; +PlsrSeg_t g_plsr_segs[PLSR_SEG_MAX]; static OS_EVENT *s_param_sem; /* 分帧收齐后唤醒 ParamBlockTask */ static volatile uint16_t s_expect_frames; /* 本批期望总帧数 = 5 + 段数 */ @@ -249,27 +249,27 @@ uint8_t PlsrParamBlockApplyFromHold(void) for (i = 0U; i < n; i++) { base = (uint16_t)(PLSR_REG_SEG1_BASE + i * PLSR_SEG_STRIDE); - g_plsr_segments[i].freq_hz = + g_plsr_segs[i].freq_hz = (int32_t)PlsrParamReadDWord((uint16_t)(base + PLSR_SEG_OFF_FREQ_L)); - if (PlsrParamIsSegFreqValid(g_plsr_segments[i].freq_hz) == 0U) + if (PlsrParamIsSegFreqValid(g_plsr_segs[i].freq_hz) == 0U) { fault = PLSR_ERR_FREQ_ILLEGAL; } - g_plsr_segments[i].pulse_cnt = + g_plsr_segs[i].pulse_cnt = (int32_t)PlsrParamReadDWord((uint16_t)(base + PLSR_SEG_OFF_PULSE_L)); - if (PlsrParamIsPulseCntValid(g_plsr_segments[i].pulse_cnt) == 0U) + if (PlsrParamIsPulseCntValid(g_plsr_segs[i].pulse_cnt) == 0U) { fault = PLSR_ERR_PULSE_RANGE; } - g_plsr_segments[i].wait_type = + g_plsr_segs[i].wait_type = (PlsrWaitType_e)ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_WAIT)); - g_plsr_segments[i].wait_ms = + g_plsr_segs[i].wait_ms = ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_WAIT_MS)); - g_plsr_segments[i].act_ms = + g_plsr_segs[i].act_ms = ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_ACT_MS)); - g_plsr_segments[i].jump_seg = + g_plsr_segs[i].jump_seg = ReadHoldReg((uint16_t)(base + PLSR_SEG_OFF_JUMP)); } } diff --git a/plsr/param/plsr_param.h b/plsr/param/plsr_param.h index b946659..318a02e 100644 --- a/plsr/param/plsr_param.h +++ b/plsr/param/plsr_param.h @@ -4,7 +4,7 @@ * * @details 模块职责 * 1. 定义公共参数区 0x1000~0x1013、段表区 0x1100+i*0x10 的地址与字段语义； - * 2. 维护 g_plsr_config / g_plsr_segments[]； + * 2. 维护 g_plsr_config / g_plsr_segs[]； * 3. 支持上位机「5 公共帧 + N 段帧」分帧 FC10 写入：收齐后 ApplyFromHold； * 4. 上电由 PersistLoad 填 hold；ExportToHold 仅保留作调试接口。 * @@ -168,7 +168,7 @@ typedef struct { /* ---------- 运行参数 ---------- */ extern PlsrCfg_t g_plsr_config; -extern PlsrSeg_t g_plsr_segments[PLSR_SEG_MAX]; +extern PlsrSeg_t g_plsr_segs[PLSR_SEG_MAX]; /** @return 有效段数，钳制到 [0, PLSR_SEG_MAX] */ uint16_t PlsrParamGetSegCount(void); @@ -202,7 +202,7 @@ void PlsrParamBlockTask(void *pArg); void PlsrParamBlockOnHoldWrite(uint16_t start_addr, uint16_t quantity); /** - * @brief 保持寄存器 → g_plsr_config / g_plsr_segments，边拷贝边校验 + * @brief 保持寄存器 → g_plsr_config / g_plsr_segs，边拷贝边校验 * @return 1=全部通过；0=存在门禁故障 */ uint8_t PlsrParamBlockApplyFromHold(void); diff --git a/plsr/plsr.h b/plsr/plsr.h index dc3638a..64228d7 100644 --- a/plsr/plsr.h +++ b/plsr/plsr.h @@ -26,7 +26,7 @@ /* run_control 拥有的共享运行状态；段下标从 0 开始。 */ extern volatile uint8_t g_plsr_busy; -extern volatile uint16_t g_plsr_current_segment_index; +extern volatile uint16_t g_plsr_cur_seg_idx; extern volatile int32_t g_plsr_accumulated_pulses; #include "plsr_param.h" diff --git a/plsr/pulse_driver/plsr_pulse_driver.c b/plsr/pulse_driver/plsr_pulse_driver.c index d496bd4..bde8777 100644 --- a/plsr/pulse_driver/plsr_pulse_driver.c +++ b/plsr/pulse_driver/plsr_pulse_driver.c @@ -33,7 +33,7 @@ TIM_HandleTypeDef htim13; #define PLSR_Y3_PORT GPIOF #define PLSR_Y3_PIN GPIO_PIN_9 -volatile uint32_t g_plsr_output_frequency_hz; +volatile uint32_t g_plsr_output_freq_hz; volatile uint8_t g_plsr_pwm_running; uint8_t g_plsr_direction_forward; uint8_t g_plsr_direction_valid; @@ -272,7 +272,7 @@ static void PlsrPulseDriverTimInitOne(TIM_HandleTypeDef *htim, TIM_TypeDef *inst /** @brief 驱动初始化 */ void PlsrPulseDriverInit(void) { - g_plsr_output_frequency_hz = 0U; + g_plsr_output_freq_hz = 0U; g_plsr_pwm_running = 0U; g_plsr_direction_forward = 0U; g_plsr_direction_valid = 0U; @@ -554,7 +554,7 @@ static void PlsrPulseDriverLoadRegs(uint32_t freq_hz) if (freq_hz == 0U) { PlsrPulseDriverStopHtim(htim); - g_plsr_output_frequency_hz = 0U; + g_plsr_output_freq_hz = 0U; g_plsr_pwm_running = 0U; s_last_psc = 0xFFFFFFFFUL; s_last_arr = 0xFFFFFFFFUL; @@ -578,7 +578,7 @@ static void PlsrPulseDriverLoadRegs(uint32_t freq_hz) ccr = 1U; } - g_plsr_output_frequency_hz = freq_hz; + g_plsr_output_freq_hz = freq_hz; s_last_psc = psc; s_last_arr = arr; s_last_ccr = ccr; @@ -677,7 +677,7 @@ static void PlsrPulseDriverCommitOutput(void) */ static void PlsrPulseDriverStartCommon(uint32_t freq_hz) { - if ((s_regs_prepared == 0U) || (freq_hz != g_plsr_output_frequency_hz) || (freq_hz == 0U)) + if ((s_regs_prepared == 0U) || (freq_hz != g_plsr_output_freq_hz) || (freq_hz == 0U)) { PlsrPulseDriverLoadRegs(freq_hz); } @@ -702,7 +702,7 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz) uint32_t clk_hz; TIM_HandleTypeDef *htim; - if (freq_hz == g_plsr_output_frequency_hz) + if (freq_hz == g_plsr_output_freq_hz) { return; } @@ -715,7 +715,7 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz) if (freq_hz == 0U) { PlsrPulseDriverStopHtim(htim); - g_plsr_output_frequency_hz = 0U; + g_plsr_output_freq_hz = 0U; g_plsr_pwm_running = 0U; s_last_psc = 0xFFFFFFFFUL; s_last_arr = 0xFFFFFFFFUL; @@ -734,7 +734,7 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz) if ((psc == s_last_psc) && (arr == s_last_arr)) { - g_plsr_output_frequency_hz = freq_hz; + g_plsr_output_freq_hz = freq_hz; return; } @@ -760,7 +760,7 @@ void PlsrPulseDriverSetFreq(uint32_t freq_hz) __HAL_TIM_ENABLE_IT(htim, TIM_IT_UPDATE); } - g_plsr_output_frequency_hz = freq_hz; + g_plsr_output_freq_hz = freq_hz; s_last_psc = psc; s_last_arr = arr; } @@ -811,7 +811,7 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) } /* 匀速热路径：频率未变 → 不算量化、不写寄存器 */ - if (freq_hz == g_plsr_output_frequency_hz) + if (freq_hz == g_plsr_output_freq_hz) { return; } @@ -844,7 +844,7 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) if ((psc == s_last_psc) && (arr == s_last_arr)) { - g_plsr_output_frequency_hz = freq_hz; + g_plsr_output_freq_hz = freq_hz; return; } @@ -857,7 +857,7 @@ void PlsrPulseDriverSetFreqIsr(uint32_t freq_hz) __HAL_TIM_SET_AUTORELOAD(htim, arr); __HAL_TIM_SET_COMPARE(htim, TIM_CHANNEL_1, ccr); - g_plsr_output_frequency_hz = freq_hz; + g_plsr_output_freq_hz = freq_hz; s_last_psc = psc; s_last_arr = arr; s_last_ccr = ccr; @@ -909,7 +909,7 @@ void PlsrPulseDriverStop(void) PlsrPulseDriverStopHtim(&htim10); PlsrPulseDriverStopHtim(&htim11); PlsrPulseDriverStopHtim(&htim13); - g_plsr_output_frequency_hz = 0U; + g_plsr_output_freq_hz = 0U; g_plsr_pwm_running = 0U; s_last_psc = 0xFFFFFFFFUL; s_last_arr = 0xFFFFFFFFUL; diff --git a/plsr/pulse_driver/plsr_pulse_driver.h b/plsr/pulse_driver/plsr_pulse_driver.h index 2831339..464720a 100644 --- a/plsr/pulse_driver/plsr_pulse_driver.h +++ b/plsr/pulse_driver/plsr_pulse_driver.h @@ -35,7 +35,7 @@ extern TIM_HandleTypeDef htim11; extern TIM_HandleTypeDef htim13; /* 驱动拥有的真实输出状态，run_control 直接读取，不再保存第二份。 */ -extern volatile uint32_t g_plsr_output_frequency_hz; +extern volatile uint32_t g_plsr_output_freq_hz; extern volatile uint8_t g_plsr_pwm_running; extern uint8_t g_plsr_direction_forward; extern uint8_t g_plsr_direction_valid; diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index 8b533ab..cb44136 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -39,24 +39,24 @@ typedef enum { } RunPhase_e; volatile uint8_t g_plsr_busy; -volatile uint16_t g_plsr_current_segment_index; +volatile uint16_t g_plsr_cur_seg_idx; volatile int32_t g_plsr_accumulated_pulses; static volatile RcState_e s_state; static volatile uint8_t s_forward;/*当前端脉冲方向*/ -static volatile int32_t s_segment_pulses_done; -static volatile int32_t s_segment_pulse_target; +static volatile int32_t s_seg_pulses_done; +static volatile int32_t s_seg_pulse_target; static int32_t s_absolute_origin; static uint8_t s_absolute_origin_locked; static volatile uint8_t s_act_timer_armed; static volatile uint8_t s_act_expired; static volatile uint8_t s_act_waiting_for_pulse_boundary; -static volatile uint8_t s_keep_frequency_after_act; +static volatile uint8_t s_keep_freq_after_act; static volatile uint8_t s_wait_time_expired; static uint8_t s_waiting_for_input_signal; static volatile RunPhase_e s_phase; static uint8_t s_continue_without_stopping; -static uint32_t s_next_segment_start_frequency_hz; -static uint8_t s_next_segment_start_frequency_valid; +static uint32_t s_next_seg_start_freq_hz; +static uint8_t s_next_seg_start_freq_valid; static uint8_t s_stop_after_last_pulse; static volatile uint8_t s_waiting_for_last_period; static OS_EVENT *s_wake_sem; @@ -69,1582 +69,1631 @@ typedef struct { uint16_t decel_ms; /* 本段减速参考时间 ms */ } PlsrSegPlanEndpoints_t; -static void PlsrRunControlAfterSegDone(void); -static void PlsrRunControlBeginSeg(uint16_t seg_idx0); +/* 静态辅助函数前置声明 */ +static void PlsrRunControlWakeDrain(void); +static void PlsrRunControlWakePost(void); +static int16_t PlsrRunControlNextSeg(uint16_t cur_seg_idx); +static uint8_t PlsrRunControlIsEmptyPlaceholderSeg(uint16_t seg_idx); +static int16_t PlsrRunControlNextEffectiveSeg(uint16_t cur_seg_idx); +static uint8_t PlsrRunControlIsForward(uint16_t seg_idx, int32_t acc_pulse); +static int32_t PlsrRunControlAbsLocalPos(void); static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg); static void PlsrRunControlFinishAll(void); +static void PlsrRunControlBeginSeg(uint16_t seg_idx); +static void PlsrRunControlClearChainAndStop(void); +static void PlsrRunControlApplyKeepOrStop(uint8_t allow_act_handoff); +static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg); +static uint8_t PlsrRunControlWillFollowKeepAt(uint16_t cur_seg, int32_t local_pos); static void PlsrRunControlArmActExtOnPulseStart(void); static void PlsrRunControlCutSegToNext(void); static void PlsrRunControlActExpire(void); static void PlsrRunControlEnterPostWaitOrNext(void); -static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg); -static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t f_tgt, - PlsrSegPlanEndpoints_t *endpoints); -static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, - uint32_t f_tgt, uint32_t f_end, - uint16_t accel_ms, uint16_t decel_ms); +static void PlsrRunControlAfterSegDone(void); static void PlsrRunControlCheckPulseCntFault(void); static void PlsrRunControlCheckApproachCurveFault(uint8_t by_pulse_budget); -static void PlsrRunControlOnApproachDone(void); static void PlsrRunControlCheckDecelCurveFault(void); -static int32_t PlsrRunControlAbsLocalPos(void); +static void PlsrRunControlOnApproachDone(void); static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz, uint32_t default_spd, uint32_t *out_hz); -static int16_t PlsrRunControlNextSeg(uint16_t cur_seg_0based); -static uint8_t PlsrRunControlIsForward(uint16_t seg_0based, int32_t acc_pulse); -static void PlsrRunControlClearChainAndStop(void); -static void PlsrRunControlApplyKeepOrStop(uint8_t allow_act_handoff); -static void PlsrRunControlWakePost(void); -static void PlsrRunControlWakeDrain(void); +static void PlsrRunControlApplyOutFreq(uint32_t freq, uint8_t do_start); +static void PlsrRunControlApplyOutFreqIsr(uint32_t freq); +static uint8_t PlsrRunControlHasDecel(void); +static uint32_t PlsrRunControlRemainPulses(void); +static uint8_t PlsrRunControlShouldEnterDecel(uint32_t remain); +static uint32_t PlsrRunControlDecelEnterBudget(uint32_t remain); +static uint32_t PlsrRunControlDecelOutFreq(uint32_t freq, uint32_t remain); +static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget); +static void PlsrRunControlPrimeDecelOnStart(void); +static void PlsrRunControlEnterConstHold(uint8_t do_start); +static void PlsrRunControlEnterConstHoldIsr(void); +static void PlsrRunControlRefreshProfile(uint8_t do_start); +static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx, uint32_t f_tgt, + PlsrSegPlanEndpoints_t *endpoints); +static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, + uint32_t f_tgt, uint32_t f_end, + uint16_t accel_ms, uint16_t decel_ms); -/** - * @brief 解析下一段索引 - * @note jump=0：末段结束，非末段顺序下一段；jump=1..N：跳到对应段；其它结束 - */ -static int16_t PlsrRunControlNextSeg(uint16_t cur_seg_based) -{ - uint16_t n = PlsrParamGetSegCount(); - uint16_t jump = g_plsr_segments[cur_seg_based].jump_seg; +/*============================================================================*/ +/* 公共 API */ +/*============================================================================*/ - if (jump == 0U) - { - if ((n >= 1U) && (cur_seg_based + 1U < n)) - { - return (int16_t)(cur_seg_based + 1U); - } - return -1; - } - if ((jump >= 1U) && (jump <= n)) - { - return (int16_t)(jump - 1U); - } - return -1; +/** @brief 运行控制初始化 */ +void PlsrRunControlInit(void) +{ + s_state = RC_IDLE; + g_plsr_busy = 0U; + s_forward = 1U; + g_plsr_cur_seg_idx = 0U; + s_seg_pulses_done = 0; + s_seg_pulse_target = 0; + g_plsr_accumulated_pulses = 0; + s_absolute_origin = 0; + s_absolute_origin_locked = 0U; + s_continue_without_stopping = 0U; + s_next_seg_start_freq_hz = 0U; + s_next_seg_start_freq_valid = 0U; + g_plsr_accel_plan.accel_pulses = 0U; + g_plsr_accel_plan.decel_pulses = 0U; + g_plsr_accel_runtime.total_pulses = 0U; + s_stop_after_last_pulse = 0U; + s_waiting_for_last_period = 0U; + s_phase = PH_CONST; + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; + s_waiting_for_input_signal = 0U; + /* s_wake_sem 在 OSInit 后由 WakeInit 创建 */ } /** - * @brief 判断空段，无效占位段：频率 0 且脉冲 0 + * @brief 创建任务唤醒信号量 */ -static uint8_t PlsrRunControlIsEmptyPlaceholderSeg(uint16_t seg_based) +void PlsrRunControlWakeInit(void) { - return ((g_plsr_segments[seg_based].freq_hz == 0) && - (g_plsr_segments[seg_based].pulse_cnt == 0)) ? 1U : 0U; + if (s_wake_sem == (OS_EVENT *)0) + { + s_wake_sem = OSSemCreate(0U); + } } /** - * @brief 从 cur 的下一段起，沿 jump 跳过「频率0且脉冲0」无效段，找下一有效段 - * @return 有效段 0-based；无则 -1 + * @brief PlsrTask 休眠 */ -static int16_t PlsrRunControlNextEffectiveSeg(uint16_t cur_seg_based) +void PlsrRunControlWakePend(uint16_t timeout_ticks) { - int16_t next = PlsrRunControlNextSeg(cur_seg_based); - uint16_t guard = 0U; + INT8U err; - while ((next >= 0) && (guard < PLSR_SEG_MAX)) + if (s_wake_sem == (OS_EVENT *)0) { - guard++; - if (PlsrRunControlIsEmptyPlaceholderSeg((uint16_t)next) == 0U) + if (timeout_ticks < 1U) { - return next; + timeout_ticks = 1U; } - next = PlsrRunControlNextSeg((uint16_t)next); + OSTimeDly((INT32U)timeout_ticks); + return; } - return -1; + OSSemPend(s_wake_sem, timeout_ticks, &err); + (void)err; } /** - * @brief 判断段方向：相对看 pulse_cnt 符号；绝对看目标−局部位置 + * @brief 是否在 TIM5 精确等待 */ -static uint8_t PlsrRunControlIsForward(uint16_t seg_based, int32_t acc_pulse) +uint8_t PlsrRunControlIsPreciseWait(void) { - int32_t move; - - if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) + if (s_state == RC_DIR_WAIT) { - move = g_plsr_segments[seg_based].pulse_cnt - acc_pulse; + return 1U; } - else + if ((s_state == RC_WAIT_COND) && (s_waiting_for_input_signal == 0U)) { - move = g_plsr_segments[seg_based].pulse_cnt; + return 1U; } - return (move >= 0) ? 1U : 0U; -} - -/** - * @brief 停表并清频率链 - */ -static void PlsrRunControlClearChainAndStop(void) -{ - PlsrPulseDriverStop(); - s_continue_without_stopping = 0U; - s_next_segment_start_frequency_valid = 0U; - s_next_segment_start_frequency_hz = 0U; + return 0U; } -/** - * @brief 中途切段：同向且有有效频率则不停表衔接，否则停表清链 - * @param allow_act_handoff 1=允许 s_keep_frequency_after_act（边界停表后仍衔接） - */ -static void PlsrRunControlApplyKeepOrStop(uint8_t allow_act_handoff) +/** @brief 启动多段序列 */ +uint8_t PlsrStart(uint16_t start_seg) { - int16_t next_seg_idx; - uint8_t keep; - uint8_t pwm_ok; + uint16_t n; + uint16_t first_seg; + uint32_t f_chk; - next_seg_idx = PlsrRunControlNextSeg(g_plsr_current_segment_index); - pwm_ok = (g_plsr_pwm_running != 0U) || - ((allow_act_handoff != 0U) && (s_keep_frequency_after_act != 0U)); - keep = 0U; - if ((next_seg_idx >= 0) && (pwm_ok != 0U) && (g_plsr_output_frequency_hz >= 1U) && - (PlsrRunControlIsForward((uint16_t)next_seg_idx, PlsrRunControlAbsLocalPos()) == - s_forward)) + if (g_plsr_busy != 0U) { - keep = 1U; + return 0U; } - if (keep != 0U) + n = PlsrParamGetSegCount(); + if (n < 1U) { - PlsrPulseDriverClearOnePulseStop(); - s_continue_without_stopping = 1U; - s_next_segment_start_frequency_valid = 1U; - s_next_segment_start_frequency_hz = g_plsr_output_frequency_hz; + return 0U; } - else + if ((start_seg < 1U) || (start_seg > n)) { - PlsrRunControlClearChainAndStop(); - PlsrPulseDriverClearOnePulseStop(); + start_seg = PlsrParamGetStartSeg(); } -} -/* @brief 开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ -static void PlsrRunControlArmActExtOnPulseStart(void) -{ - uint16_t act_time_ms; - - if ((g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_EXT) || - (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_EXT_OR_DONE)) + first_seg = (uint16_t)(start_seg - 1U); + /* 启动前先检查首段频率，非法时不置忙、不开定时器。 */ + if (PlsrRunControlGetSegTargetFreq(g_plsr_segs[first_seg].freq_hz, + g_plsr_config.default_speed, + &f_chk) == 0U) { - PlsrSignalIoClearEdges(); + PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); + return 0U; } - - s_act_timer_armed = 0U; - s_act_expired = 0U; - s_act_waiting_for_pulse_boundary = 0U; - s_keep_frequency_after_act = 0U; - s_wait_time_expired = 0U; - - if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_ACT) + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; + /* + * 绝对原点：仅在首次 Start（或 ClearAccPulse 之后的第一次）锁定。 + * 相对跑完再切绝对，仍相对「第一次启动时刻」坐标，不以当前位置重定原点。 + */ + if (s_absolute_origin_locked == 0U) { - act_time_ms = g_plsr_segments[g_plsr_current_segment_index].act_ms; - s_act_timer_armed = 1U; - PlsrSignalIoScheduleDelayMs((uint32_t)act_time_ms); + s_absolute_origin = g_plsr_accumulated_pulses; + s_absolute_origin_locked = 1U; } + PlsrSignalIoClearEdges(); + PlsrRunControlBeginSeg(first_seg); + return 1U; } -/** - * @brief EXT信号触发切段 - * - * 同向且 PWM 在跑：不停表，下一段 f_from = 当前频率。 - * 反向或已停表：停表，下一段从起速爬。 - */ -static void PlsrRunControlCutSegToNext(void) +/** @brief 急停 */ +void PlsrStop(void) { PlsrSignalIoCancelDelay(); + PlsrPulseDriverStop(); + PlsrPulseDriverUnlockPsc(); + PlsrPulseDriverClearDir(); + g_plsr_busy = 0U; s_state = RC_IDLE; - s_act_timer_armed = 0U; - s_act_expired = 0U; - s_act_waiting_for_pulse_boundary = 0U; - s_keep_frequency_after_act = 0U; - s_wait_time_expired = 0U; - - PlsrRunControlApplyKeepOrStop(0U); - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); + s_continue_without_stopping = 0U; /*连续运行标志*/ + s_next_seg_start_freq_valid = 0U;/*衔接频率是否有效*/ + s_next_seg_start_freq_hz = 0U; /*段间衔接频率*/ + s_act_timer_armed = 0U;/*ACT定时器是否已启动*/ + s_act_expired = 0U;/*ACT到期请求标志*/ + s_act_waiting_for_pulse_boundary = 0U;/*ACT 时间到，等待脉冲边界切段*/ + s_keep_freq_after_act = 0U;/*ACT 切段后是否保持当前频率衔接*/ + s_wait_time_expired = 0U;/*段后等待到期请求标志*/ + s_waiting_for_last_period = 0U; } -/** - * @brief ACT 时间到（脉冲可能还没发完）：丢掉本段剩余，从当前频率切下一段 - * - * 同向：下一段 f_from=当前频率（逐层跳，不进本段减速）。 - * 反向或无频率：停表，下一段从起速爬。 - * act_handoff_keep：对齐脉冲边界先停表后仍保持衔接。 - */ -static void PlsrRunControlActExpire(void) +/** @brief 运行中改频 */ +uint8_t PlsrChangeFreq(uint32_t new_tgt_hz) { - s_act_timer_armed = 0U; - s_act_expired = 0U; - s_wait_time_expired = 0U; - s_state = RC_IDLE; + uint32_t remain; + uint32_t f_end; - PlsrRunControlApplyKeepOrStop(1U); - s_keep_frequency_after_act = 0U; + if (s_state != RC_RUN) + { + return 0U; + } + /* 0x04：动态改频目标非法则拒收，保持原规划 */ + if ((new_tgt_hz < 1U) || (new_tgt_hz > 100000U)) + { + PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); + return 0U; + } + if (s_seg_pulses_done >= s_seg_pulse_target) + { + return 0U; + } + + remain = (uint32_t)(s_seg_pulse_target - s_seg_pulses_done); + f_end = g_plsr_accel_plan.end_freq_hz; + PlsrRunControlPlanSeg(remain, g_plsr_output_freq_hz, new_tgt_hz, f_end, + g_plsr_config.accel_ms, g_plsr_config.decel_ms); + s_seg_pulse_target = (int32_t)remain; + s_seg_pulses_done = 0; + PlsrRunControlRefreshProfile(0U); + return 1U; +} - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); +/** @brief 清累计与绝对原点 */ +void PlsrClearAccPulse(void) +{ + g_plsr_accumulated_pulses = 0; + s_absolute_origin = 0; + s_absolute_origin_locked = 0U; /* 下次 Start 重新锁定原点 */ } -/** - * @brief 段发完后的等待策略入口 - * - * WAIT时间/信号/EXT信号 → 停表清链后进 RC_WAIT_COND 或立即 GotoNext； - * EXT_OR_DONE 脉冲先完 → 立刻下一段；其它 → GotoNextOrFinish。 - */ -static void PlsrRunControlEnterPostWaitOrNext(void) +/** @brief 上电恢复累计 */ +void PlsrRunControlRestoreAccPulse(int32_t acc_pulse) { - uint16_t wms; + g_plsr_accumulated_pulses = acc_pulse; + s_absolute_origin = 0; + s_absolute_origin_locked = 0U; /* 下次 Start 以当前累计为绝对原点 */ +} - if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_TIME) +/** @brief 毫秒任务节拍 */ +void PlsrRunControlTickMs(void) +{ + if (s_act_expired != 0U) { - wms = g_plsr_segments[g_plsr_current_segment_index].wait_ms; - if (wms == 0U) + s_act_expired = 0U; + if ((s_state == RC_RUN) || (s_keep_freq_after_act != 0U)) { - wms = 1U; + PlsrRunControlActExpire(); } - PlsrRunControlClearChainAndStop(); - s_waiting_for_input_signal = 0U; - PlsrRunControlWakeDrain();/*清理过期令牌*/ - s_state = RC_WAIT_COND; - PlsrSignalIoScheduleDelayMs((uint32_t)wms); return; } - if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_SIGNAL) + if (s_wait_time_expired != 0U) { - /* 与 WAIT 时间相同：本段发完停表，再等 EXTI 确认后的下降沿 */ - PlsrRunControlClearChainAndStop(); - if (PlsrSignalIoTakeWaitFalling() != 0U) + s_wait_time_expired = 0U; + if (s_state == RC_WAIT_COND) { - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); - return; + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); } - s_waiting_for_input_signal = 1U; - s_state = RC_WAIT_COND; + return; + } + if (s_state == RC_DIR_WAIT) + { + /* 精确延时只靠 TIM5；此处占位，避免任务空转改状态 */ return; } - if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_EXT) + if (s_state == RC_RUN) { - /* EXT信号：脉冲发完后仍要等 EXT 沿才跳转 */ - PlsrRunControlClearChainAndStop(); - /* 只清消抖中的假沿；已确认的 fell 保留，供下面 TakeExtFalling 消费 */ - PlsrSignalIoClearPending(); - if (PlsrSignalIoTakeExtFalling() != 0U) + PlsrPulseDriverApplyPending(); + + if ((g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) || + (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE)) { - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); - return; + if (PlsrSignalIoTakeExtFalling() != 0U) + { + PlsrRunControlCutSegToNext(); + return; + } } - s_waiting_for_input_signal = 1U; - s_state = RC_WAIT_COND; return; } - /* - * EXT_OR_DONE：脉冲先发完 = 条件已满足，立刻下一段。 - * EXT 先到：RUN 里已 CutSegToNext，走不到这里。 - */ - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); + if (s_state == RC_WAIT_COND) + { + if (s_waiting_for_input_signal != 0U) + { + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) + { + if (PlsrSignalIoTakeExtFalling() != 0U) + { + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); + } + } + else + { + if (PlsrSignalIoTakeWaitFalling() != 0U) + { + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); + } + } + } + /* 时间 WAIT / ACT 剩余：TIM5 OnDelayTimer 置 wait_expire_req */ + } } /** - * @brief 解析下一段：无则返回-1进 FinishAll，有则 BeginSeg + * @brief TIM5 one-shot 到期 + * @note ACT：收完当前脉冲并锁低输出，OPM 在周期末停表；不预测加减速脉冲数 */ -static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg) +void PlsrRunControlOnDelayTimer(void) { - int16_t next_seg_idx = PlsrRunControlNextSeg(cur_seg); + if (s_state == RC_DIR_WAIT) + { + s_state = RC_RUN; + PlsrRunControlRefreshProfile(1U); + PlsrRunControlArmActExtOnPulseStart(); + PlsrRunControlWakePost(); + return; + } - if (next_seg_idx < 0) + if ((s_state == RC_RUN) && (s_act_timer_armed != 0U)) { - PlsrRunControlFinishAll(); + /* 到点：当前脉冲下降沿后锁低，周期结束时 OPM 停表。 */ + s_act_timer_armed = 0U; + s_act_waiting_for_pulse_boundary = 1U; + PlsrPulseDriverArmActStop(); + return; } - else + + if ((s_state == RC_WAIT_COND) && (s_waiting_for_input_signal == 0U)) { - PlsrRunControlBeginSeg((uint16_t)next_seg_idx); + s_wait_time_expired = 1U; + PlsrRunControlWakePost(); } } -/** - * 0x01 脉冲个数诊断 - * 时机：段正常结束进入 AfterSegDone 时。 - * 条件：本段目标 s_segment_pulse_target>0 且实际发出 s_segment_pulses_done 与目标不等。 - * 处理：告警，不中断后续切段/收尾逻辑。 - */ -static void PlsrRunControlCheckPulseCntFault(void) +/** @brief 脉冲 UPDATE ISR */ +void PlsrOnPulseIsr(void) { - if ((s_segment_pulse_target > 0) && (s_segment_pulses_done != s_segment_pulse_target)) + uint32_t remain; + uint32_t next; + + if (s_state != RC_RUN) { - PlsrCommandSetFault(PLSR_ERR_PULSE_CNT); + return; } -} -/* 0x03 加速曲线诊断 */ -static void PlsrRunControlCheckApproachCurveFault(uint8_t by_pulse_budget) -{ - if (g_plsr_accel_plan.accel_pulses == 0U) + if (s_waiting_for_last_period != 0U) { + s_waiting_for_last_period = 0U; + s_act_waiting_for_pulse_boundary = 0U; + + PlsrPulseDriverHoldOutputLow(); + PlsrRunControlAfterSegDone(); return; } - if (by_pulse_budget != 0U) + + s_seg_pulses_done++; + if (s_forward != 0U) { - if (g_plsr_accel_runtime.completed_pulses != g_plsr_accel_plan.accel_pulses) - { - PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); - } + g_plsr_accumulated_pulses++; } - else if (g_plsr_accel_runtime.completed_pulses > g_plsr_accel_plan.accel_pulses) + else { - PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); + g_plsr_accumulated_pulses--; } -} -/* 0x03 减速曲线诊断 */ -static void PlsrRunControlCheckDecelCurveFault(void) -{ - if ((s_phase == PH_DECEL) && (g_plsr_accel_runtime.total_pulses > 0U) && - (g_plsr_accel_runtime.completed_pulses < g_plsr_accel_runtime.total_pulses)) + if (s_act_waiting_for_pulse_boundary != 0U) { - PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); - } -} + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 1U; -/** - * @brief 段正常结束：诊断 → 链式频率 → FOLLOW keep 或停表 → EnterPostWaitOrNext - */ -static void PlsrRunControlAfterSegDone(void) -{ - uint8_t keep_pwm; + PlsrPulseDriverHoldOutputLow(); + s_state = RC_IDLE; + s_act_expired = 1U; + PlsrRunControlWakePost(); + return; + } - /* 段末诊断：0x01 脉冲个数；若在减速相再核 0x03 */ - PlsrRunControlCheckPulseCntFault(); - PlsrRunControlCheckDecelCurveFault(); + if (s_seg_pulses_done >= s_seg_pulse_target) + { + if (s_stop_after_last_pulse != 0U) + { + PlsrPulseDriverArmSegEndStop(); + s_waiting_for_last_period = 1U; + return; + } + /* FOLLOW 不停表衔接：立刻切段，PWM 连续 */ + PlsrRunControlAfterSegDone(); + return; + } - /* 先脱离 RUN，避免脉冲 ISR 重入 */ - s_state = RC_IDLE; + remain = PlsrRunControlRemainPulses(); - /* 脉冲已发完但 ACT 时间未到 → 停表，等到到期再开下一段 */ - if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_ACT) + if (s_phase == PH_CONST) { - PlsrPulseDriverStop(); - PlsrPulseDriverClearOnePulseStop(); - s_continue_without_stopping = 0U; - s_next_segment_start_frequency_valid = 0U; - s_next_segment_start_frequency_hz = 0U; - s_act_expired = 0U; - if (s_act_timer_armed != 0U) + /* 匀速：不改频、不写 ARR，仅在 remain 进入减速窗口时切相并查表改频 */ + PlsrPulseDriverClearStartPeriodProtect(); + if (PlsrRunControlShouldEnterDecel(remain) != 0U) { - uint32_t remain_ms = PlsrSignalIoDelayRemainMs(); + PlsrRunControlEnterDecel(g_plsr_accel_plan.target_freq_hz, + PlsrRunControlDecelEnterBudget(remain)); + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); + next = PlsrRunControlDecelOutFreq(next, remain); + g_plsr_accel_runtime.cur_freq_hz = next; + PlsrRunControlApplyOutFreqIsr(next); + } + } + else if (s_phase == PH_APPROACH) + { + /* 每拍 Step 得下一频；若加速相结束则进 CONST 或 DECEL */ + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); - PlsrSignalIoCancelDelay();//段发完后，wait时间取消旧定时器，重新计算剩余wait时间 - s_act_timer_armed = 0U; - if (remain_ms >= 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_freq_hz >= g_plsr_accel_plan.start_freq_hz) && + (next >= g_plsr_accel_plan.target_freq_hz)) || + ((g_plsr_accel_plan.target_freq_hz < g_plsr_accel_plan.start_freq_hz) && + (next <= g_plsr_accel_plan.target_freq_hz))) + { + next = g_plsr_accel_plan.target_freq_hz; + PlsrRunControlOnApproachDone(); + if (PlsrRunControlShouldEnterDecel(remain) != 0U) { - s_waiting_for_input_signal = 0U; - PlsrRunControlWakeDrain(); - s_state = RC_WAIT_COND; - PlsrSignalIoScheduleDelayMs(remain_ms); - return; + PlsrRunControlEnterDecel(next, + PlsrRunControlDecelEnterBudget(remain)); + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); + next = PlsrRunControlDecelOutFreq(next, remain); + g_plsr_accel_runtime.cur_freq_hz = next; + PlsrRunControlApplyOutFreqIsr(next); + } + else + { + PlsrRunControlEnterConstHoldIsr(); } } else { - PlsrSignalIoCancelDelay();//计时已到，取消定时 + PlsrRunControlApplyOutFreqIsr(next); } - s_act_timer_armed = 0U; - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); - return; - } - - s_act_timer_armed = 0U; - s_act_expired = 0U; - s_wait_time_expired = 0U; - /*硬件脉冲是否关闭，1不关，0关闭*/ - keep_pwm = (s_stop_after_last_pulse == 0U) ? 1U : 0U; - - if (g_plsr_config.send_mode == PLSR_SEND_COMPLETE) - { - /* 段正常结束：下一段起速优先用本段落地频（EXT_OR_DONE 完成时常为峰值） */ - s_next_segment_start_frequency_hz = g_plsr_accel_plan.end_frequency_hz; - s_next_segment_start_frequency_valid = 1U; } - - if (keep_pwm != 0U) - { - /* 后续同向且无门禁：不停表直接 BeginSeg */ - s_continue_without_stopping = 1U; - } - else + else /* PH_DECEL */ { - PlsrPulseDriverStop(); - PlsrPulseDriverClearOnePulseStop(); - s_continue_without_stopping = 0U; - if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) + 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_freq_hz; + } + else { - s_next_segment_start_frequency_valid = 0U; + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); } + next = PlsrRunControlDecelOutFreq(next, remain); + g_plsr_accel_runtime.cur_freq_hz = next; + PlsrRunControlApplyOutFreqIsr(next); } - - PlsrRunControlEnterPostWaitOrNext(); } +/*============================================================================*/ +/* 唤醒 */ +/*============================================================================*/ + /** - * @brief 哪些等待类型禁止后续模式不停表衔接 - * @return 1=必须停表等条件；0=允许（含 EXT_OR_DONE 脉冲先完） + * ISR/任务：唤醒 PlsrTask（二进制语义：先排空再 Post，避免残留令牌使下一段秒醒） */ -static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg) +static void PlsrRunControlWakeDrain(void) { - if ((g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_TIME) || - (g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_SIGNAL) || - (g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_ACT) || - (g_plsr_segments[cur_seg].wait_type == PLSR_WAIT_EXT)) + if (s_wake_sem != (OS_EVENT *)0) { - return 1U; + while (OSSemAccept(s_wake_sem) > 0U) + { + /* 全部丢弃令牌 */ + } } - return 0U; } -/* 解析本段规划端点：freq_start / freq_end / accel_ms / decel_ms */ -static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx0, uint32_t f_tgt, - PlsrSegPlanEndpoints_t *endpoints) +static void PlsrRunControlWakePost(void) { - endpoints->accel_ms = g_plsr_config.accel_ms; - endpoints->decel_ms = g_plsr_config.decel_ms; - - if ((s_continue_without_stopping != 0U) && (g_plsr_output_frequency_hz >= 1U)) - { - endpoints->freq_start_hz = g_plsr_output_frequency_hz; - } - else if ((g_plsr_pwm_running != 0U) && (g_plsr_output_frequency_hz >= 1U)) - { - endpoints->freq_start_hz = g_plsr_output_frequency_hz; - } - else if ((s_next_segment_start_frequency_valid != 0U) && (s_next_segment_start_frequency_hz >= 1U)) - { - /* 完成模式段末停表后：用本段落地频作起速（如 EXT_OR_DONE 的匀速峰值） */ - endpoints->freq_start_hz = s_next_segment_start_frequency_hz; - s_next_segment_start_frequency_valid = 0U; - s_next_segment_start_frequency_hz = 0U; - } - else - { - /* 停表后冷启动 */ - endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(g_plsr_config.start_speed, - f_tgt, - g_plsr_config.default_speed, - endpoints->accel_ms, - endpoints->decel_ms); - } - - /* 终止频率 */ + if (s_wake_sem != (OS_EVENT *)0) { - int16_t next_eff = PlsrRunControlNextEffectiveSeg(seg_idx0); - - if ((g_plsr_segments[seg_idx0].wait_type == PLSR_WAIT_EXT_OR_DONE) && - (next_eff >= 0)) - { - int32_t end_pos = PlsrRunControlAbsLocalPos(); - uint8_t next_fwd; - - if (s_forward != 0U) - { - end_pos += s_segment_pulse_target; - } - else - { - end_pos -= s_segment_pulse_target; - } - next_fwd = PlsrRunControlIsForward((uint16_t)next_eff, end_pos); - - /* 与下一有效段同向 → 后续/完成衔接（换向才落到下方止速） */ - if (next_fwd == s_forward) - { - if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) - { - if (PlsrRunControlGetSegTargetFreq( - g_plsr_segments[(uint16_t)next_eff].freq_hz, - g_plsr_config.default_speed, - &endpoints->freq_end_hz) != 0U) - { - return 1U; - } - endpoints->freq_end_hz = f_tgt; - return 1U; - } - endpoints->freq_end_hz = f_tgt; - return 1U; - } - /* 与下一有效段反向：落入下方规划到止速 */ - } + (void)OSSemPost(s_wake_sem); } - - endpoints->freq_end_hz = PlsrAccelCurveResolveEndHz(g_plsr_config.end_speed, - f_tgt, - g_plsr_config.default_speed, - endpoints->accel_ms, - endpoints->decel_ms); - return 1U; } +/*============================================================================*/ +/* 段导航 */ +/*============================================================================*/ + /** - * @brief 段末是否可不停表衔接下一段 - * @param local_pos 用于判下一段方向的局部坐标 - * @note 须 FOLLOW + 有同向下一段 + 无 BlocksFollowKeep + * @brief 解析下一段索引 + * @note jump=0：末段结束，非末段顺序下一段；jump=1..N：跳到对应段；其它结束 */ -static uint8_t PlsrRunControlWillFollowKeepAt(uint16_t cur_seg, int32_t local_pos) +static int16_t PlsrRunControlNextSeg(uint16_t cur_seg_idx) { - int16_t next_eff; + uint16_t n = PlsrParamGetSegCount(); + uint16_t jump = g_plsr_segs[cur_seg_idx].jump_seg; - if (g_plsr_config.send_mode != PLSR_SEND_FOLLOW) - { - return 0U; - } - if (PlsrRunControlNextSeg(cur_seg) < 0) - { - return 0U; - } - next_eff = PlsrRunControlNextEffectiveSeg(cur_seg); - if (next_eff < 0) - { - return 0U; - } - /* - * 直接下一段为无效占位时不能「跳过」该段，但仍可与下一有效段同向 keep - * （EXT_OR_DONE 同向不应因中间占位而强制停到 0 再爬）。 - */ - if (PlsrRunControlIsForward((uint16_t)next_eff, local_pos) != s_forward) + if (jump == 0U) { - return 0U; + if ((n >= 1U) && (cur_seg_idx + 1U < n)) + { + return (int16_t)(cur_seg_idx + 1U); + } + return -1; } - if (PlsrRunControlBlocksFollowKeep(cur_seg) != 0U) + if ((jump >= 1U) && (jump <= n)) { - return 0U; + return (int16_t)(jump - 1U); } - return 1U; + return -1; } /** - * 解析段目标频率。 - * freq_hz==0 用公共默认速度；结果须落在 [1, 100000]，否则返回 0（调用方写 0x04）。 + * @brief 判断空段，无效占位段：频率 0 且脉冲 0 */ -static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz, - uint32_t default_spd, - uint32_t *out_hz) +static uint8_t PlsrRunControlIsEmptyPlaceholderSeg(uint16_t seg_idx) { - uint32_t f; - - if (freq_hz < 0) - { - return 0U; - } - if (freq_hz == 0) - { - f = default_spd; - } - else - { - f = (uint32_t)freq_hz; - } - /* 合法范围：1Hz ~ 100kHz（含） */ - if ((f < 1U) || (f > 100000U)) - { - return 0U; - } - *out_hz = f; - return 1U; + return ((g_plsr_segs[seg_idx].freq_hz == 0) && + (g_plsr_segs[seg_idx].pulse_cnt == 0)) ? 1U : 0U; } -/** 任务侧运行输出：0=停表；任务上下文 SetFreq / Start */ -static void PlsrRunControlApplyOutFreq(uint32_t freq, uint8_t do_start) +/** + * @brief 从 cur 的下一段起，沿 jump 跳过「频率0且脉冲0」无效段，找下一有效段 + * @return 有效段 0-based；无则 -1 + */ +static int16_t PlsrRunControlNextEffectiveSeg(uint16_t cur_seg_idx) { - if (freq == 0U) - { - PlsrPulseDriverStop(); - return; - } + int16_t next = PlsrRunControlNextSeg(cur_seg_idx); + uint16_t guard = 0U; - if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) - { - PlsrPulseDriverStart(freq); - } - else if (freq != g_plsr_output_frequency_hz) + while ((next >= 0) && (guard < PLSR_SEG_MAX)) { - PlsrPulseDriverSetFreq(freq); + guard++; + if (PlsrRunControlIsEmptyPlaceholderSeg((uint16_t)next) == 0U) + { + return next; + } + next = PlsrRunControlNextSeg((uint16_t)next); } + return -1; } -/** 脉冲 ISR：只走 ARR 预装载路径 */ -static void PlsrRunControlApplyOutFreqIsr(uint32_t freq) +/** + * @brief 判断段方向：相对看 pulse_cnt 符号；绝对看目标−局部位置 + */ +static uint8_t PlsrRunControlIsForward(uint16_t seg_idx, int32_t acc_pulse) { - if (freq < 1U) + int32_t move; + + if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) { - freq = 1U; + move = g_plsr_segs[seg_idx].pulse_cnt - acc_pulse; } - PlsrPulseDriverSetFreqIsr(freq); -} - -static uint8_t PlsrRunControlHasDecel(void) -{ - /* - * 第二相脉冲预算 >0 即需要收尾斜坡。 - * 谷底形（起/止速 > 目标）：第二相是从目标再加速回止速，f_end > f_tgt。 - */ - return (g_plsr_accel_plan.decel_pulses > 0U) ? 1U : 0U; -} - -static uint32_t PlsrRunControlRemainPulses(void) -{ - if (s_segment_pulse_target > s_segment_pulses_done) + else { - return (uint32_t)(s_segment_pulse_target - s_segment_pulses_done); + move = g_plsr_segs[seg_idx].pulse_cnt; } - return 0U; + return (move >= 0) ? 1U : 0U; } -/* remain==dec_n 时进入减速 */ -static uint8_t PlsrRunControlShouldEnterDecel(uint32_t remain) +/** 绝对模式：相对本次启动原点的位置 */ +static int32_t PlsrRunControlAbsLocalPos(void) { - if (PlsrRunControlHasDecel() == 0U) - { - return 0U; - } - return (remain <= g_plsr_accel_plan.decel_pulses) ? 1U : 0U; + return g_plsr_accumulated_pulses - s_absolute_origin; } -static uint32_t PlsrRunControlDecelEnterBudget(uint32_t remain) +/** + * @brief 解析下一段：无则返回-1进 FinishAll，有则 BeginSeg + */ +static void PlsrRunControlGotoNextOrFinish(uint16_t cur_seg) { - uint32_t planned = g_plsr_accel_plan.decel_pulses; + int16_t next_seg_idx = PlsrRunControlNextSeg(cur_seg); - if (planned < 1U) - { - planned = 1U; - } - if (remain >= planned) + if (next_seg_idx < 0) { - return planned; + PlsrRunControlFinishAll(); } - if (remain < 1U) + else { - return 1U; + PlsrRunControlBeginSeg((uint16_t)next_seg_idx); } - return remain; } -/** 绝对模式：相对本次启动原点的位置 */ -static int32_t PlsrRunControlAbsLocalPos(void) +/** + * @brief 全部段结束：停 PWM、解锁 PSC、清方向与全部运行标志 + */ +static void PlsrRunControlFinishAll(void) { - return g_plsr_accumulated_pulses - s_absolute_origin; + PlsrSignalIoCancelDelay(); + PlsrPulseDriverStop(); + PlsrPulseDriverUnlockPsc(); + PlsrPulseDriverClearDir(); + g_plsr_busy = 0U; + s_state = RC_IDLE; + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; + s_waiting_for_last_period = 0U; } -/** - * 减速相写出频率：仅 remain==1允许钉止速； - * 更早写入若已碰到 end，则让出 1Hz，避免末两拍同频。 - */ -static uint32_t PlsrRunControlDecelOutFreq(uint32_t freq, uint32_t remain) +/* 开一段：算位移/方向 → PlanSeg → 选无缝/同向直开/换向延时 */ +static void PlsrRunControlBeginSeg(uint16_t seg_idx) { - uint32_t end_hz = g_plsr_accel_plan.end_frequency_hz; + int32_t cnt; + int32_t move; + uint32_t f_tgt; + uint32_t total; + PlsrSegPlanEndpoints_t endpoints; - if (end_hz < 1U) + if (seg_idx >= PlsrParamGetSegCount()) { - end_hz = 1U; + PlsrRunControlFinishAll(); + return; } - if (remain <= 1U) - { - return end_hz; - } + g_plsr_cur_seg_idx = seg_idx; + s_waiting_for_last_period = 0U; + cnt = g_plsr_segs[seg_idx].pulse_cnt; - if (g_plsr_accel_runtime.frequency_rising == 0U) + if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) { - if ((freq <= end_hz) && (end_hz < 100000U)) - { - return end_hz + 1U; - } + /* 以首次启动锁定原点：move = 段目标 − 相对原点位置 */ + move = cnt - PlsrRunControlAbsLocalPos(); } - else if ((freq >= end_hz) && (end_hz > 1U)) + else { - return end_hz - 1U; + move = cnt; } - return freq; -} - -/** - * @brief 进入减速相：budget 为曲线步进总数，BeginDec - */ -static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget) -{ - PlsrAccelPlan_t decel_plan = g_plsr_accel_plan; - if (budget < 1U) + if (move == 0) { - budget = 1U; + /* 相对 0 脉冲 / 无效占位(频率0且脉冲0) / 绝对已在目标：立刻段后逻辑 */ + g_plsr_busy = 1U; + s_seg_pulse_target = 0; + s_seg_pulses_done = 0; + PlsrRunControlAfterSegDone(); + return; } - s_phase = PH_DECEL; - decel_plan.accel_pulses = 0U; - decel_plan.constant_pulses = 0U; - decel_plan.decel_pulses = budget; - decel_plan.target_frequency_hz = from_hz; - PlsrAccelBeginDeceleration(&g_plsr_accel_runtime, &decel_plan); - g_plsr_accel_runtime.current_frequency_hz = from_hz; -} + if (move >= 0) + { + s_forward = 1U; + s_seg_pulse_target = move; + } + else + { + /* + * 反向位移目标 = -move。move==INT_MIN（如上位机写入 2147483648 + * 被解读为 -2147483648）时取反溢出，s_seg_pulse_target 仍为负数， + * 首拍后 s_seg_pulses_done>=s_seg_pulse_target 恒真，会误判段结束且累计每启一次减 1。 + */ + if (move == (int32_t)(-2147483647L - 1L)) + { + PlsrCommandSetFault(PLSR_ERR_PULSE_RANGE); + PlsrRunControlFinishAll(); + return; + } + s_forward = 0U; + s_seg_pulse_target = -move; + } + + /* 0x04：本段（或默认速度）频率非法 */ + if (PlsrRunControlGetSegTargetFreq(g_plsr_segs[seg_idx].freq_hz, + g_plsr_config.default_speed, &f_tgt) == 0U) + { + PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); + PlsrRunControlFinishAll(); + return; + } -/* 开表纯减速：只 Start(f1)。下一拍起由 ISR NextFrequency 写入 f2…f_end */ -static void PlsrRunControlPrimeDecelOnStart(void) -{ - uint32_t next; + /* 策略层解析起/止速与加减速；段内只跑 PlanSeg */ + (void)PlsrRunControlResolveSegPlanEndpoints(seg_idx, f_tgt, &endpoints); - next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); - PlsrRunControlApplyOutFreq(next, 1U); -} + total = (uint32_t)s_seg_pulse_target; + PlsrRunControlPlanSeg(total, endpoints.freq_start_hz, f_tgt, endpoints.freq_end_hz, + endpoints.accel_ms, endpoints.decel_ms); + s_seg_pulses_done = 0; + g_plsr_busy = 1U; + { + /* 用本段结束后的坐标判 keep，与 AfterSegDone 一致；结果锁进 s_stop_after_last_pulse */ + int32_t end_pos = PlsrRunControlAbsLocalPos(); -/** 离开加速相时触发 0x03 加速诊断 */ -static void PlsrRunControlOnApproachDone(void) -{ - uint8_t by_budget; + if (s_forward != 0U) + { + end_pos += s_seg_pulse_target; + } + else + { + end_pos -= s_seg_pulse_target; + } + s_stop_after_last_pulse = + (PlsrRunControlWillFollowKeepAt(seg_idx, end_pos) == 0U) ? 1U : 0U; + } - if (s_phase != PH_APPROACH) + if (total == 0U) { + PlsrRunControlAfterSegDone(); return; } - /* n 已走到预算 → 按预算收尾精确比对；否则按提前到频只查超额 */ - by_budget = ((g_plsr_accel_plan.accel_pulses > 0U) - && (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_plan.accel_pulses)) ? 1U : 0U; - PlsrRunControlCheckApproachCurveFault(by_budget); + + if (s_continue_without_stopping != 0U) + { + /* 分支 A：段间无缝（后续模式或 ACT切段） */ + PlsrPulseDriverClearOnePulseStop(); + /* + * 必须先 RC_RUN 再 RefreshProfile/Start：否则冷启动首拍 UPDATE 时 + * OnPulseIsr 仍见非 RUN 直接 return，影子停在起跳频，第 1、2 拍同频。 + */ + s_state = RC_RUN; + s_continue_without_stopping = 0U; + /* + * FOLLOW 真不停表：do_start=0 只改频。 + * ACT 经 OPM 边界停表后 g_plsr_pwm_running 已为 0：须 Start 重开，否则无脉冲卡死。 + */ + PlsrRunControlRefreshProfile((g_plsr_pwm_running == 0U) ? 1U : 0U); + PlsrRunControlArmActExtOnPulseStart(); + } + else if ((g_plsr_direction_valid != 0U) && (s_forward == g_plsr_direction_forward)) + { + /* 分支 B：真换向但本段与上一段同向，方向脚已正确，跳过延时 */ + PlsrPulseDriverSetDir(s_forward); + s_state = RC_RUN; + PlsrRunControlRefreshProfile(1U); + PlsrRunControlArmActExtOnPulseStart(); + } + else + { + /* 分支 C：需要换向或首次开跑 — 先打方向，再决定立刻开或 DIR_WAIT */ + PlsrPulseDriverSetDir(s_forward); + if (g_plsr_config.dir_delay_ms == 0U) + { + s_state = RC_RUN; + PlsrRunControlRefreshProfile(1U); + PlsrRunControlArmActExtOnPulseStart(); + } + else + { + /* + * 先置 DIR_WAIT 再开 TIM5，避免极短延时下 IRQ 早于状态切换。 + */ + s_state = RC_DIR_WAIT; + PlsrRunControlWakeDrain(); + PlsrSignalIoScheduleDelayMs((uint32_t)g_plsr_config.dir_delay_ms); + if (g_plsr_accel_runtime.cur_freq_hz >= 1U) + { + PlsrPulseDriverPrepare(g_plsr_accel_runtime.cur_freq_hz); + } + } + } } -/* 进匀速：写一次 f_tgt，之后 ISR 不改 ARR（复用缓存）*/ -static void PlsrRunControlEnterConstHold(uint8_t do_start) +/*============================================================================*/ +/* 衔接 / 停表 */ +/*============================================================================*/ + +/** + * @brief 停表并清频率链 + */ +static void PlsrRunControlClearChainAndStop(void) { - uint32_t f_hold = g_plsr_accel_plan.target_frequency_hz; + PlsrPulseDriverStop(); + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; +} - if (f_hold < 1U) +/** + * @brief 中途切段：同向且有有效频率则不停表衔接，否则停表清链 + * @param allow_act_handoff 1=允许 s_keep_freq_after_act（边界停表后仍衔接） + */ +static void PlsrRunControlApplyKeepOrStop(uint8_t allow_act_handoff) +{ + int16_t next_seg_idx; + uint8_t keep; + uint8_t pwm_ok; + + next_seg_idx = PlsrRunControlNextSeg(g_plsr_cur_seg_idx); + pwm_ok = (g_plsr_pwm_running != 0U) || + ((allow_act_handoff != 0U) && (s_keep_freq_after_act != 0U)); + keep = 0U; + if ((next_seg_idx >= 0) && (pwm_ok != 0U) && (g_plsr_output_freq_hz >= 1U) && + (PlsrRunControlIsForward((uint16_t)next_seg_idx, PlsrRunControlAbsLocalPos()) == + s_forward)) { - f_hold = 1U; + keep = 1U; + } + + if (keep != 0U) + { + PlsrPulseDriverClearOnePulseStop(); + s_continue_without_stopping = 1U; + s_next_seg_start_freq_valid = 1U; + s_next_seg_start_freq_hz = g_plsr_output_freq_hz; + } + else + { + PlsrRunControlClearChainAndStop(); + PlsrPulseDriverClearOnePulseStop(); } - s_phase = PH_CONST; - PlsrAccelBeginConstantSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); - PlsrPulseDriverClearPending(); - PlsrPulseDriverClearStartPeriodProtect(); - PlsrRunControlApplyOutFreq(f_hold, do_start); } -static void PlsrRunControlEnterConstHoldIsr(void) +/** + * @brief 哪些等待类型禁止后续模式不停表衔接 + * @return 1=必须停表等条件；0=允许（含 EXT_OR_DONE 脉冲先完） + */ +static uint8_t PlsrRunControlBlocksFollowKeep(uint16_t cur_seg) { - uint32_t f_hold = g_plsr_accel_plan.target_frequency_hz; - - if (f_hold < 1U) + if ((g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_TIME) || + (g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_SIGNAL) || + (g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_ACT) || + (g_plsr_segs[cur_seg].wait_type == PLSR_WAIT_EXT)) { - f_hold = 1U; + return 1U; } - s_phase = PH_CONST; - PlsrAccelBeginConstantSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); - PlsrPulseDriverClearPending(); - PlsrPulseDriverClearStartPeriodProtect(); - PlsrRunControlApplyOutFreqIsr(f_hold); + return 0U; } /** - * 任务侧：按当前段内相 s_phase，决定开表/改频用哪个频率。 - * @param do_start 1=需要 Start PWM（RefreshProfile(1)）；0=已在跑只改频 + * @brief 段末是否可不停表衔接下一段 + * @param local_pos 用于判下一段方向的局部坐标 + * @note 须 FOLLOW + 有同向下一段 + 无 BlocksFollowKeep */ -static void PlsrRunControlRefreshProfile(uint8_t do_start) +static uint8_t PlsrRunControlWillFollowKeepAt(uint16_t cur_seg, int32_t local_pos) { - uint32_t next; - uint32_t done_n; - uint32_t remain; + int16_t next_eff; - done_n = 0U; - if (s_segment_pulses_done > 0) + if (g_plsr_config.send_mode != PLSR_SEND_FOLLOW) { - done_n = (uint32_t)s_segment_pulses_done; + return 0U; } - remain = PlsrRunControlRemainPulses(); - - if ((s_phase == PH_CONST) && - (PlsrRunControlShouldEnterDecel(remain) != 0U)) + if (PlsrRunControlNextSeg(cur_seg) < 0) { - PlsrRunControlEnterDecel(g_plsr_accel_plan.target_frequency_hz, - PlsrRunControlDecelEnterBudget(remain)); + return 0U; } - - if (s_phase == PH_CONST) + next_eff = PlsrRunControlNextEffectiveSeg(cur_seg); + if (next_eff < 0) { - if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) - { - PlsrRunControlApplyOutFreq(g_plsr_accel_plan.target_frequency_hz, do_start); - } - return; + return 0U; } - - /* 纯减速开表：Start(f1)，f2…f_end 由后续 UPDATE ISR 写入 */ - if ((s_phase == PH_DECEL) && - (do_start != 0U) && - (s_segment_pulses_done == 0) && - (g_plsr_accel_runtime.completed_pulses == 0U)) + /* + * 直接下一段为无效占位时不能「跳过」该段，但仍可与下一有效段同向 keep + * （EXT_OR_DONE 同向不应因中间占位而强制停到 0 再爬）。 + */ + if (PlsrRunControlIsForward((uint16_t)next_eff, local_pos) != s_forward) { - PlsrRunControlPrimeDecelOnStart(); - return; + return 0U; } - - next = PlsrAccelCurveFreqAtPulse(&g_plsr_accel_plan, done_n); - if (next < 1U) + if (PlsrRunControlBlocksFollowKeep(cur_seg) != 0U) { - next = 1U; + return 0U; } - g_plsr_accel_runtime.current_frequency_hz = next; + return 1U; +} - if (s_phase == PH_APPROACH) +/*============================================================================*/ +/* ACT / EXT / 段后等待 */ +/*============================================================================*/ + +/* @brief 开始输出时：EXT 类清旧沿；ACT 类启动段内定时 */ +static void PlsrRunControlArmActExtOnPulseStart(void) +{ + uint16_t act_time_ms; + + if ((g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) || + (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE)) { - /* - * 加速相结束条件（任一满足即进 CONST 或 DECEL）： - * acc_n==0 规划无加速（纯匀速/纯减） - * active==0 曲线认为已到 f1 - * n >= acc_n 脉冲预算用尽 - * next >= f_tgt 升频且下一拍频率已达/超过目标 - * next <= f_tgt 降频且下一拍频率已达/低于目标 - */ - if ((g_plsr_accel_plan.accel_pulses == 0U) || - (done_n >= 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))) - { - next = g_plsr_accel_plan.target_frequency_hz; - PlsrRunControlOnApproachDone();/* 加速结束诊断曲线 */ - if (PlsrRunControlShouldEnterDecel(remain) != 0U) - { - PlsrRunControlEnterDecel(next, - PlsrRunControlDecelEnterBudget(remain)); - } - else - { - PlsrRunControlEnterConstHold(do_start); - return; - } - } + PlsrSignalIoClearEdges(); } - - if ((next != g_plsr_output_frequency_hz) || (do_start != 0U) || (g_plsr_pwm_running == 0U)) + + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; + + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_ACT) { - PlsrRunControlApplyOutFreq(next, do_start); + act_time_ms = g_plsr_segs[g_plsr_cur_seg_idx].act_ms; + s_act_timer_armed = 1U; + PlsrSignalIoScheduleDelayMs((uint32_t)act_time_ms); } } /** - * @brief 规划本段并锁 PSC / 进初始相 + * @brief EXT信号触发切段 * - * 包装 accel_curve Plan + Prebuild，并据结果选 PH_APPROACH/CONST/DECEL。 - * 0x02：规划峰值达不到段表 f_tgt 时置故障（三角波），仍按压低后的峰运行。 + * 同向且 PWM 在跑：不停表，下一段 f_from = 当前频率。 + * 反向或已停表：停表，下一段从起速爬。 */ -static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, - uint32_t f_tgt, uint32_t f_end, - uint16_t accel_ms, uint16_t decel_ms) +static void PlsrRunControlCutSegToNext(void) { - uint32_t f_start; - uint32_t f_min; - uint32_t f_max; + PlsrSignalIoCancelDelay(); + s_state = RC_IDLE; + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_act_waiting_for_pulse_boundary = 0U; + s_keep_freq_after_act = 0U; + s_wait_time_expired = 0U; - PlsrAccelCurvePlan(&g_plsr_accel_plan, - total, - f_from, - f_tgt, - f_end, - g_plsr_config.default_speed, - accel_ms, - decel_ms, - g_plsr_config.accel_mode); + PlsrRunControlApplyKeepOrStop(0U); + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); +} - /* 0x02 频率是否与设定一致诊断/\或\/ */ - if ((f_tgt > f_from) && (g_plsr_accel_plan.target_frequency_hz < f_tgt)) - { - PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); - } - else if ((f_tgt < f_from) && (g_plsr_accel_plan.target_frequency_hz > f_tgt)) - { - PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); - } +/** + * @brief ACT 时间到（脉冲可能还没发完）：丢掉本段剩余，从当前频率切下一段 + * + * 同向：下一段 f_from=当前频率（逐层跳，不进本段减速）。 + * 反向或无频率：停表，下一段从起速爬。 + * act_handoff_keep：对齐脉冲边界先停表后仍保持衔接。 + */ +static void PlsrRunControlActExpire(void) +{ + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_wait_time_expired = 0U; + s_state = RC_IDLE; - g_plsr_accel_runtime.total_pulses = 0U; + PlsrRunControlApplyKeepOrStop(1U); + s_keep_freq_after_act = 0U; - /* S/正弦：在任务上下文预建频率表，ISR 只查表 */ - PlsrAccelPrebuildFrequencyTables(&g_plsr_accel_plan); + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); +} - /* 运行层使用规划后的实际起始频率。 */ - f_start = g_plsr_accel_plan.start_frequency_hz; +/** + * @brief 段发完后的等待策略入口 + * + * WAIT时间/信号/EXT信号 → 停表清链后进 RC_WAIT_COND 或立即 GotoNext； + * EXT_OR_DONE 脉冲先完 → 立刻下一段；其它 → GotoNextOrFinish。 + */ +static void PlsrRunControlEnterPostWaitOrNext(void) +{ + uint16_t wms; - /* 脉冲域：ISR 用 PlsrAccelNextFrequency（直线现场算 / S·正弦查预建表）*/ - /* 本段频率范围锁 PSC，升降只改 ARR；跨度过大则 LockPscRange 自动不锁 */ - /* 本段可能出现的最低/最高频率，取 入口、目标、出口 三者的 min/max */ - f_min = g_plsr_accel_plan.target_frequency_hz; - f_max = g_plsr_accel_plan.target_frequency_hz; - if (f_start > f_max) + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_TIME) { - f_max = f_start; + wms = g_plsr_segs[g_plsr_cur_seg_idx].wait_ms; + if (wms == 0U) + { + wms = 1U; + } + PlsrRunControlClearChainAndStop(); + s_waiting_for_input_signal = 0U; + PlsrRunControlWakeDrain();/*清理过期令牌*/ + s_state = RC_WAIT_COND; + PlsrSignalIoScheduleDelayMs((uint32_t)wms); + return; } - if (f_start >= 1U) + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_SIGNAL) { - if (f_start < f_min) + /* 与 WAIT 时间相同：本段发完停表，再等 EXTI 确认后的下降沿 */ + PlsrRunControlClearChainAndStop(); + if (PlsrSignalIoTakeWaitFalling() != 0U) { - f_min = f_start; + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); + return; } + s_waiting_for_input_signal = 1U; + s_state = RC_WAIT_COND; + return; } - else + + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_EXT) { - f_min = 1U; + /* EXT信号：脉冲发完后仍要等 EXT 沿才跳转 */ + PlsrRunControlClearChainAndStop(); + /* 只清消抖中的假沿；已确认的 fell 保留，供下面 TakeExtFalling 消费 */ + PlsrSignalIoClearPending(); + if (PlsrSignalIoTakeExtFalling() != 0U) + { + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); + return; + } + s_waiting_for_input_signal = 1U; + s_state = RC_WAIT_COND; + return; + } + + /* + * EXT_OR_DONE：脉冲先发完 = 条件已满足，立刻下一段。 + * EXT 先到：RUN 里已 CutSegToNext，走不到这里。 + */ + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); +} + +/** + * @brief 段正常结束：诊断 → 链式频率 → FOLLOW keep 或停表 → EnterPostWaitOrNext + */ +static void PlsrRunControlAfterSegDone(void) +{ + uint8_t keep_pwm; + + /* 段末诊断：0x01 脉冲个数；若在减速相再核 0x03 */ + PlsrRunControlCheckPulseCntFault(); + PlsrRunControlCheckDecelCurveFault(); + + /* 先脱离 RUN，避免脉冲 ISR 重入 */ + s_state = RC_IDLE; + + /* 脉冲已发完但 ACT 时间未到 → 停表，等到到期再开下一段 */ + if (g_plsr_segs[g_plsr_cur_seg_idx].wait_type == PLSR_WAIT_ACT) + { + PlsrPulseDriverStop(); + PlsrPulseDriverClearOnePulseStop(); + s_continue_without_stopping = 0U; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; + s_act_expired = 0U; + if (s_act_timer_armed != 0U) + { + uint32_t remain_ms = PlsrSignalIoDelayRemainMs(); + + PlsrSignalIoCancelDelay();//段发完后，wait时间取消旧定时器，重新计算剩余wait时间 + s_act_timer_armed = 0U; + if (remain_ms >= 1U) + { + s_waiting_for_input_signal = 0U; + PlsrRunControlWakeDrain(); + s_state = RC_WAIT_COND; + PlsrSignalIoScheduleDelayMs(remain_ms); + return; + } + } + else + { + PlsrSignalIoCancelDelay();//计时已到，取消定时 + } + s_act_timer_armed = 0U; + PlsrRunControlGotoNextOrFinish(g_plsr_cur_seg_idx); + return; } - if (g_plsr_accel_plan.end_frequency_hz > f_max) + + s_act_timer_armed = 0U; + s_act_expired = 0U; + s_wait_time_expired = 0U; + /*硬件脉冲是否关闭，1不关，0关闭*/ + keep_pwm = (s_stop_after_last_pulse == 0U) ? 1U : 0U; + + if (g_plsr_config.send_mode == PLSR_SEND_COMPLETE) { - f_max = g_plsr_accel_plan.end_frequency_hz; + /* 段正常结束：下一段起速优先用本段落地频（EXT_OR_DONE 完成时常为峰值） */ + s_next_seg_start_freq_hz = g_plsr_accel_plan.end_freq_hz; + s_next_seg_start_freq_valid = 1U; } - if (g_plsr_accel_plan.end_frequency_hz >= 1U) + + if (keep_pwm != 0U) { - if (g_plsr_accel_plan.end_frequency_hz < f_min) - { - f_min = g_plsr_accel_plan.end_frequency_hz; - } + /* 后续同向且无门禁：不停表直接 BeginSeg */ + s_continue_without_stopping = 1U; } else { - if (f_min > 1U) + PlsrPulseDriverStop(); + PlsrPulseDriverClearOnePulseStop(); + s_continue_without_stopping = 0U; + if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) { - f_min = 1U; + s_next_seg_start_freq_valid = 0U; } } - if (f_min < 1U) - { - f_min = 1U; - } - if (f_max < f_min) - { - f_max = f_min; - } - /* 后续同向不停表时 PWM 仍在跑：禁止重锁 PSC */ - if (g_plsr_pwm_running == 0U) + + PlsrRunControlEnterPostWaitOrNext(); +} + +/*============================================================================*/ +/* 故障诊断 */ +/*============================================================================*/ + +/** + * 0x01 脉冲个数诊断 + * 时机：段正常结束进入 AfterSegDone 时。 + * 条件：本段目标 s_seg_pulse_target>0 且实际发出 s_seg_pulses_done 与目标不等。 + * 处理：告警，不中断后续切段/收尾逻辑。 + */ +static void PlsrRunControlCheckPulseCntFault(void) +{ + if ((s_seg_pulse_target > 0) && (s_seg_pulses_done != s_seg_pulse_target)) { - PlsrPulseDriverLockPscRange(f_min, f_max); + PlsrCommandSetFault(PLSR_ERR_PULSE_CNT); } +} - /* 保持 AccelCurvePlan 算出的脉冲预算；运行时逐拍计算下一频率 */ - if ((g_plsr_accel_plan.decel_pulses >= total) && - (total > 0U) && - (g_plsr_accel_plan.end_frequency_hz != g_plsr_accel_plan.target_frequency_hz) && - (f_start >= g_plsr_accel_plan.target_frequency_hz)) +/* 0x03 加速曲线诊断 */ +static void PlsrRunControlCheckApproachCurveFault(uint8_t by_pulse_budget) +{ + if (g_plsr_accel_plan.accel_pulses == 0U) { - /*纯减开段，脉冲数不足以减速到目标频率 */ - PlsrRunControlEnterDecel(f_start, g_plsr_accel_plan.decel_pulses); + return; } - else if (f_start == g_plsr_accel_plan.target_frequency_hz) + if (by_pulse_budget != 0U) { - s_phase = PH_CONST; - PlsrAccelBeginConstantSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); + if (g_plsr_accel_runtime.completed_pulses != g_plsr_accel_plan.accel_pulses) + { + PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); + } } - else + else if (g_plsr_accel_runtime.completed_pulses > g_plsr_accel_plan.accel_pulses) { - s_phase = PH_APPROACH; - PlsrAccelBeginAcceleration(&g_plsr_accel_runtime, &g_plsr_accel_plan); + PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); } } -/** - * @brief 全部段结束：停 PWM、解锁 PSC、清方向与全部运行标志 - */ -static void PlsrRunControlFinishAll(void) +/* 0x03 减速曲线诊断 */ +static void PlsrRunControlCheckDecelCurveFault(void) { - PlsrSignalIoCancelDelay(); - PlsrPulseDriverStop(); - PlsrPulseDriverUnlockPsc(); - PlsrPulseDriverClearDir(); - g_plsr_busy = 0U; - s_state = RC_IDLE; - s_continue_without_stopping = 0U; - s_next_segment_start_frequency_valid = 0U; - s_next_segment_start_frequency_hz = 0U; - s_act_timer_armed = 0U; - s_act_expired = 0U; - s_act_waiting_for_pulse_boundary = 0U; - s_keep_frequency_after_act = 0U; - s_wait_time_expired = 0U; - s_waiting_for_last_period = 0U; + if ((s_phase == PH_DECEL) && (g_plsr_accel_runtime.total_pulses > 0U) && + (g_plsr_accel_runtime.completed_pulses < g_plsr_accel_runtime.total_pulses)) + { + PlsrCommandSetFault(PLSR_ERR_CURVE_MISMATCH); + } } -/* 开一段：算位移/方向 → PlanSeg → 选无缝/同向直开/换向延时 */ -static void PlsrRunControlBeginSeg(uint16_t seg_idx0) +/** 离开加速相时触发 0x03 加速诊断 */ +static void PlsrRunControlOnApproachDone(void) { - int32_t cnt; - int32_t move; - uint32_t f_tgt; - uint32_t total; - PlsrSegPlanEndpoints_t endpoints; + uint8_t by_budget; - if (seg_idx0 >= PlsrParamGetSegCount()) + if (s_phase != PH_APPROACH) { - PlsrRunControlFinishAll(); return; } + /* n 已走到预算 → 按预算收尾精确比对；否则按提前到频只查超额 */ + by_budget = ((g_plsr_accel_plan.accel_pulses > 0U) + && (g_plsr_accel_runtime.completed_pulses >= g_plsr_accel_plan.accel_pulses)) ? 1U : 0U; + PlsrRunControlCheckApproachCurveFault(by_budget); +} - g_plsr_current_segment_index = seg_idx0; - s_waiting_for_last_period = 0U; - cnt = g_plsr_segments[seg_idx0].pulse_cnt; +/*============================================================================*/ +/* 频率 / 相控制 */ +/*============================================================================*/ - if (g_plsr_config.run_mode == PLSR_POS_ABSOLUTE) - { - /* 以首次启动锁定原点：move = 段目标 − 相对原点位置 */ - move = cnt - PlsrRunControlAbsLocalPos(); - } - else - { - move = cnt; - } +/** + * 解析段目标频率。 + * freq_hz==0 用公共默认速度；结果须落在 [1, 100000]，否则返回 0（调用方写 0x04）。 + */ +static uint8_t PlsrRunControlGetSegTargetFreq(int32_t freq_hz, + uint32_t default_spd, + uint32_t *out_hz) +{ + uint32_t f; - if (move == 0) + if (freq_hz < 0) { - /* 相对 0 脉冲 / 无效占位(频率0且脉冲0) / 绝对已在目标：立刻段后逻辑 */ - g_plsr_busy = 1U; - s_segment_pulse_target = 0; - s_segment_pulses_done = 0; - PlsrRunControlAfterSegDone(); - return; + return 0U; } - - if (move >= 0) + if (freq_hz == 0) { - s_forward = 1U; - s_segment_pulse_target = move; + f = default_spd; } else { - /* - * 反向位移目标 = -move。move==INT_MIN（如上位机写入 2147483648 - * 被解读为 -2147483648）时取反溢出，s_segment_pulse_target 仍为负数， - * 首拍后 s_segment_pulses_done>=s_segment_pulse_target 恒真，会误判段结束且累计每启一次减 1。 - */ - if (move == (int32_t)(-2147483647L - 1L)) - { - PlsrCommandSetFault(PLSR_ERR_PULSE_RANGE); - PlsrRunControlFinishAll(); - return; - } - s_forward = 0U; - s_segment_pulse_target = -move; - } - - /* 0x04：本段（或默认速度）频率非法 */ - if (PlsrRunControlGetSegTargetFreq(g_plsr_segments[seg_idx0].freq_hz, - g_plsr_config.default_speed, &f_tgt) == 0U) - { - PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); - PlsrRunControlFinishAll(); - return; + f = (uint32_t)freq_hz; } - - /* 策略层解析起/止速与加减速；段内只跑 PlanSeg */ - (void)PlsrRunControlResolveSegPlanEndpoints(seg_idx0, f_tgt, &endpoints); - - total = (uint32_t)s_segment_pulse_target; - PlsrRunControlPlanSeg(total, endpoints.freq_start_hz, f_tgt, endpoints.freq_end_hz, - endpoints.accel_ms, endpoints.decel_ms); - s_segment_pulses_done = 0; - g_plsr_busy = 1U; + /* 合法范围：1Hz ~ 100kHz（含） */ + if ((f < 1U) || (f > 100000U)) { - /* 用本段结束后的坐标判 keep，与 AfterSegDone 一致；结果锁进 s_stop_after_last_pulse */ - int32_t end_pos = PlsrRunControlAbsLocalPos(); - - if (s_forward != 0U) - { - end_pos += s_segment_pulse_target; - } - else - { - end_pos -= s_segment_pulse_target; - } - s_stop_after_last_pulse = - (PlsrRunControlWillFollowKeepAt(seg_idx0, end_pos) == 0U) ? 1U : 0U; + return 0U; } + *out_hz = f; + return 1U; +} - if (total == 0U) +/** 任务侧运行输出：0=停表；任务上下文 SetFreq / Start */ +static void PlsrRunControlApplyOutFreq(uint32_t freq, uint8_t do_start) +{ + if (freq == 0U) { - PlsrRunControlAfterSegDone(); + PlsrPulseDriverStop(); return; } - if (s_continue_without_stopping != 0U) - { - /* 分支 A：段间无缝（后续模式或 ACT切段） */ - PlsrPulseDriverClearOnePulseStop(); - /* - * 必须先 RC_RUN 再 RefreshProfile/Start：否则冷启动首拍 UPDATE 时 - * OnPulseIsr 仍见非 RUN 直接 return，影子停在起跳频，第 1、2 拍同频。 - */ - s_state = RC_RUN; - s_continue_without_stopping = 0U; - /* - * FOLLOW 真不停表：do_start=0 只改频。 - * ACT 经 OPM 边界停表后 g_plsr_pwm_running 已为 0：须 Start 重开，否则无脉冲卡死。 - */ - PlsrRunControlRefreshProfile((g_plsr_pwm_running == 0U) ? 1U : 0U); - PlsrRunControlArmActExtOnPulseStart(); - } - else if ((g_plsr_direction_valid != 0U) && (s_forward == g_plsr_direction_forward)) + if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) { - /* 分支 B：真换向但本段与上一段同向，方向脚已正确，跳过延时 */ - PlsrPulseDriverSetDir(s_forward); - s_state = RC_RUN; - PlsrRunControlRefreshProfile(1U); - PlsrRunControlArmActExtOnPulseStart(); + PlsrPulseDriverStart(freq); } - else + else if (freq != g_plsr_output_freq_hz) { - /* 分支 C：需要换向或首次开跑 — 先打方向，再决定立刻开或 DIR_WAIT */ - PlsrPulseDriverSetDir(s_forward); - if (g_plsr_config.dir_delay_ms == 0U) - { - s_state = RC_RUN; - PlsrRunControlRefreshProfile(1U); - PlsrRunControlArmActExtOnPulseStart(); - } - else - { - /* - * 先置 DIR_WAIT 再开 TIM5，避免极短延时下 IRQ 早于状态切换。 - */ - s_state = RC_DIR_WAIT; - PlsrRunControlWakeDrain(); - PlsrSignalIoScheduleDelayMs((uint32_t)g_plsr_config.dir_delay_ms); - if (g_plsr_accel_runtime.current_frequency_hz >= 1U) - { - PlsrPulseDriverPrepare(g_plsr_accel_runtime.current_frequency_hz); - } - } + PlsrPulseDriverSetFreq(freq); } } - -/** @brief 运行控制初始化 */ -void PlsrRunControlInit(void) -{ - s_state = RC_IDLE; - g_plsr_busy = 0U; - s_forward = 1U; - g_plsr_current_segment_index = 0U; - s_segment_pulses_done = 0; - s_segment_pulse_target = 0; - g_plsr_accumulated_pulses = 0; - s_absolute_origin = 0; - s_absolute_origin_locked = 0U; - s_continue_without_stopping = 0U; - s_next_segment_start_frequency_hz = 0U; - s_next_segment_start_frequency_valid = 0U; - g_plsr_accel_plan.accel_pulses = 0U; - g_plsr_accel_plan.decel_pulses = 0U; - g_plsr_accel_runtime.total_pulses = 0U; - s_stop_after_last_pulse = 0U; - s_waiting_for_last_period = 0U; - s_phase = PH_CONST; - s_act_timer_armed = 0U; - s_act_expired = 0U; - s_act_waiting_for_pulse_boundary = 0U; - s_keep_frequency_after_act = 0U; - s_wait_time_expired = 0U; - s_waiting_for_input_signal = 0U; - /* s_wake_sem 在 OSInit 后由 WakeInit 创建 */ + +/** 脉冲 ISR：只走 ARR 预装载路径 */ +static void PlsrRunControlApplyOutFreqIsr(uint32_t freq) +{ + if (freq < 1U) + { + freq = 1U; + } + PlsrPulseDriverSetFreqIsr(freq); } -/** - * ISR/任务：唤醒 PlsrTask（二进制语义：先排空再 Post，避免残留令牌使下一段秒醒） - */ -static void PlsrRunControlWakeDrain(void) +static uint8_t PlsrRunControlHasDecel(void) { - if (s_wake_sem != (OS_EVENT *)0) + /* + * 第二相脉冲预算 >0 即需要收尾斜坡。 + * 谷底形（起/止速 > 目标）：第二相是从目标再加速回止速，f_end > f_tgt。 + */ + return (g_plsr_accel_plan.decel_pulses > 0U) ? 1U : 0U; +} + +static uint32_t PlsrRunControlRemainPulses(void) +{ + if (s_seg_pulse_target > s_seg_pulses_done) { - while (OSSemAccept(s_wake_sem) > 0U) - { - /* 全部丢弃令牌 */ - } + return (uint32_t)(s_seg_pulse_target - s_seg_pulses_done); } + return 0U; } -static void PlsrRunControlWakePost(void) +/* remain==dec_n 时进入减速 */ +static uint8_t PlsrRunControlShouldEnterDecel(uint32_t remain) { - if (s_wake_sem != (OS_EVENT *)0) + if (PlsrRunControlHasDecel() == 0U) { - (void)OSSemPost(s_wake_sem); + return 0U; } + return (remain <= g_plsr_accel_plan.decel_pulses) ? 1U : 0U; } -/** - * @brief 创建任务唤醒信号量 - */ -void PlsrRunControlWakeInit(void) +static uint32_t PlsrRunControlDecelEnterBudget(uint32_t remain) { - if (s_wake_sem == (OS_EVENT *)0) + uint32_t planned = g_plsr_accel_plan.decel_pulses; + + if (planned < 1U) { - s_wake_sem = OSSemCreate(0U); + planned = 1U; } + if (remain >= planned) + { + return planned; + } + if (remain < 1U) + { + return 1U; + } + return remain; } /** - * @brief PlsrTask 休眠 + * 减速相写出频率：仅 remain==1允许钉止速； + * 更早写入若已碰到 end，则让出 1Hz，避免末两拍同频。 */ -void PlsrRunControlWakePend(uint16_t timeout_ticks) +static uint32_t PlsrRunControlDecelOutFreq(uint32_t freq, uint32_t remain) { - INT8U err; + uint32_t end_hz = g_plsr_accel_plan.end_freq_hz; - if (s_wake_sem == (OS_EVENT *)0) + if (end_hz < 1U) { - if (timeout_ticks < 1U) + end_hz = 1U; + } + + if (remain <= 1U) + { + return end_hz; + } + + if (g_plsr_accel_runtime.freq_rising == 0U) + { + if ((freq <= end_hz) && (end_hz < 100000U)) { - timeout_ticks = 1U; + return end_hz + 1U; } - OSTimeDly((INT32U)timeout_ticks); - return; } - OSSemPend(s_wake_sem, timeout_ticks, &err); - (void)err; + else if ((freq >= end_hz) && (end_hz > 1U)) + { + return end_hz - 1U; + } + return freq; } /** - * @brief 是否在 TIM5 精确等待 + * @brief 进入减速相：budget 为曲线步进总数，BeginDec */ -uint8_t PlsrRunControlIsPreciseWait(void) +static void PlsrRunControlEnterDecel(uint32_t from_hz, uint32_t budget) { - if (s_state == RC_DIR_WAIT) + PlsrAccelPlan_t decel_plan = g_plsr_accel_plan; + + if (budget < 1U) { - return 1U; + budget = 1U; } - if ((s_state == RC_WAIT_COND) && (s_waiting_for_input_signal == 0U)) + s_phase = PH_DECEL; + + decel_plan.accel_pulses = 0U; + decel_plan.const_pulses = 0U; + decel_plan.decel_pulses = budget; + decel_plan.target_freq_hz = from_hz; + PlsrAccelBeginDecel(&g_plsr_accel_runtime, &decel_plan); + g_plsr_accel_runtime.cur_freq_hz = from_hz; +} + +/* 开表纯减速：只 Start(f1)。下一拍起由 ISR NextFreq 写入 f2…f_end */ +static void PlsrRunControlPrimeDecelOnStart(void) +{ + uint32_t next; + + next = PlsrAccelNextFreq(&g_plsr_accel_runtime); + PlsrRunControlApplyOutFreq(next, 1U); +} + +/* 进匀速：写一次 f_tgt，之后 ISR 不改 ARR（复用缓存）*/ +static void PlsrRunControlEnterConstHold(uint8_t do_start) +{ + uint32_t f_hold = g_plsr_accel_plan.target_freq_hz; + + if (f_hold < 1U) { - return 1U; + f_hold = 1U; } - return 0U; + s_phase = PH_CONST; + PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); + PlsrPulseDriverClearPending(); + PlsrPulseDriverClearStartPeriodProtect(); + PlsrRunControlApplyOutFreq(f_hold, do_start); } -/** @brief 启动多段序列 */ -uint8_t PlsrStart(uint16_t start_seg_1based) +static void PlsrRunControlEnterConstHoldIsr(void) { - uint16_t n; - uint16_t first_seg; - uint32_t f_chk; + uint32_t f_hold = g_plsr_accel_plan.target_freq_hz; - if (g_plsr_busy != 0U) + if (f_hold < 1U) { - return 0U; + f_hold = 1U; } + s_phase = PH_CONST; + PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); + PlsrPulseDriverClearPending(); + PlsrPulseDriverClearStartPeriodProtect(); + PlsrRunControlApplyOutFreqIsr(f_hold); +} - n = PlsrParamGetSegCount(); - if (n < 1U) +/** + * 任务侧：按当前段内相 s_phase，决定开表/改频用哪个频率。 + * @param do_start 1=需要 Start PWM（RefreshProfile(1)）；0=已在跑只改频 + */ +static void PlsrRunControlRefreshProfile(uint8_t do_start) +{ + uint32_t next; + uint32_t done_n; + uint32_t remain; + + done_n = 0U; + if (s_seg_pulses_done > 0) { - return 0U; + done_n = (uint32_t)s_seg_pulses_done; } - if ((start_seg_1based < 1U) || (start_seg_1based > n)) + remain = PlsrRunControlRemainPulses(); + + if ((s_phase == PH_CONST) && + (PlsrRunControlShouldEnterDecel(remain) != 0U)) { - start_seg_1based = PlsrParamGetStartSeg(); + PlsrRunControlEnterDecel(g_plsr_accel_plan.target_freq_hz, + PlsrRunControlDecelEnterBudget(remain)); } - first_seg = (uint16_t)(start_seg_1based - 1U); - /* 启动前先检查首段频率，非法时不置忙、不开定时器。 */ - if (PlsrRunControlGetSegTargetFreq(g_plsr_segments[first_seg].freq_hz, - g_plsr_config.default_speed, - &f_chk) == 0U) + if (s_phase == PH_CONST) { - PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); - return 0U; + if ((do_start != 0U) || (g_plsr_pwm_running == 0U)) + { + PlsrRunControlApplyOutFreq(g_plsr_accel_plan.target_freq_hz, do_start); + } + return; } - s_continue_without_stopping = 0U; - s_next_segment_start_frequency_valid = 0U; - s_next_segment_start_frequency_hz = 0U; - /* - * 绝对原点：仅在首次 Start（或 ClearAccPulse 之后的第一次）锁定。 - * 相对跑完再切绝对，仍相对「第一次启动时刻」坐标，不以当前位置重定原点。 - */ - if (s_absolute_origin_locked == 0U) + + /* 纯减速开表：Start(f1)，f2…f_end 由后续 UPDATE ISR 写入 */ + if ((s_phase == PH_DECEL) && + (do_start != 0U) && + (s_seg_pulses_done == 0) && + (g_plsr_accel_runtime.completed_pulses == 0U)) { - s_absolute_origin = g_plsr_accumulated_pulses; - s_absolute_origin_locked = 1U; + PlsrRunControlPrimeDecelOnStart(); + return; } - PlsrSignalIoClearEdges(); - PlsrRunControlBeginSeg(first_seg); - return 1U; -} - -/** @brief 急停 */ -void PlsrStop(void) -{ - PlsrSignalIoCancelDelay(); - PlsrPulseDriverStop(); - PlsrPulseDriverUnlockPsc(); - PlsrPulseDriverClearDir(); - g_plsr_busy = 0U; - s_state = RC_IDLE; - s_continue_without_stopping = 0U; /*连续运行标志*/ - s_next_segment_start_frequency_valid = 0U;/*衔接频率是否有效*/ - s_next_segment_start_frequency_hz = 0U; /*段间衔接频率*/ - s_act_timer_armed = 0U;/*ACT定时器是否已启动*/ - s_act_expired = 0U;/*ACT到期请求标志*/ - s_act_waiting_for_pulse_boundary = 0U;/*ACT 时间到，等待脉冲边界切段*/ - s_keep_frequency_after_act = 0U;/*ACT 切段后是否保持当前频率衔接*/ - s_wait_time_expired = 0U;/*段后等待到期请求标志*/ - s_waiting_for_last_period = 0U; -} - -/** @brief 运行中改频 */ -uint8_t PlsrChangeFreq(uint32_t new_tgt_hz) -{ - uint32_t remain; - uint32_t f_end; - if (s_state != RC_RUN) + next = PlsrAccelCurveFreqAtPulse(&g_plsr_accel_plan, done_n); + if (next < 1U) { - return 0U; + next = 1U; } - /* 0x04：动态改频目标非法则拒收，保持原规划 */ - if ((new_tgt_hz < 1U) || (new_tgt_hz > 100000U)) + g_plsr_accel_runtime.cur_freq_hz = next; + + if (s_phase == PH_APPROACH) { - PlsrCommandSetFault(PLSR_ERR_FREQ_ILLEGAL); - return 0U; + /* + * 加速相结束条件（任一满足即进 CONST 或 DECEL）： + * acc_n==0 规划无加速（纯匀速/纯减） + * active==0 曲线认为已到 f1 + * n >= acc_n 脉冲预算用尽 + * next >= f_tgt 升频且下一拍频率已达/超过目标 + * next <= f_tgt 降频且下一拍频率已达/低于目标 + */ + if ((g_plsr_accel_plan.accel_pulses == 0U) || + (done_n >= g_plsr_accel_plan.accel_pulses) || + ((g_plsr_accel_plan.target_freq_hz >= g_plsr_accel_plan.start_freq_hz) && + (next >= g_plsr_accel_plan.target_freq_hz)) || + ((g_plsr_accel_plan.target_freq_hz < g_plsr_accel_plan.start_freq_hz) && + (next <= g_plsr_accel_plan.target_freq_hz))) + { + next = g_plsr_accel_plan.target_freq_hz; + PlsrRunControlOnApproachDone();/* 加速结束诊断曲线 */ + if (PlsrRunControlShouldEnterDecel(remain) != 0U) + { + PlsrRunControlEnterDecel(next, + PlsrRunControlDecelEnterBudget(remain)); + } + else + { + PlsrRunControlEnterConstHold(do_start); + return; + } + } } - if (s_segment_pulses_done >= s_segment_pulse_target) + + if ((next != g_plsr_output_freq_hz) || (do_start != 0U) || (g_plsr_pwm_running == 0U)) { - return 0U; + PlsrRunControlApplyOutFreq(next, do_start); } - - remain = (uint32_t)(s_segment_pulse_target - s_segment_pulses_done); - f_end = g_plsr_accel_plan.end_frequency_hz; - PlsrRunControlPlanSeg(remain, g_plsr_output_frequency_hz, new_tgt_hz, f_end, - g_plsr_config.accel_ms, g_plsr_config.decel_ms); - s_segment_pulse_target = (int32_t)remain; - s_segment_pulses_done = 0; - PlsrRunControlRefreshProfile(0U); - return 1U; } -/** @brief 毫秒任务节拍 */ -void PlsrRunControlTickMs(void) +/*============================================================================*/ +/* 规划 / 开段 */ +/*============================================================================*/ + +/* 解析本段规划端点：freq_start / freq_end / accel_ms / decel_ms */ +static uint8_t PlsrRunControlResolveSegPlanEndpoints(uint16_t seg_idx, uint32_t f_tgt, + PlsrSegPlanEndpoints_t *endpoints) { - if (s_act_expired != 0U) + endpoints->accel_ms = g_plsr_config.accel_ms; + endpoints->decel_ms = g_plsr_config.decel_ms; + + if ((s_continue_without_stopping != 0U) && (g_plsr_output_freq_hz >= 1U)) { - s_act_expired = 0U; - if ((s_state == RC_RUN) || (s_keep_frequency_after_act != 0U)) - { - PlsrRunControlActExpire(); - } - return; + endpoints->freq_start_hz = g_plsr_output_freq_hz; } - if (s_wait_time_expired != 0U) + else if ((g_plsr_pwm_running != 0U) && (g_plsr_output_freq_hz >= 1U)) { - s_wait_time_expired = 0U; - if (s_state == RC_WAIT_COND) - { - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); - } - return; + endpoints->freq_start_hz = g_plsr_output_freq_hz; } - if (s_state == RC_DIR_WAIT) + else if ((s_next_seg_start_freq_valid != 0U) && (s_next_seg_start_freq_hz >= 1U)) { - /* 精确延时只靠 TIM5；此处占位，避免任务空转改状态 */ - return; + /* 完成模式段末停表后：用本段落地频作起速（如 EXT_OR_DONE 的匀速峰值） */ + endpoints->freq_start_hz = s_next_seg_start_freq_hz; + s_next_seg_start_freq_valid = 0U; + s_next_seg_start_freq_hz = 0U; } - - if (s_state == RC_RUN) + else { - PlsrPulseDriverApplyPending(); - - if ((g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_EXT) || - (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_EXT_OR_DONE)) - { - if (PlsrSignalIoTakeExtFalling() != 0U) - { - PlsrRunControlCutSegToNext(); - return; - } - } - return; + /* 停表后冷启动 */ + endpoints->freq_start_hz = PlsrAccelCurveResolveStartHz(g_plsr_config.start_speed, + f_tgt, + g_plsr_config.default_speed, + endpoints->accel_ms, + endpoints->decel_ms); } - if (s_state == RC_WAIT_COND) + /* 终止频率 */ { - if (s_waiting_for_input_signal != 0U) + int16_t next_eff = PlsrRunControlNextEffectiveSeg(seg_idx); + + if ((g_plsr_segs[seg_idx].wait_type == PLSR_WAIT_EXT_OR_DONE) && + (next_eff >= 0)) { - if (g_plsr_segments[g_plsr_current_segment_index].wait_type == PLSR_WAIT_EXT) + int32_t end_pos = PlsrRunControlAbsLocalPos(); + uint8_t next_fwd; + + if (s_forward != 0U) { - if (PlsrSignalIoTakeExtFalling() != 0U) - { - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); - } + end_pos += s_seg_pulse_target; } else { - if (PlsrSignalIoTakeWaitFalling() != 0U) + end_pos -= s_seg_pulse_target; + } + next_fwd = PlsrRunControlIsForward((uint16_t)next_eff, end_pos); + + /* 与下一有效段同向 → 后续/完成衔接（换向才落到下方止速） */ + if (next_fwd == s_forward) + { + if (g_plsr_config.send_mode == PLSR_SEND_FOLLOW) { - PlsrRunControlGotoNextOrFinish(g_plsr_current_segment_index); + if (PlsrRunControlGetSegTargetFreq( + g_plsr_segs[(uint16_t)next_eff].freq_hz, + g_plsr_config.default_speed, + &endpoints->freq_end_hz) != 0U) + { + return 1U; + } + endpoints->freq_end_hz = f_tgt; + return 1U; } + endpoints->freq_end_hz = f_tgt; + return 1U; } + /* 与下一有效段反向：落入下方规划到止速 */ } - /* 时间 WAIT / ACT 剩余：TIM5 OnDelayTimer 置 wait_expire_req */ } + + endpoints->freq_end_hz = PlsrAccelCurveResolveEndHz(g_plsr_config.end_speed, + f_tgt, + g_plsr_config.default_speed, + endpoints->accel_ms, + endpoints->decel_ms); + return 1U; } /** - * @brief TIM5 one-shot 到期 - * @note ACT：收完当前脉冲并锁低输出，OPM 在周期末停表；不预测加减速脉冲数 + * @brief 规划本段并锁 PSC / 进初始相 + * + * 包装 accel_curve Plan + Prebuild，并据结果选 PH_APPROACH/CONST/DECEL。 + * 0x02：规划峰值达不到段表 f_tgt 时置故障（三角波），仍按压低后的峰运行。 */ -void PlsrRunControlOnDelayTimer(void) +static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, + uint32_t f_tgt, uint32_t f_end, + uint16_t accel_ms, uint16_t decel_ms) { - if (s_state == RC_DIR_WAIT) - { - s_state = RC_RUN; - PlsrRunControlRefreshProfile(1U); - PlsrRunControlArmActExtOnPulseStart(); - PlsrRunControlWakePost(); - return; - } + uint32_t f_start; + uint32_t f_min; + uint32_t f_max; - if ((s_state == RC_RUN) && (s_act_timer_armed != 0U)) + PlsrAccelCurvePlan(&g_plsr_accel_plan, + total, + f_from, + f_tgt, + f_end, + g_plsr_config.default_speed, + accel_ms, + decel_ms, + g_plsr_config.accel_mode); + + /* 0x02 频率是否与设定一致诊断/\或\/ */ + if ((f_tgt > f_from) && (g_plsr_accel_plan.target_freq_hz < f_tgt)) { - /* 到点：当前脉冲下降沿后锁低，周期结束时 OPM 停表。 */ - s_act_timer_armed = 0U; - s_act_waiting_for_pulse_boundary = 1U; - PlsrPulseDriverArmActStop(); - return; + PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); } - - if ((s_state == RC_WAIT_COND) && (s_waiting_for_input_signal == 0U)) + else if ((f_tgt < f_from) && (g_plsr_accel_plan.target_freq_hz > f_tgt)) { - s_wait_time_expired = 1U; - PlsrRunControlWakePost(); + PlsrCommandSetFault(PLSR_ERR_FREQ_UNREACH); } -} -/** @brief 脉冲 UPDATE ISR */ -void PlsrOnPulseIsr(void) -{ - uint32_t remain; - uint32_t next; + g_plsr_accel_runtime.total_pulses = 0U; - if (s_state != RC_RUN) - { - return; - } + /* S/正弦：在任务上下文预建频率表，ISR 只查表 */ + PlsrAccelPrebuildFreqTables(&g_plsr_accel_plan); - if (s_waiting_for_last_period != 0U) + /* 运行层使用规划后的实际起始频率。 */ + f_start = g_plsr_accel_plan.start_freq_hz; + + /* 脉冲域：ISR 用 PlsrAccelNextFreq（直线现场算 / S·正弦查预建表）*/ + /* 本段频率范围锁 PSC，升降只改 ARR；跨度过大则 LockPscRange 自动不锁 */ + /* 本段可能出现的最低/最高频率，取 入口、目标、出口 三者的 min/max */ + f_min = g_plsr_accel_plan.target_freq_hz; + f_max = g_plsr_accel_plan.target_freq_hz; + if (f_start > f_max) { - s_waiting_for_last_period = 0U; - s_act_waiting_for_pulse_boundary = 0U; - - PlsrPulseDriverHoldOutputLow(); - PlsrRunControlAfterSegDone(); - return; + f_max = f_start; } - - s_segment_pulses_done++; - if (s_forward != 0U) + if (f_start >= 1U) { - g_plsr_accumulated_pulses++; + if (f_start < f_min) + { + f_min = f_start; + } } else { - g_plsr_accumulated_pulses--; + f_min = 1U; } - - if (s_act_waiting_for_pulse_boundary != 0U) + if (g_plsr_accel_plan.end_freq_hz > f_max) { - s_act_waiting_for_pulse_boundary = 0U; - s_keep_frequency_after_act = 1U; - - PlsrPulseDriverHoldOutputLow(); - s_state = RC_IDLE; - s_act_expired = 1U; - PlsrRunControlWakePost(); - return; + f_max = g_plsr_accel_plan.end_freq_hz; } - - if (s_segment_pulses_done >= s_segment_pulse_target) + if (g_plsr_accel_plan.end_freq_hz >= 1U) { - if (s_stop_after_last_pulse != 0U) + if (g_plsr_accel_plan.end_freq_hz < f_min) { - PlsrPulseDriverArmSegEndStop(); - s_waiting_for_last_period = 1U; - return; + f_min = g_plsr_accel_plan.end_freq_hz; } - /* FOLLOW 不停表衔接：立刻切段，PWM 连续 */ - PlsrRunControlAfterSegDone(); - return; } - - remain = PlsrRunControlRemainPulses(); - - if (s_phase == PH_CONST) + else { - /* 匀速：不改频、不写 ARR，仅在 remain 进入减速窗口时切相并查表改频 */ - PlsrPulseDriverClearStartPeriodProtect(); - if (PlsrRunControlShouldEnterDecel(remain) != 0U) + if (f_min > 1U) { - 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); + f_min = 1U; } } - else if (s_phase == PH_APPROACH) + if (f_min < 1U) { - /* 每拍 Step 得下一频；若加速相结束则进 CONST 或 DECEL */ - next = PlsrAccelNextFrequency(&g_plsr_accel_runtime); - - 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))) - { - next = g_plsr_accel_plan.target_frequency_hz; - PlsrRunControlOnApproachDone(); - if (PlsrRunControlShouldEnterDecel(remain) != 0U) - { - 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 - { - PlsrRunControlEnterConstHoldIsr(); - } - } - else - { - PlsrRunControlApplyOutFreqIsr(next); - } + f_min = 1U; } - else /* PH_DECEL */ + if (f_max < f_min) { - 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; - PlsrRunControlApplyOutFreqIsr(next); + f_max = f_min; + } + /* 后续同向不停表时 PWM 仍在跑：禁止重锁 PSC */ + if (g_plsr_pwm_running == 0U) + { + PlsrPulseDriverLockPscRange(f_min, f_max); } -} -/** @brief 清累计与绝对原点 */ -void PlsrClearAccPulse(void) -{ - g_plsr_accumulated_pulses = 0; - s_absolute_origin = 0; - s_absolute_origin_locked = 0U; /* 下次 Start 重新锁定原点 */ + /* 保持 AccelCurvePlan 算出的脉冲预算；运行时逐拍计算下一频率 */ + if ((g_plsr_accel_plan.decel_pulses >= total) && + (total > 0U) && + (g_plsr_accel_plan.end_freq_hz != g_plsr_accel_plan.target_freq_hz) && + (f_start >= g_plsr_accel_plan.target_freq_hz)) + { + /*纯减开段，脉冲数不足以减速到目标频率 */ + PlsrRunControlEnterDecel(f_start, g_plsr_accel_plan.decel_pulses); + } + else if (f_start == g_plsr_accel_plan.target_freq_hz) + { + s_phase = PH_CONST; + PlsrAccelBeginConstSpeed(&g_plsr_accel_runtime, &g_plsr_accel_plan); + } + else + { + s_phase = PH_APPROACH; + PlsrAccelBeginAccel(&g_plsr_accel_runtime, &g_plsr_accel_plan); + } } -/** @brief 上电恢复累计 */ -void PlsrRunControlRestoreAccPulse(int32_t acc_pulse) -{ - g_plsr_accumulated_pulses = acc_pulse; - s_absolute_origin = 0; - s_absolute_origin_locked = 0U; /* 下次 Start 以当前累计为绝对原点 */ -}