Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Simple Spec -- A Simple Discrete-time STFT Program.
#include <math.h>
#include <sndfile.h>
#include <string.h>
#include <assert.h>
#include <getopt.h>
#define PI 3.141592653589793
#define EPS 1e-8
#define MAX_BIN_NUM 65536
/* SHIFT_NUM should be less than or equal to BIN_NUM */
#define BUFF_SIZE (MAX_BIN_NUM * 10)
typedef struct Comp {
/* comp of the form: a + bi */
double a, b;
} Comp;
double pcos[2][MAX_BIN_NUM], psin[2][MAX_BIN_NUM];
typedef void (*win_func_t)(double *w, int n);
typedef void (*amp_func_t)(void);
typedef void (*init_func_t)(void);
typedef void (*finale_func_t)(void);
void rect_window(double *, int);
void hann_window(double *, int);
void hamming_window(double *, int);
void linear_amp(void);
void normalize_amp(void);
void decibel_amp(void);
void dtmf_amp(void);
void dtmf_init(void);
void dtmf_finale(void);
void common_init(void) {}
void common_finale(void) {}
int bin_num = 1024;
int sample_rate = 44100;
int resample_rate = 4410;
int shift_num = 256;
int channels = 2;
win_func_t wfunc = hann_window;
amp_func_t afunc = decibel_amp;
init_func_t ifunc = common_init;
init_func_t ffunc = common_finale;
Comp sig[MAX_BIN_NUM], freq[MAX_BIN_NUM];
double win[MAX_BIN_NUM];
short buff[BUFF_SIZE * 2];
void fft_init() {
int i;
for (i = 0; i < bin_num; i++)
{
pcos[0][i] = cos(-PI / (double)i);
pcos[1][i] = cos(PI / (double)i);
psin[0][i] = sin(-PI / (double)i);
psin[1][i] = sin(PI / (double)i);
}
}
#define comp_mul_self(c, c2) \
do { \
double ca = c->a; \
c->a = ca * c2->a - c->b * c2->b; \
c->b = c->b * c2->a + ca * c2->b; \
} while (0)
void fft(const Comp *sig, Comp *freq, int s, int n, int inv) {
int i, hn = n >> 1;
Comp ep, ei;
Comp *pi = &ei, *pp = &ep;
ep.a = pcos[inv][hn];
ep.b = psin[inv][hn];
if (!hn) *freq = *sig;
else
{
fft(sig, freq, s << 1, hn, inv);
fft(sig + s, freq + hn, s << 1, hn, inv);
pi->a = 1;
pi->b = 0;
for (i = 0; i < hn; i++)
{
Comp even = freq[i], *pe = freq + i, *po = pe + hn;
comp_mul_self(po, pi);
pe->a += po->a;
pe->b += po->b;
po->a = even.a - po->a;
po->b = even.b - po->b;
comp_mul_self(pi, pp);
}
}
}
/* Global variables for DTMF Recognition */
int key_freq[8] = {697, 770, 852, 941, 1209, 1336, 1477, 1633};
int key_char[16] = {'1', '2', '3', 'A', '4', '5', '6', 'B', '7', '8', '9', 'C', '*', '0', '#', 'D'};
int energy_range[8][2], base_range[8][2];
int hit_len = 4;
int miss_len = 1;
int alive[8], palive[8], hit_cnt[8], miss_cnt[8];
char dial_seq[1024], *dtail;
double tolerance = 0.015, thres = 0.9;
void dtmf_init() {
double basef = resample_rate / (double)bin_num;
int mask[MAX_BIN_NUM];
int i, j, hb = bin_num >> 1;
memset(mask, 0, sizeof mask);
for (i = 0; i < 8; i++)
{
double kf = key_freq[i];
double pos = kf / basef;
int l, r;
for (l = floor(pos) - 1; l >= 0 && fabs(l * basef - kf) / kf < tolerance; l--);
for (r = ceil(pos) + 1; r < hb && fabs(r * basef - kf) / kf < tolerance; r++);
l++; r--;
for (j = l; j <= r; j++)
{
if (mask[j]) assert(0 && "You can not tolerate that much!");
mask[j] = 1;
}
energy_range[i][0] = l;
energy_range[i][1] = r;
}
for (i = 0; i < 8; i++)
{
int l, r;
for (l = energy_range[i][0] - 1; l >= 0 && !mask[l]; l--);
for (r = energy_range[i][1] + 1; r < hb && !mask[r]; r++);
base_range[i][0] = l + 1;
base_range[i][1] = r - 1;
}
for (i = 0; i < 8; i++)
{
int el = energy_range[i][0], er = energy_range[i][1];
int bl = base_range[i][0], br = base_range[i][1];
fprintf(stderr, "Tone %d Hz: %d(%.3f) ~ %d(%.3f), %d(%.3f) ~ %d(%.3f)\n",
key_freq[i], el, el * basef, er, er * basef,
bl, bl * basef, br, br * basef);
}
memset(palive, 0, sizeof palive);
dtail = dial_seq;
}
void dtmf_finale() {
*dtail = '\0';
printf("The dailed sequence is: %s\n", dial_seq);
}
void spectral_analyzer(SNDFILE *sf) {
short *fptr = buff;
sf_count_t fcnt = 0, cnt = 0, bcnt = 0, bound;
while (sf_readf_short(sf, buff + (bcnt << 1), 1))
{
if (!cnt)
{
bound = buff - fptr + ((++bcnt) << 1);
while ((fcnt << 1) < bound)
{
while ((fcnt << 1) < bound && fcnt < bin_num)
fcnt++;
if (fcnt == bin_num)
{
int i;
double max = 0;
for (i = 0; i < bin_num; i++)
{
sig[i].a = win[i] * (fptr[i << 1] + fptr[(i << 1) + 1]) / 2.0;
sig[i].b = 0;
}
fft(sig, freq, 1, bin_num, 0);
afunc();
fptr += shift_num << 1;
bound = buff - fptr + (bcnt << 1);
fcnt = 0;
}
}
if (bcnt == BUFF_SIZE)
{
/* move to the beginning */
memmove(buff, fptr, (fcnt << 1) * sizeof(buff[0]));
bcnt = fcnt;
fptr = buff;
}
}
/* naive resampling */
if (++cnt == sample_rate / resample_rate)
cnt = 0;
}
}
int is_raw = 0;
struct option long_options[] = {
{"win", required_argument, NULL, 'w'},
{"bin", required_argument, NULL, 'b'},
{"delta", required_argument, NULL, 'd'},
{"raw", no_argument, &is_raw, 1},
{"freq", required_argument, NULL, 'f'},
{"amp", required_argument, NULL, 'a'},
{"help", no_argument, NULL, 'h'},
{0, 0, 0, 0}
};
int str_to_int(char *repr, int *flag) {
char *endptr;
int val = (int)strtol(repr, &endptr, 10);
if (endptr == repr || endptr != repr + strlen(repr))
{
*flag = 0;
return 0;
}
*flag = 1;
return val;
}
int main(int argc, char **argv) {
SF_INFO info;
SNDFILE *sf;
int opt, ind = 0;
while ((opt = getopt_long(argc, argv, "w:b:d:rf:a:h", long_options, &ind)) != -1)
{
switch (opt)
{
case 'w':
if (!strcmp(optarg, "rect"))
wfunc = rect_window;
else if (!strcmp(optarg, "hann"))
wfunc = hann_window;
else if (!strcmp(optarg, "hamming"))
wfunc = hamming_window;
else
return fprintf(stderr, "Invalid window: %s.\n", optarg), 1;
break;
case 'b':
{
int flag;
bin_num = str_to_int(optarg, &flag);
if (!flag)
return fprintf(stderr, "Please specify an integer.\n"), 1;
break;
}
case 'd':
{
int flag;
shift_num = str_to_int(optarg, &flag);
if (!flag)
return fprintf(stderr, "Please specify an integer.\n"), 1;
break;
}
case 'r':
is_raw = 1;
break;
case 'f':
{
int flag;
resample_rate = str_to_int(optarg, &flag);
if (!flag)
return fprintf(stderr, "Please specify an integer.\n"), 1;
break;
}
case 'a':
if (!strcmp(optarg, "linear"))
afunc = linear_amp;
else if (!strcmp(optarg, "decibel"))
afunc = decibel_amp;
else if (!strcmp(optarg, "norm"))
afunc = normalize_amp;
else if (!strcmp(optarg, "dtmf"))
{
afunc = dtmf_amp;
ifunc = dtmf_init;
ffunc = dtmf_finale;
/* Default settings for DTMF */
bin_num = 256;
sample_rate = 8000;
resample_rate = 8000;
shift_num = 128;
channels = 1;
}
else
return fprintf(stderr, "Invalid amplifier: %s.\n", optarg), 1;
break;
case 'h':
fprintf(stderr,
"Simple Spec -- A Simple Discrete-time STFT Program.\n\n"
"Usage: simple_spec [OPTION]... [FILE]\n\n"
" -w, --win\t\twindow function: rect, hann, hamming\n"
" -b, --bin\t\tthe width of a window\n"
" -d, --delta\t\tthe shift of a window\n"
" --raw\t\t\tread raw PCM16 data\n"
" -f, --freq\t\tthe frequency used in resampling\n"
" -a, --amp\t\tthe way of calculating amplitude: linear, decibel, norm, dtmf\n"
" -h, --help\t\tshow this info\n"
"\nAuthor: Ted Yin <ted.sybil@gmail.com>\n");
return 0;
default:
return 1;
}
}
if (is_raw)
{
info.format = SF_FORMAT_PCM_16;
info.samplerate = sample_rate;
info.channels = channels;
}
else
info.format = 0;
if (optind == argc)
{
fprintf(stderr, "Please specify a file name.\n");
return 1;
}
if (!(sf = sf_open(argv[optind], SFM_READ, &info)))
{
fprintf(stderr, "Failed while opening file: %s.\n", argv[optind]);
return 1;
}
fprintf(stderr, "Frames: %lu\n", info.frames);
fprintf(stderr, "Sample Rate: %d Hz\n", info.samplerate);
sample_rate = info.samplerate;
resample_rate = sample_rate / (sample_rate / resample_rate);
fft_init();
ifunc();
wfunc(win, bin_num);
sf_seek(sf, 0, SEEK_CUR);
spectral_analyzer(sf);
ffunc();
sf_close(sf);
return 0;
}
void hann_window(double *w, int n) {
int i;
for (i = 0; i < n; i++)
w[i] = 0.5 * (1 - cos(2 * PI * i / (n - 1)));
}
void hamming_window(double *w, int n) {
int i;
for (i = 0; i < n; i++)
w[i] = 0.53836 - 0.46164 * cos(2 * PI * i / (n - 1));
}
void rect_window(double *w, int n) {
int i;
for (i = 0; i < n; i++)
w[i] = 1;
}
void linear_amp(void) {
int hb = bin_num >> 1, i;
double a, b;
for (i = 0; i < hb; i++)
{
a = freq[i].a;
b = freq[i].b;
printf("%.6f ", sqrt(a * a + b * b));
}
puts("");
}
void dtmf_amp(void) {
int state[8];
int hb = bin_num >> 1, i, j, flag;
double e0 = 0, e, a, b;
double dis[8];
memset(state, 0, sizeof state);
for (i = 0; i < hb; i++)
{
double a = freq[i].a;
double b = freq[i].b;
b = sqrt(a * a + b * b);
e0 += b * b;
}
e0 /= hb;
for (i = 0; i < 8; i++)
{
int el = energy_range[i][0], er = energy_range[i][1];
e = 0;
for (j = el; j <= er; j++)
{
a = freq[j].a;
b = freq[j].b;
e += a * a + b * b; /* V^2 */
}
e /= er - el + 1;
if (e0 > EPS && e / e0 > 10)
{ /* hit */
dis[i] = e;
if (++hit_cnt[i] == hit_len)
{
hit_cnt[i] = 0;
miss_cnt[i] = 0;
alive[i] = 1;
}
}
else
{
dis[i] = 0;
if (++miss_cnt[i] == miss_len)
{
hit_cnt[i] = 0;
miss_cnt[i] = 0;
alive[i] = 0;
}
}
}
flag = 0;
for (i = 0; i < 8; i++)
flag += alive[i];
if (flag >= 2)
{
double mhi = 0, mlo = 0;
int hi = -1, lo = -1;
flag = 0;
for (i = 0; i < 8; i++)
if (alive[i] != palive[i])
{
flag = 1;
break;
}
if (flag)
{
for (i = 0; i < 4; i++)
if (alive[i] && dis[i] > mhi)
{
hi = i * 4;
mhi = dis[i];
}
for (i = 4; i < 8; i++)
if (alive[i] && dis[i] > mlo)
{
lo = i - 4;
mlo = dis[i];
}
if (hi > -1 && lo > -1)
{
*dtail++ = key_char[hi + lo];
printf("Dialed: %c\n", key_char[hi + lo]);
}
}
}
memmove(palive, alive, sizeof alive);
for (i = 0; i < 8; i++)
printf("%c", alive[i] ? '*' : '.');
puts("");
}
void normalize_amp(void) {
double res[MAX_BIN_NUM], max = 0;
int hb = bin_num >> 1, i;
for (i = 0; i < hb; i++)
{
double a = freq[i].a;
double b = freq[i].b;
res[i] = sqrt(a * a + b * b);
if (res[i] > max)
max = res[i];
}
if (max < EPS)
{
for (i = 0; i < hb; i++)
printf("%.6f ", 0.0);
}
else
{
for (i = 0; i < hb; i++)
printf("%.6f ", res[i] / max);
}
puts("");
}
void decibel_amp(void) {
double res[MAX_BIN_NUM], max = 0;
int hb = bin_num >> 1, i;
for (i = 0; i < hb; i++)
{
double a = freq[i].a;
double b = freq[i].b;
b = a * a + b * b;
b = b < EPS ? 0 : 10 * log10(b);
printf("%.6f ", b);
}
puts("");
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.