Взялся за реализацию. Прочитал про алгоритм тут:
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
Модифицированный алгоритм Герцеля
Jan 21 2016, 05:58
