diff --git a/plsr/accel_curve/plsr_accel_curve.c b/plsr/accel_curve/plsr_accel_curve.c index b4354d1..cf02788 100644 --- a/plsr/accel_curve/plsr_accel_curve.c +++ b/plsr/accel_curve/plsr_accel_curve.c @@ -8,23 +8,151 @@ PlsrAccelPlan_t g_plsr_accel_plan; /* 当前段三相规划结果 */ /* 整段规划全局一份 */ PlsrAccelRuntime_t g_plsr_accel_runtime; /* 当前相 ISR 逐拍状态 */ /* 当前相运行态全局一份 */ -/* 正弦 raised-cosine 形状表（千分比 0..1000） */ -static const uint16_t s_plsr_sine_shape_tab[33] = { /* 半个余弦形状的进度表，0到1000 */ - 0U, 2U, 10U, 22U, 38U, 59U, 84U, 113U, /* 前半段：进度小，形状值也小 */ - 146U, 183U, 222U, 264U, 309U, 355U, 402U, 451U, /* 中间往上爬 */ - 500U, 549U, 598U, 645U, 691U, 736U, 778U, 817U, /* 过半：500是中点 */ - 854U, 887U, 916U, 941U, 962U, 978U, 990U, 998U, /* 后半段往1000收 */ - 1000U /* 终点1000=形状走完 */ +/* + * 形状表（Q24，1.0 = 1<<24）：ISR 按 t/T 查表再线性插值，不再预建 4096 点频率表。 + * 257 点 → 256 段；末点固定 PLSR_Q24_ONE。 + */ +#define PLSR_Q24_ONE (1UL << 24) /* Q24 单位 1.0 */ +#define PLSR_SHAPE_TAB_LEN 257U +#define PLSR_SHAPE_TAB_LAST (PLSR_SHAPE_TAB_LEN - 1U) /* 256：段数=点数-1 */ + +/* 正弦 raised-cosine：shape≈(1-cos(πp))/2，Q24 */ +static const uint32_t s_plsr_sine_shape_tab[PLSR_SHAPE_TAB_LEN] = { + 0U, 632U, 2526U, 5684U, + 10104U, 15786U, 22729U, 30932U, + 40393U, 51112U, 63086U, 76314U, + 90794U, 106523U, 123500U, 141721U, + 161185U, 181887U, 203825U, 226996U, + 251396U, 277021U, 303868U, 331933U, + 361211U, 391697U, 423388U, 456279U, + 490364U, 525638U, 562097U, 599734U, + 638545U, 678522U, 719661U, 761954U, + 805396U, 849980U, 895699U, 942547U, + 990516U, 1039599U, 1089789U, 1141078U, + 1193459U, 1246923U, 1301463U, 1357070U, + 1413735U, 1471452U, 1530209U, 1590000U, + 1650815U, 1712644U, 1775479U, 1839309U, + 1904126U, 1969920U, 2036680U, 2104396U, + 2173059U, 2242659U, 2313183U, 2384623U, + 2456966U, 2530203U, 2604323U, 2679313U, + 2755163U, 2831862U, 2909397U, 2987758U, + 3066931U, 3146907U, 3227671U, 3309213U, + 3391520U, 3474579U, 3558379U, 3642906U, + 3728147U, 3814090U, 3900723U, 3988031U, + 4076002U, 4164622U, 4253878U, 4343757U, + 4434246U, 4525329U, 4616995U, 4709228U, + 4802016U, 4895344U, 4989197U, 5083563U, + 5178427U, 5273774U, 5369590U, 5465860U, + 5562571U, 5659707U, 5757255U, 5855198U, + 5953524U, 6052216U, 6151259U, 6250640U, + 6350343U, 6450352U, 6550654U, 6651232U, + 6752072U, 6853158U, 6954476U, 7056009U, + 7157744U, 7259663U, 7361753U, 7463997U, + 7566381U, 7668888U, 7771504U, 7874212U, + 7976999U, 8079847U, 8182741U, 8285667U, + 8388608U, 8491549U, 8594475U, 8697369U, + 8800217U, 8903004U, 9005712U, 9108328U, + 9210835U, 9313219U, 9415463U, 9517553U, + 9619472U, 9721207U, 9822740U, 9924058U, + 10025144U, 10125984U, 10226562U, 10326864U, + 10426873U, 10526576U, 10625957U, 10725000U, + 10823692U, 10922018U, 11019961U, 11117509U, + 11214645U, 11311356U, 11407626U, 11503442U, + 11598789U, 11693653U, 11788019U, 11881872U, + 11975200U, 12067988U, 12160221U, 12251887U, + 12342970U, 12433459U, 12523338U, 12612594U, + 12701214U, 12789185U, 12876493U, 12963126U, + 13049069U, 13134310U, 13218837U, 13302637U, + 13385696U, 13468003U, 13549545U, 13630309U, + 13710285U, 13789458U, 13867819U, 13945354U, + 14022053U, 14097903U, 14172893U, 14247013U, + 14320250U, 14392593U, 14464033U, 14534557U, + 14604157U, 14672820U, 14740536U, 14807296U, + 14873090U, 14937907U, 15001737U, 15064572U, + 15126401U, 15187216U, 15247007U, 15305764U, + 15363481U, 15420146U, 15475753U, 15530293U, + 15583757U, 15636138U, 15687427U, 15737617U, + 15786700U, 15834669U, 15881517U, 15927236U, + 15971820U, 16015262U, 16057555U, 16098694U, + 16138671U, 16177482U, 16215119U, 16251578U, + 16286852U, 16320937U, 16353828U, 16385519U, + 16416005U, 16445283U, 16473348U, 16500195U, + 16525820U, 16550220U, 16573391U, 16595329U, + 16616031U, 16635495U, 16653716U, 16670693U, + 16686422U, 16700902U, 16714130U, 16726104U, + 16736823U, 16746284U, 16754487U, 16761430U, + 16767112U, 16771532U, 16774690U, 16776584U, + 16777216U }; -/* S/正弦频率预计算表：PlanSeg 时填好；ISR 只切换/查表。匀速不改频。 */ -#define PLSR_RAMP_TBL_MAX 4096U /* 表最长4096项，超了就抽样 */ -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; /* 减速表抽样步长 */ /* 减速表几个脉冲读一项 */ +/* S 曲线形状（Tj:Ta:Tj=1:2:1 归一化频差进度），Q24 */ +static const uint32_t s_plsr_s_shape_tab[PLSR_SHAPE_TAB_LEN] = { + 0U, 683U, 2731U, 6144U, + 10923U, 17067U, 24576U, 33451U, + 43691U, 55296U, 68267U, 82603U, + 98304U, 115371U, 133803U, 153600U, + 174763U, 197291U, 221184U, 246443U, + 273067U, 301056U, 330411U, 361131U, + 393216U, 426667U, 461483U, 497664U, + 535211U, 574123U, 614400U, 656043U, + 699051U, 743424U, 789163U, 836267U, + 884736U, 934571U, 985771U, 1038336U, + 1092267U, 1147563U, 1204224U, 1262251U, + 1321643U, 1382400U, 1444523U, 1508011U, + 1572864U, 1639083U, 1706667U, 1775616U, + 1845931U, 1917611U, 1990656U, 2065067U, + 2140843U, 2217984U, 2296491U, 2376363U, + 2457600U, 2540203U, 2624171U, 2709504U, + 2796203U, 2883584U, 2970965U, 3058347U, + 3145728U, 3233109U, 3320491U, 3407872U, + 3495253U, 3582635U, 3670016U, 3757397U, + 3844779U, 3932160U, 4019541U, 4106923U, + 4194304U, 4281685U, 4369067U, 4456448U, + 4543829U, 4631211U, 4718592U, 4805973U, + 4893355U, 4980736U, 5068117U, 5155499U, + 5242880U, 5330261U, 5417643U, 5505024U, + 5592405U, 5679787U, 5767168U, 5854549U, + 5941931U, 6029312U, 6116693U, 6204075U, + 6291456U, 6378837U, 6466219U, 6553600U, + 6640981U, 6728363U, 6815744U, 6903125U, + 6990507U, 7077888U, 7165269U, 7252651U, + 7340032U, 7427413U, 7514795U, 7602176U, + 7689557U, 7776939U, 7864320U, 7951701U, + 8039083U, 8126464U, 8213845U, 8301227U, + 8388608U, 8475989U, 8563371U, 8650752U, + 8738133U, 8825515U, 8912896U, 9000277U, + 9087659U, 9175040U, 9262421U, 9349803U, + 9437184U, 9524565U, 9611947U, 9699328U, + 9786709U, 9874091U, 9961472U, 10048853U, + 10136235U, 10223616U, 10310997U, 10398379U, + 10485760U, 10573141U, 10660523U, 10747904U, + 10835285U, 10922667U, 11010048U, 11097429U, + 11184811U, 11272192U, 11359573U, 11446955U, + 11534336U, 11621717U, 11709099U, 11796480U, + 11883861U, 11971243U, 12058624U, 12146005U, + 12233387U, 12320768U, 12408149U, 12495531U, + 12582912U, 12670293U, 12757675U, 12845056U, + 12932437U, 13019819U, 13107200U, 13194581U, + 13281963U, 13369344U, 13456725U, 13544107U, + 13631488U, 13718869U, 13806251U, 13893632U, + 13981013U, 14067712U, 14153045U, 14237013U, + 14319616U, 14400853U, 14480725U, 14559232U, + 14636373U, 14712149U, 14786560U, 14859605U, + 14931285U, 15001600U, 15070549U, 15138133U, + 15204352U, 15269205U, 15332693U, 15394816U, + 15455573U, 15514965U, 15572992U, 15629653U, + 15684949U, 15738880U, 15791445U, 15842645U, + 15892480U, 15940949U, 15988053U, 16033792U, + 16078165U, 16121173U, 16162816U, 16203093U, + 16242005U, 16279552U, 16315733U, 16350549U, + 16384000U, 16416085U, 16446805U, 16476160U, + 16504149U, 16530773U, 16556032U, 16579925U, + 16602453U, 16623616U, 16643413U, 16661845U, + 16678912U, 16694613U, 16708949U, 16721920U, + 16733525U, 16743765U, 16752640U, 16760149U, + 16766293U, 16771072U, 16774485U, 16776533U, + 16777216U +}; /* 静态辅助函数前置声明 */ static uint32_t PlsrAccelCurveClampFreqHz(uint32_t freq_hz); @@ -44,25 +172,23 @@ static uint32_t PlsrAccelCurveResolveBoundaryFreq(uint32_t configured_freq_hz, uint8_t resolving_start); static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, uint32_t end_val, - uint32_t ratio_permille); + uint32_t ratio_q24); 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 uint32_t PlsrAccelCurveShapeQ24(const uint32_t *shape_tab, uint32_t progress_q24); +static uint32_t PlsrAccelCurveShapeSineQ24(uint32_t progress_q24); +static uint32_t PlsrAccelCurveShapeSQ24(uint32_t progress_q24); 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, +static uint32_t PlsrAccelCurveSCurveFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, 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); @@ -103,14 +229,6 @@ static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, 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 */ @@ -384,53 +502,12 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, } /** - * PlanSeg 后预建加/减速表（任务上下文，勿在 ISR 里做）。 - * 直线不建表；S/正弦各相用临时 runtime 仿真填表。 + * 兼容旧调用：S/正弦不再预建 4096 点频率表。 + * ISR 内时间累计 + 257 点 Q24 形状表插值取频。 */ 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; /* 加速步长先按1 */ - s_decel_table_stride = 1U; /* 减速步长先按1 */ - - if ((plan == (const PlsrAccelPlan_t *)0) || /* 没plan或 */ - (plan->curve_mode == PLSR_ACCEL_LINEAR)) /* 直线模式 */ - { - return; /* 直线 ISR 走 NextFreqLinear，无需表 */ - } - - /* 加速表：起 → 峰 */ - 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); /* 步长写回 */ - } + (void)plan; /* 保留 API，PlanSeg 仍可调用；无事可做 */ } /** @brief 进入加速相 */ @@ -441,7 +518,6 @@ void PlsrAccelBeginAccel(PlsrAccelRuntime_t *runtime, { return; /* 缺谁都不开相 */ } - /*计算加速或者减速的时间*/ PlsrAccelRuntimeBeginRamp(runtime, /* 填运行态：起→峰 */ plan->start_freq_hz, /* 入口频 */ plan->target_freq_hz, /* 峰值 */ @@ -449,9 +525,10 @@ void PlsrAccelBeginAccel(PlsrAccelRuntime_t *runtime, plan->accel_rate_hz_per_s, /* 加速斜率 */ plan->decel_rate_hz_per_s, /* 减速斜率 */ plan->curve_mode); /* 曲线模式 */ - /*选择频率表*/ - PlsrAccelRuntimeSelectFreqTable(runtime, 1U, s_accel_table_length, /* 绑加速预建表 id=1 */ - s_accel_table_stride); /* 步长一起绑 */ + /* S/正弦：ISR 走时间累计+形状表，不绑预建频表 */ + runtime->freq_table_id = 0U; + runtime->table_length = 0U; + runtime->table_stride = 1U; } /** @brief 进入减速相 */ @@ -469,9 +546,9 @@ void PlsrAccelBeginDecel(PlsrAccelRuntime_t *runtime, 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, /* 绑减速预建表 id=2 */ - s_decel_table_stride); /* 步长一起绑 */ + runtime->freq_table_id = 0U; + runtime->table_length = 0U; + runtime->table_stride = 1U; } /** @brief 进入匀速相 */ @@ -502,13 +579,10 @@ void PlsrAccelBeginConstSpeed(PlsrAccelRuntime_t *runtime, /** * @brief ISR 里走：本相再往前一拍，返回下一拍该写成多大频率 - * 分支：直线逼近 / 查预建表 / 建表失败则按时间一步步算；末拍前避免提前钉死到终点频。 + * 直线：现场 v^2 逼近；S/正弦：时间累计 + 257 点 Q24 形状表线性插值（不查 4096 预建频表）。 */ uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime) { - const uint32_t *freq_table; /* 指向加速表或减速表 */ - uint32_t table_index; /* 表下标 */ - if (runtime == (PlsrAccelRuntime_t *)0) /* 空指针 */ { return 0U; /* 没runtime返回0 */ @@ -534,33 +608,10 @@ uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime) runtime->cur_freq_hz = PlsrAccelNextFreqLinear(runtime); /* 用平方目标±1逼近下一频 */ } } - 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) ? /* 步长至少1 */ - 1U : runtime->table_stride; /* 防0步长 */ - - table_index = (runtime->completed_pulses - 1U) / table_stride; /* completed从1起，除步长得下标 */ - if (table_index >= runtime->table_length) /* 越界？ */ - { - table_index = runtime->table_length - 1U; /* 夹到最后一项 */ - } - freq_table = (runtime->freq_table_id == 1U) ? /* 1=加速表，其它=减速表 */ - s_accel_freq_table : /* 加速频率表 */ - s_decel_freq_table; /* 减速频率表 */ - runtime->cur_freq_hz = freq_table[table_index]; /* 查表得本拍频率 */ - } - } else { - /* 建表失败兜底：仍走时间步进 */ - runtime->cur_freq_hz = PlsrAccelNextFreqTimed(runtime); /* 时间域取下一频 */ + /* S/正弦：elapsed += 1/f_prev，再查形状表插值得频 */ + runtime->cur_freq_hz = PlsrAccelNextFreqTimed(runtime); } /* 本相脉冲用尽：钉终点并停相 */ @@ -569,69 +620,25 @@ uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime) runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉到本相终点频 */ runtime->is_active = 0U; /* 停相 */ } - else if (runtime->curve_mode == PLSR_ACCEL_LINEAR) /* 直线未用尽 */ + else if ((runtime->total_pulses > 0U) && + (runtime->completed_pulses < runtime->total_pulses)) /* 未到最后一拍 */ { /* - * 未到本相最后一拍：不要钳到 end。 - * 否则减速在 n=N-1 就落到止速，ARPE 下会表现为末两拍同频。 + * 末拍前勿提前钉死 end，否则 ARPE 下末两拍同频。 + * 直线与 S/正弦统一：碰到终点则让出 1Hz。 */ if (runtime->freq_rising == 0U) /* 往下掉频 */ { - if ((runtime->cur_freq_hz <= runtime->end_freq_hz) && /* 已经碰到或越过终点 */ - (runtime->end_freq_hz < 100000U)) /* 终点还没顶到100k */ - { - runtime->cur_freq_hz = runtime->end_freq_hz + 1U; /* 故意高1Hz，末拍再落地 */ - } - } - else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && /* 往上爬且已到/过终点 */ - (runtime->end_freq_hz > 1U) && /* 终点不是1 */ - (runtime->total_pulses > 0U) && /* 还有总拍数 */ - (runtime->completed_pulses < runtime->total_pulses)) /* 还没到最后一拍 */ - { - runtime->cur_freq_hz = runtime->end_freq_hz - 1U; /* 故意低1Hz，别提前钉死 */ - } - } - 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; /* 让出1Hz */ - } - } - else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && /* 往上爬已>=终点 */ - (runtime->end_freq_hz > 1U)) /* 终点有效 */ - { - runtime->cur_freq_hz = runtime->end_freq_hz - 1U; /* 让出1Hz */ - } - } - } - else if (runtime->freq_table_id == 0U) /* 时间步进兜底 */ - { - /* 时间步进路径：到时或越过终点则停 */ - 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) /* 频已到终点 */ + if ((runtime->cur_freq_hz <= runtime->end_freq_hz) && + (runtime->end_freq_hz < 100000U)) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉终点 */ - runtime->is_active = 0U; /* 停相 */ + runtime->cur_freq_hz = runtime->end_freq_hz + 1U; /* 故意高1Hz */ } } - else if (runtime->cur_freq_hz <= runtime->end_freq_hz) /* 往下掉且已到/过终点 */ + else if ((runtime->cur_freq_hz >= runtime->end_freq_hz) && + (runtime->end_freq_hz > 1U)) { - runtime->cur_freq_hz = runtime->end_freq_hz; /* 钉终点 */ - runtime->is_active = 0U; /* 停相 */ + runtime->cur_freq_hz = runtime->end_freq_hz - 1U; /* 故意低1Hz */ } } return PlsrAccelCurveClampFreqHz(runtime->cur_freq_hz); /* 最后钳一次再返回 */ @@ -807,23 +814,25 @@ static uint32_t PlsrAccelCurveResolveBoundaryFreq(uint32_t configured_freq_hz, return jump_freq_hz; /* 否则用起跳频 */ } -/** 千分比线性插值：ratio=0→start，ratio≥1000→end */ +/** Q24 线性插值：ratio=0→start，ratio≥1.0→end */ static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, uint32_t end_val, - uint32_t ratio_permille) + uint32_t ratio_q24) { - if (ratio_permille >= 1000U) /* 进度>=1000 */ + uint64_t delta; /* |end-start| * ratio */ + + if (ratio_q24 >= PLSR_Q24_ONE) /* 进度>=1.0 */ { return end_val; /* 直接终点 */ } if (end_val >= start_val) /* 终点>=起点：往上插 */ { - return start_val + /* start + 差*千分比 */ - ((end_val - start_val) * ratio_permille) / 1000UL; /* 按千分比摊 */ + delta = ((uint64_t)(end_val - start_val) * (uint64_t)ratio_q24) >> 24; /* 差 * Q24 */ + return start_val + (uint32_t)delta; } - return start_val - /* 往下插 */ - ((start_val - end_val) * ratio_permille) / 1000UL; /* start - 差*千分比 */ -} /* 按千分比摊 */ + delta = ((uint64_t)(start_val - end_val) * (uint64_t)ratio_q24) >> 24; + return start_val - (uint32_t)delta; +} /*============================================================================*/ /* 直线 */ @@ -1011,35 +1020,55 @@ static uint32_t PlsrAccelNextFreqLinear(PlsrAccelRuntime_t *runtime) /* S / 正弦 */ /*============================================================================*/ -/** raised-cosine 形状：进度千分比 → 形状千分比（查 33 点表再插值） */ -static uint32_t PlsrAccelCurveShapeSinePermille(uint32_t progress_permille) +/** 进度 Q24 → 形状 Q24：查 257 点表，相邻点线性插值 */ +static uint32_t PlsrAccelCurveShapeQ24(const uint32_t *shape_tab, uint32_t progress_q24) { - uint32_t table_index; /* 表下标 */ - uint32_t seg_progress; /* 段内进度 */ + uint64_t scaled; /* progress * 段数，高 8+ 位为下标，低 24 位为段内小数 */ + uint32_t table_index; /* 表下标 0..255 */ + uint32_t seg_frac_q24; /* 段内进度 Q24 */ uint32_t min_val; /* 段左端形状值 */ uint32_t max_val; /* 段右端形状值 */ - if (progress_permille >= 1000U) /* 进度满了 */ + if (shape_tab == (const uint32_t *)0) + { + return progress_q24; /* 无表：退回线性进度 */ + } + if (progress_q24 >= PLSR_Q24_ONE) + { + return PLSR_Q24_ONE; + } + /* progress∈[0,1) → 下标 0..255 + 段内 Q24 小数 */ + scaled = (uint64_t)progress_q24 * (uint64_t)PLSR_SHAPE_TAB_LAST; + table_index = (uint32_t)(scaled >> 24); + if (table_index >= PLSR_SHAPE_TAB_LAST) { - return 1000U; /* 形状也满1000 */ + return PLSR_Q24_ONE; } - /* progress/1000 → 表下标 0..32，相邻点线性插值 */ - table_index = (progress_permille * 32UL) / 1000UL; /* 映射到0..32格 */ - if (table_index >= 32U) /* 末格？ */ + seg_frac_q24 = (uint32_t)(scaled & 0x00FFFFFFUL); + min_val = shape_tab[table_index]; + max_val = shape_tab[table_index + 1U]; + if (max_val >= min_val) { - return 1000U; /* 直接1000 */ + return min_val + (uint32_t)((((uint64_t)(max_val - min_val) * (uint64_t)seg_frac_q24) >> 24)); } - 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); /* 线性摊 */ + return min_val - (uint32_t)((((uint64_t)(min_val - max_val) * (uint64_t)seg_frac_q24) >> 24)); +} + +static uint32_t PlsrAccelCurveShapeSineQ24(uint32_t progress_q24) +{ + return PlsrAccelCurveShapeQ24(s_plsr_sine_shape_tab, progress_q24); +} + +static uint32_t PlsrAccelCurveShapeSQ24(uint32_t progress_q24) +{ + return PlsrAccelCurveShapeQ24(s_plsr_s_shape_tab, progress_q24); } /** * 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) + * 规划/开相仍用此式填 ramp_time；ISR 取频改走形状表。 */ 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, @@ -1099,121 +1128,49 @@ static void PlsrAccelCurveCalcSineCurveTime(uint32_t start_freq_hz, uint32_t end } } -/** Δ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); /* Δf=A*t/1e6，匀加速走过的频差 */ -} - -/** Δ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) -{ - uint64_t calc; /* 中间累乘 */ - - if ((jerk_time_us < 1U) || (elapsed_time_us < 1U) || (accel_hz_per_s < 1U)) /* 缺参数 */ - { - return 0U; /* 变不出频差 */ - } - /* ((A * t / jerk_time_us) * t) / 2e6 */ - calc = ((uint64_t)accel_hz_per_s * (uint64_t)elapsed_time_us) / (uint64_t)jerk_time_us; /* 先 A*t/Tj */ - calc = (calc * (uint64_t)elapsed_time_us) / 2000000ULL; /* 再 *t / 2e6 → ½*(A/Tj)*t² */ - if (calc > 0xFFFFFFFFULL) /* 超32位？ */ - { - return 0xFFFFFFFFUL; /* 钉满 */ - } - return (uint32_t)calc; /* 这段加加速度爬出的频差 */ -} - /** - * S 曲线时刻取频：jerk↑ → 恒 a → jerk↓，Δf 再映射到 start→end。 - * 公式：jerk 段 ½(A/Tj)t²；恒 a：½A·Tj + A(t-Tj)；末 jerk 对称扣回。 + * S 曲线时刻取频：f = f0 + Δf·shape(t/T) + * shape 为 Tj:Ta:Tj=1:2:1 归一化进度，查 257 点 Q24 表再线性插值。 */ -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, +static uint32_t PlsrAccelCurveSCurveFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t ramp_time_us, uint32_t elapsed_time_us) { - 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; /* 升还是降 */ - - freq_diff_hz = PlsrAccelCurveAbsDiff(start_freq_hz, end_freq_hz); /* 算总|Δf| */ - freq_rising = (end_freq_hz >= start_freq_hz) ? 1U : 0U; /* 看方向 */ - - if ((freq_diff_hz < 1U) || (ramp_time_us < 1U) || (accel_hz_per_s < 1U)) /* 参数不齐 */ - { - return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 直接终点 */ - } - if (elapsed_time_us >= ramp_time_us) /* 时间到了 */ - { - return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 钉终点 */ - } + uint32_t progress_q24; + uint32_t ratio_q24; + uint32_t freq_hz; - if ((jerk_time_us < 1U) || (elapsed_time_us <= jerk_time_us)) /* 还在第一段：加速度从0爬到最大 */ + if ((ramp_time_us < 1U) || (start_freq_hz == end_freq_hz)) { - /* jerk 升：Δf = 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); /* Δf=½*(A/Tj)*t² */ + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - else + if (elapsed_time_us >= ramp_time_us) { - decel_start_time_us = jerk_time_us + const_accel_time_us; /* 恒加速段结束= Tj+Ta */ - if (elapsed_time_us <= decel_start_time_us) /* 还在恒加速 */ - { - /* 恒加速：Δf = 0.5*A*Tj + A*(t-Tj) */ - freq_change_hz = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, jerk_time_us, jerk_time_us); /* 先加上第一段贡献 ½*A*Tj */ - freq_change_hz += PlsrAccelCurveDeltaHzFromAt(accel_hz_per_s, elapsed_time_us - jerk_time_us); /* 再加上 A*(t-Tj) */ - } - else - { - /* jerk 降：Δf = 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) /* τ别超Tj */ - { - decel_elapsed_us = jerk_time_us; /* 夹到Tj */ - } - 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); /* 末段先当匀加速加上A*τ */ - { - uint32_t freq_drop = PlsrAccelCurveDeltaHzFromJerk(accel_hz_per_s, decel_elapsed_us, jerk_time_us); /* 再扣回½*(A/Tj)*τ²（加速度往0收） */ - if (freq_change_hz > freq_drop) /* 够扣？ */ - { - freq_change_hz -= freq_drop; /* 扣掉 */ - } - else - { - freq_change_hz = 0U; /* 不够就清零 */ - } - } - } - } - - if (freq_change_hz > freq_diff_hz) - { /* 别超过总频差 */ - freq_change_hz = freq_diff_hz; /* 钳到总频差 */ + return PlsrAccelCurveClampFreqHz(end_freq_hz); } - if (freq_rising != 0U) /* 升频：起点+变化 */ + /* p = t/T，Q24 */ + progress_q24 = (uint32_t)((((uint64_t)elapsed_time_us << 24) / (uint64_t)ramp_time_us)); + if (progress_q24 > PLSR_Q24_ONE) { - return PlsrAccelCurveClampFreqHz(start_freq_hz + freq_change_hz); /* 钳后返回 */ + progress_q24 = PLSR_Q24_ONE; } - if (start_freq_hz > freq_change_hz) /* 降频且扣得动 */ + ratio_q24 = PlsrAccelCurveShapeSQ24(progress_q24); + freq_hz = PlsrAccelCurveInterpolate(start_freq_hz, end_freq_hz, ratio_q24); + if (freq_hz < 1U) { - return PlsrAccelCurveClampFreqHz(start_freq_hz - freq_change_hz); /* 起点-变化 */ + freq_hz = 1U; } - return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 扣穿了就终点 */ + return PlsrAccelCurveClampFreqHz(freq_hz); } /** * 正弦 raised-cosine：f = f0 + Δf·shape(t/T) - * shape≈(1-cos(πp))/2，查 33 点千分比表再插值。 + * shape≈(1-cos(πp))/2，查 257 点 Q24 表再插值。 */ static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, uint32_t ramp_time_us, uint32_t elapsed_time_us) { - uint32_t progress_permille; /* 时间进度千分比 */ - uint32_t ratio_permille; /* 形状千分比 */ + uint32_t progress_q24; /* 时间进度 Q24 */ + uint32_t ratio_q24; /* 形状 Q24 */ uint32_t freq_hz; /* 插值后频率 */ if ((ramp_time_us < 1U) || (start_freq_hz == end_freq_hz)) /* 没时间或同频 */ @@ -1224,13 +1181,13 @@ static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t start_freq_hz, uint32_t en { return PlsrAccelCurveClampFreqHz(end_freq_hz); /* 钉终点 */ } - progress_permille = (uint32_t)(((uint64_t)elapsed_time_us * 1000ULL) / (uint64_t)ramp_time_us); /* p=t/T */ - if (progress_permille > 1000U) /* 进度超了？ */ + progress_q24 = (uint32_t)((((uint64_t)elapsed_time_us << 24) / (uint64_t)ramp_time_us)); /* p=t/T */ + if (progress_q24 > PLSR_Q24_ONE) { - progress_permille = 1000U; /* 夹到1000 */ + progress_q24 = PLSR_Q24_ONE; } - ratio_permille = PlsrAccelCurveShapeSinePermille(progress_permille); /* 半个余弦形状：进度→形状 */ - freq_hz = PlsrAccelCurveInterpolate(start_freq_hz, end_freq_hz, ratio_permille); /* 按形状在起止间插频 */ + ratio_q24 = PlsrAccelCurveShapeSineQ24(progress_q24); + freq_hz = PlsrAccelCurveInterpolate(start_freq_hz, end_freq_hz, ratio_q24); if (freq_hz < 1U) /* 保护 */ { freq_hz = 1U; /* 至少1 */ @@ -1280,16 +1237,13 @@ static uint32_t PlsrAccelCurveTimedFreqAtPulse( } elapsed_time_us += 1000000UL / cur_freq_hz; /* Δt = 1/f（µs） */ - if (curve_mode == PLSR_ACCEL_S) /* S：按累计时间取频 */ + if (curve_mode == PLSR_ACCEL_S) /* S：形状表插值 */ { cur_freq_hz = - PlsrAccelCurveSCurveFreqAtTime(start_freq_hz, /* 起点频率 */ - end_freq_hz, /* 终点频率 */ - rate_hz_per_s, /* 峰值加速度 A */ - jerk_time_us, /* Tj */ - const_accel_time_us, /* Ta */ - ramp_time_us, /* 斜坡总时间 T */ - elapsed_time_us); /* 已经过了多少微秒 */ + PlsrAccelCurveSCurveFreqAtTime(start_freq_hz, + end_freq_hz, + ramp_time_us, + elapsed_time_us); } else { @@ -1385,11 +1339,10 @@ static uint32_t PlsrAccelNextFreqTimed(PlsrAccelRuntime_t *runtime) return PlsrAccelCurveClampFreqHz(runtime->end_freq_hz); /* 返回终点频 */ } - if (runtime->curve_mode == PLSR_ACCEL_S) /* S曲线 */ + if (runtime->curve_mode == PLSR_ACCEL_S) /* S曲线：257 点 Q24 形状表 */ { - 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, /* 带上Tj/Ta/T和已过时间 */ - runtime->elapsed_time_us); /* 已过微秒 */ + freq_hz = PlsrAccelCurveSCurveFreqAtTime(runtime->start_freq_hz, runtime->end_freq_hz, + runtime->ramp_time_us, runtime->elapsed_time_us); } else { @@ -1587,13 +1540,13 @@ static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, /* 有脉冲且起止不同才激活 */ runtime->is_active = ((total_pulses > 0U) && (start_freq_hz != end_freq_hz)) ? 1U : 0U; /* 有拍且起止不同才算活跃 */ - runtime->freq_table_id = 0U; /* 开相先不绑表；BeginAcc/Dec 再 Select */ + runtime->freq_table_id = 0U; /* 不用预建频表 */ runtime->table_length = 0U; /* 表长清 */ - runtime->table_stride = 1U; /* 步长默认1 */ + runtime->table_stride = 1U; /* 保留字段 */ PlsrAccelRuntimeCalcTimes(runtime); /* 按模式预填时间参数 */ } -/** 离散仿真一步：与建表 / 脉冲预算共用（时间步进） */ +/** 离散仿真一步：与脉冲预算共用（时间步进 + 形状表） */ static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime) { if (runtime->completed_pulses < 0xFFFFFFFFUL) /* 防溢出 */ @@ -1625,100 +1578,3 @@ static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime) } } -/** - * 将本相离散频率写入 freq_table[]，返回写入长度；*table_stride_out 为抽样步长。 - * 脉冲数 > 容量时 stride=ceil(N/cap)，ISR 用 completed/stride 取下标。 - */ -static uint32_t PlsrAccelRuntimeBuildFreqTable(PlsrAccelRuntime_t *runtime, - uint32_t *freq_table, - uint32_t table_capacity, - uint32_t *table_stride_out) -{ - PlsrAccelRuntime_t simulation; /* 独立仿真副本 */ - uint32_t loop_index; /* 脉冲循环 */ - uint32_t table_index; /* 写入下标 */ - uint32_t table_stride; /* 抽样步长 */ - uint32_t table_length; /* 有效表长 */ - - *table_stride_out = 1U; /* 默认步长1 */ - 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; /* 直线或不活跃：不建表 */ - } - - /* 超容量则抽样，保证能塞进表 */ - table_stride = 1U; /* 先按1 */ - if (runtime->total_pulses > table_capacity) /* 脉冲比表容量多？ */ - { - table_stride = (runtime->total_pulses + table_capacity - 1U) / table_capacity; /* 步长=ceil(总拍/容量)，向上取整 */ - } - *table_stride_out = table_stride; /* 告诉调用方步长 */ - - /* 独立副本仿真，避免改坏调用方 runtime */ - simulation = *runtime; /* 拷一份runtime */ - simulation.completed_pulses = 0U; /* 仿真从0拍起 */ - simulation.cur_freq_hz = runtime->start_freq_hz; /* 频回到起点 */ - simulation.is_active = 1U; /* 强制活跃 */ - PlsrAccelRuntimeCalcTimes(&simulation); /* 重算时间参数 */ - if (simulation.ramp_time_us < 1U) /* 斜坡总时间无效 */ - { - freq_table[0] = runtime->end_freq_hz; /* 表里塞一个终点 */ - return 1U; /* 长度1 */ - } - - table_length = 0U; /* 表长从0计 */ - for (loop_index = 0U; loop_index < runtime->total_pulses; loop_index++) /* 按总拍逐拍仿真 */ - { - PlsrAccelRuntimeSimulateOnePulse(&simulation); /* 推一拍 */ - table_index = (simulation.completed_pulses - 1U) / table_stride; /* completed/stride → 下标 */ - if (table_index >= table_capacity) /* 越界夹住 */ - { - table_index = table_capacity - 1U; /* 夹到末格 */ - } - freq_table[table_index] = simulation.cur_freq_hz; /* 写入当前频 */ - if ((table_index + 1U) > table_length) /* 刷新有效长度 */ - { - table_length = table_index + 1U; /* 长度至少盖住该下标 */ - } - /* 提前到达终点：余下表项填 end */ - if (simulation.is_active == 0U) /* 提前停了？ */ - { - while ((loop_index + 1U) < runtime->total_pulses) /* 把剩下拍都填成终点 */ - { - loop_index++; /* 假推进loop */ - table_index = loop_index / table_stride; /* 按拍号算下标 */ - if (table_index >= table_capacity) /* 越界？ */ - { - table_index = table_capacity - 1U; /* 夹住 */ - } - freq_table[table_index] = runtime->end_freq_hz; /* 填终点频 */ - if ((table_index + 1U) > table_length) /* 更新长度 */ - { - table_length = table_index + 1U; /* 盖住 */ - } - } - break; /* 跳出主循环 */ - } - } - - /* 末项强制终点频，避免抽样截断偏离 */ - if (table_length > 0U) /* 有表项？ */ - { - freq_table[table_length - 1U] = runtime->end_freq_hz; /* 最后一格强制终点频 */ - } - return table_length; /* 返回表长 */ -} - -/** 绑加速表(id=1)或减速表(id=2)；长度为 0 则退回不用表 */ -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; /* 步长至少1 */ - runtime->freq_table_id = (table_length > 0U) ? freq_table_id : 0U; /* 有长度才绑id，否则退回0=不用表 */ -} - diff --git a/plsr/accel_curve/plsr_accel_curve.h b/plsr/accel_curve/plsr_accel_curve.h index cd1422f..8ce5d1e 100644 --- a/plsr/accel_curve/plsr_accel_curve.h +++ b/plsr/accel_curve/plsr_accel_curve.h @@ -62,7 +62,7 @@ uint32_t PlsrAccelCurveFreqAtPulse(const PlsrAccelPlan_t *plan, /** * 单相逐脉冲运行态（ISR 热路径，每来一个 UPDATE 调 Step 一次） * 直线：每次按目标平方值逐步逼近下一频率 - * S/正弦：查预计算频率表（开相 BeginAcc/BeginDec 时填好） + * S/正弦：时间累计 + 257 点 Q24 形状表线性插值（不再查预建频率表） */ typedef struct { uint32_t cur_freq_hz; /* 当前输出频率 */ @@ -75,11 +75,11 @@ typedef struct { uint32_t jerk_time_us; /* S 曲线单个 jerk 阶段时间 */ uint32_t const_accel_time_us; /* S 曲线恒加速阶段时间 */ uint32_t ramp_time_us; /* 整个斜坡时间 */ - uint32_t table_stride; /* 每几个脉冲读取一个表项 */ - uint32_t table_length; /* 表中有效项数 */ + uint32_t table_stride; /* 保留字段（不再用于预建频表） */ + uint32_t table_length; /* 保留字段（不再用于预建频表） */ uint8_t freq_rising; /* 1=频率升高，0=降低 */ uint8_t is_active; /* 1=当前相仍在运行 */ - uint8_t freq_table_id; /* 0=不用表，1=加速表，2=减速表 */ + uint8_t freq_table_id; /* 保留：恒为 0（不用预建频表） */ PlsrAccelMode_e curve_mode; /* 直线 / S / 正弦 */ } PlsrAccelRuntime_t; @@ -87,13 +87,13 @@ typedef struct { extern PlsrAccelPlan_t g_plsr_accel_plan; /* 本段三相规划结果 */ extern PlsrAccelRuntime_t g_plsr_accel_runtime; /* 当前相 ISR 逐拍状态 */ -/** PlanSeg 后调用：预建 S/正弦加减速频率表（勿在 ISR） */ +/** 兼容旧调用：S/正弦不再预建频率表（空操作） */ void PlsrAccelPrebuildFreqTables(const PlsrAccelPlan_t *plan); -/** 进入加速相：起→峰，并绑加速预建表 */ +/** 进入加速相：起→峰；S/正弦在 ISR 内形状表取频 */ void PlsrAccelBeginAccel(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan); -/** 进入减速相：峰→止，并绑减速预建表 */ +/** 进入减速相：峰→止；S/正弦在 ISR 内形状表取频 */ void PlsrAccelBeginDecel(PlsrAccelRuntime_t *runtime, const PlsrAccelPlan_t *plan); /** 进入匀速相：钉峰值，ISR 不再改频 */ diff --git a/plsr/run_control/plsr_run_control.c b/plsr/run_control/plsr_run_control.c index dc7a04b..04736ae 100644 --- a/plsr/run_control/plsr_run_control.c +++ b/plsr/run_control/plsr_run_control.c @@ -1672,7 +1672,7 @@ static void PlsrRunControlPlanSeg(uint32_t total, uint32_t f_from, g_plsr_accel_runtime.total_pulses = 0U; /* 开跑前清当前相步进计数 */ /* S/正弦：在任务上下文预建频率表，ISR 只查表 */ - PlsrAccelPrebuildFreqTables(&g_plsr_accel_plan); /* S/正弦表预建好 */ + PlsrAccelPrebuildFreqTables(&g_plsr_accel_plan); /* 兼容空操作：S/正弦 ISR 内形状表取频 */ /* 运行层使用规划后的实际起始频率 */ f_start = g_plsr_accel_plan.start_freq_hz; /* 规划后的实际入口 */