diff --git a/Makefile b/Makefile
index dfe3df2..905bf3a 100644
--- a/Makefile
+++ b/Makefile
@@ -98,3 +98,5 @@ emcc-beautify:
 	bash -c 'type js-beautify >/dev/null 2>&1; if [ $$? -eq 0 ]; then js-beautify sdr.js/sdr.js >sdr.js/sdr.js.beautiful; mv sdr.js/sdr.js.beautiful sdr.js/sdr.js; fi'
 codequality:
 	@bash -c 'if [ `cat csdr.c | grep badsyntax | grep -v return | wc -l` -ne 1 ]; then echo "error at code quality check: badsyntax() used in csdr.c without return."; exit 1; else exit 0; fi'
+v:
+	vim csdr.c libcsdr.c
diff --git a/csdr.c b/csdr.c
index feff244..2951d8f 100755
--- a/csdr.c
+++ b/csdr.c
@@ -124,7 +124,7 @@ char usage[]=
 "    rtty_baudot2ascii_u8_u8\n"
 "    serial_line_decoder_u8_u8\n"
 "    octave_complex_c <samples_to_plot> <out_of_n_samples>\n"
-"    timing_recovery_cc <algorithm> <decimation> [--add_q [--output_error | --output_indexes | --octave <debug_n>]] \n"
+"    timing_recovery_cc <algorithm> <decimation> [mu [--add_q [--output_error | --output_indexes | --octave <debug_n>]]] \n"
 "    psk31_varicode_encoder_u8_u8\n"
 "    psk31_varicode_decoder_u8_u8\n"
 "    differential_encoder_u8_u8\n"
@@ -133,7 +133,7 @@ char usage[]=
 "    psk_modulator_u8_c <n_psk>\n"
 "    psk31_interpolate_sine_cc <interpolation>\n"
 "    duplicate_samples_ntimes_u8_u8 <sample_size_bytes> <ntimes>\n"
-"    bpsk_costas_loop_cc <samples_per_bits>\n"
+"    bpsk_costas_loop_cc <loop_bandwidth> <damping_factor> <gain> [--dd | --decision_directed]\n"
 "    binary_slicer_f_u8\n"
 "    simple_agc_cc <rate> [reference [max_gain]]\n"
 "    firdes_resonator_c <rate> <length> [window [--octave]]\n"
@@ -148,6 +148,7 @@ char usage[]=
 "    add_n_zero_samples_at_beginning_f <n_zero_samples>\n"
 "    generic_slicer_f_u8 <n_symbols>\n"
 "    plain_interpolate_cc <n_symbols>\n"
+"    add_const_cc <i> <q>\n"
 "    ?<search_the_function_list>\n"
 "    =<evaluate_python_expression>\n"
 "    \n"
@@ -396,7 +397,7 @@ int main(int argc, char *argv[])
         clone_(the_bufsize); //After sending the buffer size out, just copy stdin to stdout
     }
 
-    if(!strcmp(argv[1],"clone"))
+    if(!strcmp(argv[1],"clone") || !strcmp(argv[1],"REM"))
     {
         if(!sendbufsize(initialize_buffers())) return -2;
         clone_(the_bufsize);
@@ -2525,7 +2526,7 @@ int main(int argc, char *argv[])
         }
     }
 
-    if(!strcmp(argv[1],"timing_recovery_cc")) //<algorithm> <decimation> [--add_q [--output_error | --output_indexes | --octave <debug_n>]] 
+    if(!strcmp(argv[1],"timing_recovery_cc")) //<algorithm> <decimation> [loop_gain [max_error [--add_q] [--output_error | --output_indexes | --octave <debug_n>]]] 
     {
         if(argc<=2) return badsyntax("need required parameter (algorithm)");
         timing_recovery_algorithm_t algorithm = timing_recovery_get_algorithm_from_string(argv[2]);
@@ -2536,26 +2537,32 @@ int main(int argc, char *argv[])
         sscanf(argv[3],"%d",&decimation);
         if(decimation<=4 || decimation&3) return badsyntax("decimation factor should be a positive integer divisible by 4");
 
-        int add_q = (argc>=5 && !strcmp(argv[4], "--add_q"));
+        float loop_gain = 0.5;
+        if(argc>4) sscanf(argv[4],"%f",&loop_gain);
+
+        float max_error = 2;
+        if(argc>5) sscanf(argv[5],"%f",&max_error);
+
+        int add_q = !!(argc>=7 && !strcmp(argv[6], "--add_q"));
 
         int debug_n = 0;
         int output_error = 0;
         int output_indexes = 0;
-        if(argc>=7 && !strcmp(argv[5], "--octave")) debug_n = atoi(argv[6]);
+        if(argc+add_q>=8 && !strcmp(argv[6+add_q], "--octave")) debug_n = atoi(argv[7+add_q]);
         if(debug_n<0) return badsyntax("debug_n should be >= 0");
 
-        if(argc>=6 && !strcmp(argv[5], "--output_error")) output_error = 1;
+        if(argc>=(8+add_q) && !strcmp(argv[7+add_q], "--output_error")) output_error = 1;
         float* timing_error = NULL;
         if(output_error) timing_error = (float*)malloc(sizeof(float)*the_bufsize);
 
-        if(argc>=6 && !strcmp(argv[5], "--output_indexes")) output_indexes = 1;
+        if(argc>=(8+add_q) && !strcmp(argv[7+add_q], "--output_indexes")) output_indexes = 1;
         unsigned* sampled_indexes = NULL;
         if(output_indexes) sampled_indexes = (unsigned*)malloc(sizeof(float)*the_bufsize);
 
         if(!initialize_buffers()) return -2;
         sendbufsize(the_bufsize/decimation);
 
-        timing_recovery_state_t state = timing_recovery_init(algorithm, decimation, add_q);
+        timing_recovery_state_t state = timing_recovery_init(algorithm, decimation, add_q, loop_gain, max_error);
 
         int debug_i=0;
         state.debug_writefiles = 1;
@@ -2756,14 +2763,24 @@ int main(int argc, char *argv[])
         }
     }
 
