diff --git a/libpsk/include/dsp/fir_decimate.hpp b/libpsk/include/dsp/fir_decimate.hpp
index 0951867..1eaa2fb 100644
--- a/libpsk/include/dsp/fir_decimate.hpp
+++ b/libpsk/include/dsp/fir_decimate.hpp
@@ -7,6 +7,9 @@
 /* ------------------------------------------------------------------------- */
 #ifndef DSP_FIR_DECIMATE_HPP_
 #define DSP_FIR_DECIMATE_HPP_
+/* ------------------------------------------------------------------------- */
+#include <array>
+
 /* ------------------------------------------------------------------------- */
 namespace dsp {
 /* ------------------------------------------------------------------------- */
@@ -15,19 +18,36 @@ namespace dsp {
  * CT - coefficient type
  * N  - decimate length
  */
-template<typename DT, typename CT, size_t LEN> class fir_decimate
+template<typename DT, typename CT, size_t TAPS> 
+class fir_decimate
 {
 public:
-	fir_decimate( constexpr CT )
-	{
-
+	explicit constexpr fir_decimate( const CT * const coefs )
+        : m_coefs( coefs )
+	{       
 	}
-	DT operator()( DT value )
+    //Insert operator
+	void operator()( DT value )
 	{
-	}
+	    m_state[ m_last_idx++ ] = value;
+        if( m_last_idx == TAPS ) m_last_idx = 0;
+    }
+    //Calculate fir operator
+    DT operator()()
+    {
+          DT acc = 0;
+          size_t index = m_last_idx;
+          for(size_t i = 0; i < TAPS; ++i) 
+          {
+            index = index != 0 ? index-1 : TAPS-1;
+            acc += m_state[index] * m_coefs[i];
+          }
+          return acc;
+    }
 private:
-	DT m_filter[LEN];
-	 constexpr CT m_coefs[];
+    std::array<DT, TAPS> m_state {};
+	const CT * const m_coefs {};
+    size_t m_last_idx {};
 };
 
 /* ------------------------------------------------------------------------- */
diff --git a/libpsk/src/decoder.cpp b/libpsk/src/decoder.cpp
index 27c5645..d9f6aaf 100644
--- a/libpsk/src/decoder.cpp
+++ b/libpsk/src/decoder.cpp
@@ -1015,6 +1015,7 @@ void decoder::operator()( const sample_type* samples, std::size_t sample_size )
 				acc += (*Firptr++) * (*Kptr++);
 			}
 			m_que2[m_fir2_state] = acc;
+            std::cout << "ACC " << acc << std::endl;
 			//1.21 AA6YQ second decimation filter not required for PSK125
 			if( m_baudrate==baudrate::b125 || ((m_sample_cnt%mod16_8) == 0) ) //calc second decimation filter every 8 or 16samples
 			{