diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 0d80d2a..638b8a2 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -3,7 +3,7 @@ set(LIBSDR_SOURCES
     buffer.cc node.cc queue.cc traits.cc
     portaudio.cc utils.cc wavfile.cc
     exception.cc logger.cc psk31.cc interpolate.cc)
-set(LIBSDR_HEADERS sdr.hh
+set(LIBSDR_HEADERS sdr.hh math.hh
     buffer.hh node.hh queue.hh buffernode.hh filternode.hh traits.hh autocast.hh
     siggen.hh portaudio.hh utils.hh wavfile.hh demod.hh firfilter.hh
     fftplan.hh fftplan_native.hh exception.hh baseband.hh freqshift.hh subsample.hh
diff --git a/src/autocast.hh b/src/autocast.hh
index ffdaaa4..dc3caa7 100644
--- a/src/autocast.hh
+++ b/src/autocast.hh
@@ -184,7 +184,7 @@ protected:
     uint8_t *values = reinterpret_cast<uint8_t *>(in.data());
     for (size_t i=0; i<N; i++) {
       reinterpret_cast<std::complex<int16_t> *>(out.data())[i]
-          = std::complex<int16_t>((int16_t(values[i])-(1<<6))<<8);
+          = std::complex<int16_t>((int16_t(values[i])-127)<<8);
     }
     return 4*N;
   }
@@ -195,7 +195,7 @@ protected:
     int8_t *values = reinterpret_cast<int8_t *>(in.data());
     for (size_t i=0; i<N; i++) {
       reinterpret_cast<std::complex<int16_t> *>(out.data())[i]
-          = std::complex<int16_t>(int16_t(values[i])<<8);
+          = std::complex<int16_t>(int16_t(values[i])*(1<<8));
     }
     return 4*N;
   }
@@ -206,8 +206,8 @@ protected:
     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<int16_t> *>(out.data())[i] =
-          std::complex<int16_t>((int16_t(values[i].real())-127)<<8,
-                                (int16_t(values[i].imag())-127)<<8);
+          std::complex<int16_t>((int16_t(values[i].real())-127)*(1<<8),
+                                (int16_t(values[i].imag())-127)*(1<<8));
     }
     return 4*N;
   }
@@ -218,8 +218,8 @@ protected:
     std::complex<int8_t> *values = reinterpret_cast<std::complex<int8_t> *>(in.data());
     for (size_t i=0; i<N; i++) {
       reinterpret_cast<std::complex<int16_t> *>(out.data())[i] =
-          std::complex<int16_t>(int16_t(values[i].real())<<8,
-                                int16_t(values[i].imag())<<8);
+          std::complex<int16_t>(int16_t(values[i].real())*(1<<8),
+                                int16_t(values[i].imag())*(1<<8));
     }
     return 4*N;
   }
@@ -230,7 +230,7 @@ protected:
     uint16_t *values = reinterpret_cast<uint16_t *>(in.data());
     for (size_t i=0; i<N; i++) {
       reinterpret_cast<std::complex<int16_t> *>(out.data())[i]
-          = std::complex<int16_t>(int32_t(values[i])-(2<<15));
+          = std::complex<int16_t>(int32_t(values[i])-(1<<15));
     }
     return 4*N;
   }
diff --git a/src/demod.hh b/src/demod.hh
index a27723f..87acee4 100644
--- a/src/demod.hh
+++ b/src/demod.hh
@@ -6,6 +6,7 @@
 #include "config.hh"
 #include "combine.hh"
 #include "logger.hh"
