From afa8cd8424aac67b3c0c925701693b8a2e09c3d0 Mon Sep 17 00:00:00 2001
From: ismagom <gomezi@tcd.ie>
Date: Thu, 30 Jan 2014 14:45:59 +0000
Subject: [PATCH] Cell search program working

---
 examples/cell_search.c        |   2 +-
 examples/equalizer_test.c     |  13 +-
 examples/pss_scan_usrp.c      | 327 ++++++++++++++++++++++------------
 include/resampling/interp.h   |   1 +
 include/sync/sfo.h            |   2 +-
 include/sync/sync.h           |   2 +-
 lib/ch_estimation/src/chest.c |   6 +-
 lib/resampling/src/interp.c   |  11 ++
 lib/sync/src/pss.c            |   8 +-
 lib/sync/src/sfo.c            |  16 ++
 lib/sync/src/sync.c           |   9 +-
 lib/utils/src/vector.c        |   6 +-
 matlab/common/read_complex.m  |  20 +++
 matlab/common/write_complex.m |  22 +++
 matlab/sync/check_pss.m       |  19 +-
 matlab/sync/find_pss.m        |  31 ++--
 matlab/sync/ifo_pss.m         |  13 ++
 matlab/sync/ifo_pss_corr.m    |  10 ++
 matlab/sync/sfo_estimate.m    |  19 +-
 uhd/uhd_imp.cpp               |   2 +-
 20 files changed, 375 insertions(+), 164 deletions(-)
 create mode 100644 matlab/common/read_complex.m
 create mode 100644 matlab/common/write_complex.m
 create mode 100644 matlab/sync/ifo_pss.m
 create mode 100644 matlab/sync/ifo_pss_corr.m

diff --git a/examples/cell_search.c b/examples/cell_search.c
index 0df21c6e9..e39ee7e58 100644
--- a/examples/cell_search.c
+++ b/examples/cell_search.c
@@ -176,7 +176,7 @@ int main(int argc, char **argv) {
 		exit(-1);
 	}
 
