Implementation of Goertzel algorithm in C

I am implementing BFSK frequency hopping communication system on a DSP processor. It was suggested by some of the forum members to use Goertzel algorithm for the demodulation of frequency hopping at specific frequencies. I have tried implementing the goertzel algorithm in C. the code is follows:

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;
}

When I use the function to calculate the result at specific frequencies for a given dataset, I am not getting the correct results. However, if I use the same dataset and calculate the goertzel result using MATLAB goertzel() function, then I get the results perfectly. I am implemented the algorithm using C, with the help of some online tutorials that I found over the internet. I just want to get the view of you guys if the function is implementing the goertzel algorithm correctly.

If you are saying that the Matlab implementation is good because its results match the result for that frequency of a DFT or FFT of your data, then it's probably because the Matlab implementation is normalizing the results by a scaling factor as is done with the FFT.

Change your code to take this into account and see if it improves your results. Note that I also changed the function and result names to reflect that your goertzel is calculating the magnitude, not the complete complex result, for clarity:

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

    float   scalingFactor = numSamples / 2.0;

    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;
    }

    // calculate the real and imaginary results
    // scaling appropriately
    real = (q1 - q2 * cosine) / scalingFactor;
    imag = (q2 * sine) / scalingFactor;

    magnitude = sqrtf(real*real + imag*imag);
    return magnitude;
}