[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 <ratio>\tFrame size (default 2) (samplerate will be divided by this number to get frame size same as -c)\n"
		"\n"
		"\t-f <freq>\tAdd frequency in Hz to detect (use multiple times, if no added 440 Hz will be...)\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"
		"\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 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:q")) != -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 '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 = power[i] > treshold; //Is over 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)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"
	68	"\t-d <ratio>\tFrame size (default 2) (samplerate will be divided by this number to get frame size same as -c)\n"
	69	"\n"
8f751717	70	"\t-f <freq>\tAdd frequency in Hz to detect (use multiple times, if no added 440 Hz will be...)\n"
84329d34 H	71	"\n"
84329d34 H	72	"\t-t <treshold>\tSet treshold (used to hide magnitudes lower than treshold) (defaults -1)\n"
c9be90d5 TM	73	"\t-n <format>\tSet output format\n"
	74	"\t\tf: float (default)\n"
	75	"\t\ti: integer\n"
	76	"\t\tb: binary (0\|1)\n"
	77	"\t\tB: Boolean (false\|true)\n"
19a509d1 TM	78	"\t-l <filter>\tSet filter\n"
	79	"\t\tf: Falldown: print only when over treshold or just falled under (default)\n"
	80	"\t\tt: Treshold: print only when over treshold\n"
	81	"\t\tc: Crossed: print only when treshold crossed\n"
84329d34 H	82	"\t-q\t\tQuiet mode: print only values\n"
	83	"\n"
	84	"\t-?\t\tPrint help\n"
	85	"\n"
	86	);
	87
	88	printf(
	89	"Usage examples:\n"
	90	"\tarecord \| %s\n"
	91	"\tsox input.mp3 -b 8 -c 1 -r 8000 -t wav - \| %s\n"
	92	"\t%s -n -q -l -r 8000 -d 20 -t $tresh -f 697 [-f 770 ...]\n"
	93	"\n"
	94	,argv[0],argv[0],argv[0]
	95	);
	96
	97	printf(
	98	"Frequencies for DTMF decoding:\n"
	99	"\t-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10\n"
	100	);
59934436 H	101	}
	102
	103	void addfreq(int *freqs, int freq) {
	104	int i = 0;
	105	while(freqs[i]!=-1) i++;
	106	freqs[i]=freq;
	107	freqs[i+1]=-1;
	108	}
	109
	110	int main(int argc, char ** argv) {
4b43521a H	111	int samplerate = 8000;
4b43521a H	112	int samplecount = 4000;
59934436	113	int treshold = -1;
19a509d1	114	char filter = 0;
c9be90d5	115	char format=0;
59934436 H	116	char verbose=1;
	117	int freqs[argc+1]; freqs[0]=-1;
	118
	119	int opt;
19a509d1	120	while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:l:q")) != -1) {
59934436	121	switch (opt) {
c7611c89 H	122	case 'i':
	123	freopen(optarg, "r", stdin);
	124	break;
	125	case 'o':
	126	freopen(optarg, "w", stdout);
	127	break;
	128	case 'a':
	129	freopen(optarg, "a", stdout);
	130	break;
59934436 H	131	case 'r':
	132	samplerate = atoi(optarg);
	133	break;
c7611c89	134	case 'c':
59934436 H	135	samplecount = atoi(optarg);
59934436 H	136	break;
f5c8b03d H	137	case 'd':
	138	samplecount = samplerate/atoi(optarg);
	139	break;
59934436 H	140	case 'f':
	141	addfreq(freqs, atoi(optarg));
	142	break;
	143	case 't':
	144	treshold = atoi(optarg);
	145	break;
c7611c89	146	case 'n':
c9be90d5	147	format = optarg[0];
59934436	148	break;
c7611c89	149	case 'l':
19a509d1	150	filter = optarg[0];
59934436 H	151	break;
	152	case 'q':
	153	verbose = 0;
	154	break;
	155	case '?':
	156	print_help(argv);
	157	return 0;
	158	break;
	159	}
	160	}
	161
	162	if(freqs[0]==-1) addfreq(freqs, 440);
4b43521a H	163	float samples[samplecount];
4b43521a H	164	float position = 0;
59934436 H	165
	166	if(verbose) {
	167	fprintf(stderr,
	168	"#Detected tone: %d Hz\n"
	169	"#Samplerate: %d Hz\n"
	170	"#Frame lenght: %d samples\n"
	171	"#Treshold: %d\n"
	172	"#\n"
	173	,freqs[0],samplerate,samplecount,treshold);
	174	fflush(stderr);
	175
	176	printf("#Position");
	177	int i; for(i=0;freqs[i]!=-1;i++) {
	178	printf("\t%2dHz",freqs[i]);
	179	}
	180	puts("");
	181	}
	182
19a509d1 TM	183	int i;
	184	char print=0, printnow=0;
	185	char laststate[argc]; for(i=0;freqs[i]!=-1;i++) laststate[i]=-1;
4b43521a	186	while(!feof(stdin)) {
59934436 H	187
59934436 H	188	//Sample data
4b43521a H	189	for(i=0;i<samplecount && !feof(stdin);i++) {
	190	unsigned char sample;
	191	fread(&sample,1,1,stdin);
	192	samples[i]=sample;
	193	//printf("%d\n", sample);
	194	}
59934436 H	195
	196	//Apply goertzel
	197	float power[argc];
	198	print=0;
	199	for(i=0;freqs[i]!=-1;i++) {
	200	power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
	201
19a509d1 TM	202	//Decide if we will print
	203	printnow = power[i] > treshold; //Is over treshold?
	204	switch(filter) {
	205	case 'c': //Print if treshold crossed
	206	print = print \|\| (laststate[i] != printnow);
	207	break;
	208	default:
	209	case 'f': //Print if over treshold or falled down
	210	print = print \|\| (laststate[i] != printnow);
	211	case 't': //Print if over treshold
	212	print = print \|\| printnow;
	213	}
	214	laststate[i] = printnow; //Store last state
59934436	215	}
59934436 H	216	fflush(stdout);
	217
	218	//Print data
	219	if(print) {
	220	printf("%8.2f", position);
	221	for(i=0;freqs[i]!=-1;i++) {
	222	printf("\t");
c9be90d5 TM	223	switch(format) {
	224	case 'i':
	225	printf("%d",(int)power[i]);
	226	break;
	227	case 'b':
	228	printf("%d",power[i]>treshold);
	229	break;
	230	case 'B':
	231	if(power[i]>treshold) printf("true");
	232	else printf("false");
	233	break;
	234	case 'f':
	235	default:
	236	printf("%.4f",power[i]);
	237	}
59934436 H	238	}
	239	puts("");
	240	fflush(stdout);
	241	}
	242
	243	//Increase time
4b43521a	244	position += ((float)samplecount/(float)samplerate);
4b43521a H	245	}
4b43521a H	246	}