Модифицированный алгоритм Герцеля Опции

[Lost_Viking]

Взялся за реализацию. Прочитал про алгоритм тут: dsplib
Написал такую реализацию:

Код

#define Fd 9000 //частота дискретизации
#define f1 600  //частота тона
#define f_search 600 //искомая алгоритмом частота
#define N 70 //размер массива
int main()
{
unsigned char ADC[350]; //Массив, эмулирующий поток из АЦП
unsigned char buff[N]; //Буфер, в который пишутся данные из АЦП
float I[2];
float Q[2];
float a;
float b;
float c = 2* M_PI *f1/Fd; //Заранее рассчитаем то, что не будет меняться
float module; //здесь будет лежать модуль комплексного числа
I[0]=0; I[1]=0; //инициализируем массивы
Q[0]=0; Q[1]=0;
for (int i =0; i<350; i++)
{
    ADC[i] = (unsigned char) 100*sin(c*i)+100; //заполняем "поток" АЦП синусом с амплитудой от 0 до 100

}

a=cos(2*M_PI*f_search/N); //рассчитываем к-ты
b=sin(2*M_PI*f_search/N);

for (int n=0; n<70; n++) //начальное заполнение буфера
{
    I[0]=I[1]; //Готовим предыдущее I
    Q[0]=Q[1];
    buff[n]=ADC[n]; // Просто заполняем буфер данными из АЦП
    I[1]=a*(I[0]+buff[n])-b*Q[0]; //Все по формуле 16 из статьи
    Q[1]=a*Q[0]+b*(I[0]+buff[n]);

}
for (int n=70; n<350; n++) //Когда буфер заполнился, то запускаем цикл с выталкиванием
{
    I[0]=I[1];
    Q[0]=Q[1];
    I[1]=a*(I[0]+ADC[n]-buff[0])-b*Q[0]; //В качестве i(n) выступает ADC(n), где n=70. Следовательно buff(n-N)=buff(0)
    Q[1]=a*Q[0]+b*(I[0]+ADC[n]-buff[0]);
    for (int nn=0; nn<69; nn++) //перемещаем элементы массива на 1 влево
    {
        buff[nn]=buff[nn+1];
    }
    buff[69]=ADC[n]; //Заносим полученное значение АЦП на данном цикле работы алгоритма в последний эллемент буфера
    module = sqrt(I[1]*I[1]+Q[1]*Q[1]); //вычисляем модуль комплексного числа
}
//конец работы алгоритма
}

Для удобства чтения скрин из саблайма: screenshot

В итоге имею изменяющийся модуль, хотя, как я понимаю, его значение должно быть постоянным и максимальным про наличии искомой частоты:

Скрин вывода программы переменной module:screenshot

Что я делаю не так?

Сообщение отредактировал Lost_Viking - Jan 21 2016, 08:19