Estoy implementando el sistema de comunicación de salto de frecuencia BFSK en un procesador DSP. Algunos de los miembros del foro sugirieron usar el algoritmo de Goertzel para la demodulación del salto de frecuencia en frecuencias específicas. He intentado implementar el algoritmo de goertzel en C. el código es el siguiente:

float goertzel(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
    int     k,i;
    float   floatnumSamples;
    float   omega,sine,cosine,coeff,q0,q1,q2,result,real,imag;

    floatnumSamples = (float) numSamples;
    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
    omega = (2.0 * M_PI * k) / floatnumSamples;
    sine = sin(omega);
    cosine = cos(omega);
    coeff = 2.0 * cosine;
    q0=0;
    q1=0;
    q2=0;

    for(i=0; i<numSamples; i++)
    {
        q0 = coeff * q1 - q2 + data[i];
        q2 = q1;
        q1 = q0;
    }
    real = (q1 - q2 * cosine);
    imag = (q2 * sine);
    result = sqrtf(real*real + imag*imag);
    return result;
}

Cuando uso la función para calcular el resultado en frecuencias específicas para un conjunto de datos determinado, no obtengo los resultados correctos. Sin embargo, si uso el mismo conjunto de datos y calculo el resultado de goertzel utilizando la función goertzel () de MATLAB, obtengo los resultados a la perfección. Se implementó el algoritmo usando C, con la ayuda de algunos tutoriales en línea que encontré en Internet. Solo quiero verles a ustedes si la función está implementando el algoritmo de Goertzel correctamente.