diff --git a/FM.cpp b/FM.cpp
index 7c90e01..2258109 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -36,12 +36,15 @@ m_kerchunkTimer(),
 m_ackMinTimer(),
 m_ackDelayTimer(),
 m_hangTimer(),
-m_filterStage1(  724,   1448,   724, 32768, -37895, 21352),
+m_filterStage1(  724,   1448,   724, 32768, -37895, 21352),//3rd order Cheby Filter 300 to 2700Hz, 0.2dB passband ripple, sampling rate 24kHz
 m_filterStage2(32768,      0,-32768, 32768, -50339, 19052),
 m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460),
+m_preemphasis(32768,  13967, 0, 32768, -18801, 0),//75µS 24kHz sampling rate
+m_deemphasis (32768, -18801, 0, 32768,  13967, 0),//75µS 24kHz sampling rate
 m_blanking(),
 m_useCOS(true),
-m_rfAudioBoost(1U)
+m_rfAudioBoost(1U),
+m_downsampler(1024)//Size might need adjustement
 {
 }
 
@@ -74,13 +77,17 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
     } else if (CTCSS_NOT_READY(ctcssState) && m_modemState == STATE_FM && i == length - 1) {
       //Not enough samples for CTCSS but we already are in FM, trigger the state machine
       //but do not trigger the state machine on every single sample, save CPU!
-        bool validCTCSS = CTCSS_VALID(ctcssState);
-        stateMachine(validCTCSS && cos, i + 1U);
+      bool validCTCSS = CTCSS_VALID(ctcssState);
+      stateMachine(validCTCSS && cos, i + 1U);
     }
-    
+
+    currentSample = m_deemphasis.filter(currentSample);
+
     // Only let audio through when relaying audio
-    if (m_state == FS_RELAYING || m_state == FS_KERCHUNK)
+    if (m_state == FS_RELAYING || m_state == FS_KERCHUNK) {    
+      m_downsampler.addSample(currentSample);
       currentSample = m_blanking.process(currentSample);
+    }
     else
       currentSample = 0U;
 
@@ -99,7 +106,9 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
       currentSample += m_timeoutTone.getAudio();
 
-    currentSample = q15_t(m_filterStage3.filter(m_filterStage2.filter(m_filterStage1.filter(currentSample))));
+    currentSample = m_filterStage3.filter(m_filterStage2.filter(m_filterStage1.filter(currentSample)));
+
+    currentSample = m_preemphasis.filter(currentSample);
 
     currentSample += m_ctcssTX.getAudio();
 
diff --git a/FM.h b/FM.h
index d66f2ab..a852a80 100644
--- a/FM.h
+++ b/FM.h
@@ -28,6 +28,7 @@
 #include "FMKeyer.h"
 #include "FMTimer.h"
 #include "FMDirectForm1.h"
