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

#include <stdio.h>
#include <math.h>
#include <getopt.h>

float goertzel_mag(int numSamples,float TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
    int     k,i;
    float   floatnumSamples;
    float   omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;

    float   scalingFactor = numSamples / 2.0;

    floatnumSamples = (float) numSamples;
    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / (float)SAMPLING_RATE));
    omega = (2.0 * M_PI * k) / floatnumSamples;
    sine = sin(omega);
    cosine = cos(omega);
    coeff = 2.0 * cosine;
    q0=0;
    q1=0;
    q2=0;

    for(i=0; i<numSamples; i++)
    {
        q0 = coeff * q1 - q2 + data[i];
        q2 = q1;
        q1 = q0;
    }

    // calculate the real and imaginary results
    // scaling appropriately
    real = (q1 * cosine - q2) / scalingFactor;
    imag = (q1 * sine) / scalingFactor;

    magnitude = sqrtf(real*real + imag*imag);
    //phase = atan(imag/real)
    return magnitude;
}

void print_help(char ** argv) {
	printf(
		"%s takes raw (wav) audio stream and computes power (or magnitude)\n"
		"of desired frequencies using Goertzel algorithm for time frames\n"
		"of fixed length (specified in samples or relative to sample rate).\n"
		"This can be used in various frequency detection applications\n"
		"like guitar tuning, DTMF decoding and many others...\n"
		"\n"
		"http://en.wikipedia.org/wiki/Goertzel_algorithm\n"
		"\n"
		"Curently only raw unsigned 8bit (u8) mono audio is supported, but\n"
		"samplerate may vary. You can convert other formats before processing.\n"
		"\n"
		"On lower samplerates and frame sizes this may perform sub-optimally. Eg.:\n"
		"When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples)\n"
		"it actually detects something around 438,3Hz rather than 400Hz...\n"
		"If you can't increase samplerate way around this is just to increase sensitivity.\n"
		"\n"
		,argv[0]
	);

	printf(
		"Arguments:\n"
		"\t-i <file>\tInput from file (default STDIN)\n"
		"\t-o <file>\tOutput to file (default STDOUT)\n"
		"\t-a <file>\tOutput to file (append) (default STDOUT)\n"
		"\n"
		"\t-r <samplerate>\tInput samplerate (deault 8000 Hz)\n"
		"\t-c <count>\tFrame size in samples (default 4000 Samples)\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, 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-q\t\tQuiet mode: print only values\n"
		"\n"
		"\t-?\t\tPrint help\n"
		"\n"
	);

	printf(
		"Usage examples:\n"
		"\tarecord | %s\n"
		"\tsox input.mp3 -b 8 -c 1 -r 8000 -t wav - | %s\n"
		"\t%s -n -q -l -r 8000 -d 20 -t $tresh -f 697 [-f 770 ...]\n"
		"\n"
		,argv[0],argv[0],argv[0]
	);

	printf(
		"Frequencies for DTMF decoding:\n"
		"\t-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10\n"
	);
}

