/home/mrazik/Documents/web/old/hdo/goertzel.c

//
// C Interface: goertzel
//
// Description: 
//
//
// Author: Miroslav Mraz <Mrazik@volny.cz>, (C) 2009
//
// Copyright: See COPYING file that comes with this distribution
//
//
#include <stdio.h>
#include <math.h>
#include <utility/trace.h>

#include "goertzel.h"
/*----------------------------------------------------------------------------
 *  calc_coeffs
 *----------------------------------------------------------------------------
 */
// NOCOMPUTE definovano v Makefile
// #define NOCOMPUTE  // ! pokud se toto definuje, zmenší se délka kódu podstatně, počítá vždy ve float
aritfmt calc_coeff (float nfreq) {
#ifdef  NOCOMPUTE
  return 1706;    // napevno pro 216,6 Hz
#else //  NOCOMPUTE - je to tu pro ukázku, jak se to počítá
  aritfmt  reval;
  float coeff;
  coeff  = 2.0 * 3.141592654 * nfreq;
  coeff  = 2.0 * cos (coeff);
#ifndef  COMPUTE_FLOAT
  // aby se to dalo počítat v int, tak to tady zvětšíme
  coeff *= (float)(1 << ISHIFT);
#endif //  COMPUTE_FLOAT

  reval  = (aritfmt) coeff;
  // TRACE_INFO ("coeff = %d\r\n", reval);
  return reval;
#endif // NOCOMPUTE
}
/*----------------------------------------------------------------------------
 *  goertzel
 *----------------------------------------------------------------------------
 */
int goertzel (aritfmt coeff, short *samples, int n) {
  register aritfmt  rv,q0,q1=0,q2=0;    // prefix register je použit pro případnou optimalizaci
  register int      i;
  //aritfmt  min=0,max=0;               // hledáme přetečení
#ifdef  COMPUTE_FLOAT
  aritfmt  a,b,c;
#endif // COMPUTE_FLOAT

  // pro všechny vzorky
  for (i=0; i<n; i++) {
    q0   = coeff * q1;
    // pokud byl coeff zvětšen, je třeba to tu zase zmenšit. Bez FPU je to ale rychlejší než počítat ve float.
#ifndef COMPUTE_FLOAT
    q0 >>= ISHIFT;                        // zmenšení pro int
#endif // COMPUTE_FLOAT
    q0  += (aritfmt) samples[i] - q2;     // vlastní výpočet

    q2   = q1;                            // posuv o vzorek
    q1   = q0;                            // rekurze
  }
  // po n vzorcích vypočteme kvadrát výkonu signálu, počítá se to jen jednou za čas (120 ms)
#ifdef COMPUTE_FLOAT
  a  = (float) q1;
  b  = (float) q2;
  c  = (float) coeff;
  a /= (float)(1 << HSHIFT);              // zmenšíme, jinak jsou tam moc velká čísla a přeteče to
  b /= (float)(1 << HSHIFT);
  // výpočet kvadrátu výkonu signálu pro danou frekvenci
  rv = (a * a) + (b * b) - (c * a * b);
  TRACE_INFO ("Presna hodnota = %f\n", r);
  return (int) r;
#else
  // Předchozí jde udělat i v integer-32, ale není to tak přesné
  q1 >>= HSHIFT;
  q2 >>= HSHIFT;
  rv   = q1 * q2;
  // TRACE_INFO ("q1*q2=%d\n", r);
  rv  *= -coeff;
  rv >>= ISHIFT;              // tady nutno zmenšit tak, jak bylo zvětšeno v calc_coeff()
  rv  += q1 * q1 + q2 * q2;   // výkon by byl sqrt (rv), není nutné počítat, napětí stačí
  return (int) rv;
#endif // COMPUTE_FLOAT
}