Сам фильтр прекрано функционирует (проверено в сверке с double-версией: разнится в 1 бит), но с фильтрующей конструкцией видимо что-то не так. Возможно кто-то из опытных увидит где принципиальная ошибка и подскажет.
Исходная частота дискретизации 4096Гц, частота на выходе Фарроу 1706,56 Гц.
частота основной гармоники сигнала 53.33Гц
Многие данные умножены на константу (специфика работы в fixed point)
Код: Выделить всё
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define K 32
short FIX_MPY(short a, short b)
{
/* shift right one less bit (i.e. 15-1) */
int c = ((int)a * (int)b) >> 14;
/* last bit shifted out = rounding-bit */
b = c & 0x01;
/* last shift + rounding bit */
a = (c >> 1) + b;
return a;
}
unsigned short my_sqrt( unsigned int arg )
{
unsigned short res, i, step;
static unsigned short aNew[1]; // to use pointer
unsigned short * pNew = aNew; // pointer is used to allow 1 cycle mpy
unsigned int sq;
res = 0;
step = 0x8000;
*pNew = res + step;
for( i = 0; i < 16; i++ )
{
sq = (( unsigned int)(*pNew))*(( unsigned int)(*pNew));
if ( sq <= arg )
res += step;
*pNew = res + (step>>1);
step >>= 1;
}
return res;
}
// Функция сохранения результатов анализа в текстовый файл для
//построения графиков.
void saveToTXT(short* s, int n, char* fileName){
int i;
FILE *file = fopen(fileName, "w");
if(!file) return;
for( i = 0; i < n; i++)
fprintf(file,"%d\n",s[i]);
fclose(file);
}
// Функция расчета кубического полинома
// на основе модифицированного фильтра Фарроу
short farrow3(short *y, short x){
short tmp;
short a3 = ((y[3]-y[0])/6)+((y[1]-y[2])/2);
short a1 = ((y[3]-y[1])/2)-a3;
short a2 = y[3]-a3-a1-y[2];
tmp = FIX_MPY(x, a3)+a2;
tmp = FIX_MPY(x, tmp)+a1;
tmp = FIX_MPY(x, tmp)+y[2];
return tmp;
}
int main()
{
int i, j;
unsigned int x;
short f = 5333; //априорная частота основной гармоники
const unsigned int F0 = 409600; //частота дискретизации исходного сигнала
unsigned int F1 = f*K; //частота дискретизации выходного сигнала
unsigned int N = (K*F0)/F1; // кол-во исходных отсчётов
//исходный сигнал с частотой основной гармоники f и содержащий 3-ю гармонику с частотой 3*f
short y0[76] =
{
0,5327,10448,15171,19324,22767,25401,27170,28068,28133,27449,26139,24355,
22272,20075,17945,16055,14550,13546,13120,13302,14081,15395,17147,19200,
21391,23543,25468,26989,27941,28190,27635,26218,23928,20801,16919,12407,
7424,2155,-3197,-8425,-13330,-17731,-21474,-24443,-26564,-27810,-28202,
-27807,-26732,-25119,-23135,-20961,-18783,-16777,-15101,-13884,-13219,
-13155,-13699,-14807,-16398,-18347,-20504,-22694,-24732,-26437,-27636,
-28182,-27960,-26894,-24953,-22156,-18565,-14289,-9474
};
// выделяю память под результат ресэмплинга
short* y1 = (short*) malloc(K*sizeof(short));
short* t1 = (short*) malloc(K*sizeof(short));
unsigned int t0 = 32768/4096; //шаг дискретизации входного сигнала
unsigned int t1 = 32768/(F1/100); //шаг дискретизации выходного сигнала
//фильтрующая конструкция
// произвожу ресэмплинг
for(i = 0; i<K; i++){
j = ((int)i*F0/F1)-1; // пересчитываю индекс исходного сигнала
if(j<0) j =0; // если индекс отрицателььный то приравниваю его к 0
x = (T1*i - T0*j)/1000;
y1[i] = farrow3(y0+j, x); // фильтр Фарроу
}
//сохраняю результат в файлы
saveToTXT(y0, N, "исходный сигнал.txt");
saveToTXT(y1, K, "resampling.txt");
getchar();
return 0;
}
j пересчитывается с точность как в double-исходнике, помогите правильно x посчитать. Никак не удавалось получить идентичный с double-версией результат для x. Сделал как есть - на выходе что то вразумительное, - но видно, что несовсем((