龙贝格观测器Pll的C代码(iqmath数学运算加速 定点计算已做好了接口 调用调试即可 对应开发板可出出售 演示是stm32f1平台龙贝格观测器PLL的C代码最近整利索了直接上干货。这套算法用iqmath数学库做了定点数加速实测在STM32F103这类M3核的芯片上跑得飞起。先扔个初始化函数镇楼#define PLL_KP _IQ(0.6) //这参数别瞎改调废了别找我 #define PLL_KI _IQ(0.02) #define PLL_FREQ _IQ(50.0) void PLL_Init(PLL_Obj *obj){ obj-theta _IQ(0.0); obj-cosVal _IQ(1.0); obj-sinVal _IQ(0.0); obj-err _IQ(0.0); obj-intgrl _IQ(0.0); }注意IQ这个宏直接把浮点数转成Q格式定点数。STM32F1没硬件浮点单元用Q15格式省资源得很。比如IQ(0.6)实际是0.6乘以32768再取整占2字节但能表示±1之间的数。核心算法在中断服务例程里跑5个乘法搞定一次迭代void PLL_ISR_Handler(PLL_Obj *obj){ _iq adc_sample ADC_GetValue() 4; //12位AD取高8位转Q7格式 _iq err _IQmpy(adc_sample, obj-sinVal); //PI环节 obj-intgrl _IQmpy(PLL_KI, err); _iq delta _IQmpy(PLL_KP, err) obj-intgrl; //更新角度 obj-theta delta PLL_FREQ; obj-theta _IQfrac(obj-theta); //取小数部分相当于模运算 //更新正余弦 obj-cosVal _IQcos(obj-theta); obj-sinVal _IQsin(obj-theta); }这里有个骚操作——直接把PI调节器输出叠加到额定频率上相当于自动补偿频偏。IQfrac这个函数处理角度溢出比用if判断快三倍实测在72MHz主频下整个PLL运算耗时20μs。龙贝格观测器Pll的C代码(iqmath数学运算加速 定点计算已做好了接口 调用调试即可 对应开发板可出出售 演示是stm32f1平台调试时记得把iqmath库的全局Q格式设为Q15在头文件里改这句#define GLOBAL_Q 15 //Q15精度足够改大了溢出别哭配套开发板留了六路信号检测口板上带硬件滤波电路。要测试的话用信号发生器给个50Hz正弦波接上板子跑起来LED灯变绿就是锁定了。代码仓库里有个骚气的调试模式把下面这行取消注释就能看实时波形//#define DEBUG_PLL //取消注释后通过USART1输出内部变量实测波形捕获效果比某国外芯片方案还稳关键是能省下8块钱成本。有兄弟问能不能跑在F4上当然可以把IQmath换成ARM的DSP库直接起飞不过杀鸡用牛刀了属于是。最后说下硬件兼容性这套代码对AD采样率要求不高1kHz以上就能用。GPIO口配置成TIM触发模式和PWM同步触发采样抗干扰能力直接提升一个档次。有需要开发板的私带光耦隔离的版本加30块做电机控制的老铁们懂的都懂。