T T
AD9857 IQ DDS Digital Up Converter Experiment using Nucleo F401
Embed:
(wiki syntax)
Show/hide line numbers
dsp.cpp
00001 #include "mbed.h" 00002 #include "dsp.h" 00003 00004 extern Serial pc; 00005 00006 00007 int16_t htstat_buf[HILBERT_TRANSFORM_FIRSTATE_LENGTH]; 00008 int16_t cap_buf[2][CAPTURE_LEN]; 00009 00010 int16_t fir_state[FIRSTATE_LENGTH]; 00011 int16_t fir_buf[CAPTURE_LEN]; 00012 00013 int16_t cic_buf[CICBUF_LEN]; 00014 int16_t dma_buf[2][DMA_DATALEN]; 00015 00016 CICState cic; 00017 00018 void cic_interpolate_x10(CICState *cic, uint32_t *src, int src_len, uint32_t *dst) 00019 { 00020 uint32_t p0 = cic->p0; 00021 uint32_t s0 = cic->s0; 00022 int i; 00023 int j = 0; 00024 for (i = 0; i < src_len; i++) { 00025 uint32_t s = src[i]; 00026 uint32_t d0 = __QSUB16(s, p0); 00027 s0 = __QADD16(s0, d0); 00028 uint32_t s1 = __QADD16(s0, d0); 00029 uint32_t s2 = __QADD16(s1, d0); 00030 uint32_t s3 = __QADD16(s2, d0); 00031 uint32_t s4 = __QADD16(s3, d0); 00032 dst[j ] = s0 >> 2; 00033 dst[j+1] = s1 >> 2; 00034 dst[j+2] = s2 >> 2; 00035 dst[j+3] = s3 >> 2; 00036 dst[j+4] = s4 >> 2; 00037 j += 5; 00038 s0 = s4; 00039 s0 = __QADD16(s0, d0); 00040 s1 = __QADD16(s0, d0); 00041 s2 = __QADD16(s1, d0); 00042 s3 = __QADD16(s2, d0); 00043 s4 = __QADD16(s3, d0); 00044 dst[j ] = s0 >> 2; 00045 dst[j+1] = s1 >> 2; 00046 dst[j+2] = s2 >> 2; 00047 dst[j+3] = s3 >> 2; 00048 dst[j+4] = s4 >> 2; 00049 j += 5; 00050 s0 = s4; 00051 p0 = s; 00052 } 00053 cic->s0 = s0; 00054 cic->p0 = p0; 00055 } 00056 00057 const int16_t fir_coeff[8][16] = { 00058 { -19, -6, 102, -22, -357, 264, 1798, 2064, 630, -337, -119, 111, 15, -24, 0, 0}, 00059 { -13, -20, 76, 51, -311, -29, 1441, 2206, 1037, -231, -223, 96, 45, -25, -6, 0}, 00060 { -6, -25, 45, 96, -223, -231, 1037, 2206, 1441, -29, -311, 51, 76, -20, -13, 0}, 00061 { 0, -24, 15, 111, -119, -337, 630, 2064, 1798, 264, -357, -22, 102, -6, -19, 0}, 00062 { 0, -19, -6, 102, -22, -357, 264, 1798, 2064, 630, -337, -119, 111, 15, -24, 0}, 00063 { 0, -13, -20, 76, 51, -311, -29, 1441, 2206, 1037, -231, -223, 96, 45, -25, -6}, 00064 { 0, -6, -25, 45, 96, -223, -231, 1037, 2206, 1441, -29, -311, 51, 76, -20, -13}, 00065 { 0, 0, -24, 15, 111, -119, -337, 630, 2064, 1798, 264, -357, -22, 102, -6, -19} 00066 }; 00067 00068 void fir_resample_x4(uint32_t *src_state, uint32_t *dst, int dst_len) 00069 { 00070 uint32_t *src = src_state; 00071 int index = 0; 00072 int i; 00073 for (i = 0; i < dst_len; i++) { 00074 uint32_t *kp = (uint32_t*)fir_coeff[index]; 00075 uint32_t acc_i = 0; 00076 uint32_t acc_q = 0; 00077 00078 // 8cycle x 8 00079 #define CELL(n) do { \ 00080 uint32_t k = kp[n]; \ 00081 uint32_t p0 = src[n*2]; \ 00082 uint32_t p1 = src[n*2+1]; \ 00083 uint32_t i01 = __PKHTB(p1, p0, 16); \ 00084 uint32_t q01 = __PKHBT(p0, p1, 16); \ 00085 acc_i = __SMLAD(k, i01, acc_i); \ 00086 acc_q = __SMLAD(k, q01, acc_q); \ 00087 } while(0) 00088 00089 CELL(0); CELL(1); CELL(2); CELL(3); 00090 CELL(4); CELL(5); CELL(6); CELL(7); 00091 *dst++ = __PKHTB(__SSAT16(acc_i, 16), __SSAT16(acc_q, 16), 16); 00092 00093 index++; 00094 if (index >= 8) { 00095 index = 0; 00096 src += 2; 00097 } 00098 } 00099 00100 for (i = 0; i < FIRSTATE_LENGTH/2; i++) 00101 *src_state++ = *src++; 00102 } 00103 00104 void 00105 interpolate_test() 00106 { 00107 int freq = 1000; 00108 int ampl = 3000; 00109 int rate = 48000; 00110 for (int i = 0; i < CAPTURE_LEN; i++){ 00111 cap_buf[0][i*2] = sin(2*3.141592 * i * freq / rate) * ampl; // Lch 00112 cap_buf[0][i*2+1] = cos(2*3.141592 * i * freq / rate) * ampl; // Rch 00113 } 00114 #if 0 00115 for (int i = 0; i < 48; i++){ 00116 pc.printf("%d, ", cap_buf[i*2]); 00117 } 00118 pc.printf("\n\r"); 00119 #endif 00120 fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN); 00121 cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]); 00122 fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN); 00123 cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]); 00124 #if 0 00125 for (int i = 0; i < 48*25/4; i++){ 00126 pc.printf("%d, ", cic_buf[i*2]); 00127 } 00128 pc.printf("\n\r"); 00129 #endif 00130 #if 0 00131 for (int i = DMA_DATALEN/2-400; i < DMA_DATALEN/2; i++){ 00132 pc.printf("%d, ", dma_buf[i*2]); 00133 } 00134 pc.printf("\n\r"); 00135 for (int i = 0; i < 400; i++){ 00136 pc.printf("%d, ", dma_buf[i*2]); 00137 } 00138 pc.printf("\n\r"); 00139 #endif 00140 } 00141 00142 // hilbert transform by 127 taps fir 00143 int16_t hilbert127_fir_coeff[32] = { 00144 20848, 6917, 4112, 2897, 2212, 1768, 1454, 1219, 1036, 00145 887, 764, 661, 572, 496, 429, 371, 319, 274, 00146 234, 198, 167, 140, 117, 97, 79, 65, 53, 00147 44, 36, 31, 28, 26 00148 }; 00149 00150 void 00151 hilbert_transform(uint32_t *src, uint32_t *dst, int dst_len, int sign) 00152 { 00153 int j; 00154 int len = sizeof htstat_buf / sizeof(uint32_t); 00155 src -= len; 00156 // 240 * (208 + 12) = 52800cycle = 0.62ms@84MHz 00157 for (j = 0; j < dst_len; j++) { 00158 int i; 00159 int32_t acc = 0; 00160 #define OFFSET 64 00161 // 16 * (10 + 3) = 208 cycle 00162 for (i = 0; i < 32; i += 2) { 00163 // 10 cycle 00164 uint32_t c = *(uint32_t*)&hilbert127_fir_coeff[i]; 00165 uint32_t a0 = src[OFFSET - i*2 - 1]; 00166 uint32_t a1 = src[OFFSET - i*2 - 3]; 00167 uint32_t b0 = src[OFFSET + i*2 + 1]; 00168 uint32_t b1 = src[OFFSET + i*2 + 3]; 00169 // fetch only R-ch (top half of word) from 2 successive samples 00170 uint32_t a = __PKHTB(a1, a0, 16); 00171 // and also do at symmetry samples 00172 uint32_t b = __PKHTB(b1, b0, 16); 00173 uint32_t d = __QSUB16(b, a); 00174 acc = __SMLAD(c, d, acc); 00175 } 00176 acc *= sign; 00177 int32_t real = src[OFFSET]; 00178 real /= 2; 00179 *dst++ = __PKHTB(real, acc, 16); 00180 src++; 00181 } 00182 } 00183 00184 void 00185 hilbert_transform_save_fir_state(uint32_t *src_tail) 00186 { 00187 int len = sizeof htstat_buf / sizeof(uint32_t); 00188 uint32_t *dst = (uint32_t*)&htstat_buf[0]; 00189 uint32_t *src = src_tail - len; 00190 int i; 00191 for (i = 0; i < len; i++) { 00192 *dst++ = *src++; 00193 } 00194 } 00195 00196 const int16_t cos_sin_table[257][4] = { 00197 { -32767, 10, 0, -804 }, 00198 { -32757, 29, -804, -804 }, 00199 { -32728, 50, -1608, -802 }, 00200 { -32678, 69, -2410, -802 }, 00201 { -32609, 88, -3212, -799 }, 00202 { -32521, 109, -4011, -797 }, 00203 { -32412, 127, -4808, -794 }, 00204 { -32285, 148, -5602, -791 }, 00205 { -32137, 166, -6393, -786 }, 00206 { -31971, 186, -7179, -783 }, 00207 { -31785, 205, -7962, -777 }, 00208 { -31580, 224, -8739, -773 }, 00209 { -31356, 243, -9512, -766 }, 00210 { -31113, 261, -10278, -761 }, 00211 { -30852, 281, -11039, -754 }, 00212 { -30571, 298, -11793, -746 }, 00213 { -30273, 317, -12539, -740 }, 00214 { -29956, 335, -13279, -731 }, 00215 { -29621, 353, -14010, -722 }, 00216 { -29268, 370, -14732, -714 }, 00217 { -28898, 388, -15446, -705 }, 00218 { -28510, 405, -16151, -695 }, 00219 { -28105, 422, -16846, -684 }, 00220 { -27683, 438, -17530, -674 }, 00221 { -27245, 455, -18204, -664 }, 00222 { -26790, 471, -18868, -651 }, 00223 { -26319, 487, -19519, -640 }, 00224 { -25832, 503, -20159, -628 }, 00225 { -25329, 518, -20787, -616 }, 00226 { -24811, 532, -21403, -602 }, 00227 { -24279, 548, -22005, -589 }, 00228 { -23731, 561, -22594, -576 }, 00229 { -23170, 576, -23170, -561 }, 00230 { -22594, 589, -23731, -548 }, 00231 { -22005, 602, -24279, -532 }, 00232 { -21403, 616, -24811, -518 }, 00233 { -20787, 628, -25329, -503 }, 00234 { -20159, 640, -25832, -487 }, 00235 { -19519, 651, -26319, -471 }, 00236 { -18868, 664, -26790, -455 }, 00237 { -18204, 674, -27245, -438 }, 00238 { -17530, 684, -27683, -422 }, 00239 { -16846, 695, -28105, -405 }, 00240 { -16151, 705, -28510, -388 }, 00241 { -15446, 714, -28898, -370 }, 00242 { -14732, 722, -29268, -353 }, 00243 { -14010, 731, -29621, -335 }, 00244 { -13279, 740, -29956, -317 }, 00245 { -12539, 746, -30273, -298 }, 00246 { -11793, 754, -30571, -281 }, 00247 { -11039, 761, -30852, -261 }, 00248 { -10278, 766, -31113, -243 }, 00249 { -9512, 773, -31356, -224 }, 00250 { -8739, 777, -31580, -205 }, 00251 { -7962, 783, -31785, -186 }, 00252 { -7179, 786, -31971, -166 }, 00253 { -6393, 791, -32137, -148 }, 00254 { -5602, 794, -32285, -127 }, 00255 { -4808, 797, -32412, -109 }, 00256 { -4011, 799, -32521, -88 }, 00257 { -3212, 802, -32609, -69 }, 00258 { -2410, 802, -32678, -50 }, 00259 { -1608, 804, -32728, -29 }, 00260 { -804, 804, -32757, -10 }, 00261 { 0, 804, -32767, 10 }, 00262 { 804, 804, -32757, 29 }, 00263 { 1608, 802, -32728, 50 }, 00264 { 2410, 802, -32678, 69 }, 00265 { 3212, 799, -32609, 88 }, 00266 { 4011, 797, -32521, 109 }, 00267 { 4808, 794, -32412, 127 }, 00268 { 5602, 791, -32285, 148 }, 00269 { 6393, 786, -32137, 166 }, 00270 { 7179, 783, -31971, 186 }, 00271 { 7962, 777, -31785, 205 }, 00272 { 8739, 773, -31580, 224 }, 00273 { 9512, 766, -31356, 243 }, 00274 { 10278, 761, -31113, 261 }, 00275 { 11039, 754, -30852, 281 }, 00276 { 11793, 746, -30571, 298 }, 00277 { 12539, 740, -30273, 317 }, 00278 { 13279, 731, -29956, 335 }, 00279 { 14010, 722, -29621, 353 }, 00280 { 14732, 714, -29268, 370 }, 00281 { 15446, 705, -28898, 388 }, 00282 { 16151, 695, -28510, 405 }, 00283 { 16846, 684, -28105, 422 }, 00284 { 17530, 674, -27683, 438 }, 00285 { 18204, 664, -27245, 455 }, 00286 { 18868, 651, -26790, 471 }, 00287 { 19519, 640, -26319, 487 }, 00288 { 20159, 628, -25832, 503 }, 00289 { 20787, 616, -25329, 518 }, 00290 { 21403, 602, -24811, 532 }, 00291 { 22005, 589, -24279, 548 }, 00292 { 22594, 576, -23731, 561 }, 00293 { 23170, 561, -23170, 576 }, 00294 { 23731, 548, -22594, 589 }, 00295 { 24279, 532, -22005, 602 }, 00296 { 24811, 518, -21403, 616 }, 00297 { 25329, 503, -20787, 628 }, 00298 { 25832, 487, -20159, 640 }, 00299 { 26319, 471, -19519, 651 }, 00300 { 26790, 455, -18868, 664 }, 00301 { 27245, 438, -18204, 674 }, 00302 { 27683, 422, -17530, 684 }, 00303 { 28105, 405, -16846, 695 }, 00304 { 28510, 388, -16151, 705 }, 00305 { 28898, 370, -15446, 714 }, 00306 { 29268, 353, -14732, 722 }, 00307 { 29621, 335, -14010, 731 }, 00308 { 29956, 317, -13279, 740 }, 00309 { 30273, 298, -12539, 746 }, 00310 { 30571, 281, -11793, 754 }, 00311 { 30852, 261, -11039, 761 }, 00312 { 31113, 243, -10278, 766 }, 00313 { 31356, 224, -9512, 773 }, 00314 { 31580, 205, -8739, 777 }, 00315 { 31785, 186, -7962, 783 }, 00316 { 31971, 166, -7179, 786 }, 00317 { 32137, 148, -6393, 791 }, 00318 { 32285, 127, -5602, 794 }, 00319 { 32412, 109, -4808, 797 }, 00320 { 32521, 88, -4011, 799 }, 00321 { 32609, 69, -3212, 802 }, 00322 { 32678, 50, -2410, 802 }, 00323 { 32728, 29, -1608, 804 }, 00324 { 32757, 10, -804, 804 }, 00325 { 32767, -10, 0, 804 }, 00326 { 32757, -29, 804, 804 }, 00327 { 32728, -50, 1608, 802 }, 00328 { 32678, -69, 2410, 802 }, 00329 { 32609, -88, 3212, 799 }, 00330 { 32521, -109, 4011, 797 }, 00331 { 32412, -127, 4808, 794 }, 00332 { 32285, -148, 5602, 791 }, 00333 { 32137, -166, 6393, 786 }, 00334 { 31971, -186, 7179, 783 }, 00335 { 31785, -205, 7962, 777 }, 00336 { 31580, -224, 8739, 773 }, 00337 { 31356, -243, 9512, 766 }, 00338 { 31113, -261, 10278, 761 }, 00339 { 30852, -281, 11039, 754 }, 00340 { 30571, -298, 11793, 746 }, 00341 { 30273, -317, 12539, 740 }, 00342 { 29956, -335, 13279, 731 }, 00343 { 29621, -353, 14010, 722 }, 00344 { 29268, -370, 14732, 714 }, 00345 { 28898, -388, 15446, 705 }, 00346 { 28510, -405, 16151, 695 }, 00347 { 28105, -422, 16846, 684 }, 00348 { 27683, -438, 17530, 674 }, 00349 { 27245, -455, 18204, 664 }, 00350 { 26790, -471, 18868, 651 }, 00351 { 26319, -487, 19519, 640 }, 00352 { 25832, -503, 20159, 628 }, 00353 { 25329, -518, 20787, 616 }, 00354 { 24811, -532, 21403, 602 }, 00355 { 24279, -548, 22005, 589 }, 00356 { 23731, -561, 22594, 576 }, 00357 { 23170, -576, 23170, 561 }, 00358 { 22594, -589, 23731, 548 }, 00359 { 22005, -602, 24279, 532 }, 00360 { 21403, -616, 24811, 518 }, 00361 { 20787, -628, 25329, 503 }, 00362 { 20159, -640, 25832, 487 }, 00363 { 19519, -651, 26319, 471 }, 00364 { 18868, -664, 26790, 455 }, 00365 { 18204, -674, 27245, 438 }, 00366 { 17530, -684, 27683, 422 }, 00367 { 16846, -695, 28105, 405 }, 00368 { 16151, -705, 28510, 388 }, 00369 { 15446, -714, 28898, 370 }, 00370 { 14732, -722, 29268, 353 }, 00371 { 14010, -731, 29621, 335 }, 00372 { 13279, -740, 29956, 317 }, 00373 { 12539, -746, 30273, 298 }, 00374 { 11793, -754, 30571, 281 }, 00375 { 11039, -761, 30852, 261 }, 00376 { 10278, -766, 31113, 243 }, 00377 { 9512, -773, 31356, 224 }, 00378 { 8739, -777, 31580, 205 }, 00379 { 7962, -783, 31785, 186 }, 00380 { 7179, -786, 31971, 166 }, 00381 { 6393, -791, 32137, 148 }, 00382 { 5602, -794, 32285, 127 }, 00383 { 4808, -797, 32412, 109 }, 00384 { 4011, -799, 32521, 88 }, 00385 { 3212, -802, 32609, 69 }, 00386 { 2410, -802, 32678, 50 }, 00387 { 1608, -804, 32728, 29 }, 00388 { 804, -804, 32757, 10 }, 00389 { 0, -804, 32767, -10 }, 00390 { -804, -804, 32757, -29 }, 00391 { -1608, -802, 32728, -50 }, 00392 { -2410, -802, 32678, -69 }, 00393 { -3212, -799, 32609, -88 }, 00394 { -4011, -797, 32521, -109 }, 00395 { -4808, -794, 32412, -127 }, 00396 { -5602, -791, 32285, -148 }, 00397 { -6393, -786, 32137, -166 }, 00398 { -7179, -783, 31971, -186 }, 00399 { -7962, -777, 31785, -205 }, 00400 { -8739, -773, 31580, -224 }, 00401 { -9512, -766, 31356, -243 }, 00402 { -10278, -761, 31113, -261 }, 00403 { -11039, -754, 30852, -281 }, 00404 { -11793, -746, 30571, -298 }, 00405 { -12539, -740, 30273, -317 }, 00406 { -13279, -731, 29956, -335 }, 00407 { -14010, -722, 29621, -353 }, 00408 { -14732, -714, 29268, -370 }, 00409 { -15446, -705, 28898, -388 }, 00410 { -16151, -695, 28510, -405 }, 00411 { -16846, -684, 28105, -422 }, 00412 { -17530, -674, 27683, -438 }, 00413 { -18204, -664, 27245, -455 }, 00414 { -18868, -651, 26790, -471 }, 00415 { -19519, -640, 26319, -487 }, 00416 { -20159, -628, 25832, -503 }, 00417 { -20787, -616, 25329, -518 }, 00418 { -21403, -602, 24811, -532 }, 00419 { -22005, -589, 24279, -548 }, 00420 { -22594, -576, 23731, -561 }, 00421 { -23170, -561, 23170, -576 }, 00422 { -23731, -548, 22594, -589 }, 00423 { -24279, -532, 22005, -602 }, 00424 { -24811, -518, 21403, -616 }, 00425 { -25329, -503, 20787, -628 }, 00426 { -25832, -487, 20159, -640 }, 00427 { -26319, -471, 19519, -651 }, 00428 { -26790, -455, 18868, -664 }, 00429 { -27245, -438, 18204, -674 }, 00430 { -27683, -422, 17530, -684 }, 00431 { -28105, -405, 16846, -695 }, 00432 { -28510, -388, 16151, -705 }, 00433 { -28898, -370, 15446, -714 }, 00434 { -29268, -353, 14732, -722 }, 00435 { -29621, -335, 14010, -731 }, 00436 { -29956, -317, 13279, -740 }, 00437 { -30273, -298, 12539, -746 }, 00438 { -30571, -281, 11793, -754 }, 00439 { -30852, -261, 11039, -761 }, 00440 { -31113, -243, 10278, -766 }, 00441 { -31356, -224, 9512, -773 }, 00442 { -31580, -205, 8739, -777 }, 00443 { -31785, -186, 7962, -783 }, 00444 { -31971, -166, 7179, -786 }, 00445 { -32137, -148, 6393, -791 }, 00446 { -32285, -127, 5602, -794 }, 00447 { -32412, -109, 4808, -797 }, 00448 { -32521, -88, 4011, -799 }, 00449 { -32609, -69, 3212, -802 }, 00450 { -32678, -50, 2410, -802 }, 00451 { -32728, -29, 1608, -804 }, 00452 { -32757, -10, 804, -804 }, 00453 { -32767, -10, 0, 0 } 00454 }; 00455 00456 static inline 00457 uint32_t cos_sin(int16_t x) 00458 { 00459 int idx = x / 256; 00460 int mod = x & 0xff; 00461 uint32_t r = __PKHBT(0x0100, mod, 16); 00462 uint32_t *e = (uint32_t *)&cos_sin_table[idx+128]; 00463 uint32_t cd = e[0]; 00464 uint32_t sd = e[1]; 00465 int32_t c = __SMUAD(r, cd); 00466 int32_t s = __SMUAD(r, sd); 00467 c >>= 8; 00468 s >>= 8; 00469 return __PKHBT(s, c, 16); 00470 } 00471 00472 FMModState fmmod; 00473 00474 void 00475 fmmod_init(FMModState *fmmod) 00476 { 00477 fmmod->phase = 0; 00478 } 00479 00480 void 00481 frequency_modulation(FMModState *fmmod, uint32_t *src, uint32_t *dst, int dst_len) 00482 { 00483 int j; 00484 uint16_t phase = fmmod->phase; 00485 for (j = 0; j < dst_len; j++) { 00486 uint32_t s = *src++; 00487 // fetch only R-ch (top half of word) 00488 int16_t x = s >> 16; 00489 phase += x; 00490 *dst++ = cos_sin(phase); 00491 } 00492 fmmod->phase = phase; 00493 } 00494 00495 void 00496 amplitude_modulation(uint32_t *src, uint32_t *dst, int dst_len) 00497 { 00498 int j; 00499 for (j = 0; j < dst_len; j++) { 00500 uint32_t s = *src++; 00501 // fetch only R-ch (top half of word) 00502 int16_t x = s >> 16; 00503 // add DC and set zero at quadrature 00504 x = __SSAT(x + 0x3800, 16); 00505 *dst++ = __PKHBT(x, x, 16); 00506 } 00507 }
Generated on Fri Jul 15 2022 18:34:42 by
1.7.2