+#include "math.hh"
 
 
 namespace sdr {
@@ -247,43 +248,33 @@ 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<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;
-    if (b >= 0) { angle = pi4 - pi4*(b-aabs) / (b+aabs); }
-    else { angle = pi34 - pi4*(b+aabs) / (aabs-b); }
-    return (a >= 0) ? angle : -angle;
-  }
-
   /** The actual demodulation. */
   void _process(const Buffer< std::complex<iScalar> > &in, const Buffer<oScalar> &out)
   {
     // The last input value
     std::complex<iScalar> last_value = _last_value;
     // calc first value
-    SScalar a = SScalar(in[0].real())*SScalar(last_value.real())
-        + SScalar(in[0].imag())*SScalar(last_value.imag());
-    SScalar b = SScalar(in[0].imag())*SScalar(last_value.real())
-        - SScalar(in[0].real())*SScalar(last_value.imag());
-
+    SScalar a = (SScalar(in[0].real())*SScalar(last_value.real()))/2
+        + (SScalar(in[0].imag())*SScalar(last_value.imag()))/2;
+    SScalar b = (SScalar(in[0].imag())*SScalar(last_value.real()))/2
+        - (SScalar(in[0].real())*SScalar(last_value.imag()))/2;
+    a >>= Traits<iScalar>::shift; b >>= Traits<iScalar>::shift;
     // update last value
     last_value = in[0];
     // calc output (prob. overwriting the last value)
-    out[0] = _fast_atan2(a, b);
+    out[0] = fast_atan2<iScalar, oScalar>(a, b);
+    //out[0] = (1<<12)*(std::atan2(float(a),float(b))/M_PI);
 
     // Calc remaining values
     for (size_t i=1; i<in.size(); i++) {
-      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());
+      a = (SScalar(in[i].real())*SScalar(last_value.real()))/2
+          + (SScalar(in[i].imag())*SScalar(last_value.imag()))/2;
+      b = (SScalar(in[i].imag())*SScalar(last_value.real()))/2
+          - (SScalar(in[i].real())*SScalar(last_value.imag()))/2;
+      a >>= Traits<iScalar>::shift; b >>= Traits<iScalar>::shift;
       last_value = in[i];
-      out[i] = _fast_atan2(a, b);
+      out[i] = fast_atan2<iScalar,oScalar>(a, b);
+      //out[i] = (1<<12)*(std::atan2(float(a),float(b))/M_PI);
     }
 
     // Store last value
