-
Notifications
You must be signed in to change notification settings - Fork 29
/
Copy pathcap5.c
218 lines (194 loc) · 7.21 KB
/
cap5.c
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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
/*
* This file is part of dsp.
*
* Copyright (c) 2022-2025 Michael Barbour <[email protected]>
*
* Permission to use, copy, modify, and distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
* ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
* ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
* OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
#include <float.h>
#include <math.h>
#include "cap5.h"
void ap3_reset(struct ap3_state *state)
{
ap2_reset(&state->ap2);
ap1_reset(&state->ap1);
}
void cap5_reset(struct cap5_state *state)
{
ap2_reset(&state->a1);
ap3_reset(&state->a2);
}
void cap5_butterworth_ap(double complex ap[3])
{
for (int i = 0; i < 3; ++i) {
const double theta = (2*i+1)*M_PI/(2.0*5);
ap[i] = -sin(theta) + cos(theta)*I; /* normalized pole in s-plane */
}
}
void cap5_chebyshev_ap(int gen_type2, double stop_dB, double complex ap[3])
{
if (stop_dB > 100.0) {
cap5_butterworth_ap(ap);
return;
}
const double epsilon = sqrt(pow(10.0, stop_dB/10.0) - 1.0);
const double sigma = asinh(epsilon)/5;
const double scale = cosh(acosh(epsilon)/5);
for (int i = 0; i < 3; ++i) {
const double theta = (2*i+1)*M_PI/(2.0*5);
ap[i] = -sinh(sigma)*sin(theta) + cosh(sigma)*cos(theta)*I; /* normalized pole in s-plane */
ap[i] = ap[i] / scale; /* scale so H(1) = sqrt(0.5) */
if (gen_type2) ap[i] = 1.0/ap[i];
}
}
static inline int fz_sgn(double x)
{
if (x < 0.0) return -1;
else if (x > 0.0) return 1;
return 0;
}
#define FIND_ZERO_MAX_ITER 100
static double find_zero(double (*fn)(double, const void *), const void *arg, double a, double b, double tol)
{
double c = a, fn_a = fn(a, arg), fn_b = fn(b, arg);
if (tol < DBL_EPSILON) tol = DBL_EPSILON*2;
for (int i = 0, side = 0; i < FIND_ZERO_MAX_ITER; ++i) {
c = (fn_a*b - fn_b*a) / (fn_a - fn_b);
if (fabs(b-a) < tol*fabs(b+a)) return c;
const double fn_c = fn(c, arg);
if (fz_sgn(fn_b) == fz_sgn(fn_c)) {
b = c; fn_b = fn_c;
if (side == -1) fn_a /= 2.0;
side = -1;
}
else if (fz_sgn(fn_a) == fz_sgn(fn_c)) {
a = c; fn_a = fn_c;
if (side == 1) fn_b /= 2.0;
side = 1;
}
else {
if (i == 0) return -NAN; /* no zero within interval [a, b] */
return c;
}
}
return -NAN; /* failed to converge */
}
static double ellip_q_err(double k, const void *arg)
{
const double target_q = *((double *) arg);
//LOG_FMT(LL_VERBOSE, "%s(): k=%.15e; target_q=%.15e", __func__, k, target_q);
const double kp = sqrt(sqrt(1.0-k*k));
const double l = (1.0-kp)/((1.0+kp)*2.0);
return (l + 2.0*pow(l, 5) + 15.0*pow(l, 9) + 150.0*pow(l, 13)) - target_q;
}
/* Evaluate allpass given by poles ap at jw (=j*w) */
static double complex eval_allpass_ap(const double complex *ap, int n, double complex jw)
{
const int has_real = (cimag(ap[n-1]) == 0); /* real root is always last */
double complex num = (has_real) ? jw + ap[n-1] : 1.0;
double complex den = (has_real) ? jw - ap[n-1] : 1.0;
for (int i = 0, np = (has_real)?n-1:n; i < np; ++i) {
num *= (jw + ap[i]) * (jw + conj(ap[i])); /* conjugates not stored */
den *= (jw - ap[i]) * (jw - conj(ap[i]));
}
return num / den;
}
/* Dot product treating a and b as vectors */
static inline double geom_dot(double complex a, double complex b)
{
return creal(a)*creal(b) + cimag(a)*cimag(b);
}
struct ellip_wc_err_arg {
const double complex *ap0, *ap1;
int n0, n1;
};
static double ellip_wc_err(double w, const void *arg)
{
const double complex jw = w*I;
struct ellip_wc_err_arg *a = (struct ellip_wc_err_arg *) arg;
//LOG_FMT(LL_VERBOSE, "%s(): w=%.15e", __func__, w);
return geom_dot(eval_allpass_ap(a->ap0, a->n0, jw), eval_allpass_ap(a->ap1, a->n1, jw));
}
void cap5_elliptic_ap(double stop_dB_lp, double stop_dB_hp, double complex ap[3])
{
if (stop_dB_lp > 100.0) {
cap5_chebyshev_ap(0, stop_dB_hp, ap);
return;
}
else if (stop_dB_hp > 100.0) {
cap5_chebyshev_ap(1, stop_dB_lp, ap);
return;
}
const double e2 = 1.0 / (pow(10.0, stop_dB_hp/10.0) - 1.0);
const double D = (pow(10.0, stop_dB_lp/10.0) - 1.0) / e2;
const double q = 1.0 / (exp2(4.0/5) * pow(D, 1.0/5));
const double k = find_zero(ellip_q_err, &q, 0.0, 1.0, 0);
if (!isnormal(k)) goto fail_fz;
const double L = log((sqrt(1.0+e2)+1.0) / (sqrt(1.0+e2)-1.0)) / (2.0*5);
double sigma0_s0 = sinh(L), sigma0_s1 = 0.0;
for (int m = 1; m < 6; ++m) {
const int sgn = (m&1)?-1:1;
sigma0_s0 += sgn * pow(q, m*(m+1)) * sinh((2*m+1)*L);
sigma0_s1 += sgn * pow(q, m*m) * cosh(2*m*L);
}
const double sigma0 = fabs((2.0*sqrt(sqrt(q))*sigma0_s0) / (1.0+2.0*sigma0_s1));
const double sigma02 = sigma0*sigma0;
const double W = sqrt((1.0+k*sigma02) * (1.0+sigma02/k));
for (int i = 0; i < 2; ++i) {
const double mu = 2.0-i;
double omega_s0 = sin(M_PI*mu/5), omega_s1 = 0.0;
for (int m = 1; m < 6; ++m) {
const int sgn = (m&1)?-1:1;
omega_s0 += sgn * pow(q, m*(m+1)) * sin((2*m+1)*M_PI*mu/5);
omega_s1 += sgn * pow(q, m*m) * cos(2*m*M_PI*mu/5);
}
const double omega = (2.0*sqrt(sqrt(q))*omega_s0) / (1.0+2.0*omega_s1);
const double omega2 = omega*omega;
const double Vi = sqrt((1.0-k*omega2)*(1.0-omega2/k));
ap[i] = (-2.0*sigma0*Vi + 2.0*omega*W*I) / (2.0*(1.0+sigma02*omega2)); /* complex poles (conjugates not stored) */
}
ap[2] = -sigma0; /* real pole */
if (fabs(stop_dB_lp - stop_dB_hp) > 0.01) {
/* scale so that magnitude at w=1 is sqrt(0.5) */
const double complex ap0[1] = { ap[1] };
const double complex ap1[2] = { ap[0], ap[2] };
const struct ellip_wc_err_arg err_arg = { .ap0 = ap0, .ap1 = ap1, .n0 = 1, .n1 = 2 };
const double half_width = sqrt(1.0/k);
const double wc = find_zero(ellip_wc_err, &err_arg, 1.0/half_width, half_width, 0);
if (!isnormal(wc)) goto fail_fz;
for (int i = 0; i < 3; ++i) ap[i] /= wc;
}
return;
fail_fz:
LOG_FMT(LL_ERROR, "%s(): BUG: failed to converge; falling back to butterworth", __func__);
cap5_butterworth_ap(ap);
}
void cap5_init(struct cap5_state *state, double fs, double fc, const double complex ap[3])
{
const double fc_w = 2.0*fs*tan(M_PI*fc/fs); /* pre-warped corner frequency */
double complex p[3];
for (int i = 0; i < 3; ++i) {
p[i] = ap[i]*fc_w; /* scale */
p[i] = (2.0*fs + p[i]) / (2.0*fs - p[i]); /* bilinear transform */
//LOG_FMT(LL_VERBOSE, "%s(): fc=%gHz: p[%d] = %f%+fi", __func__, fc, i, creal(p[i]), cimag(p[i]));
}
state->a2.ap2.c0 = -2.0*creal(p[0]);
state->a2.ap2.c1 = creal(p[0])*creal(p[0]) + cimag(p[0])*cimag(p[0]);
state->a1.c0 = -2.0*creal(p[1]);
state->a1.c1 = creal(p[1])*creal(p[1]) + cimag(p[1])*cimag(p[1]);
state->a2.ap1.c0 = -creal(p[2]);
//LOG_FMT(LL_VERBOSE, "%s(): fc=%gHz: a1: c0=%g c1=%g", __func__, fc, state->a1.c0, state->a1.c1);
//LOG_FMT(LL_VERBOSE, "%s(): fc=%gHz: a2.ap2: c0=%g c1=%g", __func__, fc, state->a2.ap2.c0, state->a2.ap2.c1);
//LOG_FMT(LL_VERBOSE, "%s(): fc=%gHz: a2.ap1: c0=%g", __func__, fc, state->a2.ap1.c0);
cap5_reset(state);
}