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