2
// Fidlib digital filter designer code
3
// -----------------------------------
5
// Copyright (c) 2002-2004 Jim Peters <http://uazu.net/>. This
6
// file is released under the GNU Lesser General Public License
7
// (LGPL) version 2.1 as published by the Free Software
8
// Foundation. See the file COPYING_LIB for details, or visit
9
// <http://www.fsf.org/licenses/licenses.html>.
11
// The code in this file was written to go with the Fiview app
12
// (http://uazu.net/fiview/), but it may be used as a library for
13
// other applications. The idea behind this library is to allow
14
// filters to be designed at run-time, which gives much greater
15
// flexibility to filtering applications.
17
// This file depends on the fidmkf.h file which provides the
18
// filter types from Tony Fisher's 'mkfilter' package. See that
19
// file for references and links used there.
22
// Here are some of the sources I used whilst writing this code:
24
// Robert Bristow-Johnson's EQ cookbook formulae:
25
// http://www.harmony-central.com/Computer/Programming/Audio-EQ-Cookbook.txt
28
#define VERSION "0.9.10"
31
// Filter specification string
32
// ---------------------------
34
// The filter specification string can be used to completely
35
// specify the filter, or it can be used with the frequency or
36
// frequency range missing, in which case default values are
37
// picked up from values passed directly to the routine.
39
// The spec consists of a series of letters usually followed by
40
// the order of the filter and then by any other parameters
41
// required, preceded by slashes. For example:
43
// LpBu4/20.4 Lowpass butterworth, 4th order, -3.01dB at 20.4Hz
44
// BpBu2/3-4 Bandpass butterworth, 2nd order, from 3 to 4Hz
45
// BpBu2/=3-4 Same filter, but adjusted exactly to the range given
46
// BsRe/1000/10 Bandstop resonator, Q=1000, frequency 10Hz
48
// The routines fid_design() or fid_parse() are used to convert
49
// this spec-string into filter coefficients and a description
56
// FidFilter *filt, *filt2;
60
// void *fbuf1, *fbuf2;
62
// void my_error_func(char *err);
64
// // Design a filter, and optionally get its long description
65
// filt= fid_design(spec, rate, freq0, freq1, adj, &desc);
67
// // List all the possible filter types
68
// fid_list_filters(stdout);
69
// okay= fid_list_filters_buf(buf, buf+sizeof(buf));
71
// // Calculate the response of the filter at a given frequency
72
// // (frequency is given as a proportion of the sampling rate, in
73
// // the range 0 to 0.5). If phase is returned, then this is
74
// // given in the range 0 to 1 (for 0 to 2*pi).
75
// resp= fid_response(filt, freq);
76
// resp= fid_response_pha(filt, freq, &phase);
78
// // Estimate the signal delay caused by a particular filter, in samples
79
// delay= fid_calc_delay(filt);
81
// // Run a given filter (this will do JIT filter compilation if this is
82
// // implemented for this processor / OS)
83
// run= fid_run_new(filt, &funcp);
84
// fbuf1= fid_run_newbuf(run);
85
// fbuf2= fid_run_newbuf(run);
87
// out_1= funcp(fbuf1, in_1);
88
// out_2= funcp(fbuf2, in_2);
89
// if (restart_required) fid_run_zapbuf(fbuf1);
92
// fid_run_freebuf(fbuf2);
93
// fid_run_freebuf(fbuf1);
96
// // If you need to allocate your own buffers separately for some
97
// // reason, then do it this way:
98
// run= fid_run_new(filt, &funcp);
99
// len= fid_run_bufsize(run);
100
// fbuf1= Alloc(len); fid_run_initbuf(run, fbuf1);
101
// fbuf2= Alloc(len); fid_run_initbuf(run, fbuf2);
103
// out_1= funcp(fbuf1, in_1);
104
// out_2= funcp(fbuf2, in_2);
105
// if (restart_required) fid_run_zapbuf(fbuf1);
110
// fid_run_free(run);
112
// // Convert an arbitrary filter into a new filter which is a single
113
// // IIR/FIR pair. This is done by convolving the coefficients. This
114
// // flattened filter will give the same result, in theory. However,
115
// // in practice this will be less accurate, especially in cases where
116
// // the limits of the floating point format are being reached (e.g.
117
// // subtracting numbers with small highly significant differences).
118
// // The routine also ensures that the IIR first coefficient is 1.0.
119
// filt2= fid_flatten(filt);
122
// // Parse an entire filter-spec string possibly containing several FIR,
123
// // IIR and predefined filters and return it as a FidFilter at the given
124
// // location. Stops at the first ,; or unmatched )]} character, or the end
125
// // of the string. Returns a strdup'd error string on error, or else 0.
126
// err= fid_parse(double rate, char **pp, FidFilter **ffp);
128
// // Set up your own fatal-error handler (default is to dump a message
129
// // to STDERR and exit on fatal conditions)
130
// fid_set_error_handler(&my_error_func);
132
// // Get the version number of the library as a string (e.g. "1.0.0")
133
// txt= fid_version();
135
// // Design a filter and reduce it to a list of all the non-const
136
// // coefficients, which is returned in the given double[]. The number
137
// // of coefficients expected must be provided (as a check).
138
// #define N_COEF <whatever>
139
// double coef[N_COEF], gain;
140
// gain= fid_design_coef(coef, N_COEF, spec, rate, freq0, freq1, adj);
142
// // Rewrite a filter spec in a full and/or separated-out form
144
// double minf0, minf1;
146
// fid_rewrite_spec(spec, freq0, freq1, adj, &full, &min, &minf0, &minf1, &minadj);
148
// free(full); free(min);
150
// // Create a FidFilter based on coefficients provided in the
151
// // given double array.
152
// static double array[]= { 'I', 3, 1.0, 0.55, 0.77, 'F', 3, 1, -2, 1, 0 };
153
// filt= fid_cv_array(array);
155
// // Join a number of filters into a single filter (and free them too,
156
// // if the first argument is 1)
157
// filt= fid_cat(0, filt1, filt2, filt3, filt4, 0);
162
// Format of returned filter
163
// -------------------------
165
// The filter returned is a single chunk of allocated memory in
166
// which is stored a number of FidFilter instances. Each
167
// instance has variable length according to the coefficients
168
// contained in it. It is probably easier to think of this as a
169
// stream of items in memory. Each sub-filter starts with its
170
// type as a short -- either 'I' for IIR filters, or 'F' for FIR
171
// filters. (Other types may be added later, e.g. AM modulation
172
// elements, or whatever). This is followed by a short bitmap
173
// which indicates which of the coefficients are constants,
174
// aiding code-generation. Next comes the count of the following
175
// coefficients, as an int. (These header fields normally takes 8
176
// bytes, the same as a double, but this might depend on the
177
// platform). Then follow the coefficients, as doubles. The next
178
// sub-filter follows on straight after that. The end of the list
179
// is marked by 8 zero bytes, meaning typ==0, cbm==0 and len==0.
181
// The filter can be read with the aid of the FidFilter structure
182
// (giving typ, cbm, len and val[] elements) and the FFNEXT()
183
// macro: using ff= FFNEXT(ff) steps to the next FidFilter
184
// structure along the chain.
186
// Note that within the sub-filters, coefficients are listed in
187
// the order that they apply to data, from current-sample
188
// backwards in time, i.e. most recent first (so an FIR val[] of
189
// 0, 0, 1 represents a two-sample delay FIR filter). IIR
190
// filters are *not* necessarily adjusted so that their first
193
// Most filters have their gain pre-adjusted so that some
194
// suitable part of the response is at gain==1.0. However, this
195
// depends on the filter type.
199
// Check that a target macro has been set. This macro selects
200
// various fixes required on various platforms:
202
// T_LINUX Linux, or probably any UNIX-like platform with GCC
203
// T_MINGW MinGW -- either building on Win32 or cross-compiling
204
// T_MSVC Microsoft Visual C
206
// (On MSVC, add "T_MSVC" to the preprocessor definitions in the
207
// project settings, or add /D "T_MSVC" to the compiler
214
#error Please define one of the T_* target macros (e.g. -DT_LINUX); see fidlib.c
221
// Select which method of filter execution is preferred.
222
// RF_CMDLIST is recommended (and is the default).
224
// RF_COMBINED -- easy to understand code, lower accuracy
225
// RF_CMDLIST -- faster pre-compiled code
226
// RF_JIT -- fastest JIT run-time generated code (no longer supported)
252
#define M_PI 3.14159265358979323846
255
extern FidFilter *mkfilter(char *, ...);
258
// Target-specific fixes
261
// Macro for local inline routines that shouldn't be visible externally
263
#define STATIC_INLINE static __inline
265
#define STATIC_INLINE static inline
268
// MinGW and MSVC fixes
269
#if defined(T_MINGW) || defined(T_MSVC)
271
#define vsnprintf _vsnprintf
274
#define snprintf _snprintf
276
// Not sure if we strictly need this still
278
my_asinh(double val) {
279
return log(val + sqrt(val*val + 1.0));
281
#define asinh(xx) my_asinh(xx)
289
static void (*error_handler)(char *err)= 0;
292
error(char *fmt, ...) {
297
vsnprintf(buf, sizeof(buf), fmt, ap); // Ignore overflow
298
buf[sizeof(buf)-1]= 0;
299
if (error_handler) error_handler(buf);
301
// If error handler routine returns, we dump to STDERR and exit anyway
302
fprintf(stderr, "fidlib error: %s\n", buf);
307
strdupf(char *fmt, ...) {
312
len= vsnprintf(buf, sizeof(buf), fmt, ap);
313
if (len < 0 || len >= sizeof(buf)-1)
314
error("strdupf exceeded buffer");
316
if (!rv) error("Out of memory");
322
void *vp= calloc(1, size);
323
if (!vp) error("Out of memory");
327
#define ALLOC(type) ((type*)Alloc(sizeof(type)))
328
#define ALLOC_ARR(cnt, type) ((type*)Alloc((cnt) * sizeof(type)))
332
// Complex multiply: aa *= bb;
336
cmul(double *aa, double *bb) {
337
double rr= aa[0] * bb[0] - aa[1] * bb[1];
338
double ii= aa[0] * bb[1] + aa[1] * bb[0];
344
// Complex square: aa *= aa;
349
double rr= aa[0] * aa[0] - aa[1] * aa[1];
350
double ii= 2 * aa[0] * aa[1];
356
// Complex multiply by real: aa *= bb;
360
cmulr(double *aa, double fact) {
366
// Complex conjugate: aa= aa*
375
// Complex divide: aa /= bb;
379
cdiv(double *aa, double *bb) {
380
double rr= aa[0] * bb[0] + aa[1] * bb[1];
381
double ii= -aa[0] * bb[1] + aa[1] * bb[0];
382
double fact= 1.0 / (bb[0] * bb[0] + bb[1] * bb[1]);
388
// Complex reciprocal: aa= 1/aa
393
double fact= 1.0 / (aa[0] * aa[0] + aa[1] * aa[1]);
399
// Complex assign: aa= bb
403
cass(double *aa, double *bb) {
404
memcpy(aa, bb, 2*sizeof(double)); // Assigning doubles is really slow
408
// Complex assign: aa= (rr + ii*j)
412
cassz(double *aa, double rr, double ii) {
418
// Complex add: aa += bb
422
cadd(double *aa, double *bb) {
428
// Complex add: aa += (rr + ii*j)
432
caddz(double *aa, double rr, double ii) {
438
// Complex subtract: aa -= bb
442
csub(double *aa, double *bb) {
448
// Complex subtract: aa -= (rr + ii*j)
452
csubz(double *aa, double rr, double ii) {
458
// Complex negate: aa= -aa
468
// Evaluate a complex polynomial given the coefficients.
469
// rv[0]+i*rv[1] is the result, in[0]+i*in[1] is the input value.
470
// Coefficients are real values.
474
evaluate(double *rv, double *coef, int n_coef, double *in) {
475
double pz[2]; // Powers of Z
477
// Handle first iteration by hand
482
// Handle second iteration by hand
485
rv[0] += *coef * pz[0];
486
rv[1] += *coef * pz[1];
489
// Loop for remainder
492
rv[0] += *coef * pz[0];
493
rv[1] += *coef * pz[1];
506
fid_set_error_handler(void (*rout)(char*)) {
517
// Get the response and phase of a filter at the given frequency
518
// (expressed as a proportion of the sampling rate, 0->0.5).
519
// Phase is returned as a number from 0 to 1, representing a
520
// phase between 0 and two-pi.
524
fid_response_pha(FidFilter *filt, double freq, double *phase) {
525
double top[2], bot[2];
526
double theta= freq * 2 * M_PI;
539
evaluate(resp, filt->val, cnt, zz);
540
if (filt->typ == 'I')
542
else if (filt->typ == 'F')
545
error("Unknown filter type %d in fid_response_pha()", filt->typ);
552
double pha= atan2(top[1], top[0]) / (2 * M_PI);
553
if (pha < 0) pha += 1.0;
557
return hypot(top[1], top[0]);
561
// Get the response of a filter at the given frequency (expressed
562
// as a proportion of the sampling rate, 0->0.5).
564
// Code duplicate, as I didn't want the overhead of a function
565
// call to fid_response_pha. Almost every call in this routine
570
fid_response(FidFilter *filt, double freq) {
571
double top[2], bot[2];
572
double theta= freq * 2 * M_PI;
585
evaluate(resp, filt->val, cnt, zz);
586
if (filt->typ == 'I')
588
else if (filt->typ == 'F')
591
error("Unknown filter type %d in fid_response()", filt->typ);
597
return hypot(top[1], top[0]);
602
// Estimate the delay that a filter causes to the signal by
603
// looking for the point at which 50% of the filter calculations
604
// are complete. This involves running test impulses through the
605
// filter several times. The estimated delay in samples is
608
// Delays longer than 8,000,000 samples are not handled well, as
609
// the code drops out at this point rather than get stuck in an
614
fid_calc_delay(FidFilter *filt) {
618
double tot, tot100, tot50;
621
run= fid_run_new(filt, &dostep);
623
// Run through to find at least the 99.9% point of filter; the r2
624
// (tot100) filter runs at 4x the speed of the other one to act as
625
// a reference point much further ahead in the impulse response.
626
f1= fid_run_newbuf(run);
627
f2= fid_run_newbuf(run);
629
tot= fabs(dostep(f1, 1.0));
630
tot100= fabs(dostep(f2, 1.0));
631
tot100 += fabs(dostep(f2, 0.0));
632
tot100 += fabs(dostep(f2, 0.0));
633
tot100 += fabs(dostep(f2, 0.0));
635
for (cnt= 1; cnt < 0x1000000; cnt++) {
636
tot += fabs(dostep(f1, 0.0));
637
tot100 += fabs(dostep(f2, 0.0));
638
tot100 += fabs(dostep(f2, 0.0));
639
tot100 += fabs(dostep(f2, 0.0));
640
tot100 += fabs(dostep(f2, 0.0));
642
if (tot/tot100 >= 0.999) break;
647
// Now find the 50% point
649
f1= fid_run_newbuf(run);
650
tot= fabs(dostep(f1, 1.0));
651
for (cnt= 0; tot < tot50; cnt++)
652
tot += fabs(dostep(f1, 0.0));
662
// 'mkfilter'-derived code
669
// Stack a number of identical filters, generating the required
670
// FidFilter* return value
674
stack_filter(int order, int n_head, int n_val, ...) {
675
FidFilter *rv= FFALLOC(n_head * order, n_val * order);
680
if (order == 0) return rv;
685
for (a= 0; a<n_head; a++) {
686
p->typ= va_arg(ap, int);
687
p->cbm= va_arg(ap, int);
688
p->len= va_arg(ap, int);
689
for (b= 0; b<p->len; b++)
690
p->val[b]= va_arg(ap, double);
696
len= ((char*)p)-((char*)q);
697
if (len != FFCSIZE(n_head-1, n_val))
698
error("Internal error; bad call to stack_filter(); length mismatch (%d,%d)",
699
len, FFCSIZE(n_head-1, n_val));
701
// Make as many additional copies as necessary
702
while (order-- > 0) {
704
p= (FidFilter*)(len + (char*)p);
707
// List is already terminated due to zeroed allocation
712
// Search for a peak between two given frequencies. It is
713
// assumed that the gradient goes upwards from 'f0' to the peak,
714
// and then down again to 'f3'. If there are any other curves,
715
// this routine will get confused and will come up with some
716
// frequency, although probably not the right one.
718
// Returns the frequency of the peak.
722
search_peak(FidFilter *ff, double f0, double f3) {
727
// Binary search, modified, taking two intermediate points. Do 20
728
// subdivisions, which should give 1/2^20 == 1e-6 accuracy compared
729
// to original range.
730
for (a= 0; a<20; a++) {
731
f1= 0.51 * f0 + 0.49 * f3;
732
f2= 0.49 * f0 + 0.51 * f3;
733
if (f1 == f2) break; // We're hitting FP limit
734
r1= fid_response(ff, f1);
735
r2= fid_response(ff, f2);
736
if (r1 > r2) // Peak is either to the left, or between f1/f2
738
else // Peak is either to the right, or between f1/f2
745
// Handle the different 'back-ends' for Bessel, Butterworth and
746
// Chebyshev filters. First argument selects between bilinear
747
// (0) and matched-Z (non-0). The BL and MZ macros makes this a
748
// bit more obvious in the code.
750
// Overall filter gain is adjusted to give the peak at 1.0. This
751
// is easy for all types except for band-pass, where a search is
752
// required to find the precise peak. This is much slower than
760
do_lowpass(int mz, double freq) {
762
lowpass(prewarp(freq));
763
if (mz) s2z_matchedZ(); else s2z_bilinear();
764
rv= z2fidfilter(1.0, ~0); // FIR is constant
765
rv->val[0]= 1.0 / fid_response(rv, 0.0);
770
do_highpass(int mz, double freq) {
772
highpass(prewarp(freq));
773
if (mz) s2z_matchedZ(); else s2z_bilinear();
774
rv= z2fidfilter(1.0, ~0); // FIR is constant
775
rv->val[0]= 1.0 / fid_response(rv, 0.5);
780
do_bandpass(int mz, double f0, double f1) {
782
bandpass(prewarp(f0), prewarp(f1));
783
if (mz) s2z_matchedZ(); else s2z_bilinear();
784
rv= z2fidfilter(1.0, ~0); // FIR is constant
785
rv->val[0]= 1.0 / fid_response(rv, search_peak(rv, f0, f1));
790
do_bandstop(int mz, double f0, double f1) {
792
bandstop(prewarp(f0), prewarp(f1));
793
if (mz) s2z_matchedZ(); else s2z_bilinear();
794
rv= z2fidfilter(1.0, 5); // FIR second coefficient is *non-const* for bandstop
795
rv->val[0]= 1.0 / fid_response(rv, 0.0); // Use 0Hz response as reference
801
// Information passed to individual filter design routines:
803
// double* rout(double rate, double f0, double f1,
804
// int order, int n_arg, double *arg);
806
// 'rate' is the sampling rate, or 1 if not set
807
// 'f0' and 'f1' give the frequency or frequency range as a
808
// proportion of the sampling rate
809
// 'order' is the order of the filter (the integer passed immediately
811
// 'n_arg' is the number of additional arguments for the filter
812
// 'arg' gives the additional argument values: arg[n]
814
// Note that #O #o #F and #R are mapped to the f0/f1/order
815
// arguments, and are not included in the arg[] array.
817
// See the previous description for the required meaning of the
818
// return value FidFilter list.
822
// Filter design routines and supporting code
826
des_bpre(double rate, double f0, double f1, int order, int n_arg, double *arg) {
827
bandpass_res(f0, arg[0]);
828
return z2fidfilter(1.0, ~0); // FIR constant
832
des_bsre(double rate, double f0, double f1, int order, int n_arg, double *arg) {
833
bandstop_res(f0, arg[0]);
834
return z2fidfilter(1.0, 0); // FIR not constant, depends on freq
838
des_apre(double rate, double f0, double f1, int order, int n_arg, double *arg) {
839
allpass_res(f0, arg[0]);
840
return z2fidfilter(1.0, 0); // FIR not constant, depends on freq
844
des_pi(double rate, double f0, double f1, int order, int n_arg, double *arg) {
845
prop_integral(prewarp(f0));
847
return z2fidfilter(1.0, 0); // FIR not constant, depends on freq
851
des_piz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
852
prop_integral(prewarp(f0));
854
return z2fidfilter(1.0, 0); // FIR not constant, depends on freq
858
des_lpbe(double rate, double f0, double f1, int order, int n_arg, double *arg) {
860
return do_lowpass(BL, f0);
864
des_hpbe(double rate, double f0, double f1, int order, int n_arg, double *arg) {
866
return do_highpass(BL, f0);
870
des_bpbe(double rate, double f0, double f1, int order, int n_arg, double *arg) {
872
return do_bandpass(BL, f0, f1);
876
des_bsbe(double rate, double f0, double f1, int order, int n_arg, double *arg) {
878
return do_bandstop(BL, f0, f1);
882
des_lpbez(double rate, double f0, double f1, int order, int n_arg, double *arg) {
884
return do_lowpass(MZ, f0);
888
des_hpbez(double rate, double f0, double f1, int order, int n_arg, double *arg) {
890
return do_highpass(MZ, f0);
894
des_bpbez(double rate, double f0, double f1, int order, int n_arg, double *arg) {
896
return do_bandpass(MZ, f0, f1);
900
des_bsbez(double rate, double f0, double f1, int order, int n_arg, double *arg) {
902
return do_bandstop(MZ, f0, f1);
905
static FidFilter* // Butterworth-Bessel cross
906
des_lpbube(double rate, double f0, double f1, int order, int n_arg, double *arg) {
909
bessel(order); memcpy(tmp, pol, order * sizeof(double));
911
for (a= 0; a<order; a++) pol[a] += (tmp[a]-pol[a]) * 0.01 * arg[0];
912
//for (a= 1; a<order; a+=2) pol[a] += arg[1] * 0.01;
913
return do_lowpass(BL, f0);
917
des_lpbu(double rate, double f0, double f1, int order, int n_arg, double *arg) {
919
return do_lowpass(BL, f0);
923
des_hpbu(double rate, double f0, double f1, int order, int n_arg, double *arg) {
925
return do_highpass(BL, f0);
929
des_bpbu(double rate, double f0, double f1, int order, int n_arg, double *arg) {
931
return do_bandpass(BL, f0, f1);
935
des_bsbu(double rate, double f0, double f1, int order, int n_arg, double *arg) {
937
return do_bandstop(BL, f0, f1);
941
des_lpbuz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
943
return do_lowpass(MZ, f0);
947
des_hpbuz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
949
return do_highpass(MZ, f0);
953
des_bpbuz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
955
return do_bandpass(MZ, f0, f1);
959
des_bsbuz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
961
return do_bandstop(MZ, f0, f1);
965
des_lpch(double rate, double f0, double f1, int order, int n_arg, double *arg) {
966
chebyshev(order, arg[0]);
967
return do_lowpass(BL, f0);
971
des_hpch(double rate, double f0, double f1, int order, int n_arg, double *arg) {
972
chebyshev(order, arg[0]);
973
return do_highpass(BL, f0);
977
des_bpch(double rate, double f0, double f1, int order, int n_arg, double *arg) {
978
chebyshev(order, arg[0]);
979
return do_bandpass(BL, f0, f1);
983
des_bsch(double rate, double f0, double f1, int order, int n_arg, double *arg) {
984
chebyshev(order, arg[0]);
985
return do_bandstop(BL, f0, f1);
989
des_lpchz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
990
chebyshev(order, arg[0]);
991
return do_lowpass(MZ, f0);
995
des_hpchz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
996
chebyshev(order, arg[0]);
997
return do_highpass(MZ, f0);
1001
des_bpchz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1002
chebyshev(order, arg[0]);
1003
return do_bandpass(MZ, f0, f1);
1007
des_bschz(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1008
chebyshev(order, arg[0]);
1009
return do_bandstop(MZ, f0, f1);
1013
des_lpbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1014
double omega= 2 * M_PI * f0;
1015
double cosv= cos(omega);
1016
double alpha= sin(omega) / 2 / arg[0];
1017
return stack_filter(order, 3, 7,
1018
'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1019
'F', 0x7, 3, 1.0, 2.0, 1.0,
1020
'F', 0x0, 1, (1-cosv) * 0.5);
1024
des_hpbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1025
double omega= 2 * M_PI * f0;
1026
double cosv= cos(omega);
1027
double alpha= sin(omega) / 2 / arg[0];
1028
return stack_filter(order, 3, 7,
1029
'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1030
'F', 0x7, 3, 1.0, -2.0, 1.0,
1031
'F', 0x0, 1, (1+cosv) * 0.5);
1035
des_bpbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1036
double omega= 2 * M_PI * f0;
1037
double cosv= cos(omega);
1038
double alpha= sin(omega) / 2 / arg[0];
1039
return stack_filter(order, 3, 7,
1040
'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1041
'F', 0x7, 3, 1.0, 0.0, -1.0,
1042
'F', 0x0, 1, alpha);
1046
des_bsbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1047
double omega= 2 * M_PI * f0;
1048
double cosv= cos(omega);
1049
double alpha= sin(omega) / 2 / arg[0];
1050
return stack_filter(order, 2, 6,
1051
'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1052
'F', 0x5, 3, 1.0, -2 * cosv, 1.0);
1056
des_apbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1057
double omega= 2 * M_PI * f0;
1058
double cosv= cos(omega);
1059
double alpha= sin(omega) / 2 / arg[0];
1060
return stack_filter(order, 2, 6,
1061
'I', 0x0, 3, 1 + alpha, -2 * cosv, 1 - alpha,
1062
'F', 0x0, 3, 1 - alpha, -2 * cosv, 1 + alpha);
1066
des_pkbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1067
double omega= 2 * M_PI * f0;
1068
double cosv= cos(omega);
1069
double alpha= sin(omega) / 2 / arg[0];
1070
double A= pow(10, arg[1]/40);
1071
return stack_filter(order, 2, 6,
1072
'I', 0x0, 3, 1 + alpha/A, -2 * cosv, 1 - alpha/A,
1073
'F', 0x0, 3, 1 + alpha*A, -2 * cosv, 1 - alpha*A);
1077
des_lsbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1078
double omega= 2 * M_PI * f0;
1079
double cosv= cos(omega);
1080
double sinv= sin(omega);
1081
double A= pow(10, arg[1]/40);
1082
double beta= sqrt((A*A+1)/arg[0] - (A-1)*(A-1));
1083
return stack_filter(order, 2, 6,
1085
(A+1) + (A-1)*cosv + beta*sinv,
1086
-2 * ((A-1) + (A+1)*cosv),
1087
(A+1) + (A-1)*cosv - beta*sinv,
1089
A*((A+1) - (A-1)*cosv + beta*sinv),
1090
2*A*((A-1) - (A+1)*cosv),
1091
A*((A+1) - (A-1)*cosv - beta*sinv));
1095
des_hsbq(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1096
double omega= 2 * M_PI * f0;
1097
double cosv= cos(omega);
1098
double sinv= sin(omega);
1099
double A= pow(10, arg[1]/40);
1100
double beta= sqrt((A*A+1)/arg[0] - (A-1)*(A-1));
1101
return stack_filter(order, 2, 6,
1103
(A+1) - (A-1)*cosv + beta*sinv,
1104
2 * ((A-1) - (A+1)*cosv),
1105
(A+1) - (A-1)*cosv - beta*sinv,
1107
A*((A+1) + (A-1)*cosv + beta*sinv),
1108
-2*A*((A-1) + (A+1)*cosv),
1109
A*((A+1) + (A-1)*cosv - beta*sinv));
1113
des_lpbl(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1114
double wid= 0.4109205/f0;
1116
int max= (int)floor(wid);
1118
FidFilter *ff= (FidFilter*)Alloc(FFCSIZE(1, max*2+1));
1122
ff->val[max]= tot= 1.0;
1123
for (a= 1; a<=max; a++) {
1125
0.5 * cos(M_PI * a / wid) +
1126
0.08 * cos(M_PI * 2.0 * a / wid);
1127
ff->val[max-a]= val;
1128
ff->val[max+a]= val;
1132
for (a= 0; a<=max*2; a++) ff->val[a] *= adj;
1137
des_lphm(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1138
double wid= 0.3262096/f0;
1140
int max= (int)floor(wid);
1142
FidFilter *ff= (FidFilter*)Alloc(FFCSIZE(1, max*2+1));
1146
ff->val[max]= tot= 1.0;
1147
for (a= 1; a<=max; a++) {
1149
0.46 * cos(M_PI * a / wid);
1150
ff->val[max-a]= val;
1151
ff->val[max+a]= val;
1155
for (a= 0; a<=max*2; a++) ff->val[a] *= adj;
1160
des_lphn(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1161
double wid= 0.360144/f0;
1163
int max= (int)floor(wid);
1165
FidFilter *ff= (FidFilter*)Alloc(FFCSIZE(1, max*2+1));
1169
ff->val[max]= tot= 1.0;
1170
for (a= 1; a<=max; a++) {
1172
0.5 * cos(M_PI * a / wid);
1173
ff->val[max-a]= val;
1174
ff->val[max+a]= val;
1178
for (a= 0; a<=max*2; a++) ff->val[a] *= adj;
1183
des_lpba(double rate, double f0, double f1, int order, int n_arg, double *arg) {
1184
double wid= 0.3189435/f0;
1186
int max= (int)floor(wid);
1188
FidFilter *ff= (FidFilter*)Alloc(FFCSIZE(1, max*2+1));
1192
ff->val[max]= tot= 1.0;
1193
for (a= 1; a<=max; a++) {
1194
double val= 1.0 - a/wid;
1195
ff->val[max-a]= val;
1196
ff->val[max+a]= val;
1200
for (a= 0; a<=max*2; a++) ff->val[a] *= adj;
1210
FidFilter *(*rout)(double,double,double,int,int,double*); // Designer routine address
1211
char *fmt; // Format for spec-string
1212
char *txt; // Human-readable description of filter
1214
{ des_bpre, "BpRe/#V/#F",
1215
"Bandpass resonator, Q=#V (0 means Inf), frequency #F" },
1216
{ des_bsre, "BsRe/#V/#F",
1217
"Bandstop resonator, Q=#V (0 means Inf), frequency #F" },
1218
{ des_apre, "ApRe/#V/#F",
1219
"Allpass resonator, Q=#V (0 means Inf), frequency #F" },
1221
"Proportional-integral filter, frequency #F" },
1222
{ des_piz, "PiZ/#F",
1223
"Proportional-integral filter, matched z-transform, frequency #F" },
1224
{ des_lpbe, "LpBe#O/#F",
1225
"Lowpass Bessel filter, order #O, -3.01dB frequency #F" },
1226
{ des_hpbe, "HpBe#O/#F",
1227
"Highpass Bessel filter, order #O, -3.01dB frequency #F" },
1228
{ des_bpbe, "BpBe#O/#R",
1229
"Bandpass Bessel filter, order #O, -3.01dB frequencies #R" },
1230
{ des_bsbe, "BsBe#O/#R",
1231
"Bandstop Bessel filter, order #O, -3.01dB frequencies #R" },
1232
{ des_lpbu, "LpBu#O/#F",
1233
"Lowpass Butterworth filter, order #O, -3.01dB frequency #F" },
1234
{ des_hpbu, "HpBu#O/#F",
1235
"Highpass Butterworth filter, order #O, -3.01dB frequency #F" },
1236
{ des_bpbu, "BpBu#O/#R",
1237
"Bandpass Butterworth filter, order #O, -3.01dB frequencies #R" },
1238
{ des_bsbu, "BsBu#O/#R",
1239
"Bandstop Butterworth filter, order #O, -3.01dB frequencies #R" },
1240
{ des_lpch, "LpCh#O/#V/#F",
1241
"Lowpass Chebyshev filter, order #O, passband ripple #VdB, -3.01dB frequency #F" },
1242
{ des_hpch, "HpCh#O/#V/#F",
1243
"Highpass Chebyshev filter, order #O, passband ripple #VdB, -3.01dB frequency #F" },
1244
{ des_bpch, "BpCh#O/#V/#R",
1245
"Bandpass Chebyshev filter, order #O, passband ripple #VdB, -3.01dB frequencies #R" },
1246
{ des_bsch, "BsCh#O/#V/#R",
1247
"Bandstop Chebyshev filter, order #O, passband ripple #VdB, -3.01dB frequencies #R" },
1248
{ des_lpbez, "LpBeZ#O/#F",
1249
"Lowpass Bessel filter, matched z-transform, order #O, -3.01dB frequency #F" },
1250
{ des_hpbez, "HpBeZ#O/#F",
1251
"Highpass Bessel filter, matched z-transform, order #O, -3.01dB frequency #F" },
1252
{ des_bpbez, "BpBeZ#O/#R",
1253
"Bandpass Bessel filter, matched z-transform, order #O, -3.01dB frequencies #R" },
1254
{ des_bsbez, "BsBeZ#O/#R",
1255
"Bandstop Bessel filter, matched z-transform, order #O, -3.01dB frequencies #R" },
1256
{ des_lpbuz, "LpBuZ#O/#F",
1257
"Lowpass Butterworth filter, matched z-transform, order #O, -3.01dB frequency #F" },
1258
{ des_hpbuz, "HpBuZ#O/#F",
1259
"Highpass Butterworth filter, matched z-transform, order #O, -3.01dB frequency #F" },
1260
{ des_bpbuz, "BpBuZ#O/#R",
1261
"Bandpass Butterworth filter, matched z-transform, order #O, -3.01dB frequencies #R" },
1262
{ des_bsbuz, "BsBuZ#O/#R",
1263
"Bandstop Butterworth filter, matched z-transform, order #O, -3.01dB frequencies #R" },
1264
{ des_lpchz, "LpChZ#O/#V/#F",
1265
"Lowpass Chebyshev filter, matched z-transform, order #O, "
1266
"passband ripple #VdB, -3.01dB frequency #F" },
1267
{ des_hpchz, "HpChZ#O/#V/#F",
1268
"Highpass Chebyshev filter, matched z-transform, order #O, "
1269
"passband ripple #VdB, -3.01dB frequency #F" },
1270
{ des_bpchz, "BpChZ#O/#V/#R",
1271
"Bandpass Chebyshev filter, matched z-transform, order #O, "
1272
"passband ripple #VdB, -3.01dB frequencies #R" },
1273
{ des_bschz, "BsChZ#O/#V/#R",
1274
"Bandstop Chebyshev filter, matched z-transform, order #O, "
1275
"passband ripple #VdB, -3.01dB frequencies #R" },
1276
{ des_lpbube, "LpBuBe#O/#V/#F",
1277
"Lowpass Butterworth-Bessel #V% cross, order #O, -3.01dB frequency #F" },
1278
{ des_lpbq, "LpBq#o/#V/#F",
1279
"Lowpass biquad filter, order #O, Q=#V, -3.01dB frequency #F" },
1280
{ des_hpbq, "HpBq#o/#V/#F",
1281
"Highpass biquad filter, order #O, Q=#V, -3.01dB frequency #F" },
1282
{ des_bpbq, "BpBq#o/#V/#F",
1283
"Bandpass biquad filter, order #O, Q=#V, centre frequency #F" },
1284
{ des_bsbq, "BsBq#o/#V/#F",
1285
"Bandstop biquad filter, order #O, Q=#V, centre frequency #F" },
1286
{ des_apbq, "ApBq#o/#V/#F",
1287
"Allpass biquad filter, order #O, Q=#V, centre frequency #F" },
1288
{ des_pkbq, "PkBq#o/#V/#V/#F",
1289
"Peaking biquad filter, order #O, Q=#V, dBgain=#V, frequency #F" },
1290
{ des_lsbq, "LsBq#o/#V/#V/#F",
1291
"Lowpass shelving biquad filter, S=#V, dBgain=#V, frequency #F" },
1292
{ des_hsbq, "HsBq#o/#V/#V/#F",
1293
"Highpass shelving biquad filter, S=#V, dBgain=#V, frequency #F" },
1294
{ des_lpbl, "LpBl/#F",
1295
"Lowpass Blackman window, -3.01dB frequency #F" },
1296
{ des_lphm, "LpHm/#F",
1297
"Lowpass Hamming window, -3.01dB frequency #F" },
1298
{ des_lphn, "LpHn/#F",
1299
"Lowpass Hann window, -3.01dB frequency #F" },
1300
{ des_lpba, "LpBa/#F",
1301
"Lowpass Bartlet (triangular) window, -3.01dB frequency #F" },
1306
// Design a filter. Spec and range are passed as arguments. The
1307
// return value is a pointer to a FidFilter as documented earlier
1308
// in this file. This needs to be free()d once finished with.
1310
// If 'f_adj' is set, then the frequencies fed to the design code
1311
// are adjusted automatically to get true sqrt(0.5) (-3.01dB)
1312
// values at the provided frequencies. (This is obviously a
1313
// slower operation)
1315
// If 'descp' is non-0, then a long description of the filter is
1316
// generated and returned as a strdup'd string at the given
1319
// Any problem with the spec causes the program to die with an
1322
// 'spec' gives the specification string. The 'rate' argument
1323
// gives the sampling rate for the data that will be passed to
1324
// the filter. This is only used to interpret the frequencies
1325
// given in the spec or given in 'freq0' and 'freq1'. Use 1.0 if
1326
// the frequencies are given as a proportion of the sampling
1327
// rate, in the range 0 to 0.5. 'freq0' and 'freq1' provide the
1328
// default frequency or frequency range if this is not included
1329
// in the specification string. These should be -ve if there is
1330
// no default range (causing an error if they are omitted from
1334
typedef struct Spec Spec;
1335
static char* parse_spec(Spec*);
1336
static FidFilter *auto_adjust_single(Spec *sp, double rate, double f0);
1337
static FidFilter *auto_adjust_dual(Spec *sp, double rate, double f0, double f1);
1341
double in_f0, in_f1;
1343
double argarr[MAXARG];
1348
int minlen; // Minimum length of spec-string, assuming f0/f1 passed separately
1349
int n_freq; // Number of frequencies provided: 0,1,2
1350
int fi; // Filter index (filter[fi])
1354
fid_design(const char *spec, double rate, double freq0, double freq1, int f_adj, char **descp) {
1360
// Parse the filter-spec
1365
err= parse_spec(&sp);
1366
if (err) error("%s", err);
1370
// Adjust frequencies to range 0-0.5, and check them
1372
if (f0 > 0.5) error("Frequency of %gHz out of range with sampling rate of %gHz", f0*rate, rate);
1374
if (f1 > 0.5) error("Frequency of %gHz out of range with sampling rate of %gHz", f1*rate, rate);
1376
// Okay we now have a successful spec-match to filter[sp.fi], and sp.n_arg
1377
// args are now in sp.argarr[]
1379
// Generate the filter
1381
rv= filter[sp.fi].rout(rate, f0, f1, sp.order, sp.n_arg, sp.argarr);
1382
else if (strstr(filter[sp.fi].fmt, "#R"))
1383
rv= auto_adjust_dual(&sp, rate, f0, f1);
1385
rv= auto_adjust_single(&sp, rate, f0);
1387
// Generate a long description if required
1389
char *fmt= filter[sp.fi].txt;
1390
int max= strlen(fmt) + 60 + sp.n_arg * 20;
1391
char *desc= (char*)Alloc(max);
1394
double *arg= sp.argarr;
1395
int n_arg= sp.n_arg;
1397
while ((ch= *fmt++)) {
1405
p += sprintf(p, "%d", sp.order);
1408
p += sprintf(p, "%g", f0*rate);
1411
p += sprintf(p, "%g-%g", f0*rate, f1*rate);
1415
error("Internal error -- disagreement between filter short-spec\n"
1416
" and long-description over number of arguments");
1418
p += sprintf(p, "%g", *arg++);
1421
error("Internal error: unknown format in long description: #%c", fmt[-1]);
1425
if (p-desc >= max) error("Internal error: exceeded estimated description buffer");
1433
// Auto-adjust input frequency to give correct sqrt(0.5)
1434
// (~-3.01dB) point to 6 figures
1437
#define M301DB (0.707106781186548)
1440
auto_adjust_single(Spec *sp, double rate, double f0) {
1442
FidFilter *(*design)(double,double,double,int,int,double*)= filter[sp->fi].rout;
1446
int incr; // Increasing (1) or decreasing (0)
1449
#define DESIGN(aa) design(rate, aa, aa, sp->order, sp->n_arg, sp->argarr)
1450
#define TEST(aa) { if (rv) {free(rv);rv= 0;} rv= DESIGN(aa); resp= fid_response(rv, f0); }
1452
// Try and establish a range within which we can find the point
1453
a0= f0; TEST(a0); r0= resp;
1454
for (a= 2; 1; a*=2) {
1455
a2= f0/a; TEST(a2); r2= resp;
1456
if ((r0 < M301DB) != (r2 < M301DB)) break;
1457
a2= 0.5-((0.5-f0)/a); TEST(a2); r2= resp;
1458
if ((r0 < M301DB) != (r2 < M301DB)) break;
1459
if (a == 32) // No success
1460
error("auto_adjust_single internal error -- can't establish enclosing range");
1465
a1= a0; a0= a2; a2= a1;
1471
a1= 0.5 * (a0 + a2);
1472
if (a1 == a0 || a1 == a2) break; // Limit of double, sanity check
1474
if (resp >= 0.9999995 * M301DB && resp < 1.0000005 * M301DB) break;
1475
if (incr == (resp > M301DB))
1489
// Auto-adjust input frequencies to give response of sqrt(0.5)
1490
// (~-3.01dB) correct to 6sf at the given frequency-points
1494
auto_adjust_dual(Spec *sp, double rate, double f0, double f1) {
1495
double mid= 0.5 * (f0+f1);
1496
double wid= 0.5 * fabs(f1-f0);
1497
FidFilter *(*design)(double,double,double,int,int,double*)= filter[sp->fi].rout;
1503
double r0, r1, err0, err1;
1508
#define DESIGN(mm,ww) { if (rv) {free(rv);rv= 0;} \
1509
rv= design(rate, mm-ww, mm+ww, sp->order, sp->n_arg, sp->argarr); \
1510
r0= fid_response(rv, f0); r1= fid_response(rv, f1); \
1511
err0= fabs(M301DB-r0); err1= fabs(M301DB-r1); cnt_design++; }
1513
#define INC_WID ((r0+r1 < 1.0) == bpass)
1514
#define INC_MID ((r0 > r1) == bpass)
1515
#define MATCH (err0 < 0.000000499 && err1 < 0.000000499)
1516
#define PERR (err0+err1)
1519
bpass= (fid_response(rv, 0) < 0.5);
1522
// Try delta changes until we get there
1523
for (cnt= 0; 1; cnt++, delta *= 0.51) {
1524
DESIGN(mid, wid); // I know -- this is redundant
1529
mid1= mid + (INC_MID ? delta : -delta);
1530
wid1= wid + (INC_WID ? delta : -delta);
1532
if (mid0 - wid1 > 0.0 && mid0 + wid1 < 0.5) {
1535
if (PERR < perr) { perr= PERR; mid= mid0; wid= wid1; }
1538
if (mid1 - wid0 > 0.0 && mid1 + wid0 < 0.5) {
1541
if (PERR < perr) { perr= PERR; mid= mid1; wid= wid0; }
1544
if (mid1 - wid1 > 0.0 && mid1 + wid1 < 0.5) {
1547
if (PERR < perr) { perr= PERR; mid= mid1; wid= wid1; }
1551
error("auto_adjust_dual -- design not converging");
1565
// Expand a specification string to the given buffer; if out of
1566
// space, drops dead
1570
expand_spec(char *buf, char *bufend, char *str) {
1574
while ((ch= *str++)) {
1575
if (p + 10 >= bufend)
1576
error("Buffer overflow in fidlib expand_spec()");
1579
case 'o': p += sprintf(p, "<optional-order>"); break;
1580
case 'O': p += sprintf(p, "<order>"); break;
1581
case 'F': p += sprintf(p, "<freq>"); break;
1582
case 'R': p += sprintf(p, "<range>"); break;
1583
case 'V': p += sprintf(p, "<value>"); break;
1584
default: p += sprintf(p, "<%c>", str[-1]); break;
1594
// Design a filter and reduce it to a list of all the non-const
1595
// coefficients. Arguments are as for fid_filter(). The
1596
// coefficients are written into the given double array. If the
1597
// number of coefficients doesn't match the array length given,
1598
// then a fatal error is generated.
1600
// Note that all 1-element FIRs and IIR first-coefficients are
1601
// merged into a single gain coefficient, which is returned
1602
// rather than being included in the coefficient list. This is
1603
// to allow it to be merged with other gains within a stack of
1606
// The algorithm used here (merging 1-element FIRs and adjusting
1607
// IIR first-coefficients) must match that used in the code-
1608
// generating code, or else the coefficients won't match up. The
1609
// 'n_coef' argument provides a partial safeguard.
1613
fid_design_coef(double *coef, int n_coef, const char *spec, double rate,
1614
double freq0, double freq1, int adj) {
1615
FidFilter *filt= fid_design(spec, rate, freq0, freq1, adj, 0);
1616
FidFilter *ff= filt;
1620
double *iir, *fir, iir_adj;
1621
static double const_one= 1;
1623
int iir_cbm, fir_cbm;
1626
if (ff->typ == 'F' && ff->len == 1) {
1632
if (ff->typ != 'I' && ff->typ != 'F')
1633
error("fid_design_coef can't handle FidFilter type: %c", ff->typ);
1635
// Initialise to safe defaults
1636
iir= fir= &const_one;
1638
iir_cbm= fir_cbm= ~0;
1640
// See if we have an IIR filter
1641
if (ff->typ == 'I') {
1645
iir_adj= 1.0 / ff->val[0];
1650
// See if we have an FIR filter
1651
if (ff->typ == 'F') {
1658
// Dump out all non-const coefficients in reverse order
1659
len= n_fir > n_iir ? n_fir : n_iir;
1660
for (a= len-1; a>=0; a--) {
1661
// Output IIR if present and non-const
1662
if (a < n_iir && a>0 &&
1663
!(iir_cbm & (1<<(a<15?a:15)))) {
1664
if (cnt++ < n_coef) *coef++= iir_adj * iir[a];
1667
// Output FIR if present and non-const
1669
!(fir_cbm & (1<<(a<15?a:15)))) {
1670
if (cnt++ < n_coef) *coef++= fir[a];
1676
error("fid_design_coef called with the wrong number of coefficients.\n"
1677
" Given %d, expecting %d: (\"%s\",%g,%g,%g,%d)",
1678
n_coef, cnt, spec, rate, freq0, freq1, adj);
1685
// List all the known filters to the given file handle
1689
fid_list_filters(FILE *out) {
1692
for (a= 0; filter[a].fmt; a++) {
1694
expand_spec(buf, buf+sizeof(buf), filter[a].fmt);
1695
fprintf(out, "%s\n ", buf);
1696
expand_spec(buf, buf+sizeof(buf), filter[a].txt);
1697
fprintf(out, "%s\n", buf);
1702
// List all the known filters to the given buffer; the buffer is
1703
// NUL-terminated; returns 1 okay, 0 not enough space
1707
fid_list_filters_buf(char *buf, char *bufend) {
1711
for (a= 0; filter[a].fmt; a++) {
1712
expand_spec(tmp, tmp+sizeof(tmp), filter[a].fmt);
1713
buf += (cnt= snprintf(buf, bufend-buf, "%s\n ", tmp));
1714
if (cnt < 0 || buf >= bufend) return 0;
1715
expand_spec(tmp, tmp+sizeof(tmp), filter[a].txt);
1716
buf += (cnt= snprintf(buf, bufend-buf, "%s\n", tmp));
1717
if (cnt < 0 || buf >= bufend) return 0;
1723
// Do a convolution of parameters in place
1727
convolve(double *dst, int n_dst, double *src, int n_src) {
1728
int len= n_dst + n_src - 1;
1731
for (a= len-1; a>=0; a--) {
1733
for (b= 0; b<n_src; b++)
1734
if (a-b >= 0 && a-b < n_dst)
1735
val += src[b] * dst[a-b];
1743
// Generate a combined filter -- merge all the IIR/FIR
1744
// sub-filters into a single IIR/FIR pair, and make sure the IIR
1745
// first coefficient is 1.0.
1749
fid_flatten(FidFilter *filt) {
1750
int m_fir= 1; // Maximum values
1752
int n_fir, n_iir; // Stored counts during convolution
1759
// Find the size of the output filter
1764
else if (ff->typ == 'F')
1767
error("fid_flatten doesn't know about type %d", ff->typ);
1771
// Setup the output array
1772
rv= FFALLOC(2, m_iir + m_fir);
1781
iir[0]= 1.0; n_iir= 1;
1782
fir[0]= 1.0; n_fir= 1;
1784
// Do the convolution
1788
n_iir= convolve(iir, n_iir, ff->val, ff->len);
1790
n_fir= convolve(fir, n_fir, ff->val, ff->len);
1795
if (n_iir != m_iir ||
1797
error("Internal error in fid_combine() -- array under/overflow");
1801
for (a= 0; a<n_iir; a++) iir[a] *= adj;
1802
for (a= 0; a<n_fir; a++) fir[a] *= adj;
1808
// Parse a filter-spec and freq0/freq1 arguments. Returns a
1809
// strdup'd error string on error, or else 0.
1813
parse_spec(Spec *sp) {
1826
for (a= 0; 1; a++) {
1827
char *fmt= filter[a].fmt;
1831
if (!fmt) return strdupf("Spec-string \"%s\" matches no known format", sp->spec);
1833
while (*p && (ch= *fmt++)) {
1835
if (ch == *p++) continue;
1839
if (isalpha(*p)) { p= 0; break; }
1841
// Handling a format character
1842
switch (ch= *fmt++) {
1844
return strdupf("Internal error: Unknown format #%c in format: %s",
1845
fmt[-1], filter[a].fmt);
1848
sp->order= (int)strtol(p, &q, 10);
1850
if (ch == 'O') goto bad;
1854
return strdupf("Bad order %d in spec-string \"%s\"", sp->order, sp->spec);
1858
*arg++= strtod(p, &q);
1859
if (p == q) goto bad;
1862
sp->minlen= p-1-sp->spec;
1864
sp->adj= (p[0] == '=');
1866
sp->f0= strtod(p, &q);
1868
if (p == q) goto bad;
1871
sp->minlen= p-1-sp->spec;
1873
sp->adj= (p[0] == '=');
1875
sp->f0= strtod(p, &q);
1876
if (p == q) goto bad;
1878
if (*p++ != '-') goto bad;
1879
sp->f1= strtod(p, &q);
1880
if (p == q) goto bad;
1881
if (sp->f0 > sp->f1)
1882
return strdupf("Backwards frequency range in spec-string \"%s\"", sp->spec);
1887
if (p == 0) continue;
1889
if (fmt[0] == '/' && fmt[1] == '#' && fmt[2] == 'F') {
1890
sp->minlen= p-sp->spec;
1892
if (sp->in_f0 < 0.0)
1893
return strdupf("Frequency omitted from filter-spec, and no default provided");
1896
sp->adj= sp->in_adj;
1898
} else if (fmt[0] == '/' && fmt[1] == '#' && fmt[2] == 'R') {
1899
sp->minlen= p-sp->spec;
1901
if (sp->in_f0 < 0.0 || sp->in_f1 < 0.0)
1902
return strdupf("Frequency omitted from filter-spec, and no default provided");
1905
sp->adj= sp->in_adj;
1909
// Check for trailing unmatched format characters
1912
return strdupf("Bad match of spec-string \"%s\" to format \"%s\"",
1913
sp->spec, filter[a].fmt);
1915
if (sp->n_arg > MAXARG)
1916
return strdupf("Internal error -- maximum arguments exceeded");
1918
// Set the minlen to the whole string if unset
1919
if (sp->minlen < 0) sp->minlen= p-sp->spec;
1921
// Save values, return
1930
// Parse a filter-spec and freq0/freq1 arguments and rewrite them
1931
// to give an all-in-one filter spec and/or a minimum spec plus
1932
// separate freq0/freq1 arguments. The all-in-one spec is
1933
// returned in *spec1p (strdup'd), and the minimum separated-out
1934
// spec is returned in *spec2p (strdup'd), *freq0p and *freq1p.
1935
// If either of spec1p or spec2p is 0, then that particular
1936
// spec-string is not generated.
1940
fid_rewrite_spec(const char *spec, double freq0, double freq1, int adj,
1942
char **spec2p, double *freq0p, double *freq1p, int *adjp) {
1949
err= parse_spec(&sp);
1950
if (err) error("%s", err);
1956
switch (sp.n_freq) {
1957
case 1: sprintf(buf, "/%s%.15g", sp.adj ? "=" : "", sp.f0); break;
1958
case 2: sprintf(buf, "/%s%.15g-%.15g", sp.adj ? "=" : "", sp.f0, sp.f1); break;
1962
rv= (char*)Alloc(sp.minlen + len + 1);
1963
memcpy(rv, spec, sp.minlen);
1964
strcpy(rv+sp.minlen, buf);
1969
char *rv= (char*)Alloc(sp.minlen + 1);
1970
memcpy(rv, spec, sp.minlen);
1979
// Create a FidFilter from the given double array. The double[]
1980
// should contain one or more sections, each starting with the
1981
// filter type (either 'I' or 'F', as a double), then a count of
1982
// the number of coefficients following, then the coefficients
1983
// themselves. The end of the list is marked with a type of 0.
1985
// This is really just a convenience function, allowing a filter
1986
// to be conveniently dumped to C source code and then
1989
// Note that for more general filter generation, FidFilter
1990
// instances can be created simply by allocating the memory and
1991
// filling them in (see fidlib.h).
1995
fid_cv_array(double *arr) {
2001
// Scan through for sizes
2002
for (dp= arr; *dp; ) {
2006
if (typ != 'F' && typ != 'I')
2007
error("Bad type in array passed to fid_cv_array: %g", dp[-1]);
2011
error("Bad length in array passed to fid_cv_array: %g", dp[-1]);
2018
rv= ff= (FidFilter*)Alloc(FFCSIZE(n_head, n_val));
2020
// Scan through to fill in FidFilter
2021
for (dp= arr; *dp; ) {
2029
memcpy(ff->val, dp, len * sizeof(double));
2034
// Final element already zero'd thanks to allocation
2040
// Create a single filter from the given list of filters in
2041
// order. If 'freeme' is set, then all the listed filters are
2042
// free'd once read; otherwise they are left untouched. The
2043
// newly allocated resultant filter is returned, which should be
2044
// released with free() when finished with.
2048
fid_cat(int freeme, ...) {
2050
FidFilter *rv, *ff, *ff0;
2055
// Find the memory required to store the combined filter
2056
va_start(ap, freeme);
2057
while ((ff0= va_arg(ap, FidFilter*))) {
2058
for (ff= ff0; ff->typ; ff= FFNEXT(ff))
2060
len += ((char*)ff) - ((char*)ff0);
2064
rv= (FidFilter*)Alloc(FFCSIZE(0,0) + len);
2067
va_start(ap, freeme);
2068
while ((ff0= va_arg(ap, FidFilter*))) {
2069
for (ff= ff0; ff->typ; ff= FFNEXT(ff))
2071
cnt= ((char*)ff) - ((char*)ff0);
2072
memcpy(dst, ff0, cnt);
2074
if (freeme) free(ff0);
2078
// Final element already zero'd
2083
// Support for fid_parse
2086
// Skip white space (including comments)
2092
if (isspace(*p)) { p++; continue; }
2094
while (*p && *p != '\n') p++;
2102
// Grab a word from the input into the given buffer. Returns 0: end
2103
// of file or error, else 1: success. Error is indicated when the
2104
// word doesn't fit in the buffer.
2106
grabWord(char **pp, char *buf, int buflen) {
2115
if (*q == ',' || *q == ';' || *q == ')' || *q == ']' || *q == '}') {
2118
while (*q && *q != '#' && !isspace(*q) &&
2119
(*q != ',' && *q != ';' && *q != ')' && *q != ']' && *q != '}'))
2123
if (len >= buflen) return 0;
2125
memcpy(buf, p, len);
2133
// Parse an entire filter specification, perhaps consisting of
2134
// several FIR, IIR and predefined filters. Stops at the first
2135
// ,; or unmatched )]}. Returns either 0 on success, or else a
2136
// strdup'd error string.
2138
// This duplicates code from Fiview filter.c, I know, but this
2139
// may have to expand in the future to handle '+' operations, and
2140
// special filter types like tunable heterodyne filters. At that
2141
// point, the filter.c code will have to be modified to call a
2142
// version of this routine.
2146
fid_parse(double rate, char **pp, FidFilter **ffp) {
2149
#define INIT_LEN 128
2150
char *rv= (char*)Alloc(INIT_LEN);
2151
char *rvend= rv + INIT_LEN;
2156
int xtra= FFCSIZE(0,0);
2157
int typ= -1; // First time through
2161
#define ERR(ptr, msg) { *pp= ptr; *ffp= 0; return msg; }
2162
#define INCBUF { tmp= (char*)realloc(rv, (rvend-rv) * 2); if (!tmp) error("Out of memory"); \
2163
rvend= (rvend-rv) * 2 + tmp; rvp= (rvp-rv) + tmp; \
2164
curr= (FidFilter*)(((char*)curr) - rv + tmp); rv= tmp; }
2168
if (!grabWord(&p, buf, sizeof(buf))) {
2169
if (*p) ERR(p, strdupf("Filter element unexpectedly long -- syntax error?"));
2172
if (!buf[0] || !buf[1]) switch (buf[0]) {
2181
// End of filter, return it
2182
tmp= (char*)realloc(rv, (rvp-rv) + xtra);
2183
if (!tmp) error("Out of memory");
2184
curr= (FidFilter*)((rvp-rv) + tmp);
2185
curr->typ= 0; curr->cbm= 0; curr->len= 0;
2186
*pp= buf[0] ? (p-1) : p;
2187
*ffp= (FidFilter*)tmp;
2190
if (typ > 0) ERR(rew, strdupf("Filter syntax error; unexpected '/'"));
2194
if (typ > 0) ERR(rew, strdupf("Filter syntax error; unexpected 'x'"));
2199
if (typ < 0) typ= 'F'; // Assume 'x' if missing
2200
if (!typ) ERR(p, strdupf("Expecting a 'x' or '/' before this"));
2202
if (1 != sscanf(buf, "%lf %c", &val, &dmy)) {
2203
// Must be a predefined filter
2211
if (typ != 'F') ERR(rew, strdupf("Predefined filters cannot be used with '/'"));
2213
// Parse the filter-spec
2214
memset(&sp, 0, sizeof(sp));
2216
sp.in_f0= sp.in_f1= -1;
2217
if ((err= parse_spec(&sp))) ERR(rew, err);
2221
// Adjust frequencies to range 0-0.5, and check them
2223
if (f0 > 0.5) ERR(rew, strdupf("Frequency of %gHz out of range with "
2224
"sampling rate of %gHz", f0*rate, rate));
2226
if (f1 > 0.5) ERR(rew, strdupf("Frequency of %gHz out of range with "
2227
"sampling rate of %gHz", f1*rate, rate));
2229
// Okay we now have a successful spec-match to filter[sp.fi], and sp.n_arg
2230
// args are now in sp.argarr[]
2232
// Generate the filter
2234
ff= filter[sp.fi].rout(rate, f0, f1, sp.order, sp.n_arg, sp.argarr);
2235
else if (strstr(filter[sp.fi].fmt, "#R"))
2236
ff= auto_adjust_dual(&sp, rate, f0, f1);
2238
ff= auto_adjust_single(&sp, rate, f0);
2240
// Append it to our FidFilter to return
2241
for (ff1= ff; ff1->typ; ff1= FFNEXT(ff1)) ;
2242
len= ((char*)ff1-(char*)ff);
2243
while (rvp + len + xtra >= rvend) INCBUF;
2244
memcpy(rvp, ff, len); rvp += len;
2250
// Must be a list of coefficients
2251
curr= (FidFilter*)rvp;
2253
while (rvp + sizeof(double) >= rvend) INCBUF;
2258
rvp += sizeof(double);
2260
// See how many more coefficients we can pick up
2263
if (!grabWord(&p, buf, sizeof(buf))) {
2264
if (*p) ERR(p, strdupf("Filter element unexpectedly long -- syntax error?"));
2267
if (1 != sscanf(buf, "%lf %c", &val, &dmy)) {
2271
while (rvp + sizeof(double) >= rvend) INCBUF;
2274
rvp += sizeof(double);
2283
return strdupf("Internal error, shouldn't reach here");
2288
// Filter-running code
2292
#include "fidrf_combined.h"
2296
#include "fidrf_cmdlist.h"
2300
#include "fidrf_jit.h"