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

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

float goertzel_mag(int numSamples,int 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) / 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 - q2 * cosine) / scalingFactor;
    imag = (q2 * 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(int *freqs, int 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;

	int freqs[argc+1]; freqs[0]=-1;

	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':
				addfreq(freqs, atoi(optarg));
				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: %d 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%2dHz",freqs[i]);
		}
		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("%.4f",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
4b43521a H	5	float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
4b43521a H	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;
	14	k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
	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
	32	real = (q1 - q2 * cosine) / scalingFactor;
	33	imag = (q2 * sine) / scalingFactor;
	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	}
	105
	106	void addfreq(int *freqs, int freq) {
	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
59934436 H	124	int freqs[argc+1]; freqs[0]=-1;
	125
	126	int opt;
7d11ab97	127	while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:uq")) != -1) {
59934436	128	switch (opt) {
c7611c89 H	129	case 'i':
	130	freopen(optarg, "r", stdin);
	131	break;
	132	case 'o':
	133	freopen(optarg, "w", stdout);
	134	break;
	135	case 'a':
	136	freopen(optarg, "a", stdout);
	137	break;
59934436 H	138	case 'r':
	139	samplerate = atoi(optarg);
	140	break;
c7611c89	141	case 'c':
59934436 H	142	samplecount = atoi(optarg);
59934436 H	143	break;
f5c8b03d H	144	case 'd':
	145	samplecount = samplerate/atoi(optarg);
	146	break;
59934436 H	147	case 'f':
	148	addfreq(freqs, atoi(optarg));
	149	break;
	150	case 't':
	151	treshold = atoi(optarg);
	152	break;
c7611c89	153	case 'n':
c9be90d5	154	format = optarg[0];
59934436	155	break;
c7611c89	156	case 'l':
19a509d1	157	filter = optarg[0];
59934436	158	break;
7d11ab97 TM	159	case 'u':
	160	under = 1;
	161	break;
59934436 H	162	case 'q':
	163	verbose = 0;
	164	break;
	165	case '?':
	166	print_help(argv);
	167	return 0;
	168	break;
	169	}
	170	}
	171
	172	if(freqs[0]==-1) addfreq(freqs, 440);
4b43521a H	173	float samples[samplecount];
4b43521a H	174	float position = 0;
59934436 H	175
	176	if(verbose) {
	177	fprintf(stderr,
	178	"#Detected tone: %d Hz\n"
	179	"#Samplerate: %d Hz\n"
	180	"#Frame lenght: %d samples\n"
	181	"#Treshold: %d\n"
	182	"#\n"
	183	,freqs[0],samplerate,samplecount,treshold);
	184	fflush(stderr);
	185
	186	printf("#Position");
	187	int i; for(i=0;freqs[i]!=-1;i++) {
	188	printf("\t%2dHz",freqs[i]);
	189	}
	190	puts("");
	191	}
	192
19a509d1 TM	193	int i;
	194	char print=0, printnow=0;
	195	char laststate[argc]; for(i=0;freqs[i]!=-1;i++) laststate[i]=-1;
4b43521a	196	while(!feof(stdin)) {
59934436 H	197
59934436 H	198	//Sample data
4b43521a H	199	for(i=0;i<samplecount && !feof(stdin);i++) {
	200	unsigned char sample;
	201	fread(&sample,1,1,stdin);
	202	samples[i]=sample;
	203	//printf("%d\n", sample);
	204	}
59934436 H	205
	206	//Apply goertzel
	207	float power[argc];
	208	print=0;
	209	for(i=0;freqs[i]!=-1;i++) {
	210	power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
	211
19a509d1	212	//Decide if we will print
7d11ab97	213	printnow = under ? power[i] < treshold : power[i] > treshold; //Is over/under treshold?
19a509d1 TM	214	switch(filter) {
	215	case 'c': //Print if treshold crossed
	216	print = print \|\| (laststate[i] != printnow);
	217	break;
	218	default:
	219	case 'f': //Print if over treshold or falled down
	220	print = print \|\| (laststate[i] != printnow);
	221	case 't': //Print if over treshold
	222	print = print \|\| printnow;
	223	}
	224	laststate[i] = printnow; //Store last state
59934436	225	}
59934436 H	226	fflush(stdout);
	227
	228	//Print data
	229	if(print) {
	230	printf("%8.2f", position);
	231	for(i=0;freqs[i]!=-1;i++) {
	232	printf("\t");
c9be90d5 TM	233	switch(format) {
c9be90d5 TM	234	case 'i':
7d11ab97	235	printf("%d",(int)round(power[i]));
c9be90d5 TM	236	break;
	237	case 'b':
	238	printf("%d",power[i]>treshold);
	239	break;
	240	case 'B':
	241	if(power[i]>treshold) printf("true");
	242	else printf("false");
	243	break;
	244	case 'f':
	245	default:
	246	printf("%.4f",power[i]);
	247	}
59934436 H	248	}
	249	puts("");
	250	fflush(stdout);
	251	}
	252
	253	//Increase time
4b43521a	254	position += ((float)samplecount/(float)samplerate);
4b43521a H	255	}
4b43521a H	256	}