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

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


/*
	Usage examples
	arecord | ./goertzel
	sox input.mp3 -b 8 -c 1 -r 8000 -t wav - | ./goertzel

	Arguments for DTMF decoding:
	-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10
*/

float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
	/*
		On lower samplerates and frame sizes this may perform sub-optimally. Eg.:
		When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples)
		it actually detects something around 438,3Hz rather than 400Hz...
		If you can't increase samplerate way around this is just to increase sensitivity.
	*/

    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("help me %s\n", argv[0]);
}

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 samples[] = {0,1,2,3,4,5,6,7,8,9,8,7,6,5,4,3,2,1};
	int samplecount = 18;
	float power = goertzel(samplecount, samples, 1.2, 18);
	printf("G: %f\n", power);
	*/

	int samplerate = 8000;
	int samplecount = 4000;
	int treshold = -1;
	char noreturn = 0;
	char repeat = 1;
	char integers=0;
	char verbose=1;
	int freqs[argc+1]; freqs[0]=-1;

	int opt;
	while ((opt = getopt(argc, argv, "?d:r:s:f:t:iqna")) != -1) {
		switch (opt) {
			case 'r':
				samplerate = atoi(optarg);
				break;
			case 's':
				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 'i':
				integers = 1;
				break;
			case 'a':
				repeat = 0;
				break;
			case 'n':
				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");
				if(integers)
					printf("%d",(int)power[i]);
				else
					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
	6	/*
	7	Usage examples
	8	arecord \| ./goertzel
	9	sox input.mp3 -b 8 -c 1 -r 8000 -t wav - \| ./goertzel
59934436 H	10
	11	Arguments for DTMF decoding:
	12	-f 697 -f 770 -f 852 -f 941 -f 1209 -f 1336 -f 1477 -f 1633 -t 10
4b43521a H	13	*/
	14
	15	float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
	16	{
	17	/*
	18	On lower samplerates and frame sizes this may perform sub-optimally. Eg.:
	19	When set to detect 440Hz (at 8000Hz samplerate and ~4000 samples)
	20	it actually detects something around 438,3Hz rather than 400Hz...
	21	If you can't increase samplerate way around this is just to increase sensitivity.
	22	*/
	23
	24	int k,i;
	25	float floatnumSamples;
	26	float omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;
	27
	28	float scalingFactor = numSamples / 2.0;
	29
	30	floatnumSamples = (float) numSamples;
	31	k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
	32	omega = (2.0 * M_PI * k) / floatnumSamples;
	33	sine = sin(omega);
	34	cosine = cos(omega);
	35	coeff = 2.0 * cosine;
	36	q0=0;
	37	q1=0;
	38	q2=0;
	39
	40	for(i=0; i<numSamples; i++)
	41	{
	42	q0 = coeff * q1 - q2 + data[i];
	43	q2 = q1;
	44	q1 = q0;
	45	}
	46
	47	// calculate the real and imaginary results
	48	// scaling appropriately
	49	real = (q1 - q2 * cosine) / scalingFactor;
	50	imag = (q2 * sine) / scalingFactor;
	51
	52	magnitude = sqrtf(realreal + imagimag);
	53	return magnitude;
	54	}
	55
59934436 H	56	void print_help(char ** argv) {
	57	printf("help me %s\n", argv[0]);
	58	}
	59
	60	void addfreq(int *freqs, int freq) {
	61	int i = 0;
	62	while(freqs[i]!=-1) i++;
	63	freqs[i]=freq;
	64	freqs[i+1]=-1;
	65	}
	66
	67	int main(int argc, char ** argv) {
4b43521a H	68	/*
	69	int samples[] = {0,1,2,3,4,5,6,7,8,9,8,7,6,5,4,3,2,1};
	70	int samplecount = 18;
	71	float power = goertzel(samplecount, samples, 1.2, 18);
	72	printf("G: %f\n", power);
	73	*/
	74
	75	int samplerate = 8000;
	76	int samplecount = 4000;
59934436 H	77	int treshold = -1;
59934436 H	78	char noreturn = 0;
4b50f692	79	char repeat = 1;
59934436 H	80	char integers=0;
	81	char verbose=1;
	82	int freqs[argc+1]; freqs[0]=-1;
	83
	84	int opt;
f5c8b03d	85	while ((opt = getopt(argc, argv, "?d:r:s:f:t:iqna")) != -1) {
59934436 H	86	switch (opt) {
	87	case 'r':
	88	samplerate = atoi(optarg);
	89	break;
	90	case 's':
	91	samplecount = atoi(optarg);
	92	break;
f5c8b03d H	93	case 'd':
	94	samplecount = samplerate/atoi(optarg);
	95	break;
59934436 H	96	case 'f':
	97	addfreq(freqs, atoi(optarg));
	98	break;
	99	case 't':
	100	treshold = atoi(optarg);
	101	break;
	102	case 'i':
	103	integers = 1;
	104	break;
4b50f692 H	105	case 'a':
	106	repeat = 0;
	107	break;
59934436 H	108	case 'n':
	109	noreturn = 1;
	110	break;
	111	case 'q':
	112	verbose = 0;
	113	break;
	114	case '?':
	115	print_help(argv);
	116	return 0;
	117	break;
	118	}
	119	}
	120
	121	if(freqs[0]==-1) addfreq(freqs, 440);
4b43521a H	122	float samples[samplecount];
4b43521a H	123	float position = 0;
59934436 H	124
	125	if(verbose) {
	126	fprintf(stderr,
	127	"#Detected tone: %d Hz\n"
	128	"#Samplerate: %d Hz\n"
	129	"#Frame lenght: %d samples\n"
	130	"#Treshold: %d\n"
	131	"#\n"
	132	,freqs[0],samplerate,samplecount,treshold);
	133	fflush(stderr);
	134
	135	printf("#Position");
	136	int i; for(i=0;freqs[i]!=-1;i++) {
	137	printf("\t%2dHz",freqs[i]);
	138	}
	139	puts("");
	140	}
	141
	142	char print=0, printnow=0, printlast = 0;
4b43521a H	143	while(!feof(stdin)) {
4b43521a H	144	int i;
59934436 H	145
59934436 H	146	//Sample data
4b43521a H	147	for(i=0;i<samplecount && !feof(stdin);i++) {
	148	unsigned char sample;
	149	fread(&sample,1,1,stdin);
	150	samples[i]=sample;
	151	//printf("%d\n", sample);
	152	}
59934436 H	153
	154	//Apply goertzel
	155	float power[argc];
	156	print=0;
	157	for(i=0;freqs[i]!=-1;i++) {
	158	power[i] = goertzel_mag(samplecount, freqs[i], samplerate, samples);
	159
	160	//Set print true if over treshold or if changed to false (print for the last time after going under treshold)
	161	printnow = power[i] > treshold;
4b50f692	162	print = !(!repeat && printlast && !(!printnow)) && (print \|\| printnow \|\| (printlast && !noreturn));
59934436 H	163	}
	164	printlast = printnow;
	165	fflush(stdout);
	166
	167	//Print data
	168	if(print) {
	169	printf("%8.2f", position);
	170	for(i=0;freqs[i]!=-1;i++) {
	171	printf("\t");
	172	if(integers)
	173	printf("%d",(int)power[i]);
	174	else
	175	printf("%.4f",power[i]);
	176	}
	177	puts("");
	178	fflush(stdout);
	179	}
	180
	181	//Increase time
4b43521a	182	position += ((float)samplecount/(float)samplerate);
4b43521a H	183	}
4b43521a H	184	}