PI-PLL锁相环+低通滤波器估算角速度 C语言

下面是一个简单的 C 语言代码示例，使用 PI-PLL 锁相环和低通滤波器估算角速度：

#include <stdio.h>
#include <math.h>

#define Ts 0.001     // 采样周期
#define Kp 0.5       // PI 控制器比例增益
#define Ki 0.02      // PI 控制器积分增益
#define Kf 0.01      // 低通滤波器增益
#define N 10         // 低通滤波器阶数
#define fc 50        // 低通滤波器截止频率
#define Vref 220     // 参考电压
#define Vout_max 500 // 输出电压最大值
#define Vout_min 0   // 输出电压最小值

float phase_err = 0;     // 相位误差
float freq_err = 0;      // 频率误差
float Vout = 0;          // 输出电压
float Vout_prev = 0;     // 上一次的输出电压
float freq_est = 50;     // 估算的角频率
float phase_est = 0;     // 估算的相位角
float freq_filtered = 0; // 经过低通滤波后的角频率

// PI 控制器
void pi_controller()
{
    static float integral = 0;
    float error = phase_err;
    integral += error * Ki * Ts;
    Vout = Vout_prev + (error * Kp + integral) * Ts;
    Vout = fmax(fmin(Vout, Vout_max), Vout_min); // 限制输出电压在最大值和最小值之间
    Vout_prev = Vout;
}

// 低通滤波器
void lowpass_filter()
{
    static float x[N + 1] = {0};
    static float y[N + 1] = {0};
    float w = 2 * M_PI * fc * Ts;
    float alpha = sin(w) / (2 * pow(cos(w / 2), N));
    x[0] = freq_est;
    y[0] = alpha * (x[0] + 2 * x[1] + x[2]) + 2 * (1 - alpha) * y[1] - pow(1 - alpha, 2) * y[2];
    freq_filtered = y[0];
    for (int i = N; i >= 1; i--)
    {
        x[i] = x[i - 1];
        y[i] = y[i - 1];
    }
}

int main()
{
    float t = 0;
    while (1)
    {
        // 更新相位误差和频率误差
        phase_err = atan2f(sin(phase_est - 2 * M_PI * freq_est * t), cos(phase_est - 2 * M_PI * freq_est * t));
        freq_err = freq_est - freq_filtered;
        // 使用 PI 控制器计算输出电压
        pi_controller();
        // 更新估算的相位角和角频率
        phase_est += 2 * M_PI * freq_est * Ts;
        freq_est += (Kf * freq_err - phase_err * Kp) * Ts;
        // 经过低通滤波器
        lowpass_filter();
        // 输出结果
        printf("t:%f\tfreq_est:%f\n", t, freq_est);
        // 更新时间
        t += Ts;
    }
    return 0;
}

注：以上代码仅供参考，实际应用中还需要根据具体情况进行调整和优化。