Итак, товарищи, я задался идеей реализовать алгоритм Гёрцеля. С уклоном в эмбед, конечно. И потому с фиксированной точкой. На всякий случай - я в курсе, что есть готовые реализации, но мне интересно, почему не работает у меня.
Итак, для начала я реализовал этот алгоритм с плавающей точкой:
Код:
float Gz_GetFreqMagnitude(uint8_t block[BLOCK_SIZE],gz_tgt_freq_t *tgt_f)
{
float Q0,Q1=0,Q2=0;
uint8_t i;
for (i=0; i<BLOCK_SIZE; i++)
{
Q0=(tgt_f->cf)*Q1-Q2+(block[i]/255.0);
Q2=Q1;
Q1=Q0;
}
return sqrt(Q1*Q1+Q2*Q2-Q1*Q2*(tgt_f->cf));
}
tgt_f - структура, содержащая служебную информацию и в том числе необходимый поворотный множитель.
Код:
typedef struct {
float tgt_freq;
float k;
float w;
float cos_w;
float sin_w;
float cf;
} gz_tgt_freq_t;
Она вычисляется другой функцией:
Код:
void Gz_ComputeFreqDescriptor(gz_tgt_freq_t *dsc)
{
dsc->k=floor(0.5+((BLOCK_SIZE*(dsc->tgt_freq))/SAMPLING_RATE_HZ));
dsc->w=((2*M_PI)/BLOCK_SIZE)*(dsc->k);
dsc->cos_w=cos(dsc->w);
dsc->sin_w=sin(dsc->w);
dsc->cf=2*(dsc->cos_w);
}
Сначала заполняется поле tgt_freq, потом вызывается Gz_ComputeFreqDescriptor(), потом - Gz_GetFreqMagnitude() с вычисленными значениями в структуре.
Эта реализация с float работает. Чудеса начинаются при попытке перенести ее на фиксированную точку. Для этой благородной цели я написал файл, определяющий операции с фиксированной точкой:
Код:
typedef int32_t fixed;
#define FIX_BITS 10
#define fix_fill(wpart,fpart) (((wpart) << FIX_BITS) | (((fpart) * (1 << FIX_BITS)) / 1000))
#define fix_add(num1,num2) ((num1) + (num2))
#define fix_sub(num1,num2) ((num1) - (num2))
#define fix_mul(num1,num2) (((num1) * (num2)) >> FIX_BITS)
#define fix_div(num1,num2) (((num1) << FIX_BITS) / (num2))
#define fix_whole(num) (uint32_t)((num) >> FIX_BITS)
#define fix_frac(num) (uint32_t)((((num) & ((1 << FIX_BITS)-1)) * 1000) / (1 << FIX_BITS))
#define fix_sqr(num) (fix_mul((num),(num)))
И в отдельности он тоже работает!Но, когда я пытаюсь совместить эти два файла, начинаются чудеса. Совмещаю я так:
Код:
uint32_t GzInt_GetFreqMagnitude(uint8_t block[BLOCK_SIZE],fixed *tgt_f)
{
fixed Q0,Q1=0,Q2=0;
uint8_t i;
for (i=0; i<BLOCK_SIZE; i++)
{
//Q0=(tgt_f->cf)*Q1-Q2+(block[i]/255.0);
Q0=fix_add(fix_sub(fix_mul((*tgt_f),Q1),Q2),fix_fill(block[i],0));
Q2=Q1;
Q1=Q0;
}
//return sqrt(Q1*Q1+Q2*Q2-Q1*Q2*(tgt_f->cf));
return fix_whole(fix_sub(fix_add(fix_sqr(Q1),fix_sqr(Q2)),fix_mul(fix_mul(Q1,Q2),(*tgt_f))));
}
Коэффициент пересчитывается так:
Код:
fixed Gz_ConvertCoefficient(gz_tgt_freq_t *float_coeff)
{
//printf("Freq: %.2f,coeff: %.5f\n",float_coeff->tgt_freq,float_coeff->cf);
double f_part,i_part;
fixed fixed_coeff;
f_part=modf(float_coeff->cf,&i_part);
fixed_coeff=fix_fill((int32_t)i_part,(int32_t)(f_part*1000));
//printf("IF: %i;%i\n",(int32_t)i_part,(int32_t)(f_part*1000));
//printf("IG: %i,%i\n",fix_whole(fixed_coeff),fix_frac(fixed_coeff));
return fixed_coeff;
}
Виден отладочный вывод - я проверял, все переводится 100% правильно. Но сам алгоритм рассыпается. Версия с плавающей точкой при тестах дает четкий максимум на корректной частоте. С фиксированной - какой-то левый случайный набор значений.
Отчего так? Неужели алгоритму Гёрцеля нужна такая точность, что он рассыпается от ограниченной точности фиксированной точки?
Я уже всю голову сломал, ничего не помогает. И главное - по отдельности все работает!
Вход
Регистрация
Правила
FAQ
Поиск
