|
|
@@ -8,23 +8,151 @@ |
|
|
PlsrAccelPlan_t g_plsr_accel_plan; /* 当前段三相规划结果 */ /* 整段规划全局一份 */ |
|
|
PlsrAccelPlan_t g_plsr_accel_plan; /* 当前段三相规划结果 */ /* 整段规划全局一份 */ |
|
|
PlsrAccelRuntime_t g_plsr_accel_runtime; /* 当前相 ISR 逐拍状态 */ /* 当前相运行态全局一份 */ |
|
|
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); |
|
|
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); |
|
|
uint8_t resolving_start); |
|
|
static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, |
|
|
static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, |
|
|
uint32_t end_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, |
|
|
static uint32_t PlsrAccelCurveCalcLinearRampPulses(uint32_t start_freq_hz, uint32_t end_freq_hz, |
|
|
uint32_t accel_hz_per_s); |
|
|
uint32_t accel_hz_per_s); |
|
|
static uint32_t PlsrAccelCurveLinearFreqAtPulse(uint32_t start_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 pulse_count, uint8_t freq_rising, |
|
|
uint32_t target_limit_hz); |
|
|
uint32_t target_limit_hz); |
|
|
static uint32_t PlsrAccelNextFreqLinear(PlsrAccelRuntime_t *runtime); |
|
|
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, |
|
|
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 *jerk_time_us_out, |
|
|
uint32_t *const_accel_time_us_out, |
|
|
uint32_t *const_accel_time_us_out, |
|
|
uint32_t *ramp_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, |
|
|
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 *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); |
|
|
uint32_t ramp_time_us, uint32_t elapsed_time_us); |
|
|
static uint32_t PlsrAccelCurveSineFreqAtTime(uint32_t start_freq_hz, uint32_t end_freq_hz, |
|
|
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 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, |
|
|
uint32_t decel_rate_hz_per_s, |
|
|
PlsrAccelMode_e curve_mode); |
|
|
PlsrAccelMode_e curve_mode); |
|
|
static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime); |
|
|
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 */ |
|
|
/* 公共 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) |
|
|
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 进入加速相 */ |
|
|
/** @brief 进入加速相 */ |
|
|
@@ -441,7 +518,6 @@ void PlsrAccelBeginAccel(PlsrAccelRuntime_t *runtime, |
|
|
{ |
|
|
{ |
|
|
return; /* 缺谁都不开相 */ |
|
|
return; /* 缺谁都不开相 */ |
|
|
} |
|
|
} |
|
|
/*计算加速或者减速的时间*/ |
|
|
|
|
|
PlsrAccelRuntimeBeginRamp(runtime, /* 填运行态:起→峰 */ |
|
|
PlsrAccelRuntimeBeginRamp(runtime, /* 填运行态:起→峰 */ |
|
|
plan->start_freq_hz, /* 入口频 */ |
|
|
plan->start_freq_hz, /* 入口频 */ |
|
|
plan->target_freq_hz, /* 峰值 */ |
|
|
plan->target_freq_hz, /* 峰值 */ |
|
|
@@ -449,9 +525,10 @@ void PlsrAccelBeginAccel(PlsrAccelRuntime_t *runtime, |
|
|
plan->accel_rate_hz_per_s, /* 加速斜率 */ |
|
|
plan->accel_rate_hz_per_s, /* 加速斜率 */ |
|
|
plan->decel_rate_hz_per_s, /* 减速斜率 */ |
|
|
plan->decel_rate_hz_per_s, /* 减速斜率 */ |
|
|
plan->curve_mode); /* 曲线模式 */ |
|
|
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 进入减速相 */ |
|
|
/** @brief 进入减速相 */ |
|
|
@@ -469,9 +546,9 @@ void PlsrAccelBeginDecel(PlsrAccelRuntime_t *runtime, |
|
|
plan->accel_rate_hz_per_s, /* 斜率 */ |
|
|
plan->accel_rate_hz_per_s, /* 斜率 */ |
|
|
plan->decel_rate_hz_per_s, /* 斜率 */ |
|
|
plan->decel_rate_hz_per_s, /* 斜率 */ |
|
|
plan->curve_mode); /* 模式 */ |
|
|
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 进入匀速相 */ |
|
|
/** @brief 进入匀速相 */ |
|
|
@@ -502,13 +579,10 @@ void PlsrAccelBeginConstSpeed(PlsrAccelRuntime_t *runtime, |
|
|
|
|
|
|
|
|
/** |
|
|
/** |
|
|
* @brief ISR 里走:本相再往前一拍,返回下一拍该写成多大频率 |
|
|
* @brief ISR 里走:本相再往前一拍,返回下一拍该写成多大频率 |
|
|
* 分支:直线逼近 / 查预建表 / 建表失败则按时间一步步算;末拍前避免提前钉死到终点频。 |
|
|
|
|
|
|
|
|
* 直线:现场 v^2 逼近;S/正弦:时间累计 + 257 点 Q24 形状表线性插值(不查 4096 预建频表)。 |
|
|
*/ |
|
|
*/ |
|
|
uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime) |
|
|
uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime) |
|
|
{ |
|
|
{ |
|
|
const uint32_t *freq_table; /* 指向加速表或减速表 */ |
|
|
|
|
|
uint32_t table_index; /* 表下标 */ |
|
|
|
|
|
|
|
|
|
|
|
if (runtime == (PlsrAccelRuntime_t *)0) /* 空指针 */ |
|
|
if (runtime == (PlsrAccelRuntime_t *)0) /* 空指针 */ |
|
|
{ |
|
|
{ |
|
|
return 0U; /* 没runtime返回0 */ |
|
|
return 0U; /* 没runtime返回0 */ |
|
|
@@ -534,33 +608,10 @@ uint32_t PlsrAccelNextFreq(PlsrAccelRuntime_t *runtime) |
|
|
runtime->cur_freq_hz = PlsrAccelNextFreqLinear(runtime); /* 用平方目标±1逼近下一频 */ |
|
|
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 |
|
|
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->cur_freq_hz = runtime->end_freq_hz; /* 钉到本相终点频 */ |
|
|
runtime->is_active = 0U; /* 停相 */ |
|
|
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->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); /* 最后钳一次再返回 */ |
|
|
return PlsrAccelCurveClampFreqHz(runtime->cur_freq_hz); /* 最后钳一次再返回 */ |
|
|
@@ -807,23 +814,25 @@ static uint32_t PlsrAccelCurveResolveBoundaryFreq(uint32_t configured_freq_hz, |
|
|
return jump_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, |
|
|
static uint32_t PlsrAccelCurveInterpolate(uint32_t start_val, |
|
|
uint32_t end_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; /* 直接终点 */ |
|
|
return end_val; /* 直接终点 */ |
|
|
} |
|
|
} |
|
|
if (end_val >= start_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 / 正弦 */ |
|
|
/* 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 min_val; /* 段左端形状值 */ |
|
|
uint32_t max_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): |
|
|
* S 曲线时间参数(加速度梯形 1:2:1): |
|
|
* Tj : Ta : Tj = 1 : 2 : 1 |
|
|
* Tj : Ta : Tj = 1 : 2 : 1 |
|
|
* Δf = A*(Ta+Tj) ⇒ Tj=Δf/(3A), Ta=2Δf/(3A), T=4Δf/(3A) |
|
|
* Δ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, |
|
|
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 *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 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) |
|
|
* 正弦 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, |
|
|
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 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; /* 插值后频率 */ |
|
|
uint32_t freq_hz; /* 插值后频率 */ |
|
|
|
|
|
|
|
|
if ((ramp_time_us < 1U) || (start_freq_hz == end_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); /* 钉终点 */ |
|
|
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) /* 保护 */ |
|
|
if (freq_hz < 1U) /* 保护 */ |
|
|
{ |
|
|
{ |
|
|
freq_hz = 1U; /* 至少1 */ |
|
|
freq_hz = 1U; /* 至少1 */ |
|
|
@@ -1280,16 +1237,13 @@ static uint32_t PlsrAccelCurveTimedFreqAtPulse( |
|
|
} |
|
|
} |
|
|
elapsed_time_us += 1000000UL / cur_freq_hz; /* Δt = 1/f(µs) */ |
|
|
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 = |
|
|
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 |
|
|
else |
|
|
{ |
|
|
{ |
|
|
@@ -1385,11 +1339,10 @@ static uint32_t PlsrAccelNextFreqTimed(PlsrAccelRuntime_t *runtime) |
|
|
return PlsrAccelCurveClampFreqHz(runtime->end_freq_hz); /* 返回终点频 */ |
|
|
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 |
|
|
else |
|
|
{ |
|
|
{ |
|
|
@@ -1587,13 +1540,13 @@ static void PlsrAccelRuntimeBeginRamp(PlsrAccelRuntime_t *runtime, |
|
|
/* 有脉冲且起止不同才激活 */ |
|
|
/* 有脉冲且起止不同才激活 */ |
|
|
runtime->is_active = ((total_pulses > 0U) && |
|
|
runtime->is_active = ((total_pulses > 0U) && |
|
|
(start_freq_hz != end_freq_hz)) ? 1U : 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_length = 0U; /* 表长清 */ |
|
|
runtime->table_stride = 1U; /* 步长默认1 */ |
|
|
|
|
|
|
|
|
runtime->table_stride = 1U; /* 保留字段 */ |
|
|
PlsrAccelRuntimeCalcTimes(runtime); /* 按模式预填时间参数 */ |
|
|
PlsrAccelRuntimeCalcTimes(runtime); /* 按模式预填时间参数 */ |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
/** 离散仿真一步:与建表 / 脉冲预算共用(时间步进) */ |
|
|
|
|
|
|
|
|
/** 离散仿真一步:与脉冲预算共用(时间步进 + 形状表) */ |
|
|
static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime) |
|
|
static void PlsrAccelRuntimeSimulateOnePulse(PlsrAccelRuntime_t *runtime) |
|
|
{ |
|
|
{ |
|
|
if (runtime->completed_pulses < 0xFFFFFFFFUL) /* 防溢出 */ |
|
|
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=不用表 */ |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|