1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
|
#include <inttypes.h>
#include <math.h>
static double desired_7kHz_lowpass[] = {1.0, 0.0};
static double weights_7kHz_lowpass[] = {0.2, 2.0};
double *calc_coefficients_7kHz_lowpass(int rate)
{
double *result = (double *)malloc(32*sizeof(double));
double bands[4];
bands[0] = 0.0; bands[1] = 6800.0/rate;
bands[2] = 8500.0/rate; bands[3] = 0.5;
remez(result, 32, 2, bands,
desired_7kHz_lowpass, weights_7kHz_lowpass, BANDPASS);
return result;
}
#if 0
static double desired_125Hz_lowpass[] = {1.0, 0.0};
static double weights_125Hz_lowpass[] = {0.2, 2.0};
double *calc_coefficients_125Hz_lowpass(int rate)
{
double *result = (double *)malloc(256*sizeof(double));
double bands[4];
bands[0] = 0.0; bands[1] = 125.0/rate;
bands[2] = 175.0/rate; bands[3] = 0.5;
remez(result, 256, 2, bands,
desired_125Hz_lowpass, weights_125Hz_lowpass, BANDPASS);
return result;
}
#endif
int16_t firfilter(int16_t *buf, int pos, int len, int count, double *coefficients)
{
double result = 0.0;
int count1, count2;
int16_t *ptr;
if (pos >= count) {
pos -= count;
count1 = count; count2 = 0;
}
else {
count2 = pos;
count1 = count - pos;
pos = len - count1;
}
//fprintf(stderr, "pos=%d, count1=%d, count2=%d\n", pos, count1, count2);
// high part of window
ptr = &buf[pos];
while (count1--) result += *ptr++ * *coefficients++;
// wrapped part of window
while (count2--) result += *buf++ * *coefficients++;
return result;
}
void dump_filter_coefficients(double *coefficients)
{
int i;
fprintf(stderr, "pl_surround: Filter coefficients are: \n");
for (i=0; (i<32); i++) {
fprintf(stderr, " [%2d]: %23.20f\n", i, coefficients[i]);
}
}
#ifdef TESTING
#define PI 3.1415926536
// For testing purposes, fill a buffer with some sine-wave tone
void sinewave(int16_t *output, int samples, int incr, int freq, double phase, int samplerate)
{
double radians_per_sample = 2*PI / ((0.0+samplerate) / freq), r;
//fprintf(stderr, "samples=%d tone freq=%d, samplerate=%d, radians/sample=%f\n",
// samples, freq, samplerate, radians_per_sample);
r = phase;
while (samples--) {
*output = sin(r)*10000; output = &output[incr];
r += radians_per_sample;
}
}
// Pass various frequencies through a FIR filter and report amplitudes
void testfilter(double *coefficients, int count, int samplerate)
{
int16_t wavein[8192]; //, waveout[2048];
int sample, samples, maxsample, minsample, totsample;
int nyquist=samplerate/2;
int freq, i;
for (freq=25; freq<nyquist; freq+=25) {
// Make input tone
sinewave(wavein, 8192, 1, freq, 0.0, samplerate);
//for (i=0; i<32; i++)
// fprintf(stderr, "%5d\n", wavein[i]);
// Filter through the filter, measure results
maxsample=0; minsample=1000000; totsample=0; samples=0;
for (i=2048; i<8192; i++) {
//waveout[i] = wavein[i];
sample = abs(firfilter(wavein, i, 8192, count, coefficients));
if (sample > maxsample) maxsample=sample;
if (sample < minsample) minsample=sample;
totsample += sample; samples++;
}
// Report results
fprintf(stderr, "%5d %5d %5d %5d %f\n", freq, totsample/samples, maxsample, minsample, 10*log((totsample/samples)/6500.0));
}
}
#endif
|
