T T / Mbed 2 deprecated Nucleo_IQDDS

AD9857 IQ DDS Digital Up Converter Experiment using Nucleo F401

Dependencies:   mbed

Digital Signal Processing for IQ Quadradure Modulation DDS AD9857 using Nucleo F401.

see http://ttrftech.tumblr.com/post/114310226891/

dsp.cpp@6:ed82052bd505, 2015-03-23 (annotated)

Committer:
edy555
Date:
Mon Mar 23 21:53:48 2015 +0000
Revision:
6:ed82052bd505
Parent:
5:75c26157a53a 
use phase accumration in fm modulation, archive very good result!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edy555 0:55201637d936 1 #include "mbed.h"
edy555 0:55201637d936 2 #include "dsp.h"
edy555 0:55201637d936 3
edy555 0:55201637d936 4 extern Serial pc;
edy555 0:55201637d936 5
edy555 0:55201637d936 6
edy555 0:55201637d936 7 int16_t htstat_buf[HILBERT_TRANSFORM_FIRSTATE_LENGTH];
edy555 0:55201637d936 8 int16_t cap_buf[2][CAPTURE_LEN];
edy555 0:55201637d936 9
edy555 0:55201637d936 10 int16_t fir_state[FIRSTATE_LENGTH];
edy555 0:55201637d936 11 int16_t fir_buf[CAPTURE_LEN];
edy555 0:55201637d936 12
edy555 0:55201637d936 13 int16_t cic_buf[CICBUF_LEN];
edy555 0:55201637d936 14 int16_t dma_buf[2][DMA_DATALEN];
edy555 0:55201637d936 15
edy555 0:55201637d936 16 CICState cic;
edy555 0:55201637d936 17
edy555 0:55201637d936 18 void cic_interpolate_x10(CICState *cic, uint32_t *src, int src_len, uint32_t *dst)
edy555 0:55201637d936 19 {
edy555 0:55201637d936 20 uint32_t p0 = cic->p0;
edy555 0:55201637d936 21 uint32_t s0 = cic->s0;
edy555 0:55201637d936 22 int i;
edy555 0:55201637d936 23 int j = 0;
edy555 0:55201637d936 24 for (i = 0; i < src_len; i++) {
edy555 0:55201637d936 25 uint32_t s = src[i];
edy555 3:e6897d74d6bf 26 uint32_t d0 = __QSUB16(s, p0);
edy555 3:e6897d74d6bf 27 s0 = __QADD16(s0, d0);
edy555 3:e6897d74d6bf 28 uint32_t s1 = __QADD16(s0, d0);
edy555 3:e6897d74d6bf 29 uint32_t s2 = __QADD16(s1, d0);
edy555 3:e6897d74d6bf 30 uint32_t s3 = __QADD16(s2, d0);
edy555 3:e6897d74d6bf 31 uint32_t s4 = __QADD16(s3, d0);
edy555 3:e6897d74d6bf 32 dst[j ] = s0 >> 2;
edy555 3:e6897d74d6bf 33 dst[j+1] = s1 >> 2;
edy555 3:e6897d74d6bf 34 dst[j+2] = s2 >> 2;
edy555 3:e6897d74d6bf 35 dst[j+3] = s3 >> 2;
edy555 3:e6897d74d6bf 36 dst[j+4] = s4 >> 2;
edy555 0:55201637d936 37 j += 5;
edy555 0:55201637d936 38 s0 = s4;
edy555 3:e6897d74d6bf 39 s0 = __QADD16(s0, d0);
edy555 3:e6897d74d6bf 40 s1 = __QADD16(s0, d0);
edy555 3:e6897d74d6bf 41 s2 = __QADD16(s1, d0);
edy555 3:e6897d74d6bf 42 s3 = __QADD16(s2, d0);
edy555 3:e6897d74d6bf 43 s4 = __QADD16(s3, d0);
edy555 3:e6897d74d6bf 44 dst[j ] = s0 >> 2;
edy555 3:e6897d74d6bf 45 dst[j+1] = s1 >> 2;
edy555 3:e6897d74d6bf 46 dst[j+2] = s2 >> 2;
edy555 3:e6897d74d6bf 47 dst[j+3] = s3 >> 2;
edy555 3:e6897d74d6bf 48 dst[j+4] = s4 >> 2;
edy555 0:55201637d936 49 j += 5;
edy555 0:55201637d936 50 s0 = s4;
edy555 0:55201637d936 51 p0 = s;
edy555 0:55201637d936 52 }
edy555 0:55201637d936 53 cic->s0 = s0;
edy555 0:55201637d936 54 cic->p0 = p0;
edy555 0:55201637d936 55 }
edy555 0:55201637d936 56
edy555 0:55201637d936 57 const int16_t fir_coeff[8][16] = {
edy555 0:55201637d936 58 { -19, -6, 102, -22, -357, 264, 1798, 2064, 630, -337, -119, 111, 15, -24, 0, 0},
edy555 0:55201637d936 59 { -13, -20, 76, 51, -311, -29, 1441, 2206, 1037, -231, -223, 96, 45, -25, -6, 0},
edy555 0:55201637d936 60 { -6, -25, 45, 96, -223, -231, 1037, 2206, 1441, -29, -311, 51, 76, -20, -13, 0},
edy555 0:55201637d936 61 { 0, -24, 15, 111, -119, -337, 630, 2064, 1798, 264, -357, -22, 102, -6, -19, 0},
edy555 0:55201637d936 62 { 0, -19, -6, 102, -22, -357, 264, 1798, 2064, 630, -337, -119, 111, 15, -24, 0},
edy555 0:55201637d936 63 { 0, -13, -20, 76, 51, -311, -29, 1441, 2206, 1037, -231, -223, 96, 45, -25, -6},
edy555 0:55201637d936 64 { 0, -6, -25, 45, 96, -223, -231, 1037, 2206, 1441, -29, -311, 51, 76, -20, -13},
edy555 0:55201637d936 65 { 0, 0, -24, 15, 111, -119, -337, 630, 2064, 1798, 264, -357, -22, 102, -6, -19}
edy555 0:55201637d936 66 };
edy555 0:55201637d936 67
edy555 0:55201637d936 68 void fir_resample_x4(uint32_t *src_state, uint32_t *dst, int dst_len)
edy555 0:55201637d936 69 {
edy555 0:55201637d936 70 uint32_t *src = src_state;
edy555 0:55201637d936 71 int index = 0;
edy555 0:55201637d936 72 int i;
edy555 0:55201637d936 73 for (i = 0; i < dst_len; i++) {
edy555 0:55201637d936 74 uint32_t *kp = (uint32_t*)fir_coeff[index];
edy555 0:55201637d936 75 uint32_t acc_i = 0;
edy555 0:55201637d936 76 uint32_t acc_q = 0;
edy555 0:55201637d936 77
edy555 0:55201637d936 78 // 8cycle x 8
edy555 0:55201637d936 79 #define CELL(n) do { \
edy555 0:55201637d936 80 uint32_t k = kp[n]; \
edy555 0:55201637d936 81 uint32_t p0 = src[n*2]; \
edy555 0:55201637d936 82 uint32_t p1 = src[n*2+1]; \
edy555 0:55201637d936 83 uint32_t i01 = __PKHTB(p1, p0, 16); \
edy555 0:55201637d936 84 uint32_t q01 = __PKHBT(p0, p1, 16); \
edy555 0:55201637d936 85 acc_i = __SMLAD(k, i01, acc_i); \
edy555 0:55201637d936 86 acc_q = __SMLAD(k, q01, acc_q); \
edy555 0:55201637d936 87 } while(0)
edy555 0:55201637d936 88
edy555 0:55201637d936 89 CELL(0); CELL(1); CELL(2); CELL(3);
edy555 0:55201637d936 90 CELL(4); CELL(5); CELL(6); CELL(7);
edy555 3:e6897d74d6bf 91 *dst++ = __PKHTB(__SSAT16(acc_i, 16), __SSAT16(acc_q, 16), 16);
edy555 0:55201637d936 92
edy555 0:55201637d936 93 index++;
edy555 0:55201637d936 94 if (index >= 8) {
edy555 0:55201637d936 95 index = 0;
edy555 0:55201637d936 96 src += 2;
edy555 0:55201637d936 97 }
edy555 0:55201637d936 98 }
edy555 0:55201637d936 99
edy555 0:55201637d936 100 for (i = 0; i < FIRSTATE_LENGTH/2; i++)
edy555 0:55201637d936 101 *src_state++ = *src++;
edy555 0:55201637d936 102 }
edy555 0:55201637d936 103
edy555 0:55201637d936 104 void
edy555 0:55201637d936 105 interpolate_test()
edy555 0:55201637d936 106 {
edy555 0:55201637d936 107 int freq = 1000;
edy555 0:55201637d936 108 int ampl = 3000;
edy555 0:55201637d936 109 int rate = 48000;
edy555 0:55201637d936 110 for (int i = 0; i < CAPTURE_LEN; i++){
edy555 0:55201637d936 111 cap_buf[0][i*2] = sin(2*3.141592 * i * freq / rate) * ampl; // Lch
edy555 0:55201637d936 112 cap_buf[0][i*2+1] = cos(2*3.141592 * i * freq / rate) * ampl; // Rch
edy555 0:55201637d936 113 }
edy555 0:55201637d936 114 #if 0
edy555 0:55201637d936 115 for (int i = 0; i < 48; i++){
edy555 0:55201637d936 116 pc.printf("%d, ", cap_buf[i*2]);
edy555 0:55201637d936 117 }
edy555 0:55201637d936 118 pc.printf("\n\r");
edy555 0:55201637d936 119 #endif
edy555 0:55201637d936 120 fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN);
edy555 0:55201637d936 121 cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]);
edy555 0:55201637d936 122 fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN);
edy555 0:55201637d936 123 cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]);
edy555 0:55201637d936 124 #if 0
edy555 0:55201637d936 125 for (int i = 0; i < 48*25/4; i++){
edy555 0:55201637d936 126 pc.printf("%d, ", cic_buf[i*2]);
edy555 0:55201637d936 127 }
edy555 0:55201637d936 128 pc.printf("\n\r");
edy555 0:55201637d936 129 #endif
edy555 0:55201637d936 130 #if 0
edy555 0:55201637d936 131 for (int i = DMA_DATALEN/2-400; i < DMA_DATALEN/2; i++){
edy555 0:55201637d936 132 pc.printf("%d, ", dma_buf[i*2]);
edy555 0:55201637d936 133 }
edy555 0:55201637d936 134 pc.printf("\n\r");
edy555 0:55201637d936 135 for (int i = 0; i < 400; i++){
edy555 0:55201637d936 136 pc.printf("%d, ", dma_buf[i*2]);
edy555 0:55201637d936 137 }
edy555 0:55201637d936 138 pc.printf("\n\r");
edy555 0:55201637d936 139 #endif
edy555 0:55201637d936 140 }
edy555 0:55201637d936 141
edy555 0:55201637d936 142 // hilbert transform by 127 taps fir
edy555 0:55201637d936 143 int16_t hilbert127_fir_coeff[32] = {
edy555 0:55201637d936 144 20848, 6917, 4112, 2897, 2212, 1768, 1454, 1219, 1036,
edy555 0:55201637d936 145 887, 764, 661, 572, 496, 429, 371, 319, 274,
edy555 0:55201637d936 146 234, 198, 167, 140, 117, 97, 79, 65, 53,
edy555 0:55201637d936 147 44, 36, 31, 28, 26
edy555 0:55201637d936 148 };
edy555 0:55201637d936 149
edy555 0:55201637d936 150 void
edy555 0:55201637d936 151 hilbert_transform(uint32_t *src, uint32_t *dst, int dst_len, int sign)
edy555 0:55201637d936 152 {
edy555 0:55201637d936 153 int j;
edy555 0:55201637d936 154 int len = sizeof htstat_buf / sizeof(uint32_t);
edy555 0:55201637d936 155 src -= len;
edy555 0:55201637d936 156 // 240 * (208 + 12) = 52800cycle = 0.62ms@84MHz
edy555 0:55201637d936 157 for (j = 0; j < dst_len; j++) {
edy555 0:55201637d936 158 int i;
edy555 0:55201637d936 159 int32_t acc = 0;
edy555 0:55201637d936 160 #define OFFSET 64
edy555 0:55201637d936 161 // 16 * (10 + 3) = 208 cycle
edy555 0:55201637d936 162 for (i = 0; i < 32; i += 2) {
edy555 0:55201637d936 163 // 10 cycle
edy555 0:55201637d936 164 uint32_t c = *(uint32_t*)&hilbert127_fir_coeff[i];
edy555 0:55201637d936 165 uint32_t a0 = src[OFFSET - i*2 - 1];
edy555 0:55201637d936 166 uint32_t a1 = src[OFFSET - i*2 - 3];
edy555 0:55201637d936 167 uint32_t b0 = src[OFFSET + i*2 + 1];
edy555 0:55201637d936 168 uint32_t b1 = src[OFFSET + i*2 + 3];
edy555 0:55201637d936 169 // fetch only R-ch (top half of word) from 2 successive samples
edy555 0:55201637d936 170 uint32_t a = __PKHTB(a1, a0, 16);
edy555 0:55201637d936 171 // and also do at symmetry samples
edy555 0:55201637d936 172 uint32_t b = __PKHTB(b1, b0, 16);
edy555 0:55201637d936 173 uint32_t d = __QSUB16(b, a);
edy555 0:55201637d936 174 acc = __SMLAD(c, d, acc);
edy555 0:55201637d936 175 }
edy555 0:55201637d936 176 acc *= sign;
edy555 3:e6897d74d6bf 177 int32_t real = src[OFFSET];
edy555 3:e6897d74d6bf 178 real /= 2;
edy555 3:e6897d74d6bf 179 *dst++ = __PKHTB(real, acc, 16);
edy555 0:55201637d936 180 src++;
edy555 0:55201637d936 181 }
edy555 0:55201637d936 182 }
edy555 0:55201637d936 183
edy555 0:55201637d936 184 void
edy555 0:55201637d936 185 hilbert_transform_save_fir_state(uint32_t *src_tail)
edy555 0:55201637d936 186 {
edy555 0:55201637d936 187 int len = sizeof htstat_buf / sizeof(uint32_t);
edy555 0:55201637d936 188 uint32_t *dst = (uint32_t*)&htstat_buf[0];
edy555 0:55201637d936 189 uint32_t *src = src_tail - len;
edy555 0:55201637d936 190 int i;
edy555 0:55201637d936 191 for (i = 0; i < len; i++) {
edy555 0:55201637d936 192 *dst++ = *src++;
edy555 0:55201637d936 193 }
edy555 0:55201637d936 194 }
edy555 0:55201637d936 195
edy555 0:55201637d936 196 const int16_t cos_sin_table[257][4] = {
edy555 5:75c26157a53a 197 { -32767, 10, 0, -804 },
edy555 5:75c26157a53a 198 { -32757, 29, -804, -804 },
edy555 5:75c26157a53a 199 { -32728, 50, -1608, -802 },
edy555 5:75c26157a53a 200 { -32678, 69, -2410, -802 },
edy555 5:75c26157a53a 201 { -32609, 88, -3212, -799 },
edy555 5:75c26157a53a 202 { -32521, 109, -4011, -797 },
edy555 5:75c26157a53a 203 { -32412, 127, -4808, -794 },
edy555 5:75c26157a53a 204 { -32285, 148, -5602, -791 },
edy555 5:75c26157a53a 205 { -32137, 166, -6393, -786 },
edy555 5:75c26157a53a 206 { -31971, 186, -7179, -783 },
edy555 5:75c26157a53a 207 { -31785, 205, -7962, -777 },
edy555 5:75c26157a53a 208 { -31580, 224, -8739, -773 },
edy555 5:75c26157a53a 209 { -31356, 243, -9512, -766 },
edy555 5:75c26157a53a 210 { -31113, 261, -10278, -761 },
edy555 5:75c26157a53a 211 { -30852, 281, -11039, -754 },
edy555 5:75c26157a53a 212 { -30571, 298, -11793, -746 },
edy555 5:75c26157a53a 213 { -30273, 317, -12539, -740 },
edy555 5:75c26157a53a 214 { -29956, 335, -13279, -731 },
edy555 5:75c26157a53a 215 { -29621, 353, -14010, -722 },
edy555 5:75c26157a53a 216 { -29268, 370, -14732, -714 },
edy555 5:75c26157a53a 217 { -28898, 388, -15446, -705 },
edy555 5:75c26157a53a 218 { -28510, 405, -16151, -695 },
edy555 5:75c26157a53a 219 { -28105, 422, -16846, -684 },
edy555 5:75c26157a53a 220 { -27683, 438, -17530, -674 },
edy555 5:75c26157a53a 221 { -27245, 455, -18204, -664 },
edy555 5:75c26157a53a 222 { -26790, 471, -18868, -651 },
edy555 5:75c26157a53a 223 { -26319, 487, -19519, -640 },
edy555 5:75c26157a53a 224 { -25832, 503, -20159, -628 },
edy555 5:75c26157a53a 225 { -25329, 518, -20787, -616 },
edy555 5:75c26157a53a 226 { -24811, 532, -21403, -602 },
edy555 5:75c26157a53a 227 { -24279, 548, -22005, -589 },
edy555 5:75c26157a53a 228 { -23731, 561, -22594, -576 },
edy555 5:75c26157a53a 229 { -23170, 576, -23170, -561 },
edy555 5:75c26157a53a 230 { -22594, 589, -23731, -548 },
edy555 5:75c26157a53a 231 { -22005, 602, -24279, -532 },
edy555 5:75c26157a53a 232 { -21403, 616, -24811, -518 },
edy555 5:75c26157a53a 233 { -20787, 628, -25329, -503 },
edy555 5:75c26157a53a 234 { -20159, 640, -25832, -487 },
edy555 5:75c26157a53a 235 { -19519, 651, -26319, -471 },
edy555 5:75c26157a53a 236 { -18868, 664, -26790, -455 },
edy555 5:75c26157a53a 237 { -18204, 674, -27245, -438 },
edy555 5:75c26157a53a 238 { -17530, 684, -27683, -422 },
edy555 5:75c26157a53a 239 { -16846, 695, -28105, -405 },
edy555 5:75c26157a53a 240 { -16151, 705, -28510, -388 },
edy555 5:75c26157a53a 241 { -15446, 714, -28898, -370 },
edy555 5:75c26157a53a 242 { -14732, 722, -29268, -353 },
edy555 5:75c26157a53a 243 { -14010, 731, -29621, -335 },
edy555 5:75c26157a53a 244 { -13279, 740, -29956, -317 },
edy555 5:75c26157a53a 245 { -12539, 746, -30273, -298 },
edy555 5:75c26157a53a 246 { -11793, 754, -30571, -281 },
edy555 5:75c26157a53a 247 { -11039, 761, -30852, -261 },
edy555 5:75c26157a53a 248 { -10278, 766, -31113, -243 },
edy555 5:75c26157a53a 249 { -9512, 773, -31356, -224 },
edy555 5:75c26157a53a 250 { -8739, 777, -31580, -205 },
edy555 5:75c26157a53a 251 { -7962, 783, -31785, -186 },
edy555 5:75c26157a53a 252 { -7179, 786, -31971, -166 },
edy555 5:75c26157a53a 253 { -6393, 791, -32137, -148 },
edy555 5:75c26157a53a 254 { -5602, 794, -32285, -127 },
edy555 5:75c26157a53a 255 { -4808, 797, -32412, -109 },
edy555 5:75c26157a53a 256 { -4011, 799, -32521, -88 },
edy555 5:75c26157a53a 257 { -3212, 802, -32609, -69 },
edy555 5:75c26157a53a 258 { -2410, 802, -32678, -50 },
edy555 5:75c26157a53a 259 { -1608, 804, -32728, -29 },
edy555 5:75c26157a53a 260 { -804, 804, -32757, -10 },
edy555 5:75c26157a53a 261 { 0, 804, -32767, 10 },
edy555 5:75c26157a53a 262 { 804, 804, -32757, 29 },
edy555 5:75c26157a53a 263 { 1608, 802, -32728, 50 },
edy555 5:75c26157a53a 264 { 2410, 802, -32678, 69 },
edy555 5:75c26157a53a 265 { 3212, 799, -32609, 88 },
edy555 5:75c26157a53a 266 { 4011, 797, -32521, 109 },
edy555 5:75c26157a53a 267 { 4808, 794, -32412, 127 },
edy555 5:75c26157a53a 268 { 5602, 791, -32285, 148 },
edy555 5:75c26157a53a 269 { 6393, 786, -32137, 166 },
edy555 5:75c26157a53a 270 { 7179, 783, -31971, 186 },
edy555 5:75c26157a53a 271 { 7962, 777, -31785, 205 },
edy555 5:75c26157a53a 272 { 8739, 773, -31580, 224 },
edy555 5:75c26157a53a 273 { 9512, 766, -31356, 243 },
edy555 5:75c26157a53a 274 { 10278, 761, -31113, 261 },
edy555 5:75c26157a53a 275 { 11039, 754, -30852, 281 },
edy555 5:75c26157a53a 276 { 11793, 746, -30571, 298 },
edy555 5:75c26157a53a 277 { 12539, 740, -30273, 317 },
edy555 5:75c26157a53a 278 { 13279, 731, -29956, 335 },
edy555 5:75c26157a53a 279 { 14010, 722, -29621, 353 },
edy555 5:75c26157a53a 280 { 14732, 714, -29268, 370 },
edy555 5:75c26157a53a 281 { 15446, 705, -28898, 388 },
edy555 5:75c26157a53a 282 { 16151, 695, -28510, 405 },
edy555 5:75c26157a53a 283 { 16846, 684, -28105, 422 },
edy555 5:75c26157a53a 284 { 17530, 674, -27683, 438 },
edy555 5:75c26157a53a 285 { 18204, 664, -27245, 455 },
edy555 5:75c26157a53a 286 { 18868, 651, -26790, 471 },
edy555 5:75c26157a53a 287 { 19519, 640, -26319, 487 },
edy555 5:75c26157a53a 288 { 20159, 628, -25832, 503 },
edy555 5:75c26157a53a 289 { 20787, 616, -25329, 518 },
edy555 5:75c26157a53a 290 { 21403, 602, -24811, 532 },
edy555 5:75c26157a53a 291 { 22005, 589, -24279, 548 },
edy555 5:75c26157a53a 292 { 22594, 576, -23731, 561 },
edy555 5:75c26157a53a 293 { 23170, 561, -23170, 576 },
edy555 5:75c26157a53a 294 { 23731, 548, -22594, 589 },
edy555 5:75c26157a53a 295 { 24279, 532, -22005, 602 },
edy555 5:75c26157a53a 296 { 24811, 518, -21403, 616 },
edy555 5:75c26157a53a 297 { 25329, 503, -20787, 628 },
edy555 5:75c26157a53a 298 { 25832, 487, -20159, 640 },
edy555 5:75c26157a53a 299 { 26319, 471, -19519, 651 },
edy555 5:75c26157a53a 300 { 26790, 455, -18868, 664 },
edy555 5:75c26157a53a 301 { 27245, 438, -18204, 674 },
edy555 5:75c26157a53a 302 { 27683, 422, -17530, 684 },
edy555 5:75c26157a53a 303 { 28105, 405, -16846, 695 },
edy555 5:75c26157a53a 304 { 28510, 388, -16151, 705 },
edy555 5:75c26157a53a 305 { 28898, 370, -15446, 714 },
edy555 5:75c26157a53a 306 { 29268, 353, -14732, 722 },
edy555 5:75c26157a53a 307 { 29621, 335, -14010, 731 },
edy555 5:75c26157a53a 308 { 29956, 317, -13279, 740 },
edy555 5:75c26157a53a 309 { 30273, 298, -12539, 746 },
edy555 5:75c26157a53a 310 { 30571, 281, -11793, 754 },
edy555 5:75c26157a53a 311 { 30852, 261, -11039, 761 },
edy555 5:75c26157a53a 312 { 31113, 243, -10278, 766 },
edy555 5:75c26157a53a 313 { 31356, 224, -9512, 773 },
edy555 5:75c26157a53a 314 { 31580, 205, -8739, 777 },
edy555 5:75c26157a53a 315 { 31785, 186, -7962, 783 },
edy555 5:75c26157a53a 316 { 31971, 166, -7179, 786 },
edy555 5:75c26157a53a 317 { 32137, 148, -6393, 791 },
edy555 5:75c26157a53a 318 { 32285, 127, -5602, 794 },
edy555 5:75c26157a53a 319 { 32412, 109, -4808, 797 },
edy555 5:75c26157a53a 320 { 32521, 88, -4011, 799 },
edy555 5:75c26157a53a 321 { 32609, 69, -3212, 802 },
edy555 5:75c26157a53a 322 { 32678, 50, -2410, 802 },
edy555 5:75c26157a53a 323 { 32728, 29, -1608, 804 },
edy555 5:75c26157a53a 324 { 32757, 10, -804, 804 },
edy555 5:75c26157a53a 325 { 32767, -10, 0, 804 },
edy555 5:75c26157a53a 326 { 32757, -29, 804, 804 },
edy555 5:75c26157a53a 327 { 32728, -50, 1608, 802 },
edy555 5:75c26157a53a 328 { 32678, -69, 2410, 802 },
edy555 5:75c26157a53a 329 { 32609, -88, 3212, 799 },
edy555 5:75c26157a53a 330 { 32521, -109, 4011, 797 },
edy555 5:75c26157a53a 331 { 32412, -127, 4808, 794 },
edy555 5:75c26157a53a 332 { 32285, -148, 5602, 791 },
edy555 5:75c26157a53a 333 { 32137, -166, 6393, 786 },
edy555 5:75c26157a53a 334 { 31971, -186, 7179, 783 },
edy555 5:75c26157a53a 335 { 31785, -205, 7962, 777 },
edy555 5:75c26157a53a 336 { 31580, -224, 8739, 773 },
edy555 5:75c26157a53a 337 { 31356, -243, 9512, 766 },
edy555 5:75c26157a53a 338 { 31113, -261, 10278, 761 },
edy555 5:75c26157a53a 339 { 30852, -281, 11039, 754 },
edy555 5:75c26157a53a 340 { 30571, -298, 11793, 746 },
edy555 5:75c26157a53a 341 { 30273, -317, 12539, 740 },
edy555 5:75c26157a53a 342 { 29956, -335, 13279, 731 },
edy555 5:75c26157a53a 343 { 29621, -353, 14010, 722 },
edy555 5:75c26157a53a 344 { 29268, -370, 14732, 714 },
edy555 5:75c26157a53a 345 { 28898, -388, 15446, 705 },
edy555 5:75c26157a53a 346 { 28510, -405, 16151, 695 },
edy555 5:75c26157a53a 347 { 28105, -422, 16846, 684 },
edy555 5:75c26157a53a 348 { 27683, -438, 17530, 674 },
edy555 5:75c26157a53a 349 { 27245, -455, 18204, 664 },
edy555 5:75c26157a53a 350 { 26790, -471, 18868, 651 },
edy555 5:75c26157a53a 351 { 26319, -487, 19519, 640 },
edy555 5:75c26157a53a 352 { 25832, -503, 20159, 628 },
edy555 5:75c26157a53a 353 { 25329, -518, 20787, 616 },
edy555 5:75c26157a53a 354 { 24811, -532, 21403, 602 },
edy555 5:75c26157a53a 355 { 24279, -548, 22005, 589 },
edy555 5:75c26157a53a 356 { 23731, -561, 22594, 576 },
edy555 5:75c26157a53a 357 { 23170, -576, 23170, 561 },
edy555 5:75c26157a53a 358 { 22594, -589, 23731, 548 },
edy555 5:75c26157a53a 359 { 22005, -602, 24279, 532 },
edy555 5:75c26157a53a 360 { 21403, -616, 24811, 518 },
edy555 5:75c26157a53a 361 { 20787, -628, 25329, 503 },
edy555 5:75c26157a53a 362 { 20159, -640, 25832, 487 },
edy555 5:75c26157a53a 363 { 19519, -651, 26319, 471 },
edy555 5:75c26157a53a 364 { 18868, -664, 26790, 455 },
edy555 5:75c26157a53a 365 { 18204, -674, 27245, 438 },
edy555 5:75c26157a53a 366 { 17530, -684, 27683, 422 },
edy555 5:75c26157a53a 367 { 16846, -695, 28105, 405 },
edy555 5:75c26157a53a 368 { 16151, -705, 28510, 388 },
edy555 5:75c26157a53a 369 { 15446, -714, 28898, 370 },
edy555 5:75c26157a53a 370 { 14732, -722, 29268, 353 },
edy555 5:75c26157a53a 371 { 14010, -731, 29621, 335 },
edy555 5:75c26157a53a 372 { 13279, -740, 29956, 317 },
edy555 5:75c26157a53a 373 { 12539, -746, 30273, 298 },
edy555 5:75c26157a53a 374 { 11793, -754, 30571, 281 },
edy555 5:75c26157a53a 375 { 11039, -761, 30852, 261 },
edy555 5:75c26157a53a 376 { 10278, -766, 31113, 243 },
edy555 5:75c26157a53a 377 { 9512, -773, 31356, 224 },
edy555 5:75c26157a53a 378 { 8739, -777, 31580, 205 },
edy555 5:75c26157a53a 379 { 7962, -783, 31785, 186 },
edy555 5:75c26157a53a 380 { 7179, -786, 31971, 166 },
edy555 5:75c26157a53a 381 { 6393, -791, 32137, 148 },
edy555 5:75c26157a53a 382 { 5602, -794, 32285, 127 },
edy555 5:75c26157a53a 383 { 4808, -797, 32412, 109 },
edy555 5:75c26157a53a 384 { 4011, -799, 32521, 88 },
edy555 5:75c26157a53a 385 { 3212, -802, 32609, 69 },
edy555 5:75c26157a53a 386 { 2410, -802, 32678, 50 },
edy555 5:75c26157a53a 387 { 1608, -804, 32728, 29 },
edy555 5:75c26157a53a 388 { 804, -804, 32757, 10 },
edy555 5:75c26157a53a 389 { 0, -804, 32767, -10 },
edy555 5:75c26157a53a 390 { -804, -804, 32757, -29 },
edy555 5:75c26157a53a 391 { -1608, -802, 32728, -50 },
edy555 5:75c26157a53a 392 { -2410, -802, 32678, -69 },
edy555 5:75c26157a53a 393 { -3212, -799, 32609, -88 },
edy555 5:75c26157a53a 394 { -4011, -797, 32521, -109 },
edy555 5:75c26157a53a 395 { -4808, -794, 32412, -127 },
edy555 5:75c26157a53a 396 { -5602, -791, 32285, -148 },
edy555 5:75c26157a53a 397 { -6393, -786, 32137, -166 },
edy555 5:75c26157a53a 398 { -7179, -783, 31971, -186 },
edy555 5:75c26157a53a 399 { -7962, -777, 31785, -205 },
edy555 5:75c26157a53a 400 { -8739, -773, 31580, -224 },
edy555 5:75c26157a53a 401 { -9512, -766, 31356, -243 },
edy555 5:75c26157a53a 402 { -10278, -761, 31113, -261 },
edy555 5:75c26157a53a 403 { -11039, -754, 30852, -281 },
edy555 5:75c26157a53a 404 { -11793, -746, 30571, -298 },
edy555 5:75c26157a53a 405 { -12539, -740, 30273, -317 },
edy555 5:75c26157a53a 406 { -13279, -731, 29956, -335 },
edy555 5:75c26157a53a 407 { -14010, -722, 29621, -353 },
edy555 5:75c26157a53a 408 { -14732, -714, 29268, -370 },
edy555 5:75c26157a53a 409 { -15446, -705, 28898, -388 },
edy555 5:75c26157a53a 410 { -16151, -695, 28510, -405 },
edy555 5:75c26157a53a 411 { -16846, -684, 28105, -422 },
edy555 5:75c26157a53a 412 { -17530, -674, 27683, -438 },
edy555 5:75c26157a53a 413 { -18204, -664, 27245, -455 },
edy555 5:75c26157a53a 414 { -18868, -651, 26790, -471 },
edy555 5:75c26157a53a 415 { -19519, -640, 26319, -487 },
edy555 5:75c26157a53a 416 { -20159, -628, 25832, -503 },
edy555 5:75c26157a53a 417 { -20787, -616, 25329, -518 },
edy555 5:75c26157a53a 418 { -21403, -602, 24811, -532 },
edy555 5:75c26157a53a 419 { -22005, -589, 24279, -548 },
edy555 5:75c26157a53a 420 { -22594, -576, 23731, -561 },
edy555 5:75c26157a53a 421 { -23170, -561, 23170, -576 },
edy555 5:75c26157a53a 422 { -23731, -548, 22594, -589 },
edy555 5:75c26157a53a 423 { -24279, -532, 22005, -602 },
edy555 5:75c26157a53a 424 { -24811, -518, 21403, -616 },
edy555 5:75c26157a53a 425 { -25329, -503, 20787, -628 },
edy555 5:75c26157a53a 426 { -25832, -487, 20159, -640 },
edy555 5:75c26157a53a 427 { -26319, -471, 19519, -651 },
edy555 5:75c26157a53a 428 { -26790, -455, 18868, -664 },
edy555 5:75c26157a53a 429 { -27245, -438, 18204, -674 },
edy555 5:75c26157a53a 430 { -27683, -422, 17530, -684 },
edy555 5:75c26157a53a 431 { -28105, -405, 16846, -695 },
edy555 5:75c26157a53a 432 { -28510, -388, 16151, -705 },
edy555 5:75c26157a53a 433 { -28898, -370, 15446, -714 },
edy555 5:75c26157a53a 434 { -29268, -353, 14732, -722 },
edy555 5:75c26157a53a 435 { -29621, -335, 14010, -731 },
edy555 5:75c26157a53a 436 { -29956, -317, 13279, -740 },
edy555 5:75c26157a53a 437 { -30273, -298, 12539, -746 },
edy555 5:75c26157a53a 438 { -30571, -281, 11793, -754 },
edy555 5:75c26157a53a 439 { -30852, -261, 11039, -761 },
edy555 5:75c26157a53a 440 { -31113, -243, 10278, -766 },
edy555 5:75c26157a53a 441 { -31356, -224, 9512, -773 },
edy555 5:75c26157a53a 442 { -31580, -205, 8739, -777 },
edy555 5:75c26157a53a 443 { -31785, -186, 7962, -783 },
edy555 5:75c26157a53a 444 { -31971, -166, 7179, -786 },
edy555 5:75c26157a53a 445 { -32137, -148, 6393, -791 },
edy555 5:75c26157a53a 446 { -32285, -127, 5602, -794 },
edy555 5:75c26157a53a 447 { -32412, -109, 4808, -797 },
edy555 5:75c26157a53a 448 { -32521, -88, 4011, -799 },
edy555 5:75c26157a53a 449 { -32609, -69, 3212, -802 },
edy555 5:75c26157a53a 450 { -32678, -50, 2410, -802 },
edy555 5:75c26157a53a 451 { -32728, -29, 1608, -804 },
edy555 5:75c26157a53a 452 { -32757, -10, 804, -804 },
edy555 5:75c26157a53a 453 { -32767, -10, 0, 0 }
edy555 0:55201637d936 454 };
edy555 0:55201637d936 455
edy555 0:55201637d936 456 static inline
edy555 0:55201637d936 457 uint32_t cos_sin(int16_t x)
edy555 0:55201637d936 458 {
edy555 5:75c26157a53a 459 int idx = x / 256;
edy555 0:55201637d936 460 int mod = x & 0xff;
edy555 5:75c26157a53a 461 uint32_t r = __PKHBT(0x0100, mod, 16);
edy555 0:55201637d936 462 uint32_t *e = (uint32_t *)&cos_sin_table[idx+128];
edy555 5:75c26157a53a 463 uint32_t cd = e[0];
edy555 5:75c26157a53a 464 uint32_t sd = e[1];
edy555 5:75c26157a53a 465 int32_t c = __SMUAD(r, cd);
edy555 5:75c26157a53a 466 int32_t s = __SMUAD(r, sd);
edy555 0:55201637d936 467 c >>= 8;
edy555 0:55201637d936 468 s >>= 8;
edy555 0:55201637d936 469 return __PKHBT(s, c, 16);
edy555 0:55201637d936 470 }
edy555 0:55201637d936 471
edy555 0:55201637d936 472 FMModState fmmod;
edy555 0:55201637d936 473
edy555 0:55201637d936 474 void
edy555 0:55201637d936 475 fmmod_init(FMModState *fmmod)
edy555 0:55201637d936 476 {
edy555 6:ed82052bd505 477 fmmod->phase = 0;
edy555 0:55201637d936 478 }
edy555 0:55201637d936 479
edy555 0:55201637d936 480 void
edy555 0:55201637d936 481 frequency_modulation(FMModState *fmmod, uint32_t *src, uint32_t *dst, int dst_len)
edy555 0:55201637d936 482 {
edy555 0:55201637d936 483 int j;
edy555 6:ed82052bd505 484 uint16_t phase = fmmod->phase;
edy555 0:55201637d936 485 for (j = 0; j < dst_len; j++) {
edy555 0:55201637d936 486 uint32_t s = *src++;
edy555 0:55201637d936 487 // fetch only R-ch (top half of word)
edy555 0:55201637d936 488 int16_t x = s >> 16;
edy555 6:ed82052bd505 489 phase += x;
edy555 6:ed82052bd505 490 *dst++ = cos_sin(phase);
edy555 0:55201637d936 491 }
edy555 6:ed82052bd505 492 fmmod->phase = phase;
edy555 0:55201637d936 493 }
edy555 0:55201637d936 494
edy555 0:55201637d936 495 void
edy555 0:55201637d936 496 amplitude_modulation(uint32_t *src, uint32_t *dst, int dst_len)
edy555 0:55201637d936 497 {
edy555 0:55201637d936 498 int j;
edy555 0:55201637d936 499 for (j = 0; j < dst_len; j++) {
edy555 0:55201637d936 500 uint32_t s = *src++;
edy555 5:75c26157a53a 501 // fetch only R-ch (top half of word)
edy555 0:55201637d936 502 int16_t x = s >> 16;
edy555 0:55201637d936 503 // add DC and set zero at quadrature
edy555 4:dfd05c26edbe 504 x = __SSAT(x + 0x3800, 16);
edy555 4:dfd05c26edbe 505 *dst++ = __PKHBT(x, x, 16);
edy555 0:55201637d936 506 }
edy555 0:55201637d936 507 }