From aeec3586bc0caf2e07cb18751722b316566a6e62 Mon Sep 17 00:00:00 2001
From: Hannes Matuschek <hmatuschek@gmail.com>
Date: Thu, 24 Jul 2014 00:00:53 +0200
Subject: [PATCH] Finally fixed FM demodulator.

---
 src/autocast.hh | 27 ++++++++++++++++++++++++---
 src/demod.hh    | 17 +++++++++--------
 2 files changed, 33 insertions(+), 11 deletions(-)

diff --git a/src/autocast.hh b/src/autocast.hh
index da7c26b..1be4d06 100644
--- a/src/autocast.hh
+++ b/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,7 +118,18 @@ protected:
     return N;
   }
 
-  /** int8 -> complex int 8. */
+  /** 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();
     for (size_t i=0; i<N; i++) {
@@ -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;
diff --git a/src/demod.hh b/src/demod.hh
index f0f3f7b..daed6c9 100644
--- a/src/demod.hh
+++ b/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