iodelay.cpp 7.04 KB
Newer Older
sletz's avatar
sletz committed
1
/*
sletz's avatar
sletz committed
2
    Copyright (C) 2003-2008 Fons Adriaensen <fons@kokkinizita.net>
3
    
sletz's avatar
sletz committed
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

// --------------------------------------------------------------------------------

#include <stdlib.h>
#include <stdio.h>
23
#include <math.h>
sletz's avatar
sletz committed
24
#include <unistd.h>
sletz's avatar
sletz committed
25
#include <jack/jack.h>
sletz's avatar
sletz committed
26

27
struct Freq
sletz's avatar
sletz committed
28
29
30
31
32
{
    int   p;
    int   f;
    float xa;
    float ya;
33
34
35
36
    float x1;
    float y1;
    float x2;
    float y2;
sletz's avatar
sletz committed
37
38
39
};


40
41
struct MTDM
{
sletz's avatar
sletz committed
42
43
    double  _del;
    double  _err;
44
    float   _wlp;
sletz's avatar
sletz committed
45
46
    int     _cnt;
    int     _inv;
47
48

    struct Freq _freq [13];
sletz's avatar
sletz committed
49
50
};

51
52

struct MTDM * mtdm_new (double fsamp)
sletz's avatar
sletz committed
53
54
{
    int   i;
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
    struct Freq  *F;

    struct MTDM *retval = (MTDM *)malloc( sizeof(struct MTDM) );

    if (retval==NULL)
	    return NULL;

    retval->_cnt = 0;
    retval->_inv = 0;

    retval->_freq [0].f  = 4096;
    retval->_freq [1].f  = 2048;
    retval->_freq [2].f  = 3072;
    retval->_freq [3].f  = 2560;
    retval->_freq [4].f  = 2304;
    retval->_freq [5].f  = 2176; 
    retval->_freq [6].f  = 1088;
    retval->_freq [7].f  = 1312;
    retval->_freq [8].f  = 1552;
    retval->_freq [9].f  = 1800;
    retval->_freq [10].f = 3332;
    retval->_freq [11].f = 3586;
    retval->_freq [12].f = 3841;
    retval->_wlp = 200.0f / fsamp;
    for (i = 0, F = retval->_freq; i < 13; i++, F++) {
Stephane Letz's avatar
Stephane Letz committed
80
81
        F->p = 128;
        F->xa = F->ya = 0.0f;
82
83
        F->x1 = F->y1 = 0.0f;
        F->x2 = F->y2 = 0.0f;
sletz's avatar
sletz committed
84
    }
85
86

    return retval;
sletz's avatar
sletz committed
87
88
}

89
int mtdm_process (struct MTDM *self, size_t len, float *ip, float *op)
sletz's avatar
sletz committed
90
91
92
{
    int    i;
    float  vip, vop, a, c, s;
93
    struct Freq   *F;
sletz's avatar
sletz committed
94
95
96
97

    while (len--)
    {
        vop = 0.0f;
Stephane Letz's avatar
Stephane Letz committed
98
        vip = *ip++;
99
        for (i = 0, F = self->_freq; i < 13; i++, F++)
Stephane Letz's avatar
Stephane Letz committed
100
        {
101
            a = 2 * (float) M_PI * (F->p & 65535) / 65536.0; 
Stephane Letz's avatar
Stephane Letz committed
102
            F->p += F->f;
103
104
105
            c =  cosf (a); 
            s = -sinf (a); 
            vop += (i ? 0.01f : 0.20f) * s;
Stephane Letz's avatar
Stephane Letz committed
106
107
            F->xa += s * vip;
            F->ya += c * vip;
108
        } 
Stephane Letz's avatar
Stephane Letz committed
109
        *op++ = vop;
110
        if (++self->_cnt == 16)
Stephane Letz's avatar
Stephane Letz committed
111
        {
112
            for (i = 0, F = self->_freq; i < 13; i++, F++)
Stephane Letz's avatar
Stephane Letz committed
113
            {
114
115
116
117
                F->x1 += self->_wlp * (F->xa - F->x1 + 1e-20);
                F->y1 += self->_wlp * (F->ya - F->y1 + 1e-20);
                F->x2 += self->_wlp * (F->x1 - F->x2 + 1e-20);
                F->y2 += self->_wlp * (F->y1 - F->y2 + 1e-20);
Stephane Letz's avatar
Stephane Letz committed
118
119
                F->xa = F->ya = 0.0f;
            }
120
            self->_cnt = 0;
Stephane Letz's avatar
Stephane Letz committed
121
        }
sletz's avatar
sletz committed
122
123
124
125
126
    }

    return 0;
}

127
int mtdm_resolve (struct MTDM *self)
sletz's avatar
sletz committed
128
129
130
{
    int     i, k, m;
    double  d, e, f0, p;
131
    struct Freq *F = self->_freq;
sletz's avatar
sletz committed
132

133
134
135
136
137
    if (hypot (F->x2, F->y2) < 0.001) return -1;
    d = atan2 (F->y2, F->x2) / (2 * M_PI);
    if (self->_inv) d += 0.5;
    if (d > 0.5) d -= 1.0;
    f0 = self->_freq [0].f;
sletz's avatar
sletz committed
138
    m = 1;
139
140
    self->_err = 0.0;
    for (i = 0; i < 12; i++)
sletz's avatar
sletz committed
141
    {
Stephane Letz's avatar
Stephane Letz committed
142
        F++;
143
144
        p = atan2 (F->y2, F->x2) / (2 * M_PI) - d * F->f / f0;
        if (self->_inv) p += 0.5;
Stephane Letz's avatar
Stephane Letz committed
145
        p -= floor (p);
146
        p *= 2;
Stephane Letz's avatar
Stephane Letz committed
147
148
        k = (int)(floor (p + 0.5));
        e = fabs (p - k);
149
150
151
152
153
154
        if (e > self->_err) self->_err = e;
        if (e > 0.4) return 1; 
        d += m * (k & 1);
        m *= 2;
    }  
    self->_del = 16 * d;
sletz's avatar
sletz committed
155
156
157
158

    return 0;
}

159
160
161
162
void mtdm_invert (struct MTDM *self)
{
	self->_inv ^= 1;
}
sletz's avatar
sletz committed
163
// --------------------------------------------------------------------------------
sletz's avatar
sletz committed
164

165
static struct MTDM    *mtdm;
sletz's avatar
sletz committed
166
167
168
169
static jack_client_t  *jack_handle;
static jack_port_t    *jack_capt;
static jack_port_t    *jack_play;

sletz's avatar
sletz committed
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
jack_latency_range_t   capture_latency = {-1, -1};
jack_latency_range_t   playback_latency = {-1, -1};

void
latency_cb (jack_latency_callback_mode_t mode, void *arg)
{
	jack_latency_range_t range;

	range.min = range.max = 0;

	if (mode == JackCaptureLatency) {
		jack_port_set_latency_range (jack_play, mode, &range);
		jack_port_get_latency_range (jack_capt, mode, &range);
		if ((range.min != capture_latency.min) || (range.max != capture_latency.max)) {
			capture_latency = range;
			printf ("new capture latency: [%d, %d]\n", range.min, range.max);
		}
	} else {
		jack_port_set_latency_range (jack_capt, mode, &range);
		jack_port_get_latency_range (jack_play, mode, &range);
		if ((range.min != playback_latency.min) || (range.max != playback_latency.max)) {
			playback_latency = range;
			printf ("new playback latency: [%d, %d]\n", range.min, range.max);
		}
	}

}

sletz's avatar
sletz committed
198
int jack_callback (jack_nframes_t nframes, void *arg)
sletz's avatar
sletz committed
199
200
201
202
203
{
    float *ip, *op;

    ip = (float *)(jack_port_get_buffer (jack_capt, nframes));
    op = (float *)(jack_port_get_buffer (jack_play, nframes));
204
    mtdm_process (mtdm, nframes, ip, op);
sletz's avatar
sletz committed
205
    return 0;
sletz's avatar
sletz committed
206
207
208
209
}

int main (int ac, char *av [])
{
sletz's avatar
sletz committed
210
211
    float          t;
    jack_status_t  s;
sletz's avatar
sletz committed
212

sletz's avatar
sletz committed
213
214
    jack_handle = jack_client_open ("jack_delay", JackNoStartServer, &s);
    if (jack_handle == 0)
sletz's avatar
sletz committed
215
    {
sletz's avatar
sletz committed
216
217
        fprintf (stderr, "Can't connect to Jack, is the server running ?\n");
        exit (1);
sletz's avatar
sletz committed
218
219
    }

220
221
    mtdm = mtdm_new(jack_get_sample_rate(jack_handle));

sletz's avatar
sletz committed
222
    jack_set_process_callback (jack_handle, jack_callback, 0);
sletz's avatar
sletz committed
223

sletz's avatar
sletz committed
224
225
226
    if (jack_set_latency_callback)
	    jack_set_latency_callback (jack_handle, latency_cb, 0);

sletz's avatar
sletz committed
227
228
229
    jack_capt = jack_port_register (jack_handle, "in",  JACK_DEFAULT_AUDIO_TYPE, JackPortIsInput, 0);
    jack_play = jack_port_register (jack_handle, "out", JACK_DEFAULT_AUDIO_TYPE, JackPortIsOutput, 0);

sletz's avatar
sletz committed
230
231
    t = 1000.0f / jack_get_sample_rate (jack_handle);

sletz's avatar
sletz committed
232
233
    if (jack_activate (jack_handle))
    {
sletz's avatar
sletz committed
234
        fprintf(stderr, "Can't activate Jack");
sletz's avatar
sletz committed
235
236
237
238
239
        return 1;
    }

    while (1)
    {
240
241
242
243
244
245
246
247
 
    #ifdef WIN32 
        Sleep (250); 
    #else 
        usleep (250000); 
 	#endif        
        if (mtdm_resolve (mtdm) < 0) printf ("Signal below threshold...\n");
        else 
sletz's avatar
sletz committed
248
        {
249
            jack_nframes_t systemic_latency;
250
251

            if (mtdm->_err > 0.3) 
sletz's avatar
sletz committed
252
            {
253
254
                mtdm_invert ( mtdm );
                mtdm_resolve ( mtdm );
sletz's avatar
sletz committed
255
            }
256
            systemic_latency = (jack_nframes_t) floor (mtdm->_del - (capture_latency.max + playback_latency.max));
257

258
            printf ("%10.3lf frames %10.3lf ms total roundtrip latency\n\textra loopback latency: %u frames\n\tuse %u for the backend arguments -I and -O", mtdm->_del, mtdm->_del * t, 
259
                    systemic_latency, systemic_latency/2);
260
261
            if (mtdm->_err > 0.2) printf (" ??");
                if (mtdm->_inv) printf (" Inv");
sletz's avatar
sletz committed
262
263
            printf ("\n");
        }
sletz's avatar
sletz committed
264
265
266
267
268
    }

    return 0;
}

269
// --------------------------------------------------------------------------------