hanyongwei
/
TrainCamp_hanyongwei_StageTwo
1
0
Разклонения 0
Код Задачи 0 Заявки за сливане 0 Версии 0 Уики Activity
Преглед на файлове

修改为定点数Q24，采用查形状表+线性插值 

Signed-off-by: hanyongwei <2043702190@qq.com>
dev2
hanyongwei преди 2 седмици
родител
92742fffc1
ревизия
62b49681a3
променени са 3 файла, в които са добавени 262 реда и са изтрити 406 реда
Разделен изглед
Diff Options
Показване на статистика Download Patch File Download Diff File
  1. +254
    -398
      plsr/accel_curve/plsr_accel_curve.c
  2. +7
    -7
      plsr/accel_curve/plsr_accel_curve.h
  3. +1
    -1
      plsr/run_control/plsr_run_control.c

+ 254
- 398
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=不用表 */
}


+ 7
- 7
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 不再改频 */


+ 1
- 1
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; /* 规划后的实际入口 */


Пиши Преглед
Зареждане…
Отказ
Запис