From adf5260e29b12d21edcea50ee9d2cbe26842ecfc Mon Sep 17 00:00:00 2001 From: ismagom Date: Mon, 3 Mar 2014 23:54:02 +0000 Subject: [PATCH] CFO correction using LUT --- examples/mib_scan_usrp.c | 9 +- examples/mib_test.c | 10 +- examples/mib_track.c | 10 +- examples/synch_test.c | 8 +- lte/include/lte.h | 4 +- lte/include/lte/sync/cfo.h | 55 +++++++++ lte/include/lte/utils/{nco.h => cexptab.h} | 30 ++--- lte/lib/sync/src/cfo.c | 80 +++++++++++++ lte/lib/utils/src/cexptab.c | 80 +++++++++++++ lte/lib/utils/src/nco.c | 131 --------------------- 10 files changed, 262 insertions(+), 155 deletions(-) create mode 100644 lte/include/lte/sync/cfo.h rename lte/include/lte/utils/{nco.h => cexptab.h} (63%) create mode 100644 lte/lib/sync/src/cfo.c create mode 100644 lte/lib/utils/src/cexptab.c delete mode 100644 lte/lib/utils/src/nco.c diff --git a/examples/mib_scan_usrp.c b/examples/mib_scan_usrp.c index 3ee312193..16654ff53 100644 --- a/examples/mib_scan_usrp.c +++ b/examples/mib_scan_usrp.c @@ -65,6 +65,7 @@ pbch_t pbch; lte_fft_t fft; chest_t chest; sync_t sfind, strack; +cfo_t cfocorr; float *cfo_v; int *idx_v, *idx_valid, *t; @@ -182,6 +183,10 @@ int base_init(int frame_length) { fprintf(stderr, "Error initializing FFT\n"); return -1; } + if (cfo_init(&cfocorr, FLEN)) { + fprintf(stderr, "Error initiating CFO\n"); + return -1; + } idx_v = malloc(nof_frames_track * sizeof(int)); if (!idx_v) { @@ -229,10 +234,12 @@ void base_free() { #ifndef DISABLE_UHD cuhd_close(&uhd); #endif + sync_free(&sfind); sync_free(&strack); lte_fft_free(&fft); chest_free(&chest); + cfo_free(&cfocorr); free(input_buffer); free(fft_buffer); @@ -506,7 +513,7 @@ int main(int argc, char **argv) { // Correct CFO INFO("Correcting CFO=%.4f\n", cfo[freq]); - nco_cexp_f_direct(&input_buffer[FLEN], (-cfo[freq])/128, FLEN); + cfo_correct(&cfocorr, &input_buffer[FLEN], (-cfo[freq])/128); if (nslot == 0) { if (mib_decoder_run(&input_buffer[FLEN+find_idx], &mib)) { diff --git a/examples/mib_test.c b/examples/mib_test.c index dffce8274..8992952ce 100644 --- a/examples/mib_test.c +++ b/examples/mib_test.c @@ -49,6 +49,7 @@ pbch_t pbch; lte_fft_t fft; chest_t chest; sync_t synch; +cfo_t cfocorr; void usage(char *prog) { printf("Usage: %s [onlt] -i input_file\n", prog); @@ -122,6 +123,11 @@ int base_init() { } } + if (cfo_init(&cfocorr, FLEN)) { + fprintf(stderr, "Error initiating CFO\n"); + return -1; + } + if (chest_init(&chest, LINEAR, CPNORM, 6, NOF_PORTS)) { fprintf(stderr, "Error initializing equalizer\n"); return -1; @@ -265,8 +271,10 @@ int main(int argc, char **argv) { fprintf(stderr, "Error reading %d samples\n", FLEN); break; } + INFO("Correcting CFO=%.4f\n", cfo); - nco_cexp_f_direct(input_buffer, -cfo/128, FLEN); + cfo_correct(&cfocorr, input_buffer, -cfo/128); + switch(state) { case SYNC: INFO("State Sync, Slot idx=%d\n", frame_cnt); diff --git a/examples/mib_track.c b/examples/mib_track.c index d41c7df83..6273f7032 100644 --- a/examples/mib_track.c +++ b/examples/mib_track.c @@ -64,6 +64,7 @@ pbch_t pbch; lte_fft_t fft; chest_t chest; sync_t sfind, strack; +cfo_t cfocorr; plot_real_t poutfft; plot_complex_t pce; @@ -200,6 +201,11 @@ int base_init(int frame_length) { return -1; } + if (cfo_init(&cfocorr, FLEN)) { + fprintf(stderr, "Error initiating CFO\n"); + return -1; + } + if (lte_fft_init(&fft, CPNORM, 6)) { fprintf(stderr, "Error initializing FFT\n"); return -1; @@ -223,6 +229,7 @@ void base_free() { sync_free(&strack); lte_fft_free(&fft); chest_free(&chest); + cfo_free(&cfocorr); free(input_buffer); free(fft_buffer); @@ -385,7 +392,8 @@ int main(int argc, char **argv) { // Correct CFO INFO("Correcting CFO=%.4f\n", cfo); - nco_cexp_f_direct(input_buffer, (-cfo)/128, FLEN); + + cfo_correct(&cfocorr, input_buffer, -cfo/128); if (nslot == 0) { INFO("Finding MIB at idx %d\n", find_idx); diff --git a/examples/synch_test.c b/examples/synch_test.c index 216ab74a9..babf3cdd4 100644 --- a/examples/synch_test.c +++ b/examples/synch_test.c @@ -106,6 +106,7 @@ int main(int argc, char **argv) { filesink_t fsink; pss_synch_t pss[3]; // One for each N_id_2 sss_synch_t sss[3]; // One for each N_id_2 + cfo_t cfocorr; int peak_pos[3]; float *cfo; float peak_value[3]; @@ -155,6 +156,11 @@ int main(int argc, char **argv) { exit(-1); } + if (cfo_init(&cfocorr, frame_length)) { + fprintf(stderr, "Error initiating CFO\n"); + return -1; + } + /* We have 2 options here: * a) We create 3 pss objects, each initialized with a different N_id_2 * b) We create 1 pss object which scans for each N_id_2 one after another. @@ -192,7 +198,7 @@ int main(int argc, char **argv) { gettimeofday(&tdata[1], NULL); if (force_cfo != CFO_AUTO) { - nco_cexp_f_direct(input, -force_cfo/128, frame_length); + cfo_correct(&cfocorr, input, -force_cfo/128); } if (force_N_id_2 != -1) { diff --git a/lte/include/lte.h b/lte/include/lte.h index 7e8d2a644..520003cac 100644 --- a/lte/include/lte.h +++ b/lte/include/lte.h @@ -38,7 +38,7 @@ #include "lte/utils/dft.h" #include "lte/utils/matrix.h" #include "lte/utils/mux.h" -#include "lte/utils/nco.h" +#include "lte/utils/cexptab.h" #include "lte/utils/pack.h" #include "lte/utils/vector.h" @@ -83,6 +83,6 @@ #include "lte/sync/sfo.h" #include "lte/sync/sss.h" #include "lte/sync/sync.h" - +#include "lte/sync/cfo.h" #endif diff --git a/lte/include/lte/sync/cfo.h b/lte/include/lte/sync/cfo.h new file mode 100644 index 000000000..76bc9aafa --- /dev/null +++ b/lte/include/lte/sync/cfo.h @@ -0,0 +1,55 @@ +/** + * + * \section COPYRIGHT + * + * Copyright 2013-2014 The libLTE Developers. See the + * COPYRIGHT file at the top-level directory of this distribution. + * + * \section LICENSE + * + * This file is part of the libLTE library. + * + * libLTE is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, either version 3 of + * the License, or (at your option) any later version. + * + * libLTE is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * A copy of the GNU Lesser General Public License can be found in + * the LICENSE file in the top-level directory of this distribution + * and at http://www.gnu.org/licenses/. + * + */ + + +#ifndef _cfo_ +#define _cfo_ + +#include + +typedef _Complex float cf_t; + +/** If the frequency is changed more than the tolerance, a new table is generated */ +#define CFO_TOLERANCE 0.00001 + +#define CFO_CEXPTAB_SIZE 4096 + +typedef struct { + float last_freq; + float tol; + int nsamples; + cexptab_t tab; + cf_t *cur_cexp; +}cfo_t; + +int cfo_init(cfo_t *h, int nsamples); +void cfo_free(cfo_t *h); + +void cfo_set_tol(cfo_t *h, float tol); +void cfo_correct(cfo_t *h, cf_t *x, float freq); + +#endif diff --git a/lte/include/lte/utils/nco.h b/lte/include/lte/utils/cexptab.h similarity index 63% rename from lte/include/lte/utils/nco.h rename to lte/include/lte/utils/cexptab.h index 4fae2ec7a..2063eb0cd 100644 --- a/lte/include/lte/utils/nco.h +++ b/lte/include/lte/utils/cexptab.h @@ -26,28 +26,22 @@ */ -#ifndef NCO_ -#define NCO_ +#ifndef _cexptab_ +#define _cexptab_ #include +typedef _Complex float cf_t; + typedef struct { int size; - float *cost; - float *sint; -}nco_t; - -void nco_init(nco_t *nco, int size); -void nco_destroy(nco_t *nco); - -float nco_sin(nco_t *nco, float phase); -float nco_cos(nco_t *nco, float phase); -void nco_sincos(nco_t *nco, float phase, float *sin, float *cos); -_Complex float nco_cexp(nco_t *nco, float arg); - -void nco_sin_f(nco_t *nco, float *x, float freq, int len); -void nco_cos_f(nco_t *nco, float *x, float freq, int len); -void nco_cexp_f(nco_t *nco, _Complex float *x, float freq, int len); -void nco_cexp_f_direct(_Complex float *x, float freq, int len); + cf_t *tab; +}cexptab_t; + +int cexptab_init(cexptab_t *nco, int size); +void cexptab_free(cexptab_t *nco); + +void cexptab_gen(cexptab_t *nco, cf_t *x, float freq, int len); +void cexptab_gen_direct(cf_t *x, float freq, int len); #endif diff --git a/lte/lib/sync/src/cfo.c b/lte/lib/sync/src/cfo.c new file mode 100644 index 000000000..74f058151 --- /dev/null +++ b/lte/lib/sync/src/cfo.c @@ -0,0 +1,80 @@ +/** + * + * \section COPYRIGHT + * + * Copyright 2013-2014 The libLTE Developers. See the + * COPYRIGHT file at the top-level directory of this distribution. + * + * \section LICENSE + * + * This file is part of the libLTE library. + * + * libLTE is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, either version 3 of + * the License, or (at your option) any later version. + * + * libLTE is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * A copy of the GNU Lesser General Public License can be found in + * the LICENSE file in the top-level directory of this distribution + * and at http://www.gnu.org/licenses/. + * + */ + +#include +#include +#include + +#include "lte/utils/cexptab.h" +#include "lte/sync/cfo.h" +#include "lte/utils/vector.h" +#include "lte/utils/debug.h" + +int cfo_init(cfo_t *h, int nsamples) { + int ret = -1; + bzero(h, sizeof(cfo_t)); + + if (cexptab_init(&h->tab, CFO_CEXPTAB_SIZE)) { + goto clean; + } + h->cur_cexp = malloc(sizeof(cf_t) * nsamples); + if (!h->cur_cexp) { + goto clean; + } + h->tol = CFO_TOLERANCE; + h->last_freq = 0; + h->nsamples = nsamples; + cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, h->nsamples); + + ret = 0; +clean: + if (ret == -1) { + cfo_free(h); + } + return ret; +} + +void cfo_free(cfo_t *h) { + cexptab_free(&h->tab); + if (h->cur_cexp) { + free(h->cur_cexp); + } + bzero(h, sizeof(cf_t)); +} + +void cfo_set_tol(cfo_t *h, float tol) { + h->tol = tol; +} + +void cfo_correct(cfo_t *h, cf_t *x, float freq) { + if (fabs(h->last_freq - freq) > h->tol) { + h->last_freq = freq; + cexptab_gen(&h->tab, h->cur_cexp, h->last_freq, h->nsamples); + INFO("CFO generating new table for frequency %.4f\n", freq); + } + vec_prod_ccc(h->cur_cexp, x, x, h->nsamples); +} diff --git a/lte/lib/utils/src/cexptab.c b/lte/lib/utils/src/cexptab.c new file mode 100644 index 000000000..235b69389 --- /dev/null +++ b/lte/lib/utils/src/cexptab.c @@ -0,0 +1,80 @@ +/** + * + * \section COPYRIGHT + * + * Copyright 2013-2014 The libLTE Developers. See the + * COPYRIGHT file at the top-level directory of this distribution. + * + * \section LICENSE + * + * This file is part of the libLTE library. + * + * libLTE is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, either version 3 of + * the License, or (at your option) any later version. + * + * libLTE is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * A copy of the GNU Lesser General Public License can be found in + * the LICENSE file in the top-level directory of this distribution + * and at http://www.gnu.org/licenses/. + * + */ + +#include +#include +#include +#include +#include + +#include "lte/utils/cexptab.h" + +int cexptab_init(cexptab_t *h, int size) { + int i; + + h->size = size; + h->tab = malloc(sizeof(cf_t) * size); + if (h->tab) { + for (i = 0; i < size; i++) { + h->tab[i] = cexpf(_Complex_I * 2 * M_PI * (float) i / size); + } + return 0; + } else { + return -1; + } +} + +void cexptab_free(cexptab_t *h) { + if (h->tab) { + free(h->tab); + } + bzero(h, sizeof(cexptab_t)); +} + +void cexptab_gen(cexptab_t *h, cf_t *x, float freq, int len) { + int i; + unsigned int idx; + float phase_inc = freq * h->size; + float phase=0; + + for (i = 0; i < len; i++) { + idx = (unsigned int) phase; + x[i] = h->tab[idx]; + phase += phase_inc; + if (phase >= (float) h->size) { + phase -= (float) h->size; + } + } +} + +void cexptab_gen_direct(cf_t *x, float freq, int len) { + int i; + for (i = 0; i < len; i++) { + x[i] = cexpf(_Complex_I * 2 * M_PI * freq * i); + } +} + diff --git a/lte/lib/utils/src/nco.c b/lte/lib/utils/src/nco.c deleted file mode 100644 index 0d184715a..000000000 --- a/lte/lib/utils/src/nco.c +++ /dev/null @@ -1,131 +0,0 @@ -/** - * - * \section COPYRIGHT - * - * Copyright 2013-2014 The libLTE Developers. See the - * COPYRIGHT file at the top-level directory of this distribution. - * - * \section LICENSE - * - * This file is part of the libLTE library. - * - * libLTE is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as - * published by the Free Software Foundation, either version 3 of - * the License, or (at your option) any later version. - * - * libLTE is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Lesser General Public License for more details. - * - * A copy of the GNU Lesser General Public License can be found in - * the LICENSE file in the top-level directory of this distribution - * and at http://www.gnu.org/licenses/. - * - */ - - - - -#include -#include -#include -#include -#include - -#include "lte/utils/nco.h" - -void nco_init(nco_t *nco, int size) { - int i; - - nco->size=size; - nco->cost=malloc(size*sizeof(float)); - nco->sint=malloc(size*sizeof(float)); - assert(nco->cost && nco->sint); - - for (i=0;icost[i] = cosf(2*M_PI*i/size); - nco->sint[i] = sinf(2*M_PI*i/size); - } -} - -void nco_destroy(nco_t *nco) { - if (nco->cost) { - free(nco->cost); - } - if (nco->sint) { - free(nco->sint); - } - nco->size=0; - bzero(nco, sizeof(nco_t)); -} - -unsigned int nco_idx(float phase, int size) { - while(phase>=2*M_PI) { - phase-=2*M_PI; - } - unsigned int idx = (unsigned int) (phase*size/(2*M_PI)); - return idx; -} - -inline float nco_sin(nco_t *nco, float phase) { - return nco->sint[nco_idx(phase,nco->size)]; -} -inline float nco_cos(nco_t *nco, float phase) { - return nco->cost[nco_idx(phase,nco->size)]; -} -inline void nco_sincos(nco_t *nco, float phase, float *sin, float *cos) { - unsigned int idx = nco_idx(phase,nco->size); - *sin = nco->sint[idx]; - *cos = nco->cost[idx]; -} - -inline _Complex float nco_cexp(nco_t *nco, float arg) { - float s,c; - nco_sincos(nco,arg,&s,&c); - return c+I*s; -} - -void nco_sin_f(nco_t *nco, float *x, float freq, int len) { - int i; - unsigned int idx; - - idx=0; - for (i=0;isize/len))%nco->size; - x[i] = nco->sint[idx]; - } -} - - -void nco_cos_f(nco_t *nco, float *x, float freq, int len) { - int i; - unsigned int idx; - - idx=0; - for (i=0;isize/len))%nco->size; - x[i] = nco->cost[idx]; - } -} - - -void nco_cexp_f(nco_t *nco, _Complex float *x, float freq, int len) { - int i; - unsigned int idx; - - idx=0; - for (i=0;isize/len))%nco->size; - x[i] = nco->cost[idx] + I*nco->sint[idx]; - } -} - -void nco_cexp_f_direct(_Complex float *x, float freq, int len) { - int i; - for (i=0;i