void addfreq(float *freqs, float freq) {
	int i = 0;
	while(freqs[i]!=-1) i++;
	freqs[i]=freq;
	freqs[i+1]=-1;
}

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;

	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:uq")) != -1) {
		switch (opt) {
			case 'i':
				freopen(optarg, "r", stdin);
				break;
			case 'o':
				freopen(optarg, "w", stdout);
				break;
			case 'a':
				freopen(optarg, "a", stdout);
				break;
			case 'r':
				samplerate = atoi(optarg);
				break;
			case 'c':
				samplecount = atoi(optarg);
				break;
			case 'd':
				samplecount = samplerate/atoi(optarg);
				break;
			case 'f':
				sscanf(optarg,"%f",&floatarg);
				addfreq(freqs, floatarg);
				break;
			case 't':
				treshold = atoi(optarg);
				break;
			case 'n':
				format = optarg[0];
				break;
			case 'l':
				filter = optarg[0];
				break;
			case 'u':
				under = 1;
				break;
			case 'q':
				verbose = 0;
				break;
			case '?':
				print_help(argv);
				return 0;
				break;
		}
	}

	if(freqs[0]==-1) addfreq(freqs, 440);
	float samples[samplecount];
	float position = 0;

	if(verbose) {
		fprintf(stderr,
			"#Detected tone: %.2f Hz\n"
			"#Samplerate: %d Hz\n"
			"#Frame lenght: %d samples\n"
			"#Treshold: %d\n"
			"#\n"
			,freqs[0],samplerate,samplecount,treshold);
		fflush(stderr);

		printf("#Position");
		int i; for(i=0;freqs[i]!=-1;i++) {
			printf("\t%2.0fHz",freqs[i]); //TODO: print decimal places
		}
		puts("");
	}

	int i;
	char print=0, printnow=0;
	char laststate[argc]; for(i=0;freqs[i]!=-1;i++) laststate[i]=-1;
	while(!feof(stdin)) {

		//Sample data
		for(i=0;i<samplecount && !feof(stdin);i++) {
			unsigned char sample;
			fread(&sample,1,1,stdin);
			samples[i]=sample;
			//printf("%d\n", sample);
		}

		//Apply goertzel
		float power[argc];
		print=0;
		for(i=0;freqs[i]!=-1;i++) {
			power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);

			//Decide if we will print
			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);
					break;
				default:
				case 'f': //Print if over treshold or falled down
					print = print || (laststate[i] != printnow);
				case 't': //Print if over treshold
					print = print || printnow;
			}
			laststate[i] = printnow; //Store last state
		}
		fflush(stdout);

		//Print data
		if(print) {
			printf("%8.2f", position);
			for(i=0;freqs[i]!=-1;i++) {
				printf("\t");
				switch(format) {
					case 'i':
						printf("%d",(int)round(power[i]));
						break;
					case 'b':
						printf("%d",power[i]>treshold);
						break;
					case 'B':
						if(power[i]>treshold) printf("true");
							else printf("false");
						break;
					case 'f':
					default:
						printf("%7.5f",power[i]);
				}
			}
			puts("");
			fflush(stdout);
		}

		//Increase time
		position += ((float)samplecount/(float)samplerate);
	}
}
Commit	Line	Data
4b43521a H	1	#include <stdio.h>
4b43521a H	2	#include <math.h>
59934436 H	3	#include <getopt.h>
59934436 H	4
9bd0f299	5	float goertzel_mag(int numSamples,float TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
4b43521a	6	{
4b43521a H	7	int k,i;
	8	float floatnumSamples;
	9	float omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
	10
	11	float scalingFactor = numSamples / 2.0;
	12
	13	floatnumSamples = (float) numSamples;
9bd0f299	14	k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / (float)SAMPLING_RATE));
4b43521a H	15	omega = (2.0 * M_PI * k) / floatnumSamples;
	16	sine = sin(omega);
	17	cosine = cos(omega);
	18	coeff = 2.0 * cosine;
	19	q0=0;
	20	q1=0;
	21	q2=0;
	22
	23	for(i=0; i<numSamples; i++)
	24	{
	25	q0 = coeff * q1 - q2 + data[i];
	26	q2 = q1;
	27	q1 = q0;
	28	}
	29
	30	// calculate the real and imaginary results
	31	// scaling appropriately
227018fa	32	real = (q1 * cosine - q2) / scalingFactor;
599af17a	33	imag = (q1 * sine) / scalingFactor;
4b43521a H	34
4b43521a H	35	magnitude = sqrtf(realreal + imagimag);
21e17c18	36	//phase = atan(imag/real)
4b43521a H	37	return magnitude;
	38	}
	39
