Diff: dsp.cpp

Revision:

6:ed82052bd505

Parent:

5:75c26157a53a

--- a/dsp.cpp	Mon Mar 23 21:43:29 2015 +0000
+++ b/dsp.cpp	Mon Mar 23 21:53:48 2015 +0000
@@ -474,43 +474,22 @@
 void
 fmmod_init(FMModState *fmmod)
 {
-    fmmod->vec = 0x7fff0000; 
+    fmmod->phase = 0; 
 }
     
 void
 frequency_modulation(FMModState *fmmod, uint32_t *src, uint32_t *dst, int dst_len)
 {
     int j;
-    uint32_t vec = fmmod->vec;
+    uint16_t phase = fmmod->phase;
     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*5);
-        int32_t real = __SMUSD(vec, cs);
-        int32_t imag = __SMUADX(vec, cs);
-        real >>= 15;
-        imag >>= 15;
-        vec = __PKHBT(imag, real, 16);
-        *dst++ = vec;
+        phase += x;
+        *dst++ = cos_sin(phase);
     }
-#if 0
-    uint32_t mag = sqrt((float)__SMUAD(vec, vec));
-    int32_t veci = ((vec & 0xffff) << 16) / (int32_t)(mag<<1);
-    int32_t vecq = (vec & 0xffff0000) / (int32_t)(mag<<1);
-    vec = __PKHBT(veci, vecq, 16);
-#endif
-#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, (int32_t)vec >> 12, 0);
-        vec = __QADD16(vec, d);
-    }
-#endif
-    fmmod->vec = vec;
+    fmmod->phase = phase;
 }
 
 void