龙贝格观测器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。
调试时记得把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块，做电机控制的老铁们懂的都懂。