+#include "FMDownsampler.h"
 
 enum FM_STATE {
   FS_LISTENING,
@@ -75,9 +76,12 @@ private:
   CFMDirectFormI       m_filterStage1;
   CFMDirectFormI       m_filterStage2;
   CFMDirectFormI       m_filterStage3;
+  CFMDirectFormI       m_preemphasis;
+  CFMDirectFormI       m_deemphasis;
   CFMBlanking          m_blanking;
   bool                 m_useCOS;
   q15_t                m_rfAudioBoost;
+  CFMDownsampler       m_downsampler;
 
   void stateMachine(bool validSignal, uint8_t length);
   void listeningState(bool validSignal);
diff --git a/FMDirectForm1.h b/FMDirectForm1.h
index 4b90245..450274d 100644
--- a/FMDirectForm1.h
+++ b/FMDirectForm1.h
@@ -79,7 +79,7 @@ public:
 	inline q15_t filter(const q15_t in)
 	{
 		// calculate the output
-		register q31_t out_upscaled = c_b0 * in    //F4FXL puting stauration here made everything quiet, not sure why
+		register q31_t out_upscaled = c_b0 * in
 									+ c_b1 * m_x1
 									+ c_b2 * m_x2
 									- c_a1 * m_y1
diff --git a/FMDownsampleRB.cpp b/FMDownsampleRB.cpp
new file mode 100644
index 0000000..7d2cc2a
--- /dev/null
+++ b/FMDownsampleRB.cpp
@@ -0,0 +1,101 @@
+/*
+ *   Copyright (C) 2020 by Jonathan Naylor G4KLX
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+
+#include "FMDownsampleRB.h"
+
+CFMDownsampleRB::CFMDownsampleRB(uint16_t length) :
+m_length(length),
+m_head(0U),
+m_tail(0U),
+m_full(false),
+m_overflow(false)
+{
+  m_samples = new uint8_t[length];
+}
+
+uint16_t CFMDownsampleRB::getSpace() const
+{
+  uint16_t n = 0U;
+
+  if (m_tail == m_head)
+    n = m_full ? 0U : m_length;
+  else if (m_tail < m_head)
+    n = m_length - m_head + m_tail;
+  else
+    n = m_tail - m_head;
+
+  if (n > m_length)
+    n = 0U;
+
+  return n;
+}
+
+uint16_t CFMDownsampleRB::getData() const
+{
+  if (m_tail == m_head)
+    return m_full ? m_length : 0U;
+  else if (m_tail < m_head)
+    return m_head - m_tail;
+  else
+    return m_length - m_tail + m_head;
+}
+
+bool CFMDownsampleRB::put(uint8_t sample)
+{
+  if (m_full) {
+    m_overflow = true;
+    return false;
+  }
+
+  m_samples[m_head] = sample;
+
+  m_head++;
+  if (m_head >= m_length)
+    m_head = 0U;
+
+  if (m_head == m_tail)
+    m_full = true;
+
+  return true;
+}
+
+bool CFMDownsampleRB::get(uint8_t& sample)
+{
+  if (m_head == m_tail && !m_full)
+    return false;
+
+  sample = m_samples[m_tail];
+
+  m_full = false;
+
+  m_tail++;
+  if (m_tail >= m_length)
+    m_tail = 0U;
+
+  return true;
+}
+
+bool CFMDownsampleRB::hasOverflowed()
+{
+  bool overflow = m_overflow;
+
+  m_overflow = false;
+
+  return overflow;
+}
diff --git a/FMDownsampleRB.h b/FMDownsampleRB.h
new file mode 100644
index 0000000..6e8e466
--- /dev/null
+++ b/FMDownsampleRB.h
@@ -0,0 +1,68 @@
+/*
+ *   Copyright (C) 2020 by Jonathan Naylor G4KLX
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#if !defined(FMRB_H)
+#define  FMRB_H
+
+#if defined(STM32F4XX)
+#include "stm32f4xx.h"
+#elif defined(STM32F7XX)
+#include "stm32f7xx.h"
+#elif defined(STM32F105xC)
+#include "stm32f1xx.h"
+#include <cstddef>
+#else
+#include <Arduino.h>
+#endif
+
+#if defined(__SAM3X8E__) || defined(STM32F105xC)
+#define  ARM_MATH_CM3
+#elif defined(STM32F7XX)
+#define  ARM_MATH_CM7
+#elif defined(STM32F4XX) || defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
+#define  ARM_MATH_CM4
+#else
+#error "Unknown processor type"
+#endif
+
+#include <arm_math.h>
+
+class CFMDownsampleRB {
+public:
+  CFMDownsampleRB(uint16_t length);
+  
+  uint16_t getSpace() const;
+  
+  uint16_t getData() const;
+
+  bool put(uint8_t sample);
+
+  bool get(uint8_t& sample);
+
+  bool hasOverflowed();
+
+private:
+  uint16_t          m_length;
+  volatile uint8_t* m_samples;
+  volatile uint16_t m_head;
+  volatile uint16_t m_tail;
+  volatile bool     m_full;
+  bool              m_overflow;
+};
+
+#endif
diff --git a/FMDownsampler.cpp b/FMDownsampler.cpp
new file mode 100644
index 0000000..51e9580
--- /dev/null
+++ b/FMDownsampler.cpp
@@ -0,0 +1,63 @@
+/*
+ *   Copyright (C) 2020 by Jonathan Naylor G4KLX
+ *   Copyright (C) 2020 by Geoffrey Merck F4FXL - KC3FRA
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "Config.h"
+#include "FMDownsampler.h"
+
+
+CFMDownsampler::CFMDownsampler(uint16_t length) :
+m_ringBuffer(length),//length might need tweaking
+m_packIndex(0),
+m_downSampleIndex(0)
+{
+    m_samplePack = 0;
+}
+
+void CFMDownsampler::addSample(q15_t sample)
+{
+    //only take one of three samples
+    if(m_downSampleIndex == 0) {
+      switch(m_packIndex){
+        case 0:
+          m_samplePack = int32_t(sample) << 12;
+        break;
+        case 1:{
+            m_samplePack |= int32_t(sample);
+            
+            //we did not use MSB; skip it
+            m_ringBuffer.put(m_samplePackBytes[1]); 
+            m_ringBuffer.put(m_samplePackBytes[2]); 
+            m_ringBuffer.put(m_samplePackBytes[3]); 
+
+            m_samplePack = 0;
+        }
+        break;
+        default:
+            //should never happen
+        break;
+        }
+        m_packIndex++;
+        if(m_packIndex >= 2)
+            m_packIndex = 0;  
+    }
+
+    m_downSampleIndex++;
+    if(m_downSampleIndex >= 3)
+        m_downSampleIndex = 0;
+}
\ No newline at end of file
diff --git a/FMDownsampler.h b/FMDownsampler.h
new file mode 100644
index 0000000..fd17e0a
--- /dev/null
+++ b/FMDownsampler.h
@@ -0,0 +1,43 @@
+/*
+ *   Copyright (C) 2020 by Jonathan Naylor G4KLX
+ *   Copyright (C) 2020 by Geoffrey Merck F4FXL - KC3FRA
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#if !defined(FMDOWNSAMPLER_H)
+#define FMDOWNSAMPLER_H
+
+#include "Config.h"
+#include "FMDownsampleRB.h"
+
+class CFMDownsampler {
+public:
+  CFMDownsampler(uint16_t length);
+  void addSample(q15_t sample);
+  inline bool getPackedData(uint8_t& data){ return m_ringBuffer.get(data); };
+  inline bool hasOverflowed() { return m_ringBuffer.hasOverflowed(); };
+
+private:
+  CFMDownsampleRB m_ringBuffer;
+  union {
+    int32_t m_samplePack;
+    int8_t  m_samplePackBytes[4];
+  };
+  uint8_t m_packIndex;
+  uint8_t m_downSampleIndex;
+};
+
+#endif
\ No newline at end of file
diff --git a/FMGenerateFilterCoefficients.py b/Tools/FMGenerateFilterCoefficients.py
similarity index 66%
rename from FMGenerateFilterCoefficients.py
rename to Tools/FMGenerateFilterCoefficients.py
index 84ea62b..7d8a3db 100644
--- a/FMGenerateFilterCoefficients.py
+++ b/Tools/FMGenerateFilterCoefficients.py
@@ -17,21 +17,22 @@ f2 = 2700
 rp = 0.2
 
 # scaling factor in bits, do not change !
-q = 15
+q = 0
 # scaling factor as facor...
 scaling_factor = 2**q
 
 # let's generate a sequence of 2nd order IIR filters
-#sos = signal.butter(2,[f1/fs*2,f2/fs*2],'pass',output='sos')
-sos = signal.cheby1(3,rp,[f1/fs*2,f2/fs*2],'bandpass', output='sos')
+sos = signal.cheby1(3,rp,[f1, f2],'bandpass', output='sos', fs=fs)
+#sos = signal.cheby1(1, rp, 2122, 'lowpass', output='sos', fs=fs) #deemphasis filter
+#sos = signal.cheby1(1, rp, 2122, 'highpass', output='sos', fs=fs) #deemphasis filter
 
-sos = np.round((sos) * scaling_factor)
+#sos = np.round((sos) * scaling_factor)
 
 # print coefficients
 for biquad in sos:
     for coeff in biquad:
-        print(int(coeff),",",sep="",end="")
-        #print((coeff),",",sep="",end="")
+        #print(int(coeff),",",sep="",end="")
+        print((coeff),",",sep="",end="")
     print("")
 
 # plot the frequency response
diff --git a/Tools/emphasis.txt b/Tools/emphasis.txt
new file mode 100644
index 0000000..39660d1
--- /dev/null
+++ b/Tools/emphasis.txt
@@ -0,0 +1,58 @@
+% GNU Octave script to generate pre and deemphasis filters
+% https://dsp.stackexchange.com/questions/34605/biquad-cookbook-formula-for-broadcast-fm-de-emphasis
+% PACKAGES
+
+pkg load control
+pkg load signal
+
+clear all;
+clc;
+
+fs = 24000;
+samplingtime = 1/fs;
+
+% analog prototype
+A2 = [1];
+B2 = [0.000075 1];
+
+% Pre
+Ds = tf(B2, A2);
+% De
+% Ds = tf(A2, B2);
+ 
+Ds = Ds/dcgain(Ds);
+
+% MZT
+T1 = 0.000075; % 75us
+z1 = -exp(-1.0/(fs*T1));
+p1 = 1+z1;
+
+a0 = 1.0;
+a1 = p1;
+a2 = 0;
+
+b0 = 1.0;
+b1 = z1;
+b2 = 0;
+
+% swap between a1, b1 to select pre- or de-emphasis
+
+# Pre
+Bmzt = [b0 a1 b2]
+Amzt = [a0 b1 a2]
+% De
+% Bmzt = [b0 b1 b2]
+% Amzt = [a0 a1 a2]
+
+DzMZT = tf(Amzt, Bmzt, samplingtime);
+DzMZT = DzMZT/dcgain(DzMZT);
+
+%% Plot
+wmin = 2 * pi * 20.0; % 20Hz
+wmax = 2 * pi * ((fs/2.0) - (fs/2 - 20000)); %20kHz
+
+figure(1);
+bode(Ds, 'b', DzMZT, 'c', {wmin, wmax});
+legend('Analog prototype', 'MZT 2nd order','location', 'northwest');
+grid on;
+pause();