216 lines
5.1 KiB
C++
216 lines
5.1 KiB
C++
/* Goertzel Algorithm
|
|
* http://cms.edn.com/uploads/SourceCode/09banks.txt
|
|
* https://github.com/pramasoul/jrand/blob/master/goertzel.c
|
|
*
|
|
* reads 8khz 8bit samples from stdin, for example using `arecord | dtmf`
|
|
*/
|
|
|
|
#define PI 3.141592653589793
|
|
|
|
#include <cstdio>
|
|
#include <cmath>
|
|
#include <cstdint>
|
|
|
|
#define FLOATING double
|
|
#define SAMPLE uint8_t
|
|
|
|
#define SAMPLING_RATE 8000.0 //8kHz
|
|
#define N 205 //Block size
|
|
|
|
struct gzbin {
|
|
FLOATING coeff;
|
|
FLOATING Q1;
|
|
FLOATING Q2;
|
|
FLOATING sine;
|
|
FLOATING cosine;
|
|
};
|
|
|
|
|
|
FLOATING dtmf_freq[8] = {
|
|
697.0, 770.0, 852.0, 941.0,
|
|
1209.0, 1336.0, 1477.0, 1633.0};
|
|
struct gzbin gzbins[8] = {};
|
|
|
|
SAMPLE testData[N];
|
|
|
|
/* Call this routine before every "block" (size=N) of samples. */
|
|
void ResetGoertzel(struct gzbin *bin)
|
|
{
|
|
bin->Q2 = 0;
|
|
bin->Q1 = 0;
|
|
}
|
|
|
|
/* Call this once, to precompute the constants. */
|
|
void InitGoertzel(struct gzbin *bin, FLOATING target_frequency)
|
|
{
|
|
int k;
|
|
FLOATING floatN;
|
|
FLOATING omega;
|
|
|
|
floatN = (FLOATING) N;
|
|
k = floor(0.5 + ((floatN * target_frequency) / SAMPLING_RATE));
|
|
omega = (2.0 * PI * k) / floatN;
|
|
bin->sine = sin(omega);
|
|
bin->cosine = cos(omega);
|
|
bin->coeff = 2.0 * bin->cosine;
|
|
|
|
printf("For SAMPLING_RATE = %f", SAMPLING_RATE);
|
|
printf(" N = %d", N);
|
|
printf(" and FREQUENCY = %f,\n", target_frequency);
|
|
printf("k = %d and coeff = %f\n\n", k, bin->coeff);
|
|
|
|
ResetGoertzel(bin);
|
|
}
|
|
|
|
/* Call this routine for every sample. */
|
|
void ProcessSample(struct gzbin *bin, SAMPLE sample)
|
|
{
|
|
FLOATING Q0;
|
|
Q0 = bin->coeff * bin->Q1 - bin->Q2 + (FLOATING) sample;
|
|
bin->Q2 = bin->Q1;
|
|
bin->Q1 = Q0;
|
|
}
|
|
|
|
/* Optimized Goertzel */
|
|
/* Call this after every block to get the RELATIVE magnitude squared. */
|
|
FLOATING GetMagnitudeSquared(struct gzbin *bin)
|
|
{
|
|
FLOATING result;
|
|
|
|
result = bin->Q1 * bin->Q1 + bin->Q2 * bin->Q2 - bin->Q1 * bin->Q2 * bin->coeff;
|
|
return result;
|
|
}
|
|
|
|
|
|
char dtmf_decode(int x, int y) {
|
|
switch (y) {
|
|
case 0:
|
|
switch (x) {
|
|
case 0:
|
|
return '1';
|
|
case 1:
|
|
return '2';
|
|
case 2:
|
|
return '3';
|
|
case 3:
|
|
return 'A';
|
|
default:
|
|
return ' ';
|
|
};
|
|
case 1:
|
|
switch (x) {
|
|
case 0:
|
|
return '4';
|
|
case 1:
|
|
return '5';
|
|
case 2:
|
|
return '6';
|
|
case 3:
|
|
return 'B';
|
|
default:
|
|
return ' ';
|
|
};
|
|
case 2:
|
|
switch (x) {
|
|
case 0:
|
|
return '7';
|
|
case 1:
|
|
return '8';
|
|
case 2:
|
|
return '9';
|
|
case 3:
|
|
return 'C';
|
|
default:
|
|
return ' ';
|
|
};
|
|
case 3:
|
|
switch (x) {
|
|
case 0:
|
|
return '*';
|
|
case 1:
|
|
return '0';
|
|
case 2:
|
|
return '#';
|
|
case 3:
|
|
return 'D';
|
|
default:
|
|
return ' ';
|
|
};
|
|
default:
|
|
return ' ';
|
|
}
|
|
}
|
|
|
|
int main()
|
|
{
|
|
FLOATING frequency_magnitudes[8] = {};
|
|
|
|
for (int i = 0; i <8; ++i) {
|
|
InitGoertzel(&gzbins[i], dtmf_freq[i]);
|
|
}
|
|
|
|
while (true) {
|
|
size_t bytes_read = fread(testData, sizeof(SAMPLE), N, stdin);
|
|
if (bytes_read != N) {
|
|
fprintf(stderr, "Failed to read %d from stdin, got: %zu\n", N, bytes_read);
|
|
exit(1);
|
|
}
|
|
|
|
printf("\n\n");
|
|
|
|
for (int i = 0; i < 8; ++i) {
|
|
struct gzbin *bin = &gzbins[i];
|
|
for (unsigned char sample: testData) {
|
|
ProcessSample(bin, sample);
|
|
}
|
|
FLOATING magnitudeSquared = GetMagnitudeSquared(bin);
|
|
frequency_magnitudes[i] = magnitudeSquared;
|
|
printf("Freq: %10.1f; Mag: %20f\n", dtmf_freq[i], magnitudeSquared);
|
|
ResetGoertzel(bin);
|
|
}
|
|
|
|
FLOATING max = 0.0;
|
|
FLOATING min = INFINITY;
|
|
int max_y = 0;
|
|
for (int y = 0; y < 4; ++y) {
|
|
FLOATING magnitude = frequency_magnitudes[y];
|
|
if (magnitude > max) {
|
|
max = magnitude;
|
|
max_y = y;
|
|
}
|
|
if (magnitude < min)
|
|
min = magnitude;
|
|
}
|
|
|
|
if ((max - min) < 1000) {
|
|
printf("\n");
|
|
continue;
|
|
}
|
|
|
|
max = 0.0;
|
|
min = INFINITY;
|
|
int max_x = 0;
|
|
for (int x = 4; x < 8; ++x) {
|
|
FLOATING magnitude = frequency_magnitudes[x];
|
|
if (magnitude > max) {
|
|
max = magnitude;
|
|
max_x = x-4;
|
|
}
|
|
if (magnitude < min)
|
|
min = magnitude;
|
|
}
|
|
|
|
if ((max - min) < 10000) {
|
|
printf("\n");
|
|
continue;
|
|
}
|
|
|
|
printf("x: %d, y: %d, char: %c\n", max_x, max_y, dtmf_decode(max_x, max_y));
|
|
|
|
|
|
}
|
|
|
|
|
|
return 0;
|
|
}
|