-	if (sync_init(&synch)) {
+	if (sync_init(&synch, 960)) {
 		fprintf(stderr, "Error initiating PSS/SSS\n");
 		exit(-1);
 	}
diff --git a/examples/equalizer_test.c b/examples/equalizer_test.c
index 2e626ef05..ea99d9b2d 100644
--- a/examples/equalizer_test.c
+++ b/examples/equalizer_test.c
@@ -85,6 +85,7 @@ int main(int argc, char **argv) {
 	cf_t *input = NULL;
 	cf_t *outfft = NULL;
 	cf_t *ce = NULL;
+	int i;
 
 	if (argc < 3) {
 		usage(argv[0]);
@@ -135,6 +136,7 @@ int main(int argc, char **argv) {
 	bzero(input, sizeof(cf_t) * in_slot_length());
 	bzero(outfft, sizeof(cf_t) * slot_length());
 
+	fprintf(f, "ce=zeros(%d, %d);\n", nof_slots * CP_NSYMB(cp), nof_prb * RE_X_RB);
 	/* read all file or nof_slots */
 	slot_cnt = 0;
 	while (in_slot_length() == filesource_read(&fsrc, input, in_slot_length())
@@ -144,11 +146,12 @@ int main(int argc, char **argv) {
 
 		chest_ce_slot_port(&eq, outfft, ce, slot_cnt%20, 0);
 
-		chest_fprint(&eq, f, slot_cnt%20, 0);
+		//chest_fprint(&eq, f, slot_cnt%20, 0);
 
-		fprintf(f, "ce=[");
-		vec_fprint_c(f, ce, nof_prb * RE_X_RB * CP_NSYMB(cp));
-		fprintf(f, "];\n");
+		for (i=0;i<CP_NSYMB(cp);i++) {
+			fprintf(f, "ce(%d,:)=", slot_cnt * CP_NSYMB(cp) + i + 1);
+			vec_fprint_c(f, &ce[i * nof_prb * RE_X_RB], nof_prb * RE_X_RB);
+		}
 
 		slot_cnt++;
 	}
@@ -170,6 +173,6 @@ do_exit:
 	}
 	filesource_close(&fsrc);
 
-	printf("Done\n");
+	printf("Done processed %d slots\n", slot_cnt);
 	exit(0);
 }
diff --git a/examples/pss_scan_usrp.c b/examples/pss_scan_usrp.c
index afd700831..ca4fc26bf 100644
--- a/examples/pss_scan_usrp.c
+++ b/examples/pss_scan_usrp.c
@@ -12,20 +12,29 @@
 #include "uhd.h"
 #include "uhd_utils.h"
 
-int nof_slots=1000;
+#define MHZ 			1000000
+#define SAMP_FREQ 		1920000
+#define RSSI_FS			1000000
+#define FLEN 			9600
+#define FLEN_PERIOD		0.005
+
+#define IS_SIGNAL(i) (10*log10f(rssi[i]) + 30 > rssi_threshold)
+
+
 int band, earfcn=-1;
-float pss_threshold=15.0;
-int earfcn_start, earfcn_end = -1;
-float rssi_threshold = -42.0;
+float find_threshold = 40.0, track_threshold = 25.0;
+int earfcn_start=-1, earfcn_end = -1;
+float rssi_threshold = -30.0;
+int max_track_lost=9;
+int nof_frames_find=8, nof_frames_track=100, nof_samples_rssi=50000;
 
 cf_t *input_buffer;
 float *cfo_v;
-int *idx_v;
+int *idx_v, *idx_valid, *t;
 float *p2a_v;
 void *uhd;
 int nof_bands;
-int force_N_id_2;
-float gain = 30.0;
+float gain = 20.0;
 
 #define MAX_EARFCN 1000
 lte_earfcn_t channels[MAX_EARFCN];
@@ -34,45 +43,28 @@ float freqs[MAX_EARFCN];
 float cfo[MAX_EARFCN];
 float p2a[MAX_EARFCN];
 
-#define MHZ 			1000000
-#define SAMP_FREQ 		1920000
-#define RSSI_FS			1000000
-#define RSSI_NSAMP		50000
-#define FLEN 			9600
-#define FLEN_PERIOD		0.005
-
-#define IS_SIGNAL(i) (10*log10f(rssi[i]) + 30 > rssi_threshold)
+enum sync_state {INIT, FIND, TRACK, DONE};
 
 void print_to_matlab();
 
 void usage(char *prog) {
-	printf("Usage: %s [senvtr] -b band\n", prog);
-	printf("\t-s earfcn_start [Default %d]\n", earfcn_start);
+	printf("Usage: %s [seRrFfTtgv] -b band\n", prog);
+	printf("\t-s earfcn_start [Default All]\n");
 	printf("\t-e earfcn_end [Default All]\n");
-	printf("\t-n number of frames [Default %d]\n", nof_slots);
-	printf("\t-v [set verbose to debug, default none]\n");
-	printf("\t-t pss_threshold [Default %.2f]\n", pss_threshold);
+	printf("\t-R rssi_nof_samples [Default %d]\n", nof_samples_rssi);
 	printf("\t-r rssi_threshold [Default %.2f dBm]\n", rssi_threshold);
-	printf("\t-f force_N_id_2 [Default no]\n");
-	printf("\t-g gain [Default no %.2f dB]\n", gain);
+	printf("\t-F pss_find_nof_frames [Default %d]\n", nof_frames_find);
+	printf("\t-f pss_find_threshold [Default %.2f]\n", find_threshold);
+	printf("\t-T pss_track_nof_frames [Default %d]\n", nof_frames_track);
+	printf("\t-t pss_track_threshold [Default %.2f]\n", track_threshold);
+	printf("\t-g gain [Default %.2f dB]\n", gain);
+	printf("\t-v [set verbose to debug, default none]\n");
 }
 
 void parse_args(int argc, char **argv) {
 	int opt;
-	while ((opt = getopt(argc, argv, "gfrtbsenv")) != -1) {
+	while ((opt = getopt(argc, argv, "bseRrFfTtgv")) != -1) {
 		switch(opt) {
-		case 'g':
-			gain = atof(argv[optind]);
-			break;
-		case 'f':
-			force_N_id_2 = atoi(argv[optind]);
-			break;
-		case 't':
-			pss_threshold = atof(argv[optind]);
-			break;
-		case 'r':
-			rssi_threshold = -atof(argv[optind]);
-			break;
 		case 'b':
 			band = atoi(argv[optind]);
 			break;
@@ -82,8 +74,26 @@ void parse_args(int argc, char **argv) {
 		case 'e':
 			earfcn_end = atoi(argv[optind]);
 			break;
-		case 'n':
-			nof_slots = atoi(argv[optind]);
+		case 'R':
+			nof_samples_rssi = atoi(argv[optind]);
+			break;
+		case 'r':
+			rssi_threshold = -atof(argv[optind]);
+			break;
+		case 'F':
+			nof_frames_find = atoi(argv[optind]);
+			break;
+		case 'f':
+			find_threshold = atof(argv[optind]);
+			break;
+		case 'T':
+			nof_frames_track = atoi(argv[optind]);
+			break;
+		case 't':
+			track_threshold = atof(argv[optind]);
+			break;
+		case 'g':
+			gain = atof(argv[optind]);
 			break;
 		case 'v':
 			verbose++;
@@ -103,17 +113,27 @@ int base_init(int frame_length) {
 		exit(-1);
 	}
 
-	idx_v = malloc(nof_slots * sizeof(int));
+	idx_v = malloc(nof_frames_track * sizeof(int));
 	if (!idx_v) {
 		perror("malloc");
 		exit(-1);
 	}
-	cfo_v = malloc(nof_slots * sizeof(float));
+	idx_valid = malloc(nof_frames_track * sizeof(int));
+	if (!idx_valid) {
+		perror("malloc");
+		exit(-1);
+	}
+	t = malloc(nof_frames_track * sizeof(int));
+	if (!t) {
+		perror("malloc");
+		exit(-1);
+	}
+	cfo_v = malloc(nof_frames_track * sizeof(float));
 	if (!cfo_v) {
 		perror("malloc");
 		exit(-1);
 	}
-	p2a_v = malloc(nof_slots * sizeof(float));
+	p2a_v = malloc(nof_frames_track * sizeof(float));
 	if (!p2a_v) {
 		perror("malloc");
 		exit(-1);
@@ -137,6 +157,8 @@ void base_free() {
 	uhd_close(&uhd);
 	free(input_buffer);
 	free(idx_v);
+	free(idx_valid);
+	free(t);
 	free(cfo_v);
 	free(p2a_v);
 }
@@ -158,14 +180,61 @@ float mean_valid(int *idx_v, float *x, int nof_frames) {
 	}
 }
 
-int main(int argc, char **argv) {
-	int frame_cnt;
+int preprocess_idx(int *in, int *out, int *period, int len) {
+	int i, n;
+	n=0;
+	for (i=0;i<len;i++) {
+		if (in[i] != -1) {
+			out[n] = in[i];
+			period[n] = i;
+			n++;
+		}
+	}
+	return n;
+}
+
+int rssi_scan() {
+	int n=0;
 	int i;
-	int nsamples;
+	float rssi_d[MAX_EARFCN/10];
+
+	if (nof_bands > 100) {
+		/* scan every Mhz, that is 10 freqs */
+		for (i=0;i<nof_bands;i+=10) {
+			freqs[n] = channels[i].fd * MHZ;
+			n++;
+		}
+		if (uhd_rssi_scan(uhd, freqs, rssi_d, n, (double) RSSI_FS, nof_samples_rssi)) {
+			fprintf(stderr, "Error while doing RSSI scan\n");
+			return -1;
+		}
+		/* linearly interpolate the rssi vector */
+		interp_linear_f(rssi_d, rssi, 10, n);
+	} else {
+		for (i=0;i<nof_bands;i++) {
+			freqs[i] = channels[i].fd * MHZ;
+		}
+		if (uhd_rssi_scan(uhd, freqs, rssi, nof_bands, (double) RSSI_FS, nof_samples_rssi)) {
+			fprintf(stderr, "Error while doing RSSI scan\n");
+			return -1;
+		}
+		n = nof_bands;
+	}
+
+	return n;
+}
+
+
+int main(int argc, char **argv) {
+	int frame_cnt, valid_frames;
+	int freq;
 	int cell_id;
 	sync_t synch;
 	float max_peak_to_avg;
 	float sfo;
+	int find_idx, last_found;
+	enum sync_state state;
+	int n;
 
 	if (argc < 3) {
 		usage(argv[0]);
@@ -179,104 +248,140 @@ int main(int argc, char **argv) {
 		exit(-1);
 	}
 
-	if (sync_init(&synch)) {
+	if (sync_init(&synch, FLEN)) {
 		fprintf(stderr, "Error initiating PSS/SSS\n");
 		exit(-1);
 	}
-	sync_set_threshold(&synch, pss_threshold);
 	sync_pss_det_peakmean(&synch);
 
-	if (force_N_id_2 != -1) {
-		sync_force_N_id_2(&synch, force_N_id_2);
-	}
-
 	nof_bands = lte_band_get_fd_band(band, channels, earfcn_start, earfcn_end, MAX_EARFCN);
-	printf("RSSI scan: %d freqs in band %d\n", nof_bands, band);
-	for (i=0;i<nof_bands;i++) {
-		freqs[i] = channels[i].fd * MHZ;
-	}
-
+	printf("RSSI scan: %d freqs in band %d, RSSI threshold %.2f dBm\n", nof_bands, band, rssi_threshold);
 
-	if (uhd_rssi_scan(uhd, freqs, rssi, nof_bands, (double) RSSI_FS, RSSI_NSAMP)) {
-		fprintf(stderr, "Error while doing RSSI scan\n");
+	n = rssi_scan();
+	if (n == -1) {
 		exit(-1);
 	}
 
-	printf("\nDone. Starting PSS search\n");
+	printf("\nDone. Starting PSS search on %d channels\n", n);
 	usleep(500000);
-	printf("Setting sampling frequency %.2f MHz\n", (float) SAMP_FREQ/MHZ);
+	INFO("Setting sampling frequency %.2f MHz\n", (float) SAMP_FREQ/MHZ);
 	uhd_set_rx_srate(uhd, SAMP_FREQ);
 
 	uhd_set_rx_gain(uhd, gain);
 
 	print_to_matlab();
 
-	int first = 1;
-	for (i=0;i<nof_bands;i++) {
+
+	freq=0;
+	state = INIT;
+	while(freq<nof_bands) {
 		/* scan only bands above rssi_threshold */
-		if (IS_SIGNAL(i)) {
-			uhd_set_rx_freq(uhd, (double) channels[i].fd * MHZ);
-			uhd_rx_wait_lo_locked(uhd);
+		if (!IS_SIGNAL(freq)) {
+			INFO("[%3d/%d]: Skipping EARFCN %d %.2f MHz RSSI %.2f dB\n", freq, nof_bands,
+								channels[freq].id, channels[freq].fd,10*log10f(rssi[freq]) + 30);
+			freq++;
+		} else {
+			if (state == TRACK || state == FIND) {
+				uhd_recv(uhd, &input_buffer[FLEN], FLEN, 1);
+			}
+			switch(state) {
+			case INIT:
+				DEBUG("Stopping receiver...\n",0);
+				uhd_stop_rx_stream(uhd);
+
+				/* set freq */
+				uhd_set_rx_freq(uhd, (double) channels[freq].fd * MHZ);
+				uhd_rx_wait_lo_locked(uhd);
+				DEBUG("Set freq to %.3f MHz\n", (double) channels[freq].fd);
 
-			if (first) {
-				INFO("Starting receiver...\n",0);
+				DEBUG("Starting receiver...\n",0);
 				uhd_start_rx_stream(uhd);
-				first = 0;
-			}
 
-			frame_cnt = 0;
-			nsamples = 0;
-			max_peak_to_avg = -99;
-			nsamples += uhd_recv(uhd, input_buffer, FLEN, 1);
-			cell_id = -1;
-			while(frame_cnt < nof_slots) {
-				if (frame_cnt) {
-					nsamples += uhd_recv(uhd, &input_buffer[FLEN], FLEN, 1);
+				/* init variables */
+				frame_cnt = 0;
+				max_peak_to_avg = -99;
+				cell_id = -1;
+
+				/* receive first frame */
+				uhd_recv(uhd, input_buffer, FLEN, 1);
+
+				/* set find_threshold and go to FIND state */
+				sync_set_threshold(&synch, find_threshold);
+				sync_force_N_id_2(&synch, -1);
+				state = FIND;
+				break;
+			case FIND:
+				/* find peak in all frame */
+				find_idx = sync_run(&synch, input_buffer, FLEN);
+				DEBUG("[%3d/%d]: PAR=%.2f\n", freq, nof_bands, sync_get_peak_to_avg(&synch));
+				if (find_idx != -1) {
+					/* if found peak, go to track and set lower threshold */
+					frame_cnt = -1;
+					last_found = 0;
+					sync_set_threshold(&synch, track_threshold);
+					sync_force_N_id_2(&synch, sync_get_N_id_2(&synch));
+					state = TRACK;
+					INFO("[%3d/%d]: EARFCN %d Freq. %.2f MHz PSS found PAR %.2f dB\n", freq, nof_bands,
+												channels[freq].id, channels[freq].fd,
+												10*log10f(sync_get_peak_to_avg(&synch)));
+				} else {
+					if (frame_cnt >= nof_frames_find) {
+						state = INIT;
+						printf("[%3d/%d]: EARFCN %d Freq. %.2f MHz No PSS found\r", freq, nof_bands,
+													channels[freq].id, channels[freq].fd, frame_cnt - last_found);
+						if (VERBOSE_ISINFO()) {
+							printf("\n");
+						}
+						freq++;
+					}
 				}
-
-				idx_v[frame_cnt] = sync_run(&synch, input_buffer, frame_cnt?FLEN:0);
+				break;
+			case TRACK:
+				/* TODO: find peak around find_idx */
+				idx_v[frame_cnt] = sync_run(&synch, input_buffer, FLEN);
 				p2a_v[frame_cnt] = sync_get_peak_to_avg(&synch);
+
+				/* save cell id for the best peak-to-avg */
+				if (p2a_v[frame_cnt] > max_peak_to_avg) {
+					max_peak_to_avg = p2a_v[frame_cnt];
+					cell_id = sync_get_cell_id(&synch);
+				}
 				if (idx_v[frame_cnt] != -1) {
-					/* save cell id for the best peak-to-avg */
-					if (p2a_v[frame_cnt] > max_peak_to_avg) {
-						max_peak_to_avg = p2a_v[frame_cnt];
-						cell_id = sync_get_cell_id(&synch);
-					}
 					cfo_v[frame_cnt] = sync_get_cfo(&synch);
+					last_found = frame_cnt;
 				} else {
 					cfo_v[frame_cnt] = 0.0;
 				}
-				if (frame_cnt) {
-					memcpy(input_buffer, &input_buffer[FLEN], FLEN * sizeof(cf_t));
-				}
-				if (VERBOSE_ISINFO()) {
-					printf("[%4d] - idx: %5d\tpeak-to-avg: %3.2f\tcfo=%.3f\r", frame_cnt,
-						idx_v[frame_cnt], p2a_v[frame_cnt], cfo_v[frame_cnt]);
+				/* if we missed to many frames it is not a cell, next freq */
+				if (frame_cnt - last_found > max_track_lost) {
+					INFO("\n[%3d/%d]: EARFCN %d Freq. %.2f MHz %d frames lost\n", freq, nof_bands,
+							channels[freq].id, channels[freq].fd, frame_cnt - last_found);
+
+					state = INIT;
+					freq++;
+				} else if (frame_cnt >= nof_frames_track) {
+					state = DONE;
 				}
-				frame_cnt++;
+				break;
+			case DONE:
+
+				cfo[freq] = mean_valid(idx_v, cfo_v, frame_cnt);
+				p2a[freq] = mean_valid(idx_v, p2a_v, frame_cnt);
+				valid_frames = preprocess_idx(idx_v, idx_valid, t, frame_cnt);
+				sfo = sfo_estimate_period(idx_valid, t, valid_frames, FLEN_PERIOD);
+
+				printf("\n[%3d/%d]: FOUND EARFCN %d Freq. %.2f MHz, "
+						"RSSI %3.2f dBm, PAR %2.2f dB, CFO=%+.2f KHz, SFO=%+2.3f KHz, CELL_ID=%3d\n", freq, nof_bands,
+								channels[freq].id, channels[freq].fd, 10*log10f(rssi[freq]) + 30,
+								10*log10f(p2a[freq]), cfo[freq] * 15, sfo / 1000, cell_id);
+				state = INIT;
+				freq++;
+				break;
 			}
-
-			cfo[i] = mean_valid(idx_v, cfo_v, nof_slots);
-			p2a[i] = sum_r(p2a_v, nof_slots) / nof_slots;
-			if (channels[i].id == 1900
-					|| channels[i].id == 1901) {
-				vec_fprint_i(stdout, idx_v, nof_slots);
+			if (state == TRACK || (state == FIND && frame_cnt)) {
+				memcpy(input_buffer, &input_buffer[FLEN], FLEN * sizeof(cf_t));
 			}
-
-			sfo = sfo_estimate(idx_v, nof_slots, FLEN_PERIOD);
-			if (VERBOSE_ISINFO()) {
-				printf("\n");
-			}
-
-			printf("[%3d/%d]: EARFCN %d Freq. %.2f MHz, "
-					"RSSI %3.2f dBm, PSS %2.2f dB, CFO=%+2.1f KHz, SFO=%+2.1f KHz, CELL_ID=%3d\n", i, nof_bands,
-							channels[i].id, channels[i].fd, 10*log10f(rssi[i]) + 30,
-							10*log10f(p2a[i]), cfo[i] * 15, sfo / 1000, cell_id);
-			print_to_matlab();
-
-		} else {
-			INFO("[%3d/%d]: EARFCN %d Freq. %.2f MHz. RSSI below threshold (%3.2f < %3.2f dBm)\n",
-					i, nof_bands, channels[i].id, channels[i].fd, 10*log10f(rssi[i]) + 30, rssi_threshold);
+			frame_cnt++;
 		}
 	}
 
@@ -285,7 +390,7 @@ int main(int argc, char **argv) {
 	sync_free(&synch);
 	base_free();
 
-	printf("Done\n");
+	printf("\n\nDone\n");
 	exit(0);
 }
 
diff --git a/include/resampling/interp.h b/include/resampling/interp.h
index 9d42e3a1c..207a39573 100644
--- a/include/resampling/interp.h
+++ b/include/resampling/interp.h
@@ -21,3 +21,4 @@ typedef _Complex float cf_t;
 
 void interp_linear_offset(cf_t *input, cf_t *output, int M, int len, int off_st, int off_end);
 void interp_linear(cf_t *input, cf_t *output, int M, int len);
+void interp_linear_f(float *input, float *output, int M, int len);
diff --git a/include/sync/sfo.h b/include/sync/sfo.h
index a2a45748e..a759184f1 100644
--- a/include/sync/sfo.h
+++ b/include/sync/sfo.h
@@ -20,6 +20,6 @@
 #define SFO_
 
 float sfo_estimate(int *t0, int len, float period);
-
+float sfo_estimate_period(int *t0, int *t, int len, float period);
 
 #endif
diff --git a/include/sync/sync.h b/include/sync/sync.h
index 59a2c2ad3..c3fc547fb 100644
--- a/include/sync/sync.h
+++ b/include/sync/sync.h
@@ -49,7 +49,7 @@ int sync_get_N_id_2(sync_t *q);
 int sync_get_N_id_1(sync_t *q);
 int sync_get_cell_id(sync_t *q);
 void sync_set_threshold(sync_t *q, float threshold);
-int sync_init(sync_t *q);
+int sync_init(sync_t *q, int frame_size);
 void sync_free(sync_t *q);
 
 #endif
diff --git a/lib/ch_estimation/src/chest.c b/lib/ch_estimation/src/chest.c
index 6f4ac002c..6a9020e3d 100644
--- a/lib/ch_estimation/src/chest.c
+++ b/lib/ch_estimation/src/chest.c
@@ -81,7 +81,7 @@ void chest_ce_ref(chest_t *q, cf_t *input, int nslot, int port_id, int nref) {
 	q->refsignal[port_id][nslot].refs[nref].recv_simbol = channel_ref;
 	/* FIXME: compare with treshold */
 	if (channel_ref != 0) {
-		q->refsignal[port_id][nslot].ch_est[nref] = known_ref/channel_ref;
+		q->refsignal[port_id][nslot].ch_est[nref] = channel_ref/known_ref;
 	} else {
 		q->refsignal[port_id][nslot].ch_est[nref] = 0;
 	}
@@ -118,14 +118,10 @@ void chest_ce_slot_port(chest_t *q, cf_t *input, cf_t *ce, int nslot, int port_i
 	for (i=0;i<q->nof_prb * RE_X_RB; i++) {
 		for (j=0;j<r->nsymbols;j++) {
 			x[j] = ce[r->symbols_ref[j] * q->nof_prb * RE_X_RB + i];
-			printf("x[%d]=ce[%d]=%.3f\n", j,
-					r->symbols_ref[j] * q->nof_prb * RE_X_RB + i,
-					cabsf(x[j]));
 		}
 		interp_linear_offset(x, y, r->symbols_ref[1]-r->symbols_ref[0],
 				2, r->symbols_ref[0], 3);
 		for (j=0;j<q->nof_symbols;j++) {
-			printf("ce[%d] = y[%d] =%.3f\n", j * q->nof_prb * RE_X_RB + i, j, cabsf(x[j]));
 			ce[j * q->nof_prb * RE_X_RB + i] = y[j];
 		}
 	}
diff --git a/lib/resampling/src/interp.c b/lib/resampling/src/interp.c
index 657c29e4d..392b5e511 100644
--- a/lib/resampling/src/interp.c
+++ b/lib/resampling/src/interp.c
@@ -57,3 +57,14 @@ void interp_linear_offset(cf_t *input, cf_t *output, int M, int len, int off_st,
 void interp_linear(cf_t *input, cf_t *output, int M, int len) {
 	interp_linear_offset(input, output, M, len, 0, 0);
 }
+
+
+/* Performs 1st order integer linear interpolation */
+void interp_linear_f(float *input, float *output, int M, int len) {
+	int i, j;
+	for (i=0;i<len-1;i++) {
+		for (j=0;j<M;j++) {
+			output[i*M+j] = input[i] + j * (input[i+1]-input[i]) / M;
+		}
+	}
+}
diff --git a/lib/sync/src/pss.c b/lib/sync/src/pss.c
index 3bd21a382..d4295a3e4 100644
--- a/lib/sync/src/pss.c
+++ b/lib/sync/src/pss.c
@@ -132,14 +132,14 @@ int pss_generate(cf_t *signal, int direction, int N_id_2) {
 	for (i = 0; i < PSS_LEN / 2; i++) {
 		arg = (float) sign * M_PI * root_value[root_idx]
 				* ((float) i * ((float) i + 1.0)) / 63.0;
-		__real__ signal[i] = cos(arg);
-		__imag__ signal[i] = sin(arg);
+		__real__ signal[i] = cosf(arg);
+		__imag__ signal[i] = sinf(arg);
 	}
 	for (i = PSS_LEN / 2; i < PSS_LEN; i++) {
 		arg = (float) sign * M_PI * root_value[root_idx]
 				* (((float) i + 2.0) * ((float) i + 1.0)) / 63.0;
-		__real__ signal[i] = cos(arg);
-		__imag__ signal[i] = sin(arg);
+		__real__ signal[i] = cosf(arg);
+		__imag__ signal[i] = sinf(arg);
 	}
 	return 0;
 }
diff --git a/lib/sync/src/sfo.c b/lib/sync/src/sfo.c
index 70f576272..ab5553370 100644
--- a/lib/sync/src/sfo.c
+++ b/lib/sync/src/sfo.c
@@ -16,6 +16,8 @@
  * along with OSLD-lib.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <stdio.h>
+#include <stdlib.h>
 #include "sync/sfo.h"
 
 /* Estimate SFO based on the array of time estimates t0
@@ -29,3 +31,17 @@ float sfo_estimate(int *t0, int len, float period) {
 	}
 	return sfo;
 }
+
+/* Same as sfo_estimate but period is non-uniform.
+ * Vector t is the sampling time times period for each t0
+ */
+float sfo_estimate_period(int *t0, int *t, int len, float period) {
+	int i;
+	float sfo=0.0;
+	for (i=1;i<len;i++) {
+		if (abs(t0[i]-t0[i-1]) < 5000) {
+			sfo += (t0[i]-t0[i-1])/(t[i] - t[i-1])/period;
+		}
+	}
+	return sfo/(len-2);
+}
diff --git a/lib/sync/src/sync.c b/lib/sync/src/sync.c
index 0e0a5be28..a63b375d2 100644
--- a/lib/sync/src/sync.c
+++ b/lib/sync/src/sync.c
@@ -23,7 +23,7 @@
 #include "lte/base.h"
 #include "sync/sync.h"
 
-int sync_init(sync_t *q) {
+int sync_init(sync_t *q, int frame_size) {
 	int N_id_2;
 
 	bzero(q, sizeof(sync_t));
@@ -32,7 +32,7 @@ int sync_init(sync_t *q) {
 	q->pss_mode = PEAK_MEAN;
 
 	for (N_id_2=0;N_id_2<3;N_id_2++) {
-		if (pss_synch_init(&q->pss[N_id_2], 960)) {
+		if (pss_synch_init(&q->pss[N_id_2], frame_size)) {
 			fprintf(stderr, "Error initializing PSS object\n");
 			return -1;
 		}
@@ -153,10 +153,11 @@ int sync_run(sync_t *q, cf_t *input, int read_offset) {
 	}
 	if (peak_detected) {
 
-		INFO("PSS peak detected N_id_2=%d, pos=%d value=%.2f\n", N_id_2, peak_pos[N_id_2], peak_value[N_id_2]);
 
 		q->cfo = pss_synch_cfo_compute(&q->pss[N_id_2], &input[read_offset + peak_pos[N_id_2]-128]);
-		INFO("Estimated CFO=%.4f\n", q->cfo);
+
+		INFO("PSS peak detected N_id_2=%d, pos=%d peak=%.2f par=%.2f th=%.2f cfo=%.4f\n", N_id_2,
+				peak_pos[N_id_2], peak_value[N_id_2], q->peak_to_avg, q->threshold, q->cfo);
 
 		sss_idx = read_offset + peak_pos[N_id_2]-2*(128+CP(128,CPNORM_LEN));
 		if (sss_idx>= 0) {
diff --git a/lib/utils/src/vector.c b/lib/utils/src/vector.c
index 1882cac45..1336173f5 100644
--- a/lib/utils/src/vector.c
+++ b/lib/utils/src/vector.c
@@ -117,7 +117,7 @@ void vec_fprint_c(FILE *stream, _Complex float *x, int len) {
 		//if (!((i+1)%10))
 		//	fprintf(stream, "\n");
 	}
-	fprintf(stream, "]\n");
+	fprintf(stream, "];\n");
 }
 
 void vec_fprint_f(FILE *stream, float *x, int len) {
@@ -128,7 +128,7 @@ void vec_fprint_f(FILE *stream, float *x, int len) {
 		//if (!((i+1)%10))
 		//	fprintf(stream, "\n");
 	}
-	fprintf(stream, "]\n");
+	fprintf(stream, "];\n");
 }
 
 
@@ -138,7 +138,7 @@ void vec_fprint_i(FILE *stream, int *x, int len) {
 	for (i=0;i<len;i++) {
 		fprintf(stream, "%d, ", x[i]);
 	}
-	fprintf(stream, "]\n");
+	fprintf(stream, "];\n");
 }
 
 void vec_conj(_Complex float *x, _Complex float *y, int len) {
diff --git a/matlab/common/read_complex.m b/matlab/common/read_complex.m
new file mode 100644
index 000000000..d9185a7ea
--- /dev/null
+++ b/matlab/common/read_complex.m
@@ -0,0 +1,20 @@
+function [ out ] = read_complex( filename, count )
+%READ_COMPLEX Summary of this function goes here
+%   Detailed explanation goes here
+
+    [tidin msg]=fopen(filename,'r');
+    if (tidin==-1)
+        fprintf('error opening %s: %s\n',filename, msg);
+        out=[];
+        return
+    end
+
+    if (nargin==1) 
+        count=inf;
+    end
+
+    x=fread(tidin,2*count,'single');
+    i=1:2:length(x);
+    out=x(i)+x(i+1)*1i;
+end
+
diff --git a/matlab/common/write_complex.m b/matlab/common/write_complex.m
new file mode 100644
index 000000000..a152c8672
--- /dev/null
+++ b/matlab/common/write_complex.m
@@ -0,0 +1,22 @@
+function [ out ] = write_complex( filename, x)
+%READ_COMPLEX Summary of this function goes here
+%   Detailed explanation goes here
+
+    [tidin msg]=fopen(filename,'w');
+    if (tidin==-1)
+        fprintf('error opening %s: %s\n',filename, msg);
+        out=[];
+        return
+    end
+
+    if (isreal(x))
+        y=x;
+    else
+        i=1:2:2*length(x);
+        y(i)=real(x);
+        y(i+1)=imag(x);
+    end
+    
+    fwrite(tidin,y,'single');
+end
+
diff --git a/matlab/sync/check_pss.m b/matlab/sync/check_pss.m
index bcc595c33..133e6f127 100644
--- a/matlab/sync/check_pss.m
+++ b/matlab/sync/check_pss.m
@@ -1,4 +1,4 @@
-function [ fs ] = check_pss( x, N_id_2)
+function [ fs ] = check_pss( x, N_id_2, threshold)
 %CHECK_PSS Summary of this function goes here
 %   Detailed explanation goes here
 flen=9600;
@@ -11,28 +11,23 @@ fs=zeros(nf,1);
 cfo=zeros(nf,1);
 cfo2=zeros(nf,1);
 m_p=zeros(nf,1);
-for i=1:nf-1
-    [fs(i) cfo(i) m_p(i)]=find_pss(xf(:,i),N_id_2,false);
-    if (fs(i)<0)
-        j=0;
-    end
-%    cfo2(i) = cfo_estimate_cp(xf(fs(i)+960:fs(i)+2*960,i),7,128,10,9);
+for i=1:nf
+    [fs(i) cfo(i) m_p(i)]=find_pss(xf(:,i),N_id_2,false, threshold);
 end
 
-sfo=sfo_estimate(fs, 5/1000);
+fs=fs+960;
+[sfo sfo_v]=sfo_estimate(fs, 5/1000);
 
 subplot(1,3,1)
 plot(1:nf,fs)
-legend('PSS-based');
 subplot(1,3,2)
-plot(1:nf, cfo, 1:nf, cfo2)
-legend('PSS-based','CP-based');
+plot(1:nf, cfo)
 if (nf > 0)
     axis([0 nf -0.5 0.5])
 end
 subplot(1,3,3)
 plot(m_p)
-fprintf('pss_mean=%g, pss_var=%g, cp_mean=%g, cp_var=%g m_p=%g sfo=%g Hz\n',mean(cfo),var(cfo), mean(cfo2), var(cfo2), mean(m_p), sfo)
+fprintf('cfo_mean=%g Hz, cfo_std=%g Hz, m_p=%g sfo=%g Hz\n',15000*nanmean(cfo),15000*nanstd(cfo), nanmean(m_p), sfo)
 end
 
 
diff --git a/matlab/sync/find_pss.m b/matlab/sync/find_pss.m
index 8ed366e1b..60f420592 100644
--- a/matlab/sync/find_pss.m
+++ b/matlab/sync/find_pss.m
@@ -1,6 +1,10 @@
-function [ fs eps p_m w2] = find_pss( x, N_id_2, doplot)
+function [ fs eps p_m w2] = find_pss( x, N_id_2, doplot, threshold)
     if nargin == 2
         doplot = false;
+        threshold = 0;
+    end
+    if nargin == 3
+        threshold = 0;
     end
     
     c=lte_pss_zc(N_id_2);
@@ -10,26 +14,29 @@ function [ fs eps p_m w2] = find_pss( x, N_id_2, doplot)
 
     w2=conv(x,ccf);
     if (doplot)
-        plot(abs(w2))
+        plot(10*log10(abs(w2)./mean(abs(w2))));
+        axis([0 length(w2) 0 20])
     end
     [m i]=max(abs(w2));
     fs=i-960;
     p_m = m/mean(abs(w2));
+    
     if doplot
         fprintf('Frame starts at %d, m=%g, p=%g, p/m=%g dB\n',fs, ...
             mean(abs(w2)), m, 10*log10(m/mean(abs(w2))));
     end
     
     % Estimate PSS-aided CFO
-%     if (i - 129)
-%         y=ccf.*x(i-128:i-1);
-%     
-%         y0=y(1:64);
-%         y1=y(65:length(y));    
-%     
-%         eps=angle(conj(sum(y0))*sum(y1))/pi;    
-%     else
-        eps = NaN;
-%     end
+    if (i > 200 && i<length(x)&& p_m > threshold)
+        y=ccf.*x(i-128:i-1);
+    
+        y0=y(1:64);
+        y1=y(65:length(y));    
+    
+        eps=angle(conj(sum(y0))*sum(y1))/pi;    
+    else
+         eps = NaN;
+         fs = NaN;
+    end
 end
 
diff --git a/matlab/sync/ifo_pss.m b/matlab/sync/ifo_pss.m
new file mode 100644
index 000000000..739cbe1a7
--- /dev/null
+++ b/matlab/sync/ifo_pss.m
@@ -0,0 +1,13 @@
+function [ ifo ] = ifo_pss( r_pss, x_pss)
+
+k=1;
+v=-31:31;
+c=zeros(length(v),1);
+for i=v
+    c(k) = ifo_pss_corr(i, r_pss, x_pss);
+    k=k+1;
+end
+[m i]=max(c);
+ifo=v(i);
+plot(v,c); 
+
diff --git a/matlab/sync/ifo_pss_corr.m b/matlab/sync/ifo_pss_corr.m
new file mode 100644
index 000000000..c921cba9c
--- /dev/null
+++ b/matlab/sync/ifo_pss_corr.m
@@ -0,0 +1,10 @@
+function [ corr ] = ifo_pss_corr( n, r_pss, x_pss)
+
+    x=0;
+    for i=1:length(x_pss)
+        x=x+r_pss(1+mod(i+n-1,length(r_pss)))*conj(x_pss(i));
+    end
+    corr=real(exp(1i*2*pi*9*n/128)*x);
+%    corr=abs(x);
+end
+
diff --git a/matlab/sync/sfo_estimate.m b/matlab/sync/sfo_estimate.m
index d9f46690f..f0160a868 100644
--- a/matlab/sync/sfo_estimate.m
+++ b/matlab/sync/sfo_estimate.m
@@ -1,7 +1,18 @@
-function [ sfo ] = sfo_estimate( fs, T )
+function [ sfo sfo_v ] = sfo_estimate( fs, T )
 
-sfo = 0;
-for i=2:length(fs)
-    sfo=sfo + (fs(i)-fs(i-1))/length(fs)/T;
+
+nanfs=fs(~isnan(fs));
+idx=find(~isnan(fs));
+sfo_v = zeros(length(nanfs)-1,1);
+for i=2:length(nanfs)
+    if (abs(nanfs(i)-nanfs(i-1))<9000)
+        sfo_v(i-1)=(nanfs(i)-nanfs(i-1))/T/(idx(i)-idx(i-1));
+    else
+        sfo_v(i-1)=sfo_v(i-2);
+    end
 end
 
+sfo = mean(sfo_v);
+
+
+
diff --git a/uhd/uhd_imp.cpp b/uhd/uhd_imp.cpp
index 69d285b7c..1fa66766d 100644
--- a/uhd/uhd_imp.cpp
+++ b/uhd/uhd_imp.cpp
@@ -70,7 +70,7 @@ int uhd_open(char *args, void **h) {
 	std::string _args=std::string(args);
 	handler->usrp = uhd::usrp::multi_usrp::make(_args);
 
-	uhd::msg::register_handler(&my_handler);
+	//uhd::msg::register_handler(&my_handler);
 
 	std::string otw, cpu;
 	otw="sc16";