changed sign

[mirrors/Programs.git] / c / goertzel / goertzel.c

diff --git a/c/goertzel/goertzel.c b/c/goertzel/goertzel.c
index a6070ca312c2369987b5eb4f008682f6075f8fa1..d13a874d9612379d94ca4e07c7de72da16ea7294 100644 (file)
--- a/c/goertzel/goertzel.c
+++ b/c/goertzel/goertzel.c
@@ -2,7 +2,7 @@
 #include <math.h>
 #include <getopt.h>
 
-float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
+float goertzel_mag(int numSamples,float TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
 {
     int     k,i;
     float   floatnumSamples;
@@ -11,7 +11,7 @@ float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float*
     float   scalingFactor = numSamples / 2.0;
 
     floatnumSamples = (float) numSamples;
-    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
+    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / (float)SAMPLING_RATE));
     omega = (2.0 * M_PI * k) / floatnumSamples;
     sine = sin(omega);
     cosine = cos(omega);
@@ -29,8 +29,8 @@ float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float*
 
     // calculate the real and imaginary results
     // scaling appropriately
-    real = (q1 - q2 * cosine) / scalingFactor;
-    imag = (q2 * sine) / scalingFactor;
+    real = (q1 * cosine - q2) / scalingFactor;
+    imag = (q1 * sine) / scalingFactor;
 
     magnitude = sqrtf(real*real + imag*imag);
     return magnitude;
@@ -65,20 +65,23 @@ void print_help(char ** argv) {
                "\n"
                "\t-r <samplerate>\tInput samplerate (deault 8000 Hz)\n"
                "\t-c <count>\tFrame size in samples (default 4000 Samples)\n"
-               "\t-d <ratio>\tFrame size (default 2) (samplerate will be divided by this number to get frame size same as -c)\n"
+               "\t-d <divisor>\tFrame size ( count = samplerate/divisor ) (default 2)\n"
                "\n"
-               "\t-f <freq>\tAdd frequency in Hz to detect (use multiple times, if no added 440 Hz will be...)\n"
+               "\t-f <freq>\tAdd frequency in Hz to detect (use multiple times, default 440 Hz)\n"
+               "\n"
+               "\t-n <format>\tSet number output format\n"
+               "\t\tf: float\t23.4223 (default)\n"
+               "\t\ti: integer\t23\n"
+               "\t\tb: binary\t(0|1)\n"
+               "\t\tB: Boolean\t(false|true)\n"
+               "\n"
+               "\t-t <treshold>\tSet treshold (used in filter, see -l) (defaults -1)\n"
+               "\t-l <filter>\tSet line filter\n"
+               "\t\tf: Falldown:\tprint only when over treshold or just crossed (default)\n"
+               "\t\tt: Treshold:\tprint only when over treshold\n"
+               "\t\tc: Crossed:\tprint only when treshold crossed\n"
+               "\t-u\t\tInvert\ttreshold (values under treshold will be displayed)\n"
                "\n"
-               "\t-t <treshold>\tSet treshold (used to hide magnitudes lower than treshold) (defaults -1)\n"
-               "\t-n <format>\tSet output format\n"
-               "\t\tf: float (default)\n"
-               "\t\ti: integer\n"
-               "\t\tb: binary (0|1)\n"
-               "\t\tB: Boolean (false|true)\n"
-               "\t-l <filter>\tSet filter\n"
-               "\t\tf: Falldown: print only when over treshold or just falled under (default)\n"
-               "\t\tt: Treshold: print only when over treshold\n"
-               "\t\tc: Crossed: print only when treshold crossed\n"
                "\t-q\t\tQuiet mode: print only values\n"
                "\n"
                "\t-?\t\tPrint help\n"
@@ -100,7 +103,7 @@ void print_help(char ** argv) {
        );
 }
 
-void addfreq(int *freqs, int freq) {
+void addfreq(float *freqs, float freq) {
        int i = 0;
        while(freqs[i]!=-1) i++;
        freqs[i]=freq;
@@ -110,14 +113,20 @@ void addfreq(int *freqs, int freq) {
 int main(int argc, char ** argv) {
        int samplerate = 8000;
        int samplecount = 4000;
+
        int treshold = -1;
        char filter = 0;
+       char under = 0;
+
        char format=0;
        char verbose=1;
-       int freqs[argc+1]; freqs[0]=-1;
 
+       float freqs[argc+1]; freqs[0]=-1;
+
+
+       float floatarg;
        int opt;
-       while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:q")) != -1) {
+       while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:uq")) != -1) {
                switch (opt) {
                        case 'i':
                                freopen(optarg, "r", stdin);
@@ -138,7 +147,8 @@ int main(int argc, char ** argv) {
                                samplecount = samplerate/atoi(optarg);
                                break;
                        case 'f':
-                               addfreq(freqs, atoi(optarg));
+                               sscanf(optarg,"%f",&floatarg);
+                               addfreq(freqs, floatarg);
                                break;
                        case 't':
                                treshold = atoi(optarg);
@@ -149,6 +159,9 @@ int main(int argc, char ** argv) {
                        case 'l':
                                filter = optarg[0];
                                break;
+                       case 'u':
+                               under = 1;
+                               break;
                        case 'q':
                                verbose = 0;
                                break;
@@ -165,7 +178,7 @@ int main(int argc, char ** argv) {
 
        if(verbose) {
                fprintf(stderr,
-                       "#Detected tone: %d Hz\n"
+                       "#Detected tone: %.2f Hz\n"
                        "#Samplerate: %d Hz\n"
                        "#Frame lenght: %d samples\n"
                        "#Treshold: %d\n"
@@ -175,7 +188,7 @@ int main(int argc, char ** argv) {
 
                printf("#Position");
                int i; for(i=0;freqs[i]!=-1;i++) {
-                       printf("\t%2dHz",freqs[i]);
+                       printf("\t%2.0fHz",freqs[i]); //TODO: print decimal places
                }
                puts("");
        }
@@ -200,7 +213,7 @@ int main(int argc, char ** argv) {
                        power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
 
                        //Decide if we will print
-                       printnow = power[i] > treshold; //Is over treshold?
+                       printnow = under ? power[i] < treshold : power[i] > treshold; //Is over/under treshold?
                        switch(filter) {
                                case 'c': //Print if treshold crossed
                                        print = print || (laststate[i] != printnow);
@@ -222,7 +235,7 @@ int main(int argc, char ** argv) {
                                printf("\t");
                                switch(format) {
                                        case 'i':
-                                               printf("%d",(int)power[i]);
+                                               printf("%d",(int)round(power[i]));
                                                break;
                                        case 'b':
                                                printf("%d",power[i]>treshold);
@@ -233,7 +246,7 @@ int main(int argc, char ** argv) {
                                                break;
                                        case 'f':
                                        default:
-                                               printf("%.4f",power[i]);
+                                               printf("%7.5f",power[i]);
                                }
                        }
                        puts("");