-    if(!strcmp(argv[1],"bpsk_costas_loop_cc")) //<samples_per_bits>
+    if(!strcmp(argv[1],"bpsk_costas_loop_cc")) //<loop_bandwidth> <damping_factor> <gain> [--dd | --decision_directed]
     {
-        float samples_per_bits;
-        if(argc<=2) return badsyntax("need required parameter (samples_per_bits)");
-        sscanf(argv[2],"%f",&samples_per_bits);
-        if(samples_per_bits<=0) return badsyntax("samples_per_bits should be > 0");
+        float loop_bandwidth;
+        if(argc<=2) return badsyntax("need required parameter (loop_bandwidth)");
+        sscanf(argv[2],"%f",&loop_bandwidth);
 
-        bpsk_costas_loop_state_t state = init_bpsk_costas_loop_cc(samples_per_bits);
+        float damping_factor;
+        if(argc<=3) return badsyntax("need required parameter (damping_factor)");
+        sscanf(argv[3],"%f",&damping_factor);
+
+        float gain;
+        if(argc<=4) return badsyntax("need required parameter (gain)");
+        sscanf(argv[4],"%f",&gain);
+
+        int decision_directed = !!(argc>5 && (!strcmp(argv[5], "--dd") || !strcmp(argv[5], "--decision_directed")));
+
+        bpsk_costas_loop_state_t state;
+        init_bpsk_costas_loop_cc(&state, decision_directed, damping_factor, loop_bandwidth, gain);
 
         if(!initialize_buffers()) return -2;
         sendbufsize(the_bufsize);
@@ -3083,13 +3100,17 @@ int main(int argc, char *argv[])
             return 0;
         }
 
+        int output_size=0;
+        FREAD_C;
         for(;;)
         {
             FEOF_CHECK;
-            FREAD_C;
-            apply_real_fir_cc((complexf*)input_buffer, (complexf*)output_buffer, the_bufsize, taps, num_taps);
-            FWRITE_C;
+            output_size = apply_real_fir_cc((complexf*)input_buffer, (complexf*)output_buffer, the_bufsize, taps, num_taps);
+            fwrite(output_buffer, sizeof(complexf), output_size, stdout);
+            //fprintf(stderr, "os = %d, is = %d, num_taps = %d\n", output_size, the_bufsize, num_taps);
             TRY_YIELD;
+            memmove((complexf*)input_buffer,((complexf*)input_buffer)+output_size,(the_bufsize-output_size)*sizeof(complexf)); 
+            fread(((complexf*)input_buffer)+(the_bufsize-output_size), sizeof(complexf), output_size, stdin);
         }
     }
 
@@ -3128,6 +3149,26 @@ int main(int argc, char *argv[])
         return 0;
     }
 