59934436	40	void print_help(char ** argv) {
8f751717 H	41	printf(
	42	"%s takes raw (wav) audio stream and computes power (or magnitude)\n"
	43	"of desired frequencies using Goertzel algorithm for time frames\n"
	44	"of fixed length (specified in samples or relative to sample rate).\n"
	45	"This can be used in various frequency detection applications\n"
	46	"like guitar tuning, DTMF decoding and many others...\n"
	47	"\n"
	48	"http://en.wikipedia.org/wiki/Goertzel_algorithm\n"
	49	"\n"
	50	"Curently only raw unsigned 8bit (u8) mono audio is supported, but\n"
	51	"samplerate may vary. You can convert other formats before processing.\n"
	52	"\n"
	53	"On lower samplerates and frame sizes this may perform sub-optimally. Eg.:\n"
	54	"When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples)\n"
	55	"it actually detects something around 438,3Hz rather than 400Hz...\n"
	56	"If you can't increase samplerate way around this is just to increase sensitivity.\n"
	57	"\n"
	58	,argv[0]
	59	);
84329d34 H	60
	61	printf(
	62	"Arguments:\n"
84329d34 H	63	"\t-i <file>\tInput from file (default STDIN)\n"
	64	"\t-o <file>\tOutput to file (default STDOUT)\n"
	65	"\t-a <file>\tOutput to file (append) (default STDOUT)\n"
	66	"\n"
	67	"\t-r <samplerate>\tInput samplerate (deault 8000 Hz)\n"
	68	"\t-c <count>\tFrame size in samples (default 4000 Samples)\n"
7d11ab97	69	"\t-d <divisor>\tFrame size ( count = samplerate/divisor ) (default 2)\n"
84329d34	70	"\n"
7d11ab97 TM	71	"\t-f <freq>\tAdd frequency in Hz to detect (use multiple times, default 440 Hz)\n"
	72	"\n"
	73	"\t-n <format>\tSet number output format\n"
	74	"\t\tf: float\t23.4223 (default)\n"
	75	"\t\ti: integer\t23\n"
	76	"\t\tb: binary\t(0\|1)\n"
	77	"\t\tB: Boolean\t(false\|true)\n"
	78	"\n"
	79	"\t-t <treshold>\tSet treshold (used in filter, see -l) (defaults -1)\n"
	80	"\t-l <filter>\tSet line filter\n"
	81	"\t\tf: Falldown:\tprint only when over treshold or just crossed (default)\n"
	82	"\t\tt: Treshold:\tprint only when over treshold\n"
	83	"\t\tc: Crossed:\tprint only when treshold crossed\n"
	84	"\t-u\t\tInvert\ttreshold (values under treshold will be displayed)\n"
84329d34	85	"\n"
84329d34 H	86	"\t-q\t\tQuiet mode: print only values\n"
	87	"\n"
	88	"\t-?\t\tPrint help\n"
	89	"\n"
	90	);
	91
	92	printf(
	93	"Usage examples:\n"
	94	"\tarecord \| %s\n"
	95	"\tsox input.mp3 -b 8 -c 1 -r 8000 -t wav - \| %s\n"
	96	"\t%s -n -q -l -r 8000 -d 20 -t $tresh -f 697 [-f 770 ...]\n"
	97	"\n"
	98	,argv[0],argv[0],argv[0]
	99	);
	100
	101	printf(
	102	"Frequencies for DTMF decoding:\n"
	103	"\t-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10\n"
	104	);
59934436 H	105	}
59934436 H	106
9bd0f299	107	void addfreq(float *freqs, float freq) {
59934436 H	108	int i = 0;
	109	while(freqs[i]!=-1) i++;
	110	freqs[i]=freq;
	111	freqs[i+1]=-1;
	112	}
	113
	114	int main(int argc, char ** argv) {
4b43521a H	115	int samplerate = 8000;
4b43521a H	116	int samplecount = 4000;
7d11ab97	117
59934436	118	int treshold = -1;
19a509d1	119	char filter = 0;
7d11ab97 TM	120	char under = 0;
7d11ab97 TM	121
c9be90d5	122	char format=0;
59934436	123	char verbose=1;
7d11ab97	124
9bd0f299	125	float freqs[argc+1]; freqs[0]=-1;
59934436	126
9bd0f299 H	127
9bd0f299 H	128	float floatarg;
59934436	129	int opt;
7d11ab97	130	while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:uq")) != -1) {
59934436	131	switch (opt) {
c7611c89 H	132	case 'i':
	133	freopen(optarg, "r", stdin);
	134	break;
	135	case 'o':
	136	freopen(optarg, "w", stdout);
	137	break;
	138	case 'a':
	139	freopen(optarg, "a", stdout);
	140	break;
59934436 H	141	case 'r':
	142	samplerate = atoi(optarg);
	143	break;
c7611c89	144	case 'c':
59934436 H	145	samplecount = atoi(optarg);
59934436 H	146	break;
f5c8b03d H	147	case 'd':
	148	samplecount = samplerate/atoi(optarg);
	149	break;
59934436	150	case 'f':
9bd0f299 H	151	sscanf(optarg,"%f",&floatarg);
9bd0f299 H	152	addfreq(freqs, floatarg);
59934436 H	153	break;
	154	case 't':
	155	treshold = atoi(optarg);
	156	break;
c7611c89	157	case 'n':
c9be90d5	158	format = optarg[0];
59934436	159	break;
c7611c89	160	case 'l':
19a509d1	161	filter = optarg[0];
59934436	162	break;
7d11ab97 TM	163	case 'u':
	164	under = 1;
	165	break;
59934436 H	166	case 'q':
	167	verbose = 0;
	168	break;
	169	case '?':
	170	print_help(argv);
	171	return 0;
	172	break;
	173	}
	174	}
	175
	176	if(freqs[0]==-1) addfreq(freqs, 440);
