Diff: dsp.cpp

Revision:

0:55201637d936

Child:

3:e6897d74d6bf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dsp.cpp	Sat Mar 21 08:31:20 2015 +0000
@@ -0,0 +1,522 @@
+#include "mbed.h"
+#include "dsp.h"
+
+extern Serial pc;
+
+
+int16_t htstat_buf[HILBERT_TRANSFORM_FIRSTATE_LENGTH];
+int16_t cap_buf[2][CAPTURE_LEN];
+
+int16_t fir_state[FIRSTATE_LENGTH];
+int16_t fir_buf[CAPTURE_LEN];
+
+int16_t cic_buf[CICBUF_LEN];
+int16_t dma_buf[2][DMA_DATALEN];
+
+CICState cic;
+
+void cic_interpolate_x10(CICState *cic, uint32_t *src, int src_len, uint32_t *dst)
+{
+    uint32_t p0 = cic->p0;
+    uint32_t s0 = cic->s0;
+    int i;
+    int j = 0;
+    for (i = 0; i < src_len; i++) {
+        uint32_t s = src[i];
+        uint32_t d0 = __SSUB16(s, p0);
+        s0 = __SADD16(s0, d0);        
+        uint32_t s1 = __SADD16(s0, d0);        
+        uint32_t s2 = __SADD16(s1, d0);        
+        uint32_t s3 = __SADD16(s2, d0);        
+        uint32_t s4 = __SADD16(s3, d0);        
+        dst[j  ] = s0;
+        dst[j+1] = s1;
+        dst[j+2] = s2;
+        dst[j+3] = s3;
+        dst[j+4] = s4;
+        j += 5;
+        s0 = s4;
+        s0 = __SADD16(s0, d0);        
+        s1 = __SADD16(s0, d0);
+        s2 = __SADD16(s1, d0);        
+        s3 = __SADD16(s2, d0);        
+        s4 = __SADD16(s3, d0);        
+        dst[j  ] = s0;
+        dst[j+1] = s1;
+        dst[j+2] = s2;
+        dst[j+3] = s3;
+        dst[j+4] = s4;
+        j += 5;
+        s0 = s4;
+        p0 = s;
+    }
+    cic->s0 = s0;
+    cic->p0 = p0;
+}
+
+const int16_t fir_coeff[8][16] = {
+       { -19,   -6,  102,  -22, -357,  264, 1798, 2064,  630, -337, -119,  111,   15,  -24,    0,    0},
+       { -13,  -20,   76,   51, -311,  -29, 1441, 2206, 1037, -231, -223,   96,   45,  -25,   -6,    0},
+       {  -6,  -25,   45,   96, -223, -231, 1037, 2206, 1441,  -29, -311,   51,   76,  -20,  -13,    0},
+       {   0,  -24,   15,  111, -119, -337,  630, 2064, 1798,  264, -357,  -22,  102,   -6,  -19,    0},
+       {   0,  -19,   -6,  102,  -22, -357,  264, 1798, 2064,  630, -337, -119,  111,   15,  -24,    0},
+       {   0,  -13,  -20,   76,   51, -311,  -29, 1441, 2206, 1037, -231, -223,   96,   45,  -25,   -6},
+       {   0,   -6,  -25,   45,   96, -223, -231, 1037, 2206, 1441,  -29, -311,   51,   76,  -20,  -13},
+       {   0,    0,  -24,   15,  111, -119, -337,  630, 2064, 1798,  264, -357,  -22,  102,   -6,  -19}
+};
+
+void fir_resample_x4(uint32_t *src_state, uint32_t *dst, int dst_len)
+{
+    uint32_t *src = src_state;
+    int index = 0;
+    int i;
+    for (i = 0; i < dst_len; i++) {
+        uint32_t *kp = (uint32_t*)fir_coeff[index];
+        uint32_t acc_i = 0;
+        uint32_t acc_q = 0;
+
+        // 8cycle x 8       
+#define CELL(n) do { \
+        uint32_t k = kp[n]; \
+        uint32_t p0 = src[n*2]; \
+        uint32_t p1 = src[n*2+1]; \
+        uint32_t i01 = __PKHTB(p1, p0, 16); \
+        uint32_t q01 = __PKHBT(p0, p1, 16); \
+        acc_i = __SMLAD(k, i01, acc_i); \
+        acc_q = __SMLAD(k, q01, acc_q); \
+} while(0)
+
+        CELL(0);  CELL(1);  CELL(2);  CELL(3);
+        CELL(4);  CELL(5);  CELL(6);  CELL(7);
+        *dst++ = __SSAT16(__PKHTB(acc_i, acc_q, 16), 11);
+
+        index++;
+        if (index >= 8) {
+            index = 0;
+            src += 2;
+        }
+    }
+    
+    for (i = 0; i < FIRSTATE_LENGTH/2; i++)
+        *src_state++ = *src++;
+}
+
+void
+interpolate_test()
+{
+    int freq = 1000;
+    int ampl = 3000;
+    int rate = 48000;
+    for (int i = 0; i < CAPTURE_LEN; i++){
+        cap_buf[0][i*2] = sin(2*3.141592 * i * freq / rate) * ampl; // Lch
+        cap_buf[0][i*2+1] = cos(2*3.141592 * i * freq / rate) * ampl; // Rch 
+    }
+#if 0
+    for (int i = 0; i < 48; i++){
+        pc.printf("%d, ", cap_buf[i*2]);
+    }
+    pc.printf("\n\r");
+#endif
+    fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN);
+    cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]);
+    fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN);
+    cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]);
+#if 0
+    for (int i = 0; i < 48*25/4; i++){
+        pc.printf("%d, ", cic_buf[i*2]);
+    }
+    pc.printf("\n\r");
+#endif
+#if 0
+    for (int i = DMA_DATALEN/2-400; i < DMA_DATALEN/2; i++){
+        pc.printf("%d, ", dma_buf[i*2]);
+    }
+    pc.printf("\n\r");
+    for (int i = 0; i < 400; i++){
+        pc.printf("%d, ", dma_buf[i*2]);
+    }
+    pc.printf("\n\r");
+#endif
+}
+
+// hilbert transform by 127 taps fir
+int16_t hilbert127_fir_coeff[32] = {
+        20848,  6917,  4112,  2897,  2212,  1768,  1454,  1219,  1036,
+          887,   764,   661,   572,   496,   429,   371,   319,   274,
+          234,   198,   167,   140,   117,    97,    79,    65,    53,
+           44,    36,    31,    28,    26
+};
+
+void
+hilbert_transform(uint32_t *src, uint32_t *dst, int dst_len, int sign)
+{
+    int j;
+    int len = sizeof htstat_buf / sizeof(uint32_t);
+    src -= len;
+    // 240 * (208 + 12) = 52800cycle = 0.62ms@84MHz
+    for (j = 0; j < dst_len; j++) {
+        int i;
+        int32_t acc = 0;
+#define OFFSET 64
+        // 16 * (10 + 3) = 208 cycle
+        for (i = 0; i < 32; i += 2) {
+            // 10 cycle
+            uint32_t c = *(uint32_t*)&hilbert127_fir_coeff[i];
+            uint32_t a0 = src[OFFSET - i*2 - 1];
+            uint32_t a1 = src[OFFSET - i*2 - 3];
+            uint32_t b0 = src[OFFSET + i*2 + 1];
+            uint32_t b1 = src[OFFSET + i*2 + 3];
+            // fetch only R-ch (top half of word) from 2 successive samples
+            uint32_t a = __PKHTB(a1, a0, 16);
+            // and also do at symmetry samples
+            uint32_t b = __PKHTB(b1, b0, 16);
+            uint32_t d = __QSUB16(b, a);
+            acc = __SMLAD(c, d, acc);
+        }
+        acc *= sign;
+        uint32_t real = src[OFFSET];
+        *dst++ = __PKHTB(real, acc, 15);
+        src++;
+    }
+}
+
+void
+hilbert_transform_save_fir_state(uint32_t *src_tail)
+{
+    int len = sizeof htstat_buf / sizeof(uint32_t);
+    uint32_t *dst = (uint32_t*)&htstat_buf[0];
+    uint32_t *src = src_tail - len;
+    int i;
+    for (i = 0; i < len; i++) {
+        *dst++ = *src++;
+    }
+}
+
+const int16_t cos_sin_table[257][4] = {
+    {     0,  402, -32767,    2 },
+    {   402,  402, -32765,    8 },
+    {   804,  402, -32757,   12 },
+    {  1206,  402, -32745,   17 },
+    {  1608,  401, -32728,   23 },
+    {  2009,  401, -32705,   27 },
+    {  2410,  401, -32678,   32 },
+    {  2811,  401, -32646,   37 },
+    {  3212,  400, -32609,   42 },
+    {  3612,  399, -32567,   46 },
+    {  4011,  399, -32521,   52 },
+    {  4410,  398, -32469,   57 },
+    {  4808,  397, -32412,   61 },
+    {  5205,  397, -32351,   66 },
+    {  5602,  396, -32285,   72 },
+    {  5998,  395, -32213,   76 },
+    {  6393,  393, -32137,   80 },
+    {  6786,  393, -32057,   86 },
+    {  7179,  392, -31971,   91 },
+    {  7571,  391, -31880,   95 },
+    {  7962,  389, -31785,  100 },
+    {  8351,  388, -31685,  105 },
+    {  8739,  387, -31580,  110 },
+    {  9126,  386, -31470,  114 },
+    {  9512,  384, -31356,  119 },
+    {  9896,  382, -31237,  124 },
+    { 10278,  381, -31113,  128 },
+    { 10659,  380, -30985,  133 },
+    { 11039,  378, -30852,  138 },
+    { 11417,  376, -30714,  143 },
+    { 11793,  374, -30571,  147 },
+    { 12167,  372, -30424,  151 },
+    { 12539,  371, -30273,  156 },
+    { 12910,  369, -30117,  161 },
+    { 13279,  366, -29956,  165 },
+    { 13645,  365, -29791,  170 },
+    { 14010,  362, -29621,  174 },
+    { 14372,  360, -29447,  179 },
+    { 14732,  358, -29268,  183 },
+    { 15090,  356, -29085,  187 },
+    { 15446,  354, -28898,  192 },
+    { 15800,  351, -28706,  196 },
+    { 16151,  348, -28510,  200 },
+    { 16499,  347, -28310,  205 },
+    { 16846,  343, -28105,  209 },
+    { 17189,  341, -27896,  213 },
+    { 17530,  339, -27683,  217 },
+    { 17869,  335, -27466,  221 },
+    { 18204,  333, -27245,  226 },
+    { 18537,  331, -27019,  229 },
+    { 18868,  327, -26790,  234 },
+    { 19195,  324, -26556,  237 },
+    { 19519,  322, -26319,  242 },
+    { 19841,  318, -26077,  245 },
+    { 20159,  316, -25832,  250 },
+    { 20475,  312, -25582,  253 },
+    { 20787,  309, -25329,  257 },
+    { 21096,  307, -25072,  261 },
+    { 21403,  302, -24811,  264 },
+    { 21705,  300, -24547,  268 },
+    { 22005,  296, -24279,  272 },
+    { 22301,  293, -24007,  276 },
+    { 22594,  290, -23731,  279 },
+    { 22884,  286, -23452,  282 },
+    { 23170,  282, -23170,  286 },
+    { 23452,  279, -22884,  290 },
+    { 23731,  276, -22594,  293 },
+    { 24007,  272, -22301,  296 },
+    { 24279,  268, -22005,  300 },
+    { 24547,  264, -21705,  302 },
+    { 24811,  261, -21403,  307 },
+    { 25072,  257, -21096,  309 },
+    { 25329,  253, -20787,  312 },
+    { 25582,  250, -20475,  316 },
+    { 25832,  245, -20159,  318 },
+    { 26077,  242, -19841,  322 },
+    { 26319,  237, -19519,  324 },
+    { 26556,  234, -19195,  327 },
+    { 26790,  229, -18868,  331 },
+    { 27019,  226, -18537,  333 },
+    { 27245,  221, -18204,  335 },
+    { 27466,  217, -17869,  339 },
+    { 27683,  213, -17530,  341 },
+    { 27896,  209, -17189,  343 },
+    { 28105,  205, -16846,  347 },
+    { 28310,  200, -16499,  348 },
+    { 28510,  196, -16151,  351 },
+    { 28706,  192, -15800,  354 },
+    { 28898,  187, -15446,  356 },
+    { 29085,  183, -15090,  358 },
+    { 29268,  179, -14732,  360 },
+    { 29447,  174, -14372,  362 },
+    { 29621,  170, -14010,  365 },
+    { 29791,  165, -13645,  366 },
+    { 29956,  161, -13279,  369 },
+    { 30117,  156, -12910,  371 },
+    { 30273,  151, -12539,  372 },
+    { 30424,  147, -12167,  374 },
+    { 30571,  143, -11793,  376 },
+    { 30714,  138, -11417,  378 },
+    { 30852,  133, -11039,  380 },
+    { 30985,  128, -10659,  381 },
+    { 31113,  124, -10278,  382 },
+    { 31237,  119,  -9896,  384 },
+    { 31356,  114,  -9512,  386 },
+    { 31470,  110,  -9126,  387 },
+    { 31580,  105,  -8739,  388 },
+    { 31685,  100,  -8351,  389 },
+    { 31785,   95,  -7962,  391 },
+    { 31880,   91,  -7571,  392 },
+    { 31971,   86,  -7179,  393 },
+    { 32057,   80,  -6786,  393 },
+    { 32137,   76,  -6393,  395 },
+    { 32213,   72,  -5998,  396 },
+    { 32285,   66,  -5602,  397 },
+    { 32351,   61,  -5205,  397 },
+    { 32412,   57,  -4808,  398 },
+    { 32469,   52,  -4410,  399 },
+    { 32521,   46,  -4011,  399 },
+    { 32567,   42,  -3612,  400 },
+    { 32609,   37,  -3212,  401 },
+    { 32646,   32,  -2811,  401 },
+    { 32678,   27,  -2410,  401 },
+    { 32705,   23,  -2009,  401 },
+    { 32728,   17,  -1608,  402 },
+    { 32745,   12,  -1206,  402 },
+    { 32757,    8,   -804,  402 },
+    { 32765,    2,   -402,  402 },
+    { 32767,   -2,      0,  402 },
+    { 32765,   -8,    402,  402 },
+    { 32757,  -12,    804,  402 },
+    { 32745,  -17,   1206,  402 },
+    { 32728,  -23,   1608,  401 },
+    { 32705,  -27,   2009,  401 },
+    { 32678,  -32,   2410,  401 },
+    { 32646,  -37,   2811,  401 },
+    { 32609,  -42,   3212,  400 },
+    { 32567,  -46,   3612,  399 },
+    { 32521,  -52,   4011,  399 },
+    { 32469,  -57,   4410,  398 },
+    { 32412,  -61,   4808,  397 },
+    { 32351,  -66,   5205,  397 },
+    { 32285,  -72,   5602,  396 },
+    { 32213,  -76,   5998,  395 },
+    { 32137,  -80,   6393,  393 },
+    { 32057,  -86,   6786,  393 },
+    { 31971,  -91,   7179,  392 },
+    { 31880,  -95,   7571,  391 },
+    { 31785, -100,   7962,  389 },
+    { 31685, -105,   8351,  388 },
+    { 31580, -110,   8739,  387 },
+    { 31470, -114,   9126,  386 },
+    { 31356, -119,   9512,  384 },
+    { 31237, -124,   9896,  382 },
+    { 31113, -128,  10278,  381 },
+    { 30985, -133,  10659,  380 },
+    { 30852, -138,  11039,  378 },
+    { 30714, -143,  11417,  376 },
+    { 30571, -147,  11793,  374 },
+    { 30424, -151,  12167,  372 },
+    { 30273, -156,  12539,  371 },
+    { 30117, -161,  12910,  369 },
+    { 29956, -165,  13279,  366 },
+    { 29791, -170,  13645,  365 },
+    { 29621, -174,  14010,  362 },
+    { 29447, -179,  14372,  360 },
+    { 29268, -183,  14732,  358 },
+    { 29085, -187,  15090,  356 },
+    { 28898, -192,  15446,  354 },
+    { 28706, -196,  15800,  351 },
+    { 28510, -200,  16151,  348 },
+    { 28310, -205,  16499,  347 },
+    { 28105, -209,  16846,  343 },
+    { 27896, -213,  17189,  341 },
+    { 27683, -217,  17530,  339 },
+    { 27466, -221,  17869,  335 },
+    { 27245, -226,  18204,  333 },
+    { 27019, -229,  18537,  331 },
+    { 26790, -234,  18868,  327 },
+    { 26556, -237,  19195,  324 },
+    { 26319, -242,  19519,  322 },
+    { 26077, -245,  19841,  318 },
+    { 25832, -250,  20159,  316 },
+    { 25582, -253,  20475,  312 },
+    { 25329, -257,  20787,  309 },
+    { 25072, -261,  21096,  307 },
+    { 24811, -264,  21403,  302 },
+    { 24547, -268,  21705,  300 },
+    { 24279, -272,  22005,  296 },
+    { 24007, -276,  22301,  293 },
+    { 23731, -279,  22594,  290 },
+    { 23452, -282,  22884,  286 },
+    { 23170, -286,  23170,  282 },
+    { 22884, -290,  23452,  279 },
+    { 22594, -293,  23731,  276 },
+    { 22301, -296,  24007,  272 },
+    { 22005, -300,  24279,  268 },
+    { 21705, -302,  24547,  264 },
+    { 21403, -307,  24811,  261 },
+    { 21096, -309,  25072,  257 },
+    { 20787, -312,  25329,  253 },
+    { 20475, -316,  25582,  250 },
+    { 20159, -318,  25832,  245 },
+    { 19841, -322,  26077,  242 },
+    { 19519, -324,  26319,  237 },
+    { 19195, -327,  26556,  234 },
+    { 18868, -331,  26790,  229 },
+    { 18537, -333,  27019,  226 },
+    { 18204, -335,  27245,  221 },
+    { 17869, -339,  27466,  217 },
+    { 17530, -341,  27683,  213 },
+    { 17189, -343,  27896,  209 },
+    { 16846, -347,  28105,  205 },
+    { 16499, -348,  28310,  200 },
+    { 16151, -351,  28510,  196 },
+    { 15800, -354,  28706,  192 },
+    { 15446, -356,  28898,  187 },
+    { 15090, -358,  29085,  183 },
+    { 14732, -360,  29268,  179 },
+    { 14372, -362,  29447,  174 },
+    { 14010, -365,  29621,  170 },
+    { 13645, -366,  29791,  165 },
+    { 13279, -369,  29956,  161 },
+    { 12910, -371,  30117,  156 },
+    { 12539, -372,  30273,  151 },
+    { 12167, -374,  30424,  147 },
+    { 11793, -376,  30571,  143 },
+    { 11417, -378,  30714,  138 },
+    { 11039, -380,  30852,  133 },
+    { 10659, -381,  30985,  128 },
+    { 10278, -382,  31113,  124 },
+    {  9896, -384,  31237,  119 },
+    {  9512, -386,  31356,  114 },
+    {  9126, -387,  31470,  110 },
+    {  8739, -388,  31580,  105 },
+    {  8351, -389,  31685,  100 },
+    {  7962, -391,  31785,   95 },
+    {  7571, -392,  31880,   91 },
+    {  7179, -393,  31971,   86 },
+    {  6786, -393,  32057,   80 },
+    {  6393, -395,  32137,   76 },
+    {  5998, -396,  32213,   72 },
+    {  5602, -397,  32285,   66 },
+    {  5205, -397,  32351,   61 },
+    {  4808, -398,  32412,   57 },
+    {  4410, -399,  32469,   52 },
+    {  4011, -399,  32521,   46 },
+    {  3612, -400,  32567,   42 },
+    {  3212, -401,  32609,   37 },
+    {  2811, -401,  32646,   32 },
+    {  2410, -401,  32678,   27 },
+    {  2009, -401,  32705,   23 },
+    {  1608, -402,  32728,   17 },
+    {  1206, -402,  32745,   12 },
+    {   804, -402,  32757,    8 },
+    {   402, -402,  32765,    2 },
+    {     0, -402,  32767,    0 }
+};
+
+static inline 
+uint32_t cos_sin(int16_t x)
+{
+    int idx = x >> 8;
+    int mod = x & 0xff;
+    int r = __PKHBT(0x00000100, mod, 16);
+    uint32_t *e = (uint32_t *)&cos_sin_table[idx+128];
+    uint32_t c = e[0];
+    uint32_t s = e[1];
+    c = __SMUAD(r, c);
+    s = __SMUAD(r, s);
+    c >>= 8;
+    s >>= 8;
+    return __PKHBT(s, c, 16);
+}
+
+FMModState fmmod;
+
+void
+fmmod_init(FMModState *fmmod)
+{
+    fmmod->vec = 0x7fff0000; 
+}
+    
+void
+frequency_modulation(FMModState *fmmod, uint32_t *src, uint32_t *dst, int dst_len)
+{
+    int j;
+    uint32_t vec = fmmod->vec;
+    for (j = 0; j < dst_len; j++) {
+        uint32_t s = *src++;
+        // fetch only R-ch (top half of word)
+        int16_t x = s >> 16;
+        uint32_t cs = cos_sin(x);
+        uint32_t real = __SMUSD(vec, cs);
+        uint32_t imag = __SMUADX(vec, cs);
+        real >>= 15;
+        imag >>= 15;
+        vec = __PKHBT(imag, real, 16);
+        *dst++ = vec;
+    }
+#if 1
+    uint32_t mag = __SMUAD(vec, vec);
+    if (mag < 0x10000) {
+        // force initialize
+        vec = 0x7fff0000;
+    } else if (mag < 0x3ff00000) {
+        uint32_t d = __PKHBT((int16_t)(vec&0xffff) >> 12, vec >> 12, 0);
+        vec = __QADD16(vec, d);
+    }
+#endif
+    fmmod->vec = vec;
+}
+
+void
+amplitude_modulation(uint32_t *src, uint32_t *dst, int dst_len)
+{
+    int j;
+    for (j = 0; j < dst_len; j++) {
+        uint32_t s = *src++;
+        // fetch only R-ch (top half of word) and halving it
+        int16_t x = s >> 16;
+        // add DC and set zero at quadrature
+        x /= 2;
+        x += 0x4000;
+        *dst++ = x & 0xffff;
+    }        
+}
\ No newline at end of file