SRSLTE: added holding time before AGC starts measuring after setting gain

lib/include/srslte/phy/agc/agc.h

@@ -38,19 +38,41 @@
 #include "srslte/config.h"
 
 #define SRSLTE_AGC_CALLBACK(NAME) void(NAME)(void* h, float gain_db)
-#define SRSLTE_AGC_DEFAULT_TARGET 0.3
+#define SRSLTE_AGC_DEFAULT_TARGET (0.3f)  /* Average RMS target or maximum peak target*/
-#define SRSLTE_AGC_DEFAULT_BW 0.7
+#define SRSLTE_AGC_DEFAULT_BW (0.3f)      /* Moving average coefficient */
+#define SRSLTE_AGC_HOLD_COUNT (20)        /* Number of frames to wait after setting the gain before start measuring */
+#define SRSLTE_AGC_MIN_MEASUREMENTS (10)  /* Minimum number of measurements  */
+#define SRSLTE_AGC_MIN_GAIN_OFFSET (2.0f) /* Mimum of gain offset to set the radio gain */
 
 typedef enum SRSLTE_API { SRSLTE_AGC_MODE_ENERGY = 0, SRSLTE_AGC_MODE_PEAK_AMPLITUDE } srslte_agc_mode_t;
 
+/*
+ * The AGC has been implemented using 3 states:
+ *  - init: it simply starts the process of measuring
+ *  - measure: performs a minimum of SRSLTE_AGC_MIN_MEASUREMENTS and does not set the gain until it needs
+ *             SRSLTE_AGC_MIN_GAIN_OFFSET dB more of gain. The gain is set in the enter hold transition.
+ *  - hold: waits for SRSLTE_AGC_HOLD_COUNT frames as a Rx gain transition period. After this period, it enters measure
+ *          state.
+ *
+ * FSM abstraction:
+ *
+ * +------+   Enter measure  +---------+   Enter hold   +------+
+ * | init | ---------------->| Measure |--------------->| Hold |
+ * +------+                  +---------+                +------+
+ *                                ^      Enter measure      |
+ *                                +-------------------------+
+ */
+
+typedef enum { SRSLTE_AGC_STATE_INIT = 0, SRSLTE_AGC_STATE_MEASURE, SRSLTE_AGC_STATE_HOLD } srslte_agc_state_t;
+
 typedef struct SRSLTE_API {
   float bandwidth;
-  float gain;
+  float gain_db;
+  float gain_offset_db;
   float min_gain_db;
   float max_gain_db;
   float default_gain_db;
   float y_out;
-  bool  lock;
   bool  isfirst;
   void* uhd_handler;
   SRSLTE_AGC_CALLBACK(*set_gain_callback);
@@ -58,11 +80,11 @@ typedef struct SRSLTE_API {
   float              target;
   uint32_t           nof_frames;
   uint32_t           frame_cnt;
+  uint32_t           hold_cnt;
   float*             y_tmp;
+  srslte_agc_state_t state;
 } srslte_agc_t;
 
-SRSLTE_API int srslte_agc_init(srslte_agc_t* q, srslte_agc_mode_t mode);
-
 SRSLTE_API int srslte_agc_init_acc(srslte_agc_t* q, srslte_agc_mode_t mode, uint32_t nof_frames);
 
 SRSLTE_API int srslte_agc_init_uhd(srslte_agc_t*     q,
@@ -77,20 +99,10 @@ SRSLTE_API void srslte_agc_reset(srslte_agc_t* q);
 
 SRSLTE_API void srslte_agc_set_gain_range(srslte_agc_t* q, float min_gain_db, float max_gain_db);
 
-SRSLTE_API void srslte_agc_set_bandwidth(srslte_agc_t* q, 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);
-
-SRSLTE_API float srslte_agc_get_output_level(srslte_agc_t* q);
-
 SRSLTE_API float srslte_agc_get_gain(srslte_agc_t* q);
 
 SRSLTE_API void srslte_agc_set_gain(srslte_agc_t* q, float init_gain_value_db);
 
-SRSLTE_API void srslte_agc_lock(srslte_agc_t* q, bool enable);
-
 SRSLTE_API void srslte_agc_process(srslte_agc_t* q, cf_t* signal, uint32_t len);
 
 #endif // SRSLTE_AGC_H

lib/src/phy/agc/agc.c

@@ -20,22 +20,11 @@
  */
 
 #include <complex.h>
-#include <math.h>
-#include <stdlib.h>
-#include <string.h>
-#include <strings.h>
-
-#include "srslte/phy/utils/debug.h"
 
 #include "srslte/phy/agc/agc.h"
 #include "srslte/phy/utils/debug.h"
 #include "srslte/phy/utils/vector.h"
 
-int srslte_agc_init(srslte_agc_t* q, srslte_agc_mode_t mode)
-{
-  return srslte_agc_init_acc(q, mode, 0);
-}
-
 int srslte_agc_init_acc(srslte_agc_t* q, srslte_agc_mode_t mode, uint32_t nof_frames)
 {
   bzero(q, sizeof(srslte_agc_t));
@@ -44,7 +33,7 @@ int srslte_agc_init_acc(srslte_agc_t* q, srslte_agc_mode_t mode, uint32_t nof_fr
   q->max_gain_db = 90.0;
   q->min_gain_db = 0.0;
   if (nof_frames > 0) {
-    q->y_tmp = srslte_vec_malloc(sizeof(float) * nof_frames);
+    q->y_tmp = srslte_vec_f_malloc(nof_frames);
     if (!q->y_tmp) {
       return SRSLTE_ERROR;
     }
@@ -81,11 +70,9 @@ void srslte_agc_free(srslte_agc_t* q)
 
 void srslte_agc_reset(srslte_agc_t* q)
 {
+  q->state     = SRSLTE_AGC_STATE_INIT;
   q->bandwidth = SRSLTE_AGC_DEFAULT_BW;
-  q->lock      = false;
+  q->gain_db   = q->default_gain_db;
-  q->gain      = srslte_convert_dB_to_power(q->default_gain_db);
-  q->y_out     = 1.0;
-  q->isfirst   = true;
   if (q->set_gain_callback && q->uhd_handler) {
     q->set_gain_callback(q->uhd_handler, q->default_gain_db);
   }
@@ -100,51 +87,23 @@ void srslte_agc_set_gain_range(srslte_agc_t* q, float min_gain_db, float max_gai
   }
 }
 
-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 q->target / q->gain;
-}
-
-float srslte_agc_get_output_level(srslte_agc_t* q)
-{
-  return q->y_out;
-}
-
 float srslte_agc_get_gain(srslte_agc_t* q)
 {
-  return q->gain;
+  return q->gain_db;
 }
 
 void srslte_agc_set_gain(srslte_agc_t* q, float init_gain_value_db)
 {
-  q->gain = srslte_convert_dB_to_power(init_gain_value_db);
+  q->gain_db = init_gain_value_db;
-}
-
-void srslte_agc_lock(srslte_agc_t* q, bool enable)
-{
-  q->lock = enable;
 }
 
-void srslte_agc_process(srslte_agc_t* q, cf_t* signal, uint32_t len)
+/*
+ * Transition functions
+ */
+static inline void agc_enter_state_hold(srslte_agc_t* q)
 {
-  if (!q->lock) {
+  // Bound gain in dB
-    float gain_db     = srslte_convert_power_to_dB(q->gain);
+  float gain_db = q->gain_db + q->gain_offset_db;
-
-    float y = 0;
-    // Apply current gain to input signal
-    if (!q->uhd_handler) {
-      srslte_vec_sc_prod_cfc(signal, q->gain, signal, len);
-    } else {
   if (gain_db < q->min_gain_db) {
     gain_db = q->min_gain_db + 5.0;
     INFO("Warning: Rx signal strength is too high. Forcing minimum Rx gain %.2fdB\n", gain_db);
@@ -158,8 +117,33 @@ void srslte_agc_process(srslte_agc_t* q, cf_t* signal, uint32_t len)
 
   // Set gain
   q->set_gain_callback(q->uhd_handler, gain_db);
-      q->gain = srslte_convert_dB_to_power(gain_db);
+  q->gain_db = gain_db;
+
+  // Set holding period
+  q->hold_cnt = 0;
+  q->state    = SRSLTE_AGC_STATE_HOLD;
+}
+
+static inline void agc_enter_state_measure(srslte_agc_t* q)
+{
+  q->hold_cnt = 0;
+  q->isfirst  = true;
+  q->state    = SRSLTE_AGC_STATE_MEASURE;
+}
+
+/*
+ * Running state functions
+ */
+static inline void agc_run_state_init(srslte_agc_t* q)
+{
+  agc_enter_state_measure(q);
 }
+
+static inline void agc_run_state_measure(srslte_agc_t* q, cf_t* signal, uint32_t len)
+{
+
+  // Perform measurement of the frame
+  float  y = 0;
   float* t;
   switch (q->mode) {
     case SRSLTE_AGC_MODE_ENERGY:
@@ -174,6 +158,7 @@ void srslte_agc_process(srslte_agc_t* q, cf_t* signal, uint32_t len)
       return;
   }
 
+  // Perform averaging if configured
   if (q->nof_frames > 0) {
     q->y_tmp[q->frame_cnt++] = y;
     if (q->frame_cnt == q->nof_frames) {
@@ -192,17 +177,61 @@ void srslte_agc_process(srslte_agc_t* q, cf_t* signal, uint32_t len)
     }
   }
 
+  // Update gain
   if (q->isfirst) {
     q->y_out          = y;
     q->isfirst        = false;
+    q->gain_offset_db = 0.0f;
   } else {
     if (q->frame_cnt == 0) {
-        q->y_out = (1 - q->bandwidth) * q->y_out + q->bandwidth * y;
+      q->y_out          = SRSLTE_VEC_EMA(y, q->y_out, q->bandwidth);
-        if (!q->lock) {
+      q->gain_offset_db = srslte_convert_amplitude_to_dB(q->target) - srslte_convert_amplitude_to_dB(q->y_out);
-          q->gain *= q->target / q->y_out;
+      INFO("AGC gain offset: %.2f y_out=%.3f, y=%.3f target=%.1f\n", q->gain_offset_db, q->y_out, y, q->target);
     }
-        INFO("AGC gain: %.2f y_out=%.3f, y=%.3f target=%.1f\n", gain_db, q->y_out, y, q->target);
+  }
+
+  // Check minimum of frame measurements count
+  if (q->hold_cnt < SRSLTE_AGC_MIN_MEASUREMENTS) {
+    // Increment hold counter
+    q->hold_cnt++;
+  } else if (fabsf(q->gain_offset_db) > SRSLTE_AGC_MIN_GAIN_OFFSET) {
+    // Wait for a minimum of measurements and a minimum gain offset before setting gain
+    agc_enter_state_hold(q);
   }
 }
+
+static inline void agc_run_state_hold(srslte_agc_t* q)
+{
+  // Increment holding counter
+  q->hold_cnt++;
+
+  // Check holding counter
+  if (q->hold_cnt >= SRSLTE_AGC_HOLD_COUNT) {
+    // Enter state measure
+    agc_enter_state_measure(q);
+  }
+}
+
+/*
+ * AGC FSM entry function
+ */
+void srslte_agc_process(srslte_agc_t* q, cf_t* signal, uint32_t len)
+{
+  // Apply current gain to input signal
+  if (!q->uhd_handler) {
+    srslte_vec_sc_prod_cfc(signal, srslte_convert_dB_to_amplitude(q->gain_db), signal, len);
+  }
+
+  // Run FSM state
+  switch (q->state) {
+    case SRSLTE_AGC_STATE_HOLD:
+      agc_run_state_hold(q);
+      break;
+    case SRSLTE_AGC_STATE_MEASURE:
+      agc_run_state_measure(q, signal, len);
+      break;
+    case SRSLTE_AGC_STATE_INIT:
+    default:
+      agc_run_state_init(q);
   }
 }