4b43521a H	177	float samples[samplecount];
4b43521a H	178	float position = 0;
59934436 H	179
	180	if(verbose) {
	181	fprintf(stderr,
9bd0f299	182	"#Detected tone: %.2f Hz\n"
59934436 H	183	"#Samplerate: %d Hz\n"
	184	"#Frame lenght: %d samples\n"
	185	"#Treshold: %d\n"
	186	"#\n"
	187	,freqs[0],samplerate,samplecount,treshold);
	188	fflush(stderr);
	189
	190	printf("#Position");
	191	int i; for(i=0;freqs[i]!=-1;i++) {
9bd0f299	192	printf("\t%2.0fHz",freqs[i]); //TODO: print decimal places
59934436 H	193	}
	194	puts("");
	195	}
	196
19a509d1 TM	197	int i;
	198	char print=0, printnow=0;
	199	char laststate[argc]; for(i=0;freqs[i]!=-1;i++) laststate[i]=-1;
4b43521a	200	while(!feof(stdin)) {
59934436 H	201
59934436 H	202	//Sample data
4b43521a H	203	for(i=0;i<samplecount && !feof(stdin);i++) {
	204	unsigned char sample;
	205	fread(&sample,1,1,stdin);
	206	samples[i]=sample;
	207	//printf("%d\n", sample);
	208	}
59934436 H	209
	210	//Apply goertzel
	211	float power[argc];
	212	print=0;
	213	for(i=0;freqs[i]!=-1;i++) {
	214	power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
	215
19a509d1	216	//Decide if we will print
7d11ab97	217	printnow = under ? power[i] < treshold : power[i] > treshold; //Is over/under treshold?
19a509d1 TM	218	switch(filter) {
	219	case 'c': //Print if treshold crossed
	220	print = print \|\| (laststate[i] != printnow);
	221	break;
	222	default:
	223	case 'f': //Print if over treshold or falled down
	224	print = print \|\| (laststate[i] != printnow);
	225	case 't': //Print if over treshold
	226	print = print \|\| printnow;
	227	}
	228	laststate[i] = printnow; //Store last state
59934436	229	}
59934436 H	230	fflush(stdout);
	231
	232	//Print data
	233	if(print) {
	234	printf("%8.2f", position);
	235	for(i=0;freqs[i]!=-1;i++) {
	236	printf("\t");
c9be90d5 TM	237	switch(format) {
c9be90d5 TM	238	case 'i':
7d11ab97	239	printf("%d",(int)round(power[i]));
c9be90d5 TM	240	break;
	241	case 'b':
	242	printf("%d",power[i]>treshold);
	243	break;
	244	case 'B':
	245	if(power[i]>treshold) printf("true");
	246	else printf("false");
	247	break;
	248	case 'f':
	249	default:
9bd0f299	250	printf("%7.5f",power[i]);
c9be90d5	251	}
59934436 H	252	}
	253	puts("");
	254	fflush(stdout);
	255	}
	256
	257	//Increase time
4b43521a	258	position += ((float)samplecount/(float)samplerate);
4b43521a H	259	}
4b43521a H	260	}