diff --git a/cuhd/include/srslte/cuhd/cuhd.h b/cuhd/include/srslte/cuhd/cuhd.h
index 1608eab1e..87d6858e3 100644
--- a/cuhd/include/srslte/cuhd/cuhd.h
+++ b/cuhd/include/srslte/cuhd/cuhd.h
@@ -58,6 +58,8 @@ SRSLTE_API double cuhd_set_rx_srate(void *h,
 SRSLTE_API double cuhd_set_rx_gain(void *h, 
                                    double gain);
 
+SRSLTE_API double cuhd_get_rx_gain(void *h);
+
 SRSLTE_API double cuhd_set_rx_freq(void *h, 
                                    double freq);
 
diff --git a/cuhd/lib/cuhd_imp.cpp b/cuhd/lib/cuhd_imp.cpp
index a0781259e..3b1da83c3 100644
--- a/cuhd/lib/cuhd_imp.cpp
+++ b/cuhd/lib/cuhd_imp.cpp
@@ -164,6 +164,12 @@ double cuhd_set_rx_gain(void *h, double gain)
   return handler->usrp->get_rx_gain();
 }
 
+double cuhd_get_rx_gain(void *h)
+{
+  cuhd_handler *handler = static_cast < cuhd_handler * >(h);
+  return handler->usrp->get_rx_gain();
+}
+
 double cuhd_set_rx_freq(void *h, double freq)
 {
   cuhd_handler *handler = static_cast < cuhd_handler * >(h);
diff --git a/srslte/examples/cuhd_utils.c b/srslte/examples/cuhd_utils.c
index ab709593f..41e7baec6 100644
--- a/srslte/examples/cuhd_utils.c
+++ b/srslte/examples/cuhd_utils.c
@@ -60,6 +60,8 @@ int cuhd_mib_decoder(void *uhd, uint32_t max_nof_frames, srslte_cell_t *cell) {
     goto clean_exit; 
   }
   
+  srslte_ue_sync_start_agc(&ue_mib.ue_sync, cuhd_set_rx_gain);
+
   int srate = srslte_sampling_freq_hz(SRSLTE_UE_MIB_NOF_PRB);
   INFO("Setting sampling frequency %.2f MHz for PSS search\n", (float) srate/1000000);
   cuhd_set_rx_srate(uhd, (float) srate);
@@ -107,6 +109,8 @@ int cuhd_cell_search(void *uhd, cell_search_cfg_t *config,
   if (config->threshold) {
     srslte_ue_cellsearch_set_threshold(&cs, config->threshold);
   }
+  
+  srslte_ue_sync_start_agc(&cs.ue_sync, cuhd_set_rx_gain);
 
   INFO("Setting sampling frequency %.2f MHz for PSS search\n", SRSLTE_CS_SAMP_FREQ/1000000);
   cuhd_set_rx_srate(uhd, SRSLTE_CS_SAMP_FREQ);
diff --git a/srslte/examples/pdsch_ue.c b/srslte/examples/pdsch_ue.c
index bcd5dbe8b..caba04b44 100644
--- a/srslte/examples/pdsch_ue.c
+++ b/srslte/examples/pdsch_ue.c
@@ -369,6 +369,8 @@ int main(int argc, char **argv) {
   bool decode_pdsch; 
   int pdcch_tx=0; 
           
+  srslte_ue_sync_start_agc(&ue_sync, cuhd_set_rx_gain);
+  
   INFO("\nEntering main loop...\n\n", 0);
   /* Main loop */
   while (!go_exit && (sf_cnt < prog_args.nof_subframes || prog_args.nof_subframes == -1)) {
@@ -377,7 +379,7 @@ int main(int argc, char **argv) {
     if (ret < 0) {
       fprintf(stderr, "Error calling srslte_ue_sync_work()\n");
     }
-            
+     
     /* srslte_ue_sync_get_buffer returns 1 if successfully read 1 aligned subframe */
     if (ret == 1) {
       switch (state) {
@@ -458,7 +460,16 @@ int main(int argc, char **argv) {
           if (srslte_ue_sync_get_sfidx(&ue_sync) != 5 && srslte_ue_sync_get_sfidx(&ue_sync) != 0) {
             pdcch_tx++;
           }
-          
+
+          float max=-999;
+          for (int i=0;i<SRSLTE_SF_LEN_PRB(cell.nof_prb);i++) {
+            if (fabs(crealf(sf_buffer[i])) > max) {
+              max = fabs(crealf(sf_buffer[i]));
+            }
+            if (fabs(cimagf(sf_buffer[i])) > max) {
+              max = fabs(cimagf(sf_buffer[i]));
+            }
+          }
           
           // Plot and Printf
           if (srslte_ue_sync_get_sfidx(&ue_sync) == 5) {
@@ -468,13 +479,15 @@ int main(int argc, char **argv) {
                   (float) 100*ue_dl.pkt_errors/ue_dl.pkts_total,ue_dl.pkt_errors);                
 #else
             printf("CFO: %+6.2f KHz, SFO: %+6.2f Khz, "
-                  "RSRP: %+5.1f dBm, RSRQ: %5.1f dB, SNR: %4.1f dB, "
-                  "PDCCH-Miss: %5.2f%% (%u), PDSCH-BLER: %5.2f%% (%u)\r",
+                  "RSRP: %+5.1f dBm, SNR: %4.1f dB, "
+                  "PDCCH-Miss: %5.2f%% (%u), PDSCH-BLER: %5.2f%% Peak: %.2f Gain: %.1f dB/%.1f\r",
                   srslte_ue_sync_get_cfo(&ue_sync)/1000, srslte_ue_sync_get_sfo(&ue_sync)/1000, 
                   10*log10(rsrp*1000)-gain_offset, 
-                  10*log10(rsrq), 10*log10(snr), 
+                  10*log10(snr), 
                   100*(1-(float) ue_dl.nof_detected/nof_trials), pdcch_tx-ue_dl.nof_detected, 
-                  (float) 100*ue_dl.pkt_errors/ue_dl.pkts_total, ue_dl.pkt_errors);                
+                  (float) 100*ue_dl.pkt_errors/ue_dl.pkts_total, max, cuhd_get_rx_gain(uhd), 
+                  10*log10(ue_sync.agc.gain)
+                  );                
             
 #endif            
           }
diff --git a/srslte/include/srslte/agc/agc.h b/srslte/include/srslte/agc/agc.h
index 4fd1b15ed..3ab72898e 100644
--- a/srslte/include/srslte/agc/agc.h
+++ b/srslte/include/srslte/agc/agc.h
@@ -43,24 +43,42 @@
 
 #include "srslte/config.h"
 
-#define SRSLTE_AGC_DEFAULT_BW  (5e-2)
+#define SRSLTE_AGC_DEFAULT_TARGET 0.5
+#define SRSLTE_AGC_DEFAULT_BW     (5e-1)
+
+typedef enum SRSLTE_API {
+  SRSLTE_AGC_MODE_ENERGY = 0, 
+  SRSLTE_AGC_MODE_PEAK_AMPLITUDE  
+} srslte_agc_mode_t; 
 
 typedef struct SRSLTE_API{
   float bandwidth;
-  float gain; 
+  double gain; 
   float y_out;
   bool lock;
   bool isfirst; 
+  void *uhd_handler; 
+  double (*set_gain_callback) (void*,double);
+  srslte_agc_mode_t mode; 
+  float target;
 } srslte_agc_t;
 
-SRSLTE_API int srslte_agc_init (srslte_agc_t *q);
+SRSLTE_API int srslte_agc_init(srslte_agc_t *q, srslte_agc_mode_t mode);
+
+SRSLTE_API int srslte_agc_init_uhd(srslte_agc_t *q, 
+                                   srslte_agc_mode_t mode, 
+                                   double (set_gain_callback)(void*, double), 
+                                   void *uhd_handler); 
 
 SRSLTE_API void srslte_agc_free(srslte_agc_t *q);
 
 SRSLTE_API void srslte_agc_reset(srslte_agc_t *q);
 
 SRSLTE_API void srslte_agc_set_bandwidth(srslte_agc_t *q, 
-                                  float bandwidth);
+                                         float bandwidth);
+
+SRSLTE_API void srslte_agc_set_target(srslte_agc_t *q, 
+                                      float target);
 
 SRSLTE_API float srslte_agc_get_rssi(srslte_agc_t *q);
 
diff --git a/srslte/include/srslte/ue/ue_sync.h b/srslte/include/srslte/ue/ue_sync.h
index c4fd312e6..1ba29ad27 100644
--- a/srslte/include/srslte/ue/ue_sync.h
+++ b/srslte/include/srslte/ue/ue_sync.h
@@ -55,6 +55,7 @@
 #include "srslte/config.h"
 #include "srslte/sync/sync.h"
 #include "srslte/sync/cfo.h"
+#include "srslte/agc/agc.h"
 #include "srslte/ch_estimation/chest_dl.h"
 #include "srslte/phch/pbch.h"
 #include "srslte/dft/ofdm.h"
@@ -69,7 +70,10 @@ typedef enum SRSLTE_API { SF_FIND, SF_TRACK} srslte_ue_sync_state_t;
 typedef struct SRSLTE_API {
   srslte_sync_t sfind;
   srslte_sync_t strack;
-
+  
+  srslte_agc_t agc; 
+  bool do_agc; 
+  
   void *stream; 
   int (*recv_callback)(void*, void*, uint32_t, srslte_timestamp_t*); 
   srslte_timestamp_t last_timestamp;
@@ -123,6 +127,9 @@ SRSLTE_API int srslte_ue_sync_init_file(srslte_ue_sync_t *q,
 
 SRSLTE_API void srslte_ue_sync_free(srslte_ue_sync_t *q);
 
+SRSLTE_API int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, 
+                                        double (set_gain_callback)(void*, double)); 
+
 SRSLTE_API uint32_t srslte_ue_sync_sf_len(srslte_ue_sync_t *q); 
 
 SRSLTE_API int srslte_ue_sync_get_buffer(srslte_ue_sync_t *q, 
diff --git a/srslte/lib/agc/src/agc.c b/srslte/lib/agc/src/agc.c
index f00735ff9..d8419ffc6 100644
--- a/srslte/lib/agc/src/agc.c
+++ b/srslte/lib/agc/src/agc.c
@@ -39,12 +39,25 @@
 #include "srslte/utils/debug.h"
 
 
-int srslte_agc_init (srslte_agc_t *q) {
+int srslte_agc_init (srslte_agc_t *q, srslte_agc_mode_t mode) {
   bzero(q, sizeof(srslte_agc_t));
+  q->mode = mode; 
+  q->target = SRSLTE_AGC_DEFAULT_TARGET; 
   srslte_agc_reset(q);
   return SRSLTE_SUCCESS;
 }
 
+int srslte_agc_init_uhd(srslte_agc_t *q, srslte_agc_mode_t mode, double (set_gain_callback)(void*, double), void *uhd_handler) {
+  if (!srslte_agc_init(q, mode)) {
+    q->set_gain_callback = set_gain_callback;
+    q->uhd_handler = uhd_handler;    
+    set_gain_callback(uhd_handler, 30.0);
+    return SRSLTE_SUCCESS;
+  } else {
+    return SRSLTE_ERROR; 
+  }
+}
+
 void srslte_agc_free(srslte_agc_t *q) {
   bzero(q, sizeof(srslte_agc_t));
 }
@@ -61,6 +74,10 @@ void srslte_agc_set_bandwidth(srslte_agc_t *q, float bandwidth) {
   q->bandwidth = bandwidth;
 }
 
+void srslte_agc_set_target(srslte_agc_t *q, float target) {
+  q->target = target;
+}
+
 float srslte_agc_get_rssi(srslte_agc_t *q) {
   return 1.0/q->gain;
 }
@@ -73,28 +90,60 @@ float srslte_agc_get_gain(srslte_agc_t *q) {
   return q->gain;
 }
 
-
 void srslte_agc_lock(srslte_agc_t *q, bool enable) {
   q->lock = enable;
 }
 
 void srslte_agc_process(srslte_agc_t *q, cf_t *input, cf_t *output, uint32_t len) {
-  
+  float gain_res = 1.0;
   // Apply current gain to input signal 
-  srslte_vec_sc_prod_cfc(input, q->gain, output, len);
-  
-  // compute output energy estimate
-  float y = sqrtf(crealf(srslte_vec_dot_prod_conj_ccc(output, output, len))/len);
+  if (!q->uhd_handler) {
+    srslte_vec_sc_prod_cfc(input, q->gain, output, len);
+  } else {
+    if (q->gain < 1) {
+      q->gain = 1.0; 
+    }
+    if (isinf(10*log10(q->gain)) || isnan(10*log10(q->gain))) {
+      q->gain = 1.0; 
+    }
+    gain_res = q->set_gain_callback(q->uhd_handler, 10*log10(q->gain));
+    gain_res = pow(10, gain_res/10);
+    if (gain_res > q->gain) {
+      q->gain = gain_res; 
+    }
+  }
+  float y = 0; 
+  switch(q->mode) {
+    case SRSLTE_AGC_MODE_ENERGY:
+      y = sqrtf(crealf(srslte_vec_dot_prod_conj_ccc(output, output, len))/len);
+      break;
+    case SRSLTE_AGC_MODE_PEAK_AMPLITUDE:
+      y = -99; 
+      for (int i=0;i<len;i++) {
+        if (fabs(crealf(input[i])) > y) {
+          y = fabs(crealf(input[i]));
+        }
+        if (fabs(cimagf(input[i])) > y) {
+          y = fabs(cimagf(input[i]));
+        }
+      }
+      break;
+    default: 
+      fprintf(stderr, "Unsupported AGC mode\n");
+      return; 
+  }
   
+  float gg = 1.0; 
   if (q->isfirst) {
     q->y_out = y; 
-    q->gain = 1/y;
+    q->gain = q->target/y;
     q->isfirst = false; 
   } else {
     q->y_out = (1-q->bandwidth) * q->y_out + q->bandwidth * y;
     if (!q->lock) {
-      q->gain *= expf(-0.5*q->bandwidth*logf(q->y_out));
+      gg = expf(-0.5*q->bandwidth*logf(q->y_out/q->target));
+      q->gain *= gg; 
     }    
   }
-  DEBUG("AGC gain: %.3f y_out=%.3f, y=%.3f\n", q->gain, q->y_out, y);
+  INFO("AGC gain: %.3f (%.2f - %.2f) y_out=%.3f, y=%.3f target=%.1f gg=%.2f\n", q->gain, 10*log10(q->gain), gain_res, q->y_out, y, q->target, gg);
 }
diff --git a/srslte/lib/ue/src/ue_sync.c b/srslte/lib/ue/src/ue_sync.c
index 955106254..e33b5bbdf 100644
--- a/srslte/lib/ue/src/ue_sync.c
+++ b/srslte/lib/ue/src/ue_sync.c
@@ -57,7 +57,7 @@ int srslte_ue_sync_init_file(srslte_ue_sync_t *q, uint32_t nof_prb, char *file_n
     bzero(q, sizeof(srslte_ue_sync_t));
     q->file_mode = true; 
     q->sf_len = SRSLTE_SF_LEN(srslte_symbol_sz(nof_prb));
-
+    
     if (srslte_filesource_init(&q->file_source, file_name, SRSLTE_COMPLEX_FLOAT_BIN)) {
       fprintf(stderr, "Error opening file %s\n", file_name);
       goto clean_exit; 
@@ -80,6 +80,12 @@ clean_exit:
   return ret; 
 }
 
+int srslte_ue_sync_start_agc(srslte_ue_sync_t *q, double (set_gain_callback)(void*, double)) {
+  int n = srslte_agc_init_uhd(&q->agc, SRSLTE_AGC_MODE_PEAK_AMPLITUDE, set_gain_callback, q->stream); 
+  q->do_agc = n==0?true:false;
+  return n; 
+}
+
 int srslte_ue_sync_init(srslte_ue_sync_t *q, 
                  srslte_cell_t cell,
                  int (recv_callback)(void*, void*, uint32_t,srslte_timestamp_t*),
@@ -193,6 +199,9 @@ void srslte_ue_sync_free(srslte_ue_sync_t *q) {
   if (q->input_buffer) {
     free(q->input_buffer);
   }
+  if (q->do_agc) {
+    srslte_agc_free(&q->agc);
+  }
   if (!q->file_mode) {
     srslte_sync_free(&q->sfind);
     srslte_sync_free(&q->strack);    
@@ -389,7 +398,9 @@ int srslte_ue_sync_get_buffer(srslte_ue_sync_t *q, cf_t **sf_symbols) {
         fprintf(stderr, "Error receiving samples\n");
         return SRSLTE_ERROR;
       }
-      
+      if (q->do_agc) {
+        srslte_agc_process(&q->agc, q->input_buffer, q->input_buffer, q->sf_len);        
+      }
       switch (q->state) {
         case SF_FIND:        
           ret = srslte_sync_find(&q->sfind, q->input_buffer, 0, &q->peak_idx);