diff --git a/src/math.hh b/src/math.hh
new file mode 100644
index 0000000..90e7387
--- /dev/null
+++ b/src/math.hh
@@ -0,0 +1,44 @@
+#ifndef __SDR_MATH_HH__
+#define __SDR_MATH_HH__
+
+#include <inttypes.h>
+
+namespace sdr {
+
+/** Template prototype for @c fast_atan2. */
+template <class iScalar, class oScalar> oScalar fast_atan2(iScalar a, iScalar b);
+
+/** Implementation of atan2 approximation using integers. */
+template <> inline int16_t fast_atan2<int8_t, int16_t>(int8_t a, int8_t b) {
+  const int32_t pi4 = (1<<12);
+  const int32_t pi34 = 3*(1<<12);
+  int32_t aabs, angle;
+  if ((0 == a) && (0 == b)) { return 0; }
+  aabs = (a >= 0) ? a : -a;
+  if (b >= 0) { angle = pi4 - pi4*(b-aabs) / (b+aabs); }
+  else { angle = pi34 - pi4*(b+aabs) / (aabs-b); }
+  return (a >= 0) ? angle : -angle;
+}
+
+template <> inline int16_t fast_atan2<uint8_t, int16_t>(uint8_t ua, uint8_t ub) {
+  int8_t a = (int16_t(ua)-(1<<7));
+  int8_t b = (int16_t(ub)-(1<<7));
+  return fast_atan2<int8_t, int16_t>(a,b);
+}
+
+/** Implementation of atan2 approximation using integers. */
+template <> inline int16_t fast_atan2<int16_t, int16_t>(int16_t a, int16_t b) {
+  //return (1<<15)*(std::atan2(float(a), float(b))/M_PI);
+  const int32_t pi4 = (1<<12);
+  const int32_t pi34 = 3*(1<<12);
+  int32_t aabs, angle;
+  if ((0 == a) && (0 == b)) { return 0; }
+  aabs = (a >= 0) ? a : -a;
+  if (b >= 0) { angle = pi4 - pi4*(b-aabs) / (b+aabs); }
+  else { angle = pi34 - pi4*(b+aabs) / (aabs-b); }
+  return (a >= 0) ? angle : -angle;
+}
+
+
+}
+#endif // MATH_HH
diff --git a/src/psk31.hh b/src/psk31.hh
index 4f23efe..e838867 100644
--- a/src/psk31.hh
+++ b/src/psk31.hh
@@ -8,6 +8,7 @@
 
 namespace sdr {
 
+/** A simple BPSK31 "demodulator". */
 template <class Scalar>
 class BPSK31: public Sink< std::complex<Scalar> >, public Source
 {
diff --git a/src/sdr.hh b/src/sdr.hh
index 123d6fa..4fbfbb4 100644
--- a/src/sdr.hh
+++ b/src/sdr.hh
@@ -6,6 +6,7 @@
 
 #include "config.hh"
 #include "operators.hh"
+#include "math.hh"
 #include "traits.hh"
 #include "exception.hh"
 #include "buffer.hh"
diff --git a/src/subsample.hh b/src/subsample.hh
index d8b89c9..0b2f919 100644
--- a/src/subsample.hh
+++ b/src/subsample.hh
@@ -5,6 +5,7 @@
 #include "buffer.hh"
 #include "traits.hh"
 #include "interpolate.hh"
+#include "logger.hh"
 
 
 namespace sdr {
diff --git a/src/wavfile.cc b/src/wavfile.cc
index 509a700..257a100 100644
--- a/src/wavfile.cc
+++ b/src/wavfile.cc
@@ -137,9 +137,9 @@ WavSource::open(const std::string &filename)
   // Configure source
   _frame_count = chunk_size/(2*n_chanels);
   if ((1 == n_chanels) && (8 == bits_per_sample)) { _type = Config::Type_u8; }
-  else if ((1==n_chanels) && (16 == bits_per_sample)) { _type = Config::Type_u16; }
+  else if ((1==n_chanels) && (16 == bits_per_sample)) { _type = Config::Type_s16; }
   else if ((2==n_chanels) && ( 8 == bits_per_sample)) { _type = Config::Type_cu8; }
-  else if ((2==n_chanels) && (16 == bits_per_sample)) { _type = Config::Type_cu16; }
+  else if ((2==n_chanels) && (16 == bits_per_sample)) { _type = Config::Type_cs16; }
   else {
     ConfigError err; err << "Can not configure WavSource: Unsupported PCM type."; throw err;
   }
@@ -166,17 +166,17 @@ WavSource::open(const std::string &filename)
     _buffer = Buffer<uint8_t>(_buffer_size);
     this->setConfig(Config(Config::Type_u8, _sample_rate, _buffer_size, 1));
     break;
-  case Config::Type_u16:
-    _buffer = Buffer<uint16_t>(_buffer_size);
-    this->setConfig(Config(Config::Type_u16, _sample_rate, _buffer_size, 1));
+  case Config::Type_s16:
+    _buffer = Buffer<int16_t>(_buffer_size);
+    this->setConfig(Config(Config::Type_s16, _sample_rate, _buffer_size, 1));
     break;
   case Config::Type_cu8:
     _buffer = Buffer< std::complex<uint8_t> >(_buffer_size);
     this->setConfig(Config(Config::Type_cu8, _sample_rate, _buffer_size, 1));
     break;
-  case Config::Type_cu16:
-    _buffer = Buffer< std::complex<uint16_t> >(_buffer_size);
-    this->setConfig(Config(Config::Type_cu16, _sample_rate, _buffer_size, 1));
+  case Config::Type_cs16:
+    _buffer = Buffer< std::complex<int16_t> >(_buffer_size);
+    this->setConfig(Config(Config::Type_cs16, _sample_rate, _buffer_size, 1));
     break;
   default: {
     ConfigError err; err << "Can not configure WavSource: Unsupported PCM type."; throw err;
@@ -192,7 +192,7 @@ WavSource::close() {
 
 bool
 WavSource::isReal() const {
-  return (Config::Type_u8 == _type) || (Config::Type_u16 == _type);
+  return (Config::Type_u8 == _type) || (Config::Type_s16 == _type);
 }
 
 void
@@ -216,20 +216,20 @@ WavSource::next()
     _frames_left -= n_frames;
     this->send(RawBuffer(_buffer, 0, n_frames*sizeof(uint8_t)), true);
     break;
-  case Config::Type_u16:
-    _file.read(_buffer.ptr(), n_frames*sizeof(uint16_t));
+  case Config::Type_s16:
+    _file.read(_buffer.ptr(), n_frames*sizeof(int16_t));
     _frames_left -= n_frames;
-    this->send(RawBuffer(_buffer, 0, n_frames*sizeof(uint16_t)), true);
+    this->send(RawBuffer(_buffer, 0, n_frames*sizeof(int16_t)), true);
     break;
   case Config::Type_cu8:
     _file.read(_buffer.ptr(), 2*n_frames*sizeof(uint8_t));
     _frames_left -= n_frames;
     this->send(RawBuffer(_buffer, 0, 2*n_frames*sizeof(uint8_t)), true);
     break;
-  case Config::Type_cu16:
-    _file.read(_buffer.ptr(), 2*n_frames*sizeof(uint16_t));
+  case Config::Type_cs16:
+    _file.read(_buffer.ptr(), 2*n_frames*sizeof(int16_t));
     _frames_left -= n_frames;
-    this->send(RawBuffer(_buffer, 0, 2*n_frames*sizeof(uint16_t)), true);
+    this->send(RawBuffer(_buffer, 0, 2*n_frames*sizeof(int16_t)), true);
     break;
   default:
     break;