+    if(!strcmp(argv[1], "add_const_cc")) //<i> <q>
+    {
+        complexf x;
+        if(argc<=2) return badsyntax("required parameter <add_i> is missing.");
+        sscanf(argv[2],"%f",&iofv(x));
+        if(argc<=2) return badsyntax("required parameter <add_q> is missing.");
+        sscanf(argv[2],"%f",&qofv(x));
+
+        if(!sendbufsize(initialize_buffers())) return -2;
+        for(;;)
+        {
+            FEOF_CHECK;
+            FREAD_C;
+            add_const_cc((complexf*)input_buffer, (complexf*)output_buffer, the_bufsize, x);
+            FWRITE_C;
+            TRY_YIELD;
+        }
+        return 0;
+    }
+
     if(!strcmp(argv[1],"none"))
     {
         return 0;
diff --git a/grc_tests/test_bpsk_costas_loop.grc b/grc_tests/test_bpsk_costas_loop.grc
index 9d2262e..0abca69 100644
--- a/grc_tests/test_bpsk_costas_loop.grc
+++ b/grc_tests/test_bpsk_costas_loop.grc
@@ -89,7 +89,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(480, 3)</value>
+      <value>(416, 3)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -207,7 +207,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(712, 571)</value>
+      <value>(32, 275)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -235,7 +235,7 @@
     </param>
     <param>
       <key>notebook</key>
-      <value>nb, 1</value>
+      <value></value>
     </param>
     <param>
       <key>num_steps</key>
@@ -270,7 +270,54 @@
     </param>
     <param>
       <key>value</key>
-      <value>2**18</value>
+      <value>2**13</value>
+    </param>
+  </block>
+  <block>
+    <key>analog_const_source_x</key>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>comment</key>
+      <value></value>
+    </param>
+    <param>
+      <key>const</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(288, 203)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>id</key>
+      <value>analog_const_source_x_0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>float</value>
     </param>
   </block>
   <block>
@@ -293,7 +340,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(16, 187)</value>
+      <value>(40, 147)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -325,7 +372,7 @@
     </param>
     <param>
       <key>type</key>
-      <value>byte</value>
+      <value>int</value>
     </param>
     <param>
       <key>repeat</key>
@@ -333,7 +380,7 @@
     </param>
   </block>
   <block>
-    <key>blocks_complex_to_float</key>
+    <key>blocks_add_const_vxx</key>
     <param>
       <key>alias</key>
       <value></value>
@@ -343,51 +390,8 @@
       <value></value>
     </param>
     <param>
-      <key>affinity</key>
-      <value></value>
-    </param>
-    <param>
-      <key>_enabled</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>_coordinate</key>
-      <value>(960, 265)</value>
-    </param>
-    <param>
-      <key>_rotation</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>id</key>
-      <value>blocks_complex_to_float_0</value>
-    </param>
-    <param>
-      <key>maxoutbuf</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>minoutbuf</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>vlen</key>
-      <value>1</value>
-    </param>
-  </block>
-  <block>
-    <key>blocks_interleave</key>
-    <param>
-      <key>alias</key>
-      <value></value>
-    </param>
-    <param>
-      <key>blocksize</key>
-      <value>1</value>
-    </param>
-    <param>
-      <key>comment</key>
-      <value></value>
+      <key>const</key>
+      <value>-1</value>
     </param>
     <param>
       <key>affinity</key>
@@ -395,11 +399,11 @@
     </param>
     <param>
       <key>_enabled</key>
-      <value>0</value>
+      <value>True</value>
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(1168, 265)</value>
+      <value>(336, 131)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -407,7 +411,7 @@
     </param>
     <param>
       <key>id</key>
-      <value>blocks_interleave_0</value>
+      <value>blocks_add_const_vxx_0</value>
     </param>
     <param>
       <key>type</key>
@@ -421,17 +425,13 @@
       <key>minoutbuf</key>
       <value>0</value>
     </param>
-    <param>
-      <key>num_streams</key>
-      <value>2</value>
-    </param>
     <param>
       <key>vlen</key>
       <value>1</value>
     </param>
   </block>
   <block>
-    <key>blocks_multiply_const_vxx</key>
+    <key>blocks_float_to_complex</key>
     <param>
       <key>alias</key>
       <value></value>
@@ -440,10 +440,6 @@
       <key>comment</key>
       <value></value>
     </param>
-    <param>
-      <key>const</key>
-      <value>amplitude</value>
-    </param>
     <param>
       <key>affinity</key>
       <value></value>
@@ -454,7 +450,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(496, 155)</value>
+      <value>(496, 161)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -462,11 +458,7 @@
     </param>
     <param>
       <key>id</key>
-      <value>blocks_multiply_const_vxx_0</value>
-    </param>
-    <param>
-      <key>type</key>
-      <value>complex</value>
+      <value>blocks_float_to_complex_0</value>
     </param>
     <param>
       <key>maxoutbuf</key>
@@ -481,6 +473,53 @@
       <value>1</value>
     </param>
   </block>
+  <block>
+    <key>blocks_int_to_float</key>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>comment</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(200, 131)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>id</key>
+      <value>blocks_int_to_float_0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>scale</key>
+      <value>0.5</value>
+    </param>
+    <param>
+      <key>vlen</key>
+      <value>1</value>
+    </param>
+  </block>
   <block>
     <key>blocks_throttle</key>
     <param>
@@ -501,7 +540,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(488, 211)</value>
+      <value>(496, 355)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -556,7 +595,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(928, 537)</value>
+      <value>(864, 345)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -587,73 +626,6 @@
       <value>False</value>
     </param>
   </block>
-  <block>
-    <key>digital_psk_mod</key>
-    <param>
-      <key>alias</key>
-      <value></value>
-    </param>
-    <param>
-      <key>comment</key>
-      <value></value>
-    </param>
-    <param>
-      <key>affinity</key>
-      <value></value>
-    </param>
-    <param>
-      <key>differential</key>
-      <value>True</value>
-    </param>
-    <param>
-      <key>_enabled</key>
-      <value>True</value>
-    </param>
-    <param>
-      <key>excess_bw</key>
-      <value>0.35</value>
-    </param>
-    <param>
-      <key>_coordinate</key>
-      <value>(208, 179)</value>
-    </param>
-    <param>
-      <key>_rotation</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>mod_code</key>
-      <value>"none"</value>
-    </param>
-    <param>
-      <key>id</key>
-      <value>digital_psk_mod_0</value>
-    </param>
-    <param>
-      <key>log</key>
-      <value>False</value>
-    </param>
-    <param>
-      <key>maxoutbuf</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>minoutbuf</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>constellation_points</key>
-      <value>2</value>
-    </param>
-    <param>
-      <key>samples_per_symbol</key>
-      <value>2</value>
-    </param>
-    <param>
-      <key>verbose</key>
-      <value>False</value>
-    </param>
-  </block>
   <block>
     <key>freq_xlating_fir_filter_xxx</key>
     <param>
@@ -662,7 +634,7 @@
     </param>
     <param>
       <key>center_freq</key>
-      <value>-freq_add</value>
+      <value>freq_add</value>
     </param>
     <param>
       <key>comment</key>
@@ -678,11 +650,11 @@
     </param>
     <param>
       <key>_enabled</key>
-      <value>1</value>
+      <value>True</value>
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(688, 251)</value>
+      <value>(232, 331)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -713,41 +685,6 @@
       <value>ccc</value>
     </param>
   </block>
-  <block>
-    <key>ha5kfu_execproc_sink_f</key>
-    <param>
-      <key>alias</key>
-      <value></value>
-    </param>
-    <param>
-      <key>commandline</key>
-      <value>zsh -c \"csdr timing_recovery_cc GARDNER 16 --add_q --octave 1 1&gt;/dev/null 2&gt; &gt;(octave -i)\"</value>
-    </param>
-    <param>
-      <key>comment</key>
-      <value></value>
-    </param>
-    <param>
-      <key>affinity</key>
-      <value></value>
-    </param>
-    <param>
-      <key>_enabled</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>_coordinate</key>
-      <value>(976, 379)</value>
-    </param>
-    <param>
-      <key>_rotation</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>id</key>
-      <value>ha5kfu_execproc_sink_f_0</value>
-    </param>
-  </block>
   <block>
     <key>ha5kfu_execproc_xx</key>
     <param>
@@ -756,7 +693,7 @@
     </param>
     <param>
       <key>commandline</key>
-      <value>csdr bpsk_costas_loop_cc 16</value>
+      <value>csdr bpsk_costas_loop_cc $(csdr =2*pi/100) 0.707 1  </value>
     </param>
     <param>
       <key>comment</key>
@@ -772,7 +709,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(128, 411)</value>
+      <value>(840, 163)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -795,286 +732,6 @@
       <value>cc</value>
     </param>
   </block>
-  <block>
-    <key>ha5kfu_execproc_xx</key>
-    <param>
-      <key>alias</key>
-      <value></value>
-    </param>
-    <param>
-      <key>commandline</key>
-      <value>csdr timing_recovery_cc GARDNER 16 --add_q | csdr fir_interpolate_cc 8</value>
-    </param>
-    <param>
-      <key>comment</key>
-      <value></value>
-    </param>
-    <param>
-      <key>affinity</key>
-      <value></value>
-    </param>
-    <param>
-      <key>_enabled</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>_coordinate</key>
-      <value>(400, 411)</value>
-    </param>
-    <param>
-      <key>_rotation</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>id</key>
-      <value>ha5kfu_execproc_xx_0_0</value>
-    </param>
-    <param>
-      <key>maxoutbuf</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>minoutbuf</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>type</key>
-      <value>cc</value>
-    </param>
-  </block>
-  <block>
-    <key>notebook</key>
-    <param>
-      <key>alias</key>
-      <value></value>
-    </param>
-    <param>
-      <key>comment</key>
-      <value></value>
-    </param>
-    <param>
-      <key>_enabled</key>
-      <value>True</value>
-    </param>
-    <param>
-      <key>_coordinate</key>
-      <value>(16, 83)</value>
-    </param>
-    <param>
-      <key>_rotation</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>grid_pos</key>
-      <value></value>
-    </param>
-    <param>
-      <key>id</key>
-      <value>nb</value>
-    </param>
-    <param>
-      <key>labels</key>
-      <value>['1', '2']</value>
-    </param>
-    <param>
-      <key>notebook</key>
-      <value></value>
-    </param>
-    <param>
-      <key>style</key>
-      <value>wx.NB_TOP</value>
-    </param>
-  </block>
-  <block>
-    <key>wxgui_fftsink2</key>
-    <param>
-      <key>avg_alpha</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>average</key>
-      <value>False</value>
-    </param>
-    <param>
-      <key>baseband_freq</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>alias</key>
-      <value></value>
-    </param>
-    <param>
-      <key>comment</key>
-      <value></value>
-    </param>
-    <param>
-      <key>affinity</key>
-      <value></value>
-    </param>
-    <param>
-      <key>_enabled</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>fft_size</key>
-      <value>1024</value>
-    </param>
-    <param>
-      <key>freqvar</key>
-      <value>None</value>
-    </param>
-    <param>
-      <key>_coordinate</key>
-      <value>(960, 59)</value>
-    </param>
-    <param>
-      <key>_rotation</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>grid_pos</key>
-      <value></value>
-    </param>
-    <param>
-      <key>id</key>
-      <value>wxgui_fftsink2_0</value>
-    </param>
-    <param>
-      <key>notebook</key>
-      <value></value>
-    </param>
-    <param>
-      <key>peak_hold</key>
-      <value>False</value>
-    </param>
-    <param>
-      <key>ref_level</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>ref_scale</key>
-      <value>2.0</value>
-    </param>
-    <param>
-      <key>fft_rate</key>
-      <value>15</value>
-    </param>
-    <param>
-      <key>samp_rate</key>
-      <value>samp_rate</value>
-    </param>
-    <param>
-      <key>title</key>
-      <value>FFT Plot</value>
-    </param>
-    <param>
-      <key>type</key>
-      <value>complex</value>
-    </param>
-    <param>
-      <key>win_size</key>
-      <value></value>
-    </param>
-    <param>
-      <key>win</key>
-      <value>None</value>
-    </param>
-    <param>
-      <key>y_divs</key>
-      <value>10</value>
-    </param>
-    <param>
-      <key>y_per_div</key>
-      <value>10</value>
-    </param>
-  </block>
-  <block>
-    <key>wxgui_scopesink2</key>
-    <param>
-      <key>ac_couple</key>
-      <value>False</value>
-    </param>
-    <param>
-      <key>alias</key>
-      <value></value>
-    </param>
-    <param>
-      <key>comment</key>
-      <value></value>
-    </param>
-    <param>
-      <key>affinity</key>
-      <value></value>
-    </param>
-    <param>
-      <key>_enabled</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>_coordinate</key>
-      <value>(632, 379)</value>
-    </param>
-    <param>
-      <key>_rotation</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>grid_pos</key>
-      <value></value>
-    </param>
-    <param>
-      <key>id</key>
-      <value>wxgui_scopesink2_0</value>
-    </param>
-    <param>
-      <key>notebook</key>
-      <value></value>
-    </param>
-    <param>
-      <key>num_inputs</key>
-      <value>1</value>
-    </param>
-    <param>
-      <key>samp_rate</key>
-      <value>(samp_rate/256)*8</value>
-    </param>
-    <param>
-      <key>t_scale</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>title</key>
-      <value>After timing recovery</value>
-    </param>
-    <param>
-      <key>trig_mode</key>
-      <value>wxgui.TRIG_MODE_AUTO</value>
-    </param>
-    <param>
-      <key>type</key>
-      <value>complex</value>
-    </param>
-    <param>
-      <key>v_offset</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>v_scale</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>win_size</key>
-      <value></value>
-    </param>
-    <param>
-      <key>xy_mode</key>
-      <value>True</value>
-    </param>
-    <param>
-      <key>y_axis_label</key>
-      <value>Counts</value>
-    </param>
-  </block>
   <block>
     <key>wxgui_scopesink2</key>
     <param>
@@ -1099,7 +756,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(432, 507)</value>
+      <value>(1104, 123)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -1115,7 +772,7 @@
     </param>
     <param>
       <key>notebook</key>
-      <value>nb, 0</value>
+      <value></value>
     </param>
     <param>
       <key>num_inputs</key>
@@ -1123,7 +780,7 @@
     </param>
     <param>
       <key>samp_rate</key>
-      <value>(samp_rate/256)*8</value>
+      <value>samp_rate</value>
     </param>
     <param>
       <key>t_scale</key>
@@ -1131,7 +788,7 @@
     </param>
     <param>
       <key>title</key>
-      <value>BPSK modulated signal after Costas Loop</value>
+      <value>After Costas Loop (csdr)</value>
     </param>
     <param>
       <key>trig_mode</key>
@@ -1186,7 +843,7 @@
     </param>
     <param>
       <key>_coordinate</key>
-      <value>(1112, 491)</value>
+      <value>(1112, 299)</value>
     </param>
     <param>
       <key>_rotation</key>
@@ -1200,93 +857,6 @@
       <key>id</key>
       <value>wxgui_scopesink2_0_0_0</value>
     </param>
-    <param>
-      <key>notebook</key>
-      <value>nb, 1</value>
-    </param>
-    <param>
-      <key>num_inputs</key>
-      <value>1</value>
-    </param>
-    <param>
-      <key>samp_rate</key>
-      <value>(samp_rate/256)*8</value>
-    </param>
-    <param>
-      <key>t_scale</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>title</key>
-      <value>After Costas Loop</value>
-    </param>
-    <param>
-      <key>trig_mode</key>
-      <value>wxgui.TRIG_MODE_AUTO</value>
-    </param>
-    <param>
-      <key>type</key>
-      <value>complex</value>
-    </param>
-    <param>
-      <key>v_offset</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>v_scale</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>win_size</key>
-      <value></value>
-    </param>
-    <param>
-      <key>xy_mode</key>
-      <value>True</value>
-    </param>
-    <param>
-      <key>y_axis_label</key>
-      <value>Counts</value>
-    </param>
-  </block>
-  <block>
-    <key>wxgui_scopesink2</key>
-    <param>
-      <key>ac_couple</key>
-      <value>False</value>
-    </param>
-    <param>
-      <key>alias</key>
-      <value></value>
-    </param>
-    <param>
-      <key>comment</key>
-      <value></value>
-    </param>
-    <param>
-      <key>affinity</key>
-      <value></value>
-    </param>
-    <param>
-      <key>_enabled</key>
-      <value>True</value>
-    </param>
-    <param>
-      <key>_coordinate</key>
-      <value>(688, 11)</value>
-    </param>
-    <param>
-      <key>_rotation</key>
-      <value>0</value>
-    </param>
-    <param>
-      <key>grid_pos</key>
-      <value></value>
-    </param>
-    <param>
-      <key>id</key>
-      <value>wxgui_scopesink2_0_1</value>
-    </param>
     <param>
       <key>notebook</key>
       <value></value>
@@ -1305,7 +875,7 @@
     </param>
     <param>
       <key>title</key>
-      <value>BPSK modulated signal with frequency offset</value>
+      <value>After Costas Loop (GNU Radio)</value>
     </param>
     <param>
       <key>trig_mode</key>
@@ -1329,7 +899,7 @@
     </param>
     <param>
       <key>xy_mode</key>
-      <value>False</value>
+      <value>True</value>
     </param>
     <param>
       <key>y_axis_label</key>
@@ -1337,38 +907,44 @@
     </param>
   </block>
   <connection>
-    <source_block_id>analog_random_source_x_0</source_block_id>
-    <sink_block_id>digital_psk_mod_0</sink_block_id>
+    <source_block_id>analog_const_source_x_0</source_block_id>
+    <sink_block_id>blocks_float_to_complex_0</sink_block_id>
     <source_key>0</source_key>
-    <sink_key>0</sink_key>
-  </connection>
-  <connection>
-    <source_block_id>blocks_complex_to_float_0</source_block_id>
-    <sink_block_id>blocks_interleave_0</sink_block_id>
-    <source_key>1</source_key>
     <sink_key>1</sink_key>
   </connection>
   <connection>
-    <source_block_id>blocks_complex_to_float_0</source_block_id>
-    <sink_block_id>blocks_interleave_0</sink_block_id>
+    <source_block_id>analog_random_source_x_0</source_block_id>
+    <sink_block_id>blocks_int_to_float_0</sink_block_id>
     <source_key>0</source_key>
     <sink_key>0</sink_key>
   </connection>
   <connection>
-    <source_block_id>blocks_interleave_0</source_block_id>
-    <sink_block_id>ha5kfu_execproc_sink_f_0</sink_block_id>
+    <source_block_id>blocks_add_const_vxx_0</source_block_id>
+    <sink_block_id>blocks_float_to_complex_0</sink_block_id>
     <source_key>0</source_key>
     <sink_key>0</sink_key>
   </connection>
   <connection>
-    <source_block_id>blocks_multiply_const_vxx_0</source_block_id>
-    <sink_block_id>blocks_throttle_0</sink_block_id>
+    <source_block_id>blocks_float_to_complex_0</source_block_id>
+    <sink_block_id>freq_xlating_fir_filter_xxx_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_int_to_float_0</source_block_id>
+    <sink_block_id>blocks_add_const_vxx_0</sink_block_id>
     <source_key>0</source_key>
     <sink_key>0</sink_key>
   </connection>
   <connection>
     <source_block_id>blocks_throttle_0</source_block_id>
-    <sink_block_id>freq_xlating_fir_filter_xxx_0</sink_block_id>
+    <sink_block_id>digital_costas_loop_cc_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_throttle_0</source_block_id>
+    <sink_block_id>ha5kfu_execproc_xx_0</sink_block_id>
     <source_key>0</source_key>
     <sink_key>0</sink_key>
   </connection>
@@ -1378,45 +954,9 @@
     <source_key>0</source_key>
     <sink_key>0</sink_key>
   </connection>
-  <connection>
-    <source_block_id>digital_psk_mod_0</source_block_id>
-    <sink_block_id>blocks_multiply_const_vxx_0</sink_block_id>
-    <source_key>0</source_key>
-    <sink_key>0</sink_key>
-  </connection>
   <connection>
     <source_block_id>freq_xlating_fir_filter_xxx_0</source_block_id>
-    <sink_block_id>blocks_complex_to_float_0</sink_block_id>
-    <source_key>0</source_key>
-    <sink_key>0</sink_key>
-  </connection>
-  <connection>
-    <source_block_id>freq_xlating_fir_filter_xxx_0</source_block_id>
-    <sink_block_id>digital_costas_loop_cc_0</sink_block_id>
-    <source_key>0</source_key>
-    <sink_key>0</sink_key>
-  </connection>
-  <connection>
-    <source_block_id>freq_xlating_fir_filter_xxx_0</source_block_id>
-    <sink_block_id>ha5kfu_execproc_xx_0</sink_block_id>
-    <source_key>0</source_key>
-    <sink_key>0</sink_key>
-  </connection>
-  <connection>
-    <source_block_id>freq_xlating_fir_filter_xxx_0</source_block_id>
-    <sink_block_id>wxgui_fftsink2_0</sink_block_id>
-    <source_key>0</source_key>
-    <sink_key>0</sink_key>
-  </connection>
-  <connection>
-    <source_block_id>freq_xlating_fir_filter_xxx_0</source_block_id>
-    <sink_block_id>wxgui_scopesink2_0_1</sink_block_id>
-    <source_key>0</source_key>
-    <sink_key>0</sink_key>
-  </connection>
-  <connection>
-    <source_block_id>ha5kfu_execproc_xx_0</source_block_id>
-    <sink_block_id>ha5kfu_execproc_xx_0_0</sink_block_id>
+    <sink_block_id>blocks_throttle_0</sink_block_id>
     <source_key>0</source_key>
     <sink_key>0</sink_key>
   </connection>
@@ -1426,10 +966,4 @@
     <source_key>0</source_key>
     <sink_key>0</sink_key>
   </connection>
-  <connection>
-    <source_block_id>ha5kfu_execproc_xx_0_0</source_block_id>
-    <sink_block_id>wxgui_scopesink2_0</sink_block_id>
-    <source_key>0</source_key>
-    <sink_key>0</sink_key>
-  </connection>
 </flow_graph>
diff --git a/libcsdr.c b/libcsdr.c
index 15a147b..d06bb5d 100755
--- a/libcsdr.c
+++ b/libcsdr.c
@@ -114,6 +114,16 @@ float (*firdes_get_window_kernel(window_t window))(float)
                                                                    |___/
 */
 
+void normalize_fir_f(float* input, float* output, int length)
+{
+    //Normalize filter kernel
+    float sum=0;
+    for(int i=0;i<length;i++) //@normalize_fir_f: normalize pass 1
+        sum+=input[i];
+    for(int i=0;i<length;i++) //@normalize_fir_f: normalize pass 2
+        output[i]=input[i]/sum;
+}
+
 void firdes_lowpass_f(float *output, int length, float cutoff_rate, window_t window)
 {   //Generates symmetric windowed sinc FIR filter real taps
     //  length should be odd
@@ -128,17 +138,7 @@ void firdes_lowpass_f(float *output, int length, float cutoff_rate, window_t win
         output[middle-i]=output[middle+i]=(sin(2*PI*cutoff_rate*i)/i)*window_function((float)i/middle);
         //printf("%g %d %d %d %d | %g\n",output[middle-i],i,middle,middle+i,middle-i,sin(2*PI*cutoff_rate*i));
     }
-
-    //Normalize filter kernel
-    float sum=0;
-    for(int i=0;i<length;i++) //@firdes_lowpass_f: normalize pass 1
-    {
-        sum+=output[i];
-    }
-    for(int i=0;i<length;i++) //@firdes_lowpass_f: normalize pass 2
-    {
-        output[i]/=sum;
-    }
+    normalize_fir_f(output,output,length);
 }
 
 void firdes_bandpass_c(complexf *output, int length, float lowcut, float highcut, window_t window)
@@ -1930,11 +1930,13 @@ void octave_plot_point_on_cplxsig(complexf* signal, int signal_size, float error
     free(points_color);
 }
 
-timing_recovery_state_t timing_recovery_init(timing_recovery_algorithm_t algorithm, int decimation_rate, int use_q)
+timing_recovery_state_t timing_recovery_init(timing_recovery_algorithm_t algorithm, int decimation_rate, int use_q, float loop_gain, float max_error)
 {
     timing_recovery_state_t to_return;
     to_return.algorithm = algorithm;
     to_return.decimation_rate = decimation_rate;
+    to_return.loop_gain = loop_gain;
+    to_return.max_error = max_error;
     to_return.use_q = use_q;
     to_return.debug_phase = -1;
     to_return.debug_count = 3;
@@ -1952,7 +1954,7 @@ void timing_recovery_trigger_debug(timing_recovery_state_t* state, int debug_pha
 
 #define MTIMINGR_HDEBUG 0
 
-void timing_recovery_cc(complexf* input, complexf* output, int input_size, float* timing_error, int* sampled_indexes,  timing_recovery_state_t* state)
+void timing_recovery_cc(complexf* input, complexf* output, int input_size, float* timing_error, int* sampled_indexes, timing_recovery_state_t* state)
 {
     //We always assume that the input starts at center of the first symbol cross before the first symbol.
     //Last time we consumed that much from the input samples that it is there.
@@ -2015,8 +2017,8 @@ void timing_recovery_cc(complexf* input, complexf* output, int input_size, float
 
             if(timing_error) timing_error[si-1]=error; //it is not written if NULL
             
-            if(error>2) error=2;
-            if(error<-2) error=-2;
+            if(error>state->max_error) error=state->max_error;
+            if(error<-state->max_error) error=-state->max_error;
             if( state->debug_force || (state->debug_phase >= si && debug_i) )
             {
                 debug_i--;
@@ -2033,7 +2035,7 @@ void timing_recovery_cc(complexf* input, complexf* output, int input_size, float
                     0);
             }
             int error_sign = (state->algorithm == TIMING_RECOVERY_ALGORITHM_GARDNER) ? -1 : 1;
-            correction_offset = num_samples_halfbit * error_sign * (error/2);
+            correction_offset = num_samples_halfbit * error_sign * error * state->loop_gain;
             current_bitstart_index += num_samples_bit + correction_offset;
             if(si>=input_size) 
             { 
@@ -2067,6 +2069,45 @@ char* timing_recovery_get_string_from_algorithm(timing_recovery_algorithm_t algo
     return "INVALID";
 }
 
+void init_bpsk_costas_loop_cc(bpsk_costas_loop_state_t* s, int decision_directed, float damping_factor, float bandwidth, float gain)
+{
+    float bandwidth_omega = 2*M_PI*bandwidth;
+    s->alpha = (damping_factor*2*bandwidth_omega)/gain;
+    float sampling_rate = 1; //the bandwidth is normalized to the sampling rate
+    s->beta = (bandwidth_omega*bandwidth_omega)/(sampling_rate*gain);
+    s->iir_temp = s->dphase = s->nco_phase = 0;
+}
+
+void bpsk_costas_loop_cc(complexf* input, complexf* output, int input_size, bpsk_costas_loop_state_t* s)
+{
+    for(int i=0;i<input_size;i++)
+    {
+        complexf nco_sample;
+        e_powj(&nco_sample, -s->nco_phase);
+        cmult(&output[i], &input[i], &nco_sample);
+        float error = 0;
+        if(s->decision_directed)
+        {
+            float output_phase = atan2(qof(output,i),iof(output,i));
+            if (fabs(output_phase)<PI/2) 
+                error = -output_phase;
+            else
+            {
+                error = PI-output_phase;
+                while(error>PI) error -= 2*PI;
+            }
+        }
+        else error = iof(output,i)*qof(output,i);
+        s->dphase = error * s->alpha + s->iir_temp;
+        s->iir_temp += error * s->beta;
+
+        //step NCO
+        s->nco_phase += s->dphase;
+        while(s->nco_phase>2*PI) s->nco_phase-=2*PI;
+    }
+}
+
+#if 0
 bpsk_costas_loop_state_t init_bpsk_costas_loop_cc(float samples_per_bits) 
 { 
     bpsk_costas_loop_state_t state;
@@ -2088,7 +2129,7 @@ bpsk_costas_loop_state_t init_bpsk_costas_loop_cc(float samples_per_bits)
     return state;
 }
 
-void bpsk_costas_loop_cc(complexf* input, complexf* output, int input_size, bpsk_costas_loop_state_t* state)
+void bpsk_costas_loop_c1mc(complexf* input, complexf* output, int input_size, bpsk_costas_loop_state_t* state)
 {
     int debug = 0;
     if(debug) fprintf(stderr, "costas:\n");
@@ -2122,6 +2163,8 @@ void bpsk_costas_loop_cc(complexf* input, complexf* output, int input_size, bpsk
     }
 }
 
+#endif
+
 void simple_agc_cc(complexf* input, complexf* output, int input_size, float rate, float reference, float max_gain, float* current_gain)
 {
     float rate_1minus=1-rate;
@@ -2205,8 +2248,8 @@ int apply_real_fir_cc(complexf* input, complexf* output, int input_size, float*
         float acci = 0, accq = 0;
         for(int ti=0;ti<taps_length;ti++)
         {
-            acci += iof(input,i)*taps[ti];
-            accq += qof(input,i)*taps[ti];
+            acci += iof(input,i+ti)*taps[ti];
+            accq += qof(input,i+ti)*taps[ti];
         }
         iof(output,i)=acci;
         qof(output,i)=accq;
@@ -2365,6 +2408,8 @@ int firdes_cosine_f(float* taps, int taps_length, int samples_per_symbol)
     //needs a taps_length 2 × samples_per_symbol + 1
     int middle_i=taps_length/2;
     for(int i=0;i<samples_per_symbol;i++) taps[middle_i+i]=taps[middle_i-i]=(1+cos(PI*i/(float)samples_per_symbol))/2;
+    //for(int i=0;i<taps_length;i++) taps[i]=powf(taps[i],2);
+    normalize_fir_f(taps, taps, taps_length);
 }
 
 int firdes_rrc_f(float* taps, int taps_length, int samples_per_symbol, float beta)
@@ -2381,6 +2426,7 @@ int firdes_rrc_f(float* taps, int taps_length, int samples_per_symbol, float bet
                 (sin(PI*(i/(float)samples_per_symbol)*(1-beta)) + 4*beta*(i/(float)samples_per_symbol)*cos(PI*(i/(float)samples_per_symbol)*(1+beta)))/
                 (PI*(i/(float)samples_per_symbol)*(1-powf(4*beta*(i/(float)samples_per_symbol),2)));
     }
+    normalize_fir_f(taps, taps, taps_length);
 }
 
 void plain_interpolate_cc(complexf* input, complexf* output, int input_size, int interpolation)
@@ -2407,6 +2453,15 @@ float* add_ff(float* input1, float* input2, float* output, int input_size)
     for(int i=0;i<input_size;i++) output[i]=input1[i]+input2[i];
 }
 
+float* add_const_cc(complexf* input, complexf* output, int input_size, complexf x)
+{
+    for(int i=0;i<input_size;i++)
+    {
+        iof(output,i)=iof(input,i)+iofv(x);
+        qof(output,i)=iof(input,i)+qofv(x);
+    }
+}
+
 int trivial_vectorize()
 {
     //this function is trivial to vectorize and should pass on both NEON and SSE
@@ -2417,4 +2472,3 @@ int trivial_vectorize()
     }
     return c[0];
 }
-void plain_interpolate_cc(complexf* input, complexf* output, int input_size, int interpolation);
diff --git a/libcsdr.h b/libcsdr.h
index c69c62f..2b4d3ab 100644
--- a/libcsdr.h
+++ b/libcsdr.h
@@ -331,9 +331,11 @@ typedef struct timing_recovery_state_s
     int debug_writefiles;
     int last_correction_offset;
     float earlylate_ratio;
+    float loop_gain;
+    float max_error;
 } timing_recovery_state_t;
 
-timing_recovery_state_t timing_recovery_init(timing_recovery_algorithm_t algorithm, int decimation_rate, int use_q);
+timing_recovery_state_t timing_recovery_init(timing_recovery_algorithm_t algorithm, int decimation_rate, int use_q, float loop_gain, float max_error);
 void timing_recovery_cc(complexf* input, complexf* output, int input_size, float* timing_error, int* sampled_indexes,  timing_recovery_state_t* state);
 timing_recovery_algorithm_t timing_recovery_get_algorithm_from_string(char* input);
 char* timing_recovery_get_string_from_algorithm(timing_recovery_algorithm_t algorithm);
@@ -346,6 +348,7 @@ void pack_bits_8to1_u8_u8(unsigned char* input, unsigned char* output, int input
 void psk31_varicode_encoder_u8_u8(unsigned char* input, unsigned char* output, int input_size, int output_max_size, int* input_processed, int* output_size);
 unsigned char differential_codec(unsigned char* input, unsigned char* output, int input_size, int encode, unsigned char state);
 
+#if 0
 typedef struct bpsk_costas_loop_state_s
 {
     float rc_filter_alpha;
@@ -358,6 +361,21 @@ typedef struct bpsk_costas_loop_state_s
 
 bpsk_costas_loop_state_t init_bpsk_costas_loop_cc(float samples_per_bits);
 void bpsk_costas_loop_cc(complexf* input, complexf* output, int input_size, bpsk_costas_loop_state_t* state);
+#endif 
+
+typedef struct bpsk_costas_loop_state_s
+{
+    float alpha;
+    float beta;
+    int decision_directed;
+    float iir_temp;
+    float dphase;
+    float nco_phase;
+} bpsk_costas_loop_state_t;
+
+void plain_interpolate_cc(complexf* input, complexf* output, int input_size, int interpolation);
+void bpsk_costas_loop_cc(complexf* input, complexf* output, int input_size, bpsk_costas_loop_state_t* state);
+
 void simple_agc_cc(complexf* input, complexf* output, int input_size, float rate, float reference, float max_gain, float* current_gain);
 void firdes_add_resonator_c(complexf* output, int length, float rate, window_t window, int add, int normalize);
 int apply_fir_cc(complexf* input, complexf* output, int input_size, complexf* taps, int taps_length);
@@ -385,3 +403,6 @@ matched_filter_type_t matched_filter_get_type_from_string(char* input);
 int apply_real_fir_cc(complexf* input, complexf* output, int input_size, float* taps, int taps_length);
 void generic_slicer_f_u8(float* input, unsigned char* output, int input_size, int n_symbols);
 void plain_interpolate_cc(complexf* input, complexf* output, int input_size, int interpolation);;
+void normalize_fir_f(float* input, float* output, int length);
+void init_bpsk_costas_loop_cc(bpsk_costas_loop_state_t* s, int decision_directed, float damping_factor, float bandwidth, float gain);
+float* add_const_cc(complexf* input, complexf* output, int input_size, complexf x);