Реализация алгоритма Гёртцеля в C
Я внедряю систему BFSK со скачкообразной перестройкой частоты на процессоре DSP. Некоторые участники форума предложили использовать алгоритм Гёртцеля для демодуляции скачкообразной перестройки частоты на определенных частотах. Я попытался реализовать алгоритм Goertzel в C. код выглядит следующим образом:
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;
}
Когда я использую функцию для вычисления результата на определенных частотах для данного набора данных, я не получаю правильные результаты. Однако, если я использую тот же набор данных и вычисляю результат goertzel с помощью функции MATLAB goertzel (), то я получаю результаты идеально. Я реализовал алгоритм, используя C, с помощью некоторых онлайн-учебников, которые я нашел через Интернет. Я просто хочу узнать мнение вас, ребята, правильно ли функция реализует алгоритм Гёртцеля.