[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\t\tDo not repeat values while still over treshold\n"
		"\t-b\t\tDo not print first value that will fall under treshold\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 noreturn = 0;
	char repeat = 1;
	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:lbq")) != -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':
				repeat = 0;
				break;
			case 'b':
				noreturn = 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("");
	}

	char print=0, printnow=0, printlast = 0;
	while(!feof(stdin)) {
		int i;

		//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);

			//Set print true if over treshold or if changed to false (print for the last time after going under treshold)
			printnow = power[i] > treshold;
			print = !(!repeat && printlast && !(!printnow)) && (print || printnow || (printlast && !noreturn));
		}
		printlast = printnow;
		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"
84329d34 H	78	"\t-l\t\tDo not repeat values while still over treshold\n"
	79	"\t-b\t\tDo not print first value that will fall under treshold\n"
	80	"\t-q\t\tQuiet mode: print only values\n"
	81	"\n"
	82	"\t-?\t\tPrint help\n"
	83	"\n"
	84	);
	85
	86	printf(
	87	"Usage examples:\n"
	88	"\tarecord \| %s\n"
	89	"\tsox input.mp3 -b 8 -c 1 -r 8000 -t wav - \| %s\n"
	90	"\t%s -n -q -l -r 8000 -d 20 -t $tresh -f 697 [-f 770 ...]\n"
	91	"\n"
	92	,argv[0],argv[0],argv[0]
	93	);
	94
	95	printf(
	96	"Frequencies for DTMF decoding:\n"
	97	"\t-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10\n"
	98	);
59934436 H	99	}
	100
	101	void addfreq(int *freqs, int freq) {
	102	int i = 0;
	103	while(freqs[i]!=-1) i++;
	104	freqs[i]=freq;
	105	freqs[i+1]=-1;
	106	}
	107
	108	int main(int argc, char ** argv) {
4b43521a H	109	int samplerate = 8000;
4b43521a H	110	int samplecount = 4000;
59934436 H	111	int treshold = -1;
59934436 H	112	char noreturn = 0;
4b50f692	113	char repeat = 1;
c9be90d5	114	char format=0;
59934436 H	115	char verbose=1;
	116	int freqs[argc+1]; freqs[0]=-1;
	117
	118	int opt;
c9be90d5	119	while ((opt = getopt(argc, argv, "?i:o:a:r:c:d:f:t:n:lbq")) != -1) {
59934436	120	switch (opt) {
c7611c89 H	121	case 'i':
	122	freopen(optarg, "r", stdin);
	123	break;
	124	case 'o':
	125	freopen(optarg, "w", stdout);
	126	break;
	127	case 'a':
	128	freopen(optarg, "a", stdout);
	129	break;
59934436 H	130	case 'r':
	131	samplerate = atoi(optarg);
	132	break;
c7611c89	133	case 'c':
59934436 H	134	samplecount = atoi(optarg);
59934436 H	135	break;
f5c8b03d H	136	case 'd':
	137	samplecount = samplerate/atoi(optarg);
	138	break;
59934436 H	139	case 'f':
	140	addfreq(freqs, atoi(optarg));
	141	break;
	142	case 't':
	143	treshold = atoi(optarg);
	144	break;
c7611c89	145	case 'n':
c9be90d5	146	format = optarg[0];
59934436	147	break;
c7611c89	148	case 'l':
4b50f692 H	149	repeat = 0;
4b50f692 H	150	break;
c7611c89	151	case 'b':
59934436 H	152	noreturn = 1;
	153	break;
	154	case 'q':
	155	verbose = 0;
	156	break;
	157	case '?':
	158	print_help(argv);
	159	return 0;
	160	break;
	161	}
	162	}
	163
	164	if(freqs[0]==-1) addfreq(freqs, 440);
4b43521a H	165	float samples[samplecount];
4b43521a H	166	float position = 0;
59934436 H	167
	168	if(verbose) {
	169	fprintf(stderr,
	170	"#Detected tone: %d Hz\n"
	171	"#Samplerate: %d Hz\n"
	172	"#Frame lenght: %d samples\n"
	173	"#Treshold: %d\n"
	174	"#\n"
	175	,freqs[0],samplerate,samplecount,treshold);
	176	fflush(stderr);
	177
	178	printf("#Position");
	179	int i; for(i=0;freqs[i]!=-1;i++) {
	180	printf("\t%2dHz",freqs[i]);
	181	}
	182	puts("");
	183	}
	184
	185	char print=0, printnow=0, printlast = 0;
4b43521a H	186	while(!feof(stdin)) {
4b43521a H	187	int i;
59934436 H	188
59934436 H	189	//Sample data
4b43521a H	190	for(i=0;i<samplecount && !feof(stdin);i++) {
	191	unsigned char sample;
	192	fread(&sample,1,1,stdin);
	193	samples[i]=sample;
	194	//printf("%d\n", sample);
	195	}
59934436 H	196
	197	//Apply goertzel
	198	float power[argc];
	199	print=0;
	200	for(i=0;freqs[i]!=-1;i++) {
	201	power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
	202
	203	//Set print true if over treshold or if changed to false (print for the last time after going under treshold)
	204	printnow = power[i] > treshold;
4b50f692	205	print = !(!repeat && printlast && !(!printnow)) && (print \|\| printnow \|\| (printlast && !noreturn));
59934436 H	206	}
	207	printlast = printnow;
	208	fflush(stdout);
	209
	210	//Print data
	211	if(print) {
	212	printf("%8.2f", position);
	213	for(i=0;freqs[i]!=-1;i++) {
	214	printf("\t");
c9be90d5 TM	215	switch(format) {
	216	case 'i':
	217	printf("%d",(int)power[i]);
	218	break;
	219	case 'b':
	220	printf("%d",power[i]>treshold);
	221	break;
	222	case 'B':
	223	if(power[i]>treshold) printf("true");
	224	else printf("false");
	225	break;
	226	case 'f':
	227	default:
	228	printf("%.4f",power[i]);
	229	}
59934436 H	230	}
	231	puts("");
	232	fflush(stdout);
	233	}
	234
	235	//Increase time
4b43521a	236	position += ((float)samplecount/(float)samplerate);
4b43521a H	237	}
4b43521a H	238	}