1st version of CFO estimation

openair1/PHY/NR_UE_TRANSPORT/pss_nr.c

@@ -33,6 +33,7 @@
 #include <stdio.h>
 #include <assert.h>
 #include <errno.h>
+#include <math.h>
 
 #include "PHY/defs_nr_UE.h"
 
@@ -751,6 +752,15 @@ static inline int64_t abs64(int64_t x)
   return (((int64_t)((int32_t*)&x)[0])*((int64_t)((int32_t*)&x)[0]) + ((int64_t)((int32_t*)&x)[1])*((int64_t)((int32_t*)&x)[1]));
 }
 
+static inline double angle64(int64_t x)
+{
+
+  double re=((int32_t*)&x)[0];
+  double im=((int32_t*)&x)[1];
+  return (atan2(im,re));
+}
+
+
 /*******************************************************************
 *
 * NAME :         pss_search_time_nr
@@ -811,6 +821,7 @@ int pss_search_time_nr(int **rxdata, ///rx data in time domain
   int64_t peak_value;
   int64_t result;
   int64_t avg[NUMBER_PSS_SEQUENCE];
+  double ffo_est;
 
 
   unsigned int length = (NR_NUMBER_OF_SUBFRAMES_PER_FRAME*frame_parms->samples_per_subframe);  /* 1 frame for now, it should be 2 TODO_NR */
@@ -859,7 +870,6 @@ int pss_search_time_nr(int **rxdata, ///rx data in time domain
 				  frame_parms->ofdm_symbol_size, 
 				  shift);
 	  pss_corr_ue[pss_index][n] += abs64(result);
-	  
           //((short*)pss_corr_ue[pss_index])[2*n] += ((short*) &result)[0];   /* real part */
           //((short*)pss_corr_ue[pss_index])[2*n+1] += ((short*) &result)[1]; /* imaginary part */
           //((short*)&synchro_out)[0] += ((short*) &result)[0];               /* real part */
@@ -874,10 +884,11 @@ int pss_search_time_nr(int **rxdata, ///rx data in time domain
         peak_value = pss_corr_ue[pss_index][n];
         peak_position = n;
         pss_source = pss_index;
+	ffo_est = angle64(result)/M_PI;
 
-#ifdef DEBUG_PSS_NR
+//#ifdef DEBUG_PSS_NR
-        printf("pss_index %d: n %6d peak_value %15llu\n", pss_index, n, (unsigned long long)pss_corr_ue[pss_index][n]);
+        printf("pss_index %d: n %6d peak_value %15llu ffo %lf\n", pss_index, n, (unsigned long long)pss_corr_ue[pss_index][n],ffo_est);
-#endif
+//#endif
       }
     }
   }

openair1/PHY/TOOLS/cdot_prod.c

@@ -19,6 +19,7 @@
  *      contact@openairinterface.org
  */
 
+#include <stdio.h>
 #include "tools_defs.h"
 #include "PHY/sse_intrin.h"
 
@@ -169,7 +170,7 @@ int64_t dot_product64(int16_t *x,
 #if defined(__x86_64__) || defined(__i386__)
   __m128i *x128,*y128,mmtmp1,mmtmp2,mmtmp3,mmcumul,mmcumul_re,mmcumul_im;
   __m128i minus_i = _mm_set_epi16(-1,1,-1,1,-1,1,-1,1);
-  int32_t result;
+  int64_t result;
 
   x128 = (__m128i*) x;
   y128 = (__m128i*) y;
@@ -180,14 +181,13 @@ int64_t dot_product64(int16_t *x,
   for (n=0; n<(N>>2); n++) {
 
 //    printf("n=%d, x128=%p, y128=%p\n",n,x128,y128);
-  //      print_shorts("x",&x128[0]);
+       // print_shorts("x",&x128[0]);
-    //    print_shorts("y",&y128[0]);
+       // print_shorts("y",&y128[0]);
 
     // this computes Re(z) = Re(x)*Re(y) + Im(x)*Im(y)
     mmtmp1 = _mm_madd_epi16(x128[0],y128[0]);
       //  print_ints("retmp",&mmtmp1);
     // mmtmp1 contains real part of 4 consecutive outputs (32-bit)
-
     // shift and accumulate results
     mmtmp1 = _mm_srai_epi32(mmtmp1,output_shift);
     mmcumul_re = _mm_add_epi32(mmcumul_re,mmtmp1);
@@ -205,7 +205,6 @@ int64_t dot_product64(int16_t *x,
     mmtmp3 = _mm_madd_epi16(x128[0],mmtmp2);
             //print_ints("imtmp",&mmtmp3);
     // mmtmp3 contains imag part of 4 consecutive outputs (32-bit)
-
     // shift and accumulate results
     mmtmp3 = _mm_srai_epi32(mmtmp3,output_shift);
     mmcumul_im = _mm_add_epi32(mmcumul_im,mmtmp3);
@@ -218,13 +217,10 @@ int64_t dot_product64(int16_t *x,
   // this gives Re Re Im Im
   mmcumul = _mm_hadd_epi32(mmcumul_re,mmcumul_im);
     //print_ints("cumul1",&mmcumul);
-
   // this gives Re Im Re Im
   mmcumul = _mm_hadd_epi32(mmcumul,mmcumul);
-
     //print_ints("cumul2",&mmcumul);
 
-
   //mmcumul = _mm_srai_epi32(mmcumul,output_shift);
   // extract the lower half
   result = _mm_extract_epi64(mmcumul,0);

openair1/SIMULATION/NR_PHY/pbchsim.c

@@ -392,8 +392,9 @@ int main(int argc, char **argv)
 
   // cfo with respect to sub-carrier spacing
   eps = cfo/scs;
-  int IFO;
 
+  // computation of integer and fractional FO to compare with estimation results
+  int IFO;
   if(eps!=0.0){
 	printf("Introducing a CFO of %lf relative to SCS of %d kHz\n",eps,(int)(scs/1000));
 	if (eps>0)	
@@ -546,7 +547,7 @@ int main(int argc, char **argv)
            0,  // interference power
            frame_parms->nb_antennas_rx,  // number of rx antennas
            frame_length_complex_samples,  // number of samples in frame
-           1.0e7/frame_length_complex_samples,   //sampling time (ns)
+           1.0e9/fs,   //sampling time (ns)
            cfo,	// frequency offset in Hz
            0.0, // drift (not implemented)
            0.0, // noise figure (not implemented)
@@ -558,8 +559,6 @@ int main(int argc, char **argv)
            0.0,  // IQ imbalance (dB),
 	   0.0); // IQ phase imbalance (rad)
 
-
-
    
       for (i=0; i<frame_length_complex_samples; i++) {
 	for (aa=0; aa<frame_parms->nb_antennas_rx; aa++) {