-
Notifications
You must be signed in to change notification settings - Fork 50
/
dsp_fft.c
449 lines (398 loc) · 14.1 KB
/
dsp_fft.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
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
/*
* dsp_fft.c
* ==>TO BE INCLUDED IN dsp.c
*
* Created: May 2022
* Author: Arjan te Marvelde
*
* Signal processing of RX and TX branch, to be run on the second processor core (CORE1).
* A branch has a dedicated routine that must run on set times.
* In this case it runs when half FFT_SIZE of samples is ready to be processed.
*
*
* The pace for sampling is set by a timer at 64usec (15.625 kHz)
* The associated timer callback routine:
* - handles data transfer to/from physical interfaces
* - starts a new ADC conversion sequence
* - maintains dsp_tick counter
* - when dsp_tick == FFT_SIZE/2 (one buffer), the dsp-loop is triggered.
*
* The ADC functions in round-robin and fifo mode, triggering IRQ after 3 conversions (ADC[0..2])
* The ADC FIFO IRQ handler reads the 3 samples from the fifo after stopping the ADC
*
* Buffer structure, built from half FFT_SIZE buffers.
* The I, Q and A external interfaces communicate each through 3x buffers.
* One buffer is being filled or emptied, depending on data direction.
* The other two are swapped with the FFT signal processing buffers.
* Since we use complex FFT, the algorithm uses 4x buffers.
*
* I, Q and A buffers are used as queues. RX case looks like:
*
* +--+--+--+ +--+--+--+
* i --> | | | | | | | | --> a
* +--+--+--+ +--+--+--+
* \ \ \ +--+--+ / /
* ---------> | | | -------
* +--+--+ FFT-DSP-iFFT
* ---------> | | |
* / / / +--+--+
* +--+--+--+
* q --> | | | |
* +--+--+--+
*
* RX, when triggered by timer callback:
* - The oldest two I and Q buffers are copied into the FFT buffers
* - FFT is executed
* - Signal processing is done
* - iFFT is executed
* - The oldest real FFT buffer is moved to the A output queue
*
* +--+--+--+ +--+--+--+
* a --> | | | | | | | | --> i
* +--+--+--+ +--+--+--+
* \ \ \ +--+--+ / /
* --------> | | | -------
* +--+--+ FFT-DSP-iFFT
* | | | -------
* +--+--+ \ \
* +--+--+--+
* | | | | --> q
* +--+--+--+
*
* TX, when triggered by timer callback:
* - The oldest two A buffers are copied to the real FFT buffer, the imaginary FFT buffer is nulled
* - FFT is executed
* - Signal processing is done
* - iFFT is executed
* - The oldest FFT buffers are appended to the I/Q output queues
*
* The bin step is the sampling frequency divided by the FFT_SIZE.
* So for S_RATE=15625 and FFT_SIZE=1024 this step is 15625/1024=15.259 Hz
* The Subcarrier offset (Fc) is at about half the Nyquist frequency: bin 256 or 3906 Hz
*
*/
#include "uSDR.h"
#include "dsp.h"
/*
* FFT buffer allocation
* Buffer size is FFT_SIZE/2 (see fix_fft.h).
* In case FFT_SIZE of 1024, a buffer is 1kB
* RX: 3 buffers for I samples, 3 buffers for Q samples, 3 buffers for Audio
* DSP: 4 buffers for FFT, complex samples and these have to be consecutive!
* TX: re-use RX buffers in reverse order
* Total of 13kByte RAM is required.
* Samples are 16 bit signed integer, but align buffers on 32bit boundaries
* dsp_tick points into I, Q and A buffers, so wrap once per two FFTs
* When tick==FFT_SIZE/2: do buffer copy
*/
#define BUFSIZE FFT_SIZE/2
int16_t I_buf[3][BUFSIZE] __attribute__((aligned(4))); // I sample queue, 3x buffer of FFT_SIZE/2
int16_t Q_buf[3][BUFSIZE] __attribute__((aligned(4))); // Q sample queue, 3x buffer of FFT_SIZE/2
int16_t A_buf[3][BUFSIZE] __attribute__((aligned(4))); // A sample queue, 3x buffer of FFT_SIZE/2
int16_t XI_buf[FFT_SIZE] __attribute__((aligned(4))); // Re FFT buffer, 1x buffer of FFT_SIZE
int16_t XQ_buf[FFT_SIZE] __attribute__((aligned(4))); // Im FFT buffer, 1x buffer of FFT_SIZE
// Sample buffer indexes, updated by timer callback
volatile int dsp_active = 0; // I, Q, A active buffer number (0..2)
volatile uint32_t dsp_tick = 0; // Index in active buffer
volatile uint32_t dsp_tickx = 0; // Load indicator DSP loop
// Spectrum bins for a frequency
#define BIN(f) (int)(((f)*FFT_SIZE+S_RATE/2)/S_RATE)
#define BIN_FC 256 // Use BIN_FC > BIN_3000 to avoid aliasing!
#define BIN_100 7 // 110Hz bin
#define BIN_300 20 // 300Hz bin
#define BIN_900 59 // 900Hz bin
#define BIN_3000 197 // 3000Hz bin
#define BIN_3600 236 // 3600Hz bin
/*
* Shift center frequency back to DC by multiply samples with e(-j*w*t)
* w = 2*pi*f, where f is FC_OFFSET (3906Hz)
* t = n*Ts, where Ts is 1/S_RATE (1/15625Hz)
* So the exponent becomes n*2*pi*FC_OFFSET/S_RATE = n*pi/2 (since FC_OFFSET = S_RATE/4)
* The complex offset is then cos(-n*pi/2) + j*sin(-n*pi/2) ==> (a,b) = ( 1, 0); ( 0,-1); (-1, 0); ( 0, 1); ...
* Complex multiply: (a+jb)*(I+jQ)=(aI-bQ) + j(aQ+bI) ==> (I,Q) = ( I, Q); ( Q,-I); (-I,-Q); (-Q, I); ...
*/
void __not_in_flash_func(dsp_shift)(void)
{
int i;
uint16_t x;
for (i=0; i<FFT_SIZE; i+=4)
{
x = XI_buf[i+1];
XI_buf[i+1] = XQ_buf[i+1];
XQ_buf[i+1] = -x;
XI_buf[i+2] = -XI_buf[i+2];
XQ_buf[i+2] = -XQ_buf[i+2];
x = XI_buf[i+3];
XI_buf[i+3] = -XQ_buf[i+3];
XQ_buf[i+3] = x;
}
}
/*
* This applies a bandpass filter to XI and XQ buffers
* lowbin and highbin edges must be between 3 and FFT_SIZE/2 - 3
* Edge is a 7 bin raised cosine flank, i.e. 100Hz wide
* Coefficients are: 0, 0.067, 0.25, 0.5, 0.75, 0.933, 1
* where the edge bin is in the center of this flank
* Note: maybe make slope less steep, e.g. 9 or 11 bins
* ____ ____
* _/ \____________________/ \_
* [-------|-------][-------|-------]
* ^ ^ ^
* 0 BUFSIZE FFTSIZE
*
* The parameter sidebands determines whether LSB (-1) USB (+1) or DSB(0) is passed.
*/
#define DSP_PASS_LSB -1
#define DSP_PASS_USB 1
#define DSP_PASS_DSB 0
void __not_in_flash_func(dsp_bandpass)(int lowbin, int highbin, int sidebands)
{
int i, lo1, lo2, hi1, hi2;
if ((lowbin<3)||(highbin>(FFT_SIZE/2-3))||(highbin-lowbin<6)) return;
// Boundaries are inclusive
lo1 = lowbin-2;
lo2 = highbin+2;
hi1 = FFT_SIZE-highbin-2;
hi2 = FFT_SIZE-lowbin+2;
// Null all bins excluded from passbands
// Calculate edges as raised cosine
XI_buf[0] = 0; XQ_buf[0] = 0; // Block DC
for (i=1; i<lo1; i++) { XI_buf[i] = 0; XQ_buf[i] = 0; }
if (sidebands==DSP_PASS_LSB) // LSB only: block USB
{
for (i=lo1; i<lo2; i++) { XI_buf[i] = 0; XQ_buf[i] = 0; }
}
else
{
i=lo1;
XI_buf[i] = XI_buf[i]*0.067; XQ_buf[i] = XQ_buf[i]*0.067; i++;
XI_buf[i] = XI_buf[i]*0.250; XQ_buf[i] = XQ_buf[i]*0.250; i++;
XI_buf[i] = XI_buf[i]*0.500; XQ_buf[i] = XQ_buf[i]*0.500; i++;
XI_buf[i] = XI_buf[i]*0.750; XQ_buf[i] = XQ_buf[i]*0.750; i++;
XI_buf[i] = XI_buf[i]*0.933; XQ_buf[i] = XQ_buf[i]*0.933;
i=lo2;
XI_buf[i] = XI_buf[i]*0.067; XQ_buf[i] = XQ_buf[i]*0.067; i--;
XI_buf[i] = XI_buf[i]*0.250; XQ_buf[i] = XQ_buf[i]*0.250; i--;
XI_buf[i] = XI_buf[i]*0.500; XQ_buf[i] = XQ_buf[i]*0.500; i--;
XI_buf[i] = XI_buf[i]*0.750; XQ_buf[i] = XQ_buf[i]*0.750; i--;
XI_buf[i] = XI_buf[i]*0.933; XQ_buf[i] = XQ_buf[i]*0.933;
}
for (i=lo2+1; i<hi1; i++) { XI_buf[i] = 0; XQ_buf[i] = 0; }
if (sidebands==DSP_PASS_USB) // USB only: block LSB
{
for (i=hi1; i<hi2; i++) { XI_buf[i] = 0; XQ_buf[i] = 0; }
}
else
{
i=hi1;
XI_buf[i] = XI_buf[i]*0.067; XQ_buf[i] = XQ_buf[i]*0.067; i++;
XI_buf[i] = XI_buf[i]*0.250; XQ_buf[i] = XQ_buf[i]*0.250; i++;
XI_buf[i] = XI_buf[i]*0.500; XQ_buf[i] = XQ_buf[i]*0.500; i++;
XI_buf[i] = XI_buf[i]*0.750; XQ_buf[i] = XQ_buf[i]*0.750; i++;
XI_buf[i] = XI_buf[i]*0.933; XQ_buf[i] = XQ_buf[i]*0.933;
i=hi2;
XI_buf[i] = XI_buf[i]*0.067; XQ_buf[i] = XQ_buf[i]*0.067; i--;
XI_buf[i] = XI_buf[i]*0.250; XQ_buf[i] = XQ_buf[i]*0.250; i--;
XI_buf[i] = XI_buf[i]*0.500; XQ_buf[i] = XQ_buf[i]*0.500; i--;
XI_buf[i] = XI_buf[i]*0.750; XQ_buf[i] = XQ_buf[i]*0.750; i--;
XI_buf[i] = XI_buf[i]*0.933; XQ_buf[i] = XQ_buf[i]*0.933;
}
for (i=hi2+1; i<FFT_SIZE; i++) { XI_buf[i] = 0; XQ_buf[i] = 0; }
}
/** CORE1: RX branch **/
/*
* Execute RX branch signal processing
* max time to spend is <32ms (BUFSIZE*TIM_US)
* The pre-processed I/Q samples are passed in I_BUF and Q_BUF
* The calculated A samples are passed in A_BUF
*/
volatile int scale0;
volatile int scale1;
bool __not_in_flash_func(rx)(void)
{
int b;
int i;
int16_t *ip, *qp, *ap, *xip, *xqp;
int16_t peak;
b = dsp_active; // Point to Active sample buffer
/*** Copy saved I/Q buffers to FFT filter buffer ***/
if (++b > 2) b = 0; // Point to Old Saved sample buffer
ip = &I_buf[b][0]; xip = &XI_buf[0];
qp = &Q_buf[b][0]; xqp = &XQ_buf[0];
for (i=0; i<BUFSIZE; i++)
{
*xip++ = *ip++;
*xqp++ = *qp++;
}
if (++b > 2) b = 0; // Point to New Saved sample buffer
ip = &I_buf[b][0]; xip = &XI_buf[BUFSIZE];
qp = &Q_buf[b][0]; xqp = &XQ_buf[BUFSIZE];
for (i=0; i<BUFSIZE; i++)
{
*xip++ = *ip++;
*xqp++ = *qp++;
}
dsp_shift(); // Downshift samples by FC_OFFSET
/*** Execute FFT ***/
scale0 = fix_fft(&XI_buf[0], &XQ_buf[0], false); // Frequency domain filter input
/*** Shift and filter sidebands ***/
// At this point USB and LSB surround DC
// The desired sidebands must be shifted to their target positions around 0
// [-------|-------][-------|-------]
// USB0 LSB0
switch (dsp_mode)
{
case MODE_USB:
// Bandpass USB
dsp_bandpass(BIN_100, BIN_3000, DSP_PASS_USB);
break;
case MODE_LSB:
// Bandpass LSB
dsp_bandpass(BIN_100, BIN_3000, DSP_PASS_LSB);
break;
case MODE_AM:
// Bandpass DSB (LSB + USB)
dsp_bandpass(BIN_100, BIN_3000, DSP_PASS_DSB);
break;
case MODE_CW:
// Bandpass CW, 600Hz
dsp_bandpass(BIN_900-BIN_300, BIN_900+BIN_300, 0);
break;
}
/*** Execute inverse FFT ***/
scale1 = fix_fft(&XI_buf[0], &XQ_buf[0], true);
/*** Export FFT buffer to A ***/
b = dsp_active; // Assume active buffer not changed, i.e. no overruns
if (++b > 2) b = 0; // Point to oldest (will be next for output)
ap = &A_buf[b][0]; xip = &XI_buf[BUFSIZE];
for (i=0; i<BUFSIZE; i++)
{
*ap++ = *xip++; // Copy newest results
}
/*** Scale down into DAC_RANGE! ***/
peak = 256;
for (i=0; i<BUFSIZE; i++)
{
A_buf[b][i] /= peak;
}
return true;
}
/** CORE1: TX branch **/
/*
* Execute TX branch signal processing
* max time to spend is <32ms (BUFSIZE*TIM_US)
* The pre-processed A samples are passed in A_BUF
* The calculated I and Q samples are passed in I_BUF and Q_BUF
*/
bool __not_in_flash_func(tx)(void)
{
int b;
int i;
int16_t *ip, *qp, *ap, *xip, *xqp;
int16_t peak;
b = dsp_active; // Point to Active sample buffer
/*** Copy saved A buffers to FFT buffers, NULL Im. part ***/
if (++b > 2) b = 0; // Point to Old Saved sample buffer
ap = &A_buf[b][0]; xip = &XI_buf[0];
xqp = &XQ_buf[0];
for (i=0; i<BUFSIZE; i++)
{
*xip++ = *ap++;
*xqp++ = 0;
}
if (++b > 2) b = 0; // Point to New Saved sample buffer
ap = &A_buf[b][0]; xip = &XI_buf[BUFSIZE];
xqp = &XQ_buf[BUFSIZE];
for (i=0; i<BUFSIZE; i++)
{
*xip++ = *ap++;
*xqp++ = 0;
}
/*** Execute FFT ***/
scale0 = fix_fft(&XI_buf[0], &XQ_buf[0], false);
/*** Shift and filter sidebands ***/
XI_buf[0] = 0; XQ_buf[0] = 0; // Block DC
switch (dsp_mode)
{
case MODE_USB:
// Bandpass Audio, USB only
dsp_bandpass(BIN_100, BIN_3000, 1);
// Shift USB up to to Fc, assumes Fc > bandwidth
for (i=1; i<BIN_3000; i++)
{
XI_buf[BIN_FC+i] = XI_buf[i];
XQ_buf[BIN_FC+i] = XQ_buf[i];
XI_buf[i] = 0;
XQ_buf[i] = 0;
}
for (i=1; i<BIN_3000; i++)
{
XI_buf[FFT_SIZE-BIN_FC-i] = XI_buf[BIN_FC+i];
XQ_buf[FFT_SIZE-BIN_FC-i] = XQ_buf[BIN_FC+i];
}
break;
case MODE_LSB:
// Bandpass Audio, LSB only
dsp_bandpass(BIN_100, BIN_3000, -1);
// Shift LSB up to Fc
for (i=1; i<BIN_3000; i++)
{
XI_buf[BIN_FC-i] = XI_buf[FFT_SIZE-i];
XQ_buf[BIN_FC-i] = XQ_buf[FFT_SIZE-i];
XI_buf[FFT_SIZE-i] = 0;
XQ_buf[FFT_SIZE-i] = 0;
}
for (i=1; i<BIN_3000; i++)
{
XI_buf[FFT_SIZE-BIN_FC+i] = XI_buf[BIN_FC-i];
XQ_buf[FFT_SIZE-BIN_FC+i] = XQ_buf[BIN_FC-i];
}
break;
case MODE_AM:
// Bandpass Audio
dsp_bandpass(BIN_100, BIN_3000, 0);
// Shift DSB up to Fc
for (i=1; i<BIN_3000; i++)
{
XI_buf[BIN_FC+i] = XI_buf[i];
XQ_buf[BIN_FC+i] = XQ_buf[i];
XI_buf[i] = 0;
XQ_buf[i] = 0;
XI_buf[BIN_FC-i] = XI_buf[FFT_SIZE-i];
XQ_buf[BIN_FC-i] = XQ_buf[FFT_SIZE-i];
XI_buf[FFT_SIZE-i] = 0;
XQ_buf[FFT_SIZE-i] = 0;
}
for (i=1; i<BIN_3000; i++)
{
XI_buf[FFT_SIZE-BIN_FC-i] = XI_buf[BIN_FC+i];
XQ_buf[FFT_SIZE-BIN_FC-i] = XQ_buf[BIN_FC+i];
XI_buf[FFT_SIZE-BIN_FC+i] = XI_buf[BIN_FC-i];
XQ_buf[FFT_SIZE-BIN_FC+i] = XQ_buf[BIN_FC-i];
}
break;
case MODE_CW:
// Create a carrier on 900Hz from Fc
break;
}
/*** Execute inverse FFT ***/
scale1 = fix_fft(&XI_buf[0], &XQ_buf[0], true);
/*** Export FFT buffer to I and Q ***/
b = dsp_active; // Assume active buffer not changed, i.e. no overruns
if (++b > 2) b = 0; // Point to oldest (will be next for output)
qp = &Q_buf[b][0]; xqp = &XQ_buf[BUFSIZE];
ip = &I_buf[b][0]; xip = &XI_buf[BUFSIZE];
for (i=0; i<BUFSIZE; i++)
{
*qp++ = *xqp++; // Copy newest results
*ip++ = *xip++; // Copy newest results
}
/*** Scale down into DAC_RANGE! ***/
peak = 256;
for (i=0; i<BUFSIZE; i++)
{
Q_buf[b][i] /= peak;
I_buf[b][i] /= peak;
}
return true;
}