写嵌入式FOC磁场定向控制的朋友应该都有这种体会电流采样代码看起来没几行平时也不怎么碰它但它常年住在中断服务函数里面而且必须在下一拍PWM边沿到来之前把电流算完交给电流环去跑。只要这段代码慢半拍电流反馈相位就滞后电机噪声、发热、振动全来了严重的甚至直接炸IGBT。这次这个系列写到第11篇前面聊过内存优化、启动时间、外设初始化这一篇专门处理一段实际工程里的FOC电流采集代码把里面白白浪费时钟周期的地方一个一个揪出来。我这次优化的项目是一个三相BLDC/PMSM电机控制器主控是Cortex-M4F内核的MCU主频168MHzFOC控制频率20kHz也就是一个控制周期50微秒。电流采样、均值滤波、坐标变换全部塞在ADC中断里中断频率也是20kHz。因为后面还得留给电流环PI和SVPWM生成时间我给自己定的优化目标很简单把电流采集整段代码从4微秒左右压到2微秒以内而且不能牺牲低速轻载下的电流精度。这篇文章适合正在做FOC电机控制、或者想优化中断实时性的嵌入式工程师代码思路可以照搬到Cortex-M3/M4、甚至无FPU的MCU上。1. 一段典型的FOC电流采集代码性能到底卡在哪里1.1 采样中断为什么成了电机控制的实时性瓶颈电流采样是整个FOC控制链路里面最靠前的一环。在一个控制周期内ADC中断要做的事情包括从ADC外设读取转换结果、做多次采样平均、减去硬件偏置并乘以灵敏度系数、做Clarke变换、做Park变换最后把d轴和q轴电流交给电流环。这些动作全部做完之后电流环才能根据误差计算占空比。听起来逻辑不复杂但问题在于它所在的位置是20kHz中断每个周期都跑累计开销非常可观。可以算一笔账假设这段代码优化前占用4.2微秒优化后降到1.4微秒省下来的时间是2.8微秒。在20kHz触发频率下每秒节省的时间是20,000乘以2.8微秒等于56毫秒也就是CPU总占用率的5.6%左右。如果一天连续运行下来相当于省出了大约80多分钟的纯CPU时间。这个数字放在智能手表这种小电池设备上也许无所谓但在工业驱动器里多出来的余量意味着可以把PWM频率从20kHz提到30kHz或者塞进一个更复杂的自适应观测器。还有一点很容易被忽略Cortex-M4F虽然自带硬件FPU浮点加减乘除都是单周期执行但一旦中断服务函数里使用了float或double类型编译器会在进入中断和退出中断时把FPU寄存器保存到栈上、再从栈上恢复。这部分现场保护开销会随着中断频率线性累积。如果中断函数里再调用sinf/cosf这种数学库函数内核会切换到函数调用模式压栈、跳转、内部迭代计算最后再弹栈返回这些周期加在一起往往比我们以为的多出好几倍。1.2 最容易被忽视的三个开销陷阱第一个陷阱就是数学库函数调用。很多工程师以为M4F有FPU调用sinf/cosf就很快。实际上编译器为了满足浮点标准精度要求会在库函数内部做参数范围规约和多项式迭代一个sinf调用在168MHz主频下常常要100到200个周期两个函数调用加起来就接近1微秒。第二个陷阱是内存访问不对齐。如果中断里访问的结构体成员、数组元素没有按照4字节地址对齐MCU会插入额外的总线周期来拆分访问严重的话一次读取要两三次总线周期。第三个陷阱是FPU现场保存。我测试过同样的运算逻辑用浮点类型编译出来的中断入口代码比用定点整型多出好几十条现场保存指令。中断不频繁时无所谓20kHz下每天要进出中断超过17亿次这个开销就非常可观了。1.3 这次优化的目标和评判标准我在动手之前先把约束条件写清楚。时间目标刚才说了ADC中断内总耗时从约4.1微秒压到2微秒以内。精度约束是额定电流下稳态电流反馈误差不超过0.5%FS低速轻载时不能出现明显的零漂和电流毛刺。可维护性约束是不能为了性能把代码写成只有自己能看懂的魔法数字系数和表长都要用宏或者常量定义关键段落必须留注释。定这组目标的原因很实际单纯把代码改快是容易的但改完之后如果低速精度烂了或者代码维护性差了那这个优化就没有工程意义。我希望做完之后这段代码既可以在当前项目里跑也能被其他同事直接搬到一个新的主控平台上去。2. 动手前先把原理理顺采样时机、两相电流与坐标变换2.1 电流采样为什么要精确对齐下桥臂导通在动手改代码之前我一直跟团队里的人强调一个原则要优化一段代码得先搞清楚这段代码外面挂着的硬约束。电流采集最硬的约束就是采样窗口。FOC控制通常采用低端采样电阻采样电阻串在逆变器下桥臂MOSFET的源极和地之间。只有当某一相的下桥臂MOSFET导通时该相电流才会流过采样电阻ADC才能采到有效的电压信号一旦上桥臂导通或者进入死区下桥臂没有电流通路采样电阻两端就没有代表相电流的电压。所以行业里才会反复强调“FOC电流采集要设置在下桥”这句话。中心对齐PWM模式下三相下桥臂全部导通的时刻正好出现在零矢量区间通常对应定时器计数到峰值或者谷值附近这是最稳定的采样窗口。触发点必须精确设置在这个窗口的中间位置离开关边沿太近就会采到振铃和尖峰离中心点太远又会跑出零矢量区间导致采样值失真。我这次工程里用的是定时器更新事件触发ADC注入组转换硬件上保证了触发时刻和PWM中心点对齐。2.2 Clarke变换为什么只需要两相电流选哪两相做过FOC的朋友都知道坐标变换第一步Clarke变换需要两相电流不需要三相全采。原理本身并不复杂电机三相绕组是星形连接电流满足基尔霍夫电流定律Ia加Ib加Ic等于零所以只要测到任意两相第三相就是前两相加负号。但选哪两相不是随意的。三电阻采样方案里如果某一相下桥臂占空比接近100%那这个桥臂的下管在大部分时间里是关断的采样窗口非常短甚至没有完整窗口此时该相电流测不准甚至测不到。所以采样相的选择要和当前扇区、PWM占空比一起考虑。我这次项目是两相采样加一相推算硬件上选了A相和B相C相用公式计算。这样省一路采样电阻和一路ADC通道在成本和PCB面积上也有优势。需要提醒的是用两相推算出来的第三相在死区和电流畸变时会有一定误差因此电流环闭环之后要关注C相电流是否有异常。2.3 采样、滤波、变换这条链路上的等待与计算开销我习惯把这段中断里的流程拆成采样、滤波、标定、变换四步性能问题往往藏在这些步骤的交界处。原始代码在ADC中断里逐个通道读结果每次读取都访问外设寄存器如果用库函数还得加上函数调用开销滤波环节用了浮点累加器和浮点除法标定环节做减法乘系数还算正常真正吃时间的是Park变换里的sinf和cosf。用表格列一下优化前的耗时估算这里我直接用DWT周期计数器量出来的数字168MHz下每个周期约5.95纳秒环节主要操作实测耗时ADC结果读取3通道顺序读取约0.7微秒均值滤波浮点累加加浮点除法约0.5微秒偏置和增益标定减偏移、乘灵敏度约0.6微秒Clarke变换乘1/√3操作约0.3微秒Park变换两次sinf/cosf库函数调用约1.9微秒其他开销全局变量写入、标志位约0.1微秒合计约4.1微秒看到没Park变换占了将近一半。这还没算中断现场保护的时间如果把FPU寄存器保存恢复的周期也算进去实际占用还会更多。3. 实战优化从浮点到定点从函数库到查表3.1 优化一多次采样平均改成移位除法电流采集环节为了抑制ADC量化噪声和PWM开关噪声一般会做多次采样取平均。原来的代码是连续采8次累加后用浮点除法除以8。这段逻辑的问题有两个一是浮点累加器每次都要执行FPU加法二是浮点除法即使有硬件FPU也比整数右移慢不少。既然平均次数固定为8而8是2的3次方除法完全可以用右移实现。优化后我先把每次ADC转换结果直接累加到int32_t变量里等累计到8次以后统一右移3位这样既省掉了浮点累加中转也绕开了除法。这里有两点经验第一累加器最好用32位整型不要用16位因为12位ADC结果最大是40958次累加就是32760好险没有超过int16_t的最大值32767但如果中间有偏置或者后续把采样次数从8次改成16次16位累加器直接溢出第二采样平均次数尽量选成2的幂次4、8、16都可以这算是一种“为优化留后门”的工程设计习惯。3.2 优化二三角函数查表替代sinf/cosf运行时计算这是整个优化里收益最大的一步。原来的Park变换要实时计算sin(theta)和cos(theta)代码写的是调用sinf和cosf。前面说过这两个库函数调用加起来大约占掉2微秒。我把电角度theta从浮点表示改成Q15格式0x0000到0xFFFF对应0度到360度然后建立一张4096点的int16_t正弦表存放在Flash里。取正弦值时直接查表取余弦值则把索引偏移表长的四分之一再查同一张表。因为表长度是4096也就是2的12次方取模操作可以直接用按位与实现省掉了耗时的取模运算符。这里有一个精度问题需要解释清楚。4096点正弦表的角度分辨率是360度除以4096约等于0.088度对电流环来说完全够用。如果用的是1024点表分辨率0.35度左右也能跑但动态响应要求高的时候会感觉到扭矩轻微波动。如果表长再增加到16384点Flash开销会到32KB以上普通MCU未必舍得。4096点、8KB Flash是性能和存储之间比较平衡的选择。查表代码的关键是索引计算#define SIN_TABLE_N 4096u #define SIN_TABLE_MASK (SIN_TABLE_N - 1u) // 电角度 theta_q15 范围 0~65535 uint16_t idx ((uint16_t)theta_q15) 4; // 高12位作索引 int16_t sin_t g_sin_table[idx]; int16_t cos_t g_sin_table[(idx (SIN_TABLE_N 2)) SIN_TABLE_MASK];这段代码执行起来就是一次移位、两次数组访问、一次按位与比sinf/cosf快了一个数量级。如果后续需要更高精度的正弦值可以在查表基础上加线性插值多花几条指令但通常电流环用不上。3.3 优化三坐标变换改为Q15定点运算与内联函数原代码里Clarke和Park变换用的是float运算虽然M4F有FPU但为了彻底去掉FPU现场保存开销我把整段坐标变换改成Q15定点运算。Q15格式简单说就是用一个16位有符号整数表示-1.0到0.99997的范围比如32767代表1.0-32768代表-1.0。ADC原始值经过偏置和灵敏度标定后会被换算到这个Q15区间。Clarke变换公式是alpha等于A相电流beta等于A相加上两倍B相再乘以1除以根号3。定点实现里1除以根号3这个系数用Q15常数18920表示#define INV_SQRT3_Q15 18920 // 0.577350269 * 32768 int32_t alpha ia_q15; int32_t beta ((ia_q15 2 * ib_q15) * INV_SQRT3_Q15) 15;乘完之后右移15位相当于把Q15乘Q15的结果从Q30降到Q15。要注意这里ia_q15和ib_q15是int32_t类型因为中间要存Clarke变换里的临时乘积必须用32位变量防止溢出。Park变换的定点写法同理乘加之后右移15位int32_t id ((int32_t)alpha * cos_t (int32_t)beta * sin_t) 15; int32_t iq ((int32_t)beta * cos_t - (int32_t)alpha * sin_t) 15;同时我把原本拆成单独函数的小计算改成static inline内联函数让编译器直接把运算逻辑嵌入到中断里省掉函数调用的压栈和跳转开销。注意这里不能用普通函数的调用否则性能提升会被调用开销吃掉一截。还有一个细节有符号数的右移和除法在负数情况下语义不完全一致。定点运算里右移实际上算的是向下取整除法而C语言整数除法向零取整。在FOC变换这种乘加累计的场景下两者差异非常小对控制精度没有实际影响但如果哪天你要把定点结果直接当成整数除法来用就要留意这个区别。3.4 优化四ADC结果读取方式与DMA配合原始代码是在ADC中断里按通道读取转换结果每次读取动作都要走一遍外设总线读取时如果用的还是库函数又多一层封装调用。这次我把ADC的规则组配置成多通道连续转换由定时器事件触发转换结果通过DMA自动搬运到内存数组里。DMA搬完之后触发一次完成中断中断服务函数里不再访问ADC外设而是直接从SRAM数组里取数。这样做的好处是采样转换和CPU运算在时间上重叠了。ADC还在转换下一组数据的时候中断里已经在算坐标变换整个流水线更平顺。有一个配置上的细节容易疏忽uint16_t类型的ADC缓冲区数组最好加上4字节对齐属性否则DMA传输和CPU访问之间可能产生总线冲突导致读取效率下降。我工程里的写法是static uint16_t adc_buf[3] __attribute__((aligned(4)));如果MCU的DMA通道特别紧张DMA搬不了这么频繁也可以在ADC中断里直接读寄存器替代调用库函数但效果会打折扣。DMA方案不是说非得用具体要看外设资源够不够。3.5 实测对比优化前后周期数变化所有优化完成后我用DWT周期计数器重新量了一遍中断耗时。DWT-CYCCNT是Cortex-M3/M4内核自带的周期计数器比猜时间靠谱得多。实测结果如下环节优化前优化后ADC结果读取约0.7微秒约0.15微秒均值滤波约0.5微秒约0.1微秒偏置和增益标定约0.6微秒约0.3微秒Clarke变换约0.3微秒约0.25微秒Park变换约1.9微秒约0.5微秒其他开销约0.1微秒约0.1微秒总耗时约4.1微秒约1.4微秒单个周期从4.1微秒降到1.4微秒减少了大约66%。20kHz中断频率下CPU占用率从8.2%降到了2.8%省出的5.4%余量非常可观。我验证过精度额定电流20A、反馈电流满量程50A的情况下稳态误差在0.3%FS以内低速6转每分钟带载时电流波形也没有明显零漂完全符合优化前定的约束。4. 优化过程中踩过的坑和排查方法4.1 低速轻载时电流反馈噪声变大怎么排查第一次把代码改成定点后我遇到最头疼的问题就是低速轻载时电流反馈噪声变大波形上能看到明显的锯齿状抖动。排查下来原因有两层第一层是Q15格式在小电流状态下分辨率不够比如额定电流50A换算到Q15区间每一步代表大概0.0015A听起来很小但低速轻载时电流只有0.5A左右量化台阶造成的影响就会被凸显出来第二层是零漂校准不彻底ADC本身的偏置在定点缩放后被放大了。解决办法是在偏移校准环节做更细的处理。我不再只减一个固定偏置值而是用软件在校验模式下自动采集多次零电流值求平均把这个平均值作为偏移量并把偏移量也用Q15格式保存。另外如果项目最终运行状态长期处于低速轻载我会建议在电流环里做成双精度切换小电流时用Q31格式大电流时用Q15格式这样既保证精度又不牺牲速度。不过这个切换逻辑有滞后切换点需要加滞回否则会在临界区来回跳。4.2 采样毛刺和尖峰干扰的几个隐蔽来源优化完代码不久我发现某次台架测试时电流波形在每一个PWM周期的边沿附近都有一个尖刺当时第一反应是代码问题查了半天发现是软硬件结合的魔鬼。第一个来源是死区。死区时间内上下桥臂同时关断电流会通过体二极管续流此时低端采样电阻上的电压和真实相电流不一样如果ADC采样点离死区太近采进去的值就是畸变值。我用的PWM死区是2.5微秒触发点必须避开这个区间。第二个来源是开关振铃。MOSFET开关瞬间栅极驱动回路和功率回路的寄生电感电容会产生高频振铃振铃频率往往在几十MHz到上百MHz幅度虽然不大但直接叠加在采样电阻信号上。如果硬件上没做RC滤波软件只能靠采样窗口错开振铃衰减期或者增加数字滤波。第三个来源更有意思我用的是多通道连续转换加DMA如果一个通道采样期间另一个通道的开关噪声串扰进来结果也会不准。排查方法是用示波器同时测采样电阻电压和ADC触发信号确认采样点位置是不是在开关边沿附近。后来我把ADC触发时刻从PWM计数的边沿附近调整到了中心位置尖刺问题基本消失。4.3 编译器优化选项与代码正确性定点化之后我还碰过一个看起来特别冤的问题开启-O2优化等级以后代码反而不按预期工作了。仔细定位发现问题出在一个共享标志变量上。我在中断里把g_sample_done置1主循环里用这个变量做等待但定义的时候没有加volatile编译器在-O2下直接把主循环里的第二次读取优化成了寄存器里旧值导致主循环永远等不到标志位更新。这类问题在中断编程里太典型了。所有跨中断和主循环共享的变量一律加volatile关键字不要存侥幸心理。另外写定点代码时也要警惕编译器对乘加运算顺序的重排。默认情况下GCC会保留计算顺序但如果你开启了严格浮点重排或者使用了某些激进优化选项可能导致中间结果精度变化。我的建议是FOC控制这类实时算法编译选项用-O2就好不要为了极限性能去开-ffast-math这个选项会放宽IEEE浮点行为一旦出现NaN电流环PI输出会直接失控。4.4 中断时长测量和CPU占用率的估算方法最后说一下大家平时最常用的测量方法。我测试中断耗时用的是GPIO翻转加示波器在ISR入口把某个IO拉高ISR退出之前拉低示波器上测高电平时间就是中断处理时间。这个方法直观但注意不要被更高优先级的中断打断否则测出来的时间偏长。要精确测周期数用DWT-CYCCNT更靠谱在中断入口读一次计数器退出前再读一次差值就是实际执行的周期数。测完之后可以对着一段时间内多个周期取平均值去掉毛刺。CPU占用率估算公式很简单单次中断耗时乘以中断频率。比如这次优化后1.4微秒20kHz频率CPU占用率是1.4微秒乘以20,000等于28毫秒每秒也就是2.8%。手机App里那些性能分析工具一般只适合用户态程序MCU上还是用这种最朴素的方法最可信。说实话这段电流采集代码优化完我最大的感觉不是CPU占用从8%降到2%这件事本身而是余量出来之后整个控制链路完全不一样了。之前在同样资源条件下PWM频率想从20kHz提到24kHz总是捉襟见肘现在不但提上去了还能在中断里塞一个转速观测器进去。最后再分享一个小习惯每次性能优化完成我都会把优化前后的周期数、改动点、实测波形截图记录在项目release notes里。下次有人问“这块还能再提频吗”的时候直接翻数据说话比任何解释都有用。