diff --git a/cuhd/include/srslte/cuhd/cuhd.h b/cuhd/include/srslte/cuhd/cuhd.h index 87d6858e3..d445956be 100644 --- a/cuhd/include/srslte/cuhd/cuhd.h +++ b/cuhd/include/srslte/cuhd/cuhd.h @@ -39,6 +39,9 @@ extern "C" { SRSLTE_API int cuhd_open(char *args, void **handler); +SRSLTE_API int cuhd_open_th(char *args, + void **handler); + SRSLTE_API int cuhd_close(void *h); SRSLTE_API int cuhd_start_rx_stream(void *h); @@ -58,6 +61,9 @@ SRSLTE_API double cuhd_set_rx_srate(void *h, SRSLTE_API double cuhd_set_rx_gain(void *h, double gain); +SRSLTE_API double cuhd_set_rx_gain_th(void *h, + double gain); + SRSLTE_API double cuhd_get_rx_gain(void *h); SRSLTE_API double cuhd_set_rx_freq(void *h, diff --git a/cuhd/lib/cuhd_handler.hpp b/cuhd/lib/cuhd_handler.hpp index 12e575b25..406f9de39 100644 --- a/cuhd/lib/cuhd_handler.hpp +++ b/cuhd/lib/cuhd_handler.hpp @@ -27,6 +27,7 @@ #include +#include class cuhd_handler { public: @@ -34,5 +35,11 @@ public: uhd::rx_streamer::sptr rx_stream; bool rx_stream_enable; uhd::tx_streamer::sptr tx_stream; - + + // The following variables are for threaded RX gain control + pthread_t thread_gain; + pthread_cond_t cond; + pthread_mutex_t mutex; + double cur_rx_gain; + double new_rx_gain; }; diff --git a/cuhd/lib/cuhd_imp.cpp b/cuhd/lib/cuhd_imp.cpp index 3b1da83c3..434b6e829 100644 --- a/cuhd/lib/cuhd_imp.cpp +++ b/cuhd/lib/cuhd_imp.cpp @@ -117,7 +117,36 @@ int cuhd_start_rx_stream_nsamples(void *h, uint32_t nsamples) return 0; } -int cuhd_open(char *args, void **h) +double cuhd_set_rx_gain_th(void *h, double gain) +{ + /* + cuhd_handler *handler = static_cast < cuhd_handler * >(h); + // round to avoid histeresis + gain = roundf(gain); + pthread_mutex_lock(&handler->mutex); + handler->new_rx_gain = gain; + pthread_cond_signal(&handler->cond); + pthread_mutex_unlock(&handler->mutex); + */ + return gain; +} + +/* This thread listens for set_rx_gain commands to the USRP */ +static void* thread_gain_fcn(void *h) { + cuhd_handler *handler = static_cast < cuhd_handler * >(h); + while(1) { + pthread_mutex_lock(&handler->mutex); + while(handler->cur_rx_gain == handler->new_rx_gain) { + pthread_cond_wait(&handler->cond, &handler->mutex); + } + handler->cur_rx_gain = handler->new_rx_gain; + pthread_mutex_unlock(&handler->mutex); + cuhd_set_rx_gain(h, handler->cur_rx_gain); + printf("set gain to %f\n", handler->cur_rx_gain); + } +} + +int cuhd_open_(char *args, void **h, bool create_thread_gain) { cuhd_handler *handler = new cuhd_handler(); std::string _args = std::string(args); @@ -139,9 +168,32 @@ int cuhd_open(char *args, void **h) *h = handler; + if (create_thread_gain) { + if (pthread_mutex_init(&handler->mutex, NULL)) { + return -1; + } + if (pthread_cond_init(&handler->cond, NULL)) { + return -1; + } + + if (pthread_create(&handler->thread_gain, NULL, thread_gain_fcn, *h)) { + perror("pthread_create"); + return -1; + } + } + return 0; } +int cuhd_open(char *args, void **h) { + return cuhd_open_(args, h, false); +} + +int cuhd_open_th(char *args, void **h) { + return cuhd_open_(args, h, true); +} + + int cuhd_close(void *h) { cuhd_stop_rx_stream(h); diff --git a/srslte/examples/cell_search.c b/srslte/examples/cell_search.c index b457533c6..ce423d951 100644 --- a/srslte/examples/cell_search.c +++ b/srslte/examples/cell_search.c @@ -55,7 +55,7 @@ int band = -1; int earfcn_start=-1, earfcn_end = -1; -cell_search_cfg_t config = {100, 10, 16}; +cell_search_cfg_t config = {100, 10, 16, true}; float uhd_gain = 60.0; @@ -167,6 +167,9 @@ int main(int argc, char **argv) { if (config.threshold) { srslte_ue_cellsearch_set_threshold(&cs, config.threshold); } + if (config.do_agc) { + 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/1000); cuhd_set_rx_srate(uhd, SRSLTE_CS_SAMP_FREQ); @@ -183,7 +186,7 @@ int main(int argc, char **argv) { srslte_cell_t cell; cell.id = found_cells[i].cell_id; cell.cp = found_cells[i].cp; - int ret = cuhd_mib_decoder(uhd, 100, &cell); + int ret = cuhd_mib_decoder(uhd, &config, &cell); if (ret < 0) { fprintf(stderr, "Error decoding MIB\n"); exit(-1); diff --git a/srslte/examples/cuhd_utils.c b/srslte/examples/cuhd_utils.c index 41e7baec6..4f7fadaaf 100644 --- a/srslte/examples/cuhd_utils.c +++ b/srslte/examples/cuhd_utils.c @@ -50,7 +50,7 @@ int cuhd_recv_wrapper_cs(void *h, void *data, uint32_t nsamples, srslte_timestam /** This function is simply a wrapper to the ue_cell_search module for cuhd devices * Return 1 if the MIB is decoded, 0 if not or -1 on error. */ -int cuhd_mib_decoder(void *uhd, uint32_t max_nof_frames, srslte_cell_t *cell) { +int cuhd_mib_decoder(void *uhd, cell_search_cfg_t *config, srslte_cell_t *cell) { int ret = SRSLTE_ERROR; srslte_ue_mib_sync_t ue_mib; uint8_t bch_payload[SRSLTE_BCH_PAYLOAD_LEN]; @@ -60,7 +60,9 @@ 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); + if (config->do_agc) { + srslte_ue_sync_start_agc(&ue_mib.ue_sync, cuhd_set_rx_gain_th); + } int srate = srslte_sampling_freq_hz(SRSLTE_UE_MIB_NOF_PRB); INFO("Setting sampling frequency %.2f MHz for PSS search\n", (float) srate/1000000); @@ -70,7 +72,7 @@ int cuhd_mib_decoder(void *uhd, uint32_t max_nof_frames, srslte_cell_t *cell) { cuhd_start_rx_stream(uhd); /* Find and decody MIB */ - ret = srslte_ue_mib_sync_decode(&ue_mib, max_nof_frames, bch_payload, &cell->nof_ports, NULL); + ret = srslte_ue_mib_sync_decode(&ue_mib, config->max_frames_pss, bch_payload, &cell->nof_ports, NULL); if (ret < 0) { fprintf(stderr, "Error decoding MIB\n"); goto clean_exit; @@ -109,9 +111,11 @@ 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); + if (config->do_agc) { + srslte_ue_sync_start_agc(&cs.ue_sync, cuhd_set_rx_gain_th); + } + INFO("Setting sampling frequency %.2f MHz for PSS search\n", SRSLTE_CS_SAMP_FREQ/1000000); cuhd_set_rx_srate(uhd, SRSLTE_CS_SAMP_FREQ); @@ -160,7 +164,7 @@ int cuhd_search_and_decode_mib(void *uhd, cell_search_cfg_t *config, int force_N ret = cuhd_cell_search(uhd, config, force_N_id_2, cell); if (ret > 0) { printf("Decoding PBCH for cell %d (N_id_2=%d)\n", cell->id, cell->id%3); - ret = cuhd_mib_decoder(uhd, config->max_frames_pbch, cell); + ret = cuhd_mib_decoder(uhd, config, cell); if (ret < 0) { fprintf(stderr, "Could not decode PBCH from CELL ID %d\n", cell->id); return SRSLTE_ERROR; diff --git a/srslte/examples/cuhd_utils.h b/srslte/examples/cuhd_utils.h index 4360b7f16..12afdcc47 100644 --- a/srslte/examples/cuhd_utils.h +++ b/srslte/examples/cuhd_utils.h @@ -32,10 +32,11 @@ typedef struct SRSLTE_API { uint32_t max_frames_pbch; // maximum number of 5ms frames to capture for MIB decoding uint32_t max_frames_pss; // maximum number of 5ms frames to capture for PSS correlation float threshold; // early-stops cell detection if mean PSR is above this value + bool do_agc; }cell_search_cfg_t; int cuhd_mib_decoder(void *uhd, - uint32_t max_nof_frames, + cell_search_cfg_t *config, srslte_cell_t *cell); int cuhd_cell_search(void *uhd, diff --git a/srslte/examples/pdsch_ue.c b/srslte/examples/pdsch_ue.c index caba04b44..1238be028 100644 --- a/srslte/examples/pdsch_ue.c +++ b/srslte/examples/pdsch_ue.c @@ -103,7 +103,7 @@ void args_default(prog_args_t *args) { args->uhd_args = ""; args->uhd_freq = -1.0; args->uhd_freq_offset = 8000000.0; - args->uhd_gain = 60.0; + args->uhd_gain = -1.0; args->net_port = -1; args->net_address = "127.0.0.1"; args->net_port_signal = -1; @@ -114,7 +114,7 @@ void usage(prog_args_t *args, char *prog) { printf("Usage: %s [agpPcildnruv] -f rx_frequency (in Hz) | -i input_file\n", prog); #ifndef DISABLE_UHD printf("\t-a UHD args [Default %s]\n", args->uhd_args); - printf("\t-g UHD RX gain [Default %.2f dB]\n", args->uhd_gain); + printf("\t-g UHD fix RX gain [Default AGC]\n"); printf("\t-o UHD RX freq offset [Default %.1f MHz]\n", args->uhd_freq_offset/1000000); #else printf("\t UHD is disabled. CUHD library not available\n"); @@ -270,12 +270,19 @@ int main(int argc, char **argv) { #ifndef DISABLE_UHD if (!prog_args.input_file_name) { printf("Opening UHD device...\n"); - if (cuhd_open(prog_args.uhd_args, &uhd)) { + if (cuhd_open_th(prog_args.uhd_args, &uhd)) { fprintf(stderr, "Error opening uhd\n"); exit(-1); } + + cuhd_set_rx_gain(uhd, 50); + /* Set receiver gain */ - cuhd_set_rx_gain(uhd, prog_args.uhd_gain); + if (prog_args.uhd_gain > 0) { + cuhd_set_rx_gain(uhd, prog_args.uhd_gain); + } else { + cell_detect_config.do_agc = true; + } /* set receiver frequency */ cuhd_set_rx_freq_offset(uhd, (double) prog_args.uhd_freq, prog_args.uhd_freq_offset); @@ -369,7 +376,9 @@ int main(int argc, char **argv) { bool decode_pdsch; int pdcch_tx=0; - srslte_ue_sync_start_agc(&ue_sync, cuhd_set_rx_gain); + if (prog_args.uhd_gain < 0) { + srslte_ue_sync_start_agc(&ue_sync, cuhd_set_rx_gain_th); + } INFO("\nEntering main loop...\n\n", 0); /* Main loop */ @@ -461,16 +470,7 @@ int main(int argc, char **argv) { pdcch_tx++; } - float max=-999; - for (int i=0;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) { #ifdef STDOUT_COMPACT @@ -480,14 +480,13 @@ int main(int argc, char **argv) { #else printf("CFO: %+6.2f KHz, SFO: %+6.2f Khz, " "RSRP: %+5.1f dBm, SNR: %4.1f dB, " - "PDCCH-Miss: %5.2f%% (%u), PDSCH-BLER: %5.2f%% Peak: %.2f Gain: %.1f dB/%.1f\r", + "PDCCH-Miss: %5.2f%% (%u), PDSCH-BLER: %5.2f%% Peak: %.2f Gain: %.1f dB\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(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, max, cuhd_get_rx_gain(uhd), - 10*log10(ue_sync.agc.gain) - ); + (float) 100*ue_dl.pkt_errors/ue_dl.pkts_total, + srslte_agc_get_output_level(&ue_sync.agc), 10*log10(srslte_agc_get_gain(&ue_sync.agc))); #endif } diff --git a/srslte/include/srslte/agc/agc.h b/srslte/include/srslte/agc/agc.h index 4e770aed7..e0b51a687 100644 --- a/srslte/include/srslte/agc/agc.h +++ b/srslte/include/srslte/agc/agc.h @@ -43,8 +43,8 @@ #include "srslte/config.h" -#define SRSLTE_AGC_DEFAULT_TARGET 0.4 -#define SRSLTE_AGC_DEFAULT_BW (5e-2) +#define SRSLTE_AGC_DEFAULT_TARGET 0.5 +#define SRSLTE_AGC_DEFAULT_BW (2e-1) typedef enum SRSLTE_API { SRSLTE_AGC_MODE_ENERGY = 0, @@ -61,12 +61,18 @@ typedef struct SRSLTE_API{ double (*set_gain_callback) (void*,double); srslte_agc_mode_t mode; float target; + uint32_t nof_frames; + uint32_t frame_cnt; + float *y_tmp; } 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, srslte_agc_mode_t mode, + uint32_t nof_frames, double (set_gain_callback)(void*, double), void *uhd_handler); @@ -90,8 +96,7 @@ SRSLTE_API void srslte_agc_lock(srslte_agc_t *q, bool enable); SRSLTE_API void srslte_agc_process(srslte_agc_t *q, - cf_t *input, - cf_t *output, + cf_t *signal, uint32_t len); #endif // AGC_ diff --git a/srslte/lib/agc/src/agc.c b/srslte/lib/agc/src/agc.c index d8419ffc6..08f324b4c 100644 --- a/srslte/lib/agc/src/agc.c +++ b/srslte/lib/agc/src/agc.c @@ -38,20 +38,31 @@ #include "srslte/utils/vector.h" #include "srslte/utils/debug.h" - int srslte_agc_init (srslte_agc_t *q, srslte_agc_mode_t mode) { + return srslte_agc_init_acc(q, mode, 1); +} + +int srslte_agc_init_acc(srslte_agc_t *q, srslte_agc_mode_t mode, uint32_t nof_frames) { bzero(q, sizeof(srslte_agc_t)); q->mode = mode; + q->nof_frames = nof_frames; + if (nof_frames > 0) { + q->y_tmp = srslte_vec_malloc(sizeof(float) * nof_frames); + if (!q->y_tmp) { + return SRSLTE_ERROR; + } + } else { + q->y_tmp = NULL; + } 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)) { +int srslte_agc_init_uhd(srslte_agc_t *q, srslte_agc_mode_t mode, uint32_t nof_frames, double (set_gain_callback)(void*, double), void *uhd_handler) { + if (!srslte_agc_init_acc(q, mode, nof_frames)) { 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; @@ -59,15 +70,21 @@ int srslte_agc_init_uhd(srslte_agc_t *q, srslte_agc_mode_t mode, double (set_gai } void srslte_agc_free(srslte_agc_t *q) { + if (q->y_tmp) { + free(q->y_tmp); + } bzero(q, sizeof(srslte_agc_t)); } void srslte_agc_reset(srslte_agc_t *q) { q->bandwidth = SRSLTE_AGC_DEFAULT_BW; q->lock = false; - q->gain = 1.0; + q->gain = pow(10,50/10); q->y_out = 1.0; q->isfirst = true; + if (q->set_gain_callback && q->uhd_handler) { + q->set_gain_callback(q->uhd_handler, 10*log10(q->gain)); + } } void srslte_agc_set_bandwidth(srslte_agc_t *q, float bandwidth) { @@ -79,7 +96,7 @@ void srslte_agc_set_target(srslte_agc_t *q, float target) { } float srslte_agc_get_rssi(srslte_agc_t *q) { - return 1.0/q->gain; + return q->target/q->gain; } float srslte_agc_get_output_level(srslte_agc_t *q) { @@ -94,56 +111,69 @@ 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; +void srslte_agc_process(srslte_agc_t *q, cf_t *signal, uint32_t len) { + float gain_db = 10*log10(q->gain); + float gain_uhd_db = 1.0; + //float gain_uhd = 1.0; + float y = 0; // Apply current gain to input signal if (!q->uhd_handler) { - srslte_vec_sc_prod_cfc(input, q->gain, output, len); + srslte_vec_sc_prod_cfc(signal, q->gain, signal, 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; + if (isinf(gain_db) || isnan(gain_db)) { + q->gain = 10.0; + } else { + gain_uhd_db = q->set_gain_callback(q->uhd_handler, gain_db); + //gain_uhd = pow(10, gain_uhd_db/10); } } - float y = 0; + float *t; switch(q->mode) { case SRSLTE_AGC_MODE_ENERGY: - y = sqrtf(crealf(srslte_vec_dot_prod_conj_ccc(output, output, len))/len); + y = sqrtf(crealf(srslte_vec_dot_prod_conj_ccc(signal, signal, len))/len); break; case SRSLTE_AGC_MODE_PEAK_AMPLITUDE: - y = -99; - for (int i=0;i y) { - y = fabs(crealf(input[i])); - } - if (fabs(cimagf(input[i])) > y) { - y = fabs(cimagf(input[i])); - } - } + t = (float*) signal; + y = t[srslte_vec_max_fi(t, 2*len)];// take only positive max to avoid abs() (should be similar) break; default: fprintf(stderr, "Unsupported AGC mode\n"); return; } - float gg = 1.0; + if (q->nof_frames > 0) { + q->y_tmp[q->frame_cnt++] = y; + if (q->frame_cnt == q->nof_frames) { + q->frame_cnt = 0; + switch(q->mode) { + case SRSLTE_AGC_MODE_ENERGY: + y = srslte_vec_acc_ff(q->y_tmp, q->nof_frames)/q->nof_frames; + break; + case SRSLTE_AGC_MODE_PEAK_AMPLITUDE: + y = q->y_tmp[srslte_vec_max_fi(q->y_tmp, q->nof_frames)]; + break; + default: + fprintf(stderr, "Unsupported AGC mode\n"); + return; + } + } + } + + double gg = 1.0; if (q->isfirst) { q->y_out = 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) { - gg = expf(-0.5*q->bandwidth*logf(q->y_out/q->target)); - q->gain *= gg; - } + if (q->frame_cnt == 0) { + q->y_out = (1-q->bandwidth) * q->y_out + q->bandwidth * y; + if (!q->lock) { + gg = expf(-0.5*q->bandwidth*logf(q->y_out/q->target)); + q->gain *= gg; + } + INFO("AGC gain: %.2f (%.2f) y_out=%.3f, y=%.3f target=%.1f gg=%.2f %d/%d\n", gain_db, gain_uhd_db, q->y_out, y, q->target, gg, q->frame_cnt, q->nof_frames); + } } - 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 fbe5c73b4..97e84a82d 100644 --- a/srslte/lib/ue/src/ue_sync.c +++ b/srslte/lib/ue/src/ue_sync.c @@ -81,11 +81,9 @@ clean_exit: } 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; - set_gain_callback(q->stream, 40); - return 0; + int n = srslte_agc_init_uhd(&q->agc, SRSLTE_AGC_MODE_PEAK_AMPLITUDE, 10, set_gain_callback, q->stream); + q->do_agc = n==0?true:false; + return n; } int srslte_ue_sync_init(srslte_ue_sync_t *q, @@ -400,9 +398,6 @@ 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); @@ -410,7 +405,10 @@ int srslte_ue_sync_get_buffer(srslte_ue_sync_t *q, cf_t **sf_symbols) { fprintf(stderr, "Error finding correlation peak (%d)\n", ret); return SRSLTE_ERROR; } - + if (q->do_agc) { + srslte_agc_process(&q->agc, q->input_buffer, q->sf_len); + } + if (ret == 1) { ret = find_peak_ok(q); } @@ -425,6 +423,10 @@ int srslte_ue_sync_get_buffer(srslte_ue_sync_t *q, cf_t **sf_symbols) { /* Every SF idx 0 and 5, find peak around known position q->peak_idx */ if (q->sf_idx == 0 || q->sf_idx == 5) { + if (q->do_agc) { + srslte_agc_process(&q->agc, q->input_buffer, q->sf_len); + } + #ifdef MEASURE_EXEC_TIME struct timeval t[3]; gettimeofday(&t[1], NULL);