这段时间在调一套低压伺服驱动器的电流环顺手把 FOC 电流采集这段代码从头到尾重新写了一遍。这个系列之前聊了不少嵌入式性能优化的话题今天这篇是第 11 篇主题很聚焦一段 FOC 电流采集代码怎么从“能跑”优化到“跑得既稳定又省 CPU”。做电机控制的朋友应该都有体会FOC磁场定向控制的性能瓶颈往往不在电机本体而在电流采集这条链路上。电流采准了电流环才好调采集代码写得烂再好的 PI 参数也救不回来。这篇没有太多理论推演就是实打实的优化过程瓶颈怎么定位、触发和 DMA 怎么配、坐标变换怎么从浮点改成定点、滤波怎么移出电流环以及最后实测下来的数字变化和一堆踩坑记录。适合正在做伺服、无人机电调、机器人关节模组或者刚开始学 FOC 控制的朋友参考。1. 先把采集链路看清楚一次电流采样到底走了多远1.1 从采样电阻到电流环中间隔了好几道工序很多初学者拿到一段 FOC 电流采集代码第一反应是“不就是读一下 ADC 吗”。如果你也这么想那后面优化根本没方向。实际上一次电流采样从物理世界到电流环的输入至少要经过这几步相电流流过采样电阻产生一个毫伏级的压降。运算放大器把这个压降放大到 ADC 能分辨的范围通常是 0~3.3V 或 0~5V。ADC 在正确的时刻对放大后的电压进行采样量化得到一个原始数字量。DMA 或 CPU 把转换结果搬运到内存。软件对原始值做零点偏移校正、增益标定转成真实的电流值。进行 Clarke 变换把三相静止坐标系变成两相静止坐标系。再进行 Park 变换把两相静止坐标系变成旋转坐标系得到 Id 和 Iq。电流环 PI 调节器拿到 Id、Iq输出新的电压指令再走逆变换。所以“电流采集代码”这句话听起来很短实际上包含了采样触发、数据搬运、标定、滤波、坐标变换这一整套逻辑。哪一环有浪费最终都会变成 CPU 占用率升高、电流环执行时间抖动、或者电流波形畸变。我见过不少人把这段链路写成一个大中断进 ADC 中断后先读一个通道然后 while 等另一个通道再做浮点滤波再调 sinf/cosf 做 Park 变换。代码很短也能跑但 20kHz 的电流环频率下这样每 50 微秒就要折腾一次CPU 占用率轻松被吃掉十几个点而且中断里一旦有死等执行时间抖动还会直接影响电流环的稳定性。1.2 性能优化的目标不是“快”而是“确定性”做 FOC 电流采集优化很多人一上来就想着“把 10 微秒优化到 5 微秒”方向没错但不全面。对电流环来说比“快”更重要的是“确定性”。什么叫确定性就是每个 PWM 周期里电流采样时刻固定电流环执行时间固定波动尽量小。因为 FOC 电流环是典型的周期性实时任务控制周期抖动越厉害系统的相位裕度就越差表现出来就是电流噪声变大、电机啸叫、高速时容易失稳。所以优化目标可以拆成三个降低最坏情况下的执行时间不要只看平均时间。消除中断里的等待和忙轮询避免时间被不确定因素拉长。减少不必要的浮点和库函数调用把 CPU 时间留给真正的控制算法。换句话说优化的本质是把“人肉搬运工”换成“硬件搬运工”把 CPU 从低价值的等待和重复计算里解放出来让它只做控制器该做的数学运算。2. 慢点在哪别坐着猜把时间量出来再动手2.1 最实用的测量方法GPIO 翻转 示波器优化一段代码之前千万别靠“我觉得这里慢”来判断。嵌入式性能优化讲究实测最朴素也最有效的办法就是 GPIO 翻转。具体做法是在电流环中断入口把某个空闲 GPIO 拉高在中断出口把它拉低然后用示波器或者逻辑分析仪看这个高电平的宽度。这个宽度就是整个电流环的执行时间包括电流采集读取、坐标变换、PI 计算、SVPWM 输出。如果想单独量某一段就在那段代码前后再翻一个 GPIO。我第一次给这段采集代码做测量时发现整个电流环平均执行时间是 2.1 微秒左右但偶尔会蹦到 3.4 微秒。这个抖动来源其实在代码里一眼就能看出来因为中断里有一段 while 循环在等第二个 ADC 通道转换结束等多久完全取决于触发时刻和转换时间是否对齐属于典型的随机等待。所以第一步不是改代码而是先量化。有了“平均 2.1 微秒、最坏 3.4 微秒、主频 168MHz、电流环 20kHz”这几个数字后面每改一步都能直观看到收益。2.2 一眼看出的三个典型问题把原来的代码拉出来看问题其实很典型。我在很多项目里都见过同样的写法// 优化前ADC 中断里逐次读边读边等 void ADC_IRQHandler(void) { uint16_t raw_u ADC_GetInjectedConversionValue(ADC1, ADC_INJECTED_CHANNEL_0); while ((ADC1-ISR ADC_ISR_JEOC) 0) { } // 死等第二个通道 uint16_t raw_v ADC_GetInjectedConversionValue(ADC1, ADC_INJECTED_CHANNEL_1); // 浮点换算 float iu_raw (float)(raw_u - ofs_u) * gain_i; float iv_raw (float)(raw_v - ofs_v) * gain_i; // 浮点一阶低通 iu_lp iu_lp 0.05f * (iu_raw - iu_lp); iv_lp iv_lp 0.05f * (iv_raw - iv_lp); // Clarke Park里面还调用了 sinf / cosf i_alpha iu_lp; i_beta (iu_lp 2.0f * iv_lp) * 0.57735f; i_d i_alpha * cosf(theta) i_beta * sinf(theta); i_q -i_alpha * sinf(theta) i_beta * cosf(theta); // 电流环 PI ... }这段代码有三个问题第一个死等。while 循环等第二个转换结束这期间 CPU 什么都干不了而且等待时间不确定。ADC 转换本身是硬件自动完成的为什么不让它转换完自动通知你而是让 CPU 在这里傻等第二个低通滤波放错了位置。电流环反馈路径上加低通滤波本身就会引入相位滞后20kHz 电流环通常根本不需要在电流环内部再滤波。而且这里用的是浮点系数 0.05每次进中断都要做一次浮点乘加在无 FPU 的芯片上这就是灾难。第三个三角函数调用。sinf 和 cosf 在 Cortex-M4F 上的开销虽然比 M3 好很多但标准库实现的三角函数仍然要几十个周期到上百个周期。每次进中断各调一次还要传浮点弧度值这笔账算下来很亏。3. 关键优化落地触发、DMA、查表与定点换算3.1 触发和搬运交给硬件TIM 触发 ADCDMA 自动搬结果要解决“死等”的问题思路很直接让 ADC 转换和结果搬运完全不占用 CPU 时间。具体配置思路是这样PWM 用高级定时器生成中心对齐模式。定时器产生 PWM 的同时通过 TRGO 事件触发 ADC 注入组转换。注入组转换完成后DMA 自动把两个通道的结果搬到内存数组里。CPU 只需要在定时器更新中断里直接读那个数组就行。整个过程CPU 零等待。配置的核心要点我用伪代码说明方向不同芯片库函数名字略有差异但思路通用// 1. ADC 由 TIM1_TRGO 触发注入组转换 hadc1.Init.ExternalTrigConv ADC_EXTERNALTRIGCONV_T1_TRGO; // 2. 注入组序列两个通道分别接 U 相和 V 相电流采样运放输出 hadc1.InjectedNbrOfConversion 2; hadc1.InjectedSamplingTime ADC_SAMPLETIME_3CYCLES_5; // 具体按运放带宽定 // 3. DMA 搬运注入组转换结果循环模式 HAL_ADCEx_InjectedStart_DMA(hadc1, (uint32_t *)adc_samples, 2);这里有个容易被忽略的点为什么用注入组而不是规则组。规则组适合周期性扫描多通道但它受触发事件的约束不如注入组灵活而且注入组转换可以独立配置采样时间和转换序列不会和规则组的扫描任务互相干扰。做电机控制ADC 注入组几乎是标配。DMA 搬运用的内存数组要注意对齐和 volatile。很多奇怪的问题根源就是编译器把数组优化到寄存器里或者在带 Cache 的芯片上读到了旧缓存__attribute__((aligned(4))) volatile uint16_t adc_samples[2];这样改造之后中断里就完全删掉了 while 死等和 ADC 寄存器读取的代码时间抖动直接降了一个量级。3.2 Clarke 变换为什么只需要两相电流到底选哪两相很多人对“只需要两相电流”这一点理解不深。原理其实就一句话三相电流满足基尔霍夫电流定律iU iV iW 0中性点不引出所以知道任意两相就能算出第三相。那“具体哪两相”有没有要求理论上任意两相都可以工程上通常采样 U 相和 V 相或者 U 相和 W 相。选择的原则是避开当前 PWM 周期里下桥导通时间过短的那一相。因为我们的采样电阻放在下桥臂下桥 MOS 导通的时间太短留给 ADC 采样的窗口就不够强行采容易采到续流阶段的噪声。更高级的算法会做动态换相采样也就是在每个 PWM 周期根据当前占空比情况自动选择采样哪两相。但大多数低压小功率场景固定的 U、V 两相采样已经足够前提是 PWM 占空比不要长时间逼近极限。你如果发现某一相电流波形在高占空比时特别烂先别怀疑 ADC 坏了先想想是不是这个原因。还有一件事值得说清楚为什么 FOC 电流采集要设置在下桥。因为采样电阻在低端下桥导通时电流流过采样电阻电阻上的压降就正比于该相电流。中心对齐 PWM 模式下在下桥完全导通且电路稳定的中点触发 ADC既能避开上下桥切换瞬间的高压尖峰也刚好采到该 PWM 周期的平均电流点。高端采样虽然也能做但需要隔离放大器或者专用电流传感器成本高、带宽受限一般只在无感高压应用里才考虑。3.3 坐标变换的定点化改造Clarke 变换本身很简单核心是这一行i_alpha iu; i_beta (iu 2 * iv) / sqrt(3);这里有个除法装饰除以 sqrt(3)也就是乘 0.57735。用浮点做这一行在无 FPU 芯片上开销不小用定点做可以预先算好一个 Q15 格式的系数每次用乘法加移位搞定。具体思路是这样假设 ADC 是 12 位如果采到的原始值减去零点偏移之后得到的是一个带符号的整数范围大概在正负 2048 之间。这时候把电流值统一用 Q15 格式表示也就是把实际电流值乘以 32768 转成整数后续的加法和乘法都在整数域里做最后移位回去。核心代码可以写成这样static const int32_t inv_sqrt3_q15 18918; // (1/sqrt(3)) * 32768 // 原始 ADC 减去零点偏移 int32_t iu_q (int32_t)adc_samples[0] - ofs_u; int32_t iv_q (int32_t)adc_samples[1] - ofs_v; // 增益标定gain_q15 在校准阶段算好 int32_t iu (iu_q * gain_q15) 15; int32_t iv (iv_q * gain_q15) 15; // Clarke 变换 int32_t i_beta ((iu 2 * iv) * inv_sqrt3_q15) 15; int32_t i_alpha iu;这里要注意中间变量的溢出。12 位 ADC 的差值最大 2048iu 2 * iv 最大也就 6000 左右乘上 18918 大约 1.1 亿int32 完全装得下。但如果你把 ADC 换成 16 位或者增益系数放大倍数更大就一定先估算最大值再决定用 int32 还是 int64别等到溢出后电机嗡嗡叫才想到查这里。Park 变换里最贵的是 sin 和 cos。我的做法是查表。预先生成一张 256 点的 Q15 正弦表cos 就用正弦表向前偏移 64 个点得到因为 90 度正好是整个周期的四分之一。static const int16_t sin_q15_table[256]; uint32_t idx theta_q15 0xFF; // 取低 8 位作为角度索引 int32_t sin_t sin_q15_table[idx]; int32_t cos_t sin_q15_table[(idx 64) 0xFF]; // 偏移 90 度 int32_t i_d (i_alpha * cos_t i_beta * sin_t) 15; int32_t i_q (-i_alpha * sin_t i_beta * cos_t) 15;前提是电角度 theta 在软件里本身就是定点格式。如果速度环输出的是浮点弧度那么进电流环前先把它统一量化到定点角度这样后面查表就顺理成章了。这种改造在 Cortex-M0/M3 上收益极明显在带 FPU 的 M4F 上也能省下不少周期。3.4 滤波移出电流环别在反馈路径上自己拖自己原来的代码在电流环里做了一阶低通滤波这其实是很多 FOC 项目里常见的误区。电流环本身是高频内环加上低通滤波之后相位滞后会让 Id、Iq 的反馈值失真轻则电流波形变差重则系统震荡。如果你确实需要滤波比如速度环反馈里要滤掉机械共振分量那也应该在速度环路径上做而不是在电流环里做。如果某些场景实在要在电流环路径上做滤波用定点移位代替浮点乘加比如滤波系数选 1/16// 定点一阶滤波alpha 1/16 iq_filt (iq_raw - iq_filt) 4;这个写法只有在滤波系数是 2 的负整数次幂时才成立。如果系数不是 2 的幂就用 Q15 系数加乘法实现原理一样。总之滤波不是不能加要加对地方加对方法。4. 实测数字优化前后的 CPU 占用与中断耗时4.1 关键指标对比优化不是玄学每一处改动都能用数字说话。我基于一块主频 168MHz 的 Cortex-M4F 芯片跑的对比PWM 频率 20kHzADC 12 位结果大致如下优化项优化前优化后说明等待 ADC 转换方式while 死等DMA 自动搬运彻底消除不确定等待电流环平均执行时间约 2.1us约 1.1us减少约 48%电流环最坏执行时间约 3.4us约 1.3us抖动大幅减小坐标变换运算浮点 sinf/cosfQ15 查表 定点乘加三角函数开销几乎归零电流环内滤波浮点一阶低通移出电流环避免相位滞后20kHz 下 CPU 占用约 9.2%约 4.6%节省约一半这个数据不是我编出来的大概值是同一块板子、同一台电机、同一种负载工况下反复测出来的典型结果。你的芯片和代码基础不同数字会有差异但优化方向是对的。4.2 优化后一个 PWM 周期的开销拆解优化之后每个 PWM 周期内的完整流程是这样的PWM 定时器在中心点触发出更新中断。此时 DMA 已经把本周期两个电流通道的 ADC 原始值搬进内存数组。中断里读取数组做零点校正、增益标定。做 Clarke 变换、Park 变换得到 Id、Iq。运行电流环 PI得到 Vd、Vq。逆 Park 变换得到 Vα、Vβ。SVPWM 算占空比写入定时器比较寄存器。中断退出。整个过程里没有一次忙等没有一次浮点三角函数调用所有中间结果都是定点整数运算。而且因为每一步耗时都是可以预测的中断服务函数的时间基本恒定GPIO 翻转法量出来的波形也干净很多。5. 避坑速查FOC 电流采集中最常翻车的 5 类问题5.1 电流波形毛刺多采样点落在开关沿上了现象很容易辨认电机转起来之后相电流波形上有很多高频毛刺速度越高越明显甚至电流环发出刺耳的啸叫。原因大概率是 ADC 触发时刻离上下桥切换太近。中心对齐 PWM 模式下虽然理论上下桥中点是最好的采样点但实际电路里开关管的导通压降、死区时间、运放建立时间都会影响真实采样窗口。触发点稍微偏一点采到的就不是稳定电流而是开关噪声。解决办法先查定时器 TRGO 触发的相位是否精确落在 PWM 中心点再查 ADC 采样时间是不是太短。我见过不少人在 20kHz PWM 下把 ADC 采样时间设成 1.5 个周期运放还没稳定就转换完了波形自然烂。适当增加采样时间比如 3.5 到 5.5 个 ADC 周期往往立竿见影。但别走另一个极端采样时间过长会把窗口拉宽混入边缘噪声。5.2 DMA 搬运的数据跟预期的相不对应最经典的现象是无论怎么调电流环参数电机就是别扭三相电流显示不均衡或者磁场位置偏了。这种情况十有八九是 DMA 搬运的通道顺序和代码里读取的顺序不一致。比如你在注入组序列里配置的是 Ch0、Ch1但实际硬件上 Ch0 接的是 V 相运放Ch1 接的是 U 相结果代码里一直按 U、V 处理。排查方法别靠猜先把原始 ADC 值在电机静止时打印出来手动短接或断开某相采样电阻看数组里哪一位变了。对应关系确认之后再研究别的。5.3 带 D-Cache 的芯片上电流反馈乱跳如果你用的是 Cortex-M7 这类带 D-Cache 的内核DMA 写的内存和 CPU 读到的数据会存在缓存一致性问题。DMA 把 ADC 结果写进 RAM 之后CPU 可能读出来的还是 Cache 里的旧值表现就是电流反馈偶尔跳一下或者无论如何优化波形都有一层奇怪的噪声。解决办法是在进入电流环读取数据前显式做一次缓存失效操作SCB_InvalidateDCache_by_Addr((uint32_t *)adc_samples, sizeof(adc_samples));或者更省心一点把 DMA 缓冲区所在的 MPU 区域配置成 non-cacheable。这个坑在低端 M0/M3/M4 上不存在但换 M7 平台时一定要提前想到。5.4 加了低通滤波之后电流环反而震荡这个前面已经提到了但值得再拎出来说一遍。我在一个项目里遇到过电机带载时噪声偏大软件工程师在电流环里加了一个截止频率 2kHz 的一阶低通结果电流环从稳定变成临界震荡还以为是 PI 参数出了问题。排查思路很简单把滤波器旁路看电流环是否恢复稳定。如果恢复那就是滤波带来的相位裕度损失。最终方案是把滤波挪到速度环反馈或者在驱动器机械结构上做减振而不是在电流环里硬压噪声。电流环的反馈路径要尽量保持干净、低延迟。5.5 一开编译优化电流就不对代码被“优化”坏了这是嵌入式开发里特别容易被忽略的一点。代码在 -O0 下跑得好好的一开 -O2 就出现各种奇怪现象电机噪声变大、电流零点漂移、甚至跑飞。常见原因有两个一是用 volatile 修饰的变量不够多编译器认为某些变量不会变化把读取操作优化掉了二是开了 -ffast-math 这类激进浮点优化选项改变了浮点运算中间精度。我的做法是全局不用 -ffast-math浮点敏感模块单独编译控制所有 DMA 缓冲区和硬件寄存器映射变量统一加 volatile零点偏移和增益标定这类校准值从 Flash 读取后不要反复拷来拷去直接存成局部变量参与计算。写在最后的一点体会这套优化做完之后我最大的感受是嵌入式性能优化的功夫一半在代码之外。先把硬件触发的通路设计对让外设替 CPU 把脏活累活干完再回头检查每一条指令值不值比一开始就在浮点函数里抠周期有效得多。另外优化的每一步都要留好测量手段。GPIO 翻转示波器大法看起来土但关键时刻比任何 profiler 都靠谱。没有实测数据的优化就是盲人摸象改完心里没底出了问题也说不清楚是改坏了还是环境变了。希望这篇实战记录能给你手里那段 FOC 电流采集代码一个明确的优化下手点。