Finally fixed FM demodulator.

src/autocast.hh

@@ -36,9 +36,9 @@ public:
       }
     } else if (Config::Type_cs8 == Traits<Scalar>::scalarId) {
       switch (src_cfg.type()) {
-      case Config::Type_u8:
+      case Config::Type_u8: _cast = _uint8_cint8; break;
       case Config::Type_s8: _cast = _int8_cint8; break;
-      case Config::Type_cu8:
+      case Config::Type_cu8: _cast = _cuint8_cint8; break;
       case Config::Type_cs8: _cast = _identity; break;
       case Config::Type_u16:
       case Config::Type_s16: _cast = _int16_cint8; break;
@@ -118,6 +118,17 @@ protected:
     return N;
   }
 
+  /** uint8 -> complex int8. */
+  static size_t _uint8_cint8(const RawBuffer &in, const RawBuffer &out) {
+    size_t N = in.bytesLen();
+    uint8_t *values = reinterpret_cast<uint8_t *>(in.data());
+    for (size_t i=0; i<N; i++) {
+      reinterpret_cast<std::complex<int8_t> *>(out.data())[i] =
+          (int16_t(values[i])-127);
+    }
+    return 2*N;
+  }
+
   /** int8 -> complex int8. */
   static size_t _int8_cint8(const RawBuffer &in, const RawBuffer &out) {
     size_t N = in.bytesLen();
@@ -127,6 +138,16 @@ protected:
     return 2*N;
   }
 
+  static size_t _cuint8_cint8(const RawBuffer &in, const RawBuffer &out) {
+    size_t N = in.bytesLen()/2;
+    std::complex<uint8_t> *values = reinterpret_cast<std::complex<uint8_t> *>(in.data());
+    for (size_t i=0; i<N; i++) {
+      reinterpret_cast<std::complex<int8_t> *>(out.data())[i] =
+          std::complex<int8_t>(int16_t(values[i].real())-127, int16_t(values[i].imag())-127);
+    }
+    return 2*N;
+  }
+
   /** int16 -> complex int 8. */
   static size_t _int16_cint8(const RawBuffer &in, const RawBuffer &out) {
     size_t N = in.bytesLen()/2;

src/demod.hh

@@ -249,8 +249,9 @@ public:
 protected:
   /** A fast approximative implementation of the std::atan2() on integers. */
   inline SScalar _fast_atan2(SScalar a, SScalar b) {
-    const SScalar pi4 = (1<<(Traits<iScalar>::shift-2));
-    const SScalar pi34 = (3<<(Traits<iScalar>::shift-2));
+    const SScalar pi4 = (1<<(Traits<oScalar>::shift-4));
+    const SScalar pi34 = 3*(1<<(Traits<oScalar>::shift-4));
+    a >>= (9-_shift); b >>= (9-_shift);
     SScalar aabs, angle;
     if ((0 == a) && (0 == b)) { return 0; }
     aabs = (a >= 0) ? a : -a;
@@ -273,16 +274,16 @@ protected:
     // update last value
     last_value = in[0];
     // calc output (prob. overwriting the last value)
-    out[0] = (_fast_atan2(a, b)<<_shift);
+    out[0] = _fast_atan2(a, b);
 
     // Calc remaining values
     for (size_t i=1; i<in.size(); i++) {
-      a = SScalar(in[i].real())*last_value.real()
-          + SScalar(in[i].imag())*last_value.imag();
-      b = SScalar(in[i].imag())*last_value.real()
-          - SScalar(in[i].real())*last_value.imag();
+      a = SScalar(in[i].real())*SScalar(last_value.real())
+          + SScalar(in[i].imag())*SScalar(last_value.imag());
+      b = SScalar(in[i].imag())*SScalar(last_value.real())
+          - SScalar(in[i].real())*SScalar(last_value.imag());
       last_value = in[i];
-      out[i] = (_fast_atan2(a, b)<<_shift);
+      out[i] = _fast_atan2(a, b);
     }
 
     // Store last value