锁相环代码-CSDN博客

本文链接：https://blog.csdn.net/2402_88029467/article/details/146543423

// 设置锁相环参数
const float REF_FREQ = 1e6; // 参考信号频率
const float VCO_FREQ_MIN = 1e6; // VCO 最小频率
const float VCO_FREQ_MAX = 10e6; // VCO 最大频率
const float Kp = 0.1; // 比例增益
const float Ki = 0.01; // 积分增益

// 初始化变量
float ref_phase = 0; // 参考信号相位
float vco_freq = VCO_FREQ_MIN; // VCO 频率
float vco_phase = 0; // VCO 相位
float phase_[error](https://wenku.csdn.net/doc/6401acf5cce7214c316edc34?spm=1055.2569.3001.10083&amp;kwd=error) = 0; // 相位误差
float freq_error = 0; // 频率误差
float integ_error = 0; // 积分误差

// 主循环
[while](https://wenku.csdn.net/doc/645cade695996c03ac3eb2ce?spm=1055.2569.3001.10083&amp;kwd=while) (1) {
    // 读取输入信号相位
    float [input](https://wenku.csdn.net/doc/6d2t36u9pa?spm=1055.2569.3001.10083&amp;kwd=input)_phase = read_input_phase();

    // 计算相位误差
    phase_error = input_phase - ref_phase;

    // 如果相位误差大于 pi，需要将误差限制在 [-pi, pi] 范围内
    if (phase_error > M_PI) {
        phase_error -= 2 * M_PI;
    } else if (phase_error < -M_PI) {
        phase_error += 2 * M_PI;
    }

    // 计算频率误差
    freq_error = phase_error / (2 * M_PI);

    // 积分误差
    integ_error += freq_error;

    // 计算控制电压
    float [control](https://wenku.csdn.net/doc/4zxi954drc?spm=1055.2569.3001.10083&amp;kwd=control)_voltage = Kp * phase_error + Ki * integ_error;

    // 更新 VCO 频率
    vco_freq += control_voltage;

    // 限制 VCO 频率在最大最小值范围内
    if (vco_freq > VCO_FREQ_MAX) {
        vco_freq = VCO_FREQ_MAX;
    } else if (vco_freq < VCO_FREQ_MIN) {
        vco_freq = VCO_FREQ_MIN;
    }

    // 计算 VCO 相位
    vco_phase += 2 * M_PI * vco_freq / REF_FREQ;

    // 将 VCO 相位限制在 [0, 2*pi] 范围内
    if (vco_phase > 2 * M_PI) {
        vco_phase -= 2 * M_PI;
    } else if (vco_phase < 0) {
        vco_phase += 2 * M_PI;
    }

    // 输出 VCO 相位
    output_vco_phase(vco_phase);

    // 更新参考信号相位
    ref_phase += 2 * M_PI * REF_FREQ / SAMPLE